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

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

511 files changed, 152073 insertions, 0 deletions

diff --git a/drivers/allwinner/axp/axp803.c b/drivers/allwinner/axp/axp803.c
new file mode 100644
index 0000000..19a9549
--- /dev/null
+++ b/drivers/allwinner/axp/axp803.c
@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <drivers/allwinner/axp.h>
+
+const uint8_t axp_chip_id = AXP803_CHIP_ID;
+const char *const axp_compatible = "x-powers,axp803";
+
+#if SUNXI_SETUP_REGULATORS == 1
+const struct axp_regulator axp_regulators[] = {
+	{"aldo1",  700, 3300, 100, NA, 0x28, 0x13, 5},
+	{"dcdc1", 1600, 3400, 100, NA, 0x20, 0x10, 0},
+	{"dcdc5",  800, 1840,  10, 32, 0x24, 0x10, 4},
+	{"dcdc6",  600, 1520,  10, 50, 0x25, 0x10, 5},
+	{"dldo1",  700, 3300, 100, NA, 0x15, 0x12, 3},
+	{"dldo2",  700, 4200, 100, 27, 0x16, 0x12, 4},
+	{"dldo3",  700, 3300, 100, NA, 0x17, 0x12, 5},
+	{"dldo4",  700, 3300, 100, NA, 0x18, 0x12, 6},
+	{"fldo1",  700, 1450,  50, NA, 0x1c, 0x13, 2},
+	{}
+};
+#endif
diff --git a/drivers/allwinner/axp/axp805.c b/drivers/allwinner/axp/axp805.c
new file mode 100644
index 0000000..3a03fec
--- /dev/null
+++ b/drivers/allwinner/axp/axp805.c
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <drivers/allwinner/axp.h>
+
+const uint8_t axp_chip_id = AXP805_CHIP_ID;
+const char *const axp_compatible = "x-powers,axp805";
+
+#if SUNXI_SETUP_REGULATORS == 1
+/*
+ * The "dcdcd" split changes the step size by a factor of 5, not 2;
+ * disallow values above the split to maintain accuracy.
+ */
+const struct axp_regulator axp_regulators[] = {
+	{"dcdca",  600, 1520,  10, 50, 0x12, 0x10, 0},
+	{"dcdcb", 1000, 2550,  50, NA, 0x13, 0x10, 1},
+	{"dcdcc",  600, 1520,  10, 50, 0x14, 0x10, 2},
+	{"dcdcd",  600, 1500,  20, NA, 0x15, 0x10, 3},
+	{"dcdce", 1100, 3400, 100, NA, 0x16, 0x10, 4},
+	{"aldo1",  700, 3300, 100, NA, 0x17, 0x10, 5},
+	{"aldo2",  700, 3300, 100, NA, 0x18, 0x10, 6},
+	{"aldo3",  700, 3300, 100, NA, 0x19, 0x10, 7},
+	{"bldo1",  700, 1900, 100, NA, 0x20, 0x11, 0},
+	{"bldo2",  700, 1900, 100, NA, 0x21, 0x11, 1},
+	{"bldo3",  700, 1900, 100, NA, 0x22, 0x11, 2},
+	{"bldo4",  700, 1900, 100, NA, 0x23, 0x11, 3},
+	{"cldo1",  700, 3300, 100, NA, 0x24, 0x11, 4},
+	{"cldo2",  700, 4200, 100, 27, 0x25, 0x11, 5},
+	{"cldo3",  700, 3300, 100, NA, 0x26, 0x11, 6},
+	{}
+};
+#endif
diff --git a/drivers/allwinner/axp/common.c b/drivers/allwinner/axp/common.c
new file mode 100644
index 0000000..f1250b0
--- /dev/null
+++ b/drivers/allwinner/axp/common.c
@@ -0,0 +1,212 @@
+/*
+ * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+
+#include <libfdt.h>
+
+#include <common/debug.h>
+#include <drivers/allwinner/axp.h>
+
+int axp_check_id(void)
+{
+	int ret;
+
+	ret = axp_read(0x03);
+	if (ret < 0)
+		return ret;
+
+	ret &= 0xcf;
+	if (ret != axp_chip_id) {
+		ERROR("PMIC: Found unknown PMIC %02x\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int axp_clrsetbits(uint8_t reg, uint8_t clr_mask, uint8_t set_mask)
+{
+	uint8_t val;
+	int ret;
+
+	ret = axp_read(reg);
+	if (ret < 0)
+		return ret;
+
+	val = (ret & ~clr_mask) | set_mask;
+
+	return axp_write(reg, val);
+}
+
+void axp_power_off(void)
+{
+	/* Set "power disable control" bit */
+	axp_setbits(0x32, BIT(7));
+}
+
+#if SUNXI_SETUP_REGULATORS == 1
+/*
+ * Retrieve the voltage from a given regulator DTB node.
+ * Both the regulator-{min,max}-microvolt properties must be present and
+ * have the same value. Return that value in millivolts.
+ */
+static int fdt_get_regulator_millivolt(const void *fdt, int node)
+{
+	const fdt32_t *prop;
+	uint32_t min_volt;
+
+	prop = fdt_getprop(fdt, node, "regulator-min-microvolt", NULL);
+	if (prop == NULL)
+		return -EINVAL;
+	min_volt = fdt32_to_cpu(*prop);
+
+	prop = fdt_getprop(fdt, node, "regulator-max-microvolt", NULL);
+	if (prop == NULL)
+		return -EINVAL;
+
+	if (fdt32_to_cpu(*prop) != min_volt)
+		return -EINVAL;
+
+	return min_volt / 1000;
+}
+
+static int setup_regulator(const void *fdt, int node,
+			   const struct axp_regulator *reg)
+{
+	uint8_t val;
+	int mvolt;
+
+	mvolt = fdt_get_regulator_millivolt(fdt, node);
+	if (mvolt < reg->min_volt || mvolt > reg->max_volt)
+		return -EINVAL;
+
+	val = (mvolt / reg->step) - (reg->min_volt / reg->step);
+	if (val > reg->split)
+		val = ((val - reg->split) / 2) + reg->split;
+
+	axp_write(reg->volt_reg, val);
+	axp_setbits(reg->switch_reg, BIT(reg->switch_bit));
+
+	INFO("PMIC: %s voltage: %d.%03dV\n", reg->dt_name,
+	     mvolt / 1000, mvolt % 1000);
+
+	return 0;
+}
+
+static bool is_node_disabled(const void *fdt, int node)
+{
+	const char *cell;
+	cell = fdt_getprop(fdt, node, "status", NULL);
+	if (cell == NULL) {
+		return false;
+	}
+	return strcmp(cell, "okay") != 0;
+}
+
+static bool should_enable_regulator(const void *fdt, int node)
+{
+	if (is_node_disabled(fdt, node)) {
+		return false;
+	}
+	if (fdt_getprop(fdt, node, "phandle", NULL) != NULL) {
+		return true;
+	}
+	if (fdt_getprop(fdt, node, "regulator-always-on", NULL) != NULL) {
+		return true;
+	}
+	return false;
+}
+
+static bool board_uses_usb0_host_mode(const void *fdt)
+{
+	int node, length;
+	const char *prop;
+
+	node = fdt_node_offset_by_compatible(fdt, -1,
+					     "allwinner,sun8i-a33-musb");
+	if (node < 0) {
+		return false;
+	}
+
+	prop = fdt_getprop(fdt, node, "dr_mode", &length);
+	if (!prop) {
+		return false;
+	}
+
+	return !strncmp(prop, "host", length);
+}
+
+void axp_setup_regulators(const void *fdt)
+{
+	int node;
+	bool sw = false;
+
+	if (fdt == NULL)
+		return;
+
+	/* locate the PMIC DT node, bail out if not found */
+	node = fdt_node_offset_by_compatible(fdt, -1, axp_compatible);
+	if (node < 0) {
+		WARN("PMIC: No PMIC DT node, skipping setup\n");
+		return;
+	}
+
+	/* This applies to AXP803 only. */
+	if (fdt_getprop(fdt, node, "x-powers,drive-vbus-en", NULL) &&
+	    board_uses_usb0_host_mode(fdt)) {
+		axp_clrbits(0x8f, BIT(4));
+		axp_setbits(0x30, BIT(2));
+		INFO("PMIC: Enabling DRIVEVBUS\n");
+	}
+
+	/* descend into the "regulators" subnode */
+	node = fdt_subnode_offset(fdt, node, "regulators");
+	if (node < 0) {
+		WARN("PMIC: No regulators DT node, skipping setup\n");
+		return;
+	}
+
+	/* iterate over all regulators to find used ones */
+	fdt_for_each_subnode(node, fdt, node) {
+		const struct axp_regulator *reg;
+		const char *name;
+		int length;
+
+		/* We only care if it's always on or referenced. */
+		if (!should_enable_regulator(fdt, node))
+			continue;
+
+		name = fdt_get_name(fdt, node, &length);
+
+		/* Enable the switch last to avoid overheating. */
+		if (!strncmp(name, "dc1sw", length) ||
+		    !strncmp(name, "sw", length)) {
+			sw = true;
+			continue;
+		}
+
+		for (reg = axp_regulators; reg->dt_name; reg++) {
+			if (!strncmp(name, reg->dt_name, length)) {
+				setup_regulator(fdt, node, reg);
+				break;
+			}
+		}
+	}
+
+	/*
+	 * On the AXP803, if DLDO2 is enabled after DC1SW, the PMIC overheats
+	 * and shuts down. So always enable DC1SW as the very last regulator.
+	 */
+	if (sw) {
+		INFO("PMIC: Enabling DC SW\n");
+		if (axp_chip_id == AXP803_CHIP_ID)
+			axp_setbits(0x12, BIT(7));
+		if (axp_chip_id == AXP805_CHIP_ID)
+			axp_setbits(0x11, BIT(7));
+	}
+}
+#endif	/* SUNXI_SETUP_REGULATORS */
diff --git a/drivers/allwinner/sunxi_msgbox.c b/drivers/allwinner/sunxi_msgbox.c
new file mode 100644
index 0000000..cc4a6ff
--- /dev/null
+++ b/drivers/allwinner/sunxi_msgbox.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdbool.h>
+
+#include <drivers/delay_timer.h>
+#include <lib/bakery_lock.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+
+#include <sunxi_mmap.h>
+
+#define REMOTE_IRQ_EN_REG	0x0040
+#define REMOTE_IRQ_STAT_REG	0x0050
+#define LOCAL_IRQ_EN_REG	0x0060
+#define LOCAL_IRQ_STAT_REG	0x0070
+
+#define RX_IRQ(n)		BIT(0 + 2 * (n))
+#define TX_IRQ(n)		BIT(1 + 2 * (n))
+
+#define FIFO_STAT_REG(n)	(0x0100 + 0x4 * (n))
+#define FIFO_STAT_MASK		GENMASK(0, 0)
+
+#define MSG_STAT_REG(n)		(0x0140 + 0x4 * (n))
+#define MSG_STAT_MASK		GENMASK(2, 0)
+
+#define MSG_DATA_REG(n)		(0x0180 + 0x4 * (n))
+
+#define RX_CHAN			1
+#define TX_CHAN			0
+
+#define MHU_MAX_SLOT_ID		31
+
+#define MHU_TIMEOUT_DELAY	10
+#define MHU_TIMEOUT_ITERS	10000
+
+static DEFINE_BAKERY_LOCK(mhu_secure_message_lock);
+
+static bool sunxi_msgbox_last_tx_done(unsigned int chan)
+{
+	uint32_t stat = mmio_read_32(SUNXI_MSGBOX_BASE + REMOTE_IRQ_STAT_REG);
+
+	return (stat & RX_IRQ(chan)) == 0U;
+}
+
+static bool sunxi_msgbox_peek_data(unsigned int chan)
+{
+	uint32_t stat = mmio_read_32(SUNXI_MSGBOX_BASE + MSG_STAT_REG(chan));
+
+	return (stat & MSG_STAT_MASK) != 0U;
+}
+
+void mhu_secure_message_start(unsigned int slot_id __unused)
+{
+	uint32_t timeout = MHU_TIMEOUT_ITERS;
+
+	bakery_lock_get(&mhu_secure_message_lock);
+
+	/* Wait for all previous messages to be acknowledged. */
+	while (!sunxi_msgbox_last_tx_done(TX_CHAN) && --timeout)
+		udelay(MHU_TIMEOUT_DELAY);
+}
+
+void mhu_secure_message_send(unsigned int slot_id)
+{
+	mmio_write_32(SUNXI_MSGBOX_BASE + MSG_DATA_REG(TX_CHAN), BIT(slot_id));
+}
+
+uint32_t mhu_secure_message_wait(void)
+{
+	uint32_t timeout = MHU_TIMEOUT_ITERS;
+	uint32_t msg = 0;
+
+	/* Wait for a message from the SCP. */
+	while (!sunxi_msgbox_peek_data(RX_CHAN) && --timeout)
+		udelay(MHU_TIMEOUT_DELAY);
+
+	/* Return the most recent message in the FIFO. */
+	while (sunxi_msgbox_peek_data(RX_CHAN))
+		msg = mmio_read_32(SUNXI_MSGBOX_BASE + MSG_DATA_REG(RX_CHAN));
+
+	return msg;
+}
+
+void mhu_secure_message_end(unsigned int slot_id)
+{
+	/* Acknowledge a response by clearing the IRQ status. */
+	mmio_write_32(SUNXI_MSGBOX_BASE + LOCAL_IRQ_STAT_REG, RX_IRQ(RX_CHAN));
+
+	bakery_lock_release(&mhu_secure_message_lock);
+}
diff --git a/drivers/allwinner/sunxi_rsb.c b/drivers/allwinner/sunxi_rsb.c
new file mode 100644
index 0000000..67f5b7e
--- /dev/null
+++ b/drivers/allwinner/sunxi_rsb.c
@@ -0,0 +1,138 @@
+/*
+ * Copyright (c) 2017-2018 ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+
+#include <sunxi_mmap.h>
+
+#define RSB_CTRL	0x00
+#define RSB_CCR		0x04
+#define RSB_INTE	0x08
+#define RSB_STAT	0x0c
+#define RSB_DADDR0	0x10
+#define RSB_DLEN	0x18
+#define RSB_DATA0	0x1c
+#define RSB_LCR		0x24
+#define RSB_PMCR	0x28
+#define RSB_CMD		0x2c
+#define RSB_SADDR	0x30
+
+#define RSBCMD_SRTA	0xE8
+#define RSBCMD_RD8	0x8B
+#define RSBCMD_RD16	0x9C
+#define RSBCMD_RD32	0xA6
+#define RSBCMD_WR8	0x4E
+#define RSBCMD_WR16	0x59
+#define RSBCMD_WR32	0x63
+
+#define MAX_TRIES	100000
+
+static int rsb_wait_bit(const char *desc, unsigned int offset, uint32_t mask)
+{
+	uint32_t reg, tries = MAX_TRIES;
+
+	do
+		reg = mmio_read_32(SUNXI_R_RSB_BASE + offset);
+	while ((reg & mask) && --tries);	/* transaction in progress */
+	if (reg & mask) {
+		ERROR("%s: timed out\n", desc);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int rsb_wait_stat(const char *desc)
+{
+	uint32_t reg;
+	int ret = rsb_wait_bit(desc, RSB_CTRL, BIT(7));
+
+	if (ret)
+		return ret;
+
+	reg = mmio_read_32(SUNXI_R_RSB_BASE + RSB_STAT);
+	if (reg == 0x01)
+		return 0;
+
+	ERROR("%s: 0x%x\n", desc, reg);
+	return -reg;
+}
+
+/* Initialize the RSB controller. */
+int rsb_init_controller(void)
+{
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_CTRL, 0x01); /* soft reset */
+
+	return rsb_wait_bit("RSB: reset controller", RSB_CTRL, BIT(0));
+}
+
+int rsb_read(uint8_t rt_addr, uint8_t reg_addr)
+{
+	int ret;
+
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_CMD, RSBCMD_RD8); /* read a byte */
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_SADDR, rt_addr << 16);
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_DADDR0, reg_addr);
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_CTRL, 0x80);/* start transaction */
+
+	ret = rsb_wait_stat("RSB: read command");
+	if (ret)
+		return ret;
+
+	return mmio_read_32(SUNXI_R_RSB_BASE + RSB_DATA0) & 0xff; /* result */
+}
+
+int rsb_write(uint8_t rt_addr, uint8_t reg_addr, uint8_t value)
+{
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_CMD, RSBCMD_WR8);	/* byte write */
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_SADDR, rt_addr << 16);
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_DADDR0, reg_addr);
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_DATA0, value);
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_CTRL, 0x80);/* start transaction */
+
+	return rsb_wait_stat("RSB: write command");
+}
+
+int rsb_set_device_mode(uint32_t device_mode)
+{
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_PMCR,
+		      (device_mode & 0x00ffffff) | BIT(31));
+
+	return rsb_wait_bit("RSB: set device to RSB", RSB_PMCR, BIT(31));
+}
+
+int rsb_set_bus_speed(uint32_t source_freq, uint32_t bus_freq)
+{
+	uint32_t reg;
+
+	if (bus_freq == 0)
+		return -EINVAL;
+
+	reg = source_freq / bus_freq;
+	if (reg < 2)
+		return -EINVAL;
+
+	reg = reg / 2 - 1;
+	reg |= (1U << 8);		/* one cycle of CD output delay */
+
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_CCR, reg);
+
+	return 0;
+}
+
+/* Initialize the RSB PMIC connection. */
+int rsb_assign_runtime_address(uint16_t hw_addr, uint8_t rt_addr)
+{
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_SADDR, hw_addr | (rt_addr << 16));
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_CMD, RSBCMD_SRTA);
+	mmio_write_32(SUNXI_R_RSB_BASE + RSB_CTRL, 0x80);
+
+	return rsb_wait_stat("RSB: set run-time address");
+}
diff --git a/drivers/amlogic/console/aarch64/meson_console.S b/drivers/amlogic/console/aarch64/meson_console.S
new file mode 100644
index 0000000..6d0a2d6
--- /dev/null
+++ b/drivers/amlogic/console/aarch64/meson_console.S
@@ -0,0 +1,262 @@
+/*
+ * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include <assert_macros.S>
+#include <console_macros.S>
+#include <drivers/amlogic/meson_console.h>
+
+	.globl console_meson_register
+	.globl console_meson_init
+	.globl console_meson_putc
+	.globl console_meson_getc
+	.globl console_meson_flush
+	.globl console_meson_core_putc
+	.globl console_meson_core_getc
+	.globl console_meson_core_flush
+
+	/* -----------------------------------------------
+	 * Hardware definitions
+	 * -----------------------------------------------
+	 */
+#define MESON_WFIFO_OFFSET			0x0
+#define MESON_RFIFO_OFFSET			0x4
+#define MESON_CONTROL_OFFSET			0x8
+#define MESON_STATUS_OFFSET			0xC
+#define MESON_MISC_OFFSET			0x10
+#define MESON_REG5_OFFSET			0x14
+
+#define MESON_CONTROL_CLR_ERROR_BIT		24
+#define MESON_CONTROL_RX_RESET_BIT		23
+#define MESON_CONTROL_TX_RESET_BIT		22
+#define MESON_CONTROL_RX_ENABLE_BIT		13
+#define MESON_CONTROL_TX_ENABLE_BIT		12
+
+#define MESON_STATUS_RX_EMPTY_BIT		20
+#define MESON_STATUS_TX_FULL_BIT		21
+#define MESON_STATUS_TX_EMPTY_BIT		22
+
+#define MESON_REG5_USE_XTAL_CLK_BIT		24
+#define MESON_REG5_USE_NEW_RATE_BIT		23
+#define MESON_REG5_NEW_BAUD_RATE_MASK		0x7FFFFF
+
+	/* -----------------------------------------------
+	 * int console_meson_register(uintptr_t base,
+	 *     uint32_t clk, uint32_t baud,
+	 *     console_t *console);
+	 * Function to initialize and register a new MESON
+	 * 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_meson_register
+	mov	x7, x30
+	mov	x6, x3
+	cbz	x6, register_fail
+	str	x0, [x6, #CONSOLE_T_BASE]
+
+	bl	console_meson_init
+	cbz	x0, register_fail
+
+	mov	x0, x6
+	mov	x30, x7
+	finish_console_register meson putc=1, getc=1, flush=1
+
+register_fail:
+	ret	x7
+endfunc console_meson_register
+
+	/* -----------------------------------------------
+	 * int console_meson_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 : x0-x3
+	 * -----------------------------------------------
+	 */
+func console_meson_init
+	cmp	w0, #0
+	beq	init_fail
+	mov_imm	w3, 24000000 /* TODO: This only works with a 24 MHz clock. */
+	cmp	w1, w3
+	bne	init_fail
+	cmp	w2, #0
+	beq	init_fail
+	/* Set baud rate: value = ((clock / 3) / baudrate) - 1 */
+	mov	w3, #3
+	udiv	w3, w1, w3
+	udiv	w3, w3, w2
+	sub	w3, w3, #1
+	orr	w3, w3, #((1 << MESON_REG5_USE_XTAL_CLK_BIT) | \
+			  (1 << MESON_REG5_USE_NEW_RATE_BIT))
+	str	w3, [x0, #MESON_REG5_OFFSET]
+	/* Reset UART and clear error flag */
+	ldr	w3, [x0, #MESON_CONTROL_OFFSET]
+	orr	w3, w3, #((1 << MESON_CONTROL_CLR_ERROR_BIT) | \
+			  (1 << MESON_CONTROL_RX_RESET_BIT) | \
+			  (1 << MESON_CONTROL_TX_RESET_BIT))
+	str	w3, [x0, #MESON_CONTROL_OFFSET]
+	bic	w3, w3, #((1 << MESON_CONTROL_CLR_ERROR_BIT) | \
+			  (1 << MESON_CONTROL_RX_RESET_BIT) | \
+			  (1 << MESON_CONTROL_TX_RESET_BIT))
+	str	w3, [x0, #MESON_CONTROL_OFFSET]
+	/* Enable transfer and receive FIFO */
+	orr	w3, w3, #((1 << MESON_CONTROL_RX_ENABLE_BIT) | \
+			  (1 << MESON_CONTROL_TX_ENABLE_BIT))
+	str	w3, [x0, #MESON_CONTROL_OFFSET]
+	/* Success */
+	mov	w0, #1
+	ret
+init_fail:
+	mov	w0, wzr
+	ret
+endfunc console_meson_init
+
+	/* --------------------------------------------------------
+	 * int console_meson_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_meson_putc
+#if ENABLE_ASSERTIONS
+	cmp	x1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x1, [x1, #CONSOLE_T_BASE]
+	b	console_meson_core_putc
+endfunc console_meson_putc
+
+	/* --------------------------------------------------------
+	 * int console_meson_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_meson_core_putc
+#if ENABLE_ASSERTIONS
+	cmp	x1, #0
+	ASM_ASSERT(ne)
+#endif
+	/* Prepend '\r' to '\n' */
+	cmp	w0, #0xA
+	b.ne	2f
+	/* Wait until the transmit FIFO isn't full */
+1:	ldr	w2, [x1, #MESON_STATUS_OFFSET]
+	tbnz	w2, #MESON_STATUS_TX_FULL_BIT, 1b
+	/* Write '\r' if needed */
+	mov	w2, #0xD
+	str	w2, [x1, #MESON_WFIFO_OFFSET]
+	/* Wait until the transmit FIFO isn't full */
+2:	ldr	w2, [x1, #MESON_STATUS_OFFSET]
+	tbnz	w2, #MESON_STATUS_TX_FULL_BIT, 2b
+	/* Write input character */
+	str	w0, [x1, #MESON_WFIFO_OFFSET]
+	ret
+endfunc console_meson_core_putc
+
+	/* ---------------------------------------------
+	 * int console_meson_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_meson_getc
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x0, [x0, #CONSOLE_T_BASE]
+	b	console_meson_core_getc
+endfunc console_meson_getc
+
+	/* ---------------------------------------------
+	 * int console_meson_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_meson_core_getc
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif
+	/* Is the receive FIFO empty? */
+	ldr	w1, [x0, #MESON_STATUS_OFFSET]
+	tbnz	w1, #MESON_STATUS_RX_EMPTY_BIT, 1f
+	/* Read one character from the RX FIFO */
+	ldr	w0, [x0, #MESON_RFIFO_OFFSET]
+	ret
+1:
+	mov	w0, #ERROR_NO_PENDING_CHAR
+	ret
+endfunc console_meson_core_getc
+
+	/* ---------------------------------------------
+	 * void console_meson_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_meson_flush
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x0, [x0, #CONSOLE_T_BASE]
+	b	console_meson_core_flush
+endfunc console_meson_flush
+
+	/* ---------------------------------------------
+	 * void console_meson_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_meson_core_flush
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif
+	/* Wait until the transmit FIFO is empty */
+1:	ldr	w1, [x0, #MESON_STATUS_OFFSET]
+	tbz	w1, #MESON_STATUS_TX_EMPTY_BIT, 1b
+	ret
+endfunc console_meson_core_flush
diff --git a/drivers/amlogic/crypto/sha_dma.c b/drivers/amlogic/crypto/sha_dma.c
new file mode 100644
index 0000000..5c16d49
--- /dev/null
+++ b/drivers/amlogic/crypto/sha_dma.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright (c) 2019, Remi Pommarel <repk@triplefau.lt>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+#include <assert.h>
+#include <crypto/sha_dma.h>
+#include <lib/mmio.h>
+#include <platform_def.h>
+
+#include "aml_private.h"
+
+#define ASD_MODE_SHA224 0x7
+#define ASD_MODE_SHA256 0x6
+
+/* SHA DMA descriptor */
+struct asd_desc {
+	uint32_t cfg;
+	uint32_t src;
+	uint32_t dst;
+};
+#define ASD_DESC_GET(x, msk, off) (((x) >> (off)) & (msk))
+#define ASD_DESC_SET(x, v, msk, off)					\
+	((x) = ((x) & ~((msk) << (off))) | (((v) & (msk)) << (off)))
+
+#define ASD_DESC_LEN_OFF 0
+#define ASD_DESC_LEN_MASK 0x1ffff
+#define ASD_DESC_LEN(d)							\
+	(ASD_DESC_GET((d)->cfg, ASD_DESC_LEN_MASK, ASD_DESC_LEN_OFF))
+#define ASD_DESC_LEN_SET(d, v)						\
+	(ASD_DESC_SET((d)->cfg, v, ASD_DESC_LEN_MASK, ASD_DESC_LEN_OFF))
+
+#define ASD_DESC_IRQ_OFF 17
+#define ASD_DESC_IRQ_MASK 0x1
+#define ASD_DESC_IRQ(d)							\
+	(ASD_DESC_GET((d)->cfg, ASD_DESC_IRQ_MASK, ASD_DESC_IRQ_OFF))
+#define ASD_DESC_IRQ_SET(d, v)						\
+	(ASD_DESC_SET((d)->cfg, v, ASD_DESC_IRQ_MASK, ASD_DESC_IRQ_OFF))
+
+#define ASD_DESC_EOD_OFF 18
+#define ASD_DESC_EOD_MASK 0x1
+#define ASD_DESC_EOD(d)							\
+	(ASD_DESC_GET((d)->cfg, ASD_DESC_EOD_MASK, ASD_DESC_EOD_OFF))
+#define ASD_DESC_EOD_SET(d, v)						\
+	(ASD_DESC_SET((d)->cfg, v, ASD_DESC_EOD_MASK, ASD_DESC_EOD_OFF))
+
+#define ASD_DESC_LOOP_OFF 19
+#define ASD_DESC_LOOP_MASK 0x1
+#define ASD_DESC_LOOP(d)						\
+	(ASD_DESC_GET((d)->cfg, ASD_DESC_LOOP_MASK, ASD_DESC_LOOP_OFF))
+#define ASD_DESC_LOOP_SET(d, v)						\
+	(ASD_DESC_SET((d)->cfg, v, ASD_DESC_LOOP_MASK, ASD_DESC_LOOP_OFF))
+
+#define ASD_DESC_MODE_OFF 20
+#define ASD_DESC_MODE_MASK 0xf
+#define ASD_DESC_MODE(d)						\
+	(ASD_DESC_GET((d)->cfg, ASD_DESC_MODE_MASK, ASD_DESC_MODE_OFF))
+#define ASD_DESC_MODE_SET(d, v)						\
+	(ASD_DESC_SET((d)->cfg, v, ASD_DESC_MODE_MASK, ASD_DESC_MODE_OFF))
+
+#define ASD_DESC_BEGIN_OFF 24
+#define ASD_DESC_BEGIN_MASK 0x1
+#define ASD_DESC_BEGIN(d)						\
+	(ASD_DESC_GET((d)->cfg, ASD_DESC_BEGIN_MASK, ASD_DESC_BEGIN_OFF))
+#define ASD_DESC_BEGIN_SET(d, v)					\
+	(ASD_DESC_SET((d)->cfg, v, ASD_DESC_BEGIN_MASK, ASD_DESC_BEGIN_OFF))
+
+#define ASD_DESC_END_OFF 25
+#define ASD_DESC_END_MASK 0x1
+#define ASD_DESC_END(d)							\
+	(ASD_DESC_GET((d)->cfg, ASD_DESC_END_MASK, ASD_DESC_END_OFF))
+#define ASD_DESC_END_SET(d, v)						\
+	(ASD_DESC_SET((d)->cfg, v, ASD_DESC_END_MASK, ASD_DESC_END_OFF))
+
+#define ASD_DESC_OP_OFF 26
+#define ASD_DESC_OP_MASK 0x2
+#define ASD_DESC_OP(d)							\
+	(ASD_DESC_GET((d)->cfg, ASD_DESC_OP_MASK, ASD_DESC_OP_OFF))
+#define ASD_DESC_OP_SET(d, v)						\
+	(ASD_DESC_SET((d)->cfg, v, ASD_DESC_OP_MASK, ASD_DESC_OP_OFF))
+
+#define ASD_DESC_ENCONLY_OFF 28
+#define ASD_DESC_ENCONLY_MASK 0x1
+#define ASD_DESC_ENCONLY(d)						\
+	(ASD_DESC_GET((d)->cfg, ASD_DESC_ENCONLY_MASK, ASD_DESC_ENCONLY_OFF))
+#define ASD_DESC_ENCONLY_SET(d, v)					\
+	(ASD_DESC_SET((d)->cfg, v, ASD_DESC_ENCONLY_MASK, ASD_DESC_ENCONLY_OFF))
+
+#define ASD_DESC_BLOCK_OFF 29
+#define ASD_DESC_BLOCK_MASK 0x1
+#define ASD_DESC_BLOCK(d)						\
+	(ASD_DESC_GET((d)->cfg, ASD_DESC_BLOCK_MASK, ASD_DESC_BLOCK_OFF))
+#define ASD_DESC_BLOCK_SET(d, v)					\
+	(ASD_DESC_SET((d)->cfg, v, ASD_DESC_BLOCK_MASK, ASD_DESC_BLOCK_OFF))
+
+#define ASD_DESC_ERR_OFF 30
+#define ASD_DESC_ERR_MASK 0x1
+#define ASD_DESC_ERR(d)						\
+	(ASD_DESC_GET((d)->cfg, ASD_DESC_ERR_MASK, ASD_DESC_ERR_OFF))
+#define ASD_DESC_ERR_SET(d, v)					\
+	(ASD_DESC_SET((d)->cfg, v, ASD_DESC_ERR_MASK, ASD_DESC_ERR_OFF))
+
+#define ASD_DESC_OWNER_OFF 31u
+#define ASD_DESC_OWNER_MASK 0x1u
+#define ASD_DESC_OWNER(d)					\
+	(ASD_DESC_GET((d)->cfg, ASD_DESC_OWNER_MASK, ASD_DESC_OWNER_OFF))
+#define ASD_DESC_OWNER_SET(d, v)				\
+	(ASD_DESC_SET((d)->cfg, v, ASD_DESC_OWNER_MASK, ASD_DESC_OWNER_OFF))
+
+static void asd_compute_sha(struct asd_ctx *ctx, void *data, size_t len,
+		int finalize)
+{
+	/* Make it cache line size aligned ? */
+	struct asd_desc desc = {
+		.src = (uint32_t)(uintptr_t)data,
+		.dst = (uint32_t)(uintptr_t)ctx->digest,
+	};
+
+	/* Check data address is 32bit compatible */
+	assert((uintptr_t)data == (uintptr_t)desc.src);
+	assert((uintptr_t)ctx->digest == (uintptr_t)desc.dst);
+	assert((uintptr_t)&desc == (uintptr_t)&desc);
+
+	ASD_DESC_LEN_SET(&desc, len);
+	ASD_DESC_OWNER_SET(&desc, 1u);
+	ASD_DESC_ENCONLY_SET(&desc, 1);
+	ASD_DESC_EOD_SET(&desc, 1);
+	if (ctx->started == 0) {
+		ASD_DESC_BEGIN_SET(&desc, 1);
+		ctx->started = 1;
+	}
+	if (finalize) {
+		ASD_DESC_END_SET(&desc, 1);
+		ctx->started = 0;
+	}
+	if (ctx->mode == ASM_SHA224)
+		ASD_DESC_MODE_SET(&desc, ASD_MODE_SHA224);
+	else
+		ASD_DESC_MODE_SET(&desc, ASD_MODE_SHA256);
+
+	flush_dcache_range((uintptr_t)&desc, sizeof(desc));
+	flush_dcache_range((uintptr_t)data, len);
+
+	mmio_write_32(AML_SHA_DMA_STATUS, 0xf);
+	mmio_write_32(AML_SHA_DMA_DESC, ((uintptr_t)&desc) | 2);
+	while (mmio_read_32(AML_SHA_DMA_STATUS) == 0)
+		continue;
+	flush_dcache_range((uintptr_t)ctx->digest, SHA256_HASHSZ);
+}
+
+void asd_sha_update(struct asd_ctx *ctx, void *data, size_t len)
+{
+	size_t nr;
+
+	if (ctx->blocksz) {
+		nr = MIN(len, SHA256_BLOCKSZ - ctx->blocksz);
+		memcpy(ctx->block + ctx->blocksz, data, nr);
+		ctx->blocksz += nr;
+		len -= nr;
+		data += nr;
+	}
+
+	if (ctx->blocksz == SHA256_BLOCKSZ) {
+		asd_compute_sha(ctx, ctx->block, SHA256_BLOCKSZ, 0);
+		ctx->blocksz = 0;
+	}
+
+	asd_compute_sha(ctx, data, len & ~(SHA256_BLOCKSZ - 1), 0);
+	data += len & ~(SHA256_BLOCKSZ - 1);
+
+	if (len & (SHA256_BLOCKSZ - 1)) {
+		nr = len & (SHA256_BLOCKSZ - 1);
+		memcpy(ctx->block + ctx->blocksz, data, nr);
+		ctx->blocksz += nr;
+	}
+}
+
+void asd_sha_finalize(struct asd_ctx *ctx)
+{
+	asd_compute_sha(ctx, ctx->block, ctx->blocksz, 1);
+}
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");
+}
diff --git a/drivers/auth/auth_mod.c b/drivers/auth/auth_mod.c
new file mode 100644
index 0000000..fa9509a
--- /dev/null
+++ b/drivers/auth/auth_mod.c
@@ -0,0 +1,458 @@
+/*
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <common/tbbr/cot_def.h>
+#include <drivers/auth/auth_common.h>
+#include <drivers/auth/auth_mod.h>
+#include <drivers/auth/crypto_mod.h>
+#include <drivers/auth/img_parser_mod.h>
+#include <drivers/fwu/fwu.h>
+#include <lib/fconf/fconf_tbbr_getter.h>
+#include <plat/common/platform.h>
+
+/* ASN.1 tags */
+#define ASN1_INTEGER                 0x02
+
+#define return_if_error(rc) \
+	do { \
+		if (rc != 0) { \
+			return rc; \
+		} \
+	} while (0)
+
+#pragma weak plat_set_nv_ctr2
+#pragma weak plat_convert_pk
+
+
+static int cmp_auth_param_type_desc(const auth_param_type_desc_t *a,
+		const auth_param_type_desc_t *b)
+{
+	if ((a->type == b->type) && (a->cookie == b->cookie)) {
+		return 0;
+	}
+	return 1;
+}
+
+/*
+ * This function obtains the requested authentication parameter data from the
+ * information extracted from the parent image after its authentication.
+ */
+static int auth_get_param(const auth_param_type_desc_t *param_type_desc,
+			  const auth_img_desc_t *img_desc,
+			  void **param, unsigned int *len)
+{
+	int i;
+
+	if (img_desc->authenticated_data == NULL)
+		return 1;
+
+	for (i = 0 ; i < COT_MAX_VERIFIED_PARAMS ; i++) {
+		if (0 == cmp_auth_param_type_desc(param_type_desc,
+				img_desc->authenticated_data[i].type_desc)) {
+			*param = img_desc->authenticated_data[i].data.ptr;
+			*len = img_desc->authenticated_data[i].data.len;
+			return 0;
+		}
+	}
+
+	return 1;
+}
+
+/*
+ * Authenticate an image by matching the data hash
+ *
+ * This function implements 'AUTH_METHOD_HASH'. To authenticate an image using
+ * this method, the image must contain:
+ *
+ *   - The data to calculate the hash from
+ *
+ * The parent image must contain:
+ *
+ *   - The hash to be matched with (including hash algorithm)
+ *
+ * For a successful authentication, both hashes must match. The function calls
+ * the crypto-module to check this matching.
+ *
+ * Parameters:
+ *   param: parameters to perform the hash authentication
+ *   img_desc: pointer to image descriptor so we can know the image type
+ *             and parent image
+ *   img: pointer to image in memory
+ *   img_len: length of image (in bytes)
+ *
+ * Return:
+ *   0 = success, Otherwise = error
+ */
+static int auth_hash(const auth_method_param_hash_t *param,
+		     const auth_img_desc_t *img_desc,
+		     void *img, unsigned int img_len)
+{
+	void *data_ptr, *hash_der_ptr;
+	unsigned int data_len, hash_der_len;
+	int rc = 0;
+
+	/* Get the hash from the parent image. This hash will be DER encoded
+	 * and contain the hash algorithm */
+	rc = auth_get_param(param->hash, img_desc->parent,
+			&hash_der_ptr, &hash_der_len);
+	return_if_error(rc);
+
+	/* Get the data to be hashed from the current image */
+	rc = img_parser_get_auth_param(img_desc->img_type, param->data,
+			img, img_len, &data_ptr, &data_len);
+	return_if_error(rc);
+
+	/* Ask the crypto module to verify this hash */
+	rc = crypto_mod_verify_hash(data_ptr, data_len,
+				    hash_der_ptr, hash_der_len);
+
+	return rc;
+}
+
+/*
+ * Authenticate by digital signature
+ *
+ * This function implements 'AUTH_METHOD_SIG'. To authenticate an image using
+ * this method, the image must contain:
+ *
+ *   - Data to be signed
+ *   - Signature
+ *   - Signature algorithm
+ *
+ * We rely on the image parser module to extract this data from the image.
+ * The parent image must contain:
+ *
+ *   - Public key (or a hash of it)
+ *
+ * If the parent image contains only a hash of the key, we will try to obtain
+ * the public key from the image itself (i.e. self-signed certificates). In that
+ * case, the signature verification is considered just an integrity check and
+ * the authentication is established by calculating the hash of the key and
+ * comparing it with the hash obtained from the parent.
+ *
+ * If the image has no parent (NULL), it means it has to be authenticated using
+ * the ROTPK stored in the platform. Again, this ROTPK could be the key itself
+ * or a hash of it.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+static int auth_signature(const auth_method_param_sig_t *param,
+			  const auth_img_desc_t *img_desc,
+			  void *img, unsigned int img_len)
+{
+	void *data_ptr, *pk_ptr, *pk_hash_ptr, *sig_ptr, *sig_alg_ptr;
+	unsigned int data_len, pk_len, pk_hash_len, sig_len, sig_alg_len;
+	unsigned int flags = 0;
+	int rc = 0;
+
+	/* Get the data to be signed from current image */
+	rc = img_parser_get_auth_param(img_desc->img_type, param->data,
+			img, img_len, &data_ptr, &data_len);
+	return_if_error(rc);
+
+	/* Get the signature from current image */
+	rc = img_parser_get_auth_param(img_desc->img_type, param->sig,
+			img, img_len, &sig_ptr, &sig_len);
+	return_if_error(rc);
+
+	/* Get the signature algorithm from current image */
+	rc = img_parser_get_auth_param(img_desc->img_type, param->alg,
+			img, img_len, &sig_alg_ptr, &sig_alg_len);
+	return_if_error(rc);
+
+	/* Get the public key from the parent. If there is no parent (NULL),
+	 * the certificate has been signed with the ROTPK, so we have to get
+	 * the PK from the platform */
+	if (img_desc->parent) {
+		rc = auth_get_param(param->pk, img_desc->parent,
+				&pk_ptr, &pk_len);
+	} else {
+		rc = plat_get_rotpk_info(param->pk->cookie, &pk_ptr, &pk_len,
+				&flags);
+	}
+	return_if_error(rc);
+
+	if (flags & (ROTPK_IS_HASH | ROTPK_NOT_DEPLOYED)) {
+		/* If the PK is a hash of the key or if the ROTPK is not
+		   deployed on the platform, retrieve the key from the image */
+		pk_hash_ptr = pk_ptr;
+		pk_hash_len = pk_len;
+		rc = img_parser_get_auth_param(img_desc->img_type,
+					param->pk, img, img_len,
+					&pk_ptr, &pk_len);
+		return_if_error(rc);
+
+		/* Ask the crypto module to verify the signature */
+		rc = crypto_mod_verify_signature(data_ptr, data_len,
+						 sig_ptr, sig_len,
+						 sig_alg_ptr, sig_alg_len,
+						 pk_ptr, pk_len);
+		return_if_error(rc);
+
+		if (flags & ROTPK_NOT_DEPLOYED) {
+			NOTICE("ROTPK is not deployed on platform. "
+				"Skipping ROTPK verification.\n");
+		} else {
+			/* platform may store the hash of a prefixed, suffixed or modified pk */
+			rc = plat_convert_pk(pk_ptr, pk_len, &pk_ptr, &pk_len);
+			return_if_error(rc);
+
+			/* Ask the crypto-module to verify the key hash */
+			rc = crypto_mod_verify_hash(pk_ptr, pk_len,
+				    pk_hash_ptr, pk_hash_len);
+		}
+	} else {
+		/* Ask the crypto module to verify the signature */
+		rc = crypto_mod_verify_signature(data_ptr, data_len,
+						 sig_ptr, sig_len,
+						 sig_alg_ptr, sig_alg_len,
+						 pk_ptr, pk_len);
+	}
+
+	return rc;
+}
+
+/*
+ * Authenticate by Non-Volatile counter
+ *
+ * To protect the system against rollback, the platform includes a non-volatile
+ * counter whose value can only be increased. All certificates include a counter
+ * value that should not be lower than the value stored in the platform. If the
+ * value is larger, the counter in the platform must be updated to the new value
+ * (provided it has been authenticated).
+ *
+ * Return: 0 = success, Otherwise = error
+ * Returns additionally,
+ * cert_nv_ctr -> NV counter value present in the certificate
+ * need_nv_ctr_upgrade = 0 -> platform NV counter upgrade is not needed
+ * need_nv_ctr_upgrade = 1 -> platform NV counter upgrade is needed
+ */
+static int auth_nvctr(const auth_method_param_nv_ctr_t *param,
+		      const auth_img_desc_t *img_desc,
+		      void *img, unsigned int img_len,
+		      unsigned int *cert_nv_ctr,
+		      bool *need_nv_ctr_upgrade)
+{
+	char *p;
+	void *data_ptr = NULL;
+	unsigned int data_len, len, i;
+	unsigned int plat_nv_ctr;
+	int rc = 0;
+	bool is_trial_run = false;
+
+	/* Get the counter value from current image. The AM expects the IPM
+	 * to return the counter value as a DER encoded integer */
+	rc = img_parser_get_auth_param(img_desc->img_type, param->cert_nv_ctr,
+				       img, img_len, &data_ptr, &data_len);
+	return_if_error(rc);
+
+	/* Parse the DER encoded integer */
+	assert(data_ptr);
+	p = (char *)data_ptr;
+	if (*p != ASN1_INTEGER) {
+		/* Invalid ASN.1 integer */
+		return 1;
+	}
+	p++;
+
+	/* NV-counters are unsigned integers up to 32-bit */
+	len = (unsigned int)(*p & 0x7f);
+	if ((*p & 0x80) || (len > 4)) {
+		return 1;
+	}
+	p++;
+
+	/* Check the number is not negative */
+	if (*p & 0x80) {
+		return 1;
+	}
+
+	/* Convert to unsigned int. This code is for a little-endian CPU */
+	*cert_nv_ctr = 0;
+	for (i = 0; i < len; i++) {
+		*cert_nv_ctr = (*cert_nv_ctr << 8) | *p++;
+	}
+
+	/* Get the counter from the platform */
+	rc = plat_get_nv_ctr(param->plat_nv_ctr->cookie, &plat_nv_ctr);
+	return_if_error(rc);
+
+	if (*cert_nv_ctr < plat_nv_ctr) {
+		/* Invalid NV-counter */
+		return 1;
+	} else if (*cert_nv_ctr > plat_nv_ctr) {
+#if PSA_FWU_SUPPORT && IMAGE_BL2
+		is_trial_run = fwu_is_trial_run_state();
+#endif /* PSA_FWU_SUPPORT && IMAGE_BL2 */
+		*need_nv_ctr_upgrade = !is_trial_run;
+	}
+
+	return 0;
+}
+
+int plat_set_nv_ctr2(void *cookie, const auth_img_desc_t *img_desc __unused,
+		unsigned int nv_ctr)
+{
+	return plat_set_nv_ctr(cookie, nv_ctr);
+}
+
+int plat_convert_pk(void *full_pk_ptr, unsigned int full_pk_len,
+		    void **hashed_pk_ptr, unsigned int *hashed_pk_len)
+{
+	*hashed_pk_ptr = full_pk_ptr;
+	*hashed_pk_len = full_pk_len;
+
+	return 0;
+}
+
+/*
+ * Return the parent id in the output parameter '*parent_id'
+ *
+ * Return value:
+ *   0 = Image has parent, 1 = Image has no parent or parent is authenticated
+ */
+int auth_mod_get_parent_id(unsigned int img_id, unsigned int *parent_id)
+{
+	const auth_img_desc_t *img_desc = NULL;
+
+	assert(parent_id != NULL);
+	/* Get the image descriptor */
+	img_desc = FCONF_GET_PROPERTY(tbbr, cot, img_id);
+
+	/* Check if the image has no parent (ROT) */
+	if (img_desc->parent == NULL) {
+		*parent_id = 0;
+		return 1;
+	}
+
+	/* Check if the parent has already been authenticated */
+	if (auth_img_flags[img_desc->parent->img_id] & IMG_FLAG_AUTHENTICATED) {
+		*parent_id = 0;
+		return 1;
+	}
+
+	*parent_id = img_desc->parent->img_id;
+	return 0;
+}
+
+/*
+ * Initialize the different modules in the authentication framework
+ */
+void auth_mod_init(void)
+{
+	/* Check we have a valid CoT registered */
+	assert(cot_desc_ptr != NULL);
+
+	/* Image parser module */
+	img_parser_init();
+}
+
+/*
+ * Authenticate a certificate/image
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int auth_mod_verify_img(unsigned int img_id,
+			void *img_ptr,
+			unsigned int img_len)
+{
+	const auth_img_desc_t *img_desc = NULL;
+	const auth_method_desc_t *auth_method = NULL;
+	void *param_ptr;
+	unsigned int param_len;
+	int rc, i;
+	unsigned int cert_nv_ctr = 0;
+	bool need_nv_ctr_upgrade = false;
+	bool sig_auth_done = false;
+	const auth_method_param_nv_ctr_t *nv_ctr_param = NULL;
+
+	/* Get the image descriptor from the chain of trust */
+	img_desc = FCONF_GET_PROPERTY(tbbr, cot, img_id);
+
+	/* Ask the parser to check the image integrity */
+	rc = img_parser_check_integrity(img_desc->img_type, img_ptr, img_len);
+	return_if_error(rc);
+
+	/* Authenticate the image using the methods indicated in the image
+	 * descriptor. */
+	if (img_desc->img_auth_methods == NULL)
+		return 1;
+	for (i = 0 ; i < AUTH_METHOD_NUM ; i++) {
+		auth_method = &img_desc->img_auth_methods[i];
+		switch (auth_method->type) {
+		case AUTH_METHOD_NONE:
+			rc = 0;
+			break;
+		case AUTH_METHOD_HASH:
+			rc = auth_hash(&auth_method->param.hash,
+					img_desc, img_ptr, img_len);
+			break;
+		case AUTH_METHOD_SIG:
+			rc = auth_signature(&auth_method->param.sig,
+					img_desc, img_ptr, img_len);
+			sig_auth_done = true;
+			break;
+		case AUTH_METHOD_NV_CTR:
+			nv_ctr_param = &auth_method->param.nv_ctr;
+			rc = auth_nvctr(nv_ctr_param,
+					img_desc, img_ptr, img_len,
+					&cert_nv_ctr, &need_nv_ctr_upgrade);
+			break;
+		default:
+			/* Unknown authentication method */
+			rc = 1;
+			break;
+		}
+		return_if_error(rc);
+	}
+
+	/*
+	 * Do platform NV counter upgrade only if the certificate gets
+	 * authenticated, and platform NV-counter upgrade is needed.
+	 */
+	if (need_nv_ctr_upgrade && sig_auth_done) {
+		rc = plat_set_nv_ctr2(nv_ctr_param->plat_nv_ctr->cookie,
+				      img_desc, cert_nv_ctr);
+		return_if_error(rc);
+	}
+
+	/* Extract the parameters indicated in the image descriptor to
+	 * authenticate the children images. */
+	if (img_desc->authenticated_data != NULL) {
+		for (i = 0 ; i < COT_MAX_VERIFIED_PARAMS ; i++) {
+			if (img_desc->authenticated_data[i].type_desc == NULL) {
+				continue;
+			}
+
+			/* Get the parameter from the image parser module */
+			rc = img_parser_get_auth_param(img_desc->img_type,
+					img_desc->authenticated_data[i].type_desc,
+					img_ptr, img_len, &param_ptr, &param_len);
+			return_if_error(rc);
+
+			/* Check parameter size */
+			if (param_len > img_desc->authenticated_data[i].data.len) {
+				return 1;
+			}
+
+			/* Copy the parameter for later use */
+			memcpy((void *)img_desc->authenticated_data[i].data.ptr,
+					(void *)param_ptr, param_len);
+		}
+	}
+
+	/* Mark image as authenticated */
+	auth_img_flags[img_desc->img_id] |= IMG_FLAG_AUTHENTICATED;
+
+	return 0;
+}
diff --git a/drivers/auth/cca/cot.c b/drivers/auth/cca/cot.c
new file mode 100644
index 0000000..d3f3087
--- /dev/null
+++ b/drivers/auth/cca/cot.c
@@ -0,0 +1,675 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+
+#include <drivers/auth/auth_mod.h>
+#include MBEDTLS_CONFIG_FILE
+#include <tools_share/cca_oid.h>
+
+#include <platform_def.h>
+
+/*
+ * Allocate static buffers to store the authentication parameters extracted from
+ * the certificates.
+ */
+static unsigned char fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char tb_fw_hash_buf[HASH_DER_LEN];
+static unsigned char tb_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char hw_config_hash_buf[HASH_DER_LEN];
+static unsigned char soc_fw_hash_buf[HASH_DER_LEN];
+static unsigned char soc_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char rmm_hash_buf[HASH_DER_LEN];
+
+#ifdef IMAGE_BL2
+static unsigned char nt_world_bl_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char nt_fw_config_hash_buf[HASH_DER_LEN];
+#if defined(SPD_spmd)
+static unsigned char sp_pkg_hash_buf[MAX_SP_IDS][HASH_DER_LEN];
+#endif /* SPD_spmd */
+
+static unsigned char core_swd_pk_buf[PK_DER_LEN];
+static unsigned char plat_pk_buf[PK_DER_LEN];
+#endif /* IMAGE_BL2 */
+
+/*
+ * Parameter type descriptors.
+ */
+static auth_param_type_desc_t trusted_nv_ctr = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_NV_CTR, TRUSTED_FW_NVCOUNTER_OID);
+static auth_param_type_desc_t subject_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, 0);
+static auth_param_type_desc_t sig = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_SIG, 0);
+static auth_param_type_desc_t sig_alg = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_SIG_ALG, 0);
+static auth_param_type_desc_t raw_data = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_RAW_DATA, 0);
+
+static auth_param_type_desc_t tb_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_BOOT_FW_HASH_OID);
+static auth_param_type_desc_t tb_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_BOOT_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t hw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, HW_CONFIG_HASH_OID);
+static auth_param_type_desc_t fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t soc_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SOC_AP_FW_HASH_OID);
+static auth_param_type_desc_t soc_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SOC_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t rmm_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, RMM_HASH_OID);
+
+#ifdef IMAGE_BL2
+static auth_param_type_desc_t non_trusted_nv_ctr = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_NV_CTR, NON_TRUSTED_FW_NVCOUNTER_OID);
+
+static auth_param_type_desc_t prot_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, PROT_PK_OID);
+static auth_param_type_desc_t swd_rot_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, SWD_ROT_PK_OID);
+static auth_param_type_desc_t core_swd_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, CORE_SWD_PK_OID);
+static auth_param_type_desc_t plat_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, PLAT_PK_OID);
+
+static auth_param_type_desc_t tos_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_HASH_OID);
+static auth_param_type_desc_t tos_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t nt_world_bl_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, NON_TRUSTED_WORLD_BOOTLOADER_HASH_OID);
+static auth_param_type_desc_t nt_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, NON_TRUSTED_FW_CONFIG_HASH_OID);
+#if defined(SPD_spmd)
+static auth_param_type_desc_t sp_pkg1_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG1_HASH_OID);
+static auth_param_type_desc_t sp_pkg2_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG2_HASH_OID);
+static auth_param_type_desc_t sp_pkg3_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG3_HASH_OID);
+static auth_param_type_desc_t sp_pkg4_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG4_HASH_OID);
+static auth_param_type_desc_t sp_pkg5_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG5_HASH_OID);
+static auth_param_type_desc_t sp_pkg6_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG6_HASH_OID);
+static auth_param_type_desc_t sp_pkg7_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG7_HASH_OID);
+static auth_param_type_desc_t sp_pkg8_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG8_HASH_OID);
+#endif /* SPD_spmd */
+#endif /* IMAGE_BL2 */
+
+/* CCA Content Certificate */
+static const auth_img_desc_t cca_content_cert = {
+	.img_id = CCA_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &subject_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &tb_fw_hash,
+			.data = {
+				.ptr = (void *)tb_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &tb_fw_config_hash,
+			.data = {
+				.ptr = (void *)tb_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &fw_config_hash,
+			.data = {
+				.ptr = (void *)fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &hw_config_hash,
+			.data = {
+				.ptr = (void *)hw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[4] = {
+			.type_desc = &soc_fw_hash,
+			.data = {
+				.ptr = (void *)soc_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[5] = {
+			.type_desc = &soc_fw_config_hash,
+			.data = {
+				.ptr = (void *)soc_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[6] = {
+			.type_desc = &rmm_hash,
+			.data = {
+				.ptr = (void *)rmm_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+#ifdef IMAGE_BL1
+static const auth_img_desc_t bl2_image = {
+	.img_id = BL2_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tb_fw_hash
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t tb_fw_config = {
+	.img_id = TB_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tb_fw_config_hash
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t fw_config = {
+	.img_id = FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &fw_config_hash
+			}
+		}
+	}
+};
+#endif /* IMAGE_BL1 */
+
+#ifdef IMAGE_BL2
+/* HW Config */
+static const auth_img_desc_t hw_config = {
+	.img_id = HW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &hw_config_hash
+			}
+		}
+	}
+};
+
+/* BL31 */
+static const auth_img_desc_t bl31_image = {
+	.img_id = BL31_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &soc_fw_hash
+			}
+		}
+	}
+};
+
+/* BL31 Config */
+static const auth_img_desc_t soc_fw_config = {
+	.img_id = SOC_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &soc_fw_config_hash
+			}
+		}
+	}
+};
+
+/* RMM */
+static const auth_img_desc_t rmm_image = {
+	.img_id = RMM_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &rmm_hash
+			}
+		}
+	}
+};
+
+/* Core SWD Key Certificate */
+static const auth_img_desc_t core_swd_key_cert = {
+	.img_id = CORE_SWD_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL, /* SWD ROOT CERT */
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &swd_rot_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &core_swd_pk,
+			.data = {
+				.ptr = (void *)core_swd_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+
+/* SPMC Content Certificate */
+static const auth_img_desc_t trusted_os_fw_content_cert = {
+	.img_id = TRUSTED_OS_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &core_swd_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &core_swd_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &tos_fw_hash,
+			.data = {
+				.ptr = (void *)tos_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &tos_fw_config_hash,
+			.data = {
+				.ptr = (void *)tos_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+/* SPMC */
+static const auth_img_desc_t bl32_image = {
+	.img_id = BL32_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_hash
+			}
+		}
+	}
+};
+
+/* SPM Config */
+static const auth_img_desc_t tos_fw_config = {
+	.img_id = TOS_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_config_hash
+			}
+		}
+	}
+};
+
+/* Platform Key Certificate */
+static const auth_img_desc_t plat_key_cert = {
+	.img_id = PLAT_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL, /* PLATFORM ROOT CERT */
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &prot_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &plat_pk,
+			.data = {
+				.ptr = (void *)plat_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+
+/* Non-Trusted Firmware */
+static const auth_img_desc_t non_trusted_fw_content_cert = {
+	.img_id = NON_TRUSTED_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &plat_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &plat_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &nt_world_bl_hash,
+			.data = {
+				.ptr = (void *)nt_world_bl_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &nt_fw_config_hash,
+			.data = {
+				.ptr = (void *)nt_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t bl33_image = {
+	.img_id = BL33_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &non_trusted_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &nt_world_bl_hash
+			}
+		}
+	}
+};
+
+/* NT FW Config */
+static const auth_img_desc_t nt_fw_config = {
+	.img_id = NT_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &non_trusted_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &nt_fw_config_hash
+			}
+		}
+	}
+};
+
+/*
+ * Secure Partitions
+ */
+#if defined(SPD_spmd)
+static const auth_img_desc_t sip_sp_content_cert = {
+	.img_id = SIP_SP_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &core_swd_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &core_swd_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &sp_pkg1_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[0],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &sp_pkg2_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[1],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &sp_pkg3_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[2],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &sp_pkg4_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[3],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+DEFINE_SIP_SP_PKG(1);
+DEFINE_SIP_SP_PKG(2);
+DEFINE_SIP_SP_PKG(3);
+DEFINE_SIP_SP_PKG(4);
+
+static const auth_img_desc_t plat_sp_content_cert = {
+	.img_id = PLAT_SP_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &plat_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &plat_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &sp_pkg5_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[4],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &sp_pkg6_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[5],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &sp_pkg7_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[6],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &sp_pkg8_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[7],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+DEFINE_PLAT_SP_PKG(5);
+DEFINE_PLAT_SP_PKG(6);
+DEFINE_PLAT_SP_PKG(7);
+DEFINE_PLAT_SP_PKG(8);
+#endif /* SPD_spmd */
+#endif /* IMAGE_BL2 */
+/*
+ * Chain of trust definition
+ */
+#ifdef IMAGE_BL1
+static const auth_img_desc_t * const cot_desc[] = {
+	[CCA_CONTENT_CERT_ID]			=	&cca_content_cert,
+	[BL2_IMAGE_ID]				=	&bl2_image,
+	[TB_FW_CONFIG_ID]			=	&tb_fw_config,
+	[FW_CONFIG_ID]				=	&fw_config,
+};
+#else /* IMAGE_BL2 */
+static const auth_img_desc_t * const cot_desc[] = {
+	[CCA_CONTENT_CERT_ID]			=	&cca_content_cert,
+	[HW_CONFIG_ID]				=	&hw_config,
+	[BL31_IMAGE_ID]				=	&bl31_image,
+	[SOC_FW_CONFIG_ID]			=	&soc_fw_config,
+	[RMM_IMAGE_ID]				=	&rmm_image,
+	[CORE_SWD_KEY_CERT_ID]			=	&core_swd_key_cert,
+	[TRUSTED_OS_FW_CONTENT_CERT_ID]		=	&trusted_os_fw_content_cert,
+	[BL32_IMAGE_ID]				=	&bl32_image,
+	[TOS_FW_CONFIG_ID]			=	&tos_fw_config,
+	[PLAT_KEY_CERT_ID]			=	&plat_key_cert,
+	[NON_TRUSTED_FW_CONTENT_CERT_ID]	=	&non_trusted_fw_content_cert,
+	[BL33_IMAGE_ID]				=	&bl33_image,
+	[NT_FW_CONFIG_ID]			=	&nt_fw_config,
+#if defined(SPD_spmd)
+	[SIP_SP_CONTENT_CERT_ID]		=	&sip_sp_content_cert,
+	[PLAT_SP_CONTENT_CERT_ID]		=	&plat_sp_content_cert,
+	[SP_PKG1_ID]				=	&sp_pkg1,
+	[SP_PKG2_ID]				=	&sp_pkg2,
+	[SP_PKG3_ID]				=	&sp_pkg3,
+	[SP_PKG4_ID]				=	&sp_pkg4,
+	[SP_PKG5_ID]				=	&sp_pkg5,
+	[SP_PKG6_ID]				=	&sp_pkg6,
+	[SP_PKG7_ID]				=	&sp_pkg7,
+	[SP_PKG8_ID]				=       &sp_pkg8,
+#endif
+};
+#endif /* IMAGE_BL1 */
+
+/* Register the CoT in the authentication module */
+REGISTER_COT(cot_desc);
diff --git a/drivers/auth/crypto_mod.c b/drivers/auth/crypto_mod.c
new file mode 100644
index 0000000..fa1adb4
--- /dev/null
+++ b/drivers/auth/crypto_mod.c
@@ -0,0 +1,175 @@
+/*
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <drivers/auth/crypto_mod.h>
+
+/* Variable exported by the crypto library through REGISTER_CRYPTO_LIB() */
+
+/*
+ * The crypto module is responsible for verifying digital signatures and hashes.
+ * It relies on a crypto library to perform the cryptographic operations.
+ *
+ * The crypto module itself does not impose any specific format on signatures,
+ * signature algorithm, keys or hashes, but most cryptographic libraries will
+ * take the parameters as the following DER encoded ASN.1 structures:
+ *
+ *     AlgorithmIdentifier ::= SEQUENCE  {
+ *         algorithm        OBJECT IDENTIFIER,
+ *         parameters       ANY DEFINED BY algorithm OPTIONAL
+ *     }
+ *
+ *     DigestInfo ::= SEQUENCE {
+ *         digestAlgorithm  AlgorithmIdentifier,
+ *         digest           OCTET STRING
+ *     }
+ *
+ *     SubjectPublicKeyInfo ::= SEQUENCE  {
+ *         algorithm        AlgorithmIdentifier,
+ *         subjectPublicKey BIT STRING
+ *     }
+ *
+ *     SignatureAlgorithm ::= AlgorithmIdentifier
+ *
+ *     SignatureValue ::= BIT STRING
+ */
+
+/*
+ * Perform some static checking and call the library initialization function
+ */
+void crypto_mod_init(void)
+{
+	assert(crypto_lib_desc.name != NULL);
+	assert(crypto_lib_desc.init != NULL);
+#if CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_ONLY || \
+CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC
+	assert(crypto_lib_desc.verify_signature != NULL);
+	assert(crypto_lib_desc.verify_hash != NULL);
+#endif /* CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_ONLY || \
+	  CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC */
+
+#if CRYPTO_SUPPORT == CRYPTO_HASH_CALC_ONLY || \
+CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC
+	assert(crypto_lib_desc.calc_hash != NULL);
+#endif /* CRYPTO_SUPPORT == CRYPTO_HASH_CALC_ONLY || \
+	  CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC */
+
+	/* Initialize the cryptographic library */
+	crypto_lib_desc.init();
+	INFO("Using crypto library '%s'\n", crypto_lib_desc.name);
+}
+
+#if CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_ONLY || \
+CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC
+/*
+ * Function to verify a digital signature
+ *
+ * Parameters:
+ *
+ *   data_ptr, data_len: signed data
+ *   sig_ptr, sig_len: the digital signature
+ *   sig_alg_ptr, sig_alg_len: the digital signature algorithm
+ *   pk_ptr, pk_len: the public key
+ */
+int crypto_mod_verify_signature(void *data_ptr, unsigned int data_len,
+				void *sig_ptr, unsigned int sig_len,
+				void *sig_alg_ptr, unsigned int sig_alg_len,
+				void *pk_ptr, unsigned int pk_len)
+{
+	assert(data_ptr != NULL);
+	assert(data_len != 0);
+	assert(sig_ptr != NULL);
+	assert(sig_len != 0);
+	assert(sig_alg_ptr != NULL);
+	assert(sig_alg_len != 0);
+	assert(pk_ptr != NULL);
+	assert(pk_len != 0);
+
+	return crypto_lib_desc.verify_signature(data_ptr, data_len,
+						sig_ptr, sig_len,
+						sig_alg_ptr, sig_alg_len,
+						pk_ptr, pk_len);
+}
+
+/*
+ * Verify a hash by comparison
+ *
+ * Parameters:
+ *
+ *   data_ptr, data_len: data to be hashed
+ *   digest_info_ptr, digest_info_len: hash to be compared
+ */
+int crypto_mod_verify_hash(void *data_ptr, unsigned int data_len,
+			   void *digest_info_ptr, unsigned int digest_info_len)
+{
+	assert(data_ptr != NULL);
+	assert(data_len != 0);
+	assert(digest_info_ptr != NULL);
+	assert(digest_info_len != 0);
+
+	return crypto_lib_desc.verify_hash(data_ptr, data_len,
+					   digest_info_ptr, digest_info_len);
+}
+#endif /* CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_ONLY || \
+	  CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC */
+
+#if CRYPTO_SUPPORT == CRYPTO_HASH_CALC_ONLY || \
+CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC
+/*
+ * Calculate a hash
+ *
+ * Parameters:
+ *
+ *   alg: message digest algorithm
+ *   data_ptr, data_len: data to be hashed
+ *   output: resulting hash
+ */
+int crypto_mod_calc_hash(enum crypto_md_algo alg, void *data_ptr,
+			 unsigned int data_len,
+			 unsigned char output[CRYPTO_MD_MAX_SIZE])
+{
+	assert(data_ptr != NULL);
+	assert(data_len != 0);
+	assert(output != NULL);
+
+	return crypto_lib_desc.calc_hash(alg, data_ptr, data_len, output);
+}
+#endif /* CRYPTO_SUPPORT == CRYPTO_HASH_CALC_ONLY || \
+	  CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC */
+
+/*
+ * Authenticated decryption of data
+ *
+ * Parameters:
+ *
+ *   dec_algo: authenticated decryption algorithm
+ *   data_ptr, len: data to be decrypted (inout param)
+ *   key, key_len, key_flags: symmetric decryption key
+ *   iv, iv_len: initialization vector
+ *   tag, tag_len: authentication tag
+ */
+int crypto_mod_auth_decrypt(enum crypto_dec_algo dec_algo, void *data_ptr,
+			    size_t len, const void *key, unsigned int key_len,
+			    unsigned int key_flags, const void *iv,
+			    unsigned int iv_len, const void *tag,
+			    unsigned int tag_len)
+{
+	assert(crypto_lib_desc.auth_decrypt != NULL);
+	assert(data_ptr != NULL);
+	assert(len != 0U);
+	assert(key != NULL);
+	assert(key_len != 0U);
+	assert(iv != NULL);
+	assert((iv_len != 0U) && (iv_len <= CRYPTO_MAX_IV_SIZE));
+	assert(tag != NULL);
+	assert((tag_len != 0U) && (tag_len <= CRYPTO_MAX_TAG_SIZE));
+
+	return crypto_lib_desc.auth_decrypt(dec_algo, data_ptr, len, key,
+					    key_len, key_flags, iv, iv_len, tag,
+					    tag_len);
+}
diff --git a/drivers/auth/cryptocell/712/cryptocell_crypto.c b/drivers/auth/cryptocell/712/cryptocell_crypto.c
new file mode 100644
index 0000000..c7ee36f
--- /dev/null
+++ b/drivers/auth/cryptocell/712/cryptocell_crypto.c
@@ -0,0 +1,306 @@
+/*
+ * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/cryptocell/712/crypto_driver.h>
+#include <drivers/arm/cryptocell/712/rsa.h>
+#include <drivers/arm/cryptocell/712/sbrom_bsv_api.h>
+#include <drivers/arm/cryptocell/712/secureboot_base_func.h>
+#include <drivers/arm/cryptocell/712/secureboot_gen_defs.h>
+#include <drivers/arm/cryptocell/712/util.h>
+#include <drivers/auth/crypto_mod.h>
+#include <drivers/auth/mbedtls/mbedtls_common.h>
+#include <lib/utils.h>
+
+#include <mbedtls/oid.h>
+#include <mbedtls/x509.h>
+
+#define LIB_NAME		"CryptoCell 712 SBROM"
+#define RSA_SALT_LEN		32
+#define RSA_EXPONENT		65537
+
+/*
+ * AlgorithmIdentifier  ::=  SEQUENCE  {
+ *     algorithm            OBJECT IDENTIFIER,
+ *     parameters           ANY DEFINED BY algorithm OPTIONAL
+ * }
+ *
+ * SubjectPublicKeyInfo  ::=  SEQUENCE  {
+ *     algorithm            AlgorithmIdentifier,
+ *     subjectPublicKey     BIT STRING
+ * }
+ *
+ * DigestInfo ::= SEQUENCE {
+ *     digestAlgorithm      AlgorithmIdentifier,
+ *     digest               OCTET STRING
+ * }
+ *
+ *  RSASSA-PSS-params ::= SEQUENCE {
+ *     hashAlgorithm        [0] HashAlgorithm,
+ *     maskGenAlgorithm     [1] MaskGenAlgorithm,
+ *     saltLength           [2] INTEGER,
+ *     trailerField         [3] TrailerField    DEFAULT trailerFieldBC
+ * }
+ */
+
+/*
+ * Initialize the library and export the descriptor
+ */
+static void init(void)
+{
+	CCError_t ret;
+	uint32_t lcs;
+
+	/* Initialize CC SBROM */
+	ret = CC_BsvSbromInit((uintptr_t)PLAT_CRYPTOCELL_BASE);
+	if (ret != CC_OK) {
+		ERROR("CryptoCell CC_BsvSbromInit() error %x\n", ret);
+		panic();
+	}
+
+	/* Initialize lifecycle state */
+	ret = CC_BsvLcsGetAndInit((uintptr_t)PLAT_CRYPTOCELL_BASE, &lcs);
+	if (ret != CC_OK) {
+		ERROR("CryptoCell CC_BsvLcsGetAndInit() error %x\n", ret);
+		panic();
+	}
+
+	/* If the lifecyclestate is `SD`, then stop further execution */
+	if (lcs == CC_BSV_SECURITY_DISABLED_LCS) {
+		ERROR("CryptoCell LCS is security-disabled\n");
+		panic();
+	}
+}
+
+/*
+ * Verify a signature.
+ *
+ * Parameters are passed using the DER encoding format following the ASN.1
+ * structures detailed above.
+ */
+static int verify_signature(void *data_ptr, unsigned int data_len,
+			    void *sig_ptr, unsigned int sig_len,
+			    void *sig_alg, unsigned int sig_alg_len,
+			    void *pk_ptr, unsigned int pk_len)
+{
+	CCError_t error;
+	CCSbNParams_t pk;
+	CCSbSignature_t signature;
+	int rc, exp;
+	mbedtls_asn1_buf sig_oid, alg_oid, params;
+	mbedtls_md_type_t md_alg;
+	mbedtls_pk_type_t pk_alg;
+	mbedtls_pk_rsassa_pss_options pss_opts;
+	size_t len;
+	uint8_t *p, *end;
+	/* Temp buf to store the public key modulo (N) in LE format */
+	uint32_t RevN[SB_RSA_MOD_SIZE_IN_WORDS];
+
+	/* Verify the signature algorithm */
+	/* Get pointers to signature OID and parameters */
+	p = sig_alg;
+	end = p + sig_alg_len;
+	rc = mbedtls_asn1_get_alg(&p, end, &sig_oid, &params);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* Get the actual signature algorithm (MD + PK) */
+	rc = mbedtls_oid_get_sig_alg(&sig_oid, &md_alg, &pk_alg);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* The CryptoCell only supports RSASSA-PSS signature */
+	if (pk_alg != MBEDTLS_PK_RSASSA_PSS || md_alg != MBEDTLS_MD_NONE)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* Verify the RSASSA-PSS params */
+	/* The trailer field is verified to be 0xBC internally by this API */
+	rc = mbedtls_x509_get_rsassa_pss_params(&params, &md_alg,
+			&pss_opts.mgf1_hash_id,
+			&pss_opts.expected_salt_len);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* The CryptoCell only supports SHA256 as hash algorithm */
+	if (md_alg != MBEDTLS_MD_SHA256 || pss_opts.mgf1_hash_id != MBEDTLS_MD_SHA256)
+		return CRYPTO_ERR_SIGNATURE;
+
+	if (pss_opts.expected_salt_len != RSA_SALT_LEN)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* Parse the public key */
+	p = pk_ptr;
+	end = p + pk_len;
+	rc = mbedtls_asn1_get_tag(&p, end, &len,
+			MBEDTLS_ASN1_CONSTRUCTED | MBEDTLS_ASN1_SEQUENCE);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	end = p + len;
+	rc = mbedtls_asn1_get_alg_null(&p, end, &alg_oid);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	if (mbedtls_oid_get_pk_alg(&alg_oid, &pk_alg) != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	if (pk_alg != MBEDTLS_PK_RSA)
+		return CRYPTO_ERR_SIGNATURE;
+
+	rc = mbedtls_asn1_get_bitstring_null(&p, end, &len);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	rc = mbedtls_asn1_get_tag(&p, end, &len,
+				MBEDTLS_ASN1_CONSTRUCTED | MBEDTLS_ASN1_SEQUENCE);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_INTEGER);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	if (*p == 0) {
+		p++; len--;
+	}
+	if (len != RSA_MOD_SIZE_IN_BYTES || ((p + len) > end))
+		return CRYPTO_ERR_SIGNATURE;
+
+	/*
+	 * The CCSbVerifySignature() API expects N and Np in BE format and
+	 * the signature in LE format. Copy N from certificate.
+	 */
+	memcpy(pk.N, p, RSA_MOD_SIZE_IN_BYTES);
+
+	/* Verify the RSA exponent */
+	p += len;
+	rc = mbedtls_asn1_get_int(&p, end, &exp);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	if (exp != RSA_EXPONENT)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/*
+	 * Calculate the Np (Barrett n' value). The RSA_CalcNp() API expects
+	 * N in LE format. Hence reverse N into a temporary buffer `RevN`.
+	 */
+	UTIL_ReverseMemCopy((uint8_t *)RevN, (uint8_t *)pk.N, sizeof(RevN));
+
+	RSA_CalcNp((uintptr_t)PLAT_CRYPTOCELL_BASE, RevN, pk.Np);
+
+	/* Np is in LE format. Reverse it to BE */
+	UTIL_ReverseBuff((uint8_t *)pk.Np, sizeof(pk.Np));
+
+	/* Get the signature (bitstring) */
+	p = sig_ptr;
+	end = p + sig_len;
+	rc = mbedtls_asn1_get_bitstring_null(&p, end, &len);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	if (len != RSA_MOD_SIZE_IN_BYTES || ((p + len) > end))
+		return CRYPTO_ERR_SIGNATURE;
+
+	/*
+	 *  The signature is BE format. Convert it to LE before calling
+	 *  CCSbVerifySignature().
+	 */
+	UTIL_ReverseMemCopy((uint8_t *)signature.sig, p, RSA_MOD_SIZE_IN_BYTES);
+
+	/*
+	 * CryptoCell utilises DMA internally to transfer data. Flush the data
+	 * from caches.
+	 */
+	flush_dcache_range((uintptr_t)data_ptr, data_len);
+
+	/* Verify the signature */
+	error = CCSbVerifySignature((uintptr_t)PLAT_CRYPTOCELL_BASE,
+			(uint32_t *)data_ptr, &pk, &signature,
+			data_len, RSA_PSS);
+	if (error != CC_OK)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* Signature verification success */
+	return CRYPTO_SUCCESS;
+}
+
+/*
+ * Match a hash
+ *
+ * Digest info is passed in DER format following the ASN.1 structure detailed
+ * above.
+ */
+static int verify_hash(void *data_ptr, unsigned int data_len,
+		       void *digest_info_ptr, unsigned int digest_info_len)
+{
+	mbedtls_asn1_buf hash_oid, params;
+	mbedtls_md_type_t md_alg;
+	uint8_t *p, *end, *hash;
+	CCHashResult_t pubKeyHash;
+	size_t len;
+	int rc;
+	CCError_t error;
+
+	/* Digest info should be an MBEDTLS_ASN1_SEQUENCE */
+	p = digest_info_ptr;
+	end = p + digest_info_len;
+	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				  MBEDTLS_ASN1_SEQUENCE);
+	if (rc != 0)
+		return CRYPTO_ERR_HASH;
+
+	/* Get the hash algorithm */
+	rc = mbedtls_asn1_get_alg(&p, end, &hash_oid, &params);
+	if (rc != 0)
+		return CRYPTO_ERR_HASH;
+
+	rc = mbedtls_oid_get_md_alg(&hash_oid, &md_alg);
+	if (rc != 0)
+		return CRYPTO_ERR_HASH;
+	/* Verify that hash algorithm is SHA256 */
+	if (md_alg != MBEDTLS_MD_SHA256)
+		return CRYPTO_ERR_HASH;
+
+	/* Hash should be octet string type */
+	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_OCTET_STRING);
+	if (rc != 0)
+		return CRYPTO_ERR_HASH;
+
+	/* Length of hash must match the algorithm's size */
+	if (len != HASH_RESULT_SIZE_IN_BYTES)
+		return CRYPTO_ERR_HASH;
+
+	/*
+	 * CryptoCell utilises DMA internally to transfer data. Flush the data
+	 * from caches.
+	 */
+	flush_dcache_range((uintptr_t)data_ptr, data_len);
+
+	hash = p;
+	error = SBROM_CryptoHash((uintptr_t)PLAT_CRYPTOCELL_BASE,
+			(uintptr_t)data_ptr, data_len, pubKeyHash);
+	if (error != CC_OK)
+		return CRYPTO_ERR_HASH;
+
+	rc = memcmp(pubKeyHash, hash, HASH_RESULT_SIZE_IN_BYTES);
+	if (rc != 0)
+		return CRYPTO_ERR_HASH;
+
+	return CRYPTO_SUCCESS;
+}
+
+/*
+ * Register crypto library descriptor
+ */
+REGISTER_CRYPTO_LIB(LIB_NAME, init, verify_signature, verify_hash, NULL);
+
diff --git a/drivers/auth/cryptocell/712/cryptocell_plat_helpers.c b/drivers/auth/cryptocell/712/cryptocell_plat_helpers.c
new file mode 100644
index 0000000..53d77db
--- /dev/null
+++ b/drivers/auth/cryptocell/712/cryptocell_plat_helpers.c
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <plat/common/platform.h>
+#include <tools_share/tbbr_oid.h>
+
+#include <common/debug.h>
+#include <drivers/arm/cryptocell/712/sbrom_bsv_api.h>
+#include <drivers/arm/cryptocell/712/nvm.h>
+#include <drivers/arm/cryptocell/712/nvm_otp.h>
+
+/*
+ * Return the ROTPK hash
+ *
+ * dst:   buffer into which the ROTPK hash will be copied into
+ * len:   length of the provided buffer, which must be at least enough for a
+ *        SHA256 hash
+ * flags: a pointer to integer that will be set to indicate the ROTPK status
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int cc_get_rotpk_hash(unsigned char *dst, unsigned int len, unsigned int *flags)
+{
+	CCError_t error;
+	uint32_t lcs;
+
+	assert(dst != NULL);
+	assert(len >= HASH_RESULT_SIZE_IN_WORDS);
+	assert(flags != NULL);
+
+	error = NVM_GetLCS(PLAT_CRYPTOCELL_BASE, &lcs);
+	if (error != CC_OK)
+		return 1;
+
+	/* If the lifecycle state is `SD`, return failure */
+	if (lcs == CC_BSV_SECURITY_DISABLED_LCS)
+		return 1;
+
+	/*
+	 * If the lifecycle state is `CM` or `DM`, ROTPK shouldn't be verified.
+	 * Return success after setting ROTPK_NOT_DEPLOYED flag
+	 */
+	if ((lcs == CC_BSV_CHIP_MANUFACTURE_LCS) ||
+			(lcs == CC_BSV_DEVICE_MANUFACTURE_LCS)) {
+		*flags = ROTPK_NOT_DEPLOYED;
+		return 0;
+	}
+
+	/* Copy the DER header */
+	error = NVM_ReadHASHPubKey(PLAT_CRYPTOCELL_BASE,
+			CC_SB_HASH_BOOT_KEY_256B,
+			(uint32_t *)dst, HASH_RESULT_SIZE_IN_WORDS);
+	if (error != CC_OK)
+		return 1;
+
+	*flags = ROTPK_IS_HASH;
+	return 0;
+}
+
+/*
+ * Return the non-volatile counter value stored in the platform. The cookie
+ * specifies the OID of the counter in the certificate.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int plat_get_nv_ctr(void *cookie, unsigned int *nv_ctr)
+{
+	CCError_t error = CC_FAIL;
+
+	if (strcmp(cookie, TRUSTED_FW_NVCOUNTER_OID) == 0) {
+		error = NVM_GetSwVersion(PLAT_CRYPTOCELL_BASE,
+				CC_SW_VERSION_COUNTER1, nv_ctr);
+	} else if (strcmp(cookie, NON_TRUSTED_FW_NVCOUNTER_OID) == 0) {
+		error = NVM_GetSwVersion(PLAT_CRYPTOCELL_BASE,
+				CC_SW_VERSION_COUNTER2, nv_ctr);
+	}
+
+	return (error != CC_OK);
+}
+
+/*
+ * Store a new non-volatile counter value in the counter specified by the OID
+ * in the cookie. This function is not expected to be called if the Lifecycle
+ * state is RMA as the values in the certificate are expected to always match
+ * the nvcounter values. But if called when the LCS is RMA, the underlying
+ * helper functions will return success but without updating the counter.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int plat_set_nv_ctr(void *cookie, unsigned int nv_ctr)
+{
+	CCError_t error = CC_FAIL;
+
+	if (strcmp(cookie, TRUSTED_FW_NVCOUNTER_OID) == 0) {
+		error = NVM_SetSwVersion(PLAT_CRYPTOCELL_BASE,
+				CC_SW_VERSION_COUNTER1, nv_ctr);
+	} else if (strcmp(cookie, NON_TRUSTED_FW_NVCOUNTER_OID) == 0) {
+		error = NVM_SetSwVersion(PLAT_CRYPTOCELL_BASE,
+				CC_SW_VERSION_COUNTER2, nv_ctr);
+	}
+
+	return (error != CC_OK);
+}
+
diff --git a/drivers/auth/cryptocell/713/cryptocell_crypto.c b/drivers/auth/cryptocell/713/cryptocell_crypto.c
new file mode 100644
index 0000000..3ac16af
--- /dev/null
+++ b/drivers/auth/cryptocell/713/cryptocell_crypto.c
@@ -0,0 +1,276 @@
+/*
+ * Copyright (c) 2017-2020 ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <drivers/arm/cryptocell/713/bsv_api.h>
+#include <drivers/arm/cryptocell/713/bsv_crypto_asym_api.h>
+#include <drivers/auth/crypto_mod.h>
+
+#include <mbedtls/oid.h>
+#include <mbedtls/x509.h>
+
+#define LIB_NAME		"CryptoCell 713 SBROM"
+#define RSA_SALT_LEN		32
+#define RSA_EXPONENT		65537
+
+/*
+ * AlgorithmIdentifier  ::=  SEQUENCE  {
+ *     algorithm            OBJECT IDENTIFIER,
+ *     parameters           ANY DEFINED BY algorithm OPTIONAL
+ * }
+ *
+ * SubjectPublicKeyInfo  ::=  SEQUENCE  {
+ *     algorithm            AlgorithmIdentifier,
+ *     subjectPublicKey     BIT STRING
+ * }
+ *
+ * DigestInfo ::= SEQUENCE {
+ *     digestAlgorithm      AlgorithmIdentifier,
+ *     digest               OCTET STRING
+ * }
+ *
+ *  RSASSA-PSS-params ::= SEQUENCE {
+ *     hashAlgorithm        [0] HashAlgorithm,
+ *     maskGenAlgorithm     [1] MaskGenAlgorithm,
+ *     saltLength           [2] INTEGER,
+ *     trailerField         [3] TrailerField    DEFAULT trailerFieldBC
+ * }
+ */
+
+/*
+ * Initialize the library and export the descriptor
+ */
+static void init(void)
+{
+	CCError_t ret;
+	uint32_t lcs;
+
+	/* Initialize CC SBROM */
+	ret = CC_BsvInit((uintptr_t)PLAT_CRYPTOCELL_BASE);
+	if (ret != CC_OK) {
+		ERROR("CryptoCell CC_BsvInit() error %x\n", ret);
+		panic();
+	}
+
+	/* Initialize lifecycle state */
+	ret = CC_BsvGetAndInitLcs((uintptr_t)PLAT_CRYPTOCELL_BASE, &lcs);
+	if (ret != CC_OK) {
+		ERROR("CryptoCell CC_BsvGetAndInitLcs() error %x\n", ret);
+		panic();
+	}
+}
+
+/*
+ * Verify a signature.
+ *
+ * Parameters are passed using the DER encoding format following the ASN.1
+ * structures detailed above.
+ */
+static int verify_signature(void *data_ptr, unsigned int data_len,
+			    void *sig_ptr, unsigned int sig_len,
+			    void *sig_alg, unsigned int sig_alg_len,
+			    void *pk_ptr, unsigned int pk_len)
+{
+	CCError_t error;
+	CCBsvNBuff_t NBuff;
+	CCBsvSignature_t signature;
+	int rc, exp;
+	mbedtls_asn1_buf sig_oid, alg_oid, params;
+	mbedtls_md_type_t md_alg;
+	mbedtls_pk_type_t pk_alg;
+	mbedtls_pk_rsassa_pss_options pss_opts;
+	size_t len;
+	uint8_t *p, *end;
+	CCHashResult_t digest;
+	CCBool_t is_verified;
+	/* This is a rather large array, we don't want it on stack */
+	static uint32_t workspace[BSV_RSA_WORKSPACE_MIN_SIZE];
+
+	/* Verify the signature algorithm */
+	/* Get pointers to signature OID and parameters */
+	p = sig_alg;
+	end = p + sig_alg_len;
+	rc = mbedtls_asn1_get_alg(&p, end, &sig_oid, &params);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* Get the actual signature algorithm (MD + PK) */
+	rc = mbedtls_oid_get_sig_alg(&sig_oid, &md_alg, &pk_alg);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* The CryptoCell only supports RSASSA-PSS signature */
+	if (pk_alg != MBEDTLS_PK_RSASSA_PSS || md_alg != MBEDTLS_MD_NONE)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* Verify the RSASSA-PSS params */
+	/* The trailer field is verified to be 0xBC internally by this API */
+	rc = mbedtls_x509_get_rsassa_pss_params(&params, &md_alg,
+			&pss_opts.mgf1_hash_id,
+			&pss_opts.expected_salt_len);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* The CryptoCell only supports SHA256 as hash algorithm */
+	if (md_alg != MBEDTLS_MD_SHA256 ||
+	    pss_opts.mgf1_hash_id != MBEDTLS_MD_SHA256)
+		return CRYPTO_ERR_SIGNATURE;
+
+	if (pss_opts.expected_salt_len != RSA_SALT_LEN)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* Parse the public key */
+	p = pk_ptr;
+	end = p + pk_len;
+	rc = mbedtls_asn1_get_tag(&p, end, &len,
+			MBEDTLS_ASN1_CONSTRUCTED | MBEDTLS_ASN1_SEQUENCE);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	end = p + len;
+	rc = mbedtls_asn1_get_alg_null(&p, end, &alg_oid);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	if (mbedtls_oid_get_pk_alg(&alg_oid, &pk_alg) != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	if (pk_alg != MBEDTLS_PK_RSA)
+		return CRYPTO_ERR_SIGNATURE;
+
+	rc = mbedtls_asn1_get_bitstring_null(&p, end, &len);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	rc = mbedtls_asn1_get_tag(&p, end, &len,
+				MBEDTLS_ASN1_CONSTRUCTED |
+				MBEDTLS_ASN1_SEQUENCE);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_INTEGER);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	if (*p == 0) {
+		p++; len--;
+	}
+	if (len != BSV_CERT_RSA_KEY_SIZE_IN_BYTES || ((p + len) > end))
+		return CRYPTO_ERR_SIGNATURE;
+
+	/*
+	 * Copy N from certificate.
+	 */
+	memcpy(NBuff, p, BSV_CERT_RSA_KEY_SIZE_IN_BYTES);
+
+	/* Verify the RSA exponent */
+	p += len;
+	rc = mbedtls_asn1_get_int(&p, end, &exp);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	if (exp != RSA_EXPONENT)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* Get the signature (bitstring) */
+	p = sig_ptr;
+	end = p + sig_len;
+	rc = mbedtls_asn1_get_bitstring_null(&p, end, &len);
+	if (rc != 0)
+		return CRYPTO_ERR_SIGNATURE;
+
+	if (len != BSV_CERT_RSA_KEY_SIZE_IN_BYTES || ((p + len) > end))
+		return CRYPTO_ERR_SIGNATURE;
+
+	/*
+	 * Copy the signature (in BE format)
+	 */
+	memcpy((uint8_t *)signature, p, BSV_CERT_RSA_KEY_SIZE_IN_BYTES);
+
+	error = CC_BsvSha256((uintptr_t)PLAT_CRYPTOCELL_BASE,
+			     data_ptr, data_len, digest);
+	if (error != CC_OK)
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* Verify the signature */
+	error = CC_BsvRsaPssVerify((uintptr_t)PLAT_CRYPTOCELL_BASE, NBuff,
+				    NULL, signature, digest, workspace,
+				    BSV_RSA_WORKSPACE_MIN_SIZE, &is_verified);
+	if ((error != CC_OK) || (is_verified != CC_TRUE))
+		return CRYPTO_ERR_SIGNATURE;
+
+	/* Signature verification success */
+	return CRYPTO_SUCCESS;
+}
+
+/*
+ * Match a hash
+ *
+ * Digest info is passed in DER format following the ASN.1 structure detailed
+ * above.
+ */
+static int verify_hash(void *data_ptr, unsigned int data_len,
+		       void *digest_info_ptr, unsigned int digest_info_len)
+{
+	mbedtls_asn1_buf hash_oid, params;
+	mbedtls_md_type_t md_alg;
+	uint8_t *p, *end, *hash;
+	CCHashResult_t pubKeyHash;
+	size_t len;
+	int rc;
+	CCError_t error;
+
+	/* Digest info should be an MBEDTLS_ASN1_SEQUENCE */
+	p = digest_info_ptr;
+	end = p + digest_info_len;
+	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				  MBEDTLS_ASN1_SEQUENCE);
+	if (rc != 0)
+		return CRYPTO_ERR_HASH;
+
+	/* Get the hash algorithm */
+	rc = mbedtls_asn1_get_alg(&p, end, &hash_oid, &params);
+	if (rc != 0)
+		return CRYPTO_ERR_HASH;
+
+	rc = mbedtls_oid_get_md_alg(&hash_oid, &md_alg);
+	if (rc != 0)
+		return CRYPTO_ERR_HASH;
+	/* Verify that hash algorithm is SHA256 */
+	if (md_alg != MBEDTLS_MD_SHA256)
+		return CRYPTO_ERR_HASH;
+
+	/* Hash should be octet string type */
+	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_OCTET_STRING);
+	if (rc != 0)
+		return CRYPTO_ERR_HASH;
+
+	/* Length of hash must match the algorithm's size */
+	if (len != HASH_RESULT_SIZE_IN_BYTES)
+		return CRYPTO_ERR_HASH;
+
+	hash = p;
+	error = CC_BsvSha256((uintptr_t)PLAT_CRYPTOCELL_BASE, data_ptr,
+			     data_len, pubKeyHash);
+	if (error != CC_OK)
+		return CRYPTO_ERR_HASH;
+
+	rc = memcmp(pubKeyHash, hash, HASH_RESULT_SIZE_IN_BYTES);
+	if (rc != 0)
+		return CRYPTO_ERR_HASH;
+
+	return CRYPTO_SUCCESS;
+}
+
+/*
+ * Register crypto library descriptor
+ */
+REGISTER_CRYPTO_LIB(LIB_NAME, init, verify_signature, verify_hash, NULL);
diff --git a/drivers/auth/cryptocell/713/cryptocell_plat_helpers.c b/drivers/auth/cryptocell/713/cryptocell_plat_helpers.c
new file mode 100644
index 0000000..17e1280
--- /dev/null
+++ b/drivers/auth/cryptocell/713/cryptocell_plat_helpers.c
@@ -0,0 +1,109 @@
+/*
+ * Copyright (c) 2017-2020 ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+#include <string.h>
+
+#include <plat/common/platform.h>
+#include <tools_share/tbbr_oid.h>
+
+#include <lib/libc/endian.h>
+#include <drivers/arm/cryptocell/713/bsv_api.h>
+#include <drivers/arm/cryptocell/713/bsv_error.h>
+
+/*
+ * Return the ROTPK hash
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int cc_get_rotpk_hash(unsigned char *dst, unsigned int len, unsigned int *flags)
+{
+	CCError_t error;
+	uint32_t lcs;
+	int i;
+	uint32_t *key = (uint32_t *)dst;
+
+	assert(dst != NULL);
+	assert(len >= HASH_RESULT_SIZE_IN_WORDS);
+	assert(flags != NULL);
+
+	error = CC_BsvLcsGet(PLAT_CRYPTOCELL_BASE, &lcs);
+	if (error != CC_OK)
+		return 1;
+
+	if ((lcs == CC_BSV_CHIP_MANUFACTURE_LCS) || (lcs == CC_BSV_RMA_LCS)) {
+		*flags = ROTPK_NOT_DEPLOYED;
+		return 0;
+	}
+
+	error = CC_BsvPubKeyHashGet(PLAT_CRYPTOCELL_BASE,
+				    CC_SB_HASH_BOOT_KEY_256B,
+				    key, HASH_RESULT_SIZE_IN_WORDS);
+
+	if (error == CC_BSV_HASH_NOT_PROGRAMMED_ERR) {
+		*flags = ROTPK_NOT_DEPLOYED;
+		return 0;
+	}
+
+	if (error == CC_OK) {
+
+		/* Keys are stored in OTP in little-endian format */
+		for (i = 0; i < HASH_RESULT_SIZE_IN_WORDS; i++)
+			key[i] = le32toh(key[i]);
+
+		*flags = ROTPK_IS_HASH;
+		return 0;
+	}
+
+	return 1;
+}
+
+/*
+ * Return the non-volatile counter value stored in the platform. The cookie
+ * specifies the OID of the counter in the certificate.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int plat_get_nv_ctr(void *cookie, unsigned int *nv_ctr)
+{
+	CCError_t error = CC_FAIL;
+
+	if (strcmp(cookie, TRUSTED_FW_NVCOUNTER_OID) == 0) {
+		error = CC_BsvSwVersionGet(PLAT_CRYPTOCELL_BASE,
+					   CC_SW_VERSION_TRUSTED, nv_ctr);
+	} else if (strcmp(cookie, NON_TRUSTED_FW_NVCOUNTER_OID) == 0) {
+		error = CC_BsvSwVersionGet(PLAT_CRYPTOCELL_BASE,
+					   CC_SW_VERSION_NON_TRUSTED, nv_ctr);
+	}
+
+	return (error != CC_OK);
+}
+
+/*
+ * Store a new non-volatile counter value in the counter specified by the OID
+ * in the cookie. This function is not expected to be called if the Lifecycle
+ * state is RMA as the values in the certificate are expected to always match
+ * the nvcounter values. But if called when the LCS is RMA, the underlying
+ * helper functions will return success but without updating the counter.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int plat_set_nv_ctr(void *cookie, unsigned int nv_ctr)
+{
+	CCError_t error = CC_FAIL;
+
+	if (strcmp(cookie, TRUSTED_FW_NVCOUNTER_OID) == 0) {
+		error = CC_BsvSwVersionSet(PLAT_CRYPTOCELL_BASE,
+					   CC_SW_VERSION_TRUSTED, nv_ctr);
+	} else if (strcmp(cookie, NON_TRUSTED_FW_NVCOUNTER_OID) == 0) {
+		error = CC_BsvSwVersionSet(PLAT_CRYPTOCELL_BASE,
+					   CC_SW_VERSION_NON_TRUSTED, nv_ctr);
+	}
+
+	return (error != CC_OK);
+}
+
diff --git a/drivers/auth/cryptocell/cryptocell_crypto.mk b/drivers/auth/cryptocell/cryptocell_crypto.mk
new file mode 100644
index 0000000..db39047
--- /dev/null
+++ b/drivers/auth/cryptocell/cryptocell_crypto.mk
@@ -0,0 +1,40 @@
+#
+# Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include drivers/auth/mbedtls/mbedtls_common.mk
+
+# The algorithm is RSA when using Cryptocell crypto driver
+TF_MBEDTLS_KEY_ALG_ID		:=	TF_MBEDTLS_RSA
+
+# Needs to be set to drive mbed TLS configuration correctly
+$(eval $(call add_define,TF_MBEDTLS_KEY_ALG_ID))
+
+$(eval $(call add_define,KEY_SIZE))
+
+# CCSBROM_LIB_PATH must be set to the Cryptocell SBROM library path
+ifeq (${CCSBROM_LIB_PATH},)
+  $(error Error: CCSBROM_LIB_PATH not set)
+endif
+
+CRYPTOCELL_VERSION ?= 712
+ifeq (${CRYPTOCELL_VERSION},712)
+  CCSBROM_LIB_FILENAME := cc_712sbromx509
+else ifeq (${CRYPTOCELL_VERSION},713)
+  CCSBROM_LIB_FILENAME	:= cc_713bsv
+else
+  $(error Error: CRYPTOCELL_VERSION set to invalid version)
+endif
+
+CRYPTOCELL_SRC_DIR	:= drivers/auth/cryptocell/${CRYPTOCELL_VERSION}/
+
+CRYPTOCELL_SOURCES	:= ${CRYPTOCELL_SRC_DIR}/cryptocell_crypto.c \
+			   ${CRYPTOCELL_SRC_DIR}/cryptocell_plat_helpers.c
+
+TF_LDFLAGS		+= -L$(CCSBROM_LIB_PATH)
+LDLIBS			+= -l$(CCSBROM_LIB_FILENAME)
+
+BL1_SOURCES		+= ${CRYPTOCELL_SOURCES}
+BL2_SOURCES		+= ${CRYPTOCELL_SOURCES}
diff --git a/drivers/auth/dualroot/cot.c b/drivers/auth/dualroot/cot.c
new file mode 100644
index 0000000..8368503
--- /dev/null
+++ b/drivers/auth/dualroot/cot.c
@@ -0,0 +1,959 @@
+/*
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+
+#include <platform_def.h>
+
+#include MBEDTLS_CONFIG_FILE
+#include <drivers/auth/auth_mod.h>
+#include <tools_share/dualroot_oid.h>
+
+/*
+ * Allocate static buffers to store the authentication parameters extracted from
+ * the certificates.
+ */
+static unsigned char fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char tb_fw_hash_buf[HASH_DER_LEN];
+static unsigned char tb_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char hw_config_hash_buf[HASH_DER_LEN];
+static unsigned char scp_fw_hash_buf[HASH_DER_LEN];
+static unsigned char nt_world_bl_hash_buf[HASH_DER_LEN];
+
+#ifdef IMAGE_BL2
+static unsigned char soc_fw_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_extra1_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_extra2_hash_buf[HASH_DER_LEN];
+static unsigned char soc_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char nt_fw_config_hash_buf[HASH_DER_LEN];
+#if defined(SPD_spmd)
+static unsigned char sp_pkg_hash_buf[MAX_SP_IDS][HASH_DER_LEN];
+#endif /* SPD_spmd */
+
+static unsigned char trusted_world_pk_buf[PK_DER_LEN];
+static unsigned char content_pk_buf[PK_DER_LEN];
+#endif
+
+/*
+ * Parameter type descriptors.
+ */
+static auth_param_type_desc_t trusted_nv_ctr = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_NV_CTR, TRUSTED_FW_NVCOUNTER_OID);
+static auth_param_type_desc_t subject_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, 0);
+static auth_param_type_desc_t sig = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_SIG, 0);
+static auth_param_type_desc_t sig_alg = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_SIG_ALG, 0);
+static auth_param_type_desc_t raw_data = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_RAW_DATA, 0);
+
+static auth_param_type_desc_t tb_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_BOOT_FW_HASH_OID);
+static auth_param_type_desc_t tb_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_BOOT_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t hw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, HW_CONFIG_HASH_OID);
+static auth_param_type_desc_t fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, FW_CONFIG_HASH_OID);
+#ifdef IMAGE_BL1
+static auth_param_type_desc_t scp_bl2u_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SCP_FWU_CFG_HASH_OID);
+static auth_param_type_desc_t bl2u_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, AP_FWU_CFG_HASH_OID);
+static auth_param_type_desc_t ns_bl2u_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, FWU_HASH_OID);
+#endif /* IMAGE_BL1 */
+
+#ifdef IMAGE_BL2
+static auth_param_type_desc_t non_trusted_nv_ctr = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_NV_CTR, NON_TRUSTED_FW_NVCOUNTER_OID);
+
+static auth_param_type_desc_t trusted_world_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, TRUSTED_WORLD_PK_OID);
+static auth_param_type_desc_t scp_fw_content_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, SCP_FW_CONTENT_CERT_PK_OID);
+static auth_param_type_desc_t soc_fw_content_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, SOC_FW_CONTENT_CERT_PK_OID);
+static auth_param_type_desc_t tos_fw_content_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, TRUSTED_OS_FW_CONTENT_CERT_PK_OID);
+static auth_param_type_desc_t prot_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, PROT_PK_OID);
+
+static auth_param_type_desc_t scp_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SCP_FW_HASH_OID);
+static auth_param_type_desc_t soc_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SOC_AP_FW_HASH_OID);
+static auth_param_type_desc_t soc_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SOC_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t tos_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_HASH_OID);
+static auth_param_type_desc_t tos_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t tos_fw_extra1_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_EXTRA1_HASH_OID);
+static auth_param_type_desc_t tos_fw_extra2_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_EXTRA2_HASH_OID);
+static auth_param_type_desc_t nt_world_bl_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, NON_TRUSTED_WORLD_BOOTLOADER_HASH_OID);
+static auth_param_type_desc_t nt_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, NON_TRUSTED_FW_CONFIG_HASH_OID);
+#if defined(SPD_spmd)
+static auth_param_type_desc_t sp_pkg1_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG1_HASH_OID);
+static auth_param_type_desc_t sp_pkg2_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG2_HASH_OID);
+static auth_param_type_desc_t sp_pkg3_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG3_HASH_OID);
+static auth_param_type_desc_t sp_pkg4_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG4_HASH_OID);
+static auth_param_type_desc_t sp_pkg5_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG5_HASH_OID);
+static auth_param_type_desc_t sp_pkg6_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG6_HASH_OID);
+static auth_param_type_desc_t sp_pkg7_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG7_HASH_OID);
+static auth_param_type_desc_t sp_pkg8_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG8_HASH_OID);
+#endif /* SPD_spmd */
+#endif /* IMAGE_BL2 */
+
+
+/* BL2 */
+static const auth_img_desc_t trusted_boot_fw_cert = {
+	.img_id = TRUSTED_BOOT_FW_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &subject_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &tb_fw_hash,
+			.data = {
+				.ptr = (void *)tb_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &tb_fw_config_hash,
+			.data = {
+				.ptr = (void *)tb_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &hw_config_hash,
+			.data = {
+				.ptr = (void *)hw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &fw_config_hash,
+			.data = {
+				.ptr = (void *)fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+#ifdef IMAGE_BL1
+static const auth_img_desc_t bl2_image = {
+	.img_id = BL2_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_boot_fw_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tb_fw_hash
+			}
+		}
+	}
+};
+#endif /* IMAGE_BL1 */
+
+/* HW Config */
+static const auth_img_desc_t hw_config = {
+	.img_id = HW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_boot_fw_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &hw_config_hash
+			}
+		}
+	}
+};
+
+/* TB FW Config */
+#ifdef IMAGE_BL1
+static const auth_img_desc_t tb_fw_config = {
+	.img_id = TB_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_boot_fw_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tb_fw_config_hash
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t fw_config = {
+	.img_id = FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_boot_fw_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &fw_config_hash
+			}
+		}
+	}
+};
+
+#endif /* IMAGE_BL1 */
+
+#ifdef IMAGE_BL2
+/* Trusted key certificate */
+static const auth_img_desc_t trusted_key_cert = {
+	.img_id = TRUSTED_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &subject_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &trusted_world_pk,
+			.data = {
+				.ptr = (void *)trusted_world_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		},
+	}
+};
+
+/* SCP Firmware */
+static const auth_img_desc_t scp_fw_key_cert = {
+	.img_id = SCP_FW_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &scp_fw_content_pk,
+			.data = {
+				.ptr = (void *)content_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t scp_fw_content_cert = {
+	.img_id = SCP_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &scp_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &scp_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &scp_fw_hash,
+			.data = {
+				.ptr = (void *)scp_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t scp_bl2_image = {
+	.img_id = SCP_BL2_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &scp_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &scp_fw_hash
+			}
+		}
+	}
+};
+
+/* SoC Firmware */
+static const auth_img_desc_t soc_fw_key_cert = {
+	.img_id = SOC_FW_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &soc_fw_content_pk,
+			.data = {
+				.ptr = (void *)content_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t soc_fw_content_cert = {
+	.img_id = SOC_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &soc_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &soc_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &soc_fw_hash,
+			.data = {
+				.ptr = (void *)soc_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &soc_fw_config_hash,
+			.data = {
+				.ptr = (void *)soc_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t bl31_image = {
+	.img_id = BL31_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &soc_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &soc_fw_hash
+			}
+		}
+	}
+};
+
+/* SOC FW Config */
+static const auth_img_desc_t soc_fw_config = {
+	.img_id = SOC_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &soc_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &soc_fw_config_hash
+			}
+		}
+	}
+};
+
+/* Trusted OS Firmware */
+static const auth_img_desc_t trusted_os_fw_key_cert = {
+	.img_id = TRUSTED_OS_FW_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &tos_fw_content_pk,
+			.data = {
+				.ptr = (void *)content_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t trusted_os_fw_content_cert = {
+	.img_id = TRUSTED_OS_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_os_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &tos_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &tos_fw_hash,
+			.data = {
+				.ptr = (void *)tos_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &tos_fw_extra1_hash,
+			.data = {
+				.ptr = (void *)tos_fw_extra1_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &tos_fw_extra2_hash,
+			.data = {
+				.ptr = (void *)tos_fw_extra2_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &tos_fw_config_hash,
+			.data = {
+				.ptr = (void *)tos_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t bl32_image = {
+	.img_id = BL32_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_hash
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t bl32_extra1_image = {
+	.img_id = BL32_EXTRA1_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_extra1_hash
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t bl32_extra2_image = {
+	.img_id = BL32_EXTRA2_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_extra2_hash
+			}
+		}
+	}
+};
+
+/* TOS FW Config */
+static const auth_img_desc_t tos_fw_config = {
+	.img_id = TOS_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_config_hash
+			}
+		}
+	}
+};
+
+/* Non-Trusted Firmware */
+static const auth_img_desc_t non_trusted_fw_content_cert = {
+	.img_id = NON_TRUSTED_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL, /* Root certificate.  */
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &prot_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &nt_world_bl_hash,
+			.data = {
+				.ptr = (void *)nt_world_bl_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &nt_fw_config_hash,
+			.data = {
+				.ptr = (void *)nt_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t bl33_image = {
+	.img_id = BL33_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &non_trusted_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &nt_world_bl_hash
+			}
+		}
+	}
+};
+
+/* NT FW Config */
+static const auth_img_desc_t nt_fw_config = {
+	.img_id = NT_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &non_trusted_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &nt_fw_config_hash
+			}
+		}
+	}
+};
+
+/*
+ * Secure Partitions
+ */
+#if defined(SPD_spmd)
+static const auth_img_desc_t sip_sp_content_cert = {
+	.img_id = SIP_SP_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &sp_pkg1_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[0],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &sp_pkg2_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[1],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &sp_pkg3_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[2],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &sp_pkg4_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[3],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+DEFINE_SIP_SP_PKG(1);
+DEFINE_SIP_SP_PKG(2);
+DEFINE_SIP_SP_PKG(3);
+DEFINE_SIP_SP_PKG(4);
+
+static const auth_img_desc_t plat_sp_content_cert = {
+	.img_id = PLAT_SP_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &prot_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &sp_pkg5_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[4],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &sp_pkg6_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[5],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &sp_pkg7_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[6],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &sp_pkg8_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[7],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+DEFINE_PLAT_SP_PKG(5);
+DEFINE_PLAT_SP_PKG(6);
+DEFINE_PLAT_SP_PKG(7);
+DEFINE_PLAT_SP_PKG(8);
+#endif /* SPD_spmd */
+
+#else  /* IMAGE_BL2 */
+
+/* FWU auth descriptor */
+static const auth_img_desc_t fwu_cert = {
+	.img_id = FWU_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &subject_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &scp_bl2u_hash,
+			.data = {
+				.ptr = (void *)scp_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &bl2u_hash,
+			.data = {
+				.ptr = (void *)tb_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &ns_bl2u_hash,
+			.data = {
+				.ptr = (void *)nt_world_bl_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+/* SCP_BL2U */
+static const auth_img_desc_t scp_bl2u_image = {
+	.img_id = SCP_BL2U_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &fwu_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &scp_bl2u_hash
+			}
+		}
+	}
+};
+
+/* BL2U */
+static const auth_img_desc_t bl2u_image = {
+	.img_id = BL2U_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &fwu_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &bl2u_hash
+			}
+		}
+	}
+};
+
+/* NS_BL2U */
+static const auth_img_desc_t ns_bl2u_image = {
+	.img_id = NS_BL2U_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &fwu_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &ns_bl2u_hash
+			}
+		}
+	}
+};
+#endif /* IMAGE_BL2 */
+
+/*
+ * Chain of trust definition
+ */
+#ifdef IMAGE_BL1
+static const auth_img_desc_t * const cot_desc[] = {
+	[TRUSTED_BOOT_FW_CERT_ID]		=	&trusted_boot_fw_cert,
+	[BL2_IMAGE_ID]				=	&bl2_image,
+	[HW_CONFIG_ID]				=	&hw_config,
+	[TB_FW_CONFIG_ID]			=	&tb_fw_config,
+	[FW_CONFIG_ID]				=	&fw_config,
+	[FWU_CERT_ID]				=	&fwu_cert,
+	[SCP_BL2U_IMAGE_ID]			=	&scp_bl2u_image,
+	[BL2U_IMAGE_ID]				=	&bl2u_image,
+	[NS_BL2U_IMAGE_ID]			=	&ns_bl2u_image
+};
+#else /* IMAGE_BL2 */
+static const auth_img_desc_t * const cot_desc[] = {
+	[TRUSTED_BOOT_FW_CERT_ID]		=	&trusted_boot_fw_cert,
+	[HW_CONFIG_ID]				=	&hw_config,
+	[TRUSTED_KEY_CERT_ID]			=	&trusted_key_cert,
+	[SCP_FW_KEY_CERT_ID]			=	&scp_fw_key_cert,
+	[SCP_FW_CONTENT_CERT_ID]		=	&scp_fw_content_cert,
+	[SCP_BL2_IMAGE_ID]			=	&scp_bl2_image,
+	[SOC_FW_KEY_CERT_ID]			=	&soc_fw_key_cert,
+	[SOC_FW_CONTENT_CERT_ID]		=	&soc_fw_content_cert,
+	[BL31_IMAGE_ID]				=	&bl31_image,
+	[SOC_FW_CONFIG_ID]			=	&soc_fw_config,
+	[TRUSTED_OS_FW_KEY_CERT_ID]		=	&trusted_os_fw_key_cert,
+	[TRUSTED_OS_FW_CONTENT_CERT_ID]		=	&trusted_os_fw_content_cert,
+	[BL32_IMAGE_ID]				=	&bl32_image,
+	[BL32_EXTRA1_IMAGE_ID]			=	&bl32_extra1_image,
+	[BL32_EXTRA2_IMAGE_ID]			=	&bl32_extra2_image,
+	[TOS_FW_CONFIG_ID]			=	&tos_fw_config,
+	[NON_TRUSTED_FW_CONTENT_CERT_ID]	=	&non_trusted_fw_content_cert,
+	[BL33_IMAGE_ID]				=	&bl33_image,
+	[NT_FW_CONFIG_ID]			=	&nt_fw_config,
+#if defined(SPD_spmd)
+	[SIP_SP_CONTENT_CERT_ID]		=	&sip_sp_content_cert,
+	[PLAT_SP_CONTENT_CERT_ID]		=	&plat_sp_content_cert,
+	[SP_PKG1_ID]				=	&sp_pkg1,
+	[SP_PKG2_ID]				=	&sp_pkg2,
+	[SP_PKG3_ID]				=	&sp_pkg3,
+	[SP_PKG4_ID]				=	&sp_pkg4,
+	[SP_PKG5_ID]				=	&sp_pkg5,
+	[SP_PKG6_ID]				=	&sp_pkg6,
+	[SP_PKG7_ID]				=	&sp_pkg7,
+	[SP_PKG8_ID]				=       &sp_pkg8,
+#endif
+};
+#endif
+
+/* Register the CoT in the authentication module */
+REGISTER_COT(cot_desc);
diff --git a/drivers/auth/img_parser_mod.c b/drivers/auth/img_parser_mod.c
new file mode 100644
index 0000000..535695d
--- /dev/null
+++ b/drivers/auth/img_parser_mod.c
@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <limits.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/auth/auth_common.h>
+#include <drivers/auth/img_parser_mod.h>
+#include <lib/utils_def.h>
+
+IMPORT_SYM(uintptr_t, __PARSER_LIB_DESCS_START__,	PARSER_LIB_DESCS_START);
+IMPORT_SYM(uintptr_t, __PARSER_LIB_DESCS_END__,		PARSER_LIB_DESCS_END);
+static unsigned int parser_lib_indices[IMG_MAX_TYPES];
+static img_parser_lib_desc_t *parser_lib_descs;
+
+#define INVALID_IDX		UINT_MAX
+
+static void validate_desc(img_parser_lib_desc_t *desc)
+{
+	assert(desc != NULL);
+	assert(desc->init != NULL);
+	assert(desc->name != NULL);
+	assert(desc->check_integrity != NULL);
+	assert(desc->get_auth_param != NULL);
+}
+
+void img_parser_init(void)
+{
+	unsigned int index, mod_num;
+
+	/* Initialise internal variables to invalid state */
+	for (index = 0; index < IMG_MAX_TYPES; index++) {
+		parser_lib_indices[index] = INVALID_IDX;
+	}
+
+	/* Calculate how many image parsers are registered. At least one parser
+	 * must be present */
+	mod_num = PARSER_LIB_DESCS_END - PARSER_LIB_DESCS_START;
+	mod_num /= sizeof(img_parser_lib_desc_t);
+	assert(mod_num > 0);
+
+	parser_lib_descs = (img_parser_lib_desc_t *) PARSER_LIB_DESCS_START;
+	for (index = 0; index < mod_num; index++) {
+
+		/* Check that the image parser library descriptor is valid */
+		validate_desc(&parser_lib_descs[index]);
+
+		/* Initialize image parser */
+		parser_lib_descs[index].init();
+
+		/* Ensure only one parser is registered for each image type */
+		assert(parser_lib_indices[parser_lib_descs[index].img_type] ==
+				INVALID_IDX);
+
+		/* Keep the index of this hash calculator */
+		parser_lib_indices[parser_lib_descs[index].img_type] = index;
+	}
+}
+
+int img_parser_check_integrity(img_type_t img_type,
+			       void *img_ptr, unsigned int img_len)
+{
+	unsigned int idx;
+
+	assert(img_ptr != NULL);
+	assert(img_len != 0);
+
+	/* No integrity checks on raw images */
+	if (img_type == IMG_RAW) {
+		return IMG_PARSER_OK;
+	}
+
+	/* Find the index of the required image parser */
+	idx = parser_lib_indices[img_type];
+	assert(idx != INVALID_IDX);
+
+	/* Call the function to check the image integrity */
+	return parser_lib_descs[idx].check_integrity(img_ptr, img_len);
+}
+
+/*
+ * Extract an authentication parameter from an image
+ *
+ * Parameters:
+ *   img_type: image type (certificate, raw image, etc)
+ *   type_desc: provides info to obtain the parameter
+ *   img_ptr: pointer to image data
+ *   img_len: image length
+ *   param_ptr: [out] stores a pointer to the parameter
+ *   param_len: [out] stores the length of the parameter
+ */
+int img_parser_get_auth_param(img_type_t img_type,
+			      const auth_param_type_desc_t *type_desc,
+			      void *img_ptr, unsigned int img_len,
+			      void **param_ptr, unsigned int *param_len)
+{
+	unsigned int idx;
+
+	assert(type_desc != NULL);
+	assert(img_ptr != NULL);
+	assert(img_len != 0);
+	assert(param_ptr != NULL);
+	assert(param_len != NULL);
+
+	/* In a raw image we can only get the data itself */
+	if (img_type == IMG_RAW) {
+		assert(type_desc->type == AUTH_PARAM_RAW_DATA);
+		*param_ptr = img_ptr;
+		*param_len = img_len;
+		return IMG_PARSER_OK;
+	}
+
+	/* Find the index of the required image parser library */
+	idx = parser_lib_indices[img_type];
+	assert(idx != INVALID_IDX);
+
+	/* Call the function to obtain the parameter */
+	return parser_lib_descs[idx].get_auth_param(type_desc, img_ptr, img_len,
+			param_ptr, param_len);
+}
diff --git a/drivers/auth/mbedtls/mbedtls_common.c b/drivers/auth/mbedtls/mbedtls_common.c
new file mode 100644
index 0000000..a12e49c
--- /dev/null
+++ b/drivers/auth/mbedtls/mbedtls_common.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+
+/* mbed TLS headers */
+#include <mbedtls/memory_buffer_alloc.h>
+#include <mbedtls/platform.h>
+
+#include <common/debug.h>
+#include <drivers/auth/mbedtls/mbedtls_common.h>
+#include MBEDTLS_CONFIG_FILE
+#include <plat/common/platform.h>
+
+static void cleanup(void)
+{
+	ERROR("EXIT from BL2\n");
+	panic();
+}
+
+/*
+ * mbed TLS initialization function
+ */
+void mbedtls_init(void)
+{
+	static int ready;
+	void *heap_addr;
+	size_t heap_size = 0;
+	int err;
+
+	if (!ready) {
+		if (atexit(cleanup))
+			panic();
+
+		err = plat_get_mbedtls_heap(&heap_addr, &heap_size);
+
+		/* Ensure heap setup is proper */
+		if (err < 0) {
+			ERROR("Mbed TLS failed to get a heap\n");
+			panic();
+		}
+		assert(heap_size >= TF_MBEDTLS_HEAP_SIZE);
+
+		/* Initialize the mbed TLS heap */
+		mbedtls_memory_buffer_alloc_init(heap_addr, heap_size);
+
+#ifdef MBEDTLS_PLATFORM_SNPRINTF_ALT
+		mbedtls_platform_set_snprintf(snprintf);
+#endif
+		ready = 1;
+	}
+}
+
+/*
+ * The following helper function simply returns the default allocated heap.
+ * It can be used by platforms for their plat_get_mbedtls_heap() implementation.
+ */
+int get_mbedtls_heap_helper(void **heap_addr, size_t *heap_size)
+{
+	static unsigned char heap[TF_MBEDTLS_HEAP_SIZE];
+
+	assert(heap_addr != NULL);
+	assert(heap_size != NULL);
+
+	*heap_addr = heap;
+	*heap_size = sizeof(heap);
+	return 0;
+}
diff --git a/drivers/auth/mbedtls/mbedtls_common.mk b/drivers/auth/mbedtls/mbedtls_common.mk
new file mode 100644
index 0000000..16ce65f
--- /dev/null
+++ b/drivers/auth/mbedtls/mbedtls_common.mk
@@ -0,0 +1,111 @@
+#
+# Copyright (c) 2015-2022, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifneq (${MBEDTLS_COMMON_MK},1)
+MBEDTLS_COMMON_MK	:=	1
+
+# MBEDTLS_DIR must be set to the mbed TLS main directory (it must contain
+# the 'include' and 'library' subdirectories).
+ifeq (${MBEDTLS_DIR},)
+  $(error Error: MBEDTLS_DIR not set)
+endif
+
+MBEDTLS_INC		=	-I${MBEDTLS_DIR}/include
+
+# Specify mbed TLS configuration file
+MBEDTLS_CONFIG_FILE	?=	"<drivers/auth/mbedtls/mbedtls_config.h>"
+$(eval $(call add_define,MBEDTLS_CONFIG_FILE))
+
+MBEDTLS_SOURCES	+=		drivers/auth/mbedtls/mbedtls_common.c
+
+
+LIBMBEDTLS_SRCS		:= $(addprefix ${MBEDTLS_DIR}/library/,	\
+					aes.c 					\
+					asn1parse.c 				\
+					asn1write.c 				\
+					cipher.c 				\
+					cipher_wrap.c 				\
+					memory_buffer_alloc.c			\
+					oid.c 					\
+					platform.c 				\
+					platform_util.c				\
+					bignum.c				\
+					gcm.c 					\
+					md.c					\
+					pk.c 					\
+					pk_wrap.c 				\
+					pkparse.c 				\
+					pkwrite.c 				\
+					sha256.c            			\
+					sha512.c            			\
+					ecdsa.c					\
+					ecp_curves.c				\
+					ecp.c					\
+					rsa.c					\
+					rsa_internal.c				\
+					x509.c 					\
+					x509_crt.c 				\
+					constant_time.c 			\
+					)
+
+# The platform may define the variable 'TF_MBEDTLS_KEY_ALG' to select the key
+# algorithm to use. If the variable is not defined, select it based on
+# algorithm used for key generation `KEY_ALG`. If `KEY_ALG` is not defined,
+# then it is set to `rsa`.
+ifeq (${TF_MBEDTLS_KEY_ALG},)
+    ifeq (${KEY_ALG}, ecdsa)
+        TF_MBEDTLS_KEY_ALG		:=	ecdsa
+    else
+        TF_MBEDTLS_KEY_ALG		:=	rsa
+    endif
+endif
+
+ifeq (${TF_MBEDTLS_KEY_SIZE},)
+    ifneq ($(findstring rsa,${TF_MBEDTLS_KEY_ALG}),)
+	ifeq (${KEY_SIZE},)
+            TF_MBEDTLS_KEY_SIZE		:=	2048
+	else
+            TF_MBEDTLS_KEY_SIZE		:=	${KEY_SIZE}
+	endif
+    endif
+endif
+
+ifeq (${HASH_ALG}, sha384)
+    TF_MBEDTLS_HASH_ALG_ID	:=	TF_MBEDTLS_SHA384
+else ifeq (${HASH_ALG}, sha512)
+    TF_MBEDTLS_HASH_ALG_ID	:=	TF_MBEDTLS_SHA512
+else
+    TF_MBEDTLS_HASH_ALG_ID	:=	TF_MBEDTLS_SHA256
+endif
+
+ifeq (${TF_MBEDTLS_KEY_ALG},ecdsa)
+    TF_MBEDTLS_KEY_ALG_ID	:=	TF_MBEDTLS_ECDSA
+else ifeq (${TF_MBEDTLS_KEY_ALG},rsa)
+    TF_MBEDTLS_KEY_ALG_ID	:=	TF_MBEDTLS_RSA
+else ifeq (${TF_MBEDTLS_KEY_ALG},rsa+ecdsa)
+    TF_MBEDTLS_KEY_ALG_ID	:=	TF_MBEDTLS_RSA_AND_ECDSA
+else
+    $(error "TF_MBEDTLS_KEY_ALG=${TF_MBEDTLS_KEY_ALG} not supported on mbed TLS")
+endif
+
+ifeq (${DECRYPTION_SUPPORT}, aes_gcm)
+    TF_MBEDTLS_USE_AES_GCM	:=	1
+else
+    TF_MBEDTLS_USE_AES_GCM	:=	0
+endif
+
+# Needs to be set to drive mbed TLS configuration correctly
+$(eval $(call add_defines,\
+    $(sort \
+        TF_MBEDTLS_KEY_ALG_ID \
+        TF_MBEDTLS_KEY_SIZE \
+        TF_MBEDTLS_HASH_ALG_ID \
+        TF_MBEDTLS_USE_AES_GCM \
+)))
+
+$(eval $(call MAKE_LIB,mbedtls))
+
+endif
diff --git a/drivers/auth/mbedtls/mbedtls_crypto.c b/drivers/auth/mbedtls/mbedtls_crypto.c
new file mode 100644
index 0000000..d231179
--- /dev/null
+++ b/drivers/auth/mbedtls/mbedtls_crypto.c
@@ -0,0 +1,394 @@
+/*
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+#include <string.h>
+
+/* mbed TLS headers */
+#include <mbedtls/gcm.h>
+#include <mbedtls/md.h>
+#include <mbedtls/memory_buffer_alloc.h>
+#include <mbedtls/oid.h>
+#include <mbedtls/platform.h>
+#include <mbedtls/x509.h>
+
+#include <common/debug.h>
+#include <drivers/auth/crypto_mod.h>
+#include <drivers/auth/mbedtls/mbedtls_common.h>
+#include <drivers/auth/mbedtls/mbedtls_config.h>
+#include <plat/common/platform.h>
+
+#define LIB_NAME		"mbed TLS"
+
+#if CRYPTO_SUPPORT == CRYPTO_HASH_CALC_ONLY || \
+CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC
+/*
+ * CRYPTO_MD_MAX_SIZE value is as per current stronger algorithm available
+ * so make sure that mbed TLS MD maximum size must be lesser than this.
+ */
+CASSERT(CRYPTO_MD_MAX_SIZE >= MBEDTLS_MD_MAX_SIZE,
+	assert_mbedtls_md_size_overflow);
+
+#endif /* CRYPTO_SUPPORT == CRYPTO_HASH_CALC_ONLY || \
+	  CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC */
+
+/*
+ * AlgorithmIdentifier  ::=  SEQUENCE  {
+ *     algorithm               OBJECT IDENTIFIER,
+ *     parameters              ANY DEFINED BY algorithm OPTIONAL
+ * }
+ *
+ * SubjectPublicKeyInfo  ::=  SEQUENCE  {
+ *     algorithm            AlgorithmIdentifier,
+ *     subjectPublicKey     BIT STRING
+ * }
+ *
+ * DigestInfo ::= SEQUENCE {
+ *     digestAlgorithm AlgorithmIdentifier,
+ *     digest OCTET STRING
+ * }
+ */
+
+/*
+ * Initialize the library and export the descriptor
+ */
+static void init(void)
+{
+	/* Initialize mbed TLS */
+	mbedtls_init();
+}
+
+#if CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_ONLY || \
+CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC
+/*
+ * Verify a signature.
+ *
+ * Parameters are passed using the DER encoding format following the ASN.1
+ * structures detailed above.
+ */
+static int verify_signature(void *data_ptr, unsigned int data_len,
+			    void *sig_ptr, unsigned int sig_len,
+			    void *sig_alg, unsigned int sig_alg_len,
+			    void *pk_ptr, unsigned int pk_len)
+{
+	mbedtls_asn1_buf sig_oid, sig_params;
+	mbedtls_asn1_buf signature;
+	mbedtls_md_type_t md_alg;
+	mbedtls_pk_type_t pk_alg;
+	mbedtls_pk_context pk = {0};
+	int rc;
+	void *sig_opts = NULL;
+	const mbedtls_md_info_t *md_info;
+	unsigned char *p, *end;
+	unsigned char hash[MBEDTLS_MD_MAX_SIZE];
+
+	/* Get pointers to signature OID and parameters */
+	p = (unsigned char *)sig_alg;
+	end = (unsigned char *)(p + sig_alg_len);
+	rc = mbedtls_asn1_get_alg(&p, end, &sig_oid, &sig_params);
+	if (rc != 0) {
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	/* Get the actual signature algorithm (MD + PK) */
+	rc = mbedtls_x509_get_sig_alg(&sig_oid, &sig_params, &md_alg, &pk_alg, &sig_opts);
+	if (rc != 0) {
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	/* Parse the public key */
+	mbedtls_pk_init(&pk);
+	p = (unsigned char *)pk_ptr;
+	end = (unsigned char *)(p + pk_len);
+	rc = mbedtls_pk_parse_subpubkey(&p, end, &pk);
+	if (rc != 0) {
+		rc = CRYPTO_ERR_SIGNATURE;
+		goto end2;
+	}
+
+	/* Get the signature (bitstring) */
+	p = (unsigned char *)sig_ptr;
+	end = (unsigned char *)(p + sig_len);
+	signature.tag = *p;
+	rc = mbedtls_asn1_get_bitstring_null(&p, end, &signature.len);
+	if (rc != 0) {
+		rc = CRYPTO_ERR_SIGNATURE;
+		goto end1;
+	}
+	signature.p = p;
+
+	/* Calculate the hash of the data */
+	md_info = mbedtls_md_info_from_type(md_alg);
+	if (md_info == NULL) {
+		rc = CRYPTO_ERR_SIGNATURE;
+		goto end1;
+	}
+	p = (unsigned char *)data_ptr;
+	rc = mbedtls_md(md_info, p, data_len, hash);
+	if (rc != 0) {
+		rc = CRYPTO_ERR_SIGNATURE;
+		goto end1;
+	}
+
+	/* Verify the signature */
+	rc = mbedtls_pk_verify_ext(pk_alg, sig_opts, &pk, md_alg, hash,
+			mbedtls_md_get_size(md_info),
+			signature.p, signature.len);
+	if (rc != 0) {
+		rc = CRYPTO_ERR_SIGNATURE;
+		goto end1;
+	}
+
+	/* Signature verification success */
+	rc = CRYPTO_SUCCESS;
+
+end1:
+	mbedtls_pk_free(&pk);
+end2:
+	mbedtls_free(sig_opts);
+	return rc;
+}
+
+/*
+ * Match a hash
+ *
+ * Digest info is passed in DER format following the ASN.1 structure detailed
+ * above.
+ */
+static int verify_hash(void *data_ptr, unsigned int data_len,
+		       void *digest_info_ptr, unsigned int digest_info_len)
+{
+	mbedtls_asn1_buf hash_oid, params;
+	mbedtls_md_type_t md_alg;
+	const mbedtls_md_info_t *md_info;
+	unsigned char *p, *end, *hash;
+	unsigned char data_hash[MBEDTLS_MD_MAX_SIZE];
+	size_t len;
+	int rc;
+
+	/* Digest info should be an MBEDTLS_ASN1_SEQUENCE */
+	p = (unsigned char *)digest_info_ptr;
+	end = p + digest_info_len;
+	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				  MBEDTLS_ASN1_SEQUENCE);
+	if (rc != 0) {
+		return CRYPTO_ERR_HASH;
+	}
+
+	/* Get the hash algorithm */
+	rc = mbedtls_asn1_get_alg(&p, end, &hash_oid, &params);
+	if (rc != 0) {
+		return CRYPTO_ERR_HASH;
+	}
+
+	rc = mbedtls_oid_get_md_alg(&hash_oid, &md_alg);
+	if (rc != 0) {
+		return CRYPTO_ERR_HASH;
+	}
+
+	md_info = mbedtls_md_info_from_type(md_alg);
+	if (md_info == NULL) {
+		return CRYPTO_ERR_HASH;
+	}
+
+	/* Hash should be octet string type */
+	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_OCTET_STRING);
+	if (rc != 0) {
+		return CRYPTO_ERR_HASH;
+	}
+
+	/* Length of hash must match the algorithm's size */
+	if (len != mbedtls_md_get_size(md_info)) {
+		return CRYPTO_ERR_HASH;
+	}
+	hash = p;
+
+	/* Calculate the hash of the data */
+	p = (unsigned char *)data_ptr;
+	rc = mbedtls_md(md_info, p, data_len, data_hash);
+	if (rc != 0) {
+		return CRYPTO_ERR_HASH;
+	}
+
+	/* Compare values */
+	rc = memcmp(data_hash, hash, mbedtls_md_get_size(md_info));
+	if (rc != 0) {
+		return CRYPTO_ERR_HASH;
+	}
+
+	return CRYPTO_SUCCESS;
+}
+#endif /* CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_ONLY || \
+	  CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC */
+
+#if CRYPTO_SUPPORT == CRYPTO_HASH_CALC_ONLY || \
+CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC
+/*
+ * Map a generic crypto message digest algorithm to the corresponding macro used
+ * by Mbed TLS.
+ */
+static inline mbedtls_md_type_t md_type(enum crypto_md_algo algo)
+{
+	switch (algo) {
+	case CRYPTO_MD_SHA512:
+		return MBEDTLS_MD_SHA512;
+	case CRYPTO_MD_SHA384:
+		return MBEDTLS_MD_SHA384;
+	case CRYPTO_MD_SHA256:
+		return MBEDTLS_MD_SHA256;
+	default:
+		/* Invalid hash algorithm. */
+		return MBEDTLS_MD_NONE;
+	}
+}
+
+/*
+ * Calculate a hash
+ *
+ * output points to the computed hash
+ */
+static int calc_hash(enum crypto_md_algo md_algo, void *data_ptr,
+		     unsigned int data_len,
+		     unsigned char output[CRYPTO_MD_MAX_SIZE])
+{
+	const mbedtls_md_info_t *md_info;
+
+	md_info = mbedtls_md_info_from_type(md_type(md_algo));
+	if (md_info == NULL) {
+		return CRYPTO_ERR_HASH;
+	}
+
+	/*
+	 * Calculate the hash of the data, it is safe to pass the
+	 * 'output' hash buffer pointer considering its size is always
+	 * bigger than or equal to MBEDTLS_MD_MAX_SIZE.
+	 */
+	return mbedtls_md(md_info, data_ptr, data_len, output);
+}
+#endif /* CRYPTO_SUPPORT == CRYPTO_HASH_CALC_ONLY || \
+	  CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC */
+
+#if TF_MBEDTLS_USE_AES_GCM
+/*
+ * Stack based buffer allocation for decryption operation. It could
+ * be configured to balance stack usage vs execution speed.
+ */
+#define DEC_OP_BUF_SIZE		128
+
+static int aes_gcm_decrypt(void *data_ptr, size_t len, const void *key,
+			   unsigned int key_len, const void *iv,
+			   unsigned int iv_len, const void *tag,
+			   unsigned int tag_len)
+{
+	mbedtls_gcm_context ctx;
+	mbedtls_cipher_id_t cipher = MBEDTLS_CIPHER_ID_AES;
+	unsigned char buf[DEC_OP_BUF_SIZE];
+	unsigned char tag_buf[CRYPTO_MAX_TAG_SIZE];
+	unsigned char *pt = data_ptr;
+	size_t dec_len;
+	int diff, i, rc;
+
+	mbedtls_gcm_init(&ctx);
+
+	rc = mbedtls_gcm_setkey(&ctx, cipher, key, key_len * 8);
+	if (rc != 0) {
+		rc = CRYPTO_ERR_DECRYPTION;
+		goto exit_gcm;
+	}
+
+	rc = mbedtls_gcm_starts(&ctx, MBEDTLS_GCM_DECRYPT, iv, iv_len, NULL, 0);
+	if (rc != 0) {
+		rc = CRYPTO_ERR_DECRYPTION;
+		goto exit_gcm;
+	}
+
+	while (len > 0) {
+		dec_len = MIN(sizeof(buf), len);
+
+		rc = mbedtls_gcm_update(&ctx, dec_len, pt, buf);
+		if (rc != 0) {
+			rc = CRYPTO_ERR_DECRYPTION;
+			goto exit_gcm;
+		}
+
+		memcpy(pt, buf, dec_len);
+		pt += dec_len;
+		len -= dec_len;
+	}
+
+	rc = mbedtls_gcm_finish(&ctx, tag_buf, sizeof(tag_buf));
+	if (rc != 0) {
+		rc = CRYPTO_ERR_DECRYPTION;
+		goto exit_gcm;
+	}
+
+	/* Check tag in "constant-time" */
+	for (diff = 0, i = 0; i < tag_len; i++)
+		diff |= ((const unsigned char *)tag)[i] ^ tag_buf[i];
+
+	if (diff != 0) {
+		rc = CRYPTO_ERR_DECRYPTION;
+		goto exit_gcm;
+	}
+
+	/* GCM decryption success */
+	rc = CRYPTO_SUCCESS;
+
+exit_gcm:
+	mbedtls_gcm_free(&ctx);
+	return rc;
+}
+
+/*
+ * Authenticated decryption of an image
+ */
+static int auth_decrypt(enum crypto_dec_algo dec_algo, void *data_ptr,
+			size_t len, const void *key, unsigned int key_len,
+			unsigned int key_flags, const void *iv,
+			unsigned int iv_len, const void *tag,
+			unsigned int tag_len)
+{
+	int rc;
+
+	assert((key_flags & ENC_KEY_IS_IDENTIFIER) == 0);
+
+	switch (dec_algo) {
+	case CRYPTO_GCM_DECRYPT:
+		rc = aes_gcm_decrypt(data_ptr, len, key, key_len, iv, iv_len,
+				     tag, tag_len);
+		if (rc != 0)
+			return rc;
+		break;
+	default:
+		return CRYPTO_ERR_DECRYPTION;
+	}
+
+	return CRYPTO_SUCCESS;
+}
+#endif /* TF_MBEDTLS_USE_AES_GCM */
+
+/*
+ * Register crypto library descriptor
+ */
+#if CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC
+#if TF_MBEDTLS_USE_AES_GCM
+REGISTER_CRYPTO_LIB(LIB_NAME, init, verify_signature, verify_hash, calc_hash,
+		    auth_decrypt);
+#else
+REGISTER_CRYPTO_LIB(LIB_NAME, init, verify_signature, verify_hash, calc_hash,
+		    NULL);
+#endif
+#elif CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_ONLY
+#if TF_MBEDTLS_USE_AES_GCM
+REGISTER_CRYPTO_LIB(LIB_NAME, init, verify_signature, verify_hash,
+		    auth_decrypt);
+#else
+REGISTER_CRYPTO_LIB(LIB_NAME, init, verify_signature, verify_hash, NULL);
+#endif
+#elif CRYPTO_SUPPORT == CRYPTO_HASH_CALC_ONLY
+REGISTER_CRYPTO_LIB(LIB_NAME, init, calc_hash);
+#endif /* CRYPTO_SUPPORT == CRYPTO_AUTH_VERIFY_AND_HASH_CALC */
diff --git a/drivers/auth/mbedtls/mbedtls_crypto.mk b/drivers/auth/mbedtls/mbedtls_crypto.mk
new file mode 100644
index 0000000..2a9fbbf
--- /dev/null
+++ b/drivers/auth/mbedtls/mbedtls_crypto.mk
@@ -0,0 +1,11 @@
+#
+# Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include drivers/auth/mbedtls/mbedtls_common.mk
+
+MBEDTLS_SOURCES	+=		drivers/auth/mbedtls/mbedtls_crypto.c
+
+
diff --git a/drivers/auth/mbedtls/mbedtls_x509.mk b/drivers/auth/mbedtls/mbedtls_x509.mk
new file mode 100644
index 0000000..a0557e2
--- /dev/null
+++ b/drivers/auth/mbedtls/mbedtls_x509.mk
@@ -0,0 +1,9 @@
+#
+# Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include drivers/auth/mbedtls/mbedtls_common.mk
+
+MBEDTLS_SOURCES	+=	drivers/auth/mbedtls/mbedtls_x509_parser.c
diff --git a/drivers/auth/mbedtls/mbedtls_x509_parser.c b/drivers/auth/mbedtls/mbedtls_x509_parser.c
new file mode 100644
index 0000000..993ef12
--- /dev/null
+++ b/drivers/auth/mbedtls/mbedtls_x509_parser.c
@@ -0,0 +1,478 @@
+/*
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * X509 parser based on mbed TLS
+ *
+ * This module implements functions to check the integrity of a X509v3
+ * certificate ASN.1 structure and extract authentication parameters from the
+ * extensions field, such as an image hash or a public key.
+ */
+
+#include <assert.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <string.h>
+
+/* mbed TLS headers */
+#include <mbedtls/asn1.h>
+#include <mbedtls/oid.h>
+#include <mbedtls/platform.h>
+
+#include <arch_helpers.h>
+#include <drivers/auth/img_parser_mod.h>
+#include <drivers/auth/mbedtls/mbedtls_common.h>
+#include <lib/utils.h>
+
+/* Maximum OID string length ("a.b.c.d.e.f ...") */
+#define MAX_OID_STR_LEN			64
+
+#define LIB_NAME	"mbed TLS X509v3"
+
+/* Temporary variables to speed up the authentication parameters search. These
+ * variables are assigned once during the integrity check and used any time an
+ * authentication parameter is requested, so we do not have to parse the image
+ * again */
+static mbedtls_asn1_buf tbs;
+static mbedtls_asn1_buf v3_ext;
+static mbedtls_asn1_buf pk;
+static mbedtls_asn1_buf sig_alg;
+static mbedtls_asn1_buf signature;
+
+/*
+ * Clear all static temporary variables.
+ */
+static void clear_temp_vars(void)
+{
+#define ZERO_AND_CLEAN(x)					\
+	do {							\
+		zeromem(&x, sizeof(x));				\
+		clean_dcache_range((uintptr_t)&x, sizeof(x));	\
+	} while (0);
+
+	ZERO_AND_CLEAN(tbs)
+	ZERO_AND_CLEAN(v3_ext);
+	ZERO_AND_CLEAN(pk);
+	ZERO_AND_CLEAN(sig_alg);
+	ZERO_AND_CLEAN(signature);
+
+#undef ZERO_AND_CLEAN
+}
+
+/*
+ * Get X509v3 extension
+ *
+ * Global variable 'v3_ext' must point to the extensions region
+ * in the certificate. No need to check for errors since the image has passed
+ * the integrity check.
+ */
+static int get_ext(const char *oid, void **ext, unsigned int *ext_len)
+{
+	int oid_len;
+	size_t len;
+	unsigned char *end_ext_data, *end_ext_octet;
+	unsigned char *p;
+	const unsigned char *end;
+	char oid_str[MAX_OID_STR_LEN];
+	mbedtls_asn1_buf extn_oid;
+	int is_critical;
+
+	assert(oid != NULL);
+
+	p = v3_ext.p;
+	end = v3_ext.p + v3_ext.len;
+
+	mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+			     MBEDTLS_ASN1_SEQUENCE);
+
+	while (p < end) {
+		zeromem(&extn_oid, sizeof(extn_oid));
+		is_critical = 0; /* DEFAULT FALSE */
+
+		mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				     MBEDTLS_ASN1_SEQUENCE);
+		end_ext_data = p + len;
+
+		/* Get extension ID */
+		extn_oid.tag = *p;
+		mbedtls_asn1_get_tag(&p, end, &extn_oid.len, MBEDTLS_ASN1_OID);
+		extn_oid.p = p;
+		p += extn_oid.len;
+
+		/* Get optional critical */
+		mbedtls_asn1_get_bool(&p, end_ext_data, &is_critical);
+
+		/* Extension data */
+		mbedtls_asn1_get_tag(&p, end_ext_data, &len,
+				     MBEDTLS_ASN1_OCTET_STRING);
+		end_ext_octet = p + len;
+
+		/* Detect requested extension */
+		oid_len = mbedtls_oid_get_numeric_string(oid_str,
+							 MAX_OID_STR_LEN,
+							 &extn_oid);
+		if ((oid_len == MBEDTLS_ERR_OID_BUF_TOO_SMALL) || (oid_len < 0)) {
+			return IMG_PARSER_ERR;
+		}
+		if (((size_t)oid_len == strlen(oid_str)) && !strcmp(oid, oid_str)) {
+			*ext = (void *)p;
+			*ext_len = (unsigned int)len;
+			return IMG_PARSER_OK;
+		}
+
+		/* Next */
+		p = end_ext_octet;
+	}
+
+	return IMG_PARSER_ERR_NOT_FOUND;
+}
+
+
+/*
+ * Check the integrity of the certificate ASN.1 structure.
+ *
+ * Extract the relevant data that will be used later during authentication.
+ *
+ * This function doesn't clear the static variables located on the top of this
+ * file in case of an error. It is only called from check_integrity(), which
+ * performs the cleanup if necessary.
+ */
+static int cert_parse(void *img, unsigned int img_len)
+{
+	int ret, is_critical;
+	size_t len;
+	unsigned char *p, *end, *crt_end;
+	mbedtls_asn1_buf sig_alg1, sig_alg2;
+
+	p = (unsigned char *)img;
+	len = img_len;
+	end = p + len;
+
+	/*
+	 * Certificate  ::=  SEQUENCE  {
+	 *      tbsCertificate       TBSCertificate,
+	 *      signatureAlgorithm   AlgorithmIdentifier,
+	 *      signatureValue       BIT STRING  }
+	 */
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				   MBEDTLS_ASN1_SEQUENCE);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+
+	if (len > (size_t)(end - p)) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	crt_end = p + len;
+
+	/*
+	 * TBSCertificate  ::=  SEQUENCE  {
+	 */
+	tbs.p = p;
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				   MBEDTLS_ASN1_SEQUENCE);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	end = p + len;
+	tbs.len = end - tbs.p;
+
+	/*
+	 * Version  ::=  INTEGER  {  v1(0), v2(1), v3(2)  }
+	 */
+	ret = mbedtls_asn1_get_tag(&p, end, &len,
+				   MBEDTLS_ASN1_CONTEXT_SPECIFIC |
+				   MBEDTLS_ASN1_CONSTRUCTED | 0);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	p += len;
+
+	/*
+	 * CertificateSerialNumber  ::=  INTEGER
+	 */
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_INTEGER);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	p += len;
+
+	/*
+	 * signature            AlgorithmIdentifier
+	 */
+	sig_alg1.p = p;
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				   MBEDTLS_ASN1_SEQUENCE);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	if ((end - p) < 1) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	sig_alg1.len = (p + len) - sig_alg1.p;
+	p += len;
+
+	/*
+	 * issuer               Name
+	 */
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				   MBEDTLS_ASN1_SEQUENCE);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	p += len;
+
+	/*
+	 * Validity ::= SEQUENCE {
+	 *      notBefore      Time,
+	 *      notAfter       Time }
+	 *
+	 */
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				   MBEDTLS_ASN1_SEQUENCE);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	p += len;
+
+	/*
+	 * subject              Name
+	 */
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				   MBEDTLS_ASN1_SEQUENCE);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	p += len;
+
+	/*
+	 * SubjectPublicKeyInfo
+	 */
+	pk.p = p;
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				   MBEDTLS_ASN1_SEQUENCE);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	pk.len = (p + len) - pk.p;
+	p += len;
+
+	/*
+	 * issuerUniqueID  [1]  IMPLICIT UniqueIdentifier OPTIONAL,
+	 */
+	ret = mbedtls_asn1_get_tag(&p, end, &len,
+				   MBEDTLS_ASN1_CONTEXT_SPECIFIC |
+				   MBEDTLS_ASN1_CONSTRUCTED | 1);
+	if (ret != 0) {
+		if (ret != MBEDTLS_ERR_ASN1_UNEXPECTED_TAG) {
+			return IMG_PARSER_ERR_FORMAT;
+		}
+	} else {
+		p += len;
+	}
+
+	/*
+	 * subjectUniqueID [2]  IMPLICIT UniqueIdentifier OPTIONAL,
+	 */
+	ret = mbedtls_asn1_get_tag(&p, end, &len,
+				   MBEDTLS_ASN1_CONTEXT_SPECIFIC |
+				   MBEDTLS_ASN1_CONSTRUCTED | 2);
+	if (ret != 0) {
+		if (ret != MBEDTLS_ERR_ASN1_UNEXPECTED_TAG) {
+			return IMG_PARSER_ERR_FORMAT;
+		}
+	} else {
+		p += len;
+	}
+
+	/*
+	 * extensions      [3]  EXPLICIT Extensions OPTIONAL
+	 */
+	ret = mbedtls_asn1_get_tag(&p, end, &len,
+				   MBEDTLS_ASN1_CONTEXT_SPECIFIC |
+				   MBEDTLS_ASN1_CONSTRUCTED | 3);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+
+	/*
+	 * Extensions  ::=  SEQUENCE SIZE (1..MAX) OF Extension
+	 */
+	v3_ext.p = p;
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				   MBEDTLS_ASN1_SEQUENCE);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	v3_ext.len = (p + len) - v3_ext.p;
+
+	/*
+	 * Check extensions integrity
+	 */
+	while (p < end) {
+		ret = mbedtls_asn1_get_tag(&p, end, &len,
+					   MBEDTLS_ASN1_CONSTRUCTED |
+					   MBEDTLS_ASN1_SEQUENCE);
+		if (ret != 0) {
+			return IMG_PARSER_ERR_FORMAT;
+		}
+
+		/* Get extension ID */
+		ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_OID);
+		if (ret != 0) {
+			return IMG_PARSER_ERR_FORMAT;
+		}
+		p += len;
+
+		/* Get optional critical */
+		ret = mbedtls_asn1_get_bool(&p, end, &is_critical);
+		if ((ret != 0) && (ret != MBEDTLS_ERR_ASN1_UNEXPECTED_TAG)) {
+			return IMG_PARSER_ERR_FORMAT;
+		}
+
+		/* Data should be octet string type */
+		ret = mbedtls_asn1_get_tag(&p, end, &len,
+					   MBEDTLS_ASN1_OCTET_STRING);
+		if (ret != 0) {
+			return IMG_PARSER_ERR_FORMAT;
+		}
+		p += len;
+	}
+
+	if (p != end) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+
+	end = crt_end;
+
+	/*
+	 *  }
+	 *  -- end of TBSCertificate
+	 *
+	 *  signatureAlgorithm   AlgorithmIdentifier
+	 */
+	sig_alg2.p = p;
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				   MBEDTLS_ASN1_SEQUENCE);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	if ((end - p) < 1) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	sig_alg2.len = (p + len) - sig_alg2.p;
+	p += len;
+
+	/* Compare both signature algorithms */
+	if (sig_alg1.len != sig_alg2.len) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	if (0 != memcmp(sig_alg1.p, sig_alg2.p, sig_alg1.len)) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	memcpy(&sig_alg, &sig_alg1, sizeof(sig_alg));
+
+	/*
+	 * signatureValue       BIT STRING
+	 */
+	signature.p = p;
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_BIT_STRING);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	signature.len = (p + len) - signature.p;
+	p += len;
+
+	/* Check certificate length */
+	if (p != end) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+
+	return IMG_PARSER_OK;
+}
+
+
+/* Exported functions */
+
+static void init(void)
+{
+	mbedtls_init();
+}
+
+/*
+ * Wrapper for cert_parse() that clears the static variables used by it in case
+ * of an error.
+ */
+static int check_integrity(void *img, unsigned int img_len)
+{
+	int rc = cert_parse(img, img_len);
+
+	if (rc != IMG_PARSER_OK)
+		clear_temp_vars();
+
+	return rc;
+}
+
+/*
+ * Extract an authentication parameter from an X509v3 certificate
+ *
+ * This function returns a pointer to the extracted data and its length.
+ * Depending on the type of parameter, a pointer to the data stored in the
+ * certificate may be returned (i.e. an octet string containing a hash). Other
+ * data may need to be copied and formatted (i.e. integers). In the later case,
+ * a buffer of the correct type needs to be statically allocated, filled and
+ * returned.
+ */
+static int get_auth_param(const auth_param_type_desc_t *type_desc,
+		void *img, unsigned int img_len,
+		void **param, unsigned int *param_len)
+{
+	int rc = IMG_PARSER_OK;
+
+	/* We do not use img because the check_integrity function has already
+	 * extracted the relevant data (v3_ext, pk, sig_alg, etc) */
+
+	switch (type_desc->type) {
+	case AUTH_PARAM_RAW_DATA:
+		/* Data to be signed */
+		*param = (void *)tbs.p;
+		*param_len = (unsigned int)tbs.len;
+		break;
+	case AUTH_PARAM_HASH:
+	case AUTH_PARAM_NV_CTR:
+		/* All these parameters are included as X509v3 extensions */
+		rc = get_ext(type_desc->cookie, param, param_len);
+		break;
+	case AUTH_PARAM_PUB_KEY:
+		if (type_desc->cookie != 0) {
+			/* Get public key from extension */
+			rc = get_ext(type_desc->cookie, param, param_len);
+		} else {
+			/* Get the subject public key */
+			*param = (void *)pk.p;
+			*param_len = (unsigned int)pk.len;
+		}
+		break;
+	case AUTH_PARAM_SIG_ALG:
+		/* Get the certificate signature algorithm */
+		*param = (void *)sig_alg.p;
+		*param_len = (unsigned int)sig_alg.len;
+		break;
+	case AUTH_PARAM_SIG:
+		/* Get the certificate signature */
+		*param = (void *)signature.p;
+		*param_len = (unsigned int)signature.len;
+		break;
+	default:
+		rc = IMG_PARSER_ERR_NOT_FOUND;
+		break;
+	}
+
+	return rc;
+}
+
+REGISTER_IMG_PARSER_LIB(IMG_CERT, LIB_NAME, init, \
+		       check_integrity, get_auth_param);
diff --git a/drivers/auth/tbbr/tbbr_cot_bl1.c b/drivers/auth/tbbr/tbbr_cot_bl1.c
new file mode 100644
index 0000000..44f8638
--- /dev/null
+++ b/drivers/auth/tbbr/tbbr_cot_bl1.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+
+#include <platform_def.h>
+#include MBEDTLS_CONFIG_FILE
+
+#include <drivers/auth/auth_mod.h>
+#include <drivers/auth/tbbr_cot_common.h>
+#if USE_TBBR_DEFS
+#include <tools_share/tbbr_oid.h>
+#else
+#include <platform_oid.h>
+#endif
+
+static auth_param_type_desc_t scp_bl2u_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SCP_FWU_CFG_HASH_OID);
+static auth_param_type_desc_t bl2u_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, AP_FWU_CFG_HASH_OID);
+static auth_param_type_desc_t ns_bl2u_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, FWU_HASH_OID);
+
+static const auth_img_desc_t bl2_image = {
+	.img_id = BL2_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_boot_fw_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tb_fw_hash
+			}
+		}
+	}
+};
+
+/*
+ * FWU auth descriptor.
+ */
+static const auth_img_desc_t fwu_cert = {
+	.img_id = FWU_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &subject_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &scp_bl2u_hash,
+			.data = {
+				.ptr = (void *)scp_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &bl2u_hash,
+			.data = {
+				.ptr = (void *)tb_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &ns_bl2u_hash,
+			.data = {
+				.ptr = (void *)nt_world_bl_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+/*
+ * SCP_BL2U
+ */
+static const auth_img_desc_t scp_bl2u_image = {
+	.img_id = SCP_BL2U_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &fwu_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &scp_bl2u_hash
+			}
+		}
+	}
+};
+/*
+ * BL2U
+ */
+static const auth_img_desc_t bl2u_image = {
+	.img_id = BL2U_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &fwu_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &bl2u_hash
+			}
+		}
+	}
+};
+/*
+ * NS_BL2U
+ */
+static const auth_img_desc_t ns_bl2u_image = {
+	.img_id = NS_BL2U_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &fwu_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &ns_bl2u_hash
+				}
+			}
+		}
+};
+/*
+ * TB_FW_CONFIG
+ */
+static const auth_img_desc_t tb_fw_config = {
+	.img_id = TB_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_boot_fw_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tb_fw_config_hash
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t fw_config = {
+	.img_id = FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_boot_fw_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &fw_config_hash
+			}
+		}
+	}
+};
+
+/*
+ * TBBR Chain of trust definition
+ */
+static const auth_img_desc_t * const cot_desc[] = {
+	[TRUSTED_BOOT_FW_CERT_ID]		=	&trusted_boot_fw_cert,
+	[BL2_IMAGE_ID]				=	&bl2_image,
+	[HW_CONFIG_ID]				=	&hw_config,
+	[TB_FW_CONFIG_ID]			=	&tb_fw_config,
+	[FW_CONFIG_ID]				=	&fw_config,
+	[FWU_CERT_ID]				=	&fwu_cert,
+	[SCP_BL2U_IMAGE_ID]			=	&scp_bl2u_image,
+	[BL2U_IMAGE_ID]				=	&bl2u_image,
+	[NS_BL2U_IMAGE_ID]			=	&ns_bl2u_image
+};
+
+/* Register the CoT in the authentication module */
+REGISTER_COT(cot_desc);
diff --git a/drivers/auth/tbbr/tbbr_cot_bl1_r64.c b/drivers/auth/tbbr/tbbr_cot_bl1_r64.c
new file mode 100644
index 0000000..78e38f6
--- /dev/null
+++ b/drivers/auth/tbbr/tbbr_cot_bl1_r64.c
@@ -0,0 +1,177 @@
+/*
+ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+
+#include <drivers/auth/auth_mod.h>
+#include MBEDTLS_CONFIG_FILE
+#include <drivers/auth/tbbr_cot_common.h>
+
+#if USE_TBBR_DEFS
+#include <tools_share/tbbr_oid.h>
+#else
+#include <platform_oid.h>
+#endif
+#include <platform_def.h>
+
+
+static unsigned char trusted_world_pk_buf[PK_DER_LEN];
+static unsigned char non_trusted_world_pk_buf[PK_DER_LEN];
+static unsigned char content_pk_buf[PK_DER_LEN];
+static unsigned char nt_fw_config_hash_buf[HASH_DER_LEN];
+
+static auth_param_type_desc_t non_trusted_nv_ctr = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_NV_CTR, NON_TRUSTED_FW_NVCOUNTER_OID);
+static auth_param_type_desc_t trusted_world_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, TRUSTED_WORLD_PK_OID);
+static auth_param_type_desc_t non_trusted_world_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, NON_TRUSTED_WORLD_PK_OID);
+static auth_param_type_desc_t nt_fw_content_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, NON_TRUSTED_FW_CONTENT_CERT_PK_OID);
+static auth_param_type_desc_t nt_world_bl_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, NON_TRUSTED_WORLD_BOOTLOADER_HASH_OID);
+static auth_param_type_desc_t nt_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, NON_TRUSTED_FW_CONFIG_HASH_OID);
+/*
+ * Trusted key certificate
+ */
+static const auth_img_desc_t trusted_key_cert = {
+	.img_id = TRUSTED_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &subject_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &trusted_world_pk,
+			.data = {
+				.ptr = (void *)trusted_world_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &non_trusted_world_pk,
+			.data = {
+				.ptr = (void *)non_trusted_world_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+/*
+ * Non-Trusted Firmware
+ */
+static const auth_img_desc_t non_trusted_fw_key_cert = {
+	.img_id = NON_TRUSTED_FW_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &nt_fw_content_pk,
+			.data = {
+				.ptr = (void *)content_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t non_trusted_fw_content_cert = {
+	.img_id = NON_TRUSTED_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &non_trusted_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &nt_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &nt_world_bl_hash,
+			.data = {
+				.ptr = (void *)nt_world_bl_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &nt_fw_config_hash,
+			.data = {
+				.ptr = (void *)nt_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl33_image = {
+	.img_id = BL33_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &non_trusted_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &nt_world_bl_hash
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t * const cot_desc[] = {
+	[TRUSTED_KEY_CERT_ID]			=	&trusted_key_cert,
+	[NON_TRUSTED_FW_KEY_CERT_ID]		=	&non_trusted_fw_key_cert,
+	[NON_TRUSTED_FW_CONTENT_CERT_ID]	=	&non_trusted_fw_content_cert,
+	[BL33_IMAGE_ID]				=	&bl33_image,
+};
+
+/* Register the CoT in the authentication module */
+REGISTER_COT(cot_desc);
diff --git a/drivers/auth/tbbr/tbbr_cot_bl2.c b/drivers/auth/tbbr/tbbr_cot_bl2.c
new file mode 100644
index 0000000..11e2f46
--- /dev/null
+++ b/drivers/auth/tbbr/tbbr_cot_bl2.c
@@ -0,0 +1,688 @@
+/*
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+
+#include <platform_def.h>
+#include MBEDTLS_CONFIG_FILE
+
+#include <drivers/auth/auth_mod.h>
+#include <drivers/auth/tbbr_cot_common.h>
+#if USE_TBBR_DEFS
+#include <tools_share/tbbr_oid.h>
+#else
+#include <platform_oid.h>
+#endif
+
+static unsigned char soc_fw_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_extra1_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_extra2_hash_buf[HASH_DER_LEN];
+static unsigned char trusted_world_pk_buf[PK_DER_LEN];
+static unsigned char non_trusted_world_pk_buf[PK_DER_LEN];
+static unsigned char content_pk_buf[PK_DER_LEN];
+static unsigned char soc_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char nt_fw_config_hash_buf[HASH_DER_LEN];
+#if defined(SPD_spmd)
+static unsigned char sp_pkg_hash_buf[MAX_SP_IDS][HASH_DER_LEN];
+#endif /* SPD_spmd */
+
+static auth_param_type_desc_t non_trusted_nv_ctr = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_NV_CTR, NON_TRUSTED_FW_NVCOUNTER_OID);
+static auth_param_type_desc_t trusted_world_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, TRUSTED_WORLD_PK_OID);
+static auth_param_type_desc_t non_trusted_world_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, NON_TRUSTED_WORLD_PK_OID);
+static auth_param_type_desc_t scp_fw_content_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, SCP_FW_CONTENT_CERT_PK_OID);
+static auth_param_type_desc_t soc_fw_content_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, SOC_FW_CONTENT_CERT_PK_OID);
+static auth_param_type_desc_t tos_fw_content_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, TRUSTED_OS_FW_CONTENT_CERT_PK_OID);
+static auth_param_type_desc_t nt_fw_content_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, NON_TRUSTED_FW_CONTENT_CERT_PK_OID);
+static auth_param_type_desc_t scp_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SCP_FW_HASH_OID);
+static auth_param_type_desc_t soc_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SOC_AP_FW_HASH_OID);
+static auth_param_type_desc_t soc_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SOC_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t tos_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_HASH_OID);
+static auth_param_type_desc_t tos_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t tos_fw_extra1_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_EXTRA1_HASH_OID);
+static auth_param_type_desc_t tos_fw_extra2_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_EXTRA2_HASH_OID);
+static auth_param_type_desc_t nt_world_bl_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, NON_TRUSTED_WORLD_BOOTLOADER_HASH_OID);
+static auth_param_type_desc_t nt_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, NON_TRUSTED_FW_CONFIG_HASH_OID);
+#if defined(SPD_spmd)
+static auth_param_type_desc_t sp_pkg1_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG1_HASH_OID);
+static auth_param_type_desc_t sp_pkg2_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG2_HASH_OID);
+static auth_param_type_desc_t sp_pkg3_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG3_HASH_OID);
+static auth_param_type_desc_t sp_pkg4_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG4_HASH_OID);
+static auth_param_type_desc_t sp_pkg5_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG5_HASH_OID);
+static auth_param_type_desc_t sp_pkg6_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG6_HASH_OID);
+static auth_param_type_desc_t sp_pkg7_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG7_HASH_OID);
+static auth_param_type_desc_t sp_pkg8_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG8_HASH_OID);
+#endif /* SPD_spmd */
+
+/*
+ * Trusted key certificate
+ */
+static const auth_img_desc_t trusted_key_cert = {
+	.img_id = TRUSTED_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &subject_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &trusted_world_pk,
+			.data = {
+				.ptr = (void *)trusted_world_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &non_trusted_world_pk,
+			.data = {
+				.ptr = (void *)non_trusted_world_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+/*
+ * SCP Firmware
+ */
+static const auth_img_desc_t scp_fw_key_cert = {
+	.img_id = SCP_FW_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &scp_fw_content_pk,
+			.data = {
+				.ptr = (void *)content_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t scp_fw_content_cert = {
+	.img_id = SCP_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &scp_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &scp_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &scp_fw_hash,
+			.data = {
+				.ptr = (void *)scp_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t scp_bl2_image = {
+	.img_id = SCP_BL2_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &scp_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &scp_fw_hash
+			}
+		}
+	}
+};
+/*
+ * SoC Firmware
+ */
+static const auth_img_desc_t soc_fw_key_cert = {
+	.img_id = SOC_FW_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &soc_fw_content_pk,
+			.data = {
+				.ptr = (void *)content_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t soc_fw_content_cert = {
+	.img_id = SOC_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &soc_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &soc_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &soc_fw_hash,
+			.data = {
+				.ptr = (void *)soc_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &soc_fw_config_hash,
+			.data = {
+				.ptr = (void *)soc_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl31_image = {
+	.img_id = BL31_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &soc_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &soc_fw_hash
+			}
+		}
+	}
+};
+/* SOC FW Config */
+static const auth_img_desc_t soc_fw_config = {
+	.img_id = SOC_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &soc_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &soc_fw_config_hash
+			}
+		}
+	}
+};
+/*
+ * Trusted OS Firmware
+ */
+static const auth_img_desc_t trusted_os_fw_key_cert = {
+	.img_id = TRUSTED_OS_FW_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &tos_fw_content_pk,
+			.data = {
+				.ptr = (void *)content_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t trusted_os_fw_content_cert = {
+	.img_id = TRUSTED_OS_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_os_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &tos_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &tos_fw_hash,
+			.data = {
+				.ptr = (void *)tos_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &tos_fw_extra1_hash,
+			.data = {
+				.ptr = (void *)tos_fw_extra1_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &tos_fw_extra2_hash,
+			.data = {
+				.ptr = (void *)tos_fw_extra2_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &tos_fw_config_hash,
+			.data = {
+				.ptr = (void *)tos_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl32_image = {
+	.img_id = BL32_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl32_extra1_image = {
+	.img_id = BL32_EXTRA1_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_extra1_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl32_extra2_image = {
+	.img_id = BL32_EXTRA2_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_extra2_hash
+			}
+		}
+	}
+};
+/* TOS FW Config */
+static const auth_img_desc_t tos_fw_config = {
+	.img_id = TOS_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_config_hash
+			}
+		}
+	}
+};
+/*
+ * Non-Trusted Firmware
+ */
+static const auth_img_desc_t non_trusted_fw_key_cert = {
+	.img_id = NON_TRUSTED_FW_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &nt_fw_content_pk,
+			.data = {
+				.ptr = (void *)content_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t non_trusted_fw_content_cert = {
+	.img_id = NON_TRUSTED_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &non_trusted_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &nt_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &nt_world_bl_hash,
+			.data = {
+				.ptr = (void *)nt_world_bl_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &nt_fw_config_hash,
+			.data = {
+				.ptr = (void *)nt_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl33_image = {
+	.img_id = BL33_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &non_trusted_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &nt_world_bl_hash
+			}
+		}
+	}
+};
+/* NT FW Config */
+static const auth_img_desc_t nt_fw_config = {
+	.img_id = NT_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &non_trusted_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &nt_fw_config_hash
+			}
+		}
+	}
+};
+/* Secure Partitions */
+#if defined(SPD_spmd)
+static const auth_img_desc_t sip_sp_content_cert = {
+	.img_id = SIP_SP_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &sp_pkg1_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[0],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &sp_pkg2_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[1],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &sp_pkg3_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[2],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &sp_pkg4_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[3],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[4] = {
+			.type_desc = &sp_pkg5_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[4],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[5] = {
+			.type_desc = &sp_pkg6_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[5],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[6] = {
+			.type_desc = &sp_pkg7_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[6],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[7] = {
+			.type_desc = &sp_pkg8_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[7],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+DEFINE_SIP_SP_PKG(1);
+DEFINE_SIP_SP_PKG(2);
+DEFINE_SIP_SP_PKG(3);
+DEFINE_SIP_SP_PKG(4);
+DEFINE_SIP_SP_PKG(5);
+DEFINE_SIP_SP_PKG(6);
+DEFINE_SIP_SP_PKG(7);
+DEFINE_SIP_SP_PKG(8);
+#endif /* SPD_spmd */
+
+static const auth_img_desc_t * const cot_desc[] = {
+	[TRUSTED_BOOT_FW_CERT_ID]		=	&trusted_boot_fw_cert,
+	[HW_CONFIG_ID]				=	&hw_config,
+	[TRUSTED_KEY_CERT_ID]			=	&trusted_key_cert,
+	[SCP_FW_KEY_CERT_ID]			=	&scp_fw_key_cert,
+	[SCP_FW_CONTENT_CERT_ID]		=	&scp_fw_content_cert,
+	[SCP_BL2_IMAGE_ID]			=	&scp_bl2_image,
+	[SOC_FW_KEY_CERT_ID]			=	&soc_fw_key_cert,
+	[SOC_FW_CONTENT_CERT_ID]		=	&soc_fw_content_cert,
+	[BL31_IMAGE_ID]				=	&bl31_image,
+	[SOC_FW_CONFIG_ID]			=	&soc_fw_config,
+	[TRUSTED_OS_FW_KEY_CERT_ID]		=	&trusted_os_fw_key_cert,
+	[TRUSTED_OS_FW_CONTENT_CERT_ID]		=	&trusted_os_fw_content_cert,
+	[BL32_IMAGE_ID]				=	&bl32_image,
+	[BL32_EXTRA1_IMAGE_ID]			=	&bl32_extra1_image,
+	[BL32_EXTRA2_IMAGE_ID]			=	&bl32_extra2_image,
+	[TOS_FW_CONFIG_ID]			=	&tos_fw_config,
+	[NON_TRUSTED_FW_KEY_CERT_ID]		=	&non_trusted_fw_key_cert,
+	[NON_TRUSTED_FW_CONTENT_CERT_ID]	=	&non_trusted_fw_content_cert,
+	[BL33_IMAGE_ID]				=	&bl33_image,
+	[NT_FW_CONFIG_ID]			=	&nt_fw_config,
+#if defined(SPD_spmd)
+	[SIP_SP_CONTENT_CERT_ID]		=	&sip_sp_content_cert,
+	[SP_PKG1_ID]				=	&sp_pkg1,
+	[SP_PKG2_ID]				=	&sp_pkg2,
+	[SP_PKG3_ID]				=	&sp_pkg3,
+	[SP_PKG4_ID]				=	&sp_pkg4,
+	[SP_PKG5_ID]				=	&sp_pkg5,
+	[SP_PKG6_ID]				=	&sp_pkg6,
+	[SP_PKG7_ID]				=	&sp_pkg7,
+	[SP_PKG8_ID]				=       &sp_pkg8,
+#endif
+};
+
+/* Register the CoT in the authentication module */
+REGISTER_COT(cot_desc);
diff --git a/drivers/auth/tbbr/tbbr_cot_common.c b/drivers/auth/tbbr/tbbr_cot_common.c
new file mode 100644
index 0000000..0983d42
--- /dev/null
+++ b/drivers/auth/tbbr/tbbr_cot_common.c
@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+
+#include <platform_def.h>
+#include MBEDTLS_CONFIG_FILE
+
+#include <drivers/auth/auth_mod.h>
+#include <drivers/auth/tbbr_cot_common.h>
+#if USE_TBBR_DEFS
+#include <tools_share/tbbr_oid.h>
+#else
+#include <platform_oid.h>
+#endif
+
+/*
+ * The platform must allocate buffers to store the authentication parameters
+ * extracted from the certificates. In this case, because of the way the CoT is
+ * established, we can reuse some of the buffers on different stages
+ */
+
+static unsigned char fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char tb_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char hw_config_hash_buf[HASH_DER_LEN];
+unsigned char tb_fw_hash_buf[HASH_DER_LEN];
+unsigned char scp_fw_hash_buf[HASH_DER_LEN];
+unsigned char nt_world_bl_hash_buf[HASH_DER_LEN];
+
+/*
+ * common Parameter type descriptors across BL1 and BL2
+ */
+auth_param_type_desc_t trusted_nv_ctr = AUTH_PARAM_TYPE_DESC(
+	AUTH_PARAM_NV_CTR, TRUSTED_FW_NVCOUNTER_OID);
+auth_param_type_desc_t subject_pk = AUTH_PARAM_TYPE_DESC(
+	AUTH_PARAM_PUB_KEY, 0);
+auth_param_type_desc_t sig = AUTH_PARAM_TYPE_DESC(
+	AUTH_PARAM_SIG, 0);
+auth_param_type_desc_t sig_alg = AUTH_PARAM_TYPE_DESC(
+	AUTH_PARAM_SIG_ALG, 0);
+auth_param_type_desc_t raw_data = AUTH_PARAM_TYPE_DESC(
+	AUTH_PARAM_RAW_DATA, 0);
+
+/* common hash used across BL1 and BL2 */
+auth_param_type_desc_t tb_fw_hash = AUTH_PARAM_TYPE_DESC(
+	AUTH_PARAM_HASH, TRUSTED_BOOT_FW_HASH_OID);
+auth_param_type_desc_t tb_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+	AUTH_PARAM_HASH, TRUSTED_BOOT_FW_CONFIG_HASH_OID);
+auth_param_type_desc_t fw_config_hash = AUTH_PARAM_TYPE_DESC(
+	AUTH_PARAM_HASH, FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t hw_config_hash = AUTH_PARAM_TYPE_DESC(
+	AUTH_PARAM_HASH, HW_CONFIG_HASH_OID);
+
+/* trusted_boot_fw_cert */
+const auth_img_desc_t trusted_boot_fw_cert = {
+	.img_id = TRUSTED_BOOT_FW_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &subject_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &tb_fw_hash,
+			.data = {
+				.ptr = (void *)tb_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &tb_fw_config_hash,
+			.data = {
+				.ptr = (void *)tb_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &hw_config_hash,
+			.data = {
+				.ptr = (void *)hw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &fw_config_hash,
+			.data = {
+				.ptr = (void *)fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+/* HW Config */
+const auth_img_desc_t hw_config = {
+	.img_id = HW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_boot_fw_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &hw_config_hash
+			}
+		}
+	}
+};
diff --git a/drivers/brcm/chimp.c b/drivers/brcm/chimp.c
new file mode 100644
index 0000000..81767bb
--- /dev/null
+++ b/drivers/brcm/chimp.c
@@ -0,0 +1,398 @@
+/*
+ * Copyright (c) 2016 - 2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+
+#include <drivers/delay_timer.h>
+
+#include <chimp.h>
+#include <chimp_nv_defs.h>
+
+#define CHIMP_DEFAULT_STARTUP_ADDR 0xb4300000
+
+/* ChiMP's view of APE scratchpad memory for fastboot */
+#define CHIMP_FASTBOOT_ADDR 0x61000000
+
+#define CHIMP_PREPARE_ACCESS_WINDOW(addr) \
+	(\
+		mmio_write_32(\
+			NIC400_NITRO_CHIMP_S_IDM_IO_CONTROL_DIRECT, \
+			addr & 0xffc00000)\
+	)
+#define CHIMP_INDIRECT_TGT_ADDR(addr) \
+	(CHIMP_INDIRECT_BASE + (addr & CHIMP_INDIRECT_ADDR_MASK))
+
+#define CHIMP_CTRL_ADDR(x) (CHIMP_REG_CTRL_BASE + x)
+
+/* For non-PAXC builds */
+#ifndef CHIMP_FB1_ENTRY
+#define CHIMP_FB1_ENTRY 0
+#endif
+
+#define CHIMP_DBG	VERBOSE
+
+void bcm_chimp_write(uintptr_t addr, uint32_t value)
+{
+	CHIMP_PREPARE_ACCESS_WINDOW(addr);
+	mmio_write_32(CHIMP_INDIRECT_TGT_ADDR(addr), value);
+}
+
+uint32_t bcm_chimp_read(uintptr_t addr)
+{
+	CHIMP_PREPARE_ACCESS_WINDOW(addr);
+	return mmio_read_32(CHIMP_INDIRECT_TGT_ADDR(addr));
+}
+
+void bcm_chimp_clrbits(uintptr_t addr, uint32_t bits)
+{
+	CHIMP_PREPARE_ACCESS_WINDOW(addr);
+	mmio_clrbits_32(CHIMP_INDIRECT_TGT_ADDR(addr), bits);
+}
+
+void bcm_chimp_setbits(uintptr_t addr, uint32_t bits)
+{
+	CHIMP_PREPARE_ACCESS_WINDOW(addr);
+	mmio_setbits_32(CHIMP_INDIRECT_TGT_ADDR(addr), bits);
+}
+
+int bcm_chimp_is_nic_mode(void)
+{
+	uint32_t val;
+
+	/* Check if ChiMP straps are set */
+	val = mmio_read_32(CDRU_CHIP_STRAP_DATA_LSW);
+	val &= CDRU_CHIP_STRAP_DATA_LSW__NIC_MODE_MASK;
+
+	return val == CDRU_CHIP_STRAP_DATA_LSW__NIC_MODE_MASK;
+}
+
+void bcm_chimp_fru_prog_done(bool is_done)
+{
+	uint32_t val;
+
+	val = is_done ? (1 << CHIMP_FRU_PROG_DONE_BIT) : 0;
+	bcm_chimp_setbits(CHIMP_REG_ECO_RESERVED, val);
+}
+
+int bcm_chimp_handshake_done(void)
+{
+	uint32_t value;
+
+	value = bcm_chimp_read(CHIMP_REG_ECO_RESERVED);
+	value &= (1 << CHIMP_FLASH_ACCESS_DONE_BIT);
+
+	return value != 0;
+}
+
+int bcm_chimp_wait_handshake(void)
+{
+	uint32_t timeout = CHIMP_HANDSHAKE_TIMEOUT_MS;
+	uint32_t status;
+
+	INFO("Waiting for ChiMP handshake...\n");
+	do {
+		if (bcm_chimp_handshake_done())
+			break;
+		/* No need to wait if ChiMP reported an error */
+		status = bcm_chimp_read_ctrl(CHIMP_REG_CTRL_BPE_STAT_REG);
+		if (status & CHIMP_ERROR_MASK) {
+			ERROR("ChiMP error 0x%x. Wait aborted\n", status);
+			break;
+		}
+		mdelay(1);
+	} while (--timeout);
+
+	if (!bcm_chimp_handshake_done()) {
+		if (timeout == 0) {
+			WARN("Timeout waiting for ChiMP handshake\n");
+		}
+	} else {
+		INFO("Got handshake from ChiMP!\n");
+	}
+
+	return bcm_chimp_handshake_done();
+}
+
+uint32_t bcm_chimp_read_ctrl(uint32_t offset)
+{
+	return bcm_chimp_read(CHIMP_CTRL_ADDR(offset));
+}
+
+static int bcm_chimp_nitro_reset(void)
+{
+	uint32_t timeout;
+
+	/* Perform tasks done by M0 in NIC mode */
+	CHIMP_DBG("Taking Nitro out of reset\n");
+	mmio_setbits_32(CDRU_MISC_RESET_CONTROL,
+		/* MHB_RESET_N */
+		(1 << CDRU_MISC_RESET_CONTROL__CDRU_MHB_RESET_N_R)  |
+		/* PCI_RESET_N */
+		(1 << CDRU_MISC_RESET_CONTROL__CDRU_PCIE_RESET_N_R) |
+		/* PM_RESET_N */
+		(1 << CDRU_MISC_RESET_CONTROL__CDRU_PM_RESET_N_R)   |
+		/* NIC_RESET_N */
+		(1 << CDRU_MISC_RESET_CONTROL__CDRU_NITRO_RESET_N_R)
+	);
+
+	/* Wait until Nitro is out of reset */
+	timeout = NIC_RESET_RELEASE_TIMEOUT_US;
+	do {
+		uint32_t value;
+
+		value = bcm_chimp_read_ctrl(CHIMP_REG_CTRL_BPE_MODE_REG);
+		if ((value & CHIMP_BPE_MODE_ID_MASK) ==
+				CHIMP_BPE_MODE_ID_PATTERN)
+			break;
+		udelay(1);
+	} while (--timeout);
+
+	if (timeout == 0) {
+		ERROR("NIC reset release timed out\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+static void bcm_nitro_secure_mode_enable(void)
+{
+	mmio_setbits_32(CDRU_NITRO_CONTROL,
+		(1 << CDRU_NITRO_CONTROL__CDRU_NITRO_SEC_MODE_R) |
+		(1 << CDRU_NITRO_CONTROL__CDRU_NITRO_SEC_OVERRIDE_R));
+	mmio_write_32(NITRO_TZPC_TZPCDECPROT0clr,
+		/* NITRO_TZPC */
+		1 << NITRO_TZPC_TZPCDECPROT0clr__DECPROT0_chimp_m_clr_R);
+}
+
+static int bcm_chimp_reset_and_initial_setup(void)
+{
+
+	int err;
+	uint32_t handshake_reg;
+
+	err = bcm_chimp_nitro_reset();
+	if (err)
+		return err;
+
+	/* Enable Nitro secure mode */
+	bcm_nitro_secure_mode_enable();
+
+	/* Force ChiMP back into reset */
+	bcm_chimp_setbits(CHIMP_CTRL_ADDR(CHIMP_REG_CTRL_BPE_MODE_REG),
+		1 << CHIMP_REG_CHIMP_REG_CTRL_BPE_MODE_REG__cm3_rst_R);
+
+	handshake_reg = (1 << SR_IN_SMARTNIC_MODE_BIT);
+
+	/* Get OTP secure Chimp boot status */
+	if (mmio_read_32(CRMU_OTP_STATUS) & (1 << CRMU_OTP_STATUS_BIT))
+		handshake_reg |= (1 << SR_CHIMP_SECURE_BOOT_BIT);
+
+	bcm_chimp_write(CHIMP_REG_ECO_RESERVED, handshake_reg);
+
+	CHIMP_DBG("ChiMP reset and initial handshake parameters set\n");
+
+	return 0;
+}
+
+static void bcm_nitro_chimp_release_reset(void)
+{
+	bcm_chimp_clrbits(CHIMP_CTRL_ADDR(CHIMP_REG_CTRL_BPE_MODE_REG),
+		1 << CHIMP_REG_CHIMP_REG_CTRL_BPE_MODE_REG__cm3_rst_R);
+
+	CHIMP_DBG("Nitro Reset Released\n");
+}
+
+static void bcm_chimp_set_fastboot(int mode)
+{
+	uint32_t fb_entry;
+
+	/* 1. Enable fastboot */
+	bcm_chimp_setbits(CHIMP_CTRL_ADDR(CHIMP_REG_CTRL_BPE_MODE_REG),
+			(1 << CHIMP_FAST_BOOT_MODE_BIT));
+	fb_entry = CHIMP_FASTBOOT_ADDR | mode;
+	if (mode == CHIMP_FASTBOOT_JUMP_IN_PLACE)
+		fb_entry = CHIMP_FB1_ENTRY;
+	/* 2. Write startup address and mode */
+	INFO("Setting fastboot type %d entry to 0x%x\n", mode, fb_entry);
+	bcm_chimp_write(
+			CHIMP_CTRL_ADDR(CHIMP_REG_CTRL_FSTBOOT_PTR_REG),
+			fb_entry);
+}
+
+#ifndef CHIMPFW_USE_SIDELOAD
+static void bcm_chimp_load_fw_from_spi(uintptr_t spi_addr, size_t size)
+{
+	uintptr_t ape_scpad;
+	uintptr_t dest;
+	size_t bytes_left;
+
+	ape_scpad = CHIMP_REG_CHIMP_APE_SCPAD;
+	dest = CHIMP_INDIRECT_TGT_ADDR(CHIMP_REG_CHIMP_APE_SCPAD);
+	bytes_left = size;
+
+	while (bytes_left) {
+		uint32_t delta;
+
+		delta = bytes_left > CHIMP_WINDOW_SIZE ?
+			bytes_left - CHIMP_WINDOW_SIZE : bytes_left;
+		CHIMP_PREPARE_ACCESS_WINDOW(ape_scpad);
+		INFO("Transferring %d byte(s) from 0x%lx to 0x%lx\n",
+			delta, spi_addr, dest);
+	/*
+	 * This single memcpy call takes significant amount of time
+	 * on Palladium. Be patient
+	 */
+		memcpy((void *)dest, (void *)spi_addr, delta);
+		bytes_left -= delta;
+		INFO("Transferred %d byte(s) from 0x%lx to 0x%lx (%lu%%)\n",
+			delta, spi_addr, dest,
+			((size - bytes_left) * 100)/size);
+		spi_addr += delta;
+		dest += delta;
+		ape_scpad += delta;
+	}
+}
+
+static int bcm_chimp_find_fw_in_spi(uintptr_t *addr, size_t *size)
+{
+	int i;
+	bnxnvm_master_block_header_t *master_block_hdr;
+	bnxnvm_directory_block_header_t *dir_block_hdr;
+	bnxnvm_directory_entry_t *dir_entry;
+	int found;
+
+	found = 0;
+
+	/* Read the master block */
+	master_block_hdr =
+		(bnxnvm_master_block_header_t *)(uintptr_t)QSPI_BASE_ADDR;
+	if (master_block_hdr->sig != BNXNVM_MASTER_BLOCK_SIG) {
+		WARN("Invalid masterblock 0x%x (expected 0x%x)\n",
+			master_block_hdr->sig,
+			BNXNVM_MASTER_BLOCK_SIG);
+		return -NV_NOT_NVRAM;
+	}
+	if ((master_block_hdr->block_size > NV_MAX_BLOCK_SIZE) ||
+		(master_block_hdr->directory_offset >=
+			master_block_hdr->nvram_size)) {
+		WARN("Invalid masterblock block size 0x%x or directory offset 0x%x\n",
+			master_block_hdr->block_size,
+			master_block_hdr->directory_offset);
+		return -NV_BAD_MB;
+	}
+
+	/* Skip to the Directory block start */
+	dir_block_hdr =
+		(bnxnvm_directory_block_header_t *)
+			((uintptr_t)QSPI_BASE_ADDR +
+				master_block_hdr->directory_offset);
+	if (dir_block_hdr->sig != BNXNVM_DIRECTORY_BLOCK_SIG) {
+		WARN("Invalid directory header 0x%x (expected 0x%x)\n",
+			dir_block_hdr->sig,
+			BNXNVM_DIRECTORY_BLOCK_SIG);
+		return -NV_BAD_DIR_HEADER;
+	}
+
+	/* Locate the firmware */
+	for (i = 0; i < dir_block_hdr->entries; i++) {
+		*addr = ((uintptr_t)dir_block_hdr + dir_block_hdr->length +
+			i * dir_block_hdr->entry_length);
+		dir_entry = (bnxnvm_directory_entry_t *)(*addr);
+		if ((dir_entry->type == BNX_DIR_TYPE_BOOTCODE) ||
+				(dir_entry->type == BNX_DIR_TYPE_BOOTCODE_2)) {
+			found = 1;
+			break;
+		}
+	}
+
+	if (!found)
+		return -NV_FW_NOT_FOUND;
+
+	*addr = QSPI_BASE_ADDR + dir_entry->item_location;
+	*size = dir_entry->data_length;
+
+	INFO("Found chimp firmware at 0x%lx, size %lu byte(s)\n",
+			*addr, *size);
+
+	return NV_OK;
+}
+#endif
+
+int bcm_chimp_initiate_fastboot(int fastboot_type)
+{
+	int err;
+
+	if ((fastboot_type != CHIMP_FASTBOOT_NITRO_RESET) &&
+			(fastboot_type <= CHIMP_FASTBOOT_JUMP_DECOMPRESS)) {
+		CHIMP_DBG("Initiating ChiMP fastboot type %d\n", fastboot_type);
+	}
+
+	/*
+	 * If we are here, M0 did not setup Nitro because NIC mode
+	 * strap was not present
+	 */
+	err = bcm_chimp_reset_and_initial_setup();
+	if (err)
+		return err;
+
+	if (fastboot_type > CHIMP_FASTBOOT_JUMP_DECOMPRESS) {
+		WARN("ChiMP setup deferred\n");
+		return -1;
+	}
+
+	if (fastboot_type != CHIMP_FASTBOOT_NITRO_RESET) {
+
+		if ((fastboot_type == CHIMP_FASTBOOT_JUMP_IN_PLACE) &&
+			(CHIMP_FB1_ENTRY == 0)) {
+			ERROR("Missing ESAL entry point for fastboot type 1.\n"
+			"Fastboot failed\n");
+			return -1;
+		}
+
+		/*
+		 * TODO: We need to think of the way to load the ChiMP fw.
+		 * This could be SPI, NAND, etc.
+		 * For now we temporarily stick to the SPI load unless
+		 * CHIMPFW_USE_SIDELOAD is defined. Note that for the SPI NVRAM
+		 * image we need to parse directory and get the image.
+		 * When we load image from other media there is no need to
+		 * parse because fw image can be directly placed into the APE's
+		 * scratchpad.
+		 * For sideload method we simply reset the ChiMP, set bpe_reg
+		 * to do fastboot with the type we define, and release from
+		 * reset so that ROM loader would initiate fastboot immediately
+		 */
+#ifndef CHIMPFW_USE_SIDELOAD
+		{
+			uintptr_t spi_addr;
+			size_t size;
+
+			err = bcm_chimp_find_fw_in_spi(&spi_addr, &size);
+			if (!err) {
+				INFO("Loading ChiMP firmware, addr 0x%lx, size %lu byte(s)\n",
+					spi_addr, size);
+				bcm_chimp_load_fw_from_spi(spi_addr, size);
+			} else {
+				ERROR("Error %d ChiMP firmware not in NVRAM directory!\n",
+					err);
+			}
+		}
+#else
+		INFO("Skip ChiMP QSPI fastboot type %d due to sideload requested\n",
+		     fastboot_type);
+#endif
+		if (!err) {
+			INFO("Instruct ChiMP to fastboot\n");
+			bcm_chimp_set_fastboot(fastboot_type);
+			INFO("Fastboot mode set\n");
+		}
+	}
+
+	bcm_nitro_chimp_release_reset();
+
+	return err;
+}
diff --git a/drivers/brcm/emmc/emmc_chal_sd.c b/drivers/brcm/emmc/emmc_chal_sd.c
new file mode 100644
index 0000000..34d761c
--- /dev/null
+++ b/drivers/brcm/emmc/emmc_chal_sd.c
@@ -0,0 +1,1017 @@
+/*
+ * Copyright (c) 2016 - 2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+#include "bcm_emmc.h"
+#include "emmc_chal_types.h"
+#include "emmc_chal_sd.h"
+#include "emmc_pboot_hal_memory_drv.h"
+
+extern void emmc_soft_reset(void);
+
+#define SD_VDD_WINDOW_1_6_TO_1_7        0x00000010	// 1.6 V to 1.7 Volts
+#define SD_VDD_WINDOW_1_7_TO_1_8        0x00000020	// 1.7 V to 1.8 Volts
+#define SD_VDD_WINDOW_1_8_TO_1_9        0x00000040	// 1.8 V to 1.9 Volts
+#define SD_VDD_WINDOW_1_9_TO_2_0        0x00000080	// 1.9 V to 2.0 Volts
+#define SD_VDD_WINDOW_2_0_TO_2_1        0x00000100	// 2.0 V to 2.1 Volts
+#define SD_VDD_WINDOW_2_1_TO_2_2        0x00000200	// 2.1 V to 2.2 Volts
+#define SD_VDD_WINDOW_2_2_TO_2_3        0x00000400	// 2.2 V to 2.3 Volts
+#define SD_VDD_WINDOW_2_3_TO_2_4        0x00000800	// 2.3 V to 2.4 Volts
+#define SD_VDD_WINDOW_2_4_TO_2_5        0x00001000	// 2.4 V to 2.5 Volts
+#define SD_VDD_WINDOW_2_5_TO_2_6        0x00002000	// 2.5 V to 2.6 Volts
+#define SD_VDD_WINDOW_2_6_TO_2_7        0x00004000	// 2.6 V to 2.7 Volts
+#define SD_VDD_WINDOW_2_7_TO_2_8        0x00008000	// 2.7 V to 2.8 Volts
+#define SD_VDD_WINDOW_2_8_TO_2_9        0x00010000	// 2.8 V to 2.9 Volts
+#define SD_VDD_WINDOW_2_9_TO_3_0        0x00020000	// 2.9 V to 3.0 Volts
+#define SD_VDD_WINDOW_3_0_TO_3_1        0x00040000	// 3.0 V to 3.1 Volts
+#define SD_VDD_WINDOW_3_1_TO_3_2        0x00080000	// 3.1 V to 3.2 Volts
+#define SD_VDD_WINDOW_3_2_TO_3_3        0x00100000	// 3.2 V to 3.3 Volts
+#define SD_VDD_WINDOW_3_3_TO_3_4        0x00200000	// 3.3 V to 3.4 Volts
+#define SD_VDD_WINDOW_3_4_TO_3_5        0x00400000	// 3.4 V to 3.5 Volts
+#define SD_VDD_WINDOW_3_5_TO_3_6        0x00800000	// 3.5 V to 3.6 Volts
+
+#define SD_VDD_WINDOW_1_6_TO_2_6        (SD_VDD_WINDOW_1_6_TO_1_7 |	\
+					 SD_VDD_WINDOW_1_7_TO_1_8 |	\
+					 SD_VDD_WINDOW_1_8_TO_1_9 |	\
+					 SD_VDD_WINDOW_1_9_TO_2_0 |	\
+					 SD_VDD_WINDOW_2_0_TO_2_1 |	\
+					 SD_VDD_WINDOW_2_1_TO_2_2 |	\
+					 SD_VDD_WINDOW_2_2_TO_2_3 |	\
+					 SD_VDD_WINDOW_2_3_TO_2_4 |	\
+					 SD_VDD_WINDOW_2_4_TO_2_5 |	\
+					 SD_VDD_WINDOW_2_5_TO_2_6)
+
+#define SD_VDD_WINDOW_2_6_TO_3_2        (SD_VDD_WINDOW_2_6_TO_2_7 |	\
+					 SD_VDD_WINDOW_2_7_TO_2_8 |	\
+					 SD_VDD_WINDOW_2_8_TO_2_9 |	\
+					 SD_VDD_WINDOW_2_9_TO_3_0 |	\
+					 SD_VDD_WINDOW_3_0_TO_3_1 |	\
+					 SD_VDD_WINDOW_3_1_TO_3_2)
+
+#define SD_VDD_WINDOW_3_2_TO_3_6        (SD_VDD_WINDOW_3_2_TO_3_3 |	\
+					 SD_VDD_WINDOW_3_3_TO_3_4 |	\
+					 SD_VDD_WINDOW_3_4_TO_3_5 |	\
+					 SD_VDD_WINDOW_3_5_TO_3_6)
+
+
+static int32_t chal_sd_set_power(struct sd_dev *handle,
+				 uint32_t voltage, uint32_t state);
+
+static void chal_sd_set_dma_boundary(struct sd_dev *handle, uint32_t boundary);
+
+static int32_t chal_sd_setup_handler(struct sd_dev *handle,
+				     uint32_t sdBbase, uint32_t hostBase);
+
+/*
+ * Configure host controller pwr settings,
+ * to match voltage requirements by SD Card
+ */
+static int32_t chal_sd_set_power(struct sd_dev *handle,
+				 uint32_t voltage, uint32_t state)
+{
+	int32_t rc, rval = SD_FAIL;
+	uint32_t time = 0;
+
+	if (handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	mmio_clrsetbits_32(handle->ctrl.sdRegBaseAddr +
+			   SD4_EMMC_TOP_CTRL_OFFSET,
+			   (SD4_EMMC_TOP_CTRL_SDVSELVDD1_MASK |
+			    SD4_EMMC_TOP_CTRL_SDPWR_MASK),
+			   (voltage << 9));
+
+	/*
+	 * Long delay is required here in emulation.  Without this, the initial
+	 * commands sent to the eMMC card timeout.  We don't know if this
+	 * delay is necessary with silicon, leaving in for safety.
+	 * It is observed that 403ms on emulation system and as per the clock
+	 * calculations it is expected to complete with in 1ms on chip
+	 */
+	do {
+		rc =  mmio_read_32(handle->ctrl.sdRegBaseAddr +
+				   SD4_EMMC_TOP_INTR_OFFSET);
+
+		if ((rc & SD4_EMMC_TOP_INTR_CRDINS_MASK) ==
+				SD4_EMMC_TOP_INTR_CRDINS_MASK)
+			break;
+
+		mdelay(1);
+	} while (time++ < EMMC_CARD_DETECT_TIMEOUT_MS);
+
+	if (time >= EMMC_CARD_DETECT_TIMEOUT_MS) {
+		ERROR("EMMC: Card insert event detection timeout\n");
+		return rval;
+	}
+
+	VERBOSE("EMMC: Card detection delay: %dms\n", time);
+
+	if (state)
+		mmio_setbits_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_CTRL_OFFSET,
+				SD4_EMMC_TOP_CTRL_SDPWR_MASK);
+
+	/* dummy write & ack to verify if the sdio is ready to send commads */
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_ARG_OFFSET, 0);
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_CMD_OFFSET, 0);
+
+	/*
+	 * 63ms observed on emulation system, As per clock calculations
+	 * it will complete  < 1ms on chip.
+	 */
+	time = 0;
+	do {
+		rc = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+				  SD4_EMMC_TOP_INTR_OFFSET);
+
+		if (rc & SD4_EMMC_TOP_INTR_ERRIRQ_MASK)
+			break;
+
+		if ((rc & SD4_EMMC_TOP_INTR_CMDDONE_MASK) ==
+				SD4_EMMC_TOP_INTR_CMDDONE_MASK)
+			break;
+
+		mdelay(1);
+	} while (time++ < EMMC_CMD_TIMEOUT_MS);
+
+	if (time >= EMMC_CMD_TIMEOUT_MS) {
+		WARN("%s %d Initial dummy command timeout is happened\n",
+		      __func__, __LINE__);
+		return rval;
+	}
+
+	VERBOSE("EMMC: Dummy Command delay: %dms\n", time);
+
+	return SD_OK;
+}
+
+/*
+ * Configure DMA Boundaries
+ */
+static void chal_sd_set_dma_boundary(struct sd_dev *handle, uint32_t boundary)
+{
+	if (handle == NULL)
+		return;
+
+	mmio_clrsetbits_32(handle->ctrl.sdRegBaseAddr +
+			   SD4_EMMC_TOP_BLOCK_OFFSET,
+			   SD4_EMMC_TOP_BLOCK_HSBS_MASK, boundary);
+}
+
+static int32_t chal_sd_setup_handler(struct sd_dev *handle, uint32_t sdBase,
+				     uint32_t hostBase)
+{
+	if (handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle->ctrl.sdRegBaseAddr = sdBase;
+	handle->ctrl.hostRegBaseAddr = hostBase;
+	handle->ctrl.present = 0;
+	handle->ctrl.rca = 0;
+	handle->ctrl.blkGapEnable = 0;
+	handle->ctrl.cmdStatus = 0;
+
+	return SD_OK;
+}
+
+/*
+ * Initialize SD Host controller
+ */
+int32_t chal_sd_init(CHAL_HANDLE *sd_handle)
+{
+	uint32_t cap_val_l = 0;
+	uint32_t ctl_val, voltage;
+	uint32_t timeout_val;
+	struct sd_dev *handle;
+	uint32_t reg_val;
+	int32_t rval = SD_FAIL;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *)sd_handle;
+
+	/*
+	 * Set SDIO Host Controller capabilities register
+	 */
+	EMMC_TRACE("Set Host Controller Capabilities register\n");
+
+	reg_val = 0;
+	reg_val |= (1 << ICFG_SDIO0_CAP0__SLOT_TYPE_R);
+	reg_val |= (0 << ICFG_SDIO0_CAP0__INT_MODE_R);
+	reg_val |= (0 << ICFG_SDIO0_CAP0__SYS_BUS_64BIT_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP0__VOLTAGE_1P8V_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP0__VOLTAGE_3P0V_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP0__VOLTAGE_3P3V_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP0__SUSPEND_RESUME_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP0__SDMA_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP0__HIGH_SPEED_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP0__ADMA2_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP0__EXTENDED_MEDIA_R);
+	reg_val |= (2 << ICFG_SDIO0_CAP0__MAX_BLOCK_LEN_R);
+	reg_val |= (0xd0 << ICFG_SDIO0_CAP0__BASE_CLK_FREQ_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP0__TIMEOUT_UNIT_R);
+	reg_val |= (0x30 << ICFG_SDIO0_CAP0__TIMEOUT_CLK_FREQ_R);
+
+	mmio_write_32(ICFG_SDIO0_CAP0, reg_val);
+
+	reg_val = 0;
+	reg_val |= (1 << ICFG_SDIO0_CAP1__SPI_BLOCK_MODE_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP1__SPI_MODE_R);
+	reg_val |= (0 << ICFG_SDIO0_CAP1__CLK_MULT_R);
+	reg_val |= (0 << ICFG_SDIO0_CAP1__RETUNING_MODE_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP1__TUNE_SDR50_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP1__TIME_RETUNE_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP1__DRIVER_D_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP1__DRIVER_C_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP1__DRIVER_A_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP1__DDR50_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP1__SDR104_R);
+	reg_val |= (1 << ICFG_SDIO0_CAP1__SDR50_R);
+
+	mmio_write_32(ICFG_SDIO0_CAP1, reg_val);
+
+	/* Reset the SDIO controller */
+	chal_sd_stop();
+
+	/* Turn on SD clock */
+	chal_sd_set_clock(sd_handle,
+			  chal_sd_freq_2_div_ctrl_setting(INIT_CLK_FREQ), 1);
+
+	/* program data time out value to the max */
+	timeout_val = SD_HOST_CORE_TIMEOUT;
+
+	ctl_val = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			       SD4_EMMC_TOP_CTRL1_OFFSET);
+	ctl_val |= ((timeout_val & 0xf) << SD4_EMMC_TOP_CTRL1_DTCNT_SHIFT);
+
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_CTRL1_OFFSET,
+		      ctl_val);
+
+	/* enable all interrupt status */
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_INTREN1_OFFSET,
+		      0);
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_INTREN2_OFFSET,
+		      0);
+
+	SD_US_DELAY(100);
+
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_INTREN1_OFFSET,
+		      SD_NOR_INTERRUPTS | SD_ERR_INTERRUPTS);
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_INTREN2_OFFSET,
+		      SD_NOR_INTERRUPTS | SD_ERR_INTERRUPTS);
+
+	/* Select SD bus voltage */
+	cap_val_l = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+				 SD4_EMMC_TOP_CAPABILITIES1_OFFSET);
+	handle->cfg.voltage = 0;
+	voltage = 0x7;
+
+	if (cap_val_l & SD4_EMMC_TOP_CAPABILITIES1_V33_MASK) {
+		handle->cfg.voltage |= SD_VDD_WINDOW_3_3_TO_3_4;
+		voltage = 0x7;
+	} else if (cap_val_l & SD4_EMMC_TOP_CAPABILITIES1_V3_MASK) {
+		handle->cfg.voltage |= SD_VDD_WINDOW_3_0_TO_3_1;
+		voltage = 0x6;
+	} else if (cap_val_l & SD4_EMMC_TOP_CAPABILITIES1_V18_MASK) {
+		handle->cfg.voltage |= SD_VDD_WINDOW_1_8_TO_1_9;
+		voltage = 0x5;
+	}
+
+	rval = chal_sd_set_power(handle, voltage, SD4_EMMC_TOP_CTRL_SDPWR_MASK);
+
+	ctl_val = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			       SD4_EMMC_TOP_HCVERSIRQ_OFFSET);
+	handle->ctrl.version = ((ctl_val >> 16) & 0xFF);
+
+	return rval;
+}
+
+void chal_sd_set_speed(CHAL_HANDLE *sd_handle, uint32_t speed)
+{
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	if (speed) {
+		EMMC_TRACE("enable HighSpeed\n");
+		mmio_setbits_32(handle->ctrl.sdRegBaseAddr +
+				SD4_EMMC_TOP_CTRL_OFFSET,
+				SD4_EMMC_TOP_CTRL_HSEN_MASK);
+	} else {
+		EMMC_TRACE("disable HighSpeed\n");
+		mmio_clrbits_32(handle->ctrl.sdRegBaseAddr +
+				SD4_EMMC_TOP_CTRL_OFFSET,
+				SD4_EMMC_TOP_CTRL_HSEN_MASK);
+	}
+}
+
+int32_t chal_sd_stop(void)
+{
+	uintptr_t idm_rst_ctrl_addr = EMMC_IDM_RESET_CTRL_ADDR;
+
+	/* Configure IO pins */
+	emmc_soft_reset();
+
+	/* Reset the SDIO controller */
+	mmio_write_32(idm_rst_ctrl_addr, 1);
+	SD_US_DELAY(100);
+	mmio_write_32(idm_rst_ctrl_addr, 0);
+	SD_US_DELAY(100);
+
+	return SD_OK;
+}
+
+/*
+ * Check if host supports specified capability
+ * returns -ve val on error, 0 if capability not supported else 1.
+ */
+int32_t chal_sd_check_cap(CHAL_HANDLE *sd_handle, uint32_t caps)
+{
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	if (caps & mmio_read_32(handle->ctrl.sdRegBaseAddr +
+				SD4_EMMC_TOP_CAPABILITIES1_OFFSET))
+		return 1;
+	else
+		return 0;
+}
+
+int32_t chal_sd_start(CHAL_HANDLE *sd_handle,
+		      uint32_t mode, uint32_t sd_base, uint32_t host_base)
+{
+
+	struct sd_dev *handle;
+	int32_t rval = SD_FAIL;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	handle->cfg.mode = SD_PIO_MODE;	/* set to PIO mode first for init */
+	handle->cfg.dma = SD_DMA_OFF;
+
+	chal_sd_setup_handler(handle, sd_base, host_base);
+
+	/* init and start hw */
+	rval = chal_sd_init(sd_handle);
+	if (rval != SD_OK)
+		return rval;
+
+	chal_sd_clear_pending_irq(sd_handle);
+
+	handle->ctrl.eventList = 0;
+	handle->cfg.mode = mode;
+
+	return SD_OK;
+}
+
+/*
+ * Function to check 8bits of err generated from auto CMD12
+ */
+int32_t chal_sd_get_atuo12_error(CHAL_HANDLE *sd_handle)
+{
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	return (mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			     SD4_EMMC_TOP_ERRSTAT_OFFSET) & 0xFF);
+}
+
+/*
+ * Read present state register
+ */
+uint32_t chal_sd_get_present_status(CHAL_HANDLE *sd_handle)
+{
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	return mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			    SD4_EMMC_TOP_PSTATE_OFFSET);
+}
+
+/*
+ * Set SD bus width
+ */
+int32_t chal_sd_config_bus_width(CHAL_HANDLE *sd_handle, int32_t width)
+{
+	uint32_t ctl_val;
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *)sd_handle;
+
+	ctl_val = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			       SD4_EMMC_TOP_CTRL_OFFSET);
+
+	switch (width) {
+#ifdef DRIVER_EMMC_ENABLE_DATA_WIDTH_8BIT
+	case SD_BUS_DATA_WIDTH_8BIT:
+		ctl_val &= ~SD_BUS_DATA_WIDTH_4BIT;
+		ctl_val |= SD_BUS_DATA_WIDTH_8BIT;
+		break;
+#endif
+	case SD_BUS_DATA_WIDTH_4BIT:
+		ctl_val &= ~SD_BUS_DATA_WIDTH_8BIT;
+		ctl_val |= SD_BUS_DATA_WIDTH_4BIT;
+		break;
+	case SD_BUS_DATA_WIDTH_1BIT:
+		ctl_val &= ~(SD_BUS_DATA_WIDTH_4BIT | SD_BUS_DATA_WIDTH_8BIT);
+		break;
+	default:
+		return SD_INV_DATA_WIDTH;
+	};
+
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_CTRL_OFFSET,
+		      ctl_val);
+
+	return SD_OK;
+}
+
+/*
+ * Function to enable or disable DMA control.
+ */
+int32_t chal_sd_set_dma(CHAL_HANDLE *sd_handle, uint32_t mode)
+{
+	uint32_t val;
+	struct sd_dev *handle;
+	int32_t rc;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *)sd_handle;
+
+	if (mode) {
+		rc = chal_sd_check_cap(sd_handle,
+				       SD4_EMMC_TOP_CAPABILITIES1_SDMA_MASK |
+				       SD4_EMMC_TOP_CAPABILITIES1_ADMA2_MASK);
+		if (rc < 0)
+			return rc;
+
+		if (rc) {
+
+			handle->cfg.dma = mode;
+			val = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+					   SD4_EMMC_TOP_CTRL_OFFSET);
+			val &= ~(SD4_EMMC_TOP_CTRL_DMASEL_MASK);
+			val |= handle->cfg.dma - 1;
+			mmio_write_32(handle->ctrl.sdRegBaseAddr +
+				      SD4_EMMC_TOP_CTRL_OFFSET, val);
+			return SD_OK;
+		}
+	}
+	handle->cfg.dma = 0;
+
+	return SD_FAIL;
+}
+
+/*
+ * Get current DMA address.
+ * Called only when there is no data transaction activity.
+ */
+uintptr_t chal_sd_get_dma_addr(CHAL_HANDLE *sd_handle)
+{
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	if (handle->cfg.dma == SD_DMA_OFF)
+		return 0;
+
+	return (uintptr_t)mmio_read_32(handle->ctrl.sdRegBaseAddr +
+				       SD4_EMMC_TOP_SYSADDR_OFFSET);
+}
+
+int32_t chal_sd_send_cmd(CHAL_HANDLE *sd_handle, uint32_t cmd_idx,
+			 uint32_t argument, uint32_t options)
+{
+	uint32_t cmd_mode_reg = 0;
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	EMMC_TRACE("%s %d cmd:%d argReg:%x options:%x\n",
+		   __func__, __LINE__, cmd_idx, argument, options);
+
+	/* Configure the value for command and mode registers */
+	cmd_mode_reg = (cmd_idx << 24) | options;
+
+	/*
+	 * 1. Write block size reg & block count reg,
+	 * this is done in the tx or rx setup
+	 */
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_BLOCK_OFFSET,
+		      handle->ctrl.blkReg);
+
+	/* 2. Write argument reg */
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_ARG_OFFSET,
+		      argument);
+	handle->ctrl.argReg = argument;
+
+	/*
+	 * 3. Write transfer mode reg & command reg, check the DMA bit which is
+	 *    set before this function call if it is selected.
+	 */
+	if (cmd_idx == 24 || cmd_idx == 25 || cmd_idx == 18 || cmd_idx == 17 ||
+	    cmd_idx == 42 || cmd_idx == 51 || cmd_idx == 53)
+		cmd_mode_reg |= ((handle->cfg.dma) ? 1 : 0);
+
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_CMD_OFFSET,
+		      cmd_mode_reg);
+
+	handle->ctrl.cmdIndex = cmd_idx;
+
+	return SD_OK;
+}
+
+int32_t chal_sd_set_dma_addr(CHAL_HANDLE *sd_handle, uintptr_t address)
+{
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	if (handle->cfg.dma == SD_DMA_OFF)
+		return SD_FAIL;
+
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_SYSADDR_OFFSET,
+		      address);
+	return SD_OK;
+}
+
+uint32_t chal_sd_freq_2_div_ctrl_setting(uint32_t desired_freq)
+{
+	/*
+	 * Divider control setting represents 1/2 of the actual divider value.
+	 *
+	 * DesiredFreq = BaseClockFreq / (2 * div_ctrl_setting)
+	 *
+	 * ==> div_ctrl_setting = BaseClockFreq / (2 * DesiredFreq)
+	 */
+	uint32_t div_ctrl_setting;
+	uint32_t actual_freq;
+
+	assert(desired_freq != 0);
+
+	/* Special case, 0 = divider of 1. */
+	if (desired_freq >= BASE_CLK_FREQ)
+		return 0;
+
+	/* Normal case, desired_freq < BASE_CLK_FREQ */
+	div_ctrl_setting = BASE_CLK_FREQ / (2 * desired_freq);
+
+	actual_freq = BASE_CLK_FREQ / (2 * div_ctrl_setting);
+
+	if (actual_freq > desired_freq) {
+		/*
+		 * Division does not result in exact freqency match.
+		 * Make sure resulting frequency does not exceed requested freq.
+		 */
+		div_ctrl_setting++;
+	}
+
+	return div_ctrl_setting;
+}
+
+int32_t chal_sd_set_clock(CHAL_HANDLE *sd_handle, uint32_t div_ctrl_setting,
+			  uint32_t on)
+{
+	uint32_t value;
+	struct sd_dev *handle;
+	uint32_t time;
+	uint32_t clk_sel_high_byte = 0xFF & (div_ctrl_setting >> 8);
+	uint32_t clk_sel_low_byte = 0xFF & div_ctrl_setting;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	EMMC_TRACE("set_clock(div_ctrl_setting=%d,on=%d)\n",
+		   div_ctrl_setting, on);
+
+	handle = (struct sd_dev *) sd_handle;
+
+	/* Read control register content. */
+	value = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			     SD4_EMMC_TOP_CTRL1_OFFSET);
+
+	/* Disable Clock */
+	value &= ~(SD4_EMMC_TOP_CTRL1_SDCLKEN_MASK);
+
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_CTRL1_OFFSET,
+		      value);
+
+	/* Clear bits of interest. */
+	value &= ~(SD4_EMMC_TOP_CTRL1_SDCLKSEL_MASK |
+		   SD4_EMMC_TOP_CTRL1_SDCLKSEL_UP_MASK);
+
+	/* Set bits of interest to new value. */
+	value |= (SD4_EMMC_TOP_CTRL1_SDCLKSEL_MASK &
+		  (clk_sel_low_byte << SD4_EMMC_TOP_CTRL1_SDCLKSEL_SHIFT));
+	value |= (SD4_EMMC_TOP_CTRL1_SDCLKSEL_UP_MASK &
+		  (clk_sel_high_byte << SD4_EMMC_TOP_CTRL1_SDCLKSEL_UP_SHIFT));
+	value |= SD4_EMMC_TOP_CTRL1_ICLKEN_MASK;
+
+	/* Write updated value back to control register. */
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_CTRL1_OFFSET,
+		      value);
+
+	time = 0;
+	do {
+		value = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+				     SD4_EMMC_TOP_CTRL1_OFFSET);
+
+		if ((value & SD4_EMMC_TOP_CTRL1_ICLKSTB_MASK) ==
+				SD4_EMMC_TOP_CTRL1_ICLKSTB_MASK)
+			break;
+
+		mdelay(1);
+	} while (time++ < EMMC_CLOCK_SETTING_TIMEOUT_MS);
+
+	if (time >= EMMC_CLOCK_SETTING_TIMEOUT_MS)
+		WARN("%s %d clock settings timeout happenedi (%dms)\n",
+			 __func__, __LINE__, time);
+
+	VERBOSE("EMMC: clock settings delay: %dms\n", time);
+
+	value = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			     SD4_EMMC_TOP_CTRL1_OFFSET);
+
+	if (on)
+		value |= SD4_EMMC_TOP_CTRL1_SDCLKEN_MASK;
+
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_CTRL1_OFFSET,
+		      value);
+
+	return SD_OK;
+}
+
+/*
+ * function to setup DMA buffer and data length, calculates block
+ * size and the number of blocks to be transferred and return
+ * the DMA buffer address.
+ */
+int32_t chal_sd_setup_xfer(CHAL_HANDLE *sd_handle,
+			   uint8_t *data, uint32_t length, int32_t dir)
+{
+	uint32_t blocks = 0;
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	if (length <= handle->cfg.blockSize) {
+		handle->ctrl.blkReg = length | handle->cfg.dmaBoundary;
+	} else {
+		blocks = length / handle->cfg.blockSize;
+		handle->ctrl.blkReg = (blocks << 16) | handle->cfg.blockSize |
+					handle->cfg.dmaBoundary;
+	}
+
+	if (handle->cfg.dma != SD_DMA_OFF) {
+		/* For DMA target address setting, physical address should be used */
+		mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_SYSADDR_OFFSET,
+				(uintptr_t)data);
+	}
+
+	return SD_OK;
+}
+
+#ifdef INCLUDE_EMMC_DRIVER_WRITE_CODE
+/*
+ * function to write one block data directly to the
+ * host controller's FIFO which is 1K uint8_t or
+ * 2K uint8_t in size.
+ * It is used in Non-DMA mode for data transmission.
+ */
+int32_t chal_sd_write_buffer(CHAL_HANDLE *sd_handle, uint32_t length,
+			     uint8_t *data)
+{
+	uint32_t i, leftOver = 0, blockSize, size, value = 0;
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	blockSize = handle->cfg.blockSize;
+
+	if (length == 0)
+		return SD_OK;
+
+	/* PIO mode, push into fifo word by word */
+	if (length >= blockSize) {
+		size = blockSize;
+	} else {
+		size = ((length >> 2) << 2);
+		leftOver = length % 4;
+	}
+
+	for (i = 0; i < size; i += 4) {
+		value = *(uint32_t *)(data + i);
+		mmio_write_32(handle->ctrl.sdRegBaseAddr +
+			      SD4_EMMC_TOP_BUFDAT_OFFSET, value);
+	}
+/*
+ * BUG ALERT:
+ *    This implementation has TWO issues that must be addressed before you
+ *    can safely INCLUDE_EMMC_DRIVER_WRITE_CODE.
+ *
+ *    (1) For the last leftOver bytes, driver writes full word, which means
+ *        some of the eMMC content (i.e. "4 - leftOver" will be erroneously
+ *        overwritten).
+ *    (2) eMMC is a block device. What happens when less than a full block of
+ *        data is submitted???
+ */
+	if (leftOver > 0) {
+		value = ((*(uint32_t *)(data + i)) << (4 - leftOver));
+		mmio_write_32(handle->ctrl.sdRegBaseAddr +
+			      SD4_EMMC_TOP_BUFDAT_OFFSET, value);
+	}
+
+	return SD_OK;
+}
+#endif /* INCLUDE_EMMC_DRIVER_WRITE_CODE */
+
+/*
+ * Function to read maximal one block data directly
+ * from the data port of the host controller (FIFO). It is used
+ * in Non-DMA mode for data transmission.
+ */
+int32_t chal_sd_read_buffer(CHAL_HANDLE *sd_handle, uint32_t length,
+			    uint8_t *data)
+{
+	uint32_t i, size, leftOver, blockSize, value;
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *)sd_handle;
+
+	value = 0;
+
+	blockSize = handle->cfg.blockSize;
+
+	/* PIO mode, extract fifo word by word */
+	if (length >= blockSize) {
+		size = blockSize;
+		leftOver = 0;
+	} else {
+		leftOver = length % 4;
+		size = ((length >> 2) << 2);
+	}
+
+	for (i = 0; i < size; i += 4) {
+		value =
+		    mmio_read_32(handle->ctrl.sdRegBaseAddr +
+				    SD4_EMMC_TOP_BUFDAT_OFFSET);
+		memcpy((void *)(data + i), &value, sizeof(uint32_t));
+	}
+
+	if (leftOver > 0) {
+		value = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+				     SD4_EMMC_TOP_BUFDAT_OFFSET);
+
+		/*
+		 * Copy remaining non-full word bytes.
+		 * (We run ARM as Little Endian)
+		 */
+		uint8_t j = 0;
+
+		for (j = 0; j < leftOver; j++) {
+			data[i + j] = (value >> (j * 8)) & 0xFF;
+		}
+	}
+
+	return SD_OK;
+}
+
+/*
+ * Resets both DAT or CMD line.
+ */
+int32_t chal_sd_reset_line(CHAL_HANDLE *sd_handle, uint32_t line)
+{
+	uint32_t control, flag;
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	flag = SD4_EMMC_TOP_CTRL1_CMDRST_MASK | SD4_EMMC_TOP_CTRL1_DATRST_MASK;
+
+	if (flag != (line | flag))
+		return SD_FAIL;
+
+	control = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			       SD4_EMMC_TOP_CTRL1_OFFSET);
+	control |= line;
+	mmio_write_32(handle->ctrl.sdRegBaseAddr + SD4_EMMC_TOP_CTRL1_OFFSET,
+		      control);
+
+	/* reset CMD and DATA line should always work, no need to timed out */
+	do {
+		control = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+				       SD4_EMMC_TOP_CTRL1_OFFSET);
+	} while (control & line);
+
+	return SD_OK;
+}
+
+/*
+ * Function to be called once a SD command is done to read
+ * back it's response data.
+ */
+int32_t chal_sd_get_response(CHAL_HANDLE *sd_handle, uint32_t *resp)
+{
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+	resp[0] = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			       SD4_EMMC_TOP_RESP0_OFFSET);
+	resp[1] = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			       SD4_EMMC_TOP_RESP2_OFFSET);
+	resp[2] = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			       SD4_EMMC_TOP_RESP4_OFFSET);
+	resp[3] = mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			       SD4_EMMC_TOP_RESP6_OFFSET);
+
+	return SD_OK;
+}
+
+/*
+ * The function is called to clean all the pending interrupts.
+ */
+int32_t chal_sd_clear_pending_irq(CHAL_HANDLE *sd_handle)
+{
+	uint32_t status = SD_OK;
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *)sd_handle;
+
+	/* Make sure clean all interrupts */
+	do {
+		mmio_write_32(handle->ctrl.sdRegBaseAddr +
+			      SD4_EMMC_TOP_INTR_OFFSET, 0xFFFFFFFF);
+		SD_US_DELAY(10);
+	} while (mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			      SD4_EMMC_TOP_INTR_OFFSET));
+
+	return status;
+}
+
+/*
+ * The function returns interrupt status register value.
+ */
+int32_t chal_sd_get_irq_status(CHAL_HANDLE *sd_handle)
+{
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	return (mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			     SD4_EMMC_TOP_INTR_OFFSET));
+}
+
+/*
+ * The function clears interrupt(s) specified in the mask.
+ */
+int32_t chal_sd_clear_irq(CHAL_HANDLE *sd_handle, uint32_t mask)
+{
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	/* Make sure clean masked interrupts */
+	do {
+		mmio_write_32(handle->ctrl.sdRegBaseAddr +
+			      SD4_EMMC_TOP_INTR_OFFSET, mask);
+		SD_US_DELAY(10);
+	} while (mask &
+		 mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			      SD4_EMMC_TOP_INTR_OFFSET));
+
+	return SD_OK;
+}
+
+/*
+ * Description: The function configures the SD host controller.
+ */
+int32_t chal_sd_config(CHAL_HANDLE *sd_handle, uint32_t speed, uint32_t retry,
+		       uint32_t boundary, uint32_t blkSize, uint32_t dma)
+{
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return SD_INVALID_HANDLE;
+
+	handle = (struct sd_dev *) sd_handle;
+
+	handle->cfg.speedMode = speed;
+	handle->cfg.retryLimit = retry;
+	handle->cfg.dmaBoundary = boundary;
+	handle->cfg.blockSize = blkSize;
+
+	chal_sd_set_dma(sd_handle, dma);
+	SD_US_DELAY(100);
+	chal_sd_set_dma_boundary(handle, boundary);
+	SD_US_DELAY(100);
+
+	chal_sd_set_speed(sd_handle, speed);
+
+	SD_US_DELAY(100);
+	return SD_OK;
+}
+
+/*
+ * Cleans up HC FIFO.
+ */
+void chal_sd_dump_fifo(CHAL_HANDLE *sd_handle)
+{
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return;
+
+	handle = (struct sd_dev *)sd_handle;
+
+	/* in case there still data in the host buffer */
+	while (mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			    SD4_EMMC_TOP_PSTATE_OFFSET) & 0x800) {
+		mmio_read_32(handle->ctrl.sdRegBaseAddr +
+			     SD4_EMMC_TOP_BUFDAT_OFFSET);
+	};
+}
+
+/*
+ * Enable or disable a SD interrupt signal.
+ */
+void chal_sd_set_irq_signal(CHAL_HANDLE *sd_handle, uint32_t mask,
+			    uint32_t state)
+{
+	struct sd_dev *handle;
+
+	if (sd_handle == NULL)
+		return;
+
+	handle = (struct sd_dev *)sd_handle;
+
+	if (state)
+		mmio_setbits_32(handle->ctrl.sdRegBaseAddr +
+				SD4_EMMC_TOP_INTREN2_OFFSET, mask);
+	else
+		mmio_clrbits_32(handle->ctrl.sdRegBaseAddr +
+				SD4_EMMC_TOP_INTREN2_OFFSET, mask);
+}
diff --git a/drivers/brcm/emmc/emmc_csl_sdcard.c b/drivers/brcm/emmc/emmc_csl_sdcard.c
new file mode 100644
index 0000000..9e2c618
--- /dev/null
+++ b/drivers/brcm/emmc/emmc_csl_sdcard.c
@@ -0,0 +1,1089 @@
+/*
+ * Copyright (c) 2016 - 2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <inttypes.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <lib/mmio.h>
+
+#include "bcm_emmc.h"
+#include "emmc_chal_types.h"
+#include "emmc_csl_sdprot.h"
+#include "emmc_chal_sd.h"
+#include "emmc_csl_sdcmd.h"
+#include "emmc_csl_sd.h"
+#include "emmc_pboot_hal_memory_drv.h"
+
+#define SD_CARD_BUSY                    0x80000000
+#define SD_CARD_RETRY_LIMIT             1000
+#define SD_CARD_HIGH_SPEED_PS           13
+#define SD_CHK_HIGH_SPEED_MODE          0x00FFFFF1
+#define SD_SET_HIGH_SPEED_MODE          0x80FFFFF1
+#define SD_MMC_ENABLE_HIGH_SPEED        0x03b90100	//0x03b90103
+#define SD_MMC_8BIT_MODE                0x03b70200
+#define SD_MMC_4BIT_MODE                0x03b70100
+#define SD_MMC_1BIT_MODE                0x03b70000
+
+#define SD_MMC_BOOT_8BIT_MODE           0x03b10200
+#define SD_MMC_BOOT_4BIT_MODE           0x03b10100
+#define SD_MMC_BOOT_1BIT_MODE           0x03b10000
+#define SDIO_HW_EMMC_EXT_CSD_BOOT_CNF   0X03B30000
+
+#ifdef USE_EMMC_FIP_TOC_CACHE
+/*
+ * Cache size mirrors the size of the global eMMC temp buffer
+ * which is used for non-image body reads such as headers, ToC etc.
+ */
+#define CACHE_SIZE           ((EMMC_BLOCK_SIZE) * 2)
+#define PARTITION_BLOCK_ADDR ((PLAT_FIP_ATTEMPT_OFFSET)/(EMMC_BLOCK_SIZE))
+
+static uint32_t cached_partition_block;
+static uint8_t cached_block[CACHE_SIZE];
+#endif
+
+static int set_card_data_width(struct sd_handle *handle, int width);
+static int abort_err(struct sd_handle *handle);
+static int err_recovery(struct sd_handle *handle, uint32_t errors);
+static int xfer_data(struct sd_handle *handle, uint32_t mode, uint32_t addr,
+		     uint32_t length, uint8_t *base);
+
+int set_boot_config(struct sd_handle *handle, uint32_t config)
+{
+	return mmc_cmd6(handle, SDIO_HW_EMMC_EXT_CSD_BOOT_CNF | config);
+}
+
+void process_csd_mmc_speed(struct sd_handle *handle, uint32_t csd_mmc_speed)
+{
+	uint32_t div_ctrl_setting;
+
+	/* CSD field TRAN_SPEED:
+	 * Bits [2:0] 0 = 100 KHz
+	 *            1 = 1 MHz
+	 *            2 = 10 MHz
+	 *            3 = 100 MHz
+	 *            4...7 Reserved.
+	 * Bits [6:3] 0 = Reserved
+	 *            1 = 1.0
+	 *            2 = 1.2
+	 *            3 = 1.3
+	 *            4 = 1.5
+	 *            5 = 2.0
+	 *            6 = 2.6
+	 *            7 = 3.0
+	 *            8 = 3.5
+	 *            9 = 4.0
+	 *            A = 4.5
+	 *            B = 5.2
+	 *            C = 5.5
+	 *            D = 6.0
+	 *            E = 7.0
+	 *            F = 8.0
+	 * For cards supporting version 4.0, 4.1, and 4.2 of the standard,
+	 * the value shall be 20 MHz (0x2A).
+	 * For cards supporting version 4.3 , the value shall be 26 MHz (0x32)
+	 */
+
+	switch (csd_mmc_speed & 0x7F) {
+	case 0x2A:
+		EMMC_TRACE("Speeding up eMMC clock to 20MHz\n");
+		div_ctrl_setting =
+		    chal_sd_freq_2_div_ctrl_setting(20 * 1000 * 1000);
+		break;
+	case 0x32:
+		EMMC_TRACE("Speeding up eMMC clock to 26MHz\n");
+		div_ctrl_setting =
+		    chal_sd_freq_2_div_ctrl_setting(26 * 1000 * 1000);
+		break;
+	default:
+		/* Unknown */
+		return;
+	}
+
+	chal_sd_set_clock((CHAL_HANDLE *) handle->device, div_ctrl_setting, 0);
+
+	chal_sd_set_clock((CHAL_HANDLE *) handle->device, div_ctrl_setting, 1);
+
+	SD_US_DELAY(1000);
+}
+
+
+/*
+ * The function changes SD/SDIO/MMC card data width if
+ * the card support configurable data width. The host controller
+ * and the card has to be in the same bus data width.
+ */
+int set_card_data_width(struct sd_handle *handle, int width)
+{
+	uint32_t data_width = 0;
+	int is_valid_arg = 1;
+	int rc = SD_FAIL;
+	char *bitwidth_str = " ";
+	char *result_str = "failed";
+
+	switch (width) {
+#ifdef DRIVER_EMMC_ENABLE_DATA_WIDTH_8BIT
+	case SD_BUS_DATA_WIDTH_8BIT:
+		data_width = SD_MMC_8BIT_MODE;
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+		bitwidth_str = "8_BIT";
+#endif
+		break;
+#endif
+	case SD_BUS_DATA_WIDTH_4BIT:
+		data_width = SD_MMC_4BIT_MODE;
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+		bitwidth_str = "4_BIT";
+#endif
+		break;
+
+	case SD_BUS_DATA_WIDTH_1BIT:
+		data_width = SD_MMC_1BIT_MODE;
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+		bitwidth_str = "1_BIT";
+#endif
+		break;
+
+	default:
+		is_valid_arg = 0;
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+		bitwidth_str = "unknown";
+#endif
+		break;
+	}
+
+	if (is_valid_arg) {
+		rc = mmc_cmd6(handle, data_width);
+		if (rc == SD_OK) {
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+			result_str = "succeeded";
+#endif
+			chal_sd_config_bus_width((CHAL_HANDLE *) handle->device,
+						 width);
+		} else {
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+			result_str = "failed";
+#endif
+		}
+	} else {
+		rc = SD_FAIL;
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+		result_str = "ignored";
+#endif
+	}
+
+	VERBOSE("SDIO Data Width(%s) %s.\n", bitwidth_str, result_str);
+
+	return rc;
+}
+
+
+/*
+ * Error handling routine. Does abort data
+ * transmission if error is found.
+ */
+static int abort_err(struct sd_handle *handle)
+{
+	uint32_t present, options, event, rel = 0;
+	struct sd_resp cmdRsp;
+
+	handle->device->ctrl.argReg = 0;
+	handle->device->ctrl.cmdIndex = SD_CMD_STOP_TRANSMISSION;
+
+	options = (SD_CMD_STOP_TRANSMISSION << 24) |
+		  (SD_CMDR_RSP_TYPE_R1b_5b << SD_CMDR_RSP_TYPE_S) |
+		  SD4_EMMC_TOP_CMD_CRC_EN_MASK |
+		  SD4_EMMC_TOP_CMD_CCHK_EN_MASK;
+
+	chal_sd_send_cmd((CHAL_HANDLE *) handle->device,
+			 handle->device->ctrl.cmdIndex,
+			 handle->device->ctrl.argReg, options);
+
+	event = wait_for_event(handle,
+			       SD4_EMMC_TOP_INTR_CMDDONE_MASK |
+			       SD_ERR_INTERRUPTS,
+			       handle->device->cfg.wfe_retry);
+
+	if (event & SD_CMD_ERROR_INT) {
+		rel = SD_ERROR_NON_RECOVERABLE;
+	} else {
+		if (event & SD_DAT_TIMEOUT) {
+			return SD_ERROR_NON_RECOVERABLE;
+		}
+
+		chal_sd_get_response((CHAL_HANDLE *) handle->device,
+				     (uint32_t *)&cmdRsp);
+
+		process_cmd_response(handle, handle->device->ctrl.cmdIndex,
+				     cmdRsp.data.r2.rsp1, cmdRsp.data.r2.rsp2,
+				     cmdRsp.data.r2.rsp3, cmdRsp.data.r2.rsp4,
+				     &cmdRsp);
+
+		SD_US_DELAY(2000);
+
+		present =
+		    chal_sd_get_present_status((CHAL_HANDLE *) handle->device);
+
+		if ((present & 0x00F00000) == 0x00F00000)
+			rel = SD_ERROR_RECOVERABLE;
+		else
+			rel = SD_ERROR_NON_RECOVERABLE;
+	}
+
+	return rel;
+}
+
+
+/*
+ * The function handles real data transmission on both DMA and
+ * none DMA mode, In None DMA mode the data transfer starts
+ * when the command is sent to the card, data has to be written
+ * into the host contollers buffer at this time one block
+ * at a time.
+ * In DMA mode, the real data transfer is done by the DMA engine
+ * and this functions just waits for the data transfer to complete.
+ *
+ */
+int process_data_xfer(struct sd_handle *handle, uint8_t *buffer, uint32_t addr,
+		      uint32_t length, int dir)
+{
+	if (dir == SD_XFER_HOST_TO_CARD) {
+#ifdef INCLUDE_EMMC_DRIVER_WRITE_CODE
+		if (handle->device->cfg.dma == SD_DMA_OFF) {
+			/*
+			 * In NON DMA mode, the real data xfer starts from here
+			 */
+			if (write_buffer(handle, length, buffer))
+				return SD_WRITE_ERROR;
+		} else {
+			wait_for_event(handle,
+				       SD4_EMMC_TOP_INTR_TXDONE_MASK |
+				       SD_ERR_INTERRUPTS,
+				       handle->device->cfg.wfe_retry);
+
+			if (handle->device->ctrl.cmdStatus == SD_OK)
+				return SD_OK;
+
+			check_error(handle, handle->device->ctrl.cmdStatus);
+			return SD_WRITE_ERROR;
+		}
+#else
+		return SD_WRITE_ERROR;
+#endif
+	} else {		/* SD_XFER_CARD_TO_HOST */
+
+		if (handle->device->cfg.dma == SD_DMA_OFF) {
+			/* In NON DMA mode, the real data
+			 * transfer starts from here
+			 */
+			if (read_buffer(handle, length, buffer))
+				return SD_READ_ERROR;
+
+		} else {	/* for DMA mode */
+
+			/*
+			 * once the data transmission is done
+			 * copy data to the host buffer.
+			 */
+			wait_for_event(handle,
+				       SD4_EMMC_TOP_INTR_TXDONE_MASK |
+				       SD_ERR_INTERRUPTS,
+				       handle->device->cfg.wfe_retry);
+
+			if (handle->device->ctrl.cmdStatus == SD_OK)
+				return SD_OK;
+
+			check_error(handle, handle->device->ctrl.cmdStatus);
+			return SD_READ_ERROR;
+		}
+	}
+	return SD_OK;
+}
+
+
+/*
+ * The function sets block size for the next SD/SDIO/MMC
+ * card read/write command.
+ */
+int select_blk_sz(struct sd_handle *handle, uint16_t size)
+{
+	return sd_cmd16(handle, size);
+}
+
+
+/*
+ * The function initalizes the SD/SDIO/MMC/CEATA and detects
+ * the card according to the flag of detection.
+ * Once this function is called, the card is put into ready state
+ * so application can do data transfer to and from the card.
+ */
+int init_card(struct sd_handle *handle, int detection)
+{
+	/*
+	 * After Reset, eMMC comes up in 1 Bit Data Width by default.
+	 * Set host side to match.
+	 */
+	chal_sd_config_bus_width((CHAL_HANDLE *) handle->device,
+				 SD_BUS_DATA_WIDTH_1BIT);
+
+#ifdef USE_EMMC_FIP_TOC_CACHE
+	cached_partition_block = 0;
+#endif
+	handle->device->ctrl.present = 0; /* init card present to be no card */
+
+	init_mmc_card(handle);
+
+	handle->device->ctrl.present = 1; /* card is detected */
+
+	/* switch the data width back */
+	if (handle->card->type != SD_CARD_MMC)
+		return SD_FAIL;
+
+	/*
+	 * Dynamically set Data Width to highest supported value.
+	 * Try different data width settings (highest to lowest).
+	 * Verify each setting by reading EXT_CSD and comparing
+	 * against the EXT_CSD contents previously read in call to
+	 * init_mmc_card() earlier. Stop at first verified data width
+	 * setting.
+	 */
+	{
+#define EXT_CSD_PROPERTIES_SECTION_START_INDEX	192
+#define EXT_CSD_PROPERTIES_SECTION_END_INDEX	511
+		uint8_t buffer[EXT_CSD_SIZE];
+#ifdef DRIVER_EMMC_ENABLE_DATA_WIDTH_8BIT
+		/* Try 8 Bit Data Width */
+		chal_sd_config_bus_width((CHAL_HANDLE *) handle->device,
+					 SD_BUS_DATA_WIDTH_8BIT);
+		if ((!set_card_data_width(handle, SD_BUS_DATA_WIDTH_8BIT)) &&
+		    (!mmc_cmd8(handle, buffer)) &&
+		    (!memcmp(&buffer[EXT_CSD_PROPERTIES_SECTION_START_INDEX],
+			     &(emmc_global_buf_ptr->u.Ext_CSD_storage[EXT_CSD_PROPERTIES_SECTION_START_INDEX]),
+			     EXT_CSD_PROPERTIES_SECTION_END_INDEX - EXT_CSD_PROPERTIES_SECTION_START_INDEX + 1)))
+
+			return SD_OK;
+#endif
+		/* Fall back to 4 Bit Data Width */
+		chal_sd_config_bus_width((CHAL_HANDLE *) handle->device,
+					 SD_BUS_DATA_WIDTH_4BIT);
+		if ((!set_card_data_width(handle, SD_BUS_DATA_WIDTH_4BIT)) &&
+		    (!mmc_cmd8(handle, buffer)) &&
+		    (!memcmp(&buffer[EXT_CSD_PROPERTIES_SECTION_START_INDEX],
+			     &(emmc_global_buf_ptr->u.Ext_CSD_storage[EXT_CSD_PROPERTIES_SECTION_START_INDEX]),
+			     EXT_CSD_PROPERTIES_SECTION_END_INDEX - EXT_CSD_PROPERTIES_SECTION_START_INDEX + 1)))
+
+			return SD_OK;
+
+		/* Fall back to 1 Bit Data Width */
+		chal_sd_config_bus_width((CHAL_HANDLE *) handle->device,
+					 SD_BUS_DATA_WIDTH_1BIT);
+		/* Just use 1 Bit Data Width then. */
+		if (!set_card_data_width(handle, SD_BUS_DATA_WIDTH_1BIT))
+			return SD_OK;
+
+	}
+	return SD_CARD_INIT_ERROR;
+}
+
+
+/*
+ * The function handles MMC/CEATA card initalization.
+ */
+int init_mmc_card(struct sd_handle *handle)
+{
+	uint32_t ocr = 0, newOcr, rc, limit = 0;
+	uint32_t cmd1_option = 0x40300000;
+	uint32_t sec_count;
+
+	handle->card->type = SD_CARD_MMC;
+
+	do {
+		SD_US_DELAY(1000);
+		newOcr = 0;
+		ocr = 0;
+		rc = sd_cmd1(handle, cmd1_option, &newOcr);
+		limit++;
+
+		if (rc == SD_OK)
+			ocr = newOcr;
+
+	} while (((ocr & SD_CARD_BUSY) == 0) && (limit < SD_CARD_RETRY_LIMIT));
+
+	if (limit >= SD_CARD_RETRY_LIMIT) {
+		handle->card->type = SD_CARD_UNKNOWN;
+		EMMC_TRACE("CMD1 Timeout: Device is not ready\n");
+		return SD_CARD_UNKNOWN;
+	}
+
+	/* Save the ocr register */
+	handle->device->ctrl.ocr = ocr;
+
+	/* Ready State */
+	rc = sd_cmd2(handle);
+	if (rc != SD_OK) {
+		handle->card->type = SD_CARD_UNKNOWN;
+		return SD_CARD_UNKNOWN;
+	}
+
+	rc = sd_cmd3(handle);
+	if (rc != SD_OK) {
+		handle->card->type = SD_CARD_UNKNOWN;
+		return SD_CARD_UNKNOWN;
+	}
+	/* read CSD */
+	rc = sd_cmd9(handle, &emmc_global_vars_ptr->cardData);
+	if (rc != SD_OK) {
+		handle->card->type = SD_CARD_UNKNOWN;
+		return SD_CARD_UNKNOWN;
+	}
+
+	/* Increase clock frequency according to what the card advertises */
+	EMMC_TRACE("From CSD...  cardData.csd.mmc.speed = 0x%X\n",
+		   emmc_global_vars_ptr->cardData.csd.mmc.speed);
+	process_csd_mmc_speed(handle,
+			      emmc_global_vars_ptr->cardData.csd.mmc.speed);
+
+	/* goto transfer mode */
+	rc = sd_cmd7(handle, handle->device->ctrl.rca);
+	if (rc != SD_OK) {
+		handle->card->type = SD_CARD_UNKNOWN;
+		return SD_CARD_UNKNOWN;
+	}
+
+	rc = mmc_cmd8(handle, emmc_global_buf_ptr->u.Ext_CSD_storage);
+	if (rc == SD_OK) {
+		/* calcul real capacity */
+		sec_count = emmc_global_buf_ptr->u.Ext_CSD_storage[212] |
+			    emmc_global_buf_ptr->u.Ext_CSD_storage[213] << 8 |
+			    emmc_global_buf_ptr->u.Ext_CSD_storage[214] << 16 |
+			    emmc_global_buf_ptr->u.Ext_CSD_storage[215] << 24;
+
+		EMMC_TRACE("Device density = %ldMBytes\n",
+			   handle->card->size / (1024 * 1024));
+
+		if (sec_count > 0) {
+			handle->card->size = (uint64_t)sec_count * 512;
+
+			EMMC_TRACE("Updated Device density = %ldMBytes\n",
+				   handle->card->size / (1024 * 1024));
+		}
+
+		if (sec_count > (2u * 1024 * 1024 * 1024) / 512) {
+			handle->device->ctrl.ocr |= SD_CARD_HIGH_CAPACITY;
+			handle->device->cfg.blockSize = 512;
+		}
+
+		if (handle->device->ctrl.ocr & SD_CARD_HIGH_CAPACITY)
+			EMMC_TRACE("Sector addressing\n");
+		else
+			EMMC_TRACE("Byte addressing\n");
+
+		EMMC_TRACE("Ext_CSD_storage[162]: 0x%02X  Ext_CSD_storage[179]: 0x%02X\n",
+			   emmc_global_buf_ptr->u.Ext_CSD_storage[162],
+			   emmc_global_buf_ptr->u.Ext_CSD_storage[179]);
+	}
+
+	return handle->card->type;
+}
+
+
+/*
+ * The function send reset command to the card.
+ * The card will be in ready status after the reset.
+ */
+int reset_card(struct sd_handle *handle)
+{
+	int res = SD_OK;
+
+	/* on reset, card's RCA should return to 0 */
+	handle->device->ctrl.rca = 0;
+
+	res = sd_cmd0(handle);
+
+	if (res != SD_OK)
+		return SD_RESET_ERROR;
+
+	return res;
+}
+
+
+/*
+ * The function sends command to the card and starts
+ * data transmission.
+ */
+static int xfer_data(struct sd_handle *handle,
+		     uint32_t mode,
+		     uint32_t addr, uint32_t length, uint8_t *base)
+{
+	int rc = SD_OK;
+
+	VERBOSE("XFER: dest: 0x%" PRIx64 ", addr: 0x%x, size: 0x%x bytes\n",
+		(uint64_t)base, addr, length);
+
+	if ((length / handle->device->cfg.blockSize) > 1) {
+		if (mode == SD_OP_READ) {
+			inv_dcache_range((uintptr_t)base, (uint64_t)length);
+			rc = sd_cmd18(handle, addr, length, base);
+		} else {
+#ifdef INCLUDE_EMMC_DRIVER_WRITE_CODE
+			flush_dcache_range((uintptr_t)base, (uint64_t)length);
+			rc = sd_cmd25(handle, addr, length, base);
+#else
+			rc = SD_DATA_XFER_ERROR;
+#endif
+		}
+	} else {
+		if (mode == SD_OP_READ) {
+			inv_dcache_range((uintptr_t)base, (uint64_t)length);
+			rc = sd_cmd17(handle, addr,
+				      handle->device->cfg.blockSize, base);
+		} else {
+#ifdef INCLUDE_EMMC_DRIVER_WRITE_CODE
+			flush_dcache_range((uintptr_t)base, (uint64_t)length);
+			rc = sd_cmd24(handle, addr,
+				      handle->device->cfg.blockSize, base);
+#else
+			rc = SD_DATA_XFER_ERROR;
+#endif
+		}
+	}
+
+	if (rc != SD_OK)
+		return SD_DATA_XFER_ERROR;
+
+	return SD_OK;
+}
+
+#ifdef INCLUDE_EMMC_DRIVER_ERASE_CODE
+int erase_card(struct sd_handle *handle, uint32_t addr, uint32_t blocks)
+{
+	uint32_t end_addr;
+
+	INFO("ERASE: addr: 0x%x, num of sectors: 0x%x\n", addr, blocks);
+
+	if (sd_cmd35(handle, addr) != SD_OK)
+		return SD_FAIL;
+
+	end_addr = addr + blocks - 1;
+	if (sd_cmd36(handle, end_addr) != SD_OK)
+		return SD_FAIL;
+
+	if (sd_cmd38(handle) != SD_OK)
+		return SD_FAIL;
+
+	return SD_OK;
+}
+#endif
+
+/*
+ * The function reads block data from a card.
+ */
+#ifdef USE_EMMC_FIP_TOC_CACHE
+int read_block(struct sd_handle *handle,
+	       uint8_t *dst, uint32_t addr, uint32_t len)
+{
+	int rel = SD_OK;
+
+	/*
+	 * Avoid doing repeated reads of the partition block
+	 * by caching.
+	 */
+	if (cached_partition_block &&
+	    addr == PARTITION_BLOCK_ADDR &&
+	    len == CACHE_SIZE) {
+		memcpy(dst, cached_block, len);
+	} else {
+		rel = xfer_data(handle, SD_OP_READ, addr, len, dst);
+
+		if (len == CACHE_SIZE && addr == PARTITION_BLOCK_ADDR) {
+			cached_partition_block = 1;
+			memcpy(cached_block, dst, len);
+		}
+	}
+
+	return rel;
+}
+#else
+int read_block(struct sd_handle *handle,
+	       uint8_t *dst, uint32_t addr, uint32_t len)
+{
+	return xfer_data(handle, SD_OP_READ, addr, len, dst);
+}
+#endif
+
+#ifdef INCLUDE_EMMC_DRIVER_WRITE_CODE
+
+/*
+ * The function writes block data to a card.
+ */
+int write_block(struct sd_handle *handle,
+		uint8_t *src, uint32_t addr, uint32_t len)
+{
+	int rel = SD_OK;
+
+	/*
+	 * Current HC has problem to get response of cmd16 after cmd12,
+	 * the delay is necessary to sure the next cmd16 will not be timed out.
+	 * The delay has to be at least 4 ms.
+	 * The code removed cmd16 and use cmd13 to get card status before
+	 * sending cmd18 or cmd25 to make sure the card is ready and thus
+	 * no need to have delay here.
+	 */
+
+	rel = xfer_data(handle, SD_OP_WRITE, addr, len, src);
+
+	EMMC_TRACE("wr_blk addr:0x%08X src:0x%08X len:0x%08X result:%d\n",
+		   addr, src, len, rel);
+
+	return rel;
+}
+
+
+/*
+ * The function is called to write one block data directly to
+ * a card's data buffer.
+ * it is used in Non-DMA mode for card data transmission.
+ */
+int write_buffer(struct sd_handle *handle, uint32_t length, uint8_t *data)
+{
+	uint32_t rem, blockSize, event;
+	uint8_t *pData = data;
+
+	blockSize = handle->device->cfg.blockSize;
+	rem = length;
+
+	if (rem == 0)
+		return SD_OK;
+
+	while (rem > 0) {
+
+		event = wait_for_event(handle,
+				       SD4_EMMC_TOP_INTR_BWRDY_MASK |
+				       SD_ERR_INTERRUPTS,
+				       handle->device->cfg.wfe_retry);
+
+		if (handle->device->ctrl.cmdStatus) {
+			check_error(handle, handle->device->ctrl.cmdStatus);
+			return SD_WRITE_ERROR;
+		}
+
+		if (rem >= blockSize)
+			chal_sd_write_buffer((CHAL_HANDLE *) handle->device,
+					     blockSize, pData);
+		else
+			chal_sd_write_buffer((CHAL_HANDLE *) handle->device,
+					     rem, pData);
+
+		if (rem > blockSize) {
+			rem -= blockSize;
+			pData += blockSize;
+		} else {
+			pData += rem;
+			rem = 0;
+		}
+	}
+
+	if ((event & SD4_EMMC_TOP_INTR_TXDONE_MASK) !=
+	    SD4_EMMC_TOP_INTR_TXDONE_MASK) {
+		event = wait_for_event(handle,
+				       SD4_EMMC_TOP_INTR_TXDONE_MASK |
+				       SD_ERR_INTERRUPTS,
+				       handle->device->cfg.wfe_retry);
+
+		if (handle->device->ctrl.cmdStatus != SD_OK) {
+			check_error(handle, handle->device->ctrl.cmdStatus);
+			return SD_WRITE_ERROR;
+		}
+	} else {
+		handle->device->ctrl.eventList &= ~SD4_EMMC_TOP_INTR_TXDONE_MASK;
+	}
+
+	return SD_OK;
+}
+#endif /* INCLUDE_EMMC_DRIVER_WRITE_CODE */
+
+
+/*
+ * The function is called to read maximal one block data
+ * directly from a card
+ * It is used in Non-DMA mode for card data transmission.
+ */
+int read_buffer(struct sd_handle *handle, uint32_t length, uint8_t *data)
+{
+	uint32_t rem, blockSize, event = 0;
+	uint8_t *pData = data;
+
+	blockSize = handle->device->cfg.blockSize;
+	rem = length;
+
+	if (rem == 0)
+		return SD_OK;
+
+	while (rem > 0) {
+		event = wait_for_event(handle,
+				       SD4_EMMC_TOP_INTR_BRRDY_MASK |
+				       SD_ERR_INTERRUPTS,
+				       handle->device->cfg.wfe_retry);
+
+		if (handle->device->ctrl.cmdStatus) {
+			check_error(handle, handle->device->ctrl.cmdStatus);
+			return SD_READ_ERROR;
+		}
+
+		if (rem >= blockSize)
+			chal_sd_read_buffer((CHAL_HANDLE *) handle->device,
+					    blockSize, pData);
+		else
+			chal_sd_read_buffer((CHAL_HANDLE *) handle->device, rem,
+					    pData);
+
+		if (rem > blockSize) {
+			rem -= blockSize;
+			pData += blockSize;
+		} else {
+			pData += rem;
+			rem = 0;
+		}
+	}
+
+	/* In case, there are extra data in the SD FIFO, just dump them. */
+	chal_sd_dump_fifo((CHAL_HANDLE *) handle->device);
+
+	if ((event & SD4_EMMC_TOP_INTR_TXDONE_MASK) !=
+	    SD4_EMMC_TOP_INTR_TXDONE_MASK) {
+		event = wait_for_event(handle, SD4_EMMC_TOP_INTR_TXDONE_MASK,
+				       handle->device->cfg.wfe_retry);
+
+		if (handle->device->ctrl.cmdStatus) {
+			check_error(handle, handle->device->ctrl.cmdStatus);
+			return SD_READ_ERROR;
+		}
+	} else {
+		handle->device->ctrl.eventList &= ~SD4_EMMC_TOP_INTR_TXDONE_MASK;
+	}
+
+	return SD_OK;
+}
+
+
+/*
+ * Error handling routine.
+ * The function just reset the DAT
+ * and CMD line if an error occures during data transmission.
+ */
+int check_error(struct sd_handle *handle, uint32_t ints)
+{
+	uint32_t rel;
+
+	chal_sd_set_irq_signal((CHAL_HANDLE *) handle->device,
+			       SD_ERR_INTERRUPTS, 0);
+
+	if (ints & SD4_EMMC_TOP_INTR_CMDERROR_MASK) {
+
+		chal_sd_reset_line((CHAL_HANDLE *) handle->device,
+				   SD4_EMMC_TOP_CTRL1_CMDRST_MASK);
+		rel = abort_err(handle);
+
+		chal_sd_reset_line((CHAL_HANDLE *) handle->device,
+				   SD4_EMMC_TOP_CTRL1_DATRST_MASK);
+		chal_sd_set_irq_signal((CHAL_HANDLE *) handle->device,
+				       SD_ERR_INTERRUPTS, 1);
+
+		return (rel == SD_ERROR_NON_RECOVERABLE) ?
+				SD_ERROR_NON_RECOVERABLE : SD_ERROR_RECOVERABLE;
+	} else {
+		rel = err_recovery(handle, ints);
+	}
+
+	chal_sd_set_irq_signal((CHAL_HANDLE *) handle->device,
+			       SD_ERR_INTERRUPTS, 1);
+
+	return rel;
+}
+
+
+/*
+ * Error recovery routine.
+ * Try to recover from the error.
+ */
+static int err_recovery(struct sd_handle *handle, uint32_t errors)
+{
+	uint32_t rel = 0;
+
+	/*
+	 * In case of timeout error, the cmd line and data line maybe
+	 * still active or stuck at atcitve so it is needed to reset
+	 * either data line or cmd line to make sure a new cmd can be sent.
+	 */
+
+	if (errors & SD_CMD_ERROR_INT)
+		chal_sd_reset_line((CHAL_HANDLE *) handle->device,
+				   SD4_EMMC_TOP_CTRL1_CMDRST_MASK);
+
+	if (errors & SD_DAT_ERROR_INT)
+		chal_sd_reset_line((CHAL_HANDLE *) handle->device,
+				   SD4_EMMC_TOP_CTRL1_DATRST_MASK);
+
+	/* Abort transaction by sending out stop command */
+	if ((handle->device->ctrl.cmdIndex == 18) ||
+	    (handle->device->ctrl.cmdIndex == 25))
+		rel = abort_err(handle);
+
+	return rel;
+}
+
+
+/*
+ * The function is called to read one block data directly from a card.
+ * It is used in Non-DMA mode for card data transmission.
+ */
+int process_cmd_response(struct sd_handle *handle,
+			 uint32_t cmdIndex,
+			 uint32_t rsp0,
+			 uint32_t rsp1,
+			 uint32_t rsp2, uint32_t rsp3, struct sd_resp *resp)
+{
+	int result = SD_OK;
+
+	/* R6 */
+	uint32_t rca = (rsp0 >> 16) & 0xffff;
+	uint32_t cardStatus = rsp0;
+
+	/* R4 */
+	uint32_t cBit = (rsp0 >> 31) & 0x1;
+	uint32_t funcs = (rsp0 >> 28) & 0x7;
+	uint32_t memPresent = (rsp0 >> 27) & 0x1;
+
+	resp->r1 = 0x3f;
+	resp->cardStatus = cardStatus;
+
+	if (cmdIndex == SD_CMD_IO_SEND_OP_COND) {
+		resp->data.r4.cardReady = cBit;
+		resp->data.r4.funcs = funcs;
+		resp->data.r4.memPresent = memPresent;
+		resp->data.r4.ocr = cardStatus;
+	}
+
+	if (cmdIndex == SD_CMD_MMC_SET_RCA) {
+		resp->data.r6.rca = rca;
+		resp->data.r6.cardStatus = cardStatus & 0xFFFF;
+	}
+
+	if (cmdIndex == SD_CMD_SELECT_DESELECT_CARD) {
+		resp->data.r7.rca = rca;
+	}
+
+	if (cmdIndex == SD_CMD_IO_RW_DIRECT) {
+		if (((rsp0 >> 16) & 0xffff) != 0)
+			result = SD_CMD_ERR_INVALID_RESPONSE;
+
+		resp->data.r5.data = rsp0 & 0xff;
+	}
+
+	if (cmdIndex == SD_CMD_IO_RW_EXTENDED) {
+		if (((rsp0 >> 16) & 0xffff) != 0)
+			result = SD_CMD_ERR_INVALID_RESPONSE;
+
+		resp->data.r5.data = rsp0 & 0xff;
+	}
+
+	if (cmdIndex == SD_ACMD_SD_SEND_OP_COND ||
+	    cmdIndex == SD_CMD_SEND_OPCOND)
+		resp->data.r3.ocr = cardStatus;
+
+	if (cmdIndex == SD_CMD_SEND_CSD ||
+	    cmdIndex == SD_CMD_SEND_CID ||
+	    cmdIndex == SD_CMD_ALL_SEND_CID) {
+		resp->data.r2.rsp4 = rsp3;
+		resp->data.r2.rsp3 = rsp2;
+		resp->data.r2.rsp2 = rsp1;
+		resp->data.r2.rsp1 = rsp0;
+	}
+
+	if ((cmdIndex == SD_CMD_READ_EXT_CSD) &&
+	    (handle->card->type == SD_CARD_SD)) {
+		if ((resp->cardStatus & 0xAA) != 0xAA) {
+			result = SD_CMD_ERR_INVALID_RESPONSE;
+		}
+	}
+
+	return result;
+}
+
+
+/*
+ * The function sets DMA buffer and data length, process
+ * block size and the number of blocks to be transferred.
+ * It returns the DMA buffer address.
+ * It copies dma data from user buffer to the DMA buffer
+ * if the operation is to write data to the SD card.
+ */
+void data_xfer_setup(struct sd_handle *handle, uint8_t *data, uint32_t length,
+		     int dir)
+{
+	chal_sd_setup_xfer((CHAL_HANDLE *)handle->device, data, length, dir);
+}
+
+
+/*
+ * The function does soft reset the host SD controller. After
+ * the function call all host controller's register are reset
+ * to default vallue;
+ *
+ * Note    This function only resets the host controller it does not
+ *          reset the controller's handler.
+ */
+int reset_host_ctrl(struct sd_handle *handle)
+{
+	chal_sd_stop();
+
+	return SD_OK;
+}
+
+static void pstate_log(struct sd_handle *handle)
+{
+	ERROR("PSTATE: 0x%x\n", mmio_read_32
+		(handle->device->ctrl.sdRegBaseAddr +
+			SD4_EMMC_TOP_PSTATE_SD4_OFFSET));
+	ERROR("ERRSTAT: 0x%x\n", mmio_read_32
+		(handle->device->ctrl.sdRegBaseAddr +
+			SD4_EMMC_TOP_ERRSTAT_OFFSET));
+}
+
+/*
+ * The function waits for one or a group of interrupts specified
+ * by mask. The function returns if any one the interrupt status
+ * is set. If interrupt mode is not enabled then it will poll
+ * the interrupt status register until a interrupt status is set
+ * an error interrupt happens. If interrupt mode is enabled then
+ * this function should be called after the interrupt
+ * is received by ISR routine.
+ */
+uint32_t wait_for_event(struct sd_handle *handle,
+			uint32_t mask, uint32_t retry)
+{
+	uint32_t regval, cmd12, time = 0;
+
+	handle->device->ctrl.cmdStatus = 0;	/* no error */
+	EMMC_TRACE("%s %d mask:0x%x timeout:%d irq_status:0x%x\n",
+		   __func__, __LINE__, mask, retry,
+		   chal_sd_get_irq_status((CHAL_HANDLE *)handle->device));
+
+	/* Polling mode */
+	do {
+		regval = chal_sd_get_irq_status((CHAL_HANDLE *)handle->device);
+
+		if (regval & SD4_EMMC_TOP_INTR_DMAIRQ_MASK) {
+			chal_sd_set_dma_addr((CHAL_HANDLE *)handle->device,
+					(uintptr_t)
+				chal_sd_get_dma_addr((CHAL_HANDLE *)
+						handle->device));
+			chal_sd_clear_irq((CHAL_HANDLE *)handle->device,
+					  SD4_EMMC_TOP_INTR_DMAIRQ_MASK);
+		}
+
+		if (time++ > retry) {
+			ERROR("EMMC: No response (cmd%d) after %dus.\n",
+			      handle->device->ctrl.cmdIndex,
+			      time * EMMC_WFE_RETRY_DELAY_US);
+			handle->device->ctrl.cmdStatus = SD_CMD_MISSING;
+			pstate_log(handle);
+			ERROR("EMMC: INT[0x%x]\n", regval);
+			break;
+		}
+
+		if (regval & SD4_EMMC_TOP_INTR_CTOERR_MASK) {
+			ERROR("EMMC: Cmd%d timeout INT[0x%x]\n",
+			      handle->device->ctrl.cmdIndex, regval);
+			handle->device->ctrl.cmdStatus =
+			    SD4_EMMC_TOP_INTR_CTOERR_MASK;
+			pstate_log(handle);
+			break;
+		}
+		if (regval & SD_CMD_ERROR_FLAGS) {
+			ERROR("EMMC: Cmd%d error INT[0x%x]\n",
+			      handle->device->ctrl.cmdIndex, regval);
+			handle->device->ctrl.cmdStatus = SD_CMD_ERROR_FLAGS;
+			pstate_log(handle);
+			break;
+		}
+
+		cmd12 = chal_sd_get_atuo12_error((CHAL_HANDLE *)handle->device);
+		if (cmd12) {
+			ERROR("EMMC: Cmd%d auto cmd12 err:0x%x\n",
+			      handle->device->ctrl.cmdIndex, cmd12);
+			handle->device->ctrl.cmdStatus = cmd12;
+			pstate_log(handle);
+			break;
+		}
+
+		if (SD_DATA_ERROR_FLAGS & regval) {
+			ERROR("EMMC: Data for cmd%d error, INT[0x%x]\n",
+			      handle->device->ctrl.cmdIndex, regval);
+			handle->device->ctrl.cmdStatus =
+			    (SD_DATA_ERROR_FLAGS & regval);
+			pstate_log(handle);
+			break;
+		}
+
+		if ((regval & mask) == 0)
+			udelay(EMMC_WFE_RETRY_DELAY_US);
+
+	} while ((regval & mask) == 0);
+
+	/* clear the interrupt since it is processed */
+	chal_sd_clear_irq((CHAL_HANDLE *)handle->device, (regval & mask));
+
+	return (regval & mask);
+}
+
+int32_t set_config(struct sd_handle *handle, uint32_t speed, uint32_t retry,
+		    uint32_t dma, uint32_t dmaBound, uint32_t blkSize,
+		    uint32_t wfe_retry)
+{
+	int32_t rel = 0;
+
+	if (handle == NULL)
+		return SD_FAIL;
+
+	handle->device->cfg.wfe_retry = wfe_retry;
+
+	rel = chal_sd_config((CHAL_HANDLE *)handle->device, speed, retry,
+			     dmaBound, blkSize, dma);
+	return rel;
+
+}
+
+int mmc_cmd1(struct sd_handle *handle)
+{
+	uint32_t newOcr, res;
+	uint32_t cmd1_option = MMC_OCR_OP_VOLT | MMC_OCR_SECTOR_ACCESS_MODE;
+
+	/*
+	 * After Reset, eMMC comes up in 1 Bit Data Width by default.
+	 * Set host side to match.
+	 */
+	chal_sd_config_bus_width((CHAL_HANDLE *) handle->device,
+				 SD_BUS_DATA_WIDTH_1BIT);
+
+#ifdef USE_EMMC_FIP_TOC_CACHE
+	cached_partition_block = 0;
+#endif
+	handle->device->ctrl.present = 0; /* init card present to be no card */
+
+	handle->card->type = SD_CARD_MMC;
+
+	res = sd_cmd1(handle, cmd1_option, &newOcr);
+
+	if (res != SD_OK) {
+		EMMC_TRACE("CMD1 Timeout: Device is not ready\n");
+		res = SD_CARD_UNKNOWN;
+	}
+	return res;
+}
diff --git a/drivers/brcm/emmc/emmc_csl_sdcmd.c b/drivers/brcm/emmc/emmc_csl_sdcmd.c
new file mode 100644
index 0000000..c62886c
--- /dev/null
+++ b/drivers/brcm/emmc/emmc_csl_sdcmd.c
@@ -0,0 +1,842 @@
+/*
+ * Copyright (c) 2016 - 2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdlib.h>
+#include <stddef.h>
+
+#include "bcm_emmc.h"
+#include "emmc_chal_types.h"
+#include "emmc_chal_sd.h"
+#include "emmc_csl_sdprot.h"
+#include "emmc_csl_sdcmd.h"
+#include "emmc_csl_sd.h"
+#include "emmc_chal_sd.h"
+#include "emmc_pboot_hal_memory_drv.h"
+
+int sd_cmd0(struct sd_handle *handle)
+{
+	int res;
+	uint32_t argument = 0x0; /* Go to IDLE state. */
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_GO_IDLE_STATE, argument, 0, NULL);
+
+	if (res == SD_OK) {
+		/* Clear all other interrupts */
+		chal_sd_clear_irq((void *)handle->device, 0xffffffff);
+	}
+
+	return res;
+}
+
+int sd_cmd1(struct sd_handle *handle, uint32_t ocr, uint32_t *ocr_output)
+{
+	int res;
+	uint32_t options;
+	struct sd_resp resp;
+
+	options = SD_CMDR_RSP_TYPE_R3_4 << SD_CMDR_RSP_TYPE_S;
+
+	if (ocr_output == NULL) {
+		EMMC_TRACE("Invalid args\n");
+		return SD_FAIL;
+	}
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_SEND_OPCOND, ocr, options, &resp);
+
+	if (res == SD_OK)
+		*ocr_output = resp.data.r3.ocr;
+
+	return res;
+}
+
+int sd_cmd2(struct sd_handle *handle)
+{
+	uint32_t options;
+	struct sd_resp resp;
+
+	/* send cmd and parse result */
+	options = SD_CMDR_RSP_TYPE_R2 << SD_CMDR_RSP_TYPE_S;
+
+	return send_cmd(handle, SD_CMD_ALL_SEND_CID, 0, options, &resp);
+}
+
+int sd_cmd3(struct sd_handle *handle)
+{
+	int res;
+	uint32_t options = 0;
+	uint32_t argument;
+	struct sd_resp resp;
+
+	/* use non zero and non 0x1 value for rca */
+	handle->device->ctrl.rca = 0x5;
+	argument = handle->device->ctrl.rca << SD_CMD7_ARG_RCA_SHIFT;
+
+	options = SD_CMDR_RSP_TYPE_R1_5_6 << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_CCHK_EN_MASK |
+		  SD4_EMMC_TOP_CMD_CRC_EN_MASK;
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_MMC_SET_RCA, argument, options, &resp);
+
+	if (res != SD_OK)
+		handle->device->ctrl.rca = 0;
+
+	return res;
+}
+
+int sd_cmd7(struct sd_handle *handle, uint32_t rca)
+{
+	int res;
+	uint32_t argument, options;
+	struct sd_resp resp;
+
+	argument = (rca << SD_CMD7_ARG_RCA_SHIFT);
+
+	/*
+	 * Response to CMD7 is:
+	 * R1 while selectiing from Stand-By State to Transfer State
+	 * R1b while selecting from Disconnected State to Programming State.
+	 *
+	 * In this driver, we only issue a CMD7 once, to go to transfer mode
+	 * during init_mmc_card().
+	 */
+	options = SD_CMDR_RSP_TYPE_R1_5_6 << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_CCHK_EN_MASK |
+		  SD4_EMMC_TOP_CMD_CRC_EN_MASK;
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_SELECT_DESELECT_CARD, argument, options,
+		       &resp);
+
+	if (res == SD_OK)
+		/* Clear all other interrupts */
+		chal_sd_clear_irq((void *)handle->device, 0xffffffff);
+
+	return res;
+}
+
+
+/*
+ * CMD8 Get CSD_EXT
+ */
+int mmc_cmd8(struct sd_handle *handle, uint8_t *extCsdReg)
+{
+	uint32_t res, options;
+	struct sd_resp resp;
+
+	data_xfer_setup(handle, extCsdReg, CEATA_EXT_CSDBLOCK_SIZE,
+			    SD_XFER_CARD_TO_HOST);
+
+	options = SD_CMDR_RSP_TYPE_R1_5_6 << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_DPS_MASK | SD4_EMMC_TOP_CMD_DTDS_MASK |
+		  SD4_EMMC_TOP_CMD_CCHK_EN_MASK | SD4_EMMC_TOP_CMD_CRC_EN_MASK;
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_READ_EXT_CSD, 0, options, &resp);
+
+	if (res == SD_OK)
+		res = process_data_xfer(handle, extCsdReg, 0,
+					CEATA_EXT_CSDBLOCK_SIZE,
+					SD_XFER_CARD_TO_HOST);
+
+	return res;
+}
+
+int sd_cmd9(struct sd_handle *handle, struct sd_card_data *card)
+{
+	int res;
+	uint32_t argument, options, iBlkNum, multiFactor = 1;
+	uint32_t maxReadBlockLen = 1, maxWriteBlockLen = 1;
+	struct sd_resp resp;
+
+	argument = handle->device->ctrl.rca << SD_CMD7_ARG_RCA_SHIFT;
+
+	options = SD_CMDR_RSP_TYPE_R2 << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_CRC_EN_MASK;
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_SEND_CSD, argument, options, &resp);
+
+	if (res != SD_OK)
+		return res;
+
+	if (handle->card->type == SD_CARD_MMC) {
+		card->csd.mmc.structure = (resp.data.r2.rsp4 >> 22) & 0x3;
+		card->csd.mmc.csdSpecVer = (resp.data.r2.rsp4 >> 18) & 0x0f;
+		card->csd.mmc.taac = (resp.data.r2.rsp4 >> 8) & 0xff;
+		card->csd.mmc.nsac = resp.data.r2.rsp4 & 0xff;
+		card->csd.mmc.speed = resp.data.r2.rsp3 >> 24;
+		card->csd.mmc.classes = (resp.data.r2.rsp3 >> 12) & 0xfff;
+		card->csd.mmc.rdBlkLen = (resp.data.r2.rsp3 >> 8) & 0xf;
+		card->csd.mmc.rdBlkPartial = (resp.data.r2.rsp3 >> 7) & 0x01;
+		card->csd.mmc.wrBlkMisalign = (resp.data.r2.rsp3 >> 6) & 0x1;
+		card->csd.mmc.rdBlkMisalign = (resp.data.r2.rsp3 >> 5) & 0x1;
+		card->csd.mmc.dsr = (resp.data.r2.rsp2 >> 4) & 0x01;
+		card->csd.mmc.size =
+		    ((resp.data.r2.rsp3 & 0x3) << 10) +
+		    ((resp.data.r2.rsp2 >> 22) & 0x3ff);
+		card->csd.mmc.vddRdCurrMin = (resp.data.r2.rsp2 >> 19) & 0x7;
+		card->csd.mmc.vddRdCurrMax = (resp.data.r2.rsp2 >> 16) & 0x7;
+		card->csd.mmc.vddWrCurrMin = (resp.data.r2.rsp2 >> 13) & 0x7;
+		card->csd.mmc.vddWrCurrMax = (resp.data.r2.rsp2 >> 10) & 0x7;
+		card->csd.mmc.devSizeMulti = (resp.data.r2.rsp2 >> 7) & 0x7;
+		card->csd.mmc.eraseGrpSize = (resp.data.r2.rsp2 >> 2) & 0x1f;
+		card->csd.mmc.eraseGrpSizeMulti =
+		    ((resp.data.r2.rsp2 & 0x3) << 3) +
+		    ((resp.data.r2.rsp1 >> 29) & 0x7);
+		card->csd.mmc.wrProtGroupSize =
+		    ((resp.data.r2.rsp1 >> 24) & 0x1f);
+		card->csd.mmc.wrProtGroupEnable =
+		    (resp.data.r2.rsp1 >> 23) & 0x1;
+		card->csd.mmc.manuDefEcc = (resp.data.r2.rsp1 >> 21) & 0x3;
+		card->csd.mmc.wrSpeedFactor = (resp.data.r2.rsp1 >> 18) & 0x7;
+		card->csd.mmc.wrBlkLen = (resp.data.r2.rsp1 >> 14) & 0xf;
+		card->csd.mmc.wrBlkPartial = (resp.data.r2.rsp1 >> 13) & 0x1;
+		card->csd.mmc.protAppl = (resp.data.r2.rsp1 >> 8) & 0x1;
+		card->csd.mmc.copyFlag = (resp.data.r2.rsp1 >> 7) & 0x1;
+		card->csd.mmc.permWrProt = (resp.data.r2.rsp1 >> 6) & 0x1;
+		card->csd.mmc.tmpWrProt = (resp.data.r2.rsp1 >> 5) & 0x1;
+		card->csd.mmc.fileFormat = (resp.data.r2.rsp1 >> 4) & 0x03;
+		card->csd.mmc.eccCode = resp.data.r2.rsp1 & 0x03;
+		maxReadBlockLen <<= card->csd.mmc.rdBlkLen;
+		maxWriteBlockLen <<= card->csd.mmc.wrBlkLen;
+
+		iBlkNum = card->csd.mmc.size + 1;
+		multiFactor = (1 << (card->csd.mmc.devSizeMulti + 2));
+
+		handle->card->size =
+		    iBlkNum * multiFactor * (1 << card->csd.mmc.rdBlkLen);
+	}
+
+	handle->card->maxRdBlkLen = maxReadBlockLen;
+	handle->card->maxWtBlkLen = maxWriteBlockLen;
+
+	if (handle->card->size < 0xA00000) {
+		/*
+		 * 10MB Too small size mean, cmd9 response is wrong,
+		 * Use default value 1G
+		 */
+		handle->card->size = 0x40000000;
+		handle->card->maxRdBlkLen = 512;
+		handle->card->maxWtBlkLen = 512;
+	}
+
+	if ((handle->card->maxRdBlkLen > 512) ||
+	    (handle->card->maxWtBlkLen > 512)) {
+		handle->card->maxRdBlkLen = 512;
+		handle->card->maxWtBlkLen = 512;
+	} else if ((handle->card->maxRdBlkLen == 0) ||
+		   (handle->card->maxWtBlkLen == 0)) {
+		handle->card->maxRdBlkLen = 512;
+		handle->card->maxWtBlkLen = 512;
+	}
+
+	handle->device->cfg.blockSize = handle->card->maxRdBlkLen;
+
+	return res;
+}
+
+int sd_cmd13(struct sd_handle *handle, uint32_t *status)
+{
+	int res;
+	uint32_t argument, options;
+	struct sd_resp resp;
+
+	argument = handle->device->ctrl.rca << SD_CMD7_ARG_RCA_SHIFT;
+
+	options = SD_CMDR_RSP_TYPE_R1_5_6 << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_CCHK_EN_MASK |
+		  SD4_EMMC_TOP_CMD_CRC_EN_MASK;
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_SEND_STATUS, argument, options, &resp);
+
+	if (res == SD_OK) {
+		*status = resp.cardStatus;
+	}
+
+	return res;
+}
+
+int sd_cmd16(struct sd_handle *handle, uint32_t length)
+{
+	int res;
+	uint32_t argument, options, ntry;
+	struct sd_resp resp;
+
+	argument = length;
+
+	options = SD_CMDR_RSP_TYPE_R1_5_6 << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_CRC_EN_MASK |
+		  SD4_EMMC_TOP_CMD_CCHK_EN_MASK;
+
+	ntry = 0;
+	do {
+		res = sd_cmd13(handle, &resp.cardStatus);
+		if (res != SD_OK) {
+			EMMC_TRACE(
+				"cmd13 failed before cmd16: rca 0x%0x, return %d, response 0x%0x\n",
+				handle->device->ctrl.rca, res, resp.cardStatus);
+			return res;
+		}
+
+		if (resp.cardStatus & 0x100)
+			break;
+
+		EMMC_TRACE("cmd13 rsp:0x%08x before cmd16\n", resp.cardStatus);
+
+		if (ntry > handle->device->cfg.retryLimit) {
+			EMMC_TRACE("cmd13 retry reach limit %d\n",
+				   handle->device->cfg.retryLimit);
+			return SD_CMD_TIMEOUT;
+		}
+
+		ntry++;
+		EMMC_TRACE("cmd13 retry %d\n", ntry);
+
+		SD_US_DELAY(1000);
+
+	} while (1);
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_SET_BLOCKLEN, argument, options, &resp);
+
+	return res;
+}
+
+int sd_cmd17(struct sd_handle *handle,
+	     uint32_t addr, uint32_t len, uint8_t *buffer)
+{
+	int res;
+	uint32_t argument, options, ntry;
+	struct sd_resp resp;
+
+	ntry = 0;
+	do {
+		res = sd_cmd13(handle, &resp.cardStatus);
+		if (res != SD_OK) {
+			EMMC_TRACE(
+				"cmd 13 failed before cmd17: rca 0x%0x, return %d, response 0x%0x\n",
+				handle->device->ctrl.rca, res, resp.cardStatus);
+			return res;
+		}
+
+		if (resp.cardStatus & 0x100)
+			break;
+
+		EMMC_TRACE("cmd13 rsp:0x%08x before cmd17\n", resp.cardStatus);
+
+		if (ntry > handle->device->cfg.retryLimit) {
+			EMMC_TRACE("cmd13 retry reach limit %d\n",
+				   handle->device->cfg.retryLimit);
+			return SD_CMD_TIMEOUT;
+		}
+
+		ntry++;
+		EMMC_TRACE("cmd13 retry %d\n", ntry);
+
+		SD_US_DELAY(1000);
+
+	} while (1);
+
+	data_xfer_setup(handle, buffer, len, SD_XFER_CARD_TO_HOST);
+
+	/* send cmd and parse result */
+	argument = addr;
+	options = SD_CMDR_RSP_TYPE_R1_5_6 << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_DPS_MASK | SD4_EMMC_TOP_CMD_DTDS_MASK |
+		  SD4_EMMC_TOP_CMD_CRC_EN_MASK | SD4_EMMC_TOP_CMD_CCHK_EN_MASK;
+
+	res = send_cmd(handle, SD_CMD_READ_SINGLE_BLOCK, argument, options,
+		       &resp);
+
+	if (res != SD_OK)
+		return res;
+
+	res = process_data_xfer(handle, buffer, addr, len, SD_XFER_CARD_TO_HOST);
+
+	return res;
+}
+
+int sd_cmd18(struct sd_handle *handle,
+	     uint32_t addr, uint32_t len, uint8_t *buffer)
+{
+	int res;
+	uint32_t argument, options, ntry;
+	struct sd_resp resp;
+
+	ntry = 0;
+	do {
+		res = sd_cmd13(handle, &resp.cardStatus);
+		if (res != SD_OK) {
+			EMMC_TRACE(
+				"cmd 13 failed before cmd18: rca 0x%0x, return %d, response 0x%0x\n",
+				handle->device->ctrl.rca, res, resp.cardStatus);
+			return res;
+		}
+
+		if (resp.cardStatus & 0x100)
+			break;
+
+		EMMC_TRACE("cmd13 rsp:0x%08x before cmd18\n", resp.cardStatus);
+
+		if (ntry > handle->device->cfg.retryLimit) {
+			EMMC_TRACE("cmd13 retry reach limit %d\n",
+				   handle->device->cfg.retryLimit);
+			return SD_CMD_TIMEOUT;
+		}
+
+		ntry++;
+		EMMC_TRACE("cmd13 retry %d\n", ntry);
+
+		SD_US_DELAY(1000);
+	} while (1);
+
+	data_xfer_setup(handle, buffer, len, SD_XFER_CARD_TO_HOST);
+
+	argument = addr;
+
+	options = SD_CMDR_RSP_TYPE_R1_5_6 << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_DPS_MASK | SD4_EMMC_TOP_CMD_DTDS_MASK |
+		  SD4_EMMC_TOP_CMD_MSBS_MASK | SD4_EMMC_TOP_CMD_CCHK_EN_MASK |
+		  SD4_EMMC_TOP_CMD_BCEN_MASK | SD4_EMMC_TOP_CMD_CRC_EN_MASK |
+		  BIT(SD4_EMMC_TOP_CMD_ACMDEN_SHIFT);
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_READ_MULTIPLE_BLOCK, argument, options,
+		       &resp);
+
+	if (res != SD_OK)
+		return res;
+
+	res = process_data_xfer(handle, buffer, addr, len, SD_XFER_CARD_TO_HOST);
+
+	return res;
+}
+
+#ifdef INCLUDE_EMMC_DRIVER_ERASE_CODE
+static int card_sts_resp(struct sd_handle *handle, uint32_t *status)
+{
+	int res;
+	uint32_t ntry = 0;
+
+	do {
+		res = sd_cmd13(handle, status);
+		if (res != SD_OK) {
+			EMMC_TRACE(
+				"cmd 13 failed before cmd35: rca 0x%0x, return %d\n",
+				handle->device->ctrl.rca, res);
+			return res;
+		}
+
+		if (*status & 0x100)
+			break;
+
+		EMMC_TRACE("cmd13 rsp:0x%08x before cmd35\n", resp.cardStatus);
+
+		if (ntry > handle->device->cfg.retryLimit) {
+			EMMC_TRACE("cmd13 retry reach limit %d\n",
+				   handle->device->cfg.retryLimit);
+			return SD_CMD_TIMEOUT;
+		}
+
+		ntry++;
+		EMMC_TRACE("cmd13 retry %d\n", ntry);
+
+		SD_US_DELAY(1000);
+	} while (1);
+
+	return SD_OK;
+}
+
+int sd_cmd35(struct sd_handle *handle, uint32_t start)
+{
+	int res;
+	uint32_t argument, options;
+	struct sd_resp resp;
+
+	res = card_sts_resp(handle, &resp.cardStatus);
+	if (res != SD_OK)
+		return res;
+
+	argument = start;
+
+	options = SD_CMDR_RSP_TYPE_R1_5_6 << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_CRC_EN_MASK |
+		  SD4_EMMC_TOP_CMD_CCHK_EN_MASK;
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_ERASE_GROUP_START,
+		       argument, options, &resp);
+
+	if (res != SD_OK)
+		return res;
+
+	return res;
+}
+
+int sd_cmd36(struct sd_handle *handle, uint32_t end)
+{
+	int res;
+	uint32_t argument, options;
+	struct sd_resp resp;
+
+	res = card_sts_resp(handle, &resp.cardStatus);
+	if (res != SD_OK)
+		return res;
+
+	argument = end;
+
+	options = SD_CMDR_RSP_TYPE_R1_5_6 << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_CRC_EN_MASK |
+		  SD4_EMMC_TOP_CMD_CCHK_EN_MASK;
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_ERASE_GROUP_END,
+		       argument, options, &resp);
+
+	if (res != SD_OK)
+		return res;
+
+	return res;
+}
+
+int sd_cmd38(struct sd_handle *handle)
+{
+	int res;
+	uint32_t argument, options;
+	struct sd_resp resp;
+
+	res = card_sts_resp(handle, &resp.cardStatus);
+	if (res != SD_OK)
+		return res;
+
+	argument = 0;
+
+	options = (SD_CMDR_RSP_TYPE_R1b_5b << SD_CMDR_RSP_TYPE_S) |
+		  SD4_EMMC_TOP_CMD_CRC_EN_MASK |
+		  SD4_EMMC_TOP_CMD_CCHK_EN_MASK;
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_ERASE, argument, options, &resp);
+
+	if (res != SD_OK)
+		return res;
+
+	return res;
+}
+#endif
+
+#ifdef INCLUDE_EMMC_DRIVER_WRITE_CODE
+
+int sd_cmd24(struct sd_handle *handle,
+	     uint32_t addr, uint32_t len, uint8_t *buffer)
+{
+	int res;
+	uint32_t argument, options, ntry;
+	struct sd_resp resp;
+
+	ntry = 0;
+	do {
+		res = sd_cmd13(handle, &resp.cardStatus);
+		if (res != SD_OK) {
+			EMMC_TRACE(
+				"cmd 13 failed before cmd24: rca 0x%0x, return %d, response 0x%0x\n",
+				handle->device->ctrl.rca, res, &resp.cardStatus);
+			return res;
+		}
+
+		if (resp.cardStatus & 0x100)
+			break;
+
+		EMMC_TRACE("cmd13 rsp:0x%08x before cmd24\n", resp.cardStatus);
+
+		if (ntry > handle->device->cfg.retryLimit) {
+			EMMC_TRACE("cmd13 retry reach limit %d\n",
+				   handle->device->cfg.retryLimit);
+			return SD_CMD_TIMEOUT;
+		}
+
+		ntry++;
+		EMMC_TRACE("cmd13 retry %d\n", ntry);
+
+		SD_US_DELAY(1000);
+
+	} while (1);
+
+	data_xfer_setup(handle, buffer, len, SD_XFER_HOST_TO_CARD);
+
+	argument = addr;
+
+	options = SD_CMDR_RSP_TYPE_R1_5_6 << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_DPS_MASK | SD4_EMMC_TOP_CMD_CRC_EN_MASK |
+		  SD4_EMMC_TOP_CMD_CCHK_EN_MASK;
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_WRITE_BLOCK, argument, options, &resp);
+
+	if (res != SD_OK)
+		return res;
+
+	res = process_data_xfer(handle, buffer, addr, len, SD_XFER_HOST_TO_CARD);
+
+	return res;
+}
+
+int sd_cmd25(struct sd_handle *handle,
+	     uint32_t addr, uint32_t len, uint8_t *buffer)
+{
+	int res = SD_OK;
+	uint32_t argument, options, ntry;
+	struct sd_resp resp;
+
+	ntry = 0;
+	do {
+		res = sd_cmd13(handle, &resp.cardStatus);
+		if (res != SD_OK) {
+			EMMC_TRACE(
+				"cmd 13 failed before cmd25: rca 0x%0x, return %d, response 0x%0x\n",
+				handle->device->ctrl.rca, res, &resp.cardStatus);
+			return res;
+		}
+
+		if (resp.cardStatus & 0x100)
+			break;
+
+		EMMC_TRACE("cmd13 rsp:0x%08x before cmd25\n", resp.cardStatus);
+
+		if (ntry > handle->device->cfg.retryLimit) {
+			EMMC_TRACE("cmd13 retry reach limit %d\n",
+				   handle->device->cfg.retryLimit);
+			return SD_CMD_TIMEOUT;
+		}
+
+		ntry++;
+		EMMC_TRACE("cmd13 retry %d\n", ntry);
+
+		SD_US_DELAY(1000);
+	} while (1);
+
+	data_xfer_setup(handle, buffer, len, SD_XFER_HOST_TO_CARD);
+
+	argument = addr;
+
+	options = SD_CMDR_RSP_TYPE_R1_5_6 << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_DPS_MASK | SD4_EMMC_TOP_CMD_MSBS_MASK |
+		  SD4_EMMC_TOP_CMD_CCHK_EN_MASK | SD4_EMMC_TOP_CMD_BCEN_MASK |
+		  SD4_EMMC_TOP_CMD_CRC_EN_MASK |
+		  BIT(SD4_EMMC_TOP_CMD_ACMDEN_SHIFT);
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_CMD_WRITE_MULTIPLE_BLOCK,
+		       argument, options, &resp);
+
+	if (res != SD_OK)
+		return res;
+
+	res = process_data_xfer(handle, buffer, addr, len, SD_XFER_HOST_TO_CARD);
+
+	return res;
+}
+#endif /* INCLUDE_EMMC_DRIVER_WRITE_CODE */
+
+int mmc_cmd6(struct sd_handle *handle, uint32_t argument)
+{
+	int res;
+	uint32_t options;
+	struct sd_resp resp;
+
+	options = SD_CMDR_RSP_TYPE_R1b_5b << SD_CMDR_RSP_TYPE_S |
+		  SD4_EMMC_TOP_CMD_CCHK_EN_MASK | SD4_EMMC_TOP_CMD_CRC_EN_MASK;
+
+	EMMC_TRACE("Sending CMD6 with argument 0x%X\n", argument);
+
+	/* send cmd and parse result */
+	res = send_cmd(handle, SD_ACMD_SET_BUS_WIDTH, argument, options, &resp);
+
+	/*
+	 * For R1b type response:
+	 * controller issues a COMMAND COMPLETE interrupt when the R1
+	 * response is received,
+	 * then controller monitors DAT0 for busy status,
+	 * controller issues a TRANSFER COMPLETE interrupt when busy signal
+	 * clears.
+	 */
+	wait_for_event(handle,
+		       SD4_EMMC_TOP_INTR_TXDONE_MASK | SD_ERR_INTERRUPTS,
+		       handle->device->cfg.wfe_retry);
+
+	if (res == SD_OK) {
+		/* Check result of Cmd6 using Cmd13 to check card status */
+
+		/* Check status using Cmd13 */
+		res = sd_cmd13(handle, &resp.cardStatus);
+
+		if (res == SD_OK) {
+			/* Check bit 7 (SWITCH_ERROR) in card status */
+			if ((resp.cardStatus & 0x80) != 0) {
+				EMMC_TRACE("cmd6 failed: SWITCH_ERROR\n");
+				res = SD_FAIL;
+			}
+		} else {
+			EMMC_TRACE("cmd13 failed after cmd6: ");
+			EMMC_TRACE("rca 0x%0x, return %d, response 0x%0x\n",
+				handle->device->ctrl.rca, res, resp.cardStatus);
+		}
+	}
+
+	return res;
+}
+
+
+#define SD_BUSY_CHECK		0x00203000
+#define DAT0_LEVEL_MASK		0x100000	/* bit20 in PSTATE */
+#define DEV_BUSY_TIMEOUT	600000		/* 60 Sec : 600000 * 100us */
+
+int send_cmd(struct sd_handle *handle, uint32_t cmdIndex, uint32_t argument,
+	     uint32_t options, struct sd_resp *resp)
+{
+	int status = SD_OK;
+	uint32_t event = 0, present, timeout = 0, retry = 0, mask = 3;
+	uint32_t temp_resp[4];
+
+	if (handle == NULL) {
+		EMMC_TRACE("Invalid handle for cmd%d\n", cmdIndex);
+		return SD_INVALID_HANDLE;
+	}
+
+	mask = (SD_BUSY_CHECK & options) ? 3 : 1;
+
+RETRY_WRITE_CMD:
+	do {
+		/* Make sure it is ok to send command */
+		present =
+		    chal_sd_get_present_status((CHAL_HANDLE *) handle->device);
+		timeout++;
+
+		if (present & mask)
+			SD_US_DELAY(1000);
+		else
+			break;
+
+	} while (timeout < EMMC_BUSY_CMD_TIMEOUT_MS);
+
+	if (timeout >= EMMC_BUSY_CMD_TIMEOUT_MS) {
+		status = SD_CMD_MISSING;
+		EMMC_TRACE("cmd%d timedout %dms\n", cmdIndex, timeout);
+	}
+
+	/* Reset both DAT and CMD line if only of them are stuck */
+	if (present & mask)
+		check_error(handle, SD4_EMMC_TOP_INTR_CMDERROR_MASK);
+
+	handle->device->ctrl.argReg = argument;
+	chal_sd_send_cmd((CHAL_HANDLE *) handle->device, cmdIndex,
+			 handle->device->ctrl.argReg, options);
+
+	handle->device->ctrl.cmdIndex = cmdIndex;
+
+	event = wait_for_event(handle,
+			       (SD4_EMMC_TOP_INTR_CMDDONE_MASK |
+				SD_ERR_INTERRUPTS),
+			       handle->device->cfg.wfe_retry);
+
+	if (handle->device->ctrl.cmdStatus == SD_CMD_MISSING) {
+		retry++;
+
+		if (retry >= handle->device->cfg.retryLimit) {
+			status = SD_CMD_MISSING;
+			EMMC_TRACE("cmd%d retry reaches the limit %d\n",
+				   cmdIndex, retry);
+		} else {
+			/* reset both DAT & CMD line if one of them is stuck */
+			present = chal_sd_get_present_status((CHAL_HANDLE *)
+							     handle->device);
+
+			if (present & mask)
+				check_error(handle,
+					    SD4_EMMC_TOP_INTR_CMDERROR_MASK);
+
+			EMMC_TRACE("cmd%d retry %d PSTATE[0x%08x]\n",
+				   cmdIndex, retry,
+				   chal_sd_get_present_status((CHAL_HANDLE *)
+							      handle->device));
+			goto RETRY_WRITE_CMD;
+		}
+	}
+
+	if (handle->device->ctrl.cmdStatus == SD_OK) {
+		if (resp != NULL) {
+			status =
+			    chal_sd_get_response((CHAL_HANDLE *) handle->device,
+						 temp_resp);
+			process_cmd_response(handle,
+					     handle->device->ctrl.cmdIndex,
+					     temp_resp[0], temp_resp[1],
+					     temp_resp[2], temp_resp[3], resp);
+		}
+
+		/* Check Device busy after CMD */
+		if ((cmdIndex == 5) || (cmdIndex == 6) || (cmdIndex == 7) ||
+		    (cmdIndex == 28) || (cmdIndex == 29) || (cmdIndex == 38)) {
+
+			timeout = 0;
+			do {
+				present =
+				    chal_sd_get_present_status((CHAL_HANDLE *)
+							       handle->device);
+
+				timeout++;
+
+				/* Dat[0]:bit20 low means device busy */
+				if ((present & DAT0_LEVEL_MASK) == 0) {
+					EMMC_TRACE("Device busy: ");
+					EMMC_TRACE(
+					  "cmd%d arg:0x%08x: PSTATE[0x%08x]\n",
+					  cmdIndex, argument, present);
+					SD_US_DELAY(100);
+				} else {
+					break;
+				}
+			} while (timeout < DEV_BUSY_TIMEOUT);
+		}
+	} else if (handle->device->ctrl.cmdStatus &&
+		   handle->device->ctrl.cmdStatus != SD_CMD_MISSING) {
+		retry++;
+		status = check_error(handle, handle->device->ctrl.cmdStatus);
+
+		EMMC_TRACE(
+			"cmd%d error: cmdStatus:0x%08x error_status:0x%08x\n",
+			cmdIndex, handle->device->ctrl.cmdStatus, status);
+
+		if ((handle->device->ctrl.cmdIndex == 1) ||
+		    (handle->device->ctrl.cmdIndex == 5)) {
+			status = event;
+		} else if ((handle->device->ctrl.cmdIndex == 7) ||
+			   (handle->device->ctrl.cmdIndex == 41)) {
+			status = event;
+		} else if ((status == SD_ERROR_RECOVERABLE) &&
+			   (retry < handle->device->cfg.retryLimit)) {
+			EMMC_TRACE("cmd%d recoverable error ", cmdIndex);
+			EMMC_TRACE("retry %d PSTATE[0x%08x].\n", retry,
+				   chal_sd_get_present_status((CHAL_HANDLE *)
+							      handle->device));
+			goto RETRY_WRITE_CMD;
+		} else {
+			EMMC_TRACE("cmd%d retry reaches the limit %d\n",
+				   cmdIndex, retry);
+			status = event;
+		}
+	}
+
+	handle->device->ctrl.blkReg = 0;
+	/* clear error status for next command */
+	handle->device->ctrl.cmdStatus = 0;
+
+	return status;
+}
diff --git a/drivers/brcm/emmc/emmc_pboot_hal_memory_drv.c b/drivers/brcm/emmc/emmc_pboot_hal_memory_drv.c
new file mode 100644
index 0000000..68f93e7
--- /dev/null
+++ b/drivers/brcm/emmc/emmc_pboot_hal_memory_drv.c
@@ -0,0 +1,621 @@
+/*
+ * Copyright (c) 2016 - 2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+
+#include <emmc_api.h>
+#include <cmn_plat_util.h>
+
+#define MAX_CMD_RETRY      10
+
+#if EMMC_USE_DMA
+#define USE_DMA 1
+#else
+#define USE_DMA 0
+#endif
+
+struct emmc_global_buffer emmc_global_buf;
+struct emmc_global_buffer *emmc_global_buf_ptr = &emmc_global_buf;
+
+struct emmc_global_vars emmc_global_vars;
+struct emmc_global_vars *emmc_global_vars_ptr = &emmc_global_vars;
+
+static struct sd_handle *sdio_gethandle(void);
+static uint32_t sdio_idle(struct sd_handle *p_sdhandle);
+
+static uint32_t sdio_read(struct sd_handle *p_sdhandle,
+			  uintptr_t mem_addr,
+			  uintptr_t storage_addr,
+			  size_t storage_size,
+			  size_t bytes_to_read);
+
+#ifdef INCLUDE_EMMC_DRIVER_WRITE_CODE
+static uint32_t sdio_write(struct sd_handle *p_sdhandle,
+			   uintptr_t mem_addr,
+			   uintptr_t data_addr,
+			   size_t bytes_to_write);
+#endif
+
+static struct sd_handle *sdio_init(void);
+static int32_t bcm_emmc_card_ready_state(struct sd_handle *p_sdhandle);
+
+static void init_globals(void)
+{
+	memset((void *)emmc_global_buf_ptr, 0, sizeof(*emmc_global_buf_ptr));
+	memset((void *)emmc_global_vars_ptr, 0, sizeof(*emmc_global_vars_ptr));
+}
+
+/*
+ * This function is used to change partition
+ */
+uint32_t emmc_partition_select(uint32_t partition)
+{
+	int rc;
+	struct sd_handle *sd_handle = sdio_gethandle();
+
+	if (sd_handle->device == 0) {
+		EMMC_TRACE("eMMC init is not done");
+		return 0;
+	}
+
+	switch (partition) {
+	case EMMC_BOOT_PARTITION1:
+		rc = set_boot_config(sd_handle,
+				     SDIO_HW_EMMC_EXT_CSD_BOOT_ACC_BOOT1);
+		EMMC_TRACE(
+		     "Change to Boot Partition 1 result:%d  (0 means SD_OK)\n",
+		     rc);
+		break;
+
+	case EMMC_BOOT_PARTITION2:
+		rc = set_boot_config(sd_handle,
+				     SDIO_HW_EMMC_EXT_CSD_BOOT_ACC_BOOT2);
+		EMMC_TRACE(
+		     "Change to Boot Partition 2 result:%d  (0 means SD_OK)\n",
+		     rc);
+		break;
+
+	case EMMC_USE_CURRENT_PARTITION:
+		rc = SD_OK;
+		EMMC_TRACE("Stay on current partition");
+		break;
+
+	case EMMC_USER_AREA:
+	default:
+		rc = set_boot_config(sd_handle,
+				     SDIO_HW_EMMC_EXT_CSD_BOOT_ACC_USER);
+		EMMC_TRACE("Change to User area result:%d  (0 means SD_OK)\n",
+			   rc);
+		break;
+
+	}
+	return (rc == SD_OK);
+}
+
+/*
+ * Initialize emmc controller for eMMC
+ * Returns 0 on fail condition
+ */
+uint32_t bcm_emmc_init(bool card_rdy_only)
+{
+	struct sd_handle *p_sdhandle;
+	uint32_t result = 0;
+
+	EMMC_TRACE("Enter emmc_controller_init()\n");
+
+	/* If eMMC is already initialized, skip init */
+	if (emmc_global_vars_ptr->init_done)
+		return 1;
+
+	init_globals();
+
+	p_sdhandle = sdio_init();
+
+	if (p_sdhandle == NULL) {
+		ERROR("eMMC init failed");
+		return result;
+	}
+
+	if (card_rdy_only) {
+		/* Put the card in Ready state, Not complete init */
+		result = bcm_emmc_card_ready_state(p_sdhandle);
+		return !result;
+	}
+
+	if (sdio_idle(p_sdhandle) == EMMC_BOOT_OK) {
+		set_config(p_sdhandle, SD_NORMAL_SPEED, MAX_CMD_RETRY, USE_DMA,
+			   SD_DMA_BOUNDARY_256K, EMMC_BLOCK_SIZE,
+			   EMMC_WFE_RETRY);
+
+		if (!select_blk_sz(p_sdhandle,
+				   p_sdhandle->device->cfg.blockSize)) {
+			emmc_global_vars_ptr->init_done = 1;
+			result = 1;
+		} else {
+			ERROR("Select Block Size failed\n");
+		}
+	} else {
+		ERROR("eMMC init failed");
+	}
+
+	/* Initialization is failed, so deinit HW setting */
+	if (result == 0)
+		emmc_deinit();
+
+	return result;
+}
+
+/*
+ * Function to de-init SDIO controller for eMMC
+ */
+void emmc_deinit(void)
+{
+	emmc_global_vars_ptr->init_done = 0;
+	emmc_global_vars_ptr->sdHandle.card = 0;
+	emmc_global_vars_ptr->sdHandle.device = 0;
+}
+
+/*
+ * Read eMMC memory
+ * Returns read_size
+ */
+uint32_t emmc_read(uintptr_t mem_addr, uintptr_t storage_addr,
+		   size_t storage_size, size_t bytes_to_read)
+{
+	struct sd_handle *sd_handle = sdio_gethandle();
+
+	if (sd_handle->device == 0) {
+		EMMC_TRACE("eMMC init is not done");
+		return 0;
+	}
+
+	return sdio_read(sdio_gethandle(), mem_addr, storage_addr,
+			 storage_size, bytes_to_read);
+}
+
+#ifdef INCLUDE_EMMC_DRIVER_ERASE_CODE
+#define EXT_CSD_ERASE_GRP_SIZE 224
+
+static int emmc_block_erase(uintptr_t mem_addr, size_t blocks)
+{
+	struct sd_handle *sd_handle = sdio_gethandle();
+
+	if (sd_handle->device == 0) {
+		ERROR("eMMC init is not done");
+		return -1;
+	}
+
+	return erase_card(sdio_gethandle(), mem_addr, blocks);
+}
+
+int emmc_erase(uintptr_t mem_addr, size_t num_of_blocks, uint32_t partition)
+{
+	int err = 0;
+	size_t block_count = 0, blocks = 0;
+	size_t erase_group = 0;
+
+	erase_group =
+	emmc_global_buf_ptr->u.Ext_CSD_storage[EXT_CSD_ERASE_GRP_SIZE]*1024;
+
+	INFO("eMMC Erase Group Size=0x%lx\n", erase_group);
+
+	emmc_partition_select(partition);
+
+	while (block_count < num_of_blocks) {
+		blocks = ((num_of_blocks - block_count) > erase_group) ?
+				  erase_group : (num_of_blocks - block_count);
+			err = emmc_block_erase(mem_addr + block_count, blocks);
+		if (err)
+			break;
+
+		block_count += blocks;
+	}
+
+	if (err == 0)
+		INFO("eMMC Erase of partition %d successful\n", partition);
+	else
+		ERROR("eMMC Erase of partition %d Failed(%i)\n", partition, err);
+
+	return err;
+}
+#endif
+
+#ifdef INCLUDE_EMMC_DRIVER_WRITE_CODE
+/*
+ * Write to eMMC memory
+ * Returns written_size
+ */
+uint32_t emmc_write(uintptr_t mem_addr, uintptr_t data_addr,
+		    size_t bytes_to_write)
+{
+	struct sd_handle *sd_handle = sdio_gethandle();
+
+	if (sd_handle->device == 0) {
+		EMMC_TRACE("eMMC init is not done");
+		return 0;
+	}
+
+	return sdio_write(sd_handle, mem_addr, data_addr, bytes_to_write);
+}
+#endif
+
+/*
+ * Send SDIO Cmd
+ * Return 0 for pass condition
+ */
+uint32_t send_sdio_cmd(uint32_t cmdIndex, uint32_t argument,
+		       uint32_t options, struct sd_resp *resp)
+{
+	struct sd_handle *sd_handle = sdio_gethandle();
+
+	if (sd_handle->device == 0) {
+		EMMC_TRACE("eMMC init is not done");
+		return 1;
+	}
+
+	return send_cmd(sd_handle, cmdIndex, argument, options, resp);
+}
+
+
+/*
+ * This function return SDIO handle
+ */
+struct sd_handle *sdio_gethandle(void)
+{
+	return &emmc_global_vars_ptr->sdHandle;
+}
+
+/*
+ * Initialize SDIO controller
+ */
+struct sd_handle *sdio_init(void)
+{
+	uint32_t SDIO_base;
+	struct sd_handle *p_sdhandle = &emmc_global_vars_ptr->sdHandle;
+
+	SDIO_base = EMMC_CTRL_REGS_BASE_ADDR;
+
+	if (SDIO_base == SDIO0_EMMCSDXC_SYSADDR)
+		EMMC_TRACE(" ---> for SDIO 0 Controller\n\n");
+
+	memset(p_sdhandle, 0, sizeof(struct sd_handle));
+
+	p_sdhandle->device = &emmc_global_vars_ptr->sdDevice;
+	p_sdhandle->card = &emmc_global_vars_ptr->sdCard;
+
+	memset(p_sdhandle->device, 0, sizeof(struct sd_dev));
+	memset(p_sdhandle->card, 0, sizeof(struct sd_card_info));
+
+	if (chal_sd_start((CHAL_HANDLE *) p_sdhandle->device,
+			  SD_PIO_MODE, SDIO_base, SDIO_base) != SD_OK)
+		return NULL;
+
+	set_config(p_sdhandle, SD_NORMAL_SPEED, MAX_CMD_RETRY, SD_DMA_OFF,
+		   SD_DMA_BOUNDARY_4K, EMMC_BLOCK_SIZE, EMMC_WFE_RETRY);
+
+	return &emmc_global_vars_ptr->sdHandle;
+}
+
+uint32_t sdio_idle(struct sd_handle *p_sdhandle)
+{
+	reset_card(p_sdhandle);
+
+	SD_US_DELAY(1000);
+
+	if (init_card(p_sdhandle, SD_CARD_DETECT_MMC) != SD_OK) {
+		reset_card(p_sdhandle);
+		reset_host_ctrl(p_sdhandle);
+		return EMMC_BOOT_NO_CARD;
+	}
+
+	return EMMC_BOOT_OK;
+}
+
+/*
+ * This function read eMMC
+ */
+uint32_t sdio_read(struct sd_handle *p_sdhandle,
+		   uintptr_t mem_addr,
+		   uintptr_t storage_addr,
+		   size_t storage_size, size_t bytes_to_read)
+{
+	uint32_t offset = 0, blockAddr, readLen = 0, rdCount;
+	uint32_t remSize, manual_copy_size;
+	uint8_t *outputBuf = (uint8_t *) storage_addr;
+	const size_t blockSize = p_sdhandle->device->cfg.blockSize;
+
+	VERBOSE("EMMC READ: dst=0x%lx, src=0x%lx, size=0x%lx\n",
+			storage_addr, mem_addr, bytes_to_read);
+
+	if (storage_size < bytes_to_read)
+		/* Don't have sufficient storage to complete the operation */
+		return 0;
+
+	/* Range check non high capacity memory */
+	if ((p_sdhandle->device->ctrl.ocr & SD_CARD_HIGH_CAPACITY) == 0) {
+		if (mem_addr > 0x80000000)
+			return 0;
+	}
+
+	/* High capacity card use block address mode */
+	if (p_sdhandle->device->ctrl.ocr & SD_CARD_HIGH_CAPACITY) {
+		blockAddr = (uint32_t) (mem_addr / blockSize);
+		offset = (uint32_t) (mem_addr - (blockAddr * blockSize));
+	} else {
+		blockAddr = (uint32_t) (mem_addr / blockSize) * blockSize;
+		offset = (uint32_t) (mem_addr - blockAddr);
+	}
+
+	remSize = bytes_to_read;
+
+	rdCount = 0;
+
+	/* Process first unaligned block of MAX_READ_LENGTH */
+	if (offset > 0) {
+		if (!read_block(p_sdhandle, emmc_global_buf_ptr->u.tempbuf,
+				blockAddr, SD_MAX_READ_LENGTH)) {
+
+			if (remSize < (blockSize - offset)) {
+				rdCount += remSize;
+				manual_copy_size = remSize;
+				remSize = 0;	/* read is done */
+			} else {
+				remSize -= (blockSize - offset);
+				rdCount += (blockSize - offset);
+				manual_copy_size = blockSize - offset;
+			}
+
+			/* Check for overflow */
+			if (manual_copy_size > storage_size ||
+			    (((uintptr_t)outputBuf + manual_copy_size) >
+			     (storage_addr + storage_size))) {
+				ERROR("EMMC READ: Overflow 1\n");
+				return 0;
+			}
+
+			memcpy(outputBuf,
+			       (void *)((uintptr_t)
+				(emmc_global_buf_ptr->u.tempbuf + offset)),
+			       manual_copy_size);
+
+			/* Update Physical address */
+			outputBuf += manual_copy_size;
+
+			if (p_sdhandle->device->ctrl.ocr & SD_CARD_HIGH_CAPACITY)
+				blockAddr++;
+			else
+				blockAddr += blockSize;
+		} else {
+			return 0;
+		}
+	}
+
+	while (remSize >= blockSize) {
+
+		if (remSize >= SD_MAX_BLK_TRANSFER_LENGTH)
+			readLen = SD_MAX_BLK_TRANSFER_LENGTH;
+		else
+			readLen = (remSize / blockSize) * blockSize;
+
+		/* Check for overflow */
+		if ((rdCount + readLen) > storage_size ||
+		    (((uintptr_t) outputBuf + readLen) >
+		     (storage_addr + storage_size))) {
+			ERROR("EMMC READ: Overflow\n");
+			return 0;
+		}
+
+		if (!read_block(p_sdhandle, outputBuf, blockAddr, readLen)) {
+			if (p_sdhandle->device->ctrl.ocr & SD_CARD_HIGH_CAPACITY)
+				blockAddr += (readLen / blockSize);
+			else
+				blockAddr += readLen;
+
+			remSize -= readLen;
+			rdCount += readLen;
+
+			/* Update Physical address */
+			outputBuf += readLen;
+		} else {
+			return 0;
+		}
+	}
+
+	/* process the last unaligned block reading */
+	if (remSize > 0) {
+		if (!read_block(p_sdhandle, emmc_global_buf_ptr->u.tempbuf,
+				blockAddr, SD_MAX_READ_LENGTH)) {
+
+			rdCount += remSize;
+			/* Check for overflow */
+			if (rdCount > storage_size ||
+			    (((uintptr_t) outputBuf + remSize) >
+			     (storage_addr + storage_size))) {
+				ERROR("EMMC READ: Overflow\n");
+				return 0;
+			}
+
+			memcpy(outputBuf,
+				emmc_global_buf_ptr->u.tempbuf, remSize);
+
+			/* Update Physical address */
+			outputBuf += remSize;
+		} else {
+			rdCount = 0;
+		}
+	}
+
+	return rdCount;
+}
+
+#ifdef INCLUDE_EMMC_DRIVER_WRITE_CODE
+static uint32_t sdio_write(struct sd_handle *p_sdhandle, uintptr_t mem_addr,
+			   uintptr_t data_addr, size_t bytes_to_write)
+{
+
+	uint32_t offset, blockAddr, writeLen, wtCount = 0;
+	uint32_t remSize, manual_copy_size = 0;
+
+	uint8_t *inputBuf = (uint8_t *)data_addr;
+
+	/* range check non high capacity memory */
+	if ((p_sdhandle->device->ctrl.ocr & SD_CARD_HIGH_CAPACITY) == 0) {
+		if (mem_addr > 0x80000000)
+			return 0;
+	}
+
+	/* the high capacity card use block address mode */
+	if (p_sdhandle->device->ctrl.ocr & SD_CARD_HIGH_CAPACITY) {
+		blockAddr =
+		    (uint32_t)(mem_addr / p_sdhandle->device->cfg.blockSize);
+		offset =
+		    (uint32_t)(mem_addr -
+			       blockAddr * p_sdhandle->device->cfg.blockSize);
+	} else {
+		blockAddr =
+		    ((uint32_t)mem_addr / p_sdhandle->device->cfg.blockSize) *
+		    p_sdhandle->device->cfg.blockSize;
+		offset = (uint32_t) mem_addr - blockAddr;
+	}
+
+	remSize = bytes_to_write;
+
+	wtCount = 0;
+
+	/* process first unaligned block */
+	if (offset > 0) {
+		if (!read_block(p_sdhandle, emmc_global_buf_ptr->u.tempbuf,
+				blockAddr, p_sdhandle->device->cfg.blockSize)) {
+
+			if (remSize <
+			    (p_sdhandle->device->cfg.blockSize - offset))
+				manual_copy_size = remSize;
+			else
+				manual_copy_size =
+				    p_sdhandle->device->cfg.blockSize - offset;
+
+			memcpy((void *)((uintptr_t)
+				(emmc_global_buf_ptr->u.tempbuf + offset)),
+			       inputBuf,
+			       manual_copy_size);
+
+			/* Update Physical address */
+
+			if (!write_block(p_sdhandle,
+					 emmc_global_buf_ptr->u.tempbuf,
+					 blockAddr,
+					 p_sdhandle->device->cfg.blockSize)) {
+
+				if (remSize <
+				    (p_sdhandle->device->cfg.blockSize -
+				     offset)) {
+					wtCount += remSize;
+					manual_copy_size = remSize;
+					remSize = 0;	/* read is done */
+				} else {
+					remSize -=
+					    (p_sdhandle->device->cfg.blockSize -
+					     offset);
+					wtCount +=
+					    (p_sdhandle->device->cfg.blockSize -
+					     offset);
+					manual_copy_size =
+					    p_sdhandle->device->cfg.blockSize -
+					    offset;
+				}
+
+				inputBuf += manual_copy_size;
+
+				if (p_sdhandle->device->ctrl.ocr &
+				    SD_CARD_HIGH_CAPACITY)
+					blockAddr++;
+				else
+					blockAddr +=
+					    p_sdhandle->device->cfg.blockSize;
+			} else
+				return 0;
+		} else {
+			return 0;
+		}
+	}
+
+	/* process block writing */
+	while (remSize >= p_sdhandle->device->cfg.blockSize) {
+		if (remSize >= SD_MAX_READ_LENGTH) {
+			writeLen = SD_MAX_READ_LENGTH;
+		} else {
+			writeLen =
+			    (remSize / p_sdhandle->device->cfg.blockSize) *
+			     p_sdhandle->device->cfg.blockSize;
+		}
+
+		if (!write_block(p_sdhandle, inputBuf, blockAddr, writeLen)) {
+			if (p_sdhandle->device->ctrl.ocr & SD_CARD_HIGH_CAPACITY)
+				blockAddr +=
+				    (writeLen /
+				     p_sdhandle->device->cfg.blockSize);
+			else
+				blockAddr += writeLen;
+
+			remSize -= writeLen;
+			wtCount += writeLen;
+			inputBuf += writeLen;
+		} else {
+			return 0;
+		}
+	}
+
+	/* process the last unaligned block reading */
+	if (remSize > 0) {
+		if (!read_block(p_sdhandle,
+				emmc_global_buf_ptr->u.tempbuf,
+				blockAddr, p_sdhandle->device->cfg.blockSize)) {
+
+			memcpy(emmc_global_buf_ptr->u.tempbuf,
+			       inputBuf, remSize);
+
+			/* Update Physical address */
+
+			if (!write_block(p_sdhandle,
+					 emmc_global_buf_ptr->u.tempbuf,
+					 blockAddr,
+					 p_sdhandle->device->cfg.blockSize)) {
+				wtCount += remSize;
+				inputBuf += remSize;
+			} else {
+				return 0;
+			}
+		} else {
+			wtCount = 0;
+		}
+	}
+
+	return wtCount;
+}
+#endif
+
+/*
+ * Function to put the card in Ready state by sending CMD0 and CMD1
+ */
+static int32_t bcm_emmc_card_ready_state(struct sd_handle *p_sdhandle)
+{
+	int32_t result = 0;
+	uint32_t argument = MMC_CMD_IDLE_RESET_ARG; /* Exit from Boot mode */
+
+	if (p_sdhandle) {
+		send_sdio_cmd(SD_CMD_GO_IDLE_STATE, argument, 0, NULL);
+
+		result = reset_card(p_sdhandle);
+		if (result != SD_OK) {
+			EMMC_TRACE("eMMC Reset error\n");
+			return SD_RESET_ERROR;
+		}
+		SD_US_DELAY(2000);
+		result = mmc_cmd1(p_sdhandle);
+	}
+
+	return result;
+}
diff --git a/drivers/brcm/i2c/i2c.c b/drivers/brcm/i2c/i2c.c
new file mode 100644
index 0000000..2096a82
--- /dev/null
+++ b/drivers/brcm/i2c/i2c.c
@@ -0,0 +1,886 @@
+/*
+ * Copyright (c) 2016 - 2021, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <i2c.h>
+#include <i2c_regs.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+/* Max instances */
+#define MAX_I2C					2U
+
+/* Transaction error codes defined in Master command register (0x30) */
+#define MSTR_STS_XACT_SUCCESS			0U
+#define MSTR_STS_LOST_ARB			1U
+#define MSTR_STS_NACK_FIRST_BYTE		2U
+ /* NACK on a byte other than the first byte */
+#define MSTR_STS_NACK_NON_FIRST_BYTE		3U
+
+#define MSTR_STS_TTIMEOUT_EXCEEDED		4U
+#define MSTR_STS_TX_TLOW_MEXT_EXCEEDED		5U
+#define MSTR_STS_RX_TLOW_MEXT_EXCEEDED		6U
+
+/* SMBUS protocol values defined in register 0x30 */
+#define SMBUS_PROT_QUICK_CMD			0U
+#define SMBUS_PROT_SEND_BYTE			1U
+#define SMBUS_PROT_RECV_BYTE			2U
+#define SMBUS_PROT_WR_BYTE			3U
+#define SMBUS_PROT_RD_BYTE			4U
+#define SMBUS_PROT_WR_WORD			5U
+#define SMBUS_PROT_RD_WORD			6U
+#define SMBUS_PROT_BLK_WR			7U
+#define SMBUS_PROT_BLK_RD			8U
+#define SMBUS_PROT_PROC_CALL			9U
+#define SMBUS_PROT_BLK_WR_BLK_RD_PROC_CALL	10U
+
+/* Number can be changed later */
+#define BUS_BUSY_COUNT				100000U
+
+#define IPROC_I2C_INVALID_ADDR			0xFFU
+
+#define I2C_SMBUS_BLOCK_MAX			32U
+
+/*
+ * Enum to specify clock speed. The user will provide it during initialization.
+ * If needed, it can be changed dynamically
+ */
+typedef enum iproc_smb_clk_freq {
+	IPROC_SMB_SPEED_100KHz = 0,
+	IPROC_SMB_SPEED_400KHz = 1,
+	IPROC_SMB_SPEED_INVALID = 255
+} smb_clk_freq_t;
+
+/* Structure used to pass information to read/write functions. */
+struct iproc_xact_info {
+	/* Bus Identifier */
+	uint32_t bus_id;
+	/* Device Address */
+	uint8_t devaddr;
+	/* Passed by caller to send SMBus command cod e*/
+	uint8_t command;
+	/* actual data passed by the caller */
+	uint8_t *data;
+	/* Size of data buffer passed */
+	uint32_t size;
+	/* Sent by caller specifying PEC, 10-bit addresses */
+	uint16_t flags;
+	/* SMBus protocol to use to perform transaction */
+	uint8_t smb_proto;
+	/* true if command field below is valid. Otherwise, false */
+	uint32_t cmd_valid;
+};
+
+static const uintptr_t smbus_base_reg_addr[MAX_I2C] = {
+	SMBUS0_REGS_BASE,
+	SMBUS1_REGS_BASE
+};
+
+/* Function to read a value from specified register. */
+static uint32_t iproc_i2c_reg_read(uint32_t bus_id, unsigned long reg_addr)
+{
+	uint32_t val;
+	uintptr_t smbus;
+
+	smbus = smbus_base_reg_addr[bus_id];
+
+	val = mmio_read_32(smbus + reg_addr);
+	VERBOSE("i2c %u: reg %p read 0x%x\n", bus_id,
+		(void *)(smbus + reg_addr), val);
+	return val;
+}
+
+/* Function to write a value ('val') in to a specified register. */
+static void iproc_i2c_reg_write(uint32_t bus_id,
+				unsigned long reg_addr,
+				uint32_t val)
+{
+	uintptr_t smbus;
+
+	smbus = smbus_base_reg_addr[bus_id];
+
+	mmio_write_32((smbus + reg_addr), val);
+	VERBOSE("i2c %u: reg %p wrote 0x%x\n", bus_id,
+		(void *)(smbus + reg_addr), val);
+}
+
+/* Function to clear and set bits in a specified register. */
+static void iproc_i2c_reg_clearset(uint32_t bus_id,
+				   unsigned long reg_addr,
+				   uint32_t clear,
+				   uint32_t set)
+{
+	uintptr_t smbus;
+
+	smbus = smbus_base_reg_addr[bus_id];
+
+	mmio_clrsetbits_32((smbus + reg_addr), clear, set);
+	VERBOSE("i2c %u: reg %p clear 0x%x, set 0x%x\n", bus_id,
+		(void *)(smbus + reg_addr), clear, set);
+}
+
+/* Function to dump all SMBUS register */
+#ifdef BCM_I2C_DEBUG
+static int iproc_dump_i2c_regs(uint32_t bus_id)
+{
+	uint32_t regval;
+
+	if (bus_id > MAX_I2C) {
+		return -1;
+	}
+
+	INFO("----------------------------------------------\n");
+	INFO("%s: Dumping SMBus %u registers...\n", __func__, bus_id);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_CFG_REG);
+	INFO("SMB_CFG_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_TIMGCFG_REG);
+	INFO("SMB_TIMGCFG_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_ADDR_REG);
+	INFO("SMB_ADDR_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_MSTRFIFOCTL_REG);
+	INFO("SMB_MSTRFIFOCTL_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_SLVFIFOCTL_REG);
+	INFO("SMB_SLVFIFOCTL_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_BITBANGCTL_REG);
+	INFO("SMB_BITBANGCTL_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_MSTRCMD_REG);
+	INFO("SMB_MSTRCMD_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_SLVCMD_REG);
+	INFO("SMB_SLVCMD_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_EVTEN_REG);
+	INFO("SMB_EVTEN_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_EVTSTS_REG);
+	INFO("SMB_EVTSTS_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_MSTRDATAWR_REG);
+	INFO("SMB_MSTRDATAWR_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_MSTRDATARD_REG);
+	INFO("SMB_MSTRDATARD_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_SLVDATAWR_REG);
+	INFO("SMB_SLVDATAWR_REG=0x%x\n", regval);
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_SLVDATARD_REG);
+	INFO("SMB_SLVDATARD_REG=0x%x\n", regval);
+
+	INFO("----------------------------------------------\n");
+	return 0;
+}
+#endif
+
+/*
+ * Function to ensure that the previous transaction was completed before
+ * initiating a new transaction. It can also be used in polling mode to
+ * check status of completion of a command
+ */
+static int iproc_i2c_startbusy_wait(uint32_t bus_id)
+{
+	uint32_t regval;
+	uint32_t retry = 0U;
+
+	/*
+	 * Check if an operation is in progress. During probe it won't be.
+	 * Want to make sure that the transaction in progress is completed.
+	 */
+	do {
+		udelay(1U);
+		regval = iproc_i2c_reg_read(bus_id, SMB_MSTRCMD_REG);
+		regval &= SMB_MSTRSTARTBUSYCMD_MASK;
+		if (retry++ > BUS_BUSY_COUNT) {
+			ERROR("%s: START_BUSY bit didn't clear, exiting\n",
+			      __func__);
+			return -1;
+		}
+
+	} while (regval != 0U);
+
+	return 0;
+}
+
+/*
+ * This function copies data to SMBus's Tx FIFO. Valid for write transactions
+ * info: Data to copy in to Tx FIFO. For read commands, the size should be
+ * set to zero by the caller
+ */
+static void iproc_i2c_write_trans_data(struct iproc_xact_info *info)
+{
+	uint32_t regval;
+	uint8_t devaddr;
+	uint32_t i;
+	uint32_t num_data_bytes = 0U;
+
+#ifdef BCM_I2C_DEBUG
+	INFO("%s:dev_addr=0x%x,cmd_valid=%d, cmd=0x%x, size=%u proto=%d\n",
+	     __func__, info->devaddr, info->cmd_valid, info->command,
+	     info->size, info->smb_proto);
+#endif
+	/* Shift devaddr by 1 bit since SMBus uses the low bit[0] for R/W_n */
+	devaddr = (info->devaddr << 1);
+
+	/*
+	 * Depending on the SMBus protocol, we need to write additional
+	 * transaction data in to Tx FIFO. Refer to section 5.5 of SMBus spec
+	 * for sequence for a transaction
+	 */
+	switch (info->smb_proto) {
+	case SMBUS_PROT_RECV_BYTE:
+		/* No additional data to be written */
+		iproc_i2c_reg_write(info->bus_id, SMB_MSTRDATAWR_REG,
+				    devaddr | 0x1U | SMB_MSTRWRSTS_MASK);
+		break;
+	case SMBUS_PROT_SEND_BYTE:
+		num_data_bytes = info->size;
+		iproc_i2c_reg_write(info->bus_id, SMB_MSTRDATAWR_REG,
+				    devaddr);
+		break;
+	case SMBUS_PROT_RD_BYTE:
+	case SMBUS_PROT_RD_WORD:
+	case SMBUS_PROT_BLK_RD:
+		/* Write slave address with R/W~ set (bit #0) */
+		iproc_i2c_reg_write(info->bus_id, SMB_MSTRDATAWR_REG,
+				    devaddr | 0x1U);
+		break;
+	case SMBUS_PROT_BLK_WR_BLK_RD_PROC_CALL:
+		iproc_i2c_reg_write(info->bus_id, SMB_MSTRDATAWR_REG,
+				    devaddr | 0x1U | SMB_MSTRWRSTS_MASK);
+		break;
+	case SMBUS_PROT_WR_BYTE:
+	case SMBUS_PROT_WR_WORD:
+		iproc_i2c_reg_write(info->bus_id, SMB_MSTRDATAWR_REG,
+				    devaddr);
+		/*
+		 * No additional bytes to be written. Data portion is written
+		 * in the 'for' loop below
+		 */
+		num_data_bytes = info->size;
+		break;
+	case SMBUS_PROT_BLK_WR:
+		iproc_i2c_reg_write(info->bus_id, SMB_MSTRDATAWR_REG,
+				    devaddr);
+		/* 3rd byte is byte count */
+		iproc_i2c_reg_write(info->bus_id, SMB_MSTRDATAWR_REG,
+				    info->size);
+		num_data_bytes = info->size;
+		break;
+	default:
+		return;
+	}
+
+	/* If the protocol needs command code, copy it */
+	if (info->cmd_valid) {
+		iproc_i2c_reg_write(info->bus_id, SMB_MSTRDATAWR_REG,
+				    info->command);
+	}
+
+	/*
+	 * Copy actual data from caller. In general, for reads,
+	 * no data is copied.
+	 */
+	for (i = 0U; num_data_bytes; --num_data_bytes, i++) {
+		/* For the last byte, set MASTER_WR_STATUS bit */
+		regval = (num_data_bytes == 1U) ?
+			 info->data[i] | SMB_MSTRWRSTS_MASK : info->data[i];
+		iproc_i2c_reg_write(info->bus_id, SMB_MSTRDATAWR_REG,
+				    regval);
+	}
+}
+
+/*
+ * This function writes to the master command register and
+ * then polls for completion
+ */
+static int iproc_i2c_write_master_command(uint32_t mastercmd,
+					  struct iproc_xact_info *info)
+{
+	uint32_t retry = 0U;
+	uint32_t regval;
+
+	iproc_i2c_reg_write(info->bus_id, SMB_MSTRCMD_REG, mastercmd);
+
+	/* Check for Master Busy status */
+	regval = iproc_i2c_reg_read(info->bus_id, SMB_MSTRCMD_REG);
+	while ((regval & SMB_MSTRSTARTBUSYCMD_MASK) != 0U) {
+		udelay(1U);
+		if (retry++ > BUS_BUSY_COUNT) {
+			ERROR("%s: START_BUSY bit didn't clear, exiting\n",
+				__func__);
+			return -1;
+		}
+		regval = iproc_i2c_reg_read(info->bus_id, SMB_MSTRCMD_REG);
+	}
+
+	/* If start_busy bit cleared, check if there are any errors */
+	if (!(regval & SMB_MSTRSTARTBUSYCMD_MASK)) {
+		/* start_busy bit cleared, check master_status field now */
+		regval &= SMB_MSTRSTS_MASK;
+		regval >>= SMB_MSTRSTS_SHIFT;
+		if (regval != MSTR_STS_XACT_SUCCESS) {
+			/* Error We can flush Tx FIFO here */
+			ERROR("%s: ERROR: %u exiting\n", __func__, regval);
+			return -1;
+		}
+	}
+	return 0;
+
+}
+/* Function to initiate data send and verify completion status */
+static int iproc_i2c_data_send(struct iproc_xact_info *info)
+{
+	int rc;
+	uint32_t mastercmd;
+
+	/* Make sure the previous transaction completed */
+	rc = iproc_i2c_startbusy_wait(info->bus_id);
+
+	if (rc < 0) {
+		WARN("%s: Send: bus is busy, exiting\n", __func__);
+		return rc;
+	}
+	/* Write transaction bytes to Tx FIFO */
+	iproc_i2c_write_trans_data(info);
+
+	/*
+	 * Program master command register (0x30) with protocol type and set
+	 * start_busy_command bit to initiate the write transaction
+	 */
+	mastercmd = (info->smb_proto << SMB_MSTRSMBUSPROTO_SHIFT) |
+	    SMB_MSTRSTARTBUSYCMD_MASK;
+
+	if (iproc_i2c_write_master_command(mastercmd, info)) {
+		return -1;
+	}
+
+	return 0;
+}
+
+/*
+ * Function to initiate data receive, verify completion status,
+ * and read from SMBUS Read FIFO
+ */
+static int iproc_i2c_data_recv(struct iproc_xact_info *info,
+			       uint32_t *num_bytes_read)
+{
+	int rc;
+	uint32_t mastercmd;
+	uint32_t regval;
+
+	/* Make sure the previous transaction completed */
+	rc = iproc_i2c_startbusy_wait(info->bus_id);
+
+	if (rc < 0) {
+		WARN("%s: Receive: Bus is busy, exiting\n", __func__);
+		return rc;
+	}
+
+	/* Program all transaction bytes into master Tx FIFO */
+	iproc_i2c_write_trans_data(info);
+
+	/*
+	 * Program master command register (0x30) with protocol type and set
+	 * start_busy_command bit to initiate the write transaction
+	 */
+	mastercmd = (info->smb_proto << SMB_MSTRSMBUSPROTO_SHIFT) |
+		     SMB_MSTRSTARTBUSYCMD_MASK | info->size;
+
+	if (iproc_i2c_write_master_command(mastercmd, info)) {
+		return -1;
+	}
+
+	/* Read received byte(s), after TX out address etc */
+	regval = iproc_i2c_reg_read(info->bus_id, SMB_MSTRDATARD_REG);
+
+	/* For block read, protocol (hw) returns byte count,as the first byte */
+	if (info->smb_proto == SMBUS_PROT_BLK_RD) {
+		uint32_t i;
+
+		*num_bytes_read = regval & SMB_MSTRRDDATA_MASK;
+		/*
+		 * Limit to reading a max of 32 bytes only; just a safeguard.
+		 * If # bytes read is a number > 32, check transaction set up,
+		 * and contact hw engg.
+		 * Assumption: PEC is disabled
+		 */
+		for (i = 0U; (i < *num_bytes_read) &&
+		     (i < I2C_SMBUS_BLOCK_MAX); i++) {
+			/* Read Rx FIFO for data bytes */
+			regval = iproc_i2c_reg_read(info->bus_id,
+						    SMB_MSTRDATARD_REG);
+			info->data[i] = regval & SMB_MSTRRDDATA_MASK;
+		}
+	} else {
+		/* 1 Byte data */
+		*info->data = regval & SMB_MSTRRDDATA_MASK;
+		*num_bytes_read = 1U;
+	}
+
+	return 0;
+}
+
+/*
+ * This function set clock frequency for SMBus block. As per hardware
+ * engineering, the clock frequency can be changed dynamically.
+ */
+static int iproc_i2c_set_clk_freq(uint32_t bus_id, smb_clk_freq_t freq)
+{
+	uint32_t val;
+
+	switch (freq) {
+	case IPROC_SMB_SPEED_100KHz:
+		val = 0U;
+		break;
+	case IPROC_SMB_SPEED_400KHz:
+		val = 1U;
+		break;
+	default:
+		return -1;
+	}
+
+	iproc_i2c_reg_clearset(bus_id, SMB_TIMGCFG_REG,
+			       SMB_TIMGCFG_MODE400_MASK,
+			       val << SMB_TIMGCFG_MODE400_SHIFT);
+
+	return 0;
+}
+
+/* Helper function to fill the iproc_xact_info structure */
+static void iproc_i2c_fill_info(struct iproc_xact_info *info, uint32_t bus_id,
+				uint8_t devaddr, uint8_t cmd, uint8_t *value,
+				uint8_t smb_proto, uint32_t cmd_valid)
+{
+	info->bus_id = bus_id;
+	info->devaddr = devaddr;
+	info->command = (uint8_t)cmd;
+	info->smb_proto = smb_proto;
+	info->data = value;
+	info->size = 1U;
+	info->flags = 0U;
+	info->cmd_valid = cmd_valid;
+}
+
+/* This function initializes the SMBUS */
+static void iproc_i2c_init(uint32_t bus_id, int speed)
+{
+	uint32_t regval;
+
+#ifdef BCM_I2C_DEBUG
+	INFO("%s: Enter Init\n", __func__);
+#endif
+
+	/* Put controller in reset */
+	regval = iproc_i2c_reg_read(bus_id, SMB_CFG_REG);
+	regval |= BIT(SMB_CFG_RST_SHIFT);
+	regval &= ~(BIT(SMB_CFG_SMBEN_SHIFT));
+	iproc_i2c_reg_write(bus_id, SMB_CFG_REG, regval);
+
+	/* Wait 100 usec per spec */
+	udelay(100U);
+
+	/* Bring controller out of reset */
+	regval &= ~(BIT(SMB_CFG_RST_SHIFT));
+	iproc_i2c_reg_write(bus_id, SMB_CFG_REG, regval);
+
+	/*
+	 * Flush Tx, Rx FIFOs. Note we are setting the Rx FIFO threshold to 0.
+	 * May be OK since we are setting RX_EVENT and RX_FIFO_FULL interrupts
+	 */
+	regval = SMB_MSTRRXFIFOFLSH_MASK | SMB_MSTRTXFIFOFLSH_MASK;
+	iproc_i2c_reg_write(bus_id, SMB_MSTRFIFOCTL_REG, regval);
+
+	/*
+	 * Enable SMbus block. Note, we are setting MASTER_RETRY_COUNT to zero
+	 * since there will be only one master
+	 */
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_CFG_REG);
+	regval |= SMB_CFG_SMBEN_MASK;
+	iproc_i2c_reg_write(bus_id, SMB_CFG_REG, regval);
+	/* Wait a minimum of 50 Usec, as per SMB hw doc. But we wait longer */
+	mdelay(10U);
+
+	/* If error then set default speed */
+	if (i2c_set_bus_speed(bus_id, speed)) {
+		i2c_set_bus_speed(bus_id, I2C_SPEED_DEFAULT);
+	}
+
+	/* Disable intrs */
+	regval = 0x0U;
+	iproc_i2c_reg_write(bus_id, SMB_EVTEN_REG, regval);
+
+	/* Clear intrs (W1TC) */
+	regval = iproc_i2c_reg_read(bus_id, SMB_EVTSTS_REG);
+	iproc_i2c_reg_write(bus_id, SMB_EVTSTS_REG, regval);
+
+#ifdef BCM_I2C_DEBUG
+	iproc_dump_i2c_regs(bus_id);
+
+	INFO("%s: Exit Init Successfully\n", __func__);
+#endif
+}
+
+/*
+ * Function Name:    i2c_init
+ *
+ * Description:
+ *	This function initializes the SMBUS.
+ *
+ * Parameters:
+ *	bus_id - I2C bus ID
+ *	speed  - I2C bus speed in Hz
+ *
+ * Return:
+ *	0 on success, or -1 on failure.
+ */
+int i2c_init(uint32_t bus_id, int speed)
+{
+	if (bus_id > MAX_I2C) {
+		WARN("%s: Invalid Bus %u\n", __func__, bus_id);
+		return -1;
+	}
+
+	iproc_i2c_init(bus_id, speed);
+	return 0U;
+}
+
+/*
+ * Function Name:    i2c_probe
+ *
+ * Description:
+ *	This function probes the I2C bus for the existence of the specified
+ *	device.
+ *
+ * Parameters:
+ *	bus_id  - I2C bus ID
+ *	devaddr - Device Address
+ *
+ * Return:
+ *	0 on success, or -1 on failure.
+ */
+int i2c_probe(uint32_t bus_id, uint8_t devaddr)
+{
+	uint32_t regval;
+	int rc;
+
+	/*
+	 * i2c_init() Initializes internal regs, disable intrs (and then clear intrs),
+	 * set fifo thresholds, etc.
+	 * Shift devaddr by 1 bit since SMBus uses the low bit[0] for R/W_n
+	 */
+	regval = (devaddr << 1U);
+	iproc_i2c_reg_write(bus_id, SMB_MSTRDATAWR_REG, regval);
+
+	regval = ((SMBUS_PROT_QUICK_CMD << SMB_MSTRSMBUSPROTO_SHIFT) |
+		  SMB_MSTRSTARTBUSYCMD_MASK);
+	iproc_i2c_reg_write(bus_id, SMB_MSTRCMD_REG, regval);
+
+	rc = iproc_i2c_startbusy_wait(bus_id);
+
+	if (rc < 0) {
+		WARN("%s: Probe: bus is busy, exiting\n", __func__);
+		return rc;
+	}
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_MSTRCMD_REG);
+	if (((regval & SMB_MSTRSTS_MASK) >> SMB_MSTRSTS_SHIFT) == 0)
+		VERBOSE("i2c device address: 0x%x\n", devaddr);
+	else
+		return -1;
+
+#ifdef BCM_I2C_DEBUG
+	iproc_dump_i2c_regs(bus_id);
+#endif
+	return 0;
+}
+
+/*
+ * Function Name:    i2c_recv_byte
+ *
+ * Description:
+ *	This function reads I2C data from a device without specifying
+ *	a command regsiter.
+ *
+ * Parameters:
+ *	bus_id  - I2C bus ID
+ *	devaddr - Device Address
+ *	value   - Data Read
+ *
+ * Return:
+ *	0 on success, or -1 on failure.
+ */
+int i2c_recv_byte(uint32_t bus_id, uint8_t devaddr, uint8_t *value)
+{
+	int rc;
+	struct iproc_xact_info info;
+	uint32_t num_bytes_read = 0;
+
+	iproc_i2c_fill_info(&info, bus_id, devaddr, 0U, value,
+			    SMBUS_PROT_RECV_BYTE, 0U);
+
+	/* Refer to i2c_smbus_read_byte for params passed. */
+	rc = iproc_i2c_data_recv(&info, &num_bytes_read);
+
+	if (rc < 0) {
+		printf("%s: %s error accessing device 0x%x\n",
+		__func__, "Read", devaddr);
+	}
+
+	return rc;
+}
+
+/*
+ * Function Name:    i2c_send_byte
+ *
+ * Description:
+ *	This function send I2C data to a device without specifying
+ *	a command regsiter.
+ *
+ * Parameters:
+ *	bus_id  - I2C bus ID
+ *	devaddr - Device Address
+ *	value   - Data Send
+ *
+ * Return:
+ *	0 on success, or -1 on failure.
+ */
+int i2c_send_byte(uint32_t bus_id, uint8_t devaddr, uint8_t value)
+{
+	int rc;
+	struct iproc_xact_info info;
+
+	iproc_i2c_fill_info(&info, bus_id, devaddr, 0U, &value,
+			    SMBUS_PROT_SEND_BYTE, 0U);
+
+	/* Refer to i2c_smbus_write_byte params passed. */
+	rc = iproc_i2c_data_send(&info);
+
+	if (rc < 0) {
+		ERROR("%s: %s error accessing device 0x%x\n",
+		__func__, "Write", devaddr);
+	}
+
+	return rc;
+}
+
+/* Helper function to read a single byte */
+static int i2c_read_byte(uint32_t bus_id,
+			 uint8_t devaddr,
+			 uint8_t regoffset,
+			 uint8_t *value)
+{
+	int rc;
+	struct iproc_xact_info info;
+	uint32_t num_bytes_read = 0U;
+
+	iproc_i2c_fill_info(&info, bus_id, devaddr, regoffset, value,
+			    SMBUS_PROT_RD_BYTE, 1U);
+
+	/* Refer to i2c_smbus_read_byte for params passed. */
+	rc = iproc_i2c_data_recv(&info, &num_bytes_read);
+
+	if (rc < 0) {
+		ERROR("%s: %s error accessing device 0x%x\n",
+		       __func__, "Read", devaddr);
+	}
+	return rc;
+}
+
+/*
+ * Function Name:    i2c_read
+ *
+ * Description:
+ *	This function reads I2C data from a device with a designated
+ *	command register
+ *
+ * Parameters:
+ *	bus_id  - I2C bus ID
+ *	devaddr - Device Address
+ *	addr    - Register Offset
+ *	alen    - Address Length, 1 for byte, 2 for word (not supported)
+ *	buffer  - Data Buffer
+ *	len     - Data Length in bytes
+ *
+ * Return:
+ *	0 on success, or -1 on failure.
+ */
+int i2c_read(uint32_t bus_id,
+	     uint8_t devaddr,
+	     uint32_t addr,
+	     int alen,
+	     uint8_t *buffer,
+	     int len)
+{
+	uint32_t i;
+
+	if (alen > 1) {
+		WARN("I2C read: addr len %d not supported\n", alen);
+		return -1;
+	}
+
+	if (addr + len > 256) {
+		WARN("I2C read: address out of range\n");
+		return -1;
+	}
+
+	for (i = 0U; i < len; i++) {
+		if (i2c_read_byte(bus_id, devaddr, addr + i, &buffer[i])) {
+			ERROR("I2C read: I/O error\n");
+			iproc_i2c_init(bus_id, i2c_get_bus_speed(bus_id));
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+/* Helper function to write a single byte */
+static int i2c_write_byte(uint32_t bus_id,
+			  uint8_t devaddr,
+			  uint8_t regoffset,
+			  uint8_t value)
+{
+	int rc;
+	struct iproc_xact_info info;
+
+	iproc_i2c_fill_info(&info, bus_id, devaddr, regoffset, &value,
+			    SMBUS_PROT_WR_BYTE, 1U);
+
+	/* Refer to i2c_smbus_write_byte params passed. */
+	rc = iproc_i2c_data_send(&info);
+
+	if (rc < 0) {
+		ERROR("%s: %s error accessing device 0x%x\n",
+		       __func__, "Write", devaddr);
+		return -1;
+	}
+
+	return 0;
+}
+
+/*
+ * Function Name:    i2c_write
+ *
+ * Description:
+ *	This function write I2C data to a device with a designated
+ *	command register
+ *
+ * Parameters:
+ *	bus_id  - I2C bus ID
+ *	devaddr - Device Address
+ *	addr    - Register Offset
+ *	alen    - Address Length, 1 for byte, 2 for word (not supported)
+ *	buffer  - Data Buffer
+ *	len     - Data Length in bytes
+ *
+ * Return:
+ *	0 on success, or -1 on failure.
+ */
+int i2c_write(uint32_t bus_id,
+	      uint8_t devaddr,
+	      uint32_t addr,
+	      int alen,
+	      uint8_t *buffer,
+	      int len)
+{
+	uint32_t i;
+
+	if (alen > 1) {
+		WARN("I2C write: addr len %d not supported\n", alen);
+		return -1;
+	}
+
+	if (addr + len > 256U) {
+		WARN("I2C write: address out of range\n");
+		return -1;
+	}
+
+	for (i = 0U; i < len; i++) {
+		if (i2c_write_byte(bus_id, devaddr, addr + i, buffer[i])) {
+			ERROR("I2C write: I/O error\n");
+			iproc_i2c_init(bus_id, i2c_get_bus_speed(bus_id));
+			return -1;
+		}
+	}
+	return 0;
+}
+
+/*
+ * Function Name:    i2c_set_bus_speed
+ *
+ * Description:
+ *	This function configures the SMBUS speed
+ *
+ * Parameters:
+ *	bus_id - I2C bus ID
+ *	speed  - I2C bus speed in Hz
+ *
+ * Return:
+ *	0 on success, or -1 on failure.
+ */
+int i2c_set_bus_speed(uint32_t bus_id, uint32_t speed)
+{
+	switch (speed) {
+	case I2C_SPEED_100KHz:
+		iproc_i2c_set_clk_freq(bus_id, IPROC_SMB_SPEED_100KHz);
+		break;
+
+	case I2C_SPEED_400KHz:
+		iproc_i2c_set_clk_freq(bus_id, IPROC_SMB_SPEED_400KHz);
+		break;
+
+	default:
+		return -1;
+	}
+	return 0;
+}
+
+/*
+ * Function Name:    i2c_get_bus_speed
+ *
+ * Description:
+ *	This function returns the SMBUS speed.
+ *
+ * Parameters:
+ *	bus_id - I2C bus ID
+ *
+ * Return:
+ *	Bus speed in Hz, 0 on failure
+ */
+uint32_t i2c_get_bus_speed(uint32_t bus_id)
+{
+	uint32_t regval;
+	uint32_t retval = 0U;
+
+	regval = iproc_i2c_reg_read(bus_id, SMB_TIMGCFG_REG);
+	regval &= SMB_TIMGCFG_MODE400_MASK;
+	regval >>= SMB_TIMGCFG_MODE400_SHIFT;
+
+	switch (regval) {
+	case IPROC_SMB_SPEED_100KHz:
+		retval = I2C_SPEED_100KHz;
+		break;
+
+	case IPROC_SMB_SPEED_400KHz:
+		retval = I2C_SPEED_400KHz;
+		break;
+
+	default:
+		break;
+	}
+	return retval;
+}
+
diff --git a/drivers/brcm/iproc_gpio.c b/drivers/brcm/iproc_gpio.c
new file mode 100644
index 0000000..f61a3bc
--- /dev/null
+++ b/drivers/brcm/iproc_gpio.c
@@ -0,0 +1,232 @@
+/*
+ * Copyright (c) 2019-2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <drivers/gpio.h>
+#include <lib/mmio.h>
+#include <plat/common/platform.h>
+
+#include <iproc_gpio.h>
+#include <platform_def.h>
+
+#define IPROC_GPIO_DATA_IN_OFFSET     0x00
+#define IPROC_GPIO_DATA_OUT_OFFSET    0x04
+#define IPROC_GPIO_OUT_EN_OFFSET      0x08
+#define IPROC_GPIO_PAD_RES_OFFSET     0x34
+#define IPROC_GPIO_RES_EN_OFFSET      0x38
+
+#define PINMUX_OFFSET(gpio)           ((gpio) * 4)
+#define PINCONF_OFFSET(gpio)          ((gpio) * 4)
+#define PINCONF_PULL_UP               BIT(4)
+#define PINCONF_PULL_DOWN             BIT(5)
+
+/*
+ * iProc GPIO bank is always 0x200 per bank,
+ * with each bank supporting 32 GPIOs.
+ */
+#define GPIO_BANK_SIZE                0x200
+#define NGPIOS_PER_BANK               32
+#define GPIO_BANK(pin)                ((pin) / NGPIOS_PER_BANK)
+
+#define IPROC_GPIO_REG(pin, reg)      (GPIO_BANK(pin) * GPIO_BANK_SIZE + (reg))
+#define IPROC_GPIO_SHIFT(pin)         ((pin) % NGPIOS_PER_BANK)
+
+#define MUX_GPIO_MODE                 0x3
+
+/*
+ * @base: base address of the gpio controller
+ * @pinconf_base: base address of the pinconf
+ * @pinmux_base: base address of the mux controller
+ * @nr_gpios: maxinum number of GPIOs
+ */
+struct iproc_gpio {
+	uintptr_t base;
+	uintptr_t pinconf_base;
+	uintptr_t pinmux_base;
+	int nr_gpios;
+};
+
+static struct iproc_gpio iproc_gpio;
+
+static void gpio_set_bit(uintptr_t base, unsigned int reg, int gpio, bool set)
+{
+	unsigned int offset = IPROC_GPIO_REG(gpio, reg);
+	unsigned int shift = IPROC_GPIO_SHIFT(gpio);
+	uint32_t val;
+
+	val = mmio_read_32(base + offset);
+	if (set)
+		val |= BIT(shift);
+	else
+		val &= ~BIT(shift);
+
+	mmio_write_32(base + offset, val);
+}
+
+static bool gpio_get_bit(uintptr_t base, unsigned int reg, int gpio)
+{
+	unsigned int offset = IPROC_GPIO_REG(gpio, reg);
+	unsigned int shift = IPROC_GPIO_SHIFT(gpio);
+
+	return !!(mmio_read_32(base + offset) & BIT(shift));
+}
+
+static void mux_to_gpio(struct iproc_gpio *g, int gpio)
+{
+	/* mux pad to GPIO if IOPAD configuration is mandatory */
+	if (g->pinmux_base)
+		mmio_write_32(g->pinmux_base + PINMUX_OFFSET(gpio),
+			      MUX_GPIO_MODE);
+}
+
+static void set_direction(int gpio, int direction)
+{
+	struct iproc_gpio *g = &iproc_gpio;
+	bool dir = (direction == GPIO_DIR_OUT) ? true : false;
+
+	assert(gpio < g->nr_gpios);
+
+	mux_to_gpio(g, gpio);
+	gpio_set_bit(g->base, IPROC_GPIO_OUT_EN_OFFSET, gpio, dir);
+}
+
+static int get_direction(int gpio)
+{
+	struct iproc_gpio *g = &iproc_gpio;
+	int dir;
+
+	assert(gpio < g->nr_gpios);
+
+	mux_to_gpio(g, gpio);
+	dir = gpio_get_bit(g->base, IPROC_GPIO_OUT_EN_OFFSET, gpio) ?
+		GPIO_DIR_OUT : GPIO_DIR_IN;
+
+	return dir;
+}
+
+static int get_value(int gpio)
+{
+	struct iproc_gpio *g = &iproc_gpio;
+	unsigned int offset;
+
+	assert(gpio < g->nr_gpios);
+
+	mux_to_gpio(g, gpio);
+
+	/*
+	 * If GPIO is configured as output, read from the GPIO_OUT register;
+	 * otherwise, read from the GPIO_IN register
+	 */
+	offset = gpio_get_bit(g->base, IPROC_GPIO_OUT_EN_OFFSET, gpio) ?
+		IPROC_GPIO_DATA_OUT_OFFSET : IPROC_GPIO_DATA_IN_OFFSET;
+
+	return gpio_get_bit(g->base, offset, gpio);
+}
+
+static void set_value(int gpio, int val)
+{
+	struct iproc_gpio *g = &iproc_gpio;
+
+	assert(gpio < g->nr_gpios);
+
+	mux_to_gpio(g, gpio);
+
+	/* make sure GPIO is configured to output, and then set the value */
+	gpio_set_bit(g->base, IPROC_GPIO_OUT_EN_OFFSET, gpio, true);
+	gpio_set_bit(g->base, IPROC_GPIO_DATA_OUT_OFFSET, gpio, !!(val));
+}
+
+static int get_pull(int gpio)
+{
+	struct iproc_gpio *g = &iproc_gpio;
+	uint32_t val;
+
+	assert(gpio < g->nr_gpios);
+	mux_to_gpio(g, gpio);
+
+	/* when there's a valid pinconf_base, use it */
+	if (g->pinconf_base) {
+		val = mmio_read_32(g->pinconf_base + PINCONF_OFFSET(gpio));
+
+		if (val & PINCONF_PULL_UP)
+			return GPIO_PULL_UP;
+		else if (val & PINCONF_PULL_DOWN)
+			return GPIO_PULL_DOWN;
+		else
+			return GPIO_PULL_NONE;
+	}
+
+	/* no pinconf_base. fall back to GPIO internal pull control */
+	if (!gpio_get_bit(g->base, IPROC_GPIO_RES_EN_OFFSET, gpio))
+		return GPIO_PULL_NONE;
+
+	return gpio_get_bit(g->base, IPROC_GPIO_PAD_RES_OFFSET, gpio) ?
+		GPIO_PULL_UP : GPIO_PULL_DOWN;
+}
+
+static void set_pull(int gpio, int pull)
+{
+	struct iproc_gpio *g = &iproc_gpio;
+	uint32_t val;
+
+	assert(gpio < g->nr_gpios);
+	mux_to_gpio(g, gpio);
+
+	/* when there's a valid pinconf_base, use it */
+	if (g->pinconf_base) {
+		val = mmio_read_32(g->pinconf_base + PINCONF_OFFSET(gpio));
+
+		if (pull == GPIO_PULL_NONE) {
+			val &= ~(PINCONF_PULL_UP | PINCONF_PULL_DOWN);
+		} else if (pull == GPIO_PULL_UP) {
+			val |= PINCONF_PULL_UP;
+			val &= ~PINCONF_PULL_DOWN;
+		} else if (pull == GPIO_PULL_DOWN) {
+			val |= PINCONF_PULL_DOWN;
+			val &= ~PINCONF_PULL_UP;
+		} else {
+			return;
+		}
+		mmio_write_32(g->pinconf_base + PINCONF_OFFSET(gpio), val);
+	}
+
+	/* no pinconf_base. fall back to GPIO internal pull control */
+	if (pull == GPIO_PULL_NONE) {
+		gpio_set_bit(g->base, IPROC_GPIO_RES_EN_OFFSET, gpio, false);
+		return;
+	}
+
+	/* enable pad register and pull up or down */
+	gpio_set_bit(g->base, IPROC_GPIO_RES_EN_OFFSET, gpio, true);
+	gpio_set_bit(g->base, IPROC_GPIO_PAD_RES_OFFSET, gpio,
+		     !!(pull == GPIO_PULL_UP));
+}
+
+const gpio_ops_t iproc_gpio_ops = {
+	.get_direction = get_direction,
+	.set_direction = set_direction,
+	.get_value = get_value,
+	.set_value = set_value,
+	.get_pull = get_pull,
+	.set_pull = set_pull,
+};
+
+void iproc_gpio_init(uintptr_t base, int nr_gpios, uintptr_t pinmux_base,
+		     uintptr_t pinconf_base)
+{
+	iproc_gpio.base = base;
+	iproc_gpio.nr_gpios = nr_gpios;
+
+	/* pinmux/pinconf base is optional for some SoCs */
+	if (pinmux_base)
+		iproc_gpio.pinmux_base = pinmux_base;
+
+	if (pinconf_base)
+		iproc_gpio.pinconf_base = pinconf_base;
+
+	gpio_init(&iproc_gpio_ops);
+}
diff --git a/drivers/brcm/mdio/mdio.c b/drivers/brcm/mdio/mdio.c
new file mode 100644
index 0000000..1cf9d66
--- /dev/null
+++ b/drivers/brcm/mdio/mdio.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2016 - 2021, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <mdio.h>
+
+static int mdio_op_status(uint32_t result)
+{
+	uint32_t timeout = 1000000U; /* loop for 1s */
+	uint32_t val;
+
+	do {
+		val = mmio_read_32(CMIC_MIIM_STAT);
+		if ((val & MDIO_STAT_DONE) == result) {
+			return 0;
+		}
+
+		udelay(1U);
+	} while (timeout-- != 0U);
+	return -1;
+}
+
+static int mdio_op(uint16_t busid, uint16_t phyid, uint32_t reg,
+	       uint16_t val, uint8_t op)
+{
+	uint32_t param;
+	int ret;
+
+	mmio_write_32(CMIC_MIIM_CTRL, 0U);
+	ret = mdio_op_status(0U);
+	if (ret != 0) {
+		goto err;
+	}
+
+	param = 0U;
+	param |= 1U << MDIO_PARAM_INTERNAL_SEL;
+	param |= (busid & MDIO_PARAM_BUSID_MASK) << MDIO_PARAM_BUSID;
+	param |= (phyid & MDIO_PARAM_PHYID_MASK) << MDIO_PARAM_PHYID;
+	param |= (val & MDIO_PARAM_DATA_MASK) << MDIO_PARAM_DATA;
+
+	mmio_write_32(CMIC_MIIM_PARAM, param);
+
+	mmio_write_32(CMIC_MIIM_ADDRESS, reg);
+
+	mmio_write_32(CMIC_MIIM_CTRL, op);
+
+	ret = mdio_op_status(1U);
+	if (ret != 0) {
+		goto err;
+	}
+
+	if (op == MDIO_CTRL_READ_OP) {
+		ret = mmio_read_32(CMIC_MIIM_READ_DATA) & MDIO_READ_DATA_MASK;
+	}
+err:
+	return ret;
+}
+
+int mdio_write(uint16_t busid, uint16_t phyid, uint32_t reg, uint16_t val)
+{
+	int ret;
+
+	ret = mdio_op(busid, phyid, reg, val, MDIO_CTRL_WRITE_OP);
+	if (ret == -1) {
+		INFO("MDIO write fail\n");
+	}
+	return ret;
+}
+
+int mdio_read(uint16_t busid, uint16_t phyid, uint32_t reg)
+{
+	int ret;
+
+	ret = mdio_op(busid, phyid, reg, 0U, MDIO_CTRL_READ_OP);
+	if (ret == -1) {
+		INFO("MDIO read fail\n");
+	}
+	return ret;
+}
diff --git a/drivers/brcm/ocotp.c b/drivers/brcm/ocotp.c
new file mode 100644
index 0000000..6ff8554
--- /dev/null
+++ b/drivers/brcm/ocotp.c
@@ -0,0 +1,204 @@
+/*
+ * Copyright (c) 2017 - 2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+
+#include <ocotp.h>
+#include <platform_def.h>
+
+#define OTP_MAP 2
+#define OTP_NUM_WORDS 2048
+/*
+ * # of tries for OTP Status. The time to execute a command varies. The slowest
+ * commands are writes which also vary based on the # of bits turned on. Writing
+ * 0xffffffff takes ~3800 us.
+ */
+#define OTPC_RETRIES_US                 5000
+
+/* Sequence to enable OTP program */
+#define OTPC_PROG_EN_SEQ             { 0xf, 0x4, 0x8, 0xd }
+
+/* OTPC Commands */
+#define OTPC_CMD_READ                0x0
+#define OTPC_CMD_OTP_PROG_ENABLE     0x2
+#define OTPC_CMD_OTP_PROG_DISABLE    0x3
+#define OTPC_CMD_PROGRAM             0x8
+#define OTPC_CMD_ECC                 0x10
+#define OTPC_ECC_ADDR                0x1A
+#define OTPC_ECC_VAL                 0x00EC0000
+
+/* OTPC Status Bits */
+#define OTPC_STAT_CMD_DONE           BIT(1)
+#define OTPC_STAT_PROG_OK            BIT(2)
+
+/* OTPC register definition */
+#define OTPC_MODE_REG_OFFSET         0x0
+#define OTPC_MODE_REG_OTPC_MODE      0
+#define OTPC_COMMAND_OFFSET          0x4
+#define OTPC_COMMAND_COMMAND_WIDTH   6
+#define OTPC_CMD_START_OFFSET        0x8
+#define OTPC_CMD_START_START         0
+#define OTPC_CPU_STATUS_OFFSET       0xc
+#define OTPC_CPUADDR_REG_OFFSET      0x28
+#define OTPC_CPUADDR_REG_OTPC_CPU_ADDRESS_WIDTH 16
+#define OTPC_CPU_WRITE_REG_OFFSET    0x2c
+
+#define OTPC_CMD_MASK  (BIT(OTPC_COMMAND_COMMAND_WIDTH) - 1)
+#define OTPC_ADDR_MASK (BIT(OTPC_CPUADDR_REG_OTPC_CPU_ADDRESS_WIDTH) - 1)
+
+#define OTPC_MODE_REG			OCOTP_REGS_BASE
+
+struct chip_otp_cfg {
+	uint32_t base;
+	uint32_t num_words;
+};
+
+struct chip_otp_cfg ocotp_cfg = {
+	.base = OTPC_MODE_REG,
+	.num_words = 2048,
+};
+
+struct otpc_priv {
+	uint32_t base;
+	struct otpc_map *map;
+	int size;
+	int state;
+};
+
+struct otpc_priv otpc_info;
+
+static inline void set_command(uint32_t base, uint32_t command)
+{
+	mmio_write_32(base + OTPC_COMMAND_OFFSET, command & OTPC_CMD_MASK);
+}
+
+static inline void set_cpu_address(uint32_t base, uint32_t addr)
+{
+	mmio_write_32(base + OTPC_CPUADDR_REG_OFFSET, addr & OTPC_ADDR_MASK);
+}
+
+static inline void set_start_bit(uint32_t base)
+{
+	mmio_write_32(base + OTPC_CMD_START_OFFSET, 1 << OTPC_CMD_START_START);
+}
+
+static inline void reset_start_bit(uint32_t base)
+{
+	mmio_write_32(base + OTPC_CMD_START_OFFSET, 0);
+}
+
+static inline void write_cpu_data(uint32_t base, uint32_t value)
+{
+	mmio_write_32(base + OTPC_CPU_WRITE_REG_OFFSET, value);
+}
+
+static int poll_cpu_status(uint32_t base, uint32_t value)
+{
+	uint32_t status;
+	uint32_t retries;
+
+	for (retries = 0; retries < OTPC_RETRIES_US; retries++) {
+		status = mmio_read_32(base + OTPC_CPU_STATUS_OFFSET);
+		if (status & value)
+			break;
+		udelay(1);
+	}
+	if (retries == OTPC_RETRIES_US)
+		return -1;
+
+	return 0;
+}
+
+static int bcm_otpc_ecc(uint32_t enable)
+{
+	struct otpc_priv *priv = &otpc_info;
+	int ret;
+
+	set_command(priv->base, OTPC_CMD_ECC);
+	set_cpu_address(priv->base, OTPC_ECC_ADDR);
+
+	if (!enable)
+		write_cpu_data(priv->base, OTPC_ECC_VAL);
+	else
+		write_cpu_data(priv->base, ~OTPC_ECC_VAL);
+
+	set_start_bit(priv->base);
+	ret = poll_cpu_status(priv->base, OTPC_STAT_CMD_DONE);
+	if (ret) {
+		ERROR("otp ecc op error: 0x%x", ret);
+		return -1;
+	}
+	reset_start_bit(priv->base);
+
+	return 0;
+}
+
+/*
+ * bcm_otpc_read read otp data in the size of 8 byte rows.
+ * bytes has to be the multiple of 8.
+ * return -1 in error case, return read bytes in success.
+ */
+int bcm_otpc_read(unsigned int offset, void *val, uint32_t bytes,
+		  uint32_t ecc_flag)
+{
+	struct otpc_priv *priv = &otpc_info;
+	uint32_t *buf = val;
+	uint32_t bytes_read;
+	uint32_t address = offset / priv->map->word_size;
+	int i, ret;
+
+	if (!priv->state) {
+		ERROR("OCOTP read failed\n");
+		return -1;
+	}
+
+	bcm_otpc_ecc(ecc_flag);
+
+	for (bytes_read = 0; (bytes_read + priv->map->word_size) <= bytes;) {
+		set_command(priv->base, OTPC_CMD_READ);
+		set_cpu_address(priv->base, address++);
+		set_start_bit(priv->base);
+		ret = poll_cpu_status(priv->base, OTPC_STAT_CMD_DONE);
+		if (ret) {
+			ERROR("otp read error: 0x%x", ret);
+			return -1;
+		}
+
+		for (i = 0; i < priv->map->otpc_row_size; i++) {
+			*buf++ = mmio_read_32(priv->base +
+					priv->map->data_r_offset[i]);
+			bytes_read += sizeof(*buf);
+		}
+
+		reset_start_bit(priv->base);
+	}
+
+	return bytes_read;
+}
+
+int bcm_otpc_init(struct otpc_map *map)
+{
+	struct otpc_priv *priv;
+
+	priv = &otpc_info;
+	priv->base = ocotp_cfg.base;
+	priv->map = map;
+
+	priv->size = 4 * ocotp_cfg.num_words;
+
+	/* Enable CPU access to OTPC. */
+	mmio_setbits_32(priv->base + OTPC_MODE_REG_OFFSET,
+			BIT(OTPC_MODE_REG_OTPC_MODE));
+	reset_start_bit(priv->base);
+	priv->state = 1;
+	VERBOSE("OTPC Initialization done\n");
+
+	return 0;
+}
diff --git a/drivers/brcm/rng.c b/drivers/brcm/rng.c
new file mode 100644
index 0000000..ee2e656
--- /dev/null
+++ b/drivers/brcm/rng.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2017 - 2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <platform_def.h>
+
+#define RNG_CTRL_REG		(RNG_BASE_ADDR + 0x00)
+#define RNG_CTRL_MASK		0x00001FFF
+#define RNG_CTRL_ENABLE		0x00000001
+#define RNG_CTRL_DISABLE	0x00000000
+
+#define RNG_SOFT_RESET_REG	(RNG_BASE_ADDR + 0x04)
+#define RNG_SOFT_RESET_MASK	0x00000001
+
+#define RNG_FIFO_DATA_REG	(RNG_BASE_ADDR + 0x20)
+
+#define RNG_FIFO_COUNT_REG	(RNG_BASE_ADDR + 0x24)
+#define RNG_FIFO_COUNT_MASK	0x000000FF
+
+#define RNG_FIFO_WORDS_MAX	16
+#define MAX_WAIT_COUNT_50US	20000
+
+
+static void rng_reset(void)
+{
+	/* Disable RBG */
+	mmio_clrbits_32(RNG_CTRL_REG, RNG_CTRL_MASK);
+
+	/* Reset RNG and RBG */
+	mmio_setbits_32(RNG_SOFT_RESET_REG, RNG_SOFT_RESET_MASK);
+
+	/* Take all out of reset */
+	mmio_clrbits_32(RNG_SOFT_RESET_REG, RNG_SOFT_RESET_MASK);
+}
+
+static void rng_enable(void)
+{
+	/* Setup RNG. */
+	mmio_clrsetbits_32(RNG_CTRL_REG, RNG_CTRL_MASK, RNG_CTRL_ENABLE);
+}
+
+int rng_init(void)
+{
+	rng_reset();
+
+	rng_enable();
+
+	return 0;
+}
+
+int rng_read(uint32_t *p_out, uint32_t *words_read)
+{
+	uint32_t available_words;
+	uint32_t i;
+	uint32_t word_processed = 0;
+	uint32_t wait_count = MAX_WAIT_COUNT_50US;
+
+	if (*words_read == 0) {
+		ERROR("RNG Parameter: No word requested\n");
+		return -1;
+	}
+
+	do {
+		available_words = mmio_read_32(RNG_FIFO_COUNT_REG);
+		available_words &= RNG_FIFO_COUNT_MASK;
+
+		if (available_words != 0) {
+			available_words = MIN(available_words,
+					*words_read - word_processed);
+
+			for (i = 0; i < available_words; i++)
+				p_out[word_processed + i] =
+					mmio_read_32(RNG_FIFO_DATA_REG);
+			word_processed += available_words;
+		} else {
+			udelay(50);
+		}
+
+		if (word_processed == *words_read)
+			break;
+
+	} while (--wait_count);
+
+	if (word_processed != *words_read) {
+		ERROR("RNG Timeout: requested %d word(s) got %d\n",
+				*words_read, word_processed);
+		*words_read = word_processed;
+		return -1;
+	}
+
+	return 0;
+}
diff --git a/drivers/brcm/scp.c b/drivers/brcm/scp.c
new file mode 100644
index 0000000..6196073
--- /dev/null
+++ b/drivers/brcm/scp.c
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2017 - 2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+
+/* MCU binary image structure: <header> <data>
+ *
+ * Header structure:
+ * <magic-start>
+ * <num-sections>
+ * {<src-offset> <src-size> <dst-addr>}*
+ * <magic-end>
+ *
+ * MCU data (<data>) consists of several sections of code/data, to be
+ *             installed (copied) into MCU memories.
+ * Header (<header>) gives information about sections contained in <data>.
+ *
+ * The installer code iterates over sections in MCU binary.
+ * For each section, it copies the section into MCU memory.
+ *
+ * The header contains:
+ *	 - <magic-start> - 32-bit magic number to mark header start
+ *	 - <num-sections> - number of sections in <data>
+ *	 - <num-sections> tuples. Each tuple describes a section.
+ *			 A tuple contains three 32-bit words.
+ *	 - <magic-end> - 32-bit magic number to mark header end
+ *
+ * Each section is describes by a tuple, consisting of three 32-bit words:
+ *	 - offset of section within MCU binary (relative to beginning of <data>)
+ *	 - section size (in bytes) in MCU binary
+ *	 - target address (in MCU memory). Section is copied to this location.
+ *
+ * All fields are 32-bit unsigned integers in little endian format.
+ * All sizes are assumed to be 32-bit aligned.
+ */
+
+#define SCP_BIN_HEADER_MAGIC_START 0xfa587D01
+#define SCP_BIN_HEADER_MAGIC_END 0xf3e06a85
+
+int download_scp_patch(void *image, unsigned int image_size)
+{
+	unsigned int *pheader = (unsigned int *)(image);
+	unsigned int header_size;
+	unsigned char *pdata;
+	void *dest;
+	unsigned int num_sections;
+	unsigned int section_src_offset;
+	unsigned int section_size;
+
+	if (pheader && (pheader[0] != SCP_BIN_HEADER_MAGIC_START)) {
+		ERROR("SCP: Could not find SCP header.\n");
+		return -1;
+	}
+
+	num_sections = pheader[1];
+	INFO("...Number of sections: %d\n", num_sections);
+	header_size = 4 * (1 + 1 + 3 * num_sections + 1);
+
+	if (image_size < header_size) {
+		ERROR("SCP: Wrong size.\n");
+		return -1;
+	}
+
+	if (*(pheader + header_size/4 - 1) != SCP_BIN_HEADER_MAGIC_END) {
+		ERROR("SCP: Could not find SCP footer.\n");
+		return -1;
+	}
+
+	VERBOSE("SCP image header validated successfully\n");
+	pdata = (unsigned char *)pheader + header_size;
+
+	for (pheader += 2; num_sections > 0; num_sections--) {
+
+		section_src_offset = pheader[0];
+		section_size = pheader[1];
+		dest = (void *)(unsigned long)pheader[2];
+
+		INFO("section: src:0x%x, size:%d, dst:0x%x\n",
+				section_src_offset, section_size, pheader[2]);
+
+		if ((section_src_offset + section_size) > image_size) {
+			ERROR("SCP: Section points to outside of patch.\n");
+			return -1;
+		}
+
+		/* copy from source to target section */
+		memcpy(dest, pdata + section_src_offset, section_size);
+		flush_dcache_range((uintptr_t)dest, section_size);
+
+		/* next section */
+		pheader += 3;
+	}
+	return 0;
+}
diff --git a/drivers/brcm/sotp.c b/drivers/brcm/sotp.c
new file mode 100644
index 0000000..63c4820
--- /dev/null
+++ b/drivers/brcm/sotp.c
@@ -0,0 +1,323 @@
+/*
+ * Copyright (c) 2016-2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <sotp.h>
+
+#include <platform_def.h>
+#include <platform_sotp.h>
+
+#ifdef USE_SOFT_SOTP
+extern uint64_t soft_sotp[];
+#endif
+
+#define SOTP_PROG_CONTROL (SOTP_REGS_OTP_BASE + 0x0000)
+#define SOTP_PROG_CONTROL__OTP_CPU_MODE_EN 15
+#define SOTP_PROG_CONTROL__OTP_DISABLE_ECC 9
+#define SOTP_PROG_CONTROL__OTP_ECC_WREN 8
+
+#define SOTP_WRDATA_0 (SOTP_REGS_OTP_BASE + 0x0004)
+#define SOTP_WRDATA_1 (SOTP_REGS_OTP_BASE + 0x0008)
+
+#define SOTP_ADDR (SOTP_REGS_OTP_BASE + 0x000c)
+#define SOTP_ADDR__OTP_ROW_ADDR_R 6
+#define SOTP_ADDR_MASK 0x3FF
+
+#define SOTP_CTRL_0 (SOTP_REGS_OTP_BASE + 0x0010)
+#define SOTP_CTRL_0__START 0
+#define SOTP_CTRL_0__OTP_CMD 1
+
+#define SOTP_STATUS_0 (SOTP_REGS_OTP_BASE + 0x0018)
+#define SOTP_STATUS__FDONE 3
+
+#define SOTP_STATUS_1 (SOTP_REGS_OTP_BASE + 0x001c)
+#define SOTP_STATUS_1__CMD_DONE 1
+#define SOTP_STATUS_1__ECC_DET 17
+
+#define SOTP_RDDATA_0 (SOTP_REGS_OTP_BASE + 0x0020)
+#define SOTP_RDDATA_1 (SOTP_REGS_OTP_BASE + 0x0024)
+
+#define SOTP_READ 0
+
+#define SOTP_PROG_WORD 10
+#define SOTP_STATUS__PROGOK 2
+#define SOTP_PROG_ENABLE 2
+
+#define SOTP_ROW_DATA_MASK 0xffffffff
+#define SOTP_ECC_ERR_BITS_MASK 0x1ff00000000
+
+#define SOTP_CHIP_CTRL_SW_OVERRIDE_CHIP_STATES 4
+#define SOTP_CHIP_CTRL_SW_MANU_PROG 5
+#define SOTP_CHIP_CTRL_SW_CID_PROG 6
+#define SOTP_CHIP_CTRL_SW_AB_DEVICE 8
+#define SOTP_CHIP_CTRL_SW_AB_DEV_MODE 9
+#define CHIP_STATE_UNPROGRAMMED 0x1
+#define CHIP_STATE_UNASSIGNED 0x2
+
+uint64_t sotp_mem_read(uint32_t offset, uint32_t sotp_add_ecc)
+{
+#ifdef USE_SOFT_SOTP
+	(void)sotp_add_ecc;
+
+	return soft_sotp[offset];
+#else
+	uint64_t read_data = 0;
+	uint64_t read_data1 = 0;
+	uint64_t read_data2 = 0;
+
+	/* Check for FDONE status */
+	while ((mmio_read_32(SOTP_STATUS_0) & BIT(SOTP_STATUS__FDONE)) !=
+	       BIT(SOTP_STATUS__FDONE))
+		;
+
+	/* Enable OTP access by CPU */
+	mmio_setbits_32(SOTP_PROG_CONTROL,
+			BIT(SOTP_PROG_CONTROL__OTP_CPU_MODE_EN));
+
+	if (sotp_add_ecc == 1) {
+		mmio_clrbits_32(SOTP_PROG_CONTROL,
+				BIT(SOTP_PROG_CONTROL__OTP_DISABLE_ECC));
+	}
+
+	if (sotp_add_ecc == 0) {
+		mmio_setbits_32(SOTP_PROG_CONTROL,
+				BIT(SOTP_PROG_CONTROL__OTP_DISABLE_ECC));
+	}
+
+	mmio_write_32(SOTP_ADDR,
+		      ((offset & SOTP_ADDR_MASK) << SOTP_ADDR__OTP_ROW_ADDR_R));
+	mmio_write_32(SOTP_CTRL_0, (SOTP_READ << SOTP_CTRL_0__OTP_CMD));
+
+	/* Start bit to tell SOTP to send command to the OTP controller */
+	mmio_setbits_32(SOTP_CTRL_0, BIT(SOTP_CTRL_0__START));
+
+	/* Wait for SOTP command done to be set */
+	while ((mmio_read_32(SOTP_STATUS_1) & BIT(SOTP_STATUS_1__CMD_DONE)) !=
+	      BIT(SOTP_STATUS_1__CMD_DONE))
+		;
+
+	/* Clr Start bit after command done */
+	mmio_clrbits_32(SOTP_CTRL_0, BIT(SOTP_CTRL_0__START));
+
+	if ((offset > SOTP_DEVICE_SECURE_CFG3_ROW) &&
+	    (mmio_read_32(SOTP_STATUS_1) & BIT(SOTP_STATUS_1__ECC_DET))) {
+		ERROR("SOTP ECC ERROR Detected row offset %d\n", offset);
+		read_data = SOTP_ECC_ERR_DETECT;
+	} else {
+		read_data1 = (uint64_t)mmio_read_32(SOTP_RDDATA_0);
+		read_data1 = read_data1 & 0xFFFFFFFF;
+		read_data2 = (uint64_t)mmio_read_32(SOTP_RDDATA_1);
+		read_data2 = (read_data2 & 0x1ff) << 32;
+		read_data = read_data1 | read_data2;
+	}
+
+	/* Command done is cleared */
+	mmio_setbits_32(SOTP_STATUS_1, BIT(SOTP_STATUS_1__CMD_DONE));
+
+	/* disable OTP access by CPU */
+	mmio_clrbits_32(SOTP_PROG_CONTROL,
+			BIT(SOTP_PROG_CONTROL__OTP_CPU_MODE_EN));
+
+	return read_data;
+#endif
+}
+
+void sotp_mem_write(uint32_t addr, uint32_t sotp_add_ecc, uint64_t wdata)
+{
+#ifdef USE_SOFT_SOTP
+	(void)sotp_add_ecc;
+
+	soft_sotp[addr] = wdata;
+#else
+	uint32_t loop;
+	uint8_t prog_array[4] = { 0x0F, 0x04, 0x08, 0x0D };
+
+	uint32_t chip_state_default =
+		(CHIP_STATE_UNASSIGNED|CHIP_STATE_UNPROGRAMMED);
+	uint32_t chip_state = mmio_read_32(SOTP_REGS_SOTP_CHIP_STATES);
+	uint32_t chip_ctrl_default = 0;
+
+	/*
+	 * The override settings is required to allow the customer to program
+	 * the application specific keys into SOTP, before the conversion to
+	 * one of the AB modes.
+	 * At the end of write operation, the chip ctrl settings will restored
+	 * to the state prior to write call
+	 */
+	if (chip_state & chip_state_default) {
+		uint32_t chip_ctrl;
+
+		chip_ctrl_default = mmio_read_32(SOTP_CHIP_CTRL);
+		INFO("SOTP: enable special prog mode\n");
+
+		chip_ctrl = BIT(SOTP_CHIP_CTRL_SW_OVERRIDE_CHIP_STATES) |
+			    BIT(SOTP_CHIP_CTRL_SW_MANU_PROG) |
+			    BIT(SOTP_CHIP_CTRL_SW_CID_PROG) |
+			    BIT(SOTP_CHIP_CTRL_SW_AB_DEVICE);
+		mmio_write_32(SOTP_CHIP_CTRL, chip_ctrl);
+	}
+
+	/*  Check for FDONE status */
+	while ((mmio_read_32(SOTP_STATUS_0) & BIT(SOTP_STATUS__FDONE)) !=
+	       BIT(SOTP_STATUS__FDONE))
+		;
+
+	/*  Enable OTP acces by CPU */
+	mmio_setbits_32(SOTP_PROG_CONTROL,
+			BIT(SOTP_PROG_CONTROL__OTP_CPU_MODE_EN));
+
+	if (addr > SOTP_DEVICE_SECURE_CFG3_ROW) {
+		if (sotp_add_ecc == 0) {
+			mmio_clrbits_32(SOTP_PROG_CONTROL,
+					BIT(SOTP_PROG_CONTROL__OTP_ECC_WREN));
+		}
+		if (sotp_add_ecc == 1) {
+			mmio_setbits_32(SOTP_PROG_CONTROL,
+					BIT(SOTP_PROG_CONTROL__OTP_ECC_WREN));
+		}
+	} else {
+		mmio_clrbits_32(SOTP_PROG_CONTROL,
+				BIT(SOTP_PROG_CONTROL__OTP_ECC_WREN));
+	}
+
+	mmio_write_32(SOTP_CTRL_0, (SOTP_PROG_ENABLE << 1));
+
+	/*
+	 * In order to avoid unintentional writes / programming of the OTP
+	 * array, the OTP Controller must be put into programming mode before
+	 * it will accept program commands. This is done by writing 0xF, 0x4,
+	 * 0x8, 0xD with program commands prior to starting the actual
+	 * programming sequence
+	 */
+	for (loop = 0; loop < 4; loop++) {
+		mmio_write_32(SOTP_WRDATA_0, prog_array[loop]);
+
+		/*
+		 * Start bit to tell SOTP to send command to the OTP controller
+		 */
+		mmio_setbits_32(SOTP_CTRL_0, BIT(SOTP_CTRL_0__START));
+
+		/*  Wait for SOTP command done to <-- be set */
+		while ((mmio_read_32(SOTP_STATUS_1) &
+			BIT(SOTP_STATUS_1__CMD_DONE)) !=
+		       BIT(SOTP_STATUS_1__CMD_DONE))
+			;
+
+		/* Command done is cleared w1c */
+		mmio_setbits_32(SOTP_STATUS_1, BIT(SOTP_STATUS_1__CMD_DONE));
+
+		/* Clr Start bit after command done */
+		mmio_clrbits_32(SOTP_CTRL_0, BIT(SOTP_CTRL_0__START));
+	}
+
+	/*  Check for PROGOK */
+	while ((mmio_read_32(SOTP_STATUS_0) & 0x4) != BIT(SOTP_STATUS__PROGOK))
+		;
+
+	/* Set  10 bit row address */
+	mmio_write_32(SOTP_ADDR,
+		      ((addr & SOTP_ADDR_MASK) << SOTP_ADDR__OTP_ROW_ADDR_R));
+
+	/* Set SOTP Row data */
+	mmio_write_32(SOTP_WRDATA_0, (wdata & SOTP_ROW_DATA_MASK));
+
+	/* Set SOTP ECC and error bits */
+	mmio_write_32(SOTP_WRDATA_1, ((wdata & SOTP_ECC_ERR_BITS_MASK) >> 32));
+
+	/* Set prog_word command */
+	mmio_write_32(SOTP_CTRL_0, (SOTP_PROG_WORD << 1));
+
+	/*  Start bit to tell SOTP to send command to the OTP controller */
+	mmio_setbits_32(SOTP_CTRL_0, BIT(SOTP_CTRL_0__START));
+
+	/*  Wait for SOTP command done to be set */
+	while ((mmio_read_32(SOTP_STATUS_1) & BIT(SOTP_STATUS_1__CMD_DONE)) !=
+	       BIT(SOTP_STATUS_1__CMD_DONE))
+		;
+
+	/* Command done is cleared w1c */
+	mmio_setbits_32(SOTP_STATUS_1, BIT(SOTP_STATUS_1__CMD_DONE));
+
+	/* disable OTP acces by CPU */
+	mmio_clrbits_32(SOTP_PROG_CONTROL,
+			BIT(SOTP_PROG_CONTROL__OTP_CPU_MODE_EN));
+
+	/* Clr Start bit after command done */
+	mmio_clrbits_32(SOTP_CTRL_0, BIT(SOTP_CTRL_0__START));
+
+	if (chip_state & chip_state_default)
+		mmio_write_32(SOTP_CHIP_CTRL, chip_ctrl_default);
+
+#endif
+}
+
+int sotp_read_key(uint8_t *key, size_t keysize, int start_row, int end_row)
+{
+	int row;
+	uint32_t status = 0;
+	uint32_t status2 = 0xFFFFFFFF;
+	uint64_t row_data;
+	uint32_t data;
+	uint32_t *temp_key = (uint32_t *)key;
+
+	row = start_row;
+	while ((keysize > 0) && (row <= end_row)) {
+		row_data = sotp_mem_read(row, SOTP_ROW_ECC);
+		if (!(row_data & (SOTP_ECC_ERR_DETECT | SOTP_FAIL_BITS))) {
+			memcpy(temp_key++, &row_data, sizeof(uint32_t));
+			keysize -= sizeof(uint32_t);
+			data = (uint32_t)(row_data & SOTP_ROW_DATA_MASK);
+			status |= data;
+			status2 &= data;
+		}
+		row++;
+	}
+
+	if ((status2 == 0xFFFFFFFF) || (status == 0) || (row > end_row))
+		return -1;
+
+	return 0;
+}
+
+int sotp_key_erased(void)
+{
+	uint64_t row_data;
+	int status = 0;
+
+	row_data = sotp_mem_read(SOTP_DEVICE_SECURE_CFG0_ROW, 0);
+	if (row_data & SOTP_DEVICE_SECURE_CFG0_OTP_ERASED_MASK)
+		status = 1;
+
+	else if (mmio_read_32(SOTP_REGS_SOTP_CHIP_STATES) &
+			SOTP_REGS_SOTP_CHIP_STATES_OTP_ERASED_MASK)
+		status = 1;
+
+	return status;
+}
+
+/*
+ * This function optimise the SOTP redundancy
+ * by considering the 00- zero and 01,10,11 - one
+ */
+uint32_t sotp_redundancy_reduction(uint32_t sotp_row_data)
+{
+	uint32_t opt_data;
+	uint32_t opt_loop;
+	uint32_t temp_data;
+
+	opt_data = 0;
+
+	for (opt_loop = 0; opt_loop < 16; opt_loop = opt_loop + 1) {
+		temp_data = ((sotp_row_data >> (opt_loop * 2)) & 0x3);
+
+		if (temp_data != 0x0)
+			opt_data = (opt_data | (1 << opt_loop));
+	}
+	return opt_data;
+}
diff --git a/drivers/brcm/spi/iproc_qspi.c b/drivers/brcm/spi/iproc_qspi.c
new file mode 100644
index 0000000..4c533d5
--- /dev/null
+++ b/drivers/brcm/spi/iproc_qspi.c
@@ -0,0 +1,317 @@
+/*
+ * Copyright (c) 2017 - 2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <endian.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+#include <spi.h>
+
+#include "iproc_qspi.h"
+
+struct bcmspi_priv spi_cfg;
+
+/* Redefined by platform to force appropriate information */
+#pragma weak plat_spi_init
+int plat_spi_init(uint32_t *max_hz)
+{
+	return 0;
+}
+
+/* Initialize & setup iproc qspi controller */
+int iproc_qspi_setup(uint32_t bus, uint32_t cs, uint32_t max_hz, uint32_t mode)
+{
+	struct bcmspi_priv *priv = NULL;
+	uint32_t spbr;
+
+	priv = &spi_cfg;
+	priv->spi_mode = mode;
+	priv->state = QSPI_STATE_DISABLED;
+	priv->bspi_hw = QSPI_BSPI_MODE_REG_BASE;
+	priv->mspi_hw = QSPI_MSPI_MODE_REG_BASE;
+
+	/* Initialize clock and platform specific */
+	if (plat_spi_init(&max_hz) != 0)
+		return -1;
+
+	priv->max_hz = max_hz;
+
+	/* MSPI: Basic hardware initialization */
+	mmio_write_32(priv->mspi_hw + MSPI_SPCR1_LSB_REG, 0);
+	mmio_write_32(priv->mspi_hw + MSPI_SPCR1_MSB_REG, 0);
+	mmio_write_32(priv->mspi_hw + MSPI_NEWQP_REG, 0);
+	mmio_write_32(priv->mspi_hw + MSPI_ENDQP_REG, 0);
+	mmio_write_32(priv->mspi_hw + MSPI_SPCR2_REG, 0);
+
+	/* MSPI: SCK configuration */
+	spbr = (QSPI_AXI_CLK - 1) / (2 * priv->max_hz) + 1;
+	spbr = MIN(spbr, SPBR_DIV_MAX);
+	spbr = MAX(spbr, SPBR_DIV_MIN);
+	mmio_write_32(priv->mspi_hw + MSPI_SPCR0_LSB_REG, spbr);
+
+	/* MSPI: Mode configuration (8 bits by default) */
+	priv->mspi_16bit = 0;
+	mmio_write_32(priv->mspi_hw + MSPI_SPCR0_MSB_REG,
+		      BIT(MSPI_SPCR0_MSB_REG_MSTR_SHIFT) |		/* Master */
+		      MSPI_SPCR0_MSB_REG_16_BITS_PER_WD_SHIFT |		/* 16 bits per word */
+		      (priv->spi_mode & MSPI_SPCR0_MSB_REG_MODE_MASK));	/* mode: CPOL / CPHA */
+
+	/* Display bus info */
+	VERBOSE("SPI: SPCR0_LSB: 0x%x\n",
+		mmio_read_32(priv->mspi_hw + MSPI_SPCR0_LSB_REG));
+	VERBOSE("SPI: SPCR0_MSB: 0x%x\n",
+		mmio_read_32(priv->mspi_hw + MSPI_SPCR0_MSB_REG));
+	VERBOSE("SPI: SPCR1_LSB: 0x%x\n",
+		mmio_read_32(priv->mspi_hw + MSPI_SPCR1_LSB_REG));
+	VERBOSE("SPI: SPCR1_MSB: 0x%x\n",
+		mmio_read_32(priv->mspi_hw + MSPI_SPCR1_MSB_REG));
+	VERBOSE("SPI: SPCR2: 0x%x\n",
+		mmio_read_32(priv->mspi_hw + MSPI_SPCR2_REG));
+	VERBOSE("SPI: CLK: %d\n", priv->max_hz);
+
+	return 0;
+}
+
+void bcmspi_enable_bspi(struct bcmspi_priv *priv)
+{
+	if (priv->state != QSPI_STATE_BSPI) {
+		/* Switch to BSPI */
+		mmio_write_32(priv->bspi_hw + BSPI_MAST_N_BOOT_CTRL_REG, 0);
+
+		priv->state = QSPI_STATE_BSPI;
+	}
+}
+
+static int bcmspi_disable_bspi(struct bcmspi_priv *priv)
+{
+	uint32_t retry;
+
+	if (priv->state == QSPI_STATE_MSPI)
+		return 0;
+
+	/* Switch to MSPI if not yet */
+	if ((mmio_read_32(priv->bspi_hw + BSPI_MAST_N_BOOT_CTRL_REG) &
+	     MSPI_CTRL_MASK) == 0) {
+		retry = QSPI_RETRY_COUNT_US_MAX;
+		do {
+			if ((mmio_read_32(
+				priv->bspi_hw + BSPI_BUSY_STATUS_REG) &
+				BSPI_BUSY_MASK) == 0) {
+				mmio_write_32(priv->bspi_hw +
+				       BSPI_MAST_N_BOOT_CTRL_REG,
+				       MSPI_CTRL_MASK);
+				udelay(1);
+				break;
+			}
+			udelay(1);
+		} while (retry--);
+
+		if ((mmio_read_32(priv->bspi_hw + BSPI_MAST_N_BOOT_CTRL_REG) &
+		     MSPI_CTRL_MASK) != MSPI_CTRL_MASK) {
+			ERROR("QSPI: Switching to QSPI error.\n");
+			return -1;
+		}
+	}
+
+	/* Update state */
+	priv->state = QSPI_STATE_MSPI;
+
+	return 0;
+}
+
+int iproc_qspi_claim_bus(void)
+{
+	struct bcmspi_priv *priv = &spi_cfg;
+
+	/* Switch to MSPI by default */
+	if (bcmspi_disable_bspi(priv) != 0)
+		return -1;
+
+	return 0;
+}
+
+void iproc_qspi_release_bus(void)
+{
+	struct bcmspi_priv *priv = &spi_cfg;
+
+	/* Switch to BSPI by default */
+	bcmspi_enable_bspi(priv);
+}
+
+static int mspi_xfer(struct bcmspi_priv *priv, uint32_t bytes,
+		     const uint8_t *tx, uint8_t *rx, uint32_t flag)
+{
+	uint32_t retry;
+	uint32_t mode = CDRAM_PCS0;
+
+	if (flag & SPI_XFER_QUAD) {
+		mode |= CDRAM_QUAD_MODE;
+		VERBOSE("SPI: QUAD mode\n");
+
+		if (!tx) {
+			VERBOSE("SPI: 4 lane input\n");
+			mode |= CDRAM_RBIT_INPUT;
+		}
+	}
+
+	/* Use 8-bit queue for odd-bytes transfer */
+	if (bytes & 1)
+		priv->mspi_16bit = 0;
+	else {
+		priv->mspi_16bit = 1;
+		mode |= CDRAM_BITS_EN;
+	}
+
+	while (bytes) {
+		uint32_t chunk;
+		uint32_t queues;
+		uint32_t i;
+
+		/* Separate code for 16bit and 8bit transfers for performance */
+		if (priv->mspi_16bit) {
+			VERBOSE("SPI: 16 bits xfer\n");
+			/* Determine how many bytes to process this time */
+			chunk = MIN(bytes, NUM_CDRAM_BYTES * 2);
+			queues = (chunk - 1) / 2 + 1;
+			bytes -= chunk;
+
+			/* Fill CDRAMs */
+			for (i = 0; i < queues; i++)
+				mmio_write_32(priv->mspi_hw + MSPI_CDRAM_REG +
+					      (i << 2), mode | CDRAM_CONT);
+
+			/* Fill TXRAMs */
+			for (i = 0; i < chunk; i++)
+				if (tx)
+					mmio_write_32(priv->mspi_hw +
+						MSPI_TXRAM_REG +
+						(i << 2), tx[i]);
+		} else {
+			VERBOSE("SPI: 8 bits xfer\n");
+			/* Determine how many bytes to process this time */
+			chunk = MIN(bytes, NUM_CDRAM_BYTES);
+			queues = chunk;
+			bytes -= chunk;
+
+			/* Fill CDRAMs and TXRAMS */
+			for (i = 0; i < chunk; i++) {
+				mmio_write_32(priv->mspi_hw + MSPI_CDRAM_REG +
+					      (i << 2), mode | CDRAM_CONT);
+				if (tx)
+					mmio_write_32(priv->mspi_hw +
+						MSPI_TXRAM_REG +
+						(i << 3), tx[i]);
+			}
+		}
+
+		/* Advance pointers */
+		if (tx)
+			tx += chunk;
+
+		/* Setup queue pointers */
+		mmio_write_32(priv->mspi_hw + MSPI_NEWQP_REG, 0);
+		mmio_write_32(priv->mspi_hw + MSPI_ENDQP_REG, queues - 1);
+
+		/* Remove CONT on the last byte command */
+		if (bytes == 0 && (flag & SPI_XFER_END))
+			mmio_write_32(priv->mspi_hw + MSPI_CDRAM_REG +
+				      ((queues - 1) << 2), mode);
+
+		/* Kick off */
+		mmio_write_32(priv->mspi_hw + MSPI_STATUS_REG, 0);
+		if (bytes == 0 && (flag & SPI_XFER_END))
+			mmio_write_32(priv->mspi_hw + MSPI_SPCR2_REG, MSPI_SPE);
+		else
+			mmio_write_32(priv->mspi_hw + MSPI_SPCR2_REG,
+				      MSPI_SPE | MSPI_CONT_AFTER_CMD);
+
+		/* Wait for completion */
+		retry = QSPI_RETRY_COUNT_US_MAX;
+		do {
+			if (mmio_read_32(priv->mspi_hw + MSPI_STATUS_REG) &
+			    MSPI_CMD_COMPLETE_MASK)
+				break;
+			udelay(1);
+		} while (retry--);
+
+		if ((mmio_read_32(priv->mspi_hw + MSPI_STATUS_REG) &
+		     MSPI_CMD_COMPLETE_MASK) == 0) {
+			ERROR("SPI: Completion timeout.\n");
+			return -1;
+		}
+
+		/* Read data out */
+		if (rx) {
+			if (priv->mspi_16bit) {
+				for (i = 0; i < chunk; i++) {
+					rx[i] = mmio_read_32(priv->mspi_hw +
+							MSPI_RXRAM_REG +
+						       (i << 2))
+						& 0xff;
+				}
+			} else {
+				for (i = 0; i < chunk; i++) {
+					rx[i] = mmio_read_32(priv->mspi_hw +
+						       MSPI_RXRAM_REG +
+						       (((i << 1) + 1) << 2))
+						& 0xff;
+				}
+			}
+			rx += chunk;
+		}
+	}
+
+	return 0;
+}
+
+int iproc_qspi_xfer(uint32_t bitlen,
+		    const void *dout, void *din, unsigned long flags)
+{
+	struct bcmspi_priv *priv;
+	const uint8_t *tx = dout;
+	uint8_t *rx = din;
+	uint32_t bytes = bitlen / 8;
+	int ret = 0;
+
+	priv = &spi_cfg;
+
+	if (priv->state == QSPI_STATE_DISABLED) {
+		ERROR("QSPI: state disabled\n");
+		return -1;
+	}
+
+	/* we can only do 8 bit transfers */
+	if (bitlen % 8) {
+		ERROR("QSPI: Only support 8 bit transfers (requested %d)\n",
+			bitlen);
+		return -1;
+	}
+
+	/* MSPI: Enable write lock at the beginning */
+	if (flags & SPI_XFER_BEGIN) {
+		/* Switch to MSPI if not yet */
+		if (bcmspi_disable_bspi(priv) != 0) {
+			ERROR("QSPI: Switch to MSPI failed\n");
+			return -1;
+		}
+
+		mmio_write_32(priv->mspi_hw + MSPI_WRITE_LOCK_REG, 1);
+	}
+
+	/* MSPI: Transfer it */
+	if (bytes)
+		ret = mspi_xfer(priv, bytes, tx, rx, flags);
+
+	/* MSPI: Disable write lock if it's done */
+	if (flags & SPI_XFER_END)
+		mmio_write_32(priv->mspi_hw + MSPI_WRITE_LOCK_REG, 0);
+
+	return ret;
+}
diff --git a/drivers/brcm/spi/iproc_qspi.h b/drivers/brcm/spi/iproc_qspi.h
new file mode 100644
index 0000000..7a8bd91
--- /dev/null
+++ b/drivers/brcm/spi/iproc_qspi.h
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2017 - 2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IPROC_QSPI_H
+#define IPROC_QSPI_H
+
+#include <platform_def.h>
+
+/*SPI configuration enable*/
+#define IPROC_QSPI_CLK_SPEED	62500000
+#define SPI_CPHA		(1 << 0)
+#define SPI_CPOL		(1 << 1)
+#define IPROC_QSPI_MODE0	0
+#define IPROC_QSPI_MODE3	(SPI_CPOL|SPI_CPHA)
+
+#define IPROC_QSPI_BUS                   0
+#define IPROC_QSPI_CS                    0
+#define IPROC_QSPI_BASE_REG              QSPI_CTRL_BASE_ADDR
+#define IPROC_QSPI_CRU_CONTROL_REG       QSPI_CLK_CTRL
+
+#define QSPI_AXI_CLK                        200000000
+
+#define QSPI_RETRY_COUNT_US_MAX             200000
+
+/* Chip attributes */
+#define QSPI_REG_BASE			IPROC_QSPI_BASE_REG
+#define CRU_CONTROL_REG			IPROC_QSPI_CRU_CONTROL_REG
+#define SPBR_DIV_MIN			8U
+#define SPBR_DIV_MAX			255U
+#define NUM_CDRAM_BYTES			16U
+
+/* Register fields */
+#define MSPI_SPCR0_MSB_BITS_8		0x00000020
+
+/* Flash opcode and parameters */
+#define CDRAM_PCS0			2
+#define CDRAM_CONT			(1 << 7)
+#define CDRAM_BITS_EN			(1 << 6)
+#define CDRAM_QUAD_MODE			(1 << 8)
+#define CDRAM_RBIT_INPUT		(1 << 10)
+
+/* MSPI registers */
+#define QSPI_MSPI_MODE_REG_BASE		(QSPI_REG_BASE + 0x200)
+#define MSPI_SPCR0_LSB_REG		0x000
+#define MSPI_SPCR0_MSB_REG		0x004
+#define MSPI_SPCR1_LSB_REG		0x008
+#define MSPI_SPCR1_MSB_REG		0x00c
+#define MSPI_NEWQP_REG			0x010
+#define MSPI_ENDQP_REG			0x014
+#define MSPI_SPCR2_REG			0x018
+#define MSPI_STATUS_REG			0x020
+#define MSPI_CPTQP_REG			0x024
+#define MSPI_TXRAM_REG			0x040
+#define MSPI_RXRAM_REG			0x0c0
+#define MSPI_CDRAM_REG			0x140
+#define MSPI_WRITE_LOCK_REG		0x180
+#define MSPI_DISABLE_FLUSH_GEN_REG	0x184
+
+#define MSPI_SPCR0_MSB_REG_MSTR_SHIFT		7
+#define MSPI_SPCR0_MSB_REG_16_BITS_PER_WD_SHIFT	(0 << 2)
+#define MSPI_SPCR0_MSB_REG_MODE_MASK		0x3
+
+/* BSPI registers */
+#define QSPI_BSPI_MODE_REG_BASE		QSPI_REG_BASE
+#define BSPI_MAST_N_BOOT_CTRL_REG	0x008
+#define BSPI_BUSY_STATUS_REG		0x00c
+
+#define MSPI_CMD_COMPLETE_MASK		1
+#define BSPI_BUSY_MASK			1
+#define MSPI_CTRL_MASK			1
+
+#define MSPI_SPE			(1 << 6)
+#define MSPI_CONT_AFTER_CMD		(1 << 7)
+
+/* State */
+enum bcm_qspi_state {
+	QSPI_STATE_DISABLED,
+	QSPI_STATE_MSPI,
+	QSPI_STATE_BSPI
+};
+
+/* QSPI private data */
+struct bcmspi_priv {
+	/* Specified SPI parameters */
+	uint32_t max_hz;
+	uint32_t spi_mode;
+
+	/* State */
+	enum bcm_qspi_state state;
+	int mspi_16bit;
+
+	/* Registers */
+	uintptr_t mspi_hw;
+	uintptr_t bspi_hw;
+};
+
+int iproc_qspi_setup(uint32_t bus, uint32_t cs,
+		     uint32_t max_hz, uint32_t mode);
+int iproc_qspi_claim_bus(void);
+void iproc_qspi_release_bus(void);
+int iproc_qspi_xfer(uint32_t bitlen, const void *dout,
+		    void *din, unsigned long flags);
+
+#endif	/* _IPROC_QSPI_H_ */
diff --git a/drivers/brcm/spi/iproc_spi.c b/drivers/brcm/spi/iproc_spi.c
new file mode 100644
index 0000000..551e587
--- /dev/null
+++ b/drivers/brcm/spi/iproc_spi.c
@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2017 - 2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <spi.h>
+
+#include "iproc_qspi.h"
+
+int spi_init(void)
+{
+	return iproc_qspi_setup(IPROC_QSPI_BUS, IPROC_QSPI_CS,
+				IPROC_QSPI_CLK_SPEED, IPROC_QSPI_MODE0);
+}
+
+int spi_claim_bus(void)
+{
+	return iproc_qspi_claim_bus();
+}
+
+void spi_release_bus(void)
+{
+	iproc_qspi_release_bus();
+}
+
+int spi_xfer(uint32_t bitlen, const void *dout,
+	     void *din, uint32_t flags)
+{
+	return iproc_qspi_xfer(bitlen, dout, din, flags);
+}
diff --git a/drivers/brcm/spi_flash.c b/drivers/brcm/spi_flash.c
new file mode 100644
index 0000000..336d230
--- /dev/null
+++ b/drivers/brcm/spi_flash.c
@@ -0,0 +1,308 @@
+/*
+ * Copyright (c) 2019-2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <errno.h>
+
+#include <sf.h>
+#include <spi.h>
+
+#define SPI_FLASH_CMD_LEN	4
+#define QSPI_WAIT_TIMEOUT_US	200000U /* usec */
+
+#define FINFO(jedec_id, ext_id, _sector_size, _n_sectors, _page_size, _flags) \
+		.id = {							\
+			((jedec_id) >> 16) & 0xff,			\
+			((jedec_id) >> 8) & 0xff,			\
+			(jedec_id) & 0xff,				\
+			((ext_id) >> 8) & 0xff,				\
+			(ext_id) & 0xff,				\
+			},						\
+		.id_len = (!(jedec_id) ? 0 : (3 + ((ext_id) ? 2 : 0))),	\
+		.sector_size = (_sector_size),				\
+		.n_sectors = (_n_sectors),				\
+		.page_size = _page_size,				\
+		.flags = (_flags),
+
+/* SPI/QSPI flash device params structure */
+const struct spi_flash_info spi_flash_ids[] = {
+	{"W25Q64CV", FINFO(0xef4017, 0x0, 64 * 1024, 128, 256, WR_QPP | SECT_4K)},
+	{"W25Q64DW", FINFO(0xef6017, 0x0, 64 * 1024, 128, 256, WR_QPP | SECT_4K)},
+	{"W25Q32",   FINFO(0xef4016, 0x0, 64 * 1024, 64,  256, SECT_4K)},
+	{"MX25l3205D",  FINFO(0xc22016, 0x0, 64 * 1024, 64, 256, SECT_4K)},
+};
+
+static void spi_flash_addr(uint32_t addr, uint8_t *cmd)
+{
+	/*
+	 * cmd[0] holds a SPI Flash command, stored earlier
+	 * cmd[1/2/3] holds 24bit flash address
+	 */
+	cmd[1] = addr >> 16;
+	cmd[2] = addr >> 8;
+	cmd[3] = addr >> 0;
+}
+
+static const struct spi_flash_info *spi_flash_read_id(void)
+{
+	const struct spi_flash_info *info;
+	uint8_t id[SPI_FLASH_MAX_ID_LEN];
+	int ret;
+
+	ret = spi_flash_cmd(CMD_READ_ID, id, SPI_FLASH_MAX_ID_LEN);
+	if (ret < 0) {
+		ERROR("SF: Error %d reading JEDEC ID\n", ret);
+		return NULL;
+	}
+
+	for (info = spi_flash_ids; info->name != NULL; info++) {
+		if (info->id_len) {
+			if (!memcmp(info->id, id, info->id_len))
+				return info;
+		}
+	}
+
+	printf("SF: unrecognized JEDEC id bytes: %02x, %02x, %02x\n",
+	       id[0], id[1], id[2]);
+	return NULL;
+}
+
+/* Enable writing on the SPI flash */
+static inline int spi_flash_cmd_write_enable(struct spi_flash *flash)
+{
+	return spi_flash_cmd(CMD_WRITE_ENABLE, NULL, 0);
+}
+
+static int spi_flash_cmd_wait(struct spi_flash *flash)
+{
+	uint8_t cmd;
+	uint32_t i;
+	uint8_t status;
+	int ret;
+
+	i = 0;
+	while (1) {
+		cmd = CMD_RDSR;
+		ret = spi_flash_cmd_read(&cmd, 1, &status, 1);
+		if (ret < 0) {
+			ERROR("SF: cmd wait failed\n");
+			break;
+		}
+		if (!(status & STATUS_WIP))
+			break;
+
+		i++;
+		if (i >= QSPI_WAIT_TIMEOUT_US) {
+			ERROR("SF: cmd wait timeout\n");
+			ret = -1;
+			break;
+		}
+		udelay(1);
+	}
+
+	return ret;
+}
+
+static int spi_flash_write_common(struct spi_flash *flash, const uint8_t *cmd,
+				  size_t cmd_len, const void *buf,
+				  size_t buf_len)
+{
+	int ret;
+
+	ret = spi_flash_cmd_write_enable(flash);
+	if (ret < 0) {
+		ERROR("SF: enabling write failed\n");
+		return ret;
+	}
+
+	ret = spi_flash_cmd_write(cmd, cmd_len, buf, buf_len);
+	if (ret < 0) {
+		ERROR("SF: write cmd failed\n");
+		return ret;
+	}
+
+	ret = spi_flash_cmd_wait(flash);
+	if (ret < 0) {
+		ERROR("SF: write timed out\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static int spi_flash_read_common(const uint8_t *cmd, size_t cmd_len,
+				 void *data, size_t data_len)
+{
+	int ret;
+
+	ret = spi_flash_cmd_read(cmd, cmd_len, data, data_len);
+	if (ret < 0) {
+		ERROR("SF: read cmd failed\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+int spi_flash_read(struct spi_flash *flash, uint32_t offset,
+		   uint32_t len, void *data)
+{
+	uint32_t read_len = 0, read_addr;
+	uint8_t cmd[SPI_FLASH_CMD_LEN];
+	int ret;
+
+	ret = spi_claim_bus();
+	if (ret) {
+		ERROR("SF: unable to claim SPI bus\n");
+		return ret;
+	}
+
+	cmd[0] = CMD_READ_NORMAL;
+	while (len) {
+		read_addr = offset;
+		read_len = MIN(flash->page_size, (len - read_len));
+		spi_flash_addr(read_addr, cmd);
+
+		ret = spi_flash_read_common(cmd, sizeof(cmd), data, read_len);
+		if (ret < 0) {
+			ERROR("SF: read failed\n");
+			break;
+		}
+
+		offset += read_len;
+		len -= read_len;
+		data += read_len;
+	}
+	SPI_DEBUG("SF read done\n");
+
+	spi_release_bus();
+	return ret;
+}
+
+int spi_flash_write(struct spi_flash *flash, uint32_t offset,
+		    uint32_t len, void *buf)
+{
+	unsigned long byte_addr, page_size;
+	uint8_t cmd[SPI_FLASH_CMD_LEN];
+	uint32_t chunk_len, actual;
+	uint32_t write_addr;
+	int ret;
+
+	ret = spi_claim_bus();
+	if (ret) {
+		ERROR("SF: unable to claim SPI bus\n");
+		return ret;
+	}
+
+	page_size = flash->page_size;
+
+	cmd[0] = flash->write_cmd;
+	for (actual = 0; actual < len; actual += chunk_len) {
+		write_addr = offset;
+		byte_addr = offset % page_size;
+		chunk_len = MIN(len - actual,
+				(uint32_t)(page_size - byte_addr));
+		spi_flash_addr(write_addr, cmd);
+
+		SPI_DEBUG("SF:0x%p=>cmd:{0x%02x 0x%02x%02x%02x} chunk_len:%d\n",
+			  buf + actual, cmd[0], cmd[1],
+			  cmd[2], cmd[3], chunk_len);
+
+		ret = spi_flash_write_common(flash, cmd, sizeof(cmd),
+					     buf + actual, chunk_len);
+		if (ret < 0) {
+			ERROR("SF: write cmd failed\n");
+			break;
+		}
+
+		offset += chunk_len;
+	}
+	SPI_DEBUG("SF write done\n");
+
+	spi_release_bus();
+	return ret;
+}
+
+int spi_flash_erase(struct spi_flash *flash, uint32_t offset, uint32_t len)
+{
+	uint8_t cmd[SPI_FLASH_CMD_LEN];
+	uint32_t erase_size, erase_addr;
+	int ret;
+
+	erase_size = flash->erase_size;
+
+	if (offset % erase_size || len % erase_size) {
+		ERROR("SF: Erase offset/length not multiple of erase size\n");
+		return -1;
+	}
+
+	ret = spi_claim_bus();
+	if (ret) {
+		ERROR("SF: unable to claim SPI bus\n");
+		return ret;
+	}
+
+	cmd[0] = flash->erase_cmd;
+	while (len) {
+		erase_addr = offset;
+		spi_flash_addr(erase_addr, cmd);
+
+		SPI_DEBUG("SF: erase %2x %2x %2x %2x (%x)\n", cmd[0], cmd[1],
+			cmd[2], cmd[3], erase_addr);
+
+		ret = spi_flash_write_common(flash, cmd, sizeof(cmd), NULL, 0);
+		if (ret < 0) {
+			ERROR("SF: erase failed\n");
+			break;
+		}
+
+		offset += erase_size;
+		len -= erase_size;
+	}
+	SPI_DEBUG("sf erase done\n");
+
+	spi_release_bus();
+	return ret;
+}
+
+int spi_flash_probe(struct spi_flash *flash)
+{
+	const struct spi_flash_info *info = NULL;
+	int ret;
+
+	ret = spi_claim_bus();
+	if (ret) {
+		ERROR("SF: Unable to claim SPI bus\n");
+		ERROR("SF: probe failed\n");
+		return ret;
+	}
+
+	info = spi_flash_read_id();
+	if (!info)
+		goto probe_fail;
+
+	INFO("Flash Name: %s sectors %x, sec size %x\n",
+	     info->name, info->n_sectors,
+	     info->sector_size);
+	flash->size = info->n_sectors * info->sector_size;
+	flash->sector_size = info->sector_size;
+	flash->page_size = info->page_size;
+	flash->flags = info->flags;
+
+	flash->read_cmd = CMD_READ_NORMAL;
+	flash->write_cmd = CMD_PAGE_PROGRAM;
+	flash->erase_cmd = CMD_ERASE_64K;
+	flash->erase_size = ERASE_SIZE_64K;
+
+probe_fail:
+	spi_release_bus();
+	return ret;
+}
diff --git a/drivers/brcm/spi_sf.c b/drivers/brcm/spi_sf.c
new file mode 100644
index 0000000..8bbb09f
--- /dev/null
+++ b/drivers/brcm/spi_sf.c
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2019-2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+
+#include <spi.h>
+
+#define BITS_PER_BYTE		8
+#define CMD_LEN1		1
+
+static int spi_flash_read_write(const uint8_t *cmd,
+				size_t cmd_len,
+				const uint8_t *data_out,
+				uint8_t *data_in,
+				size_t data_len)
+{
+	unsigned long flags = SPI_XFER_BEGIN;
+	int ret;
+
+	if (data_len == 0)
+		flags |= SPI_XFER_END;
+
+	ret = spi_xfer(cmd_len * BITS_PER_BYTE, cmd, NULL, flags);
+	if (ret) {
+		ERROR("SF: Failed to send command (%zu bytes): %d\n",
+		      cmd_len, ret);
+	} else if (data_len != 0) {
+		ret = spi_xfer(data_len * BITS_PER_BYTE, data_out,
+			       data_in, SPI_XFER_END);
+		if (ret)
+			ERROR("SF: Failed to transfer %zu bytes of data: %d\n",
+			      data_len, ret);
+	}
+
+	return ret;
+}
+
+int spi_flash_cmd_read(const uint8_t *cmd,
+		       size_t cmd_len,
+		       void *data,
+		       size_t data_len)
+{
+	return spi_flash_read_write(cmd, cmd_len, NULL, data, data_len);
+}
+
+int spi_flash_cmd(uint8_t cmd, void *response, size_t len)
+{
+	return spi_flash_cmd_read(&cmd, CMD_LEN1, response, len);
+}
+
+int spi_flash_cmd_write(const uint8_t *cmd,
+			size_t cmd_len,
+			const void *data,
+			size_t data_len)
+{
+	return spi_flash_read_write(cmd, cmd_len, data, NULL, data_len);
+}
diff --git a/drivers/cadence/uart/aarch64/cdns_console.S b/drivers/cadence/uart/aarch64/cdns_console.S
new file mode 100644
index 0000000..4c1a80e
--- /dev/null
+++ b/drivers/cadence/uart/aarch64/cdns_console.S
@@ -0,0 +1,220 @@
+/*
+ * 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/cadence/cdns_uart.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_cdns_core_init
+	.globl console_cdns_core_putc
+	.globl console_cdns_core_getc
+	.globl console_cdns_core_flush
+
+	.globl  console_cdns_putc
+	.globl  console_cdns_getc
+	.globl  console_cdns_flush
+
+	/* -----------------------------------------------
+	 * int console_cdns_core_init(uintptr_t base_addr)
+	 * Function to initialize the console without a
+	 * C Runtime to print debug information. This
+	 * function will be accessed by console_init and
+	 * crash reporting.
+	 * We assume that the bootloader already set up
+	 * the HW (baud, ...) and only enable the trans-
+	 * mitter and receiver here.
+	 * In: x0 - console base address
+	 * Out: return 1 on success else 0 on error
+	 * Clobber list : x1, x2, x3
+	 * -----------------------------------------------
+	 */
+func console_cdns_core_init
+	/* Check the input base address */
+	cbz	x0, core_init_fail
+
+	/* RX/TX enabled & reset */
+	mov	w3, #(R_UART_CR_TX_EN | R_UART_CR_RX_EN | R_UART_CR_TXRST | R_UART_CR_RXRST)
+	str	w3, [x0, #R_UART_CR]
+
+	mov	w0, #1
+	ret
+core_init_fail:
+	mov	w0, wzr
+	ret
+endfunc console_cdns_core_init
+
+	.globl console_cdns_register
+
+	/* -----------------------------------------------
+	 * int console_cdns_register(uintptr_t baseaddr,
+	 *     uint32_t clock, uint32_t baud,
+	 *     console_t *console);
+	 * Function to initialize and register a new CDNS
+	 * 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_cdns_register
+	mov	x7, x30
+	mov	x6, x3
+	cbz	x6, register_fail
+	str	x0, [x6, #CONSOLE_T_BASE]
+
+	bl	console_cdns_core_init
+	cbz	x0, register_fail
+
+	mov	x0, x6
+	mov	x30, x7
+	finish_console_register cdns putc=1, getc=1, flush=1
+
+register_fail:
+	ret	x7
+endfunc console_cdns_register
+
+	/* --------------------------------------------------------
+	 * int console_cdns_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_cdns_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 empty */
+	ldr	w2, [x1, #R_UART_SR]
+	tbz	w2, #UART_SR_INTR_TEMPTY_BIT, 1b
+	mov	w2, #0xD
+	str	w2, [x1, #R_UART_TX]
+2:
+	/* Check if the transmit FIFO is empty */
+	ldr	w2, [x1, #R_UART_SR]
+	tbz	w2, #UART_SR_INTR_TEMPTY_BIT, 2b
+	str	w0, [x1, #R_UART_TX]
+	ret
+endfunc console_cdns_core_putc
+
+	/* --------------------------------------------------------
+	 * int console_cdns_putc(int c, console_t *cdns)
+	 * 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_cdns_putc
+#if ENABLE_ASSERTIONS
+	cmp	x1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x1, [x1, #CONSOLE_T_BASE]
+	b	console_cdns_core_putc
+endfunc console_cdns_putc
+
+	/* ---------------------------------------------
+	 * int console_cdns_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_cdns_core_getc
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Check if the receive FIFO is empty */
+	ldr	w1, [x0, #R_UART_SR]
+	tbnz	w1, #UART_SR_INTR_REMPTY_BIT, no_char
+	ldr	w1, [x0, #R_UART_RX]
+	mov	w0, w1
+	ret
+no_char:
+	mov	w0, #ERROR_NO_PENDING_CHAR
+	ret
+endfunc console_cdns_core_getc
+
+	/* ---------------------------------------------
+	 * int console_cdns_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_cdns_getc
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x0, [x0, #CONSOLE_T_BASE]
+	b	console_cdns_core_getc
+endfunc console_cdns_getc
+
+	/* ---------------------------------------------
+	 * void console_cdns_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_cdns_core_flush
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	/* Placeholder */
+	ret
+endfunc console_cdns_core_flush
+
+	/* ---------------------------------------------
+	 * void console_cdns_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_cdns_flush
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x0, [x0, #CONSOLE_T_BASE]
+	b	console_cdns_core_flush
+endfunc console_cdns_flush
diff --git a/drivers/cfi/v2m/v2m_flash.c b/drivers/cfi/v2m/v2m_flash.c
new file mode 100644
index 0000000..6690189
--- /dev/null
+++ b/drivers/cfi/v2m/v2m_flash.c
@@ -0,0 +1,196 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+
+#include <drivers/cfi/v2m_flash.h>
+#include <lib/mmio.h>
+
+/*
+ * This file supplies a low level interface to the vexpress NOR flash
+ * memory of juno and fvp. This memory is organized as an interleaved
+ * memory of two chips with a 16 bit word. It means that every 32 bit
+ * access is going to access to two different chips. This is very
+ * important when we send commands or read status of the chips.
+ */
+
+/*
+ * DWS ready poll retries. The number of retries in this driver have been
+ * obtained empirically from Juno. FVP implements a zero wait state NOR flash
+ * model
+ */
+#define DWS_WORD_PROGRAM_RETRIES	1000
+#define DWS_WORD_ERASE_RETRIES		3000000
+#define DWS_WORD_LOCK_RETRIES		1000
+
+/* Helper macro to detect end of command */
+#define NOR_CMD_END (NOR_DWS | (NOR_DWS << 16l))
+
+/* Helper macros to access two flash banks in parallel */
+#define NOR_2X16(d)			((d << 16) | (d & 0xffff))
+
+static unsigned int nor_status(uintptr_t base_addr)
+{
+	unsigned long status;
+
+	nor_send_cmd(base_addr, NOR_CMD_READ_STATUS_REG);
+	status = mmio_read_32(base_addr);
+	status |= status >> 16; /* merge status from both flash banks */
+
+	return status & 0xFFFF;
+}
+
+/*
+ * Poll Write State Machine.
+ * Return values:
+ *    0      = WSM ready
+ *    -EBUSY = WSM busy after the number of retries
+ */
+static int nor_poll_dws(uintptr_t base_addr, unsigned long int retries)
+{
+	unsigned long status;
+
+	do {
+		nor_send_cmd(base_addr, NOR_CMD_READ_STATUS_REG);
+		status = mmio_read_32(base_addr);
+		if ((status & NOR_CMD_END) == NOR_CMD_END)
+			return 0;
+	} while (retries-- > 0);
+
+	return -EBUSY;
+}
+
+/*
+ * Return values:
+ *    0      = success
+ *    -EPERM = Device protected or Block locked
+ *    -EIO   = General I/O error
+ */
+static int nor_full_status_check(uintptr_t base_addr)
+{
+	unsigned long status;
+
+	/* Full status check */
+	status = nor_status(base_addr);
+
+	if (status & (NOR_PS | NOR_BLS | NOR_ESS | NOR_PSS))
+		return -EPERM;
+	if (status & (NOR_VPPS | NOR_ES))
+		return -EIO;
+	return 0;
+}
+
+void nor_send_cmd(uintptr_t base_addr, unsigned long cmd)
+{
+	mmio_write_32(base_addr, NOR_2X16(cmd));
+}
+
+/*
+ * This function programs a word in the flash. Be aware that it only
+ * can reset bits that were previously set. It cannot set bits that
+ * were previously reset. The resulting bits = old_bits & new bits.
+ * Return values:
+ *  0 = success
+ *  otherwise it returns a negative value
+ */
+int nor_word_program(uintptr_t base_addr, unsigned long data)
+{
+	uint32_t status;
+	int ret;
+
+	nor_send_cmd(base_addr, NOR_CMD_CLEAR_STATUS_REG);
+
+	/* Set the device in write word mode */
+	nor_send_cmd(base_addr, NOR_CMD_WORD_PROGRAM);
+	mmio_write_32(base_addr, data);
+
+	ret = nor_poll_dws(base_addr, DWS_WORD_PROGRAM_RETRIES);
+	if (ret == 0) {
+		/* Full status check */
+		nor_send_cmd(base_addr, NOR_CMD_READ_STATUS_REG);
+		status = mmio_read_32(base_addr);
+
+		if (status & (NOR_PS | NOR_BLS)) {
+			nor_send_cmd(base_addr, NOR_CMD_CLEAR_STATUS_REG);
+			ret = -EPERM;
+		}
+	}
+
+	if (ret == 0)
+		ret = nor_full_status_check(base_addr);
+	nor_send_cmd(base_addr, NOR_CMD_READ_ARRAY);
+
+	return ret;
+}
+
+/*
+ * Erase a full 256K block
+ * Return values:
+ *  0 = success
+ *  otherwise it returns a negative value
+ */
+int nor_erase(uintptr_t base_addr)
+{
+	int ret;
+
+	nor_send_cmd(base_addr, NOR_CMD_CLEAR_STATUS_REG);
+
+	nor_send_cmd(base_addr, NOR_CMD_BLOCK_ERASE);
+	nor_send_cmd(base_addr, NOR_CMD_BLOCK_ERASE_ACK);
+
+	ret = nor_poll_dws(base_addr, DWS_WORD_ERASE_RETRIES);
+	if (ret == 0)
+		ret = nor_full_status_check(base_addr);
+	nor_send_cmd(base_addr, NOR_CMD_READ_ARRAY);
+
+	return ret;
+}
+
+/*
+ * Lock a full 256 block
+ * Return values:
+ *  0 = success
+ *  otherwise it returns a negative value
+ */
+int nor_lock(uintptr_t base_addr)
+{
+	int ret;
+
+	nor_send_cmd(base_addr, NOR_CMD_CLEAR_STATUS_REG);
+
+	nor_send_cmd(base_addr, NOR_CMD_LOCK_UNLOCK);
+	nor_send_cmd(base_addr, NOR_LOCK_BLOCK);
+
+	ret = nor_poll_dws(base_addr, DWS_WORD_LOCK_RETRIES);
+	if (ret == 0)
+		ret = nor_full_status_check(base_addr);
+	nor_send_cmd(base_addr, NOR_CMD_READ_ARRAY);
+
+	return ret;
+}
+
+/*
+ * unlock a full 256 block
+ * Return values:
+ *  0 = success
+ *  otherwise it returns a negative value
+ */
+int nor_unlock(uintptr_t base_addr)
+{
+	int ret;
+
+	nor_send_cmd(base_addr, NOR_CMD_CLEAR_STATUS_REG);
+
+	nor_send_cmd(base_addr, NOR_CMD_LOCK_UNLOCK);
+	nor_send_cmd(base_addr, NOR_UNLOCK_BLOCK);
+
+	ret = nor_poll_dws(base_addr, DWS_WORD_LOCK_RETRIES);
+	if (ret == 0)
+		ret = nor_full_status_check(base_addr);
+	nor_send_cmd(base_addr, NOR_CMD_READ_ARRAY);
+
+	return ret;
+}
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
new file mode 100644
index 0000000..4cbc0f7
--- /dev/null
+++ b/drivers/clk/clk.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ * Author(s): Ludovic Barre, <ludovic.barre@st.com> for STMicroelectronics.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include <drivers/clk.h>
+
+static const struct clk_ops *ops;
+
+int clk_enable(unsigned long id)
+{
+	assert((ops != NULL) && (ops->enable != NULL));
+
+	return ops->enable(id);
+}
+
+void clk_disable(unsigned long id)
+{
+	assert((ops != NULL) && (ops->disable != NULL));
+
+	ops->disable(id);
+}
+
+unsigned long clk_get_rate(unsigned long id)
+{
+	assert((ops != NULL) && (ops->get_rate != NULL));
+
+	return ops->get_rate(id);
+}
+
+int clk_get_parent(unsigned long id)
+{
+	assert((ops != NULL) && (ops->get_parent != NULL));
+
+	return ops->get_parent(id);
+}
+
+bool clk_is_enabled(unsigned long id)
+{
+	assert((ops != NULL) && (ops->is_enabled != NULL));
+
+	return ops->is_enabled(id);
+}
+
+/*
+ * Initialize the clk. The fields in the provided clk
+ * ops pointer must be valid.
+ */
+void clk_register(const struct clk_ops *ops_ptr)
+{
+	assert((ops_ptr != NULL) &&
+	       (ops_ptr->enable != NULL) &&
+	       (ops_ptr->disable != NULL) &&
+	       (ops_ptr->get_rate != NULL) &&
+	       (ops_ptr->get_parent != NULL) &&
+	       (ops_ptr->is_enabled != NULL));
+
+	ops = ops_ptr;
+}
diff --git a/drivers/console/aarch32/skeleton_console.S b/drivers/console/aarch32/skeleton_console.S
new file mode 100644
index 0000000..a9e13ec
--- /dev/null
+++ b/drivers/console/aarch32/skeleton_console.S
@@ -0,0 +1,170 @@
+/*
+ * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <asm_macros.S>
+#include <console_macros.S>
+
+	/*
+	 * This file contains a skeleton console driver that can be used as a
+	 * basis for a real console driver. Console drivers in Trusted Firmware
+	 * can be instantiated multiple times. Each instance is described by a
+	 * separate console_t structure which must be registered with the common
+	 * console framework via console_register(). Console drivers should
+	 * define a console_xxx_register() function that initializes a new
+	 * console_t structure passed in from the caller and registers it after
+	 * initializing the console hardware. Drivers may define their own
+	 * structures extending console_t to store private driver information.
+	 * Console drivers *MUST* ensure that the console callbacks they
+	 * implement only change registers allowed in the clobber lists defined
+	 * in this file. (Note that in addition to the explicit clobber lists,
+	 * any function may always clobber the intra-procedure-call register
+	 * r12, but may never depend on it retaining its value across any
+	 * function call.)
+	 */
+
+	.globl	console_xxx_register
+	.globl	console_xxx_putc
+	.globl	console_xxx_getc
+	.globl	console_xxx_flush
+
+	/* -----------------------------------------------
+	 * int console_xxx_register(console_xxx_t *console,
+	 * 	...additional parameters as desired...)
+	 * Function to initialize and register the console.
+	 * The caller needs to pass an empty console_xxx_t
+	 * structure in which *MUST* be allocated in
+	 * persistent memory (e.g. a global or static local
+	 * variable, *NOT* on the stack).
+	 * In : r0 - pointer to empty console_t structure
+	 *      r1 through r7: additional parameters as desired
+	 * Out: r0 - 1 on success, 0 on error
+	 * Clobber list : r0 - r7
+	 * -----------------------------------------------
+	 */
+func console_xxx_register
+	/*
+	 * Store parameters (e.g. hardware base address) in driver-specific
+	 * console_xxx_t structure field if they will need to be retrieved
+	 * by later console callback (e.g. putc).
+	 * Example:
+	 */
+	str	r1, [r0, #CONSOLE_T_BASE]
+	str	r2, [r0, #CONSOLE_T_XXX_SOME_OTHER_VALUE]
+
+	/*
+	 * Initialize console hardware, using r1 - r7 parameters as needed.
+	 * Keep console_t pointer in r0 for later.
+	 */
+
+	/*
+	 * Macro to finish up registration and return (needs valid r0 + lr).
+	 * If any of the argument is unspecified, then the corresponding
+	 * entry in console_t is set to 0.
+	 */
+	finish_console_register xxx putc=1, getc=1, flush=1
+
+	/* Jump here if hardware init fails or parameters are invalid. */
+register_fail:
+	mov	r0, #0
+	bx	lr
+endfunc console_xxx_register
+
+	/* --------------------------------------------------------
+	 * int console_xxx_putc(int c, console_xxx_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 struct
+	 * Out: r0 - printed character on success, < 0 on error.
+	 * Clobber list : r0, r1, r2
+	 * --------------------------------------------------------
+	 */
+func console_xxx_putc
+	/*
+	 * Retrieve values we need (e.g. hardware base address) from
+	 * console_xxx_t structure pointed to by r1.
+	 * Example:
+	 */
+	ldr	r1, [r1, #CONSOLE_T_BASE]
+
+	/*
+	 * Write r0 to hardware.
+	 */
+
+	bx	lr
+
+	/* Jump here if output fails for any reason. */
+putc_error:
+	mov	r0, #-1
+	bx	lr
+endfunc console_xxx_putc
+
+	/* ---------------------------------------------
+	 * int console_xxx_getc(console_xxx_t *console)
+	 * Function to get a character from the console.
+	 * Even though console_getc() is blocking, this
+	 * callback has to be non-blocking and always
+	 * return immediately to allow polling multiple
+	 * drivers concurrently.
+	 * Returns the character grabbed on success,
+	 * ERROR_NO_PENDING_CHAR if no character was
+	 * available at this time, or any value
+	 * between -2 and -127 if there was an error.
+	 * In : r0 - pointer to console_t struct
+	 * Out: r0 - character on success,
+	 *           ERROR_NO_PENDING_CHAR if no char,
+	 *           < -1 on error
+	 * Clobber list : r0, r1
+	 * ---------------------------------------------
+	 */
+func console_xxx_getc
+	/*
+	 * Retrieve values we need (e.g. hardware base address) from
+	 * console_xxx_t structure pointed to by r0.
+	 * Example:
+	 */
+	ldr	r1, [r0, #CONSOLE_T_BASE]
+
+	/*
+	 * Try to read character into r0 from hardware.
+	 */
+
+	bx	lr
+
+	/* Jump here if there is no character available at this time. */
+getc_no_char:
+	mov	r0, #ERROR_NO_PENDING_CHAR
+	bx	lr
+
+	/* Jump here if there was any hardware error. */
+getc_error:
+	mov	r0, #-2		/* may pick error codes between -2 and -127 */
+	bx	lr
+endfunc console_xxx_getc
+
+	/* ---------------------------------------------
+	 * int console_xxx_flush(console_xxx_t *console)
+	 * Function to force a write of all buffered
+	 * data that hasn't been output.
+	 * In : r0 - pointer to console_xxx_t struct
+	 * Out: void
+	 * Clobber list : r0, r1, r2, r3, r4, r5
+	 * ---------------------------------------------
+	 */
+func console_xxx_flush
+	/*
+	 * Retrieve values we need (e.g. hardware base address) from
+	 * console_xxx_t structure pointed to by r0.
+	 * Example:
+	 */
+	ldr	r1, [r0, #CONSOLE_T_BASE]
+
+	/*
+	 * Flush all remaining output from hardware FIFOs. Do not return until
+	 * all data has been flushed or there was an unrecoverable error.
+	 */
+
+	bx	lr
+endfunc console_xxx_flush
diff --git a/drivers/console/aarch64/skeleton_console.S b/drivers/console/aarch64/skeleton_console.S
new file mode 100644
index 0000000..7ea2eec
--- /dev/null
+++ b/drivers/console/aarch64/skeleton_console.S
@@ -0,0 +1,170 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <asm_macros.S>
+#include <console_macros.S>
+
+	/*
+	 * This file contains a skeleton console driver that can be used as a
+	 * basis for a real console driver. Console drivers in Trusted Firmware
+	 * can be instantiated multiple times. Each instance is described by a
+	 * separate console_t structure which must be registered with the common
+	 * console framework via console_register(). Console drivers should
+	 * define a console_xxx_register() function that initializes a new
+	 * console_t structure passed in from the caller and registers it after
+	 * initializing the console hardware. Drivers may define their own
+	 * structures extending console_t to store private driver information.
+	 * Console drivers *MUST* ensure that the console callbacks they
+	 * implement only change registers allowed in the clobber lists defined
+	 * in this file. (Note that in addition to the explicit clobber lists,
+	 * any function may always clobber the intra-procedure-call registers
+	 * X16 and X17, but may never depend on them retaining their values
+	 * across any function call.)
+	 */
+
+	.globl	console_xxx_register
+	.globl	console_xxx_putc
+	.globl	console_xxx_getc
+	.globl	console_xxx_flush
+
+	/* -----------------------------------------------
+	 * int console_xxx_register(console_xxx_t *console,
+	 * 	...additional parameters as desired...)
+	 * Function to initialize and register the console.
+	 * The caller needs to pass an empty console_xxx_t
+	 * structure in which *MUST* be allocated in
+	 * persistent memory (e.g. a global or static local
+	 * variable, *NOT* on the stack).
+	 * In : x0 - pointer to empty console_t structure
+	 *      x1 through x7: additional parameters as desired
+	 * Out: x0 - 1 on success, 0 on error
+	 * Clobber list : x0 - x7
+	 * -----------------------------------------------
+	 */
+func console_xxx_register
+	/*
+	 * Store parameters (e.g. hardware base address) in driver-specific
+	 * console_xxx_t structure field if they will need to be retrieved
+	 * by later console callback (e.g. putc).
+	 * Example:
+	 */
+	str	x1, [x0, #CONSOLE_T_BASE]
+	str	x2, [x0, #CONSOLE_T_XXX_SOME_OTHER_VALUE]
+
+	/*
+	 * Initialize console hardware, using x1 - x7 parameters as needed.
+	 * Keep console_t pointer in x0 for later.
+	 */
+
+	/*
+	 * Macro to finish up registration and return (needs valid x0 + x30).
+	 * If any of the argument is unspecified, then the corresponding
+	 * entry in console_t is set to 0.
+	 */
+	finish_console_register xxx putc=1, getc=1, flush=1
+
+	/* Jump here if hardware init fails or parameters are invalid. */
+register_fail:
+	mov	w0, #0
+	ret
+endfunc console_xxx_register
+
+	/* --------------------------------------------------------
+	 * int console_xxx_putc(int c, console_xxx_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 struct
+	 * Out: w0 - printed character on success, < 0 on error.
+	 * Clobber list : x0, x1, x2
+	 * --------------------------------------------------------
+	 */
+func console_xxx_putc
+	/*
+	 * Retrieve values we need (e.g. hardware base address) from
+	 * console_xxx_t structure pointed to by x1.
+	 * Example:
+	 */
+	ldr	x1, [x1, #CONSOLE_T_BASE]
+
+	/*
+	 * Write w0 to hardware.
+	 */
+
+	ret
+
+	/* Jump here if output fails for any reason. */
+putc_error:
+	mov	w0, #-1
+	ret
+endfunc console_xxx_putc
+
+	/* ---------------------------------------------
+	 * int console_xxx_getc(console_xxx_t *console)
+	 * Function to get a character from the console.
+	 * Even though console_getc() is blocking, this
+	 * callback has to be non-blocking and always
+	 * return immediately to allow polling multiple
+	 * drivers concurrently.
+	 * Returns the character grabbed on success,
+	 * ERROR_NO_PENDING_CHAR if no character was
+	 * available at this time, or any value
+	 * between -2 and -127 if there was an error.
+	 * In : x0 - pointer to console_t struct
+	 * Out: w0 - character on success,
+	 *           ERROR_NO_PENDING_CHAR if no char,
+	 *           < -1 on error
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func console_xxx_getc
+	/*
+	 * Retrieve values we need (e.g. hardware base address) from
+	 * console_xxx_t structure pointed to by x0.
+	 * Example:
+	 */
+	ldr	x1, [x0, #CONSOLE_T_BASE]
+
+	/*
+	 * Try to read character into w0 from hardware.
+	 */
+
+	ret
+
+	/* Jump here if there is no character available at this time. */
+getc_no_char:
+	mov	w0, #ERROR_NO_PENDING_CHAR
+	ret
+
+	/* Jump here if there was any hardware error. */
+getc_error:
+	mov	w0, #-2		/* may pick error codes between -2 and -127 */
+	ret
+endfunc console_xxx_getc
+
+	/* ---------------------------------------------
+	 * void console_xxx_flush(console_xxx_t *console)
+	 * Function to force a write of all buffered
+	 * data that hasn't been output.
+	 * In : x0 - pointer to console_xxx_t struct
+	 * Out: void
+	 * Clobber list : x0, x1, x2, x3, x4, x5
+	 * ---------------------------------------------
+	 */
+func console_xxx_flush
+	/*
+	 * Retrieve values we need (e.g. hardware base address) from
+	 * console_xxx_t structure pointed to by x0.
+	 * Example:
+	 */
+	ldr	x1, [x0, #CONSOLE_T_BASE]
+
+	/*
+	 * Flush all remaining output from hardware FIFOs. Do not return until
+	 * all data has been flushed or there was an unrecoverable error.
+	 */
+
+	ret
+endfunc console_xxx_flush
diff --git a/drivers/console/multi_console.c b/drivers/console/multi_console.c
new file mode 100644
index 0000000..e3fb749
--- /dev/null
+++ b/drivers/console/multi_console.c
@@ -0,0 +1,139 @@
+/*
+ * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include <drivers/console.h>
+
+console_t *console_list;
+uint8_t console_state = CONSOLE_FLAG_BOOT;
+
+IMPORT_SYM(console_t *, __STACKS_START__, stacks_start)
+IMPORT_SYM(console_t *, __STACKS_END__, stacks_end)
+
+int console_register(console_t *console)
+{
+	/* Assert that the struct is not on the stack (common mistake). */
+	assert((console < stacks_start) || (console >= stacks_end));
+
+	/* Check that we won't make a circle in the list. */
+	if (console_is_registered(console) == 1)
+		return 1;
+
+	console->next = console_list;
+	console_list = console;
+
+	/* Return 1 for convenient tail-calling from console_xxx_register(). */
+	return 1;
+}
+
+console_t *console_unregister(console_t *to_be_deleted)
+{
+	console_t **ptr;
+
+	assert(to_be_deleted != NULL);
+
+	for (ptr = &console_list; *ptr != NULL; ptr = &(*ptr)->next)
+		if (*ptr == to_be_deleted) {
+			*ptr = (*ptr)->next;
+			return to_be_deleted;
+		}
+
+	return NULL;
+}
+
+int console_is_registered(console_t *to_find)
+{
+	console_t *console;
+
+	assert(to_find != NULL);
+
+	for (console = console_list; console != NULL; console = console->next)
+		if (console == to_find)
+			return 1;
+
+	return 0;
+}
+
+void console_switch_state(unsigned int new_state)
+{
+	console_state = new_state;
+}
+
+void console_set_scope(console_t *console, unsigned int scope)
+{
+	assert(console != NULL);
+
+	console->flags = (console->flags & ~CONSOLE_FLAG_SCOPE_MASK) | scope;
+}
+
+static int do_putc(int c, console_t *console)
+{
+	int ret;
+
+	if ((c == '\n') &&
+	    ((console->flags & CONSOLE_FLAG_TRANSLATE_CRLF) != 0)) {
+		ret = console->putc('\r', console);
+		if (ret < 0)
+			return ret;
+	}
+
+	return console->putc(c, console);
+}
+
+int console_putc(int c)
+{
+	int err = ERROR_NO_VALID_CONSOLE;
+	console_t *console;
+
+	for (console = console_list; console != NULL; console = console->next)
+		if ((console->flags & console_state) && (console->putc != NULL)) {
+			int ret = do_putc(c, console);
+			if ((err == ERROR_NO_VALID_CONSOLE) || (ret < err))
+				err = ret;
+		}
+	return err;
+}
+
+int putchar(int c)
+{
+	if (console_putc(c) == 0)
+		return c;
+	else
+		return EOF;
+}
+
+int console_getc(void)
+{
+	int err = ERROR_NO_VALID_CONSOLE;
+	console_t *console;
+
+	do {	/* Keep polling while at least one console works correctly. */
+		for (console = console_list; console != NULL;
+		     console = console->next)
+			if ((console->flags & console_state) && (console->getc != NULL)) {
+				int ret = console->getc(console);
+				if (ret >= 0)
+					return ret;
+				if (err != ERROR_NO_PENDING_CHAR)
+					err = ret;
+			}
+	} while (err == ERROR_NO_PENDING_CHAR);
+
+	return err;
+}
+
+void console_flush(void)
+{
+	console_t *console;
+
+	for (console = console_list; console != NULL; console = console->next)
+		if ((console->flags & console_state) && (console->flush != NULL)) {
+			console->flush(console);
+		}
+}
diff --git a/drivers/coreboot/cbmem_console/aarch64/cbmem_console.S b/drivers/coreboot/cbmem_console/aarch64/cbmem_console.S
new file mode 100644
index 0000000..db07e6c
--- /dev/null
+++ b/drivers/coreboot/cbmem_console/aarch64/cbmem_console.S
@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include <console_macros.S>
+#include <drivers/coreboot/cbmem_console.h>
+
+/*
+ * This driver implements access to coreboot's in-memory console
+ * (CBMEM console). For the original implementation, see
+ * <coreboot>/src/lib/cbmem_console.c.
+ */
+
+	.globl console_cbmc_register
+	.globl console_cbmc_putc
+	.globl console_cbmc_flush
+
+	/* -----------------------------------------------
+	 * int console_cbmc_register(uintptr_t base,
+	 *                           console_cbmc_t *console);
+	 * Registers a new CBMEM console instance. Reads
+	 * the size field from the buffer header structure
+	 * and stores it in our console_cbmc_t struct, so
+	 * that we keep the size in secure memory where we
+	 * can trust it. A malicious EL1 could manipulate
+	 * the console buffer (including the header), so we
+	 * must not trust its contents after boot.
+	 * In:  x0 - CBMEM console base address
+	 *      x1 - pointer to empty console_cbmc_t struct
+	 * Out: x0 - 1 to indicate success
+	 * Clobber list: x0, x1, x2, x7
+	 * -----------------------------------------------
+	 */
+func console_cbmc_register
+	str	x0, [x1, #CONSOLE_T_BASE]
+	ldr	w2, [x0]
+	str	w2, [x1, #CONSOLE_T_CBMC_SIZE]
+	mov	x0, x1
+	finish_console_register cbmc putc=1, flush=1
+endfunc console_cbmc_register
+
+	/* -----------------------------------------------
+	 * int console_cbmc_puts(int c, console_cbmc_t *console)
+	 * Writes a character to the CBMEM console buffer,
+	 * including overflow handling of the cursor field.
+	 * The character must be preserved in x0.
+	 * In: x0 - character to be stored
+	 *     x1 - pointer to console_cbmc_t struct
+	 * Clobber list: x1, x2, x16, x17
+	 * -----------------------------------------------
+	 */
+func console_cbmc_putc
+	ldr	w2, [x1, #CONSOLE_T_CBMC_SIZE]
+	ldr	x1, [x1, #CONSOLE_T_BASE]
+	add	x1, x1, #8		/* keep address of body in x1 */
+
+	ldr	w16, [x1, #-4]		/* load cursor (one u32 before body) */
+	and	w17, w16, #0xf0000000	/* keep flags part in w17 */
+	and	w16, w16, #0x0fffffff	/* keep actual cursor part in w16 */
+
+	cmp	w16, w2			/* sanity check that cursor < size */
+	b.lo	putc_within_bounds
+	mov	w0, #-1			/* cursor >= size must be malicious */
+	ret				/* so return error, don't write char */
+
+putc_within_bounds:
+	strb	w0, [x1, w16, uxtw]	/* body[cursor] = character */
+	add	w16, w16, #1		/* cursor++ */
+	cmp	w16, w2			/* if cursor < size... */
+	b.lo	putc_write_back		/* ...skip overflow handling */
+
+	mov	w16, #0			/* on overflow, set cursor back to 0 */
+	orr	w17, w17, #(1 << 31)	/* and set overflow flag */
+
+putc_write_back:
+	orr	w16, w16, w17		/* merge cursor and flags back */
+	str	w16, [x1, #-4]		/* write back cursor to memory */
+	ret
+endfunc	console_cbmc_putc
+
+	/* -----------------------------------------------
+	 * void console_cbmc_flush(console_cbmc_t *console)
+	 * Flushes the CBMEM console by flushing the
+	 * console buffer from the CPU's data cache.
+	 * In:  x0 - pointer to console_cbmc_t struct
+	 * Out: void
+	 * Clobber list: x0, x1, x2, x3
+	 * -----------------------------------------------
+	 */
+func console_cbmc_flush
+	ldr	x1, [x0, #CONSOLE_T_CBMC_SIZE]
+	ldr	x0, [x0, #CONSOLE_T_BASE]
+	add	x1, x1, #8		/* add size of console header */
+	b	clean_dcache_range	/* (clobbers x2 and x3) */
+endfunc console_cbmc_flush
diff --git a/drivers/delay_timer/delay_timer.c b/drivers/delay_timer/delay_timer.c
new file mode 100644
index 0000000..a3fd7bf
--- /dev/null
+++ b/drivers/delay_timer/delay_timer.c
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <platform_def.h>
+
+#include <drivers/delay_timer.h>
+#include <lib/utils_def.h>
+
+/***********************************************************
+ * The delay timer implementation
+ ***********************************************************/
+static const timer_ops_t *timer_ops;
+
+/***********************************************************
+ * Delay for the given number of microseconds. The driver must
+ * be initialized before calling this function.
+ ***********************************************************/
+void udelay(uint32_t usec)
+{
+	assert((timer_ops != NULL) &&
+		(timer_ops->clk_mult != 0U) &&
+		(timer_ops->clk_div != 0U) &&
+		(timer_ops->get_timer_value != NULL));
+
+	uint32_t start, delta;
+	uint64_t total_delta;
+
+	assert(usec < (UINT64_MAX / timer_ops->clk_div));
+
+	start = timer_ops->get_timer_value();
+
+	/* Add an extra tick to avoid delaying less than requested. */
+	total_delta =
+		div_round_up((uint64_t)usec * timer_ops->clk_div,
+						timer_ops->clk_mult) + 1U;
+	/*
+	 * Precaution for the total_delta ~ UINT32_MAX and the fact that we
+	 * cannot catch every tick of the timer.
+	 * For example 100MHz timer over 25MHz APB will miss at least 4 ticks.
+	 * 1000U is an arbitrary big number which is believed to be sufficient.
+	 */
+	assert(total_delta < (UINT32_MAX - 1000U));
+
+	do {
+		/*
+		 * If the timer value wraps around, the subtraction will
+		 * overflow and it will still give the correct result.
+		 * delta is decreasing counter
+		 */
+		delta = start - timer_ops->get_timer_value();
+
+	} while (delta < total_delta);
+}
+
+/***********************************************************
+ * Delay for the given number of milliseconds. The driver must
+ * be initialized before calling this function.
+ ***********************************************************/
+void mdelay(uint32_t msec)
+{
+	assert((msec * 1000UL) < UINT32_MAX);
+	udelay(msec * 1000U);
+}
+
+/***********************************************************
+ * Initialize the timer. The fields in the provided timer
+ * ops pointer must be valid.
+ ***********************************************************/
+void timer_init(const timer_ops_t *ops_ptr)
+{
+	assert((ops_ptr != NULL)  &&
+		(ops_ptr->clk_mult != 0U) &&
+		(ops_ptr->clk_div != 0U) &&
+		(ops_ptr->get_timer_value != NULL));
+
+	timer_ops = ops_ptr;
+}
diff --git a/drivers/delay_timer/generic_delay_timer.c b/drivers/delay_timer/generic_delay_timer.c
new file mode 100644
index 0000000..ca522e0
--- /dev/null
+++ b/drivers/delay_timer/generic_delay_timer.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020, NVIDIA Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/generic_delay_timer.h>
+#include <lib/utils_def.h>
+#include <plat/common/platform.h>
+
+static timer_ops_t ops;
+
+static uint32_t get_timer_value(void)
+{
+	/*
+	 * Generic delay timer implementation expects the timer to be a down
+	 * counter. We apply bitwise NOT operator to the tick values returned
+	 * by read_cntpct_el0() to simulate the down counter. The value is
+	 * clipped from 64 to 32 bits.
+	 */
+	return (uint32_t)(~read_cntpct_el0());
+}
+
+void generic_delay_timer_init_args(uint32_t mult, uint32_t div)
+{
+	ops.get_timer_value	= get_timer_value;
+	ops.clk_mult		= mult;
+	ops.clk_div		= div;
+
+	timer_init(&ops);
+
+	VERBOSE("Generic delay timer configured with mult=%u and div=%u\n",
+		mult, div);
+}
+
+void generic_delay_timer_init(void)
+{
+	assert(is_armv7_gentimer_present());
+
+	/* Value in ticks */
+	unsigned int mult = MHZ_TICKS_PER_SEC;
+
+	/* Value in ticks per second (Hz) */
+	unsigned int div  = plat_get_syscnt_freq2();
+
+	/* Reduce multiplier and divider by dividing them repeatedly by 10 */
+	while (((mult % 10U) == 0U) && ((div % 10U) == 0U)) {
+		mult /= 10U;
+		div /= 10U;
+	}
+
+	generic_delay_timer_init_args(mult, div);
+}
+
diff --git a/drivers/fwu/fwu.c b/drivers/fwu/fwu.c
new file mode 100644
index 0000000..ff432be
--- /dev/null
+++ b/drivers/fwu/fwu.c
@@ -0,0 +1,194 @@
+/*
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <common/tf_crc32.h>
+#include <common/tbbr/tbbr_img_def.h>
+#include <drivers/fwu/fwu.h>
+#include <drivers/fwu/fwu_metadata.h>
+#include <drivers/io/io_storage.h>
+
+#include <plat/common/platform.h>
+
+/*
+ * Assert that crc_32 is the first member of fwu_metadata structure.
+ * It avoids accessing data outside of the metadata structure during
+ * CRC32 computation if the crc_32 field gets moved due the structure
+ * member(s) addition in the future.
+ */
+CASSERT((offsetof(struct fwu_metadata, crc_32) == 0),
+	crc_32_must_be_first_member_of_structure);
+
+static struct fwu_metadata metadata;
+static bool is_metadata_initialized __unused;
+
+/*******************************************************************************
+ * Compute CRC32 of the FWU metadata, and check it against the CRC32 value
+ * present in the FWU metadata.
+ *
+ * return -1 on error, otherwise 0
+ ******************************************************************************/
+static int fwu_metadata_crc_check(void)
+{
+	unsigned char *data = (unsigned char *)&metadata;
+
+	uint32_t calc_crc = tf_crc32(0U, data + sizeof(metadata.crc_32),
+				     (sizeof(metadata) -
+				      sizeof(metadata.crc_32)));
+
+	if (metadata.crc_32 != calc_crc) {
+		return -1;
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * Check the sanity of FWU metadata.
+ *
+ * return -1 on error, otherwise 0
+ ******************************************************************************/
+static int fwu_metadata_sanity_check(void)
+{
+	/* ToDo: add more conditions for sanity check */
+	if ((metadata.active_index >= NR_OF_FW_BANKS) ||
+	    (metadata.previous_active_index >= NR_OF_FW_BANKS)) {
+		return -1;
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * Verify and load specified FWU metadata image to local FWU metadata structure.
+ *
+ * @image_id: FWU metadata image id (either FWU_METADATA_IMAGE_ID or
+ *				     BKUP_FWU_METADATA_IMAGE_ID)
+ *
+ * return a negative value on error, otherwise 0
+ ******************************************************************************/
+static int fwu_metadata_load(unsigned int image_id)
+{
+	int result;
+	uintptr_t dev_handle, image_handle, image_spec;
+	size_t bytes_read;
+
+	assert((image_id == FWU_METADATA_IMAGE_ID) ||
+	       (image_id == BKUP_FWU_METADATA_IMAGE_ID));
+
+	result = plat_fwu_set_metadata_image_source(image_id,
+						    &dev_handle,
+						    &image_spec);
+	if (result != 0) {
+		WARN("Failed to set reference to image id=%u (%i)\n",
+		     image_id, result);
+		return result;
+	}
+
+	result = io_open(dev_handle, image_spec, &image_handle);
+	if (result != 0) {
+		WARN("Failed to load image id id=%u (%i)\n",
+		     image_id, result);
+		return result;
+	}
+
+	result = io_read(image_handle, (uintptr_t)&metadata,
+			 sizeof(struct fwu_metadata), &bytes_read);
+
+	if (result != 0) {
+		WARN("Failed to read image id=%u (%i)\n", image_id, result);
+		goto exit;
+	}
+
+	if (sizeof(struct fwu_metadata) != bytes_read) {
+		/* return -1 in case of partial/no read */
+		result = -1;
+		WARN("Read bytes (%zu) instead of expected (%zu) bytes\n",
+		     bytes_read, sizeof(struct fwu_metadata));
+		goto exit;
+	}
+
+	/* sanity check on loaded parameters */
+	result = fwu_metadata_sanity_check();
+	if (result != 0) {
+		WARN("Sanity %s\n", "check failed on FWU metadata");
+		goto exit;
+	}
+
+	/* CRC check on loaded parameters */
+	result = fwu_metadata_crc_check();
+	if (result != 0) {
+		WARN("CRC %s\n", "check failed on FWU metadata");
+	}
+
+exit:
+	(void)io_close(image_handle);
+
+	return result;
+}
+
+/*******************************************************************************
+ * The system runs in the trial run state if any of the images in the active
+ * firmware bank has not been accepted yet.
+ *
+ * Returns true if the system is running in the trial state.
+ ******************************************************************************/
+bool fwu_is_trial_run_state(void)
+{
+	bool trial_run = false;
+
+	assert(is_metadata_initialized);
+
+	for (unsigned int i = 0U; i < NR_OF_IMAGES_IN_FW_BANK; i++) {
+		struct fwu_image_entry *entry = &metadata.img_entry[i];
+		struct fwu_image_properties *img_props =
+			&entry->img_props[metadata.active_index];
+		if (img_props->accepted == 0) {
+			trial_run = true;
+			break;
+		}
+	}
+
+	return trial_run;
+}
+
+const struct fwu_metadata *fwu_get_metadata(void)
+{
+	assert(is_metadata_initialized);
+
+	return &metadata;
+}
+
+/*******************************************************************************
+ * Load verified copy of FWU metadata image kept in the platform NV storage
+ * into local FWU metadata structure.
+ * Also, update platform I/O policies with the offset address and length of
+ * firmware-updated images kept in the platform NV storage.
+ ******************************************************************************/
+void fwu_init(void)
+{
+	/* Load FWU metadata which will be used to load the images in the
+	 * active bank as per PSA FWU specification
+	 */
+	int result = fwu_metadata_load(FWU_METADATA_IMAGE_ID);
+
+	if (result != 0) {
+		WARN("loading of FWU-Metadata failed, "
+		     "using Bkup-FWU-Metadata\n");
+
+		result = fwu_metadata_load(BKUP_FWU_METADATA_IMAGE_ID);
+		if (result != 0) {
+			ERROR("loading of Bkup-FWU-Metadata failed\n");
+			panic();
+		}
+	}
+
+	is_metadata_initialized = true;
+
+	plat_fwu_set_images_source(&metadata);
+}
diff --git a/drivers/fwu/fwu.mk b/drivers/fwu/fwu.mk
new file mode 100644
index 0000000..f4452e0
--- /dev/null
+++ b/drivers/fwu/fwu.mk
@@ -0,0 +1,11 @@
+#
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+FWU_SRC_DIR	:= drivers/fwu/
+
+FWU_SRCS	:= ${FWU_SRC_DIR}fwu.c
+
+BL2_SOURCES	+= ${FWU_SRCS}
diff --git a/drivers/gpio/gpio.c b/drivers/gpio/gpio.c
new file mode 100644
index 0000000..76612b2
--- /dev/null
+++ b/drivers/gpio/gpio.c
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * GPIO -- General Purpose Input/Output
+ *
+ * Defines a simple and generic interface to access GPIO device.
+ *
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <drivers/gpio.h>
+
+/*
+ * The gpio implementation
+ */
+static const gpio_ops_t *ops;
+
+int gpio_get_direction(int gpio)
+{
+	assert(ops);
+	assert(ops->get_direction != 0);
+	assert(gpio >= 0);
+
+	return ops->get_direction(gpio);
+}
+
+void gpio_set_direction(int gpio, int direction)
+{
+	assert(ops);
+	assert(ops->set_direction != 0);
+	assert((direction == GPIO_DIR_OUT) || (direction == GPIO_DIR_IN));
+	assert(gpio >= 0);
+
+	ops->set_direction(gpio, direction);
+}
+
+int gpio_get_value(int gpio)
+{
+	assert(ops);
+	assert(ops->get_value != 0);
+	assert(gpio >= 0);
+
+	return ops->get_value(gpio);
+}
+
+void gpio_set_value(int gpio, int value)
+{
+	assert(ops);
+	assert(ops->set_value != 0);
+	assert((value == GPIO_LEVEL_LOW) || (value == GPIO_LEVEL_HIGH));
+	assert(gpio >= 0);
+
+	ops->set_value(gpio, value);
+}
+
+void gpio_set_pull(int gpio, int pull)
+{
+	assert(ops);
+	assert(ops->set_pull != 0);
+	assert((pull == GPIO_PULL_NONE) || (pull == GPIO_PULL_UP) ||
+	       (pull == GPIO_PULL_DOWN));
+	assert(gpio >= 0);
+
+	ops->set_pull(gpio, pull);
+}
+
+int gpio_get_pull(int gpio)
+{
+	assert(ops);
+	assert(ops->get_pull != 0);
+	assert(gpio >= 0);
+
+	return ops->get_pull(gpio);
+}
+
+/*
+ * Initialize the gpio. The fields in the provided gpio
+ * ops pointer must be valid.
+ */
+void gpio_init(const gpio_ops_t *ops_ptr)
+{
+	assert(ops_ptr != 0  &&
+	       (ops_ptr->get_direction != 0) &&
+	       (ops_ptr->set_direction != 0) &&
+	       (ops_ptr->get_value != 0) &&
+	       (ops_ptr->set_value != 0));
+
+	ops = ops_ptr;
+}
diff --git a/drivers/imx/timer/imx_gpt.c b/drivers/imx/timer/imx_gpt.c
new file mode 100644
index 0000000..464efe9
--- /dev/null
+++ b/drivers/imx/timer/imx_gpt.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+
+#include <imx_gpt.h>
+
+#define GPTCR_SWR		BIT(15)		/* Software reset */
+#define GPTCR_24MEN		BIT(10)		/* Enable 24MHz clock input */
+#define GPTCR_CLKSOURCE_OSC	(5 << 6)        /* Clock source OSC */
+#define GPTCR_CLKSOURCE_MASK	(0x7 << 6)
+#define GPTCR_TEN		1		/* Timer enable */
+
+#define GPTPR_PRESCL_24M_SHIFT 12
+
+#define SYS_COUNTER_FREQ_IN_MHZ 3
+
+#define GPTPR_TIMER_CTRL	(imx_base_addr + 0x000)
+#define GPTPR_TIMER_PRESCL	(imx_base_addr + 0x004)
+#define GPTPR_TIMER_CNTR	(imx_base_addr + 0x024)
+
+static uintptr_t imx_base_addr;
+
+uint32_t imx_get_timer_value(void)
+{
+	return ~mmio_read_32(GPTPR_TIMER_CNTR);
+}
+
+static const timer_ops_t imx_gpt_ops = {
+	.get_timer_value	= imx_get_timer_value,
+	.clk_mult		= 1,
+	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
+};
+
+void imx_gpt_ops_init(uintptr_t base_addr)
+{
+	int val;
+
+	assert(base_addr != 0);
+
+	imx_base_addr = base_addr;
+
+	/* setup GP Timer */
+	mmio_write_32(GPTPR_TIMER_CTRL, GPTCR_SWR);
+	mmio_write_32(GPTPR_TIMER_CTRL, 0);
+
+	/* get 3MHz from 24MHz */
+	mmio_write_32(GPTPR_TIMER_PRESCL, (7 << GPTPR_PRESCL_24M_SHIFT));
+
+	val = mmio_read_32(GPTPR_TIMER_CTRL);
+	val &= ~GPTCR_CLKSOURCE_MASK;
+	val |= GPTCR_24MEN | GPTCR_CLKSOURCE_OSC | GPTCR_TEN;
+	mmio_write_32(GPTPR_TIMER_CTRL, val);
+
+	timer_init(&imx_gpt_ops);
+}
diff --git a/drivers/imx/timer/imx_gpt.h b/drivers/imx/timer/imx_gpt.h
new file mode 100644
index 0000000..2432633
--- /dev/null
+++ b/drivers/imx/timer/imx_gpt.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_GPT_H
+#define IMX_GPT_H
+
+#include <stdint.h>
+
+void imx_gpt_ops_init(uintptr_t reg_base);
+
+#endif /* IMX_GPT_H */
diff --git a/drivers/imx/uart/imx_crash_uart.S b/drivers/imx/uart/imx_crash_uart.S
new file mode 100644
index 0000000..aa987b3
--- /dev/null
+++ b/drivers/imx/uart/imx_crash_uart.S
@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) Linaro 2018 Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <arch.h>
+#include <asm_macros.S>
+#include <assert_macros.S>
+#include <imx_uart.h>
+#include <platform_def.h>
+
+	.globl	imx_crash_uart_init
+	.globl	imx_crash_uart_putc
+
+	/* -----------------------------------------------
+	 * int imx_crash_uart_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, r4
+	 * -----------------------------------------------
+	 */
+func imx_crash_uart_init
+	/* Free up r1 as a scratch reg */
+	mov     r4, r0
+	mov     r0, r1
+
+	/* Reset UART via CR2 */
+	add     r1, r4, #IMX_UART_CR2_OFFSET
+	movs    r3, #0
+	str     r3, [r4, #IMX_UART_CR2_OFFSET]
+
+	/* Wait for reset complete */
+__wait_cr2_reset:
+	ldr     r3, [r1, #0]
+	ands    r3, #IMX_UART_CR2_SRST
+	beq     __wait_cr2_reset
+
+	/* Enable UART */
+	movs    r3, #IMX_UART_CR1_UARTEN
+	mov     r1, r2
+	str     r3, [r4, #IMX_UART_CR1_OFFSET]
+
+	/*
+	 * Ignore RTC/CTS - disable reset
+	 * Magic value #16423 =>
+	 * IMX_UART_CR2_IRTS | IMX_UART_CR2_WS | IMX_UART_CR2_TXEN | IMX_UART_CR2_RXEN | IMX_UART_CR2_SRST
+	 */
+	movw    r3, #16423
+	str     r3, [r4, #IMX_UART_CR2_OFFSET]
+
+	/*
+	 * No parity, autobaud detect-old, rxdmuxsel=1 (fixed i.mx7)
+	 * Magic value => #132
+	 * IMX_UART_CR3_ADNIMP | IMX_UART_CR3_RXDMUXSEL
+	 */
+	movs    r3, #132
+	str     r3, [r4, #IMX_UART_CR3_OFFSET]
+
+	/*
+	 * Set CTS FIFO trigger to 32 bytes bits 15:10
+	 * Magic value => #32768
+	 * FIFO trigger bitmask 100000
+	 * */
+	mov     r3, #32768
+	str     r3, [r4, #IMX_UART_CR4_OFFSET]
+
+	/*
+	 * TX/RX-thresh = 2 bytes, DCE (bit6 = 0), refclk @24MHz / 4
+	 * Magic value #2562
+	 * IMX_UART_FCR_TXTL(TX_RX_THRESH) | IMX_UART_FCR_RXTL(TX_RX_THRESH) | IMX_UART_FCR_RFDIV2
+	 */
+	#ifdef IMX_UART_DTE
+	movw    r3, #2626
+	#else
+	movw    r3, #2562
+	#endif
+	str     r3, [r4, #IMX_UART_FCR_OFFSET]
+
+	/* This BIR should be set to 0x0F prior to writing the BMR */
+	movs    r3, #15
+	str     r3, [r4, #IMX_UART_BIR_OFFSET]
+
+	/* Hard-code to 115200 @ 24 MHz */
+	movs	r0, #104
+	str     r0, [r4, #IMX_UART_BMR_OFFSET]
+
+	/* Indicate success */
+	movs    r0, #1
+	bx	lr
+endfunc imx_crash_uart_init
+
+	/* --------------------------------------------------------
+	 * int imx_crash_uart_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 imx_crash_uart_putc
+	/* Output specified character to UART shift-register */
+	str	r0, [r1, #IMX_UART_TXD_OFFSET]
+
+        /* Wait for transmit IMX_UART_STAT2_OFFSET.IMX_UART_STAT2_TXDC == 1 */
+__putc_spin_ready:
+	ldr	r2, [r1, #IMX_UART_STAT2_OFFSET]
+	ands	r2, #IMX_UART_STAT2_TXDC
+	beq	__putc_spin_ready
+
+        /* Transmit complete do we need to fixup \n to \n\r */
+	cmp	r0, #10
+	beq	__putc_fixup_lf
+
+	/* No fixup necessary - exit here */
+	movs	r0, #0
+	bx	lr
+
+	/* Fixup \n to \n\r */
+__putc_fixup_lf:
+	movs	r0, #13
+	b	imx_crash_uart_putc
+endfunc imx_crash_uart_putc
diff --git a/drivers/imx/uart/imx_uart.c b/drivers/imx/uart/imx_uart.c
new file mode 100644
index 0000000..dfe2e92
--- /dev/null
+++ b/drivers/imx/uart/imx_uart.c
@@ -0,0 +1,181 @@
+/*
+ * Copyright (c) Linaro 2018 Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <platform_def.h>
+
+#include <arch.h>
+#include <lib/mmio.h>
+
+#include <imx_uart.h>
+
+/* TX/RX FIFO threshold */
+#define TX_RX_THRESH 2
+
+struct clk_div_factors {
+	uint32_t fcr_div;
+	uint32_t bmr_div;
+};
+
+static struct clk_div_factors clk_div[] = {
+	{
+		.fcr_div = IMX_UART_FCR_RFDIV1,
+		.bmr_div = 1,
+	},
+	{
+		.fcr_div = IMX_UART_FCR_RFDIV2,
+		.bmr_div = 2,
+	},
+	{
+		.fcr_div = IMX_UART_FCR_RFDIV3,
+		.bmr_div = 3,
+	},
+	{
+		.fcr_div = IMX_UART_FCR_RFDIV4,
+		.bmr_div = 4,
+	},
+	{
+		.fcr_div = IMX_UART_FCR_RFDIV5,
+		.bmr_div = 5,
+	},
+	{
+		.fcr_div = IMX_UART_FCR_RFDIV6,
+		.bmr_div = 6,
+	},
+	{
+		.fcr_div = IMX_UART_FCR_RFDIV7,
+		.bmr_div = 7,
+	},
+};
+
+static void write_reg(uintptr_t base, uint32_t offset, uint32_t val)
+{
+	mmio_write_32(base + offset, val);
+}
+
+static uint32_t read_reg(uintptr_t base, uint32_t offset)
+{
+	return mmio_read_32(base + offset);
+}
+
+int console_imx_uart_core_init(uintptr_t base_addr, unsigned int uart_clk,
+			       unsigned int baud_rate)
+{
+	uint32_t val;
+	uint8_t clk_idx = 1;
+
+	/* Reset UART */
+	write_reg(base_addr, IMX_UART_CR2_OFFSET, 0);
+	do {
+		val = read_reg(base_addr, IMX_UART_CR2_OFFSET);
+	} while (!(val & IMX_UART_CR2_SRST));
+
+	/* Enable UART */
+	write_reg(base_addr, IMX_UART_CR1_OFFSET, IMX_UART_CR1_UARTEN);
+
+	/* Ignore RTS, 8N1, enable tx/rx, disable reset */
+	val = (IMX_UART_CR2_IRTS | IMX_UART_CR2_WS | IMX_UART_CR2_TXEN |
+	       IMX_UART_CR2_RXEN | IMX_UART_CR2_SRST);
+	write_reg(base_addr, IMX_UART_CR2_OFFSET, val);
+
+	/* No parity, autobaud detect-old, rxdmuxsel=1 (fixed i.mx7) */
+	val = IMX_UART_CR3_ADNIMP | IMX_UART_CR3_RXDMUXSEL;
+	write_reg(base_addr, IMX_UART_CR3_OFFSET, val);
+
+	/* Set CTS FIFO trigger to 32 bytes bits 15:10 */
+	write_reg(base_addr, IMX_UART_CR4_OFFSET, 0x8000);
+
+	/* TX/RX-thresh = 2 bytes, DTE (bit6 = 0), refclk @24MHz / 4 */
+	val = IMX_UART_FCR_TXTL(TX_RX_THRESH) | IMX_UART_FCR_RXTL(TX_RX_THRESH) |
+	      clk_div[clk_idx].fcr_div;
+	#ifdef IMX_UART_DTE
+		/* Set DTE (bit6 = 1) */
+		val |= IMX_UART_FCR_DCEDTE;
+	#endif
+	write_reg(base_addr, IMX_UART_FCR_OFFSET, val);
+
+	/*
+	 * The equation for BAUD rate calculation is
+	 * RefClk = Supplied clock / FCR_DIVx
+	 *
+	 * BAUD  =    Refclk
+	 *         ------------
+	 *       16 x (UBMR + 1/ UBIR + 1)
+	 *
+	 * We write 0x0f into UBIR to remove the 16 mult
+	 * BAUD  =    6000000
+	 *         ------------
+	 *       16 x (UBMR + 1/ 15 + 1)
+	 */
+
+	write_reg(base_addr, IMX_UART_BIR_OFFSET, 0x0f);
+	val = ((uart_clk / clk_div[clk_idx].bmr_div) / baud_rate) - 1;
+	write_reg(base_addr, IMX_UART_BMR_OFFSET, val);
+
+	return 0;
+}
+
+/* --------------------------------------------------------
+ * 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
+ * --------------------------------------------------------
+ */
+int console_imx_uart_core_putc(int c, uintptr_t base_addr)
+{
+	uint32_t val;
+
+	if (c == '\n')
+		console_imx_uart_core_putc('\r', base_addr);
+
+	/* Write data */
+	write_reg(base_addr, IMX_UART_TXD_OFFSET, c);
+
+	/* Wait for transmit */
+	do {
+		val = read_reg(base_addr, IMX_UART_STAT2_OFFSET);
+	} while (!(val & IMX_UART_STAT2_TXDC));
+
+	return 0;
+}
+
+/*
+ * 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
+ * ---------------------------------------------
+ */
+int console_imx_uart_core_getc(uintptr_t base_addr)
+{
+	uint32_t val;
+
+	val = read_reg(base_addr, IMX_UART_TS_OFFSET);
+	if (val & IMX_UART_TS_RXEMPTY)
+		return -1;
+
+	val = read_reg(base_addr, IMX_UART_RXD_OFFSET);
+	return (int)(val & 0x000000FF);
+}
+
+/*
+ * 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
+ * ---------------------------------------------
+ */
+void console_imx_uart_core_flush(uintptr_t base_addr)
+{
+}
+
diff --git a/drivers/imx/uart/imx_uart.h b/drivers/imx/uart/imx_uart.h
new file mode 100644
index 0000000..a133024
--- /dev/null
+++ b/drivers/imx/uart/imx_uart.h
@@ -0,0 +1,163 @@
+/*
+ * Copyright (c) Linaro 2018 Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef IMX_UART_H
+#define IMX_UART_H
+
+#include <drivers/console.h>
+
+#define IMX_UART_RXD_OFFSET	0x00
+#define IMX_UART_RXD_CHARRDY	BIT(15)
+#define IMX_UART_RXD_ERR	BIT(14)
+#define IMX_UART_RXD_OVERRUN	BIT(13)
+#define IMX_UART_RXD_FRMERR	BIT(12)
+#define IMX_UART_RXD_BRK	BIT(11)
+#define IMX_UART_RXD_PRERR	BIT(10)
+
+#define IMX_UART_TXD_OFFSET	0x40
+
+#define IMX_UART_CR1_OFFSET	0x80
+#define IMX_UART_CR1_ADEN	BIT(15)
+#define IMX_UART_CR1_ADBR	BIT(14)
+#define IMX_UART_CR1_TRDYEN	BIT(13)
+#define IMX_UART_CR1_IDEN	BIT(12)
+#define IMX_UART_CR1_RRDYEN	BIT(9)
+#define IMX_UART_CR1_RXDMAEN	BIT(8)
+#define IMX_UART_CR1_IREN	BIT(7)
+#define IMX_UART_CR1_TXMPTYEN	BIT(6)
+#define IMX_UART_CR1_RTSDEN	BIT(5)
+#define IMX_UART_CR1_SNDBRK	BIT(4)
+#define IMX_UART_CR1_TXDMAEN	BIT(3)
+#define IMX_UART_CR1_ATDMAEN	BIT(2)
+#define IMX_UART_CR1_DOZE	BIT(1)
+#define IMX_UART_CR1_UARTEN	BIT(0)
+
+#define IMX_UART_CR2_OFFSET	0x84
+#define IMX_UART_CR2_ESCI	BIT(15)
+#define IMX_UART_CR2_IRTS	BIT(14)
+#define IMX_UART_CR2_CTSC	BIT(13)
+#define IMX_UART_CR2_CTS	BIT(12)
+#define IMX_UART_CR2_ESCEN	BIT(11)
+#define IMX_UART_CR2_PREN	BIT(8)
+#define IMX_UART_CR2_PROE	BIT(7)
+#define IMX_UART_CR2_STPB	BIT(6)
+#define IMX_UART_CR2_WS		BIT(5)
+#define IMX_UART_CR2_RTSEN	BIT(4)
+#define IMX_UART_CR2_ATEN	BIT(3)
+#define IMX_UART_CR2_TXEN	BIT(2)
+#define IMX_UART_CR2_RXEN	BIT(1)
+#define IMX_UART_CR2_SRST	BIT(0)
+
+#define IMX_UART_CR3_OFFSET	0x88
+#define IMX_UART_CR3_DTREN	BIT(13)
+#define IMX_UART_CR3_PARERREN	BIT(12)
+#define IMX_UART_CR3_FARERREN	BIT(11)
+#define IMX_UART_CR3_DSD	BIT(10)
+#define IMX_UART_CR3_DCD	BIT(9)
+#define IMX_UART_CR3_RI		BIT(8)
+#define IMX_UART_CR3_ADNIMP	BIT(7)
+#define IMX_UART_CR3_RXDSEN	BIT(6)
+#define IMX_UART_CR3_AIRINTEN	BIT(5)
+#define IMX_UART_CR3_AWAKEN	BIT(4)
+#define IMX_UART_CR3_DTRDEN	BIT(3)
+#define IMX_UART_CR3_RXDMUXSEL	BIT(2)
+#define IMX_UART_CR3_INVT	BIT(1)
+#define IMX_UART_CR3_ACIEN	BIT(0)
+
+#define IMX_UART_CR4_OFFSET	0x8c
+#define IMX_UART_CR4_INVR	BIT(9)
+#define IMX_UART_CR4_ENIRI	BIT(8)
+#define IMX_UART_CR4_WKEN	BIT(7)
+#define IMX_UART_CR4_IDDMAEN	BIT(6)
+#define IMX_UART_CR4_IRSC	BIT(5)
+#define IMX_UART_CR4_LPBYP	BIT(4)
+#define IMX_UART_CR4_TCEN	BIT(3)
+#define IMX_UART_CR4_BKEN	BIT(2)
+#define IMX_UART_CR4_OREN	BIT(1)
+#define IMX_UART_CR4_DREN	BIT(0)
+
+#define IMX_UART_FCR_OFFSET	0x90
+#define IMX_UART_FCR_TXTL_MASK	(BIT(15) | BIT(14) | BIT(13) | BIT(12) |\
+				 BIT(11) | BIT(10))
+#define IMX_UART_FCR_TXTL(x)	((x) << 10)
+#define IMX_UART_FCR_RFDIV_MASK	(BIT(9) | BIT(8) | BIT(7))
+#define IMX_UART_FCR_RFDIV7	(BIT(9) | BIT(8))
+#define IMX_UART_FCR_RFDIV1	(BIT(9) | BIT(7))
+#define IMX_UART_FCR_RFDIV2	BIT(9)
+#define IMX_UART_FCR_RFDIV3	(BIT(8) | BIT(7))
+#define IMX_UART_FCR_RFDIV4	BIT(8)
+#define IMX_UART_FCR_RFDIV5	BIT(7)
+#define IMX_UART_FCR_RFDIV6	0
+#define IMX_UART_FCR_DCEDTE	BIT(6)
+#define IMX_UART_FCR_RXTL_MASK	(BIT(5) | BIT(4) | BIT(3) | BIT(2) |\
+				 BIT(1) | BIT(0))
+#define IMX_UART_FCR_RXTL(x)	x
+
+#define IMX_UART_STAT1_OFFSET	0x94
+#define IMX_UART_STAT1_PARITYERR	BIT(15)
+#define IMX_UART_STAT1_RTSS	BIT(14)
+#define IMX_UART_STAT1_TRDY	BIT(13)
+#define IMX_UART_STAT1_RTSD	BIT(12)
+#define IMX_UART_STAT1_ESCF	BIT(11)
+#define IMX_UART_STAT1_FRAMEERR	BIT(10)
+#define IMX_UART_STAT1_RRDY	BIT(9)
+#define IMX_UART_STAT1_AGTIM	BIT(8)
+#define IMX_UART_STAT1_DTRD	BIT(7)
+#define IMX_UART_STAT1_RXDS	BIT(6)
+#define IMX_UART_STAT1_AIRINT	BIT(5)
+#define IMX_UART_STAT1_AWAKE	BIT(4)
+#define IMX_UART_STAT1_SAD	BIT(3)
+
+#define IMX_UART_STAT2_OFFSET	0x98
+#define IMX_UART_STAT2_ADET	BIT(15)
+#define IMX_UART_STAT2_TXFE	BIT(14)
+#define IMX_UART_STAT2_DTRF	BIT(13)
+#define IMX_UART_STAT2_IDLE	BIT(12)
+#define IMX_UART_STAT2_ACST	BIT(11)
+#define IMX_UART_STAT2_RIDELT	BIT(10)
+#define IMX_UART_STAT2_RIIN	BIT(9)
+#define IMX_UART_STAT2_IRINT	BIT(8)
+#define IMX_UART_STAT2_WAKE	BIT(7)
+#define IMX_UART_STAT2_DCDDELT	BIT(6)
+#define IMX_UART_STAT2_DCDIN	BIT(5)
+#define IMX_UART_STAT2_RTSF	BIT(4)
+#define IMX_UART_STAT2_TXDC	BIT(3)
+#define IMX_UART_STAT2_BRCD	BIT(2)
+#define IMX_UART_STAT2_ORE	BIT(1)
+#define IMX_UART_STAT2_RCR	BIT(0)
+
+#define IMX_UART_ESC_OFFSET	0x9c
+
+#define IMX_UART_TIM_OFFSET	0xa0
+
+#define IMX_UART_BIR_OFFSET	0xa4
+
+#define IMX_UART_BMR_OFFSET	0xa8
+
+#define IMX_UART_BRC_OFFSET	0xac
+
+#define IMX_UART_ONEMS_OFFSET	0xb0
+
+#define IMX_UART_TS_OFFSET	0xb4
+#define IMX_UART_TS_FRCPERR	BIT(13)
+#define IMX_UART_TS_LOOP	BIT(12)
+#define IMX_UART_TS_DBGEN	BIT(11)
+#define IMX_UART_TS_LOOPIR	BIT(10)
+#define IMX_UART_TS_RXDBG	BIT(9)
+#define IMX_UART_TS_TXEMPTY	BIT(6)
+#define IMX_UART_TS_RXEMPTY	BIT(5)
+#define IMX_UART_TS_TXFULL	BIT(4)
+#define IMX_UART_TS_RXFULL	BIT(3)
+#define IMX_UART_TS_SOFTRST	BIT(0)
+
+#ifndef __ASSEMBLER__
+
+int console_imx_uart_register(uintptr_t baseaddr,
+			      uint32_t clock,
+			      uint32_t baud,
+			      console_t *console);
+#endif /*__ASSEMBLER__*/
+
+#endif /* IMX_UART_H */
diff --git a/drivers/imx/usdhc/imx_usdhc.c b/drivers/imx/usdhc/imx_usdhc.c
new file mode 100644
index 0000000..07f55b7
--- /dev/null
+++ b/drivers/imx/usdhc/imx_usdhc.c
@@ -0,0 +1,302 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/mmc.h>
+#include <lib/mmio.h>
+
+#include <imx_usdhc.h>
+
+static void imx_usdhc_initialize(void);
+static int imx_usdhc_send_cmd(struct mmc_cmd *cmd);
+static int imx_usdhc_set_ios(unsigned int clk, unsigned int width);
+static int imx_usdhc_prepare(int lba, uintptr_t buf, size_t size);
+static int imx_usdhc_read(int lba, uintptr_t buf, size_t size);
+static int imx_usdhc_write(int lba, uintptr_t buf, size_t size);
+
+static const struct mmc_ops imx_usdhc_ops = {
+	.init		= imx_usdhc_initialize,
+	.send_cmd	= imx_usdhc_send_cmd,
+	.set_ios	= imx_usdhc_set_ios,
+	.prepare	= imx_usdhc_prepare,
+	.read		= imx_usdhc_read,
+	.write		= imx_usdhc_write,
+};
+
+static imx_usdhc_params_t imx_usdhc_params;
+
+#define IMX7_MMC_SRC_CLK_RATE (200 * 1000 * 1000)
+static void imx_usdhc_set_clk(int clk)
+{
+	int div = 1;
+	int pre_div = 1;
+	unsigned int sdhc_clk = IMX7_MMC_SRC_CLK_RATE;
+	uintptr_t reg_base = imx_usdhc_params.reg_base;
+
+	assert(clk > 0);
+
+	while (sdhc_clk / (16 * pre_div) > clk && pre_div < 256)
+		pre_div *= 2;
+
+	while (sdhc_clk / div > clk && div < 16)
+		div++;
+
+	pre_div >>= 1;
+	div -= 1;
+	clk = (pre_div << 8) | (div << 4);
+
+	mmio_clrbits32(reg_base + VENDSPEC, VENDSPEC_CARD_CLKEN);
+	mmio_clrsetbits32(reg_base + SYSCTRL, SYSCTRL_CLOCK_MASK, clk);
+	udelay(10000);
+
+	mmio_setbits32(reg_base + VENDSPEC, VENDSPEC_PER_CLKEN | VENDSPEC_CARD_CLKEN);
+}
+
+static void imx_usdhc_initialize(void)
+{
+	unsigned int timeout = 10000;
+	uintptr_t reg_base = imx_usdhc_params.reg_base;
+
+	assert((imx_usdhc_params.reg_base & MMC_BLOCK_MASK) == 0);
+
+	/* reset the controller */
+	mmio_setbits32(reg_base + SYSCTRL, SYSCTRL_RSTA);
+
+	/* wait for reset done */
+	while ((mmio_read_32(reg_base + SYSCTRL) & SYSCTRL_RSTA)) {
+		if (!timeout)
+			ERROR("IMX MMC reset timeout.\n");
+		timeout--;
+	}
+
+	mmio_write_32(reg_base + MMCBOOT, 0);
+	mmio_write_32(reg_base + MIXCTRL, 0);
+	mmio_write_32(reg_base + CLKTUNECTRLSTS, 0);
+
+	mmio_write_32(reg_base + VENDSPEC, VENDSPEC_INIT);
+	mmio_write_32(reg_base + DLLCTRL, 0);
+	mmio_setbits32(reg_base + VENDSPEC, VENDSPEC_IPG_CLKEN | VENDSPEC_PER_CLKEN);
+
+	/* Set the initial boot clock rate */
+	imx_usdhc_set_clk(MMC_BOOT_CLK_RATE);
+	udelay(100);
+
+	/* Clear read/write ready status */
+	mmio_clrbits32(reg_base + INTSTATEN, INTSTATEN_BRR | INTSTATEN_BWR);
+
+	/* configure as little endian */
+	mmio_write_32(reg_base + PROTCTRL, PROTCTRL_LE);
+
+	/* Set timeout to the maximum value */
+	mmio_clrsetbits32(reg_base + SYSCTRL, SYSCTRL_TIMEOUT_MASK,
+			  SYSCTRL_TIMEOUT(15));
+
+	/* set wartermark level as 16 for safe for MMC */
+	mmio_clrsetbits32(reg_base + WATERMARKLEV, WMKLV_MASK, 16 | (16 << 16));
+}
+
+#define FSL_CMD_RETRIES	1000
+
+static int imx_usdhc_send_cmd(struct mmc_cmd *cmd)
+{
+	uintptr_t reg_base = imx_usdhc_params.reg_base;
+	unsigned int xfertype = 0, mixctl = 0, multiple = 0, data = 0, err = 0;
+	unsigned int state, flags = INTSTATEN_CC | INTSTATEN_CTOE;
+	unsigned int cmd_retries = 0;
+
+	assert(cmd);
+
+	/* clear all irq status */
+	mmio_write_32(reg_base + INTSTAT, 0xffffffff);
+
+	/* Wait for the bus to be idle */
+	do {
+		state = mmio_read_32(reg_base + PSTATE);
+	} while (state & (PSTATE_CDIHB | PSTATE_CIHB));
+
+	while (mmio_read_32(reg_base + PSTATE) & PSTATE_DLA)
+		;
+
+	mmio_write_32(reg_base + INTSIGEN, 0);
+	udelay(1000);
+
+	switch (cmd->cmd_idx) {
+	case MMC_CMD(12):
+		xfertype |= XFERTYPE_CMDTYP_ABORT;
+		break;
+	case MMC_CMD(18):
+		multiple = 1;
+		/* fall thru for read op */
+	case MMC_CMD(17):
+	case MMC_CMD(8):
+		mixctl |= MIXCTRL_DTDSEL;
+		data = 1;
+		break;
+	case MMC_CMD(25):
+		multiple = 1;
+		/* fall thru for data op flag */
+	case MMC_CMD(24):
+		data = 1;
+		break;
+	default:
+		break;
+	}
+
+	if (multiple) {
+		mixctl |= MIXCTRL_MSBSEL;
+		mixctl |= MIXCTRL_BCEN;
+	}
+
+	if (data) {
+		xfertype |= XFERTYPE_DPSEL;
+		mixctl |= MIXCTRL_DMAEN;
+	}
+
+	if (cmd->resp_type & MMC_RSP_48 && cmd->resp_type != MMC_RESPONSE_R2)
+		xfertype |= XFERTYPE_RSPTYP_48;
+	else if (cmd->resp_type & MMC_RSP_136)
+		xfertype |= XFERTYPE_RSPTYP_136;
+	else if (cmd->resp_type & MMC_RSP_BUSY)
+		xfertype |= XFERTYPE_RSPTYP_48_BUSY;
+
+	if (cmd->resp_type & MMC_RSP_CMD_IDX)
+		xfertype |= XFERTYPE_CICEN;
+
+	if (cmd->resp_type & MMC_RSP_CRC)
+		xfertype |= XFERTYPE_CCCEN;
+
+	xfertype |= XFERTYPE_CMD(cmd->cmd_idx);
+
+	/* Send the command */
+	mmio_write_32(reg_base + CMDARG, cmd->cmd_arg);
+	mmio_clrsetbits32(reg_base + MIXCTRL, MIXCTRL_DATMASK, mixctl);
+	mmio_write_32(reg_base + XFERTYPE, xfertype);
+
+	/* Wait for the command done */
+	do {
+		state = mmio_read_32(reg_base + INTSTAT);
+		if (cmd_retries)
+			udelay(1);
+	} while ((!(state & flags)) && ++cmd_retries < FSL_CMD_RETRIES);
+
+	if ((state & (INTSTATEN_CTOE | CMD_ERR)) || cmd_retries == FSL_CMD_RETRIES) {
+		if (cmd_retries == FSL_CMD_RETRIES)
+			err = -ETIMEDOUT;
+		else
+			err = -EIO;
+		ERROR("imx_usdhc mmc cmd %d state 0x%x errno=%d\n",
+		      cmd->cmd_idx, state, err);
+		goto out;
+	}
+
+	/* Copy the response to the response buffer */
+	if (cmd->resp_type & MMC_RSP_136) {
+		unsigned int cmdrsp3, cmdrsp2, cmdrsp1, cmdrsp0;
+
+		cmdrsp3 = mmio_read_32(reg_base + CMDRSP3);
+		cmdrsp2 = mmio_read_32(reg_base + CMDRSP2);
+		cmdrsp1 = mmio_read_32(reg_base + CMDRSP1);
+		cmdrsp0 = mmio_read_32(reg_base + CMDRSP0);
+		cmd->resp_data[3] = (cmdrsp3 << 8) | (cmdrsp2 >> 24);
+		cmd->resp_data[2] = (cmdrsp2 << 8) | (cmdrsp1 >> 24);
+		cmd->resp_data[1] = (cmdrsp1 << 8) | (cmdrsp0 >> 24);
+		cmd->resp_data[0] = (cmdrsp0 << 8);
+	} else {
+		cmd->resp_data[0] = mmio_read_32(reg_base + CMDRSP0);
+	}
+
+	/* Wait until all of the blocks are transferred */
+	if (data) {
+		flags = DATA_COMPLETE;
+		do {
+			state = mmio_read_32(reg_base + INTSTAT);
+
+			if (state & (INTSTATEN_DTOE | DATA_ERR)) {
+				err = -EIO;
+				ERROR("imx_usdhc mmc data state 0x%x\n", state);
+				goto out;
+			}
+		} while ((state & flags) != flags);
+	}
+
+out:
+	/* Reset CMD and DATA on error */
+	if (err) {
+		mmio_setbits32(reg_base + SYSCTRL, SYSCTRL_RSTC);
+		while (mmio_read_32(reg_base + SYSCTRL) & SYSCTRL_RSTC)
+			;
+
+		if (data) {
+			mmio_setbits32(reg_base + SYSCTRL, SYSCTRL_RSTD);
+			while (mmio_read_32(reg_base + SYSCTRL) & SYSCTRL_RSTD)
+				;
+		}
+	}
+
+	/* clear all irq status */
+	mmio_write_32(reg_base + INTSTAT, 0xffffffff);
+
+	return err;
+}
+
+static int imx_usdhc_set_ios(unsigned int clk, unsigned int width)
+{
+	uintptr_t reg_base = imx_usdhc_params.reg_base;
+
+	imx_usdhc_set_clk(clk);
+
+	if (width == MMC_BUS_WIDTH_4)
+		mmio_clrsetbits32(reg_base + PROTCTRL, PROTCTRL_WIDTH_MASK,
+				  PROTCTRL_WIDTH_4);
+	else if (width == MMC_BUS_WIDTH_8)
+		mmio_clrsetbits32(reg_base + PROTCTRL, PROTCTRL_WIDTH_MASK,
+				  PROTCTRL_WIDTH_8);
+
+	return 0;
+}
+
+static int imx_usdhc_prepare(int lba, uintptr_t buf, size_t size)
+{
+	uintptr_t reg_base = imx_usdhc_params.reg_base;
+
+	mmio_write_32(reg_base + DSADDR, buf);
+	mmio_write_32(reg_base + BLKATT,
+		      (size / MMC_BLOCK_SIZE) << 16 | MMC_BLOCK_SIZE);
+
+	return 0;
+}
+
+static int imx_usdhc_read(int lba, uintptr_t buf, size_t size)
+{
+	return 0;
+}
+
+static int imx_usdhc_write(int lba, uintptr_t buf, size_t size)
+{
+	return 0;
+}
+
+void imx_usdhc_init(imx_usdhc_params_t *params,
+		    struct mmc_device_info *mmc_dev_info)
+{
+	assert((params != 0) &&
+	       ((params->reg_base & MMC_BLOCK_MASK) == 0) &&
+	       (params->clk_rate > 0) &&
+	       ((params->bus_width == MMC_BUS_WIDTH_1) ||
+		(params->bus_width == MMC_BUS_WIDTH_4) ||
+		(params->bus_width == MMC_BUS_WIDTH_8)));
+
+	memcpy(&imx_usdhc_params, params, sizeof(imx_usdhc_params_t));
+	mmc_init(&imx_usdhc_ops, params->clk_rate, params->bus_width,
+		 params->flags, mmc_dev_info);
+}
diff --git a/drivers/imx/usdhc/imx_usdhc.h b/drivers/imx/usdhc/imx_usdhc.h
new file mode 100644
index 0000000..e063316
--- /dev/null
+++ b/drivers/imx/usdhc/imx_usdhc.h
@@ -0,0 +1,137 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_USDHC_H
+#define IMX_USDHC_H
+
+#include <drivers/mmc.h>
+
+typedef struct imx_usdhc_params {
+	uintptr_t	reg_base;
+	int		clk_rate;
+	int		bus_width;
+	unsigned int	flags;
+} imx_usdhc_params_t;
+
+void imx_usdhc_init(imx_usdhc_params_t *params,
+		    struct mmc_device_info *mmc_dev_info);
+
+/* iMX MMC registers definition */
+#define DSADDR			0x000
+#define BLKATT			0x004
+#define CMDARG			0x008
+#define CMDRSP0			0x010
+#define CMDRSP1			0x014
+#define CMDRSP2			0x018
+#define CMDRSP3			0x01c
+
+#define XFERTYPE		0x00c
+#define XFERTYPE_CMD(x)		(((x) & 0x3f) << 24)
+#define XFERTYPE_CMDTYP_ABORT	(3 << 22)
+#define XFERTYPE_DPSEL		BIT(21)
+#define XFERTYPE_CICEN		BIT(20)
+#define XFERTYPE_CCCEN		BIT(19)
+#define XFERTYPE_RSPTYP_136	BIT(16)
+#define XFERTYPE_RSPTYP_48	BIT(17)
+#define XFERTYPE_RSPTYP_48_BUSY	(BIT(16) | BIT(17))
+
+#define PSTATE			0x024
+#define PSTATE_DAT0		BIT(24)
+#define PSTATE_DLA		BIT(2)
+#define PSTATE_CDIHB		BIT(1)
+#define PSTATE_CIHB		BIT(0)
+
+#define PROTCTRL		0x028
+#define PROTCTRL_LE		BIT(5)
+#define PROTCTRL_WIDTH_4	BIT(1)
+#define PROTCTRL_WIDTH_8	BIT(2)
+#define PROTCTRL_WIDTH_MASK	0x6
+
+#define SYSCTRL			0x02c
+#define SYSCTRL_RSTD		BIT(26)
+#define SYSCTRL_RSTC		BIT(25)
+#define SYSCTRL_RSTA		BIT(24)
+#define SYSCTRL_CLOCK_MASK	0x0000fff0
+#define SYSCTRL_TIMEOUT_MASK	0x000f0000
+#define SYSCTRL_TIMEOUT(x)	((0xf & (x)) << 16)
+
+#define INTSTAT			0x030
+#define INTSTAT_DMAE		BIT(28)
+#define INTSTAT_DEBE		BIT(22)
+#define INTSTAT_DCE		BIT(21)
+#define INTSTAT_DTOE		BIT(20)
+#define INTSTAT_CIE		BIT(19)
+#define INTSTAT_CEBE		BIT(18)
+#define INTSTAT_CCE		BIT(17)
+#define INTSTAT_DINT		BIT(3)
+#define INTSTAT_BGE		BIT(2)
+#define INTSTAT_TC		BIT(1)
+#define INTSTAT_CC		BIT(0)
+#define CMD_ERR			(INTSTAT_CIE | INTSTAT_CEBE | INTSTAT_CCE)
+#define DATA_ERR		(INTSTAT_DMAE | INTSTAT_DEBE | INTSTAT_DCE | \
+				 INTSTAT_DTOE)
+#define DATA_COMPLETE		(INTSTAT_DINT | INTSTAT_TC)
+
+#define INTSTATEN		0x034
+#define INTSTATEN_DEBE		BIT(22)
+#define INTSTATEN_DCE		BIT(21)
+#define INTSTATEN_DTOE		BIT(20)
+#define INTSTATEN_CIE		BIT(19)
+#define INTSTATEN_CEBE		BIT(18)
+#define INTSTATEN_CCE		BIT(17)
+#define INTSTATEN_CTOE		BIT(16)
+#define INTSTATEN_CINT		BIT(8)
+#define INTSTATEN_BRR		BIT(5)
+#define INTSTATEN_BWR		BIT(4)
+#define INTSTATEN_DINT		BIT(3)
+#define INTSTATEN_TC		BIT(1)
+#define INTSTATEN_CC		BIT(0)
+#define EMMC_INTSTATEN_BITS	(INTSTATEN_CC | INTSTATEN_TC | INTSTATEN_DINT | \
+				 INTSTATEN_BWR | INTSTATEN_BRR | INTSTATEN_CINT | \
+				 INTSTATEN_CTOE | INTSTATEN_CCE | INTSTATEN_CEBE | \
+				 INTSTATEN_CIE | INTSTATEN_DTOE | INTSTATEN_DCE | \
+				 INTSTATEN_DEBE)
+
+#define INTSIGEN		0x038
+
+#define WATERMARKLEV		0x044
+#define WMKLV_RD_MASK		0xff
+#define WMKLV_WR_MASK		0x00ff0000
+#define WMKLV_MASK		(WMKLV_RD_MASK | WMKLV_WR_MASK)
+
+#define MIXCTRL			0x048
+#define MIXCTRL_MSBSEL		BIT(5)
+#define MIXCTRL_DTDSEL		BIT(4)
+#define MIXCTRL_DDREN		BIT(3)
+#define MIXCTRL_AC12EN		BIT(2)
+#define MIXCTRL_BCEN		BIT(1)
+#define MIXCTRL_DMAEN		BIT(0)
+#define MIXCTRL_DATMASK		0x7f
+
+#define DLLCTRL			0x060
+
+#define CLKTUNECTRLSTS		0x068
+
+#define VENDSPEC		0x0c0
+#define VENDSPEC_RSRV1		BIT(29)
+#define VENDSPEC_CARD_CLKEN	BIT(14)
+#define VENDSPEC_PER_CLKEN	BIT(13)
+#define VENDSPEC_AHB_CLKEN	BIT(12)
+#define VENDSPEC_IPG_CLKEN	BIT(11)
+#define VENDSPEC_AC12_CHKBUSY	BIT(3)
+#define VENDSPEC_EXTDMA		BIT(0)
+#define VENDSPEC_INIT		(VENDSPEC_RSRV1	| VENDSPEC_CARD_CLKEN | \
+				 VENDSPEC_PER_CLKEN | VENDSPEC_AHB_CLKEN | \
+				 VENDSPEC_IPG_CLKEN | VENDSPEC_AC12_CHKBUSY | \
+				 VENDSPEC_EXTDMA)
+
+#define MMCBOOT			0x0c4
+
+#define mmio_clrsetbits32(addr, clear, set)	mmio_write_32(addr, (mmio_read_32(addr) & ~(clear)) | (set))
+#define mmio_clrbits32(addr, clear)		mmio_write_32(addr, mmio_read_32(addr) & ~(clear))
+#define mmio_setbits32(addr, set)		mmio_write_32(addr, mmio_read_32(addr) | (set))
+
+#endif /* IMX_USDHC_H */
diff --git a/drivers/intel/soc/stratix10/io/s10_memmap_qspi.c b/drivers/intel/soc/stratix10/io/s10_memmap_qspi.c
new file mode 100644
index 0000000..dcd1991
--- /dev/null
+++ b/drivers/intel/soc/stratix10/io/s10_memmap_qspi.c
@@ -0,0 +1,253 @@
+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_memmap.h>
+#include <drivers/io/io_storage.h>
+#include <lib/utils.h>
+
+#include "qspi/cadence_qspi.h"
+
+/* As we need to be able to keep state for seek, only one file can be open
+ * at a time. Make this a structure and point to the entity->info. When we
+ * can malloc memory we can change this to support more open files.
+ */
+typedef struct {
+	/* Use the 'in_use' flag as any value for base and file_pos could be
+	 * valid.
+	 */
+	int		in_use;
+	uintptr_t		base;
+	unsigned long long	file_pos;
+	unsigned long long	size;
+} file_state_t;
+
+static file_state_t current_file = {0};
+
+/* Identify the device type as memmap */
+static io_type_t device_type_memmap(void)
+{
+	return IO_TYPE_MEMMAP;
+}
+
+/* Memmap device functions */
+static int memmap_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int memmap_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			     io_entity_t *entity);
+static int memmap_block_seek(io_entity_t *entity, int mode,
+			     signed long long offset);
+static int memmap_block_len(io_entity_t *entity, size_t *length);
+static int memmap_block_read(io_entity_t *entity, uintptr_t buffer,
+			     size_t length, size_t *length_read);
+static int memmap_block_write(io_entity_t *entity, const uintptr_t buffer,
+			      size_t length, size_t *length_written);
+static int memmap_block_close(io_entity_t *entity);
+static int memmap_dev_close(io_dev_info_t *dev_info);
+
+
+static const io_dev_connector_t memmap_dev_connector = {
+	.dev_open = memmap_dev_open
+};
+
+
+static const io_dev_funcs_t memmap_dev_funcs = {
+	.type = device_type_memmap,
+	.open = memmap_block_open,
+	.seek = memmap_block_seek,
+	.size = memmap_block_len,
+	.read = memmap_block_read,
+	.write = memmap_block_write,
+	.close = memmap_block_close,
+	.dev_init = NULL,
+	.dev_close = memmap_dev_close,
+};
+
+
+/* No state associated with this device so structure can be const */
+static const io_dev_info_t memmap_dev_info = {
+	.funcs = &memmap_dev_funcs,
+	.info = (uintptr_t)NULL
+};
+
+
+/* Open a connection to the memmap device */
+static int memmap_dev_open(const uintptr_t dev_spec __unused,
+			   io_dev_info_t **dev_info)
+{
+	assert(dev_info != NULL);
+	*dev_info = (io_dev_info_t *)&memmap_dev_info; /* cast away const */
+
+	return 0;
+}
+
+
+
+/* Close a connection to the memmap device */
+static int memmap_dev_close(io_dev_info_t *dev_info)
+{
+	/* NOP */
+	/* TODO: Consider tracking open files and cleaning them up here */
+	return 0;
+}
+
+
+/* Open a file on the memmap device */
+static int memmap_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			     io_entity_t *entity)
+{
+	int result = -ENOMEM;
+	const io_block_spec_t *block_spec = (io_block_spec_t *)spec;
+
+	/* Since we need to track open state for seek() we only allow one open
+	 * spec at a time. When we have dynamic memory we can malloc and set
+	 * entity->info.
+	 */
+	if (current_file.in_use == 0) {
+		assert(block_spec != NULL);
+		assert(entity != NULL);
+
+		current_file.in_use = 1;
+		current_file.base = block_spec->offset;
+		/* File cursor offset for seek and incremental reads etc. */
+		current_file.file_pos = 0;
+		current_file.size = block_spec->length;
+		entity->info = (uintptr_t)&current_file;
+		result = 0;
+	} else {
+		WARN("A Memmap device is already active. Close first.\n");
+	}
+
+	return result;
+}
+
+
+/* Seek to a particular file offset on the memmap device */
+static int memmap_block_seek(io_entity_t *entity, int mode,
+			     signed long long offset)
+{
+	int result = -ENOENT;
+	file_state_t *fp;
+
+	/* We only support IO_SEEK_SET for the moment. */
+	if (mode == IO_SEEK_SET) {
+		assert(entity != NULL);
+
+		fp = (file_state_t *) entity->info;
+
+		/* Assert that new file position is valid */
+		assert((offset >= 0) &&
+		       ((unsigned long long)offset < fp->size));
+
+		/* Reset file position */
+		fp->file_pos = offset;
+		result = 0;
+	}
+
+	return result;
+}
+
+
+/* Return the size of a file on the memmap device */
+static int memmap_block_len(io_entity_t *entity, size_t *length)
+{
+	assert(entity != NULL);
+	assert(length != NULL);
+
+	*length = ((file_state_t *)entity->info)->size;
+
+	return 0;
+}
+
+
+/* Read data from a file on the memmap device */
+static int memmap_block_read(io_entity_t *entity, uintptr_t buffer,
+			     size_t length, size_t *length_read)
+{
+	file_state_t *fp;
+	unsigned long long pos_after;
+
+	assert(entity != NULL);
+	assert(length_read != NULL);
+
+	fp = (file_state_t *) entity->info;
+
+	/* Assert that file position is valid for this read operation */
+	pos_after = fp->file_pos + length;
+	assert((pos_after >= fp->file_pos) && (pos_after <= fp->size));
+
+	//memcpy((void *)buffer, (void *)(fp->base + fp->file_pos), length);
+	cad_qspi_read((void *)buffer, fp->base + fp->file_pos, length);
+	*length_read = length;
+
+	/* Set file position after read */
+	fp->file_pos = pos_after;
+
+	return 0;
+}
+
+
+/* Write data to a file on the memmap device */
+static int memmap_block_write(io_entity_t *entity, const uintptr_t buffer,
+			      size_t length, size_t *length_written)
+{
+	file_state_t *fp;
+	unsigned long long pos_after;
+
+	assert(entity != NULL);
+	assert(length_written != NULL);
+
+	fp = (file_state_t *) entity->info;
+
+	/* Assert that file position is valid for this write operation */
+	pos_after = fp->file_pos + length;
+	assert((pos_after >= fp->file_pos) && (pos_after <= fp->size));
+
+	memcpy((void *)(fp->base + fp->file_pos), (void *)buffer, length);
+
+	*length_written = length;
+
+	/* Set file position after write */
+	fp->file_pos = pos_after;
+
+	return 0;
+}
+
+
+/* Close a file on the memmap device */
+static int memmap_block_close(io_entity_t *entity)
+{
+	assert(entity != NULL);
+
+	entity->info = 0;
+
+	/* This would be a mem free() if we had malloc.*/
+	zeromem((void *)&current_file, sizeof(current_file));
+
+	return 0;
+}
+
+
+/* Exported functions */
+
+/* Register the memmap driver with the IO abstraction */
+int register_io_dev_memmap(const io_dev_connector_t **dev_con)
+{
+	int result;
+
+	assert(dev_con != NULL);
+
+	result = io_register_device(&memmap_dev_info);
+	if (result == 0)
+		*dev_con = &memmap_dev_connector;
+
+	return result;
+}
diff --git a/drivers/io/io_block.c b/drivers/io/io_block.c
new file mode 100644
index 0000000..5d45c2f
--- /dev/null
+++ b/drivers/io/io_block.c
@@ -0,0 +1,551 @@
+/*
+ * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <drivers/io/io_block.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_storage.h>
+#include <lib/utils.h>
+
+typedef struct {
+	io_block_dev_spec_t	*dev_spec;
+	uintptr_t		base;
+	unsigned long long	file_pos;
+	unsigned long long	size;
+} block_dev_state_t;
+
+#define is_power_of_2(x)	(((x) != 0U) && (((x) & ((x) - 1U)) == 0U))
+
+io_type_t device_type_block(void);
+
+static int block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+		      io_entity_t *entity);
+static int block_seek(io_entity_t *entity, int mode, signed long long offset);
+static int block_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+		      size_t *length_read);
+static int block_write(io_entity_t *entity, const uintptr_t buffer,
+		       size_t length, size_t *length_written);
+static int block_close(io_entity_t *entity);
+static int block_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int block_dev_close(io_dev_info_t *dev_info);
+
+static const io_dev_connector_t block_dev_connector = {
+	.dev_open	= block_dev_open
+};
+
+static const io_dev_funcs_t block_dev_funcs = {
+	.type		= device_type_block,
+	.open		= block_open,
+	.seek		= block_seek,
+	.size		= NULL,
+	.read		= block_read,
+	.write		= block_write,
+	.close		= block_close,
+	.dev_init	= NULL,
+	.dev_close	= block_dev_close,
+};
+
+static block_dev_state_t state_pool[MAX_IO_BLOCK_DEVICES];
+static io_dev_info_t dev_info_pool[MAX_IO_BLOCK_DEVICES];
+
+/* Track number of allocated block state */
+static unsigned int block_dev_count;
+
+io_type_t device_type_block(void)
+{
+	return IO_TYPE_BLOCK;
+}
+
+/* Locate a block state in the pool, specified by address */
+static int find_first_block_state(const io_block_dev_spec_t *dev_spec,
+				  unsigned int *index_out)
+{
+	unsigned int index;
+	int result = -ENOENT;
+
+	for (index = 0U; index < MAX_IO_BLOCK_DEVICES; ++index) {
+		/* dev_spec is used as identifier since it's unique */
+		if (state_pool[index].dev_spec == dev_spec) {
+			result = 0;
+			*index_out = index;
+			break;
+		}
+	}
+	return result;
+}
+
+/* Allocate a device info from the pool and return a pointer to it */
+static int allocate_dev_info(io_dev_info_t **dev_info)
+{
+	int result = -ENOMEM;
+	assert(dev_info != NULL);
+
+	if (block_dev_count < MAX_IO_BLOCK_DEVICES) {
+		unsigned int index = 0;
+		result = find_first_block_state(NULL, &index);
+		assert(result == 0);
+		/* initialize dev_info */
+		dev_info_pool[index].funcs = &block_dev_funcs;
+		dev_info_pool[index].info = (uintptr_t)&state_pool[index];
+		*dev_info = &dev_info_pool[index];
+		++block_dev_count;
+	}
+
+	return result;
+}
+
+
+/* Release a device info to the pool */
+static int free_dev_info(io_dev_info_t *dev_info)
+{
+	int result;
+	unsigned int index = 0;
+	block_dev_state_t *state;
+	assert(dev_info != NULL);
+
+	state = (block_dev_state_t *)dev_info->info;
+	result = find_first_block_state(state->dev_spec, &index);
+	if (result ==  0) {
+		/* free if device info is valid */
+		zeromem(state, sizeof(block_dev_state_t));
+		zeromem(dev_info, sizeof(io_dev_info_t));
+		--block_dev_count;
+	}
+
+	return result;
+}
+
+static int block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+		      io_entity_t *entity)
+{
+	block_dev_state_t *cur;
+	io_block_spec_t *region;
+
+	assert((dev_info->info != (uintptr_t)NULL) &&
+	       (spec != (uintptr_t)NULL) &&
+	       (entity->info == (uintptr_t)NULL));
+
+	region = (io_block_spec_t *)spec;
+	cur = (block_dev_state_t *)dev_info->info;
+	assert(((region->offset % cur->dev_spec->block_size) == 0) &&
+	       ((region->length % cur->dev_spec->block_size) == 0));
+
+	cur->base = region->offset;
+	cur->size = region->length;
+	cur->file_pos = 0;
+
+	entity->info = (uintptr_t)cur;
+	return 0;
+}
+
+/* parameter offset is relative address at here */
+static int block_seek(io_entity_t *entity, int mode, signed long long offset)
+{
+	block_dev_state_t *cur;
+
+	assert(entity->info != (uintptr_t)NULL);
+
+	cur = (block_dev_state_t *)entity->info;
+	assert((offset >= 0) && ((unsigned long long)offset < cur->size));
+
+	switch (mode) {
+	case IO_SEEK_SET:
+		cur->file_pos = (unsigned long long)offset;
+		break;
+	case IO_SEEK_CUR:
+		cur->file_pos += (unsigned long long)offset;
+		break;
+	default:
+		return -EINVAL;
+	}
+	assert(cur->file_pos < cur->size);
+	return 0;
+}
+
+/*
+ * This function allows the caller to read any number of bytes
+ * from any position. It hides from the caller that the low level
+ * driver only can read aligned blocks of data. For this reason
+ * we need to handle the use case where the first byte to be read is not
+ * aligned to start of the block, the last byte to be read is also not
+ * aligned to the end of a block, and there are zero or more blocks-worth
+ * of data in between.
+ *
+ * In such a case we need to read more bytes than requested (i.e. full
+ * blocks) and strip-out the leading bytes (aka skip) and the trailing
+ * bytes (aka padding). See diagram below
+ *
+ * cur->file_pos ------------
+ *                          |
+ * cur->base                |
+ *  |                       |
+ *  v                       v<----  length   ---->
+ *  --------------------------------------------------------------
+ * |           |         block#1    |        |   block#n          |
+ * |  block#0  |            +       |   ...  |     +              |
+ * |           | <- skip -> +       |        |     + <- padding ->|
+ *  ------------------------+----------------------+--------------
+ *             ^                                                  ^
+ *             |                                                  |
+ *             v    iteration#1                iteration#n        v
+ *              --------------------------------------------------
+ *             |                    |        |                    |
+ *             |<----  request ---->|  ...   |<----- request ---->|
+ *             |                    |        |                    |
+ *              --------------------------------------------------
+ *            /                   /          |                    |
+ *           /                   /           |                    |
+ *          /                   /            |                    |
+ *         /                   /             |                    |
+ *        /                   /              |                    |
+ *       /                   /               |                    |
+ *      /                   /                |                    |
+ *     /                   /                 |                    |
+ *    /                   /                  |                    |
+ *   /                   /                   |                    |
+ *  <---- request ------>                    <------ request  ----->
+ *  ---------------------                    -----------------------
+ *  |        |          |                    |          |           |
+ *  |<-skip->|<-nbytes->|           -------->|<-nbytes->|<-padding->|
+ *  |        |          |           |        |          |           |
+ *  ---------------------           |        -----------------------
+ *  ^        \           \          |        |          |
+ *  |         \           \         |        |          |
+ *  |          \           \        |        |          |
+ *  buf->offset \           \   buf->offset  |          |
+ *               \           \               |          |
+ *                \           \              |          |
+ *                 \           \             |          |
+ *                  \           \            |          |
+ *                   \           \           |          |
+ *                    \           \          |          |
+ *                     \           \         |          |
+ *                      --------------------------------
+ *                      |           |        |         |
+ * buffer-------------->|           | ...    |         |
+ *                      |           |        |         |
+ *                      --------------------------------
+ *                      <-count#1->|                   |
+ *                      <----------  count#n   -------->
+ *                      <----------  length  ---------->
+ *
+ * Additionally, the IO driver has an underlying buffer that is at least
+ * one block-size and may be big enough to allow.
+ */
+static int block_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+		      size_t *length_read)
+{
+	block_dev_state_t *cur;
+	io_block_spec_t *buf;
+	io_block_ops_t *ops;
+	int lba;
+	size_t block_size, left;
+	size_t nbytes;  /* number of bytes read in one iteration */
+	size_t request; /* number of requested bytes in one iteration */
+	size_t count;   /* number of bytes already read */
+	/*
+	 * number of leading bytes from start of the block
+	 * to the first byte to be read
+	 */
+	size_t skip;
+
+	/*
+	 * number of trailing bytes between the last byte
+	 * to be read and the end of the block
+	 */
+	size_t padding;
+
+	assert(entity->info != (uintptr_t)NULL);
+	cur = (block_dev_state_t *)entity->info;
+	ops = &(cur->dev_spec->ops);
+	buf = &(cur->dev_spec->buffer);
+	block_size = cur->dev_spec->block_size;
+	assert((length <= cur->size) &&
+	       (length > 0U) &&
+	       (ops->read != 0));
+
+	/*
+	 * We don't know the number of bytes that we are going
+	 * to read in every iteration, because it will depend
+	 * on the low level driver.
+	 */
+	count = 0;
+	for (left = length; left > 0U; left -= nbytes) {
+		/*
+		 * We must only request operations aligned to the block
+		 * size. Therefore if file_pos is not block-aligned,
+		 * we have to request the operation to start at the
+		 * previous block boundary and skip the leading bytes. And
+		 * similarly, the number of bytes requested must be a
+		 * block size multiple
+		 */
+		skip = cur->file_pos & (block_size - 1U);
+
+		/*
+		 * Calculate the block number containing file_pos
+		 * - e.g. block 3.
+		 */
+		lba = (cur->file_pos + cur->base) / block_size;
+
+		if ((skip + left) > buf->length) {
+			/*
+			 * The underlying read buffer is too small to
+			 * read all the required data - limit to just
+			 * fill the buffer, and then read again.
+			 */
+			request = buf->length;
+		} else {
+			/*
+			 * The underlying read buffer is big enough to
+			 * read all the required data. Calculate the
+			 * number of bytes to read to align with the
+			 * block size.
+			 */
+			request = skip + left;
+			request = (request + (block_size - 1U)) &
+				~(block_size - 1U);
+		}
+		request = ops->read(lba, buf->offset, request);
+
+		if (request <= skip) {
+			/*
+			 * We couldn't read enough bytes to jump over
+			 * the skip bytes, so we should have to read
+			 * again the same block, thus generating
+			 * the same error.
+			 */
+			return -EIO;
+		}
+
+		/*
+		 * Need to remove skip and padding bytes,if any, from
+		 * the read data when copying to the user buffer.
+		 */
+		nbytes = request - skip;
+		padding = (nbytes > left) ? nbytes - left : 0U;
+		nbytes -= padding;
+
+		memcpy((void *)(buffer + count),
+		       (void *)(buf->offset + skip),
+		       nbytes);
+
+		cur->file_pos += nbytes;
+		count += nbytes;
+	}
+	assert(count == length);
+	*length_read = count;
+
+	return 0;
+}
+
+/*
+ * This function allows the caller to write any number of bytes
+ * from any position. It hides from the caller that the low level
+ * driver only can write aligned blocks of data.
+ * See comments for block_read for more details.
+ */
+static int block_write(io_entity_t *entity, const uintptr_t buffer,
+		       size_t length, size_t *length_written)
+{
+	block_dev_state_t *cur;
+	io_block_spec_t *buf;
+	io_block_ops_t *ops;
+	int lba;
+	size_t block_size, left;
+	size_t nbytes;  /* number of bytes read in one iteration */
+	size_t request; /* number of requested bytes in one iteration */
+	size_t count;   /* number of bytes already read */
+	/*
+	 * number of leading bytes from start of the block
+	 * to the first byte to be read
+	 */
+	size_t skip;
+
+	/*
+	 * number of trailing bytes between the last byte
+	 * to be read and the end of the block
+	 */
+	size_t padding;
+
+	assert(entity->info != (uintptr_t)NULL);
+	cur = (block_dev_state_t *)entity->info;
+	ops = &(cur->dev_spec->ops);
+	buf = &(cur->dev_spec->buffer);
+	block_size = cur->dev_spec->block_size;
+	assert((length <= cur->size) &&
+	       (length > 0U) &&
+	       (ops->read != 0) &&
+	       (ops->write != 0));
+
+	/*
+	 * We don't know the number of bytes that we are going
+	 * to write in every iteration, because it will depend
+	 * on the low level driver.
+	 */
+	count = 0;
+	for (left = length; left > 0U; left -= nbytes) {
+		/*
+		 * We must only request operations aligned to the block
+		 * size. Therefore if file_pos is not block-aligned,
+		 * we have to request the operation to start at the
+		 * previous block boundary and skip the leading bytes. And
+		 * similarly, the number of bytes requested must be a
+		 * block size multiple
+		 */
+		skip = cur->file_pos & (block_size - 1U);
+
+		/*
+		 * Calculate the block number containing file_pos
+		 * - e.g. block 3.
+		 */
+		lba = (cur->file_pos + cur->base) / block_size;
+
+		if ((skip + left) > buf->length) {
+			/*
+			 * The underlying read buffer is too small to
+			 * read all the required data - limit to just
+			 * fill the buffer, and then read again.
+			 */
+			request = buf->length;
+		} else {
+			/*
+			 * The underlying read buffer is big enough to
+			 * read all the required data. Calculate the
+			 * number of bytes to read to align with the
+			 * block size.
+			 */
+			request = skip + left;
+			request = (request + (block_size - 1U)) &
+				~(block_size - 1U);
+		}
+
+		/*
+		 * The number of bytes that we are going to write
+		 * from the user buffer will depend of the size
+		 * of the current request.
+		 */
+		nbytes = request - skip;
+		padding = (nbytes > left) ? nbytes - left : 0U;
+		nbytes -= padding;
+
+		/*
+		 * If we have skip or padding bytes then we have to preserve
+		 * some content and it means that we have to read before
+		 * writing
+		 */
+		if ((skip > 0U) || (padding > 0U)) {
+			request = ops->read(lba, buf->offset, request);
+			/*
+			 * The read may return size less than
+			 * requested. Round down to the nearest block
+			 * boundary
+			 */
+			request &= ~(block_size - 1U);
+			if (request <= skip) {
+				/*
+				 * We couldn't read enough bytes to jump over
+				 * the skip bytes, so we should have to read
+				 * again the same block, thus generating
+				 * the same error.
+				 */
+				return -EIO;
+			}
+			nbytes = request - skip;
+			padding = (nbytes > left) ? nbytes - left : 0U;
+			nbytes -= padding;
+		}
+
+		memcpy((void *)(buf->offset + skip),
+		       (void *)(buffer + count),
+		       nbytes);
+
+		request = ops->write(lba, buf->offset, request);
+		if (request <= skip)
+			return -EIO;
+
+		/*
+		 * And the previous write operation may modify the size
+		 * of the request, so again, we have to calculate the
+		 * number of bytes that we consumed from the user
+		 * buffer
+		 */
+		nbytes = request - skip;
+		padding = (nbytes > left) ? nbytes - left : 0U;
+		nbytes -= padding;
+
+		cur->file_pos += nbytes;
+		count += nbytes;
+	}
+	assert(count == length);
+	*length_written = count;
+
+	return 0;
+}
+
+static int block_close(io_entity_t *entity)
+{
+	entity->info = (uintptr_t)NULL;
+	return 0;
+}
+
+static int block_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info)
+{
+	block_dev_state_t *cur;
+	io_block_spec_t *buffer;
+	io_dev_info_t *info;
+	size_t block_size;
+	int result;
+
+	assert(dev_info != NULL);
+	result = allocate_dev_info(&info);
+	if (result != 0)
+		return -ENOENT;
+
+	cur = (block_dev_state_t *)info->info;
+	/* dev_spec is type of io_block_dev_spec_t. */
+	cur->dev_spec = (io_block_dev_spec_t *)dev_spec;
+	buffer = &(cur->dev_spec->buffer);
+	block_size = cur->dev_spec->block_size;
+	assert((block_size > 0U) &&
+	       (is_power_of_2(block_size) != 0U) &&
+	       ((buffer->offset % block_size) == 0U) &&
+	       ((buffer->length % block_size) == 0U));
+
+	*dev_info = info;	/* cast away const */
+	(void)block_size;
+	(void)buffer;
+	return 0;
+}
+
+static int block_dev_close(io_dev_info_t *dev_info)
+{
+	return free_dev_info(dev_info);
+}
+
+/* Exported functions */
+
+/* Register the Block driver with the IO abstraction */
+int register_io_dev_block(const io_dev_connector_t **dev_con)
+{
+	int result;
+
+	assert(dev_con != NULL);
+
+	/*
+	 * Since dev_info isn't really used in io_register_device, always
+	 * use the same device info at here instead.
+	 */
+	result = io_register_device(&dev_info_pool[0]);
+	if (result == 0)
+		*dev_con = &block_dev_connector;
+	return result;
+}
diff --git a/drivers/io/io_dummy.c b/drivers/io/io_dummy.c
new file mode 100644
index 0000000..4f0cda6
--- /dev/null
+++ b/drivers/io/io_dummy.c
@@ -0,0 +1,155 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_dummy.h>
+#include <drivers/io/io_storage.h>
+
+struct file_state {
+	int in_use;
+	size_t size;
+};
+
+static struct file_state current_file = {0};
+
+/* Identify the device type as dummy */
+static io_type_t device_type_dummy(void)
+{
+	return IO_TYPE_DUMMY;
+}
+
+/* Dummy device functions */
+static int dummy_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int dummy_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			     io_entity_t *entity);
+static int dummy_block_len(io_entity_t *entity, size_t *length);
+static int dummy_block_read(io_entity_t *entity, uintptr_t buffer,
+			     size_t length, size_t *length_read);
+static int dummy_block_close(io_entity_t *entity);
+static int dummy_dev_close(io_dev_info_t *dev_info);
+
+
+static const io_dev_connector_t dummy_dev_connector = {
+	.dev_open = dummy_dev_open
+};
+
+
+static const io_dev_funcs_t dummy_dev_funcs = {
+	.type = device_type_dummy,
+	.open = dummy_block_open,
+	.seek = NULL,
+	.size = dummy_block_len,
+	.read = dummy_block_read,
+	.write = NULL,
+	.close = dummy_block_close,
+	.dev_init = NULL,
+	.dev_close = dummy_dev_close,
+};
+
+
+static const io_dev_info_t dummy_dev_info = {
+	.funcs = &dummy_dev_funcs,
+	.info = (uintptr_t)NULL
+};
+
+
+/* Open a connection to the dummy device */
+static int dummy_dev_open(const uintptr_t dev_spec __attribute__((unused)),
+			   io_dev_info_t **dev_info)
+{
+	assert(dev_info != NULL);
+	*dev_info = (io_dev_info_t *)&dummy_dev_info;
+
+	return 0;
+}
+
+
+/* Close a connection to the dummy device */
+static int dummy_dev_close(io_dev_info_t *dev_info)
+{
+	return 0;
+}
+
+
+/* Open a file on the dummy device */
+static int dummy_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			     io_entity_t *entity)
+{
+	int result;
+	const io_block_spec_t *block_spec = (io_block_spec_t *)spec;
+
+	if (current_file.in_use == 0) {
+		assert(block_spec != NULL);
+		assert(entity != NULL);
+
+		current_file.in_use = 1;
+		current_file.size = block_spec->length;
+		entity->info = (uintptr_t)&current_file;
+		result = 0;
+	} else {
+		WARN("A Dummy device is already active. Close first.\n");
+		result = -ENOMEM;
+	}
+
+	return result;
+}
+
+
+/* Return the size of a file on the dummy device */
+static int dummy_block_len(io_entity_t *entity, size_t *length)
+{
+	assert(entity != NULL);
+	assert(length != NULL);
+
+	*length =  ((struct file_state *)entity->info)->size;
+
+	return 0;
+}
+
+
+/* Read data from a file on the dummy device */
+static int dummy_block_read(io_entity_t *entity, uintptr_t buffer,
+			     size_t length, size_t *length_read)
+{
+	assert(length_read != NULL);
+
+	*length_read = length;
+
+	return 0;
+}
+
+
+/* Close a file on the dummy device */
+static int dummy_block_close(io_entity_t *entity)
+{
+	assert(entity != NULL);
+
+	entity->info = 0;
+	current_file.in_use = 0;
+
+	return 0;
+}
+
+
+/* Exported functions */
+
+/* Register the dummy driver with the IO abstraction */
+int register_io_dev_dummy(const io_dev_connector_t **dev_con)
+{
+	int result;
+
+	assert(dev_con != NULL);
+
+	result = io_register_device(&dummy_dev_info);
+	if (result == 0)
+		*dev_con = &dummy_dev_connector;
+
+	return result;
+}
diff --git a/drivers/io/io_encrypted.c b/drivers/io/io_encrypted.c
new file mode 100644
index 0000000..744ca83
--- /dev/null
+++ b/drivers/io/io_encrypted.c
@@ -0,0 +1,244 @@
+/*
+ * Copyright (c) 2020, Linaro Limited. All rights reserved.
+ * Author: Sumit Garg <sumit.garg@linaro.org>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/auth/crypto_mod.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_encrypted.h>
+#include <drivers/io/io_storage.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+#include <tools_share/firmware_encrypted.h>
+#include <tools_share/uuid.h>
+
+static uintptr_t backend_dev_handle;
+static uintptr_t backend_dev_spec;
+static uintptr_t backend_handle;
+static uintptr_t backend_image_spec;
+
+static io_dev_info_t enc_dev_info;
+
+/* Encrypted firmware driver functions */
+static int enc_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int enc_file_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			  io_entity_t *entity);
+static int enc_file_len(io_entity_t *entity, size_t *length);
+static int enc_file_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+			  size_t *length_read);
+static int enc_file_close(io_entity_t *entity);
+static int enc_dev_init(io_dev_info_t *dev_info, const uintptr_t init_params);
+static int enc_dev_close(io_dev_info_t *dev_info);
+
+static inline int is_valid_header(struct fw_enc_hdr *header)
+{
+	if (header->magic == ENC_HEADER_MAGIC)
+		return 1;
+	else
+		return 0;
+}
+
+static io_type_t device_type_enc(void)
+{
+	return IO_TYPE_ENCRYPTED;
+}
+
+static const io_dev_connector_t enc_dev_connector = {
+	.dev_open = enc_dev_open
+};
+
+static const io_dev_funcs_t enc_dev_funcs = {
+	.type = device_type_enc,
+	.open = enc_file_open,
+	.seek = NULL,
+	.size = enc_file_len,
+	.read = enc_file_read,
+	.write = NULL,
+	.close = enc_file_close,
+	.dev_init = enc_dev_init,
+	.dev_close = enc_dev_close,
+};
+
+static int enc_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info)
+{
+	assert(dev_info != NULL);
+
+	enc_dev_info.funcs = &enc_dev_funcs;
+	*dev_info = &enc_dev_info;
+
+	return 0;
+}
+
+static int enc_dev_init(io_dev_info_t *dev_info, const uintptr_t init_params)
+{
+	int result;
+	unsigned int image_id = (unsigned int)init_params;
+
+	/* Obtain a reference to the image by querying the platform layer */
+	result = plat_get_image_source(image_id, &backend_dev_handle,
+				       &backend_dev_spec);
+	if (result != 0) {
+		WARN("Failed to obtain reference to image id=%u (%i)\n",
+			image_id, result);
+		return -ENOENT;
+	}
+
+	return result;
+}
+
+static int enc_dev_close(io_dev_info_t *dev_info)
+{
+	backend_dev_handle = (uintptr_t)NULL;
+	backend_dev_spec = (uintptr_t)NULL;
+
+	return 0;
+}
+
+static int enc_file_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			 io_entity_t *entity)
+{
+	int result;
+
+	assert(spec != 0);
+	assert(entity != NULL);
+
+	backend_image_spec = spec;
+
+	result = io_open(backend_dev_handle, backend_image_spec,
+			 &backend_handle);
+	if (result != 0) {
+		WARN("Failed to open backend device (%i)\n", result);
+		result = -ENOENT;
+	}
+
+	return result;
+}
+
+static int enc_file_len(io_entity_t *entity, size_t *length)
+{
+	int result;
+
+	assert(entity != NULL);
+	assert(length != NULL);
+
+	result = io_size(backend_handle, length);
+	if (result != 0) {
+		WARN("Failed to read blob length (%i)\n", result);
+		return -ENOENT;
+	}
+
+	/*
+	 * Encryption header is attached at the beginning of the encrypted file
+	 * and is not considered a part of the payload.
+	 */
+	if (*length < sizeof(struct fw_enc_hdr))
+		return -EIO;
+
+	*length -= sizeof(struct fw_enc_hdr);
+
+	return result;
+}
+
+static int enc_file_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+			 size_t *length_read)
+{
+	int result;
+	struct fw_enc_hdr header;
+	enum fw_enc_status_t fw_enc_status;
+	size_t bytes_read;
+	uint8_t key[ENC_MAX_KEY_SIZE];
+	size_t key_len = sizeof(key);
+	unsigned int key_flags = 0;
+	const io_uuid_spec_t *uuid_spec = (io_uuid_spec_t *)backend_image_spec;
+
+	assert(entity != NULL);
+	assert(length_read != NULL);
+
+	result = io_read(backend_handle, (uintptr_t)&header, sizeof(header),
+			 &bytes_read);
+	if (result != 0) {
+		WARN("Failed to read encryption header (%i)\n", result);
+		return -ENOENT;
+	}
+
+	if (!is_valid_header(&header)) {
+		WARN("Encryption header check failed.\n");
+		return -ENOENT;
+	}
+
+	VERBOSE("Encryption header looks OK.\n");
+	fw_enc_status = header.flags & FW_ENC_STATUS_FLAG_MASK;
+
+	if ((header.iv_len > ENC_MAX_IV_SIZE) ||
+	    (header.tag_len > ENC_MAX_TAG_SIZE)) {
+		WARN("Incorrect IV or tag length\n");
+		return -ENOENT;
+	}
+
+	result = io_read(backend_handle, buffer, length, &bytes_read);
+	if (result != 0) {
+		WARN("Failed to read encrypted payload (%i)\n", result);
+		return -ENOENT;
+	}
+
+	*length_read = bytes_read;
+
+	result = plat_get_enc_key_info(fw_enc_status, key, &key_len, &key_flags,
+				       (uint8_t *)&uuid_spec->uuid,
+				       sizeof(uuid_t));
+	if (result != 0) {
+		WARN("Failed to obtain encryption key (%i)\n", result);
+		return -ENOENT;
+	}
+
+	result = crypto_mod_auth_decrypt(header.dec_algo,
+					 (void *)buffer, *length_read, key,
+					 key_len, key_flags, header.iv,
+					 header.iv_len, header.tag,
+					 header.tag_len);
+	memset(key, 0, key_len);
+
+	if (result != 0) {
+		ERROR("File decryption failed (%i)\n", result);
+		return -ENOENT;
+	}
+
+	return result;
+}
+
+static int enc_file_close(io_entity_t *entity)
+{
+	io_close(backend_handle);
+
+	backend_image_spec = (uintptr_t)NULL;
+	entity->info = 0;
+
+	return 0;
+}
+
+/* Exported functions */
+
+/* Register the Encrypted Firmware driver with the IO abstraction */
+int register_io_dev_enc(const io_dev_connector_t **dev_con)
+{
+	int result;
+
+	assert(dev_con != NULL);
+
+	result = io_register_device(&enc_dev_info);
+	if (result == 0)
+		*dev_con = &enc_dev_connector;
+
+	return result;
+}
diff --git a/drivers/io/io_fip.c b/drivers/io/io_fip.c
new file mode 100644
index 0000000..6e15295
--- /dev/null
+++ b/drivers/io/io_fip.c
@@ -0,0 +1,481 @@
+/*
+ * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_fip.h>
+#include <drivers/io/io_storage.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+#include <tools_share/firmware_image_package.h>
+#include <tools_share/uuid.h>
+
+#ifndef MAX_FIP_DEVICES
+#define MAX_FIP_DEVICES		1
+#endif
+
+/* Useful for printing UUIDs when debugging.*/
+#define PRINT_UUID2(x)								\
+	"%08x-%04hx-%04hx-%02hhx%02hhx-%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx",	\
+		x.time_low, x.time_mid, x.time_hi_and_version,			\
+		x.clock_seq_hi_and_reserved, x.clock_seq_low,			\
+		x.node[0], x.node[1], x.node[2], x.node[3],			\
+		x.node[4], x.node[5]
+
+typedef struct {
+	unsigned int file_pos;
+	fip_toc_entry_t entry;
+} fip_file_state_t;
+
+/*
+ * Maintain dev_spec per FIP Device
+ * TODO - Add backend handles and file state
+ * per FIP device here once backends like io_memmap
+ * can support multiple open files
+ */
+typedef struct {
+	uintptr_t dev_spec;
+	uint16_t plat_toc_flag;
+} fip_dev_state_t;
+
+/*
+ * Only one file can be open across all FIP device
+ * as backends like io_memmap don't support
+ * multiple open files. The file state and
+ * backend handle should be maintained per FIP device
+ * if the same support is available in the backend
+ */
+static fip_file_state_t current_fip_file = {0};
+static uintptr_t backend_dev_handle;
+static uintptr_t backend_image_spec;
+
+static fip_dev_state_t state_pool[MAX_FIP_DEVICES];
+static io_dev_info_t dev_info_pool[MAX_FIP_DEVICES];
+
+/* Track number of allocated fip devices */
+static unsigned int fip_dev_count;
+
+/* Firmware Image Package driver functions */
+static int fip_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int fip_file_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			  io_entity_t *entity);
+static int fip_file_len(io_entity_t *entity, size_t *length);
+static int fip_file_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+			  size_t *length_read);
+static int fip_file_close(io_entity_t *entity);
+static int fip_dev_init(io_dev_info_t *dev_info, const uintptr_t init_params);
+static int fip_dev_close(io_dev_info_t *dev_info);
+
+
+/* Return 0 for equal uuids. */
+static inline int compare_uuids(const uuid_t *uuid1, const uuid_t *uuid2)
+{
+	return memcmp(uuid1, uuid2, sizeof(uuid_t));
+}
+
+
+static inline int is_valid_header(fip_toc_header_t *header)
+{
+	if ((header->name == TOC_HEADER_NAME) && (header->serial_number != 0)) {
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+
+/* Identify the device type as a virtual driver */
+static io_type_t device_type_fip(void)
+{
+	return IO_TYPE_FIRMWARE_IMAGE_PACKAGE;
+}
+
+
+static const io_dev_connector_t fip_dev_connector = {
+	.dev_open = fip_dev_open
+};
+
+
+static const io_dev_funcs_t fip_dev_funcs = {
+	.type = device_type_fip,
+	.open = fip_file_open,
+	.seek = NULL,
+	.size = fip_file_len,
+	.read = fip_file_read,
+	.write = NULL,
+	.close = fip_file_close,
+	.dev_init = fip_dev_init,
+	.dev_close = fip_dev_close,
+};
+
+/* Locate a file state in the pool, specified by address */
+static int find_first_fip_state(const uintptr_t dev_spec,
+				  unsigned int *index_out)
+{
+	int result = -ENOENT;
+	unsigned int index;
+
+	for (index = 0; index < (unsigned int)MAX_FIP_DEVICES; ++index) {
+		/* dev_spec is used as identifier since it's unique */
+		if (state_pool[index].dev_spec == dev_spec) {
+			result = 0;
+			*index_out = index;
+			break;
+		}
+	}
+	return result;
+}
+
+
+/* Allocate a device info from the pool and return a pointer to it */
+static int allocate_dev_info(io_dev_info_t **dev_info)
+{
+	int result = -ENOMEM;
+
+	assert(dev_info != NULL);
+
+	if (fip_dev_count < (unsigned int)MAX_FIP_DEVICES) {
+		unsigned int index = 0;
+
+		result = find_first_fip_state(0, &index);
+		assert(result == 0);
+		/* initialize dev_info */
+		dev_info_pool[index].funcs = &fip_dev_funcs;
+		dev_info_pool[index].info =
+				(uintptr_t)&state_pool[index];
+		*dev_info = &dev_info_pool[index];
+		++fip_dev_count;
+	}
+
+	return result;
+}
+
+/* Release a device info to the pool */
+static int free_dev_info(io_dev_info_t *dev_info)
+{
+	int result;
+	unsigned int index = 0;
+	fip_dev_state_t *state;
+
+	assert(dev_info != NULL);
+
+	state = (fip_dev_state_t *)dev_info->info;
+	result = find_first_fip_state(state->dev_spec, &index);
+	if (result ==  0) {
+		/* free if device info is valid */
+		zeromem(state, sizeof(fip_dev_state_t));
+		--fip_dev_count;
+	}
+
+	return result;
+}
+
+/*
+ * Multiple FIP devices can be opened depending on the value of
+ * MAX_FIP_DEVICES. Given that there is only one backend, only a
+ * single file can be open at a time by any FIP device.
+ */
+static int fip_dev_open(const uintptr_t dev_spec,
+			 io_dev_info_t **dev_info)
+{
+	int result;
+	io_dev_info_t *info;
+	fip_dev_state_t *state;
+
+	assert(dev_info != NULL);
+#if MAX_FIP_DEVICES > 1
+	assert(dev_spec != (uintptr_t)NULL);
+#endif
+
+	result = allocate_dev_info(&info);
+	if (result != 0)
+		return -ENOMEM;
+
+	state = (fip_dev_state_t *)info->info;
+
+	state->dev_spec = dev_spec;
+
+	*dev_info = info;
+
+	return 0;
+}
+
+
+/* Do some basic package checks. */
+static int fip_dev_init(io_dev_info_t *dev_info, const uintptr_t init_params)
+{
+	int result;
+	unsigned int image_id = (unsigned int)init_params;
+	uintptr_t backend_handle;
+	fip_toc_header_t header;
+	size_t bytes_read;
+	fip_dev_state_t *state;
+
+	assert(dev_info != NULL);
+
+	state = (fip_dev_state_t *)dev_info->info;
+
+	/* Obtain a reference to the image by querying the platform layer */
+	result = plat_get_image_source(image_id, &backend_dev_handle,
+				       &backend_image_spec);
+	if (result != 0) {
+		WARN("Failed to obtain reference to image id=%u (%i)\n",
+			image_id, result);
+		result = -ENOENT;
+		goto fip_dev_init_exit;
+	}
+
+	/* Attempt to access the FIP image */
+	result = io_open(backend_dev_handle, backend_image_spec,
+			 &backend_handle);
+	if (result != 0) {
+		WARN("Failed to access image id=%u (%i)\n", image_id, result);
+		result = -ENOENT;
+		goto fip_dev_init_exit;
+	}
+
+	result = io_read(backend_handle, (uintptr_t)&header, sizeof(header),
+			&bytes_read);
+	if (result == 0) {
+		if (!is_valid_header(&header)) {
+			WARN("Firmware Image Package header check failed.\n");
+			result = -ENOENT;
+		} else {
+			VERBOSE("FIP header looks OK.\n");
+			/*
+			 * Store 16-bit Platform ToC flags field which occupies
+			 * bits [32-47] in fip header.
+			 */
+			state->plat_toc_flag = (header.flags >> 32) & 0xffff;
+		}
+	}
+
+	io_close(backend_handle);
+
+ fip_dev_init_exit:
+	return result;
+}
+
+/* Close a connection to the FIP device */
+static int fip_dev_close(io_dev_info_t *dev_info)
+{
+	/* TODO: Consider tracking open files and cleaning them up here */
+
+	/* Clear the backend. */
+	backend_dev_handle = (uintptr_t)NULL;
+	backend_image_spec = (uintptr_t)NULL;
+
+	return free_dev_info(dev_info);
+}
+
+
+/* Open a file for access from package. */
+static int fip_file_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			 io_entity_t *entity)
+{
+	int result;
+	uintptr_t backend_handle;
+	const io_uuid_spec_t *uuid_spec = (io_uuid_spec_t *)spec;
+	static const uuid_t uuid_null = { {0} }; /* Double braces for clang */
+	size_t bytes_read;
+	int found_file = 0;
+
+	assert(uuid_spec != NULL);
+	assert(entity != NULL);
+
+	/* Can only have one file open at a time for the moment. We need to
+	 * track state like file cursor position. We know the header lives at
+	 * offset zero, so this entry should never be zero for an active file.
+	 * When the system supports dynamic memory allocation we can allow more
+	 * than one open file at a time if needed.
+	 */
+	if (current_fip_file.entry.offset_address != 0U) {
+		WARN("fip_file_open : Only one open file at a time.\n");
+		return -ENFILE;
+	}
+
+	/* Attempt to access the FIP image */
+	result = io_open(backend_dev_handle, backend_image_spec,
+			 &backend_handle);
+	if (result != 0) {
+		WARN("Failed to open Firmware Image Package (%i)\n", result);
+		result = -ENOENT;
+		goto fip_file_open_exit;
+	}
+
+	/* Seek past the FIP header into the Table of Contents */
+	result = io_seek(backend_handle, IO_SEEK_SET,
+			 (signed long long)sizeof(fip_toc_header_t));
+	if (result != 0) {
+		WARN("fip_file_open: failed to seek\n");
+		result = -ENOENT;
+		goto fip_file_open_close;
+	}
+
+	found_file = 0;
+	do {
+		result = io_read(backend_handle,
+				 (uintptr_t)&current_fip_file.entry,
+				 sizeof(current_fip_file.entry),
+				 &bytes_read);
+		if (result == 0) {
+			if (compare_uuids(&current_fip_file.entry.uuid,
+					  &uuid_spec->uuid) == 0) {
+				found_file = 1;
+			}
+		} else {
+			WARN("Failed to read FIP (%i)\n", result);
+			goto fip_file_open_close;
+		}
+	} while ((found_file == 0) &&
+			(compare_uuids(&current_fip_file.entry.uuid,
+				&uuid_null) != 0));
+
+	if (found_file == 1) {
+		/* All fine. Update entity info with file state and return. Set
+		 * the file position to 0. The 'current_fip_file.entry' holds
+		 * the base and size of the file.
+		 */
+		current_fip_file.file_pos = 0;
+		entity->info = (uintptr_t)&current_fip_file;
+	} else {
+		/* Did not find the file in the FIP. */
+		current_fip_file.entry.offset_address = 0;
+		result = -ENOENT;
+	}
+
+ fip_file_open_close:
+	io_close(backend_handle);
+
+ fip_file_open_exit:
+	return result;
+}
+
+
+/* Return the size of a file in package */
+static int fip_file_len(io_entity_t *entity, size_t *length)
+{
+	assert(entity != NULL);
+	assert(length != NULL);
+
+	*length =  ((fip_file_state_t *)entity->info)->entry.size;
+
+	return 0;
+}
+
+
+/* Read data from a file in package */
+static int fip_file_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+			  size_t *length_read)
+{
+	int result;
+	fip_file_state_t *fp;
+	size_t file_offset;
+	size_t bytes_read;
+	uintptr_t backend_handle;
+
+	assert(entity != NULL);
+	assert(length_read != NULL);
+	assert(entity->info != (uintptr_t)NULL);
+
+	/* Open the backend, attempt to access the blob image */
+	result = io_open(backend_dev_handle, backend_image_spec,
+			 &backend_handle);
+	if (result != 0) {
+		WARN("Failed to open FIP (%i)\n", result);
+		result = -ENOENT;
+		goto fip_file_read_exit;
+	}
+
+	fp = (fip_file_state_t *)entity->info;
+
+	/* Seek to the position in the FIP where the payload lives */
+	file_offset = fp->entry.offset_address + fp->file_pos;
+	result = io_seek(backend_handle, IO_SEEK_SET,
+			 (signed long long)file_offset);
+	if (result != 0) {
+		WARN("fip_file_read: failed to seek\n");
+		result = -ENOENT;
+		goto fip_file_read_close;
+	}
+
+	result = io_read(backend_handle, buffer, length, &bytes_read);
+	if (result != 0) {
+		/* We cannot read our data. Fail. */
+		WARN("Failed to read payload (%i)\n", result);
+		result = -ENOENT;
+		goto fip_file_read_close;
+	} else {
+		/* Set caller length and new file position. */
+		*length_read = bytes_read;
+		fp->file_pos += bytes_read;
+	}
+
+/* Close the backend. */
+ fip_file_read_close:
+	io_close(backend_handle);
+
+ fip_file_read_exit:
+	return result;
+}
+
+
+/* Close a file in package */
+static int fip_file_close(io_entity_t *entity)
+{
+	/* Clear our current file pointer.
+	 * If we had malloc() we would free() here.
+	 */
+	if (current_fip_file.entry.offset_address != 0U) {
+		zeromem(&current_fip_file, sizeof(current_fip_file));
+	}
+
+	/* Clear the Entity info. */
+	entity->info = 0;
+
+	return 0;
+}
+
+/* Exported functions */
+
+/* Register the Firmware Image Package driver with the IO abstraction */
+int register_io_dev_fip(const io_dev_connector_t **dev_con)
+{
+	int result;
+	assert(dev_con != NULL);
+
+	/*
+	 * Since dev_info isn't really used in io_register_device, always
+	 * use the same device info at here instead.
+	 */
+	result = io_register_device(&dev_info_pool[0]);
+	if (result == 0)
+		*dev_con = &fip_dev_connector;
+
+	return result;
+}
+
+/* Function to retrieve plat_toc_flags, previously saved in FIP dev */
+int fip_dev_get_plat_toc_flag(io_dev_info_t *dev_info, uint16_t *plat_toc_flag)
+{
+	fip_dev_state_t *state;
+
+	assert(dev_info != NULL);
+
+	state = (fip_dev_state_t *)dev_info->info;
+
+	*plat_toc_flag =  state->plat_toc_flag;
+
+	return 0;
+}
diff --git a/drivers/io/io_memmap.c b/drivers/io/io_memmap.c
new file mode 100644
index 0000000..eb69163
--- /dev/null
+++ b/drivers/io/io_memmap.c
@@ -0,0 +1,251 @@
+/*
+ * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_memmap.h>
+#include <drivers/io/io_storage.h>
+#include <lib/utils.h>
+
+/* As we need to be able to keep state for seek, only one file can be open
+ * at a time. Make this a structure and point to the entity->info. When we
+ * can malloc memory we can change this to support more open files.
+ */
+typedef struct {
+	/* Use the 'in_use' flag as any value for base and file_pos could be
+	 * valid.
+	 */
+	int			in_use;
+	uintptr_t		base;
+	unsigned long long	file_pos;
+	unsigned long long	size;
+} memmap_file_state_t;
+
+static memmap_file_state_t current_memmap_file = {0};
+
+/* Identify the device type as memmap */
+static io_type_t device_type_memmap(void)
+{
+	return IO_TYPE_MEMMAP;
+}
+
+/* Memmap device functions */
+static int memmap_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int memmap_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			     io_entity_t *entity);
+static int memmap_block_seek(io_entity_t *entity, int mode,
+			     signed long long offset);
+static int memmap_block_len(io_entity_t *entity, size_t *length);
+static int memmap_block_read(io_entity_t *entity, uintptr_t buffer,
+			     size_t length, size_t *length_read);
+static int memmap_block_write(io_entity_t *entity, const uintptr_t buffer,
+			      size_t length, size_t *length_written);
+static int memmap_block_close(io_entity_t *entity);
+static int memmap_dev_close(io_dev_info_t *dev_info);
+
+
+static const io_dev_connector_t memmap_dev_connector = {
+	.dev_open = memmap_dev_open
+};
+
+
+static const io_dev_funcs_t memmap_dev_funcs = {
+	.type = device_type_memmap,
+	.open = memmap_block_open,
+	.seek = memmap_block_seek,
+	.size = memmap_block_len,
+	.read = memmap_block_read,
+	.write = memmap_block_write,
+	.close = memmap_block_close,
+	.dev_init = NULL,
+	.dev_close = memmap_dev_close,
+};
+
+
+/* No state associated with this device so structure can be const */
+static io_dev_info_t memmap_dev_info = {
+	.funcs = &memmap_dev_funcs,
+	.info = (uintptr_t)NULL
+};
+
+
+/* Open a connection to the memmap device */
+static int memmap_dev_open(const uintptr_t dev_spec __unused,
+			   io_dev_info_t **dev_info)
+{
+	assert(dev_info != NULL);
+	*dev_info = &memmap_dev_info;
+	return 0;
+}
+
+
+
+/* Close a connection to the memmap device */
+static int memmap_dev_close(io_dev_info_t *dev_info)
+{
+	/* NOP */
+	/* TODO: Consider tracking open files and cleaning them up here */
+	return 0;
+}
+
+
+/* Open a file on the memmap device */
+static int memmap_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			     io_entity_t *entity)
+{
+	int result = -ENOMEM;
+	const io_block_spec_t *block_spec = (io_block_spec_t *)spec;
+
+	/* Since we need to track open state for seek() we only allow one open
+	 * spec at a time. When we have dynamic memory we can malloc and set
+	 * entity->info.
+	 */
+	if (current_memmap_file.in_use == 0) {
+		assert(block_spec != NULL);
+		assert(entity != NULL);
+
+		current_memmap_file.in_use = 1;
+		current_memmap_file.base = block_spec->offset;
+		/* File cursor offset for seek and incremental reads etc. */
+		current_memmap_file.file_pos = 0;
+		current_memmap_file.size = block_spec->length;
+		entity->info = (uintptr_t)&current_memmap_file;
+		result = 0;
+	} else {
+		WARN("A Memmap device is already active. Close first.\n");
+	}
+
+	return result;
+}
+
+
+/* Seek to a particular file offset on the memmap device */
+static int memmap_block_seek(io_entity_t *entity, int mode,
+			     signed long long offset)
+{
+	int result = -ENOENT;
+	memmap_file_state_t *fp;
+
+	/* We only support IO_SEEK_SET for the moment. */
+	if (mode == IO_SEEK_SET) {
+		assert(entity != NULL);
+
+		fp = (memmap_file_state_t *) entity->info;
+
+		/* Assert that new file position is valid */
+		assert((offset >= 0) &&
+		       ((unsigned long long)offset < fp->size));
+
+		/* Reset file position */
+		fp->file_pos = (unsigned long long)offset;
+		result = 0;
+	}
+
+	return result;
+}
+
+
+/* Return the size of a file on the memmap device */
+static int memmap_block_len(io_entity_t *entity, size_t *length)
+{
+	assert(entity != NULL);
+	assert(length != NULL);
+
+	*length = (size_t)((memmap_file_state_t *)entity->info)->size;
+
+	return 0;
+}
+
+
+/* Read data from a file on the memmap device */
+static int memmap_block_read(io_entity_t *entity, uintptr_t buffer,
+			     size_t length, size_t *length_read)
+{
+	memmap_file_state_t *fp;
+	unsigned long long pos_after;
+
+	assert(entity != NULL);
+	assert(length_read != NULL);
+
+	fp = (memmap_file_state_t *) entity->info;
+
+	/* Assert that file position is valid for this read operation */
+	pos_after = fp->file_pos + length;
+	assert((pos_after >= fp->file_pos) && (pos_after <= fp->size));
+
+	memcpy((void *)buffer,
+	       (void *)((uintptr_t)(fp->base + fp->file_pos)), length);
+
+	*length_read = length;
+
+	/* Set file position after read */
+	fp->file_pos = pos_after;
+
+	return 0;
+}
+
+
+/* Write data to a file on the memmap device */
+static int memmap_block_write(io_entity_t *entity, const uintptr_t buffer,
+			      size_t length, size_t *length_written)
+{
+	memmap_file_state_t *fp;
+	unsigned long long pos_after;
+
+	assert(entity != NULL);
+	assert(length_written != NULL);
+
+	fp = (memmap_file_state_t *) entity->info;
+
+	/* Assert that file position is valid for this write operation */
+	pos_after = fp->file_pos + length;
+	assert((pos_after >= fp->file_pos) && (pos_after <= fp->size));
+
+	memcpy((void *)((uintptr_t)(fp->base + fp->file_pos)),
+	       (void *)buffer, length);
+
+	*length_written = length;
+
+	/* Set file position after write */
+	fp->file_pos = pos_after;
+
+	return 0;
+}
+
+
+/* Close a file on the memmap device */
+static int memmap_block_close(io_entity_t *entity)
+{
+	assert(entity != NULL);
+
+	entity->info = 0;
+
+	/* This would be a mem free() if we had malloc.*/
+	zeromem((void *)&current_memmap_file, sizeof(current_memmap_file));
+
+	return 0;
+}
+
+
+/* Exported functions */
+
+/* Register the memmap driver with the IO abstraction */
+int register_io_dev_memmap(const io_dev_connector_t **dev_con)
+{
+	int result;
+	assert(dev_con != NULL);
+
+	result = io_register_device(&memmap_dev_info);
+	if (result == 0)
+		*dev_con = &memmap_dev_connector;
+
+	return result;
+}
diff --git a/drivers/io/io_mtd.c b/drivers/io/io_mtd.c
new file mode 100644
index 0000000..5d86592
--- /dev/null
+++ b/drivers/io/io_mtd.c
@@ -0,0 +1,290 @@
+/*
+ * Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_mtd.h>
+#include <lib/utils.h>
+
+#include <platform_def.h>
+
+typedef struct {
+	io_mtd_dev_spec_t	*dev_spec;
+	uintptr_t		base;
+	unsigned long long	pos;		/* Offset in bytes */
+	unsigned long long	size;		/* Size of device in bytes */
+	unsigned long long	extra_offset;	/* Extra offset in bytes */
+} mtd_dev_state_t;
+
+io_type_t device_type_mtd(void);
+
+static int mtd_open(io_dev_info_t *dev_info, const uintptr_t spec,
+		    io_entity_t *entity);
+static int mtd_seek(io_entity_t *entity, int mode, signed long long offset);
+static int mtd_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+		    size_t *length_read);
+static int mtd_close(io_entity_t *entity);
+static int mtd_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int mtd_dev_close(io_dev_info_t *dev_info);
+
+static const io_dev_connector_t mtd_dev_connector = {
+	.dev_open	= mtd_dev_open
+};
+
+static const io_dev_funcs_t mtd_dev_funcs = {
+	.type		= device_type_mtd,
+	.open		= mtd_open,
+	.seek		= mtd_seek,
+	.read		= mtd_read,
+	.close		= mtd_close,
+	.dev_close	= mtd_dev_close,
+};
+
+static mtd_dev_state_t state_pool[MAX_IO_MTD_DEVICES];
+static io_dev_info_t dev_info_pool[MAX_IO_MTD_DEVICES];
+
+io_type_t device_type_mtd(void)
+{
+	return IO_TYPE_MTD;
+}
+
+/* Locate a MTD state in the pool, specified by address */
+static int find_first_mtd_state(const io_mtd_dev_spec_t *dev_spec,
+				unsigned int *index_out)
+{
+	unsigned int index;
+	int result = -ENOENT;
+
+	for (index = 0U; index < MAX_IO_MTD_DEVICES; index++) {
+		/* dev_spec is used as identifier since it's unique */
+		if (state_pool[index].dev_spec == dev_spec) {
+			result = 0;
+			*index_out = index;
+			break;
+		}
+	}
+
+	return result;
+}
+
+/* Allocate a device info from the pool */
+static int allocate_dev_info(io_dev_info_t **dev_info)
+{
+	unsigned int index = 0U;
+	int result;
+
+	result = find_first_mtd_state(NULL, &index);
+	if (result != 0) {
+		return -ENOMEM;
+	}
+
+	dev_info_pool[index].funcs = &mtd_dev_funcs;
+	dev_info_pool[index].info = (uintptr_t)&state_pool[index];
+	*dev_info = &dev_info_pool[index];
+
+	return 0;
+}
+
+/* Release a device info from the pool */
+static int free_dev_info(io_dev_info_t *dev_info)
+{
+	int result;
+	unsigned int index = 0U;
+	mtd_dev_state_t *state;
+
+	state = (mtd_dev_state_t *)dev_info->info;
+	result = find_first_mtd_state(state->dev_spec, &index);
+	if (result != 0) {
+		return result;
+	}
+
+	zeromem(state, sizeof(mtd_dev_state_t));
+	zeromem(dev_info, sizeof(io_dev_info_t));
+
+	return 0;
+}
+
+static int mtd_add_extra_offset(mtd_dev_state_t *cur, size_t *extra_offset)
+{
+	io_mtd_ops_t *ops = &cur->dev_spec->ops;
+	int ret;
+
+	if (ops->seek == NULL) {
+		return 0;
+	}
+
+	ret = ops->seek(cur->base, cur->pos, extra_offset);
+	if (ret != 0) {
+		ERROR("%s: Seek error %d\n", __func__, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int mtd_open(io_dev_info_t *dev_info, const uintptr_t spec,
+		    io_entity_t *entity)
+{
+	mtd_dev_state_t *cur;
+	io_block_spec_t *region;
+	size_t extra_offset = 0U;
+	int ret;
+
+	assert((dev_info->info != 0UL) && (entity->info == 0UL));
+
+	region = (io_block_spec_t *)spec;
+	cur = (mtd_dev_state_t *)dev_info->info;
+	entity->info = (uintptr_t)cur;
+	cur->base = region->offset;
+	cur->pos = 0U;
+	cur->extra_offset = 0U;
+
+	ret = mtd_add_extra_offset(cur, &extra_offset);
+	if (ret != 0) {
+		return ret;
+	}
+
+	cur->base += extra_offset;
+
+	return 0;
+}
+
+/* Seek to a specific position using offset */
+static int mtd_seek(io_entity_t *entity, int mode, signed long long offset)
+{
+	mtd_dev_state_t *cur;
+	size_t extra_offset = 0U;
+	int ret;
+
+	assert((entity->info != (uintptr_t)NULL) && (offset >= 0));
+
+	cur = (mtd_dev_state_t *)entity->info;
+
+	switch (mode) {
+	case IO_SEEK_SET:
+		if ((offset >= 0) &&
+		    ((unsigned long long)offset >= cur->size)) {
+			return -EINVAL;
+		}
+
+		cur->pos = offset;
+		break;
+	case IO_SEEK_CUR:
+		if (((cur->base + cur->pos + (unsigned long long)offset) >=
+		     cur->size) ||
+		    ((cur->base + cur->pos + (unsigned long long)offset) <
+		     cur->base + cur->pos)) {
+			return -EINVAL;
+		}
+
+		cur->pos += (unsigned long long)offset;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = mtd_add_extra_offset(cur, &extra_offset);
+	if (ret != 0) {
+		return ret;
+	}
+
+	cur->extra_offset = extra_offset;
+
+	return 0;
+}
+
+static int mtd_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+		    size_t *out_length)
+{
+	mtd_dev_state_t *cur;
+	io_mtd_ops_t *ops;
+	int ret;
+
+	assert(entity->info != (uintptr_t)NULL);
+	assert((length > 0U) && (buffer != (uintptr_t)NULL));
+
+	cur = (mtd_dev_state_t *)entity->info;
+	ops = &cur->dev_spec->ops;
+	assert(ops->read != NULL);
+
+	VERBOSE("Read at %llx into %lx, length %zu\n",
+		cur->base + cur->pos, buffer, length);
+	if ((cur->base + cur->pos + length) > cur->dev_spec->device_size) {
+		return -EINVAL;
+	}
+
+	ret = ops->read(cur->base + cur->pos + cur->extra_offset, buffer,
+			length, out_length);
+	if (ret < 0) {
+		return ret;
+	}
+
+	assert(*out_length == length);
+	cur->pos += *out_length;
+
+	return 0;
+}
+
+static int mtd_close(io_entity_t *entity)
+{
+	entity->info = (uintptr_t)NULL;
+
+	return 0;
+}
+
+static int mtd_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info)
+{
+	mtd_dev_state_t *cur;
+	io_dev_info_t *info;
+	io_mtd_ops_t *ops;
+	int result;
+
+	result = allocate_dev_info(&info);
+	if (result != 0) {
+		return -ENOENT;
+	}
+
+	cur = (mtd_dev_state_t *)info->info;
+	cur->dev_spec = (io_mtd_dev_spec_t *)dev_spec;
+	*dev_info = info;
+	ops = &(cur->dev_spec->ops);
+	if (ops->init != NULL) {
+		result = ops->init(&cur->dev_spec->device_size,
+				   &cur->dev_spec->erase_size);
+	}
+
+	if (result == 0) {
+		cur->size = cur->dev_spec->device_size;
+	} else {
+		cur->size = 0ULL;
+	}
+
+	return result;
+}
+
+static int mtd_dev_close(io_dev_info_t *dev_info)
+{
+	return free_dev_info(dev_info);
+}
+
+/* Exported functions */
+
+/* Register the MTD driver in the IO abstraction */
+int register_io_dev_mtd(const io_dev_connector_t **dev_con)
+{
+	int result;
+
+	result = io_register_device(&dev_info_pool[0]);
+	if (result == 0) {
+		*dev_con = &mtd_dev_connector;
+	}
+
+	return result;
+}
diff --git a/drivers/io/io_semihosting.c b/drivers/io/io_semihosting.c
new file mode 100644
index 0000000..1c2f84d
--- /dev/null
+++ b/drivers/io/io_semihosting.c
@@ -0,0 +1,201 @@
+/*
+ * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <platform_def.h>
+
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_semihosting.h>
+#include <drivers/io/io_storage.h>
+#include <lib/semihosting.h>
+
+/* Identify the device type as semihosting */
+static io_type_t device_type_sh(void)
+{
+	return IO_TYPE_SEMIHOSTING;
+}
+
+
+/* Semi-hosting functions, device info and handle */
+
+static int sh_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int sh_file_open(io_dev_info_t *dev_info, const uintptr_t spec,
+		io_entity_t *entity);
+static int sh_file_seek(io_entity_t *entity, int mode, signed long long offset);
+static int sh_file_len(io_entity_t *entity, size_t *length);
+static int sh_file_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+		size_t *length_read);
+static int sh_file_write(io_entity_t *entity, const uintptr_t buffer,
+		size_t length, size_t *length_written);
+static int sh_file_close(io_entity_t *entity);
+
+static const io_dev_connector_t sh_dev_connector = {
+	.dev_open = sh_dev_open
+};
+
+
+static const io_dev_funcs_t sh_dev_funcs = {
+	.type = device_type_sh,
+	.open = sh_file_open,
+	.seek = sh_file_seek,
+	.size = sh_file_len,
+	.read = sh_file_read,
+	.write = sh_file_write,
+	.close = sh_file_close,
+	.dev_init = NULL,	/* NOP */
+	.dev_close = NULL,	/* NOP */
+};
+
+
+static io_dev_info_t sh_dev_info = {
+	.funcs = &sh_dev_funcs,
+	.info = (uintptr_t)NULL
+};
+
+
+/* Open a connection to the semi-hosting device */
+static int sh_dev_open(const uintptr_t dev_spec __unused,
+		io_dev_info_t **dev_info)
+{
+	assert(dev_info != NULL);
+	*dev_info = &sh_dev_info;
+	return 0;
+}
+
+
+/* Open a file on the semi-hosting device */
+static int sh_file_open(io_dev_info_t *dev_info __unused,
+		const uintptr_t spec, io_entity_t *entity)
+{
+	int result = -ENOENT;
+	long sh_result;
+	const io_file_spec_t *file_spec = (const io_file_spec_t *)spec;
+
+	assert(file_spec != NULL);
+	assert(entity != NULL);
+
+	sh_result = semihosting_file_open(file_spec->path, file_spec->mode);
+
+	if (sh_result > 0) {
+		entity->info = (uintptr_t)sh_result;
+		result = 0;
+	}
+	return result;
+}
+
+
+/* Seek to a particular file offset on the semi-hosting device */
+static int sh_file_seek(io_entity_t *entity, int mode, signed long long offset)
+{
+	long file_handle, sh_result;
+
+	assert(entity != NULL);
+
+	file_handle = (long)entity->info;
+
+	sh_result = semihosting_file_seek(file_handle, (ssize_t)offset);
+
+	return (sh_result == 0) ? 0 : -ENOENT;
+}
+
+
+/* Return the size of a file on the semi-hosting device */
+static int sh_file_len(io_entity_t *entity, size_t *length)
+{
+	int result = -ENOENT;
+
+	assert(entity != NULL);
+	assert(length != NULL);
+
+	long sh_handle = (long)entity->info;
+	long sh_result = semihosting_file_length(sh_handle);
+
+	if (sh_result >= 0) {
+		result = 0;
+		*length = (size_t)sh_result;
+	}
+
+	return result;
+}
+
+
+/* Read data from a file on the semi-hosting device */
+static int sh_file_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+		size_t *length_read)
+{
+	int result = -ENOENT;
+	long sh_result;
+	size_t bytes = length;
+	long file_handle;
+
+	assert(entity != NULL);
+	assert(length_read != NULL);
+
+	file_handle = (long)entity->info;
+
+	sh_result = semihosting_file_read(file_handle, &bytes, buffer);
+
+	if (sh_result >= 0) {
+		*length_read = (bytes != length) ? bytes : length;
+		result = 0;
+	}
+
+	return result;
+}
+
+
+/* Write data to a file on the semi-hosting device */
+static int sh_file_write(io_entity_t *entity, const uintptr_t buffer,
+		size_t length, size_t *length_written)
+{
+	long sh_result;
+	long file_handle;
+	size_t bytes = length;
+
+	assert(entity != NULL);
+	assert(length_written != NULL);
+
+	file_handle = (long)entity->info;
+
+	sh_result = semihosting_file_write(file_handle, &bytes, buffer);
+
+	*length_written = length - bytes;
+
+	return (sh_result == 0) ? 0 : -ENOENT;
+}
+
+
+/* Close a file on the semi-hosting device */
+static int sh_file_close(io_entity_t *entity)
+{
+	long sh_result;
+	long file_handle;
+
+	assert(entity != NULL);
+
+	file_handle = (long)entity->info;
+
+	sh_result = semihosting_file_close(file_handle);
+
+	return (sh_result >= 0) ? 0 : -ENOENT;
+}
+
+
+/* Exported functions */
+
+/* Register the semi-hosting driver with the IO abstraction */
+int register_io_dev_sh(const io_dev_connector_t **dev_con)
+{
+	int result;
+	assert(dev_con != NULL);
+
+	result = io_register_device(&sh_dev_info);
+	if (result == 0)
+		*dev_con = &sh_dev_connector;
+
+	return result;
+}
diff --git a/drivers/io/io_storage.c b/drivers/io/io_storage.c
new file mode 100644
index 0000000..0534268
--- /dev/null
+++ b/drivers/io/io_storage.c
@@ -0,0 +1,328 @@
+/*
+ * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+
+#include <platform_def.h>
+
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_storage.h>
+
+/* Storage for a fixed maximum number of IO entities, definable by platform */
+static io_entity_t entity_pool[MAX_IO_HANDLES];
+
+/* Simple way of tracking used storage - each entry is NULL or a pointer to an
+ * entity */
+static io_entity_t *entity_map[MAX_IO_HANDLES];
+
+/* Track number of allocated entities */
+static unsigned int entity_count;
+
+/* Array of fixed maximum of registered devices, definable by platform */
+static const io_dev_info_t *devices[MAX_IO_DEVICES];
+
+/* Number of currently registered devices */
+static unsigned int dev_count;
+
+/* Extra validation functions only used when asserts are enabled */
+#if ENABLE_ASSERTIONS
+
+/* Return a boolean value indicating whether a device connector is valid */
+static bool is_valid_dev_connector(const io_dev_connector_t *dev_con)
+{
+	return (dev_con != NULL) && (dev_con->dev_open != NULL);
+}
+
+/* Return a boolean value indicating whether a device handle is valid */
+static bool is_valid_dev(const uintptr_t dev_handle)
+{
+	const io_dev_info_t *dev = (io_dev_info_t *)dev_handle;
+
+	return (dev != NULL) && (dev->funcs != NULL) &&
+			(dev->funcs->type != NULL) &&
+			(dev->funcs->type() < IO_TYPE_MAX);
+}
+
+
+/* Return a boolean value indicating whether an IO entity is valid */
+static bool is_valid_entity(const uintptr_t handle)
+{
+	const io_entity_t *entity = (io_entity_t *)handle;
+
+	return (entity != NULL) &&
+			(is_valid_dev((uintptr_t)entity->dev_handle));
+}
+
+
+/* Return a boolean value indicating whether a seek mode is valid */
+static bool is_valid_seek_mode(io_seek_mode_t mode)
+{
+	return ((mode != IO_SEEK_INVALID) && (mode < IO_SEEK_MAX));
+}
+
+#endif /* ENABLE_ASSERTIONS */
+/* End of extra validation functions only used when asserts are enabled */
+
+
+/* Open a connection to a specific device */
+static int io_storage_dev_open(const io_dev_connector_t *dev_con,
+		const uintptr_t dev_spec,
+		io_dev_info_t **dev_info)
+{
+	assert(dev_info != NULL);
+	assert(is_valid_dev_connector(dev_con));
+
+	return dev_con->dev_open(dev_spec, dev_info);
+}
+
+
+/* Set a handle to track an entity */
+static void set_handle(uintptr_t *handle, io_entity_t *entity)
+{
+	assert(handle != NULL);
+	*handle = (uintptr_t)entity;
+}
+
+
+/* Locate an entity in the pool, specified by address */
+static int find_first_entity(const io_entity_t *entity, unsigned int *index_out)
+{
+	int result = -ENOENT;
+	for (unsigned int index = 0; index < MAX_IO_HANDLES; ++index) {
+		if (entity_map[index] == entity) {
+			result = 0;
+			*index_out = index;
+			break;
+		}
+	}
+	return result;
+}
+
+
+/* Allocate an entity from the pool and return a pointer to it */
+static int allocate_entity(io_entity_t **entity)
+{
+	int result = -ENOMEM;
+	assert(entity != NULL);
+
+	if (entity_count < MAX_IO_HANDLES) {
+		unsigned int index = 0;
+		result = find_first_entity(NULL, &index);
+		assert(result == 0);
+		*entity = &entity_pool[index];
+		entity_map[index] = &entity_pool[index];
+		++entity_count;
+	}
+
+	return result;
+}
+
+
+/* Release an entity back to the pool */
+static int free_entity(const io_entity_t *entity)
+{
+	int result;
+	unsigned int index = 0;
+	assert(entity != NULL);
+
+	result = find_first_entity(entity, &index);
+	if (result ==  0) {
+		entity_map[index] = NULL;
+		--entity_count;
+	}
+
+	return result;
+}
+
+
+/* Exported API */
+
+/* Register a device driver */
+int io_register_device(const io_dev_info_t *dev_info)
+{
+	int result = -ENOMEM;
+	assert(dev_info != NULL);
+
+	if (dev_count < MAX_IO_DEVICES) {
+		devices[dev_count] = dev_info;
+		dev_count++;
+		result = 0;
+	}
+
+	return result;
+}
+
+
+/* Open a connection to an IO device */
+int io_dev_open(const io_dev_connector_t *dev_con, const uintptr_t dev_spec,
+		uintptr_t *handle)
+{
+	assert(handle != NULL);
+	return io_storage_dev_open(dev_con, dev_spec, (io_dev_info_t **)handle);
+}
+
+
+/* Initialise an IO device explicitly - to permit lazy initialisation or
+ * re-initialisation */
+int io_dev_init(uintptr_t dev_handle, const uintptr_t init_params)
+{
+	int result = 0;
+	assert(dev_handle != (uintptr_t)NULL);
+	assert(is_valid_dev(dev_handle));
+
+	io_dev_info_t *dev = (io_dev_info_t *)dev_handle;
+
+	/* Absence of registered function implies NOP here */
+	if (dev->funcs->dev_init != NULL) {
+		result = dev->funcs->dev_init(dev, init_params);
+	}
+
+	return result;
+}
+
+/* Close a connection to a device */
+int io_dev_close(uintptr_t dev_handle)
+{
+	int result = 0;
+	assert(dev_handle != (uintptr_t)NULL);
+	assert(is_valid_dev(dev_handle));
+
+	io_dev_info_t *dev = (io_dev_info_t *)dev_handle;
+
+	/* Absence of registered function implies NOP here */
+	if (dev->funcs->dev_close != NULL) {
+		result = dev->funcs->dev_close(dev);
+	}
+
+	return result;
+}
+
+
+/* Synchronous operations */
+
+
+/* Open an IO entity */
+int io_open(uintptr_t dev_handle, const uintptr_t spec, uintptr_t *handle)
+{
+	int result;
+	assert((spec != (uintptr_t)NULL) && (handle != NULL));
+	assert(is_valid_dev(dev_handle));
+
+	io_dev_info_t *dev = (io_dev_info_t *)dev_handle;
+	io_entity_t *entity;
+
+	result = allocate_entity(&entity);
+
+	if (result == 0) {
+		assert(dev->funcs->open != NULL);
+		result = dev->funcs->open(dev, spec, entity);
+
+		if (result == 0) {
+			entity->dev_handle = dev;
+			set_handle(handle, entity);
+		} else
+			free_entity(entity);
+	}
+	return result;
+}
+
+
+/* Seek to a specific position in an IO entity */
+int io_seek(uintptr_t handle, io_seek_mode_t mode, signed long long offset)
+{
+	int result = -ENODEV;
+	assert(is_valid_entity(handle) && is_valid_seek_mode(mode));
+
+	io_entity_t *entity = (io_entity_t *)handle;
+
+	io_dev_info_t *dev = entity->dev_handle;
+
+	if (dev->funcs->seek != NULL)
+		result = dev->funcs->seek(entity, mode, offset);
+
+	return result;
+}
+
+
+/* Determine the length of an IO entity */
+int io_size(uintptr_t handle, size_t *length)
+{
+	int result = -ENODEV;
+	assert(is_valid_entity(handle) && (length != NULL));
+
+	io_entity_t *entity = (io_entity_t *)handle;
+
+	io_dev_info_t *dev = entity->dev_handle;
+
+	if (dev->funcs->size != NULL)
+		result = dev->funcs->size(entity, length);
+
+	return result;
+}
+
+
+/* Read data from an IO entity */
+int io_read(uintptr_t handle,
+		uintptr_t buffer,
+		size_t length,
+		size_t *length_read)
+{
+	int result = -ENODEV;
+	assert(is_valid_entity(handle));
+
+	io_entity_t *entity = (io_entity_t *)handle;
+
+	io_dev_info_t *dev = entity->dev_handle;
+
+	if (dev->funcs->read != NULL)
+		result = dev->funcs->read(entity, buffer, length, length_read);
+
+	return result;
+}
+
+
+/* Write data to an IO entity */
+int io_write(uintptr_t handle,
+		const uintptr_t buffer,
+		size_t length,
+		size_t *length_written)
+{
+	int result = -ENODEV;
+	assert(is_valid_entity(handle));
+
+	io_entity_t *entity = (io_entity_t *)handle;
+
+	io_dev_info_t *dev = entity->dev_handle;
+
+	if (dev->funcs->write != NULL) {
+		result = dev->funcs->write(entity, buffer, length,
+				length_written);
+	}
+
+	return result;
+}
+
+
+/* Close an IO entity */
+int io_close(uintptr_t handle)
+{
+	int result = 0;
+	assert(is_valid_entity(handle));
+
+	io_entity_t *entity = (io_entity_t *)handle;
+
+	io_dev_info_t *dev = entity->dev_handle;
+
+	/* Absence of registered function implies NOP here */
+	if (dev->funcs->close != NULL)
+		result = dev->funcs->close(entity);
+
+	/* Ignore improbable free_entity failure */
+	(void)free_entity(entity);
+
+	return result;
+}
diff --git a/drivers/marvell/amb_adec.c b/drivers/marvell/amb_adec.c
new file mode 100644
index 0000000..d78fa25
--- /dev/null
+++ b/drivers/marvell/amb_adec.c
@@ -0,0 +1,160 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* AXI to M-Bridge decoding unit driver for Marvell Armada 8K and 8K+ SoCs */
+
+#include <inttypes.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include <armada_common.h>
+#include <mvebu.h>
+#include <mvebu_def.h>
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+#define DEBUG_ADDR_MAP
+#endif
+
+/* common defines */
+#define WIN_ENABLE_BIT			(0x1)
+
+#define MVEBU_AMB_ADEC_OFFSET		(0x70ff00)
+
+#define AMB_WIN_CR_OFFSET(win)		(amb_base + 0x0 + (0x8 * win))
+#define AMB_ATTR_OFFSET			8
+#define AMB_ATTR_MASK			0xFF
+#define AMB_SIZE_OFFSET			16
+#define AMB_SIZE_MASK			0xFF
+
+#define AMB_WIN_BASE_OFFSET(win)	(amb_base + 0x4 + (0x8 * win))
+#define AMB_BASE_OFFSET			16
+#define AMB_BASE_ADDR_MASK		((1 << (32 - AMB_BASE_OFFSET)) - 1)
+
+#define AMB_WIN_ALIGNMENT_64K		(0x10000)
+#define AMB_WIN_ALIGNMENT_1M		(0x100000)
+
+uintptr_t amb_base;
+
+static void amb_check_win(struct addr_map_win *win, uint32_t win_num)
+{
+	uint32_t base_addr;
+
+	/* make sure the base address is in 16-bit range */
+	if (win->base_addr > AMB_BASE_ADDR_MASK) {
+		WARN("Window %d: base address is too big 0x%" PRIx64 "\n",
+		       win_num, win->base_addr);
+		win->base_addr = AMB_BASE_ADDR_MASK;
+		WARN("Set the base address to 0x%" PRIx64 "\n", win->base_addr);
+	}
+
+	base_addr  = win->base_addr << AMB_BASE_OFFSET;
+	/* for AMB The base is always 1M aligned */
+	/* check if address is aligned to 1M */
+	if (IS_NOT_ALIGN(base_addr, AMB_WIN_ALIGNMENT_1M)) {
+		win->base_addr = ALIGN_UP(base_addr, AMB_WIN_ALIGNMENT_1M);
+		WARN("Window %d: base address unaligned to 0x%x\n",
+		       win_num, AMB_WIN_ALIGNMENT_1M);
+		WARN("Align up the base address to 0x%" PRIx64 "\n", win->base_addr);
+	}
+
+	/* size parameter validity check */
+	if (!IS_POWER_OF_2(win->win_size)) {
+		WARN("Window %d: window size is not power of 2 (0x%" PRIx64 ")\n",
+		       win_num, win->win_size);
+		win->win_size = ROUND_UP_TO_POW_OF_2(win->win_size);
+		WARN("Rounding size to 0x%" PRIx64 "\n", win->win_size);
+	}
+}
+
+static void amb_enable_win(struct addr_map_win *win, uint32_t win_num)
+{
+	uint32_t ctrl, base, size;
+
+	/*
+	 * size is 64KB granularity.
+	 * The number of ones specifies the size of the
+	 * window in 64 KB granularity. 0 is 64KB
+	 */
+	size = (win->win_size / AMB_WIN_ALIGNMENT_64K) - 1;
+	ctrl = (size << AMB_SIZE_OFFSET) | (win->target_id << AMB_ATTR_OFFSET);
+	base = win->base_addr << AMB_BASE_OFFSET;
+
+	mmio_write_32(AMB_WIN_BASE_OFFSET(win_num), base);
+	mmio_write_32(AMB_WIN_CR_OFFSET(win_num), ctrl);
+
+	/* enable window after configuring window size (and attributes) */
+	ctrl |= WIN_ENABLE_BIT;
+	mmio_write_32(AMB_WIN_CR_OFFSET(win_num), ctrl);
+}
+
+#ifdef DEBUG_ADDR_MAP
+static void dump_amb_adec(void)
+{
+	uint32_t ctrl, base, win_id, attr;
+	uint32_t size, size_count;
+
+	/* Dump all AMB windows */
+	printf("bank  attribute     base          size\n");
+	printf("--------------------------------------------\n");
+	for (win_id = 0; win_id < AMB_MAX_WIN_ID; win_id++) {
+		ctrl = mmio_read_32(AMB_WIN_CR_OFFSET(win_id));
+		if (ctrl & WIN_ENABLE_BIT) {
+			base = mmio_read_32(AMB_WIN_BASE_OFFSET(win_id));
+			attr = (ctrl >> AMB_ATTR_OFFSET) & AMB_ATTR_MASK;
+			size_count = (ctrl >> AMB_SIZE_OFFSET) & AMB_SIZE_MASK;
+			size = (size_count + 1) * AMB_WIN_ALIGNMENT_64K;
+			printf("amb   0x%04x        0x%08x    0x%08x\n",
+			       attr, base, size);
+		}
+	}
+}
+#endif
+
+int init_amb_adec(uintptr_t base)
+{
+	struct addr_map_win *win;
+	uint32_t win_id, win_reg;
+	uint32_t win_count;
+
+	INFO("Initializing AXI to MBus Bridge Address decoding\n");
+
+	/* Get the base address of the AMB address decoding */
+	amb_base = base + MVEBU_AMB_ADEC_OFFSET;
+
+	/* Get the array of the windows and its size */
+	marvell_get_amb_memory_map(&win, &win_count, base);
+	if (win_count <= 0)
+		INFO("no windows configurations found\n");
+
+	if (win_count > AMB_MAX_WIN_ID) {
+		INFO("number of windows is bigger than %d\n", AMB_MAX_WIN_ID);
+		return 0;
+	}
+
+	/* disable all AMB windows */
+	for (win_id = 0; win_id < AMB_MAX_WIN_ID; win_id++) {
+		win_reg = mmio_read_32(AMB_WIN_CR_OFFSET(win_id));
+		win_reg &= ~WIN_ENABLE_BIT;
+		mmio_write_32(AMB_WIN_CR_OFFSET(win_id), win_reg);
+	}
+
+	/* enable relevant windows */
+	for (win_id = 0; win_id < win_count; win_id++, win++) {
+		amb_check_win(win, win_id);
+		amb_enable_win(win, win_id);
+	}
+
+#ifdef DEBUG_ADDR_MAP
+	dump_amb_adec();
+#endif
+
+	INFO("Done AXI to MBus Bridge Address decoding Initializing\n");
+
+	return 0;
+}
diff --git a/drivers/marvell/ap807_clocks_init.c b/drivers/marvell/ap807_clocks_init.c
new file mode 100644
index 0000000..c1f8619
--- /dev/null
+++ b/drivers/marvell/ap807_clocks_init.c
@@ -0,0 +1,109 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <drivers/delay_timer.h>
+#include <drivers/marvell/aro.h>
+#include <lib/mmio.h>
+
+#include <a8k_plat_def.h>
+
+/* Notify bootloader on DRAM setup */
+#define AP807_CPU_ARO_CTRL(cluster)	\
+			(MVEBU_RFU_BASE + 0x82A8 + (0xA58 * (cluster)))
+
+/* 0 - ARO clock is enabled, 1 - ARO clock is disabled */
+#define AP807_CPU_ARO_CLK_EN_OFFSET	0
+#define AP807_CPU_ARO_CLK_EN_MASK	(0x1 << AP807_CPU_ARO_CLK_EN_OFFSET)
+
+/* 0 - ARO is the clock source, 1 - PLL is the clock source */
+#define AP807_CPU_ARO_SEL_PLL_OFFSET	5
+#define AP807_CPU_ARO_SEL_PLL_MASK	(0x1 << AP807_CPU_ARO_SEL_PLL_OFFSET)
+
+/* AP807 clusters count */
+#define AP807_CLUSTER_NUM		2
+
+/* PLL frequency values */
+#define PLL_FREQ_1200			0x2AE5F002 /* 1200 */
+#define PLL_FREQ_2000			0x2FC9F002 /* 2000 */
+#define PLL_FREQ_2200			0x2AC57001 /* 2200 */
+#define PLL_FREQ_2400			0x2AE5F001 /* 2400 */
+
+/* CPU PLL control registers */
+#define AP807_CPU_PLL_CTRL(cluster)	\
+			(MVEBU_RFU_BASE + 0x82E0 + (0x8 * (cluster)))
+
+#define AP807_CPU_PLL_PARAM(cluster)	AP807_CPU_PLL_CTRL(cluster)
+#define AP807_CPU_PLL_CFG(cluster)	(AP807_CPU_PLL_CTRL(cluster) + 0x4)
+#define AP807_CPU_PLL_CFG_BYPASS_MODE	(0x1)
+#define AP807_CPU_PLL_FRC_DSCHG		(0x2)
+#define AP807_CPU_PLL_CFG_USE_REG_FILE	(0x1 << 9)
+
+static void pll_set_freq(unsigned int freq_val)
+{
+	int i;
+
+	if (freq_val != PLL_FREQ_2200)
+		return;
+
+	for (i = 0 ; i < AP807_CLUSTER_NUM ; i++) {
+		/* Set parameter of cluster i PLL to 2.2GHz */
+		mmio_write_32(AP807_CPU_PLL_PARAM(i), freq_val);
+		/* Set apll_lpf_frc_dschg - Control
+		 * voltage of internal VCO is discharged
+		 */
+		mmio_write_32(AP807_CPU_PLL_CFG(i),
+				AP807_CPU_PLL_FRC_DSCHG);
+		/* Set use_rf_conf  load PLL parameter from register */
+		mmio_write_32(AP807_CPU_PLL_CFG(i),
+				AP807_CPU_PLL_FRC_DSCHG |
+				AP807_CPU_PLL_CFG_USE_REG_FILE);
+		/* Un-set apll_lpf_frc_dschg */
+		mmio_write_32(AP807_CPU_PLL_CFG(i),
+			      AP807_CPU_PLL_CFG_USE_REG_FILE);
+	}
+}
+
+/* Switch to ARO from PLL in ap807 */
+static void aro_to_pll(void)
+{
+	unsigned int reg;
+	int i;
+
+	for (i = 0 ; i < AP807_CLUSTER_NUM ; i++) {
+		/* switch from ARO to PLL */
+		reg = mmio_read_32(AP807_CPU_ARO_CTRL(i));
+		reg |= AP807_CPU_ARO_SEL_PLL_MASK;
+		mmio_write_32(AP807_CPU_ARO_CTRL(i), reg);
+
+		mdelay(100);
+
+		/* disable ARO clk driver */
+		reg = mmio_read_32(AP807_CPU_ARO_CTRL(i));
+		reg |= (AP807_CPU_ARO_CLK_EN_MASK);
+		mmio_write_32(AP807_CPU_ARO_CTRL(i), reg);
+	}
+}
+
+/* switch from ARO to PLL
+ * in case of default frequency option, configure PLL registers
+ * to be aligned with new default frequency.
+ */
+void ap807_clocks_init(unsigned int freq_option)
+{
+	/* Modifications in frequency table:
+	 * 0x0: 764x: change to 2000 MHz.
+	 * 0x2: 744x change to 1800 MHz, 764x change to 2200/2400.
+	 * 0x3: 3900/744x/764x change to 1200 MHz.
+	 */
+
+	if (freq_option == CPU_2200_DDR_1200_RCLK_1200)
+		pll_set_freq(PLL_FREQ_2200);
+
+	/* Switch from ARO to PLL */
+	aro_to_pll();
+
+}
diff --git a/drivers/marvell/cache_llc.c b/drivers/marvell/cache_llc.c
new file mode 100644
index 0000000..4b06b47
--- /dev/null
+++ b/drivers/marvell/cache_llc.c
@@ -0,0 +1,189 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* LLC driver is the Last Level Cache (L3C) driver
+ * for Marvell SoCs in AP806, AP807, and AP810
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <drivers/marvell/cache_llc.h>
+#include <drivers/marvell/ccu.h>
+#include <lib/mmio.h>
+
+#include <mvebu_def.h>
+
+#define CCU_HTC_CR(ap_index)		(MVEBU_CCU_BASE(ap_index) + 0x200)
+#define CCU_SET_POC_OFFSET		5
+
+extern void ca72_l2_enable_unique_clean(void);
+
+void llc_cache_sync(int ap_index)
+{
+	mmio_write_32(LLC_SYNC(ap_index), 0);
+	/* Atomic write, no need to wait */
+}
+
+void llc_flush_all(int ap_index)
+{
+	mmio_write_32(LLC_CLEAN_INV_WAY(ap_index), LLC_ALL_WAYS_MASK);
+	llc_cache_sync(ap_index);
+}
+
+void llc_clean_all(int ap_index)
+{
+	mmio_write_32(LLC_CLEAN_WAY(ap_index), LLC_ALL_WAYS_MASK);
+	llc_cache_sync(ap_index);
+}
+
+void llc_inv_all(int ap_index)
+{
+	mmio_write_32(LLC_INV_WAY(ap_index), LLC_ALL_WAYS_MASK);
+	llc_cache_sync(ap_index);
+}
+
+void llc_disable(int ap_index)
+{
+	llc_flush_all(ap_index);
+	mmio_write_32(LLC_CTRL(ap_index), 0);
+	dsbishst();
+}
+
+void llc_enable(int ap_index, int excl_mode)
+{
+	uint32_t val;
+
+	dsbsy();
+	llc_inv_all(ap_index);
+	dsbsy();
+
+	val = LLC_CTRL_EN;
+	if (excl_mode)
+		val |= LLC_EXCLUSIVE_EN;
+
+	mmio_write_32(LLC_CTRL(ap_index), val);
+	dsbsy();
+}
+
+int llc_is_exclusive(int ap_index)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(LLC_CTRL(ap_index));
+
+	if ((reg & (LLC_CTRL_EN | LLC_EXCLUSIVE_EN)) ==
+		   (LLC_CTRL_EN | LLC_EXCLUSIVE_EN))
+		return 1;
+
+	return 0;
+}
+
+void llc_runtime_enable(int ap_index)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(LLC_CTRL(ap_index));
+	if (reg & LLC_CTRL_EN)
+		return;
+
+	INFO("Enabling LLC\n");
+
+	/*
+	 * Enable L2 UniqueClean evictions with data
+	 *  Note: this configuration assumes that LLC is configured
+	 *	  in exclusive mode.
+	 *	  Later on in the code this assumption will be validated
+	 */
+	ca72_l2_enable_unique_clean();
+	llc_enable(ap_index, 1);
+
+	/* Set point of coherency to DDR.
+	 * This is required by units which have SW cache coherency
+	 */
+	reg = mmio_read_32(CCU_HTC_CR(ap_index));
+	reg |= (0x1 << CCU_SET_POC_OFFSET);
+	mmio_write_32(CCU_HTC_CR(ap_index), reg);
+}
+
+#if LLC_SRAM
+int llc_sram_enable(int ap_index, int size)
+{
+	uint32_t tc, way, ways_to_allocate;
+	uint32_t way_addr;
+
+	if ((size <= 0) || (size > LLC_SIZE) || (size % LLC_WAY_SIZE))
+		return -1;
+
+	llc_enable(ap_index, 1);
+	llc_inv_all(ap_index);
+
+	ways_to_allocate = size / LLC_WAY_SIZE;
+
+	/* Lockdown all available ways for all traffic classes */
+	for (tc = 0; tc < LLC_TC_NUM; tc++)
+		mmio_write_32(LLC_TCN_LOCK(ap_index, tc), LLC_ALL_WAYS_MASK);
+
+	/* Clear the high bits of SRAM address */
+	mmio_write_32(LLC_BANKED_MNT_AHR(ap_index), 0);
+
+	way_addr = PLAT_MARVELL_TRUSTED_RAM_BASE;
+	for (way = 0; way < ways_to_allocate; way++) {
+		/* Trigger allocation block command */
+		mmio_write_32(LLC_BLK_ALOC(ap_index),
+			      LLC_BLK_ALOC_BASE_ADDR(way_addr) |
+			      LLC_BLK_ALOC_WAY_DATA_SET |
+			      LLC_BLK_ALOC_WAY_ID(way));
+		way_addr += LLC_WAY_SIZE;
+	}
+	return 0;
+}
+
+void llc_sram_disable(int ap_index)
+{
+	uint32_t tc;
+
+	/* Disable the line lockings */
+	for (tc = 0; tc < LLC_TC_NUM; tc++)
+		mmio_write_32(LLC_TCN_LOCK(ap_index, tc), 0);
+
+	/* Invalidate all ways */
+	llc_inv_all(ap_index);
+}
+
+int llc_sram_test(int ap_index, int size, char *msg)
+{
+	uintptr_t addr, end_addr;
+	uint32_t data = 0;
+
+	if ((size <= 0) || (size > LLC_SIZE))
+		return -1;
+
+	INFO("=== LLC SRAM WRITE test %s\n", msg);
+	for (addr = PLAT_MARVELL_TRUSTED_RAM_BASE,
+	     end_addr = PLAT_MARVELL_TRUSTED_RAM_BASE + size;
+	     addr < end_addr; addr += 4) {
+		mmio_write_32(addr, addr);
+	}
+	INFO("=== LLC SRAM WRITE test %s PASSED\n", msg);
+	INFO("=== LLC SRAM READ test %s\n", msg);
+	for (addr = PLAT_MARVELL_TRUSTED_RAM_BASE,
+	     end_addr = PLAT_MARVELL_TRUSTED_RAM_BASE + size;
+	     addr < end_addr; addr += 4) {
+		data = mmio_read_32(addr);
+		if (data != addr) {
+			INFO("=== LLC SRAM READ test %s FAILED @ 0x%08lx)\n",
+			     msg, addr);
+			return -1;
+		}
+	}
+	INFO("=== LLC SRAM READ test %s PASSED (last read = 0x%08x)\n",
+	     msg, data);
+	return 0;
+}
+
+#endif /* LLC_SRAM */
diff --git a/drivers/marvell/ccu.c b/drivers/marvell/ccu.c
new file mode 100644
index 0000000..c206f11
--- /dev/null
+++ b/drivers/marvell/ccu.c
@@ -0,0 +1,417 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* CCU unit device driver for Marvell AP807, AP807 and AP810 SoCs */
+
+#include <inttypes.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/marvell/ccu.h>
+#include <lib/mmio.h>
+
+#include <armada_common.h>
+#include <mvebu.h>
+#include <mvebu_def.h>
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+#define DEBUG_ADDR_MAP
+#endif
+
+/* common defines */
+#define WIN_ENABLE_BIT			(0x1)
+/* Physical address of the base of the window = {AddrLow[19:0],20'h0} */
+#define ADDRESS_SHIFT			(20 - 4)
+#define ADDRESS_MASK			(0xFFFFFFF0)
+#define CCU_WIN_ALIGNMENT		(0x100000)
+
+/*
+ * Physical address of the highest address of window bits[31:19] = 0x6FF
+ * Physical address of the lowest address of window bits[18:6] = 0x6E0
+ * Unit Id bits [5:2] = 2
+ * RGF Window Enable bit[0] = 1
+ * 0x37f9b809 - 11011111111 0011011100000 0010 0 1
+ */
+#define ERRATA_WA_CCU_WIN4	0x37f9b809U
+
+/*
+ * Physical address of the highest address of window bits[31:19] = 0xFFF
+ * Physical address of the lowest address of window bits[18:6] = 0x800
+ * Unit Id bits [5:2] = 2
+ * RGF Window Enable bit[0] = 1
+ * 0x7ffa0009 - 111111111111 0100000000000 0010 0 1
+ */
+#define ERRATA_WA_CCU_WIN5	0x7ffa0009U
+
+/*
+ * Physical address of the highest address of window bits[31:19] = 0x1FFF
+ * Physical address of the lowest address of window bits[18:6] = 0x1000
+ * Unit Id bits [5:2] = 2
+ * RGF Window Enable bit[0] = 1
+ * 0xfffc000d - 1111111111111 1000000000000 0011 0 1
+ */
+#define ERRATA_WA_CCU_WIN6	0xfffc000dU
+
+#define IS_DRAM_TARGET(tgt)		((((tgt) == DRAM_0_TID) || \
+					((tgt) == DRAM_1_TID) || \
+					((tgt) == RAR_TID)) ? 1 : 0)
+
+#define CCU_RGF(win)			(MVEBU_CCU_BASE(MVEBU_AP0) + \
+					 0x90 + 4 * (win))
+
+/* For storage of CR, SCR, ALR, AHR abd GCR */
+static uint32_t ccu_regs_save[MVEBU_CCU_MAX_WINS * 4 + 1];
+
+#ifdef DEBUG_ADDR_MAP
+static void dump_ccu(int ap_index)
+{
+	uint32_t win_id, win_cr, alr, ahr;
+	uint8_t target_id;
+	uint64_t start, end;
+
+	/* Dump all AP windows */
+	printf("\tbank  target     start              end\n");
+	printf("\t----------------------------------------------------\n");
+	for (win_id = 0; win_id < MVEBU_CCU_MAX_WINS; win_id++) {
+		win_cr = mmio_read_32(CCU_WIN_CR_OFFSET(ap_index, win_id));
+		if (win_cr & WIN_ENABLE_BIT) {
+			target_id = (win_cr >> CCU_TARGET_ID_OFFSET) &
+				     CCU_TARGET_ID_MASK;
+			alr = mmio_read_32(CCU_WIN_ALR_OFFSET(ap_index,
+							      win_id));
+			ahr = mmio_read_32(CCU_WIN_AHR_OFFSET(ap_index,
+							      win_id));
+			start = ((uint64_t)alr << ADDRESS_SHIFT);
+			end = (((uint64_t)ahr + 0x10) << ADDRESS_SHIFT);
+			printf("\tccu%d    %02x     0x%016" PRIx64 " 0x%016" PRIx64 "\n",
+			       win_id, target_id, start, end);
+		}
+	}
+	win_cr = mmio_read_32(CCU_WIN_GCR_OFFSET(ap_index));
+	target_id = (win_cr >> CCU_GCR_TARGET_OFFSET) & CCU_GCR_TARGET_MASK;
+	printf("\tccu   GCR %d - all other transactions\n", target_id);
+}
+#endif
+
+void ccu_win_check(struct addr_map_win *win)
+{
+	/* check if address is aligned to 1M */
+	if (IS_NOT_ALIGN(win->base_addr, CCU_WIN_ALIGNMENT)) {
+		win->base_addr = ALIGN_UP(win->base_addr, CCU_WIN_ALIGNMENT);
+		NOTICE("%s: Align up the base address to 0x%" PRIx64 "\n",
+		       __func__, win->base_addr);
+	}
+
+	/* size parameter validity check */
+	if (IS_NOT_ALIGN(win->win_size, CCU_WIN_ALIGNMENT)) {
+		win->win_size = ALIGN_UP(win->win_size, CCU_WIN_ALIGNMENT);
+		NOTICE("%s: Aligning size to 0x%" PRIx64 "\n",
+		       __func__, win->win_size);
+	}
+}
+
+int ccu_is_win_enabled(int ap_index, uint32_t win_id)
+{
+	return mmio_read_32(CCU_WIN_CR_OFFSET(ap_index, win_id)) &
+			    WIN_ENABLE_BIT;
+}
+
+void ccu_enable_win(int ap_index, struct addr_map_win *win, uint32_t win_id)
+{
+	uint32_t ccu_win_reg;
+	uint32_t alr, ahr;
+	uint64_t end_addr;
+
+	if ((win_id == 0) || (win_id > MVEBU_CCU_MAX_WINS)) {
+		ERROR("Enabling wrong CCU window %d!\n", win_id);
+		return;
+	}
+
+	end_addr = (win->base_addr + win->win_size - 1);
+	alr = (uint32_t)((win->base_addr >> ADDRESS_SHIFT) & ADDRESS_MASK);
+	ahr = (uint32_t)((end_addr >> ADDRESS_SHIFT) & ADDRESS_MASK);
+
+	mmio_write_32(CCU_WIN_ALR_OFFSET(ap_index, win_id), alr);
+	mmio_write_32(CCU_WIN_AHR_OFFSET(ap_index, win_id), ahr);
+
+	ccu_win_reg = WIN_ENABLE_BIT;
+	ccu_win_reg |= (win->target_id & CCU_TARGET_ID_MASK)
+			<< CCU_TARGET_ID_OFFSET;
+	mmio_write_32(CCU_WIN_CR_OFFSET(ap_index, win_id), ccu_win_reg);
+}
+
+static void ccu_disable_win(int ap_index, uint32_t win_id)
+{
+	uint32_t win_reg;
+
+	if ((win_id == 0) || (win_id > MVEBU_CCU_MAX_WINS)) {
+		ERROR("Disabling wrong CCU window %d!\n", win_id);
+		return;
+	}
+
+	win_reg = mmio_read_32(CCU_WIN_CR_OFFSET(ap_index, win_id));
+	win_reg &= ~WIN_ENABLE_BIT;
+	mmio_write_32(CCU_WIN_CR_OFFSET(ap_index, win_id), win_reg);
+}
+
+/* Insert/Remove temporary window for using the out-of reset default
+ * CPx base address to access the CP configuration space prior to
+ * the further base address update in accordance with address mapping
+ * design.
+ *
+ * NOTE: Use the same window array for insertion and removal of
+ *       temporary windows.
+ */
+void ccu_temp_win_insert(int ap_index, struct addr_map_win *win, int size)
+{
+	uint32_t win_id;
+
+	for (int i = 0; i < size; i++) {
+		win_id = MVEBU_CCU_MAX_WINS - 1 - i;
+		ccu_win_check(win);
+		ccu_enable_win(ap_index, win, win_id);
+		win++;
+	}
+}
+
+/*
+ * NOTE: Use the same window array for insertion and removal of
+ *       temporary windows.
+ */
+void ccu_temp_win_remove(int ap_index, struct addr_map_win *win, int size)
+{
+	uint32_t win_id;
+
+	for (int i = 0; i < size; i++) {
+		uint64_t base;
+		uint32_t target;
+
+		win_id = MVEBU_CCU_MAX_WINS - 1 - i;
+
+		target = mmio_read_32(CCU_WIN_CR_OFFSET(ap_index, win_id));
+		target >>= CCU_TARGET_ID_OFFSET;
+		target &= CCU_TARGET_ID_MASK;
+
+		base = mmio_read_32(CCU_WIN_ALR_OFFSET(ap_index, win_id));
+		base <<= ADDRESS_SHIFT;
+
+		if ((win->target_id != target) || (win->base_addr != base)) {
+			ERROR("%s: Trying to remove bad window-%d!\n",
+			      __func__, win_id);
+			continue;
+		}
+		ccu_disable_win(ap_index, win_id);
+		win++;
+	}
+}
+
+/* Returns current DRAM window target (DRAM_0_TID, DRAM_1_TID, RAR_TID)
+ * NOTE: Call only once for each AP.
+ * The AP0 DRAM window is located at index 2 only at the BL31 execution start.
+ * Then it relocated to index 1 for matching the rest of APs DRAM settings.
+ * Calling this function after relocation will produce wrong results on AP0
+ */
+static uint32_t ccu_dram_target_get(int ap_index)
+{
+	/* On BLE stage the AP0 DRAM window is opened by the BootROM at index 2.
+	 * All the rest of detected APs will use window at index 1.
+	 * The AP0 DRAM window is moved from index 2 to 1 during
+	 * init_ccu() execution.
+	 */
+	const uint32_t win_id = (ap_index == 0) ? 2 : 1;
+	uint32_t target;
+
+	target = mmio_read_32(CCU_WIN_CR_OFFSET(ap_index, win_id));
+	target >>= CCU_TARGET_ID_OFFSET;
+	target &= CCU_TARGET_ID_MASK;
+
+	return target;
+}
+
+void ccu_dram_target_set(int ap_index, uint32_t target)
+{
+	/* On BLE stage the AP0 DRAM window is opened by the BootROM at index 2.
+	 * All the rest of detected APs will use window at index 1.
+	 * The AP0 DRAM window is moved from index 2 to 1
+	 * during init_ccu() execution.
+	 */
+	const uint32_t win_id = (ap_index == 0) ? 2 : 1;
+	uint32_t dram_cr;
+
+	dram_cr = mmio_read_32(CCU_WIN_CR_OFFSET(ap_index, win_id));
+	dram_cr &= ~(CCU_TARGET_ID_MASK << CCU_TARGET_ID_OFFSET);
+	dram_cr |= (target & CCU_TARGET_ID_MASK) << CCU_TARGET_ID_OFFSET;
+	mmio_write_32(CCU_WIN_CR_OFFSET(ap_index, win_id), dram_cr);
+}
+
+/* Setup CCU DRAM window and enable it */
+void ccu_dram_win_config(int ap_index, struct addr_map_win *win)
+{
+#if IMAGE_BLE /* BLE */
+	/* On BLE stage the AP0 DRAM window is opened by the BootROM at index 2.
+	 * Since the BootROM is not accessing DRAM at BLE stage,
+	 * the DRAM window can be temporarely disabled.
+	 */
+	const uint32_t win_id = (ap_index == 0) ? 2 : 1;
+#else /* end of BLE */
+	/* At the ccu_init() execution stage, DRAM windows of all APs
+	 * are arranged at index 1.
+	 * The AP0 still has the old window BootROM DRAM at index 2, so
+	 * the window-1 can be safely disabled without breaking the DRAM access.
+	 */
+	const uint32_t win_id = 1;
+#endif
+
+	ccu_disable_win(ap_index, win_id);
+	/* enable write secure (and clear read secure) */
+	mmio_write_32(CCU_WIN_SCR_OFFSET(ap_index, win_id),
+		      CCU_WIN_ENA_WRITE_SECURE);
+	ccu_win_check(win);
+	ccu_enable_win(ap_index, win, win_id);
+}
+
+/* Save content of CCU window + GCR */
+static void ccu_save_win_range(int ap_id, int win_first,
+			       int win_last, uint32_t *buffer)
+{
+	int win_id, idx;
+	/* Save CCU */
+	for (idx = 0, win_id = win_first; win_id <= win_last; win_id++) {
+		buffer[idx++] = mmio_read_32(CCU_WIN_CR_OFFSET(ap_id, win_id));
+		buffer[idx++] = mmio_read_32(CCU_WIN_SCR_OFFSET(ap_id, win_id));
+		buffer[idx++] = mmio_read_32(CCU_WIN_ALR_OFFSET(ap_id, win_id));
+		buffer[idx++] = mmio_read_32(CCU_WIN_AHR_OFFSET(ap_id, win_id));
+	}
+	buffer[idx] = mmio_read_32(CCU_WIN_GCR_OFFSET(ap_id));
+}
+
+/* Restore content of CCU window + GCR */
+static void ccu_restore_win_range(int ap_id, int win_first,
+				  int win_last, uint32_t *buffer)
+{
+	int win_id, idx;
+	/* Restore CCU */
+	for (idx = 0, win_id = win_first; win_id <= win_last; win_id++) {
+		mmio_write_32(CCU_WIN_CR_OFFSET(ap_id, win_id),  buffer[idx++]);
+		mmio_write_32(CCU_WIN_SCR_OFFSET(ap_id, win_id), buffer[idx++]);
+		mmio_write_32(CCU_WIN_ALR_OFFSET(ap_id, win_id), buffer[idx++]);
+		mmio_write_32(CCU_WIN_AHR_OFFSET(ap_id, win_id), buffer[idx++]);
+	}
+	mmio_write_32(CCU_WIN_GCR_OFFSET(ap_id), buffer[idx]);
+}
+
+void ccu_save_win_all(int ap_id)
+{
+	ccu_save_win_range(ap_id, 0, MVEBU_CCU_MAX_WINS - 1, ccu_regs_save);
+}
+
+void ccu_restore_win_all(int ap_id)
+{
+	ccu_restore_win_range(ap_id, 0, MVEBU_CCU_MAX_WINS - 1, ccu_regs_save);
+}
+
+int init_ccu(int ap_index)
+{
+	struct addr_map_win *win, *dram_win;
+	uint32_t win_id, win_reg;
+	uint32_t win_count, array_id;
+	uint32_t dram_target;
+#if IMAGE_BLE
+	/* In BootROM context CCU Window-1
+	 * has SRAM_TID target and should not be disabled
+	 */
+	const uint32_t win_start = 2;
+#else
+	const uint32_t win_start = 1;
+#endif
+
+	INFO("Initializing CCU Address decoding\n");
+
+	/* Get the array of the windows and fill the map data */
+	marvell_get_ccu_memory_map(ap_index, &win, &win_count);
+	if (win_count <= 0) {
+		INFO("No windows configurations found\n");
+	} else if (win_count > (MVEBU_CCU_MAX_WINS - 1)) {
+		ERROR("CCU mem map array > than max available windows (%d)\n",
+		      MVEBU_CCU_MAX_WINS);
+		win_count = MVEBU_CCU_MAX_WINS;
+	}
+
+	/* Need to set GCR to DRAM before all CCU windows are disabled for
+	 * securing the normal access to DRAM location, which the ATF is running
+	 * from. Once all CCU windows are set, which have to include the
+	 * dedicated DRAM window as well, the GCR can be switched to the target
+	 * defined by the platform configuration.
+	 */
+	dram_target = ccu_dram_target_get(ap_index);
+	win_reg = (dram_target & CCU_GCR_TARGET_MASK) << CCU_GCR_TARGET_OFFSET;
+	mmio_write_32(CCU_WIN_GCR_OFFSET(ap_index), win_reg);
+
+	/* If the DRAM window was already configured at the BLE stage,
+	 * only the window target considered valid, the address range should be
+	 * updated according to the platform configuration.
+	 */
+	for (dram_win = win, array_id = 0; array_id < win_count;
+	     array_id++, dram_win++) {
+		if (IS_DRAM_TARGET(dram_win->target_id)) {
+			dram_win->target_id = dram_target;
+			break;
+		}
+	}
+
+	/* Disable all AP CCU windows
+	 * Window-0 is always bypassed since it already contains
+	 * data allowing the internal configuration space access
+	 */
+	for (win_id = win_start; win_id < MVEBU_CCU_MAX_WINS; win_id++) {
+		ccu_disable_win(ap_index, win_id);
+		/* enable write secure (and clear read secure) */
+		mmio_write_32(CCU_WIN_SCR_OFFSET(ap_index, win_id),
+			      CCU_WIN_ENA_WRITE_SECURE);
+	}
+
+	/* win_id is the index of the current ccu window
+	 * array_id is the index of the current memory map window entry
+	 */
+	for (win_id = win_start, array_id = 0;
+	    ((win_id < MVEBU_CCU_MAX_WINS) && (array_id < win_count));
+	    win_id++) {
+		ccu_win_check(win);
+		ccu_enable_win(ap_index, win, win_id);
+		win++;
+		array_id++;
+	}
+
+	/* Get & set the default target according to board topology */
+	win_reg = (marvell_get_ccu_gcr_target(ap_index) & CCU_GCR_TARGET_MASK)
+		   << CCU_GCR_TARGET_OFFSET;
+	mmio_write_32(CCU_WIN_GCR_OFFSET(ap_index), win_reg);
+
+#ifdef DEBUG_ADDR_MAP
+	dump_ccu(ap_index);
+#endif
+
+	INFO("Done CCU Address decoding Initializing\n");
+
+	return 0;
+}
+
+void errata_wa_init(void)
+{
+	/*
+	 * EERATA ID: RES-3033912 - Internal Address Space Init state causes
+	 * a hang upon accesses to [0xf070_0000, 0xf07f_ffff]
+	 * Workaround: Boot Firmware (ATF) should configure CCU_RGF_WIN(4) to
+	 * split [0x6e_0000, 0x1ff_ffff] to values [0x6e_0000, 0x6f_ffff] and
+	 * [0x80_0000, 0xff_ffff] and [0x100_0000, 0x1ff_ffff],that cause
+	 * accesses to the segment of [0xf070_0000, 0xf1ff_ffff]
+	 * to act as RAZWI.
+	 */
+	mmio_write_32(CCU_RGF(4), ERRATA_WA_CCU_WIN4);
+	mmio_write_32(CCU_RGF(5), ERRATA_WA_CCU_WIN5);
+	mmio_write_32(CCU_RGF(6), ERRATA_WA_CCU_WIN6);
+}
diff --git a/drivers/marvell/comphy.h b/drivers/marvell/comphy.h
new file mode 100644
index 0000000..fab564e
--- /dev/null
+++ b/drivers/marvell/comphy.h
@@ -0,0 +1,472 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* Driver for COMPHY unit that is part or Marvell A8K SoCs */
+
+#ifndef COMPHY_H
+#define COMPHY_H
+
+/* COMPHY registers */
+#define COMMON_PHY_CFG1_REG			0x0
+#define COMMON_PHY_CFG1_PWR_UP_OFFSET		1
+#define COMMON_PHY_CFG1_PWR_UP_MASK		\
+				(0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET)
+#define COMMON_PHY_CFG1_PIPE_SELECT_OFFSET	2
+#define COMMON_PHY_CFG1_PIPE_SELECT_MASK	\
+				(0x1 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET)
+#define COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET	13
+#define COMMON_PHY_CFG1_PWR_ON_RESET_MASK	\
+				(0x1 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET)
+#define COMMON_PHY_CFG1_CORE_RSTN_OFFSET	14
+#define COMMON_PHY_CFG1_CORE_RSTN_MASK		\
+				(0x1 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET)
+#define COMMON_PHY_PHY_MODE_OFFSET		15
+#define COMMON_PHY_PHY_MODE_MASK		\
+				(0x1 << COMMON_PHY_PHY_MODE_OFFSET)
+
+#define COMMON_SELECTOR_PHY_OFFSET			0x140
+#define COMMON_SELECTOR_PIPE_OFFSET			0x144
+
+#define COMMON_PHY_SD_CTRL1				0x148
+#define COMMON_PHY_SD_CTRL1_COMPHY_0_4_PORT_OFFSET	0
+#define COMMON_PHY_SD_CTRL1_COMPHY_0_4_PORT_MASK	0xFFFF
+#define COMMON_PHY_SD_CTRL1_PCIE_X4_EN_OFFSET		24
+#define COMMON_PHY_SD_CTRL1_PCIE_X4_EN_MASK		\
+				(0x1 << COMMON_PHY_SD_CTRL1_PCIE_X4_EN_OFFSET)
+#define COMMON_PHY_SD_CTRL1_PCIE_X2_EN_OFFSET		25
+#define COMMON_PHY_SD_CTRL1_PCIE_X2_EN_MASK		\
+				(0x1 << COMMON_PHY_SD_CTRL1_PCIE_X2_EN_OFFSET)
+
+#define DFX_DEV_GEN_CTRL12			0x80
+#define DFX_DEV_GEN_PCIE_CLK_SRC_OFFSET		7
+#define DFX_DEV_GEN_PCIE_CLK_SRC_MASK		\
+				(0x3 << DFX_DEV_GEN_PCIE_CLK_SRC_OFFSET)
+
+/* HPIPE register */
+#define HPIPE_PWR_PLL_REG			0x4
+#define HPIPE_PWR_PLL_REF_FREQ_OFFSET		0
+#define HPIPE_PWR_PLL_REF_FREQ_MASK		\
+				(0x1f << HPIPE_PWR_PLL_REF_FREQ_OFFSET)
+#define HPIPE_PWR_PLL_PHY_MODE_OFFSET		5
+#define HPIPE_PWR_PLL_PHY_MODE_MASK		\
+				(0x7 << HPIPE_PWR_PLL_PHY_MODE_OFFSET)
+
+#define HPIPE_DFE_REG0				0x01C
+#define HPIPE_DFE_RES_FORCE_OFFSET		15
+#define HPIPE_DFE_RES_FORCE_MASK		\
+				(0x1 << HPIPE_DFE_RES_FORCE_OFFSET)
+
+#define HPIPE_G2_SET_1_REG			0x040
+#define HPIPE_G2_SET_1_G2_RX_SELMUPI_OFFSET	0
+#define HPIPE_G2_SET_1_G2_RX_SELMUPI_MASK	\
+				(0x7 << HPIPE_G2_SET_1_G2_RX_SELMUPI_OFFSET)
+#define HPIPE_G2_SET_1_G2_RX_SELMUPP_OFFSET	3
+#define HPIPE_G2_SET_1_G2_RX_SELMUPP_MASK	\
+				(0x7 << HPIPE_G2_SET_1_G2_RX_SELMUPP_OFFSET)
+#define HPIPE_G2_SET_1_G2_RX_SELMUFI_OFFSET	6
+#define HPIPE_G2_SET_1_G2_RX_SELMUFI_MASK	\
+				(0x3 << HPIPE_G2_SET_1_G2_RX_SELMUFI_OFFSET)
+
+#define HPIPE_G3_SETTINGS_1_REG			0x048
+#define HPIPE_G3_RX_SELMUPI_OFFSET		0
+#define HPIPE_G3_RX_SELMUPI_MASK		\
+				(0x7 << HPIPE_G3_RX_SELMUPI_OFFSET)
+#define HPIPE_G3_RX_SELMUPF_OFFSET		3
+#define HPIPE_G3_RX_SELMUPF_MASK		\
+				(0x7 << HPIPE_G3_RX_SELMUPF_OFFSET)
+#define HPIPE_G3_SETTING_BIT_OFFSET		13
+#define HPIPE_G3_SETTING_BIT_MASK		\
+				(0x1 << HPIPE_G3_SETTING_BIT_OFFSET)
+
+#define HPIPE_INTERFACE_REG			0x94
+#define HPIPE_INTERFACE_GEN_MAX_OFFSET		10
+#define HPIPE_INTERFACE_GEN_MAX_MASK		\
+				(0x3 << HPIPE_INTERFACE_GEN_MAX_OFFSET)
+#define HPIPE_INTERFACE_DET_BYPASS_OFFSET	12
+#define HPIPE_INTERFACE_DET_BYPASS_MASK		\
+				(0x1 << HPIPE_INTERFACE_DET_BYPASS_OFFSET)
+#define HPIPE_INTERFACE_LINK_TRAIN_OFFSET	14
+#define HPIPE_INTERFACE_LINK_TRAIN_MASK		\
+				(0x1 << HPIPE_INTERFACE_LINK_TRAIN_OFFSET)
+
+#define HPIPE_VDD_CAL_CTRL_REG			0x114
+#define HPIPE_EXT_SELLV_RXSAMPL_OFFSET		5
+#define HPIPE_EXT_SELLV_RXSAMPL_MASK		\
+				(0x1f << HPIPE_EXT_SELLV_RXSAMPL_OFFSET)
+
+#define HPIPE_PCIE_REG0				0x120
+#define HPIPE_PCIE_IDLE_SYNC_OFFSET		12
+#define HPIPE_PCIE_IDLE_SYNC_MASK		\
+				(0x1 << HPIPE_PCIE_IDLE_SYNC_OFFSET)
+#define HPIPE_PCIE_SEL_BITS_OFFSET		13
+#define HPIPE_PCIE_SEL_BITS_MASK		\
+				(0x3 << HPIPE_PCIE_SEL_BITS_OFFSET)
+
+#define HPIPE_LANE_ALIGN_REG			0x124
+#define HPIPE_LANE_ALIGN_OFF_OFFSET		12
+#define HPIPE_LANE_ALIGN_OFF_MASK		\
+				(0x1 << HPIPE_LANE_ALIGN_OFF_OFFSET)
+
+#define HPIPE_MISC_REG				0x13C
+#define HPIPE_MISC_CLK100M_125M_OFFSET		4
+#define HPIPE_MISC_CLK100M_125M_MASK		\
+				(0x1 << HPIPE_MISC_CLK100M_125M_OFFSET)
+#define HPIPE_MISC_ICP_FORCE_OFFSET		5
+#define HPIPE_MISC_ICP_FORCE_MASK		\
+				(0x1 << HPIPE_MISC_ICP_FORCE_OFFSET)
+#define HPIPE_MISC_TXDCLK_2X_OFFSET		6
+#define HPIPE_MISC_TXDCLK_2X_MASK		\
+				(0x1 << HPIPE_MISC_TXDCLK_2X_OFFSET)
+#define HPIPE_MISC_CLK500_EN_OFFSET		7
+#define HPIPE_MISC_CLK500_EN_MASK		\
+				(0x1 << HPIPE_MISC_CLK500_EN_OFFSET)
+#define HPIPE_MISC_REFCLK_SEL_OFFSET		10
+#define HPIPE_MISC_REFCLK_SEL_MASK		\
+				(0x1 << HPIPE_MISC_REFCLK_SEL_OFFSET)
+
+#define HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG	0x16C
+#define HPIPE_SMAPLER_OFFSET			12
+#define HPIPE_SMAPLER_MASK			(0x1 << HPIPE_SMAPLER_OFFSET)
+
+#define HPIPE_PWR_CTR_DTL_REG			0x184
+#define HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET	2
+#define HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK		\
+				(0x1 << HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET)
+
+#define HPIPE_FRAME_DET_CONTROL_REG		0x220
+#define HPIPE_FRAME_DET_LOCK_LOST_TO_OFFSET	12
+#define HPIPE_FRAME_DET_LOCK_LOST_TO_MASK	\
+				(0x1 << HPIPE_FRAME_DET_LOCK_LOST_TO_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_0_REG		0x268
+#define HPIPE_TX_TRAIN_P2P_HOLD_OFFSET		15
+#define HPIPE_TX_TRAIN_P2P_HOLD_MASK		\
+				(0x1 << HPIPE_TX_TRAIN_P2P_HOLD_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_REG			0x26C
+#define HPIPE_TX_TRAIN_CTRL_G1_OFFSET		0
+#define HPIPE_TX_TRAIN_CTRL_G1_MASK		\
+				(0x1 << HPIPE_TX_TRAIN_CTRL_G1_OFFSET)
+#define HPIPE_TX_TRAIN_CTRL_GN1_OFFSET		1
+#define HPIPE_TX_TRAIN_CTRL_GN1_MASK		\
+				(0x1 << HPIPE_TX_TRAIN_CTRL_GN1_OFFSET)
+#define HPIPE_TX_TRAIN_CTRL_G0_OFFSET		2
+#define HPIPE_TX_TRAIN_CTRL_G0_MASK		\
+				(0x1 << HPIPE_TX_TRAIN_CTRL_G0_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_4_REG		0x278
+#define HPIPE_TRX_TRAIN_TIMER_OFFSET		0
+#define HPIPE_TRX_TRAIN_TIMER_MASK		\
+				(0x3FF << HPIPE_TRX_TRAIN_TIMER_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_5_REG		0x2A4
+#define HPIPE_TX_TRAIN_START_SQ_EN_OFFSET	11
+#define HPIPE_TX_TRAIN_START_SQ_EN_MASK		\
+				(0x1 << HPIPE_TX_TRAIN_START_SQ_EN_OFFSET)
+#define HPIPE_TX_TRAIN_START_FRM_DET_EN_OFFSET	12
+#define HPIPE_TX_TRAIN_START_FRM_DET_EN_MASK	\
+				(0x1 << HPIPE_TX_TRAIN_START_FRM_DET_EN_OFFSET)
+#define HPIPE_TX_TRAIN_START_FRM_LOCK_EN_OFFSET	13
+#define HPIPE_TX_TRAIN_START_FRM_LOCK_EN_MASK	\
+				(0x1 << HPIPE_TX_TRAIN_START_FRM_LOCK_EN_OFFSET)
+#define HPIPE_TX_TRAIN_WAIT_TIME_EN_OFFSET	14
+#define HPIPE_TX_TRAIN_WAIT_TIME_EN_MASK	\
+				(0x1 << HPIPE_TX_TRAIN_WAIT_TIME_EN_OFFSET)
+
+#define HPIPE_TX_TRAIN_REG			0x31C
+#define HPIPE_TX_TRAIN_CHK_INIT_OFFSET		4
+#define HPIPE_TX_TRAIN_CHK_INIT_MASK		\
+				(0x1 << HPIPE_TX_TRAIN_CHK_INIT_OFFSET)
+#define HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_OFFSET	7
+#define HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_MASK	\
+				(0x1 << HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_OFFSET)
+
+#define HPIPE_CDR_CONTROL_REG			0x418
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_0_OFFSET	14
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_0_MASK	\
+				(0x3 << HPIPE_CDR_RX_MAX_DFE_ADAPT_0_OFFSET)
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_1_OFFSET	12
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_1_MASK	\
+				(0x3 << HPIPE_CDR_RX_MAX_DFE_ADAPT_1_OFFSET)
+#define HPIPE_CDR_MAX_DFE_ADAPT_0_OFFSET	9
+#define HPIPE_CDR_MAX_DFE_ADAPT_0_MASK		\
+				(0x7 << HPIPE_CDR_MAX_DFE_ADAPT_0_OFFSET)
+#define HPIPE_CDR_MAX_DFE_ADAPT_1_OFFSET	6
+#define HPIPE_CDR_MAX_DFE_ADAPT_1_MASK		\
+				(0x7 << HPIPE_CDR_MAX_DFE_ADAPT_1_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_11_REG		0x438
+#define HPIPE_TX_STATUS_CHECK_MODE_OFFSET	6
+#define HPIPE_TX_TX_STATUS_CHECK_MODE_MASK	\
+				(0x1 << HPIPE_TX_STATUS_CHECK_MODE_OFFSET)
+#define HPIPE_TX_NUM_OF_PRESET_OFFSET		10
+#define HPIPE_TX_NUM_OF_PRESET_MASK		\
+				(0x7 << HPIPE_TX_NUM_OF_PRESET_OFFSET)
+#define HPIPE_TX_SWEEP_PRESET_EN_OFFSET		15
+#define HPIPE_TX_SWEEP_PRESET_EN_MASK		\
+				(0x1 << HPIPE_TX_SWEEP_PRESET_EN_OFFSET)
+#define HPIPE_G2_SETTINGS_4_REG			0x44C
+#define HPIPE_G2_DFE_RES_OFFSET			8
+#define HPIPE_G2_DFE_RES_MASK			(0x3 << HPIPE_G2_DFE_RES_OFFSET)
+
+#define HPIPE_G3_SETTING_3_REG			0x450
+#define HPIPE_G3_FFE_CAP_SEL_OFFSET		0
+#define HPIPE_G3_FFE_CAP_SEL_MASK		\
+				(0xf << HPIPE_G3_FFE_CAP_SEL_OFFSET)
+#define HPIPE_G3_FFE_RES_SEL_OFFSET		4
+#define HPIPE_G3_FFE_RES_SEL_MASK		\
+				(0x7 << HPIPE_G3_FFE_RES_SEL_OFFSET)
+#define HPIPE_G3_FFE_SETTING_FORCE_OFFSET	7
+#define HPIPE_G3_FFE_SETTING_FORCE_MASK		\
+				(0x1 << HPIPE_G3_FFE_SETTING_FORCE_OFFSET)
+#define HPIPE_G3_FFE_DEG_RES_LEVEL_OFFSET	12
+#define HPIPE_G3_FFE_DEG_RES_LEVEL_MASK		\
+				(0x3 << HPIPE_G3_FFE_DEG_RES_LEVEL_OFFSET)
+#define HPIPE_G3_FFE_LOAD_RES_LEVEL_OFFSET	14
+#define HPIPE_G3_FFE_LOAD_RES_LEVEL_MASK	\
+				(0x3 << HPIPE_G3_FFE_LOAD_RES_LEVEL_OFFSET)
+
+#define HPIPE_G3_SETTING_4_REG			0x454
+#define HPIPE_G3_DFE_RES_OFFSET			8
+#define HPIPE_G3_DFE_RES_MASK			(0x3 << HPIPE_G3_DFE_RES_OFFSET)
+
+#define HPIPE_DFE_CONTROL_REG			0x470
+#define HPIPE_DFE_TX_MAX_DFE_ADAPT_OFFSET	14
+#define HPIPE_DFE_TX_MAX_DFE_ADAPT_MASK		\
+				(0x3 << HPIPE_DFE_TX_MAX_DFE_ADAPT_OFFSET)
+
+#define HPIPE_DFE_CTRL_28_REG			0x49C
+#define HPIPE_DFE_CTRL_28_PIPE4_OFFSET		7
+#define HPIPE_DFE_CTRL_28_PIPE4_MASK		\
+				(0x1 << HPIPE_DFE_CTRL_28_PIPE4_OFFSET)
+
+#define HPIPE_G3_SETTING_5_REG			0x548
+#define HPIPE_G3_SETTING_5_G3_ICP_OFFSET	0
+#define HPIPE_G3_SETTING_5_G3_ICP_MASK		\
+				(0xf << HPIPE_G3_SETTING_5_G3_ICP_OFFSET)
+
+#define HPIPE_LANE_STATUS1_REG			0x60C
+#define HPIPE_LANE_STATUS1_PCLK_EN_OFFSET	0
+#define HPIPE_LANE_STATUS1_PCLK_EN_MASK		\
+				(0x1 << HPIPE_LANE_STATUS1_PCLK_EN_OFFSET)
+
+#define HPIPE_LANE_CFG4_REG			0x620
+#define HPIPE_LANE_CFG4_DFE_EN_SEL_OFFSET	3
+#define HPIPE_LANE_CFG4_DFE_EN_SEL_MASK		\
+				(0x1 << HPIPE_LANE_CFG4_DFE_EN_SEL_OFFSET)
+
+#define HPIPE_LANE_EQU_CONFIG_0_REG		0x69C
+#define HPIPE_CFG_EQ_FS_OFFSET			0
+#define HPIPE_CFG_EQ_FS_MASK			(0x3f << HPIPE_CFG_EQ_FS_OFFSET)
+#define HPIPE_CFG_EQ_LF_OFFSET			6
+#define HPIPE_CFG_EQ_LF_MASK			(0x3f << HPIPE_CFG_EQ_LF_OFFSET)
+#define HPIPE_CFG_PHY_RC_EP_OFFSET		12
+#define HPIPE_CFG_PHY_RC_EP_MASK		\
+				(0x1 << HPIPE_CFG_PHY_RC_EP_OFFSET)
+
+#define HPIPE_LANE_EQ_CFG1_REG			0x6a0
+#define HPIPE_CFG_UPDATE_POLARITY_OFFSET	12
+#define HPIPE_CFG_UPDATE_POLARITY_MASK		\
+				(0x1 << HPIPE_CFG_UPDATE_POLARITY_OFFSET)
+
+#define HPIPE_LANE_EQ_CFG2_REG			0x6a4
+#define HPIPE_CFG_EQ_BUNDLE_DIS_OFFSET		14
+#define HPIPE_CFG_EQ_BUNDLE_DIS_MASK		\
+				(0x1 << HPIPE_CFG_EQ_BUNDLE_DIS_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG0_REG		0x6a8
+#define HPIPE_CFG_CURSOR_PRESET0_OFFSET		0
+#define HPIPE_CFG_CURSOR_PRESET0_MASK		\
+				(0x3f << HPIPE_CFG_CURSOR_PRESET0_OFFSET)
+#define HPIPE_CFG_CURSOR_PRESET1_OFFSET		6
+#define HPIPE_CFG_CURSOR_PRESET1_MASK		\
+				(0x3f << HPIPE_CFG_CURSOR_PRESET1_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG1_REG		0x6ac
+#define HPIPE_CFG_CURSOR_PRESET2_OFFSET		0
+#define HPIPE_CFG_CURSOR_PRESET2_MASK		\
+				(0x3f << HPIPE_CFG_CURSOR_PRESET2_OFFSET)
+#define HPIPE_CFG_CURSOR_PRESET3_OFFSET		6
+#define HPIPE_CFG_CURSOR_PRESET3_MASK		\
+				(0x3f << HPIPE_CFG_CURSOR_PRESET3_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG2_REG		0x6b0
+#define HPIPE_CFG_CURSOR_PRESET4_OFFSET		0
+#define HPIPE_CFG_CURSOR_PRESET4_MASK		\
+				(0x3f << HPIPE_CFG_CURSOR_PRESET4_OFFSET)
+#define HPIPE_CFG_CURSOR_PRESET5_OFFSET		6
+#define HPIPE_CFG_CURSOR_PRESET5_MASK		\
+				(0x3f << HPIPE_CFG_CURSOR_PRESET5_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG3_REG		0x6b4
+#define HPIPE_CFG_CURSOR_PRESET6_OFFSET		0
+#define HPIPE_CFG_CURSOR_PRESET6_MASK		\
+				(0x3f << HPIPE_CFG_CURSOR_PRESET6_OFFSET)
+#define HPIPE_CFG_CURSOR_PRESET7_OFFSET		6
+#define HPIPE_CFG_CURSOR_PRESET7_MASK		\
+				(0x3f << HPIPE_CFG_CURSOR_PRESET7_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG4_REG		0x6b8
+#define HPIPE_CFG_CURSOR_PRESET8_OFFSET		0
+#define HPIPE_CFG_CURSOR_PRESET8_MASK		\
+				(0x3f << HPIPE_CFG_CURSOR_PRESET8_OFFSET)
+#define HPIPE_CFG_CURSOR_PRESET9_OFFSET		6
+#define HPIPE_CFG_CURSOR_PRESET9_MASK		\
+				(0x3f << HPIPE_CFG_CURSOR_PRESET9_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG5_REG		0x6bc
+#define HPIPE_CFG_CURSOR_PRESET10_OFFSET	0
+#define HPIPE_CFG_CURSOR_PRESET10_MASK		\
+				(0x3f << HPIPE_CFG_CURSOR_PRESET10_OFFSET)
+#define HPIPE_CFG_CURSOR_PRESET11_OFFSET	6
+#define HPIPE_CFG_CURSOR_PRESET11_MASK		\
+				(0x3f << HPIPE_CFG_CURSOR_PRESET11_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG6_REG		0x6c0
+#define HPIPE_CFG_PRE_CURSOR_PRESET0_OFFSET	0
+#define HPIPE_CFG_PRE_CURSOR_PRESET0_MASK	\
+				(0x3f << HPIPE_CFG_PRE_CURSOR_PRESET0_OFFSET)
+#define HPIPE_CFG_POST_CURSOR_PRESET0_OFFSET	6
+#define HPIPE_CFG_POST_CURSOR_PRESET0_MASK	\
+				(0x3f << HPIPE_CFG_POST_CURSOR_PRESET0_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG7_REG		0x6c4
+#define HPIPE_CFG_PRE_CURSOR_PRESET1_OFFSET	0
+#define HPIPE_CFG_PRE_CURSOR_PRESET1_MASK	\
+				(0x3f << HPIPE_CFG_PRE_CURSOR_PRESET1_OFFSET)
+#define HPIPE_CFG_POST_CURSOR_PRESET1_OFFSET	6
+#define HPIPE_CFG_POST_CURSOR_PRESET1_MASK	\
+				(0x3f << HPIPE_CFG_POST_CURSOR_PRESET1_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG8_REG		0x6c8
+#define HPIPE_CFG_PRE_CURSOR_PRESET2_OFFSET	0
+#define HPIPE_CFG_PRE_CURSOR_PRESET2_MASK	\
+				(0x3f << HPIPE_CFG_PRE_CURSOR_PRESET2_OFFSET)
+#define HPIPE_CFG_POST_CURSOR_PRESET2_OFFSET	6
+#define HPIPE_CFG_POST_CURSOR_PRESET2_MASK	\
+				(0x3f << HPIPE_CFG_POST_CURSOR_PRESET2_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG9_REG		0x6cc
+#define HPIPE_CFG_PRE_CURSOR_PRESET3_OFFSET	0
+#define HPIPE_CFG_PRE_CURSOR_PRESET3_MASK	\
+				(0x3f << HPIPE_CFG_PRE_CURSOR_PRESET3_OFFSET)
+#define HPIPE_CFG_POST_CURSOR_PRESET3_OFFSET	6
+#define HPIPE_CFG_POST_CURSOR_PRESET3_MASK	\
+				(0x3f << HPIPE_CFG_POST_CURSOR_PRESET3_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG10_REG		0x6d0
+#define HPIPE_CFG_PRE_CURSOR_PRESET4_OFFSET	0
+#define HPIPE_CFG_PRE_CURSOR_PRESET4_MASK	\
+				(0x3f << HPIPE_CFG_PRE_CURSOR_PRESET4_OFFSET)
+#define HPIPE_CFG_POST_CURSOR_PRESET4_OFFSET	6
+#define HPIPE_CFG_POST_CURSOR_PRESET4_MASK	\
+				(0x3f << HPIPE_CFG_POST_CURSOR_PRESET4_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG11_REG		0x6d4
+#define HPIPE_CFG_PRE_CURSOR_PRESET5_OFFSET	0
+#define HPIPE_CFG_PRE_CURSOR_PRESET5_MASK	\
+				(0x3f << HPIPE_CFG_PRE_CURSOR_PRESET5_OFFSET)
+#define HPIPE_CFG_POST_CURSOR_PRESET5_OFFSET	6
+#define HPIPE_CFG_POST_CURSOR_PRESET5_MASK	\
+				(0x3f << HPIPE_CFG_POST_CURSOR_PRESET5_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG12_REG		0x6d8
+#define HPIPE_CFG_PRE_CURSOR_PRESET6_OFFSET	0
+#define HPIPE_CFG_PRE_CURSOR_PRESET6_MASK	\
+				(0x3f << HPIPE_CFG_PRE_CURSOR_PRESET6_OFFSET)
+#define HPIPE_CFG_POST_CURSOR_PRESET6_OFFSET	6
+#define HPIPE_CFG_POST_CURSOR_PRESET6_MASK	\
+				(0x3f << HPIPE_CFG_POST_CURSOR_PRESET6_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG13_REG		0x6dc
+#define HPIPE_CFG_PRE_CURSOR_PRESET7_OFFSET	0
+#define HPIPE_CFG_PRE_CURSOR_PRESET7_MASK	\
+				(0x3f << HPIPE_CFG_PRE_CURSOR_PRESET7_OFFSET)
+#define HPIPE_CFG_POST_CURSOR_PRESET7_OFFSET	6
+#define HPIPE_CFG_POST_CURSOR_PRESET7_MASK	\
+				(0x3f << HPIPE_CFG_POST_CURSOR_PRESET7_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG14_REG		0x6e0
+#define HPIPE_CFG_PRE_CURSOR_PRESET8_OFFSET	0
+#define HPIPE_CFG_PRE_CURSOR_PRESET8_MASK	\
+				(0x3f << HPIPE_CFG_PRE_CURSOR_PRESET8_OFFSET)
+#define HPIPE_CFG_POST_CURSOR_PRESET8_OFFSET	6
+#define HPIPE_CFG_POST_CURSOR_PRESET8_MASK	\
+				(0x3f << HPIPE_CFG_POST_CURSOR_PRESET8_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG15_REG		0x6e4
+#define HPIPE_CFG_PRE_CURSOR_PRESET9_OFFSET	0
+#define HPIPE_CFG_PRE_CURSOR_PRESET9_MASK	\
+				(0x3f << HPIPE_CFG_PRE_CURSOR_PRESET9_OFFSET)
+#define HPIPE_CFG_POST_CURSOR_PRESET9_OFFSET	6
+#define HPIPE_CFG_POST_CURSOR_PRESET9_MASK	\
+				(0x3f << HPIPE_CFG_POST_CURSOR_PRESET9_OFFSET)
+
+#define HPIPE_LANE_PRESET_CFG16_REG		0x6e8
+#define HPIPE_CFG_PRE_CURSOR_PRESET10_OFFSET	0
+#define HPIPE_CFG_PRE_CURSOR_PRESET10_MASK	\
+				(0x3f << HPIPE_CFG_PRE_CURSOR_PRESET10_OFFSET)
+#define HPIPE_CFG_POST_CURSOR_PRESET10_OFFSET	6
+#define HPIPE_CFG_POST_CURSOR_PRESET10_MASK	\
+				(0x3f << HPIPE_CFG_POST_CURSOR_PRESET10_OFFSET)
+
+#define HPIPE_LANE_EQ_REMOTE_SETTING_REG	0x6f8
+#define HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_OFFSET	0
+#define HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_MASK	\
+				(0x1 << HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_OFFSET)
+#define HPIPE_LANE_CFG_FOM_ONLY_MODE_OFFFSET	1
+#define HPIPE_LANE_CFG_FOM_ONLY_MODE_MASK	\
+				(0x1 << HPIPE_LANE_CFG_FOM_ONLY_MODE_OFFFSET)
+#define HPIPE_LANE_CFG_FOM_PRESET_VECTOR_OFFSET	2
+#define HPIPE_LANE_CFG_FOM_PRESET_VECTOR_MASK	\
+				(0xf << HPIPE_LANE_CFG_FOM_PRESET_VECTOR_OFFSET)
+
+#define HPIPE_RST_CLK_CTRL_REG			0x704
+#define HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET	0
+#define HPIPE_RST_CLK_CTRL_PIPE_RST_MASK	\
+				(0x1 << HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET)
+#define HPIPE_RST_CLK_CTRL_FIXED_PCLK_OFFSET	2
+#define HPIPE_RST_CLK_CTRL_FIXED_PCLK_MASK	\
+				(0x1 << HPIPE_RST_CLK_CTRL_FIXED_PCLK_OFFSET)
+#define HPIPE_RST_CLK_CTRL_PIPE_WIDTH_OFFSET	3
+#define HPIPE_RST_CLK_CTRL_PIPE_WIDTH_MASK	\
+				(0x1 << HPIPE_RST_CLK_CTRL_PIPE_WIDTH_OFFSET)
+#define HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_OFFSET	9
+#define HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_MASK	\
+				(0x1 << HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_OFFSET)
+
+#define HPIPE_CLK_SRC_LO_REG				0x70c
+#define HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SEL_OFFSET	1
+#define HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SEL_MASK		\
+			(0x1 << HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SEL_OFFSET)
+#define HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SCALE_OFFSET	2
+#define HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SCALE_MASK	\
+			(0x3 << HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SCALE_OFFSET)
+#define HPIPE_CLK_SRC_LO_PLL_RDY_DL_OFFSET		5
+#define HPIPE_CLK_SRC_LO_PLL_RDY_DL_MASK		\
+			(0x7 << HPIPE_CLK_SRC_LO_PLL_RDY_DL_OFFSET)
+
+#define HPIPE_CLK_SRC_HI_REG			0x710
+#define HPIPE_CLK_SRC_HI_LANE_STRT_OFFSET	0
+#define HPIPE_CLK_SRC_HI_LANE_STRT_MASK		\
+			(0x1 << HPIPE_CLK_SRC_HI_LANE_STRT_OFFSET)
+#define HPIPE_CLK_SRC_HI_LANE_BREAK_OFFSET	1
+#define HPIPE_CLK_SRC_HI_LANE_BREAK_MASK	\
+			(0x1 << HPIPE_CLK_SRC_HI_LANE_BREAK_OFFSET)
+#define HPIPE_CLK_SRC_HI_LANE_MASTER_OFFSET	2
+#define HPIPE_CLK_SRC_HI_LANE_MASTER_MASK	\
+			(0x1 << HPIPE_CLK_SRC_HI_LANE_MASTER_OFFSET)
+#define HPIPE_CLK_SRC_HI_MODE_PIPE_OFFSET	7
+#define HPIPE_CLK_SRC_HI_MODE_PIPE_MASK		\
+			(0x1 << HPIPE_CLK_SRC_HI_MODE_PIPE_OFFSET)
+
+#define HPIPE_GLOBAL_PM_CTRL			0x740
+#define HPIPE_GLOBAL_PM_RXDLOZ_WAIT_OFFSET	0
+#define HPIPE_GLOBAL_PM_RXDLOZ_WAIT_MASK	\
+			(0xFF << HPIPE_GLOBAL_PM_RXDLOZ_WAIT_OFFSET)
+
+#endif /* COMPHY_H */
diff --git a/drivers/marvell/comphy/comphy-cp110.h b/drivers/marvell/comphy/comphy-cp110.h
new file mode 100644
index 0000000..af5c715
--- /dev/null
+++ b/drivers/marvell/comphy/comphy-cp110.h
@@ -0,0 +1,914 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* Marvell CP110 SoC COMPHY unit driver */
+
+#ifndef COMPHY_CP110_H
+#define COMPHY_CP110_H
+
+#define SD_ADDR(base, lane)			(base + 0x1000 * lane)
+#define HPIPE_ADDR(base, lane)			(SD_ADDR(base, lane) + 0x800)
+#define COMPHY_ADDR(base, lane)			(base + 0x28 * lane)
+
+#define MAX_NUM_OF_FFE				8
+#define RX_TRAINING_TIMEOUT			500
+
+/* Comphy registers */
+#define COMMON_PHY_CFG1_REG			0x0
+#define COMMON_PHY_CFG1_PWR_UP_OFFSET		1
+#define COMMON_PHY_CFG1_PWR_UP_MASK		\
+				(0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET)
+#define COMMON_PHY_CFG1_PIPE_SELECT_OFFSET	2
+#define COMMON_PHY_CFG1_PIPE_SELECT_MASK	\
+				(0x1 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET)
+#define COMMON_PHY_CFG1_CORE_RSTN_OFFSET	13
+#define COMMON_PHY_CFG1_CORE_RSTN_MASK		\
+				(0x1 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET)
+#define COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET	14
+#define COMMON_PHY_CFG1_PWR_ON_RESET_MASK	\
+				(0x1 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET)
+#define COMMON_PHY_PHY_MODE_OFFSET		15
+#define COMMON_PHY_PHY_MODE_MASK		\
+				(0x1 << COMMON_PHY_PHY_MODE_OFFSET)
+
+#define COMMON_PHY_CFG6_REG			0x14
+#define COMMON_PHY_CFG6_IF_40_SEL_OFFSET	18
+#define COMMON_PHY_CFG6_IF_40_SEL_MASK		\
+				(0x1 << COMMON_PHY_CFG6_IF_40_SEL_OFFSET)
+
+#define COMMON_PHY_CFG6_REG			0x14
+#define COMMON_PHY_CFG6_IF_40_SEL_OFFSET	18
+#define COMMON_PHY_CFG6_IF_40_SEL_MASK		\
+				(0x1 << COMMON_PHY_CFG6_IF_40_SEL_OFFSET)
+
+#define COMMON_SELECTOR_PHY_REG_OFFSET		0x140
+#define COMMON_SELECTOR_PIPE_REG_OFFSET		0x144
+#define COMMON_SELECTOR_COMPHY_MASK		0xf
+#define COMMON_SELECTOR_COMPHYN_FIELD_WIDTH	4
+#define COMMON_SELECTOR_COMPHYN_SATA		0x4
+#define COMMON_SELECTOR_PIPE_COMPHY_PCIE	0x4
+#define COMMON_SELECTOR_PIPE_COMPHY_USBH	0x1
+#define COMMON_SELECTOR_PIPE_COMPHY_USBD	0x2
+
+/* SGMII/Base-X/SFI/RXAUI */
+#define COMMON_SELECTOR_COMPHY0_1_2_NETWORK	0x1
+#define COMMON_SELECTOR_COMPHY3_RXAUI		0x1
+#define COMMON_SELECTOR_COMPHY3_SGMII		0x2
+#define COMMON_SELECTOR_COMPHY4_PORT1		0x1
+#define COMMON_SELECTOR_COMPHY4_ALL_OTHERS	0x2
+#define COMMON_SELECTOR_COMPHY5_RXAUI		0x2
+#define COMMON_SELECTOR_COMPHY5_SGMII		0x1
+
+#define COMMON_PHY_SD_CTRL1			0x148
+#define COMMON_PHY_SD_CTRL1_COMPHY_0_PORT_OFFSET	0
+#define COMMON_PHY_SD_CTRL1_COMPHY_1_PORT_OFFSET	4
+#define COMMON_PHY_SD_CTRL1_COMPHY_2_PORT_OFFSET	8
+#define COMMON_PHY_SD_CTRL1_COMPHY_3_PORT_OFFSET	12
+#define COMMON_PHY_SD_CTRL1_COMPHY_0_3_PORT_MASK	0xFFFF
+#define COMMON_PHY_SD_CTRL1_COMPHY_0_1_PORT_MASK	0xFF
+#define COMMON_PHY_SD_CTRL1_PCIE_X4_EN_OFFSET	24
+#define COMMON_PHY_SD_CTRL1_PCIE_X4_EN_MASK	\
+				(0x1 << COMMON_PHY_SD_CTRL1_PCIE_X4_EN_OFFSET)
+#define COMMON_PHY_SD_CTRL1_PCIE_X2_EN_OFFSET	25
+#define COMMON_PHY_SD_CTRL1_PCIE_X2_EN_MASK	\
+				(0x1 << COMMON_PHY_SD_CTRL1_PCIE_X2_EN_OFFSET)
+#define COMMON_PHY_SD_CTRL1_RXAUI1_OFFSET	26
+#define COMMON_PHY_SD_CTRL1_RXAUI1_MASK		\
+				(0x1 << COMMON_PHY_SD_CTRL1_RXAUI1_OFFSET)
+#define COMMON_PHY_SD_CTRL1_RXAUI0_OFFSET	27
+#define COMMON_PHY_SD_CTRL1_RXAUI0_MASK		\
+				(0x1 << COMMON_PHY_SD_CTRL1_RXAUI0_OFFSET)
+
+/* DFX register */
+#define DFX_BASE				(0x400000)
+#define DFX_DEV_GEN_CTRL12_REG			(0x280)
+#define DFX_DEV_GEN_PCIE_CLK_SRC_MUX		(0x3)
+#define DFX_DEV_GEN_PCIE_CLK_SRC_OFFSET		7
+#define DFX_DEV_GEN_PCIE_CLK_SRC_MASK		\
+				(0x3 << DFX_DEV_GEN_PCIE_CLK_SRC_OFFSET)
+
+/* SerDes IP registers */
+#define SD_EXTERNAL_CONFIG0_REG				0
+#define SD_EXTERNAL_CONFIG0_SD_PU_PLL_OFFSET		1
+#define SD_EXTERNAL_CONFIG0_SD_PU_PLL_MASK		\
+			(1 << SD_EXTERNAL_CONFIG0_SD_PU_PLL_OFFSET)
+#define SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_OFFSET	3
+#define SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_MASK		\
+			(0xf << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_OFFSET)
+#define SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_OFFSET	7
+#define SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_MASK		\
+			(0xf << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_OFFSET)
+#define SD_EXTERNAL_CONFIG0_SD_PU_RX_OFFSET		11
+#define SD_EXTERNAL_CONFIG0_SD_PU_RX_MASK		\
+			(1 << SD_EXTERNAL_CONFIG0_SD_PU_RX_OFFSET)
+#define SD_EXTERNAL_CONFIG0_SD_PU_TX_OFFSET		12
+#define SD_EXTERNAL_CONFIG0_SD_PU_TX_MASK		\
+			(1 << SD_EXTERNAL_CONFIG0_SD_PU_TX_OFFSET)
+#define SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_OFFSET	14
+#define SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_MASK		\
+			(1 << SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_OFFSET)
+#define SD_EXTERNAL_CONFIG0_MEDIA_MODE_OFFSET		15
+#define SD_EXTERNAL_CONFIG0_MEDIA_MODE_MASK		\
+			(0x1 << SD_EXTERNAL_CONFIG0_MEDIA_MODE_OFFSET)
+
+#define SD_EXTERNAL_CONFIG1_REG			0x4
+#define SD_EXTERNAL_CONFIG1_TX_IDLE_OFFSET	2
+#define SD_EXTERNAL_CONFIG1_TX_IDLE_MASK	\
+			(0x1 << SD_EXTERNAL_CONFIG1_TX_IDLE_OFFSET)
+#define SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET	3
+#define SD_EXTERNAL_CONFIG1_RESET_IN_MASK	\
+			(0x1 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET)
+#define SD_EXTERNAL_CONFIG1_RX_INIT_OFFSET	4
+#define SD_EXTERNAL_CONFIG1_RX_INIT_MASK	\
+			(0x1 << SD_EXTERNAL_CONFIG1_RX_INIT_OFFSET)
+#define SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET	5
+#define SD_EXTERNAL_CONFIG1_RESET_CORE_MASK	\
+			(0x1 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET)
+#define SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET	6
+#define SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK	\
+			(0x1 << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET)
+
+#define SD_EXTERNAL_CONFIG2_REG			0x8
+#define SD_EXTERNAL_CONFIG2_PIN_DFE_EN_OFFSET	4
+#define SD_EXTERNAL_CONFIG2_PIN_DFE_EN_MASK	\
+			(0x1 << SD_EXTERNAL_CONFIG2_PIN_DFE_EN_OFFSET)
+#define SD_EXTERNAL_CONFIG2_SSC_ENABLE_OFFSET	7
+#define SD_EXTERNAL_CONFIG2_SSC_ENABLE_MASK	\
+			(0x1 << SD_EXTERNAL_CONFIG2_SSC_ENABLE_OFFSET)
+
+#define SD_EXTERNAL_STATUS_REG				0xc
+#define SD_EXTERNAL_STATUS_START_RX_TRAINING_OFFSET	7
+#define SD_EXTERNAL_STATUS_START_RX_TRAINING_MASK	\
+			(1 << SD_EXTERNAL_STATUS_START_RX_TRAINING_OFFSET)
+
+#define SD_EXTERNAL_STATUS0_REG			0x18
+#define SD_EXTERNAL_STATUS0_PLL_TX_OFFSET	2
+#define SD_EXTERNAL_STATUS0_PLL_TX_MASK		\
+			(0x1 << SD_EXTERNAL_STATUS0_PLL_TX_OFFSET)
+#define SD_EXTERNAL_STATUS0_PLL_RX_OFFSET	3
+#define SD_EXTERNAL_STATUS0_PLL_RX_MASK		\
+			(0x1 << SD_EXTERNAL_STATUS0_PLL_RX_OFFSET)
+#define SD_EXTERNAL_STATUS0_RX_INIT_OFFSET	4
+#define SD_EXTERNAL_STATUS0_RX_INIT_MASK	\
+			(0x1 << SD_EXTERNAL_STATUS0_RX_INIT_OFFSET)
+
+#define SD_EXTERNAL_STATAUS1_REG			0x1c
+#define SD_EXTERNAL_STATAUS1_REG_RX_TRAIN_COMP_OFFSET	0
+#define SD_EXTERNAL_STATAUS1_REG_RX_TRAIN_COMP_MASK	\
+	(1 << SD_EXTERNAL_STATAUS1_REG_RX_TRAIN_COMP_OFFSET)
+#define SD_EXTERNAL_STATAUS1_REG_RX_TRAIN_FAILED_OFFSET	1
+#define SD_EXTERNAL_STATAUS1_REG_RX_TRAIN_FAILED_MASK	\
+	(1 << SD_EXTERNAL_STATAUS1_REG_RX_TRAIN_FAILED_OFFSET)
+
+/* HPIPE registers */
+#define HPIPE_PWR_PLL_REG			0x4
+#define HPIPE_PWR_PLL_REF_FREQ_OFFSET		0
+#define HPIPE_PWR_PLL_REF_FREQ_MASK		\
+			(0x1f << HPIPE_PWR_PLL_REF_FREQ_OFFSET)
+#define HPIPE_PWR_PLL_PHY_MODE_OFFSET		5
+#define HPIPE_PWR_PLL_PHY_MODE_MASK		\
+			(0x7 << HPIPE_PWR_PLL_PHY_MODE_OFFSET)
+
+#define HPIPE_CAL_REG1_REG			0xc
+#define HPIPE_CAL_REG_1_EXT_TXIMP_OFFSET	10
+#define HPIPE_CAL_REG_1_EXT_TXIMP_MASK		\
+			(0x1f << HPIPE_CAL_REG_1_EXT_TXIMP_OFFSET)
+#define HPIPE_CAL_REG_1_EXT_TXIMP_EN_OFFSET	15
+#define HPIPE_CAL_REG_1_EXT_TXIMP_EN_MASK	\
+			(0x1 << HPIPE_CAL_REG_1_EXT_TXIMP_EN_OFFSET)
+
+#define HPIPE_SQUELCH_FFE_SETTING_REG		0x18
+#define HPIPE_SQUELCH_THRESH_IN_OFFSET		8
+#define HPIPE_SQUELCH_THRESH_IN_MASK		\
+			(0xf << HPIPE_SQUELCH_THRESH_IN_OFFSET)
+#define HPIPE_SQUELCH_DETECTED_OFFSET		14
+#define HPIPE_SQUELCH_DETECTED_MASK		\
+			(0x1 << HPIPE_SQUELCH_DETECTED_OFFSET)
+
+#define HPIPE_DFE_REG0				0x1c
+#define HPIPE_DFE_RES_FORCE_OFFSET		15
+#define HPIPE_DFE_RES_FORCE_MASK		\
+			(0x1 << HPIPE_DFE_RES_FORCE_OFFSET)
+
+#define HPIPE_DFE_F3_F5_REG			0x28
+#define HPIPE_DFE_F3_F5_DFE_EN_OFFSET		14
+#define HPIPE_DFE_F3_F5_DFE_EN_MASK		\
+			(0x1 << HPIPE_DFE_F3_F5_DFE_EN_OFFSET)
+#define HPIPE_DFE_F3_F5_DFE_CTRL_OFFSET		15
+#define HPIPE_DFE_F3_F5_DFE_CTRL_MASK		\
+			(0x1 << HPIPE_DFE_F3_F5_DFE_CTRL_OFFSET)
+
+#define HPIPE_ADAPTED_DFE_COEFFICIENT_1_REG	0x30
+#define HPIPE_ADAPTED_DFE_RES_OFFSET		13
+#define HPIPE_ADAPTED_DFE_RES_MASK		\
+			(0x3 << HPIPE_ADAPTED_DFE_RES_OFFSET)
+
+#define HPIPE_G1_SET_0_REG			0x34
+#define HPIPE_G1_SET_0_G1_TX_AMP_OFFSET		1
+#define HPIPE_G1_SET_0_G1_TX_AMP_MASK		\
+			(0x1f << HPIPE_G1_SET_0_G1_TX_AMP_OFFSET)
+#define HPIPE_G1_SET_0_G1_TX_AMP_ADJ_OFFSET	6
+#define HPIPE_G1_SET_0_G1_TX_AMP_ADJ_MASK	\
+			(0x1 << HPIPE_G1_SET_0_G1_TX_AMP_ADJ_OFFSET)
+#define HPIPE_G1_SET_0_G1_TX_EMPH1_OFFSET	7
+#define HPIPE_G1_SET_0_G1_TX_EMPH1_MASK		\
+			(0xf << HPIPE_G1_SET_0_G1_TX_EMPH1_OFFSET)
+#define HPIPE_G1_SET_0_G1_TX_EMPH1_EN_OFFSET	11
+#define HPIPE_G1_SET_0_G1_TX_EMPH1_EN_MASK	\
+			(0x1 << HPIPE_G1_SET_0_G1_TX_EMPH1_EN_OFFSET)
+
+#define HPIPE_G1_SET_1_REG			0x38
+#define HPIPE_G1_SET_1_G1_RX_SELMUPI_OFFSET	0
+#define HPIPE_G1_SET_1_G1_RX_SELMUPI_MASK	\
+			(0x7 << HPIPE_G1_SET_1_G1_RX_SELMUPI_OFFSET)
+#define HPIPE_G1_SET_1_G1_RX_SELMUPF_OFFSET	3
+#define HPIPE_G1_SET_1_G1_RX_SELMUPF_MASK	\
+			(0x7 << HPIPE_G1_SET_1_G1_RX_SELMUPF_OFFSET)
+#define HPIPE_G1_SET_1_G1_RX_SELMUFI_OFFSET	6
+#define HPIPE_G1_SET_1_G1_RX_SELMUFI_MASK	\
+			(0x3 << HPIPE_G1_SET_1_G1_RX_SELMUFI_OFFSET)
+#define HPIPE_G1_SET_1_G1_RX_SELMUFF_OFFSET	8
+#define HPIPE_G1_SET_1_G1_RX_SELMUFF_MASK	\
+			(0x3 << HPIPE_G1_SET_1_G1_RX_SELMUFF_OFFSET)
+#define HPIPE_G1_SET_1_G1_RX_DFE_EN_OFFSET	10
+#define HPIPE_G1_SET_1_G1_RX_DFE_EN_MASK	\
+			(0x1 << HPIPE_G1_SET_1_G1_RX_DFE_EN_OFFSET)
+#define HPIPE_G1_SET_1_G1_RX_DIGCK_DIV_OFFSET	11
+#define HPIPE_G1_SET_1_G1_RX_DIGCK_DIV_MASK	\
+			(0x3 << HPIPE_G1_SET_1_G1_RX_DIGCK_DIV_OFFSET)
+
+#define HPIPE_G2_SET_0_REG			0x3c
+#define HPIPE_G2_SET_0_G2_TX_AMP_OFFSET		1
+#define HPIPE_G2_SET_0_G2_TX_AMP_MASK		\
+			(0x1f << HPIPE_G2_SET_0_G2_TX_AMP_OFFSET)
+#define HPIPE_G2_SET_0_G2_TX_AMP_ADJ_OFFSET	6
+#define HPIPE_G2_SET_0_G2_TX_AMP_ADJ_MASK	\
+			(0x1 << HPIPE_G2_SET_0_G2_TX_AMP_ADJ_OFFSET)
+#define HPIPE_G2_SET_0_G2_TX_EMPH1_OFFSET	7
+#define HPIPE_G2_SET_0_G2_TX_EMPH1_MASK		\
+			(0xf << HPIPE_G2_SET_0_G2_TX_EMPH1_OFFSET)
+#define HPIPE_G2_SET_0_G2_TX_EMPH1_EN_OFFSET	11
+#define HPIPE_G2_SET_0_G2_TX_EMPH1_EN_MASK	\
+			(0x1 << HPIPE_G2_SET_0_G2_TX_EMPH1_EN_OFFSET)
+
+#define HPIPE_G2_SET_1_REG			0x40
+#define HPIPE_G2_SET_1_G2_RX_SELMUPI_OFFSET	0
+#define HPIPE_G2_SET_1_G2_RX_SELMUPI_MASK	\
+			(0x7 << HPIPE_G2_SET_1_G2_RX_SELMUPI_OFFSET)
+#define HPIPE_G2_SET_1_G2_RX_SELMUPF_OFFSET	3
+#define HPIPE_G2_SET_1_G2_RX_SELMUPF_MASK	\
+			(0x7 << HPIPE_G2_SET_1_G2_RX_SELMUPF_OFFSET)
+#define HPIPE_G2_SET_1_G2_RX_SELMUFI_OFFSET	6
+#define HPIPE_G2_SET_1_G2_RX_SELMUFI_MASK	\
+			(0x3 << HPIPE_G2_SET_1_G2_RX_SELMUFI_OFFSET)
+#define HPIPE_G2_SET_1_G2_RX_SELMUFF_OFFSET	8
+#define HPIPE_G2_SET_1_G2_RX_SELMUFF_MASK	\
+			(0x3 << HPIPE_G2_SET_1_G2_RX_SELMUFF_OFFSET)
+#define HPIPE_G2_SET_1_G2_RX_DFE_EN_OFFSET	10
+#define HPIPE_G2_SET_1_G2_RX_DFE_EN_MASK	\
+			(0x1 << HPIPE_G2_SET_1_G2_RX_DFE_EN_OFFSET)
+#define HPIPE_G2_SET_1_G2_RX_DIGCK_DIV_OFFSET	11
+#define HPIPE_G2_SET_1_G2_RX_DIGCK_DIV_MASK	\
+			(0x3 << HPIPE_G2_SET_1_G2_RX_DIGCK_DIV_OFFSET)
+
+#define HPIPE_G3_SET_0_REG			0x44
+#define HPIPE_G3_SET_0_G3_TX_AMP_OFFSET		1
+#define HPIPE_G3_SET_0_G3_TX_AMP_MASK		\
+			(0x1f << HPIPE_G3_SET_0_G3_TX_AMP_OFFSET)
+#define HPIPE_G3_SET_0_G3_TX_AMP_ADJ_OFFSET	6
+#define HPIPE_G3_SET_0_G3_TX_AMP_ADJ_MASK	\
+			(0x1 << HPIPE_G3_SET_0_G3_TX_AMP_ADJ_OFFSET)
+#define HPIPE_G3_SET_0_G3_TX_EMPH1_OFFSET	7
+#define HPIPE_G3_SET_0_G3_TX_EMPH1_MASK		\
+			(0xf << HPIPE_G3_SET_0_G3_TX_EMPH1_OFFSET)
+#define HPIPE_G3_SET_0_G3_TX_EMPH1_EN_OFFSET	11
+#define HPIPE_G3_SET_0_G3_TX_EMPH1_EN_MASK	\
+			(0x1 << HPIPE_G3_SET_0_G3_TX_EMPH1_EN_OFFSET)
+#define HPIPE_G3_SET_0_G3_TX_SLEW_RATE_SEL_OFFSET 12
+#define HPIPE_G3_SET_0_G3_TX_SLEW_RATE_SEL_MASK	\
+			(0x7 << HPIPE_G3_SET_0_G3_TX_SLEW_RATE_SEL_OFFSET)
+#define HPIPE_G3_SET_0_G3_TX_SLEW_CTRL_EN_OFFSET 15
+#define HPIPE_G3_SET_0_G3_TX_SLEW_CTRL_EN_MASK	\
+			(0x1 << HPIPE_G3_SET_0_G3_TX_SLEW_CTRL_EN_OFFSET)
+
+#define HPIPE_G3_SET_1_REG				0x48
+#define HPIPE_G3_SET_1_G3_RX_SELMUPI_OFFSET		0
+#define HPIPE_G3_SET_1_G3_RX_SELMUPI_MASK		\
+			(0x7 << HPIPE_G3_SET_1_G3_RX_SELMUPI_OFFSET)
+#define HPIPE_G3_SET_1_G3_RX_SELMUPF_OFFSET		3
+#define HPIPE_G3_SET_1_G3_RX_SELMUPF_MASK		\
+			(0x7 << HPIPE_G3_SET_1_G3_RX_SELMUPF_OFFSET)
+#define HPIPE_G3_SET_1_G3_RX_SELMUFI_OFFSET		6
+#define HPIPE_G3_SET_1_G3_RX_SELMUFI_MASK		\
+			(0x3 << HPIPE_G3_SET_1_G3_RX_SELMUFI_OFFSET)
+#define HPIPE_G3_SET_1_G3_RX_SELMUFF_OFFSET		8
+#define HPIPE_G3_SET_1_G3_RX_SELMUFF_MASK		\
+			(0x3 << HPIPE_G3_SET_1_G3_RX_SELMUFF_OFFSET)
+#define HPIPE_G3_SET_1_G3_RX_DFE_EN_OFFSET		10
+#define HPIPE_G3_SET_1_G3_RX_DFE_EN_MASK		\
+			(0x1 << HPIPE_G3_SET_1_G3_RX_DFE_EN_OFFSET)
+#define HPIPE_G3_SET_1_G3_RX_DIGCK_DIV_OFFSET		11
+#define HPIPE_G3_SET_1_G3_RX_DIGCK_DIV_MASK		\
+			(0x3 << HPIPE_G3_SET_1_G3_RX_DIGCK_DIV_OFFSET)
+#define HPIPE_G3_SET_1_G3_SAMPLER_INPAIRX2_EN_OFFSET	13
+#define HPIPE_G3_SET_1_G3_SAMPLER_INPAIRX2_EN_MASK	\
+			(0x1 << HPIPE_G3_SET_1_G3_SAMPLER_INPAIRX2_EN_OFFSET)
+
+#define HPIPE_PHY_TEST_CONTROL_REG		0x54
+#define HPIPE_PHY_TEST_PATTERN_SEL_OFFSET	4
+#define HPIPE_PHY_TEST_PATTERN_SEL_MASK		\
+			(0xf << HPIPE_PHY_TEST_PATTERN_SEL_OFFSET)
+#define HPIPE_PHY_TEST_RESET_OFFSET		14
+#define HPIPE_PHY_TEST_RESET_MASK		\
+			(0x1 << HPIPE_PHY_TEST_RESET_OFFSET)
+#define HPIPE_PHY_TEST_EN_OFFSET		15
+#define HPIPE_PHY_TEST_EN_MASK			\
+			(0x1 << HPIPE_PHY_TEST_EN_OFFSET)
+
+#define HPIPE_PHY_TEST_DATA_REG			0x6c
+#define HPIPE_PHY_TEST_DATA_OFFSET		0
+#define HPIPE_PHY_TEST_DATA_MASK		\
+			(0xffff << HPIPE_PHY_TEST_DATA_OFFSET)
+
+#define HPIPE_PHY_TEST_PRBS_ERROR_COUNTER_1_REG	0x80
+
+#define HPIPE_PHY_TEST_OOB_0_REGISTER		0x84
+#define HPIPE_PHY_PT_OOB_EN_OFFSET		14
+#define HPIPE_PHY_PT_OOB_EN_MASK		\
+			(0x1 << HPIPE_PHY_PT_OOB_EN_OFFSET)
+#define HPIPE_PHY_TEST_PT_TESTMODE_OFFSET	12
+#define HPIPE_PHY_TEST_PT_TESTMODE_MASK		\
+			(0x3 << HPIPE_PHY_TEST_PT_TESTMODE_OFFSET)
+
+#define HPIPE_LOOPBACK_REG			0x8c
+#define HPIPE_LOOPBACK_SEL_OFFSET		1
+#define HPIPE_LOOPBACK_SEL_MASK			\
+			(0x7 << HPIPE_LOOPBACK_SEL_OFFSET)
+#define HPIPE_CDR_LOCK_OFFSET			7
+#define HPIPE_CDR_LOCK_MASK			\
+			(0x1 << HPIPE_CDR_LOCK_OFFSET)
+#define HPIPE_CDR_LOCK_DET_EN_OFFSET		8
+#define HPIPE_CDR_LOCK_DET_EN_MASK		\
+			(0x1 << HPIPE_CDR_LOCK_DET_EN_OFFSET)
+
+#define HPIPE_SYNC_PATTERN_REG			0x090
+#define HPIPE_SYNC_PATTERN_TXD_INV_OFFSET	10
+#define HPIPE_SYNC_PATTERN_TXD_INV_MASK	\
+	(0x1 << HPIPE_SYNC_PATTERN_TXD_INV_OFFSET)
+#define HPIPE_SYNC_PATTERN_RXD_INV_OFFSET	11
+#define HPIPE_SYNC_PATTERN_RXD_INV_MASK	\
+	(0x1 << HPIPE_SYNC_PATTERN_RXD_INV_OFFSET)
+
+#define HPIPE_INTERFACE_REG			0x94
+#define HPIPE_INTERFACE_GEN_MAX_OFFSET		10
+#define HPIPE_INTERFACE_GEN_MAX_MASK		\
+			(0x3 << HPIPE_INTERFACE_GEN_MAX_OFFSET)
+#define HPIPE_INTERFACE_DET_BYPASS_OFFSET	12
+#define HPIPE_INTERFACE_DET_BYPASS_MASK		\
+			(0x1 << HPIPE_INTERFACE_DET_BYPASS_OFFSET)
+#define HPIPE_INTERFACE_LINK_TRAIN_OFFSET	14
+#define HPIPE_INTERFACE_LINK_TRAIN_MASK		\
+			(0x1 << HPIPE_INTERFACE_LINK_TRAIN_OFFSET)
+
+#define HPIPE_G1_SET_2_REG			0xf4
+#define HPIPE_G1_SET_2_G1_TX_EMPH0_OFFSET	0
+#define HPIPE_G1_SET_2_G1_TX_EMPH0_MASK		\
+			(0xf << HPIPE_G1_SET_2_G1_TX_EMPH0_OFFSET)
+#define HPIPE_G1_SET_2_G1_TX_EMPH0_EN_OFFSET	4
+#define HPIPE_G1_SET_2_G1_TX_EMPH0_EN_MASK	\
+			(0x1 << HPIPE_G1_SET_2_G1_TX_EMPH0_EN_OFFSET)
+
+#define HPIPE_G2_SET_2_REG			0xf8
+#define HPIPE_G2_SET_2_G2_TX_EMPH0_OFFSET	0
+#define HPIPE_G2_SET_2_G2_TX_EMPH0_MASK		\
+			(0xf << HPIPE_G2_SET_2_G2_TX_EMPH0_OFFSET)
+#define HPIPE_G2_SET_2_G2_TX_EMPH0_EN_OFFSET	4
+#define HPIPE_G2_SET_2_G2_TX_EMPH0_EN_MASK	\
+			(0x1 << HPIPE_G2_SET_2_G2_TX_EMPH0_EN_OFFSET)
+#define HPIPE_G2_TX_SSC_AMP_OFFSET		9
+#define HPIPE_G2_TX_SSC_AMP_MASK		\
+			(0x7f << HPIPE_G2_TX_SSC_AMP_OFFSET)
+
+#define HPIPE_G3_SET_2_REG			0xfc
+#define HPIPE_G3_SET_2_G3_TX_EMPH0_OFFSET	0
+#define HPIPE_G3_SET_2_G3_TX_EMPH0_MASK		\
+			(0xf << HPIPE_G3_SET_2_G3_TX_EMPH0_OFFSET)
+#define HPIPE_G3_SET_2_G3_TX_EMPH0_EN_OFFSET	4
+#define HPIPE_G3_SET_2_G3_TX_EMPH0_EN_MASK	\
+			(0x1 << HPIPE_G3_SET_2_G3_TX_EMPH0_EN_OFFSET)
+#define HPIPE_G3_TX_SSC_AMP_OFFSET		9
+#define HPIPE_G3_TX_SSC_AMP_MASK		\
+			(0x7f << HPIPE_G3_TX_SSC_AMP_OFFSET)
+
+#define HPIPE_VDD_CAL_0_REG			0x108
+#define HPIPE_CAL_VDD_CONT_MODE_OFFSET		15
+#define HPIPE_CAL_VDD_CONT_MODE_MASK		\
+			(0x1 << HPIPE_CAL_VDD_CONT_MODE_OFFSET)
+
+#define HPIPE_VDD_CAL_CTRL_REG			0x114
+#define HPIPE_EXT_SELLV_RXSAMPL_OFFSET		5
+#define HPIPE_EXT_SELLV_RXSAMPL_MASK		\
+			(0x1f << HPIPE_EXT_SELLV_RXSAMPL_OFFSET)
+
+#define HPIPE_PCIE_REG0				0x120
+#define HPIPE_PCIE_IDLE_SYNC_OFFSET		12
+#define HPIPE_PCIE_IDLE_SYNC_MASK		\
+			(0x1 << HPIPE_PCIE_IDLE_SYNC_OFFSET)
+#define HPIPE_PCIE_SEL_BITS_OFFSET		13
+#define HPIPE_PCIE_SEL_BITS_MASK		\
+			(0x3 << HPIPE_PCIE_SEL_BITS_OFFSET)
+
+#define HPIPE_LANE_ALIGN_REG			0x124
+#define HPIPE_LANE_ALIGN_OFF_OFFSET		12
+#define HPIPE_LANE_ALIGN_OFF_MASK		\
+			(0x1 << HPIPE_LANE_ALIGN_OFF_OFFSET)
+
+#define HPIPE_MISC_REG				0x13C
+#define HPIPE_MISC_CLK100M_125M_OFFSET		4
+#define HPIPE_MISC_CLK100M_125M_MASK		\
+			(0x1 << HPIPE_MISC_CLK100M_125M_OFFSET)
+#define HPIPE_MISC_ICP_FORCE_OFFSET		5
+#define HPIPE_MISC_ICP_FORCE_MASK		\
+			(0x1 << HPIPE_MISC_ICP_FORCE_OFFSET)
+#define HPIPE_MISC_TXDCLK_2X_OFFSET		6
+#define HPIPE_MISC_TXDCLK_2X_MASK		\
+			(0x1 << HPIPE_MISC_TXDCLK_2X_OFFSET)
+#define HPIPE_MISC_CLK500_EN_OFFSET		7
+#define HPIPE_MISC_CLK500_EN_MASK		\
+			(0x1 << HPIPE_MISC_CLK500_EN_OFFSET)
+#define HPIPE_MISC_REFCLK_SEL_OFFSET		10
+#define HPIPE_MISC_REFCLK_SEL_MASK		\
+			(0x1 << HPIPE_MISC_REFCLK_SEL_OFFSET)
+
+#define HPIPE_RX_CONTROL_1_REG			0x140
+#define HPIPE_RX_CONTROL_1_RXCLK2X_SEL_OFFSET	11
+#define HPIPE_RX_CONTROL_1_RXCLK2X_SEL_MASK	\
+			(0x1 << HPIPE_RX_CONTROL_1_RXCLK2X_SEL_OFFSET)
+#define HPIPE_RX_CONTROL_1_CLK8T_EN_OFFSET	12
+#define HPIPE_RX_CONTROL_1_CLK8T_EN_MASK	\
+			(0x1 << HPIPE_RX_CONTROL_1_CLK8T_EN_OFFSET)
+
+#define HPIPE_PWR_CTR_REG			0x148
+#define HPIPE_PWR_CTR_RST_DFE_OFFSET		0
+#define HPIPE_PWR_CTR_RST_DFE_MASK		\
+			(0x1 << HPIPE_PWR_CTR_RST_DFE_OFFSET)
+#define HPIPE_PWR_CTR_SFT_RST_OFFSET		10
+#define HPIPE_PWR_CTR_SFT_RST_MASK		\
+			(0x1 << HPIPE_PWR_CTR_SFT_RST_OFFSET)
+
+#define HPIPE_SPD_DIV_FORCE_REG				0x154
+#define HPIPE_TXDIGCK_DIV_FORCE_OFFSET			7
+#define HPIPE_TXDIGCK_DIV_FORCE_MASK			\
+			(0x1 << HPIPE_TXDIGCK_DIV_FORCE_OFFSET)
+#define HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_OFFSET		8
+#define HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_MASK		\
+			(0x3 << HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_OFFSET)
+#define HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_OFFSET	10
+#define HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_MASK	\
+			(0x1 << HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_OFFSET)
+#define HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_OFFSET		13
+#define HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_MASK		\
+			(0x3 << HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_OFFSET)
+#define HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_OFFSET	15
+#define HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_MASK	\
+			(0x1 << HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_OFFSET)
+
+/* HPIPE_RX_CLK_ALIGN90_AND_TX_IDLE_CALIBRATION_CTRL_REG */
+#define HPIPE_RX_CLK_ALIGN90_AND_TX_IDLE_CALIB_CTRL_REG	0x168
+#define HPIPE_CAL_RXCLKALIGN_90_EXT_EN_OFFSET		15
+#define HPIPE_CAL_RXCLKALIGN_90_EXT_EN_MASK		\
+			(0x1 << HPIPE_CAL_RXCLKALIGN_90_EXT_EN_OFFSET)
+#define HPIPE_CAL_OS_PH_EXT_OFFSET			8
+#define HPIPE_CAL_OS_PH_EXT_MASK			\
+			(0x7f << HPIPE_CAL_OS_PH_EXT_OFFSET)
+
+#define HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG	0x16C
+#define HPIPE_RX_SAMPLER_OS_GAIN_OFFSET		6
+#define HPIPE_RX_SAMPLER_OS_GAIN_MASK		\
+			(0x3 << HPIPE_RX_SAMPLER_OS_GAIN_OFFSET)
+#define HPIPE_SMAPLER_OFFSET			12
+#define HPIPE_SMAPLER_MASK			\
+			(0x1 << HPIPE_SMAPLER_OFFSET)
+
+#define HPIPE_TX_REG1_REG			0x174
+#define HPIPE_TX_REG1_TX_EMPH_RES_OFFSET	5
+#define HPIPE_TX_REG1_TX_EMPH_RES_MASK		\
+			(0x3 << HPIPE_TX_REG1_TX_EMPH_RES_OFFSET)
+#define HPIPE_TX_REG1_SLC_EN_OFFSET		10
+#define HPIPE_TX_REG1_SLC_EN_MASK		\
+			(0x3f << HPIPE_TX_REG1_SLC_EN_OFFSET)
+
+#define HPIPE_PWR_CTR_DTL_REG				0x184
+#define HPIPE_PWR_CTR_DTL_SQ_DET_EN_OFFSET		0
+#define HPIPE_PWR_CTR_DTL_SQ_DET_EN_MASK		\
+			(0x1 << HPIPE_PWR_CTR_DTL_SQ_DET_EN_OFFSET)
+#define HPIPE_PWR_CTR_DTL_SQ_PLOOP_EN_OFFSET		1
+#define HPIPE_PWR_CTR_DTL_SQ_PLOOP_EN_MASK		\
+			(0x1 << HPIPE_PWR_CTR_DTL_SQ_PLOOP_EN_OFFSET)
+#define HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET		2
+#define HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK			\
+			(0x1 << HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET)
+#define HPIPE_PWR_CTR_DTL_CLAMPING_SEL_OFFSET		4
+#define HPIPE_PWR_CTR_DTL_CLAMPING_SEL_MASK		\
+			(0x7 << HPIPE_PWR_CTR_DTL_CLAMPING_SEL_OFFSET)
+#define HPIPE_PWR_CTR_DTL_INTPCLK_DIV_FORCE_OFFSET	10
+#define HPIPE_PWR_CTR_DTL_INTPCLK_DIV_FORCE_MASK	\
+			(0x1 << HPIPE_PWR_CTR_DTL_INTPCLK_DIV_FORCE_OFFSET)
+#define HPIPE_PWR_CTR_DTL_CLK_MODE_OFFSET		12
+#define HPIPE_PWR_CTR_DTL_CLK_MODE_MASK			\
+			(0x3 << HPIPE_PWR_CTR_DTL_CLK_MODE_OFFSET)
+#define HPIPE_PWR_CTR_DTL_CLK_MODE_FORCE_OFFSET		14
+#define HPIPE_PWR_CTR_DTL_CLK_MODE_FORCE_MASK		\
+			(1 << HPIPE_PWR_CTR_DTL_CLK_MODE_FORCE_OFFSET)
+
+#define HPIPE_PHASE_CONTROL_REG			0x188
+#define HPIPE_OS_PH_OFFSET_OFFSET		0
+#define HPIPE_OS_PH_OFFSET_MASK			\
+			(0x7f << HPIPE_OS_PH_OFFSET_OFFSET)
+#define HPIPE_OS_PH_OFFSET_FORCE_OFFSET		7
+#define HPIPE_OS_PH_OFFSET_FORCE_MASK		\
+			(0x1 << HPIPE_OS_PH_OFFSET_FORCE_OFFSET)
+#define HPIPE_OS_PH_VALID_OFFSET		8
+#define HPIPE_OS_PH_VALID_MASK			\
+			(0x1 << HPIPE_OS_PH_VALID_OFFSET)
+
+#define HPIPE_DATA_PHASE_OFF_CTRL_REG			0x1A0
+#define HPIPE_DATA_PHASE_ADAPTED_OS_PH_OFFSET		9
+#define HPIPE_DATA_PHASE_ADAPTED_OS_PH_MASK		\
+			(0x7f << HPIPE_DATA_PHASE_ADAPTED_OS_PH_OFFSET)
+
+#define HPIPE_ADAPTED_FFE_CAPACITOR_COUNTER_CTRL_REG	0x1A4
+#define HPIPE_ADAPTED_FFE_ADAPTED_FFE_RES_OFFSET	12
+#define HPIPE_ADAPTED_FFE_ADAPTED_FFE_RES_MASK		\
+			(0x3 << HPIPE_ADAPTED_FFE_ADAPTED_FFE_RES_OFFSET)
+#define HPIPE_ADAPTED_FFE_ADAPTED_FFE_CAP_OFFSET	8
+#define HPIPE_ADAPTED_FFE_ADAPTED_FFE_CAP_MASK		\
+			(0xf << HPIPE_ADAPTED_FFE_ADAPTED_FFE_CAP_OFFSET)
+
+#define HPIPE_SQ_GLITCH_FILTER_CTRL		0x1c8
+#define HPIPE_SQ_DEGLITCH_WIDTH_P_OFFSET	0
+#define HPIPE_SQ_DEGLITCH_WIDTH_P_MASK		\
+			(0xf << HPIPE_SQ_DEGLITCH_WIDTH_P_OFFSET)
+#define HPIPE_SQ_DEGLITCH_WIDTH_N_OFFSET	4
+#define HPIPE_SQ_DEGLITCH_WIDTH_N_MASK		\
+			(0xf << HPIPE_SQ_DEGLITCH_WIDTH_N_OFFSET)
+#define HPIPE_SQ_DEGLITCH_EN_OFFSET		8
+#define HPIPE_SQ_DEGLITCH_EN_MASK		\
+			(0x1 << HPIPE_SQ_DEGLITCH_EN_OFFSET)
+
+#define HPIPE_FRAME_DETECT_CTRL_0_REG		0x214
+#define HPIPE_TRAIN_PAT_NUM_OFFSET		0x7
+#define HPIPE_TRAIN_PAT_NUM_MASK		\
+			(0x1FF << HPIPE_TRAIN_PAT_NUM_OFFSET)
+
+#define HPIPE_FRAME_DETECT_CTRL_3_REG			0x220
+#define HPIPE_PATTERN_LOCK_LOST_TIMEOUT_EN_OFFSET	12
+#define HPIPE_PATTERN_LOCK_LOST_TIMEOUT_EN_MASK		\
+			(0x1 << HPIPE_PATTERN_LOCK_LOST_TIMEOUT_EN_OFFSET)
+
+#define HPIPE_DME_REG				0x228
+#define HPIPE_DME_ETHERNET_MODE_OFFSET		7
+#define HPIPE_DME_ETHERNET_MODE_MASK		\
+			(0x1 << HPIPE_DME_ETHERNET_MODE_OFFSET)
+
+#define HPIPE_TRX_TRAIN_CTRL_0_REG		0x22c
+#define HPIPE_TRX_TX_F0T_EO_BASED_OFFSET	14
+#define HPIPE_TRX_TX_F0T_EO_BASED_MASK		\
+			(1 << HPIPE_TRX_TX_F0T_EO_BASED_OFFSET)
+#define HPIPE_TRX_UPDATE_THEN_HOLD_OFFSET	6
+#define HPIPE_TRX_UPDATE_THEN_HOLD_MASK		\
+			(1 << HPIPE_TRX_UPDATE_THEN_HOLD_OFFSET)
+#define HPIPE_TRX_TX_CTRL_CLK_EN_OFFSET		5
+#define HPIPE_TRX_TX_CTRL_CLK_EN_MASK		\
+			(1 << HPIPE_TRX_TX_CTRL_CLK_EN_OFFSET)
+#define HPIPE_TRX_RX_ANA_IF_CLK_ENE_OFFSET	4
+#define HPIPE_TRX_RX_ANA_IF_CLK_ENE_MASK	\
+			(1 << HPIPE_TRX_RX_ANA_IF_CLK_ENE_OFFSET)
+#define HPIPE_TRX_TX_TRAIN_EN_OFFSET		1
+#define HPIPE_TRX_TX_TRAIN_EN_MASK		\
+			(1 << HPIPE_TRX_TX_TRAIN_EN_OFFSET)
+#define HPIPE_TRX_RX_TRAIN_EN_OFFSET		0
+#define HPIPE_TRX_RX_TRAIN_EN_MASK		\
+			(1 << HPIPE_TRX_RX_TRAIN_EN_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_0_REG		0x268
+#define HPIPE_TX_TRAIN_P2P_HOLD_OFFSET		15
+#define HPIPE_TX_TRAIN_P2P_HOLD_MASK		\
+			(0x1 << HPIPE_TX_TRAIN_P2P_HOLD_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_REG			0x26C
+#define HPIPE_TX_TRAIN_CTRL_G1_OFFSET		0
+#define HPIPE_TX_TRAIN_CTRL_G1_MASK		\
+			(0x1 << HPIPE_TX_TRAIN_CTRL_G1_OFFSET)
+#define HPIPE_TX_TRAIN_CTRL_GN1_OFFSET		1
+#define HPIPE_TX_TRAIN_CTRL_GN1_MASK		\
+			(0x1 << HPIPE_TX_TRAIN_CTRL_GN1_OFFSET)
+#define HPIPE_TX_TRAIN_CTRL_G0_OFFSET		2
+#define HPIPE_TX_TRAIN_CTRL_G0_MASK		\
+			(0x1 << HPIPE_TX_TRAIN_CTRL_G0_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_4_REG		0x278
+#define HPIPE_TRX_TRAIN_TIMER_OFFSET		0
+#define HPIPE_TRX_TRAIN_TIMER_MASK		\
+			(0x3FF << HPIPE_TRX_TRAIN_TIMER_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_5_REG		0x2A4
+#define HPIPE_RX_TRAIN_TIMER_OFFSET		0
+#define HPIPE_RX_TRAIN_TIMER_MASK		\
+			(0x3ff << HPIPE_RX_TRAIN_TIMER_OFFSET)
+#define HPIPE_TX_TRAIN_START_SQ_EN_OFFSET	11
+#define HPIPE_TX_TRAIN_START_SQ_EN_MASK		\
+			(0x1 << HPIPE_TX_TRAIN_START_SQ_EN_OFFSET)
+#define HPIPE_TX_TRAIN_START_FRM_DET_EN_OFFSET	12
+#define HPIPE_TX_TRAIN_START_FRM_DET_EN_MASK	\
+			(0x1 << HPIPE_TX_TRAIN_START_FRM_DET_EN_OFFSET)
+#define HPIPE_TX_TRAIN_START_FRM_LOCK_EN_OFFSET	13
+#define HPIPE_TX_TRAIN_START_FRM_LOCK_EN_MASK	\
+			(0x1 << HPIPE_TX_TRAIN_START_FRM_LOCK_EN_OFFSET)
+#define HPIPE_TX_TRAIN_WAIT_TIME_EN_OFFSET	14
+#define HPIPE_TX_TRAIN_WAIT_TIME_EN_MASK	\
+			(0x1 << HPIPE_TX_TRAIN_WAIT_TIME_EN_OFFSET)
+
+#define HPIPE_INTERRUPT_1_REGISTER		0x2AC
+#define HPIPE_TRX_TRAIN_FAILED_OFFSET		6
+#define HPIPE_TRX_TRAIN_FAILED_MASK		\
+			(1 << HPIPE_TRX_TRAIN_FAILED_OFFSET)
+#define HPIPE_TRX_TRAIN_TIME_OUT_INT_OFFSET	5
+#define HPIPE_TRX_TRAIN_TIME_OUT_INT_MASK	\
+			(1 << HPIPE_TRX_TRAIN_TIME_OUT_INT_OFFSET)
+#define HPIPE_INTERRUPT_TRX_TRAIN_DONE_OFFSET	4
+#define HPIPE_INTERRUPT_TRX_TRAIN_DONE_MASK	\
+			(1 << HPIPE_INTERRUPT_TRX_TRAIN_DONE_OFFSET)
+#define HPIPE_INTERRUPT_DFE_DONE_INT_OFFSET	3
+#define HPIPE_INTERRUPT_DFE_DONE_INT_MASK	\
+			(1 << HPIPE_INTERRUPT_DFE_DONE_INT_OFFSET)
+#define HPIPE_INTERRUPT_RX_TRAIN_COMPLETE_INT_OFFSET	1
+#define HPIPE_INTERRUPT_RX_TRAIN_COMPLETE_INT_MASK	\
+			(1 << HPIPE_INTERRUPT_RX_TRAIN_COMPLETE_INT_OFFSET)
+
+#define HPIPE_TX_TRAIN_REG			0x31C
+#define HPIPE_TX_TRAIN_CHK_INIT_OFFSET		4
+#define HPIPE_TX_TRAIN_CHK_INIT_MASK		\
+			(0x1 << HPIPE_TX_TRAIN_CHK_INIT_OFFSET)
+#define HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_OFFSET	7
+#define HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_MASK	\
+			(0x1 << HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_OFFSET)
+#define HPIPE_TX_TRAIN_16BIT_AUTO_EN_OFFSET	8
+#define HPIPE_TX_TRAIN_16BIT_AUTO_EN_MASK	\
+			(0x1 << HPIPE_TX_TRAIN_16BIT_AUTO_EN_OFFSET)
+#define HPIPE_TX_TRAIN_PAT_SEL_OFFSET		9
+#define HPIPE_TX_TRAIN_PAT_SEL_MASK		\
+			(0x1 << HPIPE_TX_TRAIN_PAT_SEL_OFFSET)
+
+#define HPIPE_SAVED_DFE_VALUES_REG		0x328
+#define HPIPE_SAVED_DFE_VALUES_SAV_F0D_OFFSET	10
+#define HPIPE_SAVED_DFE_VALUES_SAV_F0D_MASK	\
+			(0x3f << HPIPE_SAVED_DFE_VALUES_SAV_F0D_OFFSET)
+
+#define HPIPE_CDR_CONTROL_REG			0x418
+#define HPIPE_CRD_MIDPOINT_PHASE_OS_OFFSET	0
+#define HPIPE_CRD_MIDPOINT_PHASE_OS_MASK	\
+			(0x3f << HPIPE_CRD_MIDPOINT_PHASE_OS_OFFSET)
+#define HPIPE_CDR_MAX_DFE_ADAPT_1_OFFSET	6
+#define HPIPE_CDR_MAX_DFE_ADAPT_1_MASK		\
+			(0x7 << HPIPE_CDR_MAX_DFE_ADAPT_1_OFFSET)
+#define HPIPE_CDR_MAX_DFE_ADAPT_0_OFFSET	9
+#define HPIPE_CDR_MAX_DFE_ADAPT_0_MASK		\
+			(0x7 << HPIPE_CDR_MAX_DFE_ADAPT_0_OFFSET)
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_1_OFFSET	12
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_1_MASK	\
+			(0x3 << HPIPE_CDR_RX_MAX_DFE_ADAPT_1_OFFSET)
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_0_OFFSET	14
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_0_MASK	\
+			(0x3 << HPIPE_CDR_RX_MAX_DFE_ADAPT_0_OFFSET)
+
+
+#define HPIPE_CDR_CONTROL1_REG			0x41c
+#define HPIPE_CRD2_CRD_MIDPOINT_SMALL_THRES_K_OFF	12
+#define HPIPE_CRD2_CRD_MIDPOINT_SMALL_THRES_K_MASK	\
+			(0xf << HPIPE_CRD2_CRD_MIDPOINT_SMALL_THRES_K_OFF)
+
+#define HPIPE_CDR_CONTROL2_REG			0x420
+#define HPIPE_CRD2_CRD_MIDPOINT_LARGE_THRES_K_OFF	12
+#define HPIPE_CRD2_CRD_MIDPOINT_LARGE_THRES_K_MASK	\
+			(0xf << HPIPE_CRD2_CRD_MIDPOINT_LARGE_THRES_K_OFF)
+
+#define HPIPE_TX_TRAIN_CTRL_11_REG		0x438
+#define HPIPE_TX_STATUS_CHECK_MODE_OFFSET	6
+#define HPIPE_TX_TX_STATUS_CHECK_MODE_MASK	\
+			(0x1 << HPIPE_TX_STATUS_CHECK_MODE_OFFSET)
+#define HPIPE_TX_NUM_OF_PRESET_OFFSET		10
+#define HPIPE_TX_NUM_OF_PRESET_MASK		\
+			(0x7 << HPIPE_TX_NUM_OF_PRESET_OFFSET)
+#define HPIPE_TX_SWEEP_PRESET_EN_OFFSET		15
+#define HPIPE_TX_SWEEP_PRESET_EN_MASK		\
+			(0x1 << HPIPE_TX_SWEEP_PRESET_EN_OFFSET)
+
+#define HPIPE_G1_SETTINGS_3_REG				0x440
+#define HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_OFFSET	0
+#define HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_MASK		\
+			(0xf << HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_OFFSET)
+#define HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_OFFSET	4
+#define HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_MASK		\
+			(0x7 << HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_OFFSET)
+#define HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_OFFSET	7
+#define HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_MASK	\
+			(0x1 << HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_OFFSET)
+#define HPIPE_G1_SETTINGS_3_G1_FBCK_SEL_OFFSET		9
+#define HPIPE_G1_SETTINGS_3_G1_FBCK_SEL_MASK		\
+			(0x1 << HPIPE_G1_SETTINGS_3_G1_FBCK_SEL_OFFSET)
+#define HPIPE_G1_SETTINGS_3_G1_FFE_DEG_RES_LEVEL_OFFSET	12
+#define HPIPE_G1_SETTINGS_3_G1_FFE_DEG_RES_LEVEL_MASK	\
+			(0x3 << HPIPE_G1_SETTINGS_3_G1_FFE_DEG_RES_LEVEL_OFFSET)
+#define HPIPE_G1_SETTINGS_3_G1_FFE_LOAD_RES_LEVEL_OFFSET	14
+#define HPIPE_G1_SETTINGS_3_G1_FFE_LOAD_RES_LEVEL_MASK	\
+		(0x3 << HPIPE_G1_SETTINGS_3_G1_FFE_LOAD_RES_LEVEL_OFFSET)
+
+#define HPIPE_G1_SETTINGS_4_REG			0x444
+#define HPIPE_G1_SETTINGS_4_G1_DFE_RES_OFFSET	8
+#define HPIPE_G1_SETTINGS_4_G1_DFE_RES_MASK	\
+			(0x3 << HPIPE_G1_SETTINGS_4_G1_DFE_RES_OFFSET)
+
+#define HPIPE_G2_SETTINGS_4_REG			0x44c
+#define HPIPE_G2_DFE_RES_OFFSET			8
+#define HPIPE_G2_DFE_RES_MASK			\
+			(0x3 << HPIPE_G2_DFE_RES_OFFSET)
+
+#define HPIPE_G3_SETTING_3_REG			0x450
+#define HPIPE_G3_FFE_CAP_SEL_OFFSET		0
+#define HPIPE_G3_FFE_CAP_SEL_MASK		\
+			(0xf << HPIPE_G3_FFE_CAP_SEL_OFFSET)
+#define HPIPE_G3_FFE_RES_SEL_OFFSET		4
+#define HPIPE_G3_FFE_RES_SEL_MASK		\
+			(0x7 << HPIPE_G3_FFE_RES_SEL_OFFSET)
+#define HPIPE_G3_FFE_SETTING_FORCE_OFFSET	7
+#define HPIPE_G3_FFE_SETTING_FORCE_MASK		\
+			(0x1 << HPIPE_G3_FFE_SETTING_FORCE_OFFSET)
+#define HPIPE_G3_FFE_DEG_RES_LEVEL_OFFSET	12
+#define HPIPE_G3_FFE_DEG_RES_LEVEL_MASK		\
+			(0x3 << HPIPE_G3_FFE_DEG_RES_LEVEL_OFFSET)
+#define HPIPE_G3_FFE_LOAD_RES_LEVEL_OFFSET	14
+#define HPIPE_G3_FFE_LOAD_RES_LEVEL_MASK	\
+			(0x3 << HPIPE_G3_FFE_LOAD_RES_LEVEL_OFFSET)
+
+#define HPIPE_G3_SETTING_4_REG			0x454
+#define HPIPE_G3_DFE_RES_OFFSET			8
+#define HPIPE_G3_DFE_RES_MASK			(0x3 << HPIPE_G3_DFE_RES_OFFSET)
+
+#define HPIPE_TX_PRESET_INDEX_REG		0x468
+#define HPIPE_TX_PRESET_INDEX_OFFSET		0
+#define HPIPE_TX_PRESET_INDEX_MASK		\
+			(0xf << HPIPE_TX_PRESET_INDEX_OFFSET)
+
+#define HPIPE_DFE_CONTROL_REG			0x470
+#define HPIPE_DFE_TX_MAX_DFE_ADAPT_OFFSET	14
+#define HPIPE_DFE_TX_MAX_DFE_ADAPT_MASK		\
+			(0x3 << HPIPE_DFE_TX_MAX_DFE_ADAPT_OFFSET)
+
+#define HPIPE_DFE_CTRL_28_REG			0x49C
+#define HPIPE_DFE_CTRL_28_PIPE4_OFFSET		7
+#define HPIPE_DFE_CTRL_28_PIPE4_MASK		\
+			(0x1 << HPIPE_DFE_CTRL_28_PIPE4_OFFSET)
+
+#define HPIPE_TRX0_REG				0x4cc /*in doc 0x133*4*/
+#define HPIPE_TRX0_GAIN_TRAIN_WITH_SAMPLER_OFF	2
+#define HPIPE_TRX0_GAIN_TRAIN_WITH_SAMPLER_MASK \
+			(0x1 << HPIPE_TRX0_GAIN_TRAIN_WITH_SAMPLER_OFF)
+#define HPIPE_TRX0_GAIN_TRAIN_WITH_C_OFF	0
+#define HPIPE_TRX0_GAIN_TRAIN_WITH_C_MASK	\
+			(0x1 << HPIPE_TRX0_GAIN_TRAIN_WITH_C_OFF)
+
+#define HPIPE_TRX_REG1				0x4d0 /*in doc 0x134*4*/
+#define HPIPE_TRX_REG1_MIN_BOOST_MODE_OFF	3
+#define HPIPE_TRX_REG1_MIN_BOOST_MODE_MASK	\
+			(0x1 << HPIPE_TRX_REG1_MIN_BOOST_MODE_OFF)
+#define HPIPE_TRX_REG1_SUMFTAP_EN_OFF		10
+#define HPIPE_TRX_REG1_SUMFTAP_EN_MASK		\
+			(0x3f << HPIPE_TRX_REG1_SUMFTAP_EN_OFF)
+
+#define HPIPE_TRX_REG2				0x4d8 /*in doc 0x136*4*/
+#define HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_OFF	11
+#define HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_MASK	\
+			(0x1f << HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_OFF)
+#define HPIPE_TRX_REG2_SUMF_BOOST_TARGET_K_OFF	7
+#define HPIPE_TRX_REG2_SUMF_BOOST_TARGET_K_MASK	\
+			(0xf << HPIPE_TRX_REG2_SUMF_BOOST_TARGET_K_OFF)
+
+#define HPIPE_G1_SETTING_5_REG			0x538
+#define HPIPE_G1_SETTING_5_G1_ICP_OFFSET	0
+#define HPIPE_G1_SETTING_5_G1_ICP_MASK		\
+			(0xf << HPIPE_G1_SETTING_5_G1_ICP_OFFSET)
+
+#define HPIPE_G3_SETTING_5_REG			0x548
+#define HPIPE_G3_SETTING_5_G3_ICP_OFFSET	0
+#define HPIPE_G3_SETTING_5_G3_ICP_MASK		\
+			(0xf << HPIPE_G3_SETTING_5_G3_ICP_OFFSET)
+
+#define HPIPE_LANE_CONFIG0_REG			0x600
+#define HPIPE_LANE_CONFIG0_TXDEEMPH0_OFFSET	0
+#define HPIPE_LANE_CONFIG0_TXDEEMPH0_MASK	\
+			(0x1 << HPIPE_LANE_CONFIG0_TXDEEMPH0_OFFSET)
+
+#define HPIPE_LANE_STATUS1_REG			0x60C
+#define HPIPE_LANE_STATUS1_PCLK_EN_OFFSET	0
+#define HPIPE_LANE_STATUS1_PCLK_EN_MASK		\
+			(0x1 << HPIPE_LANE_STATUS1_PCLK_EN_OFFSET)
+
+#define HPIPE_LANE_CFG4_REG			0x620
+#define HPIPE_LANE_CFG4_DFE_CTRL_OFFSET		0
+#define HPIPE_LANE_CFG4_DFE_CTRL_MASK		\
+			(0x7 << HPIPE_LANE_CFG4_DFE_CTRL_OFFSET)
+#define HPIPE_LANE_CFG4_DFE_EN_SEL_OFFSET	3
+#define HPIPE_LANE_CFG4_DFE_EN_SEL_MASK		\
+			(0x1 << HPIPE_LANE_CFG4_DFE_EN_SEL_OFFSET)
+#define HPIPE_LANE_CFG4_DFE_OVER_OFFSET		6
+#define HPIPE_LANE_CFG4_DFE_OVER_MASK		\
+			(0x1 << HPIPE_LANE_CFG4_DFE_OVER_OFFSET)
+#define HPIPE_LANE_CFG4_SSC_CTRL_OFFSET		7
+#define HPIPE_LANE_CFG4_SSC_CTRL_MASK		\
+			(0x1 << HPIPE_LANE_CFG4_SSC_CTRL_OFFSET)
+
+#define HPIPE_LANE_EQ_REMOTE_SETTING_REG	0x6f8
+#define HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_OFFSET	0
+#define HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_MASK	\
+			(0x1 << HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_OFFSET)
+#define HPIPE_LANE_CFG_FOM_ONLY_MODE_OFFFSET	1
+#define HPIPE_LANE_CFG_FOM_ONLY_MODE_MASK	\
+			(0x1 << HPIPE_LANE_CFG_FOM_ONLY_MODE_OFFFSET)
+#define HPIPE_LANE_CFG_FOM_PRESET_VECTOR_OFFSET	2
+#define HPIPE_LANE_CFG_FOM_PRESET_VECTOR_MASK	\
+			(0xf << HPIPE_LANE_CFG_FOM_PRESET_VECTOR_OFFSET)
+
+#define HPIPE_LANE_EQU_CONFIG_0_REG		0x69C
+#define HPIPE_CFG_PHY_RC_EP_OFFSET		12
+#define HPIPE_CFG_PHY_RC_EP_MASK		\
+			(0x1 << HPIPE_CFG_PHY_RC_EP_OFFSET)
+
+#define HPIPE_LANE_EQ_CFG1_REG			0x6a0
+#define HPIPE_CFG_UPDATE_POLARITY_OFFSET	12
+#define HPIPE_CFG_UPDATE_POLARITY_MASK		\
+			(0x1 << HPIPE_CFG_UPDATE_POLARITY_OFFSET)
+
+#define HPIPE_LANE_EQ_CFG2_REG			0x6a4
+#define HPIPE_CFG_EQ_BUNDLE_DIS_OFFSET		14
+#define HPIPE_CFG_EQ_BUNDLE_DIS_MASK		\
+			(0x1 << HPIPE_CFG_EQ_BUNDLE_DIS_OFFSET)
+
+#define HPIPE_RST_CLK_CTRL_REG			0x704
+#define HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET	0
+#define HPIPE_RST_CLK_CTRL_PIPE_RST_MASK	\
+			(0x1 << HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET)
+#define HPIPE_RST_CLK_CTRL_FIXED_PCLK_OFFSET	2
+#define HPIPE_RST_CLK_CTRL_FIXED_PCLK_MASK	\
+			(0x1 << HPIPE_RST_CLK_CTRL_FIXED_PCLK_OFFSET)
+#define HPIPE_RST_CLK_CTRL_PIPE_WIDTH_OFFSET	3
+#define HPIPE_RST_CLK_CTRL_PIPE_WIDTH_MASK	\
+			(0x1 << HPIPE_RST_CLK_CTRL_PIPE_WIDTH_OFFSET)
+#define HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_OFFSET	9
+#define HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_MASK	\
+			(0x1 << HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_OFFSET)
+
+#define HPIPE_TST_MODE_CTRL_REG			0x708
+#define HPIPE_TST_MODE_CTRL_MODE_MARGIN_OFFSET	2
+#define HPIPE_TST_MODE_CTRL_MODE_MARGIN_MASK	\
+			(0x1 << HPIPE_TST_MODE_CTRL_MODE_MARGIN_OFFSET)
+
+#define HPIPE_CLK_SRC_LO_REG				0x70c
+#define HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SEL_OFFSET	1
+#define HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SEL_MASK		\
+			(0x1 << HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SEL_OFFSET)
+#define HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SCALE_OFFSET	2
+#define HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SCALE_MASK	\
+			(0x3 << HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SCALE_OFFSET)
+#define HPIPE_CLK_SRC_LO_PLL_RDY_DL_OFFSET		5
+#define HPIPE_CLK_SRC_LO_PLL_RDY_DL_MASK		\
+			(0x7 << HPIPE_CLK_SRC_LO_PLL_RDY_DL_OFFSET)
+
+#define HPIPE_CLK_SRC_HI_REG			0x710
+#define HPIPE_CLK_SRC_HI_LANE_STRT_OFFSET	0
+#define HPIPE_CLK_SRC_HI_LANE_STRT_MASK		\
+			(0x1 << HPIPE_CLK_SRC_HI_LANE_STRT_OFFSET)
+#define HPIPE_CLK_SRC_HI_LANE_BREAK_OFFSET	1
+#define HPIPE_CLK_SRC_HI_LANE_BREAK_MASK	\
+			(0x1 << HPIPE_CLK_SRC_HI_LANE_BREAK_OFFSET)
+#define HPIPE_CLK_SRC_HI_LANE_MASTER_OFFSET	2
+#define HPIPE_CLK_SRC_HI_LANE_MASTER_MASK	\
+			(0x1 << HPIPE_CLK_SRC_HI_LANE_MASTER_OFFSET)
+#define HPIPE_CLK_SRC_HI_MODE_PIPE_OFFSET	7
+#define HPIPE_CLK_SRC_HI_MODE_PIPE_MASK		\
+			(0x1 << HPIPE_CLK_SRC_HI_MODE_PIPE_OFFSET)
+
+#define HPIPE_GLOBAL_MISC_CTRL			0x718
+#define HPIPE_GLOBAL_PM_CTRL			0x740
+#define HPIPE_GLOBAL_PM_RXDLOZ_WAIT_OFFSET	0
+#define HPIPE_GLOBAL_PM_RXDLOZ_WAIT_MASK	\
+			(0xFF << HPIPE_GLOBAL_PM_RXDLOZ_WAIT_OFFSET)
+
+/* General defines */
+#define PLL_LOCK_TIMEOUT			15000
+
+#endif /* COMPHY_CP110_H */
diff --git a/drivers/marvell/comphy/phy-comphy-3700.c b/drivers/marvell/comphy/phy-comphy-3700.c
new file mode 100644
index 0000000..1a97753
--- /dev/null
+++ b/drivers/marvell/comphy/phy-comphy-3700.c
@@ -0,0 +1,1065 @@
+/*
+ * Copyright (C) 2018-2021 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <errno.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+
+#include <mvebu.h>
+#include <mvebu_def.h>
+#include <plat_marvell.h>
+
+#include "phy-comphy-3700.h"
+#include "phy-comphy-common.h"
+
+/*
+ * COMPHY_INDIRECT_REG points to ahci address space but the ahci region used in
+ * Linux is up to 0x178 so none will access it from Linux in runtime
+ * concurrently.
+ */
+#define COMPHY_INDIRECT_REG	(MVEBU_REGS_BASE + 0xE0178)
+
+/* The USB3_GBE1_PHY range is above USB3 registers used in dts */
+#define USB3_GBE1_PHY		(MVEBU_REGS_BASE + 0x5C000)
+#define COMPHY_SD_ADDR		(MVEBU_REGS_BASE + 0x1F000)
+
+struct sgmii_phy_init_data_fix {
+	uint16_t addr;
+	uint16_t value;
+};
+
+/* Changes to 40M1G25 mode data required for running 40M3G125 init mode */
+static struct sgmii_phy_init_data_fix sgmii_phy_init_fix[] = {
+	{0x005, 0x07CC}, {0x015, 0x0000}, {0x01B, 0x0000}, {0x01D, 0x0000},
+	{0x01E, 0x0000}, {0x01F, 0x0000}, {0x020, 0x0000}, {0x021, 0x0030},
+	{0x026, 0x0888}, {0x04D, 0x0152}, {0x04F, 0xA020}, {0x050, 0x07CC},
+	{0x053, 0xE9CA}, {0x055, 0xBD97}, {0x071, 0x3015}, {0x076, 0x03AA},
+	{0x07C, 0x0FDF}, {0x0C2, 0x3030}, {0x0C3, 0x8000}, {0x0E2, 0x5550},
+	{0x0E3, 0x12A4}, {0x0E4, 0x7D00}, {0x0E6, 0x0C83}, {0x101, 0xFCC0},
+	{0x104, 0x0C10}
+};
+
+/* 40M1G25 mode init data */
+static uint16_t sgmii_phy_init[512] = {
+	/* 0       1       2       3       4       5       6       7 */
+	/*-----------------------------------------------------------*/
+	/* 8       9       A       B       C       D       E       F */
+	0x3110, 0xFD83, 0x6430, 0x412F, 0x82C0, 0x06FA, 0x4500, 0x6D26,	/* 00 */
+	0xAFC0, 0x8000, 0xC000, 0x0000, 0x2000, 0x49CC, 0x0BC9, 0x2A52,	/* 08 */
+	0x0BD2, 0x0CDE, 0x13D2, 0x0CE8, 0x1149, 0x10E0, 0x0000, 0x0000,	/* 10 */
+	0x0000, 0x0000, 0x0000, 0x0001, 0x0000, 0x4134, 0x0D2D, 0xFFFF,	/* 18 */
+	0xFFE0, 0x4030, 0x1016, 0x0030, 0x0000, 0x0800, 0x0866, 0x0000,	/* 20 */
+	0x0000, 0x0000, 0xFFFF, 0xFFFF, 0xFFFF, 0xFFFF, 0xFFFF, 0xFFFF,	/* 28 */
+	0xFFFF, 0xFFFF, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/* 30 */
+	0x0000, 0x0000, 0x000F, 0x6A62, 0x1988, 0x3100, 0x3100, 0x3100,	/* 38 */
+	0x3100, 0xA708, 0x2430, 0x0830, 0x1030, 0x4610, 0xFF00, 0xFF00,	/* 40 */
+	0x0060, 0x1000, 0x0400, 0x0040, 0x00F0, 0x0155, 0x1100, 0xA02A,	/* 48 */
+	0x06FA, 0x0080, 0xB008, 0xE3ED, 0x5002, 0xB592, 0x7A80, 0x0001,	/* 50 */
+	0x020A, 0x8820, 0x6014, 0x8054, 0xACAA, 0xFC88, 0x2A02, 0x45CF,	/* 58 */
+	0x000F, 0x1817, 0x2860, 0x064F, 0x0000, 0x0204, 0x1800, 0x6000,	/* 60 */
+	0x810F, 0x4F23, 0x4000, 0x4498, 0x0850, 0x0000, 0x000E, 0x1002,	/* 68 */
+	0x9D3A, 0x3009, 0xD066, 0x0491, 0x0001, 0x6AB0, 0x0399, 0x3780,	/* 70 */
+	0x0040, 0x5AC0, 0x4A80, 0x0000, 0x01DF, 0x0000, 0x0007, 0x0000,	/* 78 */
+	0x2D54, 0x00A1, 0x4000, 0x0100, 0xA20A, 0x0000, 0x0000, 0x0000,	/* 80 */
+	0x0000, 0x0000, 0x0000, 0x7400, 0x0E81, 0x1000, 0x1242, 0x0210,	/* 88 */
+	0x80DF, 0x0F1F, 0x2F3F, 0x4F5F, 0x6F7F, 0x0F1F, 0x2F3F, 0x4F5F,	/* 90 */
+	0x6F7F, 0x4BAD, 0x0000, 0x0000, 0x0800, 0x0000, 0x2400, 0xB651,	/* 98 */
+	0xC9E0, 0x4247, 0x0A24, 0x0000, 0xAF19, 0x1004, 0x0000, 0x0000,	/* A0 */
+	0x0000, 0x0013, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/* A8 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/* B0 */
+	0x0000, 0x0000, 0x0000, 0x0060, 0x0000, 0x0000, 0x0000, 0x0000,	/* B8 */
+	0x0000, 0x0000, 0x3010, 0xFA00, 0x0000, 0x0000, 0x0000, 0x0003,	/* C0 */
+	0x1618, 0x8200, 0x8000, 0x0400, 0x050F, 0x0000, 0x0000, 0x0000,	/* C8 */
+	0x4C93, 0x0000, 0x1000, 0x1120, 0x0010, 0x1242, 0x1242, 0x1E00,	/* D0 */
+	0x0000, 0x0000, 0x0000, 0x00F8, 0x0000, 0x0041, 0x0800, 0x0000,	/* D8 */
+	0x82A0, 0x572E, 0x2490, 0x14A9, 0x4E00, 0x0000, 0x0803, 0x0541,	/* E0 */
+	0x0C15, 0x0000, 0x0000, 0x0400, 0x2626, 0x0000, 0x0000, 0x4200,	/* E8 */
+	0x0000, 0xAA55, 0x1020, 0x0000, 0x0000, 0x5010, 0x0000, 0x0000,	/* F0 */
+	0x0000, 0x0000, 0x5000, 0x0000, 0x0000, 0x0000, 0x02F2, 0x0000,	/* F8 */
+	0x101F, 0xFDC0, 0x4000, 0x8010, 0x0110, 0x0006, 0x0000, 0x0000,	/*100 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*108 */
+	0x04CF, 0x0000, 0x04CF, 0x0000, 0x04CF, 0x0000, 0x04C6, 0x0000,	/*110 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*118 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*120 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*128 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*130 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*138 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*140 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*148 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*150 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*158 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*160 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*168 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*170 */
+	0x0000, 0x0000, 0x0000, 0x00F0, 0x08A2, 0x3112, 0x0A14, 0x0000,	/*178 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*180 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*188 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*190 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*198 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1A0 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1A8 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1B0 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1B8 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1C0 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1C8 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1D0 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1D8 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1E0 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1E8 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1F0 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000	/*1F8 */
+};
+
+/* PHY selector configures with corresponding modes */
+static int mvebu_a3700_comphy_set_phy_selector(uint8_t comphy_index,
+						uint32_t comphy_mode)
+{
+	uint32_t reg;
+	int mode = COMPHY_GET_MODE(comphy_mode);
+
+	reg = mmio_read_32(MVEBU_COMPHY_REG_BASE + COMPHY_SELECTOR_PHY_REG);
+	switch (mode) {
+	case (COMPHY_SATA_MODE):
+		/* SATA must be in Lane2 */
+		if (comphy_index == COMPHY_LANE2)
+			reg &= ~COMPHY_SELECTOR_USB3_PHY_SEL_BIT;
+		else
+			goto error;
+		break;
+
+	case (COMPHY_SGMII_MODE):
+	case (COMPHY_2500BASEX_MODE):
+		if (comphy_index == COMPHY_LANE0)
+			reg &= ~COMPHY_SELECTOR_USB3_GBE1_SEL_BIT;
+		else if (comphy_index == COMPHY_LANE1)
+			reg &= ~COMPHY_SELECTOR_PCIE_GBE0_SEL_BIT;
+		else
+			goto error;
+		break;
+
+	case (COMPHY_USB3H_MODE):
+	case (COMPHY_USB3D_MODE):
+	case (COMPHY_USB3_MODE):
+		if (comphy_index == COMPHY_LANE2)
+			reg |= COMPHY_SELECTOR_USB3_PHY_SEL_BIT;
+		else if (comphy_index == COMPHY_LANE0)
+			reg |= COMPHY_SELECTOR_USB3_GBE1_SEL_BIT;
+		else
+			goto error;
+		break;
+
+	case (COMPHY_PCIE_MODE):
+		/* PCIE must be in Lane1 */
+		if (comphy_index == COMPHY_LANE1)
+			reg |= COMPHY_SELECTOR_PCIE_GBE0_SEL_BIT;
+		else
+			goto error;
+		break;
+
+	default:
+		goto error;
+	}
+
+	mmio_write_32(MVEBU_COMPHY_REG_BASE + COMPHY_SELECTOR_PHY_REG, reg);
+	return 0;
+error:
+	ERROR("COMPHY[%d] mode[%d] is invalid\n", comphy_index, mode);
+	return -EINVAL;
+}
+
+/*
+ * This is something like the inverse of the previous function: for given
+ * lane it returns COMPHY_*_MODE.
+ *
+ * It is useful when powering the phy off.
+ *
+ * This function returns COMPHY_USB3_MODE even if the phy was configured
+ * with COMPHY_USB3D_MODE or COMPHY_USB3H_MODE. (The usb3 phy initialization
+ * code does not differentiate between these modes.)
+ * Also it returns COMPHY_SGMII_MODE even if the phy was configures with
+ * COMPHY_2500BASEX_MODE. (The sgmii phy initialization code does differentiate
+ * between these modes, but it is irrelevant when powering the phy off.)
+ */
+static int mvebu_a3700_comphy_get_mode(uint8_t comphy_index)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(MVEBU_COMPHY_REG_BASE + COMPHY_SELECTOR_PHY_REG);
+	switch (comphy_index) {
+	case COMPHY_LANE0:
+		if ((reg & COMPHY_SELECTOR_USB3_GBE1_SEL_BIT) != 0)
+			return COMPHY_USB3_MODE;
+		else
+			return COMPHY_SGMII_MODE;
+	case COMPHY_LANE1:
+		if ((reg & COMPHY_SELECTOR_PCIE_GBE0_SEL_BIT) != 0)
+			return COMPHY_PCIE_MODE;
+		else
+			return COMPHY_SGMII_MODE;
+	case COMPHY_LANE2:
+		if ((reg & COMPHY_SELECTOR_USB3_PHY_SEL_BIT) != 0)
+			return COMPHY_USB3_MODE;
+		else
+			return COMPHY_SATA_MODE;
+	}
+
+	return COMPHY_UNUSED;
+}
+
+/* It is only used for SATA and USB3 on comphy lane2. */
+static void comphy_set_indirect(uintptr_t addr, uint32_t offset, uint16_t data,
+				uint16_t mask, bool is_sata)
+{
+	/*
+	 * When Lane 2 PHY is for USB3, access the PHY registers
+	 * through indirect Address and Data registers:
+	 * INDIR_ACC_PHY_ADDR (RD00E0178h [31:0]),
+	 * INDIR_ACC_PHY_DATA (RD00E017Ch [31:0]),
+	 * within the SATA Host Controller registers, Lane 2 base register
+	 * offset is 0x200
+	 */
+	if (is_sata) {
+		mmio_write_32(addr + COMPHY_LANE2_INDIR_ADDR_OFFSET, offset);
+	} else {
+		mmio_write_32(addr + COMPHY_LANE2_INDIR_ADDR_OFFSET,
+			      offset + USB3PHY_LANE2_REG_BASE_OFFSET);
+	}
+
+	reg_set(addr + COMPHY_LANE2_INDIR_DATA_OFFSET, data, mask);
+}
+
+/* It is only used for SATA on comphy lane2. */
+static void comphy_sata_set_indirect(uintptr_t addr, uint32_t reg_offset,
+				     uint16_t data, uint16_t mask)
+{
+	comphy_set_indirect(addr, reg_offset, data, mask, true);
+}
+
+/* It is only used for USB3 indirect access on comphy lane2. */
+static void comphy_usb3_set_indirect(uintptr_t addr, uint32_t reg_offset,
+				     uint16_t data, uint16_t mask)
+{
+	comphy_set_indirect(addr, reg_offset, data, mask, false);
+}
+
+/* It is only used for USB3 direct access not on comphy lane2. */
+static void comphy_usb3_set_direct(uintptr_t addr, uint32_t reg_offset,
+				   uint16_t data, uint16_t mask)
+{
+	reg_set16((reg_offset * PHY_SHFT(USB3) + addr), data, mask);
+}
+
+static void comphy_sgmii_phy_init(uintptr_t sd_ip_addr, bool is_1gbps)
+{
+	const int fix_arr_sz = ARRAY_SIZE(sgmii_phy_init_fix);
+	int addr, fix_idx;
+	uint16_t val;
+
+	fix_idx = 0;
+	for (addr = 0; addr < 512; addr++) {
+		/*
+		 * All PHY register values are defined in full for 3.125Gbps
+		 * SERDES speed. The values required for 1.25 Gbps are almost
+		 * the same and only few registers should be "fixed" in
+		 * comparison to 3.125 Gbps values. These register values are
+		 * stored in "sgmii_phy_init_fix" array.
+		 */
+		if (!is_1gbps && sgmii_phy_init_fix[fix_idx].addr == addr) {
+			/* Use new value */
+			val = sgmii_phy_init_fix[fix_idx].value;
+			if (fix_idx < fix_arr_sz)
+				fix_idx++;
+		} else {
+			val = sgmii_phy_init[addr];
+		}
+
+		reg_set16(SGMIIPHY_ADDR(addr, sd_ip_addr), val, 0xFFFF);
+	}
+}
+
+static int mvebu_a3700_comphy_sata_power_on(uint8_t comphy_index,
+					    uint32_t comphy_mode)
+{
+	int ret;
+	uint32_t offset, data = 0, ref_clk;
+	uintptr_t comphy_indir_regs = COMPHY_INDIRECT_REG;
+	int invert = COMPHY_GET_POLARITY_INVERT(comphy_mode);
+
+	debug_enter();
+
+	/* Configure phy selector for SATA */
+	ret = mvebu_a3700_comphy_set_phy_selector(comphy_index, comphy_mode);
+	if (ret) {
+		return ret;
+	}
+
+	/* Clear phy isolation mode to make it work in normal mode */
+	offset =  COMPHY_ISOLATION_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
+	comphy_sata_set_indirect(comphy_indir_regs, offset, 0, PHY_ISOLATE_MODE);
+
+	/* 0. Check the Polarity invert bits */
+	if (invert & COMPHY_POLARITY_TXD_INVERT)
+		data |= TXD_INVERT_BIT;
+	if (invert & COMPHY_POLARITY_RXD_INVERT)
+		data |= RXD_INVERT_BIT;
+
+	offset = COMPHY_SYNC_PATTERN + SATAPHY_LANE2_REG_BASE_OFFSET;
+	comphy_sata_set_indirect(comphy_indir_regs, offset, data, TXD_INVERT_BIT |
+				 RXD_INVERT_BIT);
+
+	/* 1. Select 40-bit data width width */
+	offset = COMPHY_DIG_LOOPBACK_EN + SATAPHY_LANE2_REG_BASE_OFFSET;
+	comphy_sata_set_indirect(comphy_indir_regs, offset, DATA_WIDTH_40BIT,
+				 SEL_DATA_WIDTH_MASK);
+
+	/* 2. Select reference clock(25M) and PHY mode (SATA) */
+	offset = COMPHY_POWER_PLL_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
+	if (get_ref_clk() == 40)
+		ref_clk = REF_FREF_SEL_SERDES_40MHZ;
+	else
+		ref_clk = REF_FREF_SEL_SERDES_25MHZ;
+
+	comphy_sata_set_indirect(comphy_indir_regs, offset, ref_clk | PHY_MODE_SATA,
+				 REF_FREF_SEL_MASK | PHY_MODE_MASK);
+
+	/* 3. Use maximum PLL rate (no power save) */
+	offset = COMPHY_KVCO_CAL_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
+	comphy_sata_set_indirect(comphy_indir_regs, offset, USE_MAX_PLL_RATE_BIT,
+				 USE_MAX_PLL_RATE_BIT);
+
+	/* 4. Reset reserved bit */
+	comphy_sata_set_indirect(comphy_indir_regs, COMPHY_RESERVED_REG, 0,
+				 PHYCTRL_FRM_PIN_BIT);
+
+	/* 5. Set vendor-specific configuration (It is done in sata driver) */
+	/* XXX: in U-Boot below sequence was executed in this place, in Linux
+	 * not.  Now it is done only in U-Boot before this comphy
+	 * initialization - tests shows that it works ok, but in case of any
+	 * future problem it is left for reference.
+	 *   reg_set(MVEBU_REGS_BASE + 0xe00a0, 0, 0xffffffff);
+	 *   reg_set(MVEBU_REGS_BASE + 0xe00a4, BIT(6), BIT(6));
+	 */
+
+	/* Wait for > 55 us to allow PLL be enabled */
+	udelay(PLL_SET_DELAY_US);
+
+	/* Polling status */
+	mmio_write_32(comphy_indir_regs + COMPHY_LANE2_INDIR_ADDR_OFFSET,
+		      COMPHY_DIG_LOOPBACK_EN + SATAPHY_LANE2_REG_BASE_OFFSET);
+
+	ret = polling_with_timeout(comphy_indir_regs +
+				   COMPHY_LANE2_INDIR_DATA_OFFSET,
+				   PLL_READY_TX_BIT, PLL_READY_TX_BIT,
+				   COMPHY_PLL_TIMEOUT, REG_32BIT);
+	if (ret) {
+		return -ETIMEDOUT;
+	}
+
+	debug_exit();
+
+	return 0;
+}
+
+static int mvebu_a3700_comphy_sgmii_power_on(uint8_t comphy_index,
+					     uint32_t comphy_mode)
+{
+	int ret;
+	uint32_t mask, data;
+	uintptr_t offset;
+	uintptr_t sd_ip_addr;
+	int mode = COMPHY_GET_MODE(comphy_mode);
+	int invert = COMPHY_GET_POLARITY_INVERT(comphy_mode);
+
+	debug_enter();
+
+	/* Set selector */
+	ret = mvebu_a3700_comphy_set_phy_selector(comphy_index, comphy_mode);
+	if (ret) {
+		return ret;
+	}
+
+	/* Serdes IP Base address
+	 * COMPHY Lane0 -- USB3/GBE1
+	 * COMPHY Lane1 -- PCIe/GBE0
+	 */
+	if (comphy_index == COMPHY_LANE0) {
+		/* Get usb3 and gbe */
+		sd_ip_addr = USB3_GBE1_PHY;
+	} else
+		sd_ip_addr = COMPHY_SD_ADDR;
+
+	/*
+	 * 1. Reset PHY by setting PHY input port PIN_RESET=1.
+	 * 2. Set PHY input port PIN_TX_IDLE=1, PIN_PU_IVREF=1 to keep
+	 *    PHY TXP/TXN output to idle state during PHY initialization
+	 * 3. Set PHY input port PIN_PU_PLL=0, PIN_PU_RX=0, PIN_PU_TX=0.
+	 */
+	data = PIN_PU_IVREF_BIT | PIN_TX_IDLE_BIT | PIN_RESET_COMPHY_BIT;
+	mask = data | PIN_RESET_CORE_BIT | PIN_PU_PLL_BIT | PIN_PU_RX_BIT |
+		PIN_PU_TX_BIT;
+	offset = MVEBU_COMPHY_REG_BASE + COMPHY_PHY_CFG1_OFFSET(comphy_index);
+	reg_set(offset, data, mask);
+
+	/* 4. Release reset to the PHY by setting PIN_RESET=0. */
+	data = 0;
+	mask = PIN_RESET_COMPHY_BIT;
+	reg_set(offset, data, mask);
+
+	/*
+	 * 5. Set PIN_PHY_GEN_TX[3:0] and PIN_PHY_GEN_RX[3:0] to decide COMPHY
+	 * bit rate
+	 */
+	if (mode == COMPHY_SGMII_MODE) {
+		/* SGMII 1G, SerDes speed 1.25G */
+		data |= SD_SPEED_1_25_G << GEN_RX_SEL_OFFSET;
+		data |= SD_SPEED_1_25_G << GEN_TX_SEL_OFFSET;
+	} else if (mode == COMPHY_2500BASEX_MODE) {
+		/* 2500Base-X, SerDes speed 3.125G */
+		data |= SD_SPEED_3_125_G << GEN_RX_SEL_OFFSET;
+		data |= SD_SPEED_3_125_G << GEN_TX_SEL_OFFSET;
+	} else {
+		/* Other rates are not supported */
+		ERROR("unsupported SGMII speed on comphy lane%d\n",
+			comphy_index);
+		return -EINVAL;
+	}
+	mask = GEN_RX_SEL_MASK | GEN_TX_SEL_MASK;
+	reg_set(offset, data, mask);
+
+	/*
+	 * 6. Wait 10mS for bandgap and reference clocks to stabilize; then
+	 * start SW programming.
+	 */
+	mdelay(10);
+
+	/* 7. Program COMPHY register PHY_MODE */
+	data = PHY_MODE_SGMII;
+	mask = PHY_MODE_MASK;
+	reg_set16(SGMIIPHY_ADDR(COMPHY_POWER_PLL_CTRL, sd_ip_addr), data, mask);
+
+	/*
+	 * 8. Set COMPHY register REFCLK_SEL to select the correct REFCLK
+	 * source
+	 */
+	data = 0;
+	mask = PHY_REF_CLK_SEL;
+	reg_set16(SGMIIPHY_ADDR(COMPHY_MISC_CTRL0, sd_ip_addr), data, mask);
+
+	/*
+	 * 9. Set correct reference clock frequency in COMPHY register
+	 * REF_FREF_SEL.
+	 */
+	if (get_ref_clk() == 40)
+		data = REF_FREF_SEL_SERDES_50MHZ;
+	else
+		data = REF_FREF_SEL_SERDES_25MHZ;
+
+	mask = REF_FREF_SEL_MASK;
+	reg_set16(SGMIIPHY_ADDR(COMPHY_POWER_PLL_CTRL, sd_ip_addr), data, mask);
+
+	/* 10. Program COMPHY register PHY_GEN_MAX[1:0]
+	 * This step is mentioned in the flow received from verification team.
+	 * However the PHY_GEN_MAX value is only meaningful for other interfaces
+	 * (not SGMII). For instance, it selects SATA speed 1.5/3/6 Gbps or PCIe
+	 * speed 2.5/5 Gbps
+	 */
+
+	/*
+	 * 11. Program COMPHY register SEL_BITS to set correct parallel data
+	 * bus width
+	 */
+	data = DATA_WIDTH_10BIT;
+	mask = SEL_DATA_WIDTH_MASK;
+	reg_set16(SGMIIPHY_ADDR(COMPHY_DIG_LOOPBACK_EN, sd_ip_addr),
+		  data, mask);
+
+	/*
+	 * 12. As long as DFE function needs to be enabled in any mode,
+	 * COMPHY register DFE_UPDATE_EN[5:0] shall be programmed to 0x3F
+	 * for real chip during COMPHY power on.
+	 * The step 14 exists (and empty) in the original initialization flow
+	 * obtained from the verification team. According to the functional
+	 * specification DFE_UPDATE_EN already has the default value 0x3F
+	 */
+
+	/*
+	 * 13. Program COMPHY GEN registers.
+	 * These registers should be programmed based on the lab testing result
+	 * to achieve optimal performance. Please contact the CEA group to get
+	 * the related GEN table during real chip bring-up. We only required to
+	 * run though the entire registers programming flow defined by
+	 * "comphy_sgmii_phy_init" when the REF clock is 40 MHz. For REF clock
+	 * 25 MHz the default values stored in PHY registers are OK.
+	 */
+	debug("Running C-DPI phy init %s mode\n",
+	      mode == COMPHY_2500BASEX_MODE ? "2G5" : "1G");
+	if (get_ref_clk() == 40)
+		comphy_sgmii_phy_init(sd_ip_addr, mode != COMPHY_2500BASEX_MODE);
+
+	/*
+	 * 14. [Simulation Only] should not be used for real chip.
+	 * By pass power up calibration by programming EXT_FORCE_CAL_DONE
+	 * (R02h[9]) to 1 to shorten COMPHY simulation time.
+	 */
+
+	/*
+	 * 15. [Simulation Only: should not be used for real chip]
+	 * Program COMPHY register FAST_DFE_TIMER_EN=1 to shorten RX training
+	 * simulation time.
+	 */
+
+	/*
+	 * 16. Check the PHY Polarity invert bit
+	 */
+	data = 0x0;
+	if (invert & COMPHY_POLARITY_TXD_INVERT)
+		data |= TXD_INVERT_BIT;
+	if (invert & COMPHY_POLARITY_RXD_INVERT)
+		data |= RXD_INVERT_BIT;
+	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
+	reg_set16(SGMIIPHY_ADDR(COMPHY_SYNC_PATTERN, sd_ip_addr), data, mask);
+
+	/*
+	 * 17. Set PHY input ports PIN_PU_PLL, PIN_PU_TX and PIN_PU_RX to 1 to
+	 * start PHY power up sequence. All the PHY register programming should
+	 * be done before PIN_PU_PLL=1. There should be no register programming
+	 * for normal PHY operation from this point.
+	 */
+	reg_set(MVEBU_COMPHY_REG_BASE + COMPHY_PHY_CFG1_OFFSET(comphy_index),
+		PIN_PU_PLL_BIT | PIN_PU_RX_BIT | PIN_PU_TX_BIT,
+		PIN_PU_PLL_BIT | PIN_PU_RX_BIT | PIN_PU_TX_BIT);
+
+	/*
+	 * 18. Wait for PHY power up sequence to finish by checking output ports
+	 * PIN_PLL_READY_TX=1 and PIN_PLL_READY_RX=1.
+	 */
+	ret = polling_with_timeout(MVEBU_COMPHY_REG_BASE +
+				   COMPHY_PHY_STATUS_OFFSET(comphy_index),
+				   PHY_PLL_READY_TX_BIT | PHY_PLL_READY_RX_BIT,
+				   PHY_PLL_READY_TX_BIT | PHY_PLL_READY_RX_BIT,
+				   COMPHY_PLL_TIMEOUT, REG_32BIT);
+	if (ret) {
+		ERROR("Failed to lock PLL for SGMII PHY %d\n", comphy_index);
+		return -ETIMEDOUT;
+	}
+
+	/*
+	 * 19. Set COMPHY input port PIN_TX_IDLE=0
+	 */
+	reg_set(MVEBU_COMPHY_REG_BASE + COMPHY_PHY_CFG1_OFFSET(comphy_index),
+		0x0, PIN_TX_IDLE_BIT);
+
+	/*
+	 * 20. After valid data appear on PIN_RXDATA bus, set PIN_RX_INIT=1. To
+	 * start RX initialization. PIN_RX_INIT_DONE will be cleared to 0 by the
+	 * PHY After RX initialization is done, PIN_RX_INIT_DONE will be set to
+	 * 1 by COMPHY Set PIN_RX_INIT=0 after PIN_RX_INIT_DONE= 1. Please
+	 * refer to RX initialization part for details.
+	 */
+	reg_set(MVEBU_COMPHY_REG_BASE + COMPHY_PHY_CFG1_OFFSET(comphy_index),
+		PHY_RX_INIT_BIT, PHY_RX_INIT_BIT);
+
+	ret = polling_with_timeout(MVEBU_COMPHY_REG_BASE +
+				   COMPHY_PHY_STATUS_OFFSET(comphy_index),
+				   PHY_PLL_READY_TX_BIT | PHY_PLL_READY_RX_BIT,
+				   PHY_PLL_READY_TX_BIT | PHY_PLL_READY_RX_BIT,
+				   COMPHY_PLL_TIMEOUT, REG_32BIT);
+	if (ret) {
+		ERROR("Failed to lock PLL for SGMII PHY %d\n", comphy_index);
+		return -ETIMEDOUT;
+	}
+
+	ret = polling_with_timeout(MVEBU_COMPHY_REG_BASE +
+				   COMPHY_PHY_STATUS_OFFSET(comphy_index),
+				   PHY_RX_INIT_DONE_BIT, PHY_RX_INIT_DONE_BIT,
+				   COMPHY_PLL_TIMEOUT, REG_32BIT);
+	if (ret) {
+		ERROR("Failed to init RX of SGMII PHY %d\n", comphy_index);
+		return -ETIMEDOUT;
+	}
+
+	debug_exit();
+
+	return 0;
+}
+
+static int mvebu_a3700_comphy_sgmii_power_off(uint8_t comphy_index)
+{
+	uintptr_t offset;
+	uint32_t mask, data;
+
+	debug_enter();
+
+	data = PIN_RESET_CORE_BIT | PIN_RESET_COMPHY_BIT;
+	mask = data;
+	offset = MVEBU_COMPHY_REG_BASE + COMPHY_PHY_CFG1_OFFSET(comphy_index);
+	reg_set(offset, data, mask);
+
+	debug_exit();
+
+	return 0;
+}
+
+static int mvebu_a3700_comphy_usb3_power_on(uint8_t comphy_index,
+					    uint32_t comphy_mode)
+{
+	int ret;
+	uintptr_t reg_base = 0;
+	uintptr_t addr;
+	uint32_t mask, data, cfg, ref_clk;
+	void (*usb3_reg_set)(uintptr_t addr, uint32_t reg_offset, uint16_t data,
+			     uint16_t mask);
+	int invert = COMPHY_GET_POLARITY_INVERT(comphy_mode);
+
+	debug_enter();
+
+	/* Set phy seclector */
+	ret = mvebu_a3700_comphy_set_phy_selector(comphy_index, comphy_mode);
+	if (ret) {
+		return ret;
+	}
+
+	/* Set usb3 reg access func, Lane2 is indirect access */
+	if (comphy_index == COMPHY_LANE2) {
+		usb3_reg_set = &comphy_usb3_set_indirect;
+		reg_base = COMPHY_INDIRECT_REG;
+	} else {
+		/* Get the direct access register resource and map */
+		usb3_reg_set = &comphy_usb3_set_direct;
+		reg_base = USB3_GBE1_PHY;
+	}
+
+	/*
+	 * 0. Set PHY OTG Control(0x5d034), bit 4, Power up OTG module The
+	 * register belong to UTMI module, so it is set in UTMI phy driver.
+	 */
+
+	/*
+	 * 1. Set PRD_TXDEEMPH (3.5db de-emph)
+	 */
+	mask = PRD_TXDEEMPH0_MASK | PRD_TXMARGIN_MASK | PRD_TXSWING_MASK |
+		CFG_TX_ALIGN_POS_MASK;
+	usb3_reg_set(reg_base, COMPHY_LANE_CFG0, PRD_TXDEEMPH0_MASK, mask);
+
+	/*
+	 * 2. Set BIT0: enable transmitter in high impedance mode
+	 *    Set BIT[3:4]: delay 2 clock cycles for HiZ off latency
+	 *    Set BIT6: Tx detect Rx at HiZ mode
+	 *    Unset BIT15: set to 0 to set USB3 De-emphasize level to -3.5db
+	 *            together with bit 0 of COMPHY_LANE_CFG0 register
+	 */
+	mask = PRD_TXDEEMPH1_MASK | TX_DET_RX_MODE | GEN2_TX_DATA_DLY_MASK |
+		TX_ELEC_IDLE_MODE_EN;
+	data = TX_DET_RX_MODE | GEN2_TX_DATA_DLY_DEFT | TX_ELEC_IDLE_MODE_EN;
+	usb3_reg_set(reg_base, COMPHY_LANE_CFG1, data, mask);
+
+	/*
+	 * 3. Set Spread Spectrum Clock Enabled
+	 */
+	usb3_reg_set(reg_base, COMPHY_LANE_CFG4,
+		     SPREAD_SPECTRUM_CLK_EN, SPREAD_SPECTRUM_CLK_EN);
+
+	/*
+	 * 4. Set Override Margining Controls From the MAC:
+	 *    Use margining signals from lane configuration
+	 */
+	usb3_reg_set(reg_base, COMPHY_TEST_MODE_CTRL,
+		     MODE_MARGIN_OVERRIDE, REG_16_BIT_MASK);
+
+	/*
+	 * 5. Set Lane-to-Lane Bundle Clock Sampling Period = per PCLK cycles
+	 *    set Mode Clock Source = PCLK is generated from REFCLK
+	 */
+	usb3_reg_set(reg_base, COMPHY_CLK_SRC_LO, 0x0,
+		     (MODE_CLK_SRC | BUNDLE_PERIOD_SEL |
+		      BUNDLE_PERIOD_SCALE_MASK | BUNDLE_SAMPLE_CTRL |
+		      PLL_READY_DLY_MASK));
+
+	/*
+	 * 6. Set G2 Spread Spectrum Clock Amplitude at 4K
+	 */
+	usb3_reg_set(reg_base, COMPHY_GEN2_SET2,
+		     GS2_TX_SSC_AMP_VALUE_20, GS2_TX_SSC_AMP_MASK);
+
+	/*
+	 * 7. Unset G3 Spread Spectrum Clock Amplitude
+	 *    set G3 TX and RX Register Master Current Select
+	 */
+	mask = GS2_TX_SSC_AMP_MASK | GS2_VREG_RXTX_MAS_ISET_MASK |
+		GS2_RSVD_6_0_MASK;
+	usb3_reg_set(reg_base, COMPHY_GEN3_SET2,
+		     GS2_VREG_RXTX_MAS_ISET_60U, mask);
+
+	/*
+	 * 8. Check crystal jumper setting and program the Power and PLL Control
+	 * accordingly Change RX wait
+	 */
+	if (get_ref_clk() == 40) {
+		ref_clk = REF_FREF_SEL_PCIE_USB3_40MHZ;
+		cfg = CFG_PM_RXDLOZ_WAIT_12_UNIT;
+
+	} else {
+		/* 25 MHz */
+		ref_clk = REF_FREF_SEL_PCIE_USB3_25MHZ;
+		cfg = CFG_PM_RXDLOZ_WAIT_7_UNIT;
+	}
+
+	mask = PU_IVREF_BIT | PU_PLL_BIT | PU_RX_BIT | PU_TX_BIT |
+		PU_TX_INTP_BIT | PU_DFE_BIT | PLL_LOCK_BIT | PHY_MODE_MASK |
+		REF_FREF_SEL_MASK;
+	data = PU_IVREF_BIT | PU_PLL_BIT | PU_RX_BIT | PU_TX_BIT |
+		PU_TX_INTP_BIT | PU_DFE_BIT | PHY_MODE_USB3 | ref_clk;
+	usb3_reg_set(reg_base, COMPHY_POWER_PLL_CTRL, data,  mask);
+
+	mask = CFG_PM_OSCCLK_WAIT_MASK | CFG_PM_RXDEN_WAIT_MASK |
+		CFG_PM_RXDLOZ_WAIT_MASK;
+	data = CFG_PM_RXDEN_WAIT_1_UNIT  | cfg;
+	usb3_reg_set(reg_base, COMPHY_PWR_MGM_TIM1, data, mask);
+
+	/*
+	 * 9. Enable idle sync
+	 */
+	data = IDLE_SYNC_EN_DEFAULT_VALUE | IDLE_SYNC_EN;
+	usb3_reg_set(reg_base, COMPHY_IDLE_SYNC_EN, data, REG_16_BIT_MASK);
+
+	/*
+	 * 10. Enable the output of 500M clock
+	 */
+	data = MISC_CTRL0_DEFAULT_VALUE | CLK500M_EN;
+	usb3_reg_set(reg_base, COMPHY_MISC_CTRL0, data, REG_16_BIT_MASK);
+
+	/*
+	 * 11. Set 20-bit data width
+	 */
+	usb3_reg_set(reg_base, COMPHY_DIG_LOOPBACK_EN, DATA_WIDTH_20BIT,
+		     REG_16_BIT_MASK);
+
+	/*
+	 * 12. Override Speed_PLL value and use MAC PLL
+	 */
+	usb3_reg_set(reg_base, COMPHY_KVCO_CAL_CTRL,
+		     (SPEED_PLL_VALUE_16 | USE_MAX_PLL_RATE_BIT),
+		     REG_16_BIT_MASK);
+
+	/*
+	 * 13. Check the Polarity invert bit
+	 */
+	data = 0U;
+	if (invert & COMPHY_POLARITY_TXD_INVERT) {
+		data |= TXD_INVERT_BIT;
+	}
+	if (invert & COMPHY_POLARITY_RXD_INVERT) {
+		data |= RXD_INVERT_BIT;
+	}
+	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
+	usb3_reg_set(reg_base, COMPHY_SYNC_PATTERN, data, mask);
+
+	/*
+	 * 14. Set max speed generation to USB3.0 5Gbps
+	 */
+	usb3_reg_set(reg_base, COMPHY_SYNC_MASK_GEN, PHY_GEN_MAX_USB3_5G,
+		     PHY_GEN_MAX_MASK);
+
+	/*
+	 * 15. Set capacitor value for FFE gain peaking to 0xF
+	 */
+	usb3_reg_set(reg_base, COMPHY_GEN2_SET3,
+		     GS3_FFE_CAP_SEL_VALUE, GS3_FFE_CAP_SEL_MASK);
+
+	/*
+	 * 16. Release SW reset
+	 */
+	data = MODE_CORE_CLK_FREQ_SEL | MODE_PIPE_WIDTH_32 | MODE_REFDIV_BY_4;
+	usb3_reg_set(reg_base, COMPHY_RST_CLK_CTRL, data, REG_16_BIT_MASK);
+
+	/* Wait for > 55 us to allow PCLK be enabled */
+	udelay(PLL_SET_DELAY_US);
+
+	if (comphy_index == COMPHY_LANE2) {
+		data = COMPHY_LANE_STAT1 + USB3PHY_LANE2_REG_BASE_OFFSET;
+		mmio_write_32(reg_base + COMPHY_LANE2_INDIR_ADDR_OFFSET,
+			      data);
+
+		addr = reg_base + COMPHY_LANE2_INDIR_DATA_OFFSET;
+		ret = polling_with_timeout(addr, TXDCLK_PCLK_EN, TXDCLK_PCLK_EN,
+					   COMPHY_PLL_TIMEOUT, REG_32BIT);
+	} else {
+		ret = polling_with_timeout(LANE_STAT1_ADDR(USB3) + reg_base,
+					   TXDCLK_PCLK_EN, TXDCLK_PCLK_EN,
+					   COMPHY_PLL_TIMEOUT, REG_16BIT);
+	}
+	if (ret) {
+		ERROR("Failed to lock USB3 PLL\n");
+		return -ETIMEDOUT;
+	}
+
+	debug_exit();
+
+	return 0;
+}
+
+static int mvebu_a3700_comphy_pcie_power_on(uint8_t comphy_index,
+					    uint32_t comphy_mode)
+{
+	int ret;
+	uint32_t ref_clk;
+	uint32_t mask, data;
+	int invert = COMPHY_GET_POLARITY_INVERT(comphy_mode);
+
+	debug_enter();
+
+	/* Configure phy selector for PCIe */
+	ret = mvebu_a3700_comphy_set_phy_selector(comphy_index, comphy_mode);
+	if (ret) {
+		return ret;
+	}
+
+	/* 1. Enable max PLL. */
+	reg_set16(LANE_CFG1_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  USE_MAX_PLL_RATE_EN, USE_MAX_PLL_RATE_EN);
+
+	/* 2. Select 20 bit SERDES interface. */
+	reg_set16(CLK_SRC_LO_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  CFG_SEL_20B, CFG_SEL_20B);
+
+	/* 3. Force to use reg setting for PCIe mode */
+	reg_set16(MISC_CTRL1_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  SEL_BITS_PCIE_FORCE, SEL_BITS_PCIE_FORCE);
+
+	/* 4. Change RX wait */
+	reg_set16(PWR_MGM_TIM1_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  CFG_PM_RXDEN_WAIT_1_UNIT | CFG_PM_RXDLOZ_WAIT_12_UNIT,
+		  (CFG_PM_OSCCLK_WAIT_MASK | CFG_PM_RXDEN_WAIT_MASK |
+		   CFG_PM_RXDLOZ_WAIT_MASK));
+
+	/* 5. Enable idle sync */
+	reg_set16(IDLE_SYNC_EN_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  IDLE_SYNC_EN_DEFAULT_VALUE | IDLE_SYNC_EN, REG_16_BIT_MASK);
+
+	/* 6. Enable the output of 100M/125M/500M clock */
+	reg_set16(MISC_CTRL0_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  MISC_CTRL0_DEFAULT_VALUE | CLK500M_EN | TXDCLK_2X_SEL | CLK100M_125M_EN,
+		  REG_16_BIT_MASK);
+
+	/*
+	 * 7. Enable TX, PCIE global register, 0xd0074814, it is done in
+	 * PCI-E driver
+	 */
+
+	/*
+	 * 8. Check crystal jumper setting and program the Power and PLL
+	 * Control accordingly
+	 */
+
+	if (get_ref_clk() == 40)
+		ref_clk = REF_FREF_SEL_PCIE_USB3_40MHZ;
+	else
+		ref_clk = REF_FREF_SEL_PCIE_USB3_25MHZ;
+
+	reg_set16(PWR_PLL_CTRL_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  (PU_IVREF_BIT | PU_PLL_BIT | PU_RX_BIT | PU_TX_BIT |
+		   PU_TX_INTP_BIT | PU_DFE_BIT | ref_clk | PHY_MODE_PCIE),
+		  REG_16_BIT_MASK);
+
+	/* 9. Override Speed_PLL value and use MAC PLL */
+	reg_set16(KVCO_CAL_CTRL_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  SPEED_PLL_VALUE_16 | USE_MAX_PLL_RATE_BIT, REG_16_BIT_MASK);
+
+	/* 10. Check the Polarity invert bit */
+	data = 0U;
+	if (invert & COMPHY_POLARITY_TXD_INVERT) {
+		data |= TXD_INVERT_BIT;
+	}
+	if (invert & COMPHY_POLARITY_RXD_INVERT) {
+		data |= RXD_INVERT_BIT;
+	}
+	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
+	reg_set16(SYNC_PATTERN_ADDR(PCIE) + COMPHY_SD_ADDR, data, mask);
+
+	/* 11. Release SW reset */
+	data = MODE_CORE_CLK_FREQ_SEL | MODE_PIPE_WIDTH_32;
+	mask = data | SOFT_RESET | MODE_REFDIV_MASK;
+	reg_set16(RST_CLK_CTRL_ADDR(PCIE) + COMPHY_SD_ADDR, data, mask);
+
+	/* Wait for > 55 us to allow PCLK be enabled */
+	udelay(PLL_SET_DELAY_US);
+
+	ret = polling_with_timeout(LANE_STAT1_ADDR(PCIE) + COMPHY_SD_ADDR,
+				   TXDCLK_PCLK_EN, TXDCLK_PCLK_EN,
+				   COMPHY_PLL_TIMEOUT, REG_16BIT);
+	if (ret) {
+		ERROR("Failed to lock PCIE PLL\n");
+		return -ETIMEDOUT;
+	}
+
+	debug_exit();
+
+	return 0;
+}
+
+int mvebu_3700_comphy_power_on(uint8_t comphy_index, uint32_t comphy_mode)
+{
+	int mode = COMPHY_GET_MODE(comphy_mode);
+	int ret = 0;
+
+	debug_enter();
+
+	switch (mode) {
+	case(COMPHY_SATA_MODE):
+		ret = mvebu_a3700_comphy_sata_power_on(comphy_index,
+						       comphy_mode);
+		break;
+	case(COMPHY_SGMII_MODE):
+	case(COMPHY_2500BASEX_MODE):
+		ret = mvebu_a3700_comphy_sgmii_power_on(comphy_index,
+							comphy_mode);
+		break;
+	case (COMPHY_USB3_MODE):
+	case (COMPHY_USB3H_MODE):
+		ret = mvebu_a3700_comphy_usb3_power_on(comphy_index,
+						       comphy_mode);
+		break;
+	case (COMPHY_PCIE_MODE):
+		ret = mvebu_a3700_comphy_pcie_power_on(comphy_index,
+						       comphy_mode);
+		break;
+	default:
+		ERROR("comphy%d: unsupported comphy mode\n", comphy_index);
+		ret = -EINVAL;
+		break;
+	}
+
+	debug_exit();
+
+	return ret;
+}
+
+static int mvebu_a3700_comphy_usb3_power_off(void)
+{
+	/*
+	 * Currently the USB3 MAC will control the USB3 PHY to set it to low
+	 * state, thus do not need to power off USB3 PHY again.
+	 */
+	debug_enter();
+	debug_exit();
+
+	return 0;
+}
+
+static int mvebu_a3700_comphy_sata_power_off(void)
+{
+	uintptr_t comphy_indir_regs = COMPHY_INDIRECT_REG;
+	uint32_t offset;
+
+	debug_enter();
+
+	/* Set phy isolation mode */
+	offset = COMPHY_ISOLATION_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
+	comphy_sata_set_indirect(comphy_indir_regs, offset, PHY_ISOLATE_MODE,
+				 PHY_ISOLATE_MODE);
+
+	/* Power off PLL, Tx, Rx */
+	offset = COMPHY_POWER_PLL_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
+	comphy_sata_set_indirect(comphy_indir_regs, offset, 0,
+				 PU_PLL_BIT | PU_RX_BIT | PU_TX_BIT);
+
+	debug_exit();
+
+	return 0;
+}
+
+int mvebu_3700_comphy_power_off(uint8_t comphy_index, uint32_t comphy_mode)
+{
+	int mode = COMPHY_GET_MODE(comphy_mode);
+	int err = 0;
+
+	debug_enter();
+
+	if (!mode) {
+		/*
+		 * The user did not specify which mode should be powered off.
+		 * In this case we can identify this by reading the phy selector
+		 * register.
+		 */
+		mode = mvebu_a3700_comphy_get_mode(comphy_index);
+	}
+
+	switch (mode) {
+	case(COMPHY_SGMII_MODE):
+	case(COMPHY_2500BASEX_MODE):
+		err = mvebu_a3700_comphy_sgmii_power_off(comphy_index);
+		break;
+	case (COMPHY_USB3_MODE):
+	case (COMPHY_USB3H_MODE):
+		err = mvebu_a3700_comphy_usb3_power_off();
+		break;
+	case (COMPHY_SATA_MODE):
+		err = mvebu_a3700_comphy_sata_power_off();
+		break;
+
+	default:
+		debug("comphy%d: power off is not implemented for mode %d\n",
+		      comphy_index, mode);
+		break;
+	}
+
+	debug_exit();
+
+	return err;
+}
+
+static int mvebu_a3700_comphy_sata_is_pll_locked(void)
+{
+	uint32_t data, addr;
+	uintptr_t comphy_indir_regs = COMPHY_INDIRECT_REG;
+	int ret = 0;
+
+	debug_enter();
+
+	/* Polling status */
+	mmio_write_32(comphy_indir_regs + COMPHY_LANE2_INDIR_ADDR_OFFSET,
+	       COMPHY_DIG_LOOPBACK_EN + SATAPHY_LANE2_REG_BASE_OFFSET);
+	addr = comphy_indir_regs + COMPHY_LANE2_INDIR_DATA_OFFSET;
+	data = polling_with_timeout(addr, PLL_READY_TX_BIT, PLL_READY_TX_BIT,
+				    COMPHY_PLL_TIMEOUT, REG_32BIT);
+
+	if (data != 0) {
+		ERROR("TX PLL is not locked\n");
+		ret = -ETIMEDOUT;
+	}
+
+	debug_exit();
+
+	return ret;
+}
+
+int mvebu_3700_comphy_is_pll_locked(uint8_t comphy_index, uint32_t comphy_mode)
+{
+	int mode = COMPHY_GET_MODE(comphy_mode);
+	int ret = 0;
+
+	debug_enter();
+
+	switch (mode) {
+	case(COMPHY_SATA_MODE):
+		ret = mvebu_a3700_comphy_sata_is_pll_locked();
+		break;
+
+	default:
+		ERROR("comphy[%d] mode[%d] doesn't support PLL lock check\n",
+			comphy_index, mode);
+		ret = -EINVAL;
+		break;
+	}
+
+	debug_exit();
+
+	return ret;
+}
diff --git a/drivers/marvell/comphy/phy-comphy-3700.h b/drivers/marvell/comphy/phy-comphy-3700.h
new file mode 100644
index 0000000..ed07624
--- /dev/null
+++ b/drivers/marvell/comphy/phy-comphy-3700.h
@@ -0,0 +1,249 @@
+/*
+ * Copyright (C) 2018-2021 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef PHY_COMPHY_3700_H
+#define PHY_COMPHY_3700_H
+
+#define PLL_SET_DELAY_US			600
+#define COMPHY_PLL_TIMEOUT			1000
+#define REG_16_BIT_MASK				0xFFFF
+
+#define COMPHY_SELECTOR_PHY_REG			0xFC
+/* bit0: 0: Lane1 is GbE0; 1: Lane1 is PCIE */
+#define COMPHY_SELECTOR_PCIE_GBE0_SEL_BIT	BIT(0)
+/* bit4: 0: Lane0 is GbE1; 1: Lane0 is USB3 */
+#define COMPHY_SELECTOR_USB3_GBE1_SEL_BIT	BIT(4)
+/* bit8: 0: Lane0 is USB3 instead of GbE1, Lane2 is SATA; 1: Lane2 is USB3 */
+#define COMPHY_SELECTOR_USB3_PHY_SEL_BIT	BIT(8)
+
+/* SATA PHY register offset */
+#define SATAPHY_LANE2_REG_BASE_OFFSET		0x200
+
+/* USB3 PHY offset compared to SATA PHY */
+#define USB3PHY_LANE2_REG_BASE_OFFSET		0x200
+
+/* Comphy lane2 indirect access register offset */
+#define COMPHY_LANE2_INDIR_ADDR_OFFSET		0x0
+#define COMPHY_LANE2_INDIR_DATA_OFFSET		0x4
+
+/* PHY shift to get related register address */
+enum {
+	PCIE = 1,
+	USB3,
+};
+
+#define PCIEPHY_SHFT		2
+#define USB3PHY_SHFT		2
+#define PHY_SHFT(unit)		((unit == PCIE) ? PCIEPHY_SHFT : USB3PHY_SHFT)
+
+/* PHY register */
+#define COMPHY_POWER_PLL_CTRL		0x01
+#define PWR_PLL_CTRL_ADDR(unit)		(COMPHY_POWER_PLL_CTRL * PHY_SHFT(unit))
+#define PU_IVREF_BIT			BIT(15)
+#define PU_PLL_BIT			BIT(14)
+#define PU_RX_BIT			BIT(13)
+#define PU_TX_BIT			BIT(12)
+#define PU_TX_INTP_BIT			BIT(11)
+#define PU_DFE_BIT			BIT(10)
+#define RESET_DTL_RX_BIT		BIT(9)
+#define PLL_LOCK_BIT			BIT(8)
+#define REF_FREF_SEL_OFFSET		0
+#define REF_FREF_SEL_MASK		(0x1F << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_SERDES_25MHZ	(0x1 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_SERDES_40MHZ	(0x3 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_SERDES_50MHZ	(0x4 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_PCIE_USB3_25MHZ	(0x2 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_PCIE_USB3_40MHZ	(0x3 << REF_FREF_SEL_OFFSET)
+#define PHY_MODE_OFFSET			5
+#define PHY_MODE_MASK			(7 << PHY_MODE_OFFSET)
+#define PHY_MODE_SATA			(0x0 << PHY_MODE_OFFSET)
+#define PHY_MODE_PCIE			(0x3 << PHY_MODE_OFFSET)
+#define PHY_MODE_SGMII			(0x4 << PHY_MODE_OFFSET)
+#define PHY_MODE_USB3			(0x5 << PHY_MODE_OFFSET)
+
+#define COMPHY_KVCO_CAL_CTRL		0x02
+#define KVCO_CAL_CTRL_ADDR(unit)	(COMPHY_KVCO_CAL_CTRL * PHY_SHFT(unit))
+#define USE_MAX_PLL_RATE_BIT		BIT(12)
+#define SPEED_PLL_OFFSET		2
+#define SPEED_PLL_MASK			(0x3F << SPEED_PLL_OFFSET)
+#define SPEED_PLL_VALUE_16		(0x10 << SPEED_PLL_OFFSET)
+
+#define COMPHY_DIG_LOOPBACK_EN		0x23
+#define DIG_LOOPBACK_EN_ADDR(unit)	(COMPHY_DIG_LOOPBACK_EN * \
+					 PHY_SHFT(unit))
+#define SEL_DATA_WIDTH_OFFSET		10
+#define SEL_DATA_WIDTH_MASK		(0x3 << SEL_DATA_WIDTH_OFFSET)
+#define DATA_WIDTH_10BIT		(0x0 << SEL_DATA_WIDTH_OFFSET)
+#define DATA_WIDTH_20BIT		(0x1 << SEL_DATA_WIDTH_OFFSET)
+#define DATA_WIDTH_40BIT		(0x2 << SEL_DATA_WIDTH_OFFSET)
+#define PLL_READY_TX_BIT		BIT(4)
+
+#define COMPHY_SYNC_PATTERN		0x24
+#define SYNC_PATTERN_ADDR(unit)		(COMPHY_SYNC_PATTERN * PHY_SHFT(unit))
+#define TXD_INVERT_BIT			BIT(10)
+#define RXD_INVERT_BIT			BIT(11)
+
+#define COMPHY_SYNC_MASK_GEN		0x25
+#define PHY_GEN_MAX_OFFSET		10
+#define PHY_GEN_MAX_MASK		(3 << PHY_GEN_MAX_OFFSET)
+#define PHY_GEN_MAX_USB3_5G		(1 << PHY_GEN_MAX_OFFSET)
+
+#define COMPHY_ISOLATION_CTRL		0x26
+#define ISOLATION_CTRL_ADDR(unit)	(COMPHY_ISOLATION_REG * PHY_SHFT(unit))
+#define PHY_ISOLATE_MODE		BIT(15)
+
+#define COMPHY_GEN2_SET2		0x3e
+#define GEN2_SET2_ADDR(unit)		(COMPHY_GEN2_SET2 * PHY_SHFT(unit))
+#define GS2_TX_SSC_AMP_VALUE_20		BIT(14)
+#define GS2_TX_SSC_AMP_OFF		9
+#define GS2_TX_SSC_AMP_LEN		7
+#define GS2_TX_SSC_AMP_MASK		(((1 << GS2_TX_SSC_AMP_LEN) - 1) << \
+					 GS2_TX_SSC_AMP_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_OFF	7
+#define GS2_VREG_RXTX_MAS_ISET_60U	(0 << GS2_VREG_RXTX_MAS_ISET_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_80U	(1 << GS2_VREG_RXTX_MAS_ISET_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_100U	(2 << GS2_VREG_RXTX_MAS_ISET_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_120U	(3 << GS2_VREG_RXTX_MAS_ISET_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_MASK	(BIT(7) | BIT(8))
+#define GS2_RSVD_6_0_OFF		0
+#define GS2_RSVD_6_0_LEN		7
+#define GS2_RSVD_6_0_MASK		(((1 << GS2_RSVD_6_0_LEN) - 1) << \
+					 GS2_RSVD_6_0_OFF)
+
+#define COMPHY_GEN3_SET2		0x3f
+#define GEN3_SET2_ADDR(unit)		(COMPHY_GEN3_SET2 * PHY_SHFT(unit))
+
+#define COMPHY_IDLE_SYNC_EN		0x48
+#define IDLE_SYNC_EN_ADDR(unit)		(COMPHY_IDLE_SYNC_EN * PHY_SHFT(unit))
+#define IDLE_SYNC_EN			BIT(12)
+#define IDLE_SYNC_EN_DEFAULT_VALUE	0x60
+
+#define COMPHY_MISC_CTRL0		0x4F
+#define MISC_CTRL0_ADDR(unit)		(COMPHY_MISC_CTRL0 * PHY_SHFT(unit))
+#define CLK100M_125M_EN			BIT(4)
+#define TXDCLK_2X_SEL			BIT(6)
+#define CLK500M_EN			BIT(7)
+#define PHY_REF_CLK_SEL			BIT(10)
+#define MISC_CTRL0_DEFAULT_VALUE	0xA00D
+
+#define COMPHY_MISC_CTRL1		0x73
+#define MISC_CTRL1_ADDR(unit)		(COMPHY_MISC_CTRL1 * PHY_SHFT(unit))
+#define SEL_BITS_PCIE_FORCE		BIT(15)
+
+#define COMPHY_GEN2_SET3		0x112
+#define GS3_FFE_CAP_SEL_MASK		0xF
+#define GS3_FFE_CAP_SEL_VALUE		0xF
+
+#define COMPHY_LANE_CFG0		0x180
+#define LANE_CFG0_ADDR(unit)		(COMPHY_LANE_CFG0 * PHY_SHFT(unit))
+#define PRD_TXDEEMPH0_MASK		BIT(0)
+#define PRD_TXMARGIN_MASK		(BIT(1) | BIT(2) | BIT(3))
+#define PRD_TXSWING_MASK		BIT(4)
+#define CFG_TX_ALIGN_POS_MASK		(BIT(5) | BIT(6) | BIT(7) | BIT(8))
+
+#define COMPHY_LANE_CFG1		0x181
+#define LANE_CFG1_ADDR(unit)		(COMPHY_LANE_CFG1 * PHY_SHFT(unit))
+#define PRD_TXDEEMPH1_MASK		BIT(15)
+#define USE_MAX_PLL_RATE_EN		BIT(9)
+#define TX_DET_RX_MODE			BIT(6)
+#define GEN2_TX_DATA_DLY_MASK		(BIT(3) | BIT(4))
+#define GEN2_TX_DATA_DLY_DEFT		(2 << 3)
+#define TX_ELEC_IDLE_MODE_EN		BIT(0)
+
+#define COMPHY_LANE_STAT1		0x183
+#define LANE_STAT1_ADDR(unit)		(COMPHY_LANE_STAT1 * PHY_SHFT(unit))
+#define TXDCLK_PCLK_EN				BIT(0)
+
+#define COMPHY_LANE_CFG4		0x188
+#define LANE_CFG4_ADDR(unit)		(COMPHY_LANE_CFG4 * PHY_SHFT(unit))
+#define SPREAD_SPECTRUM_CLK_EN		BIT(7)
+
+#define COMPHY_RST_CLK_CTRL		0x1C1
+#define RST_CLK_CTRL_ADDR(unit)		(COMPHY_RST_CLK_CTRL * PHY_SHFT(unit))
+#define SOFT_RESET			BIT(0)
+#define MODE_CORE_CLK_FREQ_SEL		BIT(9)
+#define MODE_PIPE_WIDTH_32		BIT(3)
+#define MODE_REFDIV_OFFSET		4
+#define MODE_REFDIV_LEN			2
+#define MODE_REFDIV_MASK		(0x3 << MODE_REFDIV_OFFSET)
+#define MODE_REFDIV_BY_4		(0x2 << MODE_REFDIV_OFFSET)
+
+#define COMPHY_TEST_MODE_CTRL		0x1C2
+#define TEST_MODE_CTRL_ADDR(unit)	(COMPHY_TEST_MODE_CTRL * PHY_SHFT(unit))
+#define MODE_MARGIN_OVERRIDE		BIT(2)
+
+#define COMPHY_CLK_SRC_LO		0x1C3
+#define CLK_SRC_LO_ADDR(unit)		(COMPHY_CLK_SRC_LO * PHY_SHFT(unit))
+#define MODE_CLK_SRC			BIT(0)
+#define BUNDLE_PERIOD_SEL		BIT(1)
+#define BUNDLE_PERIOD_SCALE_MASK	(BIT(2) | BIT(3))
+#define BUNDLE_SAMPLE_CTRL		BIT(4)
+#define PLL_READY_DLY_MASK		(BIT(5) | BIT(6) | BIT(7))
+#define CFG_SEL_20B			BIT(15)
+
+#define COMPHY_PWR_MGM_TIM1		0x1D0
+#define PWR_MGM_TIM1_ADDR(unit)		(COMPHY_PWR_MGM_TIM1 * PHY_SHFT(unit))
+#define CFG_PM_OSCCLK_WAIT_OFF		12
+#define CFG_PM_OSCCLK_WAIT_LEN		4
+#define CFG_PM_OSCCLK_WAIT_MASK		(((1 << CFG_PM_OSCCLK_WAIT_LEN) - 1) \
+					 << CFG_PM_OSCCLK_WAIT_OFF)
+#define CFG_PM_RXDEN_WAIT_OFF		8
+#define CFG_PM_RXDEN_WAIT_LEN		4
+#define CFG_PM_RXDEN_WAIT_MASK		(((1 << CFG_PM_RXDEN_WAIT_LEN) - 1) \
+					 << CFG_PM_RXDEN_WAIT_OFF)
+#define CFG_PM_RXDEN_WAIT_1_UNIT	(1 << CFG_PM_RXDEN_WAIT_OFF)
+#define CFG_PM_RXDLOZ_WAIT_OFF		0
+#define CFG_PM_RXDLOZ_WAIT_LEN		8
+#define CFG_PM_RXDLOZ_WAIT_MASK		(((1 << CFG_PM_RXDLOZ_WAIT_LEN) - 1) \
+					 << CFG_PM_RXDLOZ_WAIT_OFF)
+#define CFG_PM_RXDLOZ_WAIT_7_UNIT	(7 << CFG_PM_RXDLOZ_WAIT_OFF)
+#define CFG_PM_RXDLOZ_WAIT_12_UNIT	(0xC << CFG_PM_RXDLOZ_WAIT_OFF)
+
+/*
+ * This register is not from PHY lane register space. It only exists in the
+ * indirect register space, before the actual PHY lane 2 registers. So the
+ * offset is absolute, not relative to SATAPHY_LANE2_REG_BASE_OFFSET.
+ * It is used only for SATA PHY initialization.
+ */
+#define COMPHY_RESERVED_REG		0x0E
+#define PHYCTRL_FRM_PIN_BIT		BIT(13)
+
+/* SGMII */
+#define COMPHY_PHY_CFG1_OFFSET(lane)	((1 - (lane)) * 0x28)
+#define PIN_PU_IVREF_BIT		BIT(1)
+#define PIN_RESET_CORE_BIT		BIT(11)
+#define PIN_RESET_COMPHY_BIT		BIT(12)
+#define PIN_PU_PLL_BIT			BIT(16)
+#define PIN_PU_RX_BIT			BIT(17)
+#define PIN_PU_TX_BIT			BIT(18)
+#define PIN_TX_IDLE_BIT			BIT(19)
+#define GEN_RX_SEL_OFFSET		22
+#define GEN_RX_SEL_MASK			(0xF << GEN_RX_SEL_OFFSET)
+#define GEN_TX_SEL_OFFSET		26
+#define GEN_TX_SEL_MASK			(0xF << GEN_TX_SEL_OFFSET)
+#define PHY_RX_INIT_BIT			BIT(30)
+#define SD_SPEED_1_25_G			0x6
+#define SD_SPEED_3_125_G		0x8
+
+/* COMPHY status reg:
+ * lane0: USB3/GbE1 PHY Status 1
+ * lane1: PCIe/GbE0 PHY Status 1
+ */
+#define COMPHY_PHY_STATUS_OFFSET(lane)	(0x18 + (1 - (lane)) * 0x28)
+#define PHY_RX_INIT_DONE_BIT		BIT(0)
+#define PHY_PLL_READY_RX_BIT		BIT(2)
+#define PHY_PLL_READY_TX_BIT		BIT(3)
+
+#define SGMIIPHY_ADDR(off, base)	((((off) & 0x00007FF) * 2) + (base))
+
+#define MAX_LANE_NR			3
+
+/* comphy API */
+int mvebu_3700_comphy_is_pll_locked(uint8_t comphy_index, uint32_t comphy_mode);
+int mvebu_3700_comphy_power_off(uint8_t comphy_index, uint32_t comphy_mode);
+int mvebu_3700_comphy_power_on(uint8_t comphy_index, uint32_t comphy_mode);
+#endif /* PHY_COMPHY_3700_H */
diff --git a/drivers/marvell/comphy/phy-comphy-common.h b/drivers/marvell/comphy/phy-comphy-common.h
new file mode 100644
index 0000000..ba5d255
--- /dev/null
+++ b/drivers/marvell/comphy/phy-comphy-common.h
@@ -0,0 +1,167 @@
+/*
+ * Copyright (C) 2018-2021 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* Marvell CP110 ana A3700 common */
+
+#ifndef PHY_COMPHY_COMMON_H
+#define PHY_COMPHY_COMMON_H
+
+/* #define DEBUG_COMPHY */
+#ifdef DEBUG_COMPHY
+#define debug(format...) printf(format)
+#else
+#define debug(format, arg...)
+#endif
+
+/* A lane is described by 4 fields:
+ *  - bit 1~0 represent comphy polarity invert
+ *  - bit 7~2 represent comphy speed
+ *  - bit 11~8 represent unit index
+ *  - bit 16~12 represent mode
+ *  - bit 17 represent comphy indication of clock source
+ *  - bit 20~18 represents pcie width (in case of pcie comphy config.)
+ *  - bit 21 represents the source of the request (Linux/Bootloader),
+ *           (reguired only for PCIe!)
+ *  - bit 31~22 reserved
+ */
+
+#define COMPHY_INVERT_OFFSET	0
+#define COMPHY_INVERT_LEN	2
+#define COMPHY_INVERT_MASK	COMPHY_MASK(COMPHY_INVERT_OFFSET, \
+						COMPHY_INVERT_LEN)
+#define COMPHY_SPEED_OFFSET	(COMPHY_INVERT_OFFSET + COMPHY_INVERT_LEN)
+#define COMPHY_SPEED_LEN	6
+#define COMPHY_SPEED_MASK	COMPHY_MASK(COMPHY_SPEED_OFFSET, \
+						COMPHY_SPEED_LEN)
+#define COMPHY_UNIT_ID_OFFSET	(COMPHY_SPEED_OFFSET + COMPHY_SPEED_LEN)
+#define COMPHY_UNIT_ID_LEN	4
+#define COMPHY_UNIT_ID_MASK	COMPHY_MASK(COMPHY_UNIT_ID_OFFSET, \
+						COMPHY_UNIT_ID_LEN)
+#define COMPHY_MODE_OFFSET	(COMPHY_UNIT_ID_OFFSET + COMPHY_UNIT_ID_LEN)
+#define COMPHY_MODE_LEN		5
+#define COMPHY_MODE_MASK	COMPHY_MASK(COMPHY_MODE_OFFSET, COMPHY_MODE_LEN)
+#define COMPHY_CLK_SRC_OFFSET	(COMPHY_MODE_OFFSET + COMPHY_MODE_LEN)
+#define COMPHY_CLK_SRC_LEN	1
+#define COMPHY_CLK_SRC_MASK	COMPHY_MASK(COMPHY_CLK_SRC_OFFSET, \
+						COMPHY_CLK_SRC_LEN)
+#define COMPHY_PCI_WIDTH_OFFSET	(COMPHY_CLK_SRC_OFFSET + COMPHY_CLK_SRC_LEN)
+#define COMPHY_PCI_WIDTH_LEN	3
+#define COMPHY_PCI_WIDTH_MASK	COMPHY_MASK(COMPHY_PCI_WIDTH_OFFSET, \
+						COMPHY_PCI_WIDTH_LEN)
+#define COMPHY_PCI_CALLER_OFFSET \
+			(COMPHY_PCI_WIDTH_OFFSET + COMPHY_PCI_WIDTH_LEN)
+#define COMPHY_PCI_CALLER_LEN	1
+#define COMPHY_PCI_CALLER_MASK	COMPHY_MASK(COMPHY_PCI_CALLER_OFFSET, \
+						COMPHY_PCI_CALLER_LEN)
+
+#define COMPHY_MASK(offset, len)	(((1 << (len)) - 1) << (offset))
+
+/* Macro which extracts mode from lane description */
+#define COMPHY_GET_MODE(x)		(((x) & COMPHY_MODE_MASK) >> \
+						COMPHY_MODE_OFFSET)
+/* Macro which extracts unit index from lane description */
+#define COMPHY_GET_ID(x)		(((x) & COMPHY_UNIT_ID_MASK) >> \
+						COMPHY_UNIT_ID_OFFSET)
+/* Macro which extracts speed from lane description */
+#define COMPHY_GET_SPEED(x)		(((x) & COMPHY_SPEED_MASK) >> \
+						COMPHY_SPEED_OFFSET)
+/* Macro which extracts clock source indication from lane description */
+#define COMPHY_GET_CLK_SRC(x)		(((x) & COMPHY_CLK_SRC_MASK) >> \
+						COMPHY_CLK_SRC_OFFSET)
+/* Macro which extracts pcie width indication from lane description */
+#define COMPHY_GET_PCIE_WIDTH(x)	(((x) & COMPHY_PCI_WIDTH_MASK) >> \
+						COMPHY_PCI_WIDTH_OFFSET)
+
+/* Macro which extracts the caller for pcie power on from lane description */
+#define COMPHY_GET_CALLER(x)		(((x) & COMPHY_PCI_CALLER_MASK) >> \
+						COMPHY_PCI_CALLER_OFFSET)
+
+/* Macro which extracts the polarity invert from lane description */
+#define COMPHY_GET_POLARITY_INVERT(x)	(((x) & COMPHY_INVERT_MASK) >> \
+						COMPHY_INVERT_OFFSET)
+
+
+#define COMPHY_SATA_MODE	0x1
+#define COMPHY_SGMII_MODE	0x2	/* SGMII 1G */
+#define COMPHY_2500BASEX_MODE	0x3	/* 2500Base-X */
+#define COMPHY_USB3H_MODE	0x4
+#define COMPHY_USB3D_MODE	0x5
+#define COMPHY_PCIE_MODE	0x6
+#define COMPHY_RXAUI_MODE	0x7
+#define COMPHY_XFI_MODE		0x8
+#define COMPHY_SFI_MODE		0x9
+#define COMPHY_USB3_MODE	0xa
+#define COMPHY_AP_MODE		0xb
+
+#define	COMPHY_UNUSED		0xFFFFFFFF
+
+/* Polarity invert macro */
+#define COMPHY_POLARITY_NO_INVERT	0
+#define COMPHY_POLARITY_TXD_INVERT	1
+#define COMPHY_POLARITY_RXD_INVERT	2
+#define COMPHY_POLARITY_ALL_INVERT	(COMPHY_POLARITY_TXD_INVERT | \
+					 COMPHY_POLARITY_RXD_INVERT)
+
+enum reg_width_type {
+	REG_16BIT = 0,
+	REG_32BIT,
+};
+
+enum {
+	COMPHY_LANE0 = 0,
+	COMPHY_LANE1,
+	COMPHY_LANE2,
+	COMPHY_LANE3,
+	COMPHY_LANE4,
+	COMPHY_LANE5,
+	COMPHY_LANE_MAX,
+};
+
+static inline uint32_t polling_with_timeout(uintptr_t addr, uint32_t val,
+					    uint32_t mask,
+					    uint32_t usec_timeout,
+					    enum reg_width_type type)
+{
+	uint32_t data;
+
+	do {
+		udelay(1);
+		if (type == REG_16BIT)
+			data = mmio_read_16(addr) & mask;
+		else
+			data = mmio_read_32(addr) & mask;
+	} while (data != val  && --usec_timeout > 0);
+
+	if (usec_timeout == 0)
+		return data;
+
+	return 0;
+}
+
+static inline void reg_set(uintptr_t addr, uint32_t data, uint32_t mask)
+{
+	debug("<atf>: WR to addr = 0x%lx, data = 0x%x (mask = 0x%x) - ",
+	      addr, data, mask);
+	debug("old value = 0x%x ==> ", mmio_read_32(addr));
+	mmio_clrsetbits_32(addr, mask, data & mask);
+
+	debug("new val 0x%x\n", mmio_read_32(addr));
+}
+
+static inline void __unused reg_set16(uintptr_t addr, uint16_t data,
+				      uint16_t mask)
+{
+
+	debug("<atf>: WR to addr = 0x%lx, data = 0x%x (mask = 0x%x) - ",
+	      addr, data, mask);
+	debug("old value = 0x%x ==> ", mmio_read_16(addr));
+	mmio_clrsetbits_16(addr, mask, data & mask);
+
+	debug("new val 0x%x\n", mmio_read_16(addr));
+}
+
+#endif /* PHY_COMPHY_COMMON_H */
diff --git a/drivers/marvell/comphy/phy-comphy-cp110.c b/drivers/marvell/comphy/phy-comphy-cp110.c
new file mode 100644
index 0000000..fa9fe41
--- /dev/null
+++ b/drivers/marvell/comphy/phy-comphy-cp110.c
@@ -0,0 +1,2528 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* Marvell CP110 SoC COMPHY unit driver */
+
+#include <errno.h>
+#include <inttypes.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <mg_conf_cm3/mg_conf_cm3.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+
+#include <mvebu_def.h>
+#include "mvebu.h"
+#include "comphy-cp110.h"
+#include "phy-comphy-cp110.h"
+#include "phy-comphy-common.h"
+
+#if __has_include("phy-porting-layer.h")
+#include "phy-porting-layer.h"
+#else
+#include "phy-default-porting-layer.h"
+#endif
+
+/* COMPHY speed macro */
+#define COMPHY_SPEED_1_25G		0 /* SGMII 1G */
+#define COMPHY_SPEED_2_5G		1
+#define COMPHY_SPEED_3_125G		2 /* 2500Base-X */
+#define COMPHY_SPEED_5G			3
+#define COMPHY_SPEED_5_15625G		4 /* XFI 5G */
+#define COMPHY_SPEED_6G			5
+#define COMPHY_SPEED_10_3125G		6 /* XFI 10G */
+#define COMPHY_SPEED_MAX		0x3F
+/* The  default speed for IO with fixed known speed */
+#define COMPHY_SPEED_DEFAULT		COMPHY_SPEED_MAX
+
+/* Commands for comphy driver */
+#define COMPHY_COMMAND_DIGITAL_PWR_OFF		0x00000001
+#define COMPHY_COMMAND_DIGITAL_PWR_ON		0x00000002
+
+#define COMPHY_PIPE_FROM_COMPHY_ADDR(x)	((x & ~0xffffff) + 0x120000)
+
+/* System controller registers */
+#define PCIE_MAC_RESET_MASK_PORT0	BIT(13)
+#define PCIE_MAC_RESET_MASK_PORT1	BIT(11)
+#define PCIE_MAC_RESET_MASK_PORT2	BIT(12)
+#define SYS_CTRL_UINIT_SOFT_RESET_REG	0x268
+#define SYS_CTRL_FROM_COMPHY_ADDR(x)	((x & ~0xffffff) + 0x440000)
+
+/* DFX register spaces */
+#define SAR_RST_PCIE0_CLOCK_CONFIG_CP0_OFFSET	(30)
+#define SAR_RST_PCIE0_CLOCK_CONFIG_CP0_MASK	(0x1UL << \
+					SAR_RST_PCIE0_CLOCK_CONFIG_CP0_OFFSET)
+#define SAR_RST_PCIE1_CLOCK_CONFIG_CP0_OFFSET	(31)
+#define SAR_RST_PCIE1_CLOCK_CONFIG_CP0_MASK	(0x1UL << \
+					SAR_RST_PCIE1_CLOCK_CONFIG_CP0_OFFSET)
+#define SAR_STATUS_0_REG			0x40600
+#define DFX_FROM_COMPHY_ADDR(x)			((x & ~0xffffff) + DFX_BASE)
+/* Common Phy training  */
+#define COMPHY_TRX_TRAIN_COMPHY_OFFS		0x1000
+#define COMPHY_TRX_TRAIN_RX_TRAIN_ENABLE	0x1
+#define COMPHY_TRX_RELATIVE_ADDR(comphy_index)	(comphy_train_base + \
+			(comphy_index) * COMPHY_TRX_TRAIN_COMPHY_OFFS)
+
+/* The same Units Soft Reset Config register are accessed in all PCIe ports
+ * initialization, so a spin lock is defined in case when more than 1 CPUs
+ * resets PCIe MAC and need to access the register in the same time. The spin
+ * lock is shared by all CP110 units.
+ */
+spinlock_t cp110_mac_reset_lock;
+
+/* These values come from the PCI Express Spec */
+enum pcie_link_width {
+	PCIE_LNK_WIDTH_RESRV	= 0x00,
+	PCIE_LNK_X1		= 0x01,
+	PCIE_LNK_X2		= 0x02,
+	PCIE_LNK_X4		= 0x04,
+	PCIE_LNK_X8		= 0x08,
+	PCIE_LNK_X12		= 0x0C,
+	PCIE_LNK_X16		= 0x10,
+	PCIE_LNK_X32		= 0x20,
+	PCIE_LNK_WIDTH_UNKNOWN  = 0xFF,
+};
+
+_Bool rx_trainng_done[AP_NUM][CP_NUM][MAX_LANE_NR] = {0};
+
+static void mvebu_cp110_get_ap_and_cp_nr(uint8_t *ap_nr, uint8_t *cp_nr,
+					 uint64_t comphy_base)
+{
+#if (AP_NUM == 1)
+	*ap_nr = 0;
+#else
+	*ap_nr = (((comphy_base & ~0xffffff) - MVEBU_AP_IO_BASE(0)) /
+			 AP_IO_OFFSET);
+#endif
+
+	*cp_nr = (((comphy_base & ~0xffffff) - MVEBU_AP_IO_BASE(*ap_nr)) /
+		 MVEBU_CP_OFFSET);
+
+	debug("cp_base 0x%" PRIx64 ", ap_io_base 0x%lx, cp_offset 0x%lx\n",
+	       comphy_base, (unsigned long)MVEBU_AP_IO_BASE(*ap_nr),
+	       (unsigned long)MVEBU_CP_OFFSET);
+}
+
+/* Clear PIPE selector - avoid collision with previous configuration */
+static void mvebu_cp110_comphy_clr_pipe_selector(uint64_t comphy_base,
+						 uint8_t comphy_index)
+{
+	uint32_t reg, mask, field;
+	uint32_t comphy_offset =
+			COMMON_SELECTOR_COMPHYN_FIELD_WIDTH * comphy_index;
+
+	mask = COMMON_SELECTOR_COMPHY_MASK << comphy_offset;
+	reg = mmio_read_32(comphy_base + COMMON_SELECTOR_PIPE_REG_OFFSET);
+	field = reg & mask;
+
+	if (field) {
+		reg &= ~mask;
+		mmio_write_32(comphy_base + COMMON_SELECTOR_PIPE_REG_OFFSET,
+			     reg);
+	}
+}
+
+/* Clear PHY selector - avoid collision with previous configuration */
+static void mvebu_cp110_comphy_clr_phy_selector(uint64_t comphy_base,
+						uint8_t comphy_index)
+{
+	uint32_t reg, mask, field;
+	uint32_t comphy_offset =
+			COMMON_SELECTOR_COMPHYN_FIELD_WIDTH * comphy_index;
+
+	mask = COMMON_SELECTOR_COMPHY_MASK << comphy_offset;
+	reg = mmio_read_32(comphy_base + COMMON_SELECTOR_PHY_REG_OFFSET);
+	field = reg & mask;
+
+	/* Clear comphy selector - if it was already configured.
+	 * (might be that this comphy was configured as PCIe/USB,
+	 * in such case, no need to clear comphy selector because PCIe/USB
+	 * are controlled by hpipe selector).
+	 */
+	if (field) {
+		reg &= ~mask;
+		mmio_write_32(comphy_base + COMMON_SELECTOR_PHY_REG_OFFSET,
+			      reg);
+	}
+}
+
+/* PHY selector configures SATA and Network modes */
+static void mvebu_cp110_comphy_set_phy_selector(uint64_t comphy_base,
+				     uint8_t comphy_index, uint32_t comphy_mode)
+{
+	uint32_t reg, mask;
+	uint32_t comphy_offset =
+			COMMON_SELECTOR_COMPHYN_FIELD_WIDTH * comphy_index;
+	int mode;
+
+	/* If phy selector is used the pipe selector should be marked as
+	 * unconnected.
+	 */
+	mvebu_cp110_comphy_clr_pipe_selector(comphy_base, comphy_index);
+
+	/* Comphy mode (compound of the IO mode and id). Here, only the IO mode
+	 * is required to distinguish between SATA and network modes.
+	 */
+	mode = COMPHY_GET_MODE(comphy_mode);
+
+	mask = COMMON_SELECTOR_COMPHY_MASK << comphy_offset;
+	reg = mmio_read_32(comphy_base + COMMON_SELECTOR_PHY_REG_OFFSET);
+	reg &= ~mask;
+
+	/* SATA port 0/1 require the same configuration */
+	if (mode == COMPHY_SATA_MODE) {
+		/* SATA selector values is always 4 */
+		reg |= COMMON_SELECTOR_COMPHYN_SATA << comphy_offset;
+	} else {
+		switch (comphy_index) {
+		case(0):
+		case(1):
+		case(2):
+			/* For comphy 0,1, and 2:
+			 * Network selector value is always 1.
+			 */
+			reg |= COMMON_SELECTOR_COMPHY0_1_2_NETWORK <<
+				comphy_offset;
+			break;
+		case(3):
+			/* For comphy 3:
+			 * 0x1 = RXAUI_Lane1
+			 * 0x2 = SGMII/Base-X Port1
+			 */
+			if (mode == COMPHY_RXAUI_MODE)
+				reg |= COMMON_SELECTOR_COMPHY3_RXAUI <<
+					comphy_offset;
+			else
+				reg |= COMMON_SELECTOR_COMPHY3_SGMII <<
+					comphy_offset;
+			break;
+		case(4):
+			 /* For comphy 4:
+			  * 0x1 = SGMII/Base-X Port1, XFI1/SFI1
+			  * 0x2 = SGMII/Base-X Port0: XFI0/SFI0, RXAUI_Lane0
+			  *
+			  * We want to check if SGMII1 is the
+			  * requested mode in order to determine which value
+			  * should be set (all other modes use the same value)
+			  * so we need to strip the mode, and check the ID
+			  * because we might handle SGMII0 too.
+			  */
+			  /* TODO: need to distinguish between CP110 and CP115
+			   * as SFI1/XFI1 available only for CP115.
+			   */
+			if ((mode == COMPHY_SGMII_MODE ||
+			     mode == COMPHY_2500BASEX_MODE ||
+			     mode == COMPHY_SFI_MODE ||
+			     mode == COMPHY_XFI_MODE ||
+			     mode == COMPHY_AP_MODE)
+			    && COMPHY_GET_ID(comphy_mode) == 1)
+				reg |= COMMON_SELECTOR_COMPHY4_PORT1 <<
+					comphy_offset;
+			else
+				reg |= COMMON_SELECTOR_COMPHY4_ALL_OTHERS <<
+					comphy_offset;
+			break;
+		case(5):
+			/* For comphy 5:
+			 * 0x1 = SGMII/Base-X Port2
+			 * 0x2 = RXAUI Lane1
+			 */
+			if (mode == COMPHY_RXAUI_MODE)
+				reg |= COMMON_SELECTOR_COMPHY5_RXAUI <<
+					comphy_offset;
+			else
+				reg |= COMMON_SELECTOR_COMPHY5_SGMII <<
+					comphy_offset;
+			break;
+		}
+	}
+
+	mmio_write_32(comphy_base + COMMON_SELECTOR_PHY_REG_OFFSET, reg);
+}
+
+/* PIPE selector configures for PCIe, USB 3.0 Host, and USB 3.0 Device mode */
+static void mvebu_cp110_comphy_set_pipe_selector(uint64_t comphy_base,
+				     uint8_t comphy_index, uint32_t comphy_mode)
+{
+	uint32_t reg;
+	uint32_t shift = COMMON_SELECTOR_COMPHYN_FIELD_WIDTH * comphy_index;
+	int mode = COMPHY_GET_MODE(comphy_mode);
+	uint32_t mask = COMMON_SELECTOR_COMPHY_MASK << shift;
+	uint32_t pipe_sel = 0x0;
+
+	/* If pipe selector is used the phy selector should be marked as
+	 * unconnected.
+	 */
+	mvebu_cp110_comphy_clr_phy_selector(comphy_base, comphy_index);
+
+	reg = mmio_read_32(comphy_base + COMMON_SELECTOR_PIPE_REG_OFFSET);
+	reg &= ~mask;
+
+	switch (mode) {
+	case (COMPHY_PCIE_MODE):
+		/* For lanes support PCIE, selector value are all same */
+		pipe_sel = COMMON_SELECTOR_PIPE_COMPHY_PCIE;
+		break;
+
+	case (COMPHY_USB3H_MODE):
+		/* Only lane 1-4 support USB host, selector value is same */
+		if (comphy_index == COMPHY_LANE0 ||
+		    comphy_index == COMPHY_LANE5)
+			ERROR("COMPHY[%d] mode[%d] is invalid\n",
+			      comphy_index, mode);
+		else
+			pipe_sel = COMMON_SELECTOR_PIPE_COMPHY_USBH;
+		break;
+
+	case (COMPHY_USB3D_MODE):
+		/* Lane 1 and 4 support USB device, selector value is same */
+		if (comphy_index == COMPHY_LANE1 ||
+		    comphy_index == COMPHY_LANE4)
+			pipe_sel = COMMON_SELECTOR_PIPE_COMPHY_USBD;
+		else
+			ERROR("COMPHY[%d] mode[%d] is invalid\n", comphy_index,
+			      mode);
+		break;
+
+	default:
+		ERROR("COMPHY[%d] mode[%d] is invalid\n", comphy_index, mode);
+		break;
+	}
+
+	mmio_write_32(comphy_base + COMMON_SELECTOR_PIPE_REG_OFFSET, reg |
+		      (pipe_sel << shift));
+}
+
+int mvebu_cp110_comphy_is_pll_locked(uint64_t comphy_base, uint8_t comphy_index)
+{
+	uintptr_t sd_ip_addr, addr;
+	uint32_t mask, data;
+	int ret = 0;
+
+	debug_enter();
+
+	sd_ip_addr = SD_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+			     comphy_index);
+
+	addr = sd_ip_addr + SD_EXTERNAL_STATUS0_REG;
+	data = SD_EXTERNAL_STATUS0_PLL_TX_MASK &
+		SD_EXTERNAL_STATUS0_PLL_RX_MASK;
+	mask = data;
+	data = polling_with_timeout(addr, data, mask,
+				    PLL_LOCK_TIMEOUT, REG_32BIT);
+	if (data != 0) {
+		if (data & SD_EXTERNAL_STATUS0_PLL_RX_MASK)
+			ERROR("RX PLL is not locked\n");
+		if (data & SD_EXTERNAL_STATUS0_PLL_TX_MASK)
+			ERROR("TX PLL is not locked\n");
+
+		ret = -ETIMEDOUT;
+	}
+
+	debug_exit();
+
+	return ret;
+}
+
+static void mvebu_cp110_polarity_invert(uintptr_t addr, uint8_t phy_polarity_invert)
+{
+	uint32_t mask, data;
+
+	/* Set RX / TX polarity */
+	data = mask = 0x0U;
+	if ((phy_polarity_invert & COMPHY_POLARITY_TXD_INVERT) != 0) {
+		data |= (1 << HPIPE_SYNC_PATTERN_TXD_INV_OFFSET);
+		mask |= HPIPE_SYNC_PATTERN_TXD_INV_MASK;
+		debug("%s: inverting TX polarity\n", __func__);
+	}
+
+	if ((phy_polarity_invert & COMPHY_POLARITY_RXD_INVERT) != 0) {
+		data |= (1 << HPIPE_SYNC_PATTERN_RXD_INV_OFFSET);
+		mask |= HPIPE_SYNC_PATTERN_RXD_INV_MASK;
+		debug("%s: inverting RX polarity\n", __func__);
+	}
+
+	reg_set(addr, data, mask);
+}
+
+static int mvebu_cp110_comphy_sata_power_on(uint64_t comphy_base,
+				     uint8_t comphy_index, uint32_t comphy_mode)
+{
+	uintptr_t hpipe_addr, sd_ip_addr, comphy_addr;
+	uint32_t mask, data;
+	uint8_t ap_nr, cp_nr, phy_polarity_invert;
+	int ret = 0;
+
+	debug_enter();
+
+	mvebu_cp110_get_ap_and_cp_nr(&ap_nr, &cp_nr, comphy_base);
+
+	const struct sata_params *sata_static_values =
+			&sata_static_values_tab[ap_nr][cp_nr][comphy_index];
+
+	phy_polarity_invert = sata_static_values->polarity_invert;
+
+	/* configure phy selector for SATA */
+	mvebu_cp110_comphy_set_phy_selector(comphy_base,
+					    comphy_index, comphy_mode);
+
+	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+				comphy_index);
+	sd_ip_addr = SD_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+			     comphy_index);
+	comphy_addr = COMPHY_ADDR(comphy_base, comphy_index);
+
+	debug(" add hpipe 0x%lx, sd 0x%lx, comphy 0x%lx\n",
+					   hpipe_addr, sd_ip_addr, comphy_addr);
+	debug("stage: RFU configurations - hard reset comphy\n");
+	/* RFU configurations - hard reset comphy */
+	mask = COMMON_PHY_CFG1_PWR_UP_MASK;
+	data = 0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET;
+	mask |= COMMON_PHY_CFG1_PIPE_SELECT_MASK;
+	data |= 0x0 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET;
+	mask |= COMMON_PHY_CFG1_PWR_ON_RESET_MASK;
+	data |= 0x0 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET;
+	mask |= COMMON_PHY_CFG1_CORE_RSTN_MASK;
+	data |= 0x0 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET;
+	reg_set(comphy_addr + COMMON_PHY_CFG1_REG, data, mask);
+
+	/* Set select data  width 40Bit - SATA mode only */
+	reg_set(comphy_addr + COMMON_PHY_CFG6_REG,
+		0x1 << COMMON_PHY_CFG6_IF_40_SEL_OFFSET,
+		COMMON_PHY_CFG6_IF_40_SEL_MASK);
+
+	/* release from hard reset in SD external */
+	mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK;
+	data = 0x1 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	/* Wait 1ms - until band gap and ref clock ready */
+	mdelay(1);
+
+	debug("stage: Comphy configuration\n");
+	/* Start comphy Configuration */
+	/* Set reference clock to comes from group 1 - choose 25Mhz */
+	reg_set(hpipe_addr + HPIPE_MISC_REG,
+		0x0 << HPIPE_MISC_REFCLK_SEL_OFFSET,
+		HPIPE_MISC_REFCLK_SEL_MASK);
+	/* Reference frequency select set 1 (for SATA = 25Mhz) */
+	mask = HPIPE_PWR_PLL_REF_FREQ_MASK;
+	data = 0x1 << HPIPE_PWR_PLL_REF_FREQ_OFFSET;
+	/* PHY mode select (set SATA = 0x0 */
+	mask |= HPIPE_PWR_PLL_PHY_MODE_MASK;
+	data |= 0x0 << HPIPE_PWR_PLL_PHY_MODE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PWR_PLL_REG, data, mask);
+	/* Set max PHY generation setting - 6Gbps */
+	reg_set(hpipe_addr + HPIPE_INTERFACE_REG,
+		0x2 << HPIPE_INTERFACE_GEN_MAX_OFFSET,
+		HPIPE_INTERFACE_GEN_MAX_MASK);
+	/* Set select data  width 40Bit (SEL_BITS[2:0]) */
+	reg_set(hpipe_addr + HPIPE_LOOPBACK_REG,
+		0x2 << HPIPE_LOOPBACK_SEL_OFFSET, HPIPE_LOOPBACK_SEL_MASK);
+
+	debug("stage: Analog parameters from ETP(HW)\n");
+	/* G1 settings */
+	mask = HPIPE_G1_SET_1_G1_RX_SELMUPI_MASK;
+	data = sata_static_values->g1_rx_selmupi <<
+			HPIPE_G1_SET_1_G1_RX_SELMUPI_OFFSET;
+	mask |= HPIPE_G1_SET_1_G1_RX_SELMUPF_MASK;
+	data |= sata_static_values->g1_rx_selmupf <<
+			HPIPE_G1_SET_1_G1_RX_SELMUPF_OFFSET;
+	mask |= HPIPE_G1_SET_1_G1_RX_SELMUFI_MASK;
+	data |= sata_static_values->g1_rx_selmufi <<
+			HPIPE_G1_SET_1_G1_RX_SELMUFI_OFFSET;
+	mask |= HPIPE_G1_SET_1_G1_RX_SELMUFF_MASK;
+	data |= sata_static_values->g1_rx_selmuff <<
+			HPIPE_G1_SET_1_G1_RX_SELMUFF_OFFSET;
+	mask |= HPIPE_G1_SET_1_G1_RX_DIGCK_DIV_MASK;
+	data |= 0x1 << HPIPE_G1_SET_1_G1_RX_DIGCK_DIV_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SET_1_REG, data, mask);
+
+	mask = HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_MASK;
+	data = 0xf << HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_OFFSET;
+	mask |= HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_MASK;
+	data |= 0x2 << HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_OFFSET;
+	mask |= HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_MASK;
+	data |= 0x1 << HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_OFFSET;
+	mask |= HPIPE_G1_SETTINGS_3_G1_FFE_DEG_RES_LEVEL_MASK;
+	data |= 0x1 << HPIPE_G1_SETTINGS_3_G1_FFE_DEG_RES_LEVEL_OFFSET;
+	mask |= HPIPE_G1_SETTINGS_3_G1_FFE_LOAD_RES_LEVEL_MASK;
+	data |= 0x1 << HPIPE_G1_SETTINGS_3_G1_FFE_LOAD_RES_LEVEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_3_REG, data, mask);
+
+	/* G2 settings */
+	mask = HPIPE_G2_SET_1_G2_RX_SELMUPI_MASK;
+	data = sata_static_values->g2_rx_selmupi <<
+			HPIPE_G2_SET_1_G2_RX_SELMUPI_OFFSET;
+	mask |= HPIPE_G2_SET_1_G2_RX_SELMUPF_MASK;
+	data |= sata_static_values->g2_rx_selmupf <<
+			HPIPE_G2_SET_1_G2_RX_SELMUPF_OFFSET;
+	mask |= HPIPE_G2_SET_1_G2_RX_SELMUFI_MASK;
+	data |= sata_static_values->g2_rx_selmufi <<
+			HPIPE_G2_SET_1_G2_RX_SELMUFI_OFFSET;
+	mask |= HPIPE_G2_SET_1_G2_RX_SELMUFF_MASK;
+	data |= sata_static_values->g2_rx_selmuff <<
+			HPIPE_G2_SET_1_G2_RX_SELMUFF_OFFSET;
+	mask |= HPIPE_G2_SET_1_G2_RX_DIGCK_DIV_MASK;
+	data |= 0x1 << HPIPE_G2_SET_1_G2_RX_DIGCK_DIV_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G2_SET_1_REG, data, mask);
+
+	/* G3 settings */
+	mask = HPIPE_G3_SET_1_G3_RX_SELMUPI_MASK;
+	data = sata_static_values->g3_rx_selmupi <<
+			HPIPE_G3_SET_1_G3_RX_SELMUPI_OFFSET;
+	mask |= HPIPE_G3_SET_1_G3_RX_SELMUPF_MASK;
+	data |= sata_static_values->g3_rx_selmupf <<
+			HPIPE_G3_SET_1_G3_RX_SELMUPF_OFFSET;
+	mask |= HPIPE_G3_SET_1_G3_RX_SELMUFI_MASK;
+	data |= sata_static_values->g3_rx_selmufi <<
+			HPIPE_G3_SET_1_G3_RX_SELMUFI_OFFSET;
+	mask |= HPIPE_G3_SET_1_G3_RX_SELMUFF_MASK;
+	data |= sata_static_values->g3_rx_selmuff <<
+			HPIPE_G3_SET_1_G3_RX_SELMUFF_OFFSET;
+	mask |= HPIPE_G3_SET_1_G3_RX_DFE_EN_MASK;
+	data |= 0x1 << HPIPE_G3_SET_1_G3_RX_DFE_EN_OFFSET;
+	mask |= HPIPE_G3_SET_1_G3_RX_DIGCK_DIV_MASK;
+	data |= 0x2 << HPIPE_G3_SET_1_G3_RX_DIGCK_DIV_OFFSET;
+	mask |= HPIPE_G3_SET_1_G3_SAMPLER_INPAIRX2_EN_MASK;
+	data |= 0x0 << HPIPE_G3_SET_1_G3_SAMPLER_INPAIRX2_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G3_SET_1_REG, data, mask);
+
+	/* DTL Control */
+	mask = HPIPE_PWR_CTR_DTL_SQ_DET_EN_MASK;
+	data = 0x1 << HPIPE_PWR_CTR_DTL_SQ_DET_EN_OFFSET;
+	mask |= HPIPE_PWR_CTR_DTL_SQ_PLOOP_EN_MASK;
+	data |= 0x1 << HPIPE_PWR_CTR_DTL_SQ_PLOOP_EN_OFFSET;
+	mask |= HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK;
+	data |= 0x1 << HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET;
+	mask |= HPIPE_PWR_CTR_DTL_CLAMPING_SEL_MASK;
+	data |= 0x1 << HPIPE_PWR_CTR_DTL_CLAMPING_SEL_OFFSET;
+	mask |= HPIPE_PWR_CTR_DTL_INTPCLK_DIV_FORCE_MASK;
+	data |= 0x1 << HPIPE_PWR_CTR_DTL_INTPCLK_DIV_FORCE_OFFSET;
+	mask |= HPIPE_PWR_CTR_DTL_CLK_MODE_MASK;
+	data |= 0x1 << HPIPE_PWR_CTR_DTL_CLK_MODE_OFFSET;
+	mask |= HPIPE_PWR_CTR_DTL_CLK_MODE_FORCE_MASK;
+	data |= 0x1 << HPIPE_PWR_CTR_DTL_CLK_MODE_FORCE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PWR_CTR_DTL_REG, data, mask);
+
+	/* Trigger sampler enable pulse */
+	mask = HPIPE_SMAPLER_MASK;
+	data = 0x1 << HPIPE_SMAPLER_OFFSET;
+	reg_set(hpipe_addr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG, data, mask);
+	mask = HPIPE_SMAPLER_MASK;
+	data = 0x0 << HPIPE_SMAPLER_OFFSET;
+	reg_set(hpipe_addr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG, data, mask);
+
+	/* VDD Calibration Control 3 */
+	mask = HPIPE_EXT_SELLV_RXSAMPL_MASK;
+	data = 0x10 << HPIPE_EXT_SELLV_RXSAMPL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_VDD_CAL_CTRL_REG, data, mask);
+
+	/* DFE Resolution Control */
+	mask = HPIPE_DFE_RES_FORCE_MASK;
+	data = 0x1 << HPIPE_DFE_RES_FORCE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_DFE_REG0, data, mask);
+
+	/* DFE F3-F5 Coefficient Control */
+	mask = HPIPE_DFE_F3_F5_DFE_EN_MASK;
+	data = 0x0 << HPIPE_DFE_F3_F5_DFE_EN_OFFSET;
+	mask |= HPIPE_DFE_F3_F5_DFE_CTRL_MASK;
+	data = 0x0 << HPIPE_DFE_F3_F5_DFE_CTRL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_DFE_F3_F5_REG, data, mask);
+
+	/* G3 Setting 3 */
+	mask = HPIPE_G3_FFE_CAP_SEL_MASK;
+	data = sata_static_values->g3_ffe_cap_sel <<
+			HPIPE_G3_FFE_CAP_SEL_OFFSET;
+	mask |= HPIPE_G3_FFE_RES_SEL_MASK;
+	data |= sata_static_values->g3_ffe_res_sel <<
+			HPIPE_G3_FFE_RES_SEL_OFFSET;
+	mask |= HPIPE_G3_FFE_SETTING_FORCE_MASK;
+	data |= 0x1 << HPIPE_G3_FFE_SETTING_FORCE_OFFSET;
+	mask |= HPIPE_G3_FFE_DEG_RES_LEVEL_MASK;
+	data |= 0x1 << HPIPE_G3_FFE_DEG_RES_LEVEL_OFFSET;
+	mask |= HPIPE_G3_FFE_LOAD_RES_LEVEL_MASK;
+	data |= 0x3 << HPIPE_G3_FFE_LOAD_RES_LEVEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G3_SETTING_3_REG, data, mask);
+
+	/* G3 Setting 4 */
+	mask = HPIPE_G3_DFE_RES_MASK;
+	data = sata_static_values->g3_dfe_res << HPIPE_G3_DFE_RES_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G3_SETTING_4_REG, data, mask);
+
+	/* Offset Phase Control */
+	mask = HPIPE_OS_PH_OFFSET_MASK;
+	data = sata_static_values->align90 << HPIPE_OS_PH_OFFSET_OFFSET;
+	mask |= HPIPE_OS_PH_OFFSET_FORCE_MASK;
+	data |= 0x1 << HPIPE_OS_PH_OFFSET_FORCE_OFFSET;
+	mask |= HPIPE_OS_PH_VALID_MASK;
+	data |= 0x0 << HPIPE_OS_PH_VALID_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHASE_CONTROL_REG, data, mask);
+	mask = HPIPE_OS_PH_VALID_MASK;
+	data = 0x1 << HPIPE_OS_PH_VALID_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHASE_CONTROL_REG, data, mask);
+	mask = HPIPE_OS_PH_VALID_MASK;
+	data = 0x0 << HPIPE_OS_PH_VALID_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHASE_CONTROL_REG, data, mask);
+
+	/* Set G1 TX amplitude and TX post emphasis value */
+	mask = HPIPE_G1_SET_0_G1_TX_AMP_MASK;
+	data = sata_static_values->g1_amp << HPIPE_G1_SET_0_G1_TX_AMP_OFFSET;
+	mask |= HPIPE_G1_SET_0_G1_TX_AMP_ADJ_MASK;
+	data |= sata_static_values->g1_tx_amp_adj <<
+			HPIPE_G1_SET_0_G1_TX_AMP_ADJ_OFFSET;
+	mask |= HPIPE_G1_SET_0_G1_TX_EMPH1_MASK;
+	data |= sata_static_values->g1_emph <<
+			HPIPE_G1_SET_0_G1_TX_EMPH1_OFFSET;
+	mask |= HPIPE_G1_SET_0_G1_TX_EMPH1_EN_MASK;
+	data |= sata_static_values->g1_emph_en <<
+			HPIPE_G1_SET_0_G1_TX_EMPH1_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SET_0_REG, data, mask);
+
+	/* Set G1 emph */
+	mask = HPIPE_G1_SET_2_G1_TX_EMPH0_EN_MASK;
+	data = sata_static_values->g1_tx_emph_en <<
+			HPIPE_G1_SET_2_G1_TX_EMPH0_EN_OFFSET;
+	mask |= HPIPE_G1_SET_2_G1_TX_EMPH0_MASK;
+	data |= sata_static_values->g1_tx_emph <<
+			HPIPE_G1_SET_2_G1_TX_EMPH0_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SET_2_REG, data, mask);
+
+	/* Set G2 TX amplitude and TX post emphasis value */
+	mask = HPIPE_G2_SET_0_G2_TX_AMP_MASK;
+	data = sata_static_values->g2_amp << HPIPE_G2_SET_0_G2_TX_AMP_OFFSET;
+	mask |= HPIPE_G2_SET_0_G2_TX_AMP_ADJ_MASK;
+	data |= sata_static_values->g2_tx_amp_adj <<
+			HPIPE_G2_SET_0_G2_TX_AMP_ADJ_OFFSET;
+	mask |= HPIPE_G2_SET_0_G2_TX_EMPH1_MASK;
+	data |= sata_static_values->g2_emph <<
+			HPIPE_G2_SET_0_G2_TX_EMPH1_OFFSET;
+	mask |= HPIPE_G2_SET_0_G2_TX_EMPH1_EN_MASK;
+	data |= sata_static_values->g2_emph_en <<
+			HPIPE_G2_SET_0_G2_TX_EMPH1_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G2_SET_0_REG, data, mask);
+
+	/* Set G2 emph */
+	mask = HPIPE_G2_SET_2_G2_TX_EMPH0_EN_MASK;
+	data = sata_static_values->g2_tx_emph_en <<
+			HPIPE_G2_SET_2_G2_TX_EMPH0_EN_OFFSET;
+	mask |= HPIPE_G2_SET_2_G2_TX_EMPH0_MASK;
+	data |= sata_static_values->g2_tx_emph <<
+			HPIPE_G2_SET_2_G2_TX_EMPH0_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G2_SET_2_REG, data, mask);
+
+	/* Set G3 TX amplitude and TX post emphasis value */
+	mask = HPIPE_G3_SET_0_G3_TX_AMP_MASK;
+	data = sata_static_values->g3_amp << HPIPE_G3_SET_0_G3_TX_AMP_OFFSET;
+	mask |= HPIPE_G3_SET_0_G3_TX_AMP_ADJ_MASK;
+	data |= sata_static_values->g3_tx_amp_adj <<
+			HPIPE_G3_SET_0_G3_TX_AMP_ADJ_OFFSET;
+	mask |= HPIPE_G3_SET_0_G3_TX_EMPH1_MASK;
+	data |= sata_static_values->g3_emph <<
+			HPIPE_G3_SET_0_G3_TX_EMPH1_OFFSET;
+	mask |= HPIPE_G3_SET_0_G3_TX_EMPH1_EN_MASK;
+	data |= sata_static_values->g3_emph_en <<
+			HPIPE_G3_SET_0_G3_TX_EMPH1_EN_OFFSET;
+	mask |= HPIPE_G3_SET_0_G3_TX_SLEW_RATE_SEL_MASK;
+	data |= 0x4 << HPIPE_G3_SET_0_G3_TX_SLEW_RATE_SEL_OFFSET;
+	mask |= HPIPE_G3_SET_0_G3_TX_SLEW_CTRL_EN_MASK;
+	data |= 0x0 << HPIPE_G3_SET_0_G3_TX_SLEW_CTRL_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G3_SET_0_REG, data, mask);
+
+	/* Set G3 emph */
+	mask = HPIPE_G3_SET_2_G3_TX_EMPH0_EN_MASK;
+	data = sata_static_values->g3_tx_emph_en <<
+			HPIPE_G3_SET_2_G3_TX_EMPH0_EN_OFFSET;
+	mask |= HPIPE_G3_SET_2_G3_TX_EMPH0_MASK;
+	data |= sata_static_values->g3_tx_emph <<
+			HPIPE_G3_SET_2_G3_TX_EMPH0_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G3_SET_2_REG, data, mask);
+
+	/* SERDES External Configuration 2 register */
+	mask = SD_EXTERNAL_CONFIG2_SSC_ENABLE_MASK;
+	data = 0x1 << SD_EXTERNAL_CONFIG2_SSC_ENABLE_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG2_REG, data, mask);
+
+	/* DFE reset sequence */
+	reg_set(hpipe_addr + HPIPE_PWR_CTR_REG,
+		0x1 << HPIPE_PWR_CTR_RST_DFE_OFFSET,
+		HPIPE_PWR_CTR_RST_DFE_MASK);
+	reg_set(hpipe_addr + HPIPE_PWR_CTR_REG,
+		0x0 << HPIPE_PWR_CTR_RST_DFE_OFFSET,
+		HPIPE_PWR_CTR_RST_DFE_MASK);
+
+	if (phy_polarity_invert != 0)
+		mvebu_cp110_polarity_invert(hpipe_addr + HPIPE_SYNC_PATTERN_REG,
+					    phy_polarity_invert);
+
+	/* SW reset for interrupt logic */
+	reg_set(hpipe_addr + HPIPE_PWR_CTR_REG,
+		0x1 << HPIPE_PWR_CTR_SFT_RST_OFFSET,
+		HPIPE_PWR_CTR_SFT_RST_MASK);
+	reg_set(hpipe_addr + HPIPE_PWR_CTR_REG,
+		0x0 << HPIPE_PWR_CTR_SFT_RST_OFFSET,
+		HPIPE_PWR_CTR_SFT_RST_MASK);
+
+	debug_exit();
+
+	return ret;
+}
+
+static int mvebu_cp110_comphy_sgmii_power_on(uint64_t comphy_base,
+				     uint8_t comphy_index, uint32_t comphy_mode)
+{
+	uintptr_t hpipe_addr, sd_ip_addr, comphy_addr, addr;
+	uint32_t mask, data, sgmii_speed = COMPHY_GET_SPEED(comphy_mode);
+	int ret = 0;
+
+	debug_enter();
+
+	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+				comphy_index);
+	sd_ip_addr = SD_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+			     comphy_index);
+	comphy_addr = COMPHY_ADDR(comphy_base, comphy_index);
+
+	/* configure phy selector for SGMII */
+	mvebu_cp110_comphy_set_phy_selector(comphy_base, comphy_index,
+					    comphy_mode);
+
+	/* Confiugre the lane */
+	debug("stage: RFU configurations - hard reset comphy\n");
+	/* RFU configurations - hard reset comphy */
+	mask = COMMON_PHY_CFG1_PWR_UP_MASK;
+	data = 0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET;
+	mask |= COMMON_PHY_CFG1_PIPE_SELECT_MASK;
+	data |= 0x0 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET;
+	reg_set(comphy_addr + COMMON_PHY_CFG1_REG, data, mask);
+
+	/* Select Baud Rate of Comphy And PD_PLL/Tx/Rx */
+	mask = SD_EXTERNAL_CONFIG0_SD_PU_PLL_MASK;
+	data = 0x0 << SD_EXTERNAL_CONFIG0_SD_PU_PLL_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_MASK;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_MASK;
+
+	if (sgmii_speed == COMPHY_SPEED_1_25G) {
+		/* SGMII 1G, SerDes speed 1.25G */
+		data |= 0x6 << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_OFFSET;
+		data |= 0x6 << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_OFFSET;
+	} else if (sgmii_speed == COMPHY_SPEED_3_125G) {
+		/* 2500Base-X, SerDes speed 3.125G */
+		data |= 0x8 << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_OFFSET;
+		data |= 0x8 << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_OFFSET;
+	} else {
+		/* Other rates are not supported */
+		ERROR("unsupported SGMII speed on comphy%d\n", comphy_index);
+		return -EINVAL;
+	}
+
+	mask |= SD_EXTERNAL_CONFIG0_SD_PU_RX_MASK;
+	data |= 0 << SD_EXTERNAL_CONFIG0_SD_PU_RX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PU_TX_MASK;
+	data |= 0 << SD_EXTERNAL_CONFIG0_SD_PU_TX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_MASK;
+	data |= 1 << SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG0_REG, data, mask);
+
+	/* Set hard reset */
+	mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK;
+	data = 0x0 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+	data |= 0x0 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+	data |= 0x0 << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	/* Release hard reset */
+	mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK;
+	data = 0x1 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	/* Wait 1ms - until band gap and ref clock ready */
+	mdelay(1);
+
+	/* Make sure that 40 data bits is disabled
+	 * This bit is not cleared by reset
+	 */
+	mask = COMMON_PHY_CFG6_IF_40_SEL_MASK;
+	data = 0 << COMMON_PHY_CFG6_IF_40_SEL_OFFSET;
+	reg_set(comphy_addr + COMMON_PHY_CFG6_REG, data, mask);
+
+	/* Start comphy Configuration */
+	debug("stage: Comphy configuration\n");
+	/* set reference clock */
+	mask = HPIPE_MISC_REFCLK_SEL_MASK;
+	data = 0x0 << HPIPE_MISC_REFCLK_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_MISC_REG, data, mask);
+	/* Power and PLL Control */
+	mask = HPIPE_PWR_PLL_REF_FREQ_MASK;
+	data = 0x1 << HPIPE_PWR_PLL_REF_FREQ_OFFSET;
+	mask |= HPIPE_PWR_PLL_PHY_MODE_MASK;
+	data |= 0x4 << HPIPE_PWR_PLL_PHY_MODE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PWR_PLL_REG, data, mask);
+	/* Loopback register */
+	mask = HPIPE_LOOPBACK_SEL_MASK;
+	data = 0x1 << HPIPE_LOOPBACK_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_LOOPBACK_REG, data, mask);
+	/* rx control 1 */
+	mask = HPIPE_RX_CONTROL_1_RXCLK2X_SEL_MASK;
+	data = 0x1 << HPIPE_RX_CONTROL_1_RXCLK2X_SEL_OFFSET;
+	mask |= HPIPE_RX_CONTROL_1_CLK8T_EN_MASK;
+	data |= 0x0 << HPIPE_RX_CONTROL_1_CLK8T_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_RX_CONTROL_1_REG, data, mask);
+	/* DTL Control */
+	mask = HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK;
+	data = 0x0 << HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PWR_CTR_DTL_REG, data, mask);
+
+	/* Set analog parameters from ETP(HW) - for now use the default data */
+	debug("stage: Analog parameters from ETP(HW)\n");
+
+	reg_set(hpipe_addr + HPIPE_G1_SET_0_REG,
+		0x1 << HPIPE_G1_SET_0_G1_TX_EMPH1_OFFSET,
+		HPIPE_G1_SET_0_G1_TX_EMPH1_MASK);
+
+	debug("stage: RFU configurations- Power Up PLL,Tx,Rx\n");
+	/* SERDES External Configuration */
+	mask = SD_EXTERNAL_CONFIG0_SD_PU_PLL_MASK;
+	data = 0x1 << SD_EXTERNAL_CONFIG0_SD_PU_PLL_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PU_RX_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG0_SD_PU_RX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PU_TX_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG0_SD_PU_TX_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG0_REG, data, mask);
+
+	ret = mvebu_cp110_comphy_is_pll_locked(comphy_base, comphy_index);
+	if (ret)
+		return ret;
+
+	/* RX init */
+	mask = SD_EXTERNAL_CONFIG1_RX_INIT_MASK;
+	data = 0x1 << SD_EXTERNAL_CONFIG1_RX_INIT_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	/* check that RX init done */
+	addr = sd_ip_addr + SD_EXTERNAL_STATUS0_REG;
+	data = SD_EXTERNAL_STATUS0_RX_INIT_MASK;
+	mask = data;
+	data = polling_with_timeout(addr, data, mask, 100, REG_32BIT);
+	if (data != 0) {
+		ERROR("RX init failed\n");
+		ret = -ETIMEDOUT;
+	}
+
+	debug("stage: RF Reset\n");
+	/* RF Reset */
+	mask = SD_EXTERNAL_CONFIG1_RX_INIT_MASK;
+	data = 0x0 << SD_EXTERNAL_CONFIG1_RX_INIT_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	debug_exit();
+
+	return ret;
+}
+
+static int mvebu_cp110_comphy_xfi_power_on(uint64_t comphy_base,
+					   uint8_t comphy_index,
+					   uint32_t comphy_mode,
+					   uint64_t comphy_train_base)
+{
+	uintptr_t hpipe_addr, sd_ip_addr, comphy_addr, addr;
+	uint32_t mask, data, speed = COMPHY_GET_SPEED(comphy_mode);
+	int ret = 0;
+	uint8_t ap_nr, cp_nr;
+
+	debug_enter();
+	mvebu_cp110_get_ap_and_cp_nr(&ap_nr, &cp_nr, comphy_base);
+
+	if (rx_trainng_done[ap_nr][cp_nr][comphy_index]) {
+		debug("Skip %s for comphy[%d][%d][%d], due to rx training\n",
+		       __func__, ap_nr, cp_nr, comphy_index);
+		return 0;
+	}
+
+	const struct xfi_params *xfi_static_values =
+			     &xfi_static_values_tab[ap_nr][cp_nr][comphy_index];
+
+	debug("%s: the ap_nr = %d, cp_nr = %d, comphy_index %d\n",
+	      __func__, ap_nr, cp_nr, comphy_index);
+
+	debug("g1_ffe_cap_sel= 0x%x, g1_ffe_res_sel= 0x%x, g1_dfe_res= 0x%x\n",
+	      xfi_static_values->g1_ffe_cap_sel,
+	      xfi_static_values->g1_ffe_res_sel,
+	      xfi_static_values->g1_dfe_res);
+
+	if (!xfi_static_values->valid) {
+		ERROR("[ap%d][cp[%d][comphy:%d]: Has no valid static params\n",
+		      ap_nr, cp_nr, comphy_index);
+		ERROR("[ap%d][cp[%d][comphy:%d]: porting layer needs update\n",
+		      ap_nr, cp_nr, comphy_index);
+		return -EINVAL;
+	}
+
+	if ((speed != COMPHY_SPEED_5_15625G) &&
+	     (speed != COMPHY_SPEED_10_3125G) &&
+	     (speed != COMPHY_SPEED_DEFAULT)) {
+		ERROR("comphy:%d: unsupported sfi/xfi speed\n", comphy_index);
+		return -EINVAL;
+	}
+
+	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+				comphy_index);
+	sd_ip_addr = SD_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+			     comphy_index);
+	comphy_addr = COMPHY_ADDR(comphy_base, comphy_index);
+
+	/* configure phy selector for XFI/SFI */
+	mvebu_cp110_comphy_set_phy_selector(comphy_base, comphy_index,
+					    comphy_mode);
+
+	debug("stage: RFU configurations - hard reset comphy\n");
+	/* RFU configurations - hard reset comphy */
+	mask = COMMON_PHY_CFG1_PWR_UP_MASK;
+	data = 0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET;
+	mask |= COMMON_PHY_CFG1_PIPE_SELECT_MASK;
+	data |= 0x0 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET;
+	reg_set(comphy_addr + COMMON_PHY_CFG1_REG, data, mask);
+
+	/* Make sure that 40 data bits is disabled
+	 * This bit is not cleared by reset
+	 */
+	mask = COMMON_PHY_CFG6_IF_40_SEL_MASK;
+	data = 0 << COMMON_PHY_CFG6_IF_40_SEL_OFFSET;
+	reg_set(comphy_addr + COMMON_PHY_CFG6_REG, data, mask);
+
+	/* Select Baud Rate of Comphy And PD_PLL/Tx/Rx */
+	mask = SD_EXTERNAL_CONFIG0_SD_PU_PLL_MASK;
+	data = 0x0 << SD_EXTERNAL_CONFIG0_SD_PU_PLL_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_MASK;
+	data |= 0xE << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_MASK;
+	data |= 0xE << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PU_RX_MASK;
+	data |= 0 << SD_EXTERNAL_CONFIG0_SD_PU_RX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PU_TX_MASK;
+	data |= 0 << SD_EXTERNAL_CONFIG0_SD_PU_TX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_MASK;
+	data |= 0 << SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG0_REG, data, mask);
+
+	/* release from hard reset */
+	mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK;
+	data = 0x0 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+	data |= 0x0 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+	data |= 0x0 << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK;
+	data = 0x1 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_TX_IDLE_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG1_TX_IDLE_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	/* Wait 1ms - until band gap and ref clock ready */
+	mdelay(1);
+
+	/*
+	 * Erratum IPCE_COMPHY-1353: toggle TX_IDLE bit in
+	 * addition to the PHY reset
+	 */
+	mask = SD_EXTERNAL_CONFIG1_TX_IDLE_MASK;
+	data = 0x0U;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	/* Start comphy Configuration */
+	debug("stage: Comphy configuration\n");
+	/* set reference clock */
+	mask = HPIPE_MISC_ICP_FORCE_MASK;
+	data = (speed == COMPHY_SPEED_5_15625G) ?
+		(0x0 << HPIPE_MISC_ICP_FORCE_OFFSET) :
+		(0x1 << HPIPE_MISC_ICP_FORCE_OFFSET);
+	mask |= HPIPE_MISC_REFCLK_SEL_MASK;
+	data |= 0x0 << HPIPE_MISC_REFCLK_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_MISC_REG, data, mask);
+	/* Power and PLL Control */
+	mask = HPIPE_PWR_PLL_REF_FREQ_MASK;
+	data = 0x1 << HPIPE_PWR_PLL_REF_FREQ_OFFSET;
+	mask |= HPIPE_PWR_PLL_PHY_MODE_MASK;
+	data |= 0x4 << HPIPE_PWR_PLL_PHY_MODE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PWR_PLL_REG, data, mask);
+	/* Loopback register */
+	mask = HPIPE_LOOPBACK_SEL_MASK;
+	data = 0x1 << HPIPE_LOOPBACK_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_LOOPBACK_REG, data, mask);
+	/* rx control 1 */
+	mask = HPIPE_RX_CONTROL_1_RXCLK2X_SEL_MASK;
+	data = 0x1 << HPIPE_RX_CONTROL_1_RXCLK2X_SEL_OFFSET;
+	mask |= HPIPE_RX_CONTROL_1_CLK8T_EN_MASK;
+	data |= 0x1 << HPIPE_RX_CONTROL_1_CLK8T_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_RX_CONTROL_1_REG, data, mask);
+	/* DTL Control */
+	mask = HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK;
+	data = 0x1 << HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PWR_CTR_DTL_REG, data, mask);
+
+	/* Transmitter/Receiver Speed Divider Force */
+	if (speed == COMPHY_SPEED_5_15625G) {
+		mask = HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_MASK;
+		data = 1 << HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_OFFSET;
+		mask |= HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_MASK;
+		data |= 1 << HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_OFFSET;
+		mask |= HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_MASK;
+		data |= 1 << HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_OFFSET;
+		mask |= HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_MASK;
+		data |= 1 << HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_OFFSET;
+	} else {
+		mask = HPIPE_TXDIGCK_DIV_FORCE_MASK;
+		data = 0x1 << HPIPE_TXDIGCK_DIV_FORCE_OFFSET;
+	}
+	reg_set(hpipe_addr + HPIPE_SPD_DIV_FORCE_REG, data, mask);
+
+	/* Set analog parameters from ETP(HW) */
+	debug("stage: Analog parameters from ETP(HW)\n");
+	/* SERDES External Configuration 2 */
+	mask = SD_EXTERNAL_CONFIG2_PIN_DFE_EN_MASK;
+	data = 0x1 << SD_EXTERNAL_CONFIG2_PIN_DFE_EN_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG2_REG, data, mask);
+	/* 0x7-DFE Resolution control */
+	mask = HPIPE_DFE_RES_FORCE_MASK;
+	data = 0x1 << HPIPE_DFE_RES_FORCE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_DFE_REG0, data, mask);
+	/* 0xd-G1_Setting_0 */
+	if (speed == COMPHY_SPEED_5_15625G) {
+		mask = HPIPE_G1_SET_0_G1_TX_EMPH1_MASK;
+		data = 0x6 << HPIPE_G1_SET_0_G1_TX_EMPH1_OFFSET;
+	} else {
+		mask = HPIPE_G1_SET_0_G1_TX_AMP_MASK;
+		data = xfi_static_values->g1_amp <<
+				HPIPE_G1_SET_0_G1_TX_AMP_OFFSET;
+		mask |= HPIPE_G1_SET_0_G1_TX_EMPH1_MASK;
+		data |= xfi_static_values->g1_emph <<
+				HPIPE_G1_SET_0_G1_TX_EMPH1_OFFSET;
+
+		mask |= HPIPE_G1_SET_0_G1_TX_EMPH1_EN_MASK;
+		data |= xfi_static_values->g1_emph_en <<
+				HPIPE_G1_SET_0_G1_TX_EMPH1_EN_OFFSET;
+		mask |= HPIPE_G1_SET_0_G1_TX_AMP_ADJ_MASK;
+		data |= xfi_static_values->g1_tx_amp_adj <<
+				HPIPE_G1_SET_0_G1_TX_AMP_ADJ_OFFSET;
+	}
+	reg_set(hpipe_addr + HPIPE_G1_SET_0_REG, data, mask);
+	/* Genration 1 setting 2 (G1_Setting_2) */
+	mask = HPIPE_G1_SET_2_G1_TX_EMPH0_MASK;
+	data = xfi_static_values->g1_tx_emph <<
+				HPIPE_G1_SET_2_G1_TX_EMPH0_OFFSET;
+	mask |= HPIPE_G1_SET_2_G1_TX_EMPH0_EN_MASK;
+	data |= xfi_static_values->g1_tx_emph_en <<
+				HPIPE_G1_SET_2_G1_TX_EMPH0_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SET_2_REG, data, mask);
+	/* Transmitter Slew Rate Control register (tx_reg1) */
+	mask = HPIPE_TX_REG1_TX_EMPH_RES_MASK;
+	data = 0x3 << HPIPE_TX_REG1_TX_EMPH_RES_OFFSET;
+	mask |= HPIPE_TX_REG1_SLC_EN_MASK;
+	data |= 0x3f << HPIPE_TX_REG1_SLC_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TX_REG1_REG, data, mask);
+	/* Impedance Calibration Control register (cal_reg1) */
+	mask = HPIPE_CAL_REG_1_EXT_TXIMP_MASK;
+	data = 0xe << HPIPE_CAL_REG_1_EXT_TXIMP_OFFSET;
+	mask |= HPIPE_CAL_REG_1_EXT_TXIMP_EN_MASK;
+	data |= 0x1 << HPIPE_CAL_REG_1_EXT_TXIMP_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_CAL_REG1_REG, data, mask);
+	/* Generation 1 Setting 5 (g1_setting_5) */
+	mask = HPIPE_G1_SETTING_5_G1_ICP_MASK;
+	data = 0 << HPIPE_CAL_REG_1_EXT_TXIMP_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SETTING_5_REG, data, mask);
+
+	/* 0xE-G1_Setting_1 */
+	mask = HPIPE_G1_SET_1_G1_RX_DFE_EN_MASK;
+	data = 0x1 << HPIPE_G1_SET_1_G1_RX_DFE_EN_OFFSET;
+	if (speed == COMPHY_SPEED_5_15625G) {
+		mask |= HPIPE_G1_SET_1_G1_RX_SELMUPI_MASK;
+		data |= 0x1 << HPIPE_G1_SET_1_G1_RX_SELMUPI_OFFSET;
+		mask |= HPIPE_G1_SET_1_G1_RX_SELMUPF_MASK;
+		data |= 0x1 << HPIPE_G1_SET_1_G1_RX_SELMUPF_OFFSET;
+	} else {
+		mask |= HPIPE_G1_SET_1_G1_RX_SELMUPI_MASK;
+		data |= xfi_static_values->g1_rx_selmupi <<
+				HPIPE_G1_SET_1_G1_RX_SELMUPI_OFFSET;
+		mask |= HPIPE_G1_SET_1_G1_RX_SELMUPF_MASK;
+		data |= xfi_static_values->g1_rx_selmupf <<
+				HPIPE_G1_SET_1_G1_RX_SELMUPF_OFFSET;
+		mask |= HPIPE_G1_SET_1_G1_RX_SELMUFI_MASK;
+		data |= xfi_static_values->g1_rx_selmufi <<
+				HPIPE_G1_SET_1_G1_RX_SELMUFI_OFFSET;
+		mask |= HPIPE_G1_SET_1_G1_RX_SELMUFF_MASK;
+		data |= xfi_static_values->g1_rx_selmuff <<
+				HPIPE_G1_SET_1_G1_RX_SELMUFF_OFFSET;
+		mask |= HPIPE_G1_SET_1_G1_RX_DIGCK_DIV_MASK;
+		data |= 0x3 << HPIPE_G1_SET_1_G1_RX_DIGCK_DIV_OFFSET;
+	}
+	reg_set(hpipe_addr + HPIPE_G1_SET_1_REG, data, mask);
+
+	/* 0xA-DFE_Reg3 */
+	mask = HPIPE_DFE_F3_F5_DFE_EN_MASK;
+	data = 0x0 << HPIPE_DFE_F3_F5_DFE_EN_OFFSET;
+	mask |= HPIPE_DFE_F3_F5_DFE_CTRL_MASK;
+	data |= 0x0 << HPIPE_DFE_F3_F5_DFE_CTRL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_DFE_F3_F5_REG, data, mask);
+
+	/* 0x111-G1_Setting_4 */
+	mask = HPIPE_G1_SETTINGS_4_G1_DFE_RES_MASK;
+	data = 0x1 << HPIPE_G1_SETTINGS_4_G1_DFE_RES_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_4_REG, data, mask);
+	/* Genration 1 setting 3 (G1_Setting_3) */
+	mask = HPIPE_G1_SETTINGS_3_G1_FBCK_SEL_MASK;
+	data = 0x1 << HPIPE_G1_SETTINGS_3_G1_FBCK_SEL_OFFSET;
+	if (speed == COMPHY_SPEED_5_15625G) {
+		/* Force FFE (Feed Forward Equalization) to 5G */
+		mask |= HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_MASK;
+		data |= 0xf << HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_OFFSET;
+		mask |= HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_MASK;
+		data |= 0x4 << HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_OFFSET;
+		mask |= HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_MASK;
+		data |= 0x1 << HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_OFFSET;
+		reg_set(hpipe_addr + HPIPE_G1_SETTINGS_3_REG, data, mask);
+	} else {
+		mask |= HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_MASK;
+		data |= xfi_static_values->g1_ffe_cap_sel <<
+			HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_OFFSET;
+		mask |= HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_MASK;
+		data |= xfi_static_values->g1_ffe_res_sel <<
+			HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_OFFSET;
+		mask |= HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_MASK;
+		data |= 0x1 << HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_OFFSET;
+		reg_set(hpipe_addr + HPIPE_G1_SETTINGS_3_REG, data, mask);
+
+		/* Use the value from CAL_OS_PH_EXT */
+		mask = HPIPE_CAL_RXCLKALIGN_90_EXT_EN_MASK;
+		data = 1 << HPIPE_CAL_RXCLKALIGN_90_EXT_EN_OFFSET;
+		reg_set(hpipe_addr +
+			HPIPE_RX_CLK_ALIGN90_AND_TX_IDLE_CALIB_CTRL_REG,
+			data, mask);
+
+		/* Update align90 */
+		mask = HPIPE_CAL_OS_PH_EXT_MASK;
+		data = xfi_static_values->align90 << HPIPE_CAL_OS_PH_EXT_OFFSET;
+		reg_set(hpipe_addr +
+			HPIPE_RX_CLK_ALIGN90_AND_TX_IDLE_CALIB_CTRL_REG,
+			data, mask);
+
+		/* Force DFE resolution (use gen table value) */
+		mask = HPIPE_DFE_RES_FORCE_MASK;
+		data = 0x0 << HPIPE_DFE_RES_FORCE_OFFSET;
+		reg_set(hpipe_addr + HPIPE_DFE_REG0, data, mask);
+
+		/* 0x111-G1 DFE_Setting_4 */
+		mask = HPIPE_G1_SETTINGS_4_G1_DFE_RES_MASK;
+		data = xfi_static_values->g1_dfe_res <<
+			HPIPE_G1_SETTINGS_4_G1_DFE_RES_OFFSET;
+		reg_set(hpipe_addr + HPIPE_G1_SETTINGS_4_REG, data, mask);
+	}
+
+	/* Connfigure RX training timer */
+	mask = HPIPE_RX_TRAIN_TIMER_MASK;
+	data = 0x13 << HPIPE_RX_TRAIN_TIMER_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TX_TRAIN_CTRL_5_REG, data, mask);
+
+	/* Enable TX train peak to peak hold */
+	mask = HPIPE_TX_TRAIN_P2P_HOLD_MASK;
+	data = 0x1 << HPIPE_TX_TRAIN_P2P_HOLD_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TX_TRAIN_CTRL_0_REG, data, mask);
+
+	/* Configure TX preset index */
+	mask = HPIPE_TX_PRESET_INDEX_MASK;
+	data = 0x2 << HPIPE_TX_PRESET_INDEX_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TX_PRESET_INDEX_REG, data, mask);
+
+	/* Disable pattern lock lost timeout */
+	mask = HPIPE_PATTERN_LOCK_LOST_TIMEOUT_EN_MASK;
+	data = 0x0 << HPIPE_PATTERN_LOCK_LOST_TIMEOUT_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_FRAME_DETECT_CTRL_3_REG, data, mask);
+
+	/* Configure TX training pattern and TX training 16bit auto */
+	mask = HPIPE_TX_TRAIN_16BIT_AUTO_EN_MASK;
+	data = 0x1 << HPIPE_TX_TRAIN_16BIT_AUTO_EN_OFFSET;
+	mask |= HPIPE_TX_TRAIN_PAT_SEL_MASK;
+	data |= 0x1 << HPIPE_TX_TRAIN_PAT_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TX_TRAIN_REG, data, mask);
+
+	/* Configure Training patten number */
+	mask = HPIPE_TRAIN_PAT_NUM_MASK;
+	data = 0x88 << HPIPE_TRAIN_PAT_NUM_OFFSET;
+	reg_set(hpipe_addr + HPIPE_FRAME_DETECT_CTRL_0_REG, data, mask);
+
+	/* Configure differencial manchester encoter to ethernet mode */
+	mask = HPIPE_DME_ETHERNET_MODE_MASK;
+	data = 0x1 << HPIPE_DME_ETHERNET_MODE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_DME_REG, data, mask);
+
+	/* Configure VDD Continuous Calibration */
+	mask = HPIPE_CAL_VDD_CONT_MODE_MASK;
+	data = 0x1 << HPIPE_CAL_VDD_CONT_MODE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_VDD_CAL_0_REG, data, mask);
+
+	/* Trigger sampler enable pulse (by toggleing the bit) */
+	mask = HPIPE_RX_SAMPLER_OS_GAIN_MASK;
+	data = 0x3 << HPIPE_RX_SAMPLER_OS_GAIN_OFFSET;
+	mask |= HPIPE_SMAPLER_MASK;
+	data |= 0x1 << HPIPE_SMAPLER_OFFSET;
+	reg_set(hpipe_addr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG, data, mask);
+	mask = HPIPE_SMAPLER_MASK;
+	data = 0x0 << HPIPE_SMAPLER_OFFSET;
+	reg_set(hpipe_addr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG, data, mask);
+
+	/* Set External RX Regulator Control */
+	mask = HPIPE_EXT_SELLV_RXSAMPL_MASK;
+	data = 0x1A << HPIPE_EXT_SELLV_RXSAMPL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_VDD_CAL_CTRL_REG, data, mask);
+
+	debug("stage: RFU configurations- Power Up PLL,Tx,Rx\n");
+	/* SERDES External Configuration */
+	mask = SD_EXTERNAL_CONFIG0_SD_PU_PLL_MASK;
+	data = 0x1 << SD_EXTERNAL_CONFIG0_SD_PU_PLL_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PU_RX_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG0_SD_PU_RX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PU_TX_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG0_SD_PU_TX_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG0_REG, data, mask);
+
+	/* check PLL rx & tx ready */
+	addr = sd_ip_addr + SD_EXTERNAL_STATUS0_REG;
+	data = SD_EXTERNAL_STATUS0_PLL_RX_MASK |
+	       SD_EXTERNAL_STATUS0_PLL_TX_MASK;
+	mask = data;
+	data = polling_with_timeout(addr, data, mask,
+				    PLL_LOCK_TIMEOUT, REG_32BIT);
+	if (data != 0) {
+		if (data & SD_EXTERNAL_STATUS0_PLL_RX_MASK)
+			ERROR("RX PLL is not locked\n");
+		if (data & SD_EXTERNAL_STATUS0_PLL_TX_MASK)
+			ERROR("TX PLL is not locked\n");
+
+		ret = -ETIMEDOUT;
+	}
+
+	/* RX init */
+	mask = SD_EXTERNAL_CONFIG1_RX_INIT_MASK;
+	data = 0x1 << SD_EXTERNAL_CONFIG1_RX_INIT_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	/* check that RX init done */
+	addr = sd_ip_addr + SD_EXTERNAL_STATUS0_REG;
+	data = SD_EXTERNAL_STATUS0_RX_INIT_MASK;
+	mask = data;
+	data = polling_with_timeout(addr, data, mask, 100, REG_32BIT);
+	if (data != 0) {
+		ERROR("RX init failed\n");
+		ret = -ETIMEDOUT;
+	}
+
+	debug("stage: RF Reset\n");
+	/* RF Reset */
+	mask =  SD_EXTERNAL_CONFIG1_RX_INIT_MASK;
+	data = 0x0 << SD_EXTERNAL_CONFIG1_RX_INIT_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	/* Force rx training on 10G port */
+	data = mmio_read_32(COMPHY_TRX_RELATIVE_ADDR(comphy_index));
+	data |= COMPHY_TRX_TRAIN_RX_TRAIN_ENABLE;
+	mmio_write_32(COMPHY_TRX_RELATIVE_ADDR(comphy_index), data);
+	mdelay(200);
+	data &= ~COMPHY_TRX_TRAIN_RX_TRAIN_ENABLE;
+	mmio_write_32(COMPHY_TRX_RELATIVE_ADDR(comphy_index), data);
+
+	debug_exit();
+
+	return ret;
+}
+
+static int mvebu_cp110_comphy_pcie_power_on(uint64_t comphy_base,
+				     uint8_t comphy_index, uint32_t comphy_mode)
+{
+	int ret = 0;
+	uint32_t reg, mask, data, pcie_width;
+	uint32_t clk_dir;
+	uintptr_t hpipe_addr, comphy_addr, addr;
+	_Bool clk_src = COMPHY_GET_CLK_SRC(comphy_mode);
+	_Bool called_from_uboot = COMPHY_GET_CALLER(comphy_mode);
+
+	/* In Armada 8K DB boards, PCIe initialization can be executed
+	 * only once (PCIe reset performed during chip power on and
+	 * it cannot be executed via GPIO later).
+	 * This means that power on can be executed only once, so let's
+	 * mark if the caller is bootloader or Linux.
+	 * If bootloader -> run power on.
+	 * If Linux -> exit.
+	 *
+	 * TODO: In MacciatoBIN, PCIe reset is connected via GPIO,
+	 * so after GPIO reset is added to Linux Kernel, it can be
+	 * powered-on by Linux.
+	 */
+	if (!called_from_uboot)
+		return ret;
+
+	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+				comphy_index);
+	comphy_addr = COMPHY_ADDR(comphy_base, comphy_index);
+	pcie_width = COMPHY_GET_PCIE_WIDTH(comphy_mode);
+
+	debug_enter();
+
+	spin_lock(&cp110_mac_reset_lock);
+
+	reg = mmio_read_32(SYS_CTRL_FROM_COMPHY_ADDR(comphy_base) +
+						SYS_CTRL_UINIT_SOFT_RESET_REG);
+	switch (comphy_index) {
+	case COMPHY_LANE0:
+		reg |= PCIE_MAC_RESET_MASK_PORT0;
+		break;
+	case COMPHY_LANE4:
+		reg |= PCIE_MAC_RESET_MASK_PORT1;
+		break;
+	case COMPHY_LANE5:
+		reg |= PCIE_MAC_RESET_MASK_PORT2;
+		break;
+	}
+
+	mmio_write_32(SYS_CTRL_FROM_COMPHY_ADDR(comphy_base) +
+					    SYS_CTRL_UINIT_SOFT_RESET_REG, reg);
+	spin_unlock(&cp110_mac_reset_lock);
+
+	/* Configure PIPE selector for PCIE */
+	mvebu_cp110_comphy_set_pipe_selector(comphy_base, comphy_index,
+					     comphy_mode);
+
+	/*
+	 * Read SAR (Sample-At-Reset) configuration for the PCIe clock
+	 * direction.
+	 *
+	 * SerDes Lane 4/5 got the PCIe ref-clock #1,
+	 * and SerDes Lane 0 got PCIe ref-clock #0
+	 */
+	reg = mmio_read_32(DFX_FROM_COMPHY_ADDR(comphy_base) +
+			   SAR_STATUS_0_REG);
+	if (comphy_index == COMPHY_LANE4 || comphy_index == COMPHY_LANE5)
+		clk_dir = (reg & SAR_RST_PCIE1_CLOCK_CONFIG_CP0_MASK) >>
+					  SAR_RST_PCIE1_CLOCK_CONFIG_CP0_OFFSET;
+	else
+		clk_dir = (reg & SAR_RST_PCIE0_CLOCK_CONFIG_CP0_MASK) >>
+					  SAR_RST_PCIE0_CLOCK_CONFIG_CP0_OFFSET;
+
+	debug("On lane %d\n", comphy_index);
+	debug("PCIe clock direction = %x\n", clk_dir);
+	debug("PCIe Width = %d\n", pcie_width);
+
+	/* enable PCIe X4 and X2 */
+	if (comphy_index == COMPHY_LANE0) {
+		if (pcie_width == PCIE_LNK_X4) {
+			data = 0x1 << COMMON_PHY_SD_CTRL1_PCIE_X4_EN_OFFSET;
+			mask = COMMON_PHY_SD_CTRL1_PCIE_X4_EN_MASK;
+			reg_set(comphy_base + COMMON_PHY_SD_CTRL1,
+				data, mask);
+		} else if (pcie_width == PCIE_LNK_X2) {
+			data = 0x1 << COMMON_PHY_SD_CTRL1_PCIE_X2_EN_OFFSET;
+			mask = COMMON_PHY_SD_CTRL1_PCIE_X2_EN_MASK;
+			reg_set(comphy_base + COMMON_PHY_SD_CTRL1, data, mask);
+		}
+	}
+
+	/* If PCIe clock is output and clock source from SerDes lane 5,
+	 * need to configure the clock-source MUX.
+	 * By default, the clock source is from lane 4
+	 */
+	if (clk_dir && clk_src && (comphy_index == COMPHY_LANE5)) {
+		data = DFX_DEV_GEN_PCIE_CLK_SRC_MUX <<
+						DFX_DEV_GEN_PCIE_CLK_SRC_OFFSET;
+		mask = DFX_DEV_GEN_PCIE_CLK_SRC_MASK;
+		reg_set(DFX_FROM_COMPHY_ADDR(comphy_base) +
+			DFX_DEV_GEN_CTRL12_REG, data, mask);
+	}
+
+	debug("stage: RFU configurations - hard reset comphy\n");
+	/* RFU configurations - hard reset comphy */
+	mask = COMMON_PHY_CFG1_PWR_UP_MASK;
+	data = 0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET;
+	mask |= COMMON_PHY_CFG1_PIPE_SELECT_MASK;
+	data |= 0x1 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET;
+	mask |= COMMON_PHY_CFG1_PWR_ON_RESET_MASK;
+	data |= 0x0 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET;
+	mask |= COMMON_PHY_CFG1_CORE_RSTN_MASK;
+	data |= 0x0 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET;
+	mask |= COMMON_PHY_PHY_MODE_MASK;
+	data |= 0x0 << COMMON_PHY_PHY_MODE_OFFSET;
+	reg_set(comphy_addr + COMMON_PHY_CFG1_REG, data, mask);
+
+	/* release from hard reset */
+	mask = COMMON_PHY_CFG1_PWR_ON_RESET_MASK;
+	data = 0x1 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET;
+	mask |= COMMON_PHY_CFG1_CORE_RSTN_MASK;
+	data |= 0x1 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET;
+	reg_set(comphy_addr + COMMON_PHY_CFG1_REG, data, mask);
+
+	/* Wait 1ms - until band gap and ref clock ready */
+	mdelay(1);
+	/* Start comphy Configuration */
+	debug("stage: Comphy configuration\n");
+	/* Set PIPE soft reset */
+	mask = HPIPE_RST_CLK_CTRL_PIPE_RST_MASK;
+	data = 0x1 << HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET;
+	/* Set PHY datapath width mode for V0 */
+	mask |= HPIPE_RST_CLK_CTRL_FIXED_PCLK_MASK;
+	data |= 0x1 << HPIPE_RST_CLK_CTRL_FIXED_PCLK_OFFSET;
+	/* Set Data bus width USB mode for V0 */
+	mask |= HPIPE_RST_CLK_CTRL_PIPE_WIDTH_MASK;
+	data |= 0x0 << HPIPE_RST_CLK_CTRL_PIPE_WIDTH_OFFSET;
+	/* Set CORE_CLK output frequency for 250Mhz */
+	mask |= HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_MASK;
+	data |= 0x0 << HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_RST_CLK_CTRL_REG, data, mask);
+	/* Set PLL ready delay for 0x2 */
+	data = 0x2 << HPIPE_CLK_SRC_LO_PLL_RDY_DL_OFFSET;
+	mask = HPIPE_CLK_SRC_LO_PLL_RDY_DL_MASK;
+	if (pcie_width != PCIE_LNK_X1) {
+		data |= 0x1 << HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SEL_OFFSET;
+		mask |= HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SEL_MASK;
+		data |= 0x1 << HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SCALE_OFFSET;
+		mask |= HPIPE_CLK_SRC_LO_BUNDLE_PERIOD_SCALE_MASK;
+	}
+	reg_set(hpipe_addr + HPIPE_CLK_SRC_LO_REG, data, mask);
+
+	/* Set PIPE mode interface to PCIe3 - 0x1  & set lane order */
+	data = 0x1 << HPIPE_CLK_SRC_HI_MODE_PIPE_OFFSET;
+	mask = HPIPE_CLK_SRC_HI_MODE_PIPE_MASK;
+	if (pcie_width != PCIE_LNK_X1) {
+		mask |= HPIPE_CLK_SRC_HI_LANE_STRT_MASK;
+		mask |= HPIPE_CLK_SRC_HI_LANE_MASTER_MASK;
+		mask |= HPIPE_CLK_SRC_HI_LANE_BREAK_MASK;
+		if (comphy_index == 0) {
+			data |= 0x1 << HPIPE_CLK_SRC_HI_LANE_STRT_OFFSET;
+			data |= 0x1 << HPIPE_CLK_SRC_HI_LANE_MASTER_OFFSET;
+		} else if (comphy_index == (pcie_width - 1)) {
+			data |= 0x1 << HPIPE_CLK_SRC_HI_LANE_BREAK_OFFSET;
+		}
+	}
+	reg_set(hpipe_addr + HPIPE_CLK_SRC_HI_REG, data, mask);
+	/* Config update polarity equalization */
+	data = 0x1 << HPIPE_CFG_UPDATE_POLARITY_OFFSET;
+	mask = HPIPE_CFG_UPDATE_POLARITY_MASK;
+	reg_set(hpipe_addr + HPIPE_LANE_EQ_CFG1_REG, data, mask);
+	/* Set PIPE version 4 to mode enable */
+	data = 0x1 << HPIPE_DFE_CTRL_28_PIPE4_OFFSET;
+	mask = HPIPE_DFE_CTRL_28_PIPE4_MASK;
+	reg_set(hpipe_addr + HPIPE_DFE_CTRL_28_REG, data, mask);
+	/* TODO: check if pcie clock is output/input - for bringup use input*/
+	/* Enable PIN clock 100M_125M */
+	mask = 0;
+	data = 0;
+	/* Only if clock is output, configure the clock-source mux */
+	if (clk_dir) {
+		mask |= HPIPE_MISC_CLK100M_125M_MASK;
+		data |= 0x1 << HPIPE_MISC_CLK100M_125M_OFFSET;
+	}
+	/* Set PIN_TXDCLK_2X Clock Freq. Selection for outputs 500MHz clock */
+	mask |= HPIPE_MISC_TXDCLK_2X_MASK;
+	data |= 0x0 << HPIPE_MISC_TXDCLK_2X_OFFSET;
+	/* Enable 500MHz Clock */
+	mask |= HPIPE_MISC_CLK500_EN_MASK;
+	data |= 0x1 << HPIPE_MISC_CLK500_EN_OFFSET;
+	if (clk_dir) { /* output */
+		/* Set reference clock comes from group 1 */
+		mask |= HPIPE_MISC_REFCLK_SEL_MASK;
+		data |= 0x0 << HPIPE_MISC_REFCLK_SEL_OFFSET;
+	} else {
+		/* Set reference clock comes from group 2 */
+		mask |= HPIPE_MISC_REFCLK_SEL_MASK;
+		data |= 0x1 << HPIPE_MISC_REFCLK_SEL_OFFSET;
+	}
+	mask |= HPIPE_MISC_ICP_FORCE_MASK;
+	data |= 0x1 << HPIPE_MISC_ICP_FORCE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_MISC_REG, data, mask);
+	if (clk_dir) { /* output */
+		/* Set reference frequcency select - 0x2 for 25MHz*/
+		mask = HPIPE_PWR_PLL_REF_FREQ_MASK;
+		data = 0x2 << HPIPE_PWR_PLL_REF_FREQ_OFFSET;
+	} else {
+		/* Set reference frequcency select - 0x0 for 100MHz*/
+		mask = HPIPE_PWR_PLL_REF_FREQ_MASK;
+		data = 0x0 << HPIPE_PWR_PLL_REF_FREQ_OFFSET;
+	}
+	/* Set PHY mode to PCIe */
+	mask |= HPIPE_PWR_PLL_PHY_MODE_MASK;
+	data |= 0x3 << HPIPE_PWR_PLL_PHY_MODE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PWR_PLL_REG, data, mask);
+
+	/* ref clock alignment */
+	if (pcie_width != PCIE_LNK_X1) {
+		mask = HPIPE_LANE_ALIGN_OFF_MASK;
+		data = 0x0 << HPIPE_LANE_ALIGN_OFF_OFFSET;
+		reg_set(hpipe_addr + HPIPE_LANE_ALIGN_REG, data, mask);
+	}
+
+	/* Set the amount of time spent in the LoZ state - set for 0x7 only if
+	 * the PCIe clock is output
+	 */
+	if (clk_dir)
+		reg_set(hpipe_addr + HPIPE_GLOBAL_PM_CTRL,
+			0x7 << HPIPE_GLOBAL_PM_RXDLOZ_WAIT_OFFSET,
+			HPIPE_GLOBAL_PM_RXDLOZ_WAIT_MASK);
+
+	/* Set Maximal PHY Generation Setting(8Gbps) */
+	mask = HPIPE_INTERFACE_GEN_MAX_MASK;
+	data = 0x2 << HPIPE_INTERFACE_GEN_MAX_OFFSET;
+	/* Bypass frame detection and sync detection for RX DATA */
+	mask |= HPIPE_INTERFACE_DET_BYPASS_MASK;
+	data |= 0x1 << HPIPE_INTERFACE_DET_BYPASS_OFFSET;
+	/* Set Link Train Mode (Tx training control pins are used) */
+	mask |= HPIPE_INTERFACE_LINK_TRAIN_MASK;
+	data |= 0x1 << HPIPE_INTERFACE_LINK_TRAIN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_INTERFACE_REG, data, mask);
+
+	/* Set Idle_sync enable */
+	mask = HPIPE_PCIE_IDLE_SYNC_MASK;
+	data = 0x1 << HPIPE_PCIE_IDLE_SYNC_OFFSET;
+	/* Select bits for PCIE Gen3(32bit) */
+	mask |= HPIPE_PCIE_SEL_BITS_MASK;
+	data |= 0x2 << HPIPE_PCIE_SEL_BITS_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PCIE_REG0, data, mask);
+
+	/* Enable Tx_adapt_g1 */
+	mask = HPIPE_TX_TRAIN_CTRL_G1_MASK;
+	data = 0x1 << HPIPE_TX_TRAIN_CTRL_G1_OFFSET;
+	/* Enable Tx_adapt_gn1 */
+	mask |= HPIPE_TX_TRAIN_CTRL_GN1_MASK;
+	data |= 0x1 << HPIPE_TX_TRAIN_CTRL_GN1_OFFSET;
+	/* Disable Tx_adapt_g0 */
+	mask |= HPIPE_TX_TRAIN_CTRL_G0_MASK;
+	data |= 0x0 << HPIPE_TX_TRAIN_CTRL_G0_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TX_TRAIN_CTRL_REG, data, mask);
+
+	/* Set reg_tx_train_chk_init */
+	mask = HPIPE_TX_TRAIN_CHK_INIT_MASK;
+	data = 0x0 << HPIPE_TX_TRAIN_CHK_INIT_OFFSET;
+	/* Enable TX_COE_FM_PIN_PCIE3_EN */
+	mask |= HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_MASK;
+	data |= 0x1 << HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TX_TRAIN_REG, data, mask);
+
+	debug("stage: TRx training parameters\n");
+	/* Set Preset sweep configurations */
+	mask = HPIPE_TX_TX_STATUS_CHECK_MODE_MASK;
+	data = 0x1 << HPIPE_TX_STATUS_CHECK_MODE_OFFSET;
+	mask |= HPIPE_TX_NUM_OF_PRESET_MASK;
+	data |= 0x7 << HPIPE_TX_NUM_OF_PRESET_OFFSET;
+	mask |= HPIPE_TX_SWEEP_PRESET_EN_MASK;
+	data |= 0x1 << HPIPE_TX_SWEEP_PRESET_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TX_TRAIN_CTRL_11_REG, data, mask);
+
+	/* Tx train start configuration */
+	mask = HPIPE_TX_TRAIN_START_SQ_EN_MASK;
+	data = 0x1 << HPIPE_TX_TRAIN_START_SQ_EN_OFFSET;
+	mask |= HPIPE_TX_TRAIN_START_FRM_DET_EN_MASK;
+	data |= 0x0 << HPIPE_TX_TRAIN_START_FRM_DET_EN_OFFSET;
+	mask |= HPIPE_TX_TRAIN_START_FRM_LOCK_EN_MASK;
+	data |= 0x0 << HPIPE_TX_TRAIN_START_FRM_LOCK_EN_OFFSET;
+	mask |= HPIPE_TX_TRAIN_WAIT_TIME_EN_MASK;
+	data |= 0x1 << HPIPE_TX_TRAIN_WAIT_TIME_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TX_TRAIN_CTRL_5_REG, data, mask);
+
+	/* Enable Tx train P2P */
+	mask = HPIPE_TX_TRAIN_P2P_HOLD_MASK;
+	data = 0x1 << HPIPE_TX_TRAIN_P2P_HOLD_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TX_TRAIN_CTRL_0_REG, data, mask);
+
+	/* Configure Tx train timeout */
+	mask = HPIPE_TRX_TRAIN_TIMER_MASK;
+	data = 0x17 << HPIPE_TRX_TRAIN_TIMER_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TX_TRAIN_CTRL_4_REG, data, mask);
+
+	/* Disable G0/G1/GN1 adaptation */
+	mask = HPIPE_TX_TRAIN_CTRL_G1_MASK | HPIPE_TX_TRAIN_CTRL_GN1_MASK
+		| HPIPE_TX_TRAIN_CTRL_G0_OFFSET;
+	data = 0;
+	reg_set(hpipe_addr + HPIPE_TX_TRAIN_CTRL_REG, data, mask);
+
+	/* Disable DTL frequency loop */
+	mask = HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK;
+	data = 0x0 << HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PWR_CTR_DTL_REG, data, mask);
+
+	/* Configure G3 DFE */
+	mask = HPIPE_G3_DFE_RES_MASK;
+	data = 0x3 << HPIPE_G3_DFE_RES_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G3_SETTING_4_REG, data, mask);
+
+	/* Use TX/RX training result for DFE */
+	mask = HPIPE_DFE_RES_FORCE_MASK;
+	data = 0x0 << HPIPE_DFE_RES_FORCE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_DFE_REG0,  data, mask);
+
+	/* Configure initial and final coefficient value for receiver */
+	mask = HPIPE_G3_SET_1_G3_RX_SELMUPI_MASK;
+	data = 0x1 << HPIPE_G3_SET_1_G3_RX_SELMUPI_OFFSET;
+
+	mask |= HPIPE_G3_SET_1_G3_RX_SELMUPF_MASK;
+	data |= 0x1 << HPIPE_G3_SET_1_G3_RX_SELMUPF_OFFSET;
+
+	mask |= HPIPE_G3_SET_1_G3_SAMPLER_INPAIRX2_EN_MASK;
+	data |= 0x0 << HPIPE_G3_SET_1_G3_SAMPLER_INPAIRX2_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G3_SET_1_REG,  data, mask);
+
+	/* Trigger sampler enable pulse */
+	mask = HPIPE_SMAPLER_MASK;
+	data = 0x1 << HPIPE_SMAPLER_OFFSET;
+	reg_set(hpipe_addr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG, data, mask);
+	udelay(5);
+	reg_set(hpipe_addr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG, 0, mask);
+
+	/* FFE resistor tuning for different bandwidth  */
+	mask = HPIPE_G3_FFE_DEG_RES_LEVEL_MASK;
+	data = 0x1 << HPIPE_G3_FFE_DEG_RES_LEVEL_OFFSET;
+	mask |= HPIPE_G3_FFE_LOAD_RES_LEVEL_MASK;
+	data |= 0x3 << HPIPE_G3_FFE_LOAD_RES_LEVEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G3_SETTING_3_REG, data, mask);
+
+	/* Pattern lock lost timeout disable */
+	mask = HPIPE_PATTERN_LOCK_LOST_TIMEOUT_EN_MASK;
+	data = 0x0 << HPIPE_PATTERN_LOCK_LOST_TIMEOUT_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_FRAME_DETECT_CTRL_3_REG, data, mask);
+
+	/* Configure DFE adaptations */
+	mask = HPIPE_CDR_RX_MAX_DFE_ADAPT_0_MASK;
+	data = 0x0 << HPIPE_CDR_RX_MAX_DFE_ADAPT_0_OFFSET;
+	mask |= HPIPE_CDR_RX_MAX_DFE_ADAPT_1_MASK;
+	data |= 0x0 << HPIPE_CDR_RX_MAX_DFE_ADAPT_1_OFFSET;
+	mask |= HPIPE_CDR_MAX_DFE_ADAPT_0_MASK;
+	data |= 0x0 << HPIPE_CDR_MAX_DFE_ADAPT_0_OFFSET;
+	mask |= HPIPE_CDR_MAX_DFE_ADAPT_1_MASK;
+	data |= 0x1 << HPIPE_CDR_MAX_DFE_ADAPT_1_OFFSET;
+	reg_set(hpipe_addr + HPIPE_CDR_CONTROL_REG, data, mask);
+
+	mask = HPIPE_DFE_TX_MAX_DFE_ADAPT_MASK;
+	data = 0x0 << HPIPE_DFE_TX_MAX_DFE_ADAPT_OFFSET;
+	reg_set(hpipe_addr + HPIPE_DFE_CONTROL_REG, data, mask);
+
+	/* Genration 2 setting 1*/
+	mask = HPIPE_G2_SET_1_G2_RX_SELMUPI_MASK;
+	data = 0x0 << HPIPE_G2_SET_1_G2_RX_SELMUPI_OFFSET;
+	mask |= HPIPE_G2_SET_1_G2_RX_SELMUPF_MASK;
+	data |= 0x1 << HPIPE_G2_SET_1_G2_RX_SELMUPF_OFFSET;
+	mask |= HPIPE_G2_SET_1_G2_RX_SELMUFI_MASK;
+	data |= 0x0 << HPIPE_G2_SET_1_G2_RX_SELMUFI_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G2_SET_1_REG, data, mask);
+
+	/* DFE enable */
+	mask = HPIPE_G2_DFE_RES_MASK;
+	data = 0x3 << HPIPE_G2_DFE_RES_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G2_SETTINGS_4_REG, data, mask);
+
+	/* Configure DFE Resolution */
+	mask = HPIPE_LANE_CFG4_DFE_EN_SEL_MASK;
+	data = 0x1 << HPIPE_LANE_CFG4_DFE_EN_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_LANE_CFG4_REG, data, mask);
+
+	/* VDD calibration control */
+	mask = HPIPE_EXT_SELLV_RXSAMPL_MASK;
+	data = 0x16 << HPIPE_EXT_SELLV_RXSAMPL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_VDD_CAL_CTRL_REG, data, mask);
+
+	/* Set PLL Charge-pump Current Control */
+	mask = HPIPE_G3_SETTING_5_G3_ICP_MASK;
+	data = 0x4 << HPIPE_G3_SETTING_5_G3_ICP_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G3_SETTING_5_REG, data, mask);
+
+	/* Set lane rqualization remote setting */
+	mask = HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_MASK;
+	data = 0x1 << HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_OFFSET;
+	mask |= HPIPE_LANE_CFG_FOM_ONLY_MODE_MASK;
+	data |= 0x1 << HPIPE_LANE_CFG_FOM_ONLY_MODE_OFFFSET;
+	mask |= HPIPE_LANE_CFG_FOM_PRESET_VECTOR_MASK;
+	data |= 0x6 << HPIPE_LANE_CFG_FOM_PRESET_VECTOR_OFFSET;
+	reg_set(hpipe_addr + HPIPE_LANE_EQ_REMOTE_SETTING_REG, data, mask);
+
+	mask = HPIPE_CFG_EQ_BUNDLE_DIS_MASK;
+	data = 0x1 << HPIPE_CFG_EQ_BUNDLE_DIS_OFFSET;
+	reg_set(hpipe_addr + HPIPE_LANE_EQ_CFG2_REG, data, mask);
+
+	debug("stage: Comphy power up\n");
+
+	/* For PCIe X4 or X2:
+	 * release from reset only after finish to configure all lanes
+	 */
+	if ((pcie_width == PCIE_LNK_X1) || (comphy_index == (pcie_width - 1))) {
+		uint32_t i, start_lane, end_lane;
+
+		if (pcie_width != PCIE_LNK_X1) {
+			/* allows writing to all lanes in one write */
+			data = 0x0;
+			if (pcie_width == PCIE_LNK_X2)
+				mask = COMMON_PHY_SD_CTRL1_COMPHY_0_1_PORT_MASK;
+			else if (pcie_width == PCIE_LNK_X4)
+				mask = COMMON_PHY_SD_CTRL1_COMPHY_0_3_PORT_MASK;
+			reg_set(comphy_base + COMMON_PHY_SD_CTRL1, data, mask);
+			start_lane = 0;
+			end_lane = pcie_width;
+
+			/* Release from PIPE soft reset
+			 * For PCIe by4 or by2:
+			 * release from soft reset all lanes - can't use
+			 * read modify write
+			 */
+			reg_set(HPIPE_ADDR(
+				COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base), 0) +
+				HPIPE_RST_CLK_CTRL_REG, 0x24, 0xffffffff);
+		} else {
+			start_lane = comphy_index;
+			end_lane = comphy_index + 1;
+
+			/* Release from PIPE soft reset
+			 * for PCIe by4 or by2:
+			 * release from soft reset all lanes
+			 */
+			reg_set(hpipe_addr + HPIPE_RST_CLK_CTRL_REG,
+				0x0 << HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET,
+				HPIPE_RST_CLK_CTRL_PIPE_RST_MASK);
+		}
+
+		if (pcie_width != PCIE_LNK_X1) {
+			/* disable writing to all lanes with one write */
+			if (pcie_width == PCIE_LNK_X2) {
+				data = (COMPHY_LANE0 <<
+				COMMON_PHY_SD_CTRL1_COMPHY_0_PORT_OFFSET) |
+				(COMPHY_LANE1 <<
+				COMMON_PHY_SD_CTRL1_COMPHY_1_PORT_OFFSET);
+				mask = COMMON_PHY_SD_CTRL1_COMPHY_0_1_PORT_MASK;
+			} else if (pcie_width == PCIE_LNK_X4) {
+				data = (COMPHY_LANE0 <<
+				COMMON_PHY_SD_CTRL1_COMPHY_0_PORT_OFFSET) |
+				(COMPHY_LANE1 <<
+				COMMON_PHY_SD_CTRL1_COMPHY_1_PORT_OFFSET) |
+				(COMPHY_LANE2 <<
+				COMMON_PHY_SD_CTRL1_COMPHY_2_PORT_OFFSET) |
+				(COMPHY_LANE3 <<
+				COMMON_PHY_SD_CTRL1_COMPHY_3_PORT_OFFSET);
+				mask = COMMON_PHY_SD_CTRL1_COMPHY_0_3_PORT_MASK;
+			}
+			reg_set(comphy_base + COMMON_PHY_SD_CTRL1,
+				data, mask);
+		}
+
+		debug("stage: Check PLL\n");
+		/* Read lane status */
+		for (i = start_lane; i < end_lane; i++) {
+			addr = HPIPE_ADDR(
+				COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base), i) +
+				HPIPE_LANE_STATUS1_REG;
+			data = HPIPE_LANE_STATUS1_PCLK_EN_MASK;
+			mask = data;
+			data = polling_with_timeout(addr, data, mask,
+						   PLL_LOCK_TIMEOUT,
+						   REG_32BIT);
+			if (data) {
+				ERROR("Failed to lock PCIE PLL\n");
+				ret = -ETIMEDOUT;
+			}
+		}
+	}
+
+	debug_exit();
+
+	return ret;
+}
+
+static int mvebu_cp110_comphy_rxaui_power_on(uint64_t comphy_base,
+				     uint8_t comphy_index, uint32_t comphy_mode)
+{
+	uintptr_t hpipe_addr, sd_ip_addr, comphy_addr, addr;
+	uint32_t mask, data;
+	int ret = 0;
+
+	debug_enter();
+
+	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+				comphy_index);
+	comphy_addr = COMPHY_ADDR(comphy_base, comphy_index);
+	sd_ip_addr = SD_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+			     comphy_index);
+
+	/* configure phy selector for RXAUI */
+	mvebu_cp110_comphy_set_phy_selector(comphy_base, comphy_index,
+					    comphy_mode);
+
+	/* RFU configurations - hard reset comphy */
+	mask = COMMON_PHY_CFG1_PWR_UP_MASK;
+	data = 0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET;
+	mask |= COMMON_PHY_CFG1_PIPE_SELECT_MASK;
+	data |= 0x0 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET;
+	reg_set(comphy_addr + COMMON_PHY_CFG1_REG, data, mask);
+
+	if (comphy_index == 2) {
+		reg_set(comphy_base + COMMON_PHY_SD_CTRL1,
+			0x1 << COMMON_PHY_SD_CTRL1_RXAUI0_OFFSET,
+			COMMON_PHY_SD_CTRL1_RXAUI0_MASK);
+	}
+	if (comphy_index == 4) {
+		reg_set(comphy_base + COMMON_PHY_SD_CTRL1,
+			0x1 << COMMON_PHY_SD_CTRL1_RXAUI1_OFFSET,
+			COMMON_PHY_SD_CTRL1_RXAUI1_MASK);
+	}
+
+	/* Select Baud Rate of Comphy And PD_PLL/Tx/Rx */
+	mask = SD_EXTERNAL_CONFIG0_SD_PU_PLL_MASK;
+	data = 0x0 << SD_EXTERNAL_CONFIG0_SD_PU_PLL_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_MASK;
+	data |= 0xB << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_MASK;
+	data |= 0xB << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PU_RX_MASK;
+	data |= 0x0 << SD_EXTERNAL_CONFIG0_SD_PU_RX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PU_TX_MASK;
+	data |= 0x0 << SD_EXTERNAL_CONFIG0_SD_PU_TX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_MASK;
+	data |= 0x0 << SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_MEDIA_MODE_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG0_MEDIA_MODE_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG0_REG, data, mask);
+
+	/* release from hard reset */
+	mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK;
+	data = 0x0 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+	data |= 0x0 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+	data |= 0x0 << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK;
+	data = 0x1 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	/* Wait 1ms - until band gap and ref clock ready */
+	mdelay(1);
+
+	/* Start comphy Configuration */
+	debug("stage: Comphy configuration\n");
+	/* set reference clock */
+	reg_set(hpipe_addr + HPIPE_MISC_REG,
+		0x0 << HPIPE_MISC_REFCLK_SEL_OFFSET,
+		HPIPE_MISC_REFCLK_SEL_MASK);
+	/* Power and PLL Control */
+	mask = HPIPE_PWR_PLL_REF_FREQ_MASK;
+	data = 0x1 << HPIPE_PWR_PLL_REF_FREQ_OFFSET;
+	mask |= HPIPE_PWR_PLL_PHY_MODE_MASK;
+	data |= 0x4 << HPIPE_PWR_PLL_PHY_MODE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PWR_PLL_REG, data, mask);
+	/* Loopback register */
+	reg_set(hpipe_addr + HPIPE_LOOPBACK_REG,
+		0x1 << HPIPE_LOOPBACK_SEL_OFFSET, HPIPE_LOOPBACK_SEL_MASK);
+	/* rx control 1 */
+	mask = HPIPE_RX_CONTROL_1_RXCLK2X_SEL_MASK;
+	data = 0x1 << HPIPE_RX_CONTROL_1_RXCLK2X_SEL_OFFSET;
+	mask |= HPIPE_RX_CONTROL_1_CLK8T_EN_MASK;
+	data |= 0x1 << HPIPE_RX_CONTROL_1_CLK8T_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_RX_CONTROL_1_REG, data, mask);
+	/* DTL Control */
+	reg_set(hpipe_addr + HPIPE_PWR_CTR_DTL_REG,
+		0x0 << HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET,
+		HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK);
+
+	/* Set analog parameters from ETP(HW) */
+	debug("stage: Analog parameters from ETP(HW)\n");
+	/* SERDES External Configuration 2 */
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG2_REG,
+		0x1 << SD_EXTERNAL_CONFIG2_PIN_DFE_EN_OFFSET,
+		SD_EXTERNAL_CONFIG2_PIN_DFE_EN_MASK);
+	/* 0x7-DFE Resolution control */
+	reg_set(hpipe_addr + HPIPE_DFE_REG0, 0x1 << HPIPE_DFE_RES_FORCE_OFFSET,
+		HPIPE_DFE_RES_FORCE_MASK);
+	/* 0xd-G1_Setting_0 */
+	reg_set(hpipe_addr + HPIPE_G1_SET_0_REG,
+		0xd << HPIPE_G1_SET_0_G1_TX_EMPH1_OFFSET,
+		HPIPE_G1_SET_0_G1_TX_EMPH1_MASK);
+	/* 0xE-G1_Setting_1 */
+	mask = HPIPE_G1_SET_1_G1_RX_SELMUPI_MASK;
+	data = 0x1 << HPIPE_G1_SET_1_G1_RX_SELMUPI_OFFSET;
+	mask |= HPIPE_G1_SET_1_G1_RX_SELMUPF_MASK;
+	data |= 0x1 << HPIPE_G1_SET_1_G1_RX_SELMUPF_OFFSET;
+	mask |= HPIPE_G1_SET_1_G1_RX_DFE_EN_MASK;
+	data |= 0x1 << HPIPE_G1_SET_1_G1_RX_DFE_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SET_1_REG, data, mask);
+	/* 0xA-DFE_Reg3 */
+	mask = HPIPE_DFE_F3_F5_DFE_EN_MASK;
+	data = 0x0 << HPIPE_DFE_F3_F5_DFE_EN_OFFSET;
+	mask |= HPIPE_DFE_F3_F5_DFE_CTRL_MASK;
+	data |= 0x0 << HPIPE_DFE_F3_F5_DFE_CTRL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_DFE_F3_F5_REG, data, mask);
+
+	/* 0x111-G1_Setting_4 */
+	mask = HPIPE_G1_SETTINGS_4_G1_DFE_RES_MASK;
+	data = 0x1 << HPIPE_G1_SETTINGS_4_G1_DFE_RES_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_4_REG, data, mask);
+
+	debug("stage: RFU configurations- Power Up PLL,Tx,Rx\n");
+	/* SERDES External Configuration */
+	mask = SD_EXTERNAL_CONFIG0_SD_PU_PLL_MASK;
+	data = 0x1 << SD_EXTERNAL_CONFIG0_SD_PU_PLL_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PU_RX_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG0_SD_PU_RX_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG0_SD_PU_TX_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG0_SD_PU_TX_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG0_REG, data, mask);
+
+
+	/* check PLL rx & tx ready */
+	addr = sd_ip_addr + SD_EXTERNAL_STATUS0_REG;
+	data = SD_EXTERNAL_STATUS0_PLL_RX_MASK |
+		SD_EXTERNAL_STATUS0_PLL_TX_MASK;
+	mask = data;
+	data = polling_with_timeout(addr, data, mask, 15000, REG_32BIT);
+	if (data != 0) {
+		debug("Read from reg = %lx - value = 0x%x\n",
+		      sd_ip_addr + SD_EXTERNAL_STATUS0_REG, data);
+		ERROR("SD_EXTERNAL_STATUS0_PLL_RX is %d, -\"-_PLL_TX is %d\n",
+		      (data & SD_EXTERNAL_STATUS0_PLL_RX_MASK),
+		      (data & SD_EXTERNAL_STATUS0_PLL_TX_MASK));
+		ret = -ETIMEDOUT;
+	}
+
+	/* RX init */
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG,
+		0x1 << SD_EXTERNAL_CONFIG1_RX_INIT_OFFSET,
+		SD_EXTERNAL_CONFIG1_RX_INIT_MASK);
+
+	/* check that RX init done */
+	addr = sd_ip_addr + SD_EXTERNAL_STATUS0_REG;
+	data = SD_EXTERNAL_STATUS0_RX_INIT_MASK;
+	mask = data;
+	data = polling_with_timeout(addr, data, mask, 100, REG_32BIT);
+	if (data != 0) {
+		debug("Read from reg = %lx - value = 0x%x\n",
+		      sd_ip_addr + SD_EXTERNAL_STATUS0_REG, data);
+		ERROR("SD_EXTERNAL_STATUS0_RX_INIT is 0\n");
+		ret = -ETIMEDOUT;
+	}
+
+	debug("stage: RF Reset\n");
+	/* RF Reset */
+	mask =  SD_EXTERNAL_CONFIG1_RX_INIT_MASK;
+	data = 0x0 << SD_EXTERNAL_CONFIG1_RX_INIT_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	debug_exit();
+
+	return ret;
+}
+
+static int mvebu_cp110_comphy_usb3_power_on(uint64_t comphy_base,
+				     uint8_t comphy_index, uint32_t comphy_mode)
+{
+	uintptr_t hpipe_addr, comphy_addr, addr;
+	uint32_t mask, data;
+	uint8_t ap_nr, cp_nr, phy_polarity_invert;
+	int ret = 0;
+
+	debug_enter();
+
+	/* Configure PIPE selector for USB3 */
+	mvebu_cp110_comphy_set_pipe_selector(comphy_base, comphy_index,
+					     comphy_mode);
+
+	mvebu_cp110_get_ap_and_cp_nr(&ap_nr, &cp_nr, comphy_base);
+
+	const struct usb_params *usb_static_values =
+			&usb_static_values_tab[ap_nr][cp_nr][comphy_index];
+
+	phy_polarity_invert = usb_static_values->polarity_invert;
+
+	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+				comphy_index);
+	comphy_addr = COMPHY_ADDR(comphy_base, comphy_index);
+
+	debug("stage: RFU configurations - hard reset comphy\n");
+	/* RFU configurations - hard reset comphy */
+	mask = COMMON_PHY_CFG1_PWR_UP_MASK;
+	data = 0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET;
+	mask |= COMMON_PHY_CFG1_PIPE_SELECT_MASK;
+	data |= 0x1 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET;
+	mask |= COMMON_PHY_CFG1_PWR_ON_RESET_MASK;
+	data |= 0x0 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET;
+	mask |= COMMON_PHY_CFG1_CORE_RSTN_MASK;
+	data |= 0x0 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET;
+	mask |= COMMON_PHY_PHY_MODE_MASK;
+	data |= 0x1 << COMMON_PHY_PHY_MODE_OFFSET;
+	reg_set(comphy_addr + COMMON_PHY_CFG1_REG, data, mask);
+
+	/* release from hard reset */
+	mask = COMMON_PHY_CFG1_PWR_ON_RESET_MASK;
+	data = 0x1 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET;
+	mask |= COMMON_PHY_CFG1_CORE_RSTN_MASK;
+	data |= 0x1 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET;
+	reg_set(comphy_addr + COMMON_PHY_CFG1_REG, data, mask);
+
+	/* Wait 1ms - until band gap and ref clock ready */
+	mdelay(1);
+
+	/* Start comphy Configuration */
+	debug("stage: Comphy configuration\n");
+	/* Set PIPE soft reset */
+	mask = HPIPE_RST_CLK_CTRL_PIPE_RST_MASK;
+	data = 0x1 << HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET;
+	/* Set PHY datapath width mode for V0 */
+	mask |= HPIPE_RST_CLK_CTRL_FIXED_PCLK_MASK;
+	data |= 0x0 << HPIPE_RST_CLK_CTRL_FIXED_PCLK_OFFSET;
+	/* Set Data bus width USB mode for V0 */
+	mask |= HPIPE_RST_CLK_CTRL_PIPE_WIDTH_MASK;
+	data |= 0x0 << HPIPE_RST_CLK_CTRL_PIPE_WIDTH_OFFSET;
+	/* Set CORE_CLK output frequency for 250Mhz */
+	mask |= HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_MASK;
+	data |= 0x0 << HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_RST_CLK_CTRL_REG, data, mask);
+	/* Set PLL ready delay for 0x2 */
+	reg_set(hpipe_addr + HPIPE_CLK_SRC_LO_REG,
+		0x2 << HPIPE_CLK_SRC_LO_PLL_RDY_DL_OFFSET,
+		HPIPE_CLK_SRC_LO_PLL_RDY_DL_MASK);
+	/* Set reference clock to come from group 1 - 25Mhz */
+	reg_set(hpipe_addr + HPIPE_MISC_REG,
+		0x0 << HPIPE_MISC_REFCLK_SEL_OFFSET,
+		HPIPE_MISC_REFCLK_SEL_MASK);
+	/* Set reference frequcency select - 0x2 */
+	mask = HPIPE_PWR_PLL_REF_FREQ_MASK;
+	data = 0x2 << HPIPE_PWR_PLL_REF_FREQ_OFFSET;
+	/* Set PHY mode to USB - 0x5 */
+	mask |= HPIPE_PWR_PLL_PHY_MODE_MASK;
+	data |= 0x5 << HPIPE_PWR_PLL_PHY_MODE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PWR_PLL_REG, data, mask);
+	/* Set the amount of time spent in the LoZ state - set for 0x7 */
+	reg_set(hpipe_addr + HPIPE_GLOBAL_PM_CTRL,
+		0x7 << HPIPE_GLOBAL_PM_RXDLOZ_WAIT_OFFSET,
+		HPIPE_GLOBAL_PM_RXDLOZ_WAIT_MASK);
+	/* Set max PHY generation setting - 5Gbps */
+	reg_set(hpipe_addr + HPIPE_INTERFACE_REG,
+		0x1 << HPIPE_INTERFACE_GEN_MAX_OFFSET,
+		HPIPE_INTERFACE_GEN_MAX_MASK);
+	/* Set select data width 20Bit (SEL_BITS[2:0]) */
+	reg_set(hpipe_addr + HPIPE_LOOPBACK_REG,
+		0x1 << HPIPE_LOOPBACK_SEL_OFFSET,
+		HPIPE_LOOPBACK_SEL_MASK);
+	/* select de-emphasize 3.5db */
+	reg_set(hpipe_addr + HPIPE_LANE_CONFIG0_REG,
+		0x1 << HPIPE_LANE_CONFIG0_TXDEEMPH0_OFFSET,
+		HPIPE_LANE_CONFIG0_TXDEEMPH0_MASK);
+	/* override tx margining from the MAC */
+	reg_set(hpipe_addr + HPIPE_TST_MODE_CTRL_REG,
+		0x1 << HPIPE_TST_MODE_CTRL_MODE_MARGIN_OFFSET,
+		HPIPE_TST_MODE_CTRL_MODE_MARGIN_MASK);
+
+	/* The polarity inversion for USB was not tested due to lack of hw
+	 * design which requires it. Support is added for customer needs.
+	 */
+	if (phy_polarity_invert)
+		mvebu_cp110_polarity_invert(hpipe_addr + HPIPE_SYNC_PATTERN_REG,
+					    phy_polarity_invert);
+
+	/* Start analog parameters from ETP(HW) */
+	debug("stage: Analog parameters from ETP(HW)\n");
+	/* Set Pin DFE_PAT_DIS -> Bit[1]: PIN_DFE_PAT_DIS = 0x0 */
+	mask = HPIPE_LANE_CFG4_DFE_CTRL_MASK;
+	data = 0x1 << HPIPE_LANE_CFG4_DFE_CTRL_OFFSET;
+	/* Set Override PHY DFE control pins for 0x1 */
+	mask |= HPIPE_LANE_CFG4_DFE_OVER_MASK;
+	data |= 0x1 << HPIPE_LANE_CFG4_DFE_OVER_OFFSET;
+	/* Set Spread Spectrum Clock Enable fot 0x1 */
+	mask |= HPIPE_LANE_CFG4_SSC_CTRL_MASK;
+	data |= 0x1 << HPIPE_LANE_CFG4_SSC_CTRL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_LANE_CFG4_REG, data, mask);
+	/* Confifure SSC amplitude */
+	mask = HPIPE_G2_TX_SSC_AMP_MASK;
+	data = 0x1f << HPIPE_G2_TX_SSC_AMP_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G2_SET_2_REG, data, mask);
+	/* End of analog parameters */
+
+	debug("stage: Comphy power up\n");
+	/* Release from PIPE soft reset */
+	reg_set(hpipe_addr + HPIPE_RST_CLK_CTRL_REG,
+		0x0 << HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET,
+		HPIPE_RST_CLK_CTRL_PIPE_RST_MASK);
+
+	/* wait 15ms - for comphy calibration done */
+	debug("stage: Check PLL\n");
+	/* Read lane status */
+	addr = hpipe_addr + HPIPE_LANE_STATUS1_REG;
+	data = HPIPE_LANE_STATUS1_PCLK_EN_MASK;
+	mask = data;
+	data = polling_with_timeout(addr, data, mask, 15000, REG_32BIT);
+	if (data != 0) {
+		debug("Read from reg = %lx - value = 0x%x\n",
+			hpipe_addr + HPIPE_LANE_STATUS1_REG, data);
+		ERROR("HPIPE_LANE_STATUS1_PCLK_EN_MASK is 0\n");
+		ret = -ETIMEDOUT;
+	}
+
+	debug_exit();
+
+	return ret;
+}
+
+static void rx_pre_train(uint64_t comphy_base, uint8_t comphy_index)
+{
+	uintptr_t hpipe_addr;
+	uint32_t mask, data;
+
+	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+				comphy_index);
+
+	debug("rx_training preparation\n\n");
+
+	mask = HPIPE_TRX0_GAIN_TRAIN_WITH_C_MASK;
+	data = (0x1 << HPIPE_TRX0_GAIN_TRAIN_WITH_C_OFF);
+	mask |= HPIPE_TRX0_GAIN_TRAIN_WITH_SAMPLER_MASK;
+	data |= (0x0 << HPIPE_TRX0_GAIN_TRAIN_WITH_SAMPLER_OFF);
+	reg_set(hpipe_addr + HPIPE_TRX0_REG, data, mask);
+
+
+	mask = HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_MASK;
+	data = (0x1e << HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_OFF);
+	mask |= HPIPE_TRX_REG2_SUMF_BOOST_TARGET_K_MASK;
+	data |= (0x0 << HPIPE_TRX_REG2_SUMF_BOOST_TARGET_K_OFF);
+	reg_set(hpipe_addr + HPIPE_TRX_REG2, data, mask);
+
+	mask = HPIPE_TRX_REG1_MIN_BOOST_MODE_MASK;
+	data = (0x1 << HPIPE_TRX_REG1_MIN_BOOST_MODE_OFF);
+	reg_set(hpipe_addr + HPIPE_TRX_REG1, data, mask);
+
+	mask = HPIPE_CRD2_CRD_MIDPOINT_SMALL_THRES_K_MASK;
+	data = (0x8 << HPIPE_CRD2_CRD_MIDPOINT_SMALL_THRES_K_OFF);
+	reg_set(hpipe_addr + HPIPE_CDR_CONTROL1_REG, data, mask);
+
+	mask = HPIPE_CRD2_CRD_MIDPOINT_LARGE_THRES_K_MASK;
+	data = (0x8 << HPIPE_CRD2_CRD_MIDPOINT_LARGE_THRES_K_OFF);
+	reg_set(hpipe_addr + HPIPE_CDR_CONTROL2_REG, data, mask);
+
+	mask = HPIPE_CRD_MIDPOINT_PHASE_OS_MASK;
+	data = (0x0 << HPIPE_CRD_MIDPOINT_PHASE_OS_OFFSET);
+	reg_set(hpipe_addr + HPIPE_CDR_CONTROL_REG, data, mask);
+
+	mask = HPIPE_TRX_REG1_SUMFTAP_EN_MASK;
+	data = (0x38 << HPIPE_TRX_REG1_SUMFTAP_EN_OFF);
+	mask |= HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_MASK;
+	data |= (0x1e << HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_OFF);
+	reg_set(hpipe_addr + HPIPE_TRX_REG1, data, mask);
+}
+
+int mvebu_cp110_comphy_xfi_rx_training(uint64_t comphy_base,
+					      uint8_t comphy_index)
+{
+	uint32_t mask, data, timeout;
+	uint32_t g1_ffe_cap_sel, g1_ffe_res_sel, align90, g1_dfe_res;
+	uintptr_t hpipe_addr;
+
+	uint8_t ap_nr, cp_nr;
+
+	mvebu_cp110_get_ap_and_cp_nr(&ap_nr, &cp_nr, comphy_base);
+
+	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+				comphy_index);
+
+	debug_enter();
+
+	rx_pre_train(comphy_base, comphy_index);
+
+	debug("Preparation for rx_training\n\n");
+
+	/* Use the FFE table */
+	mask = HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_MASK;
+	data = 0 << HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_3_REG, data, mask);
+
+	/* Use auto-calibration value */
+	mask = HPIPE_CAL_RXCLKALIGN_90_EXT_EN_MASK;
+	data = 0 << HPIPE_CAL_RXCLKALIGN_90_EXT_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_RX_CLK_ALIGN90_AND_TX_IDLE_CALIB_CTRL_REG,
+		data, mask);
+
+	/* Use Tx/Rx training results */
+	mask = HPIPE_DFE_RES_FORCE_MASK;
+	data = 0 << HPIPE_DFE_RES_FORCE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_DFE_REG0, data, mask);
+
+	debug("Enable RX training\n\n");
+
+	mask = HPIPE_TRX_RX_TRAIN_EN_MASK;
+	data = 0x1 << HPIPE_TRX_RX_TRAIN_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TRX_TRAIN_CTRL_0_REG, data, mask);
+
+	/* Check the result of RX training */
+	timeout = RX_TRAINING_TIMEOUT;
+	mask = HPIPE_INTERRUPT_TRX_TRAIN_DONE_OFFSET |
+		HPIPE_INTERRUPT_DFE_DONE_INT_OFFSET |
+		HPIPE_INTERRUPT_RX_TRAIN_COMPLETE_INT_MASK;
+	while (timeout) {
+		data = mmio_read_32(hpipe_addr + HPIPE_INTERRUPT_1_REGISTER);
+		if (data & mask)
+			break;
+		mdelay(1);
+		timeout--;
+	}
+
+	debug("RX training result: interrupt reg 0x%lx = 0x%x\n\n",
+	       hpipe_addr + HPIPE_INTERRUPT_1_REGISTER, data);
+
+	if (timeout == 0 || data & HPIPE_TRX_TRAIN_TIME_OUT_INT_MASK) {
+		ERROR("Rx training timeout...\n");
+		return -ETIMEDOUT;
+	}
+
+	if (data & HPIPE_TRX_TRAIN_FAILED_MASK) {
+		ERROR("Rx training failed...\n");
+		return -EINVAL;
+	}
+
+	mask = HPIPE_TRX_RX_TRAIN_EN_MASK;
+	data = 0x0 << HPIPE_TRX_RX_TRAIN_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_TRX_TRAIN_CTRL_0_REG, data, mask);
+
+	debug("Training done, reading results...\n\n");
+
+	mask = HPIPE_ADAPTED_FFE_ADAPTED_FFE_RES_MASK;
+	g1_ffe_res_sel = ((mmio_read_32(hpipe_addr +
+			   HPIPE_ADAPTED_FFE_CAPACITOR_COUNTER_CTRL_REG)
+			   & mask) >> HPIPE_ADAPTED_FFE_ADAPTED_FFE_RES_OFFSET);
+
+	mask = HPIPE_ADAPTED_FFE_ADAPTED_FFE_CAP_MASK;
+	g1_ffe_cap_sel = ((mmio_read_32(hpipe_addr +
+			   HPIPE_ADAPTED_FFE_CAPACITOR_COUNTER_CTRL_REG)
+			   & mask) >> HPIPE_ADAPTED_FFE_ADAPTED_FFE_CAP_OFFSET);
+
+	mask = HPIPE_DATA_PHASE_ADAPTED_OS_PH_MASK;
+	align90 = ((mmio_read_32(hpipe_addr + HPIPE_DATA_PHASE_OFF_CTRL_REG)
+		    & mask) >> HPIPE_DATA_PHASE_ADAPTED_OS_PH_OFFSET);
+
+	mask = HPIPE_ADAPTED_DFE_RES_MASK;
+	g1_dfe_res = ((mmio_read_32(hpipe_addr +
+		       HPIPE_ADAPTED_DFE_COEFFICIENT_1_REG)
+		       & mask) >> HPIPE_ADAPTED_DFE_RES_OFFSET);
+
+	debug("================================================\n");
+	debug("Switching to static configuration:\n");
+	debug("FFE_RES = 0x%x FFE_CAP = 0x%x align90 = 0x%x g1_dfe_res 0x%x\n",
+	       g1_ffe_res_sel, g1_ffe_cap_sel, align90, g1_dfe_res);
+	debug("Result after training: 0x%lx= 0x%x, 0x%lx= 0x%x, 0x%lx = 0x%x\n",
+	      (hpipe_addr + HPIPE_ADAPTED_FFE_CAPACITOR_COUNTER_CTRL_REG),
+	       mmio_read_32(hpipe_addr +
+			    HPIPE_ADAPTED_FFE_CAPACITOR_COUNTER_CTRL_REG),
+			    (hpipe_addr + HPIPE_DATA_PHASE_OFF_CTRL_REG),
+	       mmio_read_32(hpipe_addr + HPIPE_DATA_PHASE_OFF_CTRL_REG),
+			    (hpipe_addr + HPIPE_ADAPTED_DFE_COEFFICIENT_1_REG),
+	       mmio_read_32(hpipe_addr + HPIPE_ADAPTED_DFE_COEFFICIENT_1_REG));
+	debug("================================================\n");
+
+	/* Update FFE_RES */
+	mask = HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_MASK;
+	data = g1_ffe_res_sel << HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_3_REG, data, mask);
+
+	/* Update FFE_CAP */
+	mask = HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_MASK;
+	data = g1_ffe_cap_sel << HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_3_REG, data, mask);
+
+	/* Bypass the FFE table settings and use the FFE settings directly from
+	 * registers FFE_RES_SEL and FFE_CAP_SEL
+	 */
+	mask = HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_MASK;
+	data = 1 << HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_3_REG, data, mask);
+
+	/* Force DFE resolution (use gen table value) */
+	mask = HPIPE_DFE_RES_FORCE_MASK;
+	data = 0x1 << HPIPE_DFE_RES_FORCE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_DFE_REG0, data, mask);
+
+	/* 0x111-G1 DFE_Setting_4 */
+	mask = HPIPE_G1_SETTINGS_4_G1_DFE_RES_MASK;
+	data = g1_dfe_res << HPIPE_G1_SETTINGS_4_G1_DFE_RES_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_4_REG, data, mask);
+
+	printf("########################################################\n");
+	printf("# To use trained values update the ATF sources:\n");
+	printf("# plat/marvell/armada/a8k/<board_type>/board/phy-porting-layer.h ");
+	printf("file\n# with new values as below (for appropriate AP nr %d",
+	       ap_nr);
+	printf("and CP nr: %d comphy_index %d\n\n",
+	       cp_nr, comphy_index);
+	printf("static struct xfi_params xfi_static_values_tab[AP_NUM]");
+	printf("[CP_NUM][MAX_LANE_NR] = {\n");
+	printf("\t...\n");
+	printf("\t.g1_ffe_res_sel = 0x%x,\n", g1_ffe_res_sel);
+	printf("\t.g1_ffe_cap_sel = 0x%x,\n", g1_ffe_cap_sel);
+	printf("\t.align90 = 0x%x,\n", align90);
+	printf("\t.g1_dfe_res = 0x%x\n", g1_dfe_res);
+	printf("\t...\n");
+	printf("};\n\n");
+	printf("########################################################\n");
+
+	rx_trainng_done[ap_nr][cp_nr][comphy_index] = 1;
+
+	return 0;
+}
+
+/* During AP the proper mode is auto-negotiated and the mac, pcs and serdes
+ * configuration are done by the firmware loaded to the MG's CM3 for appropriate
+ * negotiated mode. Therefore there is no need to configure the mac, pcs and
+ * serdes from u-boot. The only thing that need to be setup is powering up
+ * the comphy, which is done through Common PHY<n> Configuration 1 Register
+ * (CP0: 0xF2441000, CP1: 0xF4441000). This step can't be done by MG's CM3,
+ * since it doesn't have an access to this register-set (but it has access to
+ * the network registers like: MG, AP, MAC, PCS, Serdes etc.)
+ */
+static int mvebu_cp110_comphy_ap_power_on(uint64_t comphy_base,
+					  uint8_t comphy_index,
+					  uint32_t comphy_mode)
+{
+	uint32_t mask, data;
+	uintptr_t comphy_addr = comphy_addr =
+				COMPHY_ADDR(comphy_base, comphy_index);
+
+	/* configure phy selector for XFI/SFI */
+	mvebu_cp110_comphy_set_phy_selector(comphy_base, comphy_index,
+					    comphy_mode);
+	debug_enter();
+	debug("stage: RFU configurations - hard reset comphy\n");
+	/* RFU configurations - hard reset comphy */
+	mask = COMMON_PHY_CFG1_PWR_UP_MASK;
+	data = 0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET;
+	mask |= COMMON_PHY_CFG1_PIPE_SELECT_MASK;
+	data |= 0x0 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET;
+	reg_set(comphy_addr + COMMON_PHY_CFG1_REG, data, mask);
+	debug_exit();
+
+#if MSS_SUPPORT
+	do {
+		uint8_t ap_nr, cp_nr;
+
+		/* start ap fw */
+		mvebu_cp110_get_ap_and_cp_nr(&ap_nr, &cp_nr, comphy_base);
+		mg_start_ap_fw(cp_nr, comphy_index);
+
+	} while (0);
+#endif
+	return 0;
+}
+
+/*
+ * This function allows to reset the digital synchronizers between
+ * the MAC and the PHY, it is required when the MAC changes its state.
+ */
+int mvebu_cp110_comphy_digital_reset(uint64_t comphy_base,
+				     uint8_t comphy_index,
+				     uint32_t comphy_mode, uint32_t command)
+{
+	int mode = COMPHY_GET_MODE(comphy_mode);
+	uintptr_t sd_ip_addr;
+	uint32_t mask, data;
+
+	sd_ip_addr = SD_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+			     comphy_index);
+
+	switch (mode) {
+	case (COMPHY_SGMII_MODE):
+	case (COMPHY_2500BASEX_MODE):
+	case (COMPHY_XFI_MODE):
+	case (COMPHY_SFI_MODE):
+	case (COMPHY_RXAUI_MODE):
+		mask = SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+		data = ((command == COMPHY_COMMAND_DIGITAL_PWR_OFF) ?
+			0x0 : 0x1) << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET;
+		reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+		break;
+	default:
+		ERROR("comphy%d: Digital PWR ON/OFF is not supported\n",
+			comphy_index);
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+int mvebu_cp110_comphy_power_on(uint64_t comphy_base,
+				uint8_t comphy_index,
+				uint64_t comphy_mode,
+				uint64_t comphy_train_base)
+{
+	int mode = COMPHY_GET_MODE(comphy_mode);
+	int err = 0;
+
+	debug_enter();
+
+	switch (mode) {
+	case(COMPHY_SATA_MODE):
+		err = mvebu_cp110_comphy_sata_power_on(comphy_base,
+						       comphy_index,
+						       comphy_mode);
+		break;
+	case(COMPHY_SGMII_MODE):
+	case(COMPHY_2500BASEX_MODE):
+		err = mvebu_cp110_comphy_sgmii_power_on(comphy_base,
+							comphy_index,
+							comphy_mode);
+		break;
+	/* From comphy perspective, XFI and SFI are the same */
+	case (COMPHY_XFI_MODE):
+	case (COMPHY_SFI_MODE):
+		err = mvebu_cp110_comphy_xfi_power_on(comphy_base,
+						      comphy_index,
+						      comphy_mode,
+						      comphy_train_base);
+		break;
+	case (COMPHY_PCIE_MODE):
+		err = mvebu_cp110_comphy_pcie_power_on(comphy_base,
+						       comphy_index,
+						       comphy_mode);
+		break;
+	case (COMPHY_RXAUI_MODE):
+		err = mvebu_cp110_comphy_rxaui_power_on(comphy_base,
+							comphy_index,
+							comphy_mode);
+		break;
+	case (COMPHY_USB3H_MODE):
+	case (COMPHY_USB3D_MODE):
+		err = mvebu_cp110_comphy_usb3_power_on(comphy_base,
+						       comphy_index,
+						       comphy_mode);
+		break;
+	case (COMPHY_AP_MODE):
+		err = mvebu_cp110_comphy_ap_power_on(comphy_base, comphy_index,
+						     comphy_mode);
+		break;
+	default:
+		ERROR("comphy%d: unsupported comphy mode\n", comphy_index);
+		err = -EINVAL;
+		break;
+	}
+
+	debug_exit();
+
+	return err;
+}
+
+int mvebu_cp110_comphy_power_off(uint64_t comphy_base, uint8_t comphy_index,
+				 uint64_t comphy_mode)
+{
+	uintptr_t sd_ip_addr, comphy_ip_addr;
+	uint32_t mask, data;
+	uint8_t ap_nr, cp_nr;
+	_Bool called_from_uboot = COMPHY_GET_CALLER(comphy_mode);
+
+	debug_enter();
+
+	/* Power-off might happen because of 2 things:
+	 *	1. Bootloader turns off unconnected lanes
+	 *	2. Linux turns off all lanes during boot
+	 *	   (and then reconfigure it).
+	 *
+	 * For PCIe, there's a problem:
+	 * In Armada 8K DB boards, PCIe initialization can be executed
+	 * only once (PCIe reset performed during chip power on and
+	 * it cannot be executed via GPIO later) so a lane configured to
+	 * PCIe should not be powered off by Linux.
+	 *
+	 * So, check 2 things:
+	 *	1. Is Linux called for power-off?
+	 *	2. Is the comphy configured to PCIe?
+	 * If the answer is YES for both 1 and 2, skip the power-off.
+	 *
+	 * TODO: In MacciatoBIN, PCIe reset is connected via GPIO,
+	 * so after GPIO reset is added to Linux Kernel, it can be
+	 * powered-off.
+	 */
+	if (!called_from_uboot) {
+		data = mmio_read_32(comphy_base +
+				    COMMON_SELECTOR_PIPE_REG_OFFSET);
+		data >>= (COMMON_SELECTOR_COMPHYN_FIELD_WIDTH * comphy_index);
+		data &= COMMON_SELECTOR_COMPHY_MASK;
+		if (data == COMMON_SELECTOR_PIPE_COMPHY_PCIE)
+			return 0;
+	}
+
+	mvebu_cp110_get_ap_and_cp_nr(&ap_nr, &cp_nr, comphy_base);
+
+	if (rx_trainng_done[ap_nr][cp_nr][comphy_index]) {
+		debug("Skip %s for comphy[%d][%d][%d], due to rx training\n",
+		       __func__, ap_nr, cp_nr, comphy_index);
+		return 0;
+	}
+
+	sd_ip_addr = SD_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+			     comphy_index);
+	comphy_ip_addr = COMPHY_ADDR(comphy_base, comphy_index);
+
+	/* Hard reset the comphy, for Ethernet modes and Sata */
+	mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK;
+	data = 0x0 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+	data |= 0x0 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+	data |= 0x0 << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
+	/* PCIe reset */
+	spin_lock(&cp110_mac_reset_lock);
+
+	/* The mvebu_cp110_comphy_power_off will be called only from Linux (to
+	 * override settings done by bootloader) and it will be relevant only
+	 * to PCIe (called before check if to skip pcie power off or not).
+	 */
+	data = mmio_read_32(SYS_CTRL_FROM_COMPHY_ADDR(comphy_base) +
+						 SYS_CTRL_UINIT_SOFT_RESET_REG);
+	switch (comphy_index) {
+	case COMPHY_LANE0:
+		data &= ~PCIE_MAC_RESET_MASK_PORT0;
+		break;
+	case COMPHY_LANE4:
+		data &= ~PCIE_MAC_RESET_MASK_PORT1;
+		break;
+	case COMPHY_LANE5:
+		data &= ~PCIE_MAC_RESET_MASK_PORT2;
+		break;
+	}
+
+	mmio_write_32(SYS_CTRL_FROM_COMPHY_ADDR(comphy_base) +
+					   SYS_CTRL_UINIT_SOFT_RESET_REG, data);
+	spin_unlock(&cp110_mac_reset_lock);
+
+	/* Hard reset the comphy, for PCIe and usb3 */
+	mask = COMMON_PHY_CFG1_PWR_ON_RESET_MASK;
+	data = 0x0 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET;
+	mask |= COMMON_PHY_CFG1_CORE_RSTN_MASK;
+	data |= 0x0 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET;
+	reg_set(comphy_ip_addr + COMMON_PHY_CFG1_REG, data, mask);
+
+	/* Clear comphy PHY and PIPE selector, can't rely on previous config. */
+	mvebu_cp110_comphy_clr_phy_selector(comphy_base, comphy_index);
+	mvebu_cp110_comphy_clr_pipe_selector(comphy_base, comphy_index);
+
+	debug_exit();
+
+	return 0;
+}
diff --git a/drivers/marvell/comphy/phy-comphy-cp110.h b/drivers/marvell/comphy/phy-comphy-cp110.h
new file mode 100644
index 0000000..0be6c26
--- /dev/null
+++ b/drivers/marvell/comphy/phy-comphy-cp110.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* Those are parameters for xfi mode, which need to be tune for each board type.
+ * For known DB boards the parameters was already calibrated and placed under
+ * the plat/marvell/armada/a8k/<board_type>/board/phy-porting-layer.h
+ */
+struct xfi_params {
+	uint8_t g1_ffe_res_sel;
+	uint8_t g1_ffe_cap_sel;
+	uint8_t align90;
+	uint8_t g1_dfe_res;
+	uint8_t g1_amp;
+	uint8_t g1_emph;
+	uint8_t g1_emph_en;
+	uint8_t g1_tx_amp_adj;
+	uint8_t g1_tx_emph_en;
+	uint8_t g1_tx_emph;
+	uint8_t g1_rx_selmuff;
+	uint8_t g1_rx_selmufi;
+	uint8_t g1_rx_selmupf;
+	uint8_t g1_rx_selmupi;
+	_Bool valid;
+};
+
+struct sata_params {
+	uint8_t g1_amp;
+	uint8_t g2_amp;
+	uint8_t g3_amp;
+
+	uint8_t g1_emph;
+	uint8_t g2_emph;
+	uint8_t g3_emph;
+
+	uint8_t g1_emph_en;
+	uint8_t g2_emph_en;
+	uint8_t g3_emph_en;
+
+	uint8_t g1_tx_amp_adj;
+	uint8_t g2_tx_amp_adj;
+	uint8_t g3_tx_amp_adj;
+
+	uint8_t g1_tx_emph_en;
+	uint8_t g2_tx_emph_en;
+	uint8_t g3_tx_emph_en;
+
+	uint8_t g1_tx_emph;
+	uint8_t g2_tx_emph;
+	uint8_t g3_tx_emph;
+
+	uint8_t g3_dfe_res;
+
+	uint8_t g3_ffe_res_sel;
+
+	uint8_t g3_ffe_cap_sel;
+
+	uint8_t align90;
+
+	uint8_t g1_rx_selmuff;
+	uint8_t g2_rx_selmuff;
+	uint8_t g3_rx_selmuff;
+
+	uint8_t g1_rx_selmufi;
+	uint8_t g2_rx_selmufi;
+	uint8_t g3_rx_selmufi;
+
+	uint8_t g1_rx_selmupf;
+	uint8_t g2_rx_selmupf;
+	uint8_t g3_rx_selmupf;
+
+	uint8_t g1_rx_selmupi;
+	uint8_t g2_rx_selmupi;
+	uint8_t g3_rx_selmupi;
+
+	uint8_t polarity_invert;
+
+	_Bool valid;
+};
+
+struct usb_params {
+	uint8_t polarity_invert;
+};
+
+int mvebu_cp110_comphy_is_pll_locked(uint64_t comphy_base,
+				     uint8_t comphy_index);
+int mvebu_cp110_comphy_power_off(uint64_t comphy_base,
+				 uint8_t comphy_index, uint64_t comphy_mode);
+int mvebu_cp110_comphy_power_on(uint64_t comphy_base, uint8_t comphy_index,
+				uint64_t comphy_mode,
+				uint64_t comphy_train_base);
+int mvebu_cp110_comphy_xfi_rx_training(uint64_t comphy_base,
+				       uint8_t comphy_index);
+int mvebu_cp110_comphy_digital_reset(uint64_t comphy_base, uint8_t comphy_index,
+				     uint32_t comphy_mode, uint32_t command);
+
+#define COMPHY_POLARITY_NO_INVERT	0
+#define COMPHY_POLARITY_TXD_INVERT	1
+#define COMPHY_POLARITY_RXD_INVERT	2
diff --git a/drivers/marvell/comphy/phy-default-porting-layer.h b/drivers/marvell/comphy/phy-default-porting-layer.h
new file mode 100644
index 0000000..3c63c64
--- /dev/null
+++ b/drivers/marvell/comphy/phy-default-porting-layer.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef PHY_DEFAULT_PORTING_LAYER_H
+#define PHY_DEFAULT_PORTING_LAYER_H
+
+
+#define MAX_LANE_NR		6
+
+#warning "Using default comphy params - you may need to suit them to your board"
+
+static const struct xfi_params
+	xfi_static_values_tab[AP_NUM][CP_NUM][MAX_LANE_NR] = {
+	[0 ... AP_NUM-1][0 ... CP_NUM-1][0 ... MAX_LANE_NR-1] = {
+		.g1_ffe_res_sel = 0x3, .g1_ffe_cap_sel = 0xf, .align90 = 0x5f,
+		.g1_dfe_res = 0x2, .g1_amp = 0x1c, .g1_emph = 0xe,
+		.g1_emph_en = 0x1, .g1_tx_amp_adj = 0x1, .g1_tx_emph_en = 0x1,
+		.g1_tx_emph = 0x0, .g1_rx_selmuff = 0x1, .g1_rx_selmufi = 0x0,
+		.g1_rx_selmupf = 0x2, .g1_rx_selmupi = 0x2, .valid = 1
+	}
+};
+
+static const struct sata_params
+	sata_static_values_tab[AP_NUM][CP_NUM][MAX_LANE_NR] = {
+	[0 ... AP_NUM-1][0 ... CP_NUM-1][0 ... MAX_LANE_NR-1] = {
+		.g1_amp = 0x8, .g2_amp = 0xa, .g3_amp = 0x1e,
+		.g1_emph = 0x1, .g2_emph = 0x2, .g3_emph = 0xe,
+		.g1_emph_en = 0x1, .g2_emph_en = 0x1, .g3_emph_en = 0x1,
+		.g1_tx_amp_adj = 0x1, .g2_tx_amp_adj = 0x1,
+		.g3_tx_amp_adj = 0x1,
+		.g1_tx_emph_en = 0x0, .g2_tx_emph_en = 0x0,
+		.g3_tx_emph_en = 0x0,
+		.g1_tx_emph = 0x1, .g2_tx_emph = 0x1, .g3_tx_emph = 0x1,
+		.g3_dfe_res = 0x1, .g3_ffe_res_sel = 0x4, .g3_ffe_cap_sel = 0xf,
+		.align90 = 0x61,
+		.g1_rx_selmuff = 0x3, .g2_rx_selmuff = 0x3,
+		.g3_rx_selmuff = 0x3,
+		.g1_rx_selmufi = 0x0, .g2_rx_selmufi = 0x0,
+		.g3_rx_selmufi = 0x3,
+		.g1_rx_selmupf = 0x1, .g2_rx_selmupf = 0x1,
+		.g3_rx_selmupf = 0x2,
+		.g1_rx_selmupi = 0x0, .g2_rx_selmupi = 0x0,
+		.g3_rx_selmupi = 0x2,
+		.polarity_invert = COMPHY_POLARITY_NO_INVERT,
+		.valid = 0x1
+	},
+};
+
+static const struct usb_params
+	usb_static_values_tab[AP_NUM][CP_NUM][MAX_LANE_NR] = {
+	[0 ... AP_NUM-1][0 ... CP_NUM-1][0 ... MAX_LANE_NR-1] = {
+		.polarity_invert = COMPHY_POLARITY_NO_INVERT
+	},
+};
+#endif /* PHY_DEFAULT_PORTING_LAYER_H */
diff --git a/drivers/marvell/ddr_phy_access.c b/drivers/marvell/ddr_phy_access.c
new file mode 100644
index 0000000..352d1ef
--- /dev/null
+++ b/drivers/marvell/ddr_phy_access.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2021 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include "ddr_phy_access.h"
+#include <lib/mmio.h>
+#include <drivers/marvell/ccu.h>
+#include <errno.h>
+
+#define DDR_PHY_END_ADDRESS	0x100000
+
+#ifdef DDR_PHY_DEBUG
+#define debug_printf(...) printf(__VA_ARGS__)
+#else
+#define debug_printf(...)
+#endif
+
+
+/*
+ * This routine writes 'data' to specified 'address' offset,
+ * with optional debug print support
+ */
+int snps_fw_write(uintptr_t offset, uint16_t data)
+{
+	debug_printf("In %s\n", __func__);
+
+	if (offset < DDR_PHY_END_ADDRESS) {
+		mmio_write_16(DDR_PHY_BASE_ADDR + (2 * offset), data);
+		return 0;
+	}
+	debug_printf("%s: illegal offset value: 0x%x\n", __func__, offset);
+	return -EINVAL;
+}
+
+int snps_fw_read(uintptr_t offset, uint16_t *read)
+{
+	debug_printf("In %s\n", __func__);
+
+	if (offset < DDR_PHY_END_ADDRESS) {
+		*read = mmio_read_16(DDR_PHY_BASE_ADDR + (2 * offset));
+		return 0;
+	}
+	debug_printf("%s: illegal offset value: 0x%x\n", __func__, offset);
+	return -EINVAL;
+}
+
+int mvebu_ddr_phy_write(uintptr_t offset, uint16_t data)
+{
+	return snps_fw_write(offset, data);
+}
+
+int mvebu_ddr_phy_read(uintptr_t offset, uint16_t *read)
+{
+	return snps_fw_read(offset, read);
+}
diff --git a/drivers/marvell/ddr_phy_access.h b/drivers/marvell/ddr_phy_access.h
new file mode 100644
index 0000000..5f9a668
--- /dev/null
+++ b/drivers/marvell/ddr_phy_access.h
@@ -0,0 +1,15 @@
+/*
+ * Copyright (C) 2021 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <plat_marvell.h>
+
+#define DEVICE_BASE		0xF0000000
+#define DDR_PHY_OFFSET		0x1000000
+#define DDR_PHY_BASE_ADDR	(DEVICE_BASE + DDR_PHY_OFFSET)
+
+int mvebu_ddr_phy_write(uintptr_t offset, uint16_t data);
+int mvebu_ddr_phy_read(uintptr_t offset, uint16_t *read);
diff --git a/drivers/marvell/gwin.c b/drivers/marvell/gwin.c
new file mode 100644
index 0000000..fa59cb0
--- /dev/null
+++ b/drivers/marvell/gwin.c
@@ -0,0 +1,231 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* GWIN unit device driver for Marvell AP810 SoC */
+
+#include <inttypes.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/marvell/gwin.h>
+#include <lib/mmio.h>
+
+#include <armada_common.h>
+#include <mvebu.h>
+#include <mvebu_def.h>
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+#define DEBUG_ADDR_MAP
+#endif
+
+/* common defines */
+#define WIN_ENABLE_BIT			(0x1)
+#define WIN_TARGET_MASK			(0xF)
+#define WIN_TARGET_SHIFT		(0x8)
+#define WIN_TARGET(tgt)			(((tgt) & WIN_TARGET_MASK) \
+					<< WIN_TARGET_SHIFT)
+
+/* Bits[43:26] of the physical address are the window base,
+ * which is aligned to 64MB
+ */
+#define ADDRESS_RSHIFT			(26)
+#define ADDRESS_LSHIFT			(10)
+#define GWIN_ALIGNMENT_64M		(0x4000000)
+
+/* AP registers */
+#define GWIN_CR_OFFSET(ap, win)		(MVEBU_GWIN_BASE(ap) + 0x0 + \
+						(0x10 * (win)))
+#define GWIN_ALR_OFFSET(ap, win)	(MVEBU_GWIN_BASE(ap) + 0x8 + \
+						(0x10 * (win)))
+#define GWIN_AHR_OFFSET(ap, win)	(MVEBU_GWIN_BASE(ap) + 0xc + \
+						(0x10 * (win)))
+
+#define CCU_GRU_CR_OFFSET(ap)		(MVEBU_CCU_GRU_BASE(ap))
+#define CCR_GRU_CR_GWIN_MBYPASS		(1 << 1)
+
+static void gwin_check(struct addr_map_win *win)
+{
+	/* The base is always 64M aligned */
+	if (IS_NOT_ALIGN(win->base_addr, GWIN_ALIGNMENT_64M)) {
+		win->base_addr &= ~(GWIN_ALIGNMENT_64M - 1);
+		NOTICE("%s: Align the base address to 0x%" PRIx64 "\n",
+		       __func__, win->base_addr);
+	}
+
+	/* size parameter validity check */
+	if (IS_NOT_ALIGN(win->win_size, GWIN_ALIGNMENT_64M)) {
+		win->win_size = ALIGN_UP(win->win_size, GWIN_ALIGNMENT_64M);
+		NOTICE("%s: Aligning window size to 0x%" PRIx64 "\n",
+		       __func__, win->win_size);
+	}
+}
+
+static void gwin_enable_window(int ap_index, struct addr_map_win *win,
+			       uint32_t win_num)
+{
+	uint32_t alr, ahr;
+	uint64_t end_addr;
+
+	if ((win->target_id & WIN_TARGET_MASK) != win->target_id) {
+		ERROR("target ID = %d, is invalid\n", win->target_id);
+		return;
+	}
+
+	/* calculate 64bit end-address */
+	end_addr = (win->base_addr + win->win_size - 1);
+
+	alr = (uint32_t)((win->base_addr >> ADDRESS_RSHIFT) << ADDRESS_LSHIFT);
+	ahr = (uint32_t)((end_addr >> ADDRESS_RSHIFT) << ADDRESS_LSHIFT);
+
+	/* write start address and end address for GWIN */
+	mmio_write_32(GWIN_ALR_OFFSET(ap_index, win_num), alr);
+	mmio_write_32(GWIN_AHR_OFFSET(ap_index, win_num), ahr);
+
+	/* write the target ID and enable the window */
+	mmio_write_32(GWIN_CR_OFFSET(ap_index, win_num),
+		      WIN_TARGET(win->target_id) | WIN_ENABLE_BIT);
+}
+
+static void gwin_disable_window(int ap_index, uint32_t win_num)
+{
+	uint32_t win_reg;
+
+	win_reg = mmio_read_32(GWIN_CR_OFFSET(ap_index, win_num));
+	win_reg &= ~WIN_ENABLE_BIT;
+	mmio_write_32(GWIN_CR_OFFSET(ap_index, win_num), win_reg);
+}
+
+/* Insert/Remove temporary window for using the out-of reset default
+ * CPx base address to access the CP configuration space prior to
+ * the further base address update in accordance with address mapping
+ * design.
+ *
+ * NOTE: Use the same window array for insertion and removal of
+ *       temporary windows.
+ */
+void gwin_temp_win_insert(int ap_index, struct addr_map_win *win, int size)
+{
+	uint32_t win_id;
+
+	for (int i = 0; i < size; i++) {
+		win_id = MVEBU_GWIN_MAX_WINS - i - 1;
+		gwin_check(win);
+		gwin_enable_window(ap_index, win, win_id);
+		win++;
+	}
+}
+
+/*
+ * NOTE: Use the same window array for insertion and removal of
+ *       temporary windows.
+ */
+void gwin_temp_win_remove(int ap_index, struct addr_map_win *win, int size)
+{
+	uint32_t win_id;
+
+	for (int i = 0; i < size; i++) {
+		uint64_t base;
+		uint32_t target;
+
+		win_id = MVEBU_GWIN_MAX_WINS - i - 1;
+
+		target = mmio_read_32(GWIN_CR_OFFSET(ap_index, win_id));
+		target >>= WIN_TARGET_SHIFT;
+		target &= WIN_TARGET_MASK;
+
+		base = mmio_read_32(GWIN_ALR_OFFSET(ap_index, win_id));
+		base >>= ADDRESS_LSHIFT;
+		base <<= ADDRESS_RSHIFT;
+
+		if (win->target_id != target) {
+			ERROR("%s: Trying to remove bad window-%d!\n",
+			      __func__, win_id);
+			continue;
+		}
+		gwin_disable_window(ap_index, win_id);
+		win++;
+	}
+}
+
+#ifdef DEBUG_ADDR_MAP
+static void dump_gwin(int ap_index)
+{
+	uint32_t win_num;
+
+	/* Dump all GWIN windows */
+	printf("\tbank  target     start              end\n");
+	printf("\t----------------------------------------------------\n");
+	for (win_num = 0; win_num < MVEBU_GWIN_MAX_WINS; win_num++) {
+		uint32_t cr;
+		uint64_t alr, ahr;
+
+		cr  = mmio_read_32(GWIN_CR_OFFSET(ap_index, win_num));
+		/* Window enabled */
+		if (cr & WIN_ENABLE_BIT) {
+			alr = mmio_read_32(GWIN_ALR_OFFSET(ap_index, win_num));
+			alr = (alr >> ADDRESS_LSHIFT) << ADDRESS_RSHIFT;
+			ahr = mmio_read_32(GWIN_AHR_OFFSET(ap_index, win_num));
+			ahr = (ahr >> ADDRESS_LSHIFT) << ADDRESS_RSHIFT;
+			printf("\tgwin   %d     0x%016" PRIx64 " 0x%016" PRIx64 "\n",
+			       (cr >> 8) & 0xF, alr, ahr);
+		}
+	}
+}
+#endif
+
+int init_gwin(int ap_index)
+{
+	struct addr_map_win *win;
+	uint32_t win_id;
+	uint32_t win_count;
+	uint32_t win_reg;
+
+	INFO("Initializing GWIN Address decoding\n");
+
+	/* Get the array of the windows and its size */
+	marvell_get_gwin_memory_map(ap_index, &win, &win_count);
+	if (win_count <= 0) {
+		INFO("no windows configurations found\n");
+		return 0;
+	}
+
+	if (win_count > MVEBU_GWIN_MAX_WINS) {
+		ERROR("number of windows is bigger than %d\n",
+		      MVEBU_GWIN_MAX_WINS);
+		return 0;
+	}
+
+	/* disable all windows */
+	for (win_id = 0; win_id < MVEBU_GWIN_MAX_WINS; win_id++)
+		gwin_disable_window(ap_index, win_id);
+
+	/* enable relevant windows */
+	for (win_id = 0; win_id < win_count; win_id++, win++) {
+		gwin_check(win);
+		gwin_enable_window(ap_index, win, win_id);
+	}
+
+	/* GWIN Miss feature has not verified, therefore any access towards
+	 * remote AP should be accompanied with proper configuration to
+	 * GWIN registers group and therefore the GWIN Miss feature
+	 * should be set into Bypass mode, need to make sure all GWIN regions
+	 * are defined correctly that will assure no GWIN miss occurrance
+	 * JIRA-AURORA2-1630
+	 */
+	INFO("Update GWIN miss bypass\n");
+	win_reg = mmio_read_32(CCU_GRU_CR_OFFSET(ap_index));
+	win_reg |= CCR_GRU_CR_GWIN_MBYPASS;
+	mmio_write_32(CCU_GRU_CR_OFFSET(ap_index), win_reg);
+
+#ifdef DEBUG_ADDR_MAP
+	dump_gwin(ap_index);
+#endif
+
+	INFO("Done GWIN Address decoding Initializing\n");
+
+	return 0;
+}
diff --git a/drivers/marvell/io_win.c b/drivers/marvell/io_win.c
new file mode 100644
index 0000000..124382a
--- /dev/null
+++ b/drivers/marvell/io_win.c
@@ -0,0 +1,271 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* IO Window unit device driver for Marvell AP807, AP807 and AP810 SoCs */
+
+#include <inttypes.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/marvell/io_win.h>
+#include <lib/mmio.h>
+
+#include <armada_common.h>
+#include <mvebu.h>
+#include <mvebu_def.h>
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+#define DEBUG_ADDR_MAP
+#endif
+
+/* common defines */
+#define WIN_ENABLE_BIT			(0x1)
+/* Physical address of the base of the window = {Addr[19:0],20`h0} */
+#define ADDRESS_SHIFT			(20 - 4)
+#define ADDRESS_MASK			(0xFFFFFFF0)
+#define IO_WIN_ALIGNMENT_1M		(0x100000)
+#define IO_WIN_ALIGNMENT_64K		(0x10000)
+
+/* AP registers */
+#define IO_WIN_ALR_OFFSET(ap, win)	(MVEBU_IO_WIN_BASE(ap) + 0x0 + \
+						(0x10 * win))
+#define IO_WIN_AHR_OFFSET(ap, win)	(MVEBU_IO_WIN_BASE(ap) + 0x8 + \
+						(0x10 * win))
+#define IO_WIN_CR_OFFSET(ap, win)	(MVEBU_IO_WIN_BASE(ap) + 0xC + \
+						(0x10 * win))
+
+/* For storage of CR, ALR, AHR abd GCR */
+static uint32_t io_win_regs_save[MVEBU_IO_WIN_MAX_WINS * 3 + 1];
+
+static void io_win_check(struct addr_map_win *win)
+{
+	/* for IO The base is always 1M aligned */
+	/* check if address is aligned to 1M */
+	if (IS_NOT_ALIGN(win->base_addr, IO_WIN_ALIGNMENT_1M)) {
+		win->base_addr = ALIGN_UP(win->base_addr, IO_WIN_ALIGNMENT_1M);
+		NOTICE("%s: Align up the base address to 0x%" PRIx64 "\n",
+		       __func__, win->base_addr);
+	}
+
+	/* size parameter validity check */
+	if (IS_NOT_ALIGN(win->win_size, IO_WIN_ALIGNMENT_1M)) {
+		win->win_size = ALIGN_UP(win->win_size, IO_WIN_ALIGNMENT_1M);
+		NOTICE("%s: Aligning size to 0x%" PRIx64 "\n",
+		       __func__, win->win_size);
+	}
+}
+
+static void io_win_enable_window(int ap_index, struct addr_map_win *win,
+				 uint32_t win_num)
+{
+	uint32_t alr, ahr;
+	uint64_t end_addr;
+
+	if (win->target_id < 0 || win->target_id >= MVEBU_IO_WIN_MAX_WINS) {
+		ERROR("target ID = %d, is invalid\n", win->target_id);
+		return;
+	}
+
+	if ((win_num == 0) || (win_num > MVEBU_IO_WIN_MAX_WINS)) {
+		ERROR("Enabling wrong IOW window %d!\n", win_num);
+		return;
+	}
+
+	/* calculate the end-address */
+	end_addr = (win->base_addr + win->win_size - 1);
+
+	alr = (uint32_t)((win->base_addr >> ADDRESS_SHIFT) & ADDRESS_MASK);
+	alr |= WIN_ENABLE_BIT;
+	ahr = (uint32_t)((end_addr >> ADDRESS_SHIFT) & ADDRESS_MASK);
+
+	/* write start address and end address for IO window */
+	mmio_write_32(IO_WIN_ALR_OFFSET(ap_index, win_num), alr);
+	mmio_write_32(IO_WIN_AHR_OFFSET(ap_index, win_num), ahr);
+
+	/* write window target */
+	mmio_write_32(IO_WIN_CR_OFFSET(ap_index, win_num), win->target_id);
+}
+
+static void io_win_disable_window(int ap_index, uint32_t win_num)
+{
+	uint32_t win_reg;
+
+	if ((win_num == 0) || (win_num > MVEBU_IO_WIN_MAX_WINS)) {
+		ERROR("Disabling wrong IOW window %d!\n", win_num);
+		return;
+	}
+
+	win_reg = mmio_read_32(IO_WIN_ALR_OFFSET(ap_index, win_num));
+	win_reg &= ~WIN_ENABLE_BIT;
+	mmio_write_32(IO_WIN_ALR_OFFSET(ap_index, win_num), win_reg);
+}
+
+/* Insert/Remove temporary window for using the out-of reset default
+ * CPx base address to access the CP configuration space prior to
+ * the further base address update in accordance with address mapping
+ * design.
+ *
+ * NOTE: Use the same window array for insertion and removal of
+ *       temporary windows.
+ */
+void iow_temp_win_insert(int ap_index, struct addr_map_win *win, int size)
+{
+	uint32_t win_id;
+
+	for (int i = 0; i < size; i++) {
+		win_id = MVEBU_IO_WIN_MAX_WINS - i - 1;
+		io_win_check(win);
+		io_win_enable_window(ap_index, win, win_id);
+		win++;
+	}
+}
+
+/*
+ * NOTE: Use the same window array for insertion and removal of
+ *       temporary windows.
+ */
+void iow_temp_win_remove(int ap_index, struct addr_map_win *win, int size)
+{
+	uint32_t win_id;
+
+	/* Start from the last window and do not touch Win0 */
+	for (int i = 0; i < size; i++) {
+		uint64_t base;
+		uint32_t target;
+
+		win_id = MVEBU_IO_WIN_MAX_WINS - i - 1;
+
+		target = mmio_read_32(IO_WIN_CR_OFFSET(ap_index, win_id));
+		base = mmio_read_32(IO_WIN_ALR_OFFSET(ap_index, win_id));
+		base &= ~WIN_ENABLE_BIT;
+		base <<= ADDRESS_SHIFT;
+
+		if ((win->target_id != target) || (win->base_addr != base)) {
+			ERROR("%s: Trying to remove bad window-%d!\n",
+			      __func__, win_id);
+			continue;
+		}
+		io_win_disable_window(ap_index, win_id);
+		win++;
+	}
+}
+
+#ifdef DEBUG_ADDR_MAP
+static void dump_io_win(int ap_index)
+{
+	uint32_t trgt_id, win_id;
+	uint32_t alr, ahr;
+	uint64_t start, end;
+
+	/* Dump all IO windows */
+	printf("\tbank  target     start              end\n");
+	printf("\t----------------------------------------------------\n");
+	for (win_id = 0; win_id < MVEBU_IO_WIN_MAX_WINS; win_id++) {
+		alr = mmio_read_32(IO_WIN_ALR_OFFSET(ap_index, win_id));
+		if (alr & WIN_ENABLE_BIT) {
+			alr &= ~WIN_ENABLE_BIT;
+			ahr = mmio_read_32(IO_WIN_AHR_OFFSET(ap_index, win_id));
+			trgt_id = mmio_read_32(IO_WIN_CR_OFFSET(ap_index,
+								win_id));
+			start = ((uint64_t)alr << ADDRESS_SHIFT);
+			end = (((uint64_t)ahr + 0x10) << ADDRESS_SHIFT);
+			printf("\tio-win %d     0x%016" PRIx64 " 0x%016" PRIx64 "\n",
+			       trgt_id, start, end);
+		}
+	}
+	printf("\tio-win gcr is %x\n",
+	       mmio_read_32(MVEBU_IO_WIN_BASE(ap_index) +
+	       MVEBU_IO_WIN_GCR_OFFSET));
+}
+#endif
+
+static void iow_save_win_range(int ap_id, int win_first, int win_last,
+			       uint32_t *buffer)
+{
+	int win_id, idx;
+
+	/* Save IOW */
+	for (idx = 0, win_id = win_first; win_id <= win_last; win_id++) {
+		buffer[idx++] = mmio_read_32(IO_WIN_CR_OFFSET(ap_id, win_id));
+		buffer[idx++] = mmio_read_32(IO_WIN_ALR_OFFSET(ap_id, win_id));
+		buffer[idx++] = mmio_read_32(IO_WIN_AHR_OFFSET(ap_id, win_id));
+	}
+	buffer[idx] = mmio_read_32(MVEBU_IO_WIN_BASE(ap_id) +
+				   MVEBU_IO_WIN_GCR_OFFSET);
+}
+
+static void iow_restore_win_range(int ap_id, int win_first, int win_last,
+				  uint32_t *buffer)
+{
+	int win_id, idx;
+
+	/* Restore IOW */
+	for (idx = 0, win_id = win_first; win_id <= win_last; win_id++) {
+		mmio_write_32(IO_WIN_CR_OFFSET(ap_id, win_id), buffer[idx++]);
+		mmio_write_32(IO_WIN_ALR_OFFSET(ap_id, win_id), buffer[idx++]);
+		mmio_write_32(IO_WIN_AHR_OFFSET(ap_id, win_id), buffer[idx++]);
+	}
+	mmio_write_32(MVEBU_IO_WIN_BASE(ap_id) + MVEBU_IO_WIN_GCR_OFFSET,
+		      buffer[idx++]);
+}
+
+void iow_save_win_all(int ap_id)
+{
+	iow_save_win_range(ap_id, 0, MVEBU_IO_WIN_MAX_WINS - 1,
+			   io_win_regs_save);
+}
+
+void iow_restore_win_all(int ap_id)
+{
+	iow_restore_win_range(ap_id, 0, MVEBU_IO_WIN_MAX_WINS - 1,
+			      io_win_regs_save);
+}
+
+int init_io_win(int ap_index)
+{
+	struct addr_map_win *win;
+	uint32_t win_id, win_reg;
+	uint32_t win_count;
+
+	INFO("Initializing IO WIN Address decoding\n");
+
+	/* Get the array of the windows and its size */
+	marvell_get_io_win_memory_map(ap_index, &win, &win_count);
+	if (win_count <= 0)
+		INFO("no windows configurations found\n");
+
+	if (win_count > MVEBU_IO_WIN_MAX_WINS) {
+		INFO("number of windows is bigger than %d\n",
+		     MVEBU_IO_WIN_MAX_WINS);
+		return 0;
+	}
+
+	/* Get the default target id to set the GCR */
+	win_reg = marvell_get_io_win_gcr_target(ap_index);
+	mmio_write_32(MVEBU_IO_WIN_BASE(ap_index) + MVEBU_IO_WIN_GCR_OFFSET,
+		      win_reg);
+
+	/* disable all IO windows */
+	for (win_id = 1; win_id < MVEBU_IO_WIN_MAX_WINS; win_id++)
+		io_win_disable_window(ap_index, win_id);
+
+	/* enable relevant windows, starting from win_id = 1 because
+	 * index 0 dedicated for BootROM
+	 */
+	for (win_id = 1; win_id <= win_count; win_id++, win++) {
+		io_win_check(win);
+		io_win_enable_window(ap_index, win, win_id);
+	}
+
+#ifdef DEBUG_ADDR_MAP
+	dump_io_win(ap_index);
+#endif
+
+	INFO("Done IO WIN Address decoding Initializing\n");
+
+	return 0;
+}
diff --git a/drivers/marvell/iob.c b/drivers/marvell/iob.c
new file mode 100644
index 0000000..1f39395
--- /dev/null
+++ b/drivers/marvell/iob.c
@@ -0,0 +1,214 @@
+/*
+ * Copyright (C) 2016 - 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* IOW unit device driver for Marvell CP110 and CP115 SoCs */
+
+#include <inttypes.h>
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/marvell/iob.h>
+#include <lib/mmio.h>
+
+#include <armada_common.h>
+#include <mvebu.h>
+#include <mvebu_def.h>
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+#define DEBUG_ADDR_MAP
+#endif
+
+#define MVEBU_IOB_OFFSET		(0x190000)
+#define MVEBU_IOB_MAX_WINS		16
+
+/* common defines */
+#define WIN_ENABLE_BIT			(0x1)
+/* Physical address of the base of the window = {AddrLow[19:0],20`h0} */
+#define ADDRESS_SHIFT			(20 - 4)
+#define ADDRESS_MASK			(0xFFFFFFF0)
+#define IOB_WIN_ALIGNMENT		(0x100000)
+
+/* IOB registers */
+#define IOB_WIN_CR_OFFSET(win)		(iob_base + 0x0 + (0x20 * win))
+#define IOB_TARGET_ID_OFFSET		(8)
+#define IOB_TARGET_ID_MASK		(0xF)
+
+#define IOB_WIN_SCR_OFFSET(win)		(iob_base + 0x4 + (0x20 * win))
+#define IOB_WIN_ENA_CTRL_WRITE_SECURE	(0x1)
+#define IOB_WIN_ENA_CTRL_READ_SECURE	(0x2)
+#define IOB_WIN_ENA_WRITE_SECURE	(0x4)
+#define IOB_WIN_ENA_READ_SECURE		(0x8)
+
+#define IOB_WIN_ALR_OFFSET(win)		(iob_base + 0x8 + (0x20 * win))
+#define IOB_WIN_AHR_OFFSET(win)		(iob_base + 0xC + (0x20 * win))
+
+#define IOB_WIN_DIOB_CR_OFFSET(win)	(iob_base + 0x10 + (0x20 * win))
+#define IOB_WIN_XOR0_DIOB_EN		BIT(0)
+#define IOB_WIN_XOR1_DIOB_EN		BIT(1)
+
+uintptr_t iob_base;
+
+static void iob_win_check(struct addr_map_win *win, uint32_t win_num)
+{
+	/* check if address is aligned to the size */
+	if (IS_NOT_ALIGN(win->base_addr, IOB_WIN_ALIGNMENT)) {
+		win->base_addr = ALIGN_UP(win->base_addr, IOB_WIN_ALIGNMENT);
+		ERROR("Window %d: base address unaligned to 0x%x\n",
+		      win_num, IOB_WIN_ALIGNMENT);
+		printf("Align up the base address to 0x%" PRIx64 "\n",
+		       win->base_addr);
+	}
+
+	/* size parameter validity check */
+	if (IS_NOT_ALIGN(win->win_size, IOB_WIN_ALIGNMENT)) {
+		win->win_size = ALIGN_UP(win->win_size, IOB_WIN_ALIGNMENT);
+		ERROR("Window %d: window size unaligned to 0x%x\n", win_num,
+		      IOB_WIN_ALIGNMENT);
+		printf("Aligning size to 0x%" PRIx64 "\n", win->win_size);
+	}
+}
+
+static void iob_enable_win(struct addr_map_win *win, uint32_t win_id)
+{
+	uint32_t iob_win_reg;
+	uint32_t alr, ahr;
+	uint64_t end_addr;
+	uint32_t reg_en;
+
+	/* move XOR (DMA) to use WIN1 which is used for PCI-EP address space */
+	reg_en = IOB_WIN_XOR0_DIOB_EN | IOB_WIN_XOR1_DIOB_EN;
+	iob_win_reg = mmio_read_32(IOB_WIN_DIOB_CR_OFFSET(0));
+	iob_win_reg &= ~reg_en;
+	mmio_write_32(IOB_WIN_DIOB_CR_OFFSET(0), iob_win_reg);
+
+	iob_win_reg = mmio_read_32(IOB_WIN_DIOB_CR_OFFSET(1));
+	iob_win_reg |= reg_en;
+	mmio_write_32(IOB_WIN_DIOB_CR_OFFSET(1), iob_win_reg);
+
+	end_addr = (win->base_addr + win->win_size - 1);
+	alr = (uint32_t)((win->base_addr >> ADDRESS_SHIFT) & ADDRESS_MASK);
+	ahr = (uint32_t)((end_addr >> ADDRESS_SHIFT) & ADDRESS_MASK);
+
+	mmio_write_32(IOB_WIN_ALR_OFFSET(win_id), alr);
+	mmio_write_32(IOB_WIN_AHR_OFFSET(win_id), ahr);
+
+	iob_win_reg = WIN_ENABLE_BIT;
+	iob_win_reg |= (win->target_id & IOB_TARGET_ID_MASK)
+		       << IOB_TARGET_ID_OFFSET;
+	mmio_write_32(IOB_WIN_CR_OFFSET(win_id), iob_win_reg);
+
+}
+
+#ifdef DEBUG_ADDR_MAP
+static void dump_iob(void)
+{
+	uint32_t win_id, win_cr, alr, ahr;
+	uint8_t target_id;
+	uint64_t start, end;
+	char *iob_target_name[IOB_MAX_TID] = {
+		"CFG  ", "MCI0 ", "PEX1 ", "PEX2 ",
+		"PEX0 ", "NAND ", "RUNIT", "MCI1 " };
+
+	/* Dump all IOB windows */
+	printf("bank  id target  start              end\n");
+	printf("----------------------------------------------------\n");
+	for (win_id = 0; win_id < MVEBU_IOB_MAX_WINS; win_id++) {
+		win_cr = mmio_read_32(IOB_WIN_CR_OFFSET(win_id));
+		if (win_cr & WIN_ENABLE_BIT) {
+			target_id = (win_cr >> IOB_TARGET_ID_OFFSET) &
+				     IOB_TARGET_ID_MASK;
+			alr = mmio_read_32(IOB_WIN_ALR_OFFSET(win_id));
+			start = ((uint64_t)alr << ADDRESS_SHIFT);
+			if (win_id != 0) {
+				ahr = mmio_read_32(IOB_WIN_AHR_OFFSET(win_id));
+				end = (((uint64_t)ahr + 0x10) << ADDRESS_SHIFT);
+			} else {
+				/* Window #0 size is hardcoded to 16MB, as it's
+				 * reserved for CP configuration space.
+				 */
+				end = start + (16 << 20);
+			}
+			printf("iob   %02d %s   0x%016" PRIx64 " 0x%016" PRIx64 "\n",
+			       win_id, iob_target_name[target_id],
+			       start, end);
+		}
+	}
+}
+#endif
+
+void iob_cfg_space_update(int ap_idx, int cp_idx, uintptr_t base,
+			  uintptr_t new_base)
+{
+	debug_enter();
+
+	iob_base = base + MVEBU_IOB_OFFSET;
+
+	NOTICE("Change the base address of AP%d-CP%d to %lx\n",
+	       ap_idx, cp_idx, new_base);
+	mmio_write_32(IOB_WIN_ALR_OFFSET(0), new_base >> ADDRESS_SHIFT);
+
+	iob_base = new_base + MVEBU_IOB_OFFSET;
+
+	/* Make sure the address was configured by the CPU before
+	 * any possible access to the CP.
+	 */
+	dsb();
+
+	debug_exit();
+}
+
+int init_iob(uintptr_t base)
+{
+	struct addr_map_win *win;
+	uint32_t win_id, win_reg;
+	uint32_t win_count;
+
+	INFO("Initializing IOB Address decoding\n");
+
+	/* Get the base address of the address decoding MBUS */
+	iob_base = base + MVEBU_IOB_OFFSET;
+
+	/* Get the array of the windows and fill the map data */
+	marvell_get_iob_memory_map(&win, &win_count, base);
+	if (win_count <= 0) {
+		INFO("no windows configurations found\n");
+		return 0;
+	} else if (win_count > (MVEBU_IOB_MAX_WINS - 1)) {
+		ERROR("IOB mem map array > than max available windows (%d)\n",
+		      MVEBU_IOB_MAX_WINS);
+		win_count = MVEBU_IOB_MAX_WINS;
+	}
+
+	/* disable all IOB windows, start from win_id = 1
+	 * because can't disable internal register window
+	 */
+	for (win_id = 1; win_id < MVEBU_IOB_MAX_WINS; win_id++) {
+		win_reg = mmio_read_32(IOB_WIN_CR_OFFSET(win_id));
+		win_reg &= ~WIN_ENABLE_BIT;
+		mmio_write_32(IOB_WIN_CR_OFFSET(win_id), win_reg);
+
+		win_reg = ~IOB_WIN_ENA_CTRL_WRITE_SECURE;
+		win_reg &= ~IOB_WIN_ENA_CTRL_READ_SECURE;
+		win_reg &= ~IOB_WIN_ENA_WRITE_SECURE;
+		win_reg &= ~IOB_WIN_ENA_READ_SECURE;
+		mmio_write_32(IOB_WIN_SCR_OFFSET(win_id), win_reg);
+	}
+
+	for (win_id = 1; win_id < win_count + 1; win_id++, win++) {
+		iob_win_check(win, win_id);
+		iob_enable_win(win, win_id);
+	}
+
+#ifdef DEBUG_ADDR_MAP
+	dump_iob();
+#endif
+
+	INFO("Done IOB Address decoding Initializing\n");
+
+	return 0;
+}
diff --git a/drivers/marvell/mc_trustzone/mc_trustzone.c b/drivers/marvell/mc_trustzone/mc_trustzone.c
new file mode 100644
index 0000000..648bd0e
--- /dev/null
+++ b/drivers/marvell/mc_trustzone/mc_trustzone.c
@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <inttypes.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/marvell/addr_map.h>
+#include <lib/mmio.h>
+
+#include <mvebu_def.h>
+
+#include "mc_trustzone.h"
+
+#define TZ_SIZE(x)		((x) >> 13)
+
+static int fls(int x)
+{
+	if (!x)
+		return 0;
+
+	return 32 - __builtin_clz(x);
+}
+
+/* To not duplicate types, the addr_map_win is used, but the "target"
+ * filed is referring to attributes instead of "target".
+ */
+void tz_enable_win(int ap_index, const struct addr_map_win *win, int win_id)
+{
+	int tz_size;
+	uint32_t val, base = win->base_addr;
+
+	if ((win_id < 0) || (win_id > MVEBU_TZ_MAX_WINS)) {
+		ERROR("Enabling wrong MC TrustZone window %d!\n", win_id);
+		return;
+	}
+
+	/* map the window size to trustzone register convention */
+	tz_size = fls(TZ_SIZE(win->win_size));
+
+	VERBOSE("%s: window size = 0x%" PRIx64 " maps to tz_size %d\n",
+		__func__, win->win_size, tz_size);
+	if (tz_size < 0 || tz_size > 31) {
+		ERROR("Using not allowed size for MC TrustZone window %d!\n",
+		      win_id);
+		return;
+	}
+
+	if (base & 0xfff) {
+		base = base & ~0xfff;
+		WARN("Attempt to open MC TZ win. at 0x%" PRIx64 ", truncate to 0x%x\n",
+		     win->base_addr, base);
+	}
+
+	val = base | (tz_size << 7) | win->target_id | TZ_VALID;
+
+	VERBOSE("%s: base 0x%x, tz_size moved 0x%x, attr 0x%x, val 0x%x\n",
+		__func__, base, (tz_size << 7), win->target_id, val);
+
+	mmio_write_32(MVEBU_AP_MC_TRUSTZONE_REG_LOW(ap_index, win_id), val);
+
+	VERBOSE("%s: Win%d[0x%x] configured to 0x%x\n", __func__, win_id,
+		MVEBU_AP_MC_TRUSTZONE_REG_LOW(ap_index, win_id),
+		mmio_read_32(MVEBU_AP_MC_TRUSTZONE_REG_LOW(ap_index, win_id)));
+
+	mmio_write_32(MVEBU_AP_MC_TRUSTZONE_REG_HIGH(ap_index, win_id),
+		     (win->base_addr >> 32));
+
+	VERBOSE("%s: Win%d[0x%x] configured to 0x%x\n", __func__, win_id,
+		MVEBU_AP_MC_TRUSTZONE_REG_HIGH(ap_index, win_id),
+		mmio_read_32(MVEBU_AP_MC_TRUSTZONE_REG_HIGH(ap_index, win_id)));
+}
diff --git a/drivers/marvell/mc_trustzone/mc_trustzone.h b/drivers/marvell/mc_trustzone/mc_trustzone.h
new file mode 100644
index 0000000..296dce8
--- /dev/null
+++ b/drivers/marvell/mc_trustzone/mc_trustzone.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef MC_TRUSTZONE_H
+#define MC_TRUSTZONE_H
+
+#include <drivers/marvell/addr_map.h>
+
+#define MVEBU_TZ_MAX_WINS	16
+
+#define TZ_VALID		(1 << 0)
+#define TZ_PERM(x)		((x) << 1)
+#define TZ_RZ_ENABLE		(1 << 3)
+
+/* tz attr definitions */
+#define TZ_PERM_RW		(TZ_PERM(0))
+#define TZ_PERM_RO		(TZ_PERM(1))
+#define TZ_PERM_WO		(TZ_PERM(2))
+#define TZ_PERM_ABORT		(TZ_PERM(3))
+
+void tz_enable_win(int ap_index, const struct addr_map_win *win, int win_id);
+
+#endif /* MC_TRUSTZONE_H */
diff --git a/drivers/marvell/mci.c b/drivers/marvell/mci.c
new file mode 100644
index 0000000..2b54700
--- /dev/null
+++ b/drivers/marvell/mci.c
@@ -0,0 +1,834 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* MCI bus driver for Marvell ARMADA 8K and 8K+ SoCs */
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/marvell/mci.h>
+#include <lib/mmio.h>
+
+#include <mvebu.h>
+#include <mvebu_def.h>
+#include <plat_marvell.h>
+
+/* /HB /Units /Direct_regs /Direct regs
+ * /Configuration Register Write/Read Data Register
+ */
+#define MCI_WRITE_READ_DATA_REG(mci_index)	\
+					MVEBU_MCI_REG_BASE_REMAP(mci_index)
+/* /HB /Units /Direct_regs /Direct regs
+ * /Configuration Register Access Command Register
+ */
+#define MCI_ACCESS_CMD_REG(mci_index)		\
+				(MVEBU_MCI_REG_BASE_REMAP(mci_index) + 0x4)
+
+/* Access Command fields :
+ * bit[3:0]   - Sub command: 1 => Peripheral Config Register Read,
+ *			     0 => Peripheral Config Register Write,
+ *			     2 => Peripheral Assign ID request,
+ *			     3 => Circular Config Write
+ * bit[5]     - 1 => Local (same chip access) 0 => Remote
+ * bit[15:8]  - Destination hop ID. Put Global ID (GID) here (see scheme below).
+ * bit[23:22] - 0x3 IHB PHY REG address space, 0x0 IHB Controller space
+ * bit[21:16] - Low 6 bits of offset. Hight 2 bits are taken from bit[28:27]
+ *		of IHB_PHY_CTRL
+ *		(must be set before any PHY register access occurs):
+ *		/IHB_REG /IHB_REGInterchip Hopping Bus Registers
+ *		/IHB Version Control Register
+ *
+ *		ixi_ihb_top		IHB PHY
+ *  AXI -----------------------------   -------------
+ *   <--| axi_hb_top | ihb_pipe_top |-->|           |
+ *   -->|   GID=1    |     GID=0    |<--|           |
+ *      -----------------------------   -------------
+ */
+#define MCI_INDIRECT_CTRL_READ_CMD		0x1
+#define MCI_INDIRECT_CTRL_ASSIGN_CMD		0x2
+#define MCI_INDIRECT_CTRL_CIRCULAR_CMD		0x3
+#define MCI_INDIRECT_CTRL_LOCAL_PKT		(1 << 5)
+#define MCI_INDIRECT_CTRL_CMD_DONE_OFFSET	6
+#define MCI_INDIRECT_CTRL_CMD_DONE		\
+				(1 << MCI_INDIRECT_CTRL_CMD_DONE_OFFSET)
+#define MCI_INDIRECT_CTRL_DATA_READY_OFFSET	7
+#define MCI_INDIRECT_CTRL_DATA_READY		\
+				(1 << MCI_INDIRECT_CTRL_DATA_READY_OFFSET)
+#define MCI_INDIRECT_CTRL_HOPID_OFFSET		8
+#define MCI_INDIRECT_CTRL_HOPID(id)		\
+			(((id) & 0xFF) << MCI_INDIRECT_CTRL_HOPID_OFFSET)
+#define MCI_INDIRECT_CTRL_REG_CHIPID_OFFSET	16
+#define MCI_INDIRECT_REG_CTRL_ADDR(reg_num)	\
+			(reg_num << MCI_INDIRECT_CTRL_REG_CHIPID_OFFSET)
+
+/* Hop ID values */
+#define GID_IHB_PIPE					0
+#define GID_AXI_HB					1
+#define GID_IHB_EXT					2
+
+#define MCI_DID_GLOBAL_ASSIGNMENT_REQUEST_REG		0x2
+/* Target MCi Local ID (LID, which is = self DID) */
+#define MCI_DID_GLOBAL_ASSIGN_REQ_MCI_LOCAL_ID(val)	(((val) & 0xFF) << 16)
+/* Bits [15:8]: Number of MCis on chip of target MCi */
+#define MCI_DID_GLOBAL_ASSIGN_REQ_MCI_COUNT(val)	(((val) & 0xFF) << 8)
+/* Bits [7:0]: Number of hops on chip of target MCi */
+#define MCI_DID_GLOBAL_ASSIGN_REQ_HOPS_NUM(val)		(((val) & 0xFF) << 0)
+
+/* IHB_REG domain registers */
+/* /HB /Units /IHB_REG /IHB_REGInterchip Hopping Bus Registers/
+ * Rx Memory Configuration Register (RX_MEM_CFG)
+ */
+#define MCI_CTRL_RX_MEM_CFG_REG_NUM			0x0
+#define MCI_CTRL_RX_TX_MEM_CFG_RQ_THRESH(val)		(((val) & 0xFF) << 24)
+#define MCI_CTRL_RX_TX_MEM_CFG_PQ_THRESH(val)		(((val) & 0xFF) << 16)
+#define MCI_CTRL_RX_TX_MEM_CFG_NQ_THRESH(val)		(((val) & 0xFF) << 8)
+#define MCI_CTRL_RX_TX_MEM_CFG_DELTA_THRESH(val)	(((val) & 0xF) << 4)
+#define MCI_CTRL_RX_TX_MEM_CFG_RTC(val)			(((val) & 0x3) << 2)
+#define MCI_CTRL_RX_TX_MEM_CFG_WTC(val)			(((val) & 0x3) << 0)
+#define MCI_CTRL_RX_MEM_CFG_REG_DEF_CP_VAL		\
+				(MCI_CTRL_RX_TX_MEM_CFG_RQ_THRESH(0x07) | \
+				MCI_CTRL_RX_TX_MEM_CFG_PQ_THRESH(0x3f) | \
+				MCI_CTRL_RX_TX_MEM_CFG_NQ_THRESH(0x3f) | \
+				MCI_CTRL_RX_TX_MEM_CFG_DELTA_THRESH(0xf) | \
+				MCI_CTRL_RX_TX_MEM_CFG_RTC(1) | \
+				MCI_CTRL_RX_TX_MEM_CFG_WTC(1))
+
+#define MCI_CTRL_RX_MEM_CFG_REG_DEF_AP_VAL		\
+				(MCI_CTRL_RX_TX_MEM_CFG_RQ_THRESH(0x3f) | \
+				MCI_CTRL_RX_TX_MEM_CFG_PQ_THRESH(0x03) | \
+				MCI_CTRL_RX_TX_MEM_CFG_NQ_THRESH(0x3f) | \
+				MCI_CTRL_RX_TX_MEM_CFG_DELTA_THRESH(0xf) | \
+				MCI_CTRL_RX_TX_MEM_CFG_RTC(1) | \
+				MCI_CTRL_RX_TX_MEM_CFG_WTC(1))
+
+
+/* /HB /Units /IHB_REG /IHB_REGInterchip Hopping Bus Registers/
+ * Tx Memory Configuration Register (TX_MEM_CFG)
+ */
+#define MCI_CTRL_TX_MEM_CFG_REG_NUM			0x1
+/* field mapping for TX mem config register
+ * are the same as for RX register - see register above
+ */
+#define MCI_CTRL_TX_MEM_CFG_REG_DEF_VAL			\
+				(MCI_CTRL_RX_TX_MEM_CFG_RQ_THRESH(0x20) | \
+				MCI_CTRL_RX_TX_MEM_CFG_PQ_THRESH(0x20) | \
+				MCI_CTRL_RX_TX_MEM_CFG_NQ_THRESH(0x20) | \
+				MCI_CTRL_RX_TX_MEM_CFG_DELTA_THRESH(2) | \
+				MCI_CTRL_RX_TX_MEM_CFG_RTC(1) | \
+				MCI_CTRL_RX_TX_MEM_CFG_WTC(1))
+
+/* /HB /Units /IHB_REG /IHB_REGInterchip Hopping Bus Registers
+ * /IHB Link CRC Control
+ */
+/* MCi Link CRC Control Register (MCi_CRC_CTRL) */
+#define MCI_LINK_CRC_CTRL_REG_NUM			0x4
+
+/* /HB /Units /IHB_REG /IHB_REGInterchip Hopping Bus Registers
+ * /IHB Status Register
+ */
+/* MCi Status Register (MCi_STS) */
+#define MCI_CTRL_STATUS_REG_NUM				0x5
+#define MCI_CTRL_STATUS_REG_PHY_READY			(1 << 12)
+#define MCI_CTRL_STATUS_REG_LINK_PRESENT		(1 << 15)
+#define MCI_CTRL_STATUS_REG_PHY_CID_VIO_OFFSET		24
+#define MCI_CTRL_STATUS_REG_PHY_CID_VIO_MASK		\
+				(0xF << MCI_CTRL_STATUS_REG_PHY_CID_VIO_OFFSET)
+/* Expected successful Link result, including reserved bit */
+#define MCI_CTRL_PHY_READY		(MCI_CTRL_STATUS_REG_PHY_READY | \
+					MCI_CTRL_STATUS_REG_LINK_PRESENT | \
+					MCI_CTRL_STATUS_REG_PHY_CID_VIO_MASK)
+
+/* /HB /Units /IHB_REG /IHB_REGInterchip Hopping Bus Registers/
+ * MCi PHY Speed Settings Register (MCi_PHY_SETTING)
+ */
+#define MCI_CTRL_MCI_PHY_SETTINGS_REG_NUM		0x8
+#define MCI_CTRL_MCI_PHY_SET_DLO_FIFO_FULL_TRESH(val)	(((val) & 0xF) << 28)
+#define MCI_CTRL_MCI_PHY_SET_PHY_MAX_SPEED(val)		(((val) & 0xF) << 12)
+#define MCI_CTRL_MCI_PHY_SET_PHYCLK_SEL(val)		(((val) & 0xF) << 8)
+#define MCI_CTRL_MCI_PHY_SET_REFCLK_FREQ_SEL(val)	(((val) & 0xF) << 4)
+#define MCI_CTRL_MCI_PHY_SET_AUTO_LINK_EN(val)		(((val) & 0x1) << 1)
+#define MCI_CTRL_MCI_PHY_SET_REG_DEF_VAL		\
+			(MCI_CTRL_MCI_PHY_SET_DLO_FIFO_FULL_TRESH(0x3) | \
+			MCI_CTRL_MCI_PHY_SET_PHY_MAX_SPEED(0x3) | \
+			MCI_CTRL_MCI_PHY_SET_PHYCLK_SEL(0x2) | \
+			MCI_CTRL_MCI_PHY_SET_REFCLK_FREQ_SEL(0x1))
+#define MCI_CTRL_MCI_PHY_SET_REG_DEF_VAL2		\
+			(MCI_CTRL_MCI_PHY_SET_DLO_FIFO_FULL_TRESH(0x3) | \
+			MCI_CTRL_MCI_PHY_SET_PHY_MAX_SPEED(0x3) | \
+			MCI_CTRL_MCI_PHY_SET_PHYCLK_SEL(0x5) | \
+			MCI_CTRL_MCI_PHY_SET_REFCLK_FREQ_SEL(0x1))
+
+/* /HB /Units /IHB_REG /IHB_REGInterchip Hopping Bus Registers
+ * /IHB Mode Config
+ */
+#define MCI_CTRL_IHB_MODE_CFG_REG_NUM			0x25
+#define MCI_CTRL_IHB_MODE_HBCLK_DIV(val)		((val) & 0xFF)
+#define MCI_CTRL_IHB_MODE_CHUNK_MOD_OFFSET		8
+#define MCI_CTRL_IHB_MODE_CHUNK_MOD			\
+				(1 << MCI_CTRL_IHB_MODE_CHUNK_MOD_OFFSET)
+#define MCI_CTRL_IHB_MODE_FWD_MOD_OFFSET		9
+#define MCI_CTRL_IHB_MODE_FWD_MOD			\
+				(1 << MCI_CTRL_IHB_MODE_FWD_MOD_OFFSET)
+#define MCI_CTRL_IHB_MODE_SEQFF_FINE_MOD(val)		(((val) & 0xF) << 12)
+#define MCI_CTRL_IHB_MODE_RX_COMB_THRESH(val)		(((val) & 0xFF) << 16)
+#define MCI_CTRL_IHB_MODE_TX_COMB_THRESH(val)		(((val) & 0xFF) << 24)
+
+#define MCI_CTRL_IHB_MODE_CFG_REG_DEF_VAL		\
+				(MCI_CTRL_IHB_MODE_HBCLK_DIV(6) | \
+				MCI_CTRL_IHB_MODE_FWD_MOD | \
+				MCI_CTRL_IHB_MODE_SEQFF_FINE_MOD(0xF) | \
+				MCI_CTRL_IHB_MODE_RX_COMB_THRESH(0x3f) | \
+				MCI_CTRL_IHB_MODE_TX_COMB_THRESH(0x40))
+/* AXI_HB registers */
+#define MCI_AXI_ACCESS_DATA_REG_NUM			0x0
+#define MCI_AXI_ACCESS_PCIE_MODE			1
+#define MCI_AXI_ACCESS_CACHE_CHECK_OFFSET		5
+#define MCI_AXI_ACCESS_CACHE_CHECK			\
+				(1 << MCI_AXI_ACCESS_CACHE_CHECK_OFFSET)
+#define MCI_AXI_ACCESS_FORCE_POST_WR_OFFSET		6
+#define MCI_AXI_ACCESS_FORCE_POST_WR			\
+				(1 << MCI_AXI_ACCESS_FORCE_POST_WR_OFFSET)
+#define MCI_AXI_ACCESS_DISABLE_CLK_GATING_OFFSET	9
+#define MCI_AXI_ACCESS_DISABLE_CLK_GATING		\
+				(1 << MCI_AXI_ACCESS_DISABLE_CLK_GATING_OFFSET)
+
+/* /HB /Units /HB_REG /HB_REGHopping Bus Registers
+ * /Window 0 Address Mask Register
+ */
+#define MCI_HB_CTRL_WIN0_ADDRESS_MASK_REG_NUM		0x2
+
+/* /HB /Units /HB_REG /HB_REGHopping Bus Registers
+ * /Window 0 Destination Register
+ */
+#define MCI_HB_CTRL_WIN0_DESTINATION_REG_NUM		0x3
+#define MCI_HB_CTRL_WIN0_DEST_VALID_FLAG(val)		(((val) & 0x1) << 16)
+#define MCI_HB_CTRL_WIN0_DEST_ID(val)			(((val) & 0xFF) << 0)
+
+/* /HB /Units /HB_REG /HB_REGHopping Bus Registers /Tx Control Register */
+#define MCI_HB_CTRL_TX_CTRL_REG_NUM			0xD
+#define MCI_HB_CTRL_TX_CTRL_PCIE_MODE_OFFSET		24
+#define MCI_HB_CTRL_TX_CTRL_PCIE_MODE			\
+				(1 << MCI_HB_CTRL_TX_CTRL_PCIE_MODE_OFFSET)
+#define MCI_HB_CTRL_TX_CTRL_PRI_TH_QOS(val)		(((val) & 0xF) << 12)
+#define MCI_HB_CTRL_TX_CTRL_MAX_RD_CNT(val)		(((val) & 0x1F) << 6)
+#define MCI_HB_CTRL_TX_CTRL_MAX_WR_CNT(val)		(((val) & 0x1F) << 0)
+
+/* /HB /Units /IHB_REG /IHB_REGInterchip Hopping Bus Registers
+ * /IHB Version Control Register
+ */
+#define MCI_PHY_CTRL_REG_NUM				0x7
+#define MCI_PHY_CTRL_MCI_MINOR				0x8 /* BITS [3:0] */
+#define MCI_PHY_CTRL_MCI_MAJOR_OFFSET			4
+#define MCI_PHY_CTRL_MCI_MAJOR				\
+				(1 << MCI_PHY_CTRL_MCI_MAJOR_OFFSET)
+#define MCI_PHY_CTRL_MCI_SLEEP_REQ_OFFSET		11
+#define MCI_PHY_CTRL_MCI_SLEEP_REQ			\
+				(1 << MCI_PHY_CTRL_MCI_SLEEP_REQ_OFFSET)
+/* Host=1 / Device=0 PHY mode */
+#define MCI_PHY_CTRL_MCI_PHY_MODE_OFFSET		24
+#define MCI_PHY_CTRL_MCI_PHY_MODE_HOST			\
+				(1 << MCI_PHY_CTRL_MCI_PHY_MODE_OFFSET)
+/* Register=1 / PWM=0 interface */
+#define MCI_PHY_CTRL_MCI_PHY_REG_IF_MODE_OFFSET		25
+#define MCI_PHY_CTRL_MCI_PHY_REG_IF_MODE		\
+				(1 << MCI_PHY_CTRL_MCI_PHY_REG_IF_MODE_OFFSET)
+ /* PHY code InReset=1 */
+#define MCI_PHY_CTRL_MCI_PHY_RESET_CORE_OFFSET		26
+#define MCI_PHY_CTRL_MCI_PHY_RESET_CORE			\
+				(1 << MCI_PHY_CTRL_MCI_PHY_RESET_CORE_OFFSET)
+#define MCI_PHY_CTRL_PHY_ADDR_MSB_OFFSET		27
+#define MCI_PHY_CTRL_PHY_ADDR_MSB(addr)			\
+				(((addr) & 0x3) << \
+				MCI_PHY_CTRL_PHY_ADDR_MSB_OFFSET)
+#define MCI_PHY_CTRL_PIDI_MODE_OFFSET			31
+#define MCI_PHY_CTRL_PIDI_MODE				\
+				(1U << MCI_PHY_CTRL_PIDI_MODE_OFFSET)
+
+/* Number of times to wait for the MCI link ready after MCI configurations
+ * Normally takes 34-35 successive reads
+ */
+#define LINK_READY_TIMEOUT				100
+
+enum mci_register_type {
+	MCI_REG_TYPE_PHY = 0,
+	MCI_REG_TYPE_CTRL,
+};
+
+enum {
+	MCI_CMD_WRITE,
+	MCI_CMD_READ
+};
+
+/* Write wrapper callback for debug:
+ * will print written data in case LOG_LEVEL >= 40
+ */
+static void mci_mmio_write_32(uintptr_t addr, uint32_t value)
+{
+	VERBOSE("Write:\t0x%x = 0x%x\n", (uint32_t)addr, value);
+	mmio_write_32(addr, value);
+}
+/* Read wrapper callback for debug:
+ * will print read data in case LOG_LEVEL >= 40
+ */
+static uint32_t mci_mmio_read_32(uintptr_t addr)
+{
+	uint32_t value;
+
+	value = mmio_read_32(addr);
+	VERBOSE("Read:\t0x%x = 0x%x\n", (uint32_t)addr, value);
+	return value;
+}
+
+/* MCI indirect access command completion polling:
+ * Each write/read command done via MCI indirect registers must be polled
+ * for command completions status.
+ *
+ * Returns 1 in case of error
+ * Returns 0 in case of command completed successfully.
+ */
+static int mci_poll_command_completion(int mci_index, int command_type)
+{
+	uint32_t mci_cmd_value = 0, retry_count = 100, ret = 0;
+	uint32_t completion_flags = MCI_INDIRECT_CTRL_CMD_DONE;
+
+	debug_enter();
+	/* Read commands require validating that requested data is ready */
+	if (command_type == MCI_CMD_READ)
+		completion_flags |= MCI_INDIRECT_CTRL_DATA_READY;
+
+	do {
+		/* wait 1 ms before each polling */
+		mdelay(1);
+		mci_cmd_value = mci_mmio_read_32(MCI_ACCESS_CMD_REG(mci_index));
+	} while (((mci_cmd_value & completion_flags) != completion_flags) &&
+			 (retry_count-- > 0));
+
+	if (retry_count == 0) {
+		ERROR("%s: MCI command timeout (command status = 0x%x)\n",
+		      __func__, mci_cmd_value);
+		ret = 1;
+	}
+
+	debug_exit();
+	return ret;
+}
+
+int mci_read(int mci_idx, uint32_t cmd, uint32_t *value)
+{
+	int rval;
+
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_idx), cmd);
+
+	rval = mci_poll_command_completion(mci_idx, MCI_CMD_READ);
+
+	*value = mci_mmio_read_32(MCI_WRITE_READ_DATA_REG(mci_idx));
+
+	return rval;
+}
+
+int  mci_write(int mci_idx, uint32_t cmd, uint32_t data)
+{
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_idx), data);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_idx), cmd);
+
+	return mci_poll_command_completion(mci_idx, MCI_CMD_WRITE);
+}
+
+/* Perform 3 configurations in one command: PCI mode,
+ * queues separation and cache bit
+ */
+static int mci_axi_set_pcie_mode(int mci_index)
+{
+	uint32_t reg_data, ret = 1;
+
+	debug_enter();
+	/* This configuration makes MCI IP behave consistently with AXI protocol
+	 * It should be configured at one side only (for example locally at AP).
+	 * The IP takes care of performing the same configurations at MCI on
+	 * another side (for example remotely at CP).
+	 */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_AXI_ACCESS_PCIE_MODE |
+			  MCI_AXI_ACCESS_CACHE_CHECK |
+			  MCI_AXI_ACCESS_FORCE_POST_WR |
+			  MCI_AXI_ACCESS_DISABLE_CLK_GATING);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_AXI_ACCESS_DATA_REG_NUM)  |
+			  MCI_INDIRECT_CTRL_HOPID(GID_AXI_HB) |
+			  MCI_INDIRECT_CTRL_LOCAL_PKT |
+			  MCI_INDIRECT_CTRL_CIRCULAR_CMD);
+
+	/* if Write command was successful, verify PCIe mode */
+	if (mci_poll_command_completion(mci_index, MCI_CMD_WRITE) == 0) {
+		/* Verify the PCIe mode selected */
+		mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+				  MCI_INDIRECT_REG_CTRL_ADDR(
+					MCI_HB_CTRL_TX_CTRL_REG_NUM)  |
+				  MCI_INDIRECT_CTRL_HOPID(GID_AXI_HB) |
+				  MCI_INDIRECT_CTRL_LOCAL_PKT |
+				  MCI_INDIRECT_CTRL_READ_CMD);
+		/* if read was completed, verify PCIe mode */
+		if (mci_poll_command_completion(mci_index, MCI_CMD_READ) == 0) {
+			reg_data = mci_mmio_read_32(
+					MCI_WRITE_READ_DATA_REG(mci_index));
+			if (reg_data & MCI_HB_CTRL_TX_CTRL_PCIE_MODE)
+				ret = 0;
+		}
+	}
+
+	debug_exit();
+	return ret;
+}
+
+/* Reduce sequence FIFO timer expiration threshold */
+static int mci_axi_set_fifo_thresh(int mci_index)
+{
+	uint32_t reg_data, ret = 0;
+
+	debug_enter();
+	/* This configuration reduces sequence FIFO timer expiration threshold
+	 * (to 0x7 instead of 0xA).
+	 * In MCI 1.6 version this configuration prevents possible functional
+	 * issues.
+	 * In version 1.82 the configuration prevents performance degradation
+	 */
+
+	/* Configure local AP side */
+	reg_data = MCI_PHY_CTRL_PIDI_MODE |
+		   MCI_PHY_CTRL_MCI_PHY_REG_IF_MODE |
+		   MCI_PHY_CTRL_MCI_PHY_MODE_HOST |
+		   MCI_PHY_CTRL_MCI_MAJOR |
+		   MCI_PHY_CTRL_MCI_MINOR;
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index), reg_data);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(MCI_PHY_CTRL_REG_NUM) |
+			  MCI_INDIRECT_CTRL_LOCAL_PKT);
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* Reduce the threshold */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_CTRL_IHB_MODE_CFG_REG_DEF_VAL);
+
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_CTRL_IHB_MODE_CFG_REG_NUM) |
+			  MCI_INDIRECT_CTRL_LOCAL_PKT);
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* Exit PIDI mode */
+	reg_data = MCI_PHY_CTRL_MCI_PHY_REG_IF_MODE |
+		   MCI_PHY_CTRL_MCI_PHY_MODE_HOST |
+		   MCI_PHY_CTRL_MCI_MAJOR |
+		   MCI_PHY_CTRL_MCI_MINOR;
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index), reg_data);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(MCI_PHY_CTRL_REG_NUM) |
+			  MCI_INDIRECT_CTRL_LOCAL_PKT);
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* Configure remote CP side */
+	reg_data = MCI_PHY_CTRL_PIDI_MODE |
+		   MCI_PHY_CTRL_MCI_MAJOR |
+		   MCI_PHY_CTRL_MCI_MINOR |
+		   MCI_PHY_CTRL_MCI_PHY_REG_IF_MODE;
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index), reg_data);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(MCI_PHY_CTRL_REG_NUM) |
+			  MCI_CTRL_IHB_MODE_FWD_MOD);
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* Reduce the threshold */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_CTRL_IHB_MODE_CFG_REG_DEF_VAL);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_CTRL_IHB_MODE_CFG_REG_NUM) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_IHB_EXT));
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* Exit PIDI mode */
+	reg_data = MCI_PHY_CTRL_MCI_MAJOR |
+		   MCI_PHY_CTRL_MCI_MINOR |
+		   MCI_PHY_CTRL_MCI_PHY_REG_IF_MODE;
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index), reg_data);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(MCI_PHY_CTRL_REG_NUM) |
+			  MCI_CTRL_IHB_MODE_FWD_MOD);
+
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	debug_exit();
+	return ret;
+}
+
+/* Configure:
+ * 1. AP & CP TX thresholds and delta configurations
+ * 2. DLO & DLI FIFO full threshold
+ * 3. RX thresholds and delta configurations
+ * 4. CP AR and AW outstanding
+ * 5. AP AR and AW outstanding
+ */
+static int mci_axi_set_fifo_rx_tx_thresh(int mci_index)
+{
+	uint32_t ret = 0;
+
+	debug_enter();
+	/* AP TX thresholds and delta configurations (IHB_reg 0x1) */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_CTRL_TX_MEM_CFG_REG_DEF_VAL);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_CTRL_TX_MEM_CFG_REG_NUM) |
+			  MCI_INDIRECT_CTRL_LOCAL_PKT);
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* CP TX thresholds and delta configurations (IHB_reg 0x1) */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_CTRL_TX_MEM_CFG_REG_DEF_VAL);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_CTRL_TX_MEM_CFG_REG_NUM) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_IHB_EXT));
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* AP DLO & DLI FIFO full threshold & Auto-Link enable (IHB_reg 0x8) */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_CTRL_MCI_PHY_SET_REG_DEF_VAL |
+			  MCI_CTRL_MCI_PHY_SET_AUTO_LINK_EN(1));
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_CTRL_MCI_PHY_SETTINGS_REG_NUM) |
+			  MCI_INDIRECT_CTRL_LOCAL_PKT);
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* CP DLO & DLI FIFO full threshold (IHB_reg 0x8) */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_CTRL_MCI_PHY_SET_REG_DEF_VAL);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_CTRL_MCI_PHY_SETTINGS_REG_NUM) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_IHB_EXT));
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* AP RX thresholds and delta configurations (IHB_reg 0x0) */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_CTRL_RX_MEM_CFG_REG_DEF_AP_VAL);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_CTRL_RX_MEM_CFG_REG_NUM) |
+			  MCI_INDIRECT_CTRL_LOCAL_PKT);
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* CP RX thresholds and delta configurations (IHB_reg 0x0) */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_CTRL_RX_MEM_CFG_REG_DEF_CP_VAL);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_CTRL_RX_MEM_CFG_REG_NUM) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_IHB_EXT));
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* AP AR & AW maximum AXI outstanding request cfg (HB_reg 0xd) */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_HB_CTRL_TX_CTRL_PRI_TH_QOS(8) |
+			  MCI_HB_CTRL_TX_CTRL_MAX_RD_CNT(3) |
+			  MCI_HB_CTRL_TX_CTRL_MAX_WR_CNT(3));
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_HB_CTRL_TX_CTRL_REG_NUM) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_AXI_HB) |
+			  MCI_INDIRECT_CTRL_LOCAL_PKT);
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* CP AR & AW maximum AXI outstanding request cfg (HB_reg 0xd) */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_HB_CTRL_TX_CTRL_PRI_TH_QOS(8) |
+			  MCI_HB_CTRL_TX_CTRL_MAX_RD_CNT(0xB) |
+			  MCI_HB_CTRL_TX_CTRL_MAX_WR_CNT(0x11));
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_HB_CTRL_TX_CTRL_REG_NUM) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_IHB_EXT) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_AXI_HB));
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	debug_exit();
+	return ret;
+}
+
+/* configure MCI to allow read & write transactions to arrive at the same time.
+ * Without the below configuration, MCI won't sent response to CPU for
+ * transactions which arrived simultaneously and will lead to CPU hang.
+ * The below will configure MCI to be able to pass transactions from/to CP/AP.
+ */
+static int mci_enable_simultaneous_transactions(int mci_index)
+{
+	uint32_t ret = 0;
+
+	debug_enter();
+	/* ID assignment (assigning global ID offset to CP) */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_DID_GLOBAL_ASSIGN_REQ_MCI_LOCAL_ID(2) |
+			  MCI_DID_GLOBAL_ASSIGN_REQ_MCI_COUNT(2) |
+			  MCI_DID_GLOBAL_ASSIGN_REQ_HOPS_NUM(2));
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_DID_GLOBAL_ASSIGNMENT_REQUEST_REG) |
+			  MCI_INDIRECT_CTRL_ASSIGN_CMD);
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* Assigning dest. ID=3 to all transactions entering from AXI at AP */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_HB_CTRL_WIN0_DEST_VALID_FLAG(1) |
+			  MCI_HB_CTRL_WIN0_DEST_ID(3));
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_HB_CTRL_WIN0_DESTINATION_REG_NUM) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_AXI_HB) |
+			  MCI_INDIRECT_CTRL_LOCAL_PKT);
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* Assigning dest. ID=1 to all transactions entering from AXI at CP */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
+			  MCI_HB_CTRL_WIN0_DEST_VALID_FLAG(1) |
+			  MCI_HB_CTRL_WIN0_DEST_ID(1));
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_HB_CTRL_WIN0_DESTINATION_REG_NUM) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_IHB_EXT) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_AXI_HB));
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* End address to all transactions entering from AXI at AP.
+	 * This will lead to get match for any AXI address
+	 * and receive destination ID=3
+	 */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index), 0xffffffff);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_HB_CTRL_WIN0_ADDRESS_MASK_REG_NUM) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_AXI_HB) |
+			  MCI_INDIRECT_CTRL_LOCAL_PKT);
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	/* End address to all transactions entering from AXI at CP.
+	 * This will lead to get match for any AXI address
+	 * and receive destination ID=1
+	 */
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index), 0xffffffff);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_HB_CTRL_WIN0_ADDRESS_MASK_REG_NUM) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_IHB_EXT) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_AXI_HB));
+	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
+
+	debug_exit();
+	return ret;
+}
+
+/* Check if MCI simultaneous transaction was already enabled.
+ * Currently bootrom does this mci configuration only when the boot source is
+ * SAR_MCIX4, in other cases it should be done at this stage.
+ * It is worth noticing that in case of booting from uart, the bootrom
+ * flow is different and this mci initialization is skipped even if boot
+ * source is SAR_MCIX4. Therefore new verification bases on appropriate mci's
+ * register content: if the appropriate reg contains 0x0 it means that the
+ * bootrom didn't perform required mci configuration.
+ *
+ * Returns:
+ * 0 - configuration already done
+ * 1 - configuration missing
+ */
+static _Bool mci_simulatenous_trans_missing(int mci_index)
+{
+	uint32_t reg, ret;
+
+	/* read 'Window 0 Destination ID assignment' from HB register 0x3
+	 * (TX_CFG_W0_DST_ID) to check whether ID assignment was already
+	 * performed by BootROM.
+	 */
+	debug_enter();
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
+			  MCI_INDIRECT_REG_CTRL_ADDR(
+				MCI_HB_CTRL_WIN0_DESTINATION_REG_NUM) |
+			  MCI_INDIRECT_CTRL_HOPID(GID_AXI_HB) |
+			  MCI_INDIRECT_CTRL_LOCAL_PKT |
+			  MCI_INDIRECT_CTRL_READ_CMD);
+	ret = mci_poll_command_completion(mci_index, MCI_CMD_READ);
+
+	reg = mci_mmio_read_32(MCI_WRITE_READ_DATA_REG(mci_index));
+
+	if (ret)
+		ERROR("Failed to verify MCI simultaneous read/write status\n");
+
+	debug_exit();
+	/* default ID assignment is 0, so if register doesn't contain zeros
+	 * it means that bootrom already performed required configuration.
+	 */
+	if (reg != 0)
+		return 0;
+
+	return 1;
+}
+
+/* For A1 revision, configure the MCI link for performance improvement:
+ * - set MCI to support read/write transactions to arrive at the same time
+ * - Switch AXI to PCIe mode
+ * - Reduce sequence FIFO threshold
+ * - Configure RX/TX FIFO thresholds
+ *
+ *   Note:
+ *   We don't exit on error code from any sub routine, to try (best effort) to
+ *   complete the MCI configuration.
+ *   (If we exit - Bootloader will surely fail to boot)
+ */
+int mci_configure(int mci_index)
+{
+	int rval;
+
+	debug_enter();
+	/* According to design guidelines the MCI simultaneous transaction
+	 * shouldn't be enabled more then once - therefore make sure that it
+	 * wasn't already enabled in bootrom.
+	 */
+	if (mci_simulatenous_trans_missing(mci_index)) {
+		VERBOSE("Enabling MCI simultaneous transaction for mci%d\n",
+		       mci_index);
+		/* set MCI to support read/write transactions
+		 * to arrive at the same time
+		 */
+		rval = mci_enable_simultaneous_transactions(mci_index);
+		if (rval)
+			ERROR("Failed to set MCI simultaneous read/write\n");
+	} else
+		VERBOSE("Skip MCI ID assignment - already done by bootrom\n");
+
+	/* Configure MCI for more consistent behavior with AXI protocol */
+	rval = mci_axi_set_pcie_mode(mci_index);
+	if (rval)
+		ERROR("Failed to set MCI to AXI PCIe mode\n");
+
+	/* reduce FIFO global threshold */
+	rval = mci_axi_set_fifo_thresh(mci_index);
+	if (rval)
+		ERROR("Failed to set MCI FIFO global threshold\n");
+
+	/* configure RX/TX FIFO thresholds */
+	rval = mci_axi_set_fifo_rx_tx_thresh(mci_index);
+	if (rval)
+		ERROR("Failed to set MCI RX/TX FIFO threshold\n");
+
+	debug_exit();
+	return 1;
+}
+
+int mci_get_link_status(void)
+{
+	uint32_t cmd, data;
+
+	cmd = (MCI_INDIRECT_REG_CTRL_ADDR(MCI_CTRL_STATUS_REG_NUM) |
+		MCI_INDIRECT_CTRL_LOCAL_PKT | MCI_INDIRECT_CTRL_READ_CMD);
+	if (mci_read(0, cmd, &data)) {
+		ERROR("Failed to read status register\n");
+		return -1;
+	}
+
+	/* Check if the link is ready */
+	if (data != MCI_CTRL_PHY_READY) {
+		ERROR("Bad link status %x\n", data);
+		return -1;
+	}
+
+	return 0;
+}
+
+void mci_turn_link_down(void)
+{
+	uint32_t cmd, data;
+	int rval = 0;
+
+	debug_enter();
+
+	/* Turn off auto-link */
+	cmd = (MCI_INDIRECT_REG_CTRL_ADDR(MCI_CTRL_MCI_PHY_SETTINGS_REG_NUM) |
+			MCI_INDIRECT_CTRL_LOCAL_PKT);
+	data = (MCI_CTRL_MCI_PHY_SET_REG_DEF_VAL2 |
+		MCI_CTRL_MCI_PHY_SET_AUTO_LINK_EN(0));
+	rval = mci_write(0, cmd, data);
+	if (rval)
+		ERROR("Failed to turn off auto-link\n");
+
+	/* Reset AP PHY */
+	cmd = (MCI_INDIRECT_REG_CTRL_ADDR(MCI_PHY_CTRL_REG_NUM) |
+		MCI_INDIRECT_CTRL_LOCAL_PKT);
+	data = (MCI_PHY_CTRL_MCI_MINOR |
+		MCI_PHY_CTRL_MCI_MAJOR |
+		MCI_PHY_CTRL_MCI_PHY_MODE_HOST |
+		MCI_PHY_CTRL_MCI_PHY_RESET_CORE);
+	rval = mci_write(0, cmd, data);
+	if (rval)
+		ERROR("Failed to reset AP PHY\n");
+
+	/* Clear all status & CRC values */
+	cmd = (MCI_INDIRECT_REG_CTRL_ADDR(MCI_LINK_CRC_CTRL_REG_NUM) |
+	       MCI_INDIRECT_CTRL_LOCAL_PKT);
+	data = 0x0;
+	mci_write(0, cmd, data);
+	cmd = (MCI_INDIRECT_REG_CTRL_ADDR(MCI_CTRL_STATUS_REG_NUM) |
+	       MCI_INDIRECT_CTRL_LOCAL_PKT);
+	data = 0x0;
+	rval = mci_write(0, cmd, data);
+	if (rval)
+		ERROR("Failed to reset AP PHY\n");
+
+	/* Wait 5ms before un-reset the PHY */
+	mdelay(5);
+
+	/* Un-reset AP PHY */
+	cmd = (MCI_INDIRECT_REG_CTRL_ADDR(MCI_PHY_CTRL_REG_NUM) |
+	       MCI_INDIRECT_CTRL_LOCAL_PKT);
+	data = (MCI_PHY_CTRL_MCI_MINOR | MCI_PHY_CTRL_MCI_MAJOR |
+		MCI_PHY_CTRL_MCI_PHY_MODE_HOST);
+	rval = mci_write(0, cmd, data);
+	if (rval)
+		ERROR("Failed to un-reset AP PHY\n");
+
+	debug_exit();
+}
+
+void mci_turn_link_on(void)
+{
+	uint32_t cmd, data;
+	int rval = 0;
+
+	debug_enter();
+	/* Turn on auto-link */
+	cmd = (MCI_INDIRECT_REG_CTRL_ADDR(MCI_CTRL_MCI_PHY_SETTINGS_REG_NUM) |
+			MCI_INDIRECT_CTRL_LOCAL_PKT);
+	data = (MCI_CTRL_MCI_PHY_SET_REG_DEF_VAL2 |
+		MCI_CTRL_MCI_PHY_SET_AUTO_LINK_EN(1));
+	rval = mci_write(0, cmd, data);
+	if (rval)
+		ERROR("Failed to turn on auto-link\n");
+
+	debug_exit();
+}
+
+/* Initialize MCI for performance improvements */
+int mci_link_tune(int mci_index)
+{
+	int ret;
+
+	debug_enter();
+	INFO("MCI%d initialization:\n", mci_index);
+
+	ret = mci_configure(mci_index);
+
+	debug_exit();
+	return ret;
+}
diff --git a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
new file mode 100644
index 0000000..98e1896
--- /dev/null
+++ b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2019 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <a8k_plat_def.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <mss_scp_bl2_format.h>
+
+/* CONFI REGISTERS */
+#define MG_CM3_CONFI_BASE(CP)		(MVEBU_CP_REGS_BASE(CP) + 0x100000)
+#define MG_CM3_SRAM_BASE(CP)		MG_CM3_CONFI_BASE(CP)
+#define MG_CM3_CONFI_GLOB_CFG_REG(CP)	(MG_CM3_CONFI_BASE(CP) + 0x2B500)
+#define CM3_CPU_EN_BIT			BIT(28)
+#define MG_CM3_MG_INT_MFX_REG(CP)	(MG_CM3_CONFI_BASE(CP) + 0x2B054)
+#define CM3_SYS_RESET_BIT		BIT(0)
+
+#define MG_CM3_SHARED_MEM_BASE(CP)	(MG_CM3_SRAM_BASE(CP) + 0x1FC00ULL)
+
+#define MG_SRAM_SIZE	0x20000 /* 128KB */
+
+#define MG_ACK_TIMEOUT 10
+
+/**
+ * struct ap_sharedmem_ctrl - used to pass information between the HOST and CM3
+ * @init_done:	Set by CM3 when ap_proces initialzied. Host check if CM3 set
+ *		this flag to confirm that the process is running
+ * @lane_nr:	Set by Host to mark which comphy lane should be configure. E.g.:
+ *		- A8K development board uses comphy lane 2 for eth0
+ *		- CN913x development board uses comphy lane 4 for eth0
+ */
+struct ap_sharedmem_ctrl {
+	uint32_t init_done;
+	uint32_t lane_nr;
+};
+
+int mg_image_load(uintptr_t src_addr, uint32_t size, int cp_index)
+{
+	uintptr_t mg_regs = MG_CM3_SRAM_BASE(cp_index);
+
+	if (size > MG_SRAM_SIZE) {
+		ERROR("image is too big to fit into MG CM3 memory\n");
+		return 1;
+	}
+
+	NOTICE("Loading MG image from address 0x%lx Size 0x%x to MG at 0x%lx\n",
+	       src_addr, size, mg_regs);
+
+	/* Copy image to MG CM3 SRAM */
+	memcpy((void *)mg_regs, (void *)src_addr, size);
+
+	/* Don't release MG CM3 from reset - it will be done by next step
+	 * bootloader (e.g. U-Boot), when appriopriate device-tree setup (which
+	 * has enabeld 802.3. auto-neg) will be choosen.
+	 */
+
+	return 0;
+}
+
+void mg_start_ap_fw(int cp_nr, uint8_t comphy_index)
+{
+	volatile struct ap_sharedmem_ctrl *ap_shared_ctrl =
+					(void *)MG_CM3_SHARED_MEM_BASE(cp_nr);
+	int timeout = MG_ACK_TIMEOUT;
+
+	if (mmio_read_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr)) & CM3_CPU_EN_BIT) {
+		VERBOSE("cm3 already running\n");
+		return;  /* cm3 already running */
+	}
+
+	/*
+	 * Mark which comphy lane should be used - it will be read via shared
+	 * mem by ap process
+	 */
+	ap_shared_ctrl->lane_nr = comphy_index;
+	/* Make sure it took place before enabling cm3 */
+	dmbst();
+
+	mmio_setbits_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr), CM3_CPU_EN_BIT);
+	mmio_setbits_32(MG_CM3_MG_INT_MFX_REG(cp_nr), CM3_SYS_RESET_BIT);
+
+	/* Check for ap process initialization by fw */
+	while (ap_shared_ctrl->init_done != 1 && timeout--)
+		VERBOSE("Waiting for ap process ack, timeout %d\n", timeout);
+
+	if (timeout == 0) {
+		ERROR("AP process failed, disabling cm3\n");
+		mmio_clrbits_32(MG_CM3_MG_INT_MFX_REG(cp_nr),
+				CM3_SYS_RESET_BIT);
+		mmio_clrbits_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr),
+				CM3_CPU_EN_BIT);
+	}
+}
diff --git a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
new file mode 100644
index 0000000..e2756de
--- /dev/null
+++ b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
@@ -0,0 +1,9 @@
+/*
+ * Copyright (C) 2019 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+void mg_start_ap_fw(int cp_nr, uint8_t comphy_index);
+int mg_image_load(uintptr_t src_addr, uint32_t size, int cp_index);
diff --git a/drivers/marvell/mochi/ap807_setup.c b/drivers/marvell/mochi/ap807_setup.c
new file mode 100644
index 0000000..75e9654
--- /dev/null
+++ b/drivers/marvell/mochi/ap807_setup.c
@@ -0,0 +1,339 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* AP807 Marvell SoC driver */
+
+#include <common/debug.h>
+#include <drivers/marvell/cache_llc.h>
+#include <drivers/marvell/ccu.h>
+#include <drivers/marvell/io_win.h>
+#include <drivers/marvell/iob.h>
+#include <drivers/marvell/mci.h>
+#include <drivers/marvell/mochi/ap_setup.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+
+#include <a8k_plat_def.h>
+
+#define SMMU_sACR				(MVEBU_SMMU_BASE + 0x10)
+#define SMMU_sACR_PG_64K			(1 << 16)
+
+#define CCU_GSPMU_CR				(MVEBU_CCU_BASE(MVEBU_AP0) \
+								+ 0x3F0)
+#define GSPMU_CPU_CONTROL			(0x1 << 0)
+
+#define CCU_HTC_CR				(MVEBU_CCU_BASE(MVEBU_AP0) \
+								+ 0x200)
+#define CCU_SET_POC_OFFSET			5
+
+#define DSS_CR0					(MVEBU_RFU_BASE + 0x100)
+#define DVM_48BIT_VA_ENABLE			(1 << 21)
+
+
+/* SoC RFU / IHBx4 Control */
+#define MCIX4_807_REG_START_ADDR_REG(unit_id)	(MVEBU_RFU_BASE + \
+						0x4258 + (unit_id * 0x4))
+
+/* Secure MoChi incoming access */
+#define SEC_MOCHI_IN_ACC_REG			(MVEBU_RFU_BASE + 0x4738)
+#define SEC_MOCHI_IN_ACC_IHB0_EN		(1)
+#define SEC_MOCHI_IN_ACC_IHB1_EN		(1 << 3)
+#define SEC_MOCHI_IN_ACC_IHB2_EN		(1 << 6)
+#define SEC_MOCHI_IN_ACC_PIDI_EN		(1 << 9)
+#define SEC_IN_ACCESS_ENA_ALL_MASTERS		(SEC_MOCHI_IN_ACC_IHB0_EN | \
+						 SEC_MOCHI_IN_ACC_IHB1_EN | \
+						 SEC_MOCHI_IN_ACC_IHB2_EN | \
+						 SEC_MOCHI_IN_ACC_PIDI_EN)
+#define MOCHI_IN_ACC_LEVEL_FORCE_NONSEC		(0)
+#define MOCHI_IN_ACC_LEVEL_FORCE_SEC		(1)
+#define MOCHI_IN_ACC_LEVEL_LEAVE_ORIG		(2)
+#define MOCHI_IN_ACC_LEVEL_MASK_ALL		(3)
+#define SEC_MOCHI_IN_ACC_IHB0_LEVEL(l)		((l) << 1)
+#define SEC_MOCHI_IN_ACC_IHB1_LEVEL(l)		((l) << 4)
+#define SEC_MOCHI_IN_ACC_PIDI_LEVEL(l)		((l) << 10)
+
+
+/* SYSRST_OUTn Config definitions */
+#define MVEBU_SYSRST_OUT_CONFIG_REG		(MVEBU_MISC_SOC_BASE + 0x4)
+#define WD_MASK_SYS_RST_OUT			(1 << 2)
+
+/* DSS PHY for DRAM */
+#define DSS_SCR_REG				(MVEBU_RFU_BASE + 0x208)
+#define DSS_PPROT_OFFS				4
+#define DSS_PPROT_MASK				0x7
+#define DSS_PPROT_PRIV_SECURE_DATA		0x1
+
+/* Used for Units of AP-807 (e.g. SDIO and etc) */
+#define MVEBU_AXI_ATTR_BASE			(MVEBU_REGS_BASE + 0x6F4580)
+#define MVEBU_AXI_ATTR_REG(index)		(MVEBU_AXI_ATTR_BASE + \
+							0x4 * index)
+
+#define XOR_STREAM_ID_REG(ch)	(MVEBU_REGS_BASE + 0x410010 + (ch) * 0x20000)
+#define XOR_STREAM_ID_MASK	0xFFFF
+#define SDIO_STREAM_ID_REG	(MVEBU_RFU_BASE + 0x4600)
+#define SDIO_STREAM_ID_MASK	0xFF
+
+/* Do not use the default Stream ID 0 */
+#define A807_STREAM_ID_BASE	(0x1)
+
+static uintptr_t stream_id_reg[] = {
+	XOR_STREAM_ID_REG(0),
+	XOR_STREAM_ID_REG(1),
+	XOR_STREAM_ID_REG(2),
+	XOR_STREAM_ID_REG(3),
+	SDIO_STREAM_ID_REG,
+	0
+};
+
+enum axi_attr {
+	AXI_SDIO_ATTR = 0,
+	AXI_DFX_ATTR,
+	AXI_MAX_ATTR,
+};
+
+static void ap_sec_masters_access_en(uint32_t enable)
+{
+	/* Open/Close incoming access for all masters.
+	 * The access is disabled in trusted boot mode
+	 * Could only be done in EL3
+	 */
+	if (enable != 0) {
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG, 0x0U, /* no clear */
+				   SEC_IN_ACCESS_ENA_ALL_MASTERS);
+#if LLC_SRAM
+		/* Do not change access security level
+		 * for PIDI masters
+		 */
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG,
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_MASK_ALL),
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_LEAVE_ORIG));
+#endif
+	} else {
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG,
+				   SEC_IN_ACCESS_ENA_ALL_MASTERS,
+				   0x0U /* no set */);
+#if LLC_SRAM
+		/* Return PIDI access level to the default */
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG,
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_MASK_ALL),
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_FORCE_NONSEC));
+#endif
+	}
+}
+
+static void setup_smmu(void)
+{
+	uint32_t reg;
+
+	/* Set the SMMU page size to 64 KB */
+	reg = mmio_read_32(SMMU_sACR);
+	reg |= SMMU_sACR_PG_64K;
+	mmio_write_32(SMMU_sACR, reg);
+}
+
+static void init_aurora2(void)
+{
+	uint32_t reg;
+
+	/* Enable GSPMU control by CPU */
+	reg = mmio_read_32(CCU_GSPMU_CR);
+	reg |= GSPMU_CPU_CONTROL;
+	mmio_write_32(CCU_GSPMU_CR, reg);
+
+#if LLC_ENABLE
+	/* Enable LLC for AP807 in exclusive mode */
+	llc_enable(0, 1);
+
+	/* Set point of coherency to DDR.
+	 * This is required by units which have
+	 * SW cache coherency
+	 */
+	reg = mmio_read_32(CCU_HTC_CR);
+	reg |= (0x1 << CCU_SET_POC_OFFSET);
+	mmio_write_32(CCU_HTC_CR, reg);
+#endif /* LLC_ENABLE */
+
+	errata_wa_init();
+}
+
+
+/* MCIx indirect access register are based by default at 0xf4000000/0xf6000000
+ * to avoid conflict of internal registers of units connected via MCIx, which
+ * can be based on the same address (i.e CP1 base is also 0xf4000000),
+ * the following routines remaps the MCIx indirect bases to another domain
+ */
+static void mci_remap_indirect_access_base(void)
+{
+	uint32_t mci;
+
+	for (mci = 0; mci < MCI_MAX_UNIT_ID; mci++)
+		mmio_write_32(MCIX4_807_REG_START_ADDR_REG(mci),
+				  MVEBU_MCI_REG_BASE_REMAP(mci) >>
+				  MCI_REMAP_OFF_SHIFT);
+}
+
+/* Set a unique stream id for all DMA capable devices */
+static void ap807_stream_id_init(void)
+{
+	uint32_t i;
+
+	for (i = 0;
+	     stream_id_reg[i] != 0 && i < ARRAY_SIZE(stream_id_reg); i++) {
+		uint32_t mask = stream_id_reg[i] == SDIO_STREAM_ID_REG ?
+				SDIO_STREAM_ID_MASK : XOR_STREAM_ID_MASK;
+
+		mmio_clrsetbits_32(stream_id_reg[i], mask,
+				   i + A807_STREAM_ID_BASE);
+	}
+}
+
+static void ap807_axi_attr_init(void)
+{
+	uint32_t index, data;
+
+	/* Initialize AXI attributes for AP807 */
+	/* Go over the AXI attributes and set Ax-Cache and Ax-Domain */
+	for (index = 0; index < AXI_MAX_ATTR; index++) {
+		switch (index) {
+		/* DFX works with no coherent only -
+		 * there's no option to configure the Ax-Cache and Ax-Domain
+		 */
+		case AXI_DFX_ATTR:
+			continue;
+		default:
+			/* Set Ax-Cache as cacheable, no allocate, modifiable,
+			 * bufferable.
+			 * The values are different because Read & Write
+			 * definition is different in Ax-Cache
+			 */
+			data = mmio_read_32(MVEBU_AXI_ATTR_REG(index));
+			data &= ~MVEBU_AXI_ATTR_ARCACHE_MASK;
+			data |= (CACHE_ATTR_WRITE_ALLOC |
+				 CACHE_ATTR_CACHEABLE   |
+				 CACHE_ATTR_BUFFERABLE) <<
+				 MVEBU_AXI_ATTR_ARCACHE_OFFSET;
+			data &= ~MVEBU_AXI_ATTR_AWCACHE_MASK;
+			data |= (CACHE_ATTR_READ_ALLOC |
+				 CACHE_ATTR_CACHEABLE  |
+				 CACHE_ATTR_BUFFERABLE) <<
+				 MVEBU_AXI_ATTR_AWCACHE_OFFSET;
+			/* Set Ax-Domain as Outer domain */
+			data &= ~MVEBU_AXI_ATTR_ARDOMAIN_MASK;
+			data |= DOMAIN_OUTER_SHAREABLE <<
+				MVEBU_AXI_ATTR_ARDOMAIN_OFFSET;
+			data &= ~MVEBU_AXI_ATTR_AWDOMAIN_MASK;
+			data |= DOMAIN_OUTER_SHAREABLE <<
+				MVEBU_AXI_ATTR_AWDOMAIN_OFFSET;
+			mmio_write_32(MVEBU_AXI_ATTR_REG(index), data);
+		}
+	}
+}
+
+static void misc_soc_configurations(void)
+{
+	uint32_t reg;
+
+	/* Enable 48-bit VA */
+	mmio_setbits_32(DSS_CR0, DVM_48BIT_VA_ENABLE);
+
+	/* Un-mask Watchdog reset from influencing the SYSRST_OUTn.
+	 * Otherwise, upon WD timeout, the WD reset signal won't trigger reset
+	 */
+	reg = mmio_read_32(MVEBU_SYSRST_OUT_CONFIG_REG);
+	reg &= ~(WD_MASK_SYS_RST_OUT);
+	mmio_write_32(MVEBU_SYSRST_OUT_CONFIG_REG, reg);
+}
+
+/*
+ * By default all external CPs start with configuration address space set to
+ * 0xf200_0000. To overcome this issue, go in the loop and initialize the
+ * CP one by one, using temporary window configuration which allows to access
+ * each CP and update its configuration space according to decoding
+ * windows scheme defined for each platform.
+ */
+void update_cp110_default_win(int cp_id)
+{
+	int mci_id = cp_id - 1;
+	uintptr_t cp110_base, cp110_temp_base;
+
+	/* CP110 default configuration address space */
+	cp110_temp_base = MVEBU_AP_IO_BASE(MVEBU_AP0);
+
+	struct addr_map_win iowin_temp_win = {
+		.base_addr = cp110_temp_base,
+		.win_size = MVEBU_CP_OFFSET,
+	};
+
+	iowin_temp_win.target_id = mci_id;
+	iow_temp_win_insert(0, &iowin_temp_win, 1);
+
+	/* Calculate the new CP110 - base address */
+	cp110_base = MVEBU_CP_REGS_BASE(cp_id);
+	/* Go and update the CP110 configuration address space */
+	iob_cfg_space_update(0, cp_id, cp110_temp_base, cp110_base);
+
+	/* Remove the temporary IO-WIN window */
+	iow_temp_win_remove(0, &iowin_temp_win, 1);
+}
+
+void ap_init(void)
+{
+	/* Setup Aurora2. */
+	init_aurora2();
+
+	/* configure MCI mapping */
+	mci_remap_indirect_access_base();
+
+	/* configure IO_WIN windows */
+	init_io_win(MVEBU_AP0);
+
+	/* configure CCU windows */
+	init_ccu(MVEBU_AP0);
+
+	/* Set the stream IDs for DMA masters */
+	ap807_stream_id_init();
+
+	/* configure the SMMU */
+	setup_smmu();
+
+	/* Open AP incoming access for all masters */
+	ap_sec_masters_access_en(1);
+
+	/* configure axi for AP */
+	ap807_axi_attr_init();
+
+	/* misc configuration of the SoC */
+	misc_soc_configurations();
+}
+
+static void ap807_dram_phy_access_config(void)
+{
+	uint32_t reg_val;
+	/* Update DSS port access permission to DSS_PHY */
+	reg_val = mmio_read_32(DSS_SCR_REG);
+	reg_val &= ~(DSS_PPROT_MASK << DSS_PPROT_OFFS);
+	reg_val |= ((DSS_PPROT_PRIV_SECURE_DATA & DSS_PPROT_MASK) <<
+		    DSS_PPROT_OFFS);
+	mmio_write_32(DSS_SCR_REG, reg_val);
+}
+
+void ap_ble_init(void)
+{
+	/* Enable DSS port */
+	ap807_dram_phy_access_config();
+}
+
+int ap_get_count(void)
+{
+	return 1;
+}
+
+
diff --git a/drivers/marvell/mochi/apn806_setup.c b/drivers/marvell/mochi/apn806_setup.c
new file mode 100644
index 0000000..5c71fed
--- /dev/null
+++ b/drivers/marvell/mochi/apn806_setup.c
@@ -0,0 +1,297 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* AP806 Marvell SoC driver */
+
+#include <common/debug.h>
+#include <drivers/marvell/ccu.h>
+#include <drivers/marvell/cache_llc.h>
+#include <drivers/marvell/io_win.h>
+#include <drivers/marvell/mci.h>
+#include <drivers/marvell/mochi/ap_setup.h>
+#include <lib/mmio.h>
+
+#include <a8k_plat_def.h>
+
+#define SMMU_sACR				(MVEBU_SMMU_BASE + 0x10)
+#define SMMU_sACR_PG_64K			(1 << 16)
+
+#define CCU_GSPMU_CR				(MVEBU_CCU_BASE(MVEBU_AP0) + \
+							0x3F0)
+#define GSPMU_CPU_CONTROL			(0x1 << 0)
+
+#define CCU_HTC_CR				(MVEBU_CCU_BASE(MVEBU_AP0) + \
+							0x200)
+#define CCU_SET_POC_OFFSET			5
+
+#define DSS_CR0					(MVEBU_RFU_BASE + 0x100)
+#define DVM_48BIT_VA_ENABLE			(1 << 21)
+
+/* Secure MoChi incoming access */
+#define SEC_MOCHI_IN_ACC_REG			(MVEBU_RFU_BASE + 0x4738)
+#define SEC_MOCHI_IN_ACC_IHB0_EN		(1)
+#define SEC_MOCHI_IN_ACC_IHB1_EN		(1 << 3)
+#define SEC_MOCHI_IN_ACC_IHB2_EN		(1 << 6)
+#define SEC_MOCHI_IN_ACC_PIDI_EN		(1 << 9)
+#define SEC_IN_ACCESS_ENA_ALL_MASTERS		(SEC_MOCHI_IN_ACC_IHB0_EN | \
+						 SEC_MOCHI_IN_ACC_IHB1_EN | \
+						 SEC_MOCHI_IN_ACC_IHB2_EN | \
+						 SEC_MOCHI_IN_ACC_PIDI_EN)
+#define MOCHI_IN_ACC_LEVEL_FORCE_NONSEC		(0)
+#define MOCHI_IN_ACC_LEVEL_FORCE_SEC		(1)
+#define MOCHI_IN_ACC_LEVEL_LEAVE_ORIG		(2)
+#define MOCHI_IN_ACC_LEVEL_MASK_ALL		(3)
+#define SEC_MOCHI_IN_ACC_IHB0_LEVEL(l)		((l) << 1)
+#define SEC_MOCHI_IN_ACC_IHB1_LEVEL(l)		((l) << 4)
+#define SEC_MOCHI_IN_ACC_PIDI_LEVEL(l)		((l) << 10)
+
+
+/* SYSRST_OUTn Config definitions */
+#define MVEBU_SYSRST_OUT_CONFIG_REG		(MVEBU_MISC_SOC_BASE + 0x4)
+#define WD_MASK_SYS_RST_OUT			(1 << 2)
+
+/* Generic Timer System Controller */
+#define MVEBU_MSS_GTCR_REG			(MVEBU_REGS_BASE + 0x581000)
+#define MVEBU_MSS_GTCR_ENABLE_BIT		0x1
+
+/*
+ * AXI Configuration.
+ */
+
+/* Used for Units of AP-806 (e.g. SDIO and etc) */
+#define MVEBU_AXI_ATTR_BASE			(MVEBU_REGS_BASE + 0x6F4580)
+#define MVEBU_AXI_ATTR_REG(index)		(MVEBU_AXI_ATTR_BASE + \
+							0x4 * index)
+
+#define XOR_STREAM_ID_REG(ch)	(MVEBU_REGS_BASE + 0x410010 + (ch) * 0x20000)
+#define XOR_STREAM_ID_MASK	0xFFFF
+#define SDIO_STREAM_ID_REG	(MVEBU_RFU_BASE + 0x4600)
+#define SDIO_STREAM_ID_MASK	0xFF
+
+/* Do not use the default Stream ID 0 */
+#define A806_STREAM_ID_BASE	(0x1)
+
+static uintptr_t stream_id_reg[] = {
+	XOR_STREAM_ID_REG(0),
+	XOR_STREAM_ID_REG(1),
+	XOR_STREAM_ID_REG(2),
+	XOR_STREAM_ID_REG(3),
+	SDIO_STREAM_ID_REG,
+	0
+};
+
+enum axi_attr {
+	AXI_SDIO_ATTR = 0,
+	AXI_DFX_ATTR,
+	AXI_MAX_ATTR,
+};
+
+static void apn_sec_masters_access_en(uint32_t enable)
+{
+	/* Open/Close incoming access for all masters.
+	 * The access is disabled in trusted boot mode
+	 * Could only be done in EL3
+	 */
+	if (enable != 0) {
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG, 0x0U, /* no clear */
+			      SEC_IN_ACCESS_ENA_ALL_MASTERS);
+#if LLC_SRAM
+		/* Do not change access security level
+		 * for PIDI masters
+		 */
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG,
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_MASK_ALL),
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_LEAVE_ORIG));
+#endif
+	} else {
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG,
+				   SEC_IN_ACCESS_ENA_ALL_MASTERS,
+				   0x0U /* no set */);
+#if LLC_SRAM
+		/* Return PIDI access level to the default */
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG,
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_MASK_ALL),
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_FORCE_NONSEC));
+#endif
+	}
+}
+
+static void setup_smmu(void)
+{
+	uint32_t reg;
+
+	/* Set the SMMU page size to 64 KB */
+	reg = mmio_read_32(SMMU_sACR);
+	reg |= SMMU_sACR_PG_64K;
+	mmio_write_32(SMMU_sACR, reg);
+}
+
+static void init_aurora2(void)
+{
+	uint32_t reg;
+
+	/* Enable GSPMU control by CPU */
+	reg = mmio_read_32(CCU_GSPMU_CR);
+	reg |= GSPMU_CPU_CONTROL;
+	mmio_write_32(CCU_GSPMU_CR, reg);
+
+#if LLC_ENABLE
+	/* Enable LLC for AP806 in exclusive mode */
+	llc_enable(0, 1);
+
+	/* Set point of coherency to DDR.
+	 * This is required by units which have
+	 * SW cache coherency
+	 */
+	reg = mmio_read_32(CCU_HTC_CR);
+	reg |= (0x1 << CCU_SET_POC_OFFSET);
+	mmio_write_32(CCU_HTC_CR, reg);
+#endif /* LLC_ENABLE */
+
+	errata_wa_init();
+}
+
+
+/* MCIx indirect access register are based by default at 0xf4000000/0xf6000000
+ * to avoid conflict of internal registers of units connected via MCIx, which
+ * can be based on the same address (i.e CP1 base is also 0xf4000000),
+ * the following routines remaps the MCIx indirect bases to another domain
+ */
+static void mci_remap_indirect_access_base(void)
+{
+	uint32_t mci;
+
+	for (mci = 0; mci < MCI_MAX_UNIT_ID; mci++)
+		mmio_write_32(MCIX4_REG_START_ADDRESS_REG(mci),
+			      MVEBU_MCI_REG_BASE_REMAP(mci) >>
+			      MCI_REMAP_OFF_SHIFT);
+}
+
+/* Set a unique stream id for all DMA capable devices */
+static void ap806_stream_id_init(void)
+{
+	int i;
+
+	for (i = 0; stream_id_reg[i] != 0; i++) {
+		uint32_t mask = stream_id_reg[i] == SDIO_STREAM_ID_REG ?
+				SDIO_STREAM_ID_MASK : XOR_STREAM_ID_MASK;
+
+		mmio_clrsetbits_32(stream_id_reg[i], mask,
+				   i + A806_STREAM_ID_BASE);
+	}
+}
+
+static void apn806_axi_attr_init(void)
+{
+	uint32_t index, data;
+
+	/* Initialize AXI attributes for APN806 */
+
+	/* Go over the AXI attributes and set Ax-Cache and Ax-Domain */
+	for (index = 0; index < AXI_MAX_ATTR; index++) {
+		switch (index) {
+		/* DFX works with no coherent only -
+		 * there's no option to configure the Ax-Cache and Ax-Domain
+		 */
+		case AXI_DFX_ATTR:
+			continue;
+		default:
+			/* Set Ax-Cache as cacheable, no allocate, modifiable,
+			 * bufferable
+			 * The values are different because Read & Write
+			 * definition is different in Ax-Cache
+			 */
+			data = mmio_read_32(MVEBU_AXI_ATTR_REG(index));
+			data &= ~MVEBU_AXI_ATTR_ARCACHE_MASK;
+			data |= (CACHE_ATTR_WRITE_ALLOC |
+				 CACHE_ATTR_CACHEABLE   |
+				 CACHE_ATTR_BUFFERABLE) <<
+				 MVEBU_AXI_ATTR_ARCACHE_OFFSET;
+			data &= ~MVEBU_AXI_ATTR_AWCACHE_MASK;
+			data |= (CACHE_ATTR_READ_ALLOC |
+				 CACHE_ATTR_CACHEABLE  |
+				 CACHE_ATTR_BUFFERABLE) <<
+				 MVEBU_AXI_ATTR_AWCACHE_OFFSET;
+			/* Set Ax-Domain as Outer domain */
+			data &= ~MVEBU_AXI_ATTR_ARDOMAIN_MASK;
+			data |= DOMAIN_OUTER_SHAREABLE <<
+				MVEBU_AXI_ATTR_ARDOMAIN_OFFSET;
+			data &= ~MVEBU_AXI_ATTR_AWDOMAIN_MASK;
+			data |= DOMAIN_OUTER_SHAREABLE <<
+				MVEBU_AXI_ATTR_AWDOMAIN_OFFSET;
+			mmio_write_32(MVEBU_AXI_ATTR_REG(index), data);
+		}
+	}
+}
+
+static void dss_setup(void)
+{
+	/* Enable 48-bit VA */
+	mmio_setbits_32(DSS_CR0, DVM_48BIT_VA_ENABLE);
+}
+
+void misc_soc_configurations(void)
+{
+	uint32_t reg;
+
+	/* Un-mask Watchdog reset from influencing the SYSRST_OUTn.
+	 * Otherwise, upon WD timeout, the WD reset signal won't trigger reset
+	 */
+	reg = mmio_read_32(MVEBU_SYSRST_OUT_CONFIG_REG);
+	reg &= ~(WD_MASK_SYS_RST_OUT);
+	mmio_write_32(MVEBU_SYSRST_OUT_CONFIG_REG, reg);
+}
+
+void ap_init(void)
+{
+	/* Setup Aurora2. */
+	init_aurora2();
+
+	/* configure MCI mapping */
+	mci_remap_indirect_access_base();
+
+	/* configure IO_WIN windows */
+	init_io_win(MVEBU_AP0);
+
+	/* configure CCU windows */
+	init_ccu(MVEBU_AP0);
+
+	/* configure DSS */
+	dss_setup();
+
+	/* Set the stream IDs for DMA masters */
+	ap806_stream_id_init();
+
+	/* configure the SMMU */
+	setup_smmu();
+
+	/* Open APN incoming access for all masters */
+	apn_sec_masters_access_en(1);
+
+	/* configure axi for APN*/
+	apn806_axi_attr_init();
+
+	/* misc configuration of the SoC */
+	misc_soc_configurations();
+}
+
+void ap_ble_init(void)
+{
+}
+
+int ap_get_count(void)
+{
+	return 1;
+}
+
+void update_cp110_default_win(int cp_id)
+{
+}
diff --git a/drivers/marvell/mochi/cp110_setup.c b/drivers/marvell/mochi/cp110_setup.c
new file mode 100644
index 0000000..f12da0e
--- /dev/null
+++ b/drivers/marvell/mochi/cp110_setup.c
@@ -0,0 +1,467 @@
+/*
+ * Copyright (C) 2018-2020 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* CP110 Marvell SoC driver */
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/marvell/amb_adec.h>
+#include <drivers/marvell/iob.h>
+#include <drivers/marvell/mochi/cp110_setup.h>
+#include <drivers/rambus/trng_ip_76.h>
+
+#include <efuse_def.h>
+#include <plat_marvell.h>
+
+/*
+ * AXI Configuration.
+ */
+
+ /* Used for Units of CP-110 (e.g. USB device, USB Host, and etc) */
+#define MVEBU_AXI_ATTR_OFFSET			(0x441300)
+#define MVEBU_AXI_ATTR_REG(index)		(MVEBU_AXI_ATTR_OFFSET + \
+							0x4 * index)
+
+/* AXI Protection bits */
+#define MVEBU_AXI_PROT_OFFSET				(0x441200)
+
+/* AXI Protection regs */
+#define MVEBU_AXI_PROT_REG(index)		((index <= 4) ? \
+						(MVEBU_AXI_PROT_OFFSET + \
+							0x4 * index) : \
+						(MVEBU_AXI_PROT_OFFSET + 0x18))
+#define MVEBU_AXI_PROT_REGS_NUM			(6)
+
+#define MVEBU_SOC_CFGS_OFFSET			(0x441900)
+#define MVEBU_SOC_CFG_REG(index)		(MVEBU_SOC_CFGS_OFFSET + \
+							0x4 * index)
+#define MVEBU_SOC_CFG_REG_NUM			(0)
+#define MVEBU_SOC_CFG_GLOG_SECURE_EN_MASK	(0xE)
+
+/* SATA3 MBUS to AXI regs */
+#define MVEBU_BRIDGE_WIN_DIS_REG		(MVEBU_SOC_CFGS_OFFSET + 0x10)
+#define MVEBU_BRIDGE_WIN_DIS_OFF		(0x0)
+
+/* SATA3 MBUS to AXI regs */
+#define MVEBU_SATA_M2A_AXI_PORT_CTRL_REG	(0x54ff04)
+
+/* AXI to MBUS bridge registers */
+#define MVEBU_AMB_IP_OFFSET			(0x13ff00)
+#define MVEBU_AMB_IP_BRIDGE_WIN_REG(win)	(MVEBU_AMB_IP_OFFSET + \
+							(win * 0x8))
+#define MVEBU_AMB_IP_BRIDGE_WIN_EN_OFFSET	0
+#define MVEBU_AMB_IP_BRIDGE_WIN_EN_MASK		\
+				(0x1 << MVEBU_AMB_IP_BRIDGE_WIN_EN_OFFSET)
+#define MVEBU_AMB_IP_BRIDGE_WIN_SIZE_OFFSET	16
+#define MVEBU_AMB_IP_BRIDGE_WIN_SIZE_MASK	\
+				(0xffffu << MVEBU_AMB_IP_BRIDGE_WIN_SIZE_OFFSET)
+
+#define MVEBU_SAMPLE_AT_RESET_REG	(0x440600)
+#define SAR_PCIE1_CLK_CFG_OFFSET	31
+#define SAR_PCIE1_CLK_CFG_MASK		(0x1u << SAR_PCIE1_CLK_CFG_OFFSET)
+#define SAR_PCIE0_CLK_CFG_OFFSET	30
+#define SAR_PCIE0_CLK_CFG_MASK		(0x1 << SAR_PCIE0_CLK_CFG_OFFSET)
+#define SAR_I2C_INIT_EN_OFFSET		24
+#define SAR_I2C_INIT_EN_MASK		(1 << SAR_I2C_INIT_EN_OFFSET)
+
+/*******************************************************************************
+ * PCIE clock buffer control
+ ******************************************************************************/
+#define MVEBU_PCIE_REF_CLK_BUF_CTRL			(0x4404F0)
+#define PCIE1_REFCLK_BUFF_SOURCE			0x800
+#define PCIE0_REFCLK_BUFF_SOURCE			0x400
+
+/*******************************************************************************
+ * MSS Device Push Set Register
+ ******************************************************************************/
+#define MVEBU_CP_MSS_DPSHSR_REG				(0x280040)
+#define MSS_DPSHSR_REG_PCIE_CLK_SEL			0x8
+
+/*******************************************************************************
+ * RTC Configuration
+ ******************************************************************************/
+#define MVEBU_RTC_BASE					(0x284000)
+#define MVEBU_RTC_STATUS_REG				(MVEBU_RTC_BASE + 0x0)
+#define MVEBU_RTC_STATUS_ALARM1_MASK			0x1
+#define MVEBU_RTC_STATUS_ALARM2_MASK			0x2
+#define MVEBU_RTC_IRQ_1_CONFIG_REG			(MVEBU_RTC_BASE + 0x4)
+#define MVEBU_RTC_IRQ_2_CONFIG_REG			(MVEBU_RTC_BASE + 0x8)
+#define MVEBU_RTC_TIME_REG				(MVEBU_RTC_BASE + 0xC)
+#define MVEBU_RTC_ALARM_1_REG				(MVEBU_RTC_BASE + 0x10)
+#define MVEBU_RTC_ALARM_2_REG				(MVEBU_RTC_BASE + 0x14)
+#define MVEBU_RTC_CCR_REG				(MVEBU_RTC_BASE + 0x18)
+#define MVEBU_RTC_NOMINAL_TIMING			0x2000
+#define MVEBU_RTC_NOMINAL_TIMING_MASK			0x7FFF
+#define MVEBU_RTC_TEST_CONFIG_REG			(MVEBU_RTC_BASE + 0x1C)
+#define MVEBU_RTC_BRIDGE_TIMING_CTRL0_REG		(MVEBU_RTC_BASE + 0x80)
+#define MVEBU_RTC_WRCLK_PERIOD_MASK			0xFFFF
+#define MVEBU_RTC_WRCLK_PERIOD_DEFAULT			0x3FF
+#define MVEBU_RTC_WRCLK_SETUP_OFFS			16
+#define MVEBU_RTC_WRCLK_SETUP_MASK			0xFFFF0000
+#define MVEBU_RTC_WRCLK_SETUP_DEFAULT			0x29
+#define MVEBU_RTC_BRIDGE_TIMING_CTRL1_REG		(MVEBU_RTC_BASE + 0x84)
+#define MVEBU_RTC_READ_OUTPUT_DELAY_MASK		0xFFFF
+#define MVEBU_RTC_READ_OUTPUT_DELAY_DEFAULT		0x1F
+
+/*******************************************************************************
+ * TRNG Configuration
+ ******************************************************************************/
+#define MVEBU_TRNG_BASE					(0x760000)
+#define MVEBU_EFUSE_TRNG_ENABLE_EFUSE_WORD		MVEBU_AP_LDX_220_189_EFUSE_OFFS
+#define MVEBU_EFUSE_TRNG_ENABLE_BIT_OFFSET		13	/* LD0[202] */
+
+enum axi_attr {
+	AXI_ADUNIT_ATTR = 0,
+	AXI_COMUNIT_ATTR,
+	AXI_EIP197_ATTR,
+	AXI_USB3D_ATTR,
+	AXI_USB3H0_ATTR,
+	AXI_USB3H1_ATTR,
+	AXI_SATA0_ATTR,
+	AXI_SATA1_ATTR,
+	AXI_DAP_ATTR,
+	AXI_DFX_ATTR,
+	AXI_DBG_TRC_ATTR = 12,
+	AXI_SDIO_ATTR,
+	AXI_MSS_ATTR,
+	AXI_MAX_ATTR,
+};
+
+/* Most stream IDS are configured centrally in the CP-110 RFU
+ * but some are configured inside the unit registers
+ */
+#define RFU_STREAM_ID_BASE	(0x450000)
+#define USB3H_0_STREAM_ID_REG	(RFU_STREAM_ID_BASE + 0xC)
+#define USB3H_1_STREAM_ID_REG	(RFU_STREAM_ID_BASE + 0x10)
+#define SATA_0_STREAM_ID_REG	(RFU_STREAM_ID_BASE + 0x14)
+#define SATA_1_STREAM_ID_REG	(RFU_STREAM_ID_BASE + 0x18)
+#define SDIO_STREAM_ID_REG	(RFU_STREAM_ID_BASE + 0x28)
+
+#define CP_DMA_0_STREAM_ID_REG  (0x6B0010)
+#define CP_DMA_1_STREAM_ID_REG  (0x6D0010)
+
+/* We allocate IDs 128-255 for PCIe */
+#define MAX_STREAM_ID		(0x80)
+
+static uintptr_t stream_id_reg[] = {
+	USB3H_0_STREAM_ID_REG,
+	USB3H_1_STREAM_ID_REG,
+	CP_DMA_0_STREAM_ID_REG,
+	CP_DMA_1_STREAM_ID_REG,
+	SATA_0_STREAM_ID_REG,
+	SATA_1_STREAM_ID_REG,
+	SDIO_STREAM_ID_REG,
+	0
+};
+
+static void cp110_errata_wa_init(uintptr_t base)
+{
+	uint32_t data;
+
+	/* ERRATA GL-4076863:
+	 * Reset value for global_secure_enable inputs must be changed
+	 * from '1' to '0'.
+	 * When asserted, only "secured" transactions can enter IHB
+	 * configuration space.
+	 * However, blocking AXI transactions is performed by IOB.
+	 * Performing it also at IHB/HB complicates programming model.
+	 *
+	 * Enable non-secure access in SOC configuration register
+	 */
+	data = mmio_read_32(base + MVEBU_SOC_CFG_REG(MVEBU_SOC_CFG_REG_NUM));
+	data &= ~MVEBU_SOC_CFG_GLOG_SECURE_EN_MASK;
+	mmio_write_32(base + MVEBU_SOC_CFG_REG(MVEBU_SOC_CFG_REG_NUM), data);
+}
+
+static void cp110_pcie_clk_cfg(uintptr_t base)
+{
+	uint32_t pcie0_clk, pcie1_clk, reg;
+
+	/*
+	 * Determine the pcie0/1 clock direction (input/output) from the
+	 * sample at reset.
+	 */
+	reg = mmio_read_32(base + MVEBU_SAMPLE_AT_RESET_REG);
+	pcie0_clk = (reg & SAR_PCIE0_CLK_CFG_MASK) >> SAR_PCIE0_CLK_CFG_OFFSET;
+	pcie1_clk = (reg & SAR_PCIE1_CLK_CFG_MASK) >> SAR_PCIE1_CLK_CFG_OFFSET;
+
+	/* CP110 revision A2 or CN913x */
+	if (cp110_rev_id_get(base) == MVEBU_CP110_REF_ID_A2 ||
+	    cp110_device_id_get(base) == MVEBU_CN9130_DEV_ID) {
+		/*
+		 * PCIe Reference Clock Buffer Control register must be
+		 * set according to the clock direction (input/output)
+		 */
+		reg = mmio_read_32(base + MVEBU_PCIE_REF_CLK_BUF_CTRL);
+		reg &= ~(PCIE0_REFCLK_BUFF_SOURCE | PCIE1_REFCLK_BUFF_SOURCE);
+		if (!pcie0_clk)
+			reg |= PCIE0_REFCLK_BUFF_SOURCE;
+		if (!pcie1_clk)
+			reg |= PCIE1_REFCLK_BUFF_SOURCE;
+
+		mmio_write_32(base + MVEBU_PCIE_REF_CLK_BUF_CTRL, reg);
+	}
+
+	/* CP110 revision A1 */
+	if (cp110_rev_id_get(base) == MVEBU_CP110_REF_ID_A1) {
+		if (!pcie0_clk || !pcie1_clk) {
+			/*
+			 * if one of the pcie clocks is set to input,
+			 * we need to set mss_push[131] field, otherwise,
+			 * the pcie clock might not work.
+			 */
+			reg = mmio_read_32(base + MVEBU_CP_MSS_DPSHSR_REG);
+			reg |= MSS_DPSHSR_REG_PCIE_CLK_SEL;
+			mmio_write_32(base + MVEBU_CP_MSS_DPSHSR_REG, reg);
+		}
+	}
+}
+
+/* Set a unique stream id for all DMA capable devices */
+static void cp110_stream_id_init(uintptr_t base, uint32_t stream_id)
+{
+	int i = 0;
+
+	while (stream_id_reg[i]) {
+		if (i > MAX_STREAM_ID_PER_CP) {
+			NOTICE("Only first %d (maximum) Stream IDs allocated\n",
+			       MAX_STREAM_ID_PER_CP);
+			return;
+		}
+
+		if ((stream_id_reg[i] == CP_DMA_0_STREAM_ID_REG) ||
+		    (stream_id_reg[i] == CP_DMA_1_STREAM_ID_REG))
+			mmio_write_32(base + stream_id_reg[i],
+				      stream_id << 16 |  stream_id);
+		else
+			mmio_write_32(base + stream_id_reg[i], stream_id);
+
+		/* SATA port 0/1 are in the same SATA unit, and they should use
+		 * the same STREAM ID number
+		 */
+		if (stream_id_reg[i] != SATA_0_STREAM_ID_REG)
+			stream_id++;
+
+		i++;
+	}
+}
+
+static void cp110_axi_attr_init(uintptr_t base)
+{
+	uint32_t index, data;
+
+	/* Initialize AXI attributes for Armada-7K/8K SoC */
+
+	/* Go over the AXI attributes and set Ax-Cache and Ax-Domain */
+	for (index = 0; index < AXI_MAX_ATTR; index++) {
+		switch (index) {
+		/* DFX and MSS unit works with no coherent only -
+		 * there's no option to configure the Ax-Cache and Ax-Domain
+		 */
+		case AXI_DFX_ATTR:
+		case AXI_MSS_ATTR:
+			continue;
+		default:
+			/* Set Ax-Cache as cacheable, no allocate, modifiable,
+			 * bufferable
+			 * The values are different because Read & Write
+			 * definition is different in Ax-Cache
+			 */
+			data = mmio_read_32(base + MVEBU_AXI_ATTR_REG(index));
+			data &= ~MVEBU_AXI_ATTR_ARCACHE_MASK;
+			data |= (CACHE_ATTR_WRITE_ALLOC |
+				 CACHE_ATTR_CACHEABLE   |
+				 CACHE_ATTR_BUFFERABLE) <<
+				 MVEBU_AXI_ATTR_ARCACHE_OFFSET;
+			data &= ~MVEBU_AXI_ATTR_AWCACHE_MASK;
+			data |= (CACHE_ATTR_READ_ALLOC |
+				 CACHE_ATTR_CACHEABLE  |
+				 CACHE_ATTR_BUFFERABLE) <<
+				 MVEBU_AXI_ATTR_AWCACHE_OFFSET;
+			/* Set Ax-Domain as Outer domain */
+			data &= ~MVEBU_AXI_ATTR_ARDOMAIN_MASK;
+			data |= DOMAIN_OUTER_SHAREABLE <<
+				MVEBU_AXI_ATTR_ARDOMAIN_OFFSET;
+			data &= ~MVEBU_AXI_ATTR_AWDOMAIN_MASK;
+			data |= DOMAIN_OUTER_SHAREABLE <<
+				MVEBU_AXI_ATTR_AWDOMAIN_OFFSET;
+			mmio_write_32(base + MVEBU_AXI_ATTR_REG(index), data);
+		}
+	}
+
+	/* SATA IOCC supported, cache attributes
+	 * for SATA MBUS to AXI configuration.
+	 */
+	data = mmio_read_32(base + MVEBU_SATA_M2A_AXI_PORT_CTRL_REG);
+	data &= ~MVEBU_SATA_M2A_AXI_AWCACHE_MASK;
+	data |= (CACHE_ATTR_WRITE_ALLOC |
+		 CACHE_ATTR_CACHEABLE   |
+		 CACHE_ATTR_BUFFERABLE) <<
+		 MVEBU_SATA_M2A_AXI_AWCACHE_OFFSET;
+	data &= ~MVEBU_SATA_M2A_AXI_ARCACHE_MASK;
+	data |= (CACHE_ATTR_READ_ALLOC |
+		 CACHE_ATTR_CACHEABLE  |
+		 CACHE_ATTR_BUFFERABLE) <<
+		 MVEBU_SATA_M2A_AXI_ARCACHE_OFFSET;
+	mmio_write_32(base + MVEBU_SATA_M2A_AXI_PORT_CTRL_REG, data);
+
+	/* Set all IO's AXI attribute to non-secure access. */
+	for (index = 0; index < MVEBU_AXI_PROT_REGS_NUM; index++)
+		mmio_write_32(base + MVEBU_AXI_PROT_REG(index),
+			      DOMAIN_SYSTEM_SHAREABLE);
+}
+
+void cp110_amb_init(uintptr_t base)
+{
+	uint32_t reg;
+
+	/* Open AMB bridge Window to Access COMPHY/MDIO registers */
+	reg = mmio_read_32(base + MVEBU_AMB_IP_BRIDGE_WIN_REG(0));
+	reg &= ~(MVEBU_AMB_IP_BRIDGE_WIN_SIZE_MASK |
+		 MVEBU_AMB_IP_BRIDGE_WIN_EN_MASK);
+	reg |= (0x7ff << MVEBU_AMB_IP_BRIDGE_WIN_SIZE_OFFSET) |
+	       (0x1 << MVEBU_AMB_IP_BRIDGE_WIN_EN_OFFSET);
+	mmio_write_32(base + MVEBU_AMB_IP_BRIDGE_WIN_REG(0), reg);
+}
+
+static void cp110_rtc_init(uintptr_t base)
+{
+	/* Update MBus timing parameters before accessing RTC registers */
+	mmio_clrsetbits_32(base + MVEBU_RTC_BRIDGE_TIMING_CTRL0_REG,
+			   MVEBU_RTC_WRCLK_PERIOD_MASK,
+			   MVEBU_RTC_WRCLK_PERIOD_DEFAULT);
+
+	mmio_clrsetbits_32(base + MVEBU_RTC_BRIDGE_TIMING_CTRL0_REG,
+			   MVEBU_RTC_WRCLK_SETUP_MASK,
+			   MVEBU_RTC_WRCLK_SETUP_DEFAULT <<
+			   MVEBU_RTC_WRCLK_SETUP_OFFS);
+
+	mmio_clrsetbits_32(base + MVEBU_RTC_BRIDGE_TIMING_CTRL1_REG,
+			   MVEBU_RTC_READ_OUTPUT_DELAY_MASK,
+			   MVEBU_RTC_READ_OUTPUT_DELAY_DEFAULT);
+
+	/*
+	 * Issue reset to the RTC if Clock Correction register
+	 * contents did not sustain the reboot/power-on.
+	 */
+	if ((mmio_read_32(base + MVEBU_RTC_CCR_REG) &
+	    MVEBU_RTC_NOMINAL_TIMING_MASK) != MVEBU_RTC_NOMINAL_TIMING) {
+		/* Reset Test register */
+		mmio_write_32(base + MVEBU_RTC_TEST_CONFIG_REG, 0);
+		mdelay(500);
+
+		/* Reset Status register */
+		mmio_write_32(base + MVEBU_RTC_STATUS_REG,
+			      (MVEBU_RTC_STATUS_ALARM1_MASK |
+			      MVEBU_RTC_STATUS_ALARM2_MASK));
+		udelay(62);
+
+		/* Turn off Int1 and Int2 sources & clear the Alarm count */
+		mmio_write_32(base + MVEBU_RTC_IRQ_1_CONFIG_REG, 0);
+		mmio_write_32(base + MVEBU_RTC_IRQ_2_CONFIG_REG, 0);
+		mmio_write_32(base + MVEBU_RTC_ALARM_1_REG, 0);
+		mmio_write_32(base + MVEBU_RTC_ALARM_2_REG, 0);
+
+		/* Setup nominal register access timing */
+		mmio_write_32(base + MVEBU_RTC_CCR_REG,
+			      MVEBU_RTC_NOMINAL_TIMING);
+
+		/* Reset Status register */
+		mmio_write_32(base + MVEBU_RTC_STATUS_REG,
+			      (MVEBU_RTC_STATUS_ALARM1_MASK |
+			      MVEBU_RTC_STATUS_ALARM2_MASK));
+		udelay(50);
+	}
+}
+
+static void cp110_amb_adec_init(uintptr_t base)
+{
+	/* enable AXI-MBUS by clearing "Bridge Windows Disable" */
+	mmio_clrbits_32(base + MVEBU_BRIDGE_WIN_DIS_REG,
+			(1 << MVEBU_BRIDGE_WIN_DIS_OFF));
+
+	/* configure AXI-MBUS windows for CP */
+	init_amb_adec(base);
+}
+
+static void cp110_trng_init(uintptr_t base)
+{
+	static bool done;
+	int ret;
+	uint32_t reg_val, efuse;
+
+	/* Set access to LD0 */
+	reg_val = mmio_read_32(MVEBU_AP_EFUSE_SRV_CTRL_REG);
+	reg_val &= ~EFUSE_SRV_CTRL_LD_SELECT_MASK;
+	mmio_write_32(MVEBU_AP_EFUSE_SRV_CTRL_REG, reg_val);
+
+	/* Obtain the AP LD0 bit defining TRNG presence */
+	efuse = mmio_read_32(MVEBU_EFUSE_TRNG_ENABLE_EFUSE_WORD);
+	efuse >>= MVEBU_EFUSE_TRNG_ENABLE_BIT_OFFSET;
+	efuse &= 1;
+
+	if (efuse == 0) {
+		VERBOSE("TRNG is not present, skipping");
+		return;
+	}
+
+	if (!done) {
+		ret = eip76_rng_probe(base + MVEBU_TRNG_BASE);
+		if (ret != 0) {
+			ERROR("Failed to init TRNG @ 0x%lx\n", base);
+			return;
+		}
+		done = true;
+	}
+}
+void cp110_init(uintptr_t cp110_base, uint32_t stream_id)
+{
+	INFO("%s: Initialize CPx - base = %lx\n", __func__, cp110_base);
+
+	/* configure IOB windows for CP0*/
+	init_iob(cp110_base);
+
+	/* configure AXI-MBUS windows for CP0*/
+	cp110_amb_adec_init(cp110_base);
+
+	/* configure axi for CP0*/
+	cp110_axi_attr_init(cp110_base);
+
+	/* Execute SW WA for erratas */
+	cp110_errata_wa_init(cp110_base);
+
+	/* Confiure pcie clock according to clock direction */
+	cp110_pcie_clk_cfg(cp110_base);
+
+	/* configure stream id for CP0 */
+	cp110_stream_id_init(cp110_base, stream_id);
+
+	/* Open AMB bridge for comphy for CP0 & CP1*/
+	cp110_amb_init(cp110_base);
+
+	/* Reset RTC if needed */
+	cp110_rtc_init(cp110_base);
+
+	/* TRNG init - for CP0 only */
+	cp110_trng_init(cp110_base);
+}
+
+/* Do the minimal setup required to configure the CP in BLE */
+void cp110_ble_init(uintptr_t cp110_base)
+{
+#if PCI_EP_SUPPORT
+	INFO("%s: Initialize CPx - base = %lx\n", __func__, cp110_base);
+
+	cp110_amb_init(cp110_base);
+
+	/* Configure PCIe clock */
+	cp110_pcie_clk_cfg(cp110_base);
+
+	/* Configure PCIe endpoint */
+	ble_plat_pcie_ep_setup();
+#endif
+}
diff --git a/drivers/marvell/secure_dfx_access/armada_thermal.c b/drivers/marvell/secure_dfx_access/armada_thermal.c
new file mode 100644
index 0000000..4f7191b
--- /dev/null
+++ b/drivers/marvell/secure_dfx_access/armada_thermal.c
@@ -0,0 +1,253 @@
+/*
+ * Copyright (C) 2019 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <errno.h>
+#include <lib/mmio.h>
+#include <mvebu.h>
+#include <stdbool.h>
+#include "dfx.h"
+
+/* #define DEBUG_DFX */
+#ifdef DEBUG_DFX
+#define debug(format...) NOTICE(format)
+#else
+#define debug(format, arg...)
+#endif
+
+#define TSEN_CTRL0			0xf06f8084
+ #define TSEN_CTRL0_START		BIT(0)
+ #define TSEN_CTRL0_RESET		BIT(1)
+ #define TSEN_CTRL0_ENABLE		BIT(2)
+ #define TSEN_CTRL0_AVG_BYPASS		BIT(6)
+ #define TSEN_CTRL0_CHAN_SHIFT		13
+ #define TSEN_CTRL0_CHAN_MASK		0xF
+ #define TSEN_CTRL0_OSR_SHIFT		24
+ #define TSEN_CTRL0_OSR_MAX		0x3
+ #define TSEN_CTRL0_MODE_SHIFT		30
+ #define TSEN_CTRL0_MODE_EXTERNAL	0x2U
+ #define TSEN_CTRL0_MODE_MASK		0x3U
+
+#define TSEN_CTRL1			0xf06f8088
+ #define TSEN_CTRL1_INT_EN		BIT(25)
+ #define TSEN_CTRL1_HYST_SHIFT		19
+ #define TSEN_CTRL1_HYST_MASK		(0x3 << TSEN_CTRL1_HYST_SHIFT)
+ #define TSEN_CTRL1_THRESH_SHIFT	3
+ #define TSEN_CTRL1_THRESH_MASK		(0x3ff << TSEN_CTRL1_THRESH_SHIFT)
+
+#define TSEN_STATUS			0xf06f808c
+ #define TSEN_STATUS_VALID_OFFSET	16
+ #define TSEN_STATUS_VALID_MASK		(0x1 << TSEN_STATUS_VALID_OFFSET)
+ #define TSEN_STATUS_TEMP_OUT_OFFSET	0
+ #define TSEN_STATUS_TEMP_OUT_MASK	(0x3FF << TSEN_STATUS_TEMP_OUT_OFFSET)
+
+#define DFX_SERVER_IRQ_SUM_MASK_REG	0xf06f8104
+ #define DFX_SERVER_IRQ_EN		BIT(1)
+
+#define DFX_IRQ_CAUSE_REG		0xf06f8108
+
+#define DFX_IRQ_MASK_REG		0xf06f810c
+ #define DFX_IRQ_TSEN_OVERHEAT_OFFSET	BIT(22)
+
+#define THERMAL_SEN_OUTPUT_MSB		512
+#define THERMAL_SEN_OUTPUT_COMP		1024
+
+#define COEF_M 423
+#define COEF_B -150000LL
+
+static void armada_ap806_thermal_read(u_register_t *temp)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(TSEN_STATUS);
+
+	reg = ((reg & TSEN_STATUS_TEMP_OUT_MASK) >>
+	      TSEN_STATUS_TEMP_OUT_OFFSET);
+
+	/*
+	 * TSEN output format is signed as a 2s complement number
+	 * ranging from-512 to +511. when MSB is set, need to
+	 * calculate the complement number
+	 */
+	if (reg >= THERMAL_SEN_OUTPUT_MSB)
+		reg -= THERMAL_SEN_OUTPUT_COMP;
+
+	*temp = ((COEF_M * ((signed int)reg)) - COEF_B);
+}
+
+static void armada_ap806_thermal_irq(void)
+{
+	/* Dummy read, register ROC */
+	mmio_read_32(DFX_IRQ_CAUSE_REG);
+}
+
+static void armada_ap806_thermal_overheat_irq_init(void)
+{
+	uint32_t reg;
+
+	/* Clear DFX temperature IRQ cause */
+	reg = mmio_read_32(DFX_IRQ_CAUSE_REG);
+
+	/* Enable DFX Temperature IRQ */
+	reg = mmio_read_32(DFX_IRQ_MASK_REG);
+	reg |= DFX_IRQ_TSEN_OVERHEAT_OFFSET;
+	mmio_write_32(DFX_IRQ_MASK_REG, reg);
+
+	/* Enable DFX server IRQ */
+	reg = mmio_read_32(DFX_SERVER_IRQ_SUM_MASK_REG);
+	reg |= DFX_SERVER_IRQ_EN;
+	mmio_write_32(DFX_SERVER_IRQ_SUM_MASK_REG, reg);
+
+	/* Enable overheat interrupt */
+	reg = mmio_read_32(TSEN_CTRL1);
+	reg |= TSEN_CTRL1_INT_EN;
+	mmio_write_32(TSEN_CTRL1, reg);
+}
+
+static unsigned int armada_mc_to_reg_temp(unsigned int temp_mc)
+{
+	unsigned int sample;
+
+	sample = (temp_mc + COEF_B) / COEF_M;
+
+	return sample & 0x3ff;
+}
+
+/*
+ * The documentation states:
+ * high/low watermark = threshold +/- 0.4761 * 2^(hysteresis + 2)
+ * which is the mathematical derivation for:
+ * 0x0 <=> 1.9°C, 0x1 <=> 3.8°C, 0x2 <=> 7.6°C, 0x3 <=> 15.2°C
+ */
+static unsigned int hyst_levels_mc[] = {1900, 3800, 7600, 15200};
+
+static unsigned int armada_mc_to_reg_hyst(int hyst_mc)
+{
+	int i;
+
+	/*
+	 * We will always take the smallest possible hysteresis to avoid risking
+	 * the hardware integrity by enlarging the threshold by +8°C in the
+	 * worst case.
+	 */
+	for (i = ARRAY_SIZE(hyst_levels_mc) - 1; i > 0; i--)
+		if (hyst_mc >= hyst_levels_mc[i])
+			break;
+
+	return i;
+}
+
+static void armada_ap806_thermal_threshold(int thresh_mc, int hyst_mc)
+{
+	uint32_t ctrl1;
+	unsigned int threshold = armada_mc_to_reg_temp(thresh_mc);
+	unsigned int hysteresis = armada_mc_to_reg_hyst(hyst_mc);
+
+	ctrl1 = mmio_read_32(TSEN_CTRL1);
+	/* Set Threshold */
+	if (thresh_mc >= 0) {
+		ctrl1 &= ~(TSEN_CTRL1_THRESH_MASK);
+		ctrl1 |= threshold << TSEN_CTRL1_THRESH_SHIFT;
+	}
+
+	/* Set Hysteresis */
+	if (hyst_mc >= 0) {
+		ctrl1 &= ~(TSEN_CTRL1_HYST_MASK);
+		ctrl1 |= hysteresis << TSEN_CTRL1_HYST_SHIFT;
+	}
+
+	mmio_write_32(TSEN_CTRL1, ctrl1);
+}
+
+static void armada_select_channel(int channel)
+{
+	uint32_t ctrl0;
+
+	/* Stop the measurements */
+	ctrl0 = mmio_read_32(TSEN_CTRL0);
+	ctrl0 &= ~TSEN_CTRL0_START;
+	mmio_write_32(TSEN_CTRL0, ctrl0);
+
+	/* Reset the mode, internal sensor will be automatically selected */
+	ctrl0 &= ~(TSEN_CTRL0_MODE_MASK << TSEN_CTRL0_MODE_SHIFT);
+
+	/* Other channels are external and should be selected accordingly */
+	if (channel) {
+		/* Change the mode to external */
+		ctrl0 |= TSEN_CTRL0_MODE_EXTERNAL <<
+			 TSEN_CTRL0_MODE_SHIFT;
+		/* Select the sensor */
+		ctrl0 &= ~(TSEN_CTRL0_CHAN_MASK << TSEN_CTRL0_CHAN_SHIFT);
+		ctrl0 |= (channel - 1) << TSEN_CTRL0_CHAN_SHIFT;
+	}
+
+	/* Actually set the mode/channel */
+	mmio_write_32(TSEN_CTRL0, ctrl0);
+
+	/* Re-start the measurements */
+	ctrl0 |= TSEN_CTRL0_START;
+	mmio_write_32(TSEN_CTRL0, ctrl0);
+}
+
+static void armada_ap806_thermal_init(void)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(TSEN_CTRL0);
+	reg &= ~TSEN_CTRL0_RESET;
+	reg |= TSEN_CTRL0_START | TSEN_CTRL0_ENABLE;
+
+	/* Sample every ~2ms */
+	reg |= TSEN_CTRL0_OSR_MAX << TSEN_CTRL0_OSR_SHIFT;
+
+	/* Enable average (2 samples by default) */
+	reg &= ~TSEN_CTRL0_AVG_BYPASS;
+
+	mmio_write_32(TSEN_CTRL0, reg);
+
+	debug("thermal: Initialization done\n");
+}
+
+static void armada_is_valid(u_register_t *read)
+{
+	*read = (mmio_read_32(TSEN_STATUS) & TSEN_STATUS_VALID_MASK);
+}
+
+int mvebu_dfx_thermal_handle(u_register_t func, u_register_t *read,
+			     u_register_t x2, u_register_t x3)
+{
+	debug_enter();
+
+	switch (func) {
+	case MV_SIP_DFX_THERMAL_INIT:
+		armada_ap806_thermal_init();
+		break;
+	case MV_SIP_DFX_THERMAL_READ:
+		armada_ap806_thermal_read(read);
+		break;
+	case MV_SIP_DFX_THERMAL_IRQ:
+		armada_ap806_thermal_irq();
+		break;
+	case MV_SIP_DFX_THERMAL_THRESH:
+		armada_ap806_thermal_threshold(x2, x3);
+		armada_ap806_thermal_overheat_irq_init();
+		break;
+	case MV_SIP_DFX_THERMAL_IS_VALID:
+		armada_is_valid(read);
+		break;
+	case MV_SIP_DFX_THERMAL_SEL_CHANNEL:
+		armada_select_channel(x2);
+		break;
+	default:
+		ERROR("unsupported dfx func\n");
+		return -EINVAL;
+	}
+
+	debug_exit();
+
+	return 0;
+}
diff --git a/drivers/marvell/secure_dfx_access/dfx.h b/drivers/marvell/secure_dfx_access/dfx.h
new file mode 100644
index 0000000..88c4de8
--- /dev/null
+++ b/drivers/marvell/secure_dfx_access/dfx.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) 2019 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* DFX sub-FID */
+#define MV_SIP_DFX_THERMAL_INIT		1
+#define MV_SIP_DFX_THERMAL_READ		2
+#define MV_SIP_DFX_THERMAL_IS_VALID	3
+#define MV_SIP_DFX_THERMAL_IRQ		4
+#define MV_SIP_DFX_THERMAL_THRESH	5
+#define MV_SIP_DFX_THERMAL_SEL_CHANNEL	6
+
+#define MV_SIP_DFX_SREAD		20
+#define MV_SIP_DFX_SWRITE		21
+
+int mvebu_dfx_thermal_handle(u_register_t func, u_register_t *read,
+			     u_register_t x2, u_register_t x3);
+int mvebu_dfx_misc_handle(u_register_t func, u_register_t *read,
+			  u_register_t addr, u_register_t val);
diff --git a/drivers/marvell/secure_dfx_access/misc_dfx.c b/drivers/marvell/secure_dfx_access/misc_dfx.c
new file mode 100644
index 0000000..189105f
--- /dev/null
+++ b/drivers/marvell/secure_dfx_access/misc_dfx.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) 2021 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include "dfx.h"
+#include <mvebu_def.h>
+#include <mvebu.h>
+#include <errno.h>
+
+/* #define DEBUG_DFX */
+#ifdef DEBUG_DFX
+#define debug(format...) NOTICE(format)
+#else
+#define debug(format, arg...)
+#endif
+
+#define SAR_BASE			(MVEBU_REGS_BASE + 0x6F8200)
+#define SAR_SIZE			0x4
+#define AP_DEV_ID_STATUS_REG		(MVEBU_REGS_BASE + 0x6F8240)
+#define JTAG_DEV_ID_STATUS_REG		(MVEBU_REGS_BASE + 0x6F8244)
+#define EFUSE_CTRL			(MVEBU_REGS_BASE + 0x6F8008)
+#define EFUSE_LD_BASE			(MVEBU_REGS_BASE + 0x6F8F00)
+#define EFUSE_LD_SIZE			0x1C
+#define EFUSE_HD_BASE			(MVEBU_REGS_BASE + 0x6F9000)
+#define EFUSE_HD_SIZE			0x3F8
+
+/* AP806 CPU DFS register mapping*/
+#define AP806_CA72MP2_0_PLL_CR_0_BASE		(MVEBU_REGS_BASE + 0x6F8278)
+#define AP806_CA72MP2_0_PLL_CR_1_BASE		(MVEBU_REGS_BASE + 0x6F8280)
+#define AP806_CA72MP2_0_PLL_CR_2_BASE		(MVEBU_REGS_BASE + 0x6F8284)
+#define AP806_CA72MP2_0_PLL_SR_BASE		(MVEBU_REGS_BASE + 0x6F8C94)
+
+/* AP807 CPU DFS register mapping */
+#define AP807_DEVICE_GENERAL_CR_10_BASE		(MVEBU_REGS_BASE + 0x6F8278)
+#define AP807_DEVICE_GENERAL_CR_11_BASE		(MVEBU_REGS_BASE + 0x6F827C)
+#define AP807_DEVICE_GENERAL_STATUS_6_BASE	(MVEBU_REGS_BASE + 0x6F8C98)
+
+#ifdef MVEBU_SOC_AP807
+ #define CLUSTER_OFFSET		0x8
+ #define CLK_DIVIDER_REG	AP807_DEVICE_GENERAL_CR_10_BASE
+ #define CLK_FORCE_REG		AP807_DEVICE_GENERAL_CR_11_BASE
+ #define CLK_RATIO_REG		AP807_DEVICE_GENERAL_CR_11_BASE
+ #define CLK_RATIO_STATE_REG	AP807_DEVICE_GENERAL_STATUS_6_BASE
+#else
+ #define CLUSTER_OFFSET		0x14
+ #define CLK_DIVIDER_REG		AP806_CA72MP2_0_PLL_CR_0_BASE
+ #define CLK_FORCE_REG		AP806_CA72MP2_0_PLL_CR_1_BASE
+ #define CLK_RATIO_REG		AP806_CA72MP2_0_PLL_CR_2_BASE
+ #define CLK_RATIO_STATE_REG	AP806_CA72MP2_0_PLL_SR_BASE
+#endif /* MVEBU_SOC_AP807 */
+
+static _Bool is_valid(u_register_t addr)
+{
+	switch (addr) {
+	case AP_DEV_ID_STATUS_REG:
+	case JTAG_DEV_ID_STATUS_REG:
+	case SAR_BASE ... (SAR_BASE + SAR_SIZE):
+	case EFUSE_LD_BASE ... (EFUSE_LD_BASE + EFUSE_LD_SIZE):
+	case EFUSE_HD_BASE ... (EFUSE_HD_BASE + EFUSE_HD_SIZE):
+	case EFUSE_CTRL:
+	/* cpu-clk related registers */
+	case CLK_DIVIDER_REG:
+	case CLK_DIVIDER_REG + CLUSTER_OFFSET:
+	case CLK_FORCE_REG:
+	case CLK_FORCE_REG + CLUSTER_OFFSET:
+#ifndef MVEBU_SOC_AP807
+	case CLK_RATIO_REG:
+	case CLK_RATIO_REG + CLUSTER_OFFSET:
+#endif
+	case CLK_RATIO_STATE_REG:
+	case CLK_RATIO_STATE_REG + CLUSTER_OFFSET:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static int armada_dfx_sread(u_register_t *read, u_register_t addr)
+{
+	if (!is_valid(addr))
+		return -EINVAL;
+
+	*read = mmio_read_32(addr);
+
+	return 0;
+}
+
+static int armada_dfx_swrite(u_register_t addr, u_register_t val)
+{
+	if (!is_valid(addr))
+		return -EINVAL;
+
+	mmio_write_32(addr, val);
+
+	return 0;
+}
+
+int mvebu_dfx_misc_handle(u_register_t func, u_register_t *read,
+			  u_register_t addr, u_register_t val)
+{
+	debug_enter();
+
+	debug("func %ld, addr 0x%lx, val 0x%lx\n", func, addr, val);
+
+	switch (func) {
+	case MV_SIP_DFX_SREAD:
+		return armada_dfx_sread(read, addr);
+	case MV_SIP_DFX_SWRITE:
+		return armada_dfx_swrite(addr, val);
+	default:
+		ERROR("unsupported dfx misc sub-func\n");
+		return -EINVAL;
+	}
+
+	debug_exit();
+
+	return 0;
+}
diff --git a/drivers/marvell/thermal.c b/drivers/marvell/thermal.c
new file mode 100644
index 0000000..a501ab4
--- /dev/null
+++ b/drivers/marvell/thermal.c
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/* Driver for thermal unit located in Marvell ARMADA 8K and compatible SoCs */
+
+#include <common/debug.h>
+#include <drivers/marvell/thermal.h>
+
+int marvell_thermal_init(struct tsen_config *tsen_cfg)
+{
+	if (tsen_cfg->tsen_ready == 1) {
+		INFO("thermal sensor is already initialized\n");
+		return 0;
+	}
+
+	if (tsen_cfg->ptr_tsen_probe == NULL) {
+		ERROR("initial thermal sensor configuration is missing\n");
+		return -1;
+	}
+
+	if (tsen_cfg->ptr_tsen_probe(tsen_cfg)) {
+		ERROR("thermal sensor initialization failed\n");
+		return -1;
+	}
+
+	VERBOSE("thermal sensor was initialized\n");
+
+	return 0;
+}
+
+int marvell_thermal_read(struct tsen_config *tsen_cfg, int *temp)
+{
+	if (temp == NULL) {
+		ERROR("NULL pointer for temperature read\n");
+		return -1;
+	}
+
+	if (tsen_cfg->ptr_tsen_read == NULL ||
+	    tsen_cfg->tsen_ready == 0) {
+		ERROR("thermal sensor was not initialized\n");
+		return -1;
+	}
+
+	if (tsen_cfg->ptr_tsen_read(tsen_cfg, temp)) {
+		ERROR("temperature read failed\n");
+		return -1;
+	}
+
+	return 0;
+}
diff --git a/drivers/marvell/uart/a3700_console.S b/drivers/marvell/uart/a3700_console.S
new file mode 100644
index 0000000..c7eb165
--- /dev/null
+++ b/drivers/marvell/uart/a3700_console.S
@@ -0,0 +1,271 @@
+/*
+ * Copyright (C) 2016 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <console_macros.S>
+#include <drivers/marvell/uart/a3700_console.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_a3700_core_putc
+	.globl console_a3700_core_init
+	.globl console_a3700_core_getc
+	.globl console_a3700_core_flush
+
+	.globl console_a3700_putc
+	.globl console_a3700_getc
+	.globl console_a3700_flush
+
+	/* -----------------------------------------------
+	 * int console_a3700_core_init(unsigned long 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
+	 * Clobber list : x1, x2, x3, x4
+	 * -----------------------------------------------
+	 */
+func console_a3700_core_init
+	/* Check the input base address */
+	cbz	x0, init_fail
+	/* Check baud rate and uart clock for sanity */
+	cbz	w1, init_fail
+	cbz	w2, init_fail
+
+	/*
+	 * Wait for the TX (THR and TSR) to be empty. If wait for 3ms, the TX FIFO is
+	 * still not empty, TX FIFO will reset by all means.
+	 */
+	mov	w4, #30				/* max time out 30 * 100 us */
+2:
+	/* Check whether TX (THR and TSR) is empty */
+	ldr	w3, [x0, #UART_STATUS_REG]
+	and	w3, w3, #UARTLSR_TXEMPTY
+	cmp	w3, #0
+	b.ne	4f
+
+	/* Delay */
+	mov	w3, #60000	/* 60000 cycles of below 3 instructions on 1200 MHz CPU ~~ 100 us */
+3:
+	sub	w3, w3, #1
+	cmp	w3, #0
+	b.ne	3b
+
+	/* Check whether wait timeout expired */
+	sub	w4, w4, #1
+	cmp	w4, #0
+	b.ne	2b
+
+4:
+	/* Reset UART via North Bridge Peripheral */
+	mov_imm	x4, MVEBU_NB_RESET_REG
+	ldr	w3, [x4]
+	bic	w3, w3, #MVEBU_NB_RESET_UART_N
+	str	w3, [x4]
+	orr	w3, w3, #MVEBU_NB_RESET_UART_N
+	str	w3, [x4]
+
+	/* Reset FIFO */
+	mov	w3, #UART_CTRL_RXFIFO_RESET
+	orr	w3, w3, #UART_CTRL_TXFIFO_RESET
+	str	w3, [x0, #UART_CTRL_REG]
+
+	/* Delay */
+	mov	w3, #2000
+1:
+	sub	w3, w3, #1
+	cmp	w3, #0
+	b.ne	1b
+
+	/* Program the baudrate */
+	/* Divisor = Round(Uartclock / (16 * baudrate)) */
+	lsl	w2, w2, #4
+	add	w1, w1, w2, lsr #1
+	udiv	w2, w1, w2
+	and	w2, w2, #0x3ff /* clear all other bits to use default clock */
+
+	str	w2, [x0, #UART_BAUD_REG]/* set baud rate divisor */
+
+	/* Set UART to default 16X scheme */
+	mov	w3, #0
+	str	w3, [x0, #UART_POSSR_REG]
+
+	/* No Parity, 1 Stop */
+	mov	w3, #0
+	str	w3, [x0, #UART_CTRL_REG]
+
+	mov	w0, #1
+	ret
+init_fail:
+	mov	w0, #0
+	ret
+endfunc console_a3700_core_init
+
+	.globl console_a3700_register
+
+	/* -----------------------------------------------
+	 * int console_a3700_register(console_t *console,
+		uintptr_t base, uint32_t clk, uint32_t baud)
+	 * Function to initialize and register a new a3700
+	 * 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, x3, x4, x6, x7, x14
+	 * -----------------------------------------------
+	 */
+func console_a3700_register
+	mov	x7, x30
+	mov	x6, x3
+	cbz	x6, register_fail
+	str	x0, [x6, #CONSOLE_T_BASE]
+
+	bl	console_a3700_core_init
+	cbz	x0, register_fail
+
+	mov	x0, x6
+	mov	x30, x7
+	finish_console_register a3700, putc=1, getc=1, flush=1
+
+register_fail:
+	ret	x7
+endfunc console_a3700_register
+
+	/* --------------------------------------------------------
+	 * int console_a3700_core_putc(int c, unsigned int 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_a3700_core_putc
+	/* Check the input parameter */
+	cbz	x1, putc_error
+
+	/* Prepend '\r' to '\n' */
+	cmp	w0, #0xA
+	b.ne	2f
+	/* Check if the transmit FIFO is full */
+1:	ldr	w2, [x1, #UART_STATUS_REG]
+	and	w2, w2, #UARTLSR_TXFIFOFULL
+	cmp	w2, #UARTLSR_TXFIFOFULL
+	b.eq	1b
+	mov	w2, #0xD		/* '\r' */
+	str	w2, [x1, #UART_TX_REG]
+
+	/* Check if the transmit FIFO is full */
+2:	ldr	w2, [x1, #UART_STATUS_REG]
+	and	w2, w2, #UARTLSR_TXFIFOFULL
+	cmp	w2, #UARTLSR_TXFIFOFULL
+	b.eq	2b
+	str	w0, [x1, #UART_TX_REG]
+	ret
+putc_error:
+	mov	w0, #-1
+	ret
+endfunc console_a3700_core_putc
+
+	/* --------------------------------------------------------
+	 * int console_a3700_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_a3700_putc
+	ldr	x1, [x1, #CONSOLE_T_BASE]
+	b	console_a3700_core_putc
+endfunc console_a3700_putc
+
+	/* ---------------------------------------------
+	 * int console_a3700_core_getc(void)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 if no character is available.
+	 * In : w0 - console base address
+	 * Out : w0 - character if available, else -1
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func console_a3700_core_getc
+	/* Check if there is a pending character */
+	ldr	w1, [x0, #UART_STATUS_REG]
+	and	w1, w1, #UARTLSR_RXRDY
+	cmp	w1, #UARTLSR_RXRDY
+	b.ne	getc_no_char
+	ldr	w0, [x0, #UART_RX_REG]
+	and	w0, w0, #0xff
+	ret
+getc_no_char:
+	mov	w0, #ERROR_NO_PENDING_CHAR
+	ret
+endfunc console_a3700_core_getc
+
+	/* ---------------------------------------------
+	 * int console_a3700_getc(console_t *console)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 on 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_a3700_getc
+	ldr	x0, [x0, #CONSOLE_T_BASE]
+	b	console_a3700_core_getc
+endfunc console_a3700_getc
+
+	/* ---------------------------------------------
+	 * void console_a3700_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_a3700_core_flush
+	/* Wait for the TX (THR and TSR) to be empty */
+1:	ldr	w1, [x0, #UART_STATUS_REG]
+	and	w1, w1, #UARTLSR_TXEMPTY
+	cmp	w1, #UARTLSR_TXEMPTY
+	b.ne	1b
+	ret
+endfunc console_a3700_core_flush
+
+	/* ---------------------------------------------
+	 * void console_a3700_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_a3700_flush
+	ldr	x0, [x0, #CONSOLE_T_BASE]
+	b	console_a3700_core_flush
+endfunc console_a3700_flush
+
diff --git a/drivers/measured_boot/event_log/event_log.c b/drivers/measured_boot/event_log/event_log.c
new file mode 100644
index 0000000..d661c35
--- /dev/null
+++ b/drivers/measured_boot/event_log/event_log.c
@@ -0,0 +1,321 @@
+/*
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <arch_helpers.h>
+
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/auth/crypto_mod.h>
+#include <drivers/measured_boot/event_log/event_log.h>
+
+#include <plat/common/platform.h>
+
+#if TPM_ALG_ID == TPM_ALG_SHA512
+#define	CRYPTO_MD_ID	CRYPTO_MD_SHA512
+#elif TPM_ALG_ID == TPM_ALG_SHA384
+#define	CRYPTO_MD_ID	CRYPTO_MD_SHA384
+#elif TPM_ALG_ID == TPM_ALG_SHA256
+#define	CRYPTO_MD_ID	CRYPTO_MD_SHA256
+#else
+#  error Invalid TPM algorithm.
+#endif /* TPM_ALG_ID */
+
+/* Running Event Log Pointer */
+static uint8_t *log_ptr;
+
+/* Pointer to the first byte past end of the Event Log buffer */
+static uintptr_t log_end;
+
+/* Pointer to event_log_metadata_t */
+static const event_log_metadata_t *plat_metadata_ptr;
+
+/* TCG_EfiSpecIdEvent */
+static const id_event_headers_t id_event_header = {
+	.header = {
+		.pcr_index = PCR_0,
+		.event_type = EV_NO_ACTION,
+		.digest = {0},
+		.event_size = (uint32_t)(sizeof(id_event_struct_t) +
+				(sizeof(id_event_algorithm_size_t) *
+				HASH_ALG_COUNT))
+	},
+
+	.struct_header = {
+		.signature = TCG_ID_EVENT_SIGNATURE_03,
+		.platform_class = PLATFORM_CLASS_CLIENT,
+		.spec_version_minor = TCG_SPEC_VERSION_MINOR_TPM2,
+		.spec_version_major = TCG_SPEC_VERSION_MAJOR_TPM2,
+		.spec_errata = TCG_SPEC_ERRATA_TPM2,
+		.uintn_size = (uint8_t)(sizeof(unsigned int) /
+					sizeof(uint32_t)),
+		.number_of_algorithms = HASH_ALG_COUNT
+	}
+};
+
+static const event2_header_t locality_event_header = {
+	/*
+	 * All EV_NO_ACTION events SHALL set
+	 * TCG_PCR_EVENT2.pcrIndex = 0, unless otherwise specified
+	 */
+	.pcr_index = PCR_0,
+
+	/*
+	 * All EV_NO_ACTION events SHALL set
+	 * TCG_PCR_EVENT2.eventType = 03h
+	 */
+	.event_type = EV_NO_ACTION,
+
+	/*
+	 * All EV_NO_ACTION events SHALL set TCG_PCR_EVENT2.digests to all
+	 * 0x00's for each allocated Hash algorithm
+	 */
+	.digests = {
+		.count = HASH_ALG_COUNT
+	}
+};
+
+/*
+ * Record a measurement as a TCG_PCR_EVENT2 event
+ *
+ * @param[in] hash		Pointer to hash data of TCG_DIGEST_SIZE bytes
+ * @param[in] event_type	Type of Event, Various Event Types are
+ * 				mentioned in tcg.h header
+ * @param[in] metadata_ptr	Pointer to event_log_metadata_t structure
+ *
+ * There must be room for storing this new event into the event log buffer.
+ */
+void event_log_record(const uint8_t *hash, uint32_t event_type,
+		      const event_log_metadata_t *metadata_ptr)
+{
+	void *ptr = log_ptr;
+	uint32_t name_len = 0U;
+
+	assert(hash != NULL);
+	assert(metadata_ptr != NULL);
+	/* event_log_buf_init() must have been called prior to this. */
+	assert(log_ptr != NULL);
+
+	if (metadata_ptr->name != NULL) {
+		name_len = (uint32_t)strlen(metadata_ptr->name) + 1U;
+	}
+
+	/* Check for space in Event Log buffer */
+	assert(((uintptr_t)ptr + (uint32_t)EVENT2_HDR_SIZE + name_len) <
+	       log_end);
+
+	/*
+	 * As per TCG specifications, firmware components that are measured
+	 * into PCR[0] must be logged in the event log using the event type
+	 * EV_POST_CODE.
+	 */
+	/* TCG_PCR_EVENT2.PCRIndex */
+	((event2_header_t *)ptr)->pcr_index = metadata_ptr->pcr;
+
+	/* TCG_PCR_EVENT2.EventType */
+	((event2_header_t *)ptr)->event_type = event_type;
+
+	/* TCG_PCR_EVENT2.Digests.Count */
+	ptr = (uint8_t *)ptr + offsetof(event2_header_t, digests);
+	((tpml_digest_values *)ptr)->count = HASH_ALG_COUNT;
+
+	/* TCG_PCR_EVENT2.Digests[] */
+	ptr = (uint8_t *)((uintptr_t)ptr +
+			offsetof(tpml_digest_values, digests));
+
+	/* TCG_PCR_EVENT2.Digests[].AlgorithmId */
+	((tpmt_ha *)ptr)->algorithm_id = TPM_ALG_ID;
+
+	/* TCG_PCR_EVENT2.Digests[].Digest[] */
+	ptr = (uint8_t *)((uintptr_t)ptr + offsetof(tpmt_ha, digest));
+
+	/* Copy digest */
+	(void)memcpy(ptr, (const void *)hash, TCG_DIGEST_SIZE);
+
+	/* TCG_PCR_EVENT2.EventSize */
+	ptr = (uint8_t *)((uintptr_t)ptr + TCG_DIGEST_SIZE);
+	((event2_data_t *)ptr)->event_size = name_len;
+
+	/* Copy event data to TCG_PCR_EVENT2.Event */
+	if (metadata_ptr->name != NULL) {
+		(void)memcpy((void *)(((event2_data_t *)ptr)->event),
+				(const void *)metadata_ptr->name, name_len);
+	}
+
+	/* End of event data */
+	log_ptr = (uint8_t *)((uintptr_t)ptr +
+			offsetof(event2_data_t, event) + name_len);
+}
+
+void event_log_buf_init(uint8_t *event_log_start, uint8_t *event_log_finish)
+{
+	assert(event_log_start != NULL);
+	assert(event_log_finish > event_log_start);
+
+	log_ptr = event_log_start;
+	log_end = (uintptr_t)event_log_finish;
+}
+
+/*
+ * Initialise Event Log global variables, used during the recording
+ * of various payload measurements into the Event Log buffer
+ *
+ * @param[in] event_log_start		Base address of Event Log buffer
+ * @param[in] event_log_finish		End address of Event Log buffer,
+ * 					it is a first byte past end of the
+ * 					buffer
+ */
+void event_log_init(uint8_t *event_log_start, uint8_t *event_log_finish)
+{
+	event_log_buf_init(event_log_start, event_log_finish);
+
+	/* Get pointer to platform's event_log_metadata_t structure */
+	plat_metadata_ptr = plat_event_log_get_metadata();
+	assert(plat_metadata_ptr != NULL);
+}
+
+void event_log_write_specid_event(void)
+{
+	void *ptr = log_ptr;
+
+	/* event_log_buf_init() must have been called prior to this. */
+	assert(log_ptr != NULL);
+	assert(((uintptr_t)log_ptr + ID_EVENT_SIZE) < log_end);
+
+	/*
+	 * Add Specification ID Event first
+	 *
+	 * Copy TCG_EfiSpecIDEventStruct structure header
+	 */
+	(void)memcpy(ptr, (const void *)&id_event_header,
+			sizeof(id_event_header));
+	ptr = (uint8_t *)((uintptr_t)ptr + sizeof(id_event_header));
+
+	/* TCG_EfiSpecIdEventAlgorithmSize structure */
+	((id_event_algorithm_size_t *)ptr)->algorithm_id = TPM_ALG_ID;
+	((id_event_algorithm_size_t *)ptr)->digest_size = TCG_DIGEST_SIZE;
+	ptr = (uint8_t *)((uintptr_t)ptr + sizeof(id_event_algorithm_size_t));
+
+	/*
+	 * TCG_EfiSpecIDEventStruct.vendorInfoSize
+	 * No vendor data
+	 */
+	((id_event_struct_data_t *)ptr)->vendor_info_size = 0;
+	log_ptr = (uint8_t *)((uintptr_t)ptr +
+			offsetof(id_event_struct_data_t, vendor_info));
+}
+
+/*
+ * Initialises Event Log by writing Specification ID and
+ * Startup Locality events
+ */
+void event_log_write_header(void)
+{
+	const char locality_signature[] = TCG_STARTUP_LOCALITY_SIGNATURE;
+	void *ptr;
+
+	event_log_write_specid_event();
+
+	ptr = log_ptr;
+	assert(((uintptr_t)log_ptr + LOC_EVENT_SIZE) < log_end);
+
+	/*
+	 * The Startup Locality event should be placed in the log before
+	 * any event which extends PCR[0].
+	 *
+	 * Ref. TCG PC Client Platform Firmware Profile 9.4.5.3
+	 */
+
+	/* Copy Startup Locality Event Header */
+	(void)memcpy(ptr, (const void *)&locality_event_header,
+			sizeof(locality_event_header));
+	ptr = (uint8_t *)((uintptr_t)ptr + sizeof(locality_event_header));
+
+	/* TCG_PCR_EVENT2.Digests[].AlgorithmId */
+	((tpmt_ha *)ptr)->algorithm_id = TPM_ALG_ID;
+
+	/* TCG_PCR_EVENT2.Digests[].Digest[] */
+	(void)memset(&((tpmt_ha *)ptr)->digest, 0, TCG_DIGEST_SIZE);
+	ptr = (uint8_t *)((uintptr_t)ptr +
+			offsetof(tpmt_ha, digest) + TCG_DIGEST_SIZE);
+
+	/* TCG_PCR_EVENT2.EventSize */
+	((event2_data_t *)ptr)->event_size =
+		(uint32_t)sizeof(startup_locality_event_t);
+	ptr = (uint8_t *)((uintptr_t)ptr + offsetof(event2_data_t, event));
+
+	/* TCG_EfiStartupLocalityEvent.Signature */
+	(void)memcpy(ptr, (const void *)locality_signature,
+		sizeof(TCG_STARTUP_LOCALITY_SIGNATURE));
+
+	/*
+	 * TCG_EfiStartupLocalityEvent.StartupLocality = 0:
+	 * the platform's boot firmware
+	 */
+	((startup_locality_event_t *)ptr)->startup_locality = 0U;
+	log_ptr = (uint8_t *)((uintptr_t)ptr + sizeof(startup_locality_event_t));
+}
+
+int event_log_measure(uintptr_t data_base, uint32_t data_size,
+		      unsigned char hash_data[CRYPTO_MD_MAX_SIZE])
+{
+	/* Calculate hash */
+	return crypto_mod_calc_hash(CRYPTO_MD_ID,
+				    (void *)data_base, data_size, hash_data);
+}
+
+/*
+ * Calculate and write hash of image, configuration data, etc.
+ * to Event Log.
+ *
+ * @param[in] data_base		Address of data
+ * @param[in] data_size		Size of data
+ * @param[in] data_id		Data ID
+ * @return:
+ *	0 = success
+ *    < 0 = error
+ */
+int event_log_measure_and_record(uintptr_t data_base, uint32_t data_size,
+				 uint32_t data_id)
+{
+	unsigned char hash_data[CRYPTO_MD_MAX_SIZE];
+	int rc;
+	const event_log_metadata_t *metadata_ptr = plat_metadata_ptr;
+
+	/* Get the metadata associated with this image. */
+	while ((metadata_ptr->id != EVLOG_INVALID_ID) &&
+		(metadata_ptr->id != data_id)) {
+		metadata_ptr++;
+	}
+	assert(metadata_ptr->id != EVLOG_INVALID_ID);
+
+	/* Measure the payload with algorithm selected by EventLog driver */
+	rc = event_log_measure(data_base, data_size, hash_data);
+	if (rc != 0) {
+		return rc;
+	}
+
+	event_log_record(hash_data, EV_POST_CODE, metadata_ptr);
+
+	return 0;
+}
+
+/*
+ * Get current Event Log buffer size i.e. used space of Event Log buffer
+ *
+ * @param[in]  event_log_start		Base Pointer to Event Log buffer
+ *
+ * @return: current Size of Event Log buffer
+ */
+size_t event_log_get_cur_size(uint8_t *event_log_start)
+{
+	assert(event_log_start != NULL);
+	assert(log_ptr >= event_log_start);
+
+	return (size_t)((uintptr_t)log_ptr - (uintptr_t)event_log_start);
+}
diff --git a/drivers/measured_boot/event_log/event_log.mk b/drivers/measured_boot/event_log/event_log.mk
new file mode 100644
index 0000000..5ea4c55
--- /dev/null
+++ b/drivers/measured_boot/event_log/event_log.mk
@@ -0,0 +1,41 @@
+#
+# Copyright (c) 2020-2022, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Default log level to dump the event log (LOG_LEVEL_INFO)
+EVENT_LOG_LEVEL         ?= 40
+
+# Measured Boot hash algorithm.
+# SHA-256 (or stronger) is required for all devices that are TPM 2.0 compliant.
+ifdef TPM_HASH_ALG
+    $(warning "TPM_HASH_ALG is deprecated. Please use MBOOT_EL_HASH_ALG instead.")
+    MBOOT_EL_HASH_ALG		:=	${TPM_HASH_ALG}
+else
+    MBOOT_EL_HASH_ALG		:=	sha256
+endif
+
+ifeq (${MBOOT_EL_HASH_ALG}, sha512)
+    TPM_ALG_ID			:=	TPM_ALG_SHA512
+    TCG_DIGEST_SIZE		:=	64U
+else ifeq (${MBOOT_EL_HASH_ALG}, sha384)
+    TPM_ALG_ID			:=	TPM_ALG_SHA384
+    TCG_DIGEST_SIZE		:=	48U
+else
+    TPM_ALG_ID			:=	TPM_ALG_SHA256
+    TCG_DIGEST_SIZE		:=	32U
+endif #MBOOT_EL_HASH_ALG
+
+# Set definitions for Measured Boot driver.
+$(eval $(call add_defines,\
+    $(sort \
+        TPM_ALG_ID \
+        TCG_DIGEST_SIZE \
+        EVENT_LOG_LEVEL \
+)))
+
+EVENT_LOG_SRC_DIR	:= drivers/measured_boot/event_log/
+
+EVENT_LOG_SOURCES	:= ${EVENT_LOG_SRC_DIR}event_log.c		\
+			   ${EVENT_LOG_SRC_DIR}event_print.c
diff --git a/drivers/measured_boot/event_log/event_print.c b/drivers/measured_boot/event_log/event_print.c
new file mode 100644
index 0000000..e2ba174
--- /dev/null
+++ b/drivers/measured_boot/event_log/event_print.c
@@ -0,0 +1,265 @@
+/*
+ * Copyright (c) 2020, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/measured_boot/event_log/event_log.h>
+
+#if LOG_LEVEL >= EVENT_LOG_LEVEL
+
+/*
+ * Print TCG_EfiSpecIDEventStruct
+ *
+ * @param[in/out] log_addr	Pointer to Event Log
+ * @param[in/out] log_size	Pointer to Event Log size
+ */
+static void id_event_print(uint8_t **log_addr, size_t *log_size)
+{
+	unsigned int i;
+	uint8_t info_size, *info_size_ptr;
+	void *ptr = *log_addr;
+	id_event_headers_t *event = (id_event_headers_t *)ptr;
+	id_event_algorithm_size_t *alg_ptr;
+	uint32_t event_size, number_of_algorithms;
+	size_t digest_len;
+#if ENABLE_ASSERTIONS
+	const uint8_t *end_ptr = (uint8_t *)((uintptr_t)*log_addr + *log_size);
+	bool valid = true;
+#endif
+
+	assert(*log_size >= sizeof(id_event_headers_t));
+
+	/* The fields of the event log header are defined to be PCRIndex of 0,
+	 * EventType of EV_NO_ACTION, Digest of 20 bytes of 0, and
+	 * Event content defined as TCG_EfiSpecIDEventStruct.
+	 */
+	LOG_EVENT("TCG_EfiSpecIDEvent:\n");
+	LOG_EVENT("  PCRIndex           : %u\n", event->header.pcr_index);
+	assert(event->header.pcr_index == (uint32_t)PCR_0);
+
+	LOG_EVENT("  EventType          : %u\n", event->header.event_type);
+	assert(event->header.event_type == EV_NO_ACTION);
+
+	LOG_EVENT("  Digest             :");
+	for (i = 0U; i < sizeof(event->header.digest); ++i) {
+		uint8_t val = event->header.digest[i];
+
+		(void)printf(" %02x", val);
+		if ((i & U(0xF)) == 0U) {
+			(void)printf("\n");
+			LOG_EVENT("\t\t      :");
+		}
+#if ENABLE_ASSERTIONS
+		if (val != 0U) {
+			valid = false;
+		}
+#endif
+	}
+	if ((i & U(0xF)) != 0U) {
+		(void)printf("\n");
+	}
+
+	assert(valid);
+
+	/* EventSize */
+	event_size = event->header.event_size;
+	LOG_EVENT("  EventSize          : %u\n", event_size);
+
+	LOG_EVENT("  Signature          : %s\n",
+			event->struct_header.signature);
+	LOG_EVENT("  PlatformClass      : %u\n",
+			event->struct_header.platform_class);
+	LOG_EVENT("  SpecVersion        : %u.%u.%u\n",
+			event->struct_header.spec_version_major,
+			event->struct_header.spec_version_minor,
+			event->struct_header.spec_errata);
+	LOG_EVENT("  UintnSize          : %u\n",
+			event->struct_header.uintn_size);
+
+	/* NumberOfAlgorithms */
+	number_of_algorithms = event->struct_header.number_of_algorithms;
+	LOG_EVENT("  NumberOfAlgorithms : %u\n", number_of_algorithms);
+
+	/* Address of DigestSizes[] */
+	alg_ptr = event->struct_header.digest_size;
+
+	/* Size of DigestSizes[] */
+	digest_len = number_of_algorithms * sizeof(id_event_algorithm_size_t);
+	assert(((uintptr_t)alg_ptr + digest_len) <= (uintptr_t)end_ptr);
+
+	LOG_EVENT("  DigestSizes        :\n");
+	for (i = 0U; i < number_of_algorithms; ++i) {
+		LOG_EVENT("    #%u AlgorithmId   : SHA", i);
+		uint16_t algorithm_id = alg_ptr[i].algorithm_id;
+
+		switch (algorithm_id) {
+		case TPM_ALG_SHA256:
+			(void)printf("256\n");
+			break;
+		case TPM_ALG_SHA384:
+			(void)printf("384\n");
+			break;
+		case TPM_ALG_SHA512:
+			(void)printf("512\n");
+			break;
+		default:
+			(void)printf("?\n");
+			ERROR("Algorithm 0x%x not found\n", algorithm_id);
+			assert(false);
+		}
+
+		LOG_EVENT("       DigestSize    : %u\n",
+					alg_ptr[i].digest_size);
+	}
+
+	/* Address of VendorInfoSize */
+	info_size_ptr = (uint8_t *)((uintptr_t)alg_ptr + digest_len);
+	assert((uintptr_t)info_size_ptr <= (uintptr_t)end_ptr);
+
+	info_size = *info_size_ptr++;
+	LOG_EVENT("  VendorInfoSize     : %u\n", info_size);
+
+	/* Check VendorInfo end address */
+	assert(((uintptr_t)info_size_ptr + info_size) <= (uintptr_t)end_ptr);
+
+	/* Check EventSize */
+	assert(event_size == (sizeof(id_event_struct_t) +
+				digest_len + info_size));
+	if (info_size != 0U) {
+		LOG_EVENT("  VendorInfo         :");
+		for (i = 0U; i < info_size; ++i) {
+			(void)printf(" %02x", *info_size_ptr++);
+		}
+		(void)printf("\n");
+	}
+
+	*log_size -= (uintptr_t)info_size_ptr - (uintptr_t)*log_addr;
+	*log_addr = info_size_ptr;
+}
+
+/*
+ * Print TCG_PCR_EVENT2
+ *
+ * @param[in/out] log_addr	Pointer to Event Log
+ * @param[in/out] log_size	Pointer to Event Log size
+ */
+static void event2_print(uint8_t **log_addr, size_t *log_size)
+{
+	uint32_t event_size, count;
+	size_t sha_size, digests_size = 0U;
+	void *ptr = *log_addr;
+#if ENABLE_ASSERTIONS
+	const uint8_t *end_ptr = (uint8_t *)((uintptr_t)*log_addr + *log_size);
+#endif
+
+	assert(*log_size >= sizeof(event2_header_t));
+
+	LOG_EVENT("PCR_Event2:\n");
+	LOG_EVENT("  PCRIndex           : %u\n",
+			((event2_header_t *)ptr)->pcr_index);
+	LOG_EVENT("  EventType          : %u\n",
+			((event2_header_t *)ptr)->event_type);
+
+	count = ((event2_header_t *)ptr)->digests.count;
+	LOG_EVENT("  Digests Count      : %u\n", count);
+
+	/* Address of TCG_PCR_EVENT2.Digests[] */
+	ptr = (uint8_t *)ptr + sizeof(event2_header_t);
+	assert(((uintptr_t)ptr <= (uintptr_t)end_ptr) && (count != 0U));
+
+	for (unsigned int i = 0U; i < count; ++i) {
+		/* Check AlgorithmId address */
+		assert(((uintptr_t)ptr +
+			offsetof(tpmt_ha, digest)) <= (uintptr_t)end_ptr);
+
+		LOG_EVENT("    #%u AlgorithmId   : SHA", i);
+		switch (((tpmt_ha *)ptr)->algorithm_id) {
+		case TPM_ALG_SHA256:
+			sha_size = SHA256_DIGEST_SIZE;
+			(void)printf("256\n");
+			break;
+		case TPM_ALG_SHA384:
+			sha_size = SHA384_DIGEST_SIZE;
+			(void)printf("384\n");
+			break;
+		case TPM_ALG_SHA512:
+			sha_size = SHA512_DIGEST_SIZE;
+			(void)printf("512\n");
+			break;
+		default:
+			(void)printf("?\n");
+			ERROR("Algorithm 0x%x not found\n",
+				((tpmt_ha *)ptr)->algorithm_id);
+			panic();
+		}
+
+		/* End of Digest[] */
+		ptr = (uint8_t *)((uintptr_t)ptr + offsetof(tpmt_ha, digest));
+		assert(((uintptr_t)ptr + sha_size) <= (uintptr_t)end_ptr);
+
+		/* Total size of all digests */
+		digests_size += sha_size;
+
+		LOG_EVENT("       Digest        :");
+		for (unsigned int j = 0U; j < sha_size; ++j) {
+			(void)printf(" %02x", *(uint8_t *)ptr++);
+			if ((j & U(0xF)) == U(0xF)) {
+				(void)printf("\n");
+				if (j < (sha_size - 1U)) {
+					LOG_EVENT("\t\t      :");
+				}
+			}
+		}
+	}
+
+	/* TCG_PCR_EVENT2.EventSize */
+	assert(((uintptr_t)ptr + offsetof(event2_data_t, event)) <= (uintptr_t)end_ptr);
+
+	event_size = ((event2_data_t *)ptr)->event_size;
+	LOG_EVENT("  EventSize          : %u\n", event_size);
+
+	/* Address of TCG_PCR_EVENT2.Event[EventSize] */
+	ptr = (uint8_t *)((uintptr_t)ptr + offsetof(event2_data_t, event));
+
+	/* End of TCG_PCR_EVENT2.Event[EventSize] */
+	assert(((uintptr_t)ptr + event_size) <= (uintptr_t)end_ptr);
+
+	if ((event_size == sizeof(startup_locality_event_t)) &&
+	     (strcmp((const char *)ptr, TCG_STARTUP_LOCALITY_SIGNATURE) == 0)) {
+		LOG_EVENT("  Signature          : %s\n",
+			((startup_locality_event_t *)ptr)->signature);
+		LOG_EVENT("  StartupLocality    : %u\n",
+			((startup_locality_event_t *)ptr)->startup_locality);
+	} else {
+		LOG_EVENT("  Event              : %s\n", (uint8_t *)ptr);
+	}
+
+	*log_size -= (uintptr_t)ptr + event_size - (uintptr_t)*log_addr;
+	*log_addr = (uint8_t *)ptr + event_size;
+}
+#endif	/* LOG_LEVEL >= EVENT_LOG_LEVEL */
+
+/*
+ * Print Event Log
+ *
+ * @param[in]	log_addr	Pointer to Event Log
+ * @param[in]	log_size	Event Log size
+ */
+void dump_event_log(uint8_t *log_addr, size_t log_size)
+{
+#if LOG_LEVEL >= EVENT_LOG_LEVEL
+	assert(log_addr != NULL);
+
+	/* Print TCG_EfiSpecIDEvent */
+	id_event_print(&log_addr, &log_size);
+
+	while (log_size != 0U) {
+		event2_print(&log_addr, &log_size);
+	}
+#endif
+}
diff --git a/drivers/measured_boot/rss/rss_measured_boot.c b/drivers/measured_boot/rss/rss_measured_boot.c
new file mode 100644
index 0000000..cf545a7
--- /dev/null
+++ b/drivers/measured_boot/rss/rss_measured_boot.c
@@ -0,0 +1,137 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <assert.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/auth/crypto_mod.h>
+#include <drivers/measured_boot/rss/rss_measured_boot.h>
+#include <lib/psa/measured_boot.h>
+#include <psa/crypto_types.h>
+#include <psa/crypto_values.h>
+#include <psa/error.h>
+
+#define MBOOT_ALG_SHA512 0
+#define MBOOT_ALG_SHA384 1
+#define MBOOT_ALG_SHA256 2
+
+#if MBOOT_ALG_ID == MBOOT_ALG_SHA512
+#define	CRYPTO_MD_ID		CRYPTO_MD_SHA512
+#define PSA_CRYPTO_MD_ID	PSA_ALG_SHA_512
+#elif MBOOT_ALG_ID == MBOOT_ALG_SHA384
+#define	CRYPTO_MD_ID		CRYPTO_MD_SHA384
+#define PSA_CRYPTO_MD_ID	PSA_ALG_SHA_384
+#elif MBOOT_ALG_ID == MBOOT_ALG_SHA256
+#define	CRYPTO_MD_ID		CRYPTO_MD_SHA256
+#define PSA_CRYPTO_MD_ID	PSA_ALG_SHA_256
+#else
+#  error Invalid Measured Boot algorithm.
+#endif /* MBOOT_ALG_ID */
+
+/* Pointer to struct rss_mboot_metadata */
+static struct rss_mboot_metadata *plat_metadata_ptr;
+
+/* Functions' declarations */
+void rss_measured_boot_init(void)
+{
+	/* At this point it is expected that communication channel over MHU
+	 * is already initialised by platform init.
+	 */
+	struct rss_mboot_metadata *metadata_ptr;
+
+	/* Get pointer to platform's struct rss_mboot_metadata structure */
+	plat_metadata_ptr = plat_rss_mboot_get_metadata();
+	assert(plat_metadata_ptr != NULL);
+
+	/* Use a local variable to preserve the value of the global pointer */
+	metadata_ptr = plat_metadata_ptr;
+
+	/* Init the non-const members of the metadata structure */
+	while (metadata_ptr->id != RSS_MBOOT_INVALID_ID) {
+		metadata_ptr->sw_type_size =
+			strlen((const char *)&metadata_ptr->sw_type) + 1;
+		metadata_ptr++;
+	}
+}
+
+int rss_mboot_measure_and_record(uintptr_t data_base, uint32_t data_size,
+				 uint32_t data_id)
+{
+	unsigned char hash_data[CRYPTO_MD_MAX_SIZE];
+	int rc;
+	psa_status_t ret;
+	const struct rss_mboot_metadata *metadata_ptr = plat_metadata_ptr;
+
+	/* Get the metadata associated with this image. */
+	while ((metadata_ptr->id != RSS_MBOOT_INVALID_ID) &&
+		(metadata_ptr->id != data_id)) {
+		metadata_ptr++;
+	}
+
+	/* If image is not present in metadata array then skip */
+	if (metadata_ptr->id == RSS_MBOOT_INVALID_ID) {
+		return 0;
+	}
+
+	/* Calculate hash */
+	rc = crypto_mod_calc_hash(CRYPTO_MD_ID,
+				  (void *)data_base, data_size, hash_data);
+	if (rc != 0) {
+		return rc;
+	}
+
+	ret = rss_measured_boot_extend_measurement(
+						metadata_ptr->slot,
+						metadata_ptr->signer_id,
+						metadata_ptr->signer_id_size,
+						metadata_ptr->version,
+						metadata_ptr->version_size,
+						PSA_CRYPTO_MD_ID,
+						metadata_ptr->sw_type,
+						metadata_ptr->sw_type_size,
+						hash_data,
+						MBOOT_DIGEST_SIZE,
+						metadata_ptr->lock_measurement);
+	if (ret != PSA_SUCCESS) {
+		return ret;
+	}
+
+	return 0;
+}
+
+int rss_mboot_set_signer_id(unsigned int img_id,
+			    const void *pk_ptr,
+			    size_t pk_len)
+{
+	unsigned char hash_data[CRYPTO_MD_MAX_SIZE];
+	struct rss_mboot_metadata *metadata_ptr = plat_metadata_ptr;
+	int rc;
+
+	/* Get the metadata associated with this image. */
+	while ((metadata_ptr->id != RSS_MBOOT_INVALID_ID) &&
+		(metadata_ptr->id != img_id)) {
+		metadata_ptr++;
+	}
+
+	/* If image is not present in metadata array then skip */
+	if (metadata_ptr->id == RSS_MBOOT_INVALID_ID) {
+		return 0;
+	}
+
+	/* Calculate public key hash */
+	rc = crypto_mod_calc_hash(CRYPTO_MD_ID, (void *)pk_ptr,
+				  pk_len, hash_data);
+	if (rc != 0) {
+		return rc;
+	}
+
+	/* Update metadata struct with the received signer_id */
+	(void)memcpy(metadata_ptr->signer_id, hash_data, MBOOT_DIGEST_SIZE);
+	metadata_ptr->signer_id_size = MBOOT_DIGEST_SIZE;
+
+	return 0;
+}
diff --git a/drivers/measured_boot/rss/rss_measured_boot.mk b/drivers/measured_boot/rss/rss_measured_boot.mk
new file mode 100644
index 0000000..18ee836
--- /dev/null
+++ b/drivers/measured_boot/rss/rss_measured_boot.mk
@@ -0,0 +1,32 @@
+#
+# Copyright (c) 2022, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Hash algorithm for measured boot
+# SHA-256 (or stronger) is required.
+MBOOT_RSS_HASH_ALG		:=	sha256
+
+ifeq (${MBOOT_RSS_HASH_ALG}, sha512)
+    MBOOT_ALG_ID		:=	MBOOT_ALG_SHA512
+    MBOOT_DIGEST_SIZE		:=	64U
+else ifeq (${MBOOT_RSS_HASH_ALG}, sha384)
+    MBOOT_ALG_ID		:=	MBOOT_ALG_SHA384
+    MBOOT_DIGEST_SIZE		:=	48U
+else
+    MBOOT_ALG_ID		:=	MBOOT_ALG_SHA256
+    MBOOT_DIGEST_SIZE		:=	32U
+endif #MBOOT_RSS_HASH_ALG
+
+# Set definitions for Measured Boot driver.
+$(eval $(call add_defines,\
+    $(sort \
+        MBOOT_ALG_ID \
+        MBOOT_DIGEST_SIZE \
+        MBOOT_RSS_BACKEND \
+)))
+
+MEASURED_BOOT_SRC_DIR	:= drivers/measured_boot/rss/
+
+MEASURED_BOOT_SOURCES	+= ${MEASURED_BOOT_SRC_DIR}rss_measured_boot.c
diff --git a/drivers/mentor/i2c/mi2cv.c b/drivers/mentor/i2c/mi2cv.c
new file mode 100644
index 0000000..b0270c9
--- /dev/null
+++ b/drivers/mentor/i2c/mi2cv.c
@@ -0,0 +1,614 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ * Copyright (C) 2018 Icenowy Zheng <icenowy@aosc.io>
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+/*
+ * This driver is for Mentor Graphics Inventra MI2CV IP core, which is used
+ * for Marvell and Allwinner SoCs in ATF.
+ */
+
+#include <errno.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/mentor/mi2cv.h>
+#include <lib/mmio.h>
+
+#include <mentor_i2c_plat.h>
+
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+#define DEBUG_I2C
+#endif
+
+#define I2C_TIMEOUT_VALUE		0x500
+#define I2C_MAX_RETRY_CNT		1000
+#define I2C_CMD_WRITE			0x0
+#define I2C_CMD_READ			0x1
+
+#define I2C_DATA_ADDR_7BIT_OFFS		0x1
+#define I2C_DATA_ADDR_7BIT_MASK		(0xFF << I2C_DATA_ADDR_7BIT_OFFS)
+
+#define I2C_CONTROL_ACK			0x00000004
+#define I2C_CONTROL_IFLG		0x00000008
+#define I2C_CONTROL_STOP		0x00000010
+#define I2C_CONTROL_START		0x00000020
+#define I2C_CONTROL_TWSIEN		0x00000040
+#define I2C_CONTROL_INTEN		0x00000080
+
+#define I2C_STATUS_START			0x08
+#define I2C_STATUS_REPEATED_START		0x10
+#define I2C_STATUS_ADDR_W_ACK			0x18
+#define I2C_STATUS_DATA_W_ACK			0x28
+#define I2C_STATUS_LOST_ARB_DATA_ADDR_TRANSFER	0x38
+#define I2C_STATUS_ADDR_R_ACK			0x40
+#define I2C_STATUS_DATA_R_ACK			0x50
+#define I2C_STATUS_DATA_R_NAK			0x58
+#define I2C_STATUS_LOST_ARB_GENERAL_CALL	0x78
+#define I2C_STATUS_IDLE				0xF8
+
+#define I2C_UNSTUCK_TRIGGER			0x1
+#define I2C_UNSTUCK_ONGOING			0x2
+#define I2C_UNSTUCK_ERROR			0x4
+
+static struct mentor_i2c_regs *base;
+
+static int mentor_i2c_lost_arbitration(uint32_t *status)
+{
+	*status = mmio_read_32((uintptr_t)&base->status);
+	if ((*status == I2C_STATUS_LOST_ARB_DATA_ADDR_TRANSFER) ||
+	    (*status == I2C_STATUS_LOST_ARB_GENERAL_CALL))
+		return -EAGAIN;
+
+	return 0;
+}
+
+static void mentor_i2c_interrupt_clear(void)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32((uintptr_t)&base->control);
+#ifndef I2C_INTERRUPT_CLEAR_INVERTED
+	reg &= ~(I2C_CONTROL_IFLG);
+#else
+	reg |= I2C_CONTROL_IFLG;
+#endif
+	mmio_write_32((uintptr_t)&base->control, reg);
+	/* Wait for 1 us for the clear to take effect */
+	udelay(1);
+}
+
+static bool mentor_i2c_interrupt_get(void)
+{
+	uint32_t reg;
+
+	/* get the interrupt flag bit */
+	reg = mmio_read_32((uintptr_t)&base->control);
+	reg &= I2C_CONTROL_IFLG;
+	return (reg != 0U);
+}
+
+static int mentor_i2c_wait_interrupt(void)
+{
+	uint32_t timeout = 0;
+
+	while (!mentor_i2c_interrupt_get() && (timeout++ < I2C_TIMEOUT_VALUE))
+		;
+	if (timeout >= I2C_TIMEOUT_VALUE)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static int mentor_i2c_start_bit_set(void)
+{
+	int is_int_flag = 0;
+	uint32_t status;
+
+	if (mentor_i2c_interrupt_get())
+		is_int_flag = 1;
+
+	/* set start bit */
+	mmio_write_32((uintptr_t)&base->control,
+		      mmio_read_32((uintptr_t)&base->control) |
+		      I2C_CONTROL_START);
+
+	/* in case that the int flag was set before i.e. repeated start bit */
+	if (is_int_flag) {
+		VERBOSE("%s: repeated start Bit\n", __func__);
+		mentor_i2c_interrupt_clear();
+	}
+
+	if (mentor_i2c_wait_interrupt()) {
+		ERROR("Start clear bit timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	/* check that start bit went down */
+	if ((mmio_read_32((uintptr_t)&base->control) &
+	    I2C_CONTROL_START) != 0) {
+		ERROR("Start bit didn't went down\n");
+		return -EPERM;
+	}
+
+	/* check the status */
+	if (mentor_i2c_lost_arbitration(&status)) {
+		ERROR("%s - %d: Lost arbitration, got status %x\n",
+		      __func__, __LINE__, status);
+		return -EAGAIN;
+	}
+	if ((status != I2C_STATUS_START) &&
+	    (status != I2C_STATUS_REPEATED_START)) {
+		ERROR("Got status %x after enable start bit.\n", status);
+		return -EPERM;
+	}
+
+	return 0;
+}
+
+static int mentor_i2c_stop_bit_set(void)
+{
+	int timeout;
+	uint32_t status;
+
+	/* Generate stop bit */
+	mmio_write_32((uintptr_t)&base->control,
+		      mmio_read_32((uintptr_t)&base->control) |
+		      I2C_CONTROL_STOP);
+	mentor_i2c_interrupt_clear();
+
+	timeout = 0;
+	/* Read control register, check the control stop bit */
+	while ((mmio_read_32((uintptr_t)&base->control) & I2C_CONTROL_STOP) &&
+	       (timeout++ < I2C_TIMEOUT_VALUE))
+		;
+	if (timeout >= I2C_TIMEOUT_VALUE) {
+		ERROR("Stop bit didn't went down\n");
+		return -ETIMEDOUT;
+	}
+
+	/* check that stop bit went down */
+	if ((mmio_read_32((uintptr_t)&base->control) & I2C_CONTROL_STOP) != 0) {
+		ERROR("Stop bit didn't went down\n");
+		return -EPERM;
+	}
+
+	/* check the status */
+	if (mentor_i2c_lost_arbitration(&status)) {
+		ERROR("%s - %d: Lost arbitration, got status %x\n",
+		      __func__, __LINE__, status);
+		return -EAGAIN;
+	}
+	if (status != I2C_STATUS_IDLE) {
+		ERROR("Got status %x after enable stop bit.\n", status);
+		return -EPERM;
+	}
+
+	return 0;
+}
+
+static int mentor_i2c_address_set(uint8_t chain, int command)
+{
+	uint32_t reg, status;
+
+	reg = (chain << I2C_DATA_ADDR_7BIT_OFFS) & I2C_DATA_ADDR_7BIT_MASK;
+	reg |= command;
+	mmio_write_32((uintptr_t)&base->data, reg);
+	udelay(1);
+
+	mentor_i2c_interrupt_clear();
+
+	if (mentor_i2c_wait_interrupt()) {
+		ERROR("Start clear bit timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	/* check the status */
+	if (mentor_i2c_lost_arbitration(&status)) {
+		ERROR("%s - %d: Lost arbitration, got status %x\n",
+		      __func__, __LINE__, status);
+		return -EAGAIN;
+	}
+	if (((status != I2C_STATUS_ADDR_R_ACK) && (command == I2C_CMD_READ)) ||
+	   ((status != I2C_STATUS_ADDR_W_ACK) && (command == I2C_CMD_WRITE))) {
+		/* only in debug, since in boot we try to read the SPD
+		 * of both DRAM, and we don't want error messages in cas
+		 * DIMM doesn't exist.
+		 */
+		INFO("%s: ERROR - status %x addr in %s mode.\n", __func__,
+		     status, (command == I2C_CMD_WRITE) ? "Write" : "Read");
+		return -EPERM;
+	}
+
+	return 0;
+}
+
+/*
+ * The I2C module contains a clock divider to generate the SCL clock.
+ * This function calculates and sets the <N> and <M> fields in the I2C Baud
+ * Rate Register (t=01) to obtain given 'requested_speed'.
+ * The requested_speed will be equal to:
+ * CONFIG_SYS_TCLK / (10 * (M + 1) * (2 << N))
+ * Where M is the value represented by bits[6:3] and N is the value represented
+ * by bits[2:0] of "I2C Baud Rate Register".
+ * Therefore max M which can be set is 16 (2^4) and max N is 8 (2^3). So the
+ * lowest possible baudrate is:
+ * CONFIG_SYS_TCLK/(10 * (16 +1) * (2 << 8), which equals to:
+ * CONFIG_SYS_TCLK/87040. Assuming that CONFIG_SYS_TCLK=250MHz, the lowest
+ * possible frequency is ~2,872KHz.
+ */
+static unsigned int mentor_i2c_bus_speed_set(unsigned int requested_speed)
+{
+	unsigned int n, m, freq, margin, min_margin = 0xffffffff;
+	unsigned int actual_n = 0, actual_m = 0;
+	int val;
+
+	/* Calculate N and M for the TWSI clock baud rate */
+	for (n = 0; n < 8; n++) {
+		for (m = 0; m < 16; m++) {
+			freq = CONFIG_SYS_TCLK / (10 * (m + 1) * (2 << n));
+			val = requested_speed - freq;
+			margin = (val > 0) ? val : -val;
+
+			if ((freq <= requested_speed) &&
+			    (margin < min_margin)) {
+				min_margin = margin;
+				actual_n = n;
+				actual_m = m;
+			}
+		}
+	}
+	VERBOSE("%s: actual_n = %u, actual_m = %u\n",
+		__func__, actual_n, actual_m);
+	/* Set the baud rate */
+	mmio_write_32((uintptr_t)&base->baudrate, (actual_m << 3) | actual_n);
+
+	return 0;
+}
+
+#ifdef DEBUG_I2C
+static int mentor_i2c_probe(uint8_t chip)
+{
+	int ret = 0;
+
+	ret = mentor_i2c_start_bit_set();
+	if (ret != 0) {
+		mentor_i2c_stop_bit_set();
+		ERROR("%s - %d: %s", __func__, __LINE__,
+		      "mentor_i2c_start_bit_set failed\n");
+		return -EPERM;
+	}
+
+	ret = mentor_i2c_address_set(chip, I2C_CMD_WRITE);
+	if (ret != 0) {
+		mentor_i2c_stop_bit_set();
+		ERROR("%s - %d: %s", __func__, __LINE__,
+		      "mentor_i2c_address_set failed\n");
+		return -EPERM;
+	}
+
+	mentor_i2c_stop_bit_set();
+
+	VERBOSE("%s: successful I2C probe\n", __func__);
+
+	return ret;
+}
+#endif
+
+/* regular i2c transaction */
+static int mentor_i2c_data_receive(uint8_t *p_block, uint32_t block_size)
+{
+	uint32_t reg, status, block_size_read = block_size;
+
+	/* Wait for cause interrupt */
+	if (mentor_i2c_wait_interrupt()) {
+		ERROR("Start clear bit timeout\n");
+		return -ETIMEDOUT;
+	}
+	while (block_size_read) {
+		if (block_size_read == 1) {
+			reg = mmio_read_32((uintptr_t)&base->control);
+			reg &= ~(I2C_CONTROL_ACK);
+			mmio_write_32((uintptr_t)&base->control, reg);
+		}
+		mentor_i2c_interrupt_clear();
+
+		if (mentor_i2c_wait_interrupt()) {
+			ERROR("Start clear bit timeout\n");
+			return -ETIMEDOUT;
+		}
+		/* check the status */
+		if (mentor_i2c_lost_arbitration(&status)) {
+			ERROR("%s - %d: Lost arbitration, got status %x\n",
+			      __func__, __LINE__, status);
+			return -EAGAIN;
+		}
+		if ((status != I2C_STATUS_DATA_R_ACK) &&
+		    (block_size_read != 1)) {
+			ERROR("Status %x in read transaction\n", status);
+			return -EPERM;
+		}
+		if ((status != I2C_STATUS_DATA_R_NAK) &&
+		    (block_size_read == 1)) {
+			ERROR("Status %x in Rd Terminate\n", status);
+			return -EPERM;
+		}
+
+		/* read the data */
+		*p_block = (uint8_t) mmio_read_32((uintptr_t)&base->data);
+		VERBOSE("%s: place %d read %x\n", __func__,
+			block_size - block_size_read, *p_block);
+		p_block++;
+		block_size_read--;
+	}
+
+	return 0;
+}
+
+static int mentor_i2c_data_transmit(uint8_t *p_block, uint32_t block_size)
+{
+	uint32_t status, block_size_write = block_size;
+
+	if (mentor_i2c_wait_interrupt()) {
+		ERROR("Start clear bit timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	while (block_size_write) {
+		/* write the data */
+		mmio_write_32((uintptr_t)&base->data, (uint32_t) *p_block);
+		VERBOSE("%s: index = %d, data = %x\n", __func__,
+			block_size - block_size_write, *p_block);
+		p_block++;
+		block_size_write--;
+
+		mentor_i2c_interrupt_clear();
+
+		if (mentor_i2c_wait_interrupt()) {
+			ERROR("Start clear bit timeout\n");
+			return -ETIMEDOUT;
+		}
+
+		/* check the status */
+		if (mentor_i2c_lost_arbitration(&status)) {
+			ERROR("%s - %d: Lost arbitration, got status %x\n",
+			      __func__, __LINE__, status);
+			return -EAGAIN;
+		}
+		if (status != I2C_STATUS_DATA_W_ACK) {
+			ERROR("Status %x in write transaction\n", status);
+			return -EPERM;
+		}
+	}
+
+	return 0;
+}
+
+static int mentor_i2c_target_offset_set(uint8_t chip, uint32_t addr, int alen)
+{
+	uint8_t off_block[2];
+	uint32_t off_size;
+
+	if (alen == 2) { /* 2-byte addresses support */
+		off_block[0] = (addr >> 8) & 0xff;
+		off_block[1] = addr & 0xff;
+		off_size = 2;
+	} else { /* 1-byte addresses support */
+		off_block[0] = addr & 0xff;
+		off_size = 1;
+	}
+	VERBOSE("%s: off_size = %x addr1 = %x addr2 = %x\n", __func__,
+		off_size, off_block[0], off_block[1]);
+	return mentor_i2c_data_transmit(off_block, off_size);
+}
+
+#ifdef I2C_CAN_UNSTUCK
+static int mentor_i2c_unstuck(int ret)
+{
+	uint32_t v;
+
+	if (ret != -ETIMEDOUT)
+		return ret;
+	VERBOSE("Trying to \"unstuck i2c\"... ");
+	i2c_init(base);
+	mmio_write_32((uintptr_t)&base->unstuck, I2C_UNSTUCK_TRIGGER);
+	do {
+		v = mmio_read_32((uintptr_t)&base->unstuck);
+	} while (v & I2C_UNSTUCK_ONGOING);
+
+	if (v & I2C_UNSTUCK_ERROR) {
+		VERBOSE("failed - soft reset i2c\n");
+		ret = -EPERM;
+	} else {
+		VERBOSE("ok\n");
+		i2c_init(base);
+		ret = -EAGAIN;
+	}
+	return ret;
+}
+#else
+static int mentor_i2c_unstuck(int ret)
+{
+	VERBOSE("Cannot \"unstuck i2c\" - soft reset i2c\n");
+	return -EPERM;
+}
+#endif
+
+/*
+ * API Functions
+ */
+void i2c_init(void *i2c_base)
+{
+	/* For I2C speed and slave address, now we do not set them since
+	 * we just provide the working speed and slave address otherwhere
+	 * for i2c_init
+	 */
+	base = (struct mentor_i2c_regs *)i2c_base;
+
+	/* Reset the I2C logic */
+	mmio_write_32((uintptr_t)&base->soft_reset, 0);
+
+	udelay(200);
+
+	mentor_i2c_bus_speed_set(CONFIG_SYS_I2C_SPEED);
+
+	/* Enable the I2C and slave */
+	mmio_write_32((uintptr_t)&base->control,
+		      I2C_CONTROL_TWSIEN | I2C_CONTROL_ACK);
+
+	/* set the I2C slave address */
+	mmio_write_32((uintptr_t)&base->xtnd_slave_addr, 0);
+	mmio_write_32((uintptr_t)&base->slave_address, CONFIG_SYS_I2C_SLAVE);
+
+	/* unmask I2C interrupt */
+	mmio_write_32((uintptr_t)&base->control,
+		      mmio_read_32((uintptr_t)&base->control) |
+		      I2C_CONTROL_INTEN);
+
+	udelay(10);
+}
+
+/*
+ * i2c_read: - Read multiple bytes from an i2c device
+ *
+ * The higher level routines take into account that this function is only
+ * called with len < page length of the device (see configuration file)
+ *
+ * @chip:	address of the chip which is to be read
+ * @addr:	i2c data address within the chip
+ * @alen:	length of the i2c data address (1..2 bytes)
+ * @buffer:	where to write the data
+ * @len:	how much byte do we want to read
+ * @return:	0 in case of success
+ */
+int i2c_read(uint8_t chip, uint32_t addr, int alen, uint8_t *buffer, int len)
+{
+	int ret = 0;
+	uint32_t counter = 0;
+
+#ifdef DEBUG_I2C
+	mentor_i2c_probe(chip);
+#endif
+
+	do {
+		if (ret != -EAGAIN && ret) {
+			ERROR("i2c transaction failed, after %d retries\n",
+			      counter);
+			mentor_i2c_stop_bit_set();
+			return ret;
+		}
+
+		/* wait for 1 us for the interrupt clear to take effect */
+		if (counter > 0)
+			udelay(1);
+		counter++;
+
+		ret = mentor_i2c_start_bit_set();
+		if (ret) {
+			ret = mentor_i2c_unstuck(ret);
+			continue;
+		}
+
+		/* if EEPROM device */
+		if (alen != 0) {
+			ret = mentor_i2c_address_set(chip, I2C_CMD_WRITE);
+			if (ret)
+				continue;
+
+			ret = mentor_i2c_target_offset_set(chip, addr, alen);
+			if (ret)
+				continue;
+			ret = mentor_i2c_start_bit_set();
+			if (ret)
+				continue;
+		}
+
+		ret =  mentor_i2c_address_set(chip, I2C_CMD_READ);
+		if (ret)
+			continue;
+
+		ret = mentor_i2c_data_receive(buffer, len);
+		if (ret)
+			continue;
+
+		ret =  mentor_i2c_stop_bit_set();
+	} while ((ret == -EAGAIN) && (counter < I2C_MAX_RETRY_CNT));
+
+	if (counter == I2C_MAX_RETRY_CNT) {
+		ERROR("I2C transactions failed, got EAGAIN %d times\n",
+		      I2C_MAX_RETRY_CNT);
+		ret = -EPERM;
+	}
+	mmio_write_32((uintptr_t)&base->control,
+		      mmio_read_32((uintptr_t)&base->control) |
+		      I2C_CONTROL_ACK);
+
+	udelay(1);
+	return ret;
+}
+
+/*
+ * i2c_write: -  Write multiple bytes to an i2c device
+ *
+ * The higher level routines take into account that this function is only
+ * called with len < page length of the device (see configuration file)
+ *
+ * @chip:	address of the chip which is to be written
+ * @addr:	i2c data address within the chip
+ * @alen:	length of the i2c data address (1..2 bytes)
+ * @buffer:	where to find the data to be written
+ * @len:	how much byte do we want to read
+ * @return:	0 in case of success
+ */
+int i2c_write(uint8_t chip, uint32_t addr, int alen, uint8_t *buffer, int len)
+{
+	int ret = 0;
+	uint32_t counter = 0;
+
+	do {
+		if (ret != -EAGAIN && ret) {
+			ERROR("i2c transaction failed\n");
+			mentor_i2c_stop_bit_set();
+			return ret;
+		}
+		/* wait for 1 us for the interrupt clear to take effect */
+		if (counter > 0)
+			udelay(1);
+		counter++;
+
+		ret = mentor_i2c_start_bit_set();
+		if (ret) {
+			ret = mentor_i2c_unstuck(ret);
+			continue;
+		}
+
+		ret = mentor_i2c_address_set(chip, I2C_CMD_WRITE);
+		if (ret)
+			continue;
+
+		/* if EEPROM device */
+		if (alen != 0) {
+			ret = mentor_i2c_target_offset_set(chip, addr, alen);
+			if (ret)
+				continue;
+		}
+
+		ret = mentor_i2c_data_transmit(buffer, len);
+		if (ret)
+			continue;
+
+		ret = mentor_i2c_stop_bit_set();
+	} while ((ret == -EAGAIN) && (counter < I2C_MAX_RETRY_CNT));
+
+	if (counter == I2C_MAX_RETRY_CNT) {
+		ERROR("I2C transactions failed, got EAGAIN %d times\n",
+		      I2C_MAX_RETRY_CNT);
+		ret = -EPERM;
+	}
+
+	udelay(1);
+	return ret;
+}
diff --git a/drivers/mmc/mmc.c b/drivers/mmc/mmc.c
new file mode 100644
index 0000000..8e83464
--- /dev/null
+++ b/drivers/mmc/mmc.c
@@ -0,0 +1,858 @@
+/*
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* Define a simple and generic interface to access eMMC and SD-card devices. */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/mmc.h>
+#include <lib/utils.h>
+#include <plat/common/common_def.h>
+
+#define MMC_DEFAULT_MAX_RETRIES		5
+#define SEND_OP_COND_MAX_RETRIES	100
+
+#define MULT_BY_512K_SHIFT		19
+
+static const struct mmc_ops *ops;
+static unsigned int mmc_ocr_value;
+static struct mmc_csd_emmc mmc_csd;
+static struct sd_switch_status sd_switch_func_status;
+static unsigned char mmc_ext_csd[512] __aligned(16);
+static unsigned int mmc_flags;
+static struct mmc_device_info *mmc_dev_info;
+static unsigned int rca;
+static unsigned int scr[2]__aligned(16) = { 0 };
+
+static const unsigned char tran_speed_base[16] = {
+	0, 10, 12, 13, 15, 20, 26, 30, 35, 40, 45, 52, 55, 60, 70, 80
+};
+
+static const unsigned char sd_tran_speed_base[16] = {
+	0, 10, 12, 13, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 70, 80
+};
+
+static bool is_cmd23_enabled(void)
+{
+	return ((mmc_flags & MMC_FLAG_CMD23) != 0U);
+}
+
+static bool is_sd_cmd6_enabled(void)
+{
+	return ((mmc_flags & MMC_FLAG_SD_CMD6) != 0U);
+}
+
+static int mmc_send_cmd(unsigned int idx, unsigned int arg,
+			unsigned int r_type, unsigned int *r_data)
+{
+	struct mmc_cmd cmd;
+	int ret;
+
+	zeromem(&cmd, sizeof(struct mmc_cmd));
+
+	cmd.cmd_idx = idx;
+	cmd.cmd_arg = arg;
+	cmd.resp_type = r_type;
+
+	ret = ops->send_cmd(&cmd);
+
+	if ((ret == 0) && (r_data != NULL)) {
+		int i;
+
+		for (i = 0; i < 4; i++) {
+			*r_data = cmd.resp_data[i];
+			r_data++;
+		}
+	}
+
+	if (ret != 0) {
+		VERBOSE("Send command %u error: %d\n", idx, ret);
+	}
+
+	return ret;
+}
+
+static int mmc_device_state(void)
+{
+	int retries = MMC_DEFAULT_MAX_RETRIES;
+	unsigned int resp_data[4];
+
+	do {
+		int ret;
+
+		if (retries == 0) {
+			ERROR("CMD13 failed after %d retries\n",
+			      MMC_DEFAULT_MAX_RETRIES);
+			return -EIO;
+		}
+
+		ret = mmc_send_cmd(MMC_CMD(13), rca << RCA_SHIFT_OFFSET,
+				   MMC_RESPONSE_R1, &resp_data[0]);
+		if (ret != 0) {
+			retries--;
+			continue;
+		}
+
+		if ((resp_data[0] & STATUS_SWITCH_ERROR) != 0U) {
+			return -EIO;
+		}
+
+		retries--;
+	} while ((resp_data[0] & STATUS_READY_FOR_DATA) == 0U);
+
+	return MMC_GET_STATE(resp_data[0]);
+}
+
+static int mmc_send_part_switch_cmd(unsigned int part_config)
+{
+	int ret;
+	unsigned int part_time = 0;
+
+	ret = mmc_send_cmd(MMC_CMD(6),
+			   EXTCSD_WRITE_BYTES |
+			   EXTCSD_CMD(CMD_EXTCSD_PARTITION_CONFIG) |
+			   EXTCSD_VALUE(part_config) |
+			   EXTCSD_CMD_SET_NORMAL,
+			   MMC_RESPONSE_R1B, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Partition switch timing is in 10ms units */
+	part_time = mmc_ext_csd[CMD_EXTCSD_PART_SWITCH_TIME] * 10;
+
+	mdelay(part_time);
+
+	do {
+		ret = mmc_device_state();
+		if (ret < 0) {
+			return ret;
+		}
+	} while (ret == MMC_STATE_PRG);
+
+	return 0;
+}
+
+static int mmc_set_ext_csd(unsigned int ext_cmd, unsigned int value)
+{
+	int ret;
+
+	ret = mmc_send_cmd(MMC_CMD(6),
+			   EXTCSD_WRITE_BYTES | EXTCSD_CMD(ext_cmd) |
+			   EXTCSD_VALUE(value) | EXTCSD_CMD_SET_NORMAL,
+			   MMC_RESPONSE_R1B, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	do {
+		ret = mmc_device_state();
+		if (ret < 0) {
+			return ret;
+		}
+	} while (ret == MMC_STATE_PRG);
+
+	return 0;
+}
+
+static int mmc_sd_switch(unsigned int bus_width)
+{
+	int ret;
+	int retries = MMC_DEFAULT_MAX_RETRIES;
+	unsigned int bus_width_arg = 0;
+
+	ret = ops->prepare(0, (uintptr_t)&scr, sizeof(scr));
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* CMD55: Application Specific Command */
+	ret = mmc_send_cmd(MMC_CMD(55), rca << RCA_SHIFT_OFFSET,
+			   MMC_RESPONSE_R5, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* ACMD51: SEND_SCR */
+	do {
+		ret = mmc_send_cmd(MMC_ACMD(51), 0, MMC_RESPONSE_R1, NULL);
+		if ((ret != 0) && (retries == 0)) {
+			ERROR("ACMD51 failed after %d retries (ret=%d)\n",
+			      MMC_DEFAULT_MAX_RETRIES, ret);
+			return ret;
+		}
+
+		retries--;
+	} while (ret != 0);
+
+	ret = ops->read(0, (uintptr_t)&scr, sizeof(scr));
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (((scr[0] & SD_SCR_BUS_WIDTH_4) != 0U) &&
+	    (bus_width == MMC_BUS_WIDTH_4)) {
+		bus_width_arg = 2;
+	}
+
+	/* CMD55: Application Specific Command */
+	ret = mmc_send_cmd(MMC_CMD(55), rca << RCA_SHIFT_OFFSET,
+			   MMC_RESPONSE_R5, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* ACMD6: SET_BUS_WIDTH */
+	ret = mmc_send_cmd(MMC_ACMD(6), bus_width_arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	do {
+		ret = mmc_device_state();
+		if (ret < 0) {
+			return ret;
+		}
+	} while (ret == MMC_STATE_PRG);
+
+	return 0;
+}
+
+static int mmc_set_ios(unsigned int clk, unsigned int bus_width)
+{
+	int ret;
+	unsigned int width = bus_width;
+
+	if (mmc_dev_info->mmc_dev_type != MMC_IS_EMMC) {
+		if (width == MMC_BUS_WIDTH_8) {
+			WARN("Wrong bus config for SD-card, force to 4\n");
+			width = MMC_BUS_WIDTH_4;
+		}
+		ret = mmc_sd_switch(width);
+		if (ret != 0) {
+			return ret;
+		}
+	} else if (mmc_csd.spec_vers == 4U) {
+		ret = mmc_set_ext_csd(CMD_EXTCSD_BUS_WIDTH,
+				      (unsigned int)width);
+		if (ret != 0) {
+			return ret;
+		}
+	} else {
+		VERBOSE("Wrong MMC type or spec version\n");
+	}
+
+	return ops->set_ios(clk, width);
+}
+
+static int mmc_fill_device_info(void)
+{
+	unsigned long long c_size;
+	unsigned int speed_idx;
+	unsigned int nb_blocks;
+	unsigned int freq_unit;
+	int ret = 0;
+	struct mmc_csd_sd_v2 *csd_sd_v2;
+
+	switch (mmc_dev_info->mmc_dev_type) {
+	case MMC_IS_EMMC:
+		mmc_dev_info->block_size = MMC_BLOCK_SIZE;
+
+		ret = ops->prepare(0, (uintptr_t)&mmc_ext_csd,
+				   sizeof(mmc_ext_csd));
+		if (ret != 0) {
+			return ret;
+		}
+
+		/* MMC CMD8: SEND_EXT_CSD */
+		ret = mmc_send_cmd(MMC_CMD(8), 0, MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return ret;
+		}
+
+		ret = ops->read(0, (uintptr_t)&mmc_ext_csd,
+				sizeof(mmc_ext_csd));
+		if (ret != 0) {
+			return ret;
+		}
+
+		do {
+			ret = mmc_device_state();
+			if (ret < 0) {
+				return ret;
+			}
+		} while (ret != MMC_STATE_TRAN);
+
+		nb_blocks = (mmc_ext_csd[CMD_EXTCSD_SEC_CNT] << 0) |
+			    (mmc_ext_csd[CMD_EXTCSD_SEC_CNT + 1] << 8) |
+			    (mmc_ext_csd[CMD_EXTCSD_SEC_CNT + 2] << 16) |
+			    (mmc_ext_csd[CMD_EXTCSD_SEC_CNT + 3] << 24);
+
+		mmc_dev_info->device_size = (unsigned long long)nb_blocks *
+			mmc_dev_info->block_size;
+
+		break;
+
+	case MMC_IS_SD:
+		/*
+		 * Use the same mmc_csd struct, as required fields here
+		 * (READ_BL_LEN, C_SIZE, CSIZE_MULT) are common with eMMC.
+		 */
+		mmc_dev_info->block_size = BIT_32(mmc_csd.read_bl_len);
+
+		c_size = ((unsigned long long)mmc_csd.c_size_high << 2U) |
+			 (unsigned long long)mmc_csd.c_size_low;
+		assert(c_size != 0xFFFU);
+
+		mmc_dev_info->device_size = (c_size + 1U) *
+					    BIT_64(mmc_csd.c_size_mult + 2U) *
+					    mmc_dev_info->block_size;
+
+		break;
+
+	case MMC_IS_SD_HC:
+		assert(mmc_csd.csd_structure == 1U);
+
+		mmc_dev_info->block_size = MMC_BLOCK_SIZE;
+
+		/* Need to use mmc_csd_sd_v2 struct */
+		csd_sd_v2 = (struct mmc_csd_sd_v2 *)&mmc_csd;
+		c_size = ((unsigned long long)csd_sd_v2->c_size_high << 16) |
+			 (unsigned long long)csd_sd_v2->c_size_low;
+
+		mmc_dev_info->device_size = (c_size + 1U) << MULT_BY_512K_SHIFT;
+
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	speed_idx = (mmc_csd.tran_speed & CSD_TRAN_SPEED_MULT_MASK) >>
+			 CSD_TRAN_SPEED_MULT_SHIFT;
+
+	assert(speed_idx > 0U);
+
+	if (mmc_dev_info->mmc_dev_type == MMC_IS_EMMC) {
+		mmc_dev_info->max_bus_freq = tran_speed_base[speed_idx];
+	} else {
+		mmc_dev_info->max_bus_freq = sd_tran_speed_base[speed_idx];
+	}
+
+	freq_unit = mmc_csd.tran_speed & CSD_TRAN_SPEED_UNIT_MASK;
+	while (freq_unit != 0U) {
+		mmc_dev_info->max_bus_freq *= 10U;
+		--freq_unit;
+	}
+
+	mmc_dev_info->max_bus_freq *= 10000U;
+
+	return 0;
+}
+
+static int sd_switch(unsigned int mode, unsigned char group,
+		     unsigned char func)
+{
+	unsigned int group_shift = (group - 1U) * 4U;
+	unsigned int group_mask = GENMASK(group_shift + 3U,  group_shift);
+	unsigned int arg;
+	int ret;
+
+	ret = ops->prepare(0, (uintptr_t)&sd_switch_func_status,
+			   sizeof(sd_switch_func_status));
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* MMC CMD6: SWITCH_FUNC */
+	arg = mode | SD_SWITCH_ALL_GROUPS_MASK;
+	arg &= ~group_mask;
+	arg |= func << group_shift;
+	ret = mmc_send_cmd(MMC_CMD(6), arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return ops->read(0, (uintptr_t)&sd_switch_func_status,
+			 sizeof(sd_switch_func_status));
+}
+
+static int sd_send_op_cond(void)
+{
+	int n;
+	unsigned int resp_data[4];
+
+	for (n = 0; n < SEND_OP_COND_MAX_RETRIES; n++) {
+		int ret;
+
+		/* CMD55: Application Specific Command */
+		ret = mmc_send_cmd(MMC_CMD(55), 0, MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return ret;
+		}
+
+		/* ACMD41: SD_SEND_OP_COND */
+		ret = mmc_send_cmd(MMC_ACMD(41), OCR_HCS |
+			mmc_dev_info->ocr_voltage, MMC_RESPONSE_R3,
+			&resp_data[0]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((resp_data[0] & OCR_POWERUP) != 0U) {
+			mmc_ocr_value = resp_data[0];
+
+			if ((mmc_ocr_value & OCR_HCS) != 0U) {
+				mmc_dev_info->mmc_dev_type = MMC_IS_SD_HC;
+			} else {
+				mmc_dev_info->mmc_dev_type = MMC_IS_SD;
+			}
+
+			return 0;
+		}
+
+		mdelay(10);
+	}
+
+	ERROR("ACMD41 failed after %d retries\n", SEND_OP_COND_MAX_RETRIES);
+
+	return -EIO;
+}
+
+static int mmc_reset_to_idle(void)
+{
+	int ret;
+
+	/* CMD0: reset to IDLE */
+	ret = mmc_send_cmd(MMC_CMD(0), 0, 0, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	mdelay(2);
+
+	return 0;
+}
+
+static int mmc_send_op_cond(void)
+{
+	int ret, n;
+	unsigned int resp_data[4];
+
+	ret = mmc_reset_to_idle();
+	if (ret != 0) {
+		return ret;
+	}
+
+	for (n = 0; n < SEND_OP_COND_MAX_RETRIES; n++) {
+		ret = mmc_send_cmd(MMC_CMD(1), OCR_SECTOR_MODE |
+				   OCR_VDD_MIN_2V7 | OCR_VDD_MIN_1V7,
+				   MMC_RESPONSE_R3, &resp_data[0]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((resp_data[0] & OCR_POWERUP) != 0U) {
+			mmc_ocr_value = resp_data[0];
+			return 0;
+		}
+
+		mdelay(10);
+	}
+
+	ERROR("CMD1 failed after %d retries\n", SEND_OP_COND_MAX_RETRIES);
+
+	return -EIO;
+}
+
+static int mmc_enumerate(unsigned int clk, unsigned int bus_width)
+{
+	int ret;
+	unsigned int resp_data[4];
+
+	ops->init();
+
+	ret = mmc_reset_to_idle();
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (mmc_dev_info->mmc_dev_type == MMC_IS_EMMC) {
+		ret = mmc_send_op_cond();
+	} else {
+		/* CMD8: Send Interface Condition Command */
+		ret = mmc_send_cmd(MMC_CMD(8), VHS_2_7_3_6_V | CMD8_CHECK_PATTERN,
+				   MMC_RESPONSE_R5, &resp_data[0]);
+
+		if ((ret == 0) && ((resp_data[0] & 0xffU) == CMD8_CHECK_PATTERN)) {
+			ret = sd_send_op_cond();
+		}
+	}
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* CMD2: Card Identification */
+	ret = mmc_send_cmd(MMC_CMD(2), 0, MMC_RESPONSE_R2, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* CMD3: Set Relative Address */
+	if (mmc_dev_info->mmc_dev_type == MMC_IS_EMMC) {
+		rca = MMC_FIX_RCA;
+		ret = mmc_send_cmd(MMC_CMD(3), rca << RCA_SHIFT_OFFSET,
+				   MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return ret;
+		}
+	} else {
+		ret = mmc_send_cmd(MMC_CMD(3), 0,
+				   MMC_RESPONSE_R6, &resp_data[0]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		rca = (resp_data[0] & 0xFFFF0000U) >> 16;
+	}
+
+	/* CMD9: CSD Register */
+	ret = mmc_send_cmd(MMC_CMD(9), rca << RCA_SHIFT_OFFSET,
+			   MMC_RESPONSE_R2, &resp_data[0]);
+	if (ret != 0) {
+		return ret;
+	}
+
+	memcpy(&mmc_csd, &resp_data, sizeof(resp_data));
+
+	/* CMD7: Select Card */
+	ret = mmc_send_cmd(MMC_CMD(7), rca << RCA_SHIFT_OFFSET,
+			   MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	do {
+		ret = mmc_device_state();
+		if (ret < 0) {
+			return ret;
+		}
+	} while (ret != MMC_STATE_TRAN);
+
+	ret = mmc_set_ios(clk, bus_width);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = mmc_fill_device_info();
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (is_sd_cmd6_enabled() &&
+	    (mmc_dev_info->mmc_dev_type == MMC_IS_SD_HC)) {
+		/* Try to switch to High Speed Mode */
+		ret = sd_switch(SD_SWITCH_FUNC_CHECK, 1U, 1U);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((sd_switch_func_status.support_g1 & BIT(9)) == 0U) {
+			/* High speed not supported, keep default speed */
+			return 0;
+		}
+
+		ret = sd_switch(SD_SWITCH_FUNC_SWITCH, 1U, 1U);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((sd_switch_func_status.sel_g2_g1 & 0x1U) == 0U) {
+			/* Cannot switch to high speed, keep default speed */
+			return 0;
+		}
+
+		mmc_dev_info->max_bus_freq = 50000000U;
+		ret = ops->set_ios(clk, bus_width);
+	}
+
+	return ret;
+}
+
+size_t mmc_read_blocks(int lba, uintptr_t buf, size_t size)
+{
+	int ret;
+	unsigned int cmd_idx, cmd_arg;
+
+	assert((ops != NULL) &&
+	       (ops->read != NULL) &&
+	       (size != 0U) &&
+	       ((size & MMC_BLOCK_MASK) == 0U));
+
+	ret = ops->prepare(lba, buf, size);
+	if (ret != 0) {
+		return 0;
+	}
+
+	if (is_cmd23_enabled()) {
+		/* Set block count */
+		ret = mmc_send_cmd(MMC_CMD(23), size / MMC_BLOCK_SIZE,
+				   MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return 0;
+		}
+
+		cmd_idx = MMC_CMD(18);
+	} else {
+		if (size > MMC_BLOCK_SIZE) {
+			cmd_idx = MMC_CMD(18);
+		} else {
+			cmd_idx = MMC_CMD(17);
+		}
+	}
+
+	if (((mmc_ocr_value & OCR_ACCESS_MODE_MASK) == OCR_BYTE_MODE) &&
+	    (mmc_dev_info->mmc_dev_type != MMC_IS_SD_HC)) {
+		cmd_arg = lba * MMC_BLOCK_SIZE;
+	} else {
+		cmd_arg = lba;
+	}
+
+	ret = mmc_send_cmd(cmd_idx, cmd_arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return 0;
+	}
+
+	ret = ops->read(lba, buf, size);
+	if (ret != 0) {
+		return 0;
+	}
+
+	/* Wait buffer empty */
+	do {
+		ret = mmc_device_state();
+		if (ret < 0) {
+			return 0;
+		}
+	} while ((ret != MMC_STATE_TRAN) && (ret != MMC_STATE_DATA));
+
+	if (!is_cmd23_enabled() && (size > MMC_BLOCK_SIZE)) {
+		ret = mmc_send_cmd(MMC_CMD(12), 0, MMC_RESPONSE_R1B, NULL);
+		if (ret != 0) {
+			return 0;
+		}
+	}
+
+	return size;
+}
+
+size_t mmc_write_blocks(int lba, const uintptr_t buf, size_t size)
+{
+	int ret;
+	unsigned int cmd_idx, cmd_arg;
+
+	assert((ops != NULL) &&
+	       (ops->write != NULL) &&
+	       (size != 0U) &&
+	       ((buf & MMC_BLOCK_MASK) == 0U) &&
+	       ((size & MMC_BLOCK_MASK) == 0U));
+
+	ret = ops->prepare(lba, buf, size);
+	if (ret != 0) {
+		return 0;
+	}
+
+	if (is_cmd23_enabled()) {
+		/* Set block count */
+		ret = mmc_send_cmd(MMC_CMD(23), size / MMC_BLOCK_SIZE,
+				   MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return 0;
+		}
+
+		cmd_idx = MMC_CMD(25);
+	} else {
+		if (size > MMC_BLOCK_SIZE) {
+			cmd_idx = MMC_CMD(25);
+		} else {
+			cmd_idx = MMC_CMD(24);
+		}
+	}
+
+	if ((mmc_ocr_value & OCR_ACCESS_MODE_MASK) == OCR_BYTE_MODE) {
+		cmd_arg = lba * MMC_BLOCK_SIZE;
+	} else {
+		cmd_arg = lba;
+	}
+
+	ret = mmc_send_cmd(cmd_idx, cmd_arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return 0;
+	}
+
+	ret = ops->write(lba, buf, size);
+	if (ret != 0) {
+		return 0;
+	}
+
+	/* Wait buffer empty */
+	do {
+		ret = mmc_device_state();
+		if (ret < 0) {
+			return 0;
+		}
+	} while ((ret != MMC_STATE_TRAN) && (ret != MMC_STATE_RCV));
+
+	if (!is_cmd23_enabled() && (size > MMC_BLOCK_SIZE)) {
+		ret = mmc_send_cmd(MMC_CMD(12), 0, MMC_RESPONSE_R1B, NULL);
+		if (ret != 0) {
+			return 0;
+		}
+	}
+
+	return size;
+}
+
+size_t mmc_erase_blocks(int lba, size_t size)
+{
+	int ret;
+
+	assert(ops != NULL);
+	assert((size != 0U) && ((size & MMC_BLOCK_MASK) == 0U));
+
+	ret = mmc_send_cmd(MMC_CMD(35), lba, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return 0;
+	}
+
+	ret = mmc_send_cmd(MMC_CMD(36), lba + (size / MMC_BLOCK_SIZE) - 1U,
+			   MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return 0;
+	}
+
+	ret = mmc_send_cmd(MMC_CMD(38), lba, MMC_RESPONSE_R1B, NULL);
+	if (ret != 0) {
+		return 0;
+	}
+
+	do {
+		ret = mmc_device_state();
+		if (ret < 0) {
+			return 0;
+		}
+	} while (ret != MMC_STATE_TRAN);
+
+	return size;
+}
+
+static int mmc_part_switch(unsigned int part_type)
+{
+	uint8_t part_config = mmc_ext_csd[CMD_EXTCSD_PARTITION_CONFIG];
+
+	part_config &= ~EXT_CSD_PART_CONFIG_ACC_MASK;
+	part_config |= part_type;
+
+	return mmc_send_part_switch_cmd(part_config);
+}
+
+static unsigned char mmc_current_boot_part(void)
+{
+	return PART_CFG_CURRENT_BOOT_PARTITION(mmc_ext_csd[CMD_EXTCSD_PARTITION_CONFIG]);
+}
+
+int mmc_part_switch_current_boot(void)
+{
+	unsigned char current_boot_part = mmc_current_boot_part();
+	int ret;
+
+	if (current_boot_part != 1U &&
+	    current_boot_part != 2U) {
+		ERROR("Got unexpected value for active boot partition, %u\n", current_boot_part);
+		return -EIO;
+	}
+
+	ret = mmc_part_switch(current_boot_part);
+	if (ret < 0) {
+		ERROR("Failed to switch to boot partition, %d\n", ret);
+	}
+
+	return ret;
+}
+
+int mmc_part_switch_user(void)
+{
+	int ret;
+
+	ret = mmc_part_switch(PART_CFG_BOOT_PARTITION_NO_ACCESS);
+	if (ret < 0) {
+		ERROR("Failed to switch to user partition, %d\n", ret);
+	}
+
+	return ret;
+}
+
+size_t mmc_boot_part_size(void)
+{
+	return mmc_ext_csd[CMD_EXTCSD_BOOT_SIZE_MULT] * SZ_128K;
+}
+
+size_t mmc_boot_part_read_blocks(int lba, uintptr_t buf, size_t size)
+{
+	size_t size_read;
+	int ret;
+
+	ret = mmc_part_switch_current_boot();
+	if (ret < 0) {
+		return 0;
+	}
+
+	size_read = mmc_read_blocks(lba, buf, size);
+
+	ret = mmc_part_switch_user();
+	if (ret < 0) {
+		return 0;
+	}
+
+	return size_read;
+}
+
+int mmc_init(const struct mmc_ops *ops_ptr, unsigned int clk,
+	     unsigned int width, unsigned int flags,
+	     struct mmc_device_info *device_info)
+{
+	assert((ops_ptr != NULL) &&
+	       (ops_ptr->init != NULL) &&
+	       (ops_ptr->send_cmd != NULL) &&
+	       (ops_ptr->set_ios != NULL) &&
+	       (ops_ptr->prepare != NULL) &&
+	       (ops_ptr->read != NULL) &&
+	       (ops_ptr->write != NULL) &&
+	       (device_info != NULL) &&
+	       (clk != 0) &&
+	       ((width == MMC_BUS_WIDTH_1) ||
+		(width == MMC_BUS_WIDTH_4) ||
+		(width == MMC_BUS_WIDTH_8) ||
+		(width == MMC_BUS_WIDTH_DDR_4) ||
+		(width == MMC_BUS_WIDTH_DDR_8)));
+
+	ops = ops_ptr;
+	mmc_flags = flags;
+	mmc_dev_info = device_info;
+
+	return mmc_enumerate(clk, width);
+}
diff --git a/drivers/mtd/nand/core.c b/drivers/mtd/nand/core.c
new file mode 100644
index 0000000..6ef2256
--- /dev/null
+++ b/drivers/mtd/nand/core.c
@@ -0,0 +1,176 @@
+/*
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stddef.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/nand.h>
+#include <lib/utils.h>
+
+#include <platform_def.h>
+
+/*
+ * Define a single nand_device used by specific NAND frameworks.
+ */
+static struct nand_device nand_dev;
+
+#pragma weak plat_get_scratch_buffer
+void plat_get_scratch_buffer(void **buffer_addr, size_t *buf_size)
+{
+	static uint8_t scratch_buff[PLATFORM_MTD_MAX_PAGE_SIZE];
+
+	assert(buffer_addr != NULL);
+	assert(buf_size != NULL);
+
+	*buffer_addr = (void *)scratch_buff;
+	*buf_size = sizeof(scratch_buff);
+}
+
+int nand_read(unsigned int offset, uintptr_t buffer, size_t length,
+	      size_t *length_read)
+{
+	unsigned int block = offset / nand_dev.block_size;
+	unsigned int end_block = (offset + length - 1U) / nand_dev.block_size;
+	unsigned int page_start =
+		(offset % nand_dev.block_size) / nand_dev.page_size;
+	unsigned int nb_pages = nand_dev.block_size / nand_dev.page_size;
+	unsigned int start_offset = offset % nand_dev.page_size;
+	unsigned int page;
+	unsigned int bytes_read;
+	int is_bad;
+	int ret;
+	uint8_t *scratch_buff;
+	size_t scratch_buff_size;
+
+	plat_get_scratch_buffer((void **)&scratch_buff, &scratch_buff_size);
+
+	assert(scratch_buff != NULL);
+
+	VERBOSE("Block %u - %u, page_start %u, nb %u, length %zu, offset %u\n",
+		block, end_block, page_start, nb_pages, length, offset);
+
+	*length_read = 0UL;
+
+	if (((start_offset != 0U) || (length % nand_dev.page_size) != 0U) &&
+	    (scratch_buff_size < nand_dev.page_size)) {
+		return -EINVAL;
+	}
+
+	while (block <= end_block) {
+		is_bad = nand_dev.mtd_block_is_bad(block);
+		if (is_bad < 0) {
+			return is_bad;
+		}
+
+		if (is_bad == 1) {
+			/* Skip the block */
+			uint32_t max_block =
+				nand_dev.size / nand_dev.block_size;
+
+			block++;
+			end_block++;
+			if ((block < max_block) && (end_block < max_block)) {
+				continue;
+			}
+
+			return -EIO;
+		}
+
+		for (page = page_start; page < nb_pages; page++) {
+			if ((start_offset != 0U) ||
+			    (length < nand_dev.page_size)) {
+				ret = nand_dev.mtd_read_page(
+						&nand_dev,
+						(block * nb_pages) + page,
+						(uintptr_t)scratch_buff);
+				if (ret != 0) {
+					return ret;
+				}
+
+				bytes_read = MIN((size_t)(nand_dev.page_size -
+							  start_offset),
+						 length);
+
+				memcpy((uint8_t *)buffer,
+				       scratch_buff + start_offset,
+				       bytes_read);
+
+				start_offset = 0U;
+			} else {
+				ret = nand_dev.mtd_read_page(&nand_dev,
+						(block * nb_pages) + page,
+						buffer);
+				if (ret != 0) {
+					return ret;
+				}
+
+				bytes_read = nand_dev.page_size;
+			}
+
+			length -= bytes_read;
+			buffer += bytes_read;
+			*length_read += bytes_read;
+
+			if (length == 0U) {
+				break;
+			}
+		}
+
+		page_start = 0U;
+		block++;
+	}
+
+	return 0;
+}
+
+int nand_seek_bb(uintptr_t base, unsigned int offset, size_t *extra_offset)
+{
+	unsigned int block;
+	unsigned int offset_block;
+	unsigned int max_block;
+	int is_bad;
+	size_t count_bb = 0U;
+
+	block = base / nand_dev.block_size;
+
+	if (offset != 0U) {
+		offset_block = (base + offset - 1U) / nand_dev.block_size;
+	} else {
+		offset_block = block;
+	}
+
+	max_block = nand_dev.size / nand_dev.block_size;
+
+	while (block <= offset_block) {
+		if (offset_block >= max_block) {
+			return -EIO;
+		}
+
+		is_bad = nand_dev.mtd_block_is_bad(block);
+		if (is_bad < 0) {
+			return is_bad;
+		}
+
+		if (is_bad == 1) {
+			count_bb++;
+			offset_block++;
+		}
+
+		block++;
+	}
+
+	*extra_offset = count_bb * nand_dev.block_size;
+
+	return 0;
+}
+
+struct nand_device *get_nand_device(void)
+{
+	return &nand_dev;
+}
diff --git a/drivers/mtd/nand/raw_nand.c b/drivers/mtd/nand/raw_nand.c
new file mode 100644
index 0000000..021e30b
--- /dev/null
+++ b/drivers/mtd/nand/raw_nand.c
@@ -0,0 +1,443 @@
+/*
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stddef.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/raw_nand.h>
+#include <lib/utils.h>
+
+#include <platform_def.h>
+
+#define ONFI_SIGNATURE_ADDR	0x20U
+
+/* CRC calculation */
+#define CRC_POLYNOM		0x8005U
+#define CRC_INIT_VALUE		0x4F4EU
+
+/* Status register */
+#define NAND_STATUS_READY	BIT(6)
+
+static struct rawnand_device rawnand_dev;
+
+#pragma weak plat_get_raw_nand_data
+int plat_get_raw_nand_data(struct rawnand_device *device)
+{
+	return 0;
+}
+
+static int nand_send_cmd(uint8_t cmd, unsigned int tim)
+{
+	struct nand_req req;
+
+	zeromem(&req, sizeof(struct nand_req));
+	req.nand = rawnand_dev.nand_dev;
+	req.type = NAND_REQ_CMD | cmd;
+	req.inst_delay = tim;
+
+	return rawnand_dev.ops->exec(&req);
+}
+
+static int nand_send_addr(uint8_t addr, unsigned int tim)
+{
+	struct nand_req req;
+
+	zeromem(&req, sizeof(struct nand_req));
+	req.nand = rawnand_dev.nand_dev;
+	req.type = NAND_REQ_ADDR;
+	req.addr = &addr;
+	req.inst_delay = tim;
+
+	return rawnand_dev.ops->exec(&req);
+}
+
+static int nand_send_wait(unsigned int delay, unsigned int tim)
+{
+	struct nand_req req;
+
+	zeromem(&req, sizeof(struct nand_req));
+	req.nand = rawnand_dev.nand_dev;
+	req.type = NAND_REQ_WAIT;
+	req.inst_delay = tim;
+	req.delay_ms = delay;
+
+	return rawnand_dev.ops->exec(&req);
+}
+
+
+static int nand_read_data(uint8_t *data, unsigned int length, bool use_8bit)
+{
+	struct nand_req req;
+
+	zeromem(&req, sizeof(struct nand_req));
+	req.nand = rawnand_dev.nand_dev;
+	req.type = NAND_REQ_DATAIN | (use_8bit ? NAND_REQ_BUS_WIDTH_8 : 0U);
+	req.addr = data;
+	req.length = length;
+
+	return rawnand_dev.ops->exec(&req);
+}
+
+int nand_change_read_column_cmd(unsigned int offset, uintptr_t buffer,
+				unsigned int len)
+{
+	int ret;
+	uint8_t addr[2];
+	unsigned int i;
+
+	ret = nand_send_cmd(NAND_CMD_CHANGE_1ST, 0U);
+	if (ret !=  0) {
+		return ret;
+	}
+
+	if (rawnand_dev.nand_dev->buswidth == NAND_BUS_WIDTH_16) {
+		offset /= 2U;
+	}
+
+	addr[0] = offset;
+	addr[1] = offset >> 8;
+
+	for (i = 0; i < 2U; i++) {
+		ret = nand_send_addr(addr[i], 0U);
+		if (ret !=  0) {
+			return ret;
+		}
+	}
+
+	ret = nand_send_cmd(NAND_CMD_CHANGE_2ND, NAND_TCCS_MIN);
+	if (ret !=  0) {
+		return ret;
+	}
+
+	return nand_read_data((uint8_t *)buffer, len, false);
+}
+
+int nand_read_page_cmd(unsigned int page, unsigned int offset,
+		       uintptr_t buffer, unsigned int len)
+{
+	uint8_t addr[5];
+	uint8_t i = 0U;
+	uint8_t j;
+	int ret;
+
+	VERBOSE(">%s page %u offset %u buffer 0x%lx\n", __func__, page, offset,
+		buffer);
+
+	if (rawnand_dev.nand_dev->buswidth == NAND_BUS_WIDTH_16) {
+		offset /= 2U;
+	}
+
+	addr[i++] = offset;
+	addr[i++] = offset >> 8;
+
+	addr[i++] = page;
+	addr[i++] = page >> 8;
+	if (rawnand_dev.nand_dev->size > SZ_128M) {
+		addr[i++] = page >> 16;
+	}
+
+	ret = nand_send_cmd(NAND_CMD_READ_1ST, 0U);
+	if (ret != 0) {
+		return ret;
+	}
+
+	for (j = 0U; j < i; j++) {
+		ret = nand_send_addr(addr[j], 0U);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	ret = nand_send_cmd(NAND_CMD_READ_2ND, NAND_TWB_MAX);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = nand_send_wait(PSEC_TO_MSEC(NAND_TR_MAX), NAND_TRR_MIN);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (buffer != 0U) {
+		ret = nand_read_data((uint8_t *)buffer, len, false);
+	}
+
+	return ret;
+}
+
+static int nand_status(uint8_t *status)
+{
+	int ret;
+
+	ret = nand_send_cmd(NAND_CMD_STATUS, NAND_TWHR_MIN);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (status != NULL) {
+		ret = nand_read_data(status, 1U, true);
+	}
+
+	return ret;
+}
+
+int nand_wait_ready(unsigned int delay_ms)
+{
+	uint8_t status;
+	int ret;
+	uint64_t timeout;
+
+	/* Wait before reading status */
+	udelay(1);
+
+	ret = nand_status(NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	timeout = timeout_init_us(delay_ms * 1000U);
+	while (!timeout_elapsed(timeout)) {
+		ret = nand_read_data(&status, 1U, true);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((status & NAND_STATUS_READY) != 0U) {
+			return nand_send_cmd(NAND_CMD_READ_1ST, 0U);
+		}
+
+		udelay(10);
+	}
+
+	return -ETIMEDOUT;
+}
+
+#if NAND_ONFI_DETECT
+static uint16_t nand_check_crc(uint16_t crc, uint8_t *data_in,
+			       unsigned int data_len)
+{
+	uint32_t i;
+	uint32_t j;
+	uint32_t bit;
+
+	for (i = 0U; i < data_len; i++) {
+		uint8_t cur_param = *data_in++;
+
+		for (j = BIT(7); j != 0U; j >>= 1) {
+			bit = crc & BIT(15);
+			crc <<= 1;
+
+			if ((cur_param & j) != 0U) {
+				bit ^= BIT(15);
+			}
+
+			if (bit != 0U) {
+				crc ^= CRC_POLYNOM;
+			}
+		}
+
+		crc &= GENMASK(15, 0);
+	}
+
+	return crc;
+}
+
+static int nand_read_id(uint8_t addr, uint8_t *id, unsigned int size)
+{
+	int ret;
+
+	ret = nand_send_cmd(NAND_CMD_READID, 0U);
+	if (ret !=  0) {
+		return ret;
+	}
+
+	ret = nand_send_addr(addr, NAND_TWHR_MIN);
+	if (ret !=  0) {
+		return ret;
+	}
+
+	return nand_read_data(id, size, true);
+}
+
+static int nand_reset(void)
+{
+	int ret;
+
+	ret = nand_send_cmd(NAND_CMD_RESET, NAND_TWB_MAX);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return nand_send_wait(PSEC_TO_MSEC(NAND_TRST_MAX), 0U);
+}
+
+static int nand_read_param_page(void)
+{
+	struct nand_param_page page;
+	uint8_t addr = 0U;
+	int ret;
+
+	ret = nand_send_cmd(NAND_CMD_READ_PARAM_PAGE, 0U);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = nand_send_addr(addr, NAND_TWB_MAX);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = nand_send_wait(PSEC_TO_MSEC(NAND_TR_MAX), NAND_TRR_MIN);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = nand_read_data((uint8_t *)&page, sizeof(page), true);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (strncmp((char *)&page.page_sig, "ONFI", 4) != 0) {
+		WARN("Error ONFI detection\n");
+		return -EINVAL;
+	}
+
+	if (nand_check_crc(CRC_INIT_VALUE, (uint8_t *)&page, 254U) !=
+	    page.crc16) {
+		WARN("Error reading param\n");
+		return -EINVAL;
+	}
+
+	if ((page.features & ONFI_FEAT_BUS_WIDTH_16) != 0U) {
+		rawnand_dev.nand_dev->buswidth = NAND_BUS_WIDTH_16;
+	} else {
+		rawnand_dev.nand_dev->buswidth = NAND_BUS_WIDTH_8;
+	}
+
+	rawnand_dev.nand_dev->block_size = page.num_pages_per_blk *
+					   page.bytes_per_page;
+	rawnand_dev.nand_dev->page_size = page.bytes_per_page;
+	rawnand_dev.nand_dev->size = page.num_pages_per_blk *
+				     page.bytes_per_page *
+				     page.num_blk_in_lun * page.num_lun;
+
+	if (page.nb_ecc_bits != GENMASK_32(7, 0)) {
+		rawnand_dev.nand_dev->ecc.max_bit_corr = page.nb_ecc_bits;
+		rawnand_dev.nand_dev->ecc.size = SZ_512;
+	}
+
+	VERBOSE("Page size %u, block_size %u, Size %llu, ecc %u, buswidth %u\n",
+		rawnand_dev.nand_dev->page_size,
+		rawnand_dev.nand_dev->block_size, rawnand_dev.nand_dev->size,
+		rawnand_dev.nand_dev->ecc.max_bit_corr,
+		rawnand_dev.nand_dev->buswidth);
+
+	return 0;
+}
+
+static int detect_onfi(void)
+{
+	int ret;
+	char id[4];
+
+	ret = nand_reset();
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = nand_read_id(ONFI_SIGNATURE_ADDR, (uint8_t *)id, sizeof(id));
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (strncmp(id, "ONFI", sizeof(id)) != 0) {
+		WARN("NAND Non ONFI detected\n");
+		return -ENODEV;
+	}
+
+	return nand_read_param_page();
+}
+#endif
+
+static int nand_mtd_block_is_bad(unsigned int block)
+{
+	unsigned int nbpages_per_block = rawnand_dev.nand_dev->block_size /
+					 rawnand_dev.nand_dev->page_size;
+	uint8_t bbm_marker[2];
+	uint8_t page;
+	int ret;
+
+	for (page = 0U; page < 2U; page++) {
+		ret = nand_read_page_cmd(block * nbpages_per_block,
+					 rawnand_dev.nand_dev->page_size,
+					 (uintptr_t)bbm_marker,
+					 sizeof(bbm_marker));
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((bbm_marker[0] != GENMASK_32(7, 0)) ||
+		    (bbm_marker[1] != GENMASK_32(7, 0))) {
+			WARN("Block %u is bad\n", block);
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+static int nand_mtd_read_page_raw(struct nand_device *nand, unsigned int page,
+				  uintptr_t buffer)
+{
+	return nand_read_page_cmd(page, 0U, buffer,
+				  rawnand_dev.nand_dev->page_size);
+}
+
+void nand_raw_ctrl_init(const struct nand_ctrl_ops *ops)
+{
+	rawnand_dev.ops = ops;
+}
+
+int nand_raw_init(unsigned long long *size, unsigned int *erase_size)
+{
+	rawnand_dev.nand_dev = get_nand_device();
+	if (rawnand_dev.nand_dev == NULL) {
+		return -EINVAL;
+	}
+
+	rawnand_dev.nand_dev->mtd_block_is_bad = nand_mtd_block_is_bad;
+	rawnand_dev.nand_dev->mtd_read_page = nand_mtd_read_page_raw;
+	rawnand_dev.nand_dev->ecc.mode = NAND_ECC_NONE;
+
+	if ((rawnand_dev.ops->setup == NULL) ||
+	    (rawnand_dev.ops->exec == NULL)) {
+		return -ENODEV;
+	}
+
+#if NAND_ONFI_DETECT
+	if (detect_onfi() != 0) {
+		WARN("Detect ONFI failed\n");
+	}
+#endif
+
+	if (plat_get_raw_nand_data(&rawnand_dev) != 0) {
+		return -EINVAL;
+	}
+
+	assert((rawnand_dev.nand_dev->page_size != 0U) &&
+	       (rawnand_dev.nand_dev->block_size != 0U) &&
+	       (rawnand_dev.nand_dev->size != 0U));
+
+	*size = rawnand_dev.nand_dev->size;
+	*erase_size = rawnand_dev.nand_dev->block_size;
+
+	rawnand_dev.ops->setup(rawnand_dev.nand_dev);
+
+	return 0;
+}
diff --git a/drivers/mtd/nand/spi_nand.c b/drivers/mtd/nand/spi_nand.c
new file mode 100644
index 0000000..542b614
--- /dev/null
+++ b/drivers/mtd/nand/spi_nand.c
@@ -0,0 +1,324 @@
+/*
+ * Copyright (c) 2019-2022,  STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stddef.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/spi_nand.h>
+#include <lib/utils.h>
+
+#include <platform_def.h>
+
+#define SPI_NAND_MAX_ID_LEN		4U
+#define DELAY_US_400MS			400000U
+#define MACRONIX_ID			0xC2U
+
+static struct spinand_device spinand_dev;
+
+#pragma weak plat_get_spi_nand_data
+int plat_get_spi_nand_data(struct spinand_device *device)
+{
+	return 0;
+}
+
+static int spi_nand_reg(bool read_reg, uint8_t reg, uint8_t *val,
+			enum spi_mem_data_dir dir)
+{
+	struct spi_mem_op op;
+
+	zeromem(&op, sizeof(struct spi_mem_op));
+	if (read_reg) {
+		op.cmd.opcode = SPI_NAND_OP_GET_FEATURE;
+	} else {
+		op.cmd.opcode = SPI_NAND_OP_SET_FEATURE;
+	}
+
+	op.cmd.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	op.addr.val = reg;
+	op.addr.nbytes = 1U;
+	op.addr.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	op.data.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	op.data.dir = dir;
+	op.data.nbytes = 1U;
+	op.data.buf = val;
+
+	return spi_mem_exec_op(&op);
+}
+
+static int spi_nand_read_reg(uint8_t reg, uint8_t *val)
+{
+	return spi_nand_reg(true, reg, val, SPI_MEM_DATA_IN);
+}
+
+static int spi_nand_write_reg(uint8_t reg, uint8_t val)
+{
+	return spi_nand_reg(false, reg, &val, SPI_MEM_DATA_OUT);
+}
+
+static int spi_nand_update_cfg(uint8_t mask, uint8_t val)
+{
+	int ret;
+	uint8_t cfg = spinand_dev.cfg_cache;
+
+	cfg &= ~mask;
+	cfg |= val;
+
+	if (cfg == spinand_dev.cfg_cache) {
+		return 0;
+	}
+
+	ret = spi_nand_write_reg(SPI_NAND_REG_CFG, cfg);
+	if (ret == 0) {
+		spinand_dev.cfg_cache = cfg;
+	}
+
+	return ret;
+}
+
+static int spi_nand_ecc_enable(bool enable)
+{
+	return spi_nand_update_cfg(SPI_NAND_CFG_ECC_EN,
+				   enable ? SPI_NAND_CFG_ECC_EN : 0U);
+}
+
+static int spi_nand_quad_enable(uint8_t manufacturer_id)
+{
+	bool enable = false;
+
+	if (manufacturer_id != MACRONIX_ID) {
+		return 0;
+	}
+
+	if (spinand_dev.spi_read_cache_op.data.buswidth ==
+	    SPI_MEM_BUSWIDTH_4_LINE) {
+		enable = true;
+	}
+
+	return spi_nand_update_cfg(SPI_NAND_CFG_QE,
+				   enable ? SPI_NAND_CFG_QE : 0U);
+}
+
+static int spi_nand_wait_ready(uint8_t *status)
+{
+	int ret;
+	uint64_t timeout = timeout_init_us(DELAY_US_400MS);
+
+	while (!timeout_elapsed(timeout)) {
+		ret = spi_nand_read_reg(SPI_NAND_REG_STATUS, status);
+		if (ret != 0) {
+			return ret;
+		}
+
+		VERBOSE("%s Status %x\n", __func__, *status);
+		if ((*status & SPI_NAND_STATUS_BUSY) == 0U) {
+			return 0;
+		}
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int spi_nand_reset(void)
+{
+	struct spi_mem_op op;
+	uint8_t status;
+	int ret;
+
+	zeromem(&op, sizeof(struct spi_mem_op));
+	op.cmd.opcode = SPI_NAND_OP_RESET;
+	op.cmd.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+
+	ret = spi_mem_exec_op(&op);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return spi_nand_wait_ready(&status);
+}
+
+static int spi_nand_read_id(uint8_t *id)
+{
+	struct spi_mem_op op;
+
+	zeromem(&op, sizeof(struct spi_mem_op));
+	op.cmd.opcode = SPI_NAND_OP_READ_ID;
+	op.cmd.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	op.data.dir = SPI_MEM_DATA_IN;
+	op.data.nbytes = SPI_NAND_MAX_ID_LEN;
+	op.data.buf = id;
+	op.data.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+
+	return spi_mem_exec_op(&op);
+}
+
+static int spi_nand_load_page(unsigned int page)
+{
+	struct spi_mem_op op;
+	uint32_t block_nb = page / spinand_dev.nand_dev->block_size;
+	uint32_t page_nb = page - (block_nb * spinand_dev.nand_dev->page_size);
+	uint32_t nbpages_per_block = spinand_dev.nand_dev->block_size /
+				     spinand_dev.nand_dev->page_size;
+	uint32_t block_sh = __builtin_ctz(nbpages_per_block) + 1U;
+
+	zeromem(&op, sizeof(struct spi_mem_op));
+	op.cmd.opcode = SPI_NAND_OP_LOAD_PAGE;
+	op.cmd.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	op.addr.val = (block_nb << block_sh) | page_nb;
+	op.addr.nbytes = 3U;
+	op.addr.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+
+	return spi_mem_exec_op(&op);
+}
+
+static int spi_nand_read_from_cache(unsigned int page, unsigned int offset,
+				    uint8_t *buffer, unsigned int len)
+{
+	uint32_t nbpages_per_block = spinand_dev.nand_dev->block_size /
+				     spinand_dev.nand_dev->page_size;
+	uint32_t block_nb = page / nbpages_per_block;
+	uint32_t page_sh = __builtin_ctz(spinand_dev.nand_dev->page_size) + 1U;
+
+	spinand_dev.spi_read_cache_op.addr.val = offset;
+
+	if ((spinand_dev.nand_dev->nb_planes > 1U) && ((block_nb % 2U) == 1U)) {
+		spinand_dev.spi_read_cache_op.addr.val |= 1U << page_sh;
+	}
+
+	spinand_dev.spi_read_cache_op.data.buf = buffer;
+	spinand_dev.spi_read_cache_op.data.nbytes = len;
+
+	return spi_mem_exec_op(&spinand_dev.spi_read_cache_op);
+}
+
+static int spi_nand_read_page(unsigned int page, unsigned int offset,
+			      uint8_t *buffer, unsigned int len,
+			      bool ecc_enabled)
+{
+	uint8_t status;
+	int ret;
+
+	ret = spi_nand_ecc_enable(ecc_enabled);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = spi_nand_load_page(page);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = spi_nand_wait_ready(&status);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = spi_nand_read_from_cache(page, offset, buffer, len);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (ecc_enabled && ((status & SPI_NAND_STATUS_ECC_UNCOR) != 0U)) {
+		return -EBADMSG;
+	}
+
+	return 0;
+}
+
+static int spi_nand_mtd_block_is_bad(unsigned int block)
+{
+	unsigned int nbpages_per_block = spinand_dev.nand_dev->block_size /
+					 spinand_dev.nand_dev->page_size;
+	uint8_t bbm_marker[2];
+	int ret;
+
+	ret = spi_nand_read_page(block * nbpages_per_block,
+				 spinand_dev.nand_dev->page_size,
+				 bbm_marker, sizeof(bbm_marker), false);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((bbm_marker[0] != GENMASK_32(7, 0)) ||
+	    (bbm_marker[1] != GENMASK_32(7, 0))) {
+		WARN("Block %u is bad\n", block);
+		return 1;
+	}
+
+	return 0;
+}
+
+static int spi_nand_mtd_read_page(struct nand_device *nand, unsigned int page,
+				  uintptr_t buffer)
+{
+	return spi_nand_read_page(page, 0, (uint8_t *)buffer,
+				  spinand_dev.nand_dev->page_size, true);
+}
+
+int spi_nand_init(unsigned long long *size, unsigned int *erase_size)
+{
+	uint8_t id[SPI_NAND_MAX_ID_LEN];
+	int ret;
+
+	spinand_dev.nand_dev = get_nand_device();
+	if (spinand_dev.nand_dev == NULL) {
+		return -EINVAL;
+	}
+
+	spinand_dev.nand_dev->mtd_block_is_bad = spi_nand_mtd_block_is_bad;
+	spinand_dev.nand_dev->mtd_read_page = spi_nand_mtd_read_page;
+	spinand_dev.nand_dev->nb_planes = 1;
+
+	spinand_dev.spi_read_cache_op.cmd.opcode = SPI_NAND_OP_READ_FROM_CACHE;
+	spinand_dev.spi_read_cache_op.cmd.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	spinand_dev.spi_read_cache_op.addr.nbytes = 2U;
+	spinand_dev.spi_read_cache_op.addr.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	spinand_dev.spi_read_cache_op.dummy.nbytes = 1U;
+	spinand_dev.spi_read_cache_op.dummy.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	spinand_dev.spi_read_cache_op.data.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+
+	if (plat_get_spi_nand_data(&spinand_dev) != 0) {
+		return -EINVAL;
+	}
+
+	assert((spinand_dev.nand_dev->page_size != 0U) &&
+	       (spinand_dev.nand_dev->block_size != 0U) &&
+	       (spinand_dev.nand_dev->size != 0U));
+
+	ret = spi_nand_reset();
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = spi_nand_read_id(id);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = spi_nand_read_reg(SPI_NAND_REG_CFG, &spinand_dev.cfg_cache);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = spi_nand_quad_enable(id[1]);
+	if (ret != 0) {
+		return ret;
+	}
+
+	VERBOSE("SPI_NAND Detected ID 0x%x\n", id[1]);
+
+	VERBOSE("Page size %u, Block size %u, size %llu\n",
+		spinand_dev.nand_dev->page_size,
+		spinand_dev.nand_dev->block_size,
+		spinand_dev.nand_dev->size);
+
+	*size = spinand_dev.nand_dev->size;
+	*erase_size = spinand_dev.nand_dev->block_size;
+
+	return 0;
+}
diff --git a/drivers/mtd/nor/spi_nor.c b/drivers/mtd/nor/spi_nor.c
new file mode 100644
index 0000000..2e34344
--- /dev/null
+++ b/drivers/mtd/nor/spi_nor.c
@@ -0,0 +1,387 @@
+/*
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stddef.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/spi_nor.h>
+#include <lib/utils.h>
+
+#define SR_WIP			BIT(0)	/* Write in progress */
+#define CR_QUAD_EN_SPAN		BIT(1)	/* Spansion Quad I/O */
+#define SR_QUAD_EN_MX		BIT(6)	/* Macronix Quad I/O */
+#define FSR_READY		BIT(7)	/* Device status, 0 = Busy, 1 = Ready */
+
+/* Defined IDs for supported memories */
+#define SPANSION_ID		0x01U
+#define MACRONIX_ID		0xC2U
+#define MICRON_ID		0x2CU
+
+#define BANK_SIZE		0x1000000U
+
+#define SPI_READY_TIMEOUT_US	40000U
+
+static struct nor_device nor_dev;
+
+#pragma weak plat_get_nor_data
+int plat_get_nor_data(struct nor_device *device)
+{
+	return 0;
+}
+
+static int spi_nor_reg(uint8_t reg, uint8_t *buf, size_t len,
+		       enum spi_mem_data_dir dir)
+{
+	struct spi_mem_op op;
+
+	zeromem(&op, sizeof(struct spi_mem_op));
+	op.cmd.opcode = reg;
+	op.cmd.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	op.data.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	op.data.dir = dir;
+	op.data.nbytes = len;
+	op.data.buf = buf;
+
+	return spi_mem_exec_op(&op);
+}
+
+static inline int spi_nor_read_id(uint8_t *id)
+{
+	return spi_nor_reg(SPI_NOR_OP_READ_ID, id, 1U, SPI_MEM_DATA_IN);
+}
+
+static inline int spi_nor_read_cr(uint8_t *cr)
+{
+	return spi_nor_reg(SPI_NOR_OP_READ_CR, cr, 1U, SPI_MEM_DATA_IN);
+}
+
+static inline int spi_nor_read_sr(uint8_t *sr)
+{
+	return spi_nor_reg(SPI_NOR_OP_READ_SR, sr, 1U, SPI_MEM_DATA_IN);
+}
+
+static inline int spi_nor_read_fsr(uint8_t *fsr)
+{
+	return spi_nor_reg(SPI_NOR_OP_READ_FSR, fsr, 1U, SPI_MEM_DATA_IN);
+}
+
+static inline int spi_nor_write_en(void)
+{
+	return spi_nor_reg(SPI_NOR_OP_WREN, NULL, 0U, SPI_MEM_DATA_OUT);
+}
+
+/*
+ * Check if device is ready.
+ *
+ * Return 0 if ready, 1 if busy or a negative error code otherwise
+ */
+static int spi_nor_ready(void)
+{
+	uint8_t sr;
+	int ret;
+
+	ret = spi_nor_read_sr(&sr);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((nor_dev.flags & SPI_NOR_USE_FSR) != 0U) {
+		uint8_t fsr;
+
+		ret = spi_nor_read_fsr(&fsr);
+		if (ret != 0) {
+			return ret;
+		}
+
+		return (((fsr & FSR_READY) != 0U) && ((sr & SR_WIP) == 0U)) ?
+			0 : 1;
+	}
+
+	return (((sr & SR_WIP) == 0U) ? 0 : 1);
+}
+
+static int spi_nor_wait_ready(void)
+{
+	int ret;
+	uint64_t timeout = timeout_init_us(SPI_READY_TIMEOUT_US);
+
+	while (!timeout_elapsed(timeout)) {
+		ret = spi_nor_ready();
+		if (ret <= 0) {
+			return ret;
+		}
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int spi_nor_macronix_quad_enable(void)
+{
+	uint8_t sr;
+	int ret;
+
+	ret = spi_nor_read_sr(&sr);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((sr & SR_QUAD_EN_MX) != 0U) {
+		return 0;
+	}
+
+	ret = spi_nor_write_en();
+	if (ret != 0) {
+		return ret;
+	}
+
+	sr |= SR_QUAD_EN_MX;
+	ret = spi_nor_reg(SPI_NOR_OP_WRSR, &sr, 1U, SPI_MEM_DATA_OUT);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = spi_nor_wait_ready();
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = spi_nor_read_sr(&sr);
+	if ((ret != 0) || ((sr & SR_QUAD_EN_MX) == 0U)) {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int spi_nor_write_sr_cr(uint8_t *sr_cr)
+{
+	int ret;
+
+	ret = spi_nor_write_en();
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = spi_nor_reg(SPI_NOR_OP_WRSR, sr_cr, 2U, SPI_MEM_DATA_OUT);
+	if (ret != 0) {
+		return -EINVAL;
+	}
+
+	ret = spi_nor_wait_ready();
+	if (ret != 0) {
+		return ret;
+	}
+
+	return 0;
+}
+
+static int spi_nor_quad_enable(void)
+{
+	uint8_t sr_cr[2];
+	int ret;
+
+	ret = spi_nor_read_cr(&sr_cr[1]);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((sr_cr[1] & CR_QUAD_EN_SPAN) != 0U) {
+		return 0;
+	}
+
+	sr_cr[1] |= CR_QUAD_EN_SPAN;
+	ret = spi_nor_read_sr(&sr_cr[0]);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = spi_nor_write_sr_cr(sr_cr);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = spi_nor_read_cr(&sr_cr[1]);
+	if ((ret != 0) || ((sr_cr[1] & CR_QUAD_EN_SPAN) == 0U)) {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int spi_nor_clean_bar(void)
+{
+	int ret;
+
+	if (nor_dev.selected_bank == 0U) {
+		return 0;
+	}
+
+	nor_dev.selected_bank = 0U;
+
+	ret = spi_nor_write_en();
+	if (ret != 0) {
+		return ret;
+	}
+
+	return spi_nor_reg(nor_dev.bank_write_cmd, &nor_dev.selected_bank,
+			   1U, SPI_MEM_DATA_OUT);
+}
+
+static int spi_nor_write_bar(uint32_t offset)
+{
+	uint8_t selected_bank = offset / BANK_SIZE;
+	int ret;
+
+	if (selected_bank == nor_dev.selected_bank) {
+		return 0;
+	}
+
+	ret = spi_nor_write_en();
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = spi_nor_reg(nor_dev.bank_write_cmd, &selected_bank,
+			  1U, SPI_MEM_DATA_OUT);
+	if (ret != 0) {
+		return ret;
+	}
+
+	nor_dev.selected_bank = selected_bank;
+
+	return 0;
+}
+
+static int spi_nor_read_bar(void)
+{
+	uint8_t selected_bank = 0U;
+	int ret;
+
+	ret = spi_nor_reg(nor_dev.bank_read_cmd, &selected_bank,
+			  1U, SPI_MEM_DATA_IN);
+	if (ret != 0) {
+		return ret;
+	}
+
+	nor_dev.selected_bank = selected_bank;
+
+	return 0;
+}
+
+int spi_nor_read(unsigned int offset, uintptr_t buffer, size_t length,
+		 size_t *length_read)
+{
+	size_t remain_len;
+	int ret;
+
+	*length_read = 0U;
+	nor_dev.read_op.addr.val = offset;
+	nor_dev.read_op.data.buf = (void *)buffer;
+
+	VERBOSE("%s offset %u length %zu\n", __func__, offset, length);
+
+	while (length != 0U) {
+		if ((nor_dev.flags & SPI_NOR_USE_BANK) != 0U) {
+			ret = spi_nor_write_bar(nor_dev.read_op.addr.val);
+			if (ret != 0) {
+				return ret;
+			}
+
+			remain_len = (BANK_SIZE * (nor_dev.selected_bank + 1)) -
+				nor_dev.read_op.addr.val;
+			nor_dev.read_op.data.nbytes = MIN(length, remain_len);
+		} else {
+			nor_dev.read_op.data.nbytes = length;
+		}
+
+		ret = spi_mem_exec_op(&nor_dev.read_op);
+		if (ret != 0) {
+			spi_nor_clean_bar();
+			return ret;
+		}
+
+		length -= nor_dev.read_op.data.nbytes;
+		nor_dev.read_op.addr.val += nor_dev.read_op.data.nbytes;
+		nor_dev.read_op.data.buf += nor_dev.read_op.data.nbytes;
+		*length_read += nor_dev.read_op.data.nbytes;
+	}
+
+	if ((nor_dev.flags & SPI_NOR_USE_BANK) != 0U) {
+		ret = spi_nor_clean_bar();
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+int spi_nor_init(unsigned long long *size, unsigned int *erase_size)
+{
+	int ret;
+	uint8_t id;
+
+	/* Default read command used */
+	nor_dev.read_op.cmd.opcode = SPI_NOR_OP_READ;
+	nor_dev.read_op.cmd.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	nor_dev.read_op.addr.nbytes = 3U;
+	nor_dev.read_op.addr.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	nor_dev.read_op.data.buswidth = SPI_MEM_BUSWIDTH_1_LINE;
+	nor_dev.read_op.data.dir = SPI_MEM_DATA_IN;
+
+	if (plat_get_nor_data(&nor_dev) != 0) {
+		return -EINVAL;
+	}
+
+	assert(nor_dev.size != 0U);
+
+	if (nor_dev.size > BANK_SIZE) {
+		nor_dev.flags |= SPI_NOR_USE_BANK;
+	}
+
+	*size = nor_dev.size;
+
+	ret = spi_nor_read_id(&id);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((nor_dev.flags & SPI_NOR_USE_BANK) != 0U) {
+		switch (id) {
+		case SPANSION_ID:
+			nor_dev.bank_read_cmd = SPINOR_OP_BRRD;
+			nor_dev.bank_write_cmd = SPINOR_OP_BRWR;
+			break;
+		default:
+			nor_dev.bank_read_cmd = SPINOR_OP_RDEAR;
+			nor_dev.bank_write_cmd = SPINOR_OP_WREAR;
+			break;
+		}
+	}
+
+	if (nor_dev.read_op.data.buswidth == 4U) {
+		switch (id) {
+		case MACRONIX_ID:
+			INFO("Enable Macronix quad support\n");
+			ret = spi_nor_macronix_quad_enable();
+			break;
+		case MICRON_ID:
+			break;
+		default:
+			ret = spi_nor_quad_enable();
+			break;
+		}
+	}
+
+	if ((ret == 0) && ((nor_dev.flags & SPI_NOR_USE_BANK) != 0U)) {
+		ret = spi_nor_read_bar();
+	}
+
+	return ret;
+}
diff --git a/drivers/mtd/spi-mem/spi_mem.c b/drivers/mtd/spi-mem/spi_mem.c
new file mode 100644
index 0000000..c43d519
--- /dev/null
+++ b/drivers/mtd/spi-mem/spi_mem.c
@@ -0,0 +1,289 @@
+/*
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <inttypes.h>
+#include <stdint.h>
+
+#include <drivers/spi_mem.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#define SPI_MEM_DEFAULT_SPEED_HZ 100000U
+
+/*
+ * struct spi_slave - Representation of a SPI slave.
+ *
+ * @max_hz:		Maximum speed for this slave in Hertz.
+ * @cs:			ID of the chip select connected to the slave.
+ * @mode:		SPI mode to use for this slave (see SPI mode flags).
+ * @ops:		Ops defined by the bus.
+ */
+struct spi_slave {
+	unsigned int max_hz;
+	unsigned int cs;
+	unsigned int mode;
+	const struct spi_bus_ops *ops;
+};
+
+static struct spi_slave spi_slave;
+
+static bool spi_mem_check_buswidth_req(uint8_t buswidth, bool tx)
+{
+	switch (buswidth) {
+	case 1U:
+		return true;
+
+	case 2U:
+		if ((tx && (spi_slave.mode & (SPI_TX_DUAL | SPI_TX_QUAD)) !=
+		     0U) ||
+		    (!tx && (spi_slave.mode & (SPI_RX_DUAL | SPI_RX_QUAD)) !=
+		     0U)) {
+			return true;
+		}
+		break;
+
+	case 4U:
+		if ((tx && (spi_slave.mode & SPI_TX_QUAD) != 0U) ||
+		    (!tx && (spi_slave.mode & SPI_RX_QUAD) != 0U)) {
+			return true;
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return false;
+}
+
+static bool spi_mem_supports_op(const struct spi_mem_op *op)
+{
+	if (!spi_mem_check_buswidth_req(op->cmd.buswidth, true)) {
+		return false;
+	}
+
+	if ((op->addr.nbytes != 0U) &&
+	    !spi_mem_check_buswidth_req(op->addr.buswidth, true)) {
+		return false;
+	}
+
+	if ((op->dummy.nbytes != 0U) &&
+	    !spi_mem_check_buswidth_req(op->dummy.buswidth, true)) {
+		return false;
+	}
+
+	if ((op->data.nbytes != 0U) &&
+	    !spi_mem_check_buswidth_req(op->data.buswidth,
+				       op->data.dir == SPI_MEM_DATA_OUT)) {
+		return false;
+	}
+
+	return true;
+}
+
+static int spi_mem_set_speed_mode(void)
+{
+	const struct spi_bus_ops *ops = spi_slave.ops;
+	int ret;
+
+	ret = ops->set_speed(spi_slave.max_hz);
+	if (ret != 0) {
+		VERBOSE("Cannot set speed (err=%d)\n", ret);
+		return ret;
+	}
+
+	ret = ops->set_mode(spi_slave.mode);
+	if (ret != 0) {
+		VERBOSE("Cannot set mode (err=%d)\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int spi_mem_check_bus_ops(const struct spi_bus_ops *ops)
+{
+	bool error = false;
+
+	if (ops->claim_bus == NULL) {
+		VERBOSE("Ops claim bus is not defined\n");
+		error = true;
+	}
+
+	if (ops->release_bus == NULL) {
+		VERBOSE("Ops release bus is not defined\n");
+		error = true;
+	}
+
+	if (ops->exec_op == NULL) {
+		VERBOSE("Ops exec op is not defined\n");
+		error = true;
+	}
+
+	if (ops->set_speed == NULL) {
+		VERBOSE("Ops set speed is not defined\n");
+		error = true;
+	}
+
+	if (ops->set_mode == NULL) {
+		VERBOSE("Ops set mode is not defined\n");
+		error = true;
+	}
+
+	return error ? -EINVAL : 0;
+}
+
+/*
+ * spi_mem_exec_op() - Execute a memory operation.
+ * @op: The memory operation to execute.
+ *
+ * This function first checks that @op is supported and then tries to execute
+ * it.
+ *
+ * Return: 0 in case of success, a negative error code otherwise.
+ */
+int spi_mem_exec_op(const struct spi_mem_op *op)
+{
+	const struct spi_bus_ops *ops = spi_slave.ops;
+	int ret;
+
+	VERBOSE("%s: cmd:%x mode:%d.%d.%d.%d addqr:%" PRIx64 " len:%x\n",
+		__func__, op->cmd.opcode, op->cmd.buswidth, op->addr.buswidth,
+		op->dummy.buswidth, op->data.buswidth,
+		op->addr.val, op->data.nbytes);
+
+	if (!spi_mem_supports_op(op)) {
+		WARN("Error in spi_mem_support\n");
+		return -ENOTSUP;
+	}
+
+	ret = ops->claim_bus(spi_slave.cs);
+	if (ret != 0) {
+		WARN("Error claim_bus\n");
+		return ret;
+	}
+
+	ret = ops->exec_op(op);
+
+	ops->release_bus();
+
+	return ret;
+}
+
+/*
+ * spi_mem_init_slave() - SPI slave device initialization.
+ * @fdt: Pointer to the device tree blob.
+ * @bus_node: Offset of the bus node.
+ * @ops: The SPI bus ops defined.
+ *
+ * This function first checks that @ops are supported and then tries to find
+ * a SPI slave device.
+ *
+ * Return: 0 in case of success, a negative error code otherwise.
+ */
+int spi_mem_init_slave(void *fdt, int bus_node, const struct spi_bus_ops *ops)
+{
+	int ret;
+	int mode = 0;
+	int nchips = 0;
+	int bus_subnode = 0;
+	const fdt32_t *cuint = NULL;
+
+	ret = spi_mem_check_bus_ops(ops);
+	if (ret != 0) {
+		return ret;
+	}
+
+	fdt_for_each_subnode(bus_subnode, fdt, bus_node) {
+		nchips++;
+	}
+
+	if (nchips != 1) {
+		ERROR("Only one SPI device is currently supported\n");
+		return -EINVAL;
+	}
+
+	fdt_for_each_subnode(bus_subnode, fdt, bus_node) {
+		/* Get chip select */
+		cuint = fdt_getprop(fdt, bus_subnode, "reg", NULL);
+		if (cuint == NULL) {
+			ERROR("Chip select not well defined\n");
+			return -EINVAL;
+		}
+		spi_slave.cs = fdt32_to_cpu(*cuint);
+
+		/* Get max slave frequency */
+		spi_slave.max_hz = SPI_MEM_DEFAULT_SPEED_HZ;
+		cuint = fdt_getprop(fdt, bus_subnode,
+				    "spi-max-frequency", NULL);
+		if (cuint != NULL) {
+			spi_slave.max_hz = fdt32_to_cpu(*cuint);
+		}
+
+		/* Get mode */
+		if ((fdt_getprop(fdt, bus_subnode, "spi-cpol", NULL)) != NULL) {
+			mode |= SPI_CPOL;
+		}
+		if ((fdt_getprop(fdt, bus_subnode, "spi-cpha", NULL)) != NULL) {
+			mode |= SPI_CPHA;
+		}
+		if ((fdt_getprop(fdt, bus_subnode, "spi-cs-high", NULL)) !=
+		    NULL) {
+			mode |= SPI_CS_HIGH;
+		}
+		if ((fdt_getprop(fdt, bus_subnode, "spi-3wire", NULL)) !=
+		    NULL) {
+			mode |= SPI_3WIRE;
+		}
+		if ((fdt_getprop(fdt, bus_subnode, "spi-half-duplex", NULL)) !=
+		    NULL) {
+			mode |= SPI_PREAMBLE;
+		}
+
+		/* Get dual/quad mode */
+		cuint = fdt_getprop(fdt, bus_subnode, "spi-tx-bus-width", NULL);
+		if (cuint != NULL) {
+			switch (fdt32_to_cpu(*cuint)) {
+			case 1U:
+				break;
+			case 2U:
+				mode |= SPI_TX_DUAL;
+				break;
+			case 4U:
+				mode |= SPI_TX_QUAD;
+				break;
+			default:
+				WARN("spi-tx-bus-width %u not supported\n",
+				     fdt32_to_cpu(*cuint));
+				return -EINVAL;
+			}
+		}
+
+		cuint = fdt_getprop(fdt, bus_subnode, "spi-rx-bus-width", NULL);
+		if (cuint != NULL) {
+			switch (fdt32_to_cpu(*cuint)) {
+			case 1U:
+				break;
+			case 2U:
+				mode |= SPI_RX_DUAL;
+				break;
+			case 4U:
+				mode |= SPI_RX_QUAD;
+				break;
+			default:
+				WARN("spi-rx-bus-width %u not supported\n",
+				     fdt32_to_cpu(*cuint));
+				return -EINVAL;
+			}
+		}
+
+		spi_slave.mode = mode;
+		spi_slave.ops = ops;
+	}
+
+	return spi_mem_set_speed_mode();
+}
diff --git a/drivers/nxp/auth/csf_hdr_parser/cot.c b/drivers/nxp/auth/csf_hdr_parser/cot.c
new file mode 100644
index 0000000..4502ed6
--- /dev/null
+++ b/drivers/nxp/auth/csf_hdr_parser/cot.c
@@ -0,0 +1,284 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * Copyright 2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+
+#include <drivers/auth/auth_mod.h>
+
+#if USE_TBBR_DEFS
+#include <tools_share/tbbr_oid.h>
+#else
+#include <platform_oid.h>
+#endif
+
+
+static auth_param_type_desc_t sig = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_SIG, 0);
+static auth_param_type_desc_t sig_alg = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_SIG_ALG, 0);
+static auth_param_type_desc_t sig_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, 0);
+
+static auth_param_type_desc_t non_trusted_world_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, NON_TRUSTED_WORLD_PK_OID);
+
+/*
+ * TBBR Chain of trust definition
+ */
+static const auth_img_desc_t bl31_image = {
+	.img_id = BL31_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t scp_bl2_image = {
+	.img_id = SCP_BL2_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl32_image = {
+	.img_id = BL32_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl33_image = {
+	.img_id = BL33_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+#ifdef POLICY_FUSE_PROVISION
+static const auth_img_desc_t fuse_prov_img = {
+	.img_id = FUSE_PROV_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t fuse_upgrade_img = {
+	.img_id = FUSE_UP_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+#endif
+#ifdef CONFIG_DDR_FIP_IMAGE
+static const auth_img_desc_t ddr_imem_udimm_1d_img = {
+	.img_id = DDR_IMEM_UDIMM_1D_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_imem_udimm_2d_img = {
+	.img_id = DDR_IMEM_UDIMM_2D_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_dmem_udimm_1d_img = {
+	.img_id = DDR_DMEM_UDIMM_1D_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_dmem_udimm_2d_img = {
+	.img_id = DDR_DMEM_UDIMM_2D_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_imem_rdimm_1d_img = {
+	.img_id = DDR_IMEM_RDIMM_1D_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_imem_rdimm_2d_img = {
+	.img_id = DDR_IMEM_RDIMM_2D_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_dmem_rdimm_1d_img = {
+	.img_id = DDR_DMEM_RDIMM_1D_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_dmem_rdimm_2d_img = {
+	.img_id = DDR_DMEM_RDIMM_2D_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &sig_hash
+			}
+		}
+	}
+};
+#endif
+
+static const auth_img_desc_t * const cot_desc[] = {
+	[BL31_IMAGE_ID]			=	&bl31_image,
+	[SCP_BL2_IMAGE_ID]		=	&scp_bl2_image,
+	[BL32_IMAGE_ID]			=	&bl32_image,
+	[BL33_IMAGE_ID]			=	&bl33_image,
+#ifdef POLICY_FUSE_PROVISION
+	[FUSE_PROV_IMAGE_ID]		=	&fuse_prov_img,
+	[FUSE_UP_IMAGE_ID]		=	&fuse_upgrade_img,
+#endif
+#ifdef CONFIG_DDR_FIP_IMAGE
+	[DDR_IMEM_UDIMM_1D_IMAGE_ID]	=	&ddr_imem_udimm_1d_img,
+	[DDR_IMEM_UDIMM_2D_IMAGE_ID]	=	&ddr_imem_udimm_2d_img,
+	[DDR_DMEM_UDIMM_1D_IMAGE_ID]	=	&ddr_dmem_udimm_1d_img,
+	[DDR_DMEM_UDIMM_2D_IMAGE_ID]	=	&ddr_dmem_udimm_2d_img,
+	[DDR_IMEM_RDIMM_1D_IMAGE_ID]	=	&ddr_imem_rdimm_1d_img,
+	[DDR_IMEM_RDIMM_2D_IMAGE_ID]	=	&ddr_imem_rdimm_2d_img,
+	[DDR_DMEM_RDIMM_1D_IMAGE_ID]	=	&ddr_dmem_rdimm_1d_img,
+	[DDR_DMEM_RDIMM_2D_IMAGE_ID]	=	&ddr_dmem_rdimm_2d_img,
+#endif
+};
+
+/* Register the CoT in the authentication module */
+REGISTER_COT(cot_desc);
diff --git a/drivers/nxp/auth/csf_hdr_parser/csf_hdr.mk b/drivers/nxp/auth/csf_hdr_parser/csf_hdr.mk
new file mode 100644
index 0000000..1af51f8
--- /dev/null
+++ b/drivers/nxp/auth/csf_hdr_parser/csf_hdr.mk
@@ -0,0 +1,64 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#
+
+CSF_HDR_SOURCES	:=  $(PLAT_DRIVERS_PATH)/auth/csf_hdr_parser/csf_hdr_parser.c
+
+CSF_HDR_SOURCES	+=  $(PLAT_DRIVERS_PATH)/auth/csf_hdr_parser/plat_img_parser.c
+
+PLAT_INCLUDES	+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/auth/csf_hdr_parser/
+
+$(eval $(call add_define, CSF_HEADER_PREPENDED))
+
+
+# Path to CST directory is required to generate the CSF header
+# and prepend it to image before fip image gets generated
+ifeq (${CST_DIR},)
+  $(error Error: CST_DIR not set)
+endif
+
+# Rules are created for generating and appending CSF header to images before
+# FIT image generation
+
+# CST_BL31
+define CST_BL31_RULE
+$(1): $(2)
+	@echo " Generating CSF Header for $$@ $$<"
+	$(Q)$(CST_DIR)/create_hdr_esbc --in $(2) --out $(1) --app_off ${CSF_HDR_SZ} \
+					--app $(2) ${BL31_INPUT_FILE}
+endef
+
+CST_BL31_SUFFIX := .cst
+
+# CST_BL32
+define CST_BL32_RULE
+$(1): $(2)
+	@echo " Generating CSF Header for $$@ $$<"
+	$(Q)$(CST_DIR)/create_hdr_esbc --in $(2) --out $(1) --app_off ${CSF_HDR_SZ} \
+					--app $(2) ${BL32_INPUT_FILE}
+endef
+
+CST_BL32_SUFFIX := .cst
+
+# CST_BL33
+define CST_BL33_RULE
+$(1): $(2)
+	@echo " Generating CSF Header for $$@ $$<"
+	$(Q)$(CST_DIR)/create_hdr_esbc --in $(2) --out $(1) --app_off ${CSF_HDR_SZ} \
+					--app $(2) ${BL33_INPUT_FILE}
+endef
+
+CST_BL33_SUFFIX := .cst
+
+# CST_SCP_BL2
+define CST_SCP_BL2_RULE
+$(1): $(2)
+	@echo " Generating CSF Header for $$@ $$<"
+	$(Q)$(CST_DIR)/create_hdr_esbc --in $(2) --out $(1) --app_off ${CSF_HDR_SZ} \
+					--app $(2) ${FUSE_INPUT_FILE}
+endef
+
+CST_SCP_BL2_SUFFIX := .cst
diff --git a/drivers/nxp/auth/csf_hdr_parser/csf_hdr_parser.c b/drivers/nxp/auth/csf_hdr_parser/csf_hdr_parser.c
new file mode 100644
index 0000000..b878082
--- /dev/null
+++ b/drivers/nxp/auth/csf_hdr_parser/csf_hdr_parser.c
@@ -0,0 +1,365 @@
+/*
+ * Copyright (c) 2014-2016, Freescale Semiconductor, Inc.
+ * Copyright 2017-2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <assert.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <cassert.h>
+#include <common/debug.h>
+#include <csf_hdr.h>
+#include <dcfg.h>
+#include <drivers/auth/crypto_mod.h>
+#include <lib/utils.h>
+#include <sfp.h>
+
+/* Maximum OID string length ("a.b.c.d.e.f ...") */
+#define MAX_OID_STR_LEN			64
+
+#define LIB_NAME	"NXP CSFv2"
+
+#ifdef CSF_HDR_CH3
+/* Barker Code for LS Ch3 ESBC Header */
+static const uint8_t barker_code[CSF_BARKER_LEN] = { 0x12, 0x19, 0x20, 0x01 };
+#else
+static const uint8_t barker_code[CSF_BARKER_LEN] = { 0x68, 0x39, 0x27, 0x81 };
+#endif
+
+#define CHECK_KEY_LEN(key_len)	(((key_len) == 2 * RSA_1K_KEY_SZ_BYTES) || \
+				 ((key_len) == 2 * RSA_2K_KEY_SZ_BYTES) || \
+				 ((key_len) == 2 * RSA_4K_KEY_SZ_BYTES))
+
+/* Flag to indicate if values are there in rotpk_hash_table */
+bool rotpk_not_dpld =  true;
+uint8_t rotpk_hash_table[MAX_KEY_ENTRIES][SHA256_BYTES];
+uint32_t num_rotpk_hash_entries;
+
+/*
+ * This function deploys the hashes of the various platform keys in
+ * rotpk_hash_table. This is done in case of secure boot after comparison
+ * of table's hash with the hash in SFP fuses. This installation is done
+ * only in the first header parsing.
+ */
+static int deploy_rotpk_hash_table(void *srk_buffer, uint16_t num_srk)
+{
+	void *ctx;
+	int ret = 0;
+	int i, j = 0;
+	unsigned int digest_size = SHA256_BYTES;
+	enum hash_algo algo = SHA256;
+	uint8_t hash[SHA256_BYTES];
+	uint32_t srk_hash[SHA256_BYTES/4] __aligned(CACHE_WRITEBACK_GRANULE);
+	struct srk_table *srktbl = (void *)srk_buffer;
+	struct sfp_ccsr_regs_t *sfp_ccsr_regs = (void *)(get_sfp_addr()
+							+ SFP_FUSE_REGS_OFFSET);
+
+
+	if (num_srk > MAX_KEY_ENTRIES) {
+		return -1;
+	}
+
+	ret = hash_init(algo, &ctx);
+	if (ret != 0) {
+		return -1;
+	}
+
+	/* Update hash with that of SRK table */
+	ret = hash_update(algo, ctx, (uint8_t *)((uint8_t *)srk_buffer),
+			  num_srk * sizeof(struct srk_table));
+	if (ret != 0) {
+		return -1;
+	}
+
+	/* Copy hash at destination buffer */
+	ret = hash_final(algo, ctx, hash, digest_size);
+	if (ret != 0) {
+		return -1;
+	}
+
+	/* Add comparison of hash with SFP hash here */
+	for (i = 0; i < SHA256_BYTES/4; i++) {
+		srk_hash[i] =
+			mmio_read_32((uintptr_t)&sfp_ccsr_regs->srk_hash[i]);
+	}
+
+	VERBOSE("SRK table HASH\n");
+	for (i = 0; i < 8; i++) {
+		VERBOSE("%x\n", *((uint32_t *)hash + i));
+	}
+
+	if (memcmp(hash, srk_hash, SHA256_BYTES) != 0) {
+		ERROR("Error in installing ROTPK table\n");
+		ERROR("SRK hash doesn't match the fuse hash\n");
+		return -1;
+	}
+
+	/* Hash table already deployed */
+	if (rotpk_not_dpld == false) {
+		return 0;
+	}
+
+	for (i = 0; i < num_srk; i++) {
+		ret = hash_init(algo, &ctx);
+		if (ret != 0) {
+			return -1;
+		}
+
+		/* Update hash with that of SRK table */
+		ret = hash_update(algo, ctx, srktbl[i].pkey, srktbl[i].key_len);
+		if (ret != 0) {
+			return -1;
+		}
+
+		/* Copy hash at destination buffer */
+		ret = hash_final(algo, ctx, rotpk_hash_table[i], digest_size);
+		if (ret != 0) {
+			return -1;
+		}
+		VERBOSE("Table key %d HASH\n", i);
+		for (j = 0; j < 8; j++) {
+			VERBOSE("%x\n", *((uint32_t *)rotpk_hash_table[i] + j));
+		}
+	}
+	rotpk_not_dpld = false;
+	num_rotpk_hash_entries = num_srk;
+
+	return 0;
+}
+
+/*
+ * Calculate hash of ESBC hdr and ESBC. This function calculates the
+ * single hash of ESBC header and ESBC image
+ */
+int calc_img_hash(struct csf_hdr *hdr,
+		  void *img_addr, uint32_t img_size,
+		  uint8_t *img_hash, uint32_t *hash_len)
+{
+	void *ctx;
+	int ret = 0;
+	unsigned int digest_size = SHA256_BYTES;
+	enum hash_algo algo = SHA256;
+
+	ret = hash_init(algo, &ctx);
+	/* Copy hash at destination buffer */
+	if (ret != 0) {
+		return -1;
+	}
+
+	/* Update hash for CSF Header */
+	ret = hash_update(algo, ctx, (uint8_t *)hdr, sizeof(struct csf_hdr));
+	if (ret != 0) {
+		return -1;
+	}
+
+	/* Update hash with that of SRK table */
+	ret = hash_update(algo, ctx,
+			  (uint8_t *)((uint8_t *)hdr + hdr->srk_tbl_off),
+			  hdr->len_kr.num_srk * sizeof(struct srk_table));
+	if (ret != 0) {
+		return -1;
+	}
+
+	/* Update hash for actual Image */
+	ret = hash_update(algo, ctx, (uint8_t *)(img_addr), img_size);
+	if (ret != 0) {
+		return -1;
+	}
+
+	/* Copy hash at destination buffer */
+	ret = hash_final(algo, ctx, img_hash, digest_size);
+	if (ret != 0) {
+		return -1;
+	}
+
+	*hash_len = digest_size;
+
+	VERBOSE("IMG encoded HASH\n");
+	for (int i = 0; i < 8; i++) {
+		VERBOSE("%x\n", *((uint32_t *)img_hash + i));
+	}
+
+	return 0;
+}
+
+/* This function checks if selected key is revoked or not.*/
+static uint32_t is_key_revoked(uint32_t keynum, uint32_t rev_flag)
+{
+	if (keynum == UNREVOCABLE_KEY) {
+		return 0;
+	}
+
+	if (((uint32_t)(1 << (REVOC_KEY_ALIGN - keynum)) & rev_flag) != 0) {
+		return 1;
+	}
+
+	return 0;
+}
+
+/* Parse the header to extract the type of key,
+ * Check if key is not revoked
+ * and return the key , key length and key_type
+ */
+static int32_t get_key(struct csf_hdr *hdr, uint8_t **key, uint32_t *len,
+			enum sig_alg *key_type)
+{
+	int i = 0;
+	uint32_t ret = 0U;
+	uint32_t key_num, key_revoc_flag;
+	void *esbc = hdr;
+	struct srk_table *srktbl = (void *)((uint8_t *)esbc + hdr->srk_tbl_off);
+	bool sb;
+	uint32_t mode;
+
+	/* We currently support only RSA keys and signature */
+	*key_type = RSA;
+
+	/* Check for number of SRK entries */
+	if ((hdr->len_kr.num_srk == 0) ||
+	    (hdr->len_kr.num_srk > MAX_KEY_ENTRIES)) {
+		ERROR("Error in NUM entries in SRK Table\n");
+		return -1;
+	}
+
+	/*
+	 * Check the key number field. It should be not greater than
+	 * number of entries in SRK table.
+	 */
+	key_num = hdr->len_kr.srk_sel;
+	if ((key_num == 0) || (key_num > hdr->len_kr.num_srk)) {
+		ERROR("Invalid Key number\n");
+		return -1;
+	}
+
+	/* Get revoc key from sfp */
+	key_revoc_flag = get_key_revoc();
+
+	/* Check if selected key has been revoked */
+	ret = is_key_revoked(key_num, key_revoc_flag);
+	if (ret != 0) {
+		ERROR("Selected key has been revoked\n");
+		return -1;
+	}
+
+	/* Check for valid key length - allowed key sized 1k, 2k and 4K */
+	for (i = 0; i < hdr->len_kr.num_srk; i++) {
+		if (CHECK_KEY_LEN(srktbl[i].key_len) == 0) {
+			ERROR("Invalid key length\n");
+			return -1;
+		}
+	}
+
+	/* We don't return error from here. While parsing we just try to
+	 * install the srk table. Failure needs to be taken care of in
+	 * case of secure boot. This failure will be handled at the time
+	 * of rotpk comparison in plat_get_rotpk_info function
+	 */
+	sb = check_boot_mode_secure(&mode);
+	if (sb) {
+		ret = deploy_rotpk_hash_table(srktbl, hdr->len_kr.num_srk);
+		if (ret != 0) {
+			ERROR("ROTPK FAILURE\n");
+			/* For ITS =1 , return failure */
+			if (mode != 0) {
+				return -1;
+			}
+			ERROR("SECURE BOOT DEV-ENV MODE:\n");
+			ERROR("\tCHECK ROTPK !\n");
+			ERROR("\tCONTINUING ON FAILURE...\n");
+		}
+	}
+
+	/* Return the length of the selected key */
+	*len = srktbl[key_num - 1].key_len;
+
+	/* Point key to the selected key */
+	*key =  (uint8_t *)&(srktbl[key_num - 1].pkey);
+
+	return 0;
+}
+
+/*
+ * This function would parse the CSF header and do the following:
+ * 1. Basic integrity checks
+ * 2. Key checks and extract the key from SRK/IE Table
+ * 3. Key hash comparison with SRKH in fuses in case of SRK Table
+ * 4. OEM/UID checks - To be added
+ * 5. Hash calculation for various components used in signature
+ * 6. Signature integrity checks
+ * return -> 0 on success, -1 on failure
+ */
+int validate_esbc_header(void *img_hdr, void **img_key, uint32_t *key_len,
+			 void **img_sign, uint32_t *sign_len,
+			 enum sig_alg *algo)
+{
+	struct csf_hdr *hdr = img_hdr;
+	uint8_t *s;
+	int32_t ret = 0;
+	void *esbc = (uint8_t *)img_hdr;
+	uint8_t *key;
+	uint32_t klen;
+
+	/* check barker code */
+	if (memcmp(hdr->barker, barker_code, CSF_BARKER_LEN) != 0) {
+		ERROR("Wrong barker code in header\n");
+		return -1;
+	}
+
+	ret = get_key(hdr, &key, &klen, algo);
+	if (ret != 0) {
+		return -1;
+	}
+
+	/* check signaure */
+	if (klen == (2 * hdr->sign_len)) {
+		/* check signature length */
+		if (((hdr->sign_len == RSA_1K_KEY_SZ_BYTES) ||
+		    (hdr->sign_len == RSA_2K_KEY_SZ_BYTES) ||
+		    (hdr->sign_len == RSA_4K_KEY_SZ_BYTES)) == 0) {
+			ERROR("Wrong Signature length in header\n");
+			return -1;
+		}
+	} else {
+		ERROR("RSA key length not twice the signature length\n");
+		return -1;
+	}
+
+	/* modulus most significant bit should be set */
+
+	if ((key[0] & 0x80) == 0U) {
+		ERROR("RSA Public key MSB not set\n");
+		return -1;
+	}
+
+	/* modulus value should be odd */
+	if ((key[klen / 2 - 1] & 0x1) == 0U) {
+		ERROR("Public key Modulus in header not odd\n");
+		return -1;
+	}
+
+	/* Check signature value < modulus value */
+	s =  (uint8_t *)(esbc + hdr->psign);
+
+	if (!(memcmp(s, key, hdr->sign_len) < 0)) {
+		ERROR("Signature not less than modulus");
+		return -1;
+	}
+
+	/* Populate the return addresses */
+	*img_sign = (void *)(s);
+
+	/* Save the length of signature */
+	*sign_len = hdr->sign_len;
+
+	*img_key = (uint8_t *)key;
+
+	*key_len = klen;
+
+	return ret;
+}
diff --git a/drivers/nxp/auth/csf_hdr_parser/input_bl2_ch2 b/drivers/nxp/auth/csf_hdr_parser/input_bl2_ch2
new file mode 100644
index 0000000..bf8934b
--- /dev/null
+++ b/drivers/nxp/auth/csf_hdr_parser/input_bl2_ch2
@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2014-2016, Freescale Semiconductor, Inc.
+ * Copyright 2017-2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+---------------------------------------------------
+# Specify the platform. [Mandatory]
+# Choose Platform - 1010/1040/2041/3041/4080/5020/5040/9131/9132/9164/4240/C290/LS1
+PLATFORM=LS1043
+# ESBC Flag. Specify ESBC=0 to sign u-boot and ESBC=1 to sign ESBC images.(default is 0)
+ESBC=0
+---------------------------------------------------
+# Entry Point/Image start address field in the header.[Mandatory]
+# (default=ADDRESS of first file specified in images)
+ENTRY_POINT=10000000
+---------------------------------------------------
+# Specify the file name of the keys separated by comma.
+# The number of files and key select should lie between 1 and 4 for 1040 and C290.
+# For rest of the platforms only one key is required and key select should not be provided.
+
+# USAGE (for 4080/5020/5040/3041/2041/1010/913x): PRI_KEY = <key1.pri>
+# USAGE (for 1040/C290/9164/4240/LS1): PRI_KEY = <key1.pri>, <key2.pri>, <key3.pri>, <key4.pri>
+
+# PRI_KEY (Default private key :srk.pri) - [Optional]
+PRI_KEY=srk.pri
+# PUB_KEY (Default public key :srk.pub) - [Optional]
+PUB_KEY=srk.pub
+# Please provide KEY_SELECT(between 1 to 4) (Required for 1040/C290/9164/4240/LS1 only) - [Optional]
+KEY_SELECT=
+---------------------------------------------------
+# Specify SG table address, only for (2041/3041/4080/5020/5040) with ESBC=0 - [Optional]
+SG_TABLE_ADDR=
+---------------------------------------------------
+# Specify the target where image will be loaded. (Default is NOR_16B) - [Optional]
+# Only required for Non-PBL Devices (1010/1040/9131/9132i/C290)
+# Select from - NOR_8B/NOR_16B/NAND_8B_512/NAND_8B_2K/NAND_8B_4K/NAND_16B_512/NAND_16B_2K/NAND_16B_4K/SD/MMC/SPI
+IMAGE_TARGET=
+---------------------------------------------------
+# Specify IMAGE, Max 8 images are possible. DST_ADDR is required only for Non-PBL Platform. [Mandatory]
+# USAGE : IMAGE_NO = {IMAGE_NAME, SRC_ADDR, DST_ADDR}
+IMAGE_1={bl2.bin,10000000,ffffffff}
+IMAGE_2={,,}
+IMAGE_3={,,}
+IMAGE_4={,,}
+IMAGE_5={,,}
+IMAGE_6={,,}
+IMAGE_7={,,}
+IMAGE_8={,,}
+---------------------------------------------------
+# Specify OEM AND FSL ID to be populated in header. [Optional]
+# e.g FSL_UID=11111111
+FSL_UID_0=
+FSL_UID_1=
+OEM_UID_0=
+OEM_UID_1=
+---------------------------------------------------
+# Specify the file names of csf header and sg table. (Default :hdr.out) [Optional]
+OUTPUT_HDR_FILENAME=hdr_bl2.out
+
+# Specify the file names of hash file and sign file.
+HASH_FILENAME=img_hash.out
+INPUT_SIGN_FILENAME=sign.out
+
+# Specify the signature size.It is mandatory when neither public key nor private key is specified.
+# Signature size would be [0x80 for 1k key, 0x100 for 2k key, and 0x200 for 4k key].
+SIGN_SIZE=
+---------------------------------------------------
+# Specify the output file name of sg table. (Default :sg_table.out). [Optional]
+# Please note that OUTPUT SG BIN is only required for 2041/3041/4080/5020/5040 when ESBC flag is not set.
+OUTPUT_SG_BIN=
+---------------------------------------------------
+# Following fields are Required for 4240/9164/1040/C290 only
+
+# Specify House keeping Area
+# Required for 4240/9164/1040/C290 only when ESBC flag is not set. [Mandatory]
+HK_AREA_POINTER=
+HK_AREA_SIZE=
+---------------------------------------------------
+# Following field Required for 4240/9164/1040/C290 only
+# Specify Secondary Image Flag. (0 or 1) - [Optional]
+# (Default is 0)
+SEC_IMAGE=0
+# Specify Manufacturing Protection Flag. (0 or 1) - [Optional]
+# Required only for LS1(Default is 0)
+MP_FLAG=1
+---------------------------------------------------
diff --git a/drivers/nxp/auth/csf_hdr_parser/input_bl2_ch3 b/drivers/nxp/auth/csf_hdr_parser/input_bl2_ch3
new file mode 100644
index 0000000..5fdad9c
--- /dev/null
+++ b/drivers/nxp/auth/csf_hdr_parser/input_bl2_ch3
@@ -0,0 +1,65 @@
+/*
+ * Copyright 2018-2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+---------------------------------------------------
+# Specify the platform. [Mandatory]
+# Choose Platform -
+# TRUST 3.2: LX2160
+PLATFORM=LS2088
+---------------------------------------------------
+# Entry Point/Image start address field in the header.[Mandatory]
+# (default=ADDRESS of first file specified in images)
+# Address can be 64 bit
+ENTRY_POINT=1800A000
+---------------------------------------------------
+# Specify the Key Information.
+# PUB_KEY [Mandatory] Comma Separated List
+# Usage: <srk1.pub> <srk2.pub> .....
+PUB_KEY=srk.pub
+# KEY_SELECT [Mandatory]
+# USAGE (for TRUST 3.x): (between 1 to 8)
+KEY_SELECT=1
+# PRI_KEY [Mandatory] Single Key Used for Signing
+# USAGE: <srk.pri>
+PRI_KEY=srk.pri
+---------------------------------------------------
+# Specify IMAGE, Max 8 images are possible.
+# DST_ADDR is required only for Non-PBL Platform. [Mandatory]
+# USAGE : IMAGE_NO = {IMAGE_NAME, SRC_ADDR, DST_ADDR}
+# Address can be 64 bit
+IMAGE_1={bl2.bin,1800A000,ffffffff}
+IMAGE_2={,,}
+IMAGE_3={,,}
+IMAGE_4={,,}
+IMAGE_5={,,}
+IMAGE_6={,,}
+IMAGE_7={,,}
+IMAGE_8={,,}
+---------------------------------------------------
+# Specify OEM AND FSL ID to be populated in header. [Optional]
+# e.g FSL_UID_0=11111111
+FSL_UID_0=
+FSL_UID_1=
+OEM_UID_0=
+OEM_UID_1=
+OEM_UID_2=
+OEM_UID_3=
+OEM_UID_4=
+---------------------------------------------------
+# Specify the output file names [Optional].
+# Default Values chosen in Tool
+OUTPUT_HDR_FILENAME=hdr_bl2.out
+IMAGE_HASH_FILENAME=
+RSA_SIGN_FILENAME=
+---------------------------------------------------
+# Specify The Flags. (0 or 1) - [Optional]
+MP_FLAG=0
+ISS_FLAG=1
+LW_FLAG=0
+---------------------------------------------------
+# Specify VERBOSE as 1, if you want to Display Header Information [Optional]
+VERBOSE=1
diff --git a/drivers/nxp/auth/csf_hdr_parser/input_bl2_ch3_2 b/drivers/nxp/auth/csf_hdr_parser/input_bl2_ch3_2
new file mode 100644
index 0000000..cc7c07c
--- /dev/null
+++ b/drivers/nxp/auth/csf_hdr_parser/input_bl2_ch3_2
@@ -0,0 +1,65 @@
+/*
+ * Copyright 2018-2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+---------------------------------------------------
+# Specify the platform. [Mandatory]
+# Choose Platform -
+# TRUST 3.2: LX2160
+PLATFORM=LX2160
+---------------------------------------------------
+# Entry Point/Image start address field in the header.[Mandatory]
+# (default=ADDRESS of first file specified in images)
+# Address can be 64 bit
+ENTRY_POINT=1800D000
+---------------------------------------------------
+# Specify the Key Information.
+# PUB_KEY [Mandatory] Comma Separated List
+# Usage: <srk1.pub> <srk2.pub> .....
+PUB_KEY=srk.pub
+# KEY_SELECT [Mandatory]
+# USAGE (for TRUST 3.x): (between 1 to 8)
+KEY_SELECT=1
+# PRI_KEY [Mandatory] Single Key Used for Signing
+# USAGE: <srk.pri>
+PRI_KEY=srk.pri
+---------------------------------------------------
+# Specify IMAGE, Max 8 images are possible.
+# DST_ADDR is required only for Non-PBL Platform. [Mandatory]
+# USAGE : IMAGE_NO = {IMAGE_NAME, SRC_ADDR, DST_ADDR}
+# Address can be 64 bit
+IMAGE_1={bl2.bin,1800D000,ffffffff}
+IMAGE_2={,,}
+IMAGE_3={,,}
+IMAGE_4={,,}
+IMAGE_5={,,}
+IMAGE_6={,,}
+IMAGE_7={,,}
+IMAGE_8={,,}
+---------------------------------------------------
+# Specify OEM AND FSL ID to be populated in header. [Optional]
+# e.g FSL_UID_0=11111111
+FSL_UID_0=
+FSL_UID_1=
+OEM_UID_0=
+OEM_UID_1=
+OEM_UID_2=
+OEM_UID_3=
+OEM_UID_4=
+---------------------------------------------------
+# Specify the output file names [Optional].
+# Default Values chosen in Tool
+OUTPUT_HDR_FILENAME=hdr_bl2.out
+IMAGE_HASH_FILENAME=
+RSA_SIGN_FILENAME=
+---------------------------------------------------
+# Specify The Flags. (0 or 1) - [Optional]
+MP_FLAG=0
+ISS_FLAG=1
+LW_FLAG=0
+---------------------------------------------------
+# Specify VERBOSE as 1, if you want to Display Header Information [Optional]
+VERBOSE=1
diff --git a/drivers/nxp/auth/csf_hdr_parser/input_blx_ch2 b/drivers/nxp/auth/csf_hdr_parser/input_blx_ch2
new file mode 100644
index 0000000..93b020b
--- /dev/null
+++ b/drivers/nxp/auth/csf_hdr_parser/input_blx_ch2
@@ -0,0 +1,30 @@
+/*
+ * Copyright 2017-2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+---------------------------------------------------
+# Specify the platform. [Mandatory]
+# Choose Platform - 1010/1040/2041/3041/4080/5020/5040/9131/9132/9164/4240/C290/LS1
+PLATFORM=LS1043
+# ESBC Flag. Specify ESBC=0 to sign u-boot and ESBC=1 to sign ESBC images.(default is 0)
+ESBC=1
+---------------------------------------------------
+# Specify the file name of the keys separated by comma.
+
+# PRI_KEY (Default private key :srk.pri) - [Optional]
+PRI_KEY=srk.pri
+# PUB_KEY (Default public key :srk.pub) - [Optional]
+PUB_KEY=srk.pub
+# Please provide KEY_SELECT(between 1 to 4) (Required for 1040/C290/9164/4240 only) - [Optional]
+KEY_SELECT=1
+---------------------------------------------------
+# Specify OEM AND FSL ID to be populated in header. [Optional]
+# e.g FSL_UID=11111111
+FSL_UID_0=
+FSL_UID_1=
+OEM_UID_0=
+OEM_UID_1=
+---------------------------------------------------
diff --git a/drivers/nxp/auth/csf_hdr_parser/input_blx_ch3 b/drivers/nxp/auth/csf_hdr_parser/input_blx_ch3
new file mode 100644
index 0000000..18e8e3b
--- /dev/null
+++ b/drivers/nxp/auth/csf_hdr_parser/input_blx_ch3
@@ -0,0 +1,37 @@
+/*
+ * Copyright 2017-2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+ESBC=1
+---------------------------------------------------
+# Specify the platform. [Mandatory]
+# Choose Platform -
+# TRUST 3.0: LS2085
+# TRUST 3.1: LS2088, LS1088
+PLATFORM=LS2088
+---------------------------------------------------
+# Specify the Key Information.
+# PUB_KEY [Mandatory] Comma Separated List
+# Usage: <srk1.pub> <srk2.pub> .....
+PUB_KEY=srk.pub
+# KEY_SELECT [Mandatory]
+# USAGE (for TRUST 3.x): (between 1 to 8)
+KEY_SELECT=1
+# PRI_KEY [Mandatory] Single Key Used for Signing
+# USAGE: <srk.pri>
+PRI_KEY=srk.pri
+
+---------------------------------------------------
+# Specify OEM AND FSL ID to be populated in header. [Optional]
+# e.g FSL_UID_0=11111111
+FSL_UID_0=
+FSL_UID_1=
+OEM_UID_0=
+OEM_UID_1=
+OEM_UID_2=
+OEM_UID_3=
+OEM_UID_4=
+---------------------------------------------------
diff --git a/drivers/nxp/auth/csf_hdr_parser/input_pbi_ch3 b/drivers/nxp/auth/csf_hdr_parser/input_pbi_ch3
new file mode 100644
index 0000000..9111a2a
--- /dev/null
+++ b/drivers/nxp/auth/csf_hdr_parser/input_pbi_ch3
@@ -0,0 +1,43 @@
+/*
+ * Copyright 2016-2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+---------------------------------------------------
+# Specify the platform. [Mandatory]
+# Choose Platform -
+# TRUST 3.0: LS2085
+# TRUST 3.1: LS2088, LS1088
+PLATFORM=LS2088
+---------------------------------------------------
+# Specify the Key Information.
+# PUB_KEY [Mandatory] Comma Separated List
+# Usage: <srk1.pub> <srk2.pub> .....
+PUB_KEY=srk.pub
+# KEY_SELECT [Mandatory]
+# USAGE (for TRUST 3.x): (between 1 to 8)
+KEY_SELECT=1
+# PRI_KEY [Mandatory] Single Key Used for Signing
+# USAGE: <srk.pri>
+PRI_KEY=srk.pri
+---------------------------------------------------
+# Specify OEM AND FSL ID to be populated in header. [Optional]
+# e.g FSL_UID_0=11111111
+FSL_UID_0=
+FSL_UID_1=
+OEM_UID_0=
+OEM_UID_1=
+OEM_UID_2=
+OEM_UID_3=
+OEM_UID_4=
+---------------------------------------------------
+# Specify The Flags. (0 or 1) - [Optional]
+MP_FLAG=0
+ISS_FLAG=1
+LW_FLAG=0
+---------------------------------------------------
+# Specify VERBOSE as 1, if you want to Display Header Information [Optional]
+VERBOSE=1
+---------------------------------------------------
diff --git a/drivers/nxp/auth/csf_hdr_parser/input_pbi_ch3_2 b/drivers/nxp/auth/csf_hdr_parser/input_pbi_ch3_2
new file mode 100644
index 0000000..c2d7ce4
--- /dev/null
+++ b/drivers/nxp/auth/csf_hdr_parser/input_pbi_ch3_2
@@ -0,0 +1,43 @@
+/*
+ * Copyright 2017-2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+---------------------------------------------------
+# Specify the platform. [Mandatory]
+# Choose Platform -
+# TRUST 3.0: LS2085
+# TRUST 3.1: LS2088, LS1088
+PLATFORM=LX2160
+---------------------------------------------------
+# Specify the Key Information.
+# PUB_KEY [Mandatory] Comma Separated List
+# Usage: <srk1.pub> <srk2.pub> .....
+PUB_KEY=srk.pub
+# KEY_SELECT [Mandatory]
+# USAGE (for TRUST 3.x): (between 1 to 8)
+KEY_SELECT=1
+# PRI_KEY [Mandatory] Single Key Used for Signing
+# USAGE: <srk.pri>
+PRI_KEY=srk.pri
+---------------------------------------------------
+# Specify OEM AND FSL ID to be populated in header. [Optional]
+# e.g FSL_UID_0=11111111
+FSL_UID_0=
+FSL_UID_1=
+OEM_UID_0=
+OEM_UID_1=
+OEM_UID_2=
+OEM_UID_3=
+OEM_UID_4=
+---------------------------------------------------
+# Specify The Flags. (0 or 1) - [Optional]
+MP_FLAG=0
+ISS_FLAG=1
+LW_FLAG=0
+---------------------------------------------------
+# Specify VERBOSE as 1, if you want to Display Header Information [Optional]
+VERBOSE=1
+---------------------------------------------------
diff --git a/drivers/nxp/auth/csf_hdr_parser/plat_img_parser.c b/drivers/nxp/auth/csf_hdr_parser/plat_img_parser.c
new file mode 100644
index 0000000..43b78e5
--- /dev/null
+++ b/drivers/nxp/auth/csf_hdr_parser/plat_img_parser.c
@@ -0,0 +1,180 @@
+/*
+ * Copyright (c) 2014-2016, Freescale Semiconductor, Inc.
+ * Copyright 2017-2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <assert.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <csf_hdr.h>
+#include <drivers/auth/crypto_mod.h>
+#include <drivers/auth/img_parser_mod.h>
+#include <lib/utils.h>
+#include <sfp.h>
+
+/* Temporary variables to speed up the authentication parameters search. These
+ * variables are assigned once during the integrity check and used any time an
+ * authentication parameter is requested, so we do not have to parse the image
+ * again.
+ */
+
+/* Hash of Image + CSF Header + SRK table */
+uint8_t img_hash[SHA256_BYTES] __aligned(CACHE_WRITEBACK_GRANULE);
+uint32_t hash_len;
+
+/* Key being used for authentication
+ * Points to the key in CSF header copied in DDR
+ * ESBC client key
+ */
+void *img_key;
+uint32_t key_len;
+
+/* ESBC client signature */
+void *img_sign;
+uint32_t sign_len;
+enum sig_alg alg;
+
+/* Maximum OID string length ("a.b.c.d.e.f ...") */
+#define MAX_OID_STR_LEN			64
+
+#define LIB_NAME	"NXP CSFv2"
+
+/*
+ * Clear all static temporary variables.
+ */
+static void clear_temp_vars(void)
+{
+#define ZERO_AND_CLEAN(x)					\
+	do {							\
+		zeromem(&x, sizeof(x));				\
+		clean_dcache_range((uintptr_t)&x, sizeof(x));	\
+	} while (0)
+
+	ZERO_AND_CLEAN(img_key);
+	ZERO_AND_CLEAN(img_sign);
+	ZERO_AND_CLEAN(img_hash);
+	ZERO_AND_CLEAN(key_len);
+	ZERO_AND_CLEAN(hash_len);
+	ZERO_AND_CLEAN(sign_len);
+
+#undef ZERO_AND_CLEAN
+}
+
+/* Exported functions */
+
+static void init(void)
+{
+	clear_temp_vars();
+}
+
+/*
+ * This function would check the integrity of the CSF header
+ */
+static int check_integrity(void *img, unsigned int img_len)
+{
+	int ret;
+
+	/*
+	 * The image file has been successfully loaded till here.
+	 *
+	 * Flush the image to main memory so that it can be authenticated
+	 * by CAAM, a HW accelerator regardless of cache and MMU state.
+	 */
+	flush_dcache_range((uintptr_t) img, img_len);
+
+	/*
+	 * Image is appended at an offset of 16K (IMG_OFFSET) to the header.
+	 * So the size in header should be equal to img_len - IMG_OFFSET
+	 */
+	VERBOSE("Barker code is %x\n", *(unsigned int *)img);
+	ret = validate_esbc_header(img, &img_key, &key_len, &img_sign,
+				   &sign_len, &alg);
+	if (ret < 0) {
+		ERROR("Header authentication failed\n");
+		clear_temp_vars();
+		return IMG_PARSER_ERR;
+	}
+	/* Calculate the hash of various components from the image */
+	ret = calc_img_hash(img, (uint8_t *)img + CSF_HDR_SZ,
+			    img_len - CSF_HDR_SZ, img_hash, &hash_len);
+	if (ret != 0) {
+		ERROR("Issue in hash calculation %d\n", ret);
+		clear_temp_vars();
+		return IMG_PARSER_ERR;
+	}
+
+	return IMG_PARSER_OK;
+}
+
+/*
+ * Extract an authentication parameter from CSF header
+ *
+ * CSF header has already been parsed and the required information like
+ * hash of data, signature, length stored in global variables has been
+ * extracted in chek_integrity function.  This data
+ * is returned back to the caller.
+ */
+static int get_auth_param(const auth_param_type_desc_t *type_desc,
+		void *img, unsigned int img_len,
+		void **param, unsigned int *param_len)
+{
+	int rc = IMG_PARSER_OK;
+
+	/* We do not use img because the check_integrity function has already
+	 * extracted the relevant data ( pk, sig_alg, etc)
+	 */
+
+	switch (type_desc->type) {
+
+	/* Hash will be returned for comparison with signature */
+	case AUTH_PARAM_HASH:
+		*param = (void *)img_hash;
+		*param_len = (unsigned int)SHA256_BYTES;
+		break;
+
+	/* Return the public key used for signature extracted from the SRK table
+	 * after checks with key revocation
+	 */
+	case AUTH_PARAM_PUB_KEY:
+		/* Get the subject public key */
+		/* For a 1K key - the length would be 2k/8 = 0x100 bytes
+		 * 2K RSA key - 0x200 , 4K RSA - 0x400
+		 */
+		*param = img_key;
+		*param_len = (unsigned int)key_len;
+		break;
+
+	/* Call a function to tell if signature is RSA or ECDSA. ECDSA to be
+	 * supported in later platforms like LX2 etc
+	 */
+	case AUTH_PARAM_SIG_ALG:
+		/* Algo will be signature - RSA or ECDSA  on hash */
+		*param = (void *)&alg;
+		*param_len = 4U;
+		break;
+
+	/* Return the signature */
+	case AUTH_PARAM_SIG:
+		*param = img_sign;
+		*param_len = (unsigned int)sign_len;
+		break;
+
+	case AUTH_PARAM_NV_CTR:
+
+	default:
+		rc = IMG_PARSER_ERR_NOT_FOUND;
+		break;
+	}
+
+	return rc;
+}
+
+REGISTER_IMG_PARSER_LIB(IMG_PLAT, LIB_NAME, init,
+			check_integrity, get_auth_param);
diff --git a/drivers/nxp/auth/tbbr/tbbr_cot.c b/drivers/nxp/auth/tbbr/tbbr_cot.c
new file mode 100644
index 0000000..bb21fa0
--- /dev/null
+++ b/drivers/nxp/auth/tbbr/tbbr_cot.c
@@ -0,0 +1,820 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * Copyright 2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+
+#include <drivers/auth/auth_mod.h>
+
+#if USE_TBBR_DEFS
+#include <tools_share/tbbr_oid.h>
+#else
+#include <platform_oid.h>
+#endif
+
+
+#if TF_MBEDTLS_HASH_ALG_ID == TF_MBEDTLS_SHA256
+#define HASH_DER_LEN			51
+#elif TF_MBEDTLS_HASH_ALG_ID == TF_MBEDTLS_SHA384
+#define HASH_DER_LEN			67
+#elif TF_MBEDTLS_HASH_ALG_ID == TF_MBEDTLS_SHA512
+#define HASH_DER_LEN			83
+#else
+#error "Invalid value for TF_MBEDTLS_HASH_ALG_ID"
+#endif
+
+/*
+ * The platform must allocate buffers to store the authentication parameters
+ * extracted from the certificates. In this case, because of the way the CoT is
+ * established, we can reuse some of the buffers on different stages
+ */
+
+static unsigned char nt_world_bl_hash_buf[HASH_DER_LEN];
+
+static unsigned char soc_fw_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_extra1_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_extra2_hash_buf[HASH_DER_LEN];
+static unsigned char trusted_world_pk_buf[PK_DER_LEN];
+static unsigned char non_trusted_world_pk_buf[PK_DER_LEN];
+static unsigned char content_pk_buf[PK_DER_LEN];
+static unsigned char soc_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char nt_fw_config_hash_buf[HASH_DER_LEN];
+
+#ifdef CONFIG_DDR_FIP_IMAGE
+static unsigned char ddr_fw_content_pk_buf[PK_DER_LEN];
+static unsigned char ddr_imem_udimm_1d_hash_buf[HASH_DER_LEN];
+static unsigned char ddr_imem_udimm_2d_hash_buf[HASH_DER_LEN];
+static unsigned char ddr_dmem_udimm_1d_hash_buf[HASH_DER_LEN];
+static unsigned char ddr_dmem_udimm_2d_hash_buf[HASH_DER_LEN];
+
+static unsigned char ddr_imem_rdimm_1d_hash_buf[HASH_DER_LEN];
+static unsigned char ddr_imem_rdimm_2d_hash_buf[HASH_DER_LEN];
+static unsigned char ddr_dmem_rdimm_1d_hash_buf[HASH_DER_LEN];
+static unsigned char ddr_dmem_rdimm_2d_hash_buf[HASH_DER_LEN];
+#endif
+
+/*
+ * Parameter type descriptors
+ */
+static auth_param_type_desc_t trusted_nv_ctr = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_NV_CTR, TRUSTED_FW_NVCOUNTER_OID);
+
+static auth_param_type_desc_t subject_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, 0);
+static auth_param_type_desc_t sig = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_SIG, 0);
+static auth_param_type_desc_t sig_alg = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_SIG_ALG, 0);
+static auth_param_type_desc_t raw_data = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_RAW_DATA, 0);
+
+
+static auth_param_type_desc_t non_trusted_nv_ctr = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_NV_CTR, NON_TRUSTED_FW_NVCOUNTER_OID);
+static auth_param_type_desc_t trusted_world_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, TRUSTED_WORLD_PK_OID);
+static auth_param_type_desc_t non_trusted_world_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, NON_TRUSTED_WORLD_PK_OID);
+static auth_param_type_desc_t soc_fw_content_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, SOC_FW_CONTENT_CERT_PK_OID);
+static auth_param_type_desc_t tos_fw_content_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, TRUSTED_OS_FW_CONTENT_CERT_PK_OID);
+static auth_param_type_desc_t nt_fw_content_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, NON_TRUSTED_FW_CONTENT_CERT_PK_OID);
+static auth_param_type_desc_t soc_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SOC_AP_FW_HASH_OID);
+static auth_param_type_desc_t soc_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SOC_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t tos_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_HASH_OID);
+static auth_param_type_desc_t tos_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t tos_fw_extra1_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_EXTRA1_HASH_OID);
+static auth_param_type_desc_t tos_fw_extra2_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_EXTRA2_HASH_OID);
+static auth_param_type_desc_t nt_world_bl_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, NON_TRUSTED_WORLD_BOOTLOADER_HASH_OID);
+static auth_param_type_desc_t nt_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, NON_TRUSTED_FW_CONFIG_HASH_OID);
+
+#ifdef CONFIG_DDR_FIP_IMAGE
+static auth_param_type_desc_t ddr_fw_content_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, DDR_FW_CONTENT_CERT_PK_OID);
+
+static auth_param_type_desc_t ddr_imem_udimm_1d_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, DDR_IMEM_UDIMM_1D_HASH_OID);
+static auth_param_type_desc_t ddr_imem_udimm_2d_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, DDR_IMEM_UDIMM_2D_HASH_OID);
+static auth_param_type_desc_t ddr_dmem_udimm_1d_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, DDR_DMEM_UDIMM_1D_HASH_OID);
+static auth_param_type_desc_t ddr_dmem_udimm_2d_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, DDR_DMEM_UDIMM_2D_HASH_OID);
+
+static auth_param_type_desc_t ddr_imem_rdimm_1d_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, DDR_IMEM_RDIMM_1D_HASH_OID);
+static auth_param_type_desc_t ddr_imem_rdimm_2d_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, DDR_IMEM_RDIMM_2D_HASH_OID);
+static auth_param_type_desc_t ddr_dmem_rdimm_1d_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, DDR_DMEM_RDIMM_1D_HASH_OID);
+static auth_param_type_desc_t ddr_dmem_rdimm_2d_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, DDR_DMEM_RDIMM_2D_HASH_OID);
+#endif
+
+
+/*
+ * Trusted key certificate
+ */
+static const auth_img_desc_t trusted_key_cert = {
+	.img_id = TRUSTED_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &subject_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &trusted_world_pk,
+			.data = {
+				.ptr = (void *)trusted_world_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &non_trusted_world_pk,
+			.data = {
+				.ptr = (void *)non_trusted_world_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+
+/*
+ * SoC Firmware
+ */
+static const auth_img_desc_t soc_fw_key_cert = {
+	.img_id = SOC_FW_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &soc_fw_content_pk,
+			.data = {
+				.ptr = (void *)content_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t soc_fw_content_cert = {
+	.img_id = SOC_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &soc_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &soc_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &soc_fw_hash,
+			.data = {
+				.ptr = (void *)soc_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &soc_fw_config_hash,
+			.data = {
+				.ptr = (void *)soc_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl31_image = {
+	.img_id = BL31_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &soc_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &soc_fw_hash
+			}
+		}
+	}
+};
+/* SOC FW Config */
+static const auth_img_desc_t soc_fw_config = {
+	.img_id = SOC_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &soc_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &soc_fw_config_hash
+			}
+		}
+	}
+};
+/*
+ * Trusted OS Firmware
+ */
+static const auth_img_desc_t trusted_os_fw_key_cert = {
+	.img_id = TRUSTED_OS_FW_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &tos_fw_content_pk,
+			.data = {
+				.ptr = (void *)content_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t trusted_os_fw_content_cert = {
+	.img_id = TRUSTED_OS_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_os_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &tos_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &tos_fw_hash,
+			.data = {
+				.ptr = (void *)tos_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &tos_fw_extra1_hash,
+			.data = {
+				.ptr = (void *)tos_fw_extra1_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &tos_fw_extra2_hash,
+			.data = {
+				.ptr = (void *)tos_fw_extra2_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &tos_fw_config_hash,
+			.data = {
+				.ptr = (void *)tos_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl32_image = {
+	.img_id = BL32_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl32_extra1_image = {
+	.img_id = BL32_EXTRA1_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_extra1_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl32_extra2_image = {
+	.img_id = BL32_EXTRA2_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_extra2_hash
+			}
+		}
+	}
+};
+/* TOS FW Config */
+static const auth_img_desc_t tos_fw_config = {
+	.img_id = TOS_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_config_hash
+			}
+		}
+	}
+};
+/*
+ * Non-Trusted Firmware
+ */
+static const auth_img_desc_t non_trusted_fw_key_cert = {
+	.img_id = NON_TRUSTED_FW_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &non_trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &nt_fw_content_pk,
+			.data = {
+				.ptr = (void *)content_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t non_trusted_fw_content_cert = {
+	.img_id = NON_TRUSTED_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &non_trusted_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &nt_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &nt_world_bl_hash,
+			.data = {
+				.ptr = (void *)nt_world_bl_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &nt_fw_config_hash,
+			.data = {
+				.ptr = (void *)nt_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t bl33_image = {
+	.img_id = BL33_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &non_trusted_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &nt_world_bl_hash
+			}
+		}
+	}
+};
+/* NT FW Config */
+static const auth_img_desc_t nt_fw_config = {
+	.img_id = NT_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &non_trusted_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &nt_fw_config_hash
+			}
+		}
+	}
+};
+#ifdef CONFIG_DDR_FIP_IMAGE
+/*
+ * DDR Firmware
+ */
+static const auth_img_desc_t ddr_fw_key_cert = {
+	.img_id = DDR_FW_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &trusted_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &trusted_world_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &ddr_fw_content_pk,
+			.data = {
+				.ptr = (void *)ddr_fw_content_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_udimm_fw_content_cert = {
+	.img_id = DDR_UDIMM_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &ddr_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &ddr_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &ddr_imem_udimm_1d_fw_hash,
+			.data = {
+				.ptr = (void *)ddr_imem_udimm_1d_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &ddr_imem_udimm_2d_fw_hash,
+			.data = {
+				.ptr = (void *)ddr_imem_udimm_2d_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &ddr_dmem_udimm_1d_fw_hash,
+			.data = {
+				.ptr = (void *)ddr_dmem_udimm_1d_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &ddr_dmem_udimm_2d_fw_hash,
+			.data = {
+				.ptr = (void *)ddr_dmem_udimm_2d_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+	}
+};
+
+static const auth_img_desc_t ddr_imem_udimm_1d_img = {
+	.img_id = DDR_IMEM_UDIMM_1D_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &ddr_udimm_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &ddr_imem_udimm_1d_fw_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_imem_udimm_2d_img = {
+	.img_id = DDR_IMEM_UDIMM_2D_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &ddr_udimm_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &ddr_imem_udimm_2d_fw_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_dmem_udimm_1d_img = {
+	.img_id = DDR_DMEM_UDIMM_1D_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &ddr_udimm_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &ddr_dmem_udimm_1d_fw_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_dmem_udimm_2d_img = {
+	.img_id = DDR_DMEM_UDIMM_2D_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &ddr_udimm_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &ddr_dmem_udimm_2d_fw_hash
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t ddr_rdimm_fw_content_cert = {
+	.img_id = DDR_RDIMM_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &ddr_fw_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &ddr_fw_content_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &ddr_imem_rdimm_1d_fw_hash,
+			.data = {
+				.ptr = (void *)ddr_imem_rdimm_1d_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &ddr_imem_rdimm_2d_fw_hash,
+			.data = {
+				.ptr = (void *)ddr_imem_rdimm_2d_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &ddr_dmem_rdimm_1d_fw_hash,
+			.data = {
+				.ptr = (void *)ddr_dmem_rdimm_1d_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &ddr_dmem_rdimm_2d_fw_hash,
+			.data = {
+				.ptr = (void *)ddr_dmem_rdimm_2d_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+	}
+};
+
+static const auth_img_desc_t ddr_imem_rdimm_1d_img = {
+	.img_id = DDR_IMEM_RDIMM_1D_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &ddr_rdimm_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &ddr_imem_rdimm_1d_fw_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_imem_rdimm_2d_img = {
+	.img_id = DDR_IMEM_RDIMM_2D_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &ddr_rdimm_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &ddr_imem_rdimm_2d_fw_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_dmem_rdimm_1d_img = {
+	.img_id = DDR_DMEM_RDIMM_1D_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &ddr_rdimm_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &ddr_dmem_rdimm_1d_fw_hash
+			}
+		}
+	}
+};
+static const auth_img_desc_t ddr_dmem_rdimm_2d_img = {
+	.img_id = DDR_DMEM_RDIMM_2D_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &ddr_rdimm_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &ddr_dmem_rdimm_2d_fw_hash
+			}
+		}
+	}
+};
+#endif
+
+/*
+ * TBBR Chain of trust definition
+ */
+
+static const auth_img_desc_t * const cot_desc[] = {
+	[TRUSTED_KEY_CERT_ID]			=	&trusted_key_cert,
+	[SOC_FW_KEY_CERT_ID]			=	&soc_fw_key_cert,
+	[SOC_FW_CONTENT_CERT_ID]		=	&soc_fw_content_cert,
+	[BL31_IMAGE_ID]				=	&bl31_image,
+	[SOC_FW_CONFIG_ID]			=	&soc_fw_config,
+	[TRUSTED_OS_FW_KEY_CERT_ID]		=	&trusted_os_fw_key_cert,
+	[TRUSTED_OS_FW_CONTENT_CERT_ID]		=	&trusted_os_fw_content_cert,
+	[BL32_IMAGE_ID]				=	&bl32_image,
+	[BL32_EXTRA1_IMAGE_ID]			=	&bl32_extra1_image,
+	[BL32_EXTRA2_IMAGE_ID]			=	&bl32_extra2_image,
+	[TOS_FW_CONFIG_ID]			=	&tos_fw_config,
+	[NON_TRUSTED_FW_KEY_CERT_ID]		=	&non_trusted_fw_key_cert,
+	[NON_TRUSTED_FW_CONTENT_CERT_ID]	=	&non_trusted_fw_content_cert,
+	[BL33_IMAGE_ID]				=	&bl33_image,
+	[NT_FW_CONFIG_ID]			=	&nt_fw_config,
+#ifdef CONFIG_DDR_FIP_IMAGE
+	[DDR_FW_KEY_CERT_ID]			=	&ddr_fw_key_cert,
+	[DDR_UDIMM_FW_CONTENT_CERT_ID]		=	&ddr_udimm_fw_content_cert,
+	[DDR_RDIMM_FW_CONTENT_CERT_ID]		=	&ddr_rdimm_fw_content_cert,
+	[DDR_IMEM_UDIMM_1D_IMAGE_ID]		=	&ddr_imem_udimm_1d_img,
+	[DDR_IMEM_UDIMM_2D_IMAGE_ID]		=	&ddr_imem_udimm_2d_img,
+	[DDR_DMEM_UDIMM_1D_IMAGE_ID]		=	&ddr_dmem_udimm_1d_img,
+	[DDR_DMEM_UDIMM_2D_IMAGE_ID]		=	&ddr_dmem_udimm_2d_img,
+	[DDR_IMEM_RDIMM_1D_IMAGE_ID]		=	&ddr_imem_rdimm_1d_img,
+	[DDR_IMEM_RDIMM_2D_IMAGE_ID]		=	&ddr_imem_rdimm_2d_img,
+	[DDR_DMEM_RDIMM_1D_IMAGE_ID]		=	&ddr_dmem_rdimm_1d_img,
+	[DDR_DMEM_RDIMM_2D_IMAGE_ID]		=	&ddr_dmem_rdimm_2d_img,
+#endif
+};
+
+/* Register the CoT in the authentication module */
+REGISTER_COT(cot_desc);
diff --git a/drivers/nxp/console/16550_console.S b/drivers/nxp/console/16550_console.S
new file mode 100644
index 0000000..044d3d0
--- /dev/null
+++ b/drivers/nxp/console/16550_console.S
@@ -0,0 +1,319 @@
+/*
+ * Copyright (c) 2021, 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>
+
+/* UART16550 Registers */
+#define UARTTX			0x0
+#define UARTRX			0x0
+#define UARTDLL			0x0
+#define UARTIER			0x1
+#define UARTDLLM		0x1
+#define UARTFCR			0x2
+#define UARTLCR			0x3
+#define UARTLSR			0x5
+#define UARTMCR                 0x4
+
+/* FIFO Control Register bits */
+#define UARTFCR_FIFOMD_16450	(0 << 6)
+#define UARTFCR_FIFOMD_16550	(1 << 6)
+#define UARTFCR_RXTRIG_1	(0 << 6)
+#define UARTFCR_RXTRIG_4	(1 << 6)
+#define UARTFCR_RXTRIG_8	(2 << 6)
+#define UARTFCR_RXTRIG_16	(3 << 6)
+#define UARTFCR_TXTRIG_1	(0 << 4)
+#define UARTFCR_TXTRIG_4	(1 << 4)
+#define UARTFCR_TXTRIG_8	(2 << 4)
+#define UARTFCR_TXTRIG_16	(3 << 4)
+#define UARTFCR_DMAEN		(1 << 3)	/* Enable DMA mode */
+#define UARTFCR_TXCLR		(1 << 2)	/* Clear contents of Tx FIFO */
+#define UARTFCR_RXCLR		(1 << 1)	/* Clear contents of Rx FIFO */
+#define UARTFCR_FIFOEN		(1 << 0)	/* Enable the Tx/Rx FIFO */
+#define UARTFCR_64FIFO          (1 << 5)
+
+/* Line Control Register bits */
+#define UARTLCR_DLAB		(1 << 7)	/* Divisor Latch Access */
+#define UARTLCR_SETB		(1 << 6)	/* Set BREAK Condition */
+#define UARTLCR_SETP		(1 << 5)	/* Set Parity to LCR[4] */
+#define UARTLCR_EVEN		(1 << 4)	/* Even Parity Format */
+#define UARTLCR_PAR		(1 << 3)	/* Parity */
+#define UARTLCR_STOP		(1 << 2)	/* Stop Bit */
+#define UARTLCR_WORDSZ_5	0		/* Word Length of 5 */
+#define UARTLCR_WORDSZ_6	1		/* Word Length of 6 */
+#define UARTLCR_WORDSZ_7	2		/* Word Length of 7 */
+#define UARTLCR_WORDSZ_8	3		/* Word Length of 8 */
+
+/* Line Status Register bits */
+#define UARTLSR_RXFIFOEMT	(1 << 9)	/* Rx Fifo Empty */
+#define UARTLSR_TXFIFOFULL	(1 << 8)	/* Tx Fifo Full */
+#define UARTLSR_RXFIFOERR	(1 << 7)	/* Rx Fifo Error */
+#define UARTLSR_TEMT		(1 << 6)	/* Tx Shift Register Empty */
+#define UARTLSR_THRE		(1 << 5)	/* Tx Holding Register Empty */
+#define UARTLSR_BRK		(1 << 4)	/* Break Condition Detected */
+#define UARTLSR_FERR		(1 << 3)	/* Framing Error */
+#define UARTLSR_PERR		(1 << 3)	/* Parity Error */
+#define UARTLSR_OVRF		(1 << 2)	/* Rx Overrun Error */
+#define UARTLSR_RDR		(1 << 2)	/* Rx Data Ready */
+
+#define CONSOLE_T_16550_BASE	CONSOLE_T_BASE
+
+	/*
+	 * "core" functions are low-level implementations that don't require
+	 * writable memory and are thus safe to call in BL1 crash context.
+	 */
+	.globl nxp_console_16550_core_init
+	.globl nxp_console_16550_core_putc
+	.globl nxp_console_16550_core_getc
+	.globl nxp_console_16550_core_flush
+
+	.globl console_16550_putc
+	.globl console_16550_getc
+	.globl console_16550_flush
+
+	/* -----------------------------------------------
+	 * int nxp_console_16550_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, 0 on error
+	 * Clobber list : x1, x2, x3
+	 * -----------------------------------------------
+	 */
+func nxp_console_16550_core_init
+	/* Check the input base address */
+	cbz	x0, init_fail
+	/* Check baud rate and uart clock for sanity */
+	cbz	w1, init_fail
+	cbz	w2, init_fail
+
+	/* Program the baudrate */
+	/* Divisor =  Uart clock / (16 * baudrate) */
+	lsl	w2, w2, #4
+	udiv	w2, w1, w2
+	and	w1, w2, #0xff		/* w1 = DLL */
+	lsr	w2, w2, #8
+	and	w2, w2, #0xff		/* w2 = DLLM */
+	ldrb	w3, [x0, #UARTLCR]
+	orr	w3, w3, #UARTLCR_DLAB
+	strb	w3, [x0, #UARTLCR]	/* enable DLL, DLLM programming */
+	strb	w1, [x0, #UARTDLL]	/* program DLL */
+	strb	w2, [x0, #UARTDLLM]	/* program DLLM */
+	mov	w2, #~UARTLCR_DLAB
+	and	w3, w3, w2
+	strb	w3, [x0, #UARTLCR]	/* disable DLL, DLLM programming */
+
+	/* 8n1 */
+	mov	w3, #3
+	strb	w3, [x0, #UARTLCR]
+	/* no interrupt */
+	mov	w3, #0
+	strb	w3, [x0, #UARTIER]
+	/* enable fifo, DMA */
+	mov	w3, #(UARTFCR_FIFOEN |UARTFCR_TXCLR | UARTFCR_RXCLR)
+	strb	w3, [x0, #UARTFCR]
+	/* DTR + RTS */
+	mov	w3, #3
+	str	w3, [x0, #UARTMCR]
+	mov	w0, #1
+	ret
+init_fail:
+	mov	w0, #0
+	ret
+endfunc nxp_console_16550_core_init
+
+	.globl nxp_console_16550_register
+
+	/* -----------------------------------------------
+	 * int nxp_console_16550_register(uintptr_t baseaddr,
+	 *     uint32_t clock, uint32_t baud,
+	 *     console_t *console);
+	 * Function to initialize and register a new 16550
+	 * console. Storage passed in for the console struct
+	 * *must* be persistent (i.e. not from the stack).
+	 * If w1 (UART clock) is 0, initialisation will be
+	 * skipped, relying on previous code to have done
+	 * this already. w2 is ignored then as well.
+	 * In: x0 - UART register base address
+	 *     w1 - UART clock in Hz
+	 *     w2 - Baud rate (ignored if w1 is 0)
+	 *     x3 - pointer to empty console_t struct
+	 * Out: return 1 on success, 0 on error
+	 * Clobber list : x0, x1, x2, x6, x7, x14
+	 * -----------------------------------------------
+	 */
+func nxp_console_16550_register
+	mov	x7, x30
+	mov	x6, x3
+	cbz	x6, register_fail
+	str	x0, [x6, #CONSOLE_T_16550_BASE]
+
+	/* A clock rate of zero means to skip the initialisation. */
+	cbz	w1, register_16550
+
+	bl	nxp_console_16550_core_init
+	cbz	x0, register_fail
+
+register_16550:
+	mov	x0, x6
+	mov	x30, x7
+	finish_console_register 16550 putc=1, getc=1, flush=1
+
+register_fail:
+	ret	x7
+endfunc nxp_console_16550_register
+
+	/* --------------------------------------------------------
+	 * int console_16550_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 nxp_console_16550_core_putc
+#if ENABLE_ASSERTIONS
+	cmp	x1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Prepend '\r' to '\n' */
+	cmp	w0, #'\n'
+	b.ne	2f
+	/* Check if the transmit FIFO is full */
+1:	ldrb	w2, [x1, #UARTLSR]
+	and	w2, w2, #UARTLSR_THRE        /* #(UARTLSR_TEMT | UARTLSR_THRE)*/
+	cmp	w2, #(UARTLSR_THRE)
+	b.ne	1b
+	mov	w2, #'\r'
+	strb	w2, [x1, #UARTTX]
+	ldrb	w2, [x1, #UARTFCR]
+	orr	w2, w2, #UARTFCR_TXCLR
+
+	/* Check if the transmit FIFO is full */
+2:	ldrb	w2, [x1, #UARTLSR]
+	and	w2, w2, #(UARTLSR_THRE)
+	cmp	w2, #(UARTLSR_THRE)
+	b.ne	2b
+	strb	w0, [x1, #UARTTX]
+	ret
+endfunc nxp_console_16550_core_putc
+
+	/* --------------------------------------------------------
+	 * int console_16550_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_16550_putc
+#if ENABLE_ASSERTIONS
+	cmp	x1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x1, [x1, #CONSOLE_T_16550_BASE]
+	b	nxp_console_16550_core_putc
+endfunc console_16550_putc
+
+	/* ---------------------------------------------
+	 * int console_16550_core_getc(uintptr_t base_addr)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 on if no character is available.
+	 * In :  x0 - console base address
+	 * Out : w0 - character if available, else -1
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func nxp_console_16550_core_getc
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Check if the receive FIFO is empty */
+1:	ldrb	w1, [x0, #UARTLSR]
+	tbz	w1, #UARTLSR_RDR, 1b
+	ldrb	w0, [x0, #UARTRX]
+	ret
+no_char:
+	mov	w0, #ERROR_NO_PENDING_CHAR
+	ret
+endfunc nxp_console_16550_core_getc
+
+	/* ---------------------------------------------
+	 * int console_16550_getc(console_t *console)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 on 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_16550_getc
+#if ENABLE_ASSERTIONS
+	cmp	x1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x0, [x0, #CONSOLE_T_16550_BASE]
+	b	nxp_console_16550_core_getc
+endfunc console_16550_getc
+
+	/* ---------------------------------------------
+	 * int console_16550_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 : return -1 on error else return 0.
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func nxp_console_16550_core_flush
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Loop until the transmit FIFO is empty */
+1:	ldrb	w1, [x0, #UARTLSR]
+	and	w1, w1, #(UARTLSR_THRE)
+	cmp	w1, #(UARTLSR_THRE)
+	b.ne	1b
+
+	mov	w0, #0
+	ret
+endfunc nxp_console_16550_core_flush
+
+	/* ---------------------------------------------
+	 * int console_16550_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 : return -1 on error else return 0.
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func console_16550_flush
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x0, [x0, #CONSOLE_T_16550_BASE]
+	b	nxp_console_16550_core_flush
+endfunc console_16550_flush
diff --git a/drivers/nxp/console/console.mk b/drivers/nxp/console/console.mk
new file mode 100644
index 0000000..6174650
--- /dev/null
+++ b/drivers/nxp/console/console.mk
@@ -0,0 +1,46 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#
+#------------------------------------------------------------------------------
+#
+# Select the CORE files
+#
+# -----------------------------------------------------------------------------
+
+ifeq (${ADD_CONSOLE},)
+
+ADD_CONSOLE		:= 1
+
+PLAT_INCLUDES		+=	-I$(PLAT_DRIVERS_INCLUDE_PATH)/console
+
+ifeq ($(CONSOLE), NS16550)
+NXP_CONSOLE		:=	NS16550
+
+$(eval $(call add_define_val,NXP_CONSOLE,${NXP_CONSOLE}))
+
+CONSOLE_SOURCES		:=	$(PLAT_DRIVERS_PATH)/console/16550_console.S	\
+				$(PLAT_DRIVERS_PATH)/console/console_16550.c
+else
+ifeq ($(CONSOLE), PL011)
+CONSOLE_SOURCES		:=	drivers/arm/pl011/aarch64/pl011_console.S	\
+				${PLAT_DRIVERS_PATH}/console/console_pl011.c
+else
+	$(error -> CONSOLE not set!)
+endif
+endif
+
+ifeq (${BL_COMM_CONSOLE_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${CONSOLE_SOURCES}
+else
+ifeq (${BL2_CONSOLE_NEEDED},yes)
+BL2_SOURCES		+= ${CONSOLE_SOURCES}
+endif
+ifeq (${BL31_CONSOLE_NEEDED},yes)
+BL31_SOURCES		+= ${CONSOLE_SOURCES}
+endif
+endif
+endif
+# -----------------------------------------------------------------------------
diff --git a/drivers/nxp/console/console_16550.c b/drivers/nxp/console/console_16550.c
new file mode 100644
index 0000000..fa5c5bb
--- /dev/null
+++ b/drivers/nxp/console/console_16550.c
@@ -0,0 +1,33 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <dcfg.h>
+#include <lib/utils.h>
+#include <plat_console.h>
+
+/*
+ * Perform Arm specific early platform setup. At this moment we only initialize
+ * the console and the memory layout.
+ */
+void plat_console_init(uintptr_t nxp_console_addr, uint32_t uart_clk_div,
+			uint32_t baud)
+{
+	struct sysinfo sys;
+	static console_t nxp_console;
+
+	zeromem(&sys, sizeof(sys));
+	if (get_clocks(&sys)) {
+		ERROR("System clocks are not set\n");
+		panic();
+	}
+	nxp_console_16550_register(nxp_console_addr,
+			      (sys.freq_platform/uart_clk_div),
+			       baud, &nxp_console);
+}
diff --git a/drivers/nxp/console/console_pl011.c b/drivers/nxp/console/console_pl011.c
new file mode 100644
index 0000000..93f2fc2
--- /dev/null
+++ b/drivers/nxp/console/console_pl011.c
@@ -0,0 +1,35 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <dcfg.h>
+#include <drivers/arm/pl011.h>
+#include <drivers/console.h>
+#include <lib/utils.h>
+
+/*
+ * Perform Arm specific early platform setup. At this moment we only initialize
+ * the console and the memory layout.
+ */
+void plat_console_init(uintptr_t nxp_console_addr, uint32_t uart_clk_div,
+			uint32_t baud)
+{
+	struct sysinfo sys;
+	static console_t nxp_console;
+
+	zeromem(&sys, sizeof(sys));
+	if (get_clocks(&sys)) {
+		ERROR("System clocks are not set\n");
+		panic();
+	}
+
+	console_pl011_register(nxp_console_addr,
+			      (sys.freq_platform/uart_clk_div),
+			       baud, &nxp_console);
+}
diff --git a/drivers/nxp/crypto/caam/caam.mk b/drivers/nxp/crypto/caam/caam.mk
new file mode 100644
index 0000000..f929f53
--- /dev/null
+++ b/drivers/nxp/crypto/caam/caam.mk
@@ -0,0 +1,27 @@
+#
+# Copyright 2020-2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#
+
+ifeq (${ADD_CAAM},)
+
+ADD_CAAM		:= 1
+
+CAAM_DRIVER_SOURCES	+=  $(wildcard $(PLAT_DRIVERS_PATH)/crypto/caam/src/*.c)
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/crypto/caam
+
+ifeq (${BL_COMM_CRYPTO_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${CAAM_DRIVER_SOURCES}
+else
+ifeq (${BL2_CRYPTO_NEEDED},yes)
+BL2_SOURCES		+= ${CAAM_DRIVER_SOURCES}
+endif
+ifeq (${BL31_CRYPTO_NEEDED},yes)
+BL31_SOURCES		+= ${CAAM_DRIVER_SOURCES}
+endif
+endif
+
+endif
diff --git a/drivers/nxp/crypto/caam/src/auth/auth.mk b/drivers/nxp/crypto/caam/src/auth/auth.mk
new file mode 100644
index 0000000..d1f8c75
--- /dev/null
+++ b/drivers/nxp/crypto/caam/src/auth/auth.mk
@@ -0,0 +1,12 @@
+#
+# Copyright 2018-2020 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#
+
+SEC_DRIVERS_PATH	:=	drivers/nxp/crypto/caam
+
+ifeq (${TRUSTED_BOARD_BOOT},1)
+AUTH_SOURCES +=  $(wildcard $(SEC_DRIVERS_PATH)/src/auth/*.c)
+endif
diff --git a/drivers/nxp/crypto/caam/src/auth/hash.c b/drivers/nxp/crypto/caam/src/auth/hash.c
new file mode 100644
index 0000000..1665df1
--- /dev/null
+++ b/drivers/nxp/crypto/caam/src/auth/hash.c
@@ -0,0 +1,155 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include "caam.h"
+#include <common/debug.h>
+#include <drivers/auth/crypto_mod.h>
+
+#include "hash.h"
+#include "jobdesc.h"
+#include "sec_hw_specific.h"
+
+/* Since no Allocator is available . Taking a global static ctx.
+ * This would mean that only one active ctx can be there at a time.
+ */
+
+static struct hash_ctx glbl_ctx;
+
+static void hash_done(uint32_t *desc, uint32_t status, void *arg,
+		      void *job_ring)
+{
+	INFO("Hash Desc SUCCESS with status %x\n", status);
+}
+
+/***************************************************************************
+ * Function	: hash_init
+ * Arguments	: ctx - SHA context
+ * Return	: init,
+ * Description	: This function initializes the context for SHA calculation
+ ***************************************************************************/
+int hash_init(enum hash_algo algo, void **ctx)
+{
+	if (glbl_ctx.active == false) {
+		memset(&glbl_ctx, 0, sizeof(struct hash_ctx));
+		glbl_ctx.active = true;
+		glbl_ctx.algo = algo;
+		*ctx = &glbl_ctx;
+		return 0;
+	} else {
+		return -1;
+	}
+}
+
+/***************************************************************************
+ * Function	: hash_update
+ * Arguments	: ctx - SHA context
+ *		  buffer - Data
+ *		  length - Length
+ * Return	: -1 on error
+ *		  0 on SUCCESS
+ * Description	: This function creates SG entry of the data provided
+ ***************************************************************************/
+int hash_update(enum hash_algo algo, void *context, void *data_ptr,
+		unsigned int data_len)
+{
+	struct hash_ctx *ctx = context;
+	/* MAX_SG would be MAX_SG_ENTRIES + key + hdr + sg table */
+	if (ctx->sg_num >= MAX_SG) {
+		ERROR("Reached limit for calling %s\n", __func__);
+		ctx->active = false;
+		return -EINVAL;
+
+	}
+
+	if (ctx->algo != algo) {
+		ERROR("ctx for algo not correct\n");
+		ctx->active = false;
+		return -EINVAL;
+	}
+
+#if defined(SEC_MEM_NON_COHERENT) && defined(IMAGE_BL2)
+	flush_dcache_range((uintptr_t)data_ptr, data_len);
+	dmbsy();
+#endif
+
+#ifdef CONFIG_PHYS_64BIT
+	sec_out32(&ctx->sg_tbl[ctx->sg_num].addr_hi,
+		  (uint32_t) ((uintptr_t) data_ptr >> 32));
+#else
+	sec_out32(&ctx->sg_tbl[ctx->sg_num].addr_hi, 0x0);
+#endif
+	sec_out32(&ctx->sg_tbl[ctx->sg_num].addr_lo, (uintptr_t) data_ptr);
+
+	sec_out32(&ctx->sg_tbl[ctx->sg_num].len_flag,
+		  (data_len & SG_ENTRY_LENGTH_MASK));
+
+	ctx->sg_num++;
+
+	ctx->len += data_len;
+
+	return 0;
+}
+
+/***************************************************************************
+ * Function	: hash_final
+ * Arguments	: ctx - SHA context
+ * Return	: SUCCESS or FAILURE
+ * Description	: This function sets the final bit and enqueues the decriptor
+ ***************************************************************************/
+int hash_final(enum hash_algo algo, void *context, void *hash_ptr,
+	       unsigned int hash_len)
+{
+	int ret = 0;
+	struct hash_ctx *ctx = context;
+	uint32_t final = 0U;
+
+	struct job_descriptor jobdesc __aligned(CACHE_WRITEBACK_GRANULE);
+
+	jobdesc.arg = NULL;
+	jobdesc.callback = hash_done;
+
+	if (ctx->algo != algo) {
+		ERROR("ctx for algo not correct\n");
+		ctx->active = false;
+		return -EINVAL;
+	}
+
+	final = sec_in32(&ctx->sg_tbl[ctx->sg_num - 1].len_flag) |
+	    SG_ENTRY_FINAL_BIT;
+	sec_out32(&ctx->sg_tbl[ctx->sg_num - 1].len_flag, final);
+
+	dsb();
+
+	/* create the hw_rng descriptor */
+	cnstr_hash_jobdesc(jobdesc.desc, (uint8_t *) ctx->sg_tbl,
+			   ctx->len, hash_ptr);
+
+#if defined(SEC_MEM_NON_COHERENT) && defined(IMAGE_BL2)
+	flush_dcache_range((uintptr_t)ctx->sg_tbl,
+			   (sizeof(struct sg_entry) * MAX_SG));
+	inv_dcache_range((uintptr_t)hash_ptr, hash_len);
+
+	dmbsy();
+#endif
+
+	/* Finally, generate the requested random data bytes */
+	ret = run_descriptor_jr(&jobdesc);
+	if (ret != 0) {
+		ERROR("Error in running descriptor\n");
+		ret = -1;
+	}
+	ctx->active = false;
+	return ret;
+}
diff --git a/drivers/nxp/crypto/caam/src/auth/nxp_crypto.c b/drivers/nxp/crypto/caam/src/auth/nxp_crypto.c
new file mode 100644
index 0000000..646e981
--- /dev/null
+++ b/drivers/nxp/crypto/caam/src/auth/nxp_crypto.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <stddef.h>
+#include <string.h>
+
+#include "caam.h"
+#include <common/debug.h>
+#include <drivers/auth/crypto_mod.h>
+
+#include "hash.h"
+#include "rsa.h"
+
+#define LIB_NAME		"NXP crypto"
+
+/*
+ * Initialize the library and export the descriptor
+ */
+static void init(void)
+{
+	/* Initialize NXP crypto library`:*/
+	NOTICE("Initializing & configuring SEC block.\n");
+
+	if (config_sec_block() < 0) {
+		ERROR("Init & config failure for caam.\n");
+	}
+}
+
+/*
+ * Verify a signature.
+ *
+ * For IMG_PLAT - data points to a PKCS#1.5 encoded HASH
+ * sig_alg will be RSA or ECC
+ * Parameters are passed using the DER encoding format following the ASN.1
+ * structures detailed above.
+ */
+static int verify_signature(void *data_ptr, unsigned int data_len,
+			    void *sig_ptr, unsigned int sig_len,
+			    void *sign_alg, unsigned int sig_alg_len,
+			    void *pk_ptr, unsigned int pk_len)
+{
+	int ret = CRYPTO_SUCCESS;
+
+	enum sig_alg alg = *(enum sig_alg *)sign_alg;
+
+	switch (alg) {
+	case RSA:
+		NOTICE("Verifying RSA\n");
+		ret = rsa_verify_signature(data_ptr, data_len, sig_ptr, sig_len,
+					   pk_ptr, pk_len);
+		break;
+	case ECC:
+	default:
+		ret = CRYPTO_ERR_SIGNATURE;
+		break;
+	}
+
+	if (ret != 0) {
+		ERROR("RSA verification Failed\n");
+	}
+	return ret;
+
+}
+
+/*
+ * Match a hash
+ *
+ * Digest info is passed as a table of SHA-26 hashes and digest_info_len
+ * is number of entries in the table
+ * This implementation is very specific to the CSF header parser ROTPK
+ * comparison.
+ */
+static int verify_hash(void *data_ptr, unsigned int data_len,
+		       void *digest_info_ptr, unsigned int digest_info_len)
+{
+	void *ctx = NULL;
+	int i = 0, ret = 0;
+	enum hash_algo algo = SHA256;
+	uint8_t hash[SHA256_BYTES] __aligned(CACHE_WRITEBACK_GRANULE) = {0};
+	uint32_t digest_size = SHA256_BYTES;
+	uint8_t *hash_tbl = digest_info_ptr;
+
+	NOTICE("Verifying hash\n");
+	ret = hash_init(algo, &ctx);
+	if (ret != 0) {
+		return CRYPTO_ERR_HASH;
+	}
+
+	/* Update hash with that of SRK table */
+	ret = hash_update(algo, ctx, data_ptr, data_len);
+	if (ret != 0) {
+		return CRYPTO_ERR_HASH;
+	}
+
+	/* Copy hash at destination buffer */
+	ret = hash_final(algo, ctx, hash, digest_size);
+	if (ret != 0) {
+		return CRYPTO_ERR_HASH;
+	}
+
+	VERBOSE("%s Calculated hash\n", __func__);
+	for (i = 0; i < SHA256_BYTES/4; i++) {
+		VERBOSE("%x\n", *((uint32_t *)hash + i));
+	}
+
+	for (i = 0; i < digest_info_len; i++) {
+		if (memcmp(hash, (hash_tbl + (i * digest_size)),
+			   digest_size) == 0) {
+			return CRYPTO_SUCCESS;
+		}
+	}
+
+	return CRYPTO_ERR_HASH;
+}
+
+/*
+ * Register crypto library descriptor
+ */
+REGISTER_CRYPTO_LIB(LIB_NAME, init, verify_signature, verify_hash, NULL);
diff --git a/drivers/nxp/crypto/caam/src/auth/rsa.c b/drivers/nxp/crypto/caam/src/auth/rsa.c
new file mode 100644
index 0000000..0c44462
--- /dev/null
+++ b/drivers/nxp/crypto/caam/src/auth/rsa.c
@@ -0,0 +1,179 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include "caam.h"
+#include <common/debug.h>
+#include <drivers/auth/crypto_mod.h>
+
+#include "jobdesc.h"
+#include "rsa.h"
+#include "sec_hw_specific.h"
+
+/* This array contains DER value for SHA-256 */
+static const uint8_t hash_identifier[] = {
+	0x30, 0x31, 0x30, 0x0d, 0x06, 0x09, 0x60,
+	0x86, 0x48, 0x01, 0x65, 0x03, 0x04, 0x02, 0x01, 0x05, 0x00,
+	0x04, 0x20
+};
+
+static void rsa_done(uint32_t *desc, uint32_t status, void *arg,
+		     void *job_ring)
+{
+	INFO("RSA Desc SUCCESS with status %x\n", status);
+}
+
+static int rsa_public_verif_sec(uint8_t *sign, uint8_t *to,
+				uint8_t *rsa_pub_key, uint32_t klen)
+{
+	int ret = 0;
+	struct rsa_context ctx __aligned(CACHE_WRITEBACK_GRANULE);
+	struct job_descriptor jobdesc __aligned(CACHE_WRITEBACK_GRANULE);
+
+	jobdesc.arg = NULL;
+	jobdesc.callback = rsa_done;
+
+	memset(&ctx, 0, sizeof(struct rsa_context));
+
+	ctx.pkin.a = sign;
+	ctx.pkin.a_siz = klen;
+	ctx.pkin.n = rsa_pub_key;
+	ctx.pkin.n_siz = klen;
+	ctx.pkin.e = rsa_pub_key + klen;
+	ctx.pkin.e_siz = klen;
+
+	cnstr_jobdesc_pkha_rsaexp(jobdesc.desc, &ctx.pkin, to, klen);
+
+#if defined(SEC_MEM_NON_COHERENT) && defined(IMAGE_BL2)
+	flush_dcache_range((uintptr_t)sign, klen);
+	flush_dcache_range((uintptr_t)rsa_pub_key, 2 * klen);
+	flush_dcache_range((uintptr_t)&ctx.pkin, sizeof(ctx.pkin));
+	inv_dcache_range((uintptr_t)to, klen);
+
+	dmbsy();
+	dsbsy();
+	isb();
+#endif
+
+	/* Finally, generate the requested random data bytes */
+	ret = run_descriptor_jr(&jobdesc);
+	if (ret != 0) {
+		ERROR("Error in running descriptor\n");
+		ret = -1;
+	}
+#if defined(SEC_MEM_NON_COHERENT) && defined(IMAGE_BL2)
+	inv_dcache_range((uintptr_t)to, klen);
+	dmbsy();
+	dsbsy();
+	isb();
+#endif
+	return ret;
+}
+
+/*
+ * Construct encoded hash EM' wrt PKCSv1.5. This function calculates the
+ * pointers for padding, DER value and hash. And finally, constructs EM'
+ * which includes hash of complete CSF header and ESBC image. If SG flag
+ * is on, hash of SG table and entries is also included.
+ */
+static int construct_img_encoded_hash_second(uint8_t *hash, uint8_t hash_len,
+					     uint8_t *encoded_hash_second,
+					     unsigned int key_len)
+{
+	/*
+	 * RSA PKCSv1.5 encoding format for encoded message is below
+	 * EM = 0x0 || 0x1 || PS || 0x0 || DER || Hash
+	 * PS is Padding String
+	 * DER is DER value for SHA-256
+	 * Hash is SHA-256 hash
+	 * *********************************************************
+	 * representative points to first byte of EM initially and is
+	 * filled with 0x0
+	 * representative is incremented by 1 and second byte is filled
+	 * with 0x1
+	 * padding points to third byte of EM
+	 * digest points to full length of EM - 32 bytes
+	 * hash_id (DER value) points to 19 bytes before pDigest
+	 * separator is one byte which separates padding and DER
+	 */
+
+	unsigned int len;
+	uint8_t *representative;
+	uint8_t *padding, *digest;
+	uint8_t *hash_id, *separator;
+	int i;
+	int ret = 0;
+
+	if (hash_len != SHA256_BYTES) {
+		return -1;
+	}
+
+	/* Key length = Modulus length */
+	len = (key_len / 2U) - 1U;
+	representative = encoded_hash_second;
+	representative[0] = 0U;
+	representative[1] = 1U;	/* block type 1 */
+
+	padding = &representative[2];
+	digest = &representative[1] + len - 32;
+	hash_id = digest - sizeof(hash_identifier);
+	separator = hash_id - 1;
+
+	/* fill padding area pointed by padding with 0xff */
+	memset(padding, 0xff, separator - padding);
+
+	/* fill byte pointed by separator */
+	*separator = 0U;
+
+	/* fill SHA-256 DER value  pointed by HashId */
+	memcpy(hash_id, hash_identifier, sizeof(hash_identifier));
+
+	/* fill hash pointed by Digest */
+	for (i = 0; i < SHA256_BYTES; i++) {
+		digest[i] = hash[i];
+	}
+
+	return ret;
+}
+
+int rsa_verify_signature(void *hash_ptr, unsigned int hash_len,
+			 void *sig_ptr, unsigned int sig_len,
+			 void *pk_ptr, unsigned int pk_len)
+{
+	uint8_t img_encoded_hash_second[RSA_4K_KEY_SZ_BYTES];
+	uint8_t encoded_hash[RSA_4K_KEY_SZ_BYTES] __aligned(CACHE_WRITEBACK_GRANULE);
+	int ret = 0;
+
+	ret = construct_img_encoded_hash_second(hash_ptr, hash_len,
+						img_encoded_hash_second,
+						pk_len);
+	if (ret != 0) {
+		ERROR("Encoded Hash Failure\n");
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	ret = rsa_public_verif_sec(sig_ptr, encoded_hash, pk_ptr, pk_len / 2);
+	if (ret != 0) {
+		ERROR("RSA signature Failure\n");
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	ret = memcmp(img_encoded_hash_second, encoded_hash, sig_len);
+	if (ret != 0) {
+		ERROR("Comparison Failure\n");
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	return CRYPTO_SUCCESS;
+}
diff --git a/drivers/nxp/crypto/caam/src/caam.c b/drivers/nxp/crypto/caam/src/caam.c
new file mode 100644
index 0000000..e594f7b
--- /dev/null
+++ b/drivers/nxp/crypto/caam/src/caam.c
@@ -0,0 +1,339 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include "caam.h"
+#include <common/debug.h>
+#include "jobdesc.h"
+#include "sec_hw_specific.h"
+
+static uintptr_t g_nxp_caam_addr;
+static void *job_ring;
+
+uintptr_t get_caam_addr(void)
+{
+	if (g_nxp_caam_addr == 0) {
+		ERROR("Sec Init is not done.\n");
+		panic();
+	}
+	return g_nxp_caam_addr;
+}
+
+/* This function sets the TZ bit for the Job ring number passed as @num */
+static void config_tz(int num)
+{
+	uint32_t jricid;
+
+	/* Setting TZ bit of job ring */
+	switch (num) {
+	case 0:
+		jricid = sec_in32(g_nxp_caam_addr + SEC_REG_JR0ICIDR_MS_OFFSET);
+		sec_out32(g_nxp_caam_addr + SEC_REG_JR0ICIDR_MS_OFFSET,
+			  jricid | JRICID_MS_TZ);
+		break;
+	case 1:
+		jricid = sec_in32(g_nxp_caam_addr + SEC_REG_JR1ICIDR_MS_OFFSET);
+		sec_out32(g_nxp_caam_addr + SEC_REG_JR1ICIDR_MS_OFFSET,
+			  jricid | JRICID_MS_TZ);
+		break;
+	case 2:
+		jricid = sec_in32(g_nxp_caam_addr + SEC_REG_JR2ICIDR_MS_OFFSET);
+		sec_out32(g_nxp_caam_addr + SEC_REG_JR2ICIDR_MS_OFFSET,
+			  jricid | JRICID_MS_TZ);
+		break;
+	case 3:
+		jricid = sec_in32(g_nxp_caam_addr + SEC_REG_JR3ICIDR_MS_OFFSET);
+		sec_out32(g_nxp_caam_addr + SEC_REG_JR3ICIDR_MS_OFFSET,
+			  jricid | JRICID_MS_TZ);
+		break;
+	default:
+		break;
+	}
+}
+
+/* This function checks if Virtualization is enabled for JR and
+ * accordingly sets the bot for starting JR<num> in JRSTARTR register
+ */
+static inline void start_jr(int num)
+{
+	uint32_t ctpr = sec_in32((g_nxp_caam_addr + SEC_REG_CTPR_MS_OFFSET));
+	uint32_t tmp = sec_in32((g_nxp_caam_addr + SEC_REG_JRSTARTR_OFFSET));
+	uint32_t scfgr = sec_in32((g_nxp_caam_addr + SEC_REG_SCFGR_OFFSET));
+	bool start = false;
+
+	if ((ctpr & CTPR_VIRT_EN_INC) != 0U) {
+		if (((ctpr & CTPR_VIRT_EN_POR) != 0U) ||
+		    ((scfgr & SCFGR_VIRT_EN) != 0U)) {
+			start = true;
+		}
+	} else {
+		if ((ctpr & CTPR_VIRT_EN_POR) != 0U) {
+			start = true;
+		}
+	}
+
+	if (start == true) {
+		switch (num) {
+		case 0:
+			tmp |= JRSTARTR_STARTJR0;
+			break;
+		case 1:
+			tmp |= JRSTARTR_STARTJR1;
+			break;
+		case 2:
+			tmp |= JRSTARTR_STARTJR2;
+			break;
+		case 3:
+			tmp |= JRSTARTR_STARTJR3;
+			break;
+		default:
+			break;
+		}
+	}
+	sec_out32((g_nxp_caam_addr + SEC_REG_JRSTARTR_OFFSET), tmp);
+}
+
+/* This functions configures the Job Ring
+ * JR3 is reserved for use by Secure world
+ */
+static int configure_jr(int num)
+{
+	int ret;
+	void *reg_base_addr;
+
+	switch (num) {
+	case 0:
+		reg_base_addr = (void *)(g_nxp_caam_addr + CAAM_JR0_OFFSET);
+		break;
+	case 1:
+		reg_base_addr = (void *)(g_nxp_caam_addr + CAAM_JR1_OFFSET);
+		break;
+	case 2:
+		reg_base_addr = (void *)(g_nxp_caam_addr + CAAM_JR2_OFFSET);
+		break;
+	case 3:
+		reg_base_addr = (void *)(g_nxp_caam_addr + CAAM_JR3_OFFSET);
+		break;
+	default:
+		break;
+	}
+
+	/* Initialize the JR library */
+	ret = sec_jr_lib_init();
+	if (ret != 0) {
+		ERROR("Error in sec_jr_lib_init");
+		return -1;
+	}
+
+	start_jr(num);
+
+	/* Do HW configuration of the JR */
+	job_ring = init_job_ring(SEC_NOTIFICATION_TYPE_POLL, 0, 0,
+				 reg_base_addr, 0);
+
+	if (job_ring == NULL) {
+		ERROR("Error in init_job_ring");
+		return -1;
+	}
+
+	return ret;
+}
+
+/* TBD - Configures and locks the ICID values for various JR */
+static inline void configure_icid(void)
+{
+}
+
+/* TBD configures the TZ settings of RTIC */
+static inline void configure_rtic(void)
+{
+}
+
+int sec_init(uintptr_t nxp_caam_addr)
+{
+	g_nxp_caam_addr = nxp_caam_addr;
+	return config_sec_block();
+}
+
+/* This function configure SEC block:
+ * - It does basic parameter setting
+ * - Configures the default Job ring assigned to TZ /secure world
+ * - Instantiates the RNG
+ */
+int config_sec_block(void)
+{
+	int ret = 0;
+	uint32_t mcfgr;
+
+	if (g_nxp_caam_addr == 0) {
+		ERROR("Sec Init is not done.\n");
+		return -1;
+	} else if (job_ring != NULL) {
+		NOTICE("Sec is already initialized and configured.\n");
+		return ret;
+	}
+
+	mcfgr = sec_in32(g_nxp_caam_addr + SEC_REG_MCFGR_OFFSET);
+
+	/* Modify CAAM Read/Write attributes
+	 * AXI Write - Cacheable, WB and WA
+	 * AXI Read - Cacheable, RA
+	 */
+#if defined(CONFIG_ARCH_LS2080A) || defined(CONFIG_ARCH_LS2088A)
+	mcfgr = (mcfgr & ~MCFGR_AWCACHE_MASK) | (0xb << MCFGR_AWCACHE_SHIFT);
+	mcfgr = (mcfgr & ~MCFGR_ARCACHE_MASK) | (0x6 << MCFGR_ARCACHE_SHIFT);
+#else
+	mcfgr = (mcfgr & ~MCFGR_AWCACHE_MASK) | (0x2 << MCFGR_AWCACHE_SHIFT);
+#endif
+
+	/* Set PS bit to 1 */
+#ifdef CONFIG_PHYS_64BIT
+	mcfgr |= (1 << MCFGR_PS_SHIFT);
+#endif
+	sec_out32(g_nxp_caam_addr + SEC_REG_MCFGR_OFFSET, mcfgr);
+
+	/* Asssign ICID to all Job rings and lock them for usage */
+	configure_icid();
+
+	/* Configure the RTIC */
+	configure_rtic();
+
+	/* Configure the default JR for usage */
+	ret = configure_jr(DEFAULT_JR);
+	if (ret != 0) {
+		ERROR("\nFSL_JR: configuration failure\n");
+		return -1;
+	}
+	/* Do TZ configuration of default JR for sec firmware */
+	config_tz(DEFAULT_JR);
+
+#ifdef CONFIG_RNG_INIT
+	/* Instantiate the RNG */
+	ret = hw_rng_instantiate();
+	if (ret != 0) {
+		ERROR("\nRNG Instantiation failure\n");
+		return -1;
+	}
+#endif
+
+	return ret;
+}
+
+/* This function is used for sumbitting job to the Job Ring
+ * [param] [in] - jobdesc to be submitted
+ * Return - -1 in case of error and 0 in case of SUCCESS
+ */
+int run_descriptor_jr(struct job_descriptor *jobdesc)
+{
+	int i = 0, ret = 0;
+	uint32_t *desc_addr = jobdesc->desc;
+	uint32_t desc_len = desc_length(jobdesc->desc);
+	uint32_t desc_word;
+
+	for (i = 0; i < desc_len; i++) {
+		desc_word = desc_addr[i];
+		VERBOSE("%x\n", desc_word);
+		sec_out32((uint32_t *)&desc_addr[i], desc_word);
+	}
+	dsb();
+
+#if defined(SEC_MEM_NON_COHERENT) && defined(IMAGE_BL2)
+	flush_dcache_range((uintptr_t)desc_addr, desc_len * 4);
+	dmbsy();
+	dsbsy();
+	isb();
+#endif
+
+	ret = enq_jr_desc(job_ring, jobdesc);
+	if (ret == 0) {
+		VERBOSE("JR enqueue done...\n");
+	} else {
+		ERROR("Error in Enqueue\n");
+		return ret;
+	}
+
+	VERBOSE("Dequeue in progress");
+
+	ret = dequeue_jr(job_ring, -1);
+	if (ret >= 0) {
+		VERBOSE("Dequeue of %x desc success\n", ret);
+		ret = 0;
+	} else {
+		ERROR("deq_ret %x\n", ret);
+		ret = -1;
+	}
+
+	return ret;
+}
+
+/* this function returns a random number using HW RNG Algo
+ * In case of failure, random number returned is 0
+ * prngWidth = 0 - 32 bit random number
+ * prngWidth > 0 means 64 bit random number
+ */
+unsigned long long get_random(int rngWidth)
+{
+	unsigned long long result = 0;
+	uint8_t rand_byte[64] __aligned(CACHE_WRITEBACK_GRANULE);
+	uint8_t rand_byte_swp[8];
+	int bytes = 0;
+	int i = 0;
+	int ret = 0;
+
+#ifdef CAAM_TEST
+	rand_byte[0] = U(0x12);
+	rand_byte[1] = U(0x34);
+	rand_byte[2] = U(0x56);
+	rand_byte[3] = U(0x78);
+	rand_byte[4] = U(0x9a);
+	rand_byte[5] = U(0xbc);
+	rand_byte[6] = U(0xde);
+	rand_byte[7] = U(0xf1);
+#endif
+
+	if (rngWidth == 0U) {
+		bytes = 4;
+	} else {
+		bytes = 8;
+	}
+
+	memset(rand_byte, 0, 64);
+
+	ret = get_rand_bytes_hw(rand_byte, bytes);
+
+	for (i = 0; i < bytes; i++) {
+		if (ret != 0) {
+			/* Return 0 in case of failure */
+			rand_byte_swp[i] = 0;
+		} else {
+			rand_byte_swp[i] = rand_byte[bytes - i - 1];
+			result = (result << 8) | rand_byte_swp[i];
+		}
+	}
+
+	INFO("result %llx\n", result);
+
+	return result;
+
+} /* _get_RNG() */
+
+unsigned int _get_hw_unq_key(uint64_t hw_key_phy_addr, unsigned int size)
+{
+	int ret = 0;
+	uint8_t *hw_key = (uint8_t *) ptov((phys_addr_t *) hw_key_phy_addr);
+
+	ret = get_hw_unq_key_blob_hw(hw_key, size);
+
+	return ret;
+}
diff --git a/drivers/nxp/crypto/caam/src/hw_key_blob.c b/drivers/nxp/crypto/caam/src/hw_key_blob.c
new file mode 100644
index 0000000..0720695
--- /dev/null
+++ b/drivers/nxp/crypto/caam/src/hw_key_blob.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "caam.h"
+#include <common/debug.h>
+#include "jobdesc.h"
+#include "sec_hw_specific.h"
+
+
+/* Callback function after Instantiation decsriptor is submitted to SEC
+ */
+static void blob_done(uint32_t *desc, uint32_t status, void *arg,
+		      void *job_ring)
+{
+	INFO("Blob Desc SUCCESS with status %x\n", status);
+}
+
+/* @brief Submit descriptor to create blob
+ * @retval 0 on success
+ * @retval -1 on error
+ */
+int get_hw_unq_key_blob_hw(uint8_t *hw_key, int size)
+{
+	int ret = 0;
+	int i = 0;
+
+	uint32_t key_sz = KEY_IDNFR_SZ_BYTES;
+	uint8_t key_data[KEY_IDNFR_SZ_BYTES];
+	uint8_t in_data[16];
+	uint8_t out_data[16 + KEY_BLOB_SIZE + MAC_SIZE];
+	struct job_descriptor desc __aligned(CACHE_WRITEBACK_GRANULE);
+	struct job_descriptor *jobdesc = &desc;
+	uint32_t in_sz = 16U;
+
+	/* Output blob will have 32 bytes key blob in beginning and
+	 * 16 byte HMAC identifier at end of data blob
+	 */
+	uint32_t out_sz = in_sz + KEY_BLOB_SIZE + MAC_SIZE;
+
+	uint32_t operation = CMD_OPERATION | OP_TYPE_ENCAP_PROTOCOL |
+	    OP_PCLID_BLOB | BLOB_PROTO_INFO;
+
+	memset(key_data, 0xff, KEY_IDNFR_SZ_BYTES);
+	memset(in_data, 0x00, in_sz);
+	memset(out_data, 0x00, in_sz);
+
+	jobdesc->arg = NULL;
+	jobdesc->callback = blob_done;
+
+	INFO("\nGenerating Master Key Verification Blob.\n");
+
+	/* Create the hw_rng descriptor */
+	ret = cnstr_hw_encap_blob_jobdesc(jobdesc->desc, key_data, key_sz,
+					  CLASS_2, in_data, in_sz, out_data,
+					  out_sz, operation);
+
+	/* Finally, generate the blob. */
+	ret = run_descriptor_jr(jobdesc);
+	if (ret != 0) {
+		ERROR("Error in running hw unq key blob descriptor\n");
+		return -1;
+	}
+	/* Copying alternate bytes of the Master Key Verification Blob.
+	 */
+	for (i = 0; i < size; i++) {
+		hw_key[i] = out_data[2 * i];
+	}
+
+	return ret;
+}
diff --git a/drivers/nxp/crypto/caam/src/jobdesc.c b/drivers/nxp/crypto/caam/src/jobdesc.c
new file mode 100644
index 0000000..f559c4b
--- /dev/null
+++ b/drivers/nxp/crypto/caam/src/jobdesc.c
@@ -0,0 +1,236 @@
+/*
+ * Copyright 2017-2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "caam.h"
+#include <common/debug.h>
+#include "jobdesc.h"
+#include "rsa.h"
+#include "sec_hw_specific.h"
+
+
+/* Return Length of desctiptr from first word */
+uint32_t desc_length(uint32_t *desc)
+{
+	return desc[0] & DESC_LEN_MASK;
+}
+
+/*Update start index in first word of descriptor */
+void desc_update_start_index(uint32_t *desc, uint32_t index)
+{
+	desc[0] |= (index << DESC_START_SHIFT);
+}
+
+/* Initialize the descriptor */
+void desc_init(uint32_t *desc)
+{
+	*desc = 0;
+}
+
+/* Add word in the descriptor and increment the length */
+void desc_add_word(uint32_t *desc, uint32_t word)
+{
+	uint32_t len = desc_length(desc);
+
+	/* Add Word at Last */
+	uint32_t *last = desc + len;
+	*last = word;
+
+	/* Increase the length */
+	desc[0] += 1;
+}
+
+/* Add Pointer to the descriptor */
+void desc_add_ptr(uint32_t *desc, phys_addr_t *ptr)
+{
+	uint32_t len = desc_length(desc);
+
+	/* Add Word at Last */
+	phys_addr_t *last = (phys_addr_t *) (desc + len);
+
+#ifdef CONFIG_PHYS_64BIT
+	ptr_addr_t *ptr_addr = (ptr_addr_t *) last;
+
+	ptr_addr->high = PHYS_ADDR_HI(ptr);
+	ptr_addr->low = PHYS_ADDR_LO(ptr);
+#else
+	*last = ptr;
+#endif
+
+	/* Increase the length */
+	desc[0] += (uint32_t) (sizeof(phys_addr_t) / sizeof(uint32_t));
+}
+
+/* Descriptor to generate Random words */
+int cnstr_rng_jobdesc(uint32_t *desc, uint32_t state_handle,
+		      uint32_t *add_inp, uint32_t add_ip_len,
+		      uint8_t *out_data, uint32_t len)
+{
+	phys_addr_t *phys_addr_out = vtop(out_data);
+
+	/* Current descriptor support only 64K length */
+	if (len > U(0xffff))
+		return -1;
+	/* Additional Input not supported by current descriptor */
+	if (add_ip_len > 0U)
+		return -1;
+
+	VERBOSE("Constructing descriptor\n");
+	desc_init(desc);
+	/* Class1 Alg Operation,RNG Optype, Generate */
+	desc_add_word(desc, U(0xb0800000));
+	desc_add_word(desc, U(0x82500000) | (state_handle << ALG_AAI_SH_SHIFT));
+	desc_add_word(desc, U(0x60340000) | len);
+	desc_add_ptr(desc, phys_addr_out);
+
+	return 0;
+
+}
+
+/* Construct descriptor to instantiate RNG */
+int cnstr_rng_instantiate_jobdesc(uint32_t *desc)
+{
+	desc_init(desc);
+	desc_add_word(desc, U(0xb0800000));
+	/* Class1 Alg Operation,RNG Optype, Instantiate */
+	desc_add_word(desc, U(0x82500004));
+	/* Wait for done */
+	desc_add_word(desc, U(0xa2000001));
+	/*Load to clear written */
+	desc_add_word(desc, U(0x10880004));
+	/*Pri Mode Reg clear */
+	desc_add_word(desc, U(0x00000001));
+	/* Generate secure keys */
+	desc_add_word(desc, U(0x82501000));
+
+	return 0;
+}
+
+/* Construct descriptor to generate hw key blob */
+int cnstr_hw_encap_blob_jobdesc(uint32_t *desc,
+				uint8_t *key_idnfr, uint32_t key_sz,
+				uint32_t key_class, uint8_t *plain_txt,
+				uint32_t in_sz, uint8_t *enc_blob,
+				uint32_t out_sz, uint32_t operation)
+{
+	phys_addr_t *phys_key_idnfr, *phys_addr_in, *phys_addr_out;
+	int i = 0;
+
+	phys_key_idnfr = vtop((void *)key_idnfr);
+	phys_addr_in = vtop((void *)plain_txt);
+	phys_addr_out = vtop((void *)enc_blob);
+
+	desc_init(desc);
+
+	desc_add_word(desc, U(0xb0800000));
+
+	/* Key Identifier */
+	desc_add_word(desc, (key_class | key_sz));
+	desc_add_ptr(desc, phys_key_idnfr);
+
+	/* Source Address */
+	desc_add_word(desc, U(0xf0400000));
+	desc_add_ptr(desc, phys_addr_in);
+
+	/* In Size = 0x10 */
+	desc_add_word(desc, in_sz);
+
+	/* Out Address */
+	desc_add_word(desc, U(0xf8400000));
+	desc_add_ptr(desc, phys_addr_out);
+
+	/* Out Size = 0x10 */
+	desc_add_word(desc, out_sz);
+
+	/* Operation */
+	desc_add_word(desc, operation);
+
+	for (i = 0; i < 15; i++)
+		VERBOSE("desc word %x\n", desc[i]);
+
+	return 0;
+}
+
+/***************************************************************************
+ * Function	: inline_cnstr_jobdesc_pkha_rsaexp
+ * Arguments	: desc - Pointer to Descriptor
+ *		  pkin - Pointer to Input Params
+ *		  out - Pointer to Output
+ *		  out_siz - Output Size
+ * Return	: Void
+ * Description	: Creates the descriptor for PKHA RSA
+ ***************************************************************************/
+void cnstr_jobdesc_pkha_rsaexp(uint32_t *desc,
+			       struct pk_in_params *pkin, uint8_t *out,
+			       uint32_t out_siz)
+{
+	phys_addr_t *ptr_addr_e, *ptr_addr_a, *ptr_addr_n, *ptr_addr_out;
+
+	ptr_addr_e = vtop((void *)(pkin->e));
+	ptr_addr_a = vtop((void *)(pkin->a));
+	ptr_addr_n = vtop((void *)(pkin->n));
+	ptr_addr_out = vtop((void *)(out));
+
+	desc_init(desc);
+	desc_add_word(desc, U(0xb0800000));
+	desc_add_word(desc, U(0x02010000) | pkin->e_siz);
+	desc_add_ptr(desc, ptr_addr_e);
+	desc_add_word(desc, U(0x220c0000) | pkin->a_siz);
+	desc_add_ptr(desc, ptr_addr_a);
+	desc_add_word(desc, U(0x22080000) | pkin->n_siz);
+	desc_add_ptr(desc, ptr_addr_n);
+	desc_add_word(desc, U(0x81800006));
+	desc_add_word(desc, U(0x620d0000) | out_siz);
+	desc_add_ptr(desc, ptr_addr_out);
+}
+
+/***************************************************************************
+ * Function	: inline_cnstr_jobdesc_sha256
+ * Arguments	: desc - Pointer to Descriptor
+ *		  msg - Pointer to SG Table
+ *		  msgsz - Size of SG Table
+ *		  digest - Pointer to Output Digest
+ * Return	: Void
+ * Description	: Creates the descriptor for SHA256 HASH calculation
+ ***************************************************************************/
+void cnstr_hash_jobdesc(uint32_t *desc, uint8_t *msg, uint32_t msgsz,
+			uint8_t *digest)
+{
+	/* SHA 256 , output is of length 32 words */
+	phys_addr_t *ptr_addr_in, *ptr_addr_out;
+
+	ptr_addr_in = (void *)vtop(msg);
+	ptr_addr_out = (void *)vtop(digest);
+
+	desc_init(desc);
+	desc_add_word(desc, U(0xb0800000));
+
+	/* Operation Command
+	 * OP_TYPE_CLASS2_ALG | OP_ALG_ALGSEL_SHA256 | OP_ALG_AAI_HASH |
+	 * OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT | OP_ALG_ICV_OFF)
+	 */
+	desc_add_word(desc, U(0x8443000d));
+
+	if (msgsz > U(0xffff)) {
+		desc_add_word(desc, U(0x25540000));	/* FIFO Load */
+		desc_add_ptr(desc, ptr_addr_in);	/* Pointer to msg */
+		desc_add_word(desc, msgsz);	/* Size */
+		desc_add_word(desc, U(0x54200020));	/* FIFO Store */
+		desc_add_ptr(desc, ptr_addr_out);	/* Pointer to Result */
+	} else {
+		desc_add_word(desc, U(0x25140000) | msgsz);
+		desc_add_ptr(desc, ptr_addr_in);
+		desc_add_word(desc, U(0x54200020));
+		desc_add_ptr(desc, ptr_addr_out);
+	}
+
+}
diff --git a/drivers/nxp/crypto/caam/src/rng.c b/drivers/nxp/crypto/caam/src/rng.c
new file mode 100644
index 0000000..0b9d87d
--- /dev/null
+++ b/drivers/nxp/crypto/caam/src/rng.c
@@ -0,0 +1,251 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <arch_helpers.h>
+#include "caam.h"
+#include <common/debug.h>
+#include "jobdesc.h"
+#include "sec_hw_specific.h"
+
+
+/* Callback function after Instantiation decsriptor is submitted to SEC */
+static void rng_done(uint32_t *desc, uint32_t status, void *arg,
+		     void *job_ring)
+{
+	INFO("RNG Desc SUCCESS with status %x\n", status);
+}
+
+/* Is the HW RNG instantiated?
+ * Return code:
+ * 0 - Not in the instantiated state
+ * 1 - In the instantiated state
+ * state_handle - 0 for SH0, 1 for SH1
+ */
+static int is_hw_rng_instantiated(uint32_t *state_handle)
+{
+	int ret_code = 0;
+	uint32_t rdsta;
+
+	rdsta = sec_in32(get_caam_addr() + RNG_REG_RDSTA_OFFSET);
+
+	 /*Check if either of the two state handles has been instantiated */
+	if (rdsta & RNG_STATE0_HANDLE_INSTANTIATED) {
+		*state_handle = 0;
+		ret_code = 1;
+	} else if (rdsta & RNG_STATE0_HANDLE_INSTANTIATED) {
+		*state_handle = 1;
+		ret_code = 1;
+	}
+
+	return ret_code;
+}
+
+/* @brief Kick the TRNG block of the RNG HW Engine
+ * @param [in] ent_delay       Entropy delay to be used
+ *        By default, the TRNG runs for 200 clocks per sample;
+ *        1200 clocks per sample generates better entropy.
+ * @retval 0 on success
+ * @retval -1 on error
+ */
+static void kick_trng(int ent_delay)
+{
+	uint32_t val;
+
+	/* put RNG4 into program mode */
+	val = sec_in32(get_caam_addr() + RNG_REG_RTMCTL_OFFSET);
+	val = val | RTMCTL_PRGM;
+	sec_out32(get_caam_addr() + RNG_REG_RTMCTL_OFFSET, val);
+
+	/* rtsdctl bits 0-15 contain "Entropy Delay, which defines the
+	 *  length (in system clocks) of each Entropy sample taken
+	 */
+	val = sec_in32(get_caam_addr() + RNG_REG_RTSDCTL_OFFSET);
+	val = (val & ~RTSDCTL_ENT_DLY_MASK) |
+	    (ent_delay << RTSDCTL_ENT_DLY_SHIFT);
+	sec_out32(get_caam_addr() + RNG_REG_RTSDCTL_OFFSET, val);
+	/* min. freq. count, equal to 1/4 of the entropy sample length */
+	sec_out32(get_caam_addr() + RNG_REG_RTFRQMIN_OFFSET, ent_delay >> 2);
+	/* disable maximum frequency count */
+	sec_out32(get_caam_addr() + RNG_REG_RTFRQMAX_OFFSET, RTFRQMAX_DISABLE);
+
+	/* select raw sampling in both entropy shifter
+	 *  and statistical checker
+	 */
+	val = sec_in32(get_caam_addr() + RNG_REG_RTMCTL_OFFSET);
+	val = val | RTMCTL_SAMP_MODE_RAW_ES_SC;
+	sec_out32(get_caam_addr() + RNG_REG_RTMCTL_OFFSET, val);
+
+	/* put RNG4 into run mode */
+	val = sec_in32(get_caam_addr() + RNG_REG_RTMCTL_OFFSET);
+	val = val & ~RTMCTL_PRGM;
+	sec_out32(get_caam_addr() + RNG_REG_RTMCTL_OFFSET, val);
+}
+
+/* @brief Submit descriptor to instantiate the RNG
+ * @retval 0 on success
+ * @retval -1 on error
+ */
+static int instantiate_rng(void)
+{
+	int ret = 0;
+	struct job_descriptor desc __aligned(CACHE_WRITEBACK_GRANULE);
+	struct job_descriptor *jobdesc = &desc;
+
+	jobdesc->arg = NULL;
+	jobdesc->callback = rng_done;
+
+	/* create the hw_rng descriptor */
+	cnstr_rng_instantiate_jobdesc(jobdesc->desc);
+
+	/* Finally, generate the requested random data bytes */
+	ret = run_descriptor_jr(jobdesc);
+	if (ret != 0) {
+		ERROR("Error in running descriptor\n");
+		ret = -1;
+	}
+	return ret;
+}
+
+/* Generate Random Data using HW RNG
+ * Parameters:
+ * uint8_t* add_input   - user specified optional input byte array
+ * uint32_t add_input_len - number of bytes of additional input
+ * uint8_t* out                   - user specified output byte array
+ * uint32_t out_len       - number of bytes to store in output byte array
+ * Return code:
+ * 0 - SUCCESS
+ * -1 - ERROR
+ */
+static int
+hw_rng_generate(uint32_t *add_input, uint32_t add_input_len,
+		uint8_t *out, uint32_t out_len, uint32_t state_handle)
+{
+	int ret = 0;
+	struct job_descriptor desc __aligned(CACHE_WRITEBACK_GRANULE);
+	struct job_descriptor *jobdesc = &desc;
+
+	jobdesc->arg = NULL;
+	jobdesc->callback = rng_done;
+
+#if defined(SEC_MEM_NON_COHERENT) && defined(IMAGE_BL2)
+	inv_dcache_range((uintptr_t)out, out_len);
+	dmbsy();
+#endif
+
+	/* create the hw_rng descriptor */
+	ret = cnstr_rng_jobdesc(jobdesc->desc, state_handle,
+				add_input, add_input_len, out, out_len);
+	if (ret != 0) {
+		ERROR("Descriptor construction failed\n");
+		ret = -1;
+		goto out;
+	}
+	/* Finally, generate the requested random data bytes */
+	ret = run_descriptor_jr(jobdesc);
+	if (ret != 0) {
+		ERROR("Error in running descriptor\n");
+		ret = -1;
+	}
+
+out:
+	return ret;
+}
+
+/* this function instantiates the rng
+ *
+ * Return code:
+ *  0 - All is well
+ * <0 - Error occurred somewhere
+ */
+int hw_rng_instantiate(void)
+{
+	int ret = 0;
+	int ent_delay = RTSDCTL_ENT_DLY_MIN;
+	uint32_t state_handle;
+
+	ret = is_hw_rng_instantiated(&state_handle);
+	if (ret != 0) {
+		NOTICE("RNG already instantiated\n");
+		return 0;
+	}
+	do {
+		kick_trng(ent_delay);
+		ent_delay += 400;
+		/*if instantiate_rng(...) fails, the loop will rerun
+		 *and the kick_trng(...) function will modify the
+		 *upper and lower limits of the entropy sampling
+		 *interval, leading to a sucessful initialization of
+		 */
+		ret = instantiate_rng();
+	} while ((ret == -1) && (ent_delay < RTSDCTL_ENT_DLY_MAX));
+	if (ret != 0) {
+		ERROR("RNG: Failed to instantiate RNG\n");
+		return ret;
+	}
+
+	NOTICE("RNG: INSTANTIATED\n");
+
+	/* Enable RDB bit so that RNG works faster */
+	// sec_setbits32(&sec->scfgr, SEC_SCFGR_RDBENABLE);
+
+	return ret;
+}
+
+/* Generate random bytes, and stuff them into the bytes buffer
+ *
+ * If the HW RNG has not already been instantiated,
+ *  it will be instantiated before data is generated.
+ *
+ * Parameters:
+ * uint8_t* bytes  - byte buffer large enough to hold the requested random date
+ * int byte_len - number of random bytes to generate
+ *
+ * Return code:
+ *  0 - All is well
+ *  ~0 - Error occurred somewhere
+ */
+int get_rand_bytes_hw(uint8_t *bytes, int byte_len)
+{
+	int ret_code = 0;
+	uint32_t state_handle;
+
+	/* If this is the first time this routine is called,
+	 *  then the hash_drbg will not already be instantiated.
+	 * Therefore, before generating data, instantiate the hash_drbg
+	 */
+	ret_code = is_hw_rng_instantiated(&state_handle);
+	if (ret_code == 0) {
+		INFO("Instantiating the HW RNG\n");
+
+		/* Instantiate the hw RNG */
+		ret_code = hw_rng_instantiate();
+		if (ret_code != 0) {
+			ERROR("HW RNG Instantiate failed\n");
+			return ret_code;
+		}
+	}
+	/* If  HW RNG is still not instantiated, something must have gone wrong,
+	 * it must be in the error state, we will not generate any random data
+	 */
+	if (is_hw_rng_instantiated(&state_handle) == 0) {
+		ERROR("HW RNG is in an Error state, and cannot be used\n");
+		return -1;
+	}
+	/* Generate a random 256-bit value, as 32 bytes */
+	ret_code = hw_rng_generate(0, 0, bytes, byte_len, state_handle);
+	if (ret_code != 0) {
+		ERROR("HW RNG Generate failed\n");
+		return ret_code;
+	}
+
+	return ret_code;
+}
diff --git a/drivers/nxp/crypto/caam/src/sec_hw_specific.c b/drivers/nxp/crypto/caam/src/sec_hw_specific.c
new file mode 100644
index 0000000..92b7762
--- /dev/null
+++ b/drivers/nxp/crypto/caam/src/sec_hw_specific.c
@@ -0,0 +1,635 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <arch_helpers.h>
+#include "caam.h"
+#include <common/debug.h>
+#include "jobdesc.h"
+#include "sec_hw_specific.h"
+
+
+/* Job rings used for communication with SEC HW */
+extern struct sec_job_ring_t g_job_rings[MAX_SEC_JOB_RINGS];
+
+/* The current state of SEC user space driver */
+extern volatile sec_driver_state_t g_driver_state;
+
+/* The number of job rings used by SEC user space driver */
+extern int g_job_rings_no;
+
+/* LOCAL FUNCTIONS */
+static inline void hw_set_input_ring_start_addr(struct jobring_regs *regs,
+						phys_addr_t *start_addr)
+{
+#if defined(CONFIG_PHYS_64BIT)
+	sec_out32(&regs->irba_h, PHYS_ADDR_HI(start_addr));
+#else
+	sec_out32(&regs->irba_h, 0);
+#endif
+	sec_out32(&regs->irba_l, PHYS_ADDR_LO(start_addr));
+}
+
+static inline void hw_set_output_ring_start_addr(struct jobring_regs *regs,
+						 phys_addr_t *start_addr)
+{
+#if defined(CONFIG_PHYS_64BIT)
+	sec_out32(&regs->orba_h, PHYS_ADDR_HI(start_addr));
+#else
+	sec_out32(&regs->orba_h, 0);
+#endif
+	sec_out32(&regs->orba_l, PHYS_ADDR_LO(start_addr));
+}
+
+/* ORJR - Output Ring Jobs Removed Register shows how many jobs were
+ * removed from the Output Ring for processing by software. This is done after
+ * the software has processed the entries.
+ */
+static inline void hw_remove_entries(sec_job_ring_t *jr, int num)
+{
+	struct jobring_regs *regs =
+	    (struct jobring_regs *)jr->register_base_addr;
+
+	sec_out32(&regs->orjr, num);
+}
+
+/* IRSA - Input Ring Slots Available register holds the number of entries in
+ * the Job Ring's input ring. Once a job is enqueued, the value returned is
+ * decremented by the hardware by the number of jobs enqueued.
+ */
+static inline int hw_get_available_slots(sec_job_ring_t *jr)
+{
+	struct jobring_regs *regs =
+	    (struct jobring_regs *)jr->register_base_addr;
+
+	return sec_in32(&regs->irsa);
+}
+
+/* ORSFR - Output Ring Slots Full register holds the number of jobs which were
+ * processed by the SEC and can be retrieved by the software. Once a job has
+ * been processed by software, the user will call hw_remove_one_entry in order
+ * to notify the SEC that the entry was processed
+ */
+static inline int hw_get_no_finished_jobs(sec_job_ring_t *jr)
+{
+	struct jobring_regs *regs =
+	    (struct jobring_regs *)jr->register_base_addr;
+
+	return sec_in32(&regs->orsf);
+}
+
+/* @brief Process Jump Halt Condition related errors
+ * @param [in]  error_code The error code in the descriptor status word
+ */
+static inline void hw_handle_jmp_halt_cond_err(union hw_error_code error_code)
+{
+	ERROR("JMP %x\n", error_code.error_desc.jmp_halt_cond_src.jmp);
+	ERROR("Descriptor Index: %d\n",
+	      error_code.error_desc.jmp_halt_cond_src.desc_idx);
+	ERROR(" Condition %x\n", error_code.error_desc.jmp_halt_cond_src.cond);
+}
+
+/* @brief Process DECO related errors
+ * @param [in]  error_code      The error code in the descriptor status word
+ */
+static inline void hw_handle_deco_err(union hw_error_code error_code)
+{
+	ERROR("JMP %x\n", error_code.error_desc.deco_src.jmp);
+	ERROR("Descriptor Index: 0x%x",
+	      error_code.error_desc.deco_src.desc_idx);
+
+	switch (error_code.error_desc.deco_src.desc_err) {
+	case SEC_HW_ERR_DECO_HFN_THRESHOLD:
+		WARN(" Descriptor completed but exceeds the Threshold");
+		break;
+	default:
+		ERROR("Error 0x%04x not implemented",
+		      error_code.error_desc.deco_src.desc_err);
+		break;
+	}
+}
+
+/* @brief Process  Jump Halt User Status related errors
+ * @param [in]  error_code      The error code in the descriptor status word
+ */
+static inline void hw_handle_jmp_halt_user_err(union hw_error_code error_code)
+{
+	WARN(" Not implemented");
+}
+
+/* @brief Process CCB related errors
+ * @param [in]  error_code      The error code in the descriptor status word
+ */
+static inline void hw_handle_ccb_err(union hw_error_code hw_error_code)
+{
+	WARN(" Not implemented");
+}
+
+/* @brief Process Job Ring related errors
+ * @param [in]  error_code      The error code in the descriptor status word
+ */
+static inline void hw_handle_jr_err(union hw_error_code hw_error_code)
+{
+	WARN(" Not implemented");
+}
+
+/* GLOBAL FUNCTIONS */
+
+int hw_reset_job_ring(sec_job_ring_t *job_ring)
+{
+	int ret = 0;
+	struct jobring_regs *regs =
+	    (struct jobring_regs *)job_ring->register_base_addr;
+
+	/* First reset the job ring in hw */
+	ret = hw_shutdown_job_ring(job_ring);
+	if (ret != 0) {
+		ERROR("Failed resetting job ring in hardware");
+		return ret;
+	}
+	/* In order to have the HW JR in a workable state
+	 *after a reset, I need to re-write the input
+	 * queue size, input start address, output queue
+	 * size and output start address
+	 * Write the JR input queue size to the HW register
+	 */
+	sec_out32(&regs->irs, SEC_JOB_RING_SIZE);
+
+	/* Write the JR output queue size to the HW register */
+	sec_out32(&regs->ors, SEC_JOB_RING_SIZE);
+
+	/* Write the JR input queue start address */
+	hw_set_input_ring_start_addr(regs, vtop(job_ring->input_ring));
+
+	/* Write the JR output queue start address */
+	hw_set_output_ring_start_addr(regs, vtop(job_ring->output_ring));
+
+	return 0;
+}
+
+int hw_shutdown_job_ring(sec_job_ring_t *job_ring)
+{
+	struct jobring_regs *regs =
+	    (struct jobring_regs *)job_ring->register_base_addr;
+	unsigned int timeout = SEC_TIMEOUT;
+	uint32_t tmp = 0U;
+
+	VERBOSE("Resetting Job ring\n");
+
+	/*
+	 * Mask interrupts since we are going to poll
+	 * for reset completion status
+	 * Also, at POR, interrupts are ENABLED on a JR, thus
+	 * this is the point where I can disable them without
+	 * changing the code logic too much
+	 */
+
+	jr_disable_irqs(job_ring);
+
+	/* initiate flush (required prior to reset) */
+	sec_out32(&regs->jrcr, JR_REG_JRCR_VAL_RESET);
+
+	/* dummy read */
+	tmp = sec_in32(&regs->jrcr);
+
+	do {
+		tmp = sec_in32(&regs->jrint);
+	} while (((tmp & JRINT_ERR_HALT_MASK) ==
+		  JRINT_ERR_HALT_INPROGRESS) && ((--timeout) != 0U));
+
+	if ((tmp & JRINT_ERR_HALT_MASK) != JRINT_ERR_HALT_COMPLETE ||
+	    timeout == 0U) {
+		ERROR("Failed to flush hw job ring %x\n %u", tmp, timeout);
+		/* unmask interrupts */
+		if (job_ring->jr_mode != SEC_NOTIFICATION_TYPE_POLL) {
+			jr_enable_irqs(job_ring);
+		}
+		return -1;
+	}
+	/* Initiate reset */
+	timeout = SEC_TIMEOUT;
+	sec_out32(&regs->jrcr, JR_REG_JRCR_VAL_RESET);
+
+	do {
+		tmp = sec_in32(&regs->jrcr);
+	} while (((tmp & JR_REG_JRCR_VAL_RESET) != 0U) &&
+		 ((--timeout) != 0U));
+
+	if (timeout == 0U) {
+		ERROR("Failed to reset hw job ring\n");
+		/* unmask interrupts */
+		if (job_ring->jr_mode != SEC_NOTIFICATION_TYPE_POLL) {
+			jr_enable_irqs(job_ring);
+		}
+		return -1;
+	}
+	/* unmask interrupts */
+	if (job_ring->jr_mode != SEC_NOTIFICATION_TYPE_POLL) {
+		jr_enable_irqs(job_ring);
+	}
+	return 0;
+
+}
+
+void hw_handle_job_ring_error(sec_job_ring_t *job_ring, uint32_t error_code)
+{
+	union hw_error_code hw_err_code;
+
+	hw_err_code.error = error_code;
+
+	switch (hw_err_code.error_desc.value.ssrc) {
+	case SEC_HW_ERR_SSRC_NO_SRC:
+		INFO("No Status Source ");
+		break;
+	case SEC_HW_ERR_SSRC_CCB_ERR:
+		INFO("CCB Status Source");
+		hw_handle_ccb_err(hw_err_code);
+		break;
+	case SEC_HW_ERR_SSRC_JMP_HALT_U:
+		INFO("Jump Halt User Status Source");
+		hw_handle_jmp_halt_user_err(hw_err_code);
+		break;
+	case SEC_HW_ERR_SSRC_DECO:
+		INFO("DECO Status Source");
+		hw_handle_deco_err(hw_err_code);
+		break;
+	case SEC_HW_ERR_SSRC_JR:
+		INFO("Job Ring Status Source");
+		hw_handle_jr_err(hw_err_code);
+		break;
+	case SEC_HW_ERR_SSRC_JMP_HALT_COND:
+		INFO("Jump Halt Condition Codes");
+		hw_handle_jmp_halt_cond_err(hw_err_code);
+		break;
+	default:
+		INFO("Unknown SSRC");
+		break;
+	}
+}
+
+int hw_job_ring_error(sec_job_ring_t *job_ring)
+{
+	uint32_t jrint_error_code;
+	struct jobring_regs *regs =
+	    (struct jobring_regs *)job_ring->register_base_addr;
+
+	if (JR_REG_JRINT_JRE_EXTRACT(sec_in32(&regs->jrint)) == 0) {
+		return 0;
+	}
+
+	jrint_error_code =
+	    JR_REG_JRINT_ERR_TYPE_EXTRACT(sec_in32(&regs->jrint));
+	switch (jrint_error_code) {
+	case JRINT_ERR_WRITE_STATUS:
+		ERROR("Error writing status to Output Ring ");
+		break;
+	case JRINT_ERR_BAD_INPUT_BASE:
+		ERROR("Bad Input Ring Base (not on a 4-byte boundary)\n");
+		break;
+	case JRINT_ERR_BAD_OUTPUT_BASE:
+		ERROR("Bad Output Ring Base (not on a 4-byte boundary)\n");
+		break;
+	case JRINT_ERR_WRITE_2_IRBA:
+		ERROR("Invalid write to Input Ring Base Address Register\n");
+		break;
+	case JRINT_ERR_WRITE_2_ORBA:
+		ERROR("Invalid write to Output Ring Base Address Register\n");
+		break;
+	case JRINT_ERR_RES_B4_HALT:
+		ERROR("Job Ring released before Job Ring is halted\n");
+		break;
+	case JRINT_ERR_REM_TOO_MANY:
+		ERROR("Removed too many jobs from job ring\n");
+		break;
+	case JRINT_ERR_ADD_TOO_MANY:
+		ERROR("Added too many jobs on job ring\n");
+		break;
+	default:
+		ERROR("Unknown SEC JR Error :%d\n", jrint_error_code);
+		break;
+	}
+	return jrint_error_code;
+}
+
+int hw_job_ring_set_coalescing_param(sec_job_ring_t *job_ring,
+				     uint16_t irq_coalescing_timer,
+				     uint8_t irq_coalescing_count)
+{
+	uint32_t reg_val = 0U;
+	struct jobring_regs *regs =
+	    (struct jobring_regs *)job_ring->register_base_addr;
+
+	/* Set descriptor count coalescing */
+	reg_val |= (irq_coalescing_count << JR_REG_JRCFG_LO_ICDCT_SHIFT);
+
+	/* Set coalescing timer value */
+	reg_val |= (irq_coalescing_timer << JR_REG_JRCFG_LO_ICTT_SHIFT);
+
+	/* Update parameters in HW */
+	sec_out32(&regs->jrcfg1, reg_val);
+
+	VERBOSE("Set coalescing params on jr\n");
+
+	return 0;
+}
+
+int hw_job_ring_enable_coalescing(sec_job_ring_t *job_ring)
+{
+	uint32_t reg_val = 0U;
+	struct jobring_regs *regs =
+	    (struct jobring_regs *)job_ring->register_base_addr;
+
+	/* Get the current value of the register */
+	reg_val = sec_in32(&regs->jrcfg1);
+
+	/* Enable coalescing */
+	reg_val |= JR_REG_JRCFG_LO_ICEN_EN;
+
+	/* Write in hw */
+	sec_out32(&regs->jrcfg1, reg_val);
+
+	VERBOSE("Enabled coalescing on jr\n");
+
+	return 0;
+}
+
+int hw_job_ring_disable_coalescing(sec_job_ring_t *job_ring)
+{
+	uint32_t reg_val = 0U;
+	struct jobring_regs *regs =
+	    (struct jobring_regs *)job_ring->register_base_addr;
+
+	/* Get the current value of the register */
+	reg_val = sec_in32(&regs->jrcfg1);
+
+	/* Disable coalescing */
+	reg_val &= ~JR_REG_JRCFG_LO_ICEN_EN;
+
+	/* Write in hw */
+	sec_out32(&regs->jrcfg1, reg_val);
+
+	VERBOSE("Disabled coalescing on jr");
+
+	return 0;
+
+}
+
+void hw_flush_job_ring(struct sec_job_ring_t *job_ring,
+		       uint32_t do_notify,
+		       uint32_t error_code, uint32_t *notified_descs)
+{
+	int32_t jobs_no_to_discard = 0;
+	int32_t discarded_descs_no = 0;
+	int32_t number_of_jobs_available = 0;
+
+	VERBOSE("JR pi[%d]i ci[%d]\n", job_ring->pidx, job_ring->cidx);
+	VERBOSE("error code %x\n", error_code);
+	VERBOSE("Notify_desc = %d\n", do_notify);
+
+	number_of_jobs_available = hw_get_no_finished_jobs(job_ring);
+
+	/* Discard all jobs */
+	jobs_no_to_discard = number_of_jobs_available;
+
+	VERBOSE("JR pi[%d]i ci[%d]\n", job_ring->pidx, job_ring->cidx);
+	VERBOSE("Discarding desc = %d\n", jobs_no_to_discard);
+
+	while (jobs_no_to_discard > discarded_descs_no) {
+		discarded_descs_no++;
+		/* Now increment the consumer index for the current job ring,
+		 * AFTER saving job in temporary location!
+		 * Increment the consumer index for the current job ring
+		 */
+
+		job_ring->cidx = SEC_CIRCULAR_COUNTER(job_ring->cidx,
+						      SEC_JOB_RING_SIZE);
+
+		hw_remove_entries(job_ring, 1);
+	}
+
+	if (do_notify == true) {
+		if (notified_descs == NULL) {
+			return;
+		}
+		*notified_descs = discarded_descs_no;
+	}
+}
+
+/* return >0 in case of success
+ *  -1 in case of error from SEC block
+ *  0 in case job not yet processed by SEC
+ *   or  Descriptor returned is NULL after dequeue
+ */
+int hw_poll_job_ring(struct sec_job_ring_t *job_ring, int32_t limit)
+{
+	int32_t jobs_no_to_notify = 0;
+	int32_t number_of_jobs_available = 0;
+	int32_t notified_descs_no = 0;
+	uint32_t error_descs_no = 0U;
+	uint32_t sec_error_code = 0U;
+	uint32_t do_driver_shutdown = false;
+	phys_addr_t *fnptr, *arg_addr;
+	user_callback usercall = NULL;
+	uint8_t *current_desc;
+	void *arg;
+	uintptr_t current_desc_addr;
+	phys_addr_t current_desc_loc;
+
+#if defined(SEC_MEM_NON_COHERENT) && defined(IMAGE_BL2)
+	inv_dcache_range((uintptr_t)job_ring->register_base_addr, sizeof(struct jobring_regs));
+	dmbsy();
+#endif
+
+	/* check here if any JR error that cannot be written
+	 * in the output status word has occurred
+	 */
+	sec_error_code = hw_job_ring_error(job_ring);
+	if (unlikely(sec_error_code) != 0) {
+		ERROR("Error here itself %x\n", sec_error_code);
+		return -1;
+	}
+	/* Compute the number of notifications that need to be raised to UA
+	 * If limit < 0 -> notify all done jobs
+	 * If limit > total number of done jobs -> notify all done jobs
+	 * If limit = 0 -> error
+	 * If limit > 0 && limit < total number of done jobs -> notify a number
+	 * of done jobs equal with limit
+	 */
+
+	/*compute the number of jobs available in the job ring based on the
+	 * producer and consumer index values.
+	 */
+
+	number_of_jobs_available = hw_get_no_finished_jobs(job_ring);
+	jobs_no_to_notify = (limit < 0 || limit > number_of_jobs_available) ?
+	    number_of_jobs_available : limit;
+	VERBOSE("JR - pi %d, ci %d, ", job_ring->pidx, job_ring->cidx);
+	VERBOSE("Jobs submitted %d", number_of_jobs_available);
+	VERBOSE("Jobs to notify %d\n", jobs_no_to_notify);
+
+	while (jobs_no_to_notify > notified_descs_no) {
+
+#if defined(SEC_MEM_NON_COHERENT) && defined(IMAGE_BL2)
+		inv_dcache_range(
+			(uintptr_t)(&job_ring->output_ring[job_ring->cidx]),
+			sizeof(struct sec_outring_entry));
+		dmbsy();
+#endif
+
+		/* Get job status here */
+		sec_error_code =
+		    sec_in32(&(job_ring->output_ring[job_ring->cidx].status));
+
+		/* Get completed descriptor
+		 */
+		current_desc_loc = (uintptr_t)
+		    &job_ring->output_ring[job_ring->cidx].desc;
+		current_desc_addr = sec_read_addr(current_desc_loc);
+
+		current_desc = ptov((phys_addr_t *) current_desc_addr);
+		if (current_desc == 0) {
+			ERROR("No descriptor returned from SEC");
+			assert(current_desc);
+			return 0;
+		}
+		/* now increment the consumer index for the current job ring,
+		 * AFTER saving job in temporary location!
+		 */
+		job_ring->cidx = SEC_CIRCULAR_COUNTER(job_ring->cidx,
+						      SEC_JOB_RING_SIZE);
+
+		if (sec_error_code != 0) {
+			ERROR("desc at cidx %d\n ", job_ring->cidx);
+			ERROR("generated error %x\n", sec_error_code);
+
+			sec_handle_desc_error(job_ring,
+					      sec_error_code,
+					      &error_descs_no,
+					      &do_driver_shutdown);
+			hw_remove_entries(job_ring, 1);
+
+			return -1;
+		}
+		/* Signal that the job has been processed & the slot is free */
+		hw_remove_entries(job_ring, 1);
+		notified_descs_no++;
+
+		arg_addr = (phys_addr_t *) (current_desc +
+				(MAX_DESC_SIZE_WORDS * sizeof(uint32_t)));
+
+		fnptr = (phys_addr_t *) (current_desc +
+					(MAX_DESC_SIZE_WORDS * sizeof(uint32_t)
+					+  sizeof(void *)));
+
+		arg = (void *)*(arg_addr);
+		if (*fnptr != 0) {
+			VERBOSE("Callback Function called\n");
+			usercall = (user_callback) *(fnptr);
+			(*usercall) ((uint32_t *) current_desc,
+				     sec_error_code, arg, job_ring);
+		}
+	}
+
+	return notified_descs_no;
+}
+
+void sec_handle_desc_error(sec_job_ring_t *job_ring,
+			   uint32_t sec_error_code,
+			   uint32_t *notified_descs,
+			   uint32_t *do_driver_shutdown)
+{
+	/* Analyze the SEC error on this job ring */
+	hw_handle_job_ring_error(job_ring, sec_error_code);
+}
+
+void flush_job_rings(void)
+{
+	struct sec_job_ring_t *job_ring = NULL;
+	int i = 0;
+
+	for (i = 0; i < g_job_rings_no; i++) {
+		job_ring = &g_job_rings[i];
+		/* Producer index is frozen. If consumer index is not equal
+		 * with producer index, then we have descs to flush.
+		 */
+		while (job_ring->pidx != job_ring->cidx) {
+			hw_flush_job_ring(job_ring, false, 0,	/* no error */
+					  NULL);
+		}
+	}
+}
+
+int shutdown_job_ring(struct sec_job_ring_t *job_ring)
+{
+	int ret = 0;
+
+	ret = hw_shutdown_job_ring(job_ring);
+	if (ret != 0) {
+		ERROR("Failed to shutdown hardware job ring\n");
+		return ret;
+	}
+
+	if (job_ring->coalescing_en != 0) {
+		hw_job_ring_disable_coalescing(job_ring);
+	}
+
+	if (job_ring->jr_mode != SEC_NOTIFICATION_TYPE_POLL) {
+		ret = jr_disable_irqs(job_ring);
+		if (ret != 0) {
+			ERROR("Failed to disable irqs for job ring");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+int jr_enable_irqs(struct sec_job_ring_t *job_ring)
+{
+	uint32_t reg_val = 0U;
+	struct jobring_regs *regs =
+	    (struct jobring_regs *)job_ring->register_base_addr;
+
+	/* Get the current value of the register */
+	reg_val = sec_in32(&regs->jrcfg1);
+
+	/* Enable interrupts by disabling interrupt masking*/
+	reg_val &= ~JR_REG_JRCFG_LO_IMSK_EN;
+
+	/* Update parameters in HW */
+	sec_out32(&regs->jrcfg1, reg_val);
+
+	VERBOSE("Enable interrupts on JR\n");
+
+	return 0;
+}
+
+int jr_disable_irqs(struct sec_job_ring_t *job_ring)
+{
+	uint32_t reg_val = 0U;
+	struct jobring_regs *regs =
+	    (struct jobring_regs *)job_ring->register_base_addr;
+
+	/* Get the current value of the register */
+	reg_val = sec_in32(&regs->jrcfg1);
+
+	/* Disable interrupts by enabling interrupt masking*/
+	reg_val |= JR_REG_JRCFG_LO_IMSK_EN;
+
+	/* Update parameters in HW */
+	sec_out32(&regs->jrcfg1, reg_val);
+
+	VERBOSE("Disable interrupts on JR\n");
+
+	return 0;
+}
diff --git a/drivers/nxp/crypto/caam/src/sec_jr_driver.c b/drivers/nxp/crypto/caam/src/sec_jr_driver.c
new file mode 100644
index 0000000..1fe7007
--- /dev/null
+++ b/drivers/nxp/crypto/caam/src/sec_jr_driver.c
@@ -0,0 +1,241 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include "caam.h"
+#include <common/debug.h>
+#include "jobdesc.h"
+#include "nxp_timer.h"
+#include "sec_hw_specific.h"
+#include "sec_jr_driver.h"
+
+
+/* Job rings used for communication with SEC HW  */
+struct sec_job_ring_t g_job_rings[MAX_SEC_JOB_RINGS];
+
+/* The current state of SEC user space driver */
+volatile sec_driver_state_t g_driver_state = SEC_DRIVER_STATE_IDLE;
+
+int g_job_rings_no;
+
+uint8_t ip_ring[SEC_DMA_MEM_INPUT_RING_SIZE] __aligned(CACHE_WRITEBACK_GRANULE);
+uint8_t op_ring[SEC_DMA_MEM_OUTPUT_RING_SIZE] __aligned(CACHE_WRITEBACK_GRANULE);
+
+void *init_job_ring(uint8_t jr_mode,
+		    uint16_t irq_coalescing_timer,
+		    uint8_t irq_coalescing_count,
+		    void *reg_base_addr, uint32_t irq_id)
+{
+	struct sec_job_ring_t *job_ring = &g_job_rings[g_job_rings_no++];
+	int ret = 0;
+
+	job_ring->register_base_addr = reg_base_addr;
+	job_ring->jr_mode = jr_mode;
+	job_ring->irq_fd = irq_id;
+
+	job_ring->input_ring = vtop(ip_ring);
+	memset(job_ring->input_ring, 0, SEC_DMA_MEM_INPUT_RING_SIZE);
+
+	job_ring->output_ring = (struct sec_outring_entry *)vtop(op_ring);
+	memset(job_ring->output_ring, 0, SEC_DMA_MEM_OUTPUT_RING_SIZE);
+
+	dsb();
+
+#if defined(SEC_MEM_NON_COHERENT) && defined(IMAGE_BL2)
+	flush_dcache_range((uintptr_t)(job_ring->input_ring),
+				       SEC_DMA_MEM_INPUT_RING_SIZE),
+	flush_dcache_range((uintptr_t)(job_ring->output_ring),
+				       SEC_DMA_MEM_OUTPUT_RING_SIZE),
+
+	dmbsy();
+#endif
+	/* Reset job ring in SEC hw and configure job ring registers */
+	ret = hw_reset_job_ring(job_ring);
+	if (ret != 0) {
+		ERROR("Failed to reset hardware job ring\n");
+		return NULL;
+	}
+
+	if (jr_mode == SEC_NOTIFICATION_TYPE_IRQ) {
+		/* Enable IRQ if driver work sin interrupt mode */
+		ERROR("Enabling DONE IRQ generation on job ring\n");
+		ret = jr_enable_irqs(job_ring);
+		if (ret != 0) {
+			ERROR("Failed to enable irqs for job ring\n");
+			return NULL;
+		}
+	}
+	if ((irq_coalescing_timer != 0) || (irq_coalescing_count != 0)) {
+		hw_job_ring_set_coalescing_param(job_ring,
+						 irq_coalescing_timer,
+						 irq_coalescing_count);
+
+		hw_job_ring_enable_coalescing(job_ring);
+		job_ring->coalescing_en = 1;
+	}
+
+	job_ring->jr_state = SEC_JOB_RING_STATE_STARTED;
+
+	return job_ring;
+}
+
+int sec_release(void)
+{
+	int i;
+
+	/* Validate driver state */
+	if (g_driver_state == SEC_DRIVER_STATE_RELEASE) {
+		ERROR("Driver release is already in progress");
+		return SEC_DRIVER_RELEASE_IN_PROGRESS;
+	}
+	/* Update driver state */
+	g_driver_state = SEC_DRIVER_STATE_RELEASE;
+
+	/* If any descriptors in flight , poll and wait
+	 * until all descriptors are received and silently discarded.
+	 */
+
+	flush_job_rings();
+
+	for (i = 0; i < g_job_rings_no; i++) {
+		shutdown_job_ring(&g_job_rings[i]);
+	}
+	g_job_rings_no = 0;
+	g_driver_state = SEC_DRIVER_STATE_IDLE;
+
+	return SEC_SUCCESS;
+}
+
+int sec_jr_lib_init(void)
+{
+	/* Validate driver state */
+	if (g_driver_state != SEC_DRIVER_STATE_IDLE) {
+		ERROR("Driver already initialized\n");
+		return 0;
+	}
+
+	memset(g_job_rings, 0, sizeof(g_job_rings));
+	g_job_rings_no = 0;
+
+	/* Update driver state */
+	g_driver_state = SEC_DRIVER_STATE_STARTED;
+	return 0;
+}
+
+int dequeue_jr(void *job_ring_handle, int32_t limit)
+{
+	int ret = 0;
+	int notified_descs_no = 0;
+	struct sec_job_ring_t *job_ring = (sec_job_ring_t *) job_ring_handle;
+	uint64_t start_time;
+
+	/* Validate driver state */
+	if (g_driver_state != SEC_DRIVER_STATE_STARTED) {
+		ERROR("Driver release in progress or driver not initialized\n");
+		return -1;
+	}
+
+	/* Validate input arguments */
+	if (job_ring == NULL) {
+		ERROR("job_ring_handle is NULL\n");
+		return -1;
+	}
+	if (((limit == 0) || (limit > SEC_JOB_RING_SIZE))) {
+		ERROR("Invalid limit parameter configuration\n");
+		return -1;
+	}
+
+	VERBOSE("JR Polling limit[%d]\n", limit);
+
+	/* Poll job ring
+	 * If limit < 0 -> poll JR until no more notifications are available.
+	 * If limit > 0 -> poll JR until limit is reached.
+	 */
+
+	start_time = get_timer_val(0);
+
+	while (notified_descs_no == 0) {
+		/* Run hw poll job ring */
+		notified_descs_no = hw_poll_job_ring(job_ring, limit);
+		if (notified_descs_no < 0) {
+			ERROR("Error polling SEC engine job ring ");
+			return notified_descs_no;
+		}
+		VERBOSE("Jobs notified[%d]. ", notified_descs_no);
+
+		if (get_timer_val(start_time) >= CAAM_TIMEOUT) {
+			break;
+		}
+	}
+
+	if (job_ring->jr_mode == SEC_NOTIFICATION_TYPE_IRQ) {
+
+		/* Always enable IRQ generation when in pure IRQ mode */
+		ret = jr_enable_irqs(job_ring);
+		if (ret != 0) {
+			ERROR("Failed to enable irqs for job ring");
+			return ret;
+		}
+	}
+	return notified_descs_no;
+}
+
+int enq_jr_desc(void *job_ring_handle, struct job_descriptor *jobdescr)
+{
+	struct sec_job_ring_t *job_ring;
+
+	job_ring = (struct sec_job_ring_t *)job_ring_handle;
+
+	/* Validate driver state */
+	if (g_driver_state != SEC_DRIVER_STATE_STARTED) {
+		ERROR("Driver release in progress or driver not initialized\n");
+		return -1;
+	}
+
+	/* Check job ring state */
+	if (job_ring->jr_state != SEC_JOB_RING_STATE_STARTED) {
+		ERROR("Job ring is currently resetting\n");
+		return -1;
+	}
+
+	if (SEC_JOB_RING_IS_FULL(job_ring->pidx, job_ring->cidx,
+				 SEC_JOB_RING_SIZE, SEC_JOB_RING_SIZE)) {
+		ERROR("Job ring is full\n");
+		return -1;
+	}
+
+	/* Set ptr in input ring to current descriptor  */
+	sec_write_addr(&job_ring->input_ring[job_ring->pidx],
+		       (phys_addr_t) vtop(jobdescr->desc));
+
+	dsb();
+
+#if defined(SEC_MEM_NON_COHERENT) && defined(IMAGE_BL2)
+	flush_dcache_range((uintptr_t)(&job_ring->input_ring[job_ring->pidx]),
+			   sizeof(phys_addr_t));
+
+	inv_dcache_range((uintptr_t)(&job_ring->output_ring[job_ring->cidx]),
+			   sizeof(struct sec_outring_entry));
+	dmbsy();
+#endif
+	/* Notify HW that a new job is enqueued  */
+	hw_enqueue_desc_on_job_ring(
+			(struct jobring_regs *)job_ring->register_base_addr, 1);
+
+	/* increment the producer index for the current job ring */
+	job_ring->pidx = SEC_CIRCULAR_COUNTER(job_ring->pidx,
+					      SEC_JOB_RING_SIZE);
+
+	return 0;
+}
diff --git a/drivers/nxp/csu/csu.c b/drivers/nxp/csu/csu.c
new file mode 100644
index 0000000..9f90fe0
--- /dev/null
+++ b/drivers/nxp/csu/csu.c
@@ -0,0 +1,34 @@
+/*
+ * Copyright 2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <endian.h>
+
+#include <common/debug.h>
+#include <csu.h>
+#include <lib/mmio.h>
+
+void enable_layerscape_ns_access(struct csu_ns_dev_st *csu_ns_dev,
+				 uint32_t num, uintptr_t nxp_csu_addr)
+{
+	uint32_t *base = (uint32_t *)nxp_csu_addr;
+	uint32_t *reg;
+	uint32_t val;
+	int i;
+
+	for (i = 0; i < num; i++) {
+		reg = base + csu_ns_dev[i].ind / 2U;
+		val = be32toh(mmio_read_32((uintptr_t)reg));
+		if (csu_ns_dev[i].ind % 2U == 0U) {
+			val &= 0x0000ffffU;
+			val |= csu_ns_dev[i].val << 16U;
+		} else {
+			val &= 0xffff0000U;
+			val |= csu_ns_dev[i].val;
+		}
+		mmio_write_32((uintptr_t)reg, htobe32(val));
+	}
+}
diff --git a/drivers/nxp/csu/csu.mk b/drivers/nxp/csu/csu.mk
new file mode 100644
index 0000000..bc16035
--- /dev/null
+++ b/drivers/nxp/csu/csu.mk
@@ -0,0 +1,26 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#-----------------------------------------------------------------------------
+ifeq (${CSU_ADDED},)
+
+CSU_ADDED		:= 1
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/csu
+
+CSU_SOURCES		+= $(PLAT_DRIVERS_PATH)/csu/csu.c
+
+ifeq (${BL_COMM_CSU_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${CSU_SOURCES}
+else
+ifeq (${BL2_CSU_NEEDED},yes)
+BL2_SOURCES		+= ${CSU_SOURCES}
+endif
+ifeq (${BL31_CSU_NEEDED},yes)
+BL31_SOURCES		+= ${CSU_SOURCES}
+endif
+endif
+
+endif
diff --git a/drivers/nxp/dcfg/dcfg.c b/drivers/nxp/dcfg/dcfg.c
new file mode 100644
index 0000000..e5c4db4
--- /dev/null
+++ b/drivers/nxp/dcfg/dcfg.c
@@ -0,0 +1,156 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <common/debug.h>
+#include "dcfg.h"
+#include <lib/mmio.h>
+#ifdef NXP_SFP_ENABLED
+#include <sfp.h>
+#endif
+
+static soc_info_t soc_info = {0};
+static devdisr5_info_t devdisr5_info = {0};
+static dcfg_init_info_t *dcfg_init_info;
+
+/* Read the PORSR1 register */
+uint32_t read_reg_porsr1(void)
+{
+	unsigned int *porsr1_addr = NULL;
+
+	if (dcfg_init_info->porsr1 != 0U) {
+		return dcfg_init_info->porsr1;
+	}
+
+	porsr1_addr = (void *)
+			(dcfg_init_info->g_nxp_dcfg_addr + DCFG_PORSR1_OFFSET);
+	dcfg_init_info->porsr1 = gur_in32(porsr1_addr);
+
+	return dcfg_init_info->porsr1;
+}
+
+
+const soc_info_t *get_soc_info(void)
+{
+	uint32_t reg;
+
+	if (soc_info.is_populated == true) {
+		return (const soc_info_t *) &soc_info;
+	}
+
+	reg = gur_in32(dcfg_init_info->g_nxp_dcfg_addr + DCFG_SVR_OFFSET);
+
+	soc_info.svr_reg.val = reg;
+
+	/* zero means SEC enabled. */
+	soc_info.sec_enabled =
+		(((reg & SVR_SEC_MASK) >> SVR_SEC_SHIFT) == 0) ? true : false;
+
+	soc_info.is_populated = true;
+	return (const soc_info_t *) &soc_info;
+}
+
+void dcfg_init(dcfg_init_info_t *dcfg_init_data)
+{
+	dcfg_init_info = dcfg_init_data;
+	read_reg_porsr1();
+	get_soc_info();
+}
+
+bool is_sec_enabled(void)
+{
+	return soc_info.sec_enabled;
+}
+
+const devdisr5_info_t *get_devdisr5_info(void)
+{
+	uint32_t reg;
+
+	if (devdisr5_info.is_populated == true)
+		return (const devdisr5_info_t *) &devdisr5_info;
+
+	reg = gur_in32(dcfg_init_info->g_nxp_dcfg_addr + DCFG_DEVDISR5_OFFSET);
+
+	devdisr5_info.ddrc1_present = (reg & DISR5_DDRC1_MASK) ? 0 : 1;
+#if defined(CONFIG_CHASSIS_3_2)
+	devdisr5_info.ddrc2_present = (reg & DISR5_DDRC2_MASK) ? 0 : 1;
+#endif
+	devdisr5_info.ocram_present = (reg & DISR5_OCRAM_MASK) ? 0 : 1;
+	devdisr5_info.is_populated = true;
+
+	return (const devdisr5_info_t *) &devdisr5_info;
+}
+
+int get_clocks(struct sysinfo *sys)
+{
+	unsigned int *rcwsr0 = NULL;
+	const unsigned long sysclk = dcfg_init_info->nxp_sysclk_freq;
+	const unsigned long ddrclk = dcfg_init_info->nxp_ddrclk_freq;
+
+	rcwsr0 = (void *)(dcfg_init_info->g_nxp_dcfg_addr + RCWSR0_OFFSET);
+	sys->freq_platform = sysclk;
+	sys->freq_ddr_pll0 = ddrclk;
+	sys->freq_ddr_pll1 = ddrclk;
+
+	sys->freq_platform *= (gur_in32(rcwsr0) >>
+				RCWSR0_SYS_PLL_RAT_SHIFT) &
+				RCWSR0_SYS_PLL_RAT_MASK;
+
+	sys->freq_platform /= dcfg_init_info->nxp_plat_clk_divider;
+
+	sys->freq_ddr_pll0 *= (gur_in32(rcwsr0) >>
+				RCWSR0_MEM_PLL_RAT_SHIFT) &
+				RCWSR0_MEM_PLL_RAT_MASK;
+	sys->freq_ddr_pll1 *= (gur_in32(rcwsr0) >>
+				RCWSR0_MEM2_PLL_RAT_SHIFT) &
+				RCWSR0_MEM2_PLL_RAT_MASK;
+	if (sys->freq_platform == 0) {
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+#ifdef NXP_SFP_ENABLED
+/*******************************************************************************
+ * Returns true if secur eboot is enabled on board
+ * mode = 0  (development mode - sb_en = 1)
+ * mode = 1 (production mode - ITS = 1)
+ ******************************************************************************/
+bool check_boot_mode_secure(uint32_t *mode)
+{
+	uint32_t val = 0U;
+	uint32_t *rcwsr = NULL;
+	*mode = 0U;
+
+	if (sfp_check_its() == 1) {
+		/* ITS =1 , Production mode */
+		*mode = 1U;
+		return true;
+	}
+
+	rcwsr = (void *)(dcfg_init_info->g_nxp_dcfg_addr + RCWSR_SB_EN_OFFSET);
+
+	val = (gur_in32(rcwsr) >> RCWSR_SBEN_SHIFT) &
+				RCWSR_SBEN_MASK;
+
+	if (val == RCWSR_SBEN_MASK) {
+		*mode = 0U;
+		return true;
+	}
+
+	return false;
+}
+#endif
+
+void error_handler(int error_code)
+{
+	 /* Dump error code in SCRATCH4 register */
+	INFO("Error in Fuse Provisioning: %x\n", error_code);
+	gur_out32((void *)
+		  (dcfg_init_info->g_nxp_dcfg_addr + DCFG_SCRATCH4_OFFSET),
+		  error_code);
+}
diff --git a/drivers/nxp/dcfg/dcfg.mk b/drivers/nxp/dcfg/dcfg.mk
new file mode 100644
index 0000000..206595f
--- /dev/null
+++ b/drivers/nxp/dcfg/dcfg.mk
@@ -0,0 +1,26 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${ADD_DCFG},)
+
+ADD_DCFG		:= 1
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/dcfg
+
+DCFG_SOURCES		+= $(PLAT_DRIVERS_PATH)/dcfg/dcfg.c
+
+ifeq (${BL_COMM_DCFG_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${DCFG_SOURCES}
+else
+ifeq (${BL2_DCFG_NEEDED},yes)
+BL2_SOURCES		+= ${DCFG_SOURCES}
+endif
+ifeq (${BL31_DCFG_NEEDED},yes)
+BL31_SOURCES		+= ${DCFG_SOURCES}
+endif
+endif
+
+endif
diff --git a/drivers/nxp/ddr/fsl-mmdc/ddr.mk b/drivers/nxp/ddr/fsl-mmdc/ddr.mk
new file mode 100644
index 0000000..afccb62
--- /dev/null
+++ b/drivers/nxp/ddr/fsl-mmdc/ddr.mk
@@ -0,0 +1,19 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#-----------------------------------------------------------------------------
+
+# MMDC ddr cntlr driver files
+
+DDR_DRIVERS_PATH	:=	drivers/nxp/ddr
+
+DDR_CNTLR_SOURCES	:=	${PLAT_DRIVERS_PATH}/ddr/fsl-mmdc/fsl_mmdc.c \
+				${PLAT_DRIVERS_PATH}/ddr/nxp-ddr/utility.c	\
+				${PLAT_DRIVERS_PATH}/ddr/nxp-ddr/ddr.c	\
+				${PLAT_DRIVERS_PATH}/ddr/nxp-ddr/ddrc.c
+
+PLAT_INCLUDES		+=	-I$(PLAT_DRIVERS_INCLUDE_PATH)/ddr	\
+				-I$(PLAT_DRIVERS_INCLUDE_PATH)/ddr/fsl-mmdc
+#------------------------------------------------
diff --git a/drivers/nxp/ddr/fsl-mmdc/fsl_mmdc.c b/drivers/nxp/ddr/fsl-mmdc/fsl_mmdc.c
new file mode 100644
index 0000000..7e6504e
--- /dev/null
+++ b/drivers/nxp/ddr/fsl-mmdc/fsl_mmdc.c
@@ -0,0 +1,176 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+/*
+ * Generic driver for Freescale MMDC(Multi Mode DDR Controller).
+ */
+
+#include <errno.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include "ddr_io.h"
+#include <drivers/delay_timer.h>
+#include <fsl_mmdc.h>
+
+static void set_wait_for_bits_clear(void *ptr, unsigned int value,
+				    unsigned int bits)
+{
+	int timeout = 1000;
+
+	ddr_out32(ptr, value);
+
+	while ((ddr_in32(ptr) & bits) != 0) {
+		udelay(100);
+		timeout--;
+	}
+	if (timeout <= 0) {
+		INFO("Error: %llx", (unsigned long long)ptr);
+		INFO(" wait for clear timeout.\n");
+	}
+}
+
+void mmdc_init(const struct fsl_mmdc_info *priv, uintptr_t nxp_ddr_addr)
+{
+	struct mmdc_regs *mmdc = (struct mmdc_regs *)nxp_ddr_addr;
+	unsigned int tmp;
+
+	/* 1. set configuration request */
+	ddr_out32(&mmdc->mdscr, MDSCR_ENABLE_CON_REQ);
+
+	/* 2. configure the desired timing parameters */
+	ddr_out32(&mmdc->mdotc, priv->mdotc);
+	ddr_out32(&mmdc->mdcfg0, priv->mdcfg0);
+	ddr_out32(&mmdc->mdcfg1, priv->mdcfg1);
+	ddr_out32(&mmdc->mdcfg2, priv->mdcfg2);
+
+	/* 3. configure DDR type and other miscellaneous parameters */
+	ddr_out32(&mmdc->mdmisc, priv->mdmisc);
+	ddr_out32(&mmdc->mpmur0, MMDC_MPMUR0_FRC_MSR);
+	ddr_out32(&mmdc->mdrwd, priv->mdrwd);
+	ddr_out32(&mmdc->mpodtctrl, priv->mpodtctrl);
+
+	/* 4. configure the required delay while leaving reset */
+	ddr_out32(&mmdc->mdor, priv->mdor);
+
+	/* 5. configure DDR physical parameters */
+	/* set row/column address width, burst length, data bus width */
+	tmp = priv->mdctl & ~(MDCTL_SDE0 | MDCTL_SDE1);
+	ddr_out32(&mmdc->mdctl, tmp);
+	/* configure address space partition */
+	ddr_out32(&mmdc->mdasp, priv->mdasp);
+
+	/* 6. perform a ZQ calibration - not needed here, doing in #8b */
+
+	/* 7. enable MMDC with the desired chip select */
+#if (DDRC_NUM_CS == 1)
+	ddr_out32(&mmdc->mdctl, tmp | MDCTL_SDE0);
+#elif (DDRC_NUM_CS == 2)
+	ddr_out32(&mmdc->mdctl, tmp | MDCTL_SDE0 | MDCTL_SDE1);
+#else
+#error "Unsupported DDRC_NUM_CS"
+#endif
+
+	/* 8a. dram init sequence: update MRs for ZQ, ODT, PRE, etc */
+	ddr_out32(&mmdc->mdscr, CMD_ADDR_LSB_MR_ADDR(8) |
+				MDSCR_ENABLE_CON_REQ |
+				CMD_LOAD_MODE_REG |
+				CMD_BANK_ADDR_2);
+
+	ddr_out32(&mmdc->mdscr, CMD_ADDR_LSB_MR_ADDR(0) |
+				MDSCR_ENABLE_CON_REQ |
+				CMD_LOAD_MODE_REG |
+				CMD_BANK_ADDR_3);
+
+	ddr_out32(&mmdc->mdscr, CMD_ADDR_LSB_MR_ADDR(4) |
+				MDSCR_ENABLE_CON_REQ |
+				CMD_LOAD_MODE_REG |
+				CMD_BANK_ADDR_1);
+
+	ddr_out32(&mmdc->mdscr, CMD_ADDR_MSB_MR_OP(0x19) |
+				CMD_ADDR_LSB_MR_ADDR(0x30) |
+				MDSCR_ENABLE_CON_REQ |
+				CMD_LOAD_MODE_REG | CMD_BANK_ADDR_0);
+
+	/* 8b. ZQ calibration */
+	ddr_out32(&mmdc->mdscr, CMD_ADDR_MSB_MR_OP(0x4) |
+				MDSCR_ENABLE_CON_REQ |
+				CMD_ZQ_CALIBRATION | CMD_BANK_ADDR_0);
+
+	set_wait_for_bits_clear(&mmdc->mpzqhwctrl, priv->mpzqhwctrl,
+				MPZQHWCTRL_ZQ_HW_FORCE);
+
+	/* 9a. calibrations now, wr lvl */
+	ddr_out32(&mmdc->mdscr,  CMD_ADDR_LSB_MR_ADDR(0x84) | MDSCR_WL_EN |
+				MDSCR_ENABLE_CON_REQ |
+				CMD_LOAD_MODE_REG | CMD_BANK_ADDR_1);
+
+	set_wait_for_bits_clear(&mmdc->mpwlgcr, MPWLGCR_HW_WL_EN,
+				MPWLGCR_HW_WL_EN);
+
+	mdelay(1);
+
+	ddr_out32(&mmdc->mdscr, CMD_ADDR_LSB_MR_ADDR(4) |
+				MDSCR_ENABLE_CON_REQ |
+				CMD_LOAD_MODE_REG | CMD_BANK_ADDR_1);
+
+	ddr_out32(&mmdc->mdscr, MDSCR_ENABLE_CON_REQ);
+
+	mdelay(1);
+
+	/* 9b. read DQS gating calibration */
+	ddr_out32(&mmdc->mdscr, CMD_ADDR_MSB_MR_OP(4) | MDSCR_ENABLE_CON_REQ |
+				CMD_PRECHARGE_BANK_OPEN | CMD_BANK_ADDR_0);
+
+	ddr_out32(&mmdc->mdscr, CMD_ADDR_LSB_MR_ADDR(4) | MDSCR_ENABLE_CON_REQ |
+				CMD_LOAD_MODE_REG | CMD_BANK_ADDR_3);
+
+	ddr_out32(&mmdc->mppdcmpr2, MPPDCMPR2_MPR_COMPARE_EN);
+
+	/* set absolute read delay offset */
+	if (priv->mprddlctl != 0) {
+		ddr_out32(&mmdc->mprddlctl, priv->mprddlctl);
+	} else {
+		ddr_out32(&mmdc->mprddlctl, MMDC_MPRDDLCTL_DEFAULT_DELAY);
+	}
+
+	set_wait_for_bits_clear(&mmdc->mpdgctrl0,
+				AUTO_RD_DQS_GATING_CALIBRATION_EN,
+				AUTO_RD_DQS_GATING_CALIBRATION_EN);
+
+	ddr_out32(&mmdc->mdscr,  MDSCR_ENABLE_CON_REQ | CMD_LOAD_MODE_REG |
+				CMD_BANK_ADDR_3);
+
+	/* 9c. read calibration */
+	ddr_out32(&mmdc->mdscr, CMD_ADDR_MSB_MR_OP(4) | MDSCR_ENABLE_CON_REQ |
+				CMD_PRECHARGE_BANK_OPEN | CMD_BANK_ADDR_0);
+	ddr_out32(&mmdc->mdscr, CMD_ADDR_LSB_MR_ADDR(4) | MDSCR_ENABLE_CON_REQ |
+				CMD_LOAD_MODE_REG | CMD_BANK_ADDR_3);
+	ddr_out32(&mmdc->mppdcmpr2,  MPPDCMPR2_MPR_COMPARE_EN);
+	set_wait_for_bits_clear(&mmdc->mprddlhwctl,
+				MPRDDLHWCTL_AUTO_RD_CALIBRATION_EN,
+				MPRDDLHWCTL_AUTO_RD_CALIBRATION_EN);
+
+	ddr_out32(&mmdc->mdscr, MDSCR_ENABLE_CON_REQ | CMD_LOAD_MODE_REG |
+				CMD_BANK_ADDR_3);
+
+	/* 10. configure power-down, self-refresh entry, exit parameters */
+	ddr_out32(&mmdc->mdpdc, priv->mdpdc);
+	ddr_out32(&mmdc->mapsr, MMDC_MAPSR_PWR_SAV_CTRL_STAT);
+
+	/* 11. ZQ config again? do nothing here */
+
+	/* 12. refresh scheme */
+	set_wait_for_bits_clear(&mmdc->mdref, priv->mdref,
+				MDREF_START_REFRESH);
+
+	/* 13. disable CON_REQ */
+	ddr_out32(&mmdc->mdscr, MDSCR_DISABLE_CFG_REQ);
+}
diff --git a/drivers/nxp/ddr/nxp-ddr/README.odt b/drivers/nxp/ddr/nxp-ddr/README.odt
new file mode 100644
index 0000000..8796302
--- /dev/null
+++ b/drivers/nxp/ddr/nxp-ddr/README.odt
@@ -0,0 +1,31 @@
+Table for dynamic ODT for DDR4 with PHY generation 2
+====================================================
+Two-slot system
+Only symmetric configurations are supported for interleaving. Non-symmetric
+configurations are possible but not covered here. First slot empty is possbile
+but prohibited for simplicity.
++-----------------------+-------------+---------------+-----------------------------+-----------------------------+
+|     Configuration     |             |DRAM controller|           Slot 1            |           Slot 2            |
++-----------+-----------+-------------+-------+-------+--------------+--------------+--------------+--------------+
+|           |           |             |       |       |    Rank 1    |   Rank 2     |   Rank 1     |    Rank 2    |
+|  Slot 1   |  Slot 2   | Write/Read  | Write | Read  |-------+------+-------+------+-------+------+-------+------+
+|           |           |             |       |       | Write | Read | Write | Read | Write | Read | Write | Read |
++-----------+-----------+------+------+-------+-------+-------+------+-------+------+-------+------+-------+------+
+|           |           |      |Rank 1|  off  |  60   |  240  | off  |   60  | 240  |   60  |  60  |   60  |  60  |
+|           |           |Slot 1|------+-------+-------+-------+------+-------+------+-------+------+-------+------+
+|           |           |      |Rank 2|  off  |  60   |   60  | 240  |  240  | off  |   60  |  60  |   60  |  60  |
+| Dual Rank | Dual Rank |------+------+-------+-------+-------+------+-------+------+-------+------+-------+------+
+|           |           |      |Rank 1|  off  |  60   |   60  |  60  |   60  |  60  |  240  | off  |   60  | 240  |
+|           |           |Slot 2|------+-------+-------+-------+------+-------+------+-------+------+-------+------+
+|           |           |      |Rank 2|  off  |  60   |   60  |  60  |   60  |  60  |   60  | 240  |  240  | off  |
++-----------+-----------+------+------+-------+-------+-------+------+-------+------+-------+------+-------+------+
+|           |           |  Slot 1     |  off  |  60   |   80  |  off |       |      |       |      |       |      |
+|Single Rank|Single Rank|-------------+-------+-------+-------+------+-------+------+-------+------+-------+------+
+|           |           |  Slot 2     |  off  |  60   |       |      |       |      |   80  | off  |
++-----------+-----------+------+------+-------+-------+-------+------+-------+------+-------+------+
+|           |           |      |Rank 1|  off  |  80   |   80  | off  |  off  | off  |
+| Dual Rank |           |Slot 1|------+-------+-------+-------+------+-------+------+
+|           |           |      |Rank 2|  off  |  80   |   80  | off  |  off  | off  |
++-----------+-----------+-------------+-------+-------+-------+------+-------+------+
+|Single Rank|           |  Slot 1     |  off  |  80   |   80  | off  |
++-----------+-----------+-------------+-------+-------+-------+------+
diff --git a/drivers/nxp/ddr/nxp-ddr/ddr.c b/drivers/nxp/ddr/nxp-ddr/ddr.c
new file mode 100644
index 0000000..c051b3b
--- /dev/null
+++ b/drivers/nxp/ddr/nxp-ddr/ddr.c
@@ -0,0 +1,931 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <inttypes.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <ddr.h>
+#ifndef CONFIG_DDR_NODIMM
+#include <i2c.h>
+#endif
+#include <nxp_timer.h>
+
+struct dynamic_odt {
+	unsigned int odt_rd_cfg;
+	unsigned int odt_wr_cfg;
+	unsigned int odt_rtt_norm;
+	unsigned int odt_rtt_wr;
+};
+
+#ifndef CONFIG_STATIC_DDR
+#if defined(PHY_GEN2_FW_IMAGE_BUFFER) && !defined(NXP_DDR_PHY_GEN2)
+#error Missing NXP_DDR_PHY_GEN2
+#endif
+#ifdef NXP_DDR_PHY_GEN2
+static const struct dynamic_odt single_D[4] = {
+	{	/* cs0 */
+		DDR_ODT_NEVER,
+		DDR_ODT_ALL,
+		DDR4_RTT_80_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{	/* cs1 */
+		DDR_ODT_NEVER,
+		DDR_ODT_NEVER,
+		DDR4_RTT_OFF,
+		DDR4_RTT_WR_OFF
+	},
+	{},
+	{}
+};
+
+static const struct dynamic_odt single_S[4] = {
+	{	/* cs0 */
+		DDR_ODT_NEVER,
+		DDR_ODT_ALL,
+		DDR4_RTT_80_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{},
+	{},
+	{},
+};
+
+static const struct dynamic_odt dual_DD[4] = {
+	{	/* cs0 */
+		DDR_ODT_OTHER_DIMM,
+		DDR_ODT_ALL,
+		DDR4_RTT_60_OHM,
+		DDR4_RTT_WR_240_OHM
+	},
+	{	/* cs1 */
+		DDR_ODT_OTHER_DIMM,
+		DDR_ODT_ALL,
+		DDR4_RTT_60_OHM,
+		DDR4_RTT_WR_240_OHM
+	},
+	{	/* cs2 */
+		DDR_ODT_OTHER_DIMM,
+		DDR_ODT_ALL,
+		DDR4_RTT_60_OHM,
+		DDR4_RTT_WR_240_OHM
+	},
+	{	/* cs3 */
+		DDR_ODT_OTHER_DIMM,
+		DDR_ODT_ALL,
+		DDR4_RTT_60_OHM,
+		DDR4_RTT_WR_240_OHM
+	}
+};
+
+static const struct dynamic_odt dual_SS[4] = {
+	{	/* cs0 */
+		DDR_ODT_NEVER,
+		DDR_ODT_ALL,
+		DDR4_RTT_80_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{},
+	{	/* cs2 */
+		DDR_ODT_NEVER,
+		DDR_ODT_ALL,
+		DDR4_RTT_80_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{}
+};
+
+static const struct dynamic_odt dual_D0[4] = {
+	{	/* cs0 */
+		DDR_ODT_NEVER,
+		DDR_ODT_SAME_DIMM,
+		DDR4_RTT_80_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{	/* cs1 */
+		DDR_ODT_NEVER,
+		DDR_ODT_NEVER,
+		DDR4_RTT_80_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{},
+	{}
+};
+
+static const struct dynamic_odt dual_S0[4] = {
+	{	/* cs0 */
+		DDR_ODT_NEVER,
+		DDR_ODT_CS,
+		DDR4_RTT_80_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{},
+	{},
+	{}
+};
+#else
+static const struct dynamic_odt single_D[4] = {
+	{	/* cs0 */
+		DDR_ODT_NEVER,
+		DDR_ODT_ALL,
+		DDR4_RTT_40_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{	/* cs1 */
+		DDR_ODT_NEVER,
+		DDR_ODT_NEVER,
+		DDR4_RTT_OFF,
+		DDR4_RTT_WR_OFF
+	},
+	{},
+	{}
+};
+
+static const struct dynamic_odt single_S[4] = {
+	{	/* cs0 */
+		DDR_ODT_NEVER,
+		DDR_ODT_ALL,
+		DDR4_RTT_40_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{},
+	{},
+	{},
+};
+
+static const struct dynamic_odt dual_DD[4] = {
+	{	/* cs0 */
+		DDR_ODT_NEVER,
+		DDR_ODT_SAME_DIMM,
+		DDR4_RTT_120_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{	/* cs1 */
+		DDR_ODT_OTHER_DIMM,
+		DDR_ODT_OTHER_DIMM,
+		DDR4_RTT_34_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{	/* cs2 */
+		DDR_ODT_NEVER,
+		DDR_ODT_SAME_DIMM,
+		DDR4_RTT_120_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{	/* cs3 */
+		DDR_ODT_OTHER_DIMM,
+		DDR_ODT_OTHER_DIMM,
+		DDR4_RTT_34_OHM,
+		DDR4_RTT_WR_OFF
+	}
+};
+
+static const struct dynamic_odt dual_SS[4] = {
+	{	/* cs0 */
+		DDR_ODT_OTHER_DIMM,
+		DDR_ODT_ALL,
+		DDR4_RTT_34_OHM,
+		DDR4_RTT_WR_120_OHM
+	},
+	{},
+	{	/* cs2 */
+		DDR_ODT_OTHER_DIMM,
+		DDR_ODT_ALL,
+		DDR4_RTT_34_OHM,
+		DDR4_RTT_WR_120_OHM
+	},
+	{}
+};
+
+static const struct dynamic_odt dual_D0[4] = {
+	{	/* cs0 */
+		DDR_ODT_NEVER,
+		DDR_ODT_SAME_DIMM,
+		DDR4_RTT_40_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{	/* cs1 */
+		DDR_ODT_NEVER,
+		DDR_ODT_NEVER,
+		DDR4_RTT_OFF,
+		DDR4_RTT_WR_OFF
+	},
+	{},
+	{}
+};
+
+static const struct dynamic_odt dual_S0[4] = {
+	{	/* cs0 */
+		DDR_ODT_NEVER,
+		DDR_ODT_CS,
+		DDR4_RTT_40_OHM,
+		DDR4_RTT_WR_OFF
+	},
+	{},
+	{},
+	{}
+};
+#endif /* NXP_DDR_PHY_GEN2 */
+
+/*
+ * Automatically select bank interleaving mode based on DIMMs
+ * in this order: cs0_cs1_cs2_cs3, cs0_cs1, null.
+ * This function only deal with one or two slots per controller.
+ */
+static inline unsigned int auto_bank_intlv(const int cs_in_use,
+					   const struct dimm_params *pdimm)
+{
+	switch (cs_in_use) {
+	case 0xf:
+		return DDR_BA_INTLV_CS0123;
+	case 0x3:
+		return DDR_BA_INTLV_CS01;
+	case 0x1:
+		return DDR_BA_NONE;
+	case 0x5:
+		return DDR_BA_NONE;
+	default:
+		break;
+	}
+
+	return 0U;
+}
+
+static int cal_odt(const unsigned int clk,
+		   struct memctl_opt *popts,
+		   struct ddr_conf *conf,
+		   struct dimm_params *pdimm,
+		   const int dimm_slot_per_ctrl)
+
+{
+	unsigned int i;
+	const struct dynamic_odt *pdodt = NULL;
+
+	const static struct dynamic_odt *table[2][5] = {
+		{single_S, single_D, NULL, NULL},
+		{dual_SS, dual_DD, NULL, NULL},
+	};
+
+	if (dimm_slot_per_ctrl != 1 && dimm_slot_per_ctrl != 2) {
+		ERROR("Unsupported number of DIMMs\n");
+		return -EINVAL;
+	}
+
+	pdodt = table[dimm_slot_per_ctrl - 1][pdimm->n_ranks - 1];
+	if (pdodt == dual_SS) {
+		pdodt = (conf->cs_in_use == 0x5) ? dual_SS :
+			((conf->cs_in_use == 0x1) ? dual_S0 : NULL);
+	} else if (pdodt == dual_DD) {
+		pdodt = (conf->cs_in_use == 0xf) ? dual_DD :
+			((conf->cs_in_use == 0x3) ? dual_D0 : NULL);
+	}
+	if (pdodt == dual_DD && pdimm->package_3ds) {
+		ERROR("Too many 3DS DIMMs.\n");
+		return -EINVAL;
+	}
+
+	if (pdodt == NULL) {
+		ERROR("Error determing ODT.\n");
+		return -EINVAL;
+	}
+
+	/* Pick chip-select local options. */
+	for (i = 0U; i < DDRC_NUM_CS; i++) {
+		debug("cs %d\n", i);
+		popts->cs_odt[i].odt_rd_cfg = pdodt[i].odt_rd_cfg;
+		debug("     odt_rd_cfg 0x%x\n",
+			  popts->cs_odt[i].odt_rd_cfg);
+		popts->cs_odt[i].odt_wr_cfg = pdodt[i].odt_wr_cfg;
+		debug("     odt_wr_cfg 0x%x\n",
+			  popts->cs_odt[i].odt_wr_cfg);
+		popts->cs_odt[i].odt_rtt_norm = pdodt[i].odt_rtt_norm;
+		debug("     odt_rtt_norm 0x%x\n",
+			  popts->cs_odt[i].odt_rtt_norm);
+		popts->cs_odt[i].odt_rtt_wr = pdodt[i].odt_rtt_wr;
+		debug("     odt_rtt_wr 0x%x\n",
+			  popts->cs_odt[i].odt_rtt_wr);
+		popts->cs_odt[i].auto_precharge = 0;
+		debug("     auto_precharge %d\n",
+			  popts->cs_odt[i].auto_precharge);
+	}
+
+	return 0;
+}
+
+static int cal_opts(const unsigned int clk,
+		    struct memctl_opt *popts,
+		    struct ddr_conf *conf,
+		    struct dimm_params *pdimm,
+		    const int dimm_slot_per_ctrl,
+		    const unsigned int ip_rev)
+{
+	popts->rdimm = pdimm->rdimm;
+	popts->mirrored_dimm = pdimm->mirrored_dimm;
+#ifdef CONFIG_DDR_ECC_EN
+	popts->ecc_mode = pdimm->edc_config == 0x02 ? 1 : 0;
+#endif
+	popts->ctlr_init_ecc = popts->ecc_mode;
+	debug("ctlr_init_ecc %d\n", popts->ctlr_init_ecc);
+	popts->self_refresh_in_sleep = 1;
+	popts->dynamic_power = 0;
+
+	/*
+	 * check sdram width, allow platform override
+	 * 0 = 64-bit, 1 = 32-bit, 2 = 16-bit
+	 */
+	if (pdimm->primary_sdram_width == 64) {
+		popts->data_bus_dimm = DDR_DBUS_64;
+		popts->otf_burst_chop_en = 1;
+	} else if (pdimm->primary_sdram_width == 32) {
+		popts->data_bus_dimm = DDR_DBUS_32;
+		popts->otf_burst_chop_en = 0;
+	} else if (pdimm->primary_sdram_width == 16) {
+		popts->data_bus_dimm = DDR_DBUS_16;
+		popts->otf_burst_chop_en = 0;
+	} else {
+		ERROR("primary sdram width invalid!\n");
+		return -EINVAL;
+	}
+	popts->data_bus_used = popts->data_bus_dimm;
+	popts->x4_en = (pdimm->device_width == 4) ? 1 : 0;
+	debug("x4_en %d\n", popts->x4_en);
+
+	/* for RDIMM and DDR4 UDIMM/discrete memory, address parity enable */
+	if (popts->rdimm != 0) {
+		popts->ap_en = 1; /* 0 = disable,  1 = enable */
+	} else {
+		popts->ap_en = 0; /* disabled for DDR4 UDIMM/discrete default */
+	}
+
+	if (ip_rev == 0x50500) {
+		popts->ap_en = 0;
+	}
+
+	debug("ap_en %d\n", popts->ap_en);
+
+	/* BSTTOPRE precharge interval uses 1/4 of refint value. */
+	popts->bstopre = picos_to_mclk(clk, pdimm->refresh_rate_ps) >> 2;
+	popts->tfaw_ps = pdimm->tfaw_ps;
+
+	return 0;
+}
+
+static void cal_intlv(const int num_ctlrs,
+		      struct memctl_opt *popts,
+		      struct ddr_conf *conf,
+		      struct dimm_params *pdimm)
+{
+#ifdef NXP_DDR_INTLV_256B
+	if (num_ctlrs == 2) {
+		popts->ctlr_intlv = 1;
+		popts->ctlr_intlv_mode = DDR_256B_INTLV;
+	}
+#endif
+	debug("ctlr_intlv %d\n", popts->ctlr_intlv);
+	debug("ctlr_intlv_mode %d\n", popts->ctlr_intlv_mode);
+
+	popts->ba_intlv = auto_bank_intlv(conf->cs_in_use, pdimm);
+	debug("ba_intlv 0x%x\n", popts->ba_intlv);
+}
+
+static int update_burst_length(struct memctl_opt *popts)
+{
+	/* Choose burst length. */
+	if ((popts->data_bus_used == DDR_DBUS_32) ||
+	    (popts->data_bus_used == DDR_DBUS_16)) {
+		/* 32-bit or 16-bit bus */
+		popts->otf_burst_chop_en = 0;
+		popts->burst_length = DDR_BL8;
+	} else if (popts->otf_burst_chop_en != 0) { /* on-the-fly burst chop */
+		popts->burst_length = DDR_OTF;	/* on-the-fly BC4 and BL8 */
+	} else {
+		popts->burst_length = DDR_BL8;
+	}
+	debug("data_bus_used %d\n", popts->data_bus_used);
+	debug("otf_burst_chop_en %d\n", popts->otf_burst_chop_en);
+	debug("burst_length 0x%x\n", popts->burst_length);
+	/*
+	 * If a reduced data width is requested, but the SPD
+	 * specifies a physically wider device, adjust the
+	 * computed dimm capacities accordingly before
+	 * assigning addresses.
+	 * 0 = 64-bit, 1 = 32-bit, 2 = 16-bit
+	 */
+	if (popts->data_bus_dimm > popts->data_bus_used) {
+		ERROR("Data bus configuration error\n");
+		return -EINVAL;
+	}
+	popts->dbw_cap_shift = popts->data_bus_used - popts->data_bus_dimm;
+	debug("dbw_cap_shift %d\n", popts->dbw_cap_shift);
+
+	return 0;
+}
+
+int cal_board_params(struct ddr_info *priv,
+		     const struct board_timing *dimm,
+		     int len)
+{
+	const unsigned long speed = priv->clk / 1000000;
+	const struct dimm_params *pdimm = &priv->dimm;
+	struct memctl_opt *popts = &priv->opt;
+	struct rc_timing const *prt = NULL;
+	struct rc_timing const *chosen = NULL;
+	int i;
+
+	for (i = 0; i < len; i++) {
+		if (pdimm->rc == dimm[i].rc) {
+			prt = dimm[i].p;
+			break;
+		}
+	}
+	if (prt == NULL) {
+		ERROR("Board parameters no match.\n");
+		return -EINVAL;
+	}
+	while (prt->speed_bin != 0) {
+		if (speed <= prt->speed_bin) {
+			chosen = prt;
+			break;
+		}
+		prt++;
+	}
+	if (chosen == NULL) {
+		ERROR("timing no match for speed %lu\n", speed);
+		return -EINVAL;
+	}
+	popts->clk_adj = prt->clk_adj;
+	popts->wrlvl_start = prt->wrlvl;
+	popts->wrlvl_ctl_2 = (prt->wrlvl * 0x01010101 + dimm[i].add1) &
+			     0xFFFFFFFF;
+	popts->wrlvl_ctl_3 = (prt->wrlvl * 0x01010101 + dimm[i].add2) &
+			     0xFFFFFFFF;
+
+	return 0;
+}
+
+static int synthesize_ctlr(struct ddr_info *priv)
+{
+	int ret;
+
+	ret = cal_odt(priv->clk,
+		      &priv->opt,
+		      &priv->conf,
+		      &priv->dimm,
+		      priv->dimm_on_ctlr);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = cal_opts(priv->clk,
+		       &priv->opt,
+		       &priv->conf,
+		       &priv->dimm,
+		       priv->dimm_on_ctlr,
+		       priv->ip_rev);
+
+	if (ret != 0) {
+		return ret;
+	}
+
+	cal_intlv(priv->num_ctlrs, &priv->opt, &priv->conf, &priv->dimm);
+	ret = ddr_board_options(priv);
+	if (ret != 0) {
+		ERROR("Failed matching board timing.\n");
+	}
+
+	ret = update_burst_length(&priv->opt);
+
+	return ret;
+}
+
+/* Return the bit mask of valid DIMMs found */
+static int parse_spd(struct ddr_info *priv)
+{
+	struct ddr_conf *conf = &priv->conf;
+	struct dimm_params *dimm = &priv->dimm;
+	int j, valid_mask = 0;
+
+#ifdef CONFIG_DDR_NODIMM
+	valid_mask = ddr_get_ddr_params(dimm, conf);
+	if (valid_mask < 0) {
+		ERROR("DDR params error\n");
+		return valid_mask;
+	}
+#else
+	const int *spd_addr = priv->spd_addr;
+	const int num_ctlrs = priv->num_ctlrs;
+	const int num_dimm = priv->dimm_on_ctlr;
+	struct ddr4_spd spd[2];
+	unsigned int spd_checksum[2];
+	int addr_idx = 0;
+	int spd_idx = 0;
+	int ret, addr, i;
+
+	/* Scan all DIMMs */
+	for (i = 0; i < num_ctlrs; i++) {
+		debug("Controller %d\n", i);
+		for (j = 0; j < num_dimm; j++, addr_idx++) {
+			debug("DIMM %d\n", j);
+			addr = spd_addr[addr_idx];
+			if (addr == 0) {
+				if (j == 0) {
+					ERROR("First SPD addr wrong.\n");
+					return -EINVAL;
+				}
+				continue;
+			}
+			debug("addr 0x%x\n", addr);
+			ret = read_spd(addr, &spd[spd_idx],
+				       sizeof(struct ddr4_spd));
+			if (ret != 0) {	/* invalid */
+				debug("Invalid SPD at address 0x%x\n", addr);
+				continue;
+			}
+
+			spd_checksum[spd_idx] =
+				(spd[spd_idx].crc[1] << 24) |
+				(spd[spd_idx].crc[0] << 16) |
+				(spd[spd_idx].mod_section.uc[127] << 8) |
+				(spd[spd_idx].mod_section.uc[126] << 0);
+			debug("checksum 0x%x\n", spd_checksum[spd_idx]);
+			if (spd_checksum[spd_idx] == 0) {
+				debug("Bad checksum, ignored.\n");
+				continue;
+			}
+			if (spd_idx == 0) {
+				/* first valid SPD */
+				ret = cal_dimm_params(&spd[0], dimm);
+				if (ret != 0) {
+					ERROR("SPD calculation error\n");
+					return -EINVAL;
+				}
+			}
+
+			if (spd_idx != 0 && spd_checksum[0] !=
+			    spd_checksum[spd_idx]) {
+				ERROR("Not identical DIMMs.\n");
+				return -EINVAL;
+			}
+			conf->dimm_in_use[j] = 1;
+			valid_mask |= 1 << addr_idx;
+			spd_idx = 1;
+		}
+		debug("done with controller %d\n", i);
+	}
+	switch (num_ctlrs) {
+	case 1:
+		if ((valid_mask & 0x1) == 0) {
+			ERROR("First slot cannot be empty.\n");
+			return -EINVAL;
+		}
+		break;
+	case 2:
+		switch (num_dimm) {
+		case 1:
+			if (valid_mask == 0) {
+				ERROR("Both slot empty\n");
+				return -EINVAL;
+			}
+			break;
+		case 2:
+			if (valid_mask != 0x5 &&
+			    valid_mask != 0xf &&
+			    (valid_mask & 0x7) != 0x4 &&
+			    (valid_mask & 0xd) != 0x1) {
+				ERROR("Invalid DIMM combination.\n");
+				return -EINVAL;
+			}
+			break;
+		default:
+			ERROR("Invalid number of DIMMs.\n");
+			return -EINVAL;
+		}
+		break;
+	default:
+		ERROR("Invalid number of controllers.\n");
+		return -EINVAL;
+	}
+	/* now we have valid and identical DIMMs on controllers */
+#endif	/* CONFIG_DDR_NODIMM */
+
+	debug("cal cs\n");
+	conf->cs_in_use = 0;
+	for (j = 0; j < DDRC_NUM_DIMM; j++) {
+		if (conf->dimm_in_use[j] == 0) {
+			continue;
+		}
+		switch (dimm->n_ranks) {
+		case 4:
+			ERROR("Quad-rank DIMM not supported\n");
+			return -EINVAL;
+		case 2:
+			conf->cs_on_dimm[j] = 0x3 << (j * CONFIG_CS_PER_SLOT);
+			conf->cs_in_use |= conf->cs_on_dimm[j];
+			break;
+		case 1:
+			conf->cs_on_dimm[j] = 0x1 << (j * CONFIG_CS_PER_SLOT);
+			conf->cs_in_use |= conf->cs_on_dimm[j];
+			break;
+		default:
+			ERROR("SPD error with n_ranks\n");
+			return -EINVAL;
+		}
+		debug("cs_in_use = %x\n", conf->cs_in_use);
+		debug("cs_on_dimm[%d] = %x\n", j, conf->cs_on_dimm[j]);
+	}
+#ifndef CONFIG_DDR_NODIMM
+	if (priv->dimm.rdimm != 0) {
+		NOTICE("RDIMM %s\n", priv->dimm.mpart);
+	} else {
+		NOTICE("UDIMM %s\n", priv->dimm.mpart);
+	}
+#else
+	NOTICE("%s\n", priv->dimm.mpart);
+#endif
+
+	return valid_mask;
+}
+
+static unsigned long long assign_intlv_addr(
+	const struct dimm_params *pdimm,
+	const struct memctl_opt *opt,
+	struct ddr_conf *conf,
+	const unsigned long long current_mem_base)
+{
+	int i;
+	int ctlr_density_mul = 0;
+	const unsigned long long rank_density = pdimm->rank_density >>
+						opt->dbw_cap_shift;
+	unsigned long long total_ctlr_mem;
+
+	debug("rank density 0x%llx\n", rank_density);
+	switch (opt->ba_intlv & DDR_BA_INTLV_CS0123) {
+	case DDR_BA_INTLV_CS0123:
+		ctlr_density_mul = 4;
+		break;
+	case DDR_BA_INTLV_CS01:
+		ctlr_density_mul = 2;
+		break;
+	default:
+		ctlr_density_mul = 1;
+		break;
+	}
+	debug("ctlr density mul %d\n", ctlr_density_mul);
+	switch (opt->ctlr_intlv_mode) {
+	case DDR_256B_INTLV:
+		total_ctlr_mem = 2 * ctlr_density_mul * rank_density;
+		break;
+	default:
+		ERROR("Unknown interleaving mode");
+		return 0;
+	}
+	conf->base_addr = current_mem_base;
+	conf->total_mem = total_ctlr_mem;
+
+	/* overwrite cs_in_use bitmask with controller interleaving */
+	conf->cs_in_use = (1 << ctlr_density_mul) - 1;
+	debug("Overwrite cs_in_use as %x\n", conf->cs_in_use);
+
+	/* Fill addr with each cs in use */
+	for (i = 0; i < ctlr_density_mul; i++) {
+		conf->cs_base_addr[i] = current_mem_base;
+		conf->cs_size[i] = total_ctlr_mem;
+		debug("CS %d\n", i);
+		debug("    base_addr 0x%llx\n", conf->cs_base_addr[i]);
+		debug("    size 0x%llx\n", conf->cs_size[i]);
+	}
+
+	return total_ctlr_mem;
+}
+
+static unsigned long long assign_non_intlv_addr(
+	const struct dimm_params *pdimm,
+	const struct memctl_opt *opt,
+	struct ddr_conf *conf,
+	unsigned long long current_mem_base)
+{
+	int i;
+	const unsigned long long rank_density = pdimm->rank_density >>
+						opt->dbw_cap_shift;
+	unsigned long long total_ctlr_mem = 0ULL;
+
+	debug("rank density 0x%llx\n", rank_density);
+	conf->base_addr = current_mem_base;
+
+	/* assign each cs */
+	switch (opt->ba_intlv & DDR_BA_INTLV_CS0123) {
+	case DDR_BA_INTLV_CS0123:
+		for (i = 0; i < DDRC_NUM_CS; i++) {
+			conf->cs_base_addr[i] = current_mem_base;
+			conf->cs_size[i] = rank_density << 2;
+			total_ctlr_mem += rank_density;
+		}
+		break;
+	case DDR_BA_INTLV_CS01:
+		for (i = 0; ((conf->cs_in_use & (1 << i)) != 0) && i < 2; i++) {
+			conf->cs_base_addr[i] = current_mem_base;
+			conf->cs_size[i] = rank_density << 1;
+			total_ctlr_mem += rank_density;
+		}
+		current_mem_base += total_ctlr_mem;
+		for (; ((conf->cs_in_use & (1 << i)) != 0) && i < DDRC_NUM_CS;
+		     i++) {
+			conf->cs_base_addr[i] = current_mem_base;
+			conf->cs_size[i] = rank_density;
+			total_ctlr_mem += rank_density;
+			current_mem_base += rank_density;
+		}
+		break;
+	case DDR_BA_NONE:
+		for (i = 0; ((conf->cs_in_use & (1 << i)) != 0) &&
+			     (i < DDRC_NUM_CS); i++) {
+			conf->cs_base_addr[i] = current_mem_base;
+			conf->cs_size[i] = rank_density;
+			current_mem_base += rank_density;
+			total_ctlr_mem += rank_density;
+		}
+		break;
+	default:
+		ERROR("Unsupported bank interleaving\n");
+		return 0;
+	}
+	for (i = 0; ((conf->cs_in_use & (1 << i)) != 0) &&
+		     (i < DDRC_NUM_CS); i++) {
+		debug("CS %d\n", i);
+		debug("    base_addr 0x%llx\n", conf->cs_base_addr[i]);
+		debug("    size 0x%llx\n", conf->cs_size[i]);
+	}
+
+	return total_ctlr_mem;
+}
+
+unsigned long long assign_addresses(struct ddr_info *priv)
+		   __attribute__ ((weak));
+
+unsigned long long assign_addresses(struct ddr_info *priv)
+{
+	struct memctl_opt *opt = &priv->opt;
+	const struct dimm_params *dimm = &priv->dimm;
+	struct ddr_conf *conf = &priv->conf;
+	unsigned long long current_mem_base = priv->mem_base;
+	unsigned long long total_mem;
+
+	total_mem = 0ULL;
+	debug("ctlr_intlv %d\n", opt->ctlr_intlv);
+	if (opt->ctlr_intlv != 0) {
+		total_mem = assign_intlv_addr(dimm, opt, conf,
+					      current_mem_base);
+	} else {
+		/*
+		 * Simple linear assignment if memory controllers are not
+		 * interleaved. This is only valid for SoCs with single DDRC.
+		 */
+		total_mem = assign_non_intlv_addr(dimm, opt, conf,
+						  current_mem_base);
+	}
+	conf->total_mem = total_mem;
+	debug("base 0x%llx\n", current_mem_base);
+	debug("Total mem by assignment is 0x%llx\n", total_mem);
+
+	return total_mem;
+}
+
+static int cal_ddrc_regs(struct ddr_info *priv)
+{
+	int ret;
+
+	ret = compute_ddrc(priv->clk,
+			   &priv->opt,
+			   &priv->conf,
+			   &priv->ddr_reg,
+			   &priv->dimm,
+			   priv->ip_rev);
+	if (ret != 0) {
+		ERROR("Calculating DDR registers failed\n");
+	}
+
+	return ret;
+}
+
+#endif /* CONFIG_STATIC_DDR */
+
+static int write_ddrc_regs(struct ddr_info *priv)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < priv->num_ctlrs; i++) {
+		ret = ddrc_set_regs(priv->clk, &priv->ddr_reg, priv->ddr[i], 0);
+		if (ret != 0) {
+			ERROR("Writing DDR register(s) failed\n");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+long long dram_init(struct ddr_info *priv
+#if defined(NXP_HAS_CCN504) || defined(NXP_HAS_CCN508)
+		    , uintptr_t nxp_ccn_hn_f0_addr
+#endif
+		)
+{
+	uint64_t time __unused;
+	long long dram_size;
+	int ret;
+	const uint64_t time_base = get_timer_val(0);
+	unsigned int ip_rev = get_ddrc_version(priv->ddr[0]);
+
+	int valid_spd_mask __unused;
+	int scratch = 0x0;
+
+	priv->ip_rev = ip_rev;
+
+#ifndef CONFIG_STATIC_DDR
+	INFO("time base %" PRIu64 " ms\n", time_base);
+	debug("Parse DIMM SPD(s)\n");
+	valid_spd_mask = parse_spd(priv);
+
+	if (valid_spd_mask < 0) {
+		ERROR("Parsing DIMM Error\n");
+		return valid_spd_mask;
+	}
+
+#if defined(NXP_HAS_CCN504) || defined(NXP_HAS_CCN508)
+	if (priv->num_ctlrs == 2 || priv->num_ctlrs == 1) {
+		ret = disable_unused_ddrc(priv, valid_spd_mask,
+					  nxp_ccn_hn_f0_addr);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+#endif
+
+	time = get_timer_val(time_base);
+	INFO("Time after parsing SPD %" PRIu64 " ms\n", time);
+	debug("Synthesize configurations\n");
+	ret = synthesize_ctlr(priv);
+	if (ret != 0) {
+		ERROR("Synthesize config error\n");
+		return ret;
+	}
+
+	debug("Assign binding addresses\n");
+	dram_size = assign_addresses(priv);
+	if (dram_size == 0) {
+		ERROR("Assigning address error\n");
+		return -EINVAL;
+	}
+
+	debug("Calculate controller registers\n");
+	ret = cal_ddrc_regs(priv);
+	if (ret != 0) {
+		ERROR("Calculate register error\n");
+		return ret;
+	}
+
+	ret = compute_ddr_phy(priv);
+	if (ret != 0)
+		ERROR("Calculating DDR PHY registers failed.\n");
+
+#else
+	dram_size = board_static_ddr(priv);
+	if (dram_size == 0) {
+		ERROR("Error getting static DDR settings.\n");
+		return -EINVAL;
+	}
+#endif
+
+	if (priv->warm_boot_flag == DDR_WARM_BOOT) {
+		scratch = (priv->ddr_reg).sdram_cfg[1];
+		scratch = scratch & ~(SDRAM_CFG2_D_INIT);
+		priv->ddr_reg.sdram_cfg[1] = scratch;
+	}
+
+	time = get_timer_val(time_base);
+	INFO("Time before programming controller %" PRIu64 " ms\n", time);
+	debug("Program controller registers\n");
+	ret = write_ddrc_regs(priv);
+	if (ret != 0) {
+		ERROR("Programing DDRC error\n");
+		return ret;
+	}
+
+	puts("");
+	NOTICE("%lld GB ", dram_size >> 30);
+	print_ddr_info(priv->ddr[0]);
+
+	time = get_timer_val(time_base);
+	INFO("Time used by DDR driver %" PRIu64 " ms\n", time);
+
+	return dram_size;
+}
diff --git a/drivers/nxp/ddr/nxp-ddr/ddr.mk b/drivers/nxp/ddr/nxp-ddr/ddr.mk
new file mode 100644
index 0000000..f827a1b
--- /dev/null
+++ b/drivers/nxp/ddr/nxp-ddr/ddr.mk
@@ -0,0 +1,80 @@
+#
+# Copyright 2021-2022 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq ($(PLAT_DDR_PHY), PHY_GEN2)
+$(eval $(call add_define, PHY_GEN2))
+PLAT_DDR_PHY_DIR		:= phy-gen2
+ifeq (${APPLY_MAX_CDD},yes)
+$(eval $(call add_define,NXP_APPLY_MAX_CDD))
+endif
+
+ifeq (${ERRATA_DDR_A011396}, 1)
+$(eval $(call add_define,ERRATA_DDR_A011396))
+endif
+
+ifeq (${ERRATA_DDR_A050450}, 1)
+$(eval $(call add_define,ERRATA_DDR_A050450))
+endif
+
+ifeq (${ERRATA_DDR_A050958}, 1)
+$(eval $(call add_define,ERRATA_DDR_A050958))
+endif
+
+endif
+
+ifeq ($(PLAT_DDR_PHY), PHY_GEN1)
+PLAT_DDR_PHY_DIR		:= phy-gen1
+
+ifeq (${ERRATA_DDR_A008511},1)
+$(eval $(call add_define,ERRATA_DDR_A008511))
+endif
+
+ifeq (${ERRATA_DDR_A009803},1)
+$(eval $(call add_define,ERRATA_DDR_A009803))
+endif
+
+ifeq (${ERRATA_DDR_A009942},1)
+$(eval $(call add_define,ERRATA_DDR_A009942))
+endif
+
+ifeq (${ERRATA_DDR_A010165},1)
+$(eval $(call add_define,ERRATA_DDR_A010165))
+endif
+
+endif
+
+ifeq ($(DDR_BIST), yes)
+$(eval $(call add_define, BIST_EN))
+endif
+
+ifeq ($(DDR_DEBUG), yes)
+$(eval $(call add_define, DDR_DEBUG))
+endif
+
+ifeq ($(DDR_PHY_DEBUG), yes)
+$(eval $(call add_define, DDR_PHY_DEBUG))
+endif
+
+ifeq ($(DEBUG_PHY_IO), yes)
+$(eval $(call add_define, DEBUG_PHY_IO))
+endif
+
+ifeq ($(DEBUG_WARM_RESET), yes)
+$(eval $(call add_define, DEBUG_WARM_RESET))
+endif
+
+ifeq ($(DEBUG_DDR_INPUT_CONFIG), yes)
+$(eval $(call add_define, DEBUG_DDR_INPUT_CONFIG))
+endif
+
+DDR_CNTLR_SOURCES	:= $(PLAT_DRIVERS_PATH)/ddr/nxp-ddr/ddr.c \
+			   $(PLAT_DRIVERS_PATH)/ddr/nxp-ddr/ddrc.c \
+			   $(PLAT_DRIVERS_PATH)/ddr/nxp-ddr/dimm.c \
+			   $(PLAT_DRIVERS_PATH)/ddr/nxp-ddr/regs.c \
+			   $(PLAT_DRIVERS_PATH)/ddr/nxp-ddr/utility.c \
+			   $(PLAT_DRIVERS_PATH)/ddr/$(PLAT_DDR_PHY_DIR)/phy.c
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/ddr
diff --git a/drivers/nxp/ddr/nxp-ddr/ddrc.c b/drivers/nxp/ddr/nxp-ddr/ddrc.c
new file mode 100644
index 0000000..17a2b6a
--- /dev/null
+++ b/drivers/nxp/ddr/nxp-ddr/ddrc.c
@@ -0,0 +1,594 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <common/debug.h>
+#include <ddr.h>
+#include <drivers/delay_timer.h>
+#include <immap.h>
+
+#define BIST_CR		0x80060000
+#define BIST_CR_EN	0x80000000
+#define BIST_CR_STAT	0x00000001
+#define CTLR_INTLV_MASK	0x20000000
+
+#pragma weak run_bist
+
+bool run_bist(void)
+{
+#ifdef BIST_EN
+	return true;
+#else
+	return false;
+#endif
+}
+
+/*
+ * Perform build-in test on memory
+ * timeout value in 10ms
+ */
+int bist(const struct ccsr_ddr *ddr, int timeout)
+{
+	const unsigned int test_pattern[10] = {
+		0xffffffff,
+		0x00000000,
+		0xaaaaaaaa,
+		0x55555555,
+		0xcccccccc,
+		0x33333333,
+		0x12345678,
+		0xabcdef01,
+		0xaa55aa55,
+		0x55aa55aa
+	};
+	unsigned int mtcr, err_detect, err_sbe;
+	unsigned int cs0_config;
+	unsigned int csn_bnds[4];
+	int ret = 0;
+	uint32_t i;
+#ifdef CONFIG_DDR_ADDR_DEC
+	uint32_t dec_9 = ddr_in32(&ddr->dec[9]);
+	uint32_t pos = 0U;
+	uint32_t map_save = 0U;
+	uint32_t temp32 = 0U;
+	uint32_t map, shift, highest;
+#endif
+
+	cs0_config = ddr_in32(&ddr->csn_cfg[0]);
+	if ((cs0_config & CTLR_INTLV_MASK) != 0U) {
+		/* set bnds to non-interleaving */
+		for (i = 0U; i < 4U; i++) {
+			csn_bnds[i] = ddr_in32(&ddr->bnds[i].a);
+			ddr_out32(&ddr->bnds[i].a,
+				  (csn_bnds[i] & U(0xfffefffe)) >> 1U);
+		}
+		ddr_out32(&ddr->csn_cfg[0], cs0_config & ~CTLR_INTLV_MASK);
+#ifdef CONFIG_DDR_ADDR_DEC
+		if ((dec_9 & 0x1U) != 0U) {
+			highest = (dec_9 >> 26U) == U(0x3F) ? 0U : dec_9 >> 26U;
+			pos = 37U;
+			for (i = 0U; i < 36U; i++) {      /* Go through all 37 */
+				if ((i % 4U) == 0U) {
+					temp32 = ddr_in32(&ddr->dec[i >> 2U]);
+				}
+				shift = (3U - i % 4U) * 8U + 2U;
+				map = (temp32 >> shift) & U(0x3F);
+				if (map > highest && map != U(0x3F)) {
+					highest = map;
+					pos = i;
+				}
+			}
+			debug("\nFound highest position %d, mapping to %d, ",
+			      pos, highest);
+			map_save = ddr_in32(&ddr->dec[pos >> 2]);
+			shift = (3U - pos % 4U) * 8U + 2U;
+			debug("in dec[%d], bit %d (0x%x)\n",
+			      pos >> 2U, shift, map_save);
+			temp32 = map_save & ~(U(0x3F) << shift);
+			temp32 |= 8U << shift;
+			ddr_out32(&ddr->dec[pos >> 2U], temp32);
+			timeout <<= 2U;
+			debug("Increase wait time to %d ms\n", timeout * 10);
+		}
+#endif
+	}
+	for (i = 0U; i < 10U; i++) {
+		ddr_out32(&ddr->mtp[i], test_pattern[i]);
+	}
+	mtcr = BIST_CR;
+	ddr_out32(&ddr->mtcr, mtcr);
+	do {
+		mdelay(10);
+		mtcr = ddr_in32(&ddr->mtcr);
+	} while (timeout-- > 0 && ((mtcr & BIST_CR_EN) != 0));
+	if (timeout <= 0) {
+		ERROR("Timeout\n");
+	} else {
+		debug("Timer remains %d\n", timeout);
+	}
+
+	err_detect = ddr_in32(&ddr->err_detect);
+	err_sbe = ddr_in32(&ddr->err_sbe);
+	if (err_detect != 0U || ((err_sbe & U(0xffff)) != 0U)) {
+		ERROR("ECC error detected\n");
+		ret = -EIO;
+	}
+
+	if ((cs0_config & CTLR_INTLV_MASK) != 0) {
+		for (i = 0U; i < 4U; i++) {
+			ddr_out32(&ddr->bnds[i].a, csn_bnds[i]);
+		}
+		ddr_out32(&ddr->csn_cfg[0], cs0_config);
+#ifdef CONFIG_DDR_ADDR_DEC
+		if ((dec_9 & U(0x1)) != 0U) {
+			ddr_out32(&ddr->dec[pos >> 2], map_save);
+		}
+#endif
+	}
+	if ((mtcr & BIST_CR_STAT) != 0) {
+		ERROR("Built-in self test failed\n");
+		ret = -EIO;
+	} else {
+		NOTICE("Build-in self test passed\n");
+	}
+
+	return ret;
+}
+
+void dump_ddrc(unsigned int *ddr)
+{
+#ifdef DDR_DEBUG
+	uint32_t i;
+	unsigned long val;
+
+	for (i = 0U; i < U(0x400); i++, ddr++) {
+		val = ddr_in32(ddr);
+		if (val != 0U) {	/* skip zeros */
+			debug("*0x%lx = 0x%lx\n", (unsigned long)ddr, val);
+		}
+	}
+#endif
+}
+
+#ifdef ERRATA_DDR_A009803
+static void set_wait_for_bits_clear(const void *ptr,
+				    unsigned int value,
+				    unsigned int bits)
+{
+	int timeout = 1000;
+
+	ddr_out32(ptr, value);
+	do {
+		udelay(100);
+	} while (timeout-- > 0 && ((ddr_in32(ptr) & bits) != 0));
+
+	if (timeout <= 0) {
+		ERROR("wait for clear timeout.\n");
+	}
+}
+#endif
+
+#if (DDRC_NUM_CS > 4)
+#error Invalid setting for DDRC_NUM_CS
+#endif
+
+/*
+ * If supported by the platform, writing to DDR controller takes two
+ * passes to deassert DDR reset to comply with JEDEC specs for RDIMMs.
+ */
+int ddrc_set_regs(const unsigned long clk,
+		  const struct ddr_cfg_regs *regs,
+		  const struct ccsr_ddr *ddr,
+		  int twopass)
+{
+	unsigned int i, bus_width;
+	unsigned int temp_sdram_cfg;
+	unsigned int total_mem_per_ctrl, total_mem_per_ctrl_adj;
+	const int mod_bnds = regs->cs[0].config & CTLR_INTLV_MASK;
+	int timeout;
+	int ret = 0;
+#if defined(ERRATA_DDR_A009942) || defined(ERRATA_DDR_A010165)
+	unsigned long ddr_freq;
+	unsigned int tmp;
+#ifdef ERRATA_DDR_A009942
+	unsigned int check;
+	unsigned int cpo_min = U(0xff);
+	unsigned int cpo_max = 0U;
+#endif
+#endif
+
+	if (twopass == 2U) {
+		goto after_reset;
+	}
+
+	/* Set cdr1 first in case 0.9v VDD is enabled for some SoCs*/
+	ddr_out32(&ddr->ddr_cdr1, regs->cdr[0]);
+
+	ddr_out32(&ddr->sdram_clk_cntl, regs->clk_cntl);
+
+	for (i = 0U; i < DDRC_NUM_CS; i++) {
+		if (mod_bnds != 0U) {
+			ddr_out32(&ddr->bnds[i].a,
+				  (regs->cs[i].bnds & U(0xfffefffe)) >> 1U);
+		} else {
+			ddr_out32(&ddr->bnds[i].a, regs->cs[i].bnds);
+		}
+		ddr_out32(&ddr->csn_cfg_2[i], regs->cs[i].config_2);
+	}
+
+	ddr_out32(&ddr->timing_cfg_0, regs->timing_cfg[0]);
+	ddr_out32(&ddr->timing_cfg_1, regs->timing_cfg[1]);
+	ddr_out32(&ddr->timing_cfg_2, regs->timing_cfg[2]);
+	ddr_out32(&ddr->timing_cfg_3, regs->timing_cfg[3]);
+	ddr_out32(&ddr->timing_cfg_4, regs->timing_cfg[4]);
+	ddr_out32(&ddr->timing_cfg_5, regs->timing_cfg[5]);
+	ddr_out32(&ddr->timing_cfg_6, regs->timing_cfg[6]);
+	ddr_out32(&ddr->timing_cfg_7, regs->timing_cfg[7]);
+	ddr_out32(&ddr->timing_cfg_8, regs->timing_cfg[8]);
+	ddr_out32(&ddr->timing_cfg_9, regs->timing_cfg[9]);
+	ddr_out32(&ddr->zq_cntl, regs->zq_cntl);
+	for (i = 0U; i < 4U; i++) {
+		ddr_out32(&ddr->dq_map[i], regs->dq_map[i]);
+	}
+	ddr_out32(&ddr->sdram_cfg_3, regs->sdram_cfg[2]);
+	ddr_out32(&ddr->sdram_mode, regs->sdram_mode[0]);
+	ddr_out32(&ddr->sdram_mode_2, regs->sdram_mode[1]);
+	ddr_out32(&ddr->sdram_mode_3, regs->sdram_mode[2]);
+	ddr_out32(&ddr->sdram_mode_4, regs->sdram_mode[3]);
+	ddr_out32(&ddr->sdram_mode_5, regs->sdram_mode[4]);
+	ddr_out32(&ddr->sdram_mode_6, regs->sdram_mode[5]);
+	ddr_out32(&ddr->sdram_mode_7, regs->sdram_mode[6]);
+	ddr_out32(&ddr->sdram_mode_8, regs->sdram_mode[7]);
+	ddr_out32(&ddr->sdram_mode_9, regs->sdram_mode[8]);
+	ddr_out32(&ddr->sdram_mode_10, regs->sdram_mode[9]);
+	ddr_out32(&ddr->sdram_mode_11, regs->sdram_mode[10]);
+	ddr_out32(&ddr->sdram_mode_12, regs->sdram_mode[11]);
+	ddr_out32(&ddr->sdram_mode_13, regs->sdram_mode[12]);
+	ddr_out32(&ddr->sdram_mode_14, regs->sdram_mode[13]);
+	ddr_out32(&ddr->sdram_mode_15, regs->sdram_mode[14]);
+	ddr_out32(&ddr->sdram_mode_16, regs->sdram_mode[15]);
+	ddr_out32(&ddr->sdram_md_cntl, regs->md_cntl);
+#ifdef ERRATA_DDR_A009663
+	ddr_out32(&ddr->sdram_interval,
+		  regs->interval & ~SDRAM_INTERVAL_BSTOPRE);
+#else
+	ddr_out32(&ddr->sdram_interval, regs->interval);
+#endif
+	ddr_out32(&ddr->sdram_data_init, regs->data_init);
+	if (regs->eor != 0) {
+		ddr_out32(&ddr->eor, regs->eor);
+	}
+
+	ddr_out32(&ddr->wrlvl_cntl, regs->wrlvl_cntl[0]);
+#ifndef NXP_DDR_EMU
+	/*
+	 * Skip these two registers if running on emulator
+	 * because emulator doesn't have skew between bytes.
+	 */
+
+	if (regs->wrlvl_cntl[1] != 0) {
+		ddr_out32(&ddr->ddr_wrlvl_cntl_2, regs->wrlvl_cntl[1]);
+	}
+	if (regs->wrlvl_cntl[2] != 0) {
+		ddr_out32(&ddr->ddr_wrlvl_cntl_3, regs->wrlvl_cntl[2]);
+	}
+#endif
+
+	ddr_out32(&ddr->ddr_sr_cntr, regs->ddr_sr_cntr);
+	ddr_out32(&ddr->ddr_sdram_rcw_1, regs->sdram_rcw[0]);
+	ddr_out32(&ddr->ddr_sdram_rcw_2, regs->sdram_rcw[1]);
+	ddr_out32(&ddr->ddr_sdram_rcw_3, regs->sdram_rcw[2]);
+	ddr_out32(&ddr->ddr_sdram_rcw_4, regs->sdram_rcw[3]);
+	ddr_out32(&ddr->ddr_sdram_rcw_5, regs->sdram_rcw[4]);
+	ddr_out32(&ddr->ddr_sdram_rcw_6, regs->sdram_rcw[5]);
+	ddr_out32(&ddr->ddr_cdr2, regs->cdr[1]);
+	ddr_out32(&ddr->sdram_cfg_2, regs->sdram_cfg[1]);
+	ddr_out32(&ddr->init_addr, regs->init_addr);
+	ddr_out32(&ddr->init_ext_addr, regs->init_ext_addr);
+
+#ifdef ERRATA_DDR_A009803
+	/* part 1 of 2 */
+	if ((regs->sdram_cfg[1] & SDRAM_CFG2_AP_EN) != 0) {
+		if ((regs->sdram_cfg[0] & SDRAM_CFG_RD_EN) != 0) {
+			ddr_out32(&ddr->ddr_sdram_rcw_2,
+				  regs->sdram_rcw[1] & ~0xf0);
+		}
+
+		ddr_out32(&ddr->err_disable,
+				regs->err_disable | DDR_ERR_DISABLE_APED);
+	}
+#else
+	ddr_out32(&ddr->err_disable, regs->err_disable);
+#endif
+	ddr_out32(&ddr->err_int_en, regs->err_int_en);
+
+	/* For DDRC 5.05 only */
+	if (get_ddrc_version(ddr) == 0x50500) {
+		ddr_out32(&ddr->tx_cfg[1], 0x1f1f1f1f);
+		ddr_out32(&ddr->debug[3], 0x124a02c0);
+	}
+
+	for (i = 0U; i < 4U; i++) {
+		if (regs->tx_cfg[i] != 0) {
+			ddr_out32(&ddr->tx_cfg[i], regs->tx_cfg[i]);
+		}
+	}
+	for (i = 0U; i < 64U; i++) {
+		if (regs->debug[i] != 0) {
+#ifdef ERRATA_DDR_A009942
+			if (i == 28U) {
+				continue;
+			}
+#endif
+			ddr_out32(&ddr->debug[i], regs->debug[i]);
+		}
+	}
+#ifdef CONFIG_DDR_ADDR_DEC
+	if ((regs->dec[9] & 1) != 0U) {
+		for (i = 0U; i < 10U; i++) {
+			ddr_out32(&ddr->dec[i], regs->dec[i]);
+		}
+		if (mod_bnds != 0) {
+			debug("Disable address decoding\n");
+			ddr_out32(&ddr->dec[9], 0);
+		}
+	}
+#endif
+
+#ifdef ERRATA_DDR_A008511
+	/* Part 1 of 2 */
+	/* This erraum only applies to verion 5.2.1 */
+	if (get_ddrc_version(ddr) == 0x50200) {
+		ERROR("Unsupported SoC.\n");
+	} else if (get_ddrc_version(ddr) == 0x50201) {
+		ddr_out32(&ddr->debug[37], (U(1) << 31));
+		ddr_out32(&ddr->ddr_cdr2,
+			  regs->cdr[1] | DDR_CDR2_VREF_TRAIN_EN);
+	} else {
+		debug("Erratum A008511 doesn't apply.\n");
+	}
+#endif
+
+#ifdef ERRATA_DDR_A009942
+	ddr_freq = clk / 1000000U;
+	tmp = ddr_in32(&ddr->debug[28]);
+	tmp &= U(0xff0fff00);
+	tmp |= ddr_freq <= 1333U ? U(0x0080006a) :
+		(ddr_freq <= 1600U ? U(0x0070006f) :
+		 (ddr_freq <= 1867U ? U(0x00700076) : U(0x0060007b)));
+	if (regs->debug[28] != 0) {
+		tmp &= ~0xff;
+		tmp |= regs->debug[28] & 0xff;
+	} else {
+		WARN("Warning: Optimal CPO value not set.\n");
+	}
+	ddr_out32(&ddr->debug[28], tmp);
+#endif
+
+#ifdef ERRATA_DDR_A010165
+	ddr_freq = clk / 1000000U;
+	if ((ddr_freq > 1900) && (ddr_freq < 2300)) {
+		tmp = ddr_in32(&ddr->debug[28]);
+		ddr_out32(&ddr->debug[28], tmp | 0x000a0000);
+	}
+#endif
+	/*
+	 * For RDIMMs, JEDEC spec requires clocks to be stable before reset is
+	 * deasserted. Clocks start when any chip select is enabled and clock
+	 * control register is set. Because all DDR components are connected to
+	 * one reset signal, this needs to be done in two steps. Step 1 is to
+	 * get the clocks started. Step 2 resumes after reset signal is
+	 * deasserted.
+	 */
+	if (twopass == 1) {
+		udelay(200);
+		return 0;
+	}
+
+	/* As per new sequence flow shall be write CSn_CONFIG registers needs to
+	 * be set after all the other DDR controller registers are set, then poll
+	 * for PHY_INIT_CMPLT = 1 , then wait at least 100us (micro seconds),
+	 * then set the MEM_EN = 1
+	 */
+	for (i = 0U; i < DDRC_NUM_CS; i++) {
+		if (mod_bnds != 0U && i == 0U) {
+			ddr_out32(&ddr->csn_cfg[i],
+					(regs->cs[i].config & ~CTLR_INTLV_MASK));
+		} else {
+			ddr_out32(&ddr->csn_cfg[i], regs->cs[i].config);
+		}
+	}
+
+after_reset:
+	/* Set, but do not enable the memory */
+	temp_sdram_cfg = regs->sdram_cfg[0];
+	temp_sdram_cfg &= ~(SDRAM_CFG_MEM_EN);
+	ddr_out32(&ddr->sdram_cfg, temp_sdram_cfg);
+
+	if (get_ddrc_version(ddr) < U(0x50500)) {
+		/*
+		 * 500 painful micro-seconds must elapse between
+		 * the DDR clock setup and the DDR config enable.
+		 * DDR2 need 200 us, and DDR3 need 500 us from spec,
+		 * we choose the max, that is 500 us for all of case.
+		 */
+		udelay(500);
+		/* applied memory barrier */
+		mb();
+		isb();
+	} else {
+		/* wait for PHY complete */
+		timeout = 40;
+		while (((ddr_in32(&ddr->ddr_dsr2) & 0x4) != 0) &&
+		       (timeout > 0)) {
+			udelay(500);
+			timeout--;
+		}
+		if (timeout <= 0) {
+			printf("PHY handshake timeout, ddr_dsr2 = %x\n",
+			       ddr_in32(&ddr->ddr_dsr2));
+		} else {
+			debug("PHY handshake completed, timer remains %d\n",
+			      timeout);
+		}
+	}
+
+	temp_sdram_cfg = ddr_in32(&ddr->sdram_cfg);
+	/* Let the controller go */
+	udelay(100);
+	ddr_out32(&ddr->sdram_cfg, temp_sdram_cfg | SDRAM_CFG_MEM_EN);
+
+	/* applied memory barrier */
+	mb();
+	isb();
+
+	total_mem_per_ctrl = 0;
+	for (i = 0; i < DDRC_NUM_CS; i++) {
+		if ((regs->cs[i].config & 0x80000000) == 0) {
+			continue;
+		}
+		total_mem_per_ctrl += 1 << (
+			((regs->cs[i].config >> 14) & 0x3) + 2 +
+			((regs->cs[i].config >> 8) & 0x7) + 12 +
+			((regs->cs[i].config >> 4) & 0x3) + 0 +
+			((regs->cs[i].config >> 0) & 0x7) + 8 +
+			((regs->sdram_cfg[2] >> 4) & 0x3) +
+			3 - ((regs->sdram_cfg[0] >> 19) & 0x3) -
+			26);		/* minus 26 (count of 64M) */
+	}
+	total_mem_per_ctrl_adj = total_mem_per_ctrl;
+	/*
+	 * total memory / bus width = transactions needed
+	 * transactions needed / data rate = seconds
+	 * to add plenty of buffer, double the time
+	 * For example, 2GB on 666MT/s 64-bit bus takes about 402ms
+	 * Let's wait for 800ms
+	 */
+	bus_width = 3 - ((ddr_in32(&ddr->sdram_cfg) & SDRAM_CFG_DBW_MASK)
+			>> SDRAM_CFG_DBW_SHIFT);
+	timeout = ((total_mem_per_ctrl_adj << (6 - bus_width)) * 100 /
+		   (clk >> 20)) << 2;
+	total_mem_per_ctrl_adj >>= 4;	/* shift down to gb size */
+	if ((ddr_in32(&ddr->sdram_cfg_2) & SDRAM_CFG2_D_INIT) != 0) {
+		debug("total size %d GB\n", total_mem_per_ctrl_adj);
+		debug("Need to wait up to %d ms\n", timeout * 10);
+
+		do {
+			mdelay(10);
+		} while (timeout-- > 0 &&
+			 ((ddr_in32(&ddr->sdram_cfg_2) & SDRAM_CFG2_D_INIT)) != 0);
+
+		if (timeout <= 0) {
+			if (ddr_in32(&ddr->debug[1]) & 0x3d00) {
+				ERROR("Found training error(s): 0x%x\n",
+				      ddr_in32(&ddr->debug[1]));
+			}
+			ERROR("Error: Waiting for D_INIT timeout.\n");
+			return -EIO;
+		}
+	}
+
+	if (mod_bnds != 0U) {
+		debug("Restore original bnds\n");
+		for (i = 0U; i < DDRC_NUM_CS; i++) {
+			ddr_out32(&ddr->bnds[i].a, regs->cs[i].bnds);
+		}
+		ddr_out32(&ddr->csn_cfg[0], regs->cs[0].config);
+#ifdef CONFIG_DDR_ADDR_DEC
+		if ((regs->dec[9] & U(0x1)) != 0U) {
+			debug("Restore address decoding\n");
+			ddr_out32(&ddr->dec[9], regs->dec[9]);
+		}
+#endif
+	}
+
+#ifdef ERRATA_DDR_A009803
+	/* Part 2 of 2 */
+	if ((regs->sdram_cfg[1] & SDRAM_CFG2_AP_EN) != 0) {
+		timeout = 400;
+		do {
+			mdelay(1);
+		} while (timeout-- > 0 && ((ddr_in32(&ddr->debug[1]) & 0x2) == 0));
+
+		if ((regs->sdram_cfg[0] & SDRAM_CFG_RD_EN) != 0) {
+			for (i = 0U; i < DDRC_NUM_CS; i++) {
+				if ((regs->cs[i].config & SDRAM_CS_CONFIG_EN) == 0) {
+					continue;
+				}
+				set_wait_for_bits_clear(&ddr->sdram_md_cntl,
+						MD_CNTL_MD_EN |
+						MD_CNTL_CS_SEL(i) |
+						0x070000ed,
+						MD_CNTL_MD_EN);
+				udelay(1);
+			}
+		}
+
+		ddr_out32(&ddr->err_disable,
+			  regs->err_disable & ~DDR_ERR_DISABLE_APED);
+	}
+#endif
+
+#ifdef ERRATA_DDR_A009663
+	ddr_out32(&ddr->sdram_interval, regs->interval);
+#endif
+
+#ifdef ERRATA_DDR_A009942
+	timeout = 400;
+	do {
+		mdelay(1);
+	} while (timeout-- > 0 && ((ddr_in32(&ddr->debug[1]) & 0x2) == 0));
+	tmp = (regs->sdram_cfg[0] >> 19) & 0x3;
+	check = (tmp == DDR_DBUS_64) ? 4 : ((tmp == DDR_DBUS_32) ? 2 : 1);
+	for (i = 0; i < check; i++) {
+		tmp = ddr_in32(&ddr->debug[9 + i]);
+		debug("Reading debug[%d] as 0x%x\n", i + 9, tmp);
+		cpo_min = min(cpo_min,
+			      min((tmp >> 24) & 0xff, (tmp >> 8) & 0xff));
+		cpo_max = max(cpo_max,
+			      max((tmp >> 24) & 0xff, (tmp >> 8) & 0xff));
+	}
+	if ((regs->sdram_cfg[0] & SDRAM_CFG_ECC_EN) != 0) {
+		tmp = ddr_in32(&ddr->debug[13]);
+		cpo_min = min(cpo_min, (tmp >> 24) & 0xff);
+		cpo_max = max(cpo_max, (tmp >> 24) & 0xff);
+	}
+	debug("cpo_min 0x%x\n", cpo_min);
+	debug("cpo_max 0x%x\n", cpo_max);
+	tmp = ddr_in32(&ddr->debug[28]);
+	debug("debug[28] 0x%x\n", tmp);
+	if ((cpo_min + 0x3B) < (tmp & 0xff)) {
+		WARN("Warning: A009942 requires setting cpo_sample to 0x%x\n",
+		     (cpo_min + cpo_max) / 2 + 0x27);
+	} else {
+		debug("Optimal cpo_sample 0x%x\n",
+			(cpo_min + cpo_max) / 2 + 0x27);
+	}
+#endif
+	if (run_bist() != 0) {
+		if ((ddr_in32(&ddr->debug[1]) &
+		    ((get_ddrc_version(ddr) == 0x50500) ? 0x3c00 : 0x3d00)) != 0) {
+			ERROR("Found training error(s): 0x%x\n",
+			     ddr_in32(&ddr->debug[1]));
+			return -EIO;
+		}
+		INFO("Running built-in self test ...\n");
+		/* give it 10x time to cover whole memory */
+		timeout = ((total_mem_per_ctrl << (6 - bus_width)) *
+			   100 / (clk >> 20)) * 10;
+		INFO("\tWait up to %d ms\n", timeout * 10);
+		ret = bist(ddr, timeout);
+	}
+	dump_ddrc((void *)ddr);
+
+	return ret;
+}
diff --git a/drivers/nxp/ddr/nxp-ddr/dimm.c b/drivers/nxp/ddr/nxp-ddr/dimm.c
new file mode 100644
index 0000000..a82db6c
--- /dev/null
+++ b/drivers/nxp/ddr/nxp-ddr/dimm.c
@@ -0,0 +1,399 @@
+/*
+ * Copyright 2021-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+
+#include <common/debug.h>
+#include <ddr.h>
+#include <dimm.h>
+#include <i2c.h>
+#include <lib/utils.h>
+
+int read_spd(unsigned char chip, void *buf, int len)
+{
+	unsigned char dummy = 0U;
+	int ret;
+
+	if (len < 256) {
+		ERROR("Invalid SPD length\n");
+		return -EINVAL;
+	}
+
+	i2c_write(SPD_SPA0_ADDRESS, 0, 1, &dummy, 1);
+	ret = i2c_read(chip, 0, 1, buf, 256);
+	if (ret == 0) {
+		i2c_write(SPD_SPA1_ADDRESS, 0, 1, &dummy, 1);
+		ret = i2c_read(chip, 0, 1, buf + 256, min(256, len - 256));
+	}
+	if (ret != 0) {
+		zeromem(buf, len);
+	}
+
+	return ret;
+}
+
+int crc16(unsigned char *ptr, int count)
+{
+	int i;
+	int crc = 0;
+
+	while (--count >= 0) {
+		crc = crc ^ (int)*ptr++ << 8;
+		for (i = 0; i < 8; ++i) {
+			if ((crc & 0x8000) != 0) {
+				crc = crc << 1 ^ 0x1021;
+			} else {
+				crc = crc << 1;
+			}
+		}
+	}
+	return crc & 0xffff;
+}
+
+static int ddr4_spd_check(const struct ddr4_spd *spd)
+{
+	void *p = (void *)spd;
+	int csum16;
+	int len;
+	char crc_lsb;	/* byte 126 */
+	char crc_msb;	/* byte 127 */
+
+	len = 126;
+	csum16 = crc16(p, len);
+
+	crc_lsb = (char) (csum16 & 0xff);
+	crc_msb = (char) (csum16 >> 8);
+
+	if (spd->crc[0] != crc_lsb || spd->crc[1] != crc_msb) {
+		ERROR("SPD CRC = 0x%x%x, computed CRC = 0x%x%x\n",
+		      spd->crc[1], spd->crc[0], crc_msb, crc_lsb);
+		return -EINVAL;
+	}
+
+	p = (void *)spd + 128;
+	len = 126;
+	csum16 = crc16(p, len);
+
+	crc_lsb = (char) (csum16 & 0xff);
+	crc_msb = (char) (csum16 >> 8);
+
+	if (spd->mod_section.uc[126] != crc_lsb ||
+	    spd->mod_section.uc[127] != crc_msb) {
+		ERROR("SPD CRC = 0x%x%x, computed CRC = 0x%x%x\n",
+		      spd->mod_section.uc[127], spd->mod_section.uc[126],
+		      crc_msb, crc_lsb);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static unsigned long long
+compute_ranksize(const struct ddr4_spd *spd)
+{
+	unsigned long long bsize;
+
+	int nbit_sdram_cap_bsize = 0;
+	int nbit_primary_bus_width = 0;
+	int nbit_sdram_width = 0;
+	int die_count = 0;
+	bool package_3ds;
+
+	if ((spd->density_banks & 0xf) <= 7) {
+		nbit_sdram_cap_bsize = (spd->density_banks & 0xf) + 28;
+	}
+	if ((spd->bus_width & 0x7) < 4) {
+		nbit_primary_bus_width = (spd->bus_width & 0x7) + 3;
+	}
+	if ((spd->organization & 0x7) < 4) {
+		nbit_sdram_width = (spd->organization & 0x7) + 2;
+	}
+	package_3ds = (spd->package_type & 0x3) == 0x2;
+	if (package_3ds) {
+		die_count = (spd->package_type >> 4) & 0x7;
+	}
+
+	bsize = 1ULL << (nbit_sdram_cap_bsize - 3 +
+			 nbit_primary_bus_width - nbit_sdram_width +
+			 die_count);
+
+	return bsize;
+}
+
+int cal_dimm_params(const struct ddr4_spd *spd, struct dimm_params *pdimm)
+{
+	int ret;
+	int i;
+	static const unsigned char udimm_rc_e_dq[18] = {
+		0x0c, 0x2c, 0x15, 0x35, 0x15, 0x35, 0x0b, 0x2c, 0x15,
+		0x35, 0x0b, 0x35, 0x0b, 0x2c, 0x0b, 0x35, 0x15, 0x36
+	};
+	int spd_error = 0;
+	unsigned char *ptr;
+	unsigned char val;
+
+	if (spd->mem_type != SPD_MEMTYPE_DDR4) {
+		ERROR("Not a DDR4 DIMM.\n");
+		return -EINVAL;
+	}
+
+	ret = ddr4_spd_check(spd);
+	if (ret != 0) {
+		ERROR("DIMM SPD checksum mismatch\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * The part name in ASCII in the SPD EEPROM is not null terminated.
+	 * Guarantee null termination here by presetting all bytes to 0
+	 * and copying the part name in ASCII from the SPD onto it
+	 */
+	if ((spd->info_size_crc & 0xF) > 2) {
+		memcpy(pdimm->mpart, spd->mpart, sizeof(pdimm->mpart) - 1);
+	}
+
+	/* DIMM organization parameters */
+	pdimm->n_ranks = ((spd->organization >> 3) & 0x7) + 1;
+	debug("n_ranks %d\n", pdimm->n_ranks);
+	pdimm->rank_density = compute_ranksize(spd);
+	if (pdimm->rank_density == 0) {
+		return -EINVAL;
+	}
+
+	debug("rank_density 0x%llx\n", pdimm->rank_density);
+	pdimm->capacity = pdimm->n_ranks * pdimm->rank_density;
+	debug("capacity 0x%llx\n", pdimm->capacity);
+	pdimm->die_density = spd->density_banks & 0xf;
+	debug("die density 0x%x\n", pdimm->die_density);
+	pdimm->primary_sdram_width = 1 << (3 + (spd->bus_width & 0x7));
+	debug("primary_sdram_width %d\n", pdimm->primary_sdram_width);
+	if (((spd->bus_width >> 3) & 0x3) != 0) {
+		pdimm->ec_sdram_width = 8;
+	} else {
+		pdimm->ec_sdram_width = 0;
+	}
+	debug("ec_sdram_width %d\n", pdimm->ec_sdram_width);
+	pdimm->device_width = 1 << ((spd->organization & 0x7) + 2);
+	debug("device_width %d\n", pdimm->device_width);
+	pdimm->package_3ds = (spd->package_type & 0x3) == 0x2 ?
+			     (spd->package_type >> 4) & 0x7 : 0;
+	debug("package_3ds %d\n", pdimm->package_3ds);
+
+	switch (spd->module_type & DDR4_SPD_MODULETYPE_MASK) {
+	case DDR4_SPD_RDIMM:
+	case DDR4_SPD_MINI_RDIMM:
+	case DDR4_SPD_72B_SO_RDIMM:
+		pdimm->rdimm = 1;
+		pdimm->rc = spd->mod_section.registered.ref_raw_card & 0x9f;
+		if ((spd->mod_section.registered.reg_map & 0x1) != 0) {
+			pdimm->mirrored_dimm = 1;
+		}
+		val = spd->mod_section.registered.ca_stren;
+		pdimm->rcw[3] = val >> 4;
+		pdimm->rcw[4] = ((val & 0x3) << 2) | ((val & 0xc) >> 2);
+		val = spd->mod_section.registered.clk_stren;
+		pdimm->rcw[5] = ((val & 0x3) << 2) | ((val & 0xc) >> 2);
+		pdimm->rcw[6] = 0xf;
+		/* A17 used for 16Gb+, C[2:0] used for 3DS */
+		pdimm->rcw[8] = pdimm->die_density >= 0x6 ? 0x0 : 0x8 |
+				(pdimm->package_3ds > 0x3 ? 0x0 :
+				 (pdimm->package_3ds > 0x1 ? 0x1 :
+				  (pdimm->package_3ds > 0 ? 0x2 : 0x3)));
+		if (pdimm->package_3ds != 0 || pdimm->n_ranks != 4) {
+			pdimm->rcw[13] = 0x4;
+		} else {
+			pdimm->rcw[13] = 0x5;
+		}
+		pdimm->rcw[13] |= pdimm->mirrored_dimm ? 0x8 : 0;
+		break;
+
+	case DDR4_SPD_UDIMM:
+	case DDR4_SPD_SO_DIMM:
+	case DDR4_SPD_MINI_UDIMM:
+	case DDR4_SPD_72B_SO_UDIMM:
+	case DDR4_SPD_16B_SO_DIMM:
+	case DDR4_SPD_32B_SO_DIMM:
+		pdimm->rc = spd->mod_section.unbuffered.ref_raw_card & 0x9f;
+		if ((spd->mod_section.unbuffered.addr_mapping & 0x1) != 0) {
+			pdimm->mirrored_dimm = 1;
+		}
+		if ((spd->mod_section.unbuffered.mod_height & 0xe0) == 0 &&
+		    (spd->mod_section.unbuffered.ref_raw_card == 0x04)) {
+			/* Fix SPD error found on DIMMs with raw card E0 */
+			for (i = 0; i < 18; i++) {
+				if (spd->mapping[i] == udimm_rc_e_dq[i]) {
+					continue;
+				}
+				spd_error = 1;
+				ptr = (unsigned char *)&spd->mapping[i];
+				*ptr = udimm_rc_e_dq[i];
+			}
+			if (spd_error != 0) {
+				INFO("SPD DQ mapping error fixed\n");
+			}
+		}
+		break;
+
+	default:
+		ERROR("Unknown module_type 0x%x\n", spd->module_type);
+		return -EINVAL;
+	}
+	debug("rdimm %d\n", pdimm->rdimm);
+	debug("mirrored_dimm %d\n", pdimm->mirrored_dimm);
+	debug("rc 0x%x\n", pdimm->rc);
+
+	/* SDRAM device parameters */
+	pdimm->n_row_addr = ((spd->addressing >> 3) & 0x7) + 12;
+	debug("n_row_addr %d\n", pdimm->n_row_addr);
+	pdimm->n_col_addr = (spd->addressing & 0x7) + 9;
+	debug("n_col_addr %d\n", pdimm->n_col_addr);
+	pdimm->bank_addr_bits = (spd->density_banks >> 4) & 0x3;
+	debug("bank_addr_bits %d\n", pdimm->bank_addr_bits);
+	pdimm->bank_group_bits = (spd->density_banks >> 6) & 0x3;
+	debug("bank_group_bits %d\n", pdimm->bank_group_bits);
+
+	if (pdimm->ec_sdram_width != 0) {
+		pdimm->edc_config = 0x02;
+	} else {
+		pdimm->edc_config = 0x00;
+	}
+	debug("edc_config %d\n", pdimm->edc_config);
+
+	/* DDR4 spec has BL8 -bit3, BC4 -bit2 */
+	pdimm->burst_lengths_bitmask = 0x0c;
+	debug("burst_lengths_bitmask 0x%x\n", pdimm->burst_lengths_bitmask);
+
+	/* MTB - medium timebase
+	 * The MTB in the SPD spec is 125ps,
+	 *
+	 * FTB - fine timebase
+	 * use 1/10th of ps as our unit to avoid floating point
+	 * eg, 10 for 1ps, 25 for 2.5ps, 50 for 5ps
+	 */
+	if ((spd->timebases & 0xf) == 0x0) {
+		pdimm->mtb_ps = 125;
+		pdimm->ftb_10th_ps = 10;
+
+	} else {
+		ERROR("Unknown Timebases\n");
+		return -EINVAL;
+	}
+
+	/* sdram minimum cycle time */
+	pdimm->tckmin_x_ps = spd_to_ps(spd->tck_min, spd->fine_tck_min);
+	debug("tckmin_x_ps %d\n", pdimm->tckmin_x_ps);
+
+	/* sdram max cycle time */
+	pdimm->tckmax_ps = spd_to_ps(spd->tck_max, spd->fine_tck_max);
+	debug("tckmax_ps %d\n", pdimm->tckmax_ps);
+
+	/*
+	 * CAS latency supported
+	 * bit0 - CL7
+	 * bit4 - CL11
+	 * bit8 - CL15
+	 * bit12- CL19
+	 * bit16- CL23
+	 */
+	pdimm->caslat_x  = (spd->caslat_b1 << 7)	|
+			   (spd->caslat_b2 << 15)	|
+			   (spd->caslat_b3 << 23);
+	debug("caslat_x 0x%x\n", pdimm->caslat_x);
+
+	if (spd->caslat_b4 != 0) {
+		WARN("Unhandled caslat_b4 value\n");
+	}
+
+	/*
+	 * min CAS latency time
+	 */
+	pdimm->taa_ps = spd_to_ps(spd->taa_min, spd->fine_taa_min);
+	debug("taa_ps %d\n", pdimm->taa_ps);
+
+	/*
+	 * min RAS to CAS delay time
+	 */
+	pdimm->trcd_ps = spd_to_ps(spd->trcd_min, spd->fine_trcd_min);
+	debug("trcd_ps %d\n", pdimm->trcd_ps);
+
+	/*
+	 * Min Row Precharge Delay Time
+	 */
+	pdimm->trp_ps = spd_to_ps(spd->trp_min, spd->fine_trp_min);
+	debug("trp_ps %d\n", pdimm->trp_ps);
+
+	/* min active to precharge delay time */
+	pdimm->tras_ps = (((spd->tras_trc_ext & 0xf) << 8) +
+			  spd->tras_min_lsb) * pdimm->mtb_ps;
+	debug("tras_ps %d\n", pdimm->tras_ps);
+
+	/* min active to actice/refresh delay time */
+	pdimm->trc_ps = spd_to_ps((((spd->tras_trc_ext & 0xf0) << 4) +
+				   spd->trc_min_lsb), spd->fine_trc_min);
+	debug("trc_ps %d\n", pdimm->trc_ps);
+	/* Min Refresh Recovery Delay Time */
+	pdimm->trfc1_ps = ((spd->trfc1_min_msb << 8) | (spd->trfc1_min_lsb)) *
+		       pdimm->mtb_ps;
+	debug("trfc1_ps %d\n", pdimm->trfc1_ps);
+	pdimm->trfc2_ps = ((spd->trfc2_min_msb << 8) | (spd->trfc2_min_lsb)) *
+		       pdimm->mtb_ps;
+	debug("trfc2_ps %d\n", pdimm->trfc2_ps);
+	pdimm->trfc4_ps = ((spd->trfc4_min_msb << 8) | (spd->trfc4_min_lsb)) *
+			pdimm->mtb_ps;
+	debug("trfc4_ps %d\n", pdimm->trfc4_ps);
+	/* min four active window delay time */
+	pdimm->tfaw_ps = (((spd->tfaw_msb & 0xf) << 8) | spd->tfaw_min) *
+			pdimm->mtb_ps;
+	debug("tfaw_ps %d\n", pdimm->tfaw_ps);
+
+	/* min row active to row active delay time, different bank group */
+	pdimm->trrds_ps = spd_to_ps(spd->trrds_min, spd->fine_trrds_min);
+	debug("trrds_ps %d\n", pdimm->trrds_ps);
+	/* min row active to row active delay time, same bank group */
+	pdimm->trrdl_ps = spd_to_ps(spd->trrdl_min, spd->fine_trrdl_min);
+	debug("trrdl_ps %d\n", pdimm->trrdl_ps);
+	/* min CAS to CAS Delay Time (tCCD_Lmin), same bank group */
+	pdimm->tccdl_ps = spd_to_ps(spd->tccdl_min, spd->fine_tccdl_min);
+	debug("tccdl_ps %d\n", pdimm->tccdl_ps);
+	if (pdimm->package_3ds != 0) {
+		if (pdimm->die_density > 5) {
+			debug("Unsupported logical rank density 0x%x\n",
+				  pdimm->die_density);
+			return -EINVAL;
+		}
+		pdimm->trfc_slr_ps = (pdimm->die_density <= 4) ?
+				     260000 : 350000;
+	}
+	debug("trfc_slr_ps %d\n", pdimm->trfc_slr_ps);
+
+	/* 15ns for all speed bins */
+	pdimm->twr_ps = 15000;
+	debug("twr_ps %d\n", pdimm->twr_ps);
+
+	/*
+	 * Average periodic refresh interval
+	 * tREFI = 7.8 us at normal temperature range
+	 */
+	pdimm->refresh_rate_ps = 7800000;
+	debug("refresh_rate_ps %d\n", pdimm->refresh_rate_ps);
+
+	for (i = 0; i < 18; i++) {
+		pdimm->dq_mapping[i] = spd->mapping[i];
+		debug("dq_mapping 0x%x\n", pdimm->dq_mapping[i]);
+	}
+
+	pdimm->dq_mapping_ors = ((spd->mapping[0] >> 6) & 0x3) == 0 ? 1 : 0;
+	debug("dq_mapping_ors %d\n", pdimm->dq_mapping_ors);
+
+	return 0;
+}
diff --git a/drivers/nxp/ddr/nxp-ddr/regs.c b/drivers/nxp/ddr/nxp-ddr/regs.c
new file mode 100644
index 0000000..cedd7ca
--- /dev/null
+++ b/drivers/nxp/ddr/nxp-ddr/regs.c
@@ -0,0 +1,1394 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <common/debug.h>
+#include <ddr.h>
+#include <lib/utils.h>
+
+static inline unsigned int cal_cwl(const unsigned long clk)
+{
+	const unsigned int mclk_ps = get_memory_clk_ps(clk);
+
+	return mclk_ps >= 1250U ? 9U :
+		(mclk_ps >= 1070U ? 10U :
+		 (mclk_ps >= 935U ? 11U :
+		  (mclk_ps >= 833U ? 12U :
+		   (mclk_ps >= 750U ? 14U :
+		    (mclk_ps >= 625U ? 16U : 18U)))));
+}
+
+static void cal_csn_config(int i,
+			   struct ddr_cfg_regs *regs,
+			   const struct memctl_opt *popts,
+			   const struct dimm_params *pdimm)
+{
+	unsigned int intlv_en = 0U;
+	unsigned int intlv_ctl = 0U;
+	const unsigned int cs_n_en = 1U;
+	const unsigned int ap_n_en = popts->cs_odt[i].auto_precharge;
+	const unsigned int odt_rd_cfg = popts->cs_odt[i].odt_rd_cfg;
+	const unsigned int odt_wr_cfg = popts->cs_odt[i].odt_wr_cfg;
+	const unsigned int ba_bits_cs_n = pdimm->bank_addr_bits;
+	const unsigned int row_bits_cs_n = pdimm->n_row_addr - 12U;
+	const unsigned int col_bits_cs_n = pdimm->n_col_addr - 8U;
+	const unsigned int bg_bits_cs_n = pdimm->bank_group_bits;
+
+	if (i == 0) {
+		/* These fields only available in CS0_CONFIG */
+		if (popts->ctlr_intlv != 0) {
+			switch (popts->ctlr_intlv_mode) {
+			case DDR_256B_INTLV:
+				intlv_en = popts->ctlr_intlv;
+				intlv_ctl = popts->ctlr_intlv_mode;
+				break;
+			default:
+				break;
+			}
+		}
+	}
+	regs->cs[i].config = ((cs_n_en & 0x1) << 31)		|
+			    ((intlv_en & 0x3) << 29)		|
+			    ((intlv_ctl & 0xf) << 24)		|
+			    ((ap_n_en & 0x1) << 23)		|
+			    ((odt_rd_cfg & 0x7) << 20)		|
+			    ((odt_wr_cfg & 0x7) << 16)		|
+			    ((ba_bits_cs_n & 0x3) << 14)	|
+			    ((row_bits_cs_n & 0x7) << 8)	|
+			    ((bg_bits_cs_n & 0x3) << 4)		|
+			    ((col_bits_cs_n & 0x7) << 0);
+	debug("cs%d\n", i);
+	debug("   _config = 0x%x\n", regs->cs[i].config);
+}
+
+static inline int avoid_odt_overlap(const struct ddr_conf *conf,
+				    const struct dimm_params *pdimm)
+{
+	if ((conf->cs_in_use == 0xf) != 0) {
+		return 2;
+	}
+
+#if DDRC_NUM_DIMM >= 2
+	if (conf->dimm_in_use[0] != 0 && conf->dimm_in_use[1] != 0) {
+		return 1;
+	}
+#endif
+	return 0;
+}
+
+/* Requires rcw2 set first */
+static void cal_timing_cfg(const unsigned long clk,
+			   struct ddr_cfg_regs *regs,
+			   const struct memctl_opt *popts,
+			   const struct dimm_params *pdimm,
+			   const struct ddr_conf *conf,
+			   unsigned int cas_latency,
+			   unsigned int additive_latency)
+{
+	const unsigned int mclk_ps = get_memory_clk_ps(clk);
+	/* tXP=max(4nCK, 6ns) */
+	const int txp = max((int)mclk_ps * 4, 6000);
+	/* DDR4 supports 10, 12, 14, 16, 18, 20, 24 */
+	static const int wrrec_table[] = {
+		10, 10, 10, 10, 10,
+		10, 10, 10, 10, 10,
+		12, 12, 14, 14, 16,
+		16, 18, 18, 20, 20,
+		24, 24, 24, 24,
+	};
+	int trwt_mclk = (clk / 1000000 > 1900) ? 3 : 2;
+	int twrt_mclk;
+	int trrt_mclk;
+	int twwt_mclk;
+	const int act_pd_exit_mclk = picos_to_mclk(clk, txp);
+	const int pre_pd_exit_mclk = act_pd_exit_mclk;
+	const int taxpd_mclk = 0;
+	/*
+	 * MRS_CYC = max(tMRD, tMOD)
+	 * tMRD = 8nCK, tMOD = max(24nCK, 15ns)
+	 */
+	const int tmrd_mclk = max(24U, picos_to_mclk(clk, 15000));
+	const int pretoact_mclk = picos_to_mclk(clk, pdimm->trp_ps);
+	const int acttopre_mclk = picos_to_mclk(clk, pdimm->tras_ps);
+	const int acttorw_mclk = picos_to_mclk(clk, pdimm->trcd_ps);
+	const int caslat_ctrl = (cas_latency - 1) << 1;
+	const int trfc1_min = pdimm->die_density >= 0x3 ? 16000 :
+			      (pdimm->die_density == 0x4 ? 26000 :
+			       (pdimm->die_density == 0x5 ? 35000 :
+				55000));
+	const int refrec_ctrl = picos_to_mclk(clk,
+							pdimm->trfc1_ps) - 8;
+	int wrrec_mclk = picos_to_mclk(clk, pdimm->twr_ps);
+	const int acttoact_mclk = max(picos_to_mclk(clk,
+							      pdimm->trrds_ps),
+						4U);
+	int wrtord_mclk = max(2U, picos_to_mclk(clk, 2500));
+	const unsigned int cpo = 0U;
+	const int wr_lat = cal_cwl(clk);
+	int rd_to_pre = picos_to_mclk(clk, 7500);
+	const int wr_data_delay = popts->wr_data_delay;
+	const int cke_pls = max(3U, picos_to_mclk(clk, 5000));
+#ifdef ERRATA_DDR_A050450
+	const unsigned short four_act = ((popts->twot_en == 0) &&
+					 (popts->threet_en == 0) &&
+					 (popts->tfaw_ps % 2 == 0)) ?
+						(picos_to_mclk(clk, popts->tfaw_ps) + 1) :
+						picos_to_mclk(clk, popts->tfaw_ps);
+#else
+	const unsigned short four_act = picos_to_mclk(clk,
+					 popts->tfaw_ps);
+#endif
+	const unsigned int cntl_adj = 0U;
+	const unsigned int ext_pretoact = picos_to_mclk(clk,
+							pdimm->trp_ps) >> 4U;
+	const unsigned int ext_acttopre = picos_to_mclk(clk,
+							pdimm->tras_ps) >> 4U;
+	const unsigned int ext_acttorw = picos_to_mclk(clk,
+						       pdimm->trcd_ps) >> 4U;
+	const unsigned int ext_caslat = (2U * cas_latency - 1U) >> 4U;
+	const unsigned int ext_add_lat = additive_latency >> 4U;
+	const unsigned int ext_refrec = (picos_to_mclk(clk,
+					       pdimm->trfc1_ps) - 8U) >> 4U;
+	const unsigned int ext_wrrec = (picos_to_mclk(clk, pdimm->twr_ps) +
+				  (popts->otf_burst_chop_en ? 2U : 0U)) >> 4U;
+	const unsigned int rwt_same_cs = 0U;
+	const unsigned int wrt_same_cs = 0U;
+	const unsigned int rrt_same_cs = popts->burst_length == DDR_BL8 ? 0U : 2U;
+	const unsigned int wwt_same_cs = popts->burst_length == DDR_BL8 ? 0U : 2U;
+	const unsigned int dll_lock = 2U;
+	unsigned int rodt_on = 0U;
+	const unsigned int rodt_off = 4U;
+	const unsigned int wodt_on = 1U;
+	const unsigned int wodt_off = 4U;
+	const unsigned int hs_caslat = 0U;
+	const unsigned int hs_wrlat = 0U;
+	const unsigned int hs_wrrec = 0U;
+	const unsigned int hs_clkadj = 0U;
+	const unsigned int hs_wrlvl_start = 0U;
+	const unsigned int txpr = max(5U,
+				      picos_to_mclk(clk,
+						    pdimm->trfc1_ps + 10000U));
+	const unsigned int tcksre = max(5U, picos_to_mclk(clk, 10000U));
+	const unsigned int tcksrx = max(5U, picos_to_mclk(clk, 10000U));
+	const unsigned int cs_to_cmd = 0U;
+	const unsigned int cke_rst = txpr <= 200U ? 0U :
+				     (txpr <= 256U ? 1U :
+				      (txpr <= 512U ? 2U : 3U));
+	const unsigned int cksre = tcksre <= 19U ? tcksre - 5U : 15U;
+	const unsigned int cksrx = tcksrx <= 19U ? tcksrx - 5U : 15U;
+	unsigned int par_lat = 0U;
+	const int tccdl = max(5U, picos_to_mclk(clk, pdimm->tccdl_ps));
+	int rwt_bg = cas_latency + 2 + 4 - wr_lat;
+	int wrt_bg = wr_lat + 4 + 1 - cas_latency;
+	const int rrt_bg = popts->burst_length == DDR_BL8 ?
+				tccdl - 4 : tccdl - 2;
+	const int wwt_bg = popts->burst_length == DDR_BL8 ?
+					tccdl - 4 : tccdl - 2;
+	const unsigned int acttoact_bg = picos_to_mclk(clk, pdimm->trrdl_ps);
+	const unsigned int wrtord_bg = max(4U, picos_to_mclk(clk, 7500)) +
+				       (popts->otf_burst_chop_en ? 2 : 0);
+	const unsigned int pre_all_rec = 0;
+	const unsigned int refrec_cid_mclk = pdimm->package_3ds ?
+				picos_to_mclk(clk, pdimm->trfc_slr_ps) : 0;
+	const unsigned int acttoact_cid_mclk = pdimm->package_3ds ? 4U : 0;
+
+
+	/* for two dual-rank DIMMs to avoid ODT overlap */
+	if (avoid_odt_overlap(conf, pdimm) == 2) {
+		twrt_mclk = 2;
+		twwt_mclk = 2;
+		trrt_mclk = 2;
+	} else {
+		twrt_mclk = 1;
+		twwt_mclk = 1;
+		trrt_mclk = 0;
+	}
+
+	if (popts->trwt_override != 0) {
+		trwt_mclk = popts->trwt;
+		if (popts->twrt != 0) {
+			twrt_mclk = popts->twrt;
+		}
+		if (popts->trrt != 0) {
+			trrt_mclk = popts->trrt;
+		}
+		if (popts->twwt != 0) {
+			twwt_mclk = popts->twwt;
+		}
+	}
+	regs->timing_cfg[0] = (((trwt_mclk & 0x3) << 30)		|
+			     ((twrt_mclk & 0x3) << 28)			|
+			     ((trrt_mclk & 0x3) << 26)			|
+			     ((twwt_mclk & 0x3) << 24)			|
+			     ((act_pd_exit_mclk & 0xf) << 20)		|
+			     ((pre_pd_exit_mclk & 0xF) << 16)		|
+			     ((taxpd_mclk & 0xf) << 8)			|
+			     ((tmrd_mclk & 0x1f) << 0));
+	debug("timing_cfg[0] = 0x%x\n", regs->timing_cfg[0]);
+
+	if ((wrrec_mclk < 1) || (wrrec_mclk > 24)) {
+		ERROR("WRREC doesn't support clock %d\n", wrrec_mclk);
+	} else {
+		wrrec_mclk = wrrec_table[wrrec_mclk - 1];
+	}
+
+	if (popts->otf_burst_chop_en != 0) {
+		wrrec_mclk += 2;
+		wrtord_mclk += 2;
+	}
+
+	if (pdimm->trfc1_ps < trfc1_min) {
+		ERROR("trfc1_ps (%d) < %d\n", pdimm->trfc1_ps, trfc1_min);
+	}
+
+	regs->timing_cfg[1] = (((pretoact_mclk & 0x0F) << 28)		|
+			     ((acttopre_mclk & 0x0F) << 24)		|
+			     ((acttorw_mclk & 0xF) << 20)		|
+			     ((caslat_ctrl & 0xF) << 16)		|
+			     ((refrec_ctrl & 0xF) << 12)		|
+			     ((wrrec_mclk & 0x0F) << 8)			|
+			     ((acttoact_mclk & 0x0F) << 4)		|
+			     ((wrtord_mclk & 0x0F) << 0));
+	debug("timing_cfg[1] = 0x%x\n", regs->timing_cfg[1]);
+
+	if (rd_to_pre < 4) {
+		rd_to_pre = 4;
+	}
+	if (popts->otf_burst_chop_en) {
+		rd_to_pre += 2;
+	}
+
+	regs->timing_cfg[2] = (((additive_latency & 0xf) << 28)		|
+			     ((cpo & 0x1f) << 23)			|
+			     ((wr_lat & 0xf) << 19)			|
+			     (((wr_lat & 0x10) >> 4) << 18)		|
+			     ((rd_to_pre & 0xf) << 13)			|
+			     ((wr_data_delay & 0xf) << 9)		|
+			     ((cke_pls & 0x7) << 6)			|
+			     ((four_act & 0x3f) << 0));
+	debug("timing_cfg[2] = 0x%x\n", regs->timing_cfg[2]);
+
+	regs->timing_cfg[3] = (((ext_pretoact & 0x1) << 28)		|
+			     ((ext_acttopre & 0x3) << 24)		|
+			     ((ext_acttorw & 0x1) << 22)		|
+			     ((ext_refrec & 0x3F) << 16)		|
+			     ((ext_caslat & 0x3) << 12)			|
+			     ((ext_add_lat & 0x1) << 10)		|
+			     ((ext_wrrec & 0x1) << 8)			|
+			     ((cntl_adj & 0x7) << 0));
+	debug("timing_cfg[3] = 0x%x\n", regs->timing_cfg[3]);
+
+	regs->timing_cfg[4] = (((rwt_same_cs & 0xf) << 28)		|
+			     ((wrt_same_cs & 0xf) << 24)		|
+			     ((rrt_same_cs & 0xf) << 20)		|
+			     ((wwt_same_cs & 0xf) << 16)		|
+			     ((trwt_mclk & 0xc) << 12)			|
+			     ((twrt_mclk & 0x4) << 10)			|
+			     ((trrt_mclk & 0x4) << 8)			|
+			     ((twwt_mclk & 0x4) << 6)			|
+			     (dll_lock & 0x3));
+	debug("timing_cfg[4] = 0x%x\n", regs->timing_cfg[4]);
+
+	/* rodt_on = timing_cfg_1[caslat] - timing_cfg_2[wrlat] + 1 */
+	if (cas_latency >= wr_lat) {
+		rodt_on = cas_latency - wr_lat + 1;
+	}
+
+	regs->timing_cfg[5] = (((rodt_on & 0x1f) << 24)			|
+			     ((rodt_off & 0x7) << 20)			|
+			     ((wodt_on & 0x1f) << 12)			|
+			     (wodt_off & 0x7) << 8);
+	debug("timing_cfg[5] = 0x%x\n", regs->timing_cfg[5]);
+
+	regs->timing_cfg[6] = (((hs_caslat & 0x1f) << 24)		|
+			     ((hs_wrlat & 0x1f) << 19)			|
+			     ((hs_wrrec & 0x1f) << 12)			|
+			     ((hs_clkadj & 0x1f) << 6)			|
+			     ((hs_wrlvl_start & 0x1f) << 0));
+	debug("timing_cfg[6] = 0x%x\n", regs->timing_cfg[6]);
+
+	if (popts->ap_en != 0) {
+		par_lat = (regs->sdram_rcw[1] & 0xf) + 1;
+		debug("PAR_LAT = 0x%x\n", par_lat);
+	}
+
+	regs->timing_cfg[7] = (((cke_rst & 0x3) << 28)			|
+			     ((cksre & 0xf) << 24)			|
+			     ((cksrx & 0xf) << 20)			|
+			     ((par_lat & 0xf) << 16)			|
+			     ((cs_to_cmd & 0xf) << 4));
+	debug("timing_cfg[7] = 0x%x\n", regs->timing_cfg[7]);
+
+	if (rwt_bg < tccdl) {
+		rwt_bg = tccdl - rwt_bg;
+	} else {
+		rwt_bg = 0;
+	}
+	if (wrt_bg < tccdl) {
+		wrt_bg = tccdl - wrt_bg;
+	} else {
+		wrt_bg = 0;
+	}
+	regs->timing_cfg[8] = (((rwt_bg & 0xf) << 28)			|
+			     ((wrt_bg & 0xf) << 24)			|
+			     ((rrt_bg & 0xf) << 20)			|
+			     ((wwt_bg & 0xf) << 16)			|
+			     ((acttoact_bg & 0xf) << 12)		|
+			     ((wrtord_bg & 0xf) << 8)			|
+			     ((pre_all_rec & 0x1f) << 0));
+	debug("timing_cfg[8] = 0x%x\n", regs->timing_cfg[8]);
+
+	regs->timing_cfg[9] = (refrec_cid_mclk & 0x3ff) << 16		|
+			      (acttoact_cid_mclk & 0xf) << 8;
+	debug("timing_cfg[9] = 0x%x\n", regs->timing_cfg[9]);
+}
+
+static void cal_ddr_sdram_rcw(const unsigned long clk,
+			      struct ddr_cfg_regs *regs,
+			      const struct memctl_opt *popts,
+			      const struct dimm_params *pdimm)
+{
+	const unsigned int freq = clk / 1000000U;
+	unsigned int rc0a, rc0f;
+
+	if (pdimm->rdimm == 0) {
+		return;
+	}
+
+	rc0a = freq > 3200U ? 7U :
+	       (freq > 2933U ? 6U :
+		(freq > 2666U ? 5U :
+		 (freq > 2400U ? 4U :
+		  (freq > 2133U ? 3U :
+		   (freq > 1866U ? 2U :
+		    (freq > 1600U ? 1U : 0U))))));
+	rc0f = freq > 3200U ? 3U :
+		(freq > 2400U ? 2U :
+		 (freq > 2133U ? 1U : 0U));
+	rc0f = (regs->sdram_cfg[1] & SDRAM_CFG2_AP_EN) ? rc0f : 4;
+	regs->sdram_rcw[0] =
+		pdimm->rcw[0] << 28	|
+		pdimm->rcw[1] << 24	|
+		pdimm->rcw[2] << 20	|
+		pdimm->rcw[3] << 16	|
+		pdimm->rcw[4] << 12	|
+		pdimm->rcw[5] << 8	|
+		pdimm->rcw[6] << 4	|
+		pdimm->rcw[7];
+	regs->sdram_rcw[1] =
+		pdimm->rcw[8] << 28	|
+		pdimm->rcw[9] << 24	|
+		rc0a << 20		|
+		pdimm->rcw[11] << 16	|
+		pdimm->rcw[12] << 12	|
+		pdimm->rcw[13] << 8	|
+		pdimm->rcw[14] << 4	|
+		rc0f;
+	regs->sdram_rcw[2] =
+		((freq - 1260 + 19) / 20) << 8;
+
+	debug("sdram_rcw[0] = 0x%x\n", regs->sdram_rcw[0]);
+	debug("sdram_rcw[1] = 0x%x\n", regs->sdram_rcw[1]);
+	debug("sdram_rcw[2] = 0x%x\n", regs->sdram_rcw[2]);
+}
+
+static void cal_ddr_sdram_cfg(const unsigned long clk,
+			      struct ddr_cfg_regs *regs,
+			      const struct memctl_opt *popts,
+			      const struct dimm_params *pdimm,
+			      const unsigned int ip_rev)
+{
+	const unsigned int mem_en = 1U;
+	const unsigned int sren = popts->self_refresh_in_sleep;
+	const unsigned int ecc_en = popts->ecc_mode;
+	const unsigned int rd_en = (pdimm->rdimm != 0U) ? 1U : 0U;
+	const unsigned int dyn_pwr = popts->dynamic_power;
+	const unsigned int dbw = popts->data_bus_used;
+	const unsigned int eight_be = (dbw == 1U ||
+				       popts->burst_length == DDR_BL8) ? 1U : 0U;
+	const unsigned int ncap = 0U;
+	const unsigned int threet_en = popts->threet_en;
+	const unsigned int twot_en = pdimm->rdimm ?
+					0U : popts->twot_en;
+	const unsigned int ba_intlv = popts->ba_intlv;
+	const unsigned int x32_en = 0U;
+	const unsigned int pchb8 = 0U;
+	const unsigned int hse = popts->half_strength_drive_en;
+	const unsigned int acc_ecc_en = (dbw != 0U && ecc_en == 1U) ? 1U : 0U;
+	const unsigned int mem_halt = 0U;
+#ifdef PHY_GEN2
+	const unsigned int bi = 1U;
+#else
+	const unsigned int bi = 0U;
+#endif
+	const unsigned int sdram_type = SDRAM_TYPE_DDR4;
+	unsigned int odt_cfg = 0U;
+	const unsigned int frc_sr = 0U;
+	const unsigned int sr_ie = popts->self_refresh_irq_en;
+	const unsigned int num_pr = pdimm->package_3ds + 1U;
+	const unsigned int slow = (clk < 1249000000U) ? 1U : 0U;
+	const unsigned int x4_en = popts->x4_en;
+	const unsigned int obc_cfg = popts->otf_burst_chop_en;
+	const unsigned int ap_en = ip_rev == 0x50500U ? 0U : popts->ap_en;
+	const unsigned int d_init = popts->ctlr_init_ecc;
+	const unsigned int rcw_en = popts->rdimm;
+	const unsigned int md_en = popts->mirrored_dimm;
+	const unsigned int qd_en = popts->quad_rank_present;
+	const unsigned int unq_mrs_en = ip_rev < 0x50500U ? 1U : 0U;
+	const unsigned int rd_pre = popts->quad_rank_present;
+	int i;
+
+	regs->sdram_cfg[0] = ((mem_en & 0x1) << 31)		|
+				((sren & 0x1) << 30)		|
+				((ecc_en & 0x1) << 29)		|
+				((rd_en & 0x1) << 28)		|
+				((sdram_type & 0x7) << 24)	|
+				((dyn_pwr & 0x1) << 21)		|
+				((dbw & 0x3) << 19)		|
+				((eight_be & 0x1) << 18)	|
+				((ncap & 0x1) << 17)		|
+				((threet_en & 0x1) << 16)	|
+				((twot_en & 0x1) << 15)		|
+				((ba_intlv & 0x7F) << 8)	|
+				((x32_en & 0x1) << 5)		|
+				((pchb8 & 0x1) << 4)		|
+				((hse & 0x1) << 3)		|
+				((acc_ecc_en & 0x1) << 2)	|
+				((mem_halt & 0x1) << 1)		|
+				((bi & 0x1) << 0);
+	debug("sdram_cfg[0] = 0x%x\n", regs->sdram_cfg[0]);
+
+	for (i = 0; i < DDRC_NUM_CS; i++) {
+		if (popts->cs_odt[i].odt_rd_cfg != 0 ||
+		    popts->cs_odt[i].odt_wr_cfg != 0) {
+			odt_cfg = SDRAM_CFG2_ODT_ONLY_READ;
+			break;
+		}
+	}
+
+	regs->sdram_cfg[1] = (0
+		| ((frc_sr & 0x1) << 31)
+		| ((sr_ie & 0x1) << 30)
+		| ((odt_cfg & 0x3) << 21)
+		| ((num_pr & 0xf) << 12)
+		| ((slow & 1) << 11)
+		| (x4_en << 10)
+		| (qd_en << 9)
+		| (unq_mrs_en << 8)
+		| ((obc_cfg & 0x1) << 6)
+		| ((ap_en & 0x1) << 5)
+		| ((d_init & 0x1) << 4)
+		| ((rcw_en & 0x1) << 2)
+		| ((md_en & 0x1) << 0)
+		);
+	debug("sdram_cfg[1] = 0x%x\n", regs->sdram_cfg[1]);
+
+	regs->sdram_cfg[2] = (rd_pre & 0x1) << 16	|
+				 (popts->rdimm ? 1 : 0);
+	if (pdimm->package_3ds != 0) {
+		if (((pdimm->package_3ds + 1) & 0x1) != 0) {
+			WARN("Unsupported 3DS DIMM\n");
+		} else {
+			regs->sdram_cfg[2] |= ((pdimm->package_3ds + 1) >> 1)
+						  << 4;
+		}
+	}
+	debug("sdram_cfg[2] = 0x%x\n", regs->sdram_cfg[2]);
+}
+
+
+static void cal_ddr_sdram_interval(const unsigned long clk,
+				   struct ddr_cfg_regs *regs,
+				   const struct memctl_opt *popts,
+				   const struct dimm_params *pdimm)
+{
+	const unsigned int refint = picos_to_mclk(clk, pdimm->refresh_rate_ps);
+	const unsigned int bstopre = popts->bstopre;
+
+	regs->interval = ((refint & 0xFFFF) << 16)	|
+				  ((bstopre & 0x3FFF) << 0);
+	debug("interval = 0x%x\n", regs->interval);
+}
+
+/* Require cs and cfg first */
+static void cal_ddr_sdram_mode(const unsigned long clk,
+			       struct ddr_cfg_regs *regs,
+			       const struct memctl_opt *popts,
+			       const struct ddr_conf *conf,
+			       const struct dimm_params *pdimm,
+			       unsigned int cas_latency,
+			       unsigned int additive_latency,
+			       const unsigned int ip_rev)
+{
+	int i;
+	unsigned short esdmode;		/* Extended SDRAM mode */
+	unsigned short sdmode;		/* SDRAM mode */
+
+	/* Mode Register - MR1 */
+	const unsigned int qoff = 0;
+	const unsigned int tdqs_en = 0;
+	unsigned int rtt;
+	const unsigned int wrlvl_en = 0;
+	unsigned int al = 0;
+	unsigned int dic = 0;
+	const unsigned int dll_en = 1;
+
+	/* Mode Register - MR0 */
+	unsigned int wr = 0;
+	const unsigned int dll_rst = 0;
+	const unsigned int mode = 0;
+	unsigned int caslat = 4;/* CAS# latency, default set as 6 cycles */
+	/* BT: Burst Type (0=Nibble Sequential, 1=Interleaved) */
+	const unsigned int bt = 0;
+	const unsigned int bl = popts->burst_length == DDR_BL8 ? 0 :
+				 (popts->burst_length == DDR_BC4 ? 2 : 1);
+
+	const unsigned int wr_mclk = picos_to_mclk(clk, pdimm->twr_ps);
+	/* DDR4 support WR 10, 12, 14, 16, 18, 20, 24 */
+	static const int wr_table[] = {
+		0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 6, 6
+	};
+	/* DDR4 support CAS 9, 10, 11, 12, 13, 14, 15, 16, 18, 20, 22, 24 */
+	static const int cas_latency_table[] = {
+		0, 1, 2, 3, 4, 5, 6, 7, 13, 8,
+		14, 9, 15, 10, 12, 11, 16, 17,
+		18, 19, 20, 21, 22, 23
+	};
+	const unsigned int unq_mrs_en = ip_rev < U(0x50500) ? 1U : 0U;
+	unsigned short esdmode2 = 0U;
+	unsigned short esdmode3 = 0U;
+	const unsigned int wr_crc = 0U;
+	unsigned int rtt_wr = 0U;
+	const unsigned int srt = 0U;
+	unsigned int cwl = cal_cwl(clk);
+	const unsigned int mpr = 0U;
+	const unsigned int mclk_ps = get_memory_clk_ps(clk);
+	const unsigned int wc_lat = 0U;
+	unsigned short esdmode4 = 0U;
+	unsigned short esdmode5;
+	int rtt_park_all = 0;
+	unsigned int rtt_park;
+	const bool four_cs = conf->cs_in_use == 0xf ? true : false;
+	unsigned short esdmode6 = 0U;	/* Extended SDRAM mode 6 */
+	unsigned short esdmode7 = 0U;	/* Extended SDRAM mode 7 */
+	const unsigned int tccdl_min = max(5U,
+					   picos_to_mclk(clk, pdimm->tccdl_ps));
+
+	if (popts->rtt_override != 0U) {
+		rtt = popts->rtt_override_value;
+	} else {
+		rtt = popts->cs_odt[0].odt_rtt_norm;
+	}
+
+	if (additive_latency == (cas_latency - 1)) {
+		al = 1;
+	}
+	if (additive_latency == (cas_latency - 2)) {
+		al = 2;
+	}
+
+	if (popts->quad_rank_present != 0 || popts->output_driver_impedance != 0) {
+		dic = 1;	/* output driver impedance 240/7 ohm */
+	}
+
+	esdmode = (((qoff & 0x1) << 12)				|
+		   ((tdqs_en & 0x1) << 11)			|
+		   ((rtt & 0x7) << 8)				|
+		   ((wrlvl_en & 0x1) << 7)			|
+		   ((al & 0x3) << 3)				|
+		   ((dic & 0x3) << 1)				|
+		   ((dll_en & 0x1) << 0));
+
+	if (wr_mclk >= 10 && wr_mclk <= 24) {
+		wr = wr_table[wr_mclk - 10];
+	} else {
+		ERROR("unsupported wc_mclk = %d for mode register\n", wr_mclk);
+	}
+
+	/* look up table to get the cas latency bits */
+	if (cas_latency >= 9 && cas_latency <= 32) {
+		caslat = cas_latency_table[cas_latency - 9];
+	} else {
+		WARN("Error: unsupported cas latency for mode register\n");
+	}
+
+	sdmode = (((caslat & 0x10) << 8)			|
+		  ((wr & 0x7) << 9)				|
+		  ((dll_rst & 0x1) << 8)			|
+		  ((mode & 0x1) << 7)				|
+		  (((caslat >> 1) & 0x7) << 4)			|
+		  ((bt & 0x1) << 3)				|
+		  ((caslat & 1) << 2)				|
+		  ((bl & 0x3) << 0));
+
+	regs->sdram_mode[0] = (((esdmode & 0xFFFF) << 16)	|
+				 ((sdmode & 0xFFFF) << 0));
+	debug("sdram_mode[0] = 0x%x\n", regs->sdram_mode[0]);
+
+	switch (cwl) {
+	case 9:
+	case 10:
+	case 11:
+	case 12:
+		cwl -= 9;
+		break;
+	case 14:
+		cwl -= 10;
+		break;
+	case 16:
+		cwl -= 11;
+		break;
+	case 18:
+		cwl -= 12;
+		break;
+	case 20:
+		cwl -= 13;
+		break;
+	default:
+		printf("Error CWL\n");
+		break;
+	}
+
+	if (popts->rtt_override != 0) {
+		rtt_wr = popts->rtt_wr_override_value;
+	} else {
+		rtt_wr = popts->cs_odt[0].odt_rtt_wr;
+	}
+
+	esdmode2 = ((wr_crc & 0x1) << 12)			|
+		   ((rtt_wr & 0x7) << 9)			|
+		   ((srt & 0x3) << 6)				|
+		   ((cwl & 0x7) << 3);
+	esdmode3 = ((mpr & 0x3) << 11) | ((wc_lat & 0x3) << 9);
+
+	regs->sdram_mode[1] = ((esdmode2 & 0xFFFF) << 16)	|
+				((esdmode3 & 0xFFFF) << 0);
+	debug("sdram_mode[1] = 0x%x\n", regs->sdram_mode[1]);
+
+	esdmode6 = ((tccdl_min - 4) & 0x7) << 10;
+	if (popts->vref_dimm != 0) {
+		esdmode6 |= popts->vref_dimm & 0x7f;
+	} else if ((popts->ddr_cdr2 & DDR_CDR2_VREF_RANGE_2) != 0) {
+		esdmode6 |= 1 << 6;	/* Range 2 */
+	}
+
+	regs->sdram_mode[9] = ((esdmode6 & 0xffff) << 16)	|
+				 ((esdmode7 & 0xffff) << 0);
+	debug("sdram_mode[9] = 0x%x\n", regs->sdram_mode[9]);
+
+	rtt_park = (popts->rtt_park != 0) ? popts->rtt_park : 240;
+	switch (rtt_park) {
+	case 240:
+		rtt_park = 0x4;
+		break;
+	case 120:
+		rtt_park = 0x2;
+		break;
+	case 80:
+		rtt_park = 0x6;
+		break;
+	case 60:
+		rtt_park = 0x1;
+		break;
+	case 48:
+		rtt_park = 0x5;
+		break;
+	case 40:
+		rtt_park = 0x3;
+		break;
+	case 34:
+		rtt_park = 0x7;
+		break;
+	default:
+		rtt_park = 0;
+		break;
+	}
+
+	for (i = 0; i < DDRC_NUM_CS; i++) {
+		if (i != 0 && unq_mrs_en == 0) {
+			break;
+		}
+
+		if (popts->rtt_override != 0) {
+			rtt = popts->rtt_override_value;
+			rtt_wr = popts->rtt_wr_override_value;
+		} else {
+			rtt = popts->cs_odt[i].odt_rtt_norm;
+			rtt_wr = popts->cs_odt[i].odt_rtt_wr;
+		}
+
+		esdmode &= 0xF8FF;	/* clear bit 10,9,8 for rtt */
+		esdmode |= (rtt & 0x7) << 8;
+		esdmode2 &= 0xF9FF;	/* clear bit 10, 9 */
+		esdmode2 |= (rtt_wr & 0x3) << 9;
+		esdmode5 = (popts->x4_en) ? 0 : 0x400; /* data mask */
+
+		if (rtt_park_all == 0 &&
+		    ((regs->cs[i].config & SDRAM_CS_CONFIG_EN) != 0)) {
+			esdmode5 |= rtt_park << 6;
+			rtt_park_all = four_cs ? 0 : 1;
+		}
+
+		if (((regs->sdram_cfg[1] & SDRAM_CFG2_AP_EN) != 0) &&
+		    (popts->rdimm == 0)) {
+			if (mclk_ps >= 935) {
+				esdmode5 |= DDR_MR5_CA_PARITY_LAT_4_CLK;
+			} else if (mclk_ps >= 833) {
+				esdmode5 |= DDR_MR5_CA_PARITY_LAT_5_CLK;
+			} else {
+				esdmode5 |= DDR_MR5_CA_PARITY_LAT_5_CLK;
+				WARN("mclk_ps not supported %d", mclk_ps);
+
+			}
+		}
+
+		switch (i) {
+		case 0:
+			regs->sdram_mode[8] = ((esdmode4 & 0xffff) << 16) |
+						((esdmode5 & 0xffff) << 0);
+			debug("sdram_mode[8] = 0x%x\n", regs->sdram_mode[8]);
+			break;
+		case 1:
+			regs->sdram_mode[2] = (((esdmode & 0xFFFF) << 16) |
+					      ((sdmode & 0xFFFF) << 0));
+			regs->sdram_mode[3] = ((esdmode2 & 0xFFFF) << 16) |
+					      ((esdmode3 & 0xFFFF) << 0);
+			regs->sdram_mode[10] = ((esdmode4 & 0xFFFF) << 16) |
+					       ((esdmode5 & 0xFFFF) << 0);
+			regs->sdram_mode[11] = ((esdmode6 & 0xFFFF) << 16) |
+					       ((esdmode7 & 0xFFFF) << 0);
+			debug("sdram_mode[2] = 0x%x\n", regs->sdram_mode[2]);
+			debug("sdram_mode[3] = 0x%x\n", regs->sdram_mode[3]);
+			debug("sdram_mode[10] = 0x%x\n", regs->sdram_mode[10]);
+			debug("sdram_mode[11] = 0x%x\n", regs->sdram_mode[11]);
+			break;
+		case 2:
+			regs->sdram_mode[4] = (((esdmode & 0xFFFF) << 16) |
+					      ((sdmode & 0xFFFF) << 0));
+			regs->sdram_mode[5] = ((esdmode2 & 0xFFFF) << 16) |
+					      ((esdmode3 & 0xFFFF) << 0);
+			regs->sdram_mode[12] = ((esdmode4 & 0xFFFF) << 16) |
+					       ((esdmode5 & 0xFFFF) << 0);
+			regs->sdram_mode[13] = ((esdmode6 & 0xFFFF) << 16) |
+					       ((esdmode7 & 0xFFFF) << 0);
+			debug("sdram_mode[4] = 0x%x\n", regs->sdram_mode[4]);
+			debug("sdram_mode[5] = 0x%x\n", regs->sdram_mode[5]);
+			debug("sdram_mode[12] = 0x%x\n", regs->sdram_mode[12]);
+			debug("sdram_mode[13] = 0x%x\n", regs->sdram_mode[13]);
+			break;
+		case 3:
+			regs->sdram_mode[6] = (((esdmode & 0xFFFF) << 16) |
+					      ((sdmode & 0xFFFF) << 0));
+			regs->sdram_mode[7] = ((esdmode2 & 0xFFFF) << 16) |
+					      ((esdmode3 & 0xFFFF) << 0);
+			regs->sdram_mode[14] = ((esdmode4 & 0xFFFF) << 16) |
+					       ((esdmode5 & 0xFFFF) << 0);
+			regs->sdram_mode[15] = ((esdmode6 & 0xFFFF) << 16) |
+					       ((esdmode7 & 0xFFFF) << 0);
+			debug("sdram_mode[6] = 0x%x\n", regs->sdram_mode[6]);
+			debug("sdram_mode[7] = 0x%x\n", regs->sdram_mode[7]);
+			debug("sdram_mode[14] = 0x%x\n", regs->sdram_mode[14]);
+			debug("sdram_mode[15] = 0x%x\n", regs->sdram_mode[15]);
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+#ifndef CONFIG_MEM_INIT_VALUE
+#define CONFIG_MEM_INIT_VALUE 0xDEADBEEF
+#endif
+static void cal_ddr_data_init(struct ddr_cfg_regs *regs)
+{
+	regs->data_init = CONFIG_MEM_INIT_VALUE;
+}
+
+static void cal_ddr_dq_mapping(struct ddr_cfg_regs *regs,
+			       const struct dimm_params *pdimm)
+{
+	const unsigned int acc_ecc_en = (regs->sdram_cfg[0] >> 2) & 0x1;
+/* FIXME: revert the dq mapping from DIMM */
+	regs->dq_map[0] = ((pdimm->dq_mapping[0] & 0x3F) << 26)	|
+			 ((pdimm->dq_mapping[1] & 0x3F) << 20)	|
+			 ((pdimm->dq_mapping[2] & 0x3F) << 14)	|
+			 ((pdimm->dq_mapping[3] & 0x3F) << 8)	|
+			 ((pdimm->dq_mapping[4] & 0x3F) << 2);
+
+	regs->dq_map[1] = ((pdimm->dq_mapping[5] & 0x3F) << 26)	|
+			 ((pdimm->dq_mapping[6] & 0x3F) << 20)	|
+			 ((pdimm->dq_mapping[7] & 0x3F) << 14)	|
+			 ((pdimm->dq_mapping[10] & 0x3F) << 8)	|
+			 ((pdimm->dq_mapping[11] & 0x3F) << 2);
+
+	regs->dq_map[2] = ((pdimm->dq_mapping[12] & 0x3F) << 26)	|
+			 ((pdimm->dq_mapping[13] & 0x3F) << 20)		|
+			 ((pdimm->dq_mapping[14] & 0x3F) << 14)		|
+			 ((pdimm->dq_mapping[15] & 0x3F) << 8)		|
+			 ((pdimm->dq_mapping[16] & 0x3F) << 2);
+
+	/* dq_map for ECC[4:7] is set to 0 if accumulated ECC is enabled */
+	regs->dq_map[3] = ((pdimm->dq_mapping[17] & 0x3F) << 26)	|
+			 ((pdimm->dq_mapping[8] & 0x3F) << 20)		|
+			 ((acc_ecc_en != 0) ? 0 :
+			  (pdimm->dq_mapping[9] & 0x3F) << 14)		|
+			 pdimm->dq_mapping_ors;
+	debug("dq_map[0] = 0x%x\n", regs->dq_map[0]);
+	debug("dq_map[1] = 0x%x\n", regs->dq_map[1]);
+	debug("dq_map[2] = 0x%x\n", regs->dq_map[2]);
+	debug("dq_map[3] = 0x%x\n", regs->dq_map[3]);
+}
+static void cal_ddr_zq_cntl(struct ddr_cfg_regs *regs)
+{
+	const unsigned int zqinit = 10U;	/* 1024 clocks */
+	const unsigned int zqoper = 9U;		/* 512 clocks */
+	const unsigned int zqcs = 7U;		/* 128 clocks */
+	const unsigned int zqcs_init = 5U;	/* 1024 refresh seqences */
+	const unsigned int zq_en = 1U;		/* enabled */
+
+	regs->zq_cntl = ((zq_en & 0x1) << 31)			|
+			   ((zqinit & 0xF) << 24)		|
+			   ((zqoper & 0xF) << 16)		|
+			   ((zqcs & 0xF) << 8)			|
+			   ((zqcs_init & 0xF) << 0);
+	debug("zq_cntl = 0x%x\n", regs->zq_cntl);
+}
+
+static void cal_ddr_sr_cntr(struct ddr_cfg_regs *regs,
+			    const struct memctl_opt *popts)
+{
+	const unsigned int sr_it = (popts->auto_self_refresh_en) ?
+					popts->sr_it : 0;
+
+	regs->ddr_sr_cntr = (sr_it & 0xF) << 16;
+	debug("ddr_sr_cntr = 0x%x\n", regs->ddr_sr_cntr);
+}
+
+static void cal_ddr_eor(struct ddr_cfg_regs *regs,
+			const struct memctl_opt *popts)
+{
+	if (popts->addr_hash != 0) {
+		regs->eor = 0x40000000;	/* address hash enable */
+		debug("eor = 0x%x\n", regs->eor);
+	}
+}
+
+static void cal_ddr_csn_bnds(struct ddr_cfg_regs *regs,
+			     const struct memctl_opt *popts,
+			     const struct ddr_conf *conf,
+			     const struct dimm_params *pdimm)
+{
+	int i;
+	unsigned long long ea, sa;
+
+	/* Chip Select Memory Bounds (CSn_BNDS) */
+	for (i = 0;
+		i < DDRC_NUM_CS && conf->cs_size[i];
+		i++) {
+		debug("cs_in_use = 0x%x\n", conf->cs_in_use);
+		if (conf->cs_in_use != 0) {
+			sa = conf->cs_base_addr[i];
+			ea = sa + conf->cs_size[i] - 1;
+			sa >>= 24;
+			ea >>= 24;
+			regs->cs[i].bnds = ((sa & 0xffff) << 16) |
+					   ((ea & 0xffff) << 0);
+			cal_csn_config(i, regs, popts, pdimm);
+		} else {
+			/* setting bnds to 0xffffffff for inactive CS */
+			regs->cs[i].bnds = 0xffffffff;
+		}
+
+		debug("cs[%d].bnds = 0x%x\n", i, regs->cs[i].bnds);
+	}
+}
+
+static void cal_ddr_addr_dec(struct ddr_cfg_regs *regs)
+{
+#ifdef CONFIG_DDR_ADDR_DEC
+	unsigned int ba_bits __unused;
+	char p __unused;
+	const unsigned int cs0_config = regs->cs[0].config;
+	const int cacheline = PLATFORM_CACHE_LINE_SHIFT;
+	unsigned int bg_bits;
+	unsigned int row_bits;
+	unsigned int col_bits;
+	unsigned int cs;
+	unsigned int map_row[18];
+	unsigned int map_col[11];
+	unsigned int map_ba[2];
+	unsigned int map_cid[2] = {0x3F, 0x3F};
+	unsigned int map_bg[2] = {0x3F, 0x3F};
+	unsigned int map_cs[2] = {0x3F, 0x3F};
+	unsigned int dbw;
+	unsigned int ba_intlv;
+	int placement;
+	int intlv;
+	int abort = 0;
+	int i;
+	int j;
+
+	col_bits = (cs0_config >> 0) & 0x7;
+	if (col_bits < 4) {
+		col_bits += 8;
+	} else if (col_bits < 7 || col_bits > 10) {
+		ERROR("Error %s col_bits = %d\n", __func__, col_bits);
+	}
+	row_bits = ((cs0_config >> 8) & 0x7) + 12;
+	ba_bits = ((cs0_config >> 14) & 0x3) + 2;
+	bg_bits = ((cs0_config >> 4) & 0x3) + 0;
+	intlv = (cs0_config >> 24) & 0xf;
+	ba_intlv = (regs->sdram_cfg[0] >> 8) & 0x7f;
+	switch (ba_intlv) {
+	case DDR_BA_INTLV_CS01:
+		cs = 1;
+		break;
+	case DDR_BA_INTLV_CS0123:
+		cs = 2;
+		break;
+	case DDR_BA_NONE:
+		cs = 0;
+		break;
+	default:
+		ERROR("%s ba_intlv 0x%x\n", __func__, ba_intlv);
+		return;
+	}
+	debug("col %d, row %d, ba %d, bg %d, intlv %d\n",
+			col_bits, row_bits, ba_bits, bg_bits, intlv);
+	/*
+	 * Example mapping of 15x2x2x10
+	 * ---- --rr rrrr rrrr rrrr rCBB Gccc cccI cGcc cbbb
+	 */
+	dbw = (regs->sdram_cfg[0] >> 19) & 0x3;
+	switch (dbw) {
+	case 0:	/* 64-bit */
+		placement = 3;
+		break;
+	case 1:	/* 32-bit */
+		placement = 2;
+		break;
+	default:
+		ERROR("%s dbw = %d\n", __func__, dbw);
+		return;
+	}
+	debug("cacheline size %d\n", cacheline);
+	for (i = 0; placement < cacheline; i++) {
+		map_col[i] = placement++;
+	}
+	map_bg[0] = placement++;
+	for ( ; i < col_bits; i++) {
+		map_col[i] = placement++;
+		if (placement == intlv) {
+			placement++;
+		}
+	}
+	for ( ; i < 11; i++) {
+		map_col[i] = 0x3F;	/* unused col bits */
+	}
+
+	if (bg_bits >= 2) {
+		map_bg[1] = placement++;
+	}
+	map_ba[0] = placement++;
+	map_ba[1] = placement++;
+	if (cs != 0U) {
+		map_cs[0] = placement++;
+		if (cs == 2U) {
+			map_cs[1] = placement++;
+		}
+	} else {
+		map_cs[0] = U(0x3F);
+	}
+
+	for (i = 0; i < row_bits; i++) {
+		map_row[i] = placement++;
+	}
+
+	for ( ; i < 18; i++) {
+		map_row[i] = 0x3F;	/* unused row bits */
+	}
+
+	for (i = 39; i >= 0 ; i--) {
+		if (i == intlv) {
+			placement = 8;
+			p = 'I';
+		} else if (i < 3) {
+			p = 'b';
+			placement = 0;
+		} else {
+			placement = 0;
+			p = '-';
+		}
+		for (j = 0; j < 18; j++) {
+			if (map_row[j] != i) {
+				continue;
+			}
+			if (placement != 0) {
+				abort = 1;
+				ERROR("%s wrong address bit %d\n", __func__, i);
+			}
+			placement = i;
+			p = 'r';
+		}
+		for (j = 0; j < 11; j++) {
+			if (map_col[j] != i) {
+				continue;
+			}
+			if (placement != 0) {
+				abort = 1;
+				ERROR("%s wrong address bit %d\n", __func__, i);
+			}
+			placement = i;
+			p = 'c';
+		}
+		for (j = 0; j < 2; j++) {
+			if (map_ba[j] != i) {
+				continue;
+			}
+			if (placement != 0) {
+				abort = 1;
+				ERROR("%s wrong address bit %d\n", __func__, i);
+			}
+			placement = i;
+			p = 'B';
+		}
+		for (j = 0; j < 2; j++) {
+			if (map_bg[j] != i) {
+				continue;
+			}
+			if (placement != 0) {
+				abort = 1;
+				ERROR("%s wrong address bit %d\n", __func__, i);
+			}
+			placement = i;
+			p = 'G';
+		}
+		for (j = 0; j < 2; j++) {
+			if (map_cs[j] != i) {
+				continue;
+			}
+			if (placement != 0) {
+				abort = 1;
+				ERROR("%s wrong address bit %d\n", __func__, i);
+			}
+			placement = i;
+			p = 'C';
+		}
+#ifdef DDR_DEBUG
+		printf("%c", p);
+		if ((i % 4) == 0) {
+			printf(" ");
+		}
+#endif
+	}
+#ifdef DDR_DEBUG
+	puts("\n");
+#endif
+
+	if (abort != 0) {
+		return;
+	}
+
+	regs->dec[0] = map_row[17] << 26		|
+		      map_row[16] << 18			|
+		      map_row[15] << 10			|
+		      map_row[14] << 2;
+	regs->dec[1] = map_row[13] << 26		|
+		      map_row[12] << 18			|
+		      map_row[11] << 10			|
+		      map_row[10] << 2;
+	regs->dec[2] = map_row[9] << 26			|
+		      map_row[8] << 18			|
+		      map_row[7] << 10			|
+		      map_row[6] << 2;
+	regs->dec[3] = map_row[5] << 26			|
+		      map_row[4] << 18			|
+		      map_row[3] << 10			|
+		      map_row[2] << 2;
+	regs->dec[4] = map_row[1] << 26			|
+		      map_row[0] << 18			|
+		      map_col[10] << 10			|
+		      map_col[9] << 2;
+	regs->dec[5] = map_col[8] << 26			|
+		      map_col[7] << 18			|
+		      map_col[6] << 10			|
+		      map_col[5] << 2;
+	regs->dec[6] = map_col[4] << 26			|
+		      map_col[3] << 18			|
+		      map_col[2] << 10			|
+		      map_col[1] << 2;
+	regs->dec[7] = map_col[0] << 26			|
+		      map_ba[1] << 18			|
+		      map_ba[0] << 10			|
+		      map_cid[1] << 2;
+	regs->dec[8] = map_cid[1] << 26			|
+		      map_cs[1] << 18			|
+		      map_cs[0] << 10			|
+		      map_bg[1] << 2;
+	regs->dec[9] = map_bg[0] << 26			|
+		      1;
+	for (i = 0; i < 10; i++) {
+		debug("dec[%d] = 0x%x\n", i, regs->dec[i]);
+	}
+#endif
+}
+static unsigned int skip_caslat(unsigned int tckmin_ps,
+				unsigned int taamin_ps,
+				unsigned int mclk_ps,
+				unsigned int package_3ds)
+{
+	int i, j, k;
+	struct cas {
+		const unsigned int tckmin_ps;
+		const unsigned int caslat[4];
+	};
+	struct speed {
+		const struct cas *cl;
+		const unsigned int taamin_ps[4];
+	};
+	const struct cas cl_3200[] = {
+		{625,	{0xa00000, 0xb00000, 0xf000000,} },
+		{750,	{ 0x20000,  0x60000,  0xe00000,} },
+		{833,	{  0x8000,  0x18000,   0x38000,} },
+		{937,	{  0x4000,   0x4000,    0xc000,} },
+		{1071,	{  0x1000,   0x1000,    0x3000,} },
+		{1250,	{   0x400,    0x400,     0xc00,} },
+		{1500,	{       0,    0x600,     0x200,} },
+	};
+	const struct cas cl_2933[] = {
+		{682,	{       0,  0x80000, 0x180000, 0x380000} },
+		{750,	{ 0x20000,  0x60000,  0x60000,  0xe0000} },
+		{833,	{  0x8000,  0x18000,  0x18000,  0x38000} },
+		{937,	{  0x4000,   0x4000,   0x4000,   0xc000} },
+		{1071,	{  0x1000,   0x1000,   0x1000,   0x3000} },
+		{1250,	{   0x400,    0x400,    0x400,    0xc00} },
+		{1500,	{       0,    0x200,    0x200,    0x200} },
+	};
+	const struct cas cl_2666[] = {
+		{750,	{       0,  0x20000,  0x60000,  0xe0000} },
+		{833,	{  0x8000,  0x18000,  0x18000,  0x38000} },
+		{937,	{  0x4000,   0x4000,   0x4000,   0xc000} },
+		{1071,	{  0x1000,   0x1000,   0x1000,   0x3000} },
+		{1250,	{   0x400,    0x400,    0x400,    0xc00} },
+		{1500,	{       0,        0,    0x200,    0x200} },
+	};
+	const struct cas cl_2400[] = {
+		{833,	{       0,   0x8000,  0x18000,  0x38000} },
+		{937,	{  0xc000,   0x4000,   0x4000,   0xc000} },
+		{1071,	{  0x3000,   0x1000,   0x1000,   0x3000} },
+		{1250,	{   0xc00,    0x400,    0x400,    0xc00} },
+		{1500,	{       0,    0x400,    0x200,    0x200} },
+	};
+	const struct cas cl_2133[] = {
+		{937,	{       0,   0x4000,   0xc000,} },
+		{1071,	{  0x2000,        0,   0x2000,} },
+		{1250,	{   0x800,        0,    0x800,} },
+		{1500,	{       0,    0x400,    0x200,} },
+	};
+	const struct cas cl_1866[] = {
+		{1071,	{       0,   0x1000,   0x3000,} },
+		{1250,	{   0xc00,    0x400,    0xc00,} },
+		{1500,	{       0,    0x400,    0x200,} },
+	};
+	const struct cas cl_1600[] = {
+		{1250,	{       0,    0x400,    0xc00,} },
+		{1500,	{       0,    0x400,    0x200,} },
+	};
+	const struct speed bin_0[] = {
+		{cl_3200, {12500, 13750, 15000,} },
+		{cl_2933, {12960, 13640, 13750, 15000,} },
+		{cl_2666, {12750, 13500, 13750, 15000,} },
+		{cl_2400, {12500, 13320, 13750, 15000,} },
+		{cl_2133, {13130, 13500, 15000,} },
+		{cl_1866, {12850, 13500, 15000,} },
+		{cl_1600, {12500, 13500, 15000,} }
+	};
+	const struct cas cl_3200_3ds[] = {
+		{625,	{ 0xa000000, 0xb000000, 0xf000000,} },
+		{750,	{ 0xaa00000, 0xab00000, 0xef00000,} },
+		{833,	{ 0xaac0000, 0xaac0000, 0xebc0000,} },
+		{937,	{ 0xaab0000, 0xaab0000, 0xeaf0000,} },
+		{1071,	{ 0xaaa4000, 0xaaac000, 0xeaec000,} },
+		{1250,	{ 0xaaa0000, 0xaaa2000, 0xeaeb000,} },
+	};
+	const struct cas cl_2666_3ds[] = {
+		{750,	{ 0xa00000, 0xb00000, 0xf00000,} },
+		{833,	{ 0xac0000, 0xac0000, 0xbc0000,} },
+		{937,	{ 0xab0000, 0xab0000, 0xaf0000,} },
+		{1071,	{ 0xaa4000, 0xaac000, 0xaac000,} },
+		{1250,	{ 0xaa0000, 0xaaa000, 0xaaa000,} },
+	};
+	const struct cas cl_2400_3ds[] = {
+		{833,	{ 0xe00000, 0xe40000, 0xec0000, 0xb00000} },
+		{937,	{ 0xe00000, 0xe00000, 0xea0000, 0xae0000} },
+		{1071,	{ 0xe00000, 0xe04000, 0xeac000, 0xaec000} },
+		{1250,	{ 0xe00000, 0xe00000, 0xeaa000, 0xae2000} },
+	};
+	const struct cas cl_2133_3ds[] = {
+		{937,	{  0x90000,  0xb0000,  0xf0000,} },
+		{1071,	{  0x84000,  0xac000,  0xec000,} },
+		{1250,	{  0x80000,  0xa2000,  0xe2000,} },
+	};
+	const struct cas cl_1866_3ds[] = {
+		{1071,	{        0,   0x4000,   0xc000,} },
+		{1250,	{        0,   0x1000,   0x3000,} },
+	};
+	const struct cas cl_1600_3ds[] = {
+		{1250,	{        0,   0x1000,   0x3000,} },
+	};
+	const struct speed bin_3ds[] = {
+		{cl_3200_3ds, {15000, 16250, 17140,} },
+		{cl_2666_3ds, {15000, 16500, 17140,} },
+		{cl_2400_3ds, {15000, 15830, 16670, 17140} },
+		{cl_2133_3ds, {15950, 16880, 17140,} },
+		{cl_1866_3ds, {15000, 16070, 17140,} },
+		{cl_1600_3ds, {15000, 16250, 17500,} },
+	};
+	const struct speed *bin;
+	int size;
+	unsigned int taamin_max, tck_max;
+
+	if (taamin_ps > ((package_3ds != 0) ? 21500 : 18000)) {
+		ERROR("taamin_ps %u invalid\n", taamin_ps);
+		return 0;
+	}
+	if (package_3ds != 0) {
+		bin = bin_3ds;
+		size = ARRAY_SIZE(bin_3ds);
+		taamin_max = 1250;
+		tck_max = 1500;
+	} else {
+		bin = bin_0;
+		size = ARRAY_SIZE(bin_0);
+		taamin_max = 1500;
+		tck_max = 1600;
+	}
+	if (mclk_ps < 625 || mclk_ps > tck_max) {
+		ERROR("mclk %u invalid\n", mclk_ps);
+		return 0;
+	}
+
+	for (i = 0; i < size; i++) {
+		if (bin[i].cl[0].tckmin_ps >= tckmin_ps) {
+			break;
+		}
+	}
+	if (i >= size) {
+		ERROR("speed bin not found\n");
+		return 0;
+	}
+	if (bin[i].cl[0].tckmin_ps > tckmin_ps && i > 0) {
+		i--;
+	}
+
+	for (j = 0; j < 4; j++) {
+		if ((bin[i].taamin_ps[j] == 0) ||
+		    bin[i].taamin_ps[j] >= taamin_ps) {
+			break;
+		}
+	}
+
+	if (j >= 4) {
+		ERROR("taamin_ps out of range.\n");
+		return 0;
+	}
+
+	if ((bin[i].taamin_ps[j] == 0) ||
+	    (bin[i].taamin_ps[j] > taamin_ps && j > 0)) {
+		j--;
+	}
+
+	for (k = 0; bin[i].cl[k].tckmin_ps < mclk_ps &&
+		    bin[i].cl[k].tckmin_ps < taamin_max; k++)
+		;
+	if (bin[i].cl[k].tckmin_ps > mclk_ps && k > 0) {
+		k--;
+	}
+
+	debug("Skip CL mask for this speed 0x%x\n", bin[i].cl[k].caslat[j]);
+
+	return bin[i].cl[k].caslat[j];
+}
+
+int compute_ddrc(const unsigned long clk,
+		 const struct memctl_opt *popts,
+		 const struct ddr_conf *conf,
+		 struct ddr_cfg_regs *regs,
+		 const struct dimm_params *pdimm,
+		 unsigned int ip_rev)
+{
+	unsigned int cas_latency;
+	unsigned int caslat_skip;
+	unsigned int additive_latency;
+	const unsigned int mclk_ps = get_memory_clk_ps(clk);
+	int i;
+
+	zeromem(regs, sizeof(struct ddr_cfg_regs));
+
+	if (mclk_ps < pdimm->tckmin_x_ps) {
+		ERROR("DDR Clk: MCLK cycle is %u ps.\n", mclk_ps);
+		ERROR("DDR Clk is faster than DIMM can support.\n");
+	}
+
+	/* calculate cas latency, override first */
+	cas_latency = (popts->caslat_override != 0) ?
+			popts->caslat_override_value :
+			(pdimm->taa_ps + mclk_ps - 1) / mclk_ps;
+
+	/* skip unsupported caslat based on speed bin */
+	caslat_skip = skip_caslat(pdimm->tckmin_x_ps,
+				  pdimm->taa_ps,
+				  mclk_ps,
+				  pdimm->package_3ds);
+	debug("Skip caslat 0x%x\n", caslat_skip);
+
+	/* Check if DIMM supports the cas latency */
+	i = 24;
+	while (((pdimm->caslat_x & ~caslat_skip & (1 << cas_latency)) == 0) &&
+	       (i-- > 0)) {
+		cas_latency++;
+	}
+
+	if (i <= 0) {
+		ERROR("Failed to find a proper cas latency\n");
+		return -EINVAL;
+	}
+	/* Verify cas latency does not exceed 18ns for DDR4 */
+	if (cas_latency * mclk_ps > 18000) {
+		ERROR("cas latency is too large %d\n", cas_latency);
+		return -EINVAL;
+	}
+
+	additive_latency = (popts->addt_lat_override != 0) ?
+				popts->addt_lat_override_value : 0;
+
+	cal_ddr_csn_bnds(regs, popts, conf, pdimm);
+	cal_ddr_sdram_cfg(clk, regs, popts, pdimm, ip_rev);
+	cal_ddr_sdram_rcw(clk, regs, popts, pdimm);
+	cal_timing_cfg(clk, regs, popts, pdimm, conf, cas_latency,
+		       additive_latency);
+	cal_ddr_dq_mapping(regs, pdimm);
+
+	if (ip_rev >= 0x50500) {
+		cal_ddr_addr_dec(regs);
+	}
+
+	cal_ddr_sdram_mode(clk, regs, popts, conf, pdimm, cas_latency,
+			   additive_latency, ip_rev);
+	cal_ddr_eor(regs, popts);
+	cal_ddr_data_init(regs);
+	cal_ddr_sdram_interval(clk, regs, popts, pdimm);
+	cal_ddr_zq_cntl(regs);
+	cal_ddr_sr_cntr(regs, popts);
+
+	return 0;
+}
diff --git a/drivers/nxp/ddr/nxp-ddr/utility.c b/drivers/nxp/ddr/nxp-ddr/utility.c
new file mode 100644
index 0000000..b6dffc8
--- /dev/null
+++ b/drivers/nxp/ddr/nxp-ddr/utility.c
@@ -0,0 +1,288 @@
+/*
+ * Copyright 2021-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <common/debug.h>
+#include <ddr.h>
+#include <immap.h>
+#include <lib/mmio.h>
+
+#define UL_5POW12	244140625UL
+#define ULL_2E12	2000000000000ULL
+#define UL_2POW13	(1UL << 13)
+#define ULL_8FS		0xFFFFFFFFULL
+
+#define do_div(n, base) ({				\
+	unsigned int __base = (base);			\
+	unsigned int __rem;				\
+	__rem = ((unsigned long long)(n)) % __base;	\
+	(n) = ((unsigned long long)(n)) / __base;	\
+	__rem;						\
+})
+
+#define CCN_HN_F_SAM_NODEID_MASK	0x7f
+#ifdef NXP_HAS_CCN504
+#define CCN_HN_F_SAM_NODEID_DDR0	0x4
+#define CCN_HN_F_SAM_NODEID_DDR1	0xe
+#elif defined(NXP_HAS_CCN508)
+#define CCN_HN_F_SAM_NODEID_DDR0_0	0x3
+#define CCN_HN_F_SAM_NODEID_DDR0_1	0x8
+#define CCN_HN_F_SAM_NODEID_DDR1_0	0x13
+#define CCN_HN_F_SAM_NODEID_DDR1_1	0x18
+#endif
+
+unsigned long get_ddr_freq(struct sysinfo *sys, int ctrl_num)
+{
+	if (sys->freq_ddr_pll0 == 0) {
+		get_clocks(sys);
+	}
+
+	switch (ctrl_num) {
+	case 0:
+		return sys->freq_ddr_pll0;
+	case 1:
+		return sys->freq_ddr_pll0;
+	case 2:
+		return sys->freq_ddr_pll1;
+	}
+
+	return 0;
+}
+
+unsigned int get_memory_clk_ps(const unsigned long data_rate)
+{
+	unsigned int result;
+	/* Round to nearest 10ps, being careful about 64-bit multiply/divide */
+	unsigned long long rem, mclk_ps = ULL_2E12;
+
+	/* Now perform the big divide, the result fits in 32-bits */
+	rem = do_div(mclk_ps, data_rate);
+	result = (rem >= (data_rate >> 1)) ? mclk_ps + 1 : mclk_ps;
+
+	return result;
+}
+
+unsigned int picos_to_mclk(unsigned long data_rate, unsigned int picos)
+{
+	unsigned long long clks, clks_rem;
+
+	/* Short circuit for zero picos */
+	if ((picos == 0U) || (data_rate == 0UL)) {
+		return 0U;
+	}
+
+	/* First multiply the time by the data rate (32x32 => 64) */
+	clks = picos * (unsigned long long)data_rate;
+	/*
+	 * Now divide by 5^12 and track the 32-bit remainder, then divide
+	 * by 2*(2^12) using shifts (and updating the remainder).
+	 */
+	clks_rem = do_div(clks, UL_5POW12);
+	clks_rem += (clks & (UL_2POW13-1)) * UL_5POW12;
+	clks >>= 13U;
+
+	/* If we had a remainder greater than the 1ps error, then round up */
+	if (clks_rem > data_rate) {
+		clks++;
+	}
+
+	/* Clamp to the maximum representable value */
+	if (clks > ULL_8FS) {
+		clks = ULL_8FS;
+	}
+	return (unsigned int) clks;
+}
+
+/* valid_spd_mask has been checked by parse_spd */
+int disable_unused_ddrc(struct ddr_info *priv,
+			int valid_spd_mask, uintptr_t nxp_ccn_hn_f0_addr)
+{
+#if defined(NXP_HAS_CCN504) || defined(NXP_HAS_CCN508)
+	void *hnf_sam_ctrl = (void *)(nxp_ccn_hn_f0_addr + CCN_HN_F_SAM_CTL);
+	uint32_t val, nodeid;
+#ifdef NXP_HAS_CCN504
+	uint32_t num_hnf_nodes = 4U;
+#else
+	uint32_t num_hnf_nodes = 8U;
+#endif
+	int disable_ddrc = 0;
+	int i;
+
+	if (priv->num_ctlrs < 2) {
+		debug("%s: nothing to do.\n", __func__);
+	}
+
+	switch (priv->dimm_on_ctlr) {
+	case 1:
+		disable_ddrc = ((valid_spd_mask &0x2) == 0) ? 2 : 0;
+		disable_ddrc = ((valid_spd_mask &0x1) == 0) ? 1 : disable_ddrc;
+		break;
+	case 2:
+		disable_ddrc = ((valid_spd_mask &0x4) == 0) ? 2 : 0;
+		disable_ddrc = ((valid_spd_mask &0x1) == 0) ? 1 : disable_ddrc;
+		break;
+	default:
+		ERROR("Invalid number of DIMMs %d\n", priv->dimm_on_ctlr);
+		return -EINVAL;
+	}
+
+	if (disable_ddrc != 0) {
+		debug("valid_spd_mask = 0x%x\n", valid_spd_mask);
+	}
+
+	switch (disable_ddrc) {
+	case 1:
+		priv->num_ctlrs = 1;
+		priv->spd_addr = &priv->spd_addr[priv->dimm_on_ctlr];
+		priv->ddr[0] = priv->ddr[1];
+		priv->ddr[1] = NULL;
+		priv->phy[0] = priv->phy[0];
+		priv->phy[1] = NULL;
+		debug("Disable first DDR controller\n");
+		break;
+	case 2:
+		priv->num_ctlrs = 1;
+		priv->ddr[1] = NULL;
+		priv->phy[1] = NULL;
+		debug("Disable second DDR controller\n");
+		/* fallthrough */
+	case 0:
+		break;
+	default:
+		ERROR("Program error.\n");
+		return -EINVAL;
+	}
+
+	if (disable_ddrc == 0) {
+		debug("Both controllers in use.\n");
+		return 0;
+	}
+
+	for (i = 0; i < num_hnf_nodes; i++) {
+		val = mmio_read_64((uintptr_t)hnf_sam_ctrl);
+#ifdef NXP_HAS_CCN504
+		nodeid = disable_ddrc == 1 ? CCN_HN_F_SAM_NODEID_DDR1 :
+			(disable_ddrc == 2 ? CCN_HN_F_SAM_NODEID_DDR0 :
+			 0x0);   /*Failure condition. never hit */
+#elif defined(NXP_HAS_CCN508)
+		if (disable_ddrc == 1) {
+			nodeid = (i < 2 || i >= 6) ? CCN_HN_F_SAM_NODEID_DDR1_1 :
+				CCN_HN_F_SAM_NODEID_DDR1_0;
+		} else if (disable_ddrc == 2) {
+			nodeid = (i < 2 || i >= 6) ? CCN_HN_F_SAM_NODEID_DDR0_0 :
+				CCN_HN_F_SAM_NODEID_DDR0_1;
+		} else {
+			nodeid = 0; /* Failure condition. never hit */
+		}
+#endif
+		if (nodeid != (val & CCN_HN_F_SAM_NODEID_MASK)) {
+			debug("Setting HN-F node %d\n", i);
+			debug("nodeid = 0x%x\n", nodeid);
+			val &= ~CCN_HN_F_SAM_NODEID_MASK;
+			val |= nodeid;
+			mmio_write_64((uintptr_t)hnf_sam_ctrl, val);
+		}
+		hnf_sam_ctrl += CCN_HN_F_REGION_SIZE;
+	}
+#endif
+	return 0;
+}
+
+unsigned int get_ddrc_version(const struct ccsr_ddr *ddr)
+{
+	unsigned int ver;
+
+	ver = (ddr_in32(&ddr->ip_rev1) & 0xFFFF) << 8U;
+	ver |= (ddr_in32(&ddr->ip_rev2) & 0xFF00) >> 8U;
+
+	return ver;
+}
+
+void print_ddr_info(struct ccsr_ddr *ddr)
+{
+	unsigned int cs0_config = ddr_in32(&ddr->csn_cfg[0]);
+	unsigned int sdram_cfg = ddr_in32(&ddr->sdram_cfg);
+	int cas_lat;
+
+	if ((sdram_cfg & SDRAM_CFG_MEM_EN) == 0U) {
+		printf(" (DDR not enabled)\n");
+		return;
+	}
+
+	printf("DDR");
+	switch ((sdram_cfg & SDRAM_CFG_SDRAM_TYPE_MASK) >>
+		SDRAM_CFG_SDRAM_TYPE_SHIFT) {
+	case SDRAM_TYPE_DDR4:
+		printf("4");
+		break;
+	default:
+		printf("?");
+		break;
+	}
+
+	switch (sdram_cfg & SDRAM_CFG_DBW_MASK) {
+	case SDRAM_CFG_32_BW:
+		printf(", 32-bit");
+		break;
+	case SDRAM_CFG_16_BW:
+		printf(", 16-bit");
+		break;
+	case SDRAM_CFG_8_BW:
+		printf(", 8-bit");
+		break;
+	default:
+		printf(", 64-bit");
+		break;
+	}
+
+	/* Calculate CAS latency based on timing cfg values */
+	cas_lat = ((ddr_in32(&ddr->timing_cfg_1) >> 16) & 0xf);
+	cas_lat += 2;	/* for DDRC newer than 4.4 */
+	cas_lat += ((ddr_in32(&ddr->timing_cfg_3) >> 12) & 3) << 4;
+	printf(", CL=%d", cas_lat >> 1);
+	if ((cas_lat & 0x1) != 0) {
+		printf(".5");
+	}
+
+	if ((sdram_cfg & SDRAM_CFG_ECC_EN) != 0) {
+		printf(", ECC on");
+	} else {
+		printf(", ECC off");
+	}
+
+	if ((cs0_config & 0x20000000) != 0) {
+		printf(", ");
+		switch ((cs0_config >> 24) & 0xf) {
+		case DDR_256B_INTLV:
+			printf("256B");
+			break;
+		default:
+			printf("invalid");
+			break;
+		}
+	}
+
+	if (((sdram_cfg >> 8) & 0x7f) != 0) {
+		printf(", ");
+		switch (sdram_cfg >> 8 & 0x7f) {
+		case DDR_BA_INTLV_CS0123:
+			printf("CS0+CS1+CS2+CS3");
+			break;
+		case DDR_BA_INTLV_CS01:
+			printf("CS0+CS1");
+			break;
+		default:
+			printf("invalid");
+			break;
+		}
+	}
+	printf("\n");
+}
diff --git a/drivers/nxp/ddr/phy-gen1/phy.c b/drivers/nxp/ddr/phy-gen1/phy.c
new file mode 100644
index 0000000..4b66d38
--- /dev/null
+++ b/drivers/nxp/ddr/phy-gen1/phy.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <common/debug.h>
+#include <ddr.h>
+
+static void cal_ddr_sdram_clk_cntl(struct ddr_cfg_regs *regs,
+					 const struct memctl_opt *popts)
+{
+	const unsigned int clk_adj = popts->clk_adj;
+	const unsigned int ss_en = 0U;
+
+	regs->clk_cntl = ((ss_en & U(0x1)) << 31U)		|
+				  ((clk_adj & U(0x1F)) << 22U);
+	debug("clk_cntl = 0x%x\n", regs->clk_cntl);
+}
+
+static void cal_ddr_cdr(struct ddr_cfg_regs *regs,
+			const struct memctl_opt *popts)
+{
+	regs->cdr[0] = popts->ddr_cdr1;
+	regs->cdr[1] = popts->ddr_cdr2;
+	debug("cdr[0] = 0x%x\n", regs->cdr[0]);
+	debug("cdr[1] = 0x%x\n", regs->cdr[1]);
+}
+
+static void cal_ddr_wrlvl_cntl(struct ddr_cfg_regs *regs,
+				const struct memctl_opt *popts)
+{
+	const unsigned int wrlvl_en = 1U;	/* enabled */
+	const unsigned int wrlvl_mrd = U(0x6);	/* > 40nCK */
+	const unsigned int wrlvl_odten = U(0x7);	/* 128 */
+	const unsigned int wrlvl_dqsen = U(0x5);	/* > 25nCK */
+	const unsigned int wrlvl_wlr = U(0x6);	/* > tWLO + 6 */
+	const unsigned int wrlvl_smpl = popts->wrlvl_override ?
+					popts->wrlvl_sample : U(0xf);
+	const unsigned int wrlvl_start = popts->wrlvl_start;
+
+	regs->wrlvl_cntl[0] = ((wrlvl_en & U(0x1)) << 31U)	|
+				  ((wrlvl_mrd & U(0x7)) << 24U)	|
+				  ((wrlvl_odten & U(0x7)) << 20U)	|
+				  ((wrlvl_dqsen & U(0x7)) << 16U)	|
+				  ((wrlvl_smpl & U(0xf)) << 12U)	|
+				  ((wrlvl_wlr & U(0x7)) << 8U)	|
+				  ((wrlvl_start & U(0x1F)) << 0U);
+	regs->wrlvl_cntl[1] = popts->wrlvl_ctl_2;
+	regs->wrlvl_cntl[2] = popts->wrlvl_ctl_3;
+	debug("wrlvl_cntl[0] = 0x%x\n", regs->wrlvl_cntl[0]);
+	debug("wrlvl_cntl[1] = 0x%x\n", regs->wrlvl_cntl[1]);
+	debug("wrlvl_cntl[2] = 0x%x\n", regs->wrlvl_cntl[2]);
+
+}
+
+static void cal_ddr_dbg(struct ddr_cfg_regs *regs,
+			const struct memctl_opt *popts)
+{
+	if (popts->cswl_override != 0) {
+		regs->debug[18] = popts->cswl_override;
+	}
+
+#ifdef CONFIG_SYS_FSL_DDR_EMU
+	/* disable DDR training for emulator */
+	regs->debug[2] = U(0x00000400);
+	regs->debug[4] = U(0xff800800);
+	regs->debug[5] = U(0x08000800);
+	regs->debug[6] = U(0x08000800);
+	regs->debug[7] = U(0x08000800);
+	regs->debug[8] = U(0x08000800);
+#endif
+	if (popts->cpo_sample != 0U) {
+		regs->debug[28] = popts->cpo_sample;
+		debug("debug[28] = 0x%x\n", regs->debug[28]);
+	}
+}
+
+int compute_ddr_phy(struct ddr_info *priv)
+{
+	const struct memctl_opt *popts = &priv->opt;
+	struct ddr_cfg_regs *regs = &priv->ddr_reg;
+
+	cal_ddr_sdram_clk_cntl(regs, popts);
+	cal_ddr_cdr(regs, popts);
+	cal_ddr_wrlvl_cntl(regs, popts);
+	cal_ddr_dbg(regs, popts);
+
+	return 0;
+}
diff --git a/drivers/nxp/ddr/phy-gen2/csr.h b/drivers/nxp/ddr/phy-gen2/csr.h
new file mode 100644
index 0000000..ee7b4d8
--- /dev/null
+++ b/drivers/nxp/ddr/phy-gen2/csr.h
@@ -0,0 +1,151 @@
+/*
+ * Copyright 2021 NXP
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef CSR_H
+#define CSR_H
+
+#define t_anib					0
+#define t_dbyte					0x10000
+#define t_master				0x20000
+#define t_acsm					0x40000
+#define t_initeng				0x90000
+#define t_drtub					0xc0000
+#define t_apbonly				0xd0000
+#define csr_dbyte_misc_mode_addr		0x00
+#define csr_micro_cont_mux_sel_addr		0x00
+#define csr_uct_shadow_regs			0x04
+#define csr_cal_uclk_info_addr			0x08
+#define csr_seq0bdly0_addr			0x0b
+#define csr_seq0bdly1_addr			0x0c
+#define csr_seq0bdly2_addr			0x0d
+#define csr_seq0bdly3_addr			0x0e
+#define csr_seq0bdisable_flag0_addr		0x0c
+#define csr_seq0bdisable_flag1_addr		0x0d
+#define csr_seq0bdisable_flag2_addr		0x0e
+#define csr_seq0bdisable_flag3_addr		0x0f
+#define csr_seq0bdisable_flag4_addr		0x10
+#define csr_seq0bdisable_flag5_addr		0x11
+#define csr_seq0bdisable_flag6_addr		0x12
+#define csr_seq0bdisable_flag7_addr		0x13
+#define csr_dfi_mode_addr			0x18
+#define csr_tristate_mode_ca_addr		0x19
+#define csr_dfiphyupd_addr			0x21
+#define csr_dqs_preamble_control_addr		0x24
+#define csr_master_x4config_addr		0x25
+#define csr_enable_cs_multicast_addr		0x27
+#define csr_acx4_anib_dis_addr			0x2c
+#define csr_dmipin_present_addr			0x2d
+#define csr_ard_ptr_init_val_addr		0x2e
+#define csr_dct_write_prot			0x31
+#define csr_uct_write_only_shadow		0x32
+#define csr_uct_write_prot			0x33
+#define csr_uct_dat_write_only_shadow		0x34
+#define	csr_dbyte_dll_mode_cntrl_addr		0x3a
+#define csr_atx_impedance_addr			0x43
+#define csr_dq_dqs_rcv_cntrl_addr		0x43
+#define csr_cal_offsets_addr			0x45
+#define csr_tx_impedance_ctrl1_addr		0x49
+#define csr_dq_dqs_rcv_cntrl1_addr		0x4a
+#define csr_tx_odt_drv_stren_addr		0x4d
+#define csr_cal_drv_str0_addr			0x50
+#define csr_atx_slew_rate_addr			0x55
+#define csr_proc_odt_time_ctl_addr		0x56
+#define csr_mem_alert_control_addr		0x5b
+#define csr_mem_alert_control2_addr		0x5c
+#define csr_tx_slew_rate_addr			0x5f
+#define csr_mem_reset_l_addr			0x60
+#define csr_dfi_camode_addr			0x75
+#define csr_dll_gain_ctl_addr			0x7c
+#define csr_dll_lockparam_addr			0x7d
+#define csr_ucclk_hclk_enables_addr		0x80
+#define csr_acsm_playback0x0_addr		0x80
+#define csr_acsm_playback1x0_addr		0x81
+#define csr_cal_rate_addr			0x88
+#define csr_cal_zap_addr			0x89
+#define csr_cal_misc2_addr			0x98
+#define csr_micro_reset_addr			0x99
+#define csr_dfi_rd_data_cs_dest_map_addr	0xb0
+#define csr_vref_in_global_addr			0xb2
+#define csr_dfi_wr_data_cs_dest_map_addr	0xb4
+#define csr_pll_pwr_dn_addr			0xc3
+#define csr_pll_ctrl2_addr			0xc5
+#define csr_pll_ctrl1_addr			0xc7
+#define csr_pll_test_mode_addr			0xca
+#define csr_pll_ctrl4_addr			0xcc
+#define csr_dfi_freq_xlat0_addr			0xf0
+#define csr_acsm_ctrl0_addr			0xf0
+#define csr_dfi_freq_ratio_addr			0xfa
+#define csr_acsm_ctrl13_addr			0xfd
+#define csr_tx_pre_drv_mode_lsb			8
+#define csr_tx_pre_n_lsb			4
+#define csr_tx_pre_p_lsb			0
+#define csr_atx_pre_drv_mode_lsb		8
+#define csr_atx_pre_n_lsb			4
+#define csr_atx_pre_p_lsb			0
+#define csr_wdqsextension_lsb			8
+#define csr_lp4sttc_pre_bridge_rx_en_lsb	7
+#define csr_lp4postamble_ext_lsb		6
+#define csr_lp4tgl_two_tck_tx_dqs_pre_lsb	5
+#define csr_position_dfe_init_lsb		2
+#define csr_two_tck_tx_dqs_pre_lsb		1
+#define csr_two_tck_rx_dqs_pre_lsb		0
+#define csr_dll_rx_preamble_mode_lsb		1
+#define csr_odtstren_n_lsb			6
+#define csr_drv_stren_fsdq_n_lsb		6
+#define	csr_drv_stren_fsdq_p_lsb		0
+#define csr_adrv_stren_n_lsb			5
+#define csr_adrv_stren_p_lsb			0
+#define csr_cal_drv_str_pu50_lsb		4
+#define csr_cal_once_lsb			5
+#define csr_cal_interval_lsb			0
+#define csr_cal_run_lsb				4
+#define csr_global_vref_in_dac_lsb		3
+#define csr_gain_curr_adj_lsb			7
+#define csr_major_mode_dbyte_lsb		4
+#define csr_dfe_ctrl_lsb			2
+#define csr_ext_vref_range_lsb			1
+#define csr_sel_analog_vref_lsb			0
+#define csr_malertsync_bypass_lsb		0
+#define csr_ck_dis_val_lsb			2
+#define csr_ddr2tmode_lsb			1
+#define csr_dis_dyn_adr_tri_lsb			0
+#define	csr_dbyte_disable_lsb			2
+#define csr_power_down_rcvr_lsb			0
+#define csr_power_down_rcvr_dqs_lsb		9
+#define csr_rx_pad_standby_en_lsb		10
+#define csr_rx_pad_standby_en_mask		0x400
+#define csr_x4tg_lsb				0
+#define csr_reset_to_micro_mask			0x8
+#define csr_protect_mem_reset_mask		0x2
+#define csr_stall_to_micro_mask			0x1
+#define uct_write_prot_shadow_mask		0x1
+#define csr_acsm_par_mode_mask			0x4000
+#define csr_acsm_cke_enb_lsb			0
+#define csr_dfiphyupd_threshold_lsb		8
+#define csr_dfiphyupd_threshold_msb		11
+#define csr_dfiphyupd_threshold_mask		0xf00
+#define csr_dfi_rd_destm0_lsb			0
+#define csr_dfi_rd_destm1_lsb			2
+#define csr_dfi_rd_destm2_lsb			4
+#define csr_dfi_rd_destm3_lsb			6
+#define csr_dfi_wr_destm0_lsb			0
+#define csr_dfi_wr_destm1_lsb			2
+#define csr_dfi_wr_destm2_lsb			4
+#define csr_dfi_wr_destm3_lsb			6
+#define csr_acsm_2t_mode_mask			0x40
+#define csr_cal_misc2_err_dis			13
+#define csr_cal_offset_pdc_lsb			6
+#define csr_cal_offset_pdc_msb			9
+#define csr_cal_offset_pdc_mask			0xe0
+#define csr_cal_drv_pdth_mask			0x3c0
+
+
+struct impedance_mapping {
+	int ohm;
+	int code;
+};
+
+#endif
diff --git a/drivers/nxp/ddr/phy-gen2/ddr4fw.h b/drivers/nxp/ddr/phy-gen2/ddr4fw.h
new file mode 100644
index 0000000..f17f2e7
--- /dev/null
+++ b/drivers/nxp/ddr/phy-gen2/ddr4fw.h
@@ -0,0 +1,2897 @@
+/*
+ * Copyright 2021 NXP
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef DDR4FW
+#define DDR4FW
+
+#define PHY_GEN2_MAX_IMAGE_SIZE		32768
+#define PHY_GEN2_IMEM_ADDR		0x50000
+#define PHY_GEN2_DMEM_ADDR		0x54000
+
+struct ddr4u1d {
+	uint8_t  reserved00;
+	uint8_t  msg_misc;
+	uint16_t pmu_revision;
+	uint8_t  pstate;
+	uint8_t  pll_bypass_en;
+	uint16_t dramfreq;
+	uint8_t  dfi_freq_ratio;
+	uint8_t  bpznres_val;
+	uint8_t  phy_odt_impedance;
+	uint8_t  phy_drv_impedance;
+	uint8_t  phy_vref;
+	uint8_t  dram_type;
+	uint8_t  disabled_dbyte;
+	uint8_t  enabled_dqs;
+	uint8_t  cs_present;
+	uint8_t  cs_present_d0;
+	uint8_t  cs_present_d1;
+	uint8_t  addr_mirror;
+	uint8_t  cs_test_fail;
+	uint8_t  phy_cfg;
+	uint16_t sequence_ctrl;
+	uint8_t  hdt_ctrl;
+	uint8_t  reserved19[0x1B - 0x19];
+	uint8_t  share2dvref_result;
+	uint8_t  reserved1c[0x22 - 0x1c];
+	uint16_t phy_config_override;
+	uint8_t  dfimrlmargin;
+	int8_t   cdd_rr_3_2;
+	int8_t   cdd_rr_3_1;
+	int8_t   cdd_rr_3_0;
+	int8_t   cdd_rr_2_3;
+	int8_t   cdd_rr_2_1;
+	int8_t   cdd_rr_2_0;
+	int8_t   cdd_rr_1_3;
+	int8_t   cdd_rr_1_2;
+	int8_t   cdd_rr_1_0;
+	int8_t   cdd_rr_0_3;
+	int8_t   cdd_rr_0_2;
+	int8_t   cdd_rr_0_1;
+	int8_t   cdd_ww_3_2;
+	int8_t   cdd_ww_3_1;
+	int8_t   cdd_ww_3_0;
+	int8_t   cdd_ww_2_3;
+	int8_t   cdd_ww_2_1;
+	int8_t   cdd_ww_2_0;
+	int8_t   cdd_ww_1_3;
+	int8_t   cdd_ww_1_2;
+	int8_t   cdd_ww_1_0;
+	int8_t   cdd_ww_0_3;
+	int8_t   cdd_ww_0_2;
+	int8_t   cdd_ww_0_1;
+	int8_t   cdd_rw_3_3;
+	int8_t   cdd_rw_3_2;
+	int8_t   cdd_rw_3_1;
+	int8_t   cdd_rw_3_0;
+	int8_t   cdd_rw_2_3;
+	int8_t   cdd_rw_2_2;
+	int8_t   cdd_rw_2_1;
+	int8_t   cdd_rw_2_0;
+	int8_t   cdd_rw_1_3;
+	int8_t   cdd_rw_1_2;
+	int8_t   cdd_rw_1_1;
+	int8_t   cdd_rw_1_0;
+	int8_t   cdd_rw_0_3;
+	int8_t   cdd_rw_0_2;
+	int8_t   cdd_rw_0_1;
+	int8_t   cdd_rw_0_0;
+	int8_t   cdd_wr_3_3;
+	int8_t   cdd_wr_3_2;
+	int8_t   cdd_wr_3_1;
+	int8_t   cdd_wr_3_0;
+	int8_t   cdd_wr_2_3;
+	int8_t   cdd_wr_2_2;
+	int8_t   cdd_wr_2_1;
+	int8_t   cdd_wr_2_0;
+	int8_t   cdd_wr_1_3;
+	int8_t   cdd_wr_1_2;
+	int8_t   cdd_wr_1_1;
+	int8_t   cdd_wr_1_0;
+	int8_t   cdd_wr_0_3;
+	int8_t   cdd_wr_0_2;
+	int8_t   cdd_wr_0_1;
+	int8_t   cdd_wr_0_0;
+	uint8_t  reserved5d;
+	uint16_t mr0;
+	uint16_t mr1;
+	uint16_t mr2;
+	uint16_t mr3;
+	uint16_t mr4;
+	uint16_t mr5;
+	uint16_t mr6;
+	uint8_t  x16present;
+	uint8_t  cs_setup_gddec;
+	uint16_t rtt_nom_wr_park0;
+	uint16_t rtt_nom_wr_park1;
+	uint16_t rtt_nom_wr_park2;
+	uint16_t rtt_nom_wr_park3;
+	uint16_t rtt_nom_wr_park4;
+	uint16_t rtt_nom_wr_park5;
+	uint16_t rtt_nom_wr_park6;
+	uint16_t rtt_nom_wr_park7;
+	uint8_t  acsm_odt_ctrl0;
+	uint8_t  acsm_odt_ctrl1;
+	uint8_t  acsm_odt_ctrl2;
+	uint8_t  acsm_odt_ctrl3;
+	uint8_t  acsm_odt_ctrl4;
+	uint8_t  acsm_odt_ctrl5;
+	uint8_t  acsm_odt_ctrl6;
+	uint8_t  acsm_odt_ctrl7;
+	uint8_t  vref_dq_r0nib0;
+	uint8_t  vref_dq_r0nib1;
+	uint8_t  vref_dq_r0nib2;
+	uint8_t  vref_dq_r0nib3;
+	uint8_t  vref_dq_r0nib4;
+	uint8_t  vref_dq_r0nib5;
+	uint8_t  vref_dq_r0nib6;
+	uint8_t  vref_dq_r0nib7;
+	uint8_t  vref_dq_r0nib8;
+	uint8_t  vref_dq_r0nib9;
+	uint8_t  vref_dq_r0nib10;
+	uint8_t  vref_dq_r0nib11;
+	uint8_t  vref_dq_r0nib12;
+	uint8_t  vref_dq_r0nib13;
+	uint8_t  vref_dq_r0nib14;
+	uint8_t  vref_dq_r0nib15;
+	uint8_t  vref_dq_r0nib16;
+	uint8_t  vref_dq_r0nib17;
+	uint8_t  vref_dq_r0nib18;
+	uint8_t  vref_dq_r0nib19;
+	uint8_t  vref_dq_r1nib0;
+	uint8_t  vref_dq_r1nib1;
+	uint8_t  vref_dq_r1nib2;
+	uint8_t  vref_dq_r1nib3;
+	uint8_t  vref_dq_r1nib4;
+	uint8_t  vref_dq_r1nib5;
+	uint8_t  vref_dq_r1nib6;
+	uint8_t  vref_dq_r1nib7;
+	uint8_t  vref_dq_r1nib8;
+	uint8_t  vref_dq_r1nib9;
+	uint8_t  vref_dq_r1nib10;
+	uint8_t  vref_dq_r1nib11;
+	uint8_t  vref_dq_r1nib12;
+	uint8_t  vref_dq_r1nib13;
+	uint8_t  vref_dq_r1nib14;
+	uint8_t  vref_dq_r1nib15;
+	uint8_t  vref_dq_r1nib16;
+	uint8_t  vref_dq_r1nib17;
+	uint8_t  vref_dq_r1nib18;
+	uint8_t  vref_dq_r1nib19;
+	uint8_t  vref_dq_r2nib0;
+	uint8_t  vref_dq_r2nib1;
+	uint8_t  vref_dq_r2nib2;
+	uint8_t  vref_dq_r2nib3;
+	uint8_t  vref_dq_r2nib4;
+	uint8_t  vref_dq_r2nib5;
+	uint8_t  vref_dq_r2nib6;
+	uint8_t  vref_dq_r2nib7;
+	uint8_t  vref_dq_r2nib8;
+	uint8_t  vref_dq_r2nib9;
+	uint8_t  vref_dq_r2nib10;
+	uint8_t  vref_dq_r2nib11;
+	uint8_t  vref_dq_r2nib12;
+	uint8_t  vref_dq_r2nib13;
+	uint8_t  vref_dq_r2nib14;
+	uint8_t  vref_dq_r2nib15;
+	uint8_t  vref_dq_r2nib16;
+	uint8_t  vref_dq_r2nib17;
+	uint8_t  vref_dq_r2nib18;
+	uint8_t  vref_dq_r2nib19;
+	uint8_t  vref_dq_r3nib0;
+	uint8_t  vref_dq_r3nib1;
+	uint8_t  vref_dq_r3nib2;
+	uint8_t  vref_dq_r3nib3;
+	uint8_t  vref_dq_r3nib4;
+	uint8_t  vref_dq_r3nib5;
+	uint8_t  vref_dq_r3nib6;
+	uint8_t  vref_dq_r3nib7;
+	uint8_t  vref_dq_r3nib8;
+	uint8_t  vref_dq_r3nib9;
+	uint8_t  vref_dq_r3nib10;
+	uint8_t  vref_dq_r3nib11;
+	uint8_t  vref_dq_r3nib12;
+	uint8_t  vref_dq_r3nib13;
+	uint8_t  vref_dq_r3nib14;
+	uint8_t  vref_dq_r3nib15;
+	uint8_t  vref_dq_r3nib16;
+	uint8_t  vref_dq_r3nib17;
+	uint8_t  vref_dq_r3nib18;
+	uint8_t  vref_dq_r3nib19;
+	uint8_t  reserved_d6[0x3f6 - 0xd6];
+	uint16_t alt_cas_l;
+	uint8_t  alt_wcas_l;
+	uint8_t  d4misc;
+} __packed;
+
+struct ddr4u2d {
+	uint8_t  reserved00;
+	uint8_t  msg_misc;
+	uint16_t pmu_revision;
+	uint8_t  pstate;
+	uint8_t  pll_bypass_en;
+	uint16_t dramfreq;
+	uint8_t  dfi_freq_ratio;
+	uint8_t  bpznres_val;
+	uint8_t  phy_odt_impedance;
+	uint8_t  phy_drv_impedance;
+	uint8_t  phy_vref;
+	uint8_t  dram_type;
+	uint8_t  disabled_dbyte;
+	uint8_t  enabled_dqs;
+	uint8_t  cs_present;
+	uint8_t  cs_present_d0;
+	uint8_t  cs_present_d1;
+	uint8_t  addr_mirror;
+	uint8_t  cs_test_fail;
+	uint8_t  phy_cfg;
+	uint16_t sequence_ctrl;
+	uint8_t  hdt_ctrl;
+	uint8_t  rx2d_train_opt;
+	uint8_t  tx2d_train_opt;
+	uint8_t  share2dvref_result;
+	uint8_t  delay_weight2d;
+	uint8_t  voltage_weight2d;
+	uint8_t  reserved1e[0x22 - 0x1e];
+	uint16_t phy_config_override;
+	uint8_t  dfimrlmargin;
+	uint8_t  r0_rx_clk_dly_margin;
+	uint8_t  r0_vref_dac_margin;
+	uint8_t  r0_tx_dq_dly_margin;
+	uint8_t  r0_device_vref_margin;
+	uint8_t  reserved29[0x33 - 0x29];
+	uint8_t  r1_rx_clk_dly_margin;
+	uint8_t  r1_vref_dac_margin;
+	uint8_t  r1_tx_dq_dly_margin;
+	uint8_t  r1_device_vref_margin;
+	uint8_t  reserved37[0x41 - 0x37];
+	uint8_t  r2_rx_clk_dly_margin;
+	uint8_t  r2_vref_dac_margin;
+	uint8_t  r2_tx_dq_dly_margin;
+	uint8_t  r2_device_vref_margin;
+	uint8_t  reserved45[0x4f - 0x45];
+	uint8_t  r3_rx_clk_dly_margin;
+	uint8_t  r3_vref_dac_margin;
+	uint8_t  r3_tx_dq_dly_margin;
+	uint8_t  r3_device_vref_margin;
+	uint8_t  reserved53[0x5e - 0x53];
+	uint16_t mr0;
+	uint16_t mr1;
+	uint16_t mr2;
+	uint16_t mr3;
+	uint16_t mr4;
+	uint16_t mr5;
+	uint16_t mr6;
+	uint8_t  x16present;
+	uint8_t  cs_setup_gddec;
+	uint16_t rtt_nom_wr_park0;
+	uint16_t rtt_nom_wr_park1;
+	uint16_t rtt_nom_wr_park2;
+	uint16_t rtt_nom_wr_park3;
+	uint16_t rtt_nom_wr_park4;
+	uint16_t rtt_nom_wr_park5;
+	uint16_t rtt_nom_wr_park6;
+	uint16_t rtt_nom_wr_park7;
+	uint8_t  acsm_odt_ctrl0;
+	uint8_t  acsm_odt_ctrl1;
+	uint8_t  acsm_odt_ctrl2;
+	uint8_t  acsm_odt_ctrl3;
+	uint8_t  acsm_odt_ctrl4;
+	uint8_t  acsm_odt_ctrl5;
+	uint8_t  acsm_odt_ctrl6;
+	uint8_t  acsm_odt_ctrl7;
+	uint8_t  vref_dq_r0nib0;
+	uint8_t  vref_dq_r0nib1;
+	uint8_t  vref_dq_r0nib2;
+	uint8_t  vref_dq_r0nib3;
+	uint8_t  vref_dq_r0nib4;
+	uint8_t  vref_dq_r0nib5;
+	uint8_t  vref_dq_r0nib6;
+	uint8_t  vref_dq_r0nib7;
+	uint8_t  vref_dq_r0nib8;
+	uint8_t  vref_dq_r0nib9;
+	uint8_t  vref_dq_r0nib10;
+	uint8_t  vref_dq_r0nib11;
+	uint8_t  vref_dq_r0nib12;
+	uint8_t  vref_dq_r0nib13;
+	uint8_t  vref_dq_r0nib14;
+	uint8_t  vref_dq_r0nib15;
+	uint8_t  vref_dq_r0nib16;
+	uint8_t  vref_dq_r0nib17;
+	uint8_t  vref_dq_r0nib18;
+	uint8_t  vref_dq_r0nib19;
+	uint8_t  vref_dq_r1nib0;
+	uint8_t  vref_dq_r1nib1;
+	uint8_t  vref_dq_r1nib2;
+	uint8_t  vref_dq_r1nib3;
+	uint8_t  vref_dq_r1nib4;
+	uint8_t  vref_dq_r1nib5;
+	uint8_t  vref_dq_r1nib6;
+	uint8_t  vref_dq_r1nib7;
+	uint8_t  vref_dq_r1nib8;
+	uint8_t  vref_dq_r1nib9;
+	uint8_t  vref_dq_r1nib10;
+	uint8_t  vref_dq_r1nib11;
+	uint8_t  vref_dq_r1nib12;
+	uint8_t  vref_dq_r1nib13;
+	uint8_t  vref_dq_r1nib14;
+	uint8_t  vref_dq_r1nib15;
+	uint8_t  vref_dq_r1nib16;
+	uint8_t  vref_dq_r1nib17;
+	uint8_t  vref_dq_r1nib18;
+	uint8_t  vref_dq_r1nib19;
+	uint8_t  vref_dq_r2nib0;
+	uint8_t  vref_dq_r2nib1;
+	uint8_t  vref_dq_r2nib2;
+	uint8_t  vref_dq_r2nib3;
+	uint8_t  vref_dq_r2nib4;
+	uint8_t  vref_dq_r2nib5;
+	uint8_t  vref_dq_r2nib6;
+	uint8_t  vref_dq_r2nib7;
+	uint8_t  vref_dq_r2nib8;
+	uint8_t  vref_dq_r2nib9;
+	uint8_t  vref_dq_r2nib10;
+	uint8_t  vref_dq_r2nib11;
+	uint8_t  vref_dq_r2nib12;
+	uint8_t  vref_dq_r2nib13;
+	uint8_t  vref_dq_r2nib14;
+	uint8_t  vref_dq_r2nib15;
+	uint8_t  vref_dq_r2nib16;
+	uint8_t  vref_dq_r2nib17;
+	uint8_t  vref_dq_r2nib18;
+	uint8_t  vref_dq_r2nib19;
+	uint8_t  vref_dq_r3nib0;
+	uint8_t  vref_dq_r3nib1;
+	uint8_t  vref_dq_r3nib2;
+	uint8_t  vref_dq_r3nib3;
+	uint8_t  vref_dq_r3nib4;
+	uint8_t  vref_dq_r3nib5;
+	uint8_t  vref_dq_r3nib6;
+	uint8_t  vref_dq_r3nib7;
+	uint8_t  vref_dq_r3nib8;
+	uint8_t  vref_dq_r3nib9;
+	uint8_t  vref_dq_r3nib10;
+	uint8_t  vref_dq_r3nib11;
+	uint8_t  vref_dq_r3nib12;
+	uint8_t  vref_dq_r3nib13;
+	uint8_t  vref_dq_r3nib14;
+	uint8_t  vref_dq_r3nib15;
+	uint8_t  vref_dq_r3nib16;
+	uint8_t  vref_dq_r3nib17;
+	uint8_t  vref_dq_r3nib18;
+	uint8_t  vref_dq_r3nib19;
+	uint8_t  reserved_d6[0x3f6 - 0xd6];
+	uint16_t alt_cas_l;
+	uint8_t  alt_wcas_l;
+	uint8_t  d4misc;
+} __packed;
+
+struct ddr4r1d {
+	uint8_t  reserved00;
+	uint8_t  msg_misc;
+	uint16_t pmu_revision;
+	uint8_t  pstate;
+	uint8_t  pll_bypass_en;
+	uint16_t dramfreq;
+	uint8_t  dfi_freq_ratio;
+	uint8_t  bpznres_val;
+	uint8_t  phy_odt_impedance;
+	uint8_t  phy_drv_impedance;
+	uint8_t  phy_vref;
+	uint8_t  dram_type;
+	uint8_t  disabled_dbyte;
+	uint8_t  enabled_dqs;
+	uint8_t  cs_present;
+	uint8_t  cs_present_d0;
+	uint8_t  cs_present_d1;
+	uint8_t  addr_mirror;
+	uint8_t  cs_test_fail;
+	uint8_t  phy_cfg;
+	uint16_t sequence_ctrl;
+	uint8_t  hdt_ctrl;
+	uint8_t  reserved19[0x22 - 0x19];
+	uint16_t phy_config_override;
+	uint8_t  dfimrlmargin;
+	int8_t   cdd_rr_3_2;
+	int8_t   cdd_rr_3_1;
+	int8_t   cdd_rr_3_0;
+	int8_t   cdd_rr_2_3;
+	int8_t   cdd_rr_2_1;
+	int8_t   cdd_rr_2_0;
+	int8_t   cdd_rr_1_3;
+	int8_t   cdd_rr_1_2;
+	int8_t   cdd_rr_1_0;
+	int8_t   cdd_rr_0_3;
+	int8_t   cdd_rr_0_2;
+	int8_t   cdd_rr_0_1;
+	int8_t   cdd_ww_3_2;
+	int8_t   cdd_ww_3_1;
+	int8_t   cdd_ww_3_0;
+	int8_t   cdd_ww_2_3;
+	int8_t   cdd_ww_2_1;
+	int8_t   cdd_ww_2_0;
+	int8_t   cdd_ww_1_3;
+	int8_t   cdd_ww_1_2;
+	int8_t   cdd_ww_1_0;
+	int8_t   cdd_ww_0_3;
+	int8_t   cdd_ww_0_2;
+	int8_t   cdd_ww_0_1;
+	int8_t   cdd_rw_3_3;
+	int8_t   cdd_rw_3_2;
+	int8_t   cdd_rw_3_1;
+	int8_t   cdd_rw_3_0;
+	int8_t   cdd_rw_2_3;
+	int8_t   cdd_rw_2_2;
+	int8_t   cdd_rw_2_1;
+	int8_t   cdd_rw_2_0;
+	int8_t   cdd_rw_1_3;
+	int8_t   cdd_rw_1_2;
+	int8_t   cdd_rw_1_1;
+	int8_t   cdd_rw_1_0;
+	int8_t   cdd_rw_0_3;
+	int8_t   cdd_rw_0_2;
+	int8_t   cdd_rw_0_1;
+	int8_t   cdd_rw_0_0;
+	int8_t   cdd_wr_3_3;
+	int8_t   cdd_wr_3_2;
+	int8_t   cdd_wr_3_1;
+	int8_t   cdd_wr_3_0;
+	int8_t   cdd_wr_2_3;
+	int8_t   cdd_wr_2_2;
+	int8_t   cdd_wr_2_1;
+	int8_t   cdd_wr_2_0;
+	int8_t   cdd_wr_1_3;
+	int8_t   cdd_wr_1_2;
+	int8_t   cdd_wr_1_1;
+	int8_t   cdd_wr_1_0;
+	int8_t   cdd_wr_0_3;
+	int8_t   cdd_wr_0_2;
+	int8_t   cdd_wr_0_1;
+	int8_t   cdd_wr_0_0;
+	uint8_t  reserved5d;
+	uint16_t mr0;
+	uint16_t mr1;
+	uint16_t mr2;
+	uint16_t mr3;
+	uint16_t mr4;
+	uint16_t mr5;
+	uint16_t mr6;
+	uint8_t  x16present;
+	uint8_t  cs_setup_gddec;
+	uint16_t rtt_nom_wr_park0;
+	uint16_t rtt_nom_wr_park1;
+	uint16_t rtt_nom_wr_park2;
+	uint16_t rtt_nom_wr_park3;
+	uint16_t rtt_nom_wr_park4;
+	uint16_t rtt_nom_wr_park5;
+	uint16_t rtt_nom_wr_park6;
+	uint16_t rtt_nom_wr_park7;
+	uint8_t  acsm_odt_ctrl0;
+	uint8_t  acsm_odt_ctrl1;
+	uint8_t  acsm_odt_ctrl2;
+	uint8_t  acsm_odt_ctrl3;
+	uint8_t  acsm_odt_ctrl4;
+	uint8_t  acsm_odt_ctrl5;
+	uint8_t  acsm_odt_ctrl6;
+	uint8_t  acsm_odt_ctrl7;
+	uint8_t  vref_dq_r0nib0;
+	uint8_t  vref_dq_r0nib1;
+	uint8_t  vref_dq_r0nib2;
+	uint8_t  vref_dq_r0nib3;
+	uint8_t  vref_dq_r0nib4;
+	uint8_t  vref_dq_r0nib5;
+	uint8_t  vref_dq_r0nib6;
+	uint8_t  vref_dq_r0nib7;
+	uint8_t  vref_dq_r0nib8;
+	uint8_t  vref_dq_r0nib9;
+	uint8_t  vref_dq_r0nib10;
+	uint8_t  vref_dq_r0nib11;
+	uint8_t  vref_dq_r0nib12;
+	uint8_t  vref_dq_r0nib13;
+	uint8_t  vref_dq_r0nib14;
+	uint8_t  vref_dq_r0nib15;
+	uint8_t  vref_dq_r0nib16;
+	uint8_t  vref_dq_r0nib17;
+	uint8_t  vref_dq_r0nib18;
+	uint8_t  vref_dq_r0nib19;
+	uint8_t  vref_dq_r1nib0;
+	uint8_t  vref_dq_r1nib1;
+	uint8_t  vref_dq_r1nib2;
+	uint8_t  vref_dq_r1nib3;
+	uint8_t  vref_dq_r1nib4;
+	uint8_t  vref_dq_r1nib5;
+	uint8_t  vref_dq_r1nib6;
+	uint8_t  vref_dq_r1nib7;
+	uint8_t  vref_dq_r1nib8;
+	uint8_t  vref_dq_r1nib9;
+	uint8_t  vref_dq_r1nib10;
+	uint8_t  vref_dq_r1nib11;
+	uint8_t  vref_dq_r1nib12;
+	uint8_t  vref_dq_r1nib13;
+	uint8_t  vref_dq_r1nib14;
+	uint8_t  vref_dq_r1nib15;
+	uint8_t  vref_dq_r1nib16;
+	uint8_t  vref_dq_r1nib17;
+	uint8_t  vref_dq_r1nib18;
+	uint8_t  vref_dq_r1nib19;
+	uint8_t  vref_dq_r2nib0;
+	uint8_t  vref_dq_r2nib1;
+	uint8_t  vref_dq_r2nib2;
+	uint8_t  vref_dq_r2nib3;
+	uint8_t  vref_dq_r2nib4;
+	uint8_t  vref_dq_r2nib5;
+	uint8_t  vref_dq_r2nib6;
+	uint8_t  vref_dq_r2nib7;
+	uint8_t  vref_dq_r2nib8;
+	uint8_t  vref_dq_r2nib9;
+	uint8_t  vref_dq_r2nib10;
+	uint8_t  vref_dq_r2nib11;
+	uint8_t  vref_dq_r2nib12;
+	uint8_t  vref_dq_r2nib13;
+	uint8_t  vref_dq_r2nib14;
+	uint8_t  vref_dq_r2nib15;
+	uint8_t  vref_dq_r2nib16;
+	uint8_t  vref_dq_r2nib17;
+	uint8_t  vref_dq_r2nib18;
+	uint8_t  vref_dq_r2nib19;
+	uint8_t  vref_dq_r3nib0;
+	uint8_t  vref_dq_r3nib1;
+	uint8_t  vref_dq_r3nib2;
+	uint8_t  vref_dq_r3nib3;
+	uint8_t  vref_dq_r3nib4;
+	uint8_t  vref_dq_r3nib5;
+	uint8_t  vref_dq_r3nib6;
+	uint8_t  vref_dq_r3nib7;
+	uint8_t  vref_dq_r3nib8;
+	uint8_t  vref_dq_r3nib9;
+	uint8_t  vref_dq_r3nib10;
+	uint8_t  vref_dq_r3nib11;
+	uint8_t  vref_dq_r3nib12;
+	uint8_t  vref_dq_r3nib13;
+	uint8_t  vref_dq_r3nib14;
+	uint8_t  vref_dq_r3nib15;
+	uint8_t  vref_dq_r3nib16;
+	uint8_t  vref_dq_r3nib17;
+	uint8_t  vref_dq_r3nib18;
+	uint8_t  vref_dq_r3nib19;
+	uint8_t  f0rc00_d0;
+	uint8_t  f0rc01_d0;
+	uint8_t  f0rc02_d0;
+	uint8_t  f0rc03_d0;
+	uint8_t  f0rc04_d0;
+	uint8_t  f0rc05_d0;
+	uint8_t  f0rc06_d0;
+	uint8_t  f0rc07_d0;
+	uint8_t  f0rc08_d0;
+	uint8_t  f0rc09_d0;
+	uint8_t  f0rc0a_d0;
+	uint8_t  f0rc0b_d0;
+	uint8_t  f0rc0c_d0;
+	uint8_t  f0rc0d_d0;
+	uint8_t  f0rc0e_d0;
+	uint8_t  f0rc0f_d0;
+	uint8_t  f0rc1x_d0;
+	uint8_t  f0rc2x_d0;
+	uint8_t  f0rc3x_d0;
+	uint8_t  f0rc4x_d0;
+	uint8_t  f0rc5x_d0;
+	uint8_t  f0rc6x_d0;
+	uint8_t  f0rc7x_d0;
+	uint8_t  f0rc8x_d0;
+	uint8_t  f0rc9x_d0;
+	uint8_t  f0rcax_d0;
+	uint8_t  f0rcbx_d0;
+	uint8_t  f1rc00_d0;
+	uint8_t  f1rc01_d0;
+	uint8_t  f1rc02_d0;
+	uint8_t  f1rc03_d0;
+	uint8_t  f1rc04_d0;
+	uint8_t  f1rc05_d0;
+	uint8_t  f1rc06_d0;
+	uint8_t  f1rc07_d0;
+	uint8_t  f1rc08_d0;
+	uint8_t  f1rc09_d0;
+	uint8_t  f1rc0a_d0;
+	uint8_t  f1rc0b_d0;
+	uint8_t  f1rc0c_d0;
+	uint8_t  f1rc0d_d0;
+	uint8_t  f1rc0e_d0;
+	uint8_t  f1rc0f_d0;
+	uint8_t  f1rc1x_d0;
+	uint8_t  f1rc2x_d0;
+	uint8_t  f1rc3x_d0;
+	uint8_t  f1rc4x_d0;
+	uint8_t  f1rc5x_d0;
+	uint8_t  f1rc6x_d0;
+	uint8_t  f1rc7x_d0;
+	uint8_t  f1rc8x_d0;
+	uint8_t  f1rc9x_d0;
+	uint8_t  f1rcax_d0;
+	uint8_t  f1rcbx_d0;
+	uint8_t  f0rc00_d1;
+	uint8_t  f0rc01_d1;
+	uint8_t  f0rc02_d1;
+	uint8_t  f0rc03_d1;
+	uint8_t  f0rc04_d1;
+	uint8_t  f0rc05_d1;
+	uint8_t  f0rc06_d1;
+	uint8_t  f0rc07_d1;
+	uint8_t  f0rc08_d1;
+	uint8_t  f0rc09_d1;
+	uint8_t  f0rc0a_d1;
+	uint8_t  f0rc0b_d1;
+	uint8_t  f0rc0c_d1;
+	uint8_t  f0rc0d_d1;
+	uint8_t  f0rc0e_d1;
+	uint8_t  f0rc0f_d1;
+	uint8_t  f0rc1x_d1;
+	uint8_t  f0rc2x_d1;
+	uint8_t  f0rc3x_d1;
+	uint8_t  f0rc4x_d1;
+	uint8_t  f0rc5x_d1;
+	uint8_t  f0rc6x_d1;
+	uint8_t  f0rc7x_d1;
+	uint8_t  f0rc8x_d1;
+	uint8_t  f0rc9x_d1;
+	uint8_t  f0rcax_d1;
+	uint8_t  f0rcbx_d1;
+	uint8_t  f1rc00_d1;
+	uint8_t  f1rc01_d1;
+	uint8_t  f1rc02_d1;
+	uint8_t  f1rc03_d1;
+	uint8_t  f1rc04_d1;
+	uint8_t  f1rc05_d1;
+	uint8_t  f1rc06_d1;
+	uint8_t  f1rc07_d1;
+	uint8_t  f1rc08_d1;
+	uint8_t  f1rc09_d1;
+	uint8_t  f1rc0a_d1;
+	uint8_t  f1rc0b_d1;
+	uint8_t  f1rc0c_d1;
+	uint8_t  f1rc0d_d1;
+	uint8_t  f1rc0e_d1;
+	uint8_t  f1rc0f_d1;
+	uint8_t  f1rc1x_d1;
+	uint8_t  f1rc2x_d1;
+	uint8_t  f1rc3x_d1;
+	uint8_t  f1rc4x_d1;
+	uint8_t  f1rc5x_d1;
+	uint8_t  f1rc6x_d1;
+	uint8_t  f1rc7x_d1;
+	uint8_t  f1rc8x_d1;
+	uint8_t  f1rc9x_d1;
+	uint8_t  f1rcax_d1;
+	uint8_t  f1rcbx_d1;
+	uint8_t  reserved142[0x3f6 - 0x142];
+	uint16_t alt_cas_l;
+	uint8_t  alt_wcas_l;
+	uint8_t  d4misc;
+} __packed;
+
+struct ddr4r2d {
+	uint8_t  reserved00;
+	uint8_t  msg_misc;
+	uint16_t pmu_revision;
+	uint8_t  pstate;
+	uint8_t  pll_bypass_en;
+	uint16_t dramfreq;
+	uint8_t  dfi_freq_ratio;
+	uint8_t  bpznres_val;
+	uint8_t  phy_odt_impedance;
+	uint8_t  phy_drv_impedance;
+	uint8_t  phy_vref;
+	uint8_t  dram_type;
+	uint8_t  disabled_dbyte;
+	uint8_t  enabled_dqs;
+	uint8_t  cs_present;
+	uint8_t  cs_present_d0;
+	uint8_t  cs_present_d1;
+	uint8_t  addr_mirror;
+	uint8_t  cs_test_fail;
+	uint8_t  phy_cfg;
+	uint16_t sequence_ctrl;
+	uint8_t  hdt_ctrl;
+	uint8_t  rx2d_train_opt;
+	uint8_t  tx2d_train_opt;
+	uint8_t  share2dvref_result;
+	uint8_t  delay_weight2d;
+	uint8_t  voltage_weight2d;
+	uint8_t  reserved1e[0x22-0x1e];
+	uint16_t phy_config_override;
+	uint8_t  dfimrlmargin;
+	uint8_t  r0_rx_clk_dly_margin;
+	uint8_t  r0_vref_dac_margin;
+	uint8_t  r0_tx_dq_dly_margin;
+	uint8_t  r0_device_vref_margin;
+	uint8_t  reserved29[0x33-0x29];
+	uint8_t  r1_rx_clk_dly_margin;
+	uint8_t  r1_vref_dac_margin;
+	uint8_t  r1_tx_dq_dly_margin;
+	uint8_t  r1_device_vref_margin;
+	uint8_t  reserved37[0x41-0x37];
+	uint8_t  r2_rx_clk_dly_margin;
+	uint8_t  r2_vref_dac_margin;
+	uint8_t  r2_tx_dq_dly_margin;
+	uint8_t  r2_device_vref_margin;
+	uint8_t  reserved45[0x4f - 0x45];
+	uint8_t  r3_rx_clk_dly_margin;
+	uint8_t  r3_vref_dac_margin;
+	uint8_t  r3_tx_dq_dly_margin;
+	uint8_t  r3_device_vref_margin;
+	uint8_t  reserved53[0x5e - 0x53];
+	uint16_t mr0;
+	uint16_t mr1;
+	uint16_t mr2;
+	uint16_t mr3;
+	uint16_t mr4;
+	uint16_t mr5;
+	uint16_t mr6;
+	uint8_t  x16present;
+	uint8_t  cs_setup_gddec;
+	uint16_t rtt_nom_wr_park0;
+	uint16_t rtt_nom_wr_park1;
+	uint16_t rtt_nom_wr_park2;
+	uint16_t rtt_nom_wr_park3;
+	uint16_t rtt_nom_wr_park4;
+	uint16_t rtt_nom_wr_park5;
+	uint16_t rtt_nom_wr_park6;
+	uint16_t rtt_nom_wr_park7;
+	uint8_t  acsm_odt_ctrl0;
+	uint8_t  acsm_odt_ctrl1;
+	uint8_t  acsm_odt_ctrl2;
+	uint8_t  acsm_odt_ctrl3;
+	uint8_t  acsm_odt_ctrl4;
+	uint8_t  acsm_odt_ctrl5;
+	uint8_t  acsm_odt_ctrl6;
+	uint8_t  acsm_odt_ctrl7;
+	uint8_t  vref_dq_r0nib0;
+	uint8_t  vref_dq_r0nib1;
+	uint8_t  vref_dq_r0nib2;
+	uint8_t  vref_dq_r0nib3;
+	uint8_t  vref_dq_r0nib4;
+	uint8_t  vref_dq_r0nib5;
+	uint8_t  vref_dq_r0nib6;
+	uint8_t  vref_dq_r0nib7;
+	uint8_t  vref_dq_r0nib8;
+	uint8_t  vref_dq_r0nib9;
+	uint8_t  vref_dq_r0nib10;
+	uint8_t  vref_dq_r0nib11;
+	uint8_t  vref_dq_r0nib12;
+	uint8_t  vref_dq_r0nib13;
+	uint8_t  vref_dq_r0nib14;
+	uint8_t  vref_dq_r0nib15;
+	uint8_t  vref_dq_r0nib16;
+	uint8_t  vref_dq_r0nib17;
+	uint8_t  vref_dq_r0nib18;
+	uint8_t  vref_dq_r0nib19;
+	uint8_t  vref_dq_r1nib0;
+	uint8_t  vref_dq_r1nib1;
+	uint8_t  vref_dq_r1nib2;
+	uint8_t  vref_dq_r1nib3;
+	uint8_t  vref_dq_r1nib4;
+	uint8_t  vref_dq_r1nib5;
+	uint8_t  vref_dq_r1nib6;
+	uint8_t  vref_dq_r1nib7;
+	uint8_t  vref_dq_r1nib8;
+	uint8_t  vref_dq_r1nib9;
+	uint8_t  vref_dq_r1nib10;
+	uint8_t  vref_dq_r1nib11;
+	uint8_t  vref_dq_r1nib12;
+	uint8_t  vref_dq_r1nib13;
+	uint8_t  vref_dq_r1nib14;
+	uint8_t  vref_dq_r1nib15;
+	uint8_t  vref_dq_r1nib16;
+	uint8_t  vref_dq_r1nib17;
+	uint8_t  vref_dq_r1nib18;
+	uint8_t  vref_dq_r1nib19;
+	uint8_t  vref_dq_r2nib0;
+	uint8_t  vref_dq_r2nib1;
+	uint8_t  vref_dq_r2nib2;
+	uint8_t  vref_dq_r2nib3;
+	uint8_t  vref_dq_r2nib4;
+	uint8_t  vref_dq_r2nib5;
+	uint8_t  vref_dq_r2nib6;
+	uint8_t  vref_dq_r2nib7;
+	uint8_t  vref_dq_r2nib8;
+	uint8_t  vref_dq_r2nib9;
+	uint8_t  vref_dq_r2nib10;
+	uint8_t  vref_dq_r2nib11;
+	uint8_t  vref_dq_r2nib12;
+	uint8_t  vref_dq_r2nib13;
+	uint8_t  vref_dq_r2nib14;
+	uint8_t  vref_dq_r2nib15;
+	uint8_t  vref_dq_r2nib16;
+	uint8_t  vref_dq_r2nib17;
+	uint8_t  vref_dq_r2nib18;
+	uint8_t  vref_dq_r2nib19;
+	uint8_t  vref_dq_r3nib0;
+	uint8_t  vref_dq_r3nib1;
+	uint8_t  vref_dq_r3nib2;
+	uint8_t  vref_dq_r3nib3;
+	uint8_t  vref_dq_r3nib4;
+	uint8_t  vref_dq_r3nib5;
+	uint8_t  vref_dq_r3nib6;
+	uint8_t  vref_dq_r3nib7;
+	uint8_t  vref_dq_r3nib8;
+	uint8_t  vref_dq_r3nib9;
+	uint8_t  vref_dq_r3nib10;
+	uint8_t  vref_dq_r3nib11;
+	uint8_t  vref_dq_r3nib12;
+	uint8_t  vref_dq_r3nib13;
+	uint8_t  vref_dq_r3nib14;
+	uint8_t  vref_dq_r3nib15;
+	uint8_t  vref_dq_r3nib16;
+	uint8_t  vref_dq_r3nib17;
+	uint8_t  vref_dq_r3nib18;
+	uint8_t  vref_dq_r3nib19;
+	uint8_t  f0rc00_d0;
+	uint8_t  f0rc01_d0;
+	uint8_t  f0rc02_d0;
+	uint8_t  f0rc03_d0;
+	uint8_t  f0rc04_d0;
+	uint8_t  f0rc05_d0;
+	uint8_t  f0rc06_d0;
+	uint8_t  f0rc07_d0;
+	uint8_t  f0rc08_d0;
+	uint8_t  f0rc09_d0;
+	uint8_t  f0rc0a_d0;
+	uint8_t  f0rc0b_d0;
+	uint8_t  f0rc0c_d0;
+	uint8_t  f0rc0d_d0;
+	uint8_t  f0rc0e_d0;
+	uint8_t  f0rc0f_d0;
+	uint8_t  f0rc1x_d0;
+	uint8_t  f0rc2x_d0;
+	uint8_t  f0rc3x_d0;
+	uint8_t  f0rc4x_d0;
+	uint8_t  f0rc5x_d0;
+	uint8_t  f0rc6x_d0;
+	uint8_t  f0rc7x_d0;
+	uint8_t  f0rc8x_d0;
+	uint8_t  f0rc9x_d0;
+	uint8_t  f0rcax_d0;
+	uint8_t  f0rcbx_d0;
+	uint8_t  f1rc00_d0;
+	uint8_t  f1rc01_d0;
+	uint8_t  f1rc02_d0;
+	uint8_t  f1rc03_d0;
+	uint8_t  f1rc04_d0;
+	uint8_t  f1rc05_d0;
+	uint8_t  f1rc06_d0;
+	uint8_t  f1rc07_d0;
+	uint8_t  f1rc08_d0;
+	uint8_t  f1rc09_d0;
+	uint8_t  f1rc0a_d0;
+	uint8_t  f1rc0b_d0;
+	uint8_t  f1rc0c_d0;
+	uint8_t  f1rc0d_d0;
+	uint8_t  f1rc0e_d0;
+	uint8_t  f1rc0f_d0;
+	uint8_t  f1rc1x_d0;
+	uint8_t  f1rc2x_d0;
+	uint8_t  f1rc3x_d0;
+	uint8_t  f1rc4x_d0;
+	uint8_t  f1rc5x_d0;
+	uint8_t  f1rc6x_d0;
+	uint8_t  f1rc7x_d0;
+	uint8_t  f1rc8x_d0;
+	uint8_t  f1rc9x_d0;
+	uint8_t  f1rcax_d0;
+	uint8_t  f1rcbx_d0;
+	uint8_t  f0rc00_d1;
+	uint8_t  f0rc01_d1;
+	uint8_t  f0rc02_d1;
+	uint8_t  f0rc03_d1;
+	uint8_t  f0rc04_d1;
+	uint8_t  f0rc05_d1;
+	uint8_t  f0rc06_d1;
+	uint8_t  f0rc07_d1;
+	uint8_t  f0rc08_d1;
+	uint8_t  f0rc09_d1;
+	uint8_t  f0rc0a_d1;
+	uint8_t  f0rc0b_d1;
+	uint8_t  f0rc0c_d1;
+	uint8_t  f0rc0d_d1;
+	uint8_t  f0rc0e_d1;
+	uint8_t  f0rc0f_d1;
+	uint8_t  f0rc1x_d1;
+	uint8_t  f0rc2x_d1;
+	uint8_t  f0rc3x_d1;
+	uint8_t  f0rc4x_d1;
+	uint8_t  f0rc5x_d1;
+	uint8_t  f0rc6x_d1;
+	uint8_t  f0rc7x_d1;
+	uint8_t  f0rc8x_d1;
+	uint8_t  f0rc9x_d1;
+	uint8_t  f0rcax_d1;
+	uint8_t  f0rcbx_d1;
+	uint8_t  f1rc00_d1;
+	uint8_t  f1rc01_d1;
+	uint8_t  f1rc02_d1;
+	uint8_t  f1rc03_d1;
+	uint8_t  f1rc04_d1;
+	uint8_t  f1rc05_d1;
+	uint8_t  f1rc06_d1;
+	uint8_t  f1rc07_d1;
+	uint8_t  f1rc08_d1;
+	uint8_t  f1rc09_d1;
+	uint8_t  f1rc0a_d1;
+	uint8_t  f1rc0b_d1;
+	uint8_t  f1rc0c_d1;
+	uint8_t  f1rc0d_d1;
+	uint8_t  f1rc0e_d1;
+	uint8_t  f1rc0f_d1;
+	uint8_t  f1rc1x_d1;
+	uint8_t  f1rc2x_d1;
+	uint8_t  f1rc3x_d1;
+	uint8_t  f1rc4x_d1;
+	uint8_t  f1rc5x_d1;
+	uint8_t  f1rc6x_d1;
+	uint8_t  f1rc7x_d1;
+	uint8_t  f1rc8x_d1;
+	uint8_t  f1rc9x_d1;
+	uint8_t  f1rcax_d1;
+	uint8_t  f1rcbx_d1;
+	uint8_t  reserved142[0x3f6 - 0x142];
+	uint16_t alt_cas_l;
+	uint8_t  alt_wcas_l;
+	uint8_t  d4misc;
+} __packed;
+
+struct ddr4lr1d {
+	uint8_t  reserved00;
+	uint8_t  msg_misc;
+	uint16_t pmu_revision;
+	uint8_t  pstate;
+	uint8_t  pll_bypass_en;
+	uint16_t dramfreq;
+	uint8_t  dfi_freq_ratio;
+	uint8_t  bpznres_val;
+	uint8_t  phy_odt_impedance;
+	uint8_t  phy_drv_impedance;
+	uint8_t  phy_vref;
+	uint8_t  dram_type;
+	uint8_t  disabled_dbyte;
+	uint8_t  enabled_dqs;
+	uint8_t  cs_present;
+	uint8_t  cs_present_d0;
+	uint8_t  cs_present_d1;
+	uint8_t  addr_mirror;
+	uint8_t  cs_test_fail;
+	uint8_t  phy_cfg;
+	uint16_t sequence_ctrl;
+	uint8_t  hdt_ctrl;
+	uint8_t  reserved19[0x22 - 0x19];
+	uint16_t phy_config_override;
+	uint8_t  dfimrlmargin;
+	int8_t   cdd_rr_3_2;
+	int8_t   cdd_rr_3_1;
+	int8_t   cdd_rr_3_0;
+	int8_t   cdd_rr_2_3;
+	int8_t   cdd_rr_2_1;
+	int8_t   cdd_rr_2_0;
+	int8_t   cdd_rr_1_3;
+	int8_t   cdd_rr_1_2;
+	int8_t   cdd_rr_1_0;
+	int8_t   cdd_rr_0_3;
+	int8_t   cdd_rr_0_2;
+	int8_t   cdd_rr_0_1;
+	int8_t   cdd_ww_3_2;
+	int8_t   cdd_ww_3_1;
+	int8_t   cdd_ww_3_0;
+	int8_t   cdd_ww_2_3;
+	int8_t   cdd_ww_2_1;
+	int8_t   cdd_ww_2_0;
+	int8_t   cdd_ww_1_3;
+	int8_t   cdd_ww_1_2;
+	int8_t   cdd_ww_1_0;
+	int8_t   cdd_ww_0_3;
+	int8_t   cdd_ww_0_2;
+	int8_t   cdd_ww_0_1;
+	int8_t   cdd_rw_3_3;
+	int8_t   cdd_rw_3_2;
+	int8_t   cdd_rw_3_1;
+	int8_t   cdd_rw_3_0;
+	int8_t   cdd_rw_2_3;
+	int8_t   cdd_rw_2_2;
+	int8_t   cdd_rw_2_1;
+	int8_t   cdd_rw_2_0;
+	int8_t   cdd_rw_1_3;
+	int8_t   cdd_rw_1_2;
+	int8_t   cdd_rw_1_1;
+	int8_t   cdd_rw_1_0;
+	int8_t   cdd_rw_0_3;
+	int8_t   cdd_rw_0_2;
+	int8_t   cdd_rw_0_1;
+	int8_t   cdd_rw_0_0;
+	int8_t   cdd_wr_3_3;
+	int8_t   cdd_wr_3_2;
+	int8_t   cdd_wr_3_1;
+	int8_t   cdd_wr_3_0;
+	int8_t   cdd_wr_2_3;
+	int8_t   cdd_wr_2_2;
+	int8_t   cdd_wr_2_1;
+	int8_t   cdd_wr_2_0;
+	int8_t   cdd_wr_1_3;
+	int8_t   cdd_wr_1_2;
+	int8_t   cdd_wr_1_1;
+	int8_t   cdd_wr_1_0;
+	int8_t   cdd_wr_0_3;
+	int8_t   cdd_wr_0_2;
+	int8_t   cdd_wr_0_1;
+	int8_t   cdd_wr_0_0;
+	uint8_t  reserved5d;
+	uint16_t mr0;
+	uint16_t mr1;
+	uint16_t mr2;
+	uint16_t mr3;
+	uint16_t mr4;
+	uint16_t mr5;
+	uint16_t mr6;
+	uint8_t  x16present;
+	uint8_t  cs_setup_gddec;
+	uint16_t rtt_nom_wr_park0;
+	uint16_t rtt_nom_wr_park1;
+	uint16_t rtt_nom_wr_park2;
+	uint16_t rtt_nom_wr_park3;
+	uint16_t rtt_nom_wr_park4;
+	uint16_t rtt_nom_wr_park5;
+	uint16_t rtt_nom_wr_park6;
+	uint16_t rtt_nom_wr_park7;
+	uint8_t  acsm_odt_ctrl0;
+	uint8_t  acsm_odt_ctrl1;
+	uint8_t  acsm_odt_ctrl2;
+	uint8_t  acsm_odt_ctrl3;
+	uint8_t  acsm_odt_ctrl4;
+	uint8_t  acsm_odt_ctrl5;
+	uint8_t  acsm_odt_ctrl6;
+	uint8_t  acsm_odt_ctrl7;
+	uint8_t  vref_dq_r0nib0;
+	uint8_t  vref_dq_r0nib1;
+	uint8_t  vref_dq_r0nib2;
+	uint8_t  vref_dq_r0nib3;
+	uint8_t  vref_dq_r0nib4;
+	uint8_t  vref_dq_r0nib5;
+	uint8_t  vref_dq_r0nib6;
+	uint8_t  vref_dq_r0nib7;
+	uint8_t  vref_dq_r0nib8;
+	uint8_t  vref_dq_r0nib9;
+	uint8_t  vref_dq_r0nib10;
+	uint8_t  vref_dq_r0nib11;
+	uint8_t  vref_dq_r0nib12;
+	uint8_t  vref_dq_r0nib13;
+	uint8_t  vref_dq_r0nib14;
+	uint8_t  vref_dq_r0nib15;
+	uint8_t  vref_dq_r0nib16;
+	uint8_t  vref_dq_r0nib17;
+	uint8_t  vref_dq_r0nib18;
+	uint8_t  vref_dq_r0nib19;
+	uint8_t  vref_dq_r1nib0;
+	uint8_t  vref_dq_r1nib1;
+	uint8_t  vref_dq_r1nib2;
+	uint8_t  vref_dq_r1nib3;
+	uint8_t  vref_dq_r1nib4;
+	uint8_t  vref_dq_r1nib5;
+	uint8_t  vref_dq_r1nib6;
+	uint8_t  vref_dq_r1nib7;
+	uint8_t  vref_dq_r1nib8;
+	uint8_t  vref_dq_r1nib9;
+	uint8_t  vref_dq_r1nib10;
+	uint8_t  vref_dq_r1nib11;
+	uint8_t  vref_dq_r1nib12;
+	uint8_t  vref_dq_r1nib13;
+	uint8_t  vref_dq_r1nib14;
+	uint8_t  vref_dq_r1nib15;
+	uint8_t  vref_dq_r1nib16;
+	uint8_t  vref_dq_r1nib17;
+	uint8_t  vref_dq_r1nib18;
+	uint8_t  vref_dq_r1nib19;
+	uint8_t  vref_dq_r2nib0;
+	uint8_t  vref_dq_r2nib1;
+	uint8_t  vref_dq_r2nib2;
+	uint8_t  vref_dq_r2nib3;
+	uint8_t  vref_dq_r2nib4;
+	uint8_t  vref_dq_r2nib5;
+	uint8_t  vref_dq_r2nib6;
+	uint8_t  vref_dq_r2nib7;
+	uint8_t  vref_dq_r2nib8;
+	uint8_t  vref_dq_r2nib9;
+	uint8_t  vref_dq_r2nib10;
+	uint8_t  vref_dq_r2nib11;
+	uint8_t  vref_dq_r2nib12;
+	uint8_t  vref_dq_r2nib13;
+	uint8_t  vref_dq_r2nib14;
+	uint8_t  vref_dq_r2nib15;
+	uint8_t  vref_dq_r2nib16;
+	uint8_t  vref_dq_r2nib17;
+	uint8_t  vref_dq_r2nib18;
+	uint8_t  vref_dq_r2nib19;
+	uint8_t  vref_dq_r3nib0;
+	uint8_t  vref_dq_r3nib1;
+	uint8_t  vref_dq_r3nib2;
+	uint8_t  vref_dq_r3nib3;
+	uint8_t  vref_dq_r3nib4;
+	uint8_t  vref_dq_r3nib5;
+	uint8_t  vref_dq_r3nib6;
+	uint8_t  vref_dq_r3nib7;
+	uint8_t  vref_dq_r3nib8;
+	uint8_t  vref_dq_r3nib9;
+	uint8_t  vref_dq_r3nib10;
+	uint8_t  vref_dq_r3nib11;
+	uint8_t  vref_dq_r3nib12;
+	uint8_t  vref_dq_r3nib13;
+	uint8_t  vref_dq_r3nib14;
+	uint8_t  vref_dq_r3nib15;
+	uint8_t  vref_dq_r3nib16;
+	uint8_t  vref_dq_r3nib17;
+	uint8_t  vref_dq_r3nib18;
+	uint8_t  vref_dq_r3nib19;
+	uint8_t  f0rc00_d0;
+	uint8_t  f0rc01_d0;
+	uint8_t  f0rc02_d0;
+	uint8_t  f0rc03_d0;
+	uint8_t  f0rc04_d0;
+	uint8_t  f0rc05_d0;
+	uint8_t  f0rc06_d0;
+	uint8_t  f0rc07_d0;
+	uint8_t  f0rc08_d0;
+	uint8_t  f0rc09_d0;
+	uint8_t  f0rc0a_d0;
+	uint8_t  f0rc0b_d0;
+	uint8_t  f0rc0c_d0;
+	uint8_t  f0rc0d_d0;
+	uint8_t  f0rc0e_d0;
+	uint8_t  f0rc0f_d0;
+	uint8_t  f0rc1x_d0;
+	uint8_t  f0rc2x_d0;
+	uint8_t  f0rc3x_d0;
+	uint8_t  f0rc4x_d0;
+	uint8_t  f0rc5x_d0;
+	uint8_t  f0rc6x_d0;
+	uint8_t  f0rc7x_d0;
+	uint8_t  f0rc8x_d0;
+	uint8_t  f0rc9x_d0;
+	uint8_t  f0rcax_d0;
+	uint8_t  f0rcbx_d0;
+	uint8_t  f1rc00_d0;
+	uint8_t  f1rc01_d0;
+	uint8_t  f1rc02_d0;
+	uint8_t  f1rc03_d0;
+	uint8_t  f1rc04_d0;
+	uint8_t  f1rc05_d0;
+	uint8_t  f1rc06_d0;
+	uint8_t  f1rc07_d0;
+	uint8_t  f1rc08_d0;
+	uint8_t  f1rc09_d0;
+	uint8_t  f1rc0a_d0;
+	uint8_t  f1rc0b_d0;
+	uint8_t  f1rc0c_d0;
+	uint8_t  f1rc0d_d0;
+	uint8_t  f1rc0e_d0;
+	uint8_t  f1rc0f_d0;
+	uint8_t  f1rc1x_d0;
+	uint8_t  f1rc2x_d0;
+	uint8_t  f1rc3x_d0;
+	uint8_t  f1rc4x_d0;
+	uint8_t  f1rc5x_d0;
+	uint8_t  f1rc6x_d0;
+	uint8_t  f1rc7x_d0;
+	uint8_t  f1rc8x_d0;
+	uint8_t  f1rc9x_d0;
+	uint8_t  f1rcax_d0;
+	uint8_t  f1rcbx_d0;
+	uint8_t  f0rc00_d1;
+	uint8_t  f0rc01_d1;
+	uint8_t  f0rc02_d1;
+	uint8_t  f0rc03_d1;
+	uint8_t  f0rc04_d1;
+	uint8_t  f0rc05_d1;
+	uint8_t  f0rc06_d1;
+	uint8_t  f0rc07_d1;
+	uint8_t  f0rc08_d1;
+	uint8_t  f0rc09_d1;
+	uint8_t  f0rc0a_d1;
+	uint8_t  f0rc0b_d1;
+	uint8_t  f0rc0c_d1;
+	uint8_t  f0rc0d_d1;
+	uint8_t  f0rc0e_d1;
+	uint8_t  f0rc0f_d1;
+	uint8_t  f0rc1x_d1;
+	uint8_t  f0rc2x_d1;
+	uint8_t  f0rc3x_d1;
+	uint8_t  f0rc4x_d1;
+	uint8_t  f0rc5x_d1;
+	uint8_t  f0rc6x_d1;
+	uint8_t  f0rc7x_d1;
+	uint8_t  f0rc8x_d1;
+	uint8_t  f0rc9x_d1;
+	uint8_t  f0rcax_d1;
+	uint8_t  f0rcbx_d1;
+	uint8_t  f1rc00_d1;
+	uint8_t  f1rc01_d1;
+	uint8_t  f1rc02_d1;
+	uint8_t  f1rc03_d1;
+	uint8_t  f1rc04_d1;
+	uint8_t  f1rc05_d1;
+	uint8_t  f1rc06_d1;
+	uint8_t  f1rc07_d1;
+	uint8_t  f1rc08_d1;
+	uint8_t  f1rc09_d1;
+	uint8_t  f1rc0a_d1;
+	uint8_t  f1rc0b_d1;
+	uint8_t  f1rc0c_d1;
+	uint8_t  f1rc0d_d1;
+	uint8_t  f1rc0e_d1;
+	uint8_t  f1rc0f_d1;
+	uint8_t  f1rc1x_d1;
+	uint8_t  f1rc2x_d1;
+	uint8_t  f1rc3x_d1;
+	uint8_t  f1rc4x_d1;
+	uint8_t  f1rc5x_d1;
+	uint8_t  f1rc6x_d1;
+	uint8_t  f1rc7x_d1;
+	uint8_t  f1rc8x_d1;
+	uint8_t  f1rc9x_d1;
+	uint8_t  f1rcax_d1;
+	uint8_t  f1rcbx_d1;
+	uint8_t  bc00_d0;
+	uint8_t  bc01_d0;
+	uint8_t  bc02_d0;
+	uint8_t  bc03_d0;
+	uint8_t  bc04_d0;
+	uint8_t  bc05_d0;
+	uint8_t  bc06_d0;
+	uint8_t  bc07_d0;
+	uint8_t  bc08_d0;
+	uint8_t  bc09_d0;
+	uint8_t  bc0a_d0;
+	uint8_t  bc0b_d0;
+	uint8_t  bc0c_d0;
+	uint8_t  bc0d_d0;
+	uint8_t  bc0e_d0;
+	uint8_t  f0bc6x_d0;
+	uint8_t  f0bccx_d0;
+	uint8_t  f0bcdx_d0;
+	uint8_t  f0bcex_d0;
+	uint8_t  f0bcfx_d0;
+	uint8_t  f1bccx_d0;
+	uint8_t  f1bcdx_d0;
+	uint8_t  f1bcex_d0;
+	uint8_t  f1bcfx_d0;
+	uint8_t  f0bc2x_b0_d0;
+	uint8_t  f0bc3x_b0_d0;
+	uint8_t  f0bc4x_b0_d0;
+	uint8_t  f0bc5x_b0_d0;
+	uint8_t  f0bc8x_b0_d0;
+	uint8_t  f0bc9x_b0_d0;
+	uint8_t  f0bcax_b0_d0;
+	uint8_t  f0bcbx_b0_d0;
+	uint8_t  f1bc2x_b0_d0;
+	uint8_t  f1bc3x_b0_d0;
+	uint8_t  f1bc4x_b0_d0;
+	uint8_t  f1bc5x_b0_d0;
+	uint8_t  f1bc8x_b0_d0;
+	uint8_t  f1bc9x_b0_d0;
+	uint8_t  f1bcax_b0_d0;
+	uint8_t  f1bcbx_b0_d0;
+	uint8_t  f2bc2x_b0_d0;
+	uint8_t  f2bc3x_b0_d0;
+	uint8_t  f2bc4x_b0_d0;
+	uint8_t  f2bc5x_b0_d0;
+	uint8_t  f2bc8x_b0_d0;
+	uint8_t  f2bc9x_b0_d0;
+	uint8_t  f2bcax_b0_d0;
+	uint8_t  f2bcbx_b0_d0;
+	uint8_t  f3bc2x_b0_d0;
+	uint8_t  f3bc3x_b0_d0;
+	uint8_t  f3bc4x_b0_d0;
+	uint8_t  f3bc5x_b0_d0;
+	uint8_t  f3bc8x_b0_d0;
+	uint8_t  f3bc9x_b0_d0;
+	uint8_t  f3bcax_b0_d0;
+	uint8_t  f3bcbx_b0_d0;
+	uint8_t  f0bc2x_b1_d0;
+	uint8_t  f0bc3x_b1_d0;
+	uint8_t  f0bc4x_b1_d0;
+	uint8_t  f0bc5x_b1_d0;
+	uint8_t  f0bc8x_b1_d0;
+	uint8_t  f0bc9x_b1_d0;
+	uint8_t  f0bcax_b1_d0;
+	uint8_t  f0bcbx_b1_d0;
+	uint8_t  f1bc2x_b1_d0;
+	uint8_t  f1bc3x_b1_d0;
+	uint8_t  f1bc4x_b1_d0;
+	uint8_t  f1bc5x_b1_d0;
+	uint8_t  f1bc8x_b1_d0;
+	uint8_t  f1bc9x_b1_d0;
+	uint8_t  f1bcax_b1_d0;
+	uint8_t  f1bcbx_b1_d0;
+	uint8_t  f2bc2x_b1_d0;
+	uint8_t  f2bc3x_b1_d0;
+	uint8_t  f2bc4x_b1_d0;
+	uint8_t  f2bc5x_b1_d0;
+	uint8_t  f2bc8x_b1_d0;
+	uint8_t  f2bc9x_b1_d0;
+	uint8_t  f2bcax_b1_d0;
+	uint8_t  f2bcbx_b1_d0;
+	uint8_t  f3bc2x_b1_d0;
+	uint8_t  f3bc3x_b1_d0;
+	uint8_t  f3bc4x_b1_d0;
+	uint8_t  f3bc5x_b1_d0;
+	uint8_t  f3bc8x_b1_d0;
+	uint8_t  f3bc9x_b1_d0;
+	uint8_t  f3bcax_b1_d0;
+	uint8_t  f3bcbx_b1_d0;
+	uint8_t  f0bc2x_b2_d0;
+	uint8_t  f0bc3x_b2_d0;
+	uint8_t  f0bc4x_b2_d0;
+	uint8_t  f0bc5x_b2_d0;
+	uint8_t  f0bc8x_b2_d0;
+	uint8_t  f0bc9x_b2_d0;
+	uint8_t  f0bcax_b2_d0;
+	uint8_t  f0bcbx_b2_d0;
+	uint8_t  f1bc2x_b2_d0;
+	uint8_t  f1bc3x_b2_d0;
+	uint8_t  f1bc4x_b2_d0;
+	uint8_t  f1bc5x_b2_d0;
+	uint8_t  f1bc8x_b2_d0;
+	uint8_t  f1bc9x_b2_d0;
+	uint8_t  f1bcax_b2_d0;
+	uint8_t  f1bcbx_b2_d0;
+	uint8_t  f2bc2x_b2_d0;
+	uint8_t  f2bc3x_b2_d0;
+	uint8_t  f2bc4x_b2_d0;
+	uint8_t  f2bc5x_b2_d0;
+	uint8_t  f2bc8x_b2_d0;
+	uint8_t  f2bc9x_b2_d0;
+	uint8_t  f2bcax_b2_d0;
+	uint8_t  f2bcbx_b2_d0;
+	uint8_t  f3bc2x_b2_d0;
+	uint8_t  f3bc3x_b2_d0;
+	uint8_t  f3bc4x_b2_d0;
+	uint8_t  f3bc5x_b2_d0;
+	uint8_t  f3bc8x_b2_d0;
+	uint8_t  f3bc9x_b2_d0;
+	uint8_t  f3bcax_b2_d0;
+	uint8_t  f3bcbx_b2_d0;
+	uint8_t  f0bc2x_b3_d0;
+	uint8_t  f0bc3x_b3_d0;
+	uint8_t  f0bc4x_b3_d0;
+	uint8_t  f0bc5x_b3_d0;
+	uint8_t  f0bc8x_b3_d0;
+	uint8_t  f0bc9x_b3_d0;
+	uint8_t  f0bcax_b3_d0;
+	uint8_t  f0bcbx_b3_d0;
+	uint8_t  f1bc2x_b3_d0;
+	uint8_t  f1bc3x_b3_d0;
+	uint8_t  f1bc4x_b3_d0;
+	uint8_t  f1bc5x_b3_d0;
+	uint8_t  f1bc8x_b3_d0;
+	uint8_t  f1bc9x_b3_d0;
+	uint8_t  f1bcax_b3_d0;
+	uint8_t  f1bcbx_b3_d0;
+	uint8_t  f2bc2x_b3_d0;
+	uint8_t  f2bc3x_b3_d0;
+	uint8_t  f2bc4x_b3_d0;
+	uint8_t  f2bc5x_b3_d0;
+	uint8_t  f2bc8x_b3_d0;
+	uint8_t  f2bc9x_b3_d0;
+	uint8_t  f2bcax_b3_d0;
+	uint8_t  f2bcbx_b3_d0;
+	uint8_t  f3bc2x_b3_d0;
+	uint8_t  f3bc3x_b3_d0;
+	uint8_t  f3bc4x_b3_d0;
+	uint8_t  f3bc5x_b3_d0;
+	uint8_t  f3bc8x_b3_d0;
+	uint8_t  f3bc9x_b3_d0;
+	uint8_t  f3bcax_b3_d0;
+	uint8_t  f3bcbx_b3_d0;
+	uint8_t  f0bc2x_b4_d0;
+	uint8_t  f0bc3x_b4_d0;
+	uint8_t  f0bc4x_b4_d0;
+	uint8_t  f0bc5x_b4_d0;
+	uint8_t  f0bc8x_b4_d0;
+	uint8_t  f0bc9x_b4_d0;
+	uint8_t  f0bcax_b4_d0;
+	uint8_t  f0bcbx_b4_d0;
+	uint8_t  f1bc2x_b4_d0;
+	uint8_t  f1bc3x_b4_d0;
+	uint8_t  f1bc4x_b4_d0;
+	uint8_t  f1bc5x_b4_d0;
+	uint8_t  f1bc8x_b4_d0;
+	uint8_t  f1bc9x_b4_d0;
+	uint8_t  f1bcax_b4_d0;
+	uint8_t  f1bcbx_b4_d0;
+	uint8_t  f2bc2x_b4_d0;
+	uint8_t  f2bc3x_b4_d0;
+	uint8_t  f2bc4x_b4_d0;
+	uint8_t  f2bc5x_b4_d0;
+	uint8_t  f2bc8x_b4_d0;
+	uint8_t  f2bc9x_b4_d0;
+	uint8_t  f2bcax_b4_d0;
+	uint8_t  f2bcbx_b4_d0;
+	uint8_t  f3bc2x_b4_d0;
+	uint8_t  f3bc3x_b4_d0;
+	uint8_t  f3bc4x_b4_d0;
+	uint8_t  f3bc5x_b4_d0;
+	uint8_t  f3bc8x_b4_d0;
+	uint8_t  f3bc9x_b4_d0;
+	uint8_t  f3bcax_b4_d0;
+	uint8_t  f3bcbx_b4_d0;
+	uint8_t  f0bc2x_b5_d0;
+	uint8_t  f0bc3x_b5_d0;
+	uint8_t  f0bc4x_b5_d0;
+	uint8_t  f0bc5x_b5_d0;
+	uint8_t  f0bc8x_b5_d0;
+	uint8_t  f0bc9x_b5_d0;
+	uint8_t  f0bcax_b5_d0;
+	uint8_t  f0bcbx_b5_d0;
+	uint8_t  f1bc2x_b5_d0;
+	uint8_t  f1bc3x_b5_d0;
+	uint8_t  f1bc4x_b5_d0;
+	uint8_t  f1bc5x_b5_d0;
+	uint8_t  f1bc8x_b5_d0;
+	uint8_t  f1bc9x_b5_d0;
+	uint8_t  f1bcax_b5_d0;
+	uint8_t  f1bcbx_b5_d0;
+	uint8_t  f2bc2x_b5_d0;
+	uint8_t  f2bc3x_b5_d0;
+	uint8_t  f2bc4x_b5_d0;
+	uint8_t  f2bc5x_b5_d0;
+	uint8_t  f2bc8x_b5_d0;
+	uint8_t  f2bc9x_b5_d0;
+	uint8_t  f2bcax_b5_d0;
+	uint8_t  f2bcbx_b5_d0;
+	uint8_t  f3bc2x_b5_d0;
+	uint8_t  f3bc3x_b5_d0;
+	uint8_t  f3bc4x_b5_d0;
+	uint8_t  f3bc5x_b5_d0;
+	uint8_t  f3bc8x_b5_d0;
+	uint8_t  f3bc9x_b5_d0;
+	uint8_t  f3bcax_b5_d0;
+	uint8_t  f3bcbx_b5_d0;
+	uint8_t  f0bc2x_b6_d0;
+	uint8_t  f0bc3x_b6_d0;
+	uint8_t  f0bc4x_b6_d0;
+	uint8_t  f0bc5x_b6_d0;
+	uint8_t  f0bc8x_b6_d0;
+	uint8_t  f0bc9x_b6_d0;
+	uint8_t  f0bcax_b6_d0;
+	uint8_t  f0bcbx_b6_d0;
+	uint8_t  f1bc2x_b6_d0;
+	uint8_t  f1bc3x_b6_d0;
+	uint8_t  f1bc4x_b6_d0;
+	uint8_t  f1bc5x_b6_d0;
+	uint8_t  f1bc8x_b6_d0;
+	uint8_t  f1bc9x_b6_d0;
+	uint8_t  f1bcax_b6_d0;
+	uint8_t  f1bcbx_b6_d0;
+	uint8_t  f2bc2x_b6_d0;
+	uint8_t  f2bc3x_b6_d0;
+	uint8_t  f2bc4x_b6_d0;
+	uint8_t  f2bc5x_b6_d0;
+	uint8_t  f2bc8x_b6_d0;
+	uint8_t  f2bc9x_b6_d0;
+	uint8_t  f2bcax_b6_d0;
+	uint8_t  f2bcbx_b6_d0;
+	uint8_t  f3bc2x_b6_d0;
+	uint8_t  f3bc3x_b6_d0;
+	uint8_t  f3bc4x_b6_d0;
+	uint8_t  f3bc5x_b6_d0;
+	uint8_t  f3bc8x_b6_d0;
+	uint8_t  f3bc9x_b6_d0;
+	uint8_t  f3bcax_b6_d0;
+	uint8_t  f3bcbx_b6_d0;
+	uint8_t  f0bc2x_b7_d0;
+	uint8_t  f0bc3x_b7_d0;
+	uint8_t  f0bc4x_b7_d0;
+	uint8_t  f0bc5x_b7_d0;
+	uint8_t  f0bc8x_b7_d0;
+	uint8_t  f0bc9x_b7_d0;
+	uint8_t  f0bcax_b7_d0;
+	uint8_t  f0bcbx_b7_d0;
+	uint8_t  f1bc2x_b7_d0;
+	uint8_t  f1bc3x_b7_d0;
+	uint8_t  f1bc4x_b7_d0;
+	uint8_t  f1bc5x_b7_d0;
+	uint8_t  f1bc8x_b7_d0;
+	uint8_t  f1bc9x_b7_d0;
+	uint8_t  f1bcax_b7_d0;
+	uint8_t  f1bcbx_b7_d0;
+	uint8_t  f2bc2x_b7_d0;
+	uint8_t  f2bc3x_b7_d0;
+	uint8_t  f2bc4x_b7_d0;
+	uint8_t  f2bc5x_b7_d0;
+	uint8_t  f2bc8x_b7_d0;
+	uint8_t  f2bc9x_b7_d0;
+	uint8_t  f2bcax_b7_d0;
+	uint8_t  f2bcbx_b7_d0;
+	uint8_t  f3bc2x_b7_d0;
+	uint8_t  f3bc3x_b7_d0;
+	uint8_t  f3bc4x_b7_d0;
+	uint8_t  f3bc5x_b7_d0;
+	uint8_t  f3bc8x_b7_d0;
+	uint8_t  f3bc9x_b7_d0;
+	uint8_t  f3bcax_b7_d0;
+	uint8_t  f3bcbx_b7_d0;
+	uint8_t  f0bc2x_b8_d0;
+	uint8_t  f0bc3x_b8_d0;
+	uint8_t  f0bc4x_b8_d0;
+	uint8_t  f0bc5x_b8_d0;
+	uint8_t  f0bc8x_b8_d0;
+	uint8_t  f0bc9x_b8_d0;
+	uint8_t  f0bcax_b8_d0;
+	uint8_t  f0bcbx_b8_d0;
+	uint8_t  f1bc2x_b8_d0;
+	uint8_t  f1bc3x_b8_d0;
+	uint8_t  f1bc4x_b8_d0;
+	uint8_t  f1bc5x_b8_d0;
+	uint8_t  f1bc8x_b8_d0;
+	uint8_t  f1bc9x_b8_d0;
+	uint8_t  f1bcax_b8_d0;
+	uint8_t  f1bcbx_b8_d0;
+	uint8_t  f2bc2x_b8_d0;
+	uint8_t  f2bc3x_b8_d0;
+	uint8_t  f2bc4x_b8_d0;
+	uint8_t  f2bc5x_b8_d0;
+	uint8_t  f2bc8x_b8_d0;
+	uint8_t  f2bc9x_b8_d0;
+	uint8_t  f2bcax_b8_d0;
+	uint8_t  f2bcbx_b8_d0;
+	uint8_t  f3bc2x_b8_d0;
+	uint8_t  f3bc3x_b8_d0;
+	uint8_t  f3bc4x_b8_d0;
+	uint8_t  f3bc5x_b8_d0;
+	uint8_t  f3bc8x_b8_d0;
+	uint8_t  f3bc9x_b8_d0;
+	uint8_t  f3bcax_b8_d0;
+	uint8_t  f3bcbx_b8_d0;
+	uint8_t  f5bc5x_d0;
+	uint8_t  f5bc6x_d0;
+	uint8_t  f4bc8x_d0;
+	uint8_t  f4bc9x_d0;
+	uint8_t  f4bcax_d0;
+	uint8_t  f4bcbx_d0;
+	uint8_t  f4bccx_d0;
+	uint8_t  f4bcdx_d0;
+	uint8_t  f4bcex_d0;
+	uint8_t  f4bcfx_d0;
+	uint8_t  f5bc8x_d0;
+	uint8_t  f5bc9x_d0;
+	uint8_t  f5bcax_d0;
+	uint8_t  f5bcbx_d0;
+	uint8_t  f5bccx_d0;
+	uint8_t  f5bcdx_d0;
+	uint8_t  f5bcex_d0;
+	uint8_t  f5bcfx_d0;
+	uint8_t  f6bc8x_d0;
+	uint8_t  f6bc9x_d0;
+	uint8_t  f6bcax_d0;
+	uint8_t  f6bcbx_d0;
+	uint8_t  f6bccx_d0;
+	uint8_t  f6bcdx_d0;
+	uint8_t  f6bcex_d0;
+	uint8_t  f6bcfx_d0;
+	uint8_t  f7bc8x_d0;
+	uint8_t  f7bc9x_d0;
+	uint8_t  f7bcax_d0;
+	uint8_t  f7bcbx_d0;
+	uint8_t  f7bccx_d0;
+	uint8_t  f7bcdx_d0;
+	uint8_t  f7bcex_d0;
+	uint8_t  f7bcfx_d0;
+	uint8_t  bc00_d1;
+	uint8_t  bc01_d1;
+	uint8_t  bc02_d1;
+	uint8_t  bc03_d1;
+	uint8_t  bc04_d1;
+	uint8_t  bc05_d1;
+	uint8_t  bc06_d1;
+	uint8_t  bc07_d1;
+	uint8_t  bc08_d1;
+	uint8_t  bc09_d1;
+	uint8_t  bc0a_d1;
+	uint8_t  bc0b_d1;
+	uint8_t  bc0c_d1;
+	uint8_t  bc0d_d1;
+	uint8_t  bc0e_d1;
+	uint8_t  f0bc6x_d1;
+	uint8_t  f0bccx_d1;
+	uint8_t  f0bcdx_d1;
+	uint8_t  f0bcex_d1;
+	uint8_t  f0bcfx_d1;
+	uint8_t  f1bccx_d1;
+	uint8_t  f1bcdx_d1;
+	uint8_t  f1bcex_d1;
+	uint8_t  f1bcfx_d1;
+	uint8_t  f0bc2x_b0_d1;
+	uint8_t  f0bc3x_b0_d1;
+	uint8_t  f0bc4x_b0_d1;
+	uint8_t  f0bc5x_b0_d1;
+	uint8_t  f0bc8x_b0_d1;
+	uint8_t  f0bc9x_b0_d1;
+	uint8_t  f0bcax_b0_d1;
+	uint8_t  f0bcbx_b0_d1;
+	uint8_t  f1bc2x_b0_d1;
+	uint8_t  f1bc3x_b0_d1;
+	uint8_t  f1bc4x_b0_d1;
+	uint8_t  f1bc5x_b0_d1;
+	uint8_t  f1bc8x_b0_d1;
+	uint8_t  f1bc9x_b0_d1;
+	uint8_t  f1bcax_b0_d1;
+	uint8_t  f1bcbx_b0_d1;
+	uint8_t  f2bc2x_b0_d1;
+	uint8_t  f2bc3x_b0_d1;
+	uint8_t  f2bc4x_b0_d1;
+	uint8_t  f2bc5x_b0_d1;
+	uint8_t  f2bc8x_b0_d1;
+	uint8_t  f2bc9x_b0_d1;
+	uint8_t  f2bcax_b0_d1;
+	uint8_t  f2bcbx_b0_d1;
+	uint8_t  f3bc2x_b0_d1;
+	uint8_t  f3bc3x_b0_d1;
+	uint8_t  f3bc4x_b0_d1;
+	uint8_t  f3bc5x_b0_d1;
+	uint8_t  f3bc8x_b0_d1;
+	uint8_t  f3bc9x_b0_d1;
+	uint8_t  f3bcax_b0_d1;
+	uint8_t  f3bcbx_b0_d1;
+	uint8_t  f0bc2x_b1_d1;
+	uint8_t  f0bc3x_b1_d1;
+	uint8_t  f0bc4x_b1_d1;
+	uint8_t  f0bc5x_b1_d1;
+	uint8_t  f0bc8x_b1_d1;
+	uint8_t  f0bc9x_b1_d1;
+	uint8_t  f0bcax_b1_d1;
+	uint8_t  f0bcbx_b1_d1;
+	uint8_t  f1bc2x_b1_d1;
+	uint8_t  f1bc3x_b1_d1;
+	uint8_t  f1bc4x_b1_d1;
+	uint8_t  f1bc5x_b1_d1;
+	uint8_t  f1bc8x_b1_d1;
+	uint8_t  f1bc9x_b1_d1;
+	uint8_t  f1bcax_b1_d1;
+	uint8_t  f1bcbx_b1_d1;
+	uint8_t  f2bc2x_b1_d1;
+	uint8_t  f2bc3x_b1_d1;
+	uint8_t  f2bc4x_b1_d1;
+	uint8_t  f2bc5x_b1_d1;
+	uint8_t  f2bc8x_b1_d1;
+	uint8_t  f2bc9x_b1_d1;
+	uint8_t  f2bcax_b1_d1;
+	uint8_t  f2bcbx_b1_d1;
+	uint8_t  f3bc2x_b1_d1;
+	uint8_t  f3bc3x_b1_d1;
+	uint8_t  f3bc4x_b1_d1;
+	uint8_t  f3bc5x_b1_d1;
+	uint8_t  f3bc8x_b1_d1;
+	uint8_t  f3bc9x_b1_d1;
+	uint8_t  f3bcax_b1_d1;
+	uint8_t  f3bcbx_b1_d1;
+	uint8_t  f0bc2x_b2_d1;
+	uint8_t  f0bc3x_b2_d1;
+	uint8_t  f0bc4x_b2_d1;
+	uint8_t  f0bc5x_b2_d1;
+	uint8_t  f0bc8x_b2_d1;
+	uint8_t  f0bc9x_b2_d1;
+	uint8_t  f0bcax_b2_d1;
+	uint8_t  f0bcbx_b2_d1;
+	uint8_t  f1bc2x_b2_d1;
+	uint8_t  f1bc3x_b2_d1;
+	uint8_t  f1bc4x_b2_d1;
+	uint8_t  f1bc5x_b2_d1;
+	uint8_t  f1bc8x_b2_d1;
+	uint8_t  f1bc9x_b2_d1;
+	uint8_t  f1bcax_b2_d1;
+	uint8_t  f1bcbx_b2_d1;
+	uint8_t  f2bc2x_b2_d1;
+	uint8_t  f2bc3x_b2_d1;
+	uint8_t  f2bc4x_b2_d1;
+	uint8_t  f2bc5x_b2_d1;
+	uint8_t  f2bc8x_b2_d1;
+	uint8_t  f2bc9x_b2_d1;
+	uint8_t  f2bcax_b2_d1;
+	uint8_t  f2bcbx_b2_d1;
+	uint8_t  f3bc2x_b2_d1;
+	uint8_t  f3bc3x_b2_d1;
+	uint8_t  f3bc4x_b2_d1;
+	uint8_t  f3bc5x_b2_d1;
+	uint8_t  f3bc8x_b2_d1;
+	uint8_t  f3bc9x_b2_d1;
+	uint8_t  f3bcax_b2_d1;
+	uint8_t  f3bcbx_b2_d1;
+	uint8_t  f0bc2x_b3_d1;
+	uint8_t  f0bc3x_b3_d1;
+	uint8_t  f0bc4x_b3_d1;
+	uint8_t  f0bc5x_b3_d1;
+	uint8_t  f0bc8x_b3_d1;
+	uint8_t  f0bc9x_b3_d1;
+	uint8_t  f0bcax_b3_d1;
+	uint8_t  f0bcbx_b3_d1;
+	uint8_t  f1bc2x_b3_d1;
+	uint8_t  f1bc3x_b3_d1;
+	uint8_t  f1bc4x_b3_d1;
+	uint8_t  f1bc5x_b3_d1;
+	uint8_t  f1bc8x_b3_d1;
+	uint8_t  f1bc9x_b3_d1;
+	uint8_t  f1bcax_b3_d1;
+	uint8_t  f1bcbx_b3_d1;
+	uint8_t  f2bc2x_b3_d1;
+	uint8_t  f2bc3x_b3_d1;
+	uint8_t  f2bc4x_b3_d1;
+	uint8_t  f2bc5x_b3_d1;
+	uint8_t  f2bc8x_b3_d1;
+	uint8_t  f2bc9x_b3_d1;
+	uint8_t  f2bcax_b3_d1;
+	uint8_t  f2bcbx_b3_d1;
+	uint8_t  f3bc2x_b3_d1;
+	uint8_t  f3bc3x_b3_d1;
+	uint8_t  f3bc4x_b3_d1;
+	uint8_t  f3bc5x_b3_d1;
+	uint8_t  f3bc8x_b3_d1;
+	uint8_t  f3bc9x_b3_d1;
+	uint8_t  f3bcax_b3_d1;
+	uint8_t  f3bcbx_b3_d1;
+	uint8_t  f0bc2x_b4_d1;
+	uint8_t  f0bc3x_b4_d1;
+	uint8_t  f0bc4x_b4_d1;
+	uint8_t  f0bc5x_b4_d1;
+	uint8_t  f0bc8x_b4_d1;
+	uint8_t  f0bc9x_b4_d1;
+	uint8_t  f0bcax_b4_d1;
+	uint8_t  f0bcbx_b4_d1;
+	uint8_t  f1bc2x_b4_d1;
+	uint8_t  f1bc3x_b4_d1;
+	uint8_t  f1bc4x_b4_d1;
+	uint8_t  f1bc5x_b4_d1;
+	uint8_t  f1bc8x_b4_d1;
+	uint8_t  f1bc9x_b4_d1;
+	uint8_t  f1bcax_b4_d1;
+	uint8_t  f1bcbx_b4_d1;
+	uint8_t  f2bc2x_b4_d1;
+	uint8_t  f2bc3x_b4_d1;
+	uint8_t  f2bc4x_b4_d1;
+	uint8_t  f2bc5x_b4_d1;
+	uint8_t  f2bc8x_b4_d1;
+	uint8_t  f2bc9x_b4_d1;
+	uint8_t  f2bcax_b4_d1;
+	uint8_t  f2bcbx_b4_d1;
+	uint8_t  f3bc2x_b4_d1;
+	uint8_t  f3bc3x_b4_d1;
+	uint8_t  f3bc4x_b4_d1;
+	uint8_t  f3bc5x_b4_d1;
+	uint8_t  f3bc8x_b4_d1;
+	uint8_t  f3bc9x_b4_d1;
+	uint8_t  f3bcax_b4_d1;
+	uint8_t  f3bcbx_b4_d1;
+	uint8_t  f0bc2x_b5_d1;
+	uint8_t  f0bc3x_b5_d1;
+	uint8_t  f0bc4x_b5_d1;
+	uint8_t  f0bc5x_b5_d1;
+	uint8_t  f0bc8x_b5_d1;
+	uint8_t  f0bc9x_b5_d1;
+	uint8_t  f0bcax_b5_d1;
+	uint8_t  f0bcbx_b5_d1;
+	uint8_t  f1bc2x_b5_d1;
+	uint8_t  f1bc3x_b5_d1;
+	uint8_t  f1bc4x_b5_d1;
+	uint8_t  f1bc5x_b5_d1;
+	uint8_t  f1bc8x_b5_d1;
+	uint8_t  f1bc9x_b5_d1;
+	uint8_t  f1bcax_b5_d1;
+	uint8_t  f1bcbx_b5_d1;
+	uint8_t  f2bc2x_b5_d1;
+	uint8_t  f2bc3x_b5_d1;
+	uint8_t  f2bc4x_b5_d1;
+	uint8_t  f2bc5x_b5_d1;
+	uint8_t  f2bc8x_b5_d1;
+	uint8_t  f2bc9x_b5_d1;
+	uint8_t  f2bcax_b5_d1;
+	uint8_t  f2bcbx_b5_d1;
+	uint8_t  f3bc2x_b5_d1;
+	uint8_t  f3bc3x_b5_d1;
+	uint8_t  f3bc4x_b5_d1;
+	uint8_t  f3bc5x_b5_d1;
+	uint8_t  f3bc8x_b5_d1;
+	uint8_t  f3bc9x_b5_d1;
+	uint8_t  f3bcax_b5_d1;
+	uint8_t  f3bcbx_b5_d1;
+	uint8_t  f0bc2x_b6_d1;
+	uint8_t  f0bc3x_b6_d1;
+	uint8_t  f0bc4x_b6_d1;
+	uint8_t  f0bc5x_b6_d1;
+	uint8_t  f0bc8x_b6_d1;
+	uint8_t  f0bc9x_b6_d1;
+	uint8_t  f0bcax_b6_d1;
+	uint8_t  f0bcbx_b6_d1;
+	uint8_t  f1bc2x_b6_d1;
+	uint8_t  f1bc3x_b6_d1;
+	uint8_t  f1bc4x_b6_d1;
+	uint8_t  f1bc5x_b6_d1;
+	uint8_t  f1bc8x_b6_d1;
+	uint8_t  f1bc9x_b6_d1;
+	uint8_t  f1bcax_b6_d1;
+	uint8_t  f1bcbx_b6_d1;
+	uint8_t  f2bc2x_b6_d1;
+	uint8_t  f2bc3x_b6_d1;
+	uint8_t  f2bc4x_b6_d1;
+	uint8_t  f2bc5x_b6_d1;
+	uint8_t  f2bc8x_b6_d1;
+	uint8_t  f2bc9x_b6_d1;
+	uint8_t  f2bcax_b6_d1;
+	uint8_t  f2bcbx_b6_d1;
+	uint8_t  f3bc2x_b6_d1;
+	uint8_t  f3bc3x_b6_d1;
+	uint8_t  f3bc4x_b6_d1;
+	uint8_t  f3bc5x_b6_d1;
+	uint8_t  f3bc8x_b6_d1;
+	uint8_t  f3bc9x_b6_d1;
+	uint8_t  f3bcax_b6_d1;
+	uint8_t  f3bcbx_b6_d1;
+	uint8_t  f0bc2x_b7_d1;
+	uint8_t  f0bc3x_b7_d1;
+	uint8_t  f0bc4x_b7_d1;
+	uint8_t  f0bc5x_b7_d1;
+	uint8_t  f0bc8x_b7_d1;
+	uint8_t  f0bc9x_b7_d1;
+	uint8_t  f0bcax_b7_d1;
+	uint8_t  f0bcbx_b7_d1;
+	uint8_t  f1bc2x_b7_d1;
+	uint8_t  f1bc3x_b7_d1;
+	uint8_t  f1bc4x_b7_d1;
+	uint8_t  f1bc5x_b7_d1;
+	uint8_t  f1bc8x_b7_d1;
+	uint8_t  f1bc9x_b7_d1;
+	uint8_t  f1bcax_b7_d1;
+	uint8_t  f1bcbx_b7_d1;
+	uint8_t  f2bc2x_b7_d1;
+	uint8_t  f2bc3x_b7_d1;
+	uint8_t  f2bc4x_b7_d1;
+	uint8_t  f2bc5x_b7_d1;
+	uint8_t  f2bc8x_b7_d1;
+	uint8_t  f2bc9x_b7_d1;
+	uint8_t  f2bcax_b7_d1;
+	uint8_t  f2bcbx_b7_d1;
+	uint8_t  f3bc2x_b7_d1;
+	uint8_t  f3bc3x_b7_d1;
+	uint8_t  f3bc4x_b7_d1;
+	uint8_t  f3bc5x_b7_d1;
+	uint8_t  f3bc8x_b7_d1;
+	uint8_t  f3bc9x_b7_d1;
+	uint8_t  f3bcax_b7_d1;
+	uint8_t  f3bcbx_b7_d1;
+	uint8_t  f0bc2x_b8_d1;
+	uint8_t  f0bc3x_b8_d1;
+	uint8_t  f0bc4x_b8_d1;
+	uint8_t  f0bc5x_b8_d1;
+	uint8_t  f0bc8x_b8_d1;
+	uint8_t  f0bc9x_b8_d1;
+	uint8_t  f0bcax_b8_d1;
+	uint8_t  f0bcbx_b8_d1;
+	uint8_t  f1bc2x_b8_d1;
+	uint8_t  f1bc3x_b8_d1;
+	uint8_t  f1bc4x_b8_d1;
+	uint8_t  f1bc5x_b8_d1;
+	uint8_t  f1bc8x_b8_d1;
+	uint8_t  f1bc9x_b8_d1;
+	uint8_t  f1bcax_b8_d1;
+	uint8_t  f1bcbx_b8_d1;
+	uint8_t  f2bc2x_b8_d1;
+	uint8_t  f2bc3x_b8_d1;
+	uint8_t  f2bc4x_b8_d1;
+	uint8_t  f2bc5x_b8_d1;
+	uint8_t  f2bc8x_b8_d1;
+	uint8_t  f2bc9x_b8_d1;
+	uint8_t  f2bcax_b8_d1;
+	uint8_t  f2bcbx_b8_d1;
+	uint8_t  f3bc2x_b8_d1;
+	uint8_t  f3bc3x_b8_d1;
+	uint8_t  f3bc4x_b8_d1;
+	uint8_t  f3bc5x_b8_d1;
+	uint8_t  f3bc8x_b8_d1;
+	uint8_t  f3bc9x_b8_d1;
+	uint8_t  f3bcax_b8_d1;
+	uint8_t  f3bcbx_b8_d1;
+	uint8_t  f5bc5x_d1;
+	uint8_t  f5bc6x_d1;
+	uint8_t  f4bc8x_d1;
+	uint8_t  f4bc9x_d1;
+	uint8_t  f4bcax_d1;
+	uint8_t  f4bcbx_d1;
+	uint8_t  f4bccx_d1;
+	uint8_t  f4bcdx_d1;
+	uint8_t  f4bcex_d1;
+	uint8_t  f4bcfx_d1;
+	uint8_t  f5bc8x_d1;
+	uint8_t  f5bc9x_d1;
+	uint8_t  f5bcax_d1;
+	uint8_t  f5bcbx_d1;
+	uint8_t  f5bccx_d1;
+	uint8_t  f5bcdx_d1;
+	uint8_t  f5bcex_d1;
+	uint8_t  f5bcfx_d1;
+	uint8_t  f6bc8x_d1;
+	uint8_t  f6bc9x_d1;
+	uint8_t  f6bcax_d1;
+	uint8_t  f6bcbx_d1;
+	uint8_t  f6bccx_d1;
+	uint8_t  f6bcdx_d1;
+	uint8_t  f6bcex_d1;
+	uint8_t  f6bcfx_d1;
+	uint8_t  f7bc8x_d1;
+	uint8_t  f7bc9x_d1;
+	uint8_t  f7bcax_d1;
+	uint8_t  f7bcbx_d1;
+	uint8_t  f7bccx_d1;
+	uint8_t  f7bcdx_d1;
+	uint8_t  f7bcex_d1;
+	uint8_t  f7bcfx_d1;
+	uint16_t alt_cas_l;
+	uint8_t  alt_wcas_l;
+	uint8_t  d4misc;
+} __packed;
+
+struct ddr4lr2d {
+	uint8_t  reserved00;
+	uint8_t  msg_misc;
+	uint16_t pmu_revision;
+	uint8_t  pstate;
+	uint8_t  pll_bypass_en;
+	uint16_t dramfreq;
+	uint8_t  dfi_freq_ratio;
+	uint8_t  bpznres_val;
+	uint8_t  phy_odt_impedance;
+	uint8_t  phy_drv_impedance;
+	uint8_t  phy_vref;
+	uint8_t  dram_type;
+	uint8_t  disabled_dbyte;
+	uint8_t  enabled_dqs;
+	uint8_t  cs_present;
+	uint8_t  cs_present_d0;
+	uint8_t  cs_present_d1;
+	uint8_t  addr_mirror;
+	uint8_t  cs_test_fail;
+	uint8_t  phy_cfg;
+	uint16_t sequence_ctrl;
+	uint8_t  hdt_ctrl;
+	uint8_t  rx2d_train_opt;
+	uint8_t  tx2d_train_opt;
+	uint8_t  share2dvref_result;
+	uint8_t  delay_weight2d;
+	uint8_t  voltage_weight2d;
+	uint8_t  reserved1e[0x22 - 0x1e];
+	uint16_t phy_config_override;
+	uint8_t  dfimrlmargin;
+	uint8_t  r0_rx_clk_dly_margin;
+	uint8_t  r0_vref_dac_margin;
+	uint8_t  r0_tx_dq_dly_margin;
+	uint8_t  r0_device_vref_margin;
+	uint8_t  reserved29[0x33 - 0x29];
+	uint8_t  r1_rx_clk_dly_margin;
+	uint8_t  r1_vref_dac_margin;
+	uint8_t  r1_tx_dq_dly_margin;
+	uint8_t  r1_device_vref_margin;
+	uint8_t  reserved37[0x41 - 0x37];
+	uint8_t  r2_rx_clk_dly_margin;
+	uint8_t  r2_vref_dac_margin;
+	uint8_t  r2_tx_dq_dly_margin;
+	uint8_t  r2_device_vref_margin;
+	uint8_t  reserved45[0x4f - 0x45];
+	uint8_t  r3_rx_clk_dly_margin;
+	uint8_t  r3_vref_dac_margin;
+	uint8_t  r3_tx_dq_dly_margin;
+	uint8_t  r3_device_vref_margin;
+	uint8_t  reserved53[0x5e - 0x53];
+	uint16_t mr0;
+	uint16_t mr1;
+	uint16_t mr2;
+	uint16_t mr3;
+	uint16_t mr4;
+	uint16_t mr5;
+	uint16_t mr6;
+	uint8_t  x16present;
+	uint8_t  cs_setup_gddec;
+	uint16_t rtt_nom_wr_park0;
+	uint16_t rtt_nom_wr_park1;
+	uint16_t rtt_nom_wr_park2;
+	uint16_t rtt_nom_wr_park3;
+	uint16_t rtt_nom_wr_park4;
+	uint16_t rtt_nom_wr_park5;
+	uint16_t rtt_nom_wr_park6;
+	uint16_t rtt_nom_wr_park7;
+	uint8_t  acsm_odt_ctrl0;
+	uint8_t  acsm_odt_ctrl1;
+	uint8_t  acsm_odt_ctrl2;
+	uint8_t  acsm_odt_ctrl3;
+	uint8_t  acsm_odt_ctrl4;
+	uint8_t  acsm_odt_ctrl5;
+	uint8_t  acsm_odt_ctrl6;
+	uint8_t  acsm_odt_ctrl7;
+	uint8_t  vref_dq_r0nib0;
+	uint8_t  vref_dq_r0nib1;
+	uint8_t  vref_dq_r0nib2;
+	uint8_t  vref_dq_r0nib3;
+	uint8_t  vref_dq_r0nib4;
+	uint8_t  vref_dq_r0nib5;
+	uint8_t  vref_dq_r0nib6;
+	uint8_t  vref_dq_r0nib7;
+	uint8_t  vref_dq_r0nib8;
+	uint8_t  vref_dq_r0nib9;
+	uint8_t  vref_dq_r0nib10;
+	uint8_t  vref_dq_r0nib11;
+	uint8_t  vref_dq_r0nib12;
+	uint8_t  vref_dq_r0nib13;
+	uint8_t  vref_dq_r0nib14;
+	uint8_t  vref_dq_r0nib15;
+	uint8_t  vref_dq_r0nib16;
+	uint8_t  vref_dq_r0nib17;
+	uint8_t  vref_dq_r0nib18;
+	uint8_t  vref_dq_r0nib19;
+	uint8_t  vref_dq_r1nib0;
+	uint8_t  vref_dq_r1nib1;
+	uint8_t  vref_dq_r1nib2;
+	uint8_t  vref_dq_r1nib3;
+	uint8_t  vref_dq_r1nib4;
+	uint8_t  vref_dq_r1nib5;
+	uint8_t  vref_dq_r1nib6;
+	uint8_t  vref_dq_r1nib7;
+	uint8_t  vref_dq_r1nib8;
+	uint8_t  vref_dq_r1nib9;
+	uint8_t  vref_dq_r1nib10;
+	uint8_t  vref_dq_r1nib11;
+	uint8_t  vref_dq_r1nib12;
+	uint8_t  vref_dq_r1nib13;
+	uint8_t  vref_dq_r1nib14;
+	uint8_t  vref_dq_r1nib15;
+	uint8_t  vref_dq_r1nib16;
+	uint8_t  vref_dq_r1nib17;
+	uint8_t  vref_dq_r1nib18;
+	uint8_t  vref_dq_r1nib19;
+	uint8_t  vref_dq_r2nib0;
+	uint8_t  vref_dq_r2nib1;
+	uint8_t  vref_dq_r2nib2;
+	uint8_t  vref_dq_r2nib3;
+	uint8_t  vref_dq_r2nib4;
+	uint8_t  vref_dq_r2nib5;
+	uint8_t  vref_dq_r2nib6;
+	uint8_t  vref_dq_r2nib7;
+	uint8_t  vref_dq_r2nib8;
+	uint8_t  vref_dq_r2nib9;
+	uint8_t  vref_dq_r2nib10;
+	uint8_t  vref_dq_r2nib11;
+	uint8_t  vref_dq_r2nib12;
+	uint8_t  vref_dq_r2nib13;
+	uint8_t  vref_dq_r2nib14;
+	uint8_t  vref_dq_r2nib15;
+	uint8_t  vref_dq_r2nib16;
+	uint8_t  vref_dq_r2nib17;
+	uint8_t  vref_dq_r2nib18;
+	uint8_t  vref_dq_r2nib19;
+	uint8_t  vref_dq_r3nib0;
+	uint8_t  vref_dq_r3nib1;
+	uint8_t  vref_dq_r3nib2;
+	uint8_t  vref_dq_r3nib3;
+	uint8_t  vref_dq_r3nib4;
+	uint8_t  vref_dq_r3nib5;
+	uint8_t  vref_dq_r3nib6;
+	uint8_t  vref_dq_r3nib7;
+	uint8_t  vref_dq_r3nib8;
+	uint8_t  vref_dq_r3nib9;
+	uint8_t  vref_dq_r3nib10;
+	uint8_t  vref_dq_r3nib11;
+	uint8_t  vref_dq_r3nib12;
+	uint8_t  vref_dq_r3nib13;
+	uint8_t  vref_dq_r3nib14;
+	uint8_t  vref_dq_r3nib15;
+	uint8_t  vref_dq_r3nib16;
+	uint8_t  vref_dq_r3nib17;
+	uint8_t  vref_dq_r3nib18;
+	uint8_t  vref_dq_r3nib19;
+	uint8_t  f0rc00_d0;
+	uint8_t  f0rc01_d0;
+	uint8_t  f0rc02_d0;
+	uint8_t  f0rc03_d0;
+	uint8_t  f0rc04_d0;
+	uint8_t  f0rc05_d0;
+	uint8_t  f0rc06_d0;
+	uint8_t  f0rc07_d0;
+	uint8_t  f0rc08_d0;
+	uint8_t  f0rc09_d0;
+	uint8_t  f0rc0a_d0;
+	uint8_t  f0rc0b_d0;
+	uint8_t  f0rc0c_d0;
+	uint8_t  f0rc0d_d0;
+	uint8_t  f0rc0e_d0;
+	uint8_t  f0rc0f_d0;
+	uint8_t  f0rc1x_d0;
+	uint8_t  f0rc2x_d0;
+	uint8_t  f0rc3x_d0;
+	uint8_t  f0rc4x_d0;
+	uint8_t  f0rc5x_d0;
+	uint8_t  f0rc6x_d0;
+	uint8_t  f0rc7x_d0;
+	uint8_t  f0rc8x_d0;
+	uint8_t  f0rc9x_d0;
+	uint8_t  f0rcax_d0;
+	uint8_t  f0rcbx_d0;
+	uint8_t  f1rc00_d0;
+	uint8_t  f1rc01_d0;
+	uint8_t  f1rc02_d0;
+	uint8_t  f1rc03_d0;
+	uint8_t  f1rc04_d0;
+	uint8_t  f1rc05_d0;
+	uint8_t  f1rc06_d0;
+	uint8_t  f1rc07_d0;
+	uint8_t  f1rc08_d0;
+	uint8_t  f1rc09_d0;
+	uint8_t  f1rc0a_d0;
+	uint8_t  f1rc0b_d0;
+	uint8_t  f1rc0c_d0;
+	uint8_t  f1rc0d_d0;
+	uint8_t  f1rc0e_d0;
+	uint8_t  f1rc0f_d0;
+	uint8_t  f1rc1x_d0;
+	uint8_t  f1rc2x_d0;
+	uint8_t  f1rc3x_d0;
+	uint8_t  f1rc4x_d0;
+	uint8_t  f1rc5x_d0;
+	uint8_t  f1rc6x_d0;
+	uint8_t  f1rc7x_d0;
+	uint8_t  f1rc8x_d0;
+	uint8_t  f1rc9x_d0;
+	uint8_t  f1rcax_d0;
+	uint8_t  f1rcbx_d0;
+	uint8_t  f0rc00_d1;
+	uint8_t  f0rc01_d1;
+	uint8_t  f0rc02_d1;
+	uint8_t  f0rc03_d1;
+	uint8_t  f0rc04_d1;
+	uint8_t  f0rc05_d1;
+	uint8_t  f0rc06_d1;
+	uint8_t  f0rc07_d1;
+	uint8_t  f0rc08_d1;
+	uint8_t  f0rc09_d1;
+	uint8_t  f0rc0a_d1;
+	uint8_t  f0rc0b_d1;
+	uint8_t  f0rc0c_d1;
+	uint8_t  f0rc0d_d1;
+	uint8_t  f0rc0e_d1;
+	uint8_t  f0rc0f_d1;
+	uint8_t  f0rc1x_d1;
+	uint8_t  f0rc2x_d1;
+	uint8_t  f0rc3x_d1;
+	uint8_t  f0rc4x_d1;
+	uint8_t  f0rc5x_d1;
+	uint8_t  f0rc6x_d1;
+	uint8_t  f0rc7x_d1;
+	uint8_t  f0rc8x_d1;
+	uint8_t  f0rc9x_d1;
+	uint8_t  f0rcax_d1;
+	uint8_t  f0rcbx_d1;
+	uint8_t  f1rc00_d1;
+	uint8_t  f1rc01_d1;
+	uint8_t  f1rc02_d1;
+	uint8_t  f1rc03_d1;
+	uint8_t  f1rc04_d1;
+	uint8_t  f1rc05_d1;
+	uint8_t  f1rc06_d1;
+	uint8_t  f1rc07_d1;
+	uint8_t  f1rc08_d1;
+	uint8_t  f1rc09_d1;
+	uint8_t  f1rc0a_d1;
+	uint8_t  f1rc0b_d1;
+	uint8_t  f1rc0c_d1;
+	uint8_t  f1rc0d_d1;
+	uint8_t  f1rc0e_d1;
+	uint8_t  f1rc0f_d1;
+	uint8_t  f1rc1x_d1;
+	uint8_t  f1rc2x_d1;
+	uint8_t  f1rc3x_d1;
+	uint8_t  f1rc4x_d1;
+	uint8_t  f1rc5x_d1;
+	uint8_t  f1rc6x_d1;
+	uint8_t  f1rc7x_d1;
+	uint8_t  f1rc8x_d1;
+	uint8_t  f1rc9x_d1;
+	uint8_t  f1rcax_d1;
+	uint8_t  f1rcbx_d1;
+	uint8_t  bc00_d0;
+	uint8_t  bc01_d0;
+	uint8_t  bc02_d0;
+	uint8_t  bc03_d0;
+	uint8_t  bc04_d0;
+	uint8_t  bc05_d0;
+	uint8_t  bc06_d0;
+	uint8_t  bc07_d0;
+	uint8_t  bc08_d0;
+	uint8_t  bc09_d0;
+	uint8_t  bc0a_d0;
+	uint8_t  bc0b_d0;
+	uint8_t  bc0c_d0;
+	uint8_t  bc0d_d0;
+	uint8_t  bc0e_d0;
+	uint8_t  f0bc6x_d0;
+	uint8_t  f0bccx_d0;
+	uint8_t  f0bcdx_d0;
+	uint8_t  f0bcex_d0;
+	uint8_t  f0bcfx_d0;
+	uint8_t  f1bccx_d0;
+	uint8_t  f1bcdx_d0;
+	uint8_t  f1bcex_d0;
+	uint8_t  f1bcfx_d0;
+	uint8_t  f0bc2x_b0_d0;
+	uint8_t  f0bc3x_b0_d0;
+	uint8_t  f0bc4x_b0_d0;
+	uint8_t  f0bc5x_b0_d0;
+	uint8_t  f0bc8x_b0_d0;
+	uint8_t  f0bc9x_b0_d0;
+	uint8_t  f0bcax_b0_d0;
+	uint8_t  f0bcbx_b0_d0;
+	uint8_t  f1bc2x_b0_d0;
+	uint8_t  f1bc3x_b0_d0;
+	uint8_t  f1bc4x_b0_d0;
+	uint8_t  f1bc5x_b0_d0;
+	uint8_t  f1bc8x_b0_d0;
+	uint8_t  f1bc9x_b0_d0;
+	uint8_t  f1bcax_b0_d0;
+	uint8_t  f1bcbx_b0_d0;
+	uint8_t  f2bc2x_b0_d0;
+	uint8_t  f2bc3x_b0_d0;
+	uint8_t  f2bc4x_b0_d0;
+	uint8_t  f2bc5x_b0_d0;
+	uint8_t  f2bc8x_b0_d0;
+	uint8_t  f2bc9x_b0_d0;
+	uint8_t  f2bcax_b0_d0;
+	uint8_t  f2bcbx_b0_d0;
+	uint8_t  f3bc2x_b0_d0;
+	uint8_t  f3bc3x_b0_d0;
+	uint8_t  f3bc4x_b0_d0;
+	uint8_t  f3bc5x_b0_d0;
+	uint8_t  f3bc8x_b0_d0;
+	uint8_t  f3bc9x_b0_d0;
+	uint8_t  f3bcax_b0_d0;
+	uint8_t  f3bcbx_b0_d0;
+	uint8_t  f0bc2x_b1_d0;
+	uint8_t  f0bc3x_b1_d0;
+	uint8_t  f0bc4x_b1_d0;
+	uint8_t  f0bc5x_b1_d0;
+	uint8_t  f0bc8x_b1_d0;
+	uint8_t  f0bc9x_b1_d0;
+	uint8_t  f0bcax_b1_d0;
+	uint8_t  f0bcbx_b1_d0;
+	uint8_t  f1bc2x_b1_d0;
+	uint8_t  f1bc3x_b1_d0;
+	uint8_t  f1bc4x_b1_d0;
+	uint8_t  f1bc5x_b1_d0;
+	uint8_t  f1bc8x_b1_d0;
+	uint8_t  f1bc9x_b1_d0;
+	uint8_t  f1bcax_b1_d0;
+	uint8_t  f1bcbx_b1_d0;
+	uint8_t  f2bc2x_b1_d0;
+	uint8_t  f2bc3x_b1_d0;
+	uint8_t  f2bc4x_b1_d0;
+	uint8_t  f2bc5x_b1_d0;
+	uint8_t  f2bc8x_b1_d0;
+	uint8_t  f2bc9x_b1_d0;
+	uint8_t  f2bcax_b1_d0;
+	uint8_t  f2bcbx_b1_d0;
+	uint8_t  f3bc2x_b1_d0;
+	uint8_t  f3bc3x_b1_d0;
+	uint8_t  f3bc4x_b1_d0;
+	uint8_t  f3bc5x_b1_d0;
+	uint8_t  f3bc8x_b1_d0;
+	uint8_t  f3bc9x_b1_d0;
+	uint8_t  f3bcax_b1_d0;
+	uint8_t  f3bcbx_b1_d0;
+	uint8_t  f0bc2x_b2_d0;
+	uint8_t  f0bc3x_b2_d0;
+	uint8_t  f0bc4x_b2_d0;
+	uint8_t  f0bc5x_b2_d0;
+	uint8_t  f0bc8x_b2_d0;
+	uint8_t  f0bc9x_b2_d0;
+	uint8_t  f0bcax_b2_d0;
+	uint8_t  f0bcbx_b2_d0;
+	uint8_t  f1bc2x_b2_d0;
+	uint8_t  f1bc3x_b2_d0;
+	uint8_t  f1bc4x_b2_d0;
+	uint8_t  f1bc5x_b2_d0;
+	uint8_t  f1bc8x_b2_d0;
+	uint8_t  f1bc9x_b2_d0;
+	uint8_t  f1bcax_b2_d0;
+	uint8_t  f1bcbx_b2_d0;
+	uint8_t  f2bc2x_b2_d0;
+	uint8_t  f2bc3x_b2_d0;
+	uint8_t  f2bc4x_b2_d0;
+	uint8_t  f2bc5x_b2_d0;
+	uint8_t  f2bc8x_b2_d0;
+	uint8_t  f2bc9x_b2_d0;
+	uint8_t  f2bcax_b2_d0;
+	uint8_t  f2bcbx_b2_d0;
+	uint8_t  f3bc2x_b2_d0;
+	uint8_t  f3bc3x_b2_d0;
+	uint8_t  f3bc4x_b2_d0;
+	uint8_t  f3bc5x_b2_d0;
+	uint8_t  f3bc8x_b2_d0;
+	uint8_t  f3bc9x_b2_d0;
+	uint8_t  f3bcax_b2_d0;
+	uint8_t  f3bcbx_b2_d0;
+	uint8_t  f0bc2x_b3_d0;
+	uint8_t  f0bc3x_b3_d0;
+	uint8_t  f0bc4x_b3_d0;
+	uint8_t  f0bc5x_b3_d0;
+	uint8_t  f0bc8x_b3_d0;
+	uint8_t  f0bc9x_b3_d0;
+	uint8_t  f0bcax_b3_d0;
+	uint8_t  f0bcbx_b3_d0;
+	uint8_t  f1bc2x_b3_d0;
+	uint8_t  f1bc3x_b3_d0;
+	uint8_t  f1bc4x_b3_d0;
+	uint8_t  f1bc5x_b3_d0;
+	uint8_t  f1bc8x_b3_d0;
+	uint8_t  f1bc9x_b3_d0;
+	uint8_t  f1bcax_b3_d0;
+	uint8_t  f1bcbx_b3_d0;
+	uint8_t  f2bc2x_b3_d0;
+	uint8_t  f2bc3x_b3_d0;
+	uint8_t  f2bc4x_b3_d0;
+	uint8_t  f2bc5x_b3_d0;
+	uint8_t  f2bc8x_b3_d0;
+	uint8_t  f2bc9x_b3_d0;
+	uint8_t  f2bcax_b3_d0;
+	uint8_t  f2bcbx_b3_d0;
+	uint8_t  f3bc2x_b3_d0;
+	uint8_t  f3bc3x_b3_d0;
+	uint8_t  f3bc4x_b3_d0;
+	uint8_t  f3bc5x_b3_d0;
+	uint8_t  f3bc8x_b3_d0;
+	uint8_t  f3bc9x_b3_d0;
+	uint8_t  f3bcax_b3_d0;
+	uint8_t  f3bcbx_b3_d0;
+	uint8_t  f0bc2x_b4_d0;
+	uint8_t  f0bc3x_b4_d0;
+	uint8_t  f0bc4x_b4_d0;
+	uint8_t  f0bc5x_b4_d0;
+	uint8_t  f0bc8x_b4_d0;
+	uint8_t  f0bc9x_b4_d0;
+	uint8_t  f0bcax_b4_d0;
+	uint8_t  f0bcbx_b4_d0;
+	uint8_t  f1bc2x_b4_d0;
+	uint8_t  f1bc3x_b4_d0;
+	uint8_t  f1bc4x_b4_d0;
+	uint8_t  f1bc5x_b4_d0;
+	uint8_t  f1bc8x_b4_d0;
+	uint8_t  f1bc9x_b4_d0;
+	uint8_t  f1bcax_b4_d0;
+	uint8_t  f1bcbx_b4_d0;
+	uint8_t  f2bc2x_b4_d0;
+	uint8_t  f2bc3x_b4_d0;
+	uint8_t  f2bc4x_b4_d0;
+	uint8_t  f2bc5x_b4_d0;
+	uint8_t  f2bc8x_b4_d0;
+	uint8_t  f2bc9x_b4_d0;
+	uint8_t  f2bcax_b4_d0;
+	uint8_t  f2bcbx_b4_d0;
+	uint8_t  f3bc2x_b4_d0;
+	uint8_t  f3bc3x_b4_d0;
+	uint8_t  f3bc4x_b4_d0;
+	uint8_t  f3bc5x_b4_d0;
+	uint8_t  f3bc8x_b4_d0;
+	uint8_t  f3bc9x_b4_d0;
+	uint8_t  f3bcax_b4_d0;
+	uint8_t  f3bcbx_b4_d0;
+	uint8_t  f0bc2x_b5_d0;
+	uint8_t  f0bc3x_b5_d0;
+	uint8_t  f0bc4x_b5_d0;
+	uint8_t  f0bc5x_b5_d0;
+	uint8_t  f0bc8x_b5_d0;
+	uint8_t  f0bc9x_b5_d0;
+	uint8_t  f0bcax_b5_d0;
+	uint8_t  f0bcbx_b5_d0;
+	uint8_t  f1bc2x_b5_d0;
+	uint8_t  f1bc3x_b5_d0;
+	uint8_t  f1bc4x_b5_d0;
+	uint8_t  f1bc5x_b5_d0;
+	uint8_t  f1bc8x_b5_d0;
+	uint8_t  f1bc9x_b5_d0;
+	uint8_t  f1bcax_b5_d0;
+	uint8_t  f1bcbx_b5_d0;
+	uint8_t  f2bc2x_b5_d0;
+	uint8_t  f2bc3x_b5_d0;
+	uint8_t  f2bc4x_b5_d0;
+	uint8_t  f2bc5x_b5_d0;
+	uint8_t  f2bc8x_b5_d0;
+	uint8_t  f2bc9x_b5_d0;
+	uint8_t  f2bcax_b5_d0;
+	uint8_t  f2bcbx_b5_d0;
+	uint8_t  f3bc2x_b5_d0;
+	uint8_t  f3bc3x_b5_d0;
+	uint8_t  f3bc4x_b5_d0;
+	uint8_t  f3bc5x_b5_d0;
+	uint8_t  f3bc8x_b5_d0;
+	uint8_t  f3bc9x_b5_d0;
+	uint8_t  f3bcax_b5_d0;
+	uint8_t  f3bcbx_b5_d0;
+	uint8_t  f0bc2x_b6_d0;
+	uint8_t  f0bc3x_b6_d0;
+	uint8_t  f0bc4x_b6_d0;
+	uint8_t  f0bc5x_b6_d0;
+	uint8_t  f0bc8x_b6_d0;
+	uint8_t  f0bc9x_b6_d0;
+	uint8_t  f0bcax_b6_d0;
+	uint8_t  f0bcbx_b6_d0;
+	uint8_t  f1bc2x_b6_d0;
+	uint8_t  f1bc3x_b6_d0;
+	uint8_t  f1bc4x_b6_d0;
+	uint8_t  f1bc5x_b6_d0;
+	uint8_t  f1bc8x_b6_d0;
+	uint8_t  f1bc9x_b6_d0;
+	uint8_t  f1bcax_b6_d0;
+	uint8_t  f1bcbx_b6_d0;
+	uint8_t  f2bc2x_b6_d0;
+	uint8_t  f2bc3x_b6_d0;
+	uint8_t  f2bc4x_b6_d0;
+	uint8_t  f2bc5x_b6_d0;
+	uint8_t  f2bc8x_b6_d0;
+	uint8_t  f2bc9x_b6_d0;
+	uint8_t  f2bcax_b6_d0;
+	uint8_t  f2bcbx_b6_d0;
+	uint8_t  f3bc2x_b6_d0;
+	uint8_t  f3bc3x_b6_d0;
+	uint8_t  f3bc4x_b6_d0;
+	uint8_t  f3bc5x_b6_d0;
+	uint8_t  f3bc8x_b6_d0;
+	uint8_t  f3bc9x_b6_d0;
+	uint8_t  f3bcax_b6_d0;
+	uint8_t  f3bcbx_b6_d0;
+	uint8_t  f0bc2x_b7_d0;
+	uint8_t  f0bc3x_b7_d0;
+	uint8_t  f0bc4x_b7_d0;
+	uint8_t  f0bc5x_b7_d0;
+	uint8_t  f0bc8x_b7_d0;
+	uint8_t  f0bc9x_b7_d0;
+	uint8_t  f0bcax_b7_d0;
+	uint8_t  f0bcbx_b7_d0;
+	uint8_t  f1bc2x_b7_d0;
+	uint8_t  f1bc3x_b7_d0;
+	uint8_t  f1bc4x_b7_d0;
+	uint8_t  f1bc5x_b7_d0;
+	uint8_t  f1bc8x_b7_d0;
+	uint8_t  f1bc9x_b7_d0;
+	uint8_t  f1bcax_b7_d0;
+	uint8_t  f1bcbx_b7_d0;
+	uint8_t  f2bc2x_b7_d0;
+	uint8_t  f2bc3x_b7_d0;
+	uint8_t  f2bc4x_b7_d0;
+	uint8_t  f2bc5x_b7_d0;
+	uint8_t  f2bc8x_b7_d0;
+	uint8_t  f2bc9x_b7_d0;
+	uint8_t  f2bcax_b7_d0;
+	uint8_t  f2bcbx_b7_d0;
+	uint8_t  f3bc2x_b7_d0;
+	uint8_t  f3bc3x_b7_d0;
+	uint8_t  f3bc4x_b7_d0;
+	uint8_t  f3bc5x_b7_d0;
+	uint8_t  f3bc8x_b7_d0;
+	uint8_t  f3bc9x_b7_d0;
+	uint8_t  f3bcax_b7_d0;
+	uint8_t  f3bcbx_b7_d0;
+	uint8_t  f0bc2x_b8_d0;
+	uint8_t  f0bc3x_b8_d0;
+	uint8_t  f0bc4x_b8_d0;
+	uint8_t  f0bc5x_b8_d0;
+	uint8_t  f0bc8x_b8_d0;
+	uint8_t  f0bc9x_b8_d0;
+	uint8_t  f0bcax_b8_d0;
+	uint8_t  f0bcbx_b8_d0;
+	uint8_t  f1bc2x_b8_d0;
+	uint8_t  f1bc3x_b8_d0;
+	uint8_t  f1bc4x_b8_d0;
+	uint8_t  f1bc5x_b8_d0;
+	uint8_t  f1bc8x_b8_d0;
+	uint8_t  f1bc9x_b8_d0;
+	uint8_t  f1bcax_b8_d0;
+	uint8_t  f1bcbx_b8_d0;
+	uint8_t  f2bc2x_b8_d0;
+	uint8_t  f2bc3x_b8_d0;
+	uint8_t  f2bc4x_b8_d0;
+	uint8_t  f2bc5x_b8_d0;
+	uint8_t  f2bc8x_b8_d0;
+	uint8_t  f2bc9x_b8_d0;
+	uint8_t  f2bcax_b8_d0;
+	uint8_t  f2bcbx_b8_d0;
+	uint8_t  f3bc2x_b8_d0;
+	uint8_t  f3bc3x_b8_d0;
+	uint8_t  f3bc4x_b8_d0;
+	uint8_t  f3bc5x_b8_d0;
+	uint8_t  f3bc8x_b8_d0;
+	uint8_t  f3bc9x_b8_d0;
+	uint8_t  f3bcax_b8_d0;
+	uint8_t  f3bcbx_b8_d0;
+	uint8_t  f5bc5x_d0;
+	uint8_t  f5bc6x_d0;
+	uint8_t  f4bc8x_d0;
+	uint8_t  f4bc9x_d0;
+	uint8_t  f4bcax_d0;
+	uint8_t  f4bcbx_d0;
+	uint8_t  f4bccx_d0;
+	uint8_t  f4bcdx_d0;
+	uint8_t  f4bcex_d0;
+	uint8_t  f4bcfx_d0;
+	uint8_t  f5bc8x_d0;
+	uint8_t  f5bc9x_d0;
+	uint8_t  f5bcax_d0;
+	uint8_t  f5bcbx_d0;
+	uint8_t  f5bccx_d0;
+	uint8_t  f5bcdx_d0;
+	uint8_t  f5bcex_d0;
+	uint8_t  f5bcfx_d0;
+	uint8_t  f6bc8x_d0;
+	uint8_t  f6bc9x_d0;
+	uint8_t  f6bcax_d0;
+	uint8_t  f6bcbx_d0;
+	uint8_t  f6bccx_d0;
+	uint8_t  f6bcdx_d0;
+	uint8_t  f6bcex_d0;
+	uint8_t  f6bcfx_d0;
+	uint8_t  f7bc8x_d0;
+	uint8_t  f7bc9x_d0;
+	uint8_t  f7bcax_d0;
+	uint8_t  f7bcbx_d0;
+	uint8_t  f7bccx_d0;
+	uint8_t  f7bcdx_d0;
+	uint8_t  f7bcex_d0;
+	uint8_t  f7bcfx_d0;
+	uint8_t  bc00_d1;
+	uint8_t  bc01_d1;
+	uint8_t  bc02_d1;
+	uint8_t  bc03_d1;
+	uint8_t  bc04_d1;
+	uint8_t  bc05_d1;
+	uint8_t  bc06_d1;
+	uint8_t  bc07_d1;
+	uint8_t  bc08_d1;
+	uint8_t  bc09_d1;
+	uint8_t  bc0a_d1;
+	uint8_t  bc0b_d1;
+	uint8_t  bc0c_d1;
+	uint8_t  bc0d_d1;
+	uint8_t  bc0e_d1;
+	uint8_t  f0bc6x_d1;
+	uint8_t  f0bccx_d1;
+	uint8_t  f0bcdx_d1;
+	uint8_t  f0bcex_d1;
+	uint8_t  f0bcfx_d1;
+	uint8_t  f1bccx_d1;
+	uint8_t  f1bcdx_d1;
+	uint8_t  f1bcex_d1;
+	uint8_t  f1bcfx_d1;
+	uint8_t  f0bc2x_b0_d1;
+	uint8_t  f0bc3x_b0_d1;
+	uint8_t  f0bc4x_b0_d1;
+	uint8_t  f0bc5x_b0_d1;
+	uint8_t  f0bc8x_b0_d1;
+	uint8_t  f0bc9x_b0_d1;
+	uint8_t  f0bcax_b0_d1;
+	uint8_t  f0bcbx_b0_d1;
+	uint8_t  f1bc2x_b0_d1;
+	uint8_t  f1bc3x_b0_d1;
+	uint8_t  f1bc4x_b0_d1;
+	uint8_t  f1bc5x_b0_d1;
+	uint8_t  f1bc8x_b0_d1;
+	uint8_t  f1bc9x_b0_d1;
+	uint8_t  f1bcax_b0_d1;
+	uint8_t  f1bcbx_b0_d1;
+	uint8_t  f2bc2x_b0_d1;
+	uint8_t  f2bc3x_b0_d1;
+	uint8_t  f2bc4x_b0_d1;
+	uint8_t  f2bc5x_b0_d1;
+	uint8_t  f2bc8x_b0_d1;
+	uint8_t  f2bc9x_b0_d1;
+	uint8_t  f2bcax_b0_d1;
+	uint8_t  f2bcbx_b0_d1;
+	uint8_t  f3bc2x_b0_d1;
+	uint8_t  f3bc3x_b0_d1;
+	uint8_t  f3bc4x_b0_d1;
+	uint8_t  f3bc5x_b0_d1;
+	uint8_t  f3bc8x_b0_d1;
+	uint8_t  f3bc9x_b0_d1;
+	uint8_t  f3bcax_b0_d1;
+	uint8_t  f3bcbx_b0_d1;
+	uint8_t  f0bc2x_b1_d1;
+	uint8_t  f0bc3x_b1_d1;
+	uint8_t  f0bc4x_b1_d1;
+	uint8_t  f0bc5x_b1_d1;
+	uint8_t  f0bc8x_b1_d1;
+	uint8_t  f0bc9x_b1_d1;
+	uint8_t  f0bcax_b1_d1;
+	uint8_t  f0bcbx_b1_d1;
+	uint8_t  f1bc2x_b1_d1;
+	uint8_t  f1bc3x_b1_d1;
+	uint8_t  f1bc4x_b1_d1;
+	uint8_t  f1bc5x_b1_d1;
+	uint8_t  f1bc8x_b1_d1;
+	uint8_t  f1bc9x_b1_d1;
+	uint8_t  f1bcax_b1_d1;
+	uint8_t  f1bcbx_b1_d1;
+	uint8_t  f2bc2x_b1_d1;
+	uint8_t  f2bc3x_b1_d1;
+	uint8_t  f2bc4x_b1_d1;
+	uint8_t  f2bc5x_b1_d1;
+	uint8_t  f2bc8x_b1_d1;
+	uint8_t  f2bc9x_b1_d1;
+	uint8_t  f2bcax_b1_d1;
+	uint8_t  f2bcbx_b1_d1;
+	uint8_t  f3bc2x_b1_d1;
+	uint8_t  f3bc3x_b1_d1;
+	uint8_t  f3bc4x_b1_d1;
+	uint8_t  f3bc5x_b1_d1;
+	uint8_t  f3bc8x_b1_d1;
+	uint8_t  f3bc9x_b1_d1;
+	uint8_t  f3bcax_b1_d1;
+	uint8_t  f3bcbx_b1_d1;
+	uint8_t  f0bc2x_b2_d1;
+	uint8_t  f0bc3x_b2_d1;
+	uint8_t  f0bc4x_b2_d1;
+	uint8_t  f0bc5x_b2_d1;
+	uint8_t  f0bc8x_b2_d1;
+	uint8_t  f0bc9x_b2_d1;
+	uint8_t  f0bcax_b2_d1;
+	uint8_t  f0bcbx_b2_d1;
+	uint8_t  f1bc2x_b2_d1;
+	uint8_t  f1bc3x_b2_d1;
+	uint8_t  f1bc4x_b2_d1;
+	uint8_t  f1bc5x_b2_d1;
+	uint8_t  f1bc8x_b2_d1;
+	uint8_t  f1bc9x_b2_d1;
+	uint8_t  f1bcax_b2_d1;
+	uint8_t  f1bcbx_b2_d1;
+	uint8_t  f2bc2x_b2_d1;
+	uint8_t  f2bc3x_b2_d1;
+	uint8_t  f2bc4x_b2_d1;
+	uint8_t  f2bc5x_b2_d1;
+	uint8_t  f2bc8x_b2_d1;
+	uint8_t  f2bc9x_b2_d1;
+	uint8_t  f2bcax_b2_d1;
+	uint8_t  f2bcbx_b2_d1;
+	uint8_t  f3bc2x_b2_d1;
+	uint8_t  f3bc3x_b2_d1;
+	uint8_t  f3bc4x_b2_d1;
+	uint8_t  f3bc5x_b2_d1;
+	uint8_t  f3bc8x_b2_d1;
+	uint8_t  f3bc9x_b2_d1;
+	uint8_t  f3bcax_b2_d1;
+	uint8_t  f3bcbx_b2_d1;
+	uint8_t  f0bc2x_b3_d1;
+	uint8_t  f0bc3x_b3_d1;
+	uint8_t  f0bc4x_b3_d1;
+	uint8_t  f0bc5x_b3_d1;
+	uint8_t  f0bc8x_b3_d1;
+	uint8_t  f0bc9x_b3_d1;
+	uint8_t  f0bcax_b3_d1;
+	uint8_t  f0bcbx_b3_d1;
+	uint8_t  f1bc2x_b3_d1;
+	uint8_t  f1bc3x_b3_d1;
+	uint8_t  f1bc4x_b3_d1;
+	uint8_t  f1bc5x_b3_d1;
+	uint8_t  f1bc8x_b3_d1;
+	uint8_t  f1bc9x_b3_d1;
+	uint8_t  f1bcax_b3_d1;
+	uint8_t  f1bcbx_b3_d1;
+	uint8_t  f2bc2x_b3_d1;
+	uint8_t  f2bc3x_b3_d1;
+	uint8_t  f2bc4x_b3_d1;
+	uint8_t  f2bc5x_b3_d1;
+	uint8_t  f2bc8x_b3_d1;
+	uint8_t  f2bc9x_b3_d1;
+	uint8_t  f2bcax_b3_d1;
+	uint8_t  f2bcbx_b3_d1;
+	uint8_t  f3bc2x_b3_d1;
+	uint8_t  f3bc3x_b3_d1;
+	uint8_t  f3bc4x_b3_d1;
+	uint8_t  f3bc5x_b3_d1;
+	uint8_t  f3bc8x_b3_d1;
+	uint8_t  f3bc9x_b3_d1;
+	uint8_t  f3bcax_b3_d1;
+	uint8_t  f3bcbx_b3_d1;
+	uint8_t  f0bc2x_b4_d1;
+	uint8_t  f0bc3x_b4_d1;
+	uint8_t  f0bc4x_b4_d1;
+	uint8_t  f0bc5x_b4_d1;
+	uint8_t  f0bc8x_b4_d1;
+	uint8_t  f0bc9x_b4_d1;
+	uint8_t  f0bcax_b4_d1;
+	uint8_t  f0bcbx_b4_d1;
+	uint8_t  f1bc2x_b4_d1;
+	uint8_t  f1bc3x_b4_d1;
+	uint8_t  f1bc4x_b4_d1;
+	uint8_t  f1bc5x_b4_d1;
+	uint8_t  f1bc8x_b4_d1;
+	uint8_t  f1bc9x_b4_d1;
+	uint8_t  f1bcax_b4_d1;
+	uint8_t  f1bcbx_b4_d1;
+	uint8_t  f2bc2x_b4_d1;
+	uint8_t  f2bc3x_b4_d1;
+	uint8_t  f2bc4x_b4_d1;
+	uint8_t  f2bc5x_b4_d1;
+	uint8_t  f2bc8x_b4_d1;
+	uint8_t  f2bc9x_b4_d1;
+	uint8_t  f2bcax_b4_d1;
+	uint8_t  f2bcbx_b4_d1;
+	uint8_t  f3bc2x_b4_d1;
+	uint8_t  f3bc3x_b4_d1;
+	uint8_t  f3bc4x_b4_d1;
+	uint8_t  f3bc5x_b4_d1;
+	uint8_t  f3bc8x_b4_d1;
+	uint8_t  f3bc9x_b4_d1;
+	uint8_t  f3bcax_b4_d1;
+	uint8_t  f3bcbx_b4_d1;
+	uint8_t  f0bc2x_b5_d1;
+	uint8_t  f0bc3x_b5_d1;
+	uint8_t  f0bc4x_b5_d1;
+	uint8_t  f0bc5x_b5_d1;
+	uint8_t  f0bc8x_b5_d1;
+	uint8_t  f0bc9x_b5_d1;
+	uint8_t  f0bcax_b5_d1;
+	uint8_t  f0bcbx_b5_d1;
+	uint8_t  f1bc2x_b5_d1;
+	uint8_t  f1bc3x_b5_d1;
+	uint8_t  f1bc4x_b5_d1;
+	uint8_t  f1bc5x_b5_d1;
+	uint8_t  f1bc8x_b5_d1;
+	uint8_t  f1bc9x_b5_d1;
+	uint8_t  f1bcax_b5_d1;
+	uint8_t  f1bcbx_b5_d1;
+	uint8_t  f2bc2x_b5_d1;
+	uint8_t  f2bc3x_b5_d1;
+	uint8_t  f2bc4x_b5_d1;
+	uint8_t  f2bc5x_b5_d1;
+	uint8_t  f2bc8x_b5_d1;
+	uint8_t  f2bc9x_b5_d1;
+	uint8_t  f2bcax_b5_d1;
+	uint8_t  f2bcbx_b5_d1;
+	uint8_t  f3bc2x_b5_d1;
+	uint8_t  f3bc3x_b5_d1;
+	uint8_t  f3bc4x_b5_d1;
+	uint8_t  f3bc5x_b5_d1;
+	uint8_t  f3bc8x_b5_d1;
+	uint8_t  f3bc9x_b5_d1;
+	uint8_t  f3bcax_b5_d1;
+	uint8_t  f3bcbx_b5_d1;
+	uint8_t  f0bc2x_b6_d1;
+	uint8_t  f0bc3x_b6_d1;
+	uint8_t  f0bc4x_b6_d1;
+	uint8_t  f0bc5x_b6_d1;
+	uint8_t  f0bc8x_b6_d1;
+	uint8_t  f0bc9x_b6_d1;
+	uint8_t  f0bcax_b6_d1;
+	uint8_t  f0bcbx_b6_d1;
+	uint8_t  f1bc2x_b6_d1;
+	uint8_t  f1bc3x_b6_d1;
+	uint8_t  f1bc4x_b6_d1;
+	uint8_t  f1bc5x_b6_d1;
+	uint8_t  f1bc8x_b6_d1;
+	uint8_t  f1bc9x_b6_d1;
+	uint8_t  f1bcax_b6_d1;
+	uint8_t  f1bcbx_b6_d1;
+	uint8_t  f2bc2x_b6_d1;
+	uint8_t  f2bc3x_b6_d1;
+	uint8_t  f2bc4x_b6_d1;
+	uint8_t  f2bc5x_b6_d1;
+	uint8_t  f2bc8x_b6_d1;
+	uint8_t  f2bc9x_b6_d1;
+	uint8_t  f2bcax_b6_d1;
+	uint8_t  f2bcbx_b6_d1;
+	uint8_t  f3bc2x_b6_d1;
+	uint8_t  f3bc3x_b6_d1;
+	uint8_t  f3bc4x_b6_d1;
+	uint8_t  f3bc5x_b6_d1;
+	uint8_t  f3bc8x_b6_d1;
+	uint8_t  f3bc9x_b6_d1;
+	uint8_t  f3bcax_b6_d1;
+	uint8_t  f3bcbx_b6_d1;
+	uint8_t  f0bc2x_b7_d1;
+	uint8_t  f0bc3x_b7_d1;
+	uint8_t  f0bc4x_b7_d1;
+	uint8_t  f0bc5x_b7_d1;
+	uint8_t  f0bc8x_b7_d1;
+	uint8_t  f0bc9x_b7_d1;
+	uint8_t  f0bcax_b7_d1;
+	uint8_t  f0bcbx_b7_d1;
+	uint8_t  f1bc2x_b7_d1;
+	uint8_t  f1bc3x_b7_d1;
+	uint8_t  f1bc4x_b7_d1;
+	uint8_t  f1bc5x_b7_d1;
+	uint8_t  f1bc8x_b7_d1;
+	uint8_t  f1bc9x_b7_d1;
+	uint8_t  f1bcax_b7_d1;
+	uint8_t  f1bcbx_b7_d1;
+	uint8_t  f2bc2x_b7_d1;
+	uint8_t  f2bc3x_b7_d1;
+	uint8_t  f2bc4x_b7_d1;
+	uint8_t  f2bc5x_b7_d1;
+	uint8_t  f2bc8x_b7_d1;
+	uint8_t  f2bc9x_b7_d1;
+	uint8_t  f2bcax_b7_d1;
+	uint8_t  f2bcbx_b7_d1;
+	uint8_t  f3bc2x_b7_d1;
+	uint8_t  f3bc3x_b7_d1;
+	uint8_t  f3bc4x_b7_d1;
+	uint8_t  f3bc5x_b7_d1;
+	uint8_t  f3bc8x_b7_d1;
+	uint8_t  f3bc9x_b7_d1;
+	uint8_t  f3bcax_b7_d1;
+	uint8_t  f3bcbx_b7_d1;
+	uint8_t  f0bc2x_b8_d1;
+	uint8_t  f0bc3x_b8_d1;
+	uint8_t  f0bc4x_b8_d1;
+	uint8_t  f0bc5x_b8_d1;
+	uint8_t  f0bc8x_b8_d1;
+	uint8_t  f0bc9x_b8_d1;
+	uint8_t  f0bcax_b8_d1;
+	uint8_t  f0bcbx_b8_d1;
+	uint8_t  f1bc2x_b8_d1;
+	uint8_t  f1bc3x_b8_d1;
+	uint8_t  f1bc4x_b8_d1;
+	uint8_t  f1bc5x_b8_d1;
+	uint8_t  f1bc8x_b8_d1;
+	uint8_t  f1bc9x_b8_d1;
+	uint8_t  f1bcax_b8_d1;
+	uint8_t  f1bcbx_b8_d1;
+	uint8_t  f2bc2x_b8_d1;
+	uint8_t  f2bc3x_b8_d1;
+	uint8_t  f2bc4x_b8_d1;
+	uint8_t  f2bc5x_b8_d1;
+	uint8_t  f2bc8x_b8_d1;
+	uint8_t  f2bc9x_b8_d1;
+	uint8_t  f2bcax_b8_d1;
+	uint8_t  f2bcbx_b8_d1;
+	uint8_t  f3bc2x_b8_d1;
+	uint8_t  f3bc3x_b8_d1;
+	uint8_t  f3bc4x_b8_d1;
+	uint8_t  f3bc5x_b8_d1;
+	uint8_t  f3bc8x_b8_d1;
+	uint8_t  f3bc9x_b8_d1;
+	uint8_t  f3bcax_b8_d1;
+	uint8_t  f3bcbx_b8_d1;
+	uint8_t  f5bc5x_d1;
+	uint8_t  f5bc6x_d1;
+	uint8_t  f4bc8x_d1;
+	uint8_t  f4bc9x_d1;
+	uint8_t  f4bcax_d1;
+	uint8_t  f4bcbx_d1;
+	uint8_t  f4bccx_d1;
+	uint8_t  f4bcdx_d1;
+	uint8_t  f4bcex_d1;
+	uint8_t  f4bcfx_d1;
+	uint8_t  f5bc8x_d1;
+	uint8_t  f5bc9x_d1;
+	uint8_t  f5bcax_d1;
+	uint8_t  f5bcbx_d1;
+	uint8_t  f5bccx_d1;
+	uint8_t  f5bcdx_d1;
+	uint8_t  f5bcex_d1;
+	uint8_t  f5bcfx_d1;
+	uint8_t  f6bc8x_d1;
+	uint8_t  f6bc9x_d1;
+	uint8_t  f6bcax_d1;
+	uint8_t  f6bcbx_d1;
+	uint8_t  f6bccx_d1;
+	uint8_t  f6bcdx_d1;
+	uint8_t  f6bcex_d1;
+	uint8_t  f6bcfx_d1;
+	uint8_t  f7bc8x_d1;
+	uint8_t  f7bc9x_d1;
+	uint8_t  f7bcax_d1;
+	uint8_t  f7bcbx_d1;
+	uint8_t  f7bccx_d1;
+	uint8_t  f7bcdx_d1;
+	uint8_t  f7bcex_d1;
+	uint8_t  f7bcfx_d1;
+	uint16_t alt_cas_l;
+	uint8_t  alt_wcas_l;
+	uint8_t  d4misc;
+} __packed;
+#endif
diff --git a/drivers/nxp/ddr/phy-gen2/ddrphy.mk b/drivers/nxp/ddr/phy-gen2/ddrphy.mk
new file mode 100644
index 0000000..ba5c774
--- /dev/null
+++ b/drivers/nxp/ddr/phy-gen2/ddrphy.mk
@@ -0,0 +1,20 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#
+#-----------------------------------------------------------------------------
+
+# SNPS ddr phy driver files
+
+DDR_PHY_C  =
+DDR_PHY_H  =
+
+$(DDR_PHY_C): $(DDR_PHY_H) $(COMMON_HDRS) src
+	@cp -r "$(DDR_PHY_PATH)/$@" "$(SRC_DIR)/$@"
+
+$(DDR_PHY_H): src
+	@cp -r "$(DDR_PHY_PATH)/$@" "$(SRC_DIR)/$@"
+
+#------------------------------------------------
diff --git a/drivers/nxp/ddr/phy-gen2/input.h b/drivers/nxp/ddr/phy-gen2/input.h
new file mode 100644
index 0000000..dbcd1ae
--- /dev/null
+++ b/drivers/nxp/ddr/phy-gen2/input.h
@@ -0,0 +1,106 @@
+/*
+ * Copyright 2021 NXP
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef _INPUT_H_
+#define _INPUT_H_
+
+enum dram_types {
+	DDR4,
+	DDR3,
+	LPDDR4,
+	LPDDR3,
+	LPDDR2,
+	DDR5,
+};
+
+enum dimm_types {
+	UDIMM,
+	SODIMM,
+	RDIMM,
+	LRDIMM,
+	NODIMM,
+};
+
+struct input_basic {
+	enum dram_types dram_type;
+	enum dimm_types dimm_type;
+	int lp4x_mode;		/* 0x1 = lpddr4x mode, when dram_type is lpddr4
+				 */
+				/* not used for protocols other than lpddr4 */
+	int num_dbyte;		/* number of dbytes physically instantiated */
+	int num_active_dbyte_dfi0;	/* number of active dbytes to be
+					 * controlled by dfi0
+					 */
+	int num_active_dbyte_dfi1;	/* number of active dbytes to be
+					 * controlled by  dfi1. Not used for
+					 * protocols other than lpddr3 and
+					 * lpddr4
+					 */
+	int num_anib;		/* number of anibs physically instantiated */
+	int num_rank_dfi0;	/* number of ranks in dfi0 channel */
+	int num_rank_dfi1;	/* number of ranks in dfi1 channel */
+	int dram_data_width;	/* 4,8,16 or 32 depending on protocol and dram
+				 * type
+				 */
+	int num_pstates;
+	int frequency;		/* memclk frequency in mhz -- round up */
+	int pll_bypass;		/* pll bypass enable */
+	int dfi_freq_ratio;	/* selected dfi frequency ratio */
+	int dfi1exists;		/* whether they phy config has dfi1 channel */
+	int train2d;
+	int hard_macro_ver;
+	int read_dbienable;
+	int dfi_mode;		/* no longer used */
+};
+
+struct input_advanced {
+	int d4rx_preamble_length;
+	int d4tx_preamble_length;
+	int ext_cal_res_val;	/* external pull-down resistor */
+	int is2ttiming;
+	int odtimpedance;
+	int tx_impedance;
+	int atx_impedance;
+	int mem_alert_en;
+	int mem_alert_puimp;
+	int mem_alert_vref_level;
+	int mem_alert_sync_bypass;
+	int dis_dyn_adr_tri;
+	int phy_mstr_train_interval;
+	int phy_mstr_max_req_to_ack;
+	int wdqsext;
+	int cal_interval;
+	int cal_once;
+	int dram_byte_swap;
+	int rx_en_back_off;
+	int train_sequence_ctrl;
+	int phy_gen2_umctl_opt;
+	int phy_gen2_umctl_f0rc5x;
+	int tx_slew_rise_dq;
+	int tx_slew_fall_dq;
+	int tx_slew_rise_ac;
+	int tx_slew_fall_ac;
+	int enable_high_clk_skew_fix;
+	int disable_unused_addr_lns;
+	int phy_init_sequence_num;
+	int cs_mode;		/* rdimm */
+	int cast_cs_to_cid;	/* rdimm */
+};
+
+struct input {
+	struct input_basic basic;
+	struct input_advanced adv;
+	unsigned int mr[7];
+	unsigned int cs_d0;
+	unsigned int cs_d1;
+	unsigned int mirror;
+	unsigned int odt[4];
+	unsigned int rcw[16];
+	unsigned int rcw3x;
+	unsigned int vref;
+};
+
+#endif
diff --git a/drivers/nxp/ddr/phy-gen2/messages.h b/drivers/nxp/ddr/phy-gen2/messages.h
new file mode 100644
index 0000000..7dec7df
--- /dev/null
+++ b/drivers/nxp/ddr/phy-gen2/messages.h
@@ -0,0 +1,2909 @@
+/*
+ * Copyright 2021 NXP
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef MESSAGE_H
+#define MESSAGE_H
+
+#ifdef DEBUG
+struct phy_msg {
+	uint32_t index;
+	const char *msg;
+};
+
+const static struct phy_msg messages_1d[] = {
+	{0x00000001,
+	 "PMU1:prbsGenCtl:%x\n"
+	},
+	{0x00010000,
+	 "PMU1: loading 2D acsm sequence\n"
+	},
+	{0x00020000,
+	 "PMU1: loading 1D acsm sequence\n"
+	},
+	{0x00030002,
+	 "PMU3: %d memclocks @ %d to get half of 300ns\n"
+	},
+	{0x00040000,
+	 "PMU: Error: User requested MPR read pattern for read DQS training in DDR3 Mode\n"
+	},
+	{0x00050000,
+	 "PMU3: Running 1D search for left eye edge\n"
+	},
+	{0x00060001,
+	 "PMU1: In Phase Left Edge Search cs %d\n"
+	},
+	{0x00070001,
+	 "PMU1: Out of Phase Left Edge Search cs %d\n"
+	},
+	{0x00080000,
+	 "PMU3: Running 1D search for right eye edge\n"
+	},
+	{0x00090001,
+	 "PMU1: In Phase Right Edge Search cs %d\n"
+	},
+	{0x000a0001,
+	 "PMU1: Out of Phase Right Edge Search cs %d\n"
+	},
+	{0x000b0001,
+	 "PMU1: mxRdLat training pstate %d\n"
+	},
+	{0x000c0001,
+	 "PMU1: mxRdLat search for cs %d\n"
+	},
+	{0x000d0001,
+	 "PMU0: MaxRdLat non consistent DtsmLoThldXingInd 0x%03x\n"
+	},
+	{0x000e0003,
+	 "PMU4: CS %d Dbyte %d worked with DFIMRL = %d DFICLKs\n"
+	},
+	{0x000f0004,
+	 "PMU3: MaxRdLat Read Lane err mask for csn %d, DFIMRL %2d DFIClks, dbyte %d = 0x%03x\n"
+	},
+	{0x00100003,
+	 "PMU3: MaxRdLat Read Lane err mask for csn %d DFIMRL %2d, All dbytes = 0x%03x\n"
+	},
+	{0x00110001,
+	 "PMU: Error: CS%d failed to find a DFIMRL setting that worked for all bytes during MaxRdLat training\n"
+	},
+	{0x00120002,
+	 "PMU3: Smallest passing DFIMRL for all dbytes in CS%d = %d DFIClks\n"
+	},
+	{0x00130000,
+	 "PMU: Error: No passing DFIMRL value found for any chip select during MaxRdLat training\n"
+	},
+	{0x00140003,
+	 "PMU: Error: Dbyte %d lane %d txDqDly passing region is too small (width = %d)\n"
+	},
+	{0x00150006,
+	 "PMU10: Adjusting rxclkdly db %d nib %d from %d+%d=%d->%d\n"
+	},
+	{0x00160000,
+	 "PMU4: TxDqDly Passing Regions (EyeLeft EyeRight -> EyeCenter) Units=1/32 UI\n"
+	},
+	{0x00170005,
+	 "PMU4: DB %d Lane %d: %3d %3d -> %3d\n"
+	},
+	{0x00180002,
+	 "PMU2: TXDQ delayLeft[%2d] = %3d (DISCONNECTED)\n"
+	},
+	{0x00190004,
+	 "PMU2: TXDQ delayLeft[%2d] = %3d oopScaled = %3d selectOop %d\n"
+	},
+	{0x001a0002,
+	 "PMU2: TXDQ delayRight[%2d] = %3d (DISCONNECTED)\n"
+	},
+	{0x001b0004,
+	 "PMU2: TXDQ delayRight[%2d] = %3d oopScaled = %3d selectOop %d\n"
+	},
+	{0x001c0003,
+	 "PMU: Error: Dbyte %d lane %d txDqDly passing region is too small (width = %d)\n"
+	},
+	{0x001d0000,
+	 "PMU4: TxDqDly Passing Regions (EyeLeft EyeRight -> EyeCenter) Units=1/32 UI\n"
+	},
+	{0x001e0002,
+	 "PMU4: DB %d Lane %d: (DISCONNECTED)\n"
+	},
+	{0x001f0005,
+	 "PMU4: DB %d Lane %d: %3d %3d -> %3d\n"
+	},
+	{0x00200002,
+	 "PMU3: Running 1D search csn %d for DM Right/NotLeft(%d) eye edge\n"
+	},
+	{0x00210002,
+	 "PMU3: WrDq DM byte%2d with Errcnt %d\n"
+	},
+	{0x00220002,
+	 "PMU3: WrDq DM byte%2d avgDly 0x%04x\n"
+	},
+	{0x00230002,
+	 "PMU1: WrDq DM byte%2d with Errcnt %d\n"
+	},
+	{0x00240001,
+	 "PMU: Error: Dbyte %d txDqDly DM training did not start inside the eye\n"
+	},
+	{0x00250000,
+	 "PMU4: DM TxDqDly Passing Regions (EyeLeft EyeRight -> EyeCenter) Units=1/32 UI\n"
+	},
+	{0x00260002,
+	 "PMU4: DB %d Lane %d: (DISCONNECTED)\n"
+	},
+	{0x00270005,
+	 "PMU4: DB %d Lane %d: %3d %3d -> %3d\n"
+	},
+	{0x00280003,
+	 "PMU: Error: Dbyte %d lane %d txDqDly DM passing region is too small (width = %d)\n"
+	},
+	{0x00290004,
+	 "PMU3: Errcnt for MRD/MWD search nib %2d delay = (%d, 0x%02x) = %d\n"
+	},
+	{0x002a0000,
+	 "PMU3: Precharge all open banks\n"
+	},
+	{0x002b0002,
+	 "PMU: Error: Dbyte %d nibble %d found mutliple working coarse delay setting for MRD/MWD\n"
+	},
+	{0x002c0000,
+	 "PMU4: MRD Passing Regions (coarseVal, fineLeft fineRight -> fineCenter)\n"
+	},
+	{0x002d0000,
+	 "PMU4: MWD Passing Regions (coarseVal, fineLeft fineRight -> fineCenter)\n"
+	},
+	{0x002e0004,
+	 "PMU10: Warning: DB %d nibble %d has multiple working coarse delays, %d and %d, choosing the smaller delay\n"
+	},
+	{0x002f0003,
+	 "PMU: Error: Dbyte %d nibble %d MRD/MWD passing region is too small (width = %d)\n"
+	},
+	{0x00300006,
+	 "PMU4: DB %d nibble %d: %3d, %3d %3d -> %3d\n"
+	},
+	{0x00310002,
+	 "PMU1: Start MRD/nMWD %d for csn %d\n"
+	},
+	{0x00320002,
+	 "PMU2: RXDQS delayLeft[%2d] = %3d (DISCONNECTED)\n"
+	},
+	{0x00330006,
+	 "PMU2: RXDQS delayLeft[%2d] = %3d delayOop[%2d] = %3d OopScaled %4d, selectOop %d\n"
+	},
+	{0x00340002,
+	 "PMU2: RXDQS delayRight[%2d] = %3d (DISCONNECTED)\n"
+	},
+	{0x00350006,
+	 "PMU2: RXDQS delayRight[%2d] = %3d delayOop[%2d] = %4d OopScaled %4d, selectOop %d\n"
+	},
+	{0x00360000,
+	 "PMU4: RxClkDly Passing Regions (EyeLeft EyeRight -> EyeCenter)\n"
+	},
+	{0x00370002,
+	 "PMU4: DB %d nibble %d: (DISCONNECTED)\n"
+	},
+	{0x00380005,
+	 "PMU4: DB %d nibble %d: %3d %3d -> %3d\n"
+	},
+	{0x00390003,
+	 "PMU: Error: Dbyte %d nibble %d rxClkDly passing region is too small (width = %d)\n"
+	},
+	{0x003a0002,
+	 "PMU0: goodbar = %d for RDWR_BLEN %d\n"
+	},
+	{0x003b0001,
+	 "PMU3: RxClkDly = %d\n"
+	},
+	{0x003c0005,
+	 "PMU0: db %d l %d absLane %d -> bottom %d top %d\n"
+	},
+	{0x003d0009,
+	 "PMU3: BYTE %d - %3d %3d %3d %3d %3d %3d %3d %3d\n"
+	},
+	{0x003e0002,
+	 "PMU: Error: dbyte %d lane %d's per-lane vrefDAC's had no passing region\n"
+	},
+	{0x003f0004,
+	 "PMU0: db%d l%d - %d %d\n"
+	},
+	{0x00400002,
+	 "PMU0: goodbar = %d for RDWR_BLEN %d\n"
+	},
+	{0x00410004,
+	 "PMU3: db%d l%d saw %d issues at rxClkDly %d\n"
+	},
+	{0x00420003,
+	 "PMU3: db%d l%d first saw a pass->fail edge at rxClkDly %d\n"
+	},
+	{0x00430002,
+	 "PMU3: lane %d PBD = %d\n"
+	},
+	{0x00440003,
+	 "PMU3: db%d l%d first saw a DBI pass->fail edge at rxClkDly %d\n"
+	},
+	{0x00450003,
+	 "PMU2: db%d l%d already passed rxPBD = %d\n"
+	},
+	{0x00460003,
+	 "PMU0: db%d l%d, PBD = %d\n"
+	},
+	{0x00470002,
+	 "PMU: Error: dbyte %d lane %d failed read deskew\n"
+	},
+	{0x00480003,
+	 "PMU0: db%d l%d, inc PBD = %d\n"
+	},
+	{0x00490003,
+	 "PMU1: Running lane deskew on pstate %d csn %d rdDBIEn %d\n"
+	},
+	{0x004a0000,
+	 "PMU: Error: Read deskew training has been requested, but csrMajorModeDbyte[2] is set\n"
+	},
+	{0x004b0002,
+	 "PMU1: AcsmCsMapCtrl%02d 0x%04x\n"
+	},
+	{0x004c0002,
+	 "PMU1: AcsmCsMapCtrl%02d 0x%04x\n"
+	},
+	{0x004d0001,
+	 "PMU: Error: Wrong PMU image loaded. message Block DramType = 0x%02x, but image built for D3U Type\n"
+	},
+	{0x004e0001,
+	 "PMU: Error: Wrong PMU image loaded. message Block DramType = 0x%02x, but image built for D3R Type\n"
+	},
+	{0x004f0001,
+	 "PMU: Error: Wrong PMU image loaded. message Block DramType = 0x%02x, but image built for D4U Type\n"
+	},
+	{0x00500001,
+	 "PMU: Error: Wrong PMU image loaded. message Block DramType = 0x%02x, but image built for D4R Type\n"
+	},
+	{0x00510001,
+	 "PMU: Error: Wrong PMU image loaded. message Block DramType = 0x%02x, but image built for D4LR Type\n"
+	},
+	{0x00520000,
+	 "PMU: Error: Both 2t timing mode and ddr4 geardown mode specified in the messageblock's PhyCfg and MR3 fields. Only one can be enabled\n"
+	},
+	{0x00530003,
+	 "PMU10: PHY TOTALS - NUM_DBYTES %d NUM_NIBBLES %d NUM_ANIBS %d\n"
+	},
+	{0x00540006,
+	 "PMU10: CSA=0x%02x, CSB=0x%02x, TSTAGES=0x%04x, HDTOUT=%d, MMISC=%d DRAMFreq=%dMT DramType=LPDDR3\n"
+	},
+	{0x00550006,
+	 "PMU10: CSA=0x%02x, CSB=0x%02x, TSTAGES=0x%04x, HDTOUT=%d, MMISC=%d DRAMFreq=%dMT DramType=LPDDR4\n"
+	},
+	{0x00560008,
+	 "PMU10: CS=0x%02x, TSTAGES=0x%04x, HDTOUT=%d, 2T=%d, MMISC=%d AddrMirror=%d DRAMFreq=%dMT DramType=%d\n"
+	},
+	{0x00570004,
+	 "PMU10: Pstate%d MR0=0x%04x MR1=0x%04x MR2=0x%04x\n"
+	},
+	{0x00580008,
+	 "PMU10: Pstate%d MRS MR0=0x%04x MR1=0x%04x MR2=0x%04x MR3=0x%04x MR4=0x%04x MR5=0x%04x MR6=0x%04x\n"
+	},
+	{0x00590005,
+	 "PMU10: Pstate%d MRS MR1_A0=0x%04x MR2_A0=0x%04x MR3_A0=0x%04x MR11_A0=0x%04x\n"
+	},
+	{0x005a0000,
+	 "PMU10: UseBroadcastMR set. All ranks and channels use MRXX_A0 for MR settings.\n"
+	},
+	{0x005b0005,
+	 "PMU10: Pstate%d MRS MR01_A0=0x%02x MR02_A0=0x%02x MR03_A0=0x%02x MR11_A0=0x%02x\n"
+	},
+	{0x005c0005,
+	 "PMU10: Pstate%d MRS MR12_A0=0x%02x MR13_A0=0x%02x MR14_A0=0x%02x MR22_A0=0x%02x\n"
+	},
+	{0x005d0005,
+	 "PMU10: Pstate%d MRS MR01_A1=0x%02x MR02_A1=0x%02x MR03_A1=0x%02x MR11_A1=0x%02x\n"
+	},
+	{0x005e0005,
+	 "PMU10: Pstate%d MRS MR12_A1=0x%02x MR13_A1=0x%02x MR14_A1=0x%02x MR22_A1=0x%02x\n"
+	},
+	{0x005f0005,
+	 "PMU10: Pstate%d MRS MR01_B0=0x%02x MR02_B0=0x%02x MR03_B0=0x%02x MR11_B0=0x%02x\n"
+	},
+	{0x00600005,
+	 "PMU10: Pstate%d MRS MR12_B0=0x%02x MR13_B0=0x%02x MR14_B0=0x%02x MR22_B0=0x%02x\n"
+	},
+	{0x00610005,
+	 "PMU10: Pstate%d MRS MR01_B1=0x%02x MR02_B1=0x%02x MR03_B1=0x%02x MR11_B1=0x%02x\n"
+	},
+	{0x00620005,
+	 "PMU10: Pstate%d MRS MR12_B1=0x%02x MR13_B1=0x%02x MR14_B1=0x%02x MR22_B1=0x%02x\n"
+	},
+	{0x00630002,
+	 "PMU1: AcsmOdtCtrl%02d 0x%02x\n"
+	},
+	{0x00640002,
+	 "PMU1: AcsmCsMapCtrl%02d 0x%04x\n"
+	},
+	{0x00650002,
+	 "PMU1: AcsmCsMapCtrl%02d 0x%04x\n"
+	},
+	{0x00660000,
+	 "PMU1: HwtCAMode set\n"
+	},
+	{0x00670001,
+	 "PMU3: DDR4 infinite preamble enter/exit mode %d\n"
+	},
+	{0x00680002,
+	 "PMU1: In rxenb_train() csn=%d pstate=%d\n"
+	},
+	{0x00690000,
+	 "PMU3: Finding DQS falling edge\n"
+	},
+	{0x006a0000,
+	 "PMU3: Searching for DDR3/LPDDR3/LPDDR4 read preamble\n"
+	},
+	{0x006b0009,
+	 "PMU3: dtsm fails Even Nibbles : %2x %2x %2x %2x %2x %2x %2x %2x %2x\n"
+	},
+	{0x006c0009,
+	 "PMU3: dtsm fails Odd  Nibbles : %2x %2x %2x %2x %2x %2x %2x %2x %2x\n"
+	},
+	{0x006d0002,
+	 "PMU3: Preamble search pass=%d anyfail=%d\n"
+	},
+	{0x006e0000,
+	 "PMU: Error: RxEn training preamble not found\n"
+	},
+	{0x006f0000,
+	 "PMU3: Found DQS pre-amble\n"
+	},
+	{0x00700001,
+	 "PMU: Error: Dbyte %d couldn't find the rising edge of DQS during RxEn Training\n"
+	},
+	{0x00710000,
+	 "PMU3: RxEn aligning to first rising edge of burst\n"
+	},
+	{0x00720001,
+	 "PMU3: Decreasing RxEn delay by %d fine step to allow full capture of reads\n"
+	},
+	{0x00730001,
+	 "PMU3: MREP Delay = %d\n"
+	},
+	{0x00740003,
+	 "PMU3: Errcnt for MREP nib %2d delay = %2d is %d\n"
+	},
+	{0x00750002,
+	 "PMU3: MREP nibble %d sampled a 1 at data buffer delay %d\n"
+	},
+	{0x00760002,
+	 "PMU3: MREP nibble %d saw a 0 to 1 transition at data buffer delay %d\n"
+	},
+	{0x00770000,
+	 "PMU2:  MREP did not find a 0 to 1 transition for all nibbles. Failing nibbles assumed to have rising edge close to fine delay 63\n"
+	},
+	{0x00780002,
+	 "PMU2:  Rising edge found in alias window, setting rxDly for nibble %d = %d\n"
+	},
+	{0x00790002,
+	 "PMU: Error: Failed MREP for nib %d with %d one\n"
+	},
+	{0x007a0003,
+	 "PMU2:  Rising edge not found in alias window with %d one, leaving rxDly for nibble %d = %d\n"
+	},
+	{0x007b0002,
+	 "PMU3: Training DIMM %d CSn %d\n"
+	},
+	{0x007c0001,
+	 "PMU3: exitCAtrain_lp3 cs 0x%x\n"
+	},
+	{0x007d0001,
+	 "PMU3: enterCAtrain_lp3 cs 0x%x\n"
+	},
+	{0x007e0001,
+	 "PMU3: CAtrain_switchmsb_lp3 cs 0x%x\n"
+	},
+	{0x007f0001,
+	 "PMU3: CATrain_rdwr_lp3 looking for pattern %x\n"
+	},
+	{0x00800000,
+	 "PMU3: exitCAtrain_lp4\n"
+	},
+	{0x00810001,
+	 "PMU3: DEBUG enterCAtrain_lp4 1: cs 0x%x\n"
+	},
+	{0x00820001,
+	 "PMU3: DEBUG enterCAtrain_lp4 3: Put dbyte %d in async mode\n"
+	},
+	{0x00830000,
+	 "PMU3: DEBUG enterCAtrain_lp4 5: Send MR13 to turn on CA training\n"
+	},
+	{0x00840003,
+	 "PMU3: DEBUG enterCAtrain_lp4 7: idx = %d vref = %x mr12 = %x\n"
+	},
+	{0x00850001,
+	 "PMU3: CATrain_rdwr_lp4 looking for pattern %x\n"
+	},
+	{0x00860004,
+	 "PMU3: Phase %d CAreadbackA db:%d %x xo:%x\n"
+	},
+	{0x00870005,
+	 "PMU3: DEBUG lp4SetCatrVref 1: cs=%d chan=%d mr12=%x vref=%d.%d%%\n"
+	},
+	{0x00880003,
+	 "PMU3: DEBUG lp4SetCatrVref 3: mr12 = %x send vref= %x to db=%d\n"
+	},
+	{0x00890000,
+	 "PMU10:Optimizing vref\n"
+	},
+	{0x008a0004,
+	 "PMU4:mr12:%2x cs:%d chan %d r:%4x\n"
+	},
+	{0x008b0005,
+	 "PMU3: i:%2d bstr:%2d bsto:%2d st:%d r:%d\n"
+	},
+	{0x008c0002,
+	 "Failed to find sufficient CA Vref Passing Region for CS %d ch. %d\n"
+	},
+	{0x008d0005,
+	 "PMU3:Found %d.%d%% MR12:%x for cs:%d chan %d\n"
+	},
+	{0x008e0002,
+	 "PMU3:Calculated %d for AtxImpedence from acx %d.\n"
+	},
+	{0x008f0000,
+	 "PMU3:CA Odt impedence ==0.  Use default vref.\n"
+	},
+	{0x00900003,
+	 "PMU3:Calculated %d.%d%% for Vref MR12=0x%x.\n"
+	},
+	{0x00910000,
+	 "PMU3: CAtrain_lp\n"
+	},
+	{0x00920000,
+	 "PMU3: CAtrain Begins.\n"
+	},
+	{0x00930001,
+	 "PMU3: CAtrain_lp testing dly %d\n"
+	},
+	{0x00940001,
+	 "PMU5: CA bitmap dump for cs %x\n"
+	},
+	{0x00950001,
+	 "PMU5: CAA%d "
+	},
+	{0x00960001, "%02x"
+	},
+	{0x00970000, "\n"
+	},
+	{0x00980001,
+	 "PMU5: CAB%d "
+	},
+	{0x00990001, "%02x"
+	},
+	{0x009a0000, "\n"
+	},
+	{0x009b0003,
+	 "PMU3: anibi=%d, anibichan[anibi]=%d ,chan=%d\n"
+	},
+	{0x009c0001, "%02x"
+	},
+	{0x009d0001, "\nPMU3:Raw CA setting :%x"
+	},
+	{0x009e0002, "\nPMU3:ATxDly setting:%x margin:%d\n"
+	},
+	{0x009f0002, "\nPMU3:InvClk ATxDly setting:%x margin:%d\n"
+	},
+	{0x00a00000, "\nPMU3:No Range found!\n"
+	},
+	{0x00a10003,
+	 "PMU3: 2 anibi=%d, anibichan[anibi]=%d ,chan=%d"
+	},
+	{0x00a20002, "\nPMU3: no neg clock => CA setting anib=%d, :%d\n"
+	},
+	{0x00a30001,
+	 "PMU3:Normal margin:%d\n"
+	},
+	{0x00a40001,
+	 "PMU3:Inverted margin:%d\n"
+	},
+	{0x00a50000,
+	 "PMU3:Using Inverted clock\n"
+	},
+	{0x00a60000,
+	 "PMU3:Using normal clk\n"
+	},
+	{0x00a70003,
+	 "PMU3: 3 anibi=%d, anibichan[anibi]=%d ,chan=%d\n"
+	},
+	{0x00a80002,
+	 "PMU3: Setting ATxDly for anib %x to %x\n"
+	},
+	{0x00a90000,
+	 "PMU: Error: CA Training Failed.\n"
+	},
+	{0x00aa0000,
+	 "PMU1: Writing MRs\n"
+	},
+	{0x00ab0000,
+	 "PMU4:Using MR12 values from 1D CA VREF training.\n"
+	},
+	{0x00ac0000,
+	 "PMU3:Writing all MRs to fsp 1\n"
+	},
+	{0x00ad0000,
+	 "PMU10:Lp4Quickboot mode.\n"
+	},
+	{0x00ae0000,
+	 "PMU3: Writing MRs\n"
+	},
+	{0x00af0001,
+	 "PMU10: Setting boot clock divider to %d\n"
+	},
+	{0x00b00000,
+	 "PMU3: Resetting DRAM\n"
+	},
+	{0x00b10000,
+	 "PMU3: setup for RCD initalization\n"
+	},
+	{0x00b20000,
+	 "PMU3: pmu_exit_SR from dev_init()\n"
+	},
+	{0x00b30000,
+	 "PMU3: initializing RCD\n"
+	},
+	{0x00b40000,
+	 "PMU10: **** Executing 2D Image ****\n"
+	},
+	{0x00b50001,
+	 "PMU10: **** Start DDR4 Training. PMU Firmware Revision 0x%04x ****\n"
+	},
+	{0x00b60001,
+	 "PMU10: **** Start DDR3 Training. PMU Firmware Revision 0x%04x ****\n"
+	},
+	{0x00b70001,
+	 "PMU10: **** Start LPDDR3 Training. PMU Firmware Revision 0x%04x ****\n"
+	},
+	{0x00b80001,
+	 "PMU10: **** Start LPDDR4 Training. PMU Firmware Revision 0x%04x ****\n"
+	},
+	{0x00b90000,
+	 "PMU: Error: Mismatched internal revision between DCCM and ICCM images\n"
+	},
+	{0x00ba0001,
+	 "PMU10: **** Testchip %d Specific Firmware ****\n"
+	},
+	{0x00bb0000,
+	 "PMU1: LRDIMM with EncodedCS mode, one DIMM\n"
+	},
+	{0x00bc0000,
+	 "PMU1: LRDIMM with EncodedCS mode, two DIMMs\n"
+	},
+	{0x00bd0000,
+	 "PMU1: RDIMM with EncodedCS mode, one DIMM\n"
+	},
+	{0x00be0000,
+	 "PMU2: Starting LRDIMM MREP training for all ranks\n"
+	},
+	{0x00bf0000,
+	 "PMU199: LRDIMM MREP training for all ranks completed\n"
+	},
+	{0x00c00000,
+	 "PMU2: Starting LRDIMM DWL training for all ranks\n"
+	},
+	{0x00c10000,
+	 "PMU199: LRDIMM DWL training for all ranks completed\n"
+	},
+	{0x00c20000,
+	 "PMU2: Starting LRDIMM MRD training for all ranks\n"
+	},
+	{0x00c30000,
+	 "PMU199: LRDIMM MRD training for all ranks completed\n"
+	},
+	{0x00c40000,
+	 "PMU2: Starting RXEN training for all ranks\n"
+	},
+	{0x00c50000,
+	 "PMU2: Starting write leveling fine delay training for all ranks\n"
+	},
+	{0x00c60000,
+	 "PMU2: Starting LRDIMM MWD training for all ranks\n"
+	},
+	{0x00c70000,
+	 "PMU199: LRDIMM MWD training for all ranks completed\n"
+	},
+	{0x00c80000,
+	 "PMU2: Starting write leveling fine delay training for all ranks\n"
+	},
+	{0x00c90000,
+	 "PMU2: Starting read deskew training\n"
+	},
+	{0x00ca0000,
+	 "PMU2: Starting SI friendly 1d RdDqs training for all ranks\n"
+	},
+	{0x00cb0000,
+	 "PMU2: Starting write leveling coarse delay training for all ranks\n"
+	},
+	{0x00cc0000,
+	 "PMU2: Starting 1d WrDq training for all ranks\n"
+	},
+	{0x00cd0000,
+	 "PMU2: Running DQS2DQ Oscillator for all ranks\n"
+	},
+	{0x00ce0000,
+	 "PMU2: Starting again read deskew training but with PRBS\n"
+	},
+	{0x00cf0000,
+	 "PMU2: Starting 1d RdDqs training for all ranks\n"
+	},
+	{0x00d00000,
+	 "PMU2: Starting again 1d WrDq training for all ranks\n"
+	},
+	{0x00d10000,
+	 "PMU2: Starting MaxRdLat training\n"
+	},
+	{0x00d20000,
+	 "PMU2: Starting 2d WrDq training for all ranks\n"
+	},
+	{0x00d30000,
+	 "PMU2: Starting 2d RdDqs training for all ranks\n"
+	},
+	{0x00d40002,
+	 "PMU3:read_fifo %x %x\n"
+	},
+	{0x00d50001,
+	 "PMU: Error: Invalid PhyDrvImpedance of 0x%x specified in message block.\n"
+	},
+	{0x00d60001,
+	 "PMU: Error: Invalid PhyOdtImpedance of 0x%x specified in message block.\n"
+	},
+	{0x00d70001,
+	 "PMU: Error: Invalid BPZNResVal of 0x%x specified in message block.\n"
+	},
+	{0x00d80005,
+	 "PMU3: fixRxEnBackOff csn:%d db:%d dn:%d bo:%d dly:%x\n"
+	},
+	{0x00d90001,
+	 "PMU3: fixRxEnBackOff dly:%x\n"
+	},
+	{0x00da0000,
+	 "PMU3: Entering setupPpt\n"
+	},
+	{0x00db0000,
+	 "PMU3: Start lp4PopulateHighLowBytes\n"
+	},
+	{0x00dc0002,
+	 "PMU3:Dbyte Detect: db%d received %x\n"
+	},
+	{0x00dd0002,
+	 "PMU3:getDqs2Dq read %x from dbyte %d\n"
+	},
+	{0x00de0002,
+	 "PMU3:getDqs2Dq(2) read %x from dbyte %d\n"
+	},
+	{0x00df0001,
+	 "PMU: Error: Dbyte %d read 0 from the DQS oscillator it is connected to\n"
+	},
+	{0x00e00002,
+	 "PMU4: Dbyte %d dqs2dq = %d/32 UI\n"
+	},
+	{0x00e10003,
+	 "PMU3:getDqs2Dq set dqs2dq:%d/32 ui (%d ps) from dbyte %d\n"
+	},
+	{0x00e20003,
+	 "PMU3: Setting coarse delay in AtxDly chiplet %d from 0x%02x to 0x%02x\n"
+	},
+	{0x00e30003,
+	 "PMU3: Clearing coarse delay in AtxDly chiplet %d from 0x%02x to 0x%02x\n"
+	},
+	{0x00e40000,
+	 "PMU3: Performing DDR4 geardown sync sequence\n"
+	},
+	{0x00e50000,
+	 "PMU1: Enter self refresh\n"
+	},
+	{0x00e60000,
+	 "PMU1: Exit self refresh\n"
+	},
+	{0x00e70000,
+	 "PMU: Error: No dbiEnable with lp4\n"
+	},
+	{0x00e80000,
+	 "PMU: Error: No dbiDisable with lp4\n"
+	},
+	{0x00e90001,
+	 "PMU1: DDR4 update Rx DBI Setting disable %d\n"
+	},
+	{0x00ea0001,
+	 "PMU1: DDR4 update 2nCk WPre Setting disable %d\n"
+	},
+	{0x00eb0005,
+	 "PMU1: read_delay: db%d lane%d delays[%2d] = 0x%02x (max 0x%02x)\n"
+	},
+	{0x00ec0004,
+	 "PMU1: write_delay: db%d lane%d delays[%2d] = 0x%04x\n"
+	},
+	{0x00ed0001,
+	 "PMU5: ID=%d -- db0  db1  db2  db3  db4  db5  db6  db7  db8  db9 --\n"
+	},
+	{0x00ee000b,
+	 "PMU5: [%d]:0x %04x %04x %04x %04x %04x %04x %04x %04x %04x %04x\n"
+	},
+	{0x00ef0003,
+	 "PMU2: dump delays - pstate=%d dimm=%d csn=%d\n"
+	},
+	{0x00f00000,
+	 "PMU3: Printing Mid-Training Delay Information\n"
+	},
+	{0x00f10001,
+	 "PMU5: CS%d <<KEY>> 0 TrainingCntr <<KEY>> coarse(15:10) fine(9:0)\n"
+	},
+	{0x00f20001,
+	 "PMU5: CS%d <<KEY>> 0 RxEnDly, 1 RxClkDly <<KEY>> coarse(10:6) fine(5:0)\n"
+	},
+	{0x00f30001,
+	 "PMU5: CS%d <<KEY>> 0 TxDqsDly, 1 TxDqDly <<KEY>> coarse(9:6) fine(5:0)\n"
+	},
+	{0x00f40001,
+	 "PMU5: CS%d <<KEY>> 0 RxPBDly <<KEY>> 1 Delay Unit ~= 7ps\n"
+	},
+	{0x00f50000,
+	 "PMU5: all CS <<KEY>> 0 DFIMRL <<KEY>> Units = DFI clocks\n"
+	},
+	{0x00f60000,
+	 "PMU5: all CS <<KEY>> VrefDACs <<KEY>> DAC(6:0)\n"
+	},
+	{0x00f70000,
+	 "PMU1: Set DMD in MR13 and wrDBI in MR3 for training\n"
+	},
+	{0x00f80000,
+	 "PMU: Error: getMaxRxen() failed to find largest rxen nibble delay\n"
+	},
+	{0x00f90003,
+	 "PMU2: getMaxRxen(): maxDly %d maxTg %d maxNib %d\n"
+	},
+	{0x00fa0003,
+	 "PMU2: getRankMaxRxen(): maxDly %d Tg %d maxNib %d\n"
+	},
+	{0x00fb0000,
+	 "PMU1: skipping CDD calculation in 2D image\n"
+	},
+	{0x00fc0001,
+	 "PMU3: Calculating CDDs for pstate %d\n"
+	},
+	{0x00fd0003,
+	 "PMU3: rxFromDly[%d][%d] = %d\n"
+	},
+	{0x00fe0003,
+	 "PMU3: rxToDly  [%d][%d] = %d\n"
+	},
+	{0x00ff0003,
+	 "PMU3: rxDly    [%d][%d] = %d\n"
+	},
+	{0x01000003,
+	 "PMU3: txDly    [%d][%d] = %d\n"
+	},
+	{0x01010003,
+	 "PMU3: allFine CDD_RR_%d_%d = %d\n"
+	},
+	{0x01020003,
+	 "PMU3: allFine CDD_WW_%d_%d = %d\n"
+	},
+	{0x01030003,
+	 "PMU3: CDD_RR_%d_%d = %d\n"
+	},
+	{0x01040003,
+	 "PMU3: CDD_WW_%d_%d = %d\n"
+	},
+	{0x01050003,
+	 "PMU3: allFine CDD_RW_%d_%d = %d\n"
+	},
+	{0x01060003,
+	 "PMU3: allFine CDD_WR_%d_%d = %d\n"
+	},
+	{0x01070003,
+	 "PMU3: CDD_RW_%d_%d = %d\n"
+	},
+	{0x01080003,
+	 "PMU3: CDD_WR_%d_%d = %d\n"
+	},
+	{0x01090004,
+	 "PMU3: F%dBC2x_B%d_D%d = 0x%02x\n"
+	},
+	{0x010a0004,
+	 "PMU3: F%dBC3x_B%d_D%d = 0x%02x\n"
+	},
+	{0x010b0004,
+	 "PMU3: F%dBC4x_B%d_D%d = 0x%02x\n"
+	},
+	{0x010c0004,
+	 "PMU3: F%dBC5x_B%d_D%d = 0x%02x\n"
+	},
+	{0x010d0004,
+	 "PMU3: F%dBC8x_B%d_D%d = 0x%02x\n"
+	},
+	{0x010e0004,
+	 "PMU3: F%dBC9x_B%d_D%d = 0x%02x\n"
+	},
+	{0x010f0004,
+	 "PMU3: F%dBCAx_B%d_D%d = 0x%02x\n"
+	},
+	{0x01100004,
+	 "PMU3: F%dBCBx_B%d_D%d = 0x%02x\n"
+	},
+	{0x01110000,
+	 "PMU10: Entering context_switch_postamble\n"
+	},
+	{0x01120003,
+	 "PMU10: context_switch_postamble is enabled for DIMM %d, RC0A=0x%x, RC3x=0x%x\n"
+	},
+	{0x01130000,
+	 "PMU10: Setting bcw fspace 0\n"
+	},
+	{0x01140001,
+	 "PMU10: Sending BC0A = 0x%x\n"
+	},
+	{0x01150001,
+	 "PMU10: Sending BC6x = 0x%x\n"
+	},
+	{0x01160001,
+	 "PMU10: Sending RC0A = 0x%x\n"
+	},
+	{0x01170001,
+	 "PMU10: Sending RC3x = 0x%x\n"
+	},
+	{0x01180001,
+	 "PMU10: Sending RC0A = 0x%x\n"
+	},
+	{0x01190001,
+	 "PMU1: enter_lp3: DEBUG: pstate = %d\n"
+	},
+	{0x011a0001,
+	 "PMU1: enter_lp3: DEBUG: dfifreqxlat_pstate = %d\n"
+	},
+	{0x011b0001,
+	 "PMU1: enter_lp3: DEBUG: pllbypass = %d\n"
+	},
+	{0x011c0001,
+	 "PMU1: enter_lp3: DEBUG: forcecal = %d\n"
+	},
+	{0x011d0001,
+	 "PMU1: enter_lp3: DEBUG: pllmaxrange = 0x%x\n"
+	},
+	{0x011e0001,
+	 "PMU1: enter_lp3: DEBUG: dacval_out = 0x%x\n"
+	},
+	{0x011f0001,
+	 "PMU1: enter_lp3: DEBUG: pllctrl3 = 0x%x\n"
+	},
+	{0x01200000,
+	 "PMU3: Loading DRAM with BIOS supplied MR values and entering self refresh prior to exiting PMU code.\n"
+	},
+	{0x01210002,
+	 "PMU3: Setting DataBuffer function space of dimmcs 0x%02x to %d\n"
+	},
+	{0x01220002,
+	 "PMU4: Setting RCW FxRC%Xx = 0x%02x\n"
+	},
+	{0x01230002,
+	 "PMU4: Setting RCW FxRC%02x = 0x%02x\n"
+	},
+	{0x01240001,
+	 "PMU1: DDR4 update Rd Pre Setting disable %d\n"
+	},
+	{0x01250002,
+	 "PMU2: Setting BCW FxBC%Xx = 0x%02x\n"
+	},
+	{0x01260002,
+	 "PMU2: Setting BCW BC%02x = 0x%02x\n"
+	},
+	{0x01270002,
+	 "PMU2: Setting BCW PBA mode FxBC%Xx = 0x%02x\n"
+	},
+	{0x01280002,
+	 "PMU2: Setting BCW PBA mode BC%02x = 0x%02x\n"
+	},
+	{0x01290003,
+	 "PMU4: BCW value for dimm %d, fspace %d, addr 0x%04x\n"
+	},
+	{0x012a0002,
+	 "PMU4: DB %d, value 0x%02x\n"
+	},
+	{0x012b0000,
+	 "PMU6: WARNING MREP underflow, set to min value -2 coarse, 0 fine\n"
+	},
+	{0x012c0004,
+	 "PMU6: LRDIMM Writing final data buffer fine delay value nib %2d, trainDly %3d, fineDly code %2d, new MREP fine %2d\n"
+	},
+	{0x012d0003,
+	 "PMU6: LRDIMM Writing final data buffer fine delay value nib %2d, trainDly %3d, fineDly code %2d\n"
+	},
+	{0x012e0003,
+	 "PMU6: LRDIMM Writing data buffer fine delay type %d nib %2d, code %2d\n"
+	},
+	{0x012f0002,
+	 "PMU6: Writing final data buffer coarse delay value dbyte %2d, coarse = 0x%02x\n"
+	},
+	{0x01300003,
+	 "PMU4: data 0x%04x at MB addr 0x%08x saved at CSR addr 0x%08x\n"
+	},
+	{0x01310003,
+	 "PMU4: data 0x%04x at MB addr 0x%08x restored from CSR addr 0x%08x\n"
+	},
+	{0x01320003,
+	 "PMU4: data 0x%04x at MB addr 0x%08x saved at CSR addr 0x%08x\n"
+	},
+	{0x01330003,
+	 "PMU4: data 0x%04x at MB addr 0x%08x restored from CSR addr 0x%08x\n"
+	},
+	{0x01340001,
+	 "PMU3: Update BC00, BC01, BC02 for rank-dimm 0x%02x\n"
+	},
+	{0x01350000,
+	 "PMU3: Writing D4 RDIMM RCD Control words F0RC00 -> F0RC0F\n"
+	},
+	{0x01360000,
+	 "PMU3: Disable parity in F0RC0E\n"
+	},
+	{0x01370000,
+	 "PMU3: Writing D4 RDIMM RCD Control words F1RC00 -> F1RC05\n"
+	},
+	{0x01380000,
+	 "PMU3: Writing D4 RDIMM RCD Control words F1RC1x -> F1RC9x\n"
+	},
+	{0x01390000,
+	 "PMU3: Writing D4 Data buffer Control words BC00 -> BC0E\n"
+	},
+	{0x013a0002,
+	 "PMU1: setAltCL Sending MR0 0x%x cl=%d\n"
+	},
+	{0x013b0002,
+	 "PMU1: restoreFromAltCL Sending MR0 0x%x cl=%d\n"
+	},
+	{0x013c0002,
+	 "PMU1: restoreAcsmFromAltCL Sending MR0 0x%x cl=%d\n"
+	},
+	{0x013d0002,
+	 "PMU2: Setting D3R RC%d = 0x%01x\n"
+	},
+	{0x013e0000,
+	 "PMU3: Writing D3 RDIMM RCD Control words RC0 -> RC11\n"
+	},
+	{0x013f0002,
+	 "PMU0: VrefDAC0/1 vddqStart %d dacToVddq %d\n"
+	},
+	{0x01400001,
+	 "PMU: Error: Messageblock phyVref=0x%x is above the limit for TSMC28's attenuated LPDDR4 receivers. Please see the pub databook\n"
+	},
+	{0x01410001,
+	 "PMU: Error: Messageblock phyVref=0x%x is above the limit for TSMC28's attenuated DDR4 receivers. Please see the pub databook\n"
+	},
+	{0x01420001,
+	 "PMU0: PHY VREF @ (%d/1000) VDDQ\n"
+	},
+	{0x01430002,
+	 "PMU0: initalizing phy vrefDacs to %d ExtVrefRange %x\n"
+	},
+	{0x01440002,
+	 "PMU0: initalizing global vref to %d range %d\n"
+	},
+	{0x01450002,
+	 "PMU4: Setting initial device vrefDQ for CS%d to MR6 = 0x%04x\n"
+	},
+	{0x01460003,
+	 "PMU1: In write_level_fine() csn=%d dimm=%d pstate=%d\n"
+	},
+	{0x01470000,
+	 "PMU3: Fine write leveling hardware search increasing TxDqsDly until full bursts are seen\n"
+	},
+	{0x01480000,
+	 "PMU4: WL normalized pos   : ........................|........................\n"
+	},
+	{0x01490007,
+	 "PMU4: WL margin for nib %2d: %08x%08x%08x%08x%08x%08x\n"
+	},
+	{0x014a0000,
+	 "PMU4: WL normalized pos   : ........................|........................\n"
+	},
+	{0x014b0000,
+	 "PMU3: Exiting write leveling mode\n"
+	},
+	{0x014c0001,
+	 "PMU3: got %d for cl in load_wrlvl_acsm\n"
+	},
+	{0x014d0003,
+	 "PMU1: In write_level_coarse() csn=%d dimm=%d pstate=%d\n"
+	},
+	{0x014e0003,
+	 "PMU3: left eye edge search db:%d ln:%d dly:0x%x\n"
+	},
+	{0x014f0003,
+	 "PMU3: right eye edge search db:%d ln:%d dly:0x%x\n"
+	},
+	{0x01500004,
+	 "PMU3: eye center db:%d ln:%d dly:0x%x (maxdq:%x)\n"
+	},
+	{0x01510003,
+	 "PMU3: Wrote to TxDqDly db:%d ln:%d dly:0x%x\n"
+	},
+	{0x01520003,
+	 "PMU3: Wrote to TxDqDly db:%d ln:%d dly:0x%x\n"
+	},
+	{0x01530002,
+	 "PMU3: Coarse write leveling dbyte%2d is still failing for TxDqsDly=0x%04x\n"
+	},
+	{0x01540002,
+	 "PMU4: Coarse write leveling iteration %d saw %d data miscompares across the entire phy\n"
+	},
+	{0x01550000,
+	 "PMU: Error: Failed write leveling coarse\n"
+	},
+	{0x01560001,
+	 "PMU3: got %d for cl in load_wrlvl_acsm\n"
+	},
+	{0x01570003,
+	 "PMU3: In write_level_coarse() csn=%d dimm=%d pstate=%d\n"
+	},
+	{0x01580003,
+	 "PMU3: left eye edge search db:%d ln:%d dly:0x%x\n"
+	},
+	{0x01590003,
+	 "PMU3: right eye edge search db: %d ln: %d dly: 0x%x\n"
+	},
+	{0x015a0004,
+	 "PMU3: eye center db: %d ln: %d dly: 0x%x (maxdq: 0x%x)\n"
+	},
+	{0x015b0003,
+	 "PMU3: Wrote to TxDqDly db: %d ln: %d dly: 0x%x\n"
+	},
+	{0x015c0003,
+	 "PMU3: Wrote to TxDqDly db: %d ln: %d dly: 0x%x\n"
+	},
+	{0x015d0002,
+	 "PMU3: Coarse write leveling nibble%2d is still failing for TxDqsDly=0x%04x\n"
+	},
+	{0x015e0002,
+	 "PMU4: Coarse write leveling iteration %d saw %d data miscompares across the entire phy\n"
+	},
+	{0x015f0000,
+	 "PMU: Error: Failed write leveling coarse\n"
+	},
+	{0x01600000,
+	 "PMU4: WL normalized pos   : ................................|................................\n"
+	},
+	{0x01610009,
+	 "PMU4: WL margin for nib %2d: %08x%08x%08x%08x%08x%08x%08x%08x\n"
+	},
+	{0x01620000,
+	 "PMU4: WL normalized pos   : ................................|................................\n"
+	},
+	{0x01630001,
+	 "PMU8: Adjust margin after WL coarse to be larger than %d\n"
+	},
+	{0x01640001,
+	 "PMU: Error: All margin after write leveling coarse are smaller than minMargin %d\n"
+	},
+	{0x01650002,
+	 "PMU8: Decrement nib %d TxDqsDly by %d fine step\n"
+	},
+	{0x01660003,
+	 "PMU3: In write_level_coarse() csn=%d dimm=%d pstate=%d\n"
+	},
+	{0x01670005,
+	 "PMU2: Write level: dbyte %d nib%d dq/dmbi %2d dqsfine 0x%04x dqDly 0x%04x\n"
+	},
+	{0x01680002,
+	 "PMU3: Coarse write leveling nibble%2d is still failing for TxDqsDly=0x%04x\n"
+	},
+	{0x01690002,
+	 "PMU4: Coarse write leveling iteration %d saw %d data miscompares across the entire phy\n"
+	},
+	{0x016a0000,
+	 "PMU: Error: Failed write leveling coarse\n"
+	},
+	{0x016b0001,
+	 "PMU3: DWL delay = %d\n"
+	},
+	{0x016c0003,
+	 "PMU3: Errcnt for DWL nib %2d delay = %2d is %d\n"
+	},
+	{0x016d0002,
+	 "PMU3: DWL nibble %d sampled a 1 at delay %d\n"
+	},
+	{0x016e0003,
+	 "PMU3: DWL nibble %d passed at delay %d. Rising edge was at %d\n"
+	},
+	{0x016f0000,
+	 "PMU2: DWL did nto find a rising edge of memclk for all nibbles. Failing nibbles assumed to have rising edge close to fine delay 63\n"
+	},
+	{0x01700002,
+	 "PMU2:  Rising edge found in alias window, setting wrlvlDly for nibble %d = %d\n"
+	},
+	{0x01710002,
+	 "PMU: Error: Failed DWL for nib %d with %d one\n"
+	},
+	{0x01720003,
+	 "PMU2:  Rising edge not found in alias window with %d one, leaving wrlvlDly for nibble %d = %d\n"
+	},
+	{0x04000000,
+	 "PMU: Error:Mailbox Buffer Overflowed.\n"
+	},
+	{0x04010000,
+	 "PMU: Error:Mailbox Buffer Overflowed.\n"
+	},
+	{0x04020000,
+	 "PMU: ***** Assertion Error - terminating *****\n"
+	},
+	{0x04030002,
+	 "PMU1: swapByte db %d by %d\n"
+	},
+	{0x04040003,
+	 "PMU3: get_cmd_dly max(%d ps, %d memclk) = %d\n"
+	},
+	{0x04050002,
+	 "PMU0: Write CSR 0x%06x 0x%04x\n"
+	},
+	{0x04060002,
+	 "PMU0: hwt_init_ppgc_prbs(): Polynomial: %x, Deg: %d\n"
+	},
+	{0x04070001,
+	 "PMU: Error: acsm_set_cmd to non existent instruction address %d\n"
+	},
+	{0x04080001,
+	 "PMU: Error: acsm_set_cmd with unknown ddr cmd 0x%x\n"
+	},
+	{0x0409000c,
+	 "PMU1: acsm_addr %02x, acsm_flgs %04x, ddr_cmd %02x, cmd_dly %02x, ddr_addr %04x, ddr_bnk %02x, ddr_cs %02x, cmd_rcnt %02x, AcsmSeq0/1/2/3 %04x %04x %04x %04x\n"
+	},
+	{0x040a0000,
+	 "PMU: Error: Polling on ACSM done failed to complete in acsm_poll_done()...\n"
+	},
+	{0x040b0000,
+	 "PMU1: acsm RUN\n"
+	},
+	{0x040c0000,
+	 "PMU1: acsm STOPPED\n"
+	},
+	{0x040d0002,
+	 "PMU1: acsm_init: acsm_mode %04x mxrdlat %04x\n"
+	},
+	{0x040e0002,
+	 "PMU: Error: setAcsmCLCWL: cl and cwl must be each >= 2 and 5, resp. CL=%d CWL=%d\n"
+	},
+	{0x040f0002,
+	 "PMU: Error: setAcsmCLCWL: cl and cwl must be each >= 5. CL=%d CWL=%d\n"
+	},
+	{0x04100002,
+	 "PMU1: setAcsmCLCWL: CASL %04d WCASL %04d\n"
+	},
+	{0x04110001,
+	 "PMU: Error: Reserved value of register F0RC0F found in message block: 0x%04x\n"
+	},
+	{0x04120001,
+	 "PMU3: Written MRS to CS=0x%02x\n"
+	},
+	{0x04130001,
+	 "PMU3: Written MRS to CS=0x%02x\n"
+	},
+	{0x04140000,
+	 "PMU3: Entering Boot Freq Mode.\n"
+	},
+	{0x04150001,
+	 "PMU: Error: Boot clock divider setting of %d is too small\n"
+	},
+	{0x04160000,
+	 "PMU3: Exiting Boot Freq Mode.\n"
+	},
+	{0x04170002,
+	 "PMU3: Writing MR%d OP=%x\n"
+	},
+	{0x04180000,
+	 "PMU: Error: Delay too large in slomo\n"
+	},
+	{0x04190001,
+	 "PMU3: Written MRS to CS=0x%02x\n"
+	},
+	{0x041a0000,
+	 "PMU3: Enable Channel A\n"
+	},
+	{0x041b0000,
+	 "PMU3: Enable Channel B\n"
+	},
+	{0x041c0000,
+	 "PMU3: Enable All Channels\n"
+	},
+	{0x041d0002,
+	 "PMU2: Use PDA mode to set MR%d with value 0x%02x\n"
+	},
+	{0x041e0001,
+	 "PMU3: Written Vref with PDA to CS=0x%02x\n"
+	},
+	{0x041f0000,
+	 "PMU1: start_cal: DEBUG: setting CalRun to 1\n"
+	},
+	{0x04200000,
+	 "PMU1: start_cal: DEBUG: setting CalRun to 0\n"
+	},
+	{0x04210001,
+	 "PMU1: lock_pll_dll: DEBUG: pstate = %d\n"
+	},
+	{0x04220001,
+	 "PMU1: lock_pll_dll: DEBUG: dfifreqxlat_pstate = %d\n"
+	},
+	{0x04230001,
+	 "PMU1: lock_pll_dll: DEBUG: pllbypass = %d\n"
+	},
+	{0x04240001,
+	 "PMU3: SaveLcdlSeed: Saving seed %d\n"
+	},
+	{0x04250000,
+	 "PMU1: in phy_defaults()\n"
+	},
+	{0x04260003,
+	 "PMU3: ACXConf:%d MaxNumDbytes:%d NumDfi:%d\n"
+	},
+	{0x04270005,
+	 "PMU1: setAltAcsmCLCWL setting cl=%d cwl=%d\n"
+	},
+};
+
+const static struct phy_msg messages_2d[] = {
+	{0x00000001,
+	 "PMU0: Converting %d into an MR\n"
+	},
+	{0x00010003,
+	 "PMU DEBUG: vref_idx %d -= %d, range_idx = %d\n"
+	},
+	{0x00020002,
+	 "PMU0: vrefIdx. Passing range %d, remaining vrefidx = %d\n"
+	},
+	{0x00030002,
+	 "PMU0: VrefIdx %d -> MR[6:0] 0x%02x\n"
+	},
+	{0x00040001,
+	 "PMU0: Converting MR 0x%04x to vrefIdx\n"
+	},
+	{0x00050002,
+	 "PMU0: DAC %d Range %d\n"
+	},
+	{0x00060003,
+	 "PMU0: Range %d, Range_idx %d, vref_idx offset %d\n"
+	},
+	{0x00070002,
+	 "PMU0: MR 0x%04x -> VrefIdx %d\n"
+	},
+	{0x00080001,
+	 "PMU: Error: Illegal timing group number ,%d, in getPtrVrefDq\n"
+	},
+	{0x00090003,
+	 "PMU1: VrefDqR%dNib%d = %d\n"
+	},
+	{0x000a0003,
+	 "PMU0: VrefDqR%dNib%d = %d\n"
+	},
+	{0x000b0000,
+	 "PMU0: ----------------MARGINS-------\n"
+	},
+	{0x000c0002,
+	 "PMU0: R%d_RxClkDly_Margin = %d\n"
+	},
+	{0x000d0002,
+	 "PMU0: R%d_VrefDac_Margin = %d\n"
+	},
+	{0x000e0002,
+	 "PMU0: R%d_TxDqDly_Margin = %d\n"
+	},
+	{0x000f0002,
+	 "PMU0: R%d_DeviceVref_Margin = %d\n"
+	},
+	{0x00100000,
+	 "PMU0: -----------------------\n"
+	},
+	{0x00110003,
+	 "PMU0: eye %d's for all TG's is [%d ... %d]\n"
+	},
+	{0x00120000,
+	 "PMU0: ------- settingWeight -----\n"
+	},
+	{0x00130002,
+	 "PMU0: Weight %d @ Setting %d\n"
+	},
+	{0x0014001f,
+	 "PMU4: %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d >%3d< %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d\n"
+	},
+	{0x00150002,
+	 "PMU3: Voltage Range = [%d, %d]\n"
+	},
+	{0x00160004,
+	 "PMU4: -- DB%d L%d -- centers: delay = %d, voltage = %d\n"
+	},
+	{0x00170001,
+	 "PMU5: <<KEY>> 0 TxDqDlyTg%d <<KEY>> coarse(6:6) fine(5:0)\n"
+	},
+	{0x00180001,
+	 "PMU5: <<KEY>> 0 messageBlock VrefDqR%d <<KEY>> MR6(6:0)\n"
+	},
+	{0x00190001,
+	 "PMU5: <<KEY>> 0 RxClkDlyTg%d <<KEY>> fine(5:0)\n"
+	},
+	{0x001a0003,
+	 "PMU0: tgToCsn: tg %d + 0x%04x -> csn %d\n"
+	},
+	{0x001b0002,
+	 "PMU: Error: LP4 rank %d cannot be mapped on tg %d\n"
+	},
+	{0x001c0002,
+	 "PMU3: Sending vref %d,  Mr = 0X%05x, to all devices\n"
+	},
+	{0x001d0004,
+	 "PMU4: -------- %dD Write Scanning TG %d (CS 0x%x) Lanes 0x%03x --------\n"
+	},
+	{0x001e0002,
+	 "PMU0: training lanes 0x%03x using lanes 0x%03x\n"
+	},
+	{0x001f0003,
+	 "PMU4: ------- 2D-DFE Read Scanning TG %d (CS 0x%x) Lanes 0x%03x -------\n"
+	},
+	{0x00200004,
+	 "PMU4: ------- %dD Read Scanning TG %d (CS 0x%x) Lanes 0x%03x -------\n"
+	},
+	{0x00210003,
+	 "PMU4: TG%d MR1[13,6,5]=0x%x MR6[13,9,8]=0x%x\n"
+	},
+	{0x00220002,
+	 "PMU0: training lanes 0x%03x using lanes 0x%03x\n"
+	},
+	{0x00230003,
+	 "PMU4: ------- 2D-DFE Read Scanning TG %d (CS 0x%x) Lanes 0x%03x -------\n"
+	},
+	{0x00240004,
+	 "PMU4: ------- %dD Read Scanning TG %d (CS 0x%x) Lanes 0x%03x -------\n"
+	},
+	{0x00250002,
+	 "PMU0: training lanes 0x%03x using lanes 0x%03x\n"
+	},
+	{0x00260002,
+	 "PMU3: Sending vref %d,  Mr = 0X%05x, to all devices\n"
+	},
+	{0x00270004,
+	 "PMU4: -------- %dD Write Scanning TG %d (CS 0x%x) Lanes 0x%03x --------\n"
+	},
+	{0x00280001,
+	 "PMU0: input %d\n"
+	},
+	{0x00290002,
+	 "PMU4: Programmed Voltage Search Range [%d, %d]\n"
+	},
+	{0x002a0002,
+	 "PMU3: Delay Stepsize = %d Fine, Voltage Stepsize = %d DAC\n"
+	},
+	{0x002b0002,
+	 "PMU4: Delay Weight = %d, Voltage Weight = %d\n"
+	},
+	{0x002c0003,
+	 "PMU0: raw 0x%x allFine %d incDec %d"
+	},
+	{0x002d0008,
+	 "PMU0: db%d l%d, voltage 0x%x (u_r %d) delay 0x%x (u_r %d) - lcdl %d mask 0x%x\n"
+	},
+	{0x002e0005,
+	 "PMU0: DB%d L%d, Eye %d, Seed = (0x%x, 0x%x)\n"
+	},
+	{0x002f0002,
+	 "PMU3: 2D Enables       : %d,                    1,                %d\n"
+	},
+	{0x00300006,
+	 "PMU3: 2D Delay   Ranges: OOPL[0x%04x,0x%04x], IP[0x%04x,0x%04x], OOPR[0x%04x,0x%04x]\n"
+	},
+	{0x00310002,
+	 "PMU3: 2D Voltage Search Range : [%d, %d]\n"
+	},
+	{0x00320002,
+	 "PMU4: Found Voltage Search Range [%d, %d]\n"
+	},
+	{0x00330002,
+	 "PMU0: User Weight = %d, Voltage Weight = %d\n"
+	},
+	{0x00340005,
+	 "PMU0: D(%d,%d) V(%d,%d | %d)\n"
+	},
+	{0x00350002,
+	 "PMU0: Norm Weight = %d, Voltage Weight = %d\n"
+	},
+	{0x00360002,
+	 "PMU0: seed 0 = (%d,%d) (center)\n"
+	},
+	{0x00370003,
+	 "PMU0: seed 1 = (%d,%d).min edge at idx %d\n"
+	},
+	{0x00380003,
+	 "PMU0: seed 2 = (%d,%d) max edge at idx %d\n"
+	},
+	{0x00390003,
+	 "PMU0: Search point %d = (%d,%d)\n"
+	},
+	{0x003a0005,
+	 "PMU0: YMARGIN: ^ %d, - %d, v %d. rate %d = %d\n"
+	},
+	{0x003b0003,
+	 "PMU0: XMARGIN: center %d, edge %d. = %d\n"
+	},
+	{0x003c0002,
+	 "PMU0: ----------- weighting (%d,%d) ----------------\n"
+	},
+	{0x003d0003,
+	 "PMU0: X margin - L %d R %d - Min %d\n"
+	},
+	{0x003e0003,
+	 "PMU0: Y margin - L %d R %d - Min %d\n"
+	},
+	{0x003f0003,
+	 "PMU0: center (%d,%d) weight = %d\n"
+	},
+	{0x00400003,
+	 "PMU4: Eye argest blob area %d from %d to %d\n"
+	},
+	{0x00410002,
+	 "PMU0: Compute centroid min_x %d max_x %d\n"
+	},
+	{0x00420003,
+	 "PMU0: Compute centroid sumLnDlyWidth %d sumLnVrefWidth %d sumLnWidht %d\n"
+	},
+	{0x00430000,
+	 "PMU: Error: No passing region found for 1 or more lanes. Set hdtCtrl=4 to see passing regions\n"
+	},
+	{0x00440003,
+	 "PMU0: Centroid ( %d, %d ) found with sumLnWidht %d\n"
+	},
+	{0x00450003,
+	 "PMU0: Optimal allFine Center ( %d + %d ,%d )\n"
+	},
+	{0x00460003,
+	 "PMU3: point %d starting at (%d,%d)\n"
+	},
+	{0x00470002,
+	 "PMU0: picking left (%d > %d)\n"
+	},
+	{0x00480002,
+	 "PMU0: picking right (%d > %d)\n"
+	},
+	{0x00490002,
+	 "PMU0: picking down (%d > %d)\n"
+	},
+	{0x004a0002,
+	 "PMU0: picking up (%d > %d)\n"
+	},
+	{0x004b0009,
+	 "PMU3: new center @ (%3d, %3d). Moved (%2i, %2i) -- L %d, R %d, C %d, U %d, D %d\n"
+	},
+	{0x004c0003,
+	 "PMU3: cordNum %d imporved %d to %d\n"
+	},
+	{0x004d0000,
+	 "PMU: Error: No passing region found for 1 or more lanes. Set hdtCtrl=4 to see passing regions\n"
+	},
+	{0x004e0004,
+	 "PMU0: Optimal allFine Center ( %d + %d ,%d ), found with weight %d.\n"
+	},
+	{0x004f0003,
+	 "PMU0: merging lanes=%d..%d, centerMerge_t %d\n"
+	},
+	{0x00500001,
+	 "PMU0: laneVal %d is disable\n"
+	},
+	{0x00510002,
+	 "PMU0: checking common center %d against current center %d\n"
+	},
+	{0x00520001,
+	 "PMU: Error: getCompoundEye Called on lane%d eye with non-compatible centers\n"
+	},
+	{0x00530001,
+	 "PMU0: laneItr %d is disable\n"
+	},
+	{0x00540005,
+	 "PMU0: lane %d, data_idx %d, offset_idx %d, = [%d..%d]\n"
+	},
+	{0x00550003,
+	 "PMU0: lane %d, data_idx %d, offset_idx %d, offset_idx out of range!\n"
+	},
+	{0x00560003,
+	 "PMU0: mergeData[%d] = max_v_low %d, min_v_high %d\n"
+	},
+	{0x00570005,
+	 "PMU1: writing merged center (%d,%d) back to dataBlock[%d]. doDelay %d, doVoltage %d\n"
+	},
+	{0x00580005,
+	 "PMU0: applying relative (%i,%i) back to dataBlock[%d]. doDelay %d, doVoltage %d\n"
+	},
+	{0x00590002,
+	 "PMU0: drvstren %x is idx %d in the table\n"
+	},
+	{0x005a0000,
+	 "PMU4: truncating FFE drive strength search range. Out of drive strengths to check.\n"
+	},
+	{0x005b0002,
+	 "PMU5: Weak 1 changed to pull-up %5d ohms, pull-down %5d ohms\n"
+	},
+	{0x005c0002,
+	 "PMU5: Weak 0 changed to pull-up %5d ohms, pull-down %5d ohms\n"
+	},
+	{0x005d0003,
+	 "PMU0: dlyMargin L %02d R %02d, min %02d\n"
+	},
+	{0x005e0003,
+	 "PMU0: vrefMargin T %02d B %02d, min %02d\n"
+	},
+	{0x005f0002,
+	 "PMU3: new minimum VrefMargin (%d < %d) recorded\n"
+	},
+	{0x00600002,
+	 "PMU3: new minimum DlyMargin (%d < %d) recorded\n"
+	},
+	{0x00610000,
+	 "PMU0: RX finding the per-nibble, per-tg rxClkDly values\n"
+	},
+	{0x00620003,
+	 "PMU0: Merging collected eyes [%d..%d) and analyzing for nibble %d's optimal rxClkDly\n"
+	},
+	{0x00630002,
+	 "PMU0: -- centers: delay = %d, voltage = %d\n"
+	},
+	{0x00640003,
+	 "PMU0: dumping optimized eye -- centers: delay = %d (%d), voltage = %d\n"
+	},
+	{0x00650000,
+	 "PMU0: TX optimizing txDqDelays\n"
+	},
+	{0x00660001,
+	 "PMU3: Analyzing collected eye %d for a lane's optimal TxDqDly\n"
+	},
+	{0x00670001,
+	 "PMU0: eye-lane %d is disable\n"
+	},
+	{0x00680000,
+	 "PMU0: TX optimizing device voltages\n"
+	},
+	{0x00690002,
+	 "PMU0: Merging collected eyes [%d..%d) and analyzing for optimal device txVref\n"
+	},
+	{0x006a0002,
+	 "PMU0: -- centers: delay = %d, voltage = %d\n"
+	},
+	{0x006b0003,
+	 "PMU0: dumping optimized eye -- centers: delay = %d (%d), voltage = %d\n"
+	},
+	{0x006c0000,
+	 "PMU4: VrefDac (compound all TG) Bottom Top -> Center\n"
+	},
+	{0x006d0005,
+	 "PMU4: DB%d L%d   %3d   %3d  ->  %3d (DISCONNECTED)\n"
+	},
+	{0x006e0005,
+	 "PMU4: DB%d L%d   %3d   %3d  ->  %3d\n"
+	},
+	{0x006f0005,
+	 "PMU0: writing rxClkDelay for tg%d db%1d nib%1d to 0x%02x from eye[%02d] (DISCONNECTED)\n"
+	},
+	{0x00700003,
+	 "PMU: Error: Dbyte %d nibble %d's optimal rxClkDly of 0x%x is out of bounds\n"
+	},
+	{0x00710005,
+	 "PMU0: writing rxClkDelay for tg%d db%1d nib%1d to 0x%02x from eye[%02d]\n"
+	},
+	{0x00720005,
+	 "PMU0: tx voltage for tg%2d nib%2d to %3d (%d) from eye[%02d]\n"
+	},
+	{0x00730001,
+	 "PMU0: vref Sum = %d\n"
+	},
+	{0x00740004,
+	 "PMU0: tx voltage total is %d/%d -> %d -> %d\n"
+	},
+	{0x00750007,
+	 "PMU0: writing txDqDelay for tg%1d db%1d ln%1d to  0x%02x (%d coarse, %d fine) from eye[%02d] (DISCONNECTED)\n"
+	},
+	{0x00760003,
+	 "PMU: Error: Dbyte %d lane %d's optimal txDqDly of 0x%x is out of bounds\n"
+	},
+	{0x00770007,
+	 "PMU0: writing txDqDelay for tg%1d db%1d l%1d to  0x%02x (%d coarse, %d fine) from eye[%02d]\n"
+	},
+	{0x00780002,
+	 "PMU0: %d (0=tx, 1=rx) TgMask for this simulation: %x\n"
+	},
+	{0x00790001,
+	 "PMU0: eye-byte %d is disable\n"
+	},
+	{0x007a0001,
+	 "PMU0: eye-lane %d is disable\n"
+	},
+	{0x007b0003,
+	 "PMU10: Start d4_2d_lrdimm_rx_dfe dimm %d nbTap %d biasStepMode %d\n"
+	},
+	{0x007c0001,
+	 "PMU10: DB DFE feature not fully supported, F2BCEx value is 0x%02x\n"
+	},
+	{0x007d0001,
+	 "PMU10: DB DFE feature fully supported, F2BCEx value is 0x%02x\n"
+	},
+	{0x007e0002,
+	 "PMU8: Start d4_2d_lrdimm_rx_dfe for tap %d biasStepInc %d\n"
+	},
+	{0x007f0001,
+	 "PMU7: Start d4_2d_lrdimm_rx_dfe tapCoff 0x%0x\n"
+	},
+	{0x00800003,
+	 "PMU6: d4_2d_lrdimm_rx_dfe db %d lane %d area %d\n"
+	},
+	{0x00810004,
+	 "PMU7: d4_2d_lrdimm_rx_dfe db %d lane %d max area %d best bias 0x%0x\n"
+	},
+	{0x00820001,
+	 "PMU0: eye-lane %d is disable\n"
+	},
+	{0x00830003,
+	 "PMU5: Setting 0x%x improved rank weight (%4d < %4d)\n"
+	},
+	{0x00840001,
+	 "PMU4: Setting 0x%x still optimal\n"
+	},
+	{0x00850002,
+	 "PMU5: ---- Training CS%d MR%d DRAM Equalization ----\n"
+	},
+	{0x00860001,
+	 "PMU0: eye-lane %d is disable\n"
+	},
+	{0x00870003,
+	 "PMU0: eye %d weight %d allTgWeight %d\n"
+	},
+	{0x00880002,
+	 "PMU5: FFE figure of merit improved from %d to %d\n"
+	},
+	{0x00890002,
+	 "PMU: Error: LP4 rank %d cannot be mapped on tg %d\n"
+	},
+	{0x008a0000,
+	 "PMU4: Adjusting vrefDac0 for just 1->x transitions\n"
+	},
+	{0x008b0000,
+	 "PMU4: Adjusting vrefDac1 for just 0->x transitions\n"
+	},
+	{0x008c0001,
+	 "PMU5: Strong 1, pull-up %d ohms\n"
+	},
+	{0x008d0001,
+	 "PMU5: Strong 0, pull-down %d ohms\n"
+	},
+	{0x008e0000,
+	 "PMU4: Enabling weak drive strengths (FFE)\n"
+	},
+	{0x008f0000,
+	 "PMU5: Changing all weak driver strengths\n"
+	},
+	{0x00900000,
+	 "PMU5: Finalizing weak drive strengths\n"
+	},
+	{0x00910000,
+	 "PMU4: retraining with optimal drive strength settings\n"
+	},
+	{0x00920002,
+	 "PMU0: targeting CsX = %d and CsY = %d\n"
+	},
+	{0x00930001,
+	 "PMU1:prbsGenCtl:%x\n"
+	},
+	{0x00940000,
+	 "PMU1: loading 2D acsm sequence\n"
+	},
+	{0x00950000,
+	 "PMU1: loading 1D acsm sequence\n"
+	},
+	{0x00960002,
+	 "PMU3: %d memclocks @ %d to get half of 300ns\n"
+	},
+	{0x00970000,
+	 "PMU: Error: User requested MPR read pattern for read DQS training in DDR3 Mode\n"
+	},
+	{0x00980000,
+	 "PMU3: Running 1D search for left eye edge\n"
+	},
+	{0x00990001,
+	 "PMU1: In Phase Left Edge Search cs %d\n"
+	},
+	{0x009a0001,
+	 "PMU1: Out of Phase Left Edge Search cs %d\n"
+	},
+	{0x009b0000,
+	 "PMU3: Running 1D search for right eye edge\n"
+	},
+	{0x009c0001,
+	 "PMU1: In Phase Right Edge Search cs %d\n"
+	},
+	{0x009d0001,
+	 "PMU1: Out of Phase Right Edge Search cs %d\n"
+	},
+	{0x009e0001,
+	 "PMU1: mxRdLat training pstate %d\n"
+	},
+	{0x009f0001,
+	 "PMU1: mxRdLat search for cs %d\n"
+	},
+	{0x00a00001,
+	 "PMU0: MaxRdLat non consistent DtsmLoThldXingInd 0x%03x\n"
+	},
+	{0x00a10003,
+	 "PMU4: CS %d Dbyte %d worked with DFIMRL = %d DFICLKs\n"
+	},
+	{0x00a20004,
+	 "PMU3: MaxRdLat Read Lane err mask for csn %d, DFIMRL %2d DFIClks, dbyte %d = 0x%03x\n"
+	},
+	{0x00a30003,
+	 "PMU3: MaxRdLat Read Lane err mask for csn %d DFIMRL %2d, All dbytes = 0x%03x\n"
+	},
+	{0x00a40001,
+	 "PMU: Error: CS%d failed to find a DFIMRL setting that worked for all bytes during MaxRdLat training\n"
+	},
+	{0x00a50002,
+	 "PMU3: Smallest passing DFIMRL for all dbytes in CS%d = %d DFIClks\n"
+	},
+	{0x00a60000,
+	 "PMU: Error: No passing DFIMRL value found for any chip select during MaxRdLat training\n"
+	},
+	{0x00a70003,
+	 "PMU: Error: Dbyte %d lane %d txDqDly passing region is too small (width = %d)\n"
+	},
+	{0x00a80006,
+	 "PMU10: Adjusting rxclkdly db %d nib %d from %d+%d=%d->%d\n"
+	},
+	{0x00a90000,
+	 "PMU4: TxDqDly Passing Regions (EyeLeft EyeRight -> EyeCenter) Units=1/32 UI\n"
+	},
+	{0x00aa0005,
+	 "PMU4: DB %d Lane %d: %3d %3d -> %3d\n"
+	},
+	{0x00ab0002,
+	 "PMU2: TXDQ delayLeft[%2d] = %3d (DISCONNECTED)\n"
+	},
+	{0x00ac0004,
+	 "PMU2: TXDQ delayLeft[%2d] = %3d oopScaled = %3d selectOop %d\n"
+	},
+	{0x00ad0002,
+	 "PMU2: TXDQ delayRight[%2d] = %3d (DISCONNECTED)\n"
+	},
+	{0x00ae0004,
+	 "PMU2: TXDQ delayRight[%2d] = %3d oopScaled = %3d selectOop %d\n"
+	},
+	{0x00af0003,
+	 "PMU: Error: Dbyte %d lane %d txDqDly passing region is too small (width = %d)\n"
+	},
+	{0x00b00000,
+	 "PMU4: TxDqDly Passing Regions (EyeLeft EyeRight -> EyeCenter) Units=1/32 UI\n"
+	},
+	{0x00b10002,
+	 "PMU4: DB %d Lane %d: (DISCONNECTED)\n"
+	},
+	{0x00b20005,
+	 "PMU4: DB %d Lane %d: %3d %3d -> %3d\n"
+	},
+	{0x00b30002,
+	 "PMU3: Running 1D search csn %d for DM Right/NotLeft(%d) eye edge\n"
+	},
+	{0x00b40002,
+	 "PMU3: WrDq DM byte%2d with Errcnt %d\n"
+	},
+	{0x00b50002,
+	 "PMU3: WrDq DM byte%2d avgDly 0x%04x\n"
+	},
+	{0x00b60002,
+	 "PMU1: WrDq DM byte%2d with Errcnt %d\n"
+	},
+	{0x00b70001,
+	 "PMU: Error: Dbyte %d txDqDly DM training did not start inside the eye\n"
+	},
+	{0x00b80000,
+	 "PMU4: DM TxDqDly Passing Regions (EyeLeft EyeRight -> EyeCenter) Units=1/32 UI\n"
+	},
+	{0x00b90002,
+	 "PMU4: DB %d Lane %d: (DISCONNECTED)\n"
+	},
+	{0x00ba0005,
+	 "PMU4: DB %d Lane %d: %3d %3d -> %3d\n"
+	},
+	{0x00bb0003,
+	 "PMU: Error: Dbyte %d lane %d txDqDly DM passing region is too small (width = %d)\n"
+	},
+	{0x00bc0004,
+	 "PMU3: Errcnt for MRD/MWD search nib %2d delay = (%d, 0x%02x) = %d\n"
+	},
+	{0x00bd0000,
+	 "PMU3: Precharge all open banks\n"
+	},
+	{0x00be0002,
+	 "PMU: Error: Dbyte %d nibble %d found mutliple working coarse delay setting for MRD/MWD\n"
+	},
+	{0x00bf0000,
+	 "PMU4: MRD Passing Regions (coarseVal, fineLeft fineRight -> fineCenter)\n"
+	},
+	{0x00c00000,
+	 "PMU4: MWD Passing Regions (coarseVal, fineLeft fineRight -> fineCenter)\n"
+	},
+	{0x00c10004,
+	 "PMU10: Warning: DB %d nibble %d has multiple working coarse delays, %d and %d, choosing the smaller delay\n"
+	},
+	{0x00c20003,
+	 "PMU: Error: Dbyte %d nibble %d MRD/MWD passing region is too small (width = %d)\n"
+	},
+	{0x00c30006,
+	 "PMU4: DB %d nibble %d: %3d, %3d %3d -> %3d\n"
+	},
+	{0x00c40002,
+	 "PMU1: Start MRD/nMWD %d for csn %d\n"
+	},
+	{0x00c50002,
+	 "PMU2: RXDQS delayLeft[%2d] = %3d (DISCONNECTED)\n"
+	},
+	{0x00c60006,
+	 "PMU2: RXDQS delayLeft[%2d] = %3d delayOop[%2d] = %3d OopScaled %4d, selectOop %d\n"
+	},
+	{0x00c70002,
+	 "PMU2: RXDQS delayRight[%2d] = %3d (DISCONNECTED)\n"
+	},
+	{0x00c80006,
+	 "PMU2: RXDQS delayRight[%2d] = %3d delayOop[%2d] = %4d OopScaled %4d, selectOop %d\n"
+	},
+	{0x00c90000,
+	 "PMU4: RxClkDly Passing Regions (EyeLeft EyeRight -> EyeCenter)\n"
+	},
+	{0x00ca0002,
+	 "PMU4: DB %d nibble %d: (DISCONNECTED)\n"
+	},
+	{0x00cb0005,
+	 "PMU4: DB %d nibble %d: %3d %3d -> %3d\n"
+	},
+	{0x00cc0003,
+	 "PMU: Error: Dbyte %d nibble %d rxClkDly passing region is too small (width = %d)\n"
+	},
+	{0x00cd0002,
+	 "PMU0: goodbar = %d for RDWR_BLEN %d\n"
+	},
+	{0x00ce0001,
+	 "PMU3: RxClkDly = %d\n"
+	},
+	{0x00cf0005,
+	 "PMU0: db %d l %d absLane %d -> bottom %d top %d\n"
+	},
+	{0x00d00009,
+	 "PMU3: BYTE %d - %3d %3d %3d %3d %3d %3d %3d %3d\n"
+	},
+	{0x00d10002,
+	 "PMU: Error: dbyte %d lane %d's per-lane vrefDAC's had no passing region\n"
+	},
+	{0x00d20004,
+	 "PMU0: db%d l%d - %d %d\n"
+	},
+	{0x00d30002,
+	 "PMU0: goodbar = %d for RDWR_BLEN %d\n"
+	},
+	{0x00d40004,
+	 "PMU3: db%d l%d saw %d issues at rxClkDly %d\n"
+	},
+	{0x00d50003,
+	 "PMU3: db%d l%d first saw a pass->fail edge at rxClkDly %d\n"
+	},
+	{0x00d60002,
+	 "PMU3: lane %d PBD = %d\n"
+	},
+	{0x00d70003,
+	 "PMU3: db%d l%d first saw a DBI pass->fail edge at rxClkDly %d\n"
+	},
+	{0x00d80003,
+	 "PMU2: db%d l%d already passed rxPBD = %d\n"
+	},
+	{0x00d90003,
+	 "PMU0: db%d l%d, PBD = %d\n"
+	},
+	{0x00da0002,
+	 "PMU: Error: dbyte %d lane %d failed read deskew\n"
+	},
+	{0x00db0003,
+	 "PMU0: db%d l%d, inc PBD = %d\n"
+	},
+	{0x00dc0003,
+	 "PMU1: Running lane deskew on pstate %d csn %d rdDBIEn %d\n"
+	},
+	{0x00dd0000,
+	 "PMU: Error: Read deskew training has been requested, but csrMajorModeDbyte[2] is set\n"
+	},
+	{0x00de0002,
+	 "PMU1: AcsmCsMapCtrl%02d 0x%04x\n"
+	},
+	{0x00df0002,
+	 "PMU1: AcsmCsMapCtrl%02d 0x%04x\n"
+	},
+	{0x00e00001,
+	 "PMU: Error: Wrong PMU image loaded. message Block DramType = 0x%02x, but image built for D3U Type\n"
+	},
+	{0x00e10001,
+	 "PMU: Error: Wrong PMU image loaded. message Block DramType = 0x%02x, but image built for D3R Type\n"
+	},
+	{0x00e20001,
+	 "PMU: Error: Wrong PMU image loaded. message Block DramType = 0x%02x, but image built for D4U Type\n"
+	},
+	{0x00e30001,
+	 "PMU: Error: Wrong PMU image loaded. message Block DramType = 0x%02x, but image built for D4R Type\n"
+	},
+	{0x00e40001,
+	 "PMU: Error: Wrong PMU image loaded. message Block DramType = 0x%02x, but image built for D4LR Type\n"
+	},
+	{0x00e50000,
+	 "PMU: Error: Both 2t timing mode and ddr4 geardown mode specified in the messageblock's PhyCfg and MR3 fields. Only one can be enabled\n"
+	},
+	{0x00e60003,
+	 "PMU10: PHY TOTALS - NUM_DBYTES %d NUM_NIBBLES %d NUM_ANIBS %d\n"
+	},
+	{0x00e70006,
+	 "PMU10: CSA=0x%02x, CSB=0x%02x, TSTAGES=0x%04x, HDTOUT=%d, MMISC=%d DRAMFreq=%dMT DramType=LPDDR3\n"
+	},
+	{0x00e80006,
+	 "PMU10: CSA=0x%02x, CSB=0x%02x, TSTAGES=0x%04x, HDTOUT=%d, MMISC=%d DRAMFreq=%dMT DramType=LPDDR4\n"
+	},
+	{0x00e90008,
+	 "PMU10: CS=0x%02x, TSTAGES=0x%04x, HDTOUT=%d, 2T=%d, MMISC=%d AddrMirror=%d DRAMFreq=%dMT DramType=%d\n"
+	},
+	{0x00ea0004,
+	 "PMU10: Pstate%d MR0=0x%04x MR1=0x%04x MR2=0x%04x\n"
+	},
+	{0x00eb0008,
+	 "PMU10: Pstate%d MRS MR0=0x%04x MR1=0x%04x MR2=0x%04x MR3=0x%04x MR4=0x%04x MR5=0x%04x MR6=0x%04x\n"
+	},
+	{0x00ec0005,
+	 "PMU10: Pstate%d MRS MR1_A0=0x%04x MR2_A0=0x%04x MR3_A0=0x%04x MR11_A0=0x%04x\n"
+	},
+	{0x00ed0000,
+	 "PMU10: UseBroadcastMR set. All ranks and channels use MRXX_A0 for MR settings.\n"
+	},
+	{0x00ee0005,
+	 "PMU10: Pstate%d MRS MR01_A0=0x%02x MR02_A0=0x%02x MR03_A0=0x%02x MR11_A0=0x%02x\n"
+	},
+	{0x00ef0005,
+	 "PMU10: Pstate%d MRS MR12_A0=0x%02x MR13_A0=0x%02x MR14_A0=0x%02x MR22_A0=0x%02x\n"
+	},
+	{0x00f00005,
+	 "PMU10: Pstate%d MRS MR01_A1=0x%02x MR02_A1=0x%02x MR03_A1=0x%02x MR11_A1=0x%02x\n"
+	},
+	{0x00f10005,
+	 "PMU10: Pstate%d MRS MR12_A1=0x%02x MR13_A1=0x%02x MR14_A1=0x%02x MR22_A1=0x%02x\n"
+	},
+	{0x00f20005,
+	 "PMU10: Pstate%d MRS MR01_B0=0x%02x MR02_B0=0x%02x MR03_B0=0x%02x MR11_B0=0x%02x\n"
+	},
+	{0x00f30005,
+	 "PMU10: Pstate%d MRS MR12_B0=0x%02x MR13_B0=0x%02x MR14_B0=0x%02x MR22_B0=0x%02x\n"
+	},
+	{0x00f40005,
+	 "PMU10: Pstate%d MRS MR01_B1=0x%02x MR02_B1=0x%02x MR03_B1=0x%02x MR11_B1=0x%02x\n"
+	},
+	{0x00f50005,
+	 "PMU10: Pstate%d MRS MR12_B1=0x%02x MR13_B1=0x%02x MR14_B1=0x%02x MR22_B1=0x%02x\n"
+	},
+	{0x00f60002,
+	 "PMU1: AcsmOdtCtrl%02d 0x%02x\n"
+	},
+	{0x00f70002,
+	 "PMU1: AcsmCsMapCtrl%02d 0x%04x\n"
+	},
+	{0x00f80002,
+	 "PMU1: AcsmCsMapCtrl%02d 0x%04x\n"
+	},
+	{0x00f90000,
+	 "PMU1: HwtCAMode set\n"
+	},
+	{0x00fa0001,
+	 "PMU3: DDR4 infinite preamble enter/exit mode %d\n"
+	},
+	{0x00fb0002,
+	 "PMU1: In rxenb_train() csn=%d pstate=%d\n"
+	},
+	{0x00fc0000,
+	 "PMU3: Finding DQS falling edge\n"
+	},
+	{0x00fd0000,
+	 "PMU3: Searching for DDR3/LPDDR3/LPDDR4 read preamble\n"
+	},
+	{0x00fe0009,
+	 "PMU3: dtsm fails Even Nibbles : %2x %2x %2x %2x %2x %2x %2x %2x %2x\n"
+	},
+	{0x00ff0009,
+	 "PMU3: dtsm fails Odd  Nibbles : %2x %2x %2x %2x %2x %2x %2x %2x %2x\n"
+	},
+	{0x01000002,
+	 "PMU3: Preamble search pass=%d anyfail=%d\n"
+	},
+	{0x01010000,
+	 "PMU: Error: RxEn training preamble not found\n"
+	},
+	{0x01020000,
+	 "PMU3: Found DQS pre-amble\n"
+	},
+	{0x01030001,
+	 "PMU: Error: Dbyte %d couldn't find the rising edge of DQS during RxEn Training\n"
+	},
+	{0x01040000,
+	 "PMU3: RxEn aligning to first rising edge of burst\n"
+	},
+	{0x01050001,
+	 "PMU3: Decreasing RxEn delay by %d fine step to allow full capture of reads\n"
+	},
+	{0x01060001,
+	 "PMU3: MREP Delay = %d\n"
+	},
+	{0x01070003,
+	 "PMU3: Errcnt for MREP nib %2d delay = %2d is %d\n"
+	},
+	{0x01080002,
+	 "PMU3: MREP nibble %d sampled a 1 at data buffer delay %d\n"
+	},
+	{0x01090002,
+	 "PMU3: MREP nibble %d saw a 0 to 1 transition at data buffer delay %d\n"
+	},
+	{0x010a0000,
+	 "PMU2:  MREP did not find a 0 to 1 transition for all nibbles. Failing nibbles assumed to have rising edge close to fine delay 63\n"
+	},
+	{0x010b0002,
+	 "PMU2:  Rising edge found in alias window, setting rxDly for nibble %d = %d\n"
+	},
+	{0x010c0002,
+	 "PMU: Error: Failed MREP for nib %d with %d one\n"
+	},
+	{0x010d0003,
+	 "PMU2:  Rising edge not found in alias window with %d one, leaving rxDly for nibble %d = %d\n"
+	},
+	{0x010e0002,
+	 "PMU3: Training DIMM %d CSn %d\n"
+	},
+	{0x010f0001,
+	 "PMU3: exitCAtrain_lp3 cs 0x%x\n"
+	},
+	{0x01100001,
+	 "PMU3: enterCAtrain_lp3 cs 0x%x\n"
+	},
+	{0x01110001,
+	 "PMU3: CAtrain_switchmsb_lp3 cs 0x%x\n"
+	},
+	{0x01120001,
+	 "PMU3: CATrain_rdwr_lp3 looking for pattern %x\n"
+	},
+	{0x01130000,
+	 "PMU3: exitCAtrain_lp4\n"
+	},
+	{0x01140001,
+	 "PMU3: DEBUG enterCAtrain_lp4 1: cs 0x%x\n"
+	},
+	{0x01150001,
+	 "PMU3: DEBUG enterCAtrain_lp4 3: Put dbyte %d in async mode\n"
+	},
+	{0x01160000,
+	 "PMU3: DEBUG enterCAtrain_lp4 5: Send MR13 to turn on CA training\n"
+	},
+	{0x01170003,
+	 "PMU3: DEBUG enterCAtrain_lp4 7: idx = %d vref = %x mr12 = %x\n"
+	},
+	{0x01180001,
+	 "PMU3: CATrain_rdwr_lp4 looking for pattern %x\n"
+	},
+	{0x01190004,
+	 "PMU3: Phase %d CAreadbackA db:%d %x xo:%x\n"
+	},
+	{0x011a0005,
+	 "PMU3: DEBUG lp4SetCatrVref 1: cs=%d chan=%d mr12=%x vref=%d.%d%%\n"
+	},
+	{0x011b0003,
+	 "PMU3: DEBUG lp4SetCatrVref 3: mr12 = %x send vref= %x to db=%d\n"
+	},
+	{0x011c0000,
+	 "PMU10:Optimizing vref\n"
+	},
+	{0x011d0004,
+	 "PMU4:mr12:%2x cs:%d chan %d r:%4x\n"
+	},
+	{0x011e0005,
+	 "PMU3: i:%2d bstr:%2d bsto:%2d st:%d r:%d\n"
+	},
+	{0x011f0002,
+	 "Failed to find sufficient CA Vref Passing Region for CS %d ch. %d\n"
+	},
+	{0x01200005,
+	 "PMU3:Found %d.%d%% MR12:%x for cs:%d chan %d\n"
+	},
+	{0x01210002,
+	 "PMU3:Calculated %d for AtxImpedence from acx %d.\n"
+	},
+	{0x01220000,
+	 "PMU3:CA Odt impedence ==0.  Use default vref.\n"
+	},
+	{0x01230003,
+	 "PMU3:Calculated %d.%d%% for Vref MR12=0x%x.\n"
+	},
+	{0x01240000,
+	 "PMU3: CAtrain_lp\n"
+	},
+	{0x01250000,
+	 "PMU3: CAtrain Begins.\n"
+	},
+	{0x01260001,
+	 "PMU3: CAtrain_lp testing dly %d\n"
+	},
+	{0x01270001,
+	 "PMU5: CA bitmap dump for cs %x\n"
+	},
+	{0x01280001,
+	 "PMU5: CAA%d "
+	},
+	{0x01290001, "%02x"
+	},
+	{0x012a0000, "\n"
+	},
+	{0x012b0001,
+	 "PMU5: CAB%d "
+	},
+	{0x012c0001, "%02x"
+	},
+	{0x012d0000, "\n"
+	},
+	{0x012e0003,
+	 "PMU3: anibi=%d, anibichan[anibi]=%d ,chan=%d\n"
+	},
+	{0x012f0001, "%02x"
+	},
+	{0x01300001, "\nPMU3:Raw CA setting :%x"
+	},
+	{0x01310002, "\nPMU3:ATxDly setting:%x margin:%d\n"
+	},
+	{0x01320002, "\nPMU3:InvClk ATxDly setting:%x margin:%d\n"
+	},
+	{0x01330000, "\nPMU3:No Range found!\n"
+	},
+	{0x01340003,
+	 "PMU3: 2 anibi=%d, anibichan[anibi]=%d ,chan=%d"
+	},
+	{0x01350002, "\nPMU3: no neg clock => CA setting anib=%d, :%d\n"
+	},
+	{0x01360001,
+	 "PMU3:Normal margin:%d\n"
+	},
+	{0x01370001,
+	 "PMU3:Inverted margin:%d\n"
+	},
+	{0x01380000,
+	 "PMU3:Using Inverted clock\n"
+	},
+	{0x01390000,
+	 "PMU3:Using normal clk\n"
+	},
+	{0x013a0003,
+	 "PMU3: 3 anibi=%d, anibichan[anibi]=%d ,chan=%d\n"
+	},
+	{0x013b0002,
+	 "PMU3: Setting ATxDly for anib %x to %x\n"
+	},
+	{0x013c0000,
+	 "PMU: Error: CA Training Failed.\n"
+	},
+	{0x013d0000,
+	 "PMU1: Writing MRs\n"
+	},
+	{0x013e0000,
+	 "PMU4:Using MR12 values from 1D CA VREF training.\n"
+	},
+	{0x013f0000,
+	 "PMU3:Writing all MRs to fsp 1\n"
+	},
+	{0x01400000,
+	 "PMU10:Lp4Quickboot mode.\n"
+	},
+	{0x01410000,
+	 "PMU3: Writing MRs\n"
+	},
+	{0x01420001,
+	 "PMU10: Setting boot clock divider to %d\n"
+	},
+	{0x01430000,
+	 "PMU3: Resetting DRAM\n"
+	},
+	{0x01440000,
+	 "PMU3: setup for RCD initalization\n"
+	},
+	{0x01450000,
+	 "PMU3: pmu_exit_SR from dev_init()\n"
+	},
+	{0x01460000,
+	 "PMU3: initializing RCD\n"
+	},
+	{0x01470000,
+	 "PMU10: **** Executing 2D Image ****\n"
+	},
+	{0x01480001,
+	 "PMU10: **** Start DDR4 Training. PMU Firmware Revision 0x%04x ****\n"
+	},
+	{0x01490001,
+	 "PMU10: **** Start DDR3 Training. PMU Firmware Revision 0x%04x ****\n"
+	},
+	{0x014a0001,
+	 "PMU10: **** Start LPDDR3 Training. PMU Firmware Revision 0x%04x ****\n"
+	},
+	{0x014b0001,
+	 "PMU10: **** Start LPDDR4 Training. PMU Firmware Revision 0x%04x ****\n"
+	},
+	{0x014c0000,
+	 "PMU: Error: Mismatched internal revision between DCCM and ICCM images\n"
+	},
+	{0x014d0001,
+	 "PMU10: **** Testchip %d Specific Firmware ****\n"
+	},
+	{0x014e0000,
+	 "PMU1: LRDIMM with EncodedCS mode, one DIMM\n"
+	},
+	{0x014f0000,
+	 "PMU1: LRDIMM with EncodedCS mode, two DIMMs\n"
+	},
+	{0x01500000,
+	 "PMU1: RDIMM with EncodedCS mode, one DIMM\n"
+	},
+	{0x01510000,
+	 "PMU2: Starting LRDIMM MREP training for all ranks\n"
+	},
+	{0x01520000,
+	 "PMU199: LRDIMM MREP training for all ranks completed\n"
+	},
+	{0x01530000,
+	 "PMU2: Starting LRDIMM DWL training for all ranks\n"
+	},
+	{0x01540000,
+	 "PMU199: LRDIMM DWL training for all ranks completed\n"
+	},
+	{0x01550000,
+	 "PMU2: Starting LRDIMM MRD training for all ranks\n"
+	},
+	{0x01560000,
+	 "PMU199: LRDIMM MRD training for all ranks completed\n"
+	},
+	{0x01570000,
+	 "PMU2: Starting RXEN training for all ranks\n"
+	},
+	{0x01580000,
+	 "PMU2: Starting write leveling fine delay training for all ranks\n"
+	},
+	{0x01590000,
+	 "PMU2: Starting LRDIMM MWD training for all ranks\n"
+	},
+	{0x015a0000,
+	 "PMU199: LRDIMM MWD training for all ranks completed\n"
+	},
+	{0x015b0000,
+	 "PMU2: Starting write leveling fine delay training for all ranks\n"
+	},
+	{0x015c0000,
+	 "PMU2: Starting read deskew training\n"
+	},
+	{0x015d0000,
+	 "PMU2: Starting SI friendly 1d RdDqs training for all ranks\n"
+	},
+	{0x015e0000,
+	 "PMU2: Starting write leveling coarse delay training for all ranks\n"
+	},
+	{0x015f0000,
+	 "PMU2: Starting 1d WrDq training for all ranks\n"
+	},
+	{0x01600000,
+	 "PMU2: Running DQS2DQ Oscillator for all ranks\n"
+	},
+	{0x01610000,
+	 "PMU2: Starting again read deskew training but with PRBS\n"
+	},
+	{0x01620000,
+	 "PMU2: Starting 1d RdDqs training for all ranks\n"
+	},
+	{0x01630000,
+	 "PMU2: Starting again 1d WrDq training for all ranks\n"
+	},
+	{0x01640000,
+	 "PMU2: Starting MaxRdLat training\n"
+	},
+	{0x01650000,
+	 "PMU2: Starting 2d WrDq training for all ranks\n"
+	},
+	{0x01660000,
+	 "PMU2: Starting 2d RdDqs training for all ranks\n"
+	},
+	{0x01670002,
+	 "PMU3:read_fifo %x %x\n"
+	},
+	{0x01680001,
+	 "PMU: Error: Invalid PhyDrvImpedance of 0x%x specified in message block.\n"
+	},
+	{0x01690001,
+	 "PMU: Error: Invalid PhyOdtImpedance of 0x%x specified in message block.\n"
+	},
+	{0x016a0001,
+	 "PMU: Error: Invalid BPZNResVal of 0x%x specified in message block.\n"
+	},
+	{0x016b0005,
+	 "PMU3: fixRxEnBackOff csn:%d db:%d dn:%d bo:%d dly:%x\n"
+	},
+	{0x016c0001,
+	 "PMU3: fixRxEnBackOff dly:%x\n"
+	},
+	{0x016d0000,
+	 "PMU3: Entering setupPpt\n"
+	},
+	{0x016e0000,
+	 "PMU3: Start lp4PopulateHighLowBytes\n"
+	},
+	{0x016f0002,
+	 "PMU3:Dbyte Detect: db%d received %x\n"
+	},
+	{0x01700002,
+	 "PMU3:getDqs2Dq read %x from dbyte %d\n"
+	},
+	{0x01710002,
+	 "PMU3:getDqs2Dq(2) read %x from dbyte %d\n"
+	},
+	{0x01720001,
+	 "PMU: Error: Dbyte %d read 0 from the DQS oscillator it is connected to\n"
+	},
+	{0x01730002,
+	 "PMU4: Dbyte %d dqs2dq = %d/32 UI\n"
+	},
+	{0x01740003,
+	 "PMU3:getDqs2Dq set dqs2dq:%d/32 ui (%d ps) from dbyte %d\n"
+	},
+	{0x01750003,
+	 "PMU3: Setting coarse delay in AtxDly chiplet %d from 0x%02x to 0x%02x\n"
+	},
+	{0x01760003,
+	 "PMU3: Clearing coarse delay in AtxDly chiplet %d from 0x%02x to 0x%02x\n"
+	},
+	{0x01770000,
+	 "PMU3: Performing DDR4 geardown sync sequence\n"
+	},
+	{0x01780000,
+	 "PMU1: Enter self refresh\n"
+	},
+	{0x01790000,
+	 "PMU1: Exit self refresh\n"
+	},
+	{0x017a0000,
+	 "PMU: Error: No dbiEnable with lp4\n"
+	},
+	{0x017b0000,
+	 "PMU: Error: No dbiDisable with lp4\n"
+	},
+	{0x017c0001,
+	 "PMU1: DDR4 update Rx DBI Setting disable %d\n"
+	},
+	{0x017d0001,
+	 "PMU1: DDR4 update 2nCk WPre Setting disable %d\n"
+	},
+	{0x017e0005,
+	 "PMU1: read_delay: db%d lane%d delays[%2d] = 0x%02x (max 0x%02x)\n"
+	},
+	{0x017f0004,
+	 "PMU1: write_delay: db%d lane%d delays[%2d] = 0x%04x\n"
+	},
+	{0x01800001,
+	 "PMU5: ID=%d -- db0  db1  db2  db3  db4  db5  db6  db7  db8  db9 --\n"
+	},
+	{0x0181000b,
+	 "PMU5: [%d]:0x %04x %04x %04x %04x %04x %04x %04x %04x %04x %04x\n"
+	},
+	{0x01820003,
+	 "PMU2: dump delays - pstate=%d dimm=%d csn=%d\n"
+	},
+	{0x01830000,
+	 "PMU3: Printing Mid-Training Delay Information\n"
+	},
+	{0x01840001,
+	 "PMU5: CS%d <<KEY>> 0 TrainingCntr <<KEY>> coarse(15:10) fine(9:0)\n"
+	},
+	{0x01850001,
+	 "PMU5: CS%d <<KEY>> 0 RxEnDly, 1 RxClkDly <<KEY>> coarse(10:6) fine(5:0)\n"
+	},
+	{0x01860001,
+	 "PMU5: CS%d <<KEY>> 0 TxDqsDly, 1 TxDqDly <<KEY>> coarse(9:6) fine(5:0)\n"
+	},
+	{0x01870001,
+	 "PMU5: CS%d <<KEY>> 0 RxPBDly <<KEY>> 1 Delay Unit ~= 7ps\n"
+	},
+	{0x01880000,
+	 "PMU5: all CS <<KEY>> 0 DFIMRL <<KEY>> Units = DFI clocks\n"
+	},
+	{0x01890000,
+	 "PMU5: all CS <<KEY>> VrefDACs <<KEY>> DAC(6:0)\n"
+	},
+	{0x018a0000,
+	 "PMU1: Set DMD in MR13 and wrDBI in MR3 for training\n"
+	},
+	{0x018b0000,
+	 "PMU: Error: getMaxRxen() failed to find largest rxen nibble delay\n"
+	},
+	{0x018c0003,
+	 "PMU2: getMaxRxen(): maxDly %d maxTg %d maxNib %d\n"
+	},
+	{0x018d0003,
+	 "PMU2: getRankMaxRxen(): maxDly %d Tg %d maxNib %d\n"
+	},
+	{0x018e0000,
+	 "PMU1: skipping CDD calculation in 2D image\n"
+	},
+	{0x018f0001,
+	 "PMU3: Calculating CDDs for pstate %d\n"
+	},
+	{0x01900003,
+	 "PMU3: rxFromDly[%d][%d] = %d\n"
+	},
+	{0x01910003,
+	 "PMU3: rxToDly  [%d][%d] = %d\n"
+	},
+	{0x01920003,
+	 "PMU3: rxDly    [%d][%d] = %d\n"
+	},
+	{0x01930003,
+	 "PMU3: txDly    [%d][%d] = %d\n"
+	},
+	{0x01940003,
+	 "PMU3: allFine CDD_RR_%d_%d = %d\n"
+	},
+	{0x01950003,
+	 "PMU3: allFine CDD_WW_%d_%d = %d\n"
+	},
+	{0x01960003,
+	 "PMU3: CDD_RR_%d_%d = %d\n"
+	},
+	{0x01970003,
+	 "PMU3: CDD_WW_%d_%d = %d\n"
+	},
+	{0x01980003,
+	 "PMU3: allFine CDD_RW_%d_%d = %d\n"
+	},
+	{0x01990003,
+	 "PMU3: allFine CDD_WR_%d_%d = %d\n"
+	},
+	{0x019a0003,
+	 "PMU3: CDD_RW_%d_%d = %d\n"
+	},
+	{0x019b0003,
+	 "PMU3: CDD_WR_%d_%d = %d\n"
+	},
+	{0x019c0004,
+	 "PMU3: F%dBC2x_B%d_D%d = 0x%02x\n"
+	},
+	{0x019d0004,
+	 "PMU3: F%dBC3x_B%d_D%d = 0x%02x\n"
+	},
+	{0x019e0004,
+	 "PMU3: F%dBC4x_B%d_D%d = 0x%02x\n"
+	},
+	{0x019f0004,
+	 "PMU3: F%dBC5x_B%d_D%d = 0x%02x\n"
+	},
+	{0x01a00004,
+	 "PMU3: F%dBC8x_B%d_D%d = 0x%02x\n"
+	},
+	{0x01a10004,
+	 "PMU3: F%dBC9x_B%d_D%d = 0x%02x\n"
+	},
+	{0x01a20004,
+	 "PMU3: F%dBCAx_B%d_D%d = 0x%02x\n"
+	},
+	{0x01a30004,
+	 "PMU3: F%dBCBx_B%d_D%d = 0x%02x\n"
+	},
+	{0x01a40000,
+	 "PMU10: Entering context_switch_postamble\n"
+	},
+	{0x01a50003,
+	 "PMU10: context_switch_postamble is enabled for DIMM %d, RC0A=0x%x, RC3x=0x%x\n"
+	},
+	{0x01a60000,
+	 "PMU10: Setting bcw fspace 0\n"
+	},
+	{0x01a70001,
+	 "PMU10: Sending BC0A = 0x%x\n"
+	},
+	{0x01a80001,
+	 "PMU10: Sending BC6x = 0x%x\n"
+	},
+	{0x01a90001,
+	 "PMU10: Sending RC0A = 0x%x\n"
+	},
+	{0x01aa0001,
+	 "PMU10: Sending RC3x = 0x%x\n"
+	},
+	{0x01ab0001,
+	 "PMU10: Sending RC0A = 0x%x\n"
+	},
+	{0x01ac0001,
+	 "PMU1: enter_lp3: DEBUG: pstate = %d\n"
+	},
+	{0x01ad0001,
+	 "PMU1: enter_lp3: DEBUG: dfifreqxlat_pstate = %d\n"
+	},
+	{0x01ae0001,
+	 "PMU1: enter_lp3: DEBUG: pllbypass = %d\n"
+	},
+	{0x01af0001,
+	 "PMU1: enter_lp3: DEBUG: forcecal = %d\n"
+	},
+	{0x01b00001,
+	 "PMU1: enter_lp3: DEBUG: pllmaxrange = 0x%x\n"
+	},
+	{0x01b10001,
+	 "PMU1: enter_lp3: DEBUG: dacval_out = 0x%x\n"
+	},
+	{0x01b20001,
+	 "PMU1: enter_lp3: DEBUG: pllctrl3 = 0x%x\n"
+	},
+	{0x01b30000,
+	 "PMU3: Loading DRAM with BIOS supplied MR values and entering self refresh prior to exiting PMU code.\n"
+	},
+	{0x01b40002,
+	 "PMU3: Setting DataBuffer function space of dimmcs 0x%02x to %d\n"
+	},
+	{0x01b50002,
+	 "PMU4: Setting RCW FxRC%Xx = 0x%02x\n"
+	},
+	{0x01b60002,
+	 "PMU4: Setting RCW FxRC%02x = 0x%02x\n"
+	},
+	{0x01b70001,
+	 "PMU1: DDR4 update Rd Pre Setting disable %d\n"
+	},
+	{0x01b80002,
+	 "PMU2: Setting BCW FxBC%Xx = 0x%02x\n"
+	},
+	{0x01b90002,
+	 "PMU2: Setting BCW BC%02x = 0x%02x\n"
+	},
+	{0x01ba0002,
+	 "PMU2: Setting BCW PBA mode FxBC%Xx = 0x%02x\n"
+	},
+	{0x01bb0002,
+	 "PMU2: Setting BCW PBA mode BC%02x = 0x%02x\n"
+	},
+	{0x01bc0003,
+	 "PMU4: BCW value for dimm %d, fspace %d, addr 0x%04x\n"
+	},
+	{0x01bd0002,
+	 "PMU4: DB %d, value 0x%02x\n"
+	},
+	{0x01be0000,
+	 "PMU6: WARNING MREP underflow, set to min value -2 coarse, 0 fine\n"
+	},
+	{0x01bf0004,
+	 "PMU6: LRDIMM Writing final data buffer fine delay value nib %2d, trainDly %3d, fineDly code %2d, new MREP fine %2d\n"
+	},
+	{0x01c00003,
+	 "PMU6: LRDIMM Writing final data buffer fine delay value nib %2d, trainDly %3d, fineDly code %2d\n"
+	},
+	{0x01c10003,
+	 "PMU6: LRDIMM Writing data buffer fine delay type %d nib %2d, code %2d\n"
+	},
+	{0x01c20002,
+	 "PMU6: Writing final data buffer coarse delay value dbyte %2d, coarse = 0x%02x\n"
+	},
+	{0x01c30003,
+	 "PMU4: data 0x%04x at MB addr 0x%08x saved at CSR addr 0x%08x\n"
+	},
+	{0x01c40003,
+	 "PMU4: data 0x%04x at MB addr 0x%08x restored from CSR addr 0x%08x\n"
+	},
+	{0x01c50003,
+	 "PMU4: data 0x%04x at MB addr 0x%08x saved at CSR addr 0x%08x\n"
+	},
+	{0x01c60003,
+	 "PMU4: data 0x%04x at MB addr 0x%08x restored from CSR addr 0x%08x\n"
+	},
+	{0x01c70001,
+	 "PMU3: Update BC00, BC01, BC02 for rank-dimm 0x%02x\n"
+	},
+	{0x01c80000,
+	 "PMU3: Writing D4 RDIMM RCD Control words F0RC00 -> F0RC0F\n"
+	},
+	{0x01c90000,
+	 "PMU3: Disable parity in F0RC0E\n"
+	},
+	{0x01ca0000,
+	 "PMU3: Writing D4 RDIMM RCD Control words F1RC00 -> F1RC05\n"
+	},
+	{0x01cb0000,
+	 "PMU3: Writing D4 RDIMM RCD Control words F1RC1x -> F1RC9x\n"
+	},
+	{0x01cc0000,
+	 "PMU3: Writing D4 Data buffer Control words BC00 -> BC0E\n"
+	},
+	{0x01cd0002,
+	 "PMU1: setAltCL Sending MR0 0x%x cl=%d\n"
+	},
+	{0x01ce0002,
+	 "PMU1: restoreFromAltCL Sending MR0 0x%x cl=%d\n"
+	},
+	{0x01cf0002,
+	 "PMU1: restoreAcsmFromAltCL Sending MR0 0x%x cl=%d\n"
+	},
+	{0x01d00002,
+	 "PMU2: Setting D3R RC%d = 0x%01x\n"
+	},
+	{0x01d10000,
+	 "PMU3: Writing D3 RDIMM RCD Control words RC0 -> RC11\n"
+	},
+	{0x01d20002,
+	 "PMU0: VrefDAC0/1 vddqStart %d dacToVddq %d\n"
+	},
+	{0x01d30001,
+	 "PMU: Error: Messageblock phyVref=0x%x is above the limit for TSMC28's attenuated LPDDR4 receivers. Please see the pub databook\n"
+	},
+	{0x01d40001,
+	 "PMU: Error: Messageblock phyVref=0x%x is above the limit for TSMC28's attenuated DDR4 receivers. Please see the pub databook\n"
+	},
+	{0x01d50001,
+	 "PMU0: PHY VREF @ (%d/1000) VDDQ\n"
+	},
+	{0x01d60002,
+	 "PMU0: initalizing phy vrefDacs to %d ExtVrefRange %x\n"
+	},
+	{0x01d70002,
+	 "PMU0: initalizing global vref to %d range %d\n"
+	},
+	{0x01d80002,
+	 "PMU4: Setting initial device vrefDQ for CS%d to MR6 = 0x%04x\n"
+	},
+	{0x01d90003,
+	 "PMU1: In write_level_fine() csn=%d dimm=%d pstate=%d\n"
+	},
+	{0x01da0000,
+	 "PMU3: Fine write leveling hardware search increasing TxDqsDly until full bursts are seen\n"
+	},
+	{0x01db0000,
+	 "PMU4: WL normalized pos   : ........................|........................\n"
+	},
+	{0x01dc0007,
+	 "PMU4: WL margin for nib %2d: %08x%08x%08x%08x%08x%08x\n"
+	},
+	{0x01dd0000,
+	 "PMU4: WL normalized pos   : ........................|........................\n"
+	},
+	{0x01de0000,
+	 "PMU3: Exiting write leveling mode\n"
+	},
+	{0x01df0001,
+	 "PMU3: got %d for cl in load_wrlvl_acsm\n"
+	},
+	{0x01e00003,
+	 "PMU1: In write_level_coarse() csn=%d dimm=%d pstate=%d\n"
+	},
+	{0x01e10003,
+	 "PMU3: left eye edge search db:%d ln:%d dly:0x%x\n"
+	},
+	{0x01e20003,
+	 "PMU3: right eye edge search db:%d ln:%d dly:0x%x\n"
+	},
+	{0x01e30004,
+	 "PMU3: eye center db:%d ln:%d dly:0x%x (maxdq:%x)\n"
+	},
+	{0x01e40003,
+	 "PMU3: Wrote to TxDqDly db:%d ln:%d dly:0x%x\n"
+	},
+	{0x01e50003,
+	 "PMU3: Wrote to TxDqDly db:%d ln:%d dly:0x%x\n"
+	},
+	{0x01e60002,
+	 "PMU3: Coarse write leveling dbyte%2d is still failing for TxDqsDly=0x%04x\n"
+	},
+	{0x01e70002,
+	 "PMU4: Coarse write leveling iteration %d saw %d data miscompares across the entire phy\n"
+	},
+	{0x01e80000,
+	 "PMU: Error: Failed write leveling coarse\n"
+	},
+	{0x01e90001,
+	 "PMU3: got %d for cl in load_wrlvl_acsm\n"
+	},
+	{0x01ea0003,
+	 "PMU3: In write_level_coarse() csn=%d dimm=%d pstate=%d\n"
+	},
+	{0x01eb0003,
+	 "PMU3: left eye edge search db:%d ln:%d dly:0x%x\n"
+	},
+	{0x01ec0003,
+	 "PMU3: right eye edge search db: %d ln: %d dly: 0x%x\n"
+	},
+	{0x01ed0004,
+	 "PMU3: eye center db: %d ln: %d dly: 0x%x (maxdq: 0x%x)\n"
+	},
+	{0x01ee0003,
+	 "PMU3: Wrote to TxDqDly db: %d ln: %d dly: 0x%x\n"
+	},
+	{0x01ef0003,
+	 "PMU3: Wrote to TxDqDly db: %d ln: %d dly: 0x%x\n"
+	},
+	{0x01f00002,
+	 "PMU3: Coarse write leveling nibble%2d is still failing for TxDqsDly=0x%04x\n"
+	},
+	{0x01f10002,
+	 "PMU4: Coarse write leveling iteration %d saw %d data miscompares across the entire phy\n"
+	},
+	{0x01f20000,
+	 "PMU: Error: Failed write leveling coarse\n"
+	},
+	{0x01f30000,
+	 "PMU4: WL normalized pos   : ................................|................................\n"
+	},
+	{0x01f40009,
+	 "PMU4: WL margin for nib %2d: %08x%08x%08x%08x%08x%08x%08x%08x\n"
+	},
+	{0x01f50000,
+	 "PMU4: WL normalized pos   : ................................|................................\n"
+	},
+	{0x01f60001,
+	 "PMU8: Adjust margin after WL coarse to be larger than %d\n"
+	},
+	{0x01f70001,
+	 "PMU: Error: All margin after write leveling coarse are smaller than minMargin %d\n"
+	},
+	{0x01f80002,
+	 "PMU8: Decrement nib %d TxDqsDly by %d fine step\n"
+	},
+	{0x01f90003,
+	 "PMU3: In write_level_coarse() csn=%d dimm=%d pstate=%d\n"
+	},
+	{0x01fa0005,
+	 "PMU2: Write level: dbyte %d nib%d dq/dmbi %2d dqsfine 0x%04x dqDly 0x%04x\n"
+	},
+	{0x01fb0002,
+	 "PMU3: Coarse write leveling nibble%2d is still failing for TxDqsDly=0x%04x\n"
+	},
+	{0x01fc0002,
+	 "PMU4: Coarse write leveling iteration %d saw %d data miscompares across the entire phy\n"
+	},
+	{0x01fd0000,
+	 "PMU: Error: Failed write leveling coarse\n"
+	},
+	{0x01fe0001,
+	 "PMU3: DWL delay = %d\n"
+	},
+	{0x01ff0003,
+	 "PMU3: Errcnt for DWL nib %2d delay = %2d is %d\n"
+	},
+	{0x02000002,
+	 "PMU3: DWL nibble %d sampled a 1 at delay %d\n"
+	},
+	{0x02010003,
+	 "PMU3: DWL nibble %d passed at delay %d. Rising edge was at %d\n"
+	},
+	{0x02020000,
+	 "PMU2: DWL did nto find a rising edge of memclk for all nibbles. Failing nibbles assumed to have rising edge close to fine delay 63\n"
+	},
+	{0x02030002,
+	 "PMU2:  Rising edge found in alias window, setting wrlvlDly for nibble %d = %d\n"
+	},
+	{0x02040002,
+	 "PMU: Error: Failed DWL for nib %d with %d one\n"
+	},
+	{0x02050003,
+	 "PMU2:  Rising edge not found in alias window with %d one, leaving wrlvlDly for nibble %d = %d\n"
+	},
+	{0x04000000,
+	 "PMU: Error:Mailbox Buffer Overflowed.\n"
+	},
+	{0x04010000,
+	 "PMU: Error:Mailbox Buffer Overflowed.\n"
+	},
+	{0x04020000,
+	 "PMU: ***** Assertion Error - terminating *****\n"
+	},
+	{0x04030002,
+	 "PMU1: swapByte db %d by %d\n"
+	},
+	{0x04040003,
+	 "PMU3: get_cmd_dly max(%d ps, %d memclk) = %d\n"
+	},
+	{0x04050002,
+	 "PMU0: Write CSR 0x%06x 0x%04x\n"
+	},
+	{0x04060002,
+	 "PMU0: hwt_init_ppgc_prbs(): Polynomial: %x, Deg: %d\n"
+	},
+	{0x04070001,
+	 "PMU: Error: acsm_set_cmd to non existent instruction address %d\n"
+	},
+	{0x04080001,
+	 "PMU: Error: acsm_set_cmd with unknown ddr cmd 0x%x\n"
+	},
+	{0x0409000c,
+	 "PMU1: acsm_addr %02x, acsm_flgs %04x, ddr_cmd %02x, cmd_dly %02x, ddr_addr %04x, ddr_bnk %02x, ddr_cs %02x, cmd_rcnt %02x, AcsmSeq0/1/2/3 %04x %04x %04x %04x\n"
+	},
+	{0x040a0000,
+	 "PMU: Error: Polling on ACSM done failed to complete in acsm_poll_done()...\n"
+	},
+	{0x040b0000,
+	 "PMU1: acsm RUN\n"
+	},
+	{0x040c0000,
+	 "PMU1: acsm STOPPED\n"
+	},
+	{0x040d0002,
+	 "PMU1: acsm_init: acsm_mode %04x mxrdlat %04x\n"
+	},
+	{0x040e0002,
+	 "PMU: Error: setAcsmCLCWL: cl and cwl must be each >= 2 and 5, resp. CL=%d CWL=%d\n"
+	},
+	{0x040f0002,
+	 "PMU: Error: setAcsmCLCWL: cl and cwl must be each >= 5. CL=%d CWL=%d\n"
+	},
+	{0x04100002,
+	 "PMU1: setAcsmCLCWL: CASL %04d WCASL %04d\n"
+	},
+	{0x04110001,
+	 "PMU: Error: Reserved value of register F0RC0F found in message block: 0x%04x\n"
+	},
+	{0x04120001,
+	 "PMU3: Written MRS to CS=0x%02x\n"
+	},
+	{0x04130001,
+	 "PMU3: Written MRS to CS=0x%02x\n"
+	},
+	{0x04140000,
+	 "PMU3: Entering Boot Freq Mode.\n"
+	},
+	{0x04150001,
+	 "PMU: Error: Boot clock divider setting of %d is too small\n"
+	},
+	{0x04160000,
+	 "PMU3: Exiting Boot Freq Mode.\n"
+	},
+	{0x04170002,
+	 "PMU3: Writing MR%d OP=%x\n"
+	},
+	{0x04180000,
+	 "PMU: Error: Delay too large in slomo\n"
+	},
+	{0x04190001,
+	 "PMU3: Written MRS to CS=0x%02x\n"
+	},
+	{0x041a0000,
+	 "PMU3: Enable Channel A\n"
+	},
+	{0x041b0000,
+	 "PMU3: Enable Channel B\n"
+	},
+	{0x041c0000,
+	 "PMU3: Enable All Channels\n"
+	},
+	{0x041d0002,
+	 "PMU2: Use PDA mode to set MR%d with value 0x%02x\n"
+	},
+	{0x041e0001,
+	 "PMU3: Written Vref with PDA to CS=0x%02x\n"
+	},
+	{0x041f0000,
+	 "PMU1: start_cal: DEBUG: setting CalRun to 1\n"
+	},
+	{0x04200000,
+	 "PMU1: start_cal: DEBUG: setting CalRun to 0\n"
+	},
+	{0x04210001,
+	 "PMU1: lock_pll_dll: DEBUG: pstate = %d\n"
+	},
+	{0x04220001,
+	 "PMU1: lock_pll_dll: DEBUG: dfifreqxlat_pstate = %d\n"
+	},
+	{0x04230001,
+	 "PMU1: lock_pll_dll: DEBUG: pllbypass = %d\n"
+	},
+	{0x04240001,
+	 "PMU3: SaveLcdlSeed: Saving seed %d\n"
+	},
+	{0x04250000,
+	 "PMU1: in phy_defaults()\n"
+	},
+	{0x04260003,
+	 "PMU3: ACXConf:%d MaxNumDbytes:%d NumDfi:%d\n"
+	},
+	{0x04270005,
+	 "PMU1: setAltAcsmCLCWL setting cl=%d cwl=%d\n"
+	},
+};
+#endif /* DEBUG */
+#endif
diff --git a/drivers/nxp/ddr/phy-gen2/phy.c b/drivers/nxp/ddr/phy-gen2/phy.c
new file mode 100644
index 0000000..9e52145
--- /dev/null
+++ b/drivers/nxp/ddr/phy-gen2/phy.c
@@ -0,0 +1,2678 @@
+/*
+ * Copyright 2021-2022 NXP
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include "csr.h"
+#include <ddr.h>
+#include "ddr4fw.h"
+#include <drivers/delay_timer.h>
+#ifdef NXP_WARM_BOOT
+#include <fspi_api.h>
+#endif
+#include "input.h"
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#ifdef DDR_PHY_DEBUG
+#include "messages.h"
+#endif
+#ifdef NXP_WARM_BOOT
+#include "phy.h"
+#endif
+#include "pie.h"
+
+#define TIMEOUTDEFAULT 500
+#define MAP_PHY_ADDR(pstate, n, instance, offset, c) \
+		((((pstate * n) + instance + c) << 12) + offset)
+
+static uint32_t map_phy_addr_space(uint32_t addr)
+{
+	/* 23 bit addressing */
+	uint32_t pstate =     (addr & U(0x700000)) >> 20U; /* bit 22:20 */
+	uint32_t block_type = (addr & U(0x0f0000)) >> 16U; /* bit 19:16 */
+	uint32_t instance =   (addr & U(0x00f000)) >> 12U; /* bit 15:12 */
+	uint32_t offset =     (addr & U(0x000fff));        /* bit 11:0 */
+
+	switch (block_type) {
+	case 0x0: /* 0x0 : ANIB */
+		return MAP_PHY_ADDR(pstate, 12, instance, offset, 0);
+	case 0x1: /* 0x1 : DBYTE */
+		return MAP_PHY_ADDR(pstate, 10, instance, offset, 0x30);
+	case 0x2: /* 0x2 : MASTER */
+		return MAP_PHY_ADDR(pstate, 1, 0, offset, 0x58);
+	case 0x4: /* 0x4 : ACSM */
+		return MAP_PHY_ADDR(pstate, 1, 0, offset, 0x5c);
+	case 0x5: /* 0x5 : μCTL Memory */
+		return MAP_PHY_ADDR(pstate, 0, instance, offset, 0x60);
+	case 0x7: /* 0x7 : PPGC */
+		return MAP_PHY_ADDR(pstate, 0, 0, offset, 0x68);
+	case 0x9: /* 0x9 : INITENG */
+		return MAP_PHY_ADDR(pstate, 1, 0, offset, 0x69);
+	case 0xc: /* 0xC : DRTUB */
+		return MAP_PHY_ADDR(pstate, 0, 0, offset, 0x6d);
+	case 0xd: /* 0xD : APB Only */
+		return MAP_PHY_ADDR(pstate, 0, 0, offset, 0x6e);
+	default:
+		printf("ERR: Invalid block_type = 0x%x\n", block_type);
+		return 0;
+	}
+}
+
+static inline uint16_t *phy_io_addr(void *phy, uint32_t addr)
+{
+	return phy + (map_phy_addr_space(addr) << 2);
+}
+
+static inline void phy_io_write16(uint16_t *phy, uint32_t addr, uint16_t data)
+{
+	mmio_write_16((uintptr_t)phy_io_addr(phy, addr), data);
+#ifdef DEBUG_PHY_IO
+	printf("0x%06x,0x%x\n", addr, data);
+#endif
+}
+
+static inline uint16_t phy_io_read16(uint16_t *phy, uint32_t addr)
+{
+	uint16_t reg = mmio_read_16((uintptr_t) phy_io_addr(phy, addr));
+
+#ifdef DEBUG_PHY_IO
+	printf("R: 0x%06x,0x%x\n", addr, reg);
+#endif
+
+	return reg;
+}
+
+#ifdef NXP_APPLY_MAX_CDD
+
+#define CDD_VAL_READ_ADDR (0x054012)
+#define CDD_DATA_LEN    (60)
+
+static void read_phy_reg(uint16_t *phy, uint32_t addr,
+		uint16_t *buf, uint32_t len)
+{
+	uint32_t i = 0U;
+
+	for (i = 0U; i < len/2; i++) {
+		buf[i] = phy_io_read16(phy, (addr + i));
+	}
+}
+
+static uint32_t findrank(uint32_t cs_in_use)
+{
+	uint32_t val = 0U;
+
+	switch (cs_in_use) {
+	case U(0xf):
+		val = 4U;
+		break;
+	case U(0x3):
+		val = 2U;
+		break;
+	case U(0x1):
+		val = 1U;
+		break;
+	default:
+		printf("Error - Invalid cs_in_use value\n");
+	}
+	return val;
+}
+
+static uint8_t findmax(uint8_t *buf, uint32_t len)
+{
+	uint8_t max = 0U;
+	uint32_t i = 0U;
+
+	for (i = 0U; i < len; i++) {
+		if (buf[i] > max) {
+			max = buf[i];
+		}
+	}
+
+	return max;
+}
+
+static void get_cdd_val(uint16_t **phy_ptr, uint32_t rank, uint32_t freq,
+		uint32_t *tcfg0, uint32_t *tcfg4)
+{
+	uint8_t cdd[CDD_DATA_LEN+4] = {0U};
+	uint32_t i, val = 0U;
+	uint16_t *phy;
+	uint8_t buf[16] = {U(0x0)};
+	uint8_t trr = 0U, tww = 0U, trw = 0U, twr = 0U;
+	uint8_t rrmax = 0U, wwmax = 0U, rwmax = 0U, wrmax = 0U;
+	uint8_t tmp = U(0x0);
+	uint8_t *c =  NULL;
+
+	for (i = 0U; i < NUM_OF_DDRC; i++) {
+
+		phy = phy_ptr[i];
+		if (phy == NULL) {
+			continue;
+		}
+
+		phy_io_write16(phy, t_apbonly |
+				csr_micro_cont_mux_sel_addr, U(0x0));
+
+		read_phy_reg(phy, CDD_VAL_READ_ADDR,
+				(uint16_t *)&cdd, CDD_DATA_LEN);
+
+		phy_io_write16(phy, t_apbonly |
+				csr_micro_cont_mux_sel_addr, U(0x1));
+
+	/* CDD values and address
+	 *
+	 *   0x054012    0x24    cdd[0]  CDD[X][X]
+	 *   0x054012    0x25    cdd[1]  RR[3][2]
+	 *   0x054013    0x26    cdd[2]  RR[3][1]
+	 *   0x054013    0x27    cdd[3]  RR[3][0]
+	 *   0x054014    0x28    cdd[4]  RR[2][3]
+	 *   0x054014    0x29    cdd[5]  RR[2][1]
+	 *   0x054015    0x2a    cdd[6]  RR[2][0]
+	 *   0x054015    0x2b    cdd[7]  RR[1][3]
+	 *   0x054016    0x2c    cdd[8]  RR[1][2]
+	 *   0x054016    0x2d    cdd[9]  RR[1][0]
+	 *   0x054017    0x2e    cdd[10] RR[0][3]
+	 *   0x054017    0x2f    cdd[11] RR[0][2]
+	 *   0x054018    0x30    cdd[12] RR[0][1]
+
+	 *   0x054018    0x31    cdd[13] WW[3][2]
+	 *   0x054019    0x32    cdd[14] WW[3][1]
+	 *   0x054019    0x33    cdd[15] WW[3][0]
+	 *   0x05401a    0x34    cdd[16] WW[2][3]
+	 *   0x05401a    0x35    cdd[17] WW[2][1]
+	 *   0x05401b    0x36    cdd[18] WW[2][0]
+	 *   0x05401b    0x37    cdd[19] WW[1][3]
+	 *   0x05401c    0x38    cdd[20] WW[1][2]
+	 *   0x05401c    0x39    cdd[21] WW[1][0]
+	 *   0x05401d    0x3a    cdd[22] WW[0][3]
+	 *   0x05401d    0x3b    cdd[23] WW[0][2]
+	 *   0x05401e    0x3c    cdd[24] WW[0][1]
+
+	 *   0x05401e    0x3d    cdd[25] RW[3][3]
+	 *   0x05401f    0x3e    cdd[26] RW[3][2]
+	 *   0x05401f    0x3f    cdd[27] RW[3][1]
+	 *   0x054020    0x40    cdd[28] RW[3][0]
+	 *   0x054020    0x41    cdd[29] RW[2][3]
+	 *   0x054021    0x42    cdd[30] RW[2][2]
+	 *   0x054021    0x43    cdd[31] RW[2][1]
+	 *   0x054022    0x44    cdd[32] RW[2][0]
+	 *   0x054022    0x45    cdd[33] RW[1][3]
+	 *   0x054023    0x46    cdd[34] RW[1][2]
+	 *   0x054023    0x47    cdd[35] RW[1][1]
+	 *   0x054024    0x48    cdd[36] RW[1][0]
+	 *   0x054024    0x49    cdd[37] RW[0][3]
+	 *   0x054025    0x4a    cdd[38] RW[0][2]
+	 *   0x054025    0x4b    cdd[39] RW[0][1]
+	 *   0x054026    0x4c    cdd[40] RW[0][0]
+
+	 *   0x054026    0x4d    cdd[41] WR[3][3]
+	 *   0x054027    0x4e    cdd[42] WR[3][2]
+	 *   0x054027    0x4f    cdd[43] WR[3][1]
+	 *   0x054028    0x50    cdd[44] WR[3][0]
+	 *   0x054028    0x51    cdd[45] WR[2][3]
+	 *   0x054029    0x52    cdd[46] WR[2][2]
+	 *   0x054029    0x53    cdd[47] WR[2][1]
+	 *   0x05402a    0x54    cdd[48] WR[2][0]
+	 *   0x05402a    0x55    cdd[49] WR[1][3]
+	 *   0x05402b    0x56    cdd[50] WR[1][2]
+	 *   0x05402b    0x57    cdd[51] WR[1][1]
+	 *   0x05402c    0x58    cdd[52] WR[1][0]
+	 *   0x05402c    0x59    cdd[53] WR[0][3]
+	 *   0x05402d    0x5a    cdd[54] WR[0][2]
+	 *   0x05402d    0x5b    cdd[55] WR[0][1]
+	 *   0x05402e    0x5c    cdd[56] WR[0][0]
+	 *   0x05402e    0x5d    cdd[57] CDD[Y][Y]
+	 */
+
+		switch (rank) {
+		case 1U:
+			tmp = rwmax;
+			rwmax = cdd[40];
+			if (tmp > rwmax) {
+				rwmax = tmp;
+			}
+
+			tmp = wrmax;
+			wrmax = cdd[56];
+			if (tmp > wrmax) {
+				wrmax = tmp;
+			}
+
+			break;
+
+		case 2U:
+			buf[0] = cdd[12];
+			buf[1] = cdd[9];
+			tmp = rrmax;
+			rrmax = findmax(buf, 2U);
+			if (tmp > rrmax) {
+				rrmax = tmp;
+			}
+
+			buf[0] = cdd[24];
+			buf[1] = cdd[21];
+			tmp = wwmax;
+			wwmax = findmax(buf, 2U);
+			if (tmp > wwmax) {
+				wwmax = tmp;
+			}
+
+			buf[0] = cdd[40];
+			buf[1] = cdd[39];
+			buf[2] = cdd[36];
+			buf[3] = cdd[35];
+			tmp = rwmax;
+			rwmax = findmax(buf, 4U);
+			if (tmp > rwmax) {
+				rwmax = tmp;
+			}
+
+			buf[0] = cdd[56];
+			buf[1] = cdd[55];
+			buf[2] = cdd[52];
+			buf[3] = cdd[51];
+			tmp = wrmax;
+			wrmax = findmax(buf, 4U);
+			if (tmp > wrmax) {
+				wrmax = tmp;
+			}
+
+			break;
+
+		case 4U:
+			tmp = rrmax;
+			c = &cdd[1];
+			rrmax = findmax(c, 12U);
+			if (tmp > rrmax) {
+				rrmax = tmp;
+			}
+
+			tmp = wwmax;
+			c = &cdd[13];
+			wwmax = findmax(c, 12U);
+			if (tmp > wwmax) {
+				wwmax = tmp;
+			}
+
+			tmp = rwmax;
+			c = &cdd[25];
+			rwmax = findmax(c, 16U);
+			if (tmp > rwmax) {
+				rwmax = tmp;
+			}
+
+			tmp = wrmax;
+			c = &cdd[41];
+			wrmax = findmax(c, 16U);
+			if (tmp > wrmax) {
+				wrmax = tmp;
+			}
+
+			break;
+
+		}
+	}
+
+	rrmax += 3U;
+	wwmax += 4U;
+
+	if (wwmax > 7U) {
+		wwmax = 7U;
+	}
+
+	if (rrmax > 7U) {
+		rrmax = 7U;
+	}
+
+	if (wrmax > U(0xf)) {
+		wrmax = 0U;
+	}
+
+	if (rwmax > U(0x7)) {
+		rwmax = U(0x7);
+	}
+
+	val = *tcfg0;
+	tww = (val >> 24U) & U(0x3);
+	trr = (val >> 26U) & U(0x3);
+	twr = (val >> 28U) & U(0x3);
+	trw = (val >> 30U) & U(0x3);
+
+	val = *tcfg4;
+	tww = tww | (((val >> 8U) & U(0x1)) << 2U);
+	trr = trr | (((val >> 10U) & U(0x1)) << 2U);
+	twr = twr | (((val >> 12U) & U(0x1)) << 2U);
+	trw = trw | (((val >> 14U) & U(0x3)) << 2U);
+
+	if (trr > rrmax) {
+		rrmax = trr;
+	}
+
+	if (tww > wwmax) {
+		wwmax = tww;
+	}
+
+	if (trw > rwmax) {
+		rwmax = trw;
+	}
+
+	if (twr > wrmax) {
+		wrmax = twr;
+	}
+
+	debug("CDD rrmax %x wwmax %x rwmax %x wrmax %x\n",
+			rrmax, wwmax, rwmax, wrmax);
+
+	val = ((wwmax & U(0x3)) << 24U)
+		| ((rrmax & U(0x3)) << 26U)
+		| ((wrmax & U(0x3)) << 28U)
+		| ((rwmax & U(0x3)) << 30U);
+
+	*tcfg0 = (*tcfg0 & U(0x00FFFFFF)) | (val);
+
+	val = (((wwmax >> 2U) & U(0x1)) << 8U)
+		| (((rrmax >> 2U) & U(0x1)) << 10U)
+		| (((wrmax >> 2U) & U(0x1)) << 12U)
+		| (((rwmax >> 2U) & U(0x3)) << 14U);
+
+	*tcfg4 = (*tcfg4 & U(0xffff00ff)) | val;
+}
+#endif
+
+#ifdef NXP_WARM_BOOT
+int save_phy_training_values(uint16_t **phy_ptr, uint32_t address_to_store,
+		uint32_t num_of_phy, int train2d)
+{
+	uint16_t *phy = NULL, value = 0x0;
+	uint32_t size = 1U, num_of_regs = 1U, phy_store = 0U;
+	int i = 0, j = 0, ret = -EINVAL;
+
+	ret = xspi_sector_erase(address_to_store, PHY_ERASE_SIZE);
+	if (ret != 0) {
+		return -EINVAL;
+	}
+
+	for (j = 0; j < num_of_phy; j++) {
+		/* Save training values of all PHYs */
+		phy = phy_ptr[j];
+		size = sizeof(training_1D_values);
+		num_of_regs = ARRAY_SIZE(training_1D_values);
+
+		/* Enable access to the internal CSRs */
+		phy_io_write16(phy, t_apbonly |
+				csr_micro_cont_mux_sel_addr, 0x0);
+		/* Enable clocks in case they were disabled. */
+		phy_io_write16(phy, t_drtub |
+				csr_ucclk_hclk_enables_addr, 0x3);
+		if (train2d != 0) {
+		/* Address to store training values is
+		 * to be appended for next PHY
+		 */
+			phy_store = address_to_store + (j *
+					(sizeof(training_1D_values) +
+					 sizeof(training_2D_values)));
+		} else {
+			phy_store = address_to_store + (j *
+					(sizeof(training_1D_values)));
+		}
+		debug("Saving 1D Training reg val at: %d\n", phy_store);
+		for (i = 0; i < num_of_regs; i++) {
+			value = phy_io_read16(phy, training_1D_values[i].addr);
+#ifdef DEBUG_WARM_RESET
+			debug("%d. Reg: %x, value: %x PHY: %p\n", i,
+					training_1D_values[i].addr, value,
+					phy_io_addr(phy,
+						training_1D_values[i].addr));
+#endif
+			training_1D_values[i].data = value;
+		}
+		/* Storing 1D training values on flash */
+		ret = xspi_write(phy_store, (void *)training_1D_values, size);
+		if (train2d != 0) {
+			phy_store = phy_store+size;
+			size = sizeof(training_2D_values);
+			num_of_regs = ARRAY_SIZE(training_2D_values);
+			debug("Saving 2D Training reg val at:%d\n", phy_store);
+			for (i = 0; i < num_of_regs; i++) {
+				value = phy_io_read16(phy,
+						training_2D_values[i].addr);
+				training_2D_values[i].data = value;
+#ifdef DEBUG_WARM_RESET
+				debug("%d.2D addr:0x%x,val:0x%x,PHY:0x%p\n",
+						i, training_2D_values[i].addr,
+						value, phy_io_addr(phy,
+						training_2D_values[i].addr));
+#endif
+			}
+			/* Storing 2D training values on flash */
+			ret = xspi_write(phy_store, training_2D_values,
+					size);
+		}
+		/* Disable clocks in case they were disabled. */
+		phy_io_write16(phy, t_drtub |
+				csr_ucclk_hclk_enables_addr, 0x0);
+		/* Disable access to the internal CSRs */
+		phy_io_write16(phy, t_apbonly |
+				csr_micro_cont_mux_sel_addr, 0x1);
+	}
+	if (ret != 0) {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int restore_phy_training_values(uint16_t **phy_ptr, uint32_t address_to_restore,
+		uint32_t num_of_phy, int train2d)
+{
+	uint16_t *phy = NULL;
+	uint32_t size = 1U, num_of_regs = 1U, phy_store = 0U;
+	int i = 0, j = 0, ret = -EINVAL;
+
+	debug("Restoring Training register values\n");
+	for (j = 0; j < num_of_phy; j++) {
+		phy = phy_ptr[j];
+		size = sizeof(training_1D_values);
+		num_of_regs = ARRAY_SIZE(training_1D_values);
+		if (train2d != 0) {
+		/* The address to restore training values is
+		 * to be appended for next PHY
+		 */
+			phy_store = address_to_restore + (j *
+					(sizeof(training_1D_values) +
+					 sizeof(training_2D_values)));
+		} else {
+			phy_store = address_to_restore + (j *
+					(sizeof(training_1D_values)));
+		}
+		/* Enable access to the internal CSRs */
+		phy_io_write16(phy, t_apbonly |
+				csr_micro_cont_mux_sel_addr, 0x0);
+		/* Enable clocks in case they were disabled. */
+		phy_io_write16(phy, t_drtub |
+				csr_ucclk_hclk_enables_addr, 0x3);
+
+		/* Reading 1D training values from flash*/
+		ret = xspi_read(phy_store, (uint32_t *)training_1D_values,
+				size);
+		debug("Restoring 1D Training reg val at:%08x\n", phy_store);
+		for (i = 0; i < num_of_regs; i++) {
+			phy_io_write16(phy, training_1D_values[i].addr,
+					training_1D_values[i].data);
+#ifdef DEBUG_WARM_RESET
+			debug("%d. Reg: %x, value: %x PHY: %p\n", i,
+					training_1D_values[i].addr,
+					training_1D_values[i].data,
+					phy_io_addr(phy,
+						training_1D_values[i].addr));
+#endif
+		}
+		if (train2d != 0) {
+			phy_store = phy_store + size;
+			size = sizeof(training_2D_values);
+			num_of_regs = ARRAY_SIZE(training_2D_values);
+			/* Reading 2D training values from flash */
+			ret = xspi_read(phy_store,
+					(uint32_t *)training_2D_values,	size);
+			debug("Restoring 2D Training reg val at:%08x\n",
+					phy_store);
+			for (i = 0; i < num_of_regs; i++) {
+				phy_io_write16(phy, training_2D_values[i].addr,
+						training_2D_values[i].data);
+#ifdef DEBUG_WARM_RESET
+				debug("%d. Reg: %x, value: %x PHY: %p\n", i,
+						training_2D_values[i].addr,
+						training_2D_values[i].data,
+						phy_io_addr(phy,
+						training_1D_values[i].addr));
+#endif
+			}
+		}
+		/* Disable clocks in case they were disabled. */
+		phy_io_write16(phy, t_drtub |
+				csr_ucclk_hclk_enables_addr, 0x0);
+		/* Disable access to the internal CSRs */
+		phy_io_write16(phy, t_apbonly |
+				csr_micro_cont_mux_sel_addr, 0x1);
+	}
+	if (ret != 0) {
+		return -EINVAL;
+	}
+	return 0;
+}
+#endif
+
+static void load_pieimage(uint16_t *phy,
+			  enum dimm_types dimm_type)
+{
+	int i;
+	int size;
+	const struct pie *image = NULL;
+
+	switch (dimm_type) {
+	case UDIMM:
+	case SODIMM:
+	case NODIMM:
+		image = pie_udimm;
+		size = ARRAY_SIZE(pie_udimm);
+		break;
+	case RDIMM:
+		image = pie_rdimm;
+		size = ARRAY_SIZE(pie_rdimm);
+		break;
+	case LRDIMM:
+		image = pie_lrdimm;
+		size = ARRAY_SIZE(pie_lrdimm);
+		break;
+	default:
+		printf("Unsupported DIMM type\n");
+		break;
+	}
+
+	if (image != NULL) {
+		for (i = 0; i < size; i++)
+			phy_io_write16(phy, image[i].addr, image[i].data);
+	}
+}
+
+static void prog_acsm_playback(uint16_t *phy,
+			       const struct input *input, const void *msg)
+{
+	int vec;
+	const struct ddr4r1d *msg_blk;
+	uint16_t acsmplayback[2][3];
+	uint32_t f0rc0a;
+	uint32_t f0rc3x;
+	uint32_t f0rc5x;
+
+	if (input->basic.dimm_type != RDIMM) {
+		return;
+	}
+
+	msg_blk = msg;
+	f0rc0a = (msg_blk->f0rc0a_d0 & U(0xf)) | U(0xa0);
+	f0rc3x = (msg_blk->f0rc3x_d0 & U(0xff)) | U(0x300);
+	f0rc5x = (input->adv.phy_gen2_umctl_f0rc5x & U(0xff)) | U(0x500);
+
+	acsmplayback[0][0] = U(0x3ff) & f0rc0a;
+	acsmplayback[1][0] = (U(0x1c00) & f0rc0a) >> 10U;
+	acsmplayback[0][1] = U(0x3ff) & f0rc3x;
+	acsmplayback[1][1] = (U(0x1c00) & f0rc3x) >> 10U;
+	acsmplayback[0][2] = U(0x3ff) & f0rc5x;
+	acsmplayback[1][2] = (U(0x1c00) & f0rc5x) >> 10U;
+	for (vec = 0; vec < 3; vec++) {
+		phy_io_write16(phy, t_acsm | (csr_acsm_playback0x0_addr +
+			       (vec << 1)), acsmplayback[0][vec]);
+		phy_io_write16(phy, t_acsm | (csr_acsm_playback1x0_addr +
+			       (vec << 1)), acsmplayback[1][vec]);
+	}
+}
+
+static void prog_acsm_ctr(uint16_t *phy,
+			  const struct input *input)
+{
+	if (input->basic.dimm_type != RDIMM) {
+		return;
+	}
+
+	phy_io_write16(phy, t_acsm | csr_acsm_ctrl13_addr,
+		       0xf << csr_acsm_cke_enb_lsb);
+
+	phy_io_write16(phy, t_acsm | csr_acsm_ctrl0_addr,
+		       csr_acsm_par_mode_mask | csr_acsm_2t_mode_mask);
+}
+
+static void prog_cal_rate_run(uint16_t *phy,
+			  const struct input *input)
+{
+	int cal_rate;
+	int cal_interval;
+	int cal_once;
+	uint32_t addr;
+
+	cal_interval = input->adv.cal_interval;
+	cal_once = input->adv.cal_once;
+	cal_rate = 0x1 << csr_cal_run_lsb		|
+			cal_once << csr_cal_once_lsb	|
+			cal_interval << csr_cal_interval_lsb;
+	addr = t_master | csr_cal_rate_addr;
+	phy_io_write16(phy, addr, cal_rate);
+}
+
+static void prog_seq0bdly0(uint16_t *phy,
+		    const struct input *input)
+{
+	int ps_count[4];
+	int frq;
+	uint32_t addr;
+	int lower_freq_opt = 0;
+
+	__unused const soc_info_t *soc_info;
+
+	frq = input->basic.frequency >> 1;
+	ps_count[0] = frq >> 3; /* 0.5 * frq / 4*/
+	if (input->basic.frequency < 400) {
+		lower_freq_opt = (input->basic.dimm_type == RDIMM) ? 7 : 3;
+	} else if (input->basic.frequency < 533) {
+		lower_freq_opt = (input->basic.dimm_type == RDIMM) ? 14 : 11;
+	}
+
+	/* 1.0 * frq / 4 - lower_freq */
+	ps_count[1] = (frq >> 2) - lower_freq_opt;
+	ps_count[2] = (frq << 1) +  (frq >> 1); /* 10.0 * frq / 4 */
+
+#ifdef DDR_PLL_FIX
+	soc_info = get_soc_info();
+	if (soc_info->svr_reg.bf.maj_ver == 1) {
+		ps_count[0] = 0x520; /* seq0bdly0 */
+		ps_count[1] = 0xa41; /* seq0bdly1 */
+		ps_count[2] = 0x668a; /* seq0bdly2 */
+	}
+#endif
+	if (frq > 266) {
+		ps_count[3] = 44;
+	} else if (frq > 200) {
+		ps_count[3] = 33;
+	} else {
+		ps_count[3] = 16;
+	}
+
+	addr = t_master | csr_seq0bdly0_addr;
+	phy_io_write16(phy, addr, ps_count[0]);
+
+	debug("seq0bdly0 = 0x%x\n", phy_io_read16(phy, addr));
+
+	addr = t_master | csr_seq0bdly1_addr;
+	phy_io_write16(phy, addr, ps_count[1]);
+
+	debug("seq0bdly1 = 0x%x\n", phy_io_read16(phy, addr));
+
+	addr = t_master | csr_seq0bdly2_addr;
+	phy_io_write16(phy, addr, ps_count[2]);
+
+	debug("seq0bdly2 = 0x%x\n", phy_io_read16(phy, addr));
+
+	addr = t_master | csr_seq0bdly3_addr;
+	phy_io_write16(phy, addr, ps_count[3]);
+
+	debug("seq0bdly3 = 0x%x\n", phy_io_read16(phy, addr));
+}
+
+/* Only RDIMM requires msg_blk */
+static void i_load_pie(uint16_t **phy_ptr,
+		       const struct input *input,
+		       const void *msg)
+{
+	int i;
+	uint16_t *phy;
+
+	for (i = 0; i < NUM_OF_DDRC; i++) {
+		phy = phy_ptr[i];
+		if (phy == NULL) {
+			continue;
+		}
+
+		phy_io_write16(phy,
+			       t_apbonly | csr_micro_cont_mux_sel_addr,
+			       0U);
+
+		load_pieimage(phy, input->basic.dimm_type);
+
+		prog_seq0bdly0(phy, input);
+		phy_io_write16(phy, t_initeng | csr_seq0bdisable_flag0_addr,
+			       U(0x0000));
+		phy_io_write16(phy, t_initeng | csr_seq0bdisable_flag1_addr,
+			       U(0x0173));
+		phy_io_write16(phy, t_initeng | csr_seq0bdisable_flag2_addr,
+			       U(0x0060));
+		phy_io_write16(phy, t_initeng | csr_seq0bdisable_flag3_addr,
+			       U(0x6110));
+		phy_io_write16(phy, t_initeng | csr_seq0bdisable_flag4_addr,
+			       U(0x2152));
+		phy_io_write16(phy, t_initeng | csr_seq0bdisable_flag5_addr,
+			       U(0xdfbd));
+		phy_io_write16(phy, t_initeng | csr_seq0bdisable_flag6_addr,
+			       input->basic.dimm_type == RDIMM &&
+			       input->adv.phy_gen2_umctl_opt == 1U ?
+			       U(0x6000) : U(0xffff));
+		phy_io_write16(phy, t_initeng | csr_seq0bdisable_flag7_addr,
+			       U(0x6152));
+		prog_acsm_playback(phy, input, msg);		/* rdimm */
+		prog_acsm_ctr(phy, input);			/* rdimm */
+
+		phy_io_write16(phy, t_master | csr_cal_zap_addr, U(0x1));
+		prog_cal_rate_run(phy, input);
+
+		phy_io_write16(phy, t_drtub | csr_ucclk_hclk_enables_addr,
+			       input->basic.dimm_type == RDIMM ? U(0x2) : 0U);
+
+		phy_io_write16(phy, t_apbonly | csr_micro_cont_mux_sel_addr, 1U);
+	}
+}
+
+static void phy_gen2_init_input(struct input *input)
+{
+	int i;
+
+	input->adv.dram_byte_swap		= 0;
+	input->adv.ext_cal_res_val		= 0;
+	input->adv.tx_slew_rise_dq		= 0xf;
+	input->adv.tx_slew_fall_dq		= 0xf;
+	input->adv.tx_slew_rise_ac		= 0xf;
+	input->adv.tx_slew_fall_ac		= 0xf;
+	input->adv.mem_alert_en			= 0;
+	input->adv.mem_alert_puimp		= 5;
+	input->adv.mem_alert_vref_level		= 0x29;
+	input->adv.mem_alert_sync_bypass	= 0;
+	input->adv.cal_interval			= 0x9;
+	input->adv.cal_once			= 0;
+	input->adv.dis_dyn_adr_tri		= 0;
+	input->adv.is2ttiming			= 0;
+	input->adv.d4rx_preamble_length		= 0;
+	input->adv.d4tx_preamble_length		= 0;
+
+	for (i = 0; i < 7; i++) {
+		debug("mr[%d] = 0x%x\n", i, input->mr[i]);
+	}
+
+	debug("input->cs_d0 = 0x%x\n", input->cs_d0);
+	debug("input->cs_d1 = 0x%x\n", input->cs_d1);
+	debug("input->mirror = 0x%x\n", input->mirror);
+	debug("PHY ODT impedance = %d ohm\n", input->adv.odtimpedance);
+	debug("PHY DQ driver impedance = %d ohm\n", input->adv.tx_impedance);
+	debug("PHY Addr driver impedance = %d ohm\n", input->adv.atx_impedance);
+
+	for (i = 0; i < 4; i++) {
+		debug("odt[%d] = 0x%x\n", i, input->odt[i]);
+	}
+
+	if (input->basic.dimm_type == RDIMM) {
+		for (i = 0; i < 16; i++) {
+			debug("input->rcw[%d] = 0x%x\n", i, input->rcw[i]);
+		}
+		debug("input->rcw3x = 0x%x\n", input->rcw3x);
+	}
+}
+
+/*
+ * All protocols share the same base structure of message block.
+ * RDIMM and LRDIMM have more entries defined than UDIMM.
+ * Create message blocks for 1D and 2D training.
+ * Update len with message block size.
+ */
+static int phy_gen2_msg_init(void *msg_1d,
+			     void *msg_2d,
+			     const struct input *input)
+{
+	struct ddr4u1d *msg_blk = msg_1d;
+	struct ddr4u2d *msg_blk_2d = msg_2d;
+	struct ddr4r1d *msg_blk_r;
+	struct ddr4lr1d *msg_blk_lr;
+
+	switch (input->basic.dimm_type) {
+	case UDIMM:
+	case SODIMM:
+	case NODIMM:
+		msg_blk->dram_type	= U(0x2);
+		break;
+	case RDIMM:
+		msg_blk->dram_type	= U(0x4);
+		break;
+	case LRDIMM:
+		msg_blk->dram_type	= U(0x5);
+		break;
+	default:
+		ERROR("Unsupported DIMM type\n");
+		return -EINVAL;
+	}
+	msg_blk->pstate			= 0U;
+
+	/*Enable quickRd2D, a substage of read deskew, to 1D training.*/
+	msg_blk->reserved00             = U(0x20);
+
+	/*Enable High-Effort WrDQ1D.*/
+	msg_blk->reserved00             |= U(0x40);
+
+	/* Enable 1D extra effort training.*/
+	msg_blk->reserved1c[3]		= U(0x3);
+
+	if (input->basic.dimm_type == LRDIMM) {
+		msg_blk->sequence_ctrl	= U(0x3f1f);
+	} else {
+		msg_blk->sequence_ctrl	= U(0x031f);
+	}
+	msg_blk->phy_config_override	= 0U;
+#ifdef DDR_PHY_DEBUG
+	msg_blk->hdt_ctrl		= U(0x5);
+#else
+	msg_blk->hdt_ctrl		= U(0xc9);
+#endif
+	msg_blk->msg_misc		= U(0x0);
+	msg_blk->dfimrlmargin		= U(0x1);
+	msg_blk->phy_vref		= input->vref ? input->vref : U(0x61);
+	msg_blk->cs_present		= input->cs_d0 | input->cs_d1;
+	msg_blk->cs_present_d0		= input->cs_d0;
+	msg_blk->cs_present_d1		= input->cs_d1;
+	if (input->mirror != 0) {
+		msg_blk->addr_mirror	= U(0x0a);	/* odd CS are mirrored */
+	}
+	msg_blk->share2dvref_result	= 1U;
+
+	msg_blk->acsm_odt_ctrl0		= input->odt[0];
+	msg_blk->acsm_odt_ctrl1		= input->odt[1];
+	msg_blk->acsm_odt_ctrl2		= input->odt[2];
+	msg_blk->acsm_odt_ctrl3		= input->odt[3];
+	msg_blk->enabled_dqs = (input->basic.num_active_dbyte_dfi0 +
+				input->basic.num_active_dbyte_dfi1) * 8;
+	msg_blk->x16present		= input->basic.dram_data_width == 0x10 ?
+					  msg_blk->cs_present : 0;
+	msg_blk->d4misc			= U(0x1);
+	msg_blk->cs_setup_gddec		= U(0x1);
+	msg_blk->rtt_nom_wr_park0	= 0U;
+	msg_blk->rtt_nom_wr_park1	= 0U;
+	msg_blk->rtt_nom_wr_park2	= 0U;
+	msg_blk->rtt_nom_wr_park3	= 0U;
+	msg_blk->rtt_nom_wr_park4	= 0U;
+	msg_blk->rtt_nom_wr_park5	= 0U;
+	msg_blk->rtt_nom_wr_park6	= 0U;
+	msg_blk->rtt_nom_wr_park7	= 0U;
+	msg_blk->mr0			= input->mr[0];
+	msg_blk->mr1			= input->mr[1];
+	msg_blk->mr2			= input->mr[2];
+	msg_blk->mr3			= input->mr[3];
+	msg_blk->mr4			= input->mr[4];
+	msg_blk->mr5			= input->mr[5];
+	msg_blk->mr6			= input->mr[6];
+	if ((msg_blk->mr4 & U(0x1c0)) != 0U) {
+		ERROR("Setting DRAM CAL mode is not supported\n");
+	}
+
+	msg_blk->alt_cas_l		= 0U;
+	msg_blk->alt_wcas_l		= 0U;
+
+	msg_blk->dramfreq		= input->basic.frequency * 2U;
+	msg_blk->pll_bypass_en		= input->basic.pll_bypass;
+	msg_blk->dfi_freq_ratio		= input->basic.dfi_freq_ratio == 0U ? 1U :
+					  input->basic.dfi_freq_ratio == 1U ? 2U :
+					  4U;
+	msg_blk->bpznres_val		= input->adv.ext_cal_res_val;
+	msg_blk->disabled_dbyte		= 0U;
+
+	debug("msg_blk->dram_type = 0x%x\n", msg_blk->dram_type);
+	debug("msg_blk->sequence_ctrl = 0x%x\n", msg_blk->sequence_ctrl);
+	debug("msg_blk->phy_cfg = 0x%x\n", msg_blk->phy_cfg);
+	debug("msg_blk->x16present = 0x%x\n", msg_blk->x16present);
+	debug("msg_blk->dramfreq = 0x%x\n", msg_blk->dramfreq);
+	debug("msg_blk->pll_bypass_en = 0x%x\n", msg_blk->pll_bypass_en);
+	debug("msg_blk->dfi_freq_ratio = 0x%x\n", msg_blk->dfi_freq_ratio);
+	debug("msg_blk->phy_odt_impedance = 0x%x\n",
+						msg_blk->phy_odt_impedance);
+	debug("msg_blk->phy_drv_impedance = 0x%x\n",
+						msg_blk->phy_drv_impedance);
+	debug("msg_blk->bpznres_val = 0x%x\n", msg_blk->bpznres_val);
+	debug("msg_blk->enabled_dqs = 0x%x\n", msg_blk->enabled_dqs);
+	debug("msg_blk->acsm_odt_ctrl0 = 0x%x\n", msg_blk->acsm_odt_ctrl0);
+	debug("msg_blk->acsm_odt_ctrl1 = 0x%x\n", msg_blk->acsm_odt_ctrl1);
+	debug("msg_blk->acsm_odt_ctrl2 = 0x%x\n", msg_blk->acsm_odt_ctrl2);
+	debug("msg_blk->acsm_odt_ctrl3 = 0x%x\n", msg_blk->acsm_odt_ctrl3);
+
+	/* RDIMM only */
+	if (input->basic.dimm_type == RDIMM ||
+	    input->basic.dimm_type == LRDIMM) {
+		msg_blk_r = (struct ddr4r1d *)msg_blk;
+		if (msg_blk_r->cs_present_d0 != 0U) {
+			msg_blk_r->f0rc00_d0 = input->rcw[0];
+			msg_blk_r->f0rc01_d0 = input->rcw[1];
+			msg_blk_r->f0rc02_d0 = input->rcw[2];
+			msg_blk_r->f0rc03_d0 = input->rcw[3];
+			msg_blk_r->f0rc04_d0 = input->rcw[4];
+			msg_blk_r->f0rc05_d0 = input->rcw[5];
+			msg_blk_r->f0rc06_d0 = input->rcw[6];
+			msg_blk_r->f0rc07_d0 = input->rcw[7];
+			msg_blk_r->f0rc08_d0 = input->rcw[8];
+			msg_blk_r->f0rc09_d0 = input->rcw[9];
+			msg_blk_r->f0rc0a_d0 = input->rcw[10];
+			msg_blk_r->f0rc0b_d0 = input->rcw[11];
+			msg_blk_r->f0rc0c_d0 = input->rcw[12];
+			msg_blk_r->f0rc0d_d0 = input->rcw[13];
+			msg_blk_r->f0rc0e_d0 = input->rcw[14];
+			msg_blk_r->f0rc0f_d0 = input->rcw[15];
+			msg_blk_r->f0rc3x_d0 = input->rcw3x;
+		}
+		if (msg_blk_r->cs_present_d1 != 0) {
+			msg_blk_r->f0rc00_d1 = input->rcw[0];
+			msg_blk_r->f0rc01_d1 = input->rcw[1];
+			msg_blk_r->f0rc02_d1 = input->rcw[2];
+			msg_blk_r->f0rc03_d1 = input->rcw[3];
+			msg_blk_r->f0rc04_d1 = input->rcw[4];
+			msg_blk_r->f0rc05_d1 = input->rcw[5];
+			msg_blk_r->f0rc06_d1 = input->rcw[6];
+			msg_blk_r->f0rc07_d1 = input->rcw[7];
+			msg_blk_r->f0rc08_d1 = input->rcw[8];
+			msg_blk_r->f0rc09_d1 = input->rcw[9];
+			msg_blk_r->f0rc0a_d1 = input->rcw[10];
+			msg_blk_r->f0rc0b_d1 = input->rcw[11];
+			msg_blk_r->f0rc0c_d1 = input->rcw[12];
+			msg_blk_r->f0rc0d_d1 = input->rcw[13];
+			msg_blk_r->f0rc0e_d1 = input->rcw[14];
+			msg_blk_r->f0rc0f_d1 = input->rcw[15];
+			msg_blk_r->f0rc3x_d1 = input->rcw3x;
+		}
+		if (input->basic.dimm_type == LRDIMM) {
+			msg_blk_lr = (struct ddr4lr1d *)msg_blk;
+			msg_blk_lr->bc0a_d0 = msg_blk_lr->f0rc0a_d0;
+			msg_blk_lr->bc0a_d1 = msg_blk_lr->f0rc0a_d1;
+			msg_blk_lr->f0bc6x_d0 = msg_blk_lr->f0rc3x_d0;
+			msg_blk_lr->f0bc6x_d1 = msg_blk_lr->f0rc3x_d1;
+		}
+	}
+
+	/* below is different for 1D and 2D message block */
+	if (input->basic.train2d != 0) {
+		memcpy(msg_blk_2d, msg_blk, sizeof(struct ddr4u1d));
+		/*High-Effort WrDQ1D is applicable to 2D traning also*/
+		msg_blk_2d->reserved00          |= U(0x40);
+		msg_blk_2d->sequence_ctrl	= U(0x0061);
+		msg_blk_2d->rx2d_train_opt	= 0U;
+		msg_blk_2d->tx2d_train_opt	= 0U;
+		msg_blk_2d->share2dvref_result	= 1U;
+		msg_blk_2d->delay_weight2d	= U(0x20);
+		msg_blk_2d->voltage_weight2d	= U(0x80);
+		debug("rx2d_train_opt %d, tx2d_train_opt %d\n",
+				msg_blk_2d->rx2d_train_opt,
+				msg_blk_2d->tx2d_train_opt);
+	}
+
+	msg_blk->phy_cfg = (((msg_blk->mr3 & U(0x8)) != 0U) ||
+				((msg_blk_2d->mr3 & 0x8) != 0U)) ? 0U
+				: input->adv.is2ttiming;
+
+	return 0;
+}
+
+static void prog_tx_pre_drv_mode(uint16_t *phy,
+				 const struct input *input)
+{
+	int lane, byte, b_addr, c_addr, p_addr;
+	int tx_slew_rate, tx_pre_p, tx_pre_n;
+	int tx_pre_drv_mode = 0x2;
+	uint32_t addr;
+
+	/* Program TxPreDrvMode with 0x2 */
+	/* FIXME: TxPreDrvMode depends on DramType? */
+	tx_pre_p = input->adv.tx_slew_rise_dq;
+	tx_pre_n = input->adv.tx_slew_fall_dq;
+	tx_slew_rate = tx_pre_drv_mode << csr_tx_pre_drv_mode_lsb	|
+		     tx_pre_p << csr_tx_pre_p_lsb			|
+		     tx_pre_n << csr_tx_pre_n_lsb;
+	p_addr = 0;
+	for (byte = 0; byte < input->basic.num_dbyte; byte++) {
+		c_addr = byte << 12;
+		for (lane = 0; lane <= 1; lane++) {
+			b_addr = lane << 8;
+			addr = p_addr | t_dbyte | c_addr | b_addr |
+					csr_tx_slew_rate_addr;
+			phy_io_write16(phy, addr, tx_slew_rate);
+		}
+	}
+}
+
+static void prog_atx_pre_drv_mode(uint16_t *phy,
+				  const struct input *input)
+{
+	int anib, c_addr;
+	int atx_slew_rate, atx_pre_p, atx_pre_n, atx_pre_drv_mode,
+		ck_anib_inst[2];
+	uint32_t addr;
+
+	atx_pre_n = input->adv.tx_slew_fall_ac;
+	atx_pre_p = input->adv.tx_slew_rise_ac;
+
+	if (input->basic.num_anib == 8) {
+		ck_anib_inst[0] = 1;
+		ck_anib_inst[1] = 1;
+	} else if (input->basic.num_anib == 10 || input->basic.num_anib == 12 ||
+	    input->basic.num_anib == 13) {
+		ck_anib_inst[0] = 4;
+		ck_anib_inst[1] = 5;
+	} else {
+		ERROR("Invalid number of aNIBs: %d\n", input->basic.num_anib);
+		return;
+	}
+
+	for (anib = 0; anib < input->basic.num_anib; anib++) {
+		c_addr = anib << 12;
+		if (anib == ck_anib_inst[0] || anib == ck_anib_inst[1]) {
+			atx_pre_drv_mode = 0;
+		} else {
+			atx_pre_drv_mode = 3;
+		}
+		atx_slew_rate = atx_pre_drv_mode << csr_atx_pre_drv_mode_lsb |
+				atx_pre_n << csr_atx_pre_n_lsb		     |
+				atx_pre_p << csr_atx_pre_p_lsb;
+		addr = t_anib | c_addr | csr_atx_slew_rate_addr;
+		phy_io_write16(phy, addr, atx_slew_rate);
+	}
+}
+
+static void prog_enable_cs_multicast(uint16_t *phy,
+				     const struct input *input)
+{
+	uint32_t addr = t_master | csr_enable_cs_multicast_addr;
+
+	if (input->basic.dimm_type != RDIMM &&
+	    input->basic.dimm_type != LRDIMM) {
+		return;
+	}
+
+	phy_io_write16(phy, addr, input->adv.cast_cs_to_cid);
+}
+
+static void prog_dfi_rd_data_cs_dest_map(uint16_t *phy,
+					 unsigned int ip_rev,
+					 const struct input *input,
+					 const struct ddr4lr1d *msg)
+{
+	const struct ddr4lr1d *msg_blk;
+	uint16_t dfi_xxdestm0 = 0U;
+	uint16_t dfi_xxdestm1 = 0U;
+	uint16_t dfi_xxdestm2 = 0U;
+	uint16_t dfi_xxdestm3 = 0U;
+	uint16_t dfi_rd_data_cs_dest_map;
+	uint16_t dfi_wr_data_cs_dest_map;
+	__unused const soc_info_t *soc_info;
+
+#ifdef ERRATA_DDR_A011396
+	/* Only apply to DDRC 5.05.00 */
+	soc_info = get_soc_info();
+	if ((soc_info->svr_reg.bf.maj_ver == 1U) && (ip_rev == U(0x50500))) {
+		phy_io_write16(phy,
+				t_master | csr_dfi_rd_data_cs_dest_map_addr,
+				0U);
+		return;
+	}
+#endif
+
+	msg_blk = msg;
+
+	switch (input->basic.dimm_type) {
+	case UDIMM:
+	case SODIMM:
+	case NODIMM:
+		if ((msg_blk->msg_misc & U(0x40)) != 0U) {
+			dfi_rd_data_cs_dest_map = U(0xa0);
+			dfi_wr_data_cs_dest_map = U(0xa0);
+
+			phy_io_write16(phy,
+				t_master | csr_dfi_rd_data_cs_dest_map_addr,
+				dfi_rd_data_cs_dest_map);
+			phy_io_write16(phy,
+				t_master | csr_dfi_wr_data_cs_dest_map_addr,
+				dfi_wr_data_cs_dest_map);
+		}
+		break;
+	case LRDIMM:
+		if (msg->cs_present_d1 != 0U) {
+			dfi_xxdestm2 = 1U;
+			dfi_xxdestm3 = 1U;
+		}
+
+		dfi_rd_data_cs_dest_map =
+			dfi_xxdestm0 << csr_dfi_rd_destm0_lsb	|
+			dfi_xxdestm1 << csr_dfi_rd_destm1_lsb	|
+			dfi_xxdestm2 << csr_dfi_rd_destm2_lsb	|
+			dfi_xxdestm3 << csr_dfi_rd_destm3_lsb;
+		dfi_wr_data_cs_dest_map =
+			dfi_xxdestm0 << csr_dfi_wr_destm0_lsb	|
+			dfi_xxdestm1 << csr_dfi_wr_destm1_lsb	|
+			dfi_xxdestm2 << csr_dfi_wr_destm2_lsb	|
+			dfi_xxdestm3 << csr_dfi_wr_destm3_lsb;
+		phy_io_write16(phy, t_master | csr_dfi_rd_data_cs_dest_map_addr,
+				dfi_rd_data_cs_dest_map);
+		phy_io_write16(phy, t_master | csr_dfi_wr_data_cs_dest_map_addr,
+				dfi_wr_data_cs_dest_map);
+
+		break;
+	default:
+		break;
+	}
+}
+
+static void prog_pll_ctrl(uint16_t *phy,
+			   const struct input *input)
+{
+	uint32_t addr;
+	int pll_ctrl1 = 0x21; /* 000100001b */
+	int pll_ctrl4 = 0x17f; /* 101111111b */
+	int pll_test_mode = 0x24; /* 00100100b */
+
+	addr = t_master | csr_pll_ctrl1_addr;
+	phy_io_write16(phy, addr, pll_ctrl1);
+
+	debug("pll_ctrl1 = 0x%x\n", phy_io_read16(phy, addr));
+
+	addr = t_master | csr_pll_test_mode_addr;
+	phy_io_write16(phy, addr, pll_test_mode);
+
+	debug("pll_test_mode = 0x%x\n", phy_io_read16(phy, addr));
+
+	addr = t_master | csr_pll_ctrl4_addr;
+	phy_io_write16(phy, addr, pll_ctrl4);
+
+	debug("pll_ctrl4 = 0x%x\n", phy_io_read16(phy, addr));
+}
+
+static void prog_pll_ctrl2(uint16_t *phy,
+			   const struct input *input)
+{
+	int pll_ctrl2;
+	uint32_t addr = t_master | csr_pll_ctrl2_addr;
+
+	if (input->basic.frequency / 2 < 235) {
+		pll_ctrl2 = 0x7;
+	} else if (input->basic.frequency / 2 < 313) {
+		pll_ctrl2 = 0x6;
+	} else if (input->basic.frequency / 2 < 469) {
+		pll_ctrl2 = 0xb;
+	} else if (input->basic.frequency / 2 < 625) {
+		pll_ctrl2 = 0xa;
+	} else if (input->basic.frequency / 2 < 938) {
+		pll_ctrl2 = 0x19;
+	} else if (input->basic.frequency / 2 < 1067) {
+		pll_ctrl2 = 0x18;
+	} else {
+		pll_ctrl2 = 0x19;
+	}
+
+	phy_io_write16(phy, addr, pll_ctrl2);
+
+	debug("pll_ctrl2 = 0x%x\n", phy_io_read16(phy, addr));
+}
+
+static void prog_dll_lck_param(uint16_t *phy, const struct input *input)
+{
+	uint32_t addr = t_master | csr_dll_lockparam_addr;
+
+	phy_io_write16(phy, addr, U(0x212));
+	debug("dll_lck_param = 0x%x\n", phy_io_read16(phy, addr));
+}
+
+static void prog_dll_gain_ctl(uint16_t *phy, const struct input *input)
+{
+	uint32_t addr = t_master | csr_dll_gain_ctl_addr;
+
+	phy_io_write16(phy, addr, U(0x61));
+	debug("dll_gain_ctl = 0x%x\n", phy_io_read16(phy, addr));
+}
+
+static void prog_pll_pwr_dn(uint16_t *phy,
+			   const struct input *input)
+{
+	uint32_t addr;
+
+	addr = t_master | csr_pll_pwr_dn_addr;
+	phy_io_write16(phy, addr, 0U);
+
+	debug("pll_pwrdn = 0x%x\n", phy_io_read16(phy, addr));
+}
+
+static void prog_ard_ptr_init_val(uint16_t *phy,
+				  const struct input *input)
+{
+	int ard_ptr_init_val;
+	uint32_t addr = t_master | csr_ard_ptr_init_val_addr;
+
+	if (input->basic.frequency >= 933) {
+		ard_ptr_init_val = 0x2;
+	} else {
+		ard_ptr_init_val = 0x1;
+	}
+
+	phy_io_write16(phy, addr, ard_ptr_init_val);
+}
+
+static void prog_dqs_preamble_control(uint16_t *phy,
+				      const struct input *input)
+{
+	int data;
+	uint32_t addr = t_master | csr_dqs_preamble_control_addr;
+	const int wdqsextension = 0;
+	const int lp4sttc_pre_bridge_rx_en = 0;
+	const int lp4postamble_ext = 0;
+	const int lp4tgl_two_tck_tx_dqs_pre = 0;
+	const int position_dfe_init = 2;
+	const int dll_rx_preamble_mode = 1;
+	int two_tck_tx_dqs_pre = input->adv.d4tx_preamble_length;
+	int two_tck_rx_dqs_pre = input->adv.d4rx_preamble_length;
+
+	data = wdqsextension << csr_wdqsextension_lsb			|
+	       lp4sttc_pre_bridge_rx_en << csr_lp4sttc_pre_bridge_rx_en_lsb |
+	       lp4postamble_ext << csr_lp4postamble_ext_lsb		|
+	       lp4tgl_two_tck_tx_dqs_pre << csr_lp4tgl_two_tck_tx_dqs_pre_lsb |
+	       position_dfe_init << csr_position_dfe_init_lsb		|
+	       two_tck_tx_dqs_pre << csr_two_tck_tx_dqs_pre_lsb		|
+	       two_tck_rx_dqs_pre << csr_two_tck_rx_dqs_pre_lsb;
+	phy_io_write16(phy, addr, data);
+
+	data = dll_rx_preamble_mode << csr_dll_rx_preamble_mode_lsb;
+	addr = t_master | csr_dbyte_dll_mode_cntrl_addr;
+	phy_io_write16(phy, addr, data);
+}
+
+static void prog_proc_odt_time_ctl(uint16_t *phy,
+				   const struct input *input)
+{
+	int proc_odt_time_ctl;
+	uint32_t addr = t_master | csr_proc_odt_time_ctl_addr;
+
+	if (input->adv.wdqsext != 0) {
+		proc_odt_time_ctl = 0x3;
+	} else if (input->basic.frequency <= 933) {
+		proc_odt_time_ctl = 0xa;
+	} else if (input->basic.frequency <= 1200) {
+		if (input->adv.d4rx_preamble_length == 1) {
+			proc_odt_time_ctl = 0x2;
+		} else {
+			proc_odt_time_ctl = 0x6;
+		}
+	} else {
+		if (input->adv.d4rx_preamble_length == 1) {
+			proc_odt_time_ctl = 0x3;
+		} else {
+			proc_odt_time_ctl = 0x7;
+		}
+	}
+	phy_io_write16(phy, addr, proc_odt_time_ctl);
+}
+
+static const struct impedance_mapping map[] = {
+	{	29,	0x3f	},
+	{	31,	0x3e	},
+	{	33,	0x3b	},
+	{	36,	0x3a	},
+	{	39,	0x39	},
+	{	42,	0x38	},
+	{	46,	0x1b	},
+	{	51,	0x1a	},
+	{	57,	0x19	},
+	{	64,	0x18	},
+	{	74,	0x0b	},
+	{	88,	0x0a	},
+	{	108,	0x09	},
+	{	140,	0x08	},
+	{	200,	0x03	},
+	{	360,	0x02	},
+	{	481,	0x01	},
+	{}
+};
+
+static int map_impedance(int strength)
+{
+	const struct impedance_mapping *tbl = map;
+	int val = 0;
+
+	if (strength == 0) {
+		return 0;
+	}
+
+	while (tbl->ohm != 0U) {
+		if (strength < tbl->ohm) {
+			val = tbl->code;
+			break;
+		}
+		tbl++;
+	}
+
+	return val;
+}
+
+static int map_odtstren_p(int strength, int hard_macro_ver)
+{
+	int val = -1;
+
+	if (hard_macro_ver == 4) {
+		if (strength == 0) {
+			val = 0;
+		} else if (strength == 120) {
+			val = 0x8;
+		} else if (strength == 60) {
+			val = 0x18;
+		} else if (strength == 40) {
+			val = 0x38;
+		} else {
+			printf("error: unsupported ODTStrenP %d\n", strength);
+		}
+	} else {
+		val = map_impedance(strength);
+	}
+
+	return val;
+}
+
+static void prog_tx_odt_drv_stren(uint16_t *phy,
+				  const struct input *input)
+{
+	int lane, byte, b_addr, c_addr;
+	int tx_odt_drv_stren;
+	int odtstren_p, odtstren_n;
+	uint32_t addr;
+
+	odtstren_p = map_odtstren_p(input->adv.odtimpedance,
+				input->basic.hard_macro_ver);
+	if (odtstren_p < 0) {
+		return;
+	}
+
+	odtstren_n = 0;	/* always high-z */
+	tx_odt_drv_stren = odtstren_n << csr_odtstren_n_lsb | odtstren_p;
+	for (byte = 0; byte < input->basic.num_dbyte; byte++) {
+		c_addr = byte << 12;
+		for (lane = 0; lane <= 1; lane++) {
+			b_addr = lane << 8;
+			addr = t_dbyte | c_addr | b_addr |
+				csr_tx_odt_drv_stren_addr;
+			phy_io_write16(phy, addr, tx_odt_drv_stren);
+		}
+	}
+}
+
+static int map_drvstren_fsdq_p(int strength, int hard_macro_ver)
+{
+	int val = -1;
+
+	if (hard_macro_ver == 4) {
+		if (strength == 0) {
+			val = 0x07;
+		} else if (strength == 120) {
+			val = 0x0F;
+		} else if (strength == 60) {
+			val = 0x1F;
+		} else if (strength == 40) {
+			val = 0x3F;
+		} else {
+			printf("error: unsupported drv_stren_fSDq_p %d\n",
+			       strength);
+		}
+	} else {
+		val = map_impedance(strength);
+	}
+
+	return val;
+}
+
+static int map_drvstren_fsdq_n(int strength, int hard_macro_ver)
+{
+	int val = -1;
+
+	if (hard_macro_ver == 4) {
+		if (strength == 0) {
+			val = 0x00;
+		} else if (strength == 120) {
+			val = 0x08;
+		} else if (strength == 60) {
+			val = 0x18;
+		} else if (strength == 40) {
+			val = 0x38;
+		} else {
+			printf("error: unsupported drvStrenFSDqN %d\n",
+			       strength);
+		}
+	} else {
+		val = map_impedance(strength);
+	}
+
+	return val;
+}
+
+static void prog_tx_impedance_ctrl1(uint16_t *phy,
+				    const struct input *input)
+{
+	int lane, byte, b_addr, c_addr;
+	int tx_impedance_ctrl1;
+	int drv_stren_fsdq_p, drv_stren_fsdq_n;
+	uint32_t addr;
+
+	drv_stren_fsdq_p = map_drvstren_fsdq_p(input->adv.tx_impedance,
+					input->basic.hard_macro_ver);
+	drv_stren_fsdq_n = map_drvstren_fsdq_n(input->adv.tx_impedance,
+					input->basic.hard_macro_ver);
+	tx_impedance_ctrl1 = drv_stren_fsdq_n << csr_drv_stren_fsdq_n_lsb |
+			   drv_stren_fsdq_p << csr_drv_stren_fsdq_p_lsb;
+
+	for (byte = 0; byte < input->basic.num_dbyte; byte++) {
+		c_addr = byte << 12;
+		for (lane = 0; lane <= 1; lane++) {
+			b_addr = lane << 8;
+			addr = t_dbyte | c_addr | b_addr |
+				csr_tx_impedance_ctrl1_addr;
+			phy_io_write16(phy, addr, tx_impedance_ctrl1);
+		}
+	}
+}
+
+static int map_adrv_stren_p(int strength, int hard_macro_ver)
+{
+	int val = -1;
+
+	if (hard_macro_ver == 4) {
+		if (strength == 120) {
+			val = 0x1c;
+		} else if (strength == 60) {
+			val = 0x1d;
+		} else if (strength == 40) {
+			val = 0x1f;
+		} else {
+			printf("error: unsupported aDrv_stren_p %d\n",
+			       strength);
+		}
+	} else {
+		if (strength == 120) {
+			val = 0x00;
+		} else if (strength == 60) {
+			val = 0x01;
+		} else if (strength == 40) {
+			val = 0x03;
+		} else if (strength == 30) {
+			val = 0x07;
+		} else if (strength == 24) {
+			val = 0x0f;
+		} else if (strength == 20) {
+			val = 0x1f;
+		} else {
+			printf("error: unsupported aDrv_stren_p %d\n",
+			       strength);
+		}
+	}
+
+	return val;
+}
+
+static int map_adrv_stren_n(int strength, int hard_macro_ver)
+{
+	int val = -1;
+
+	if (hard_macro_ver == 4) {
+		if (strength == 120) {
+			val = 0x00;
+		} else if (strength == 60) {
+			val = 0x01;
+		} else if (strength == 40) {
+			val = 0x03;
+		} else {
+			printf("Error: unsupported ADrvStrenP %d\n", strength);
+		}
+	} else {
+		if (strength == 120) {
+			val = 0x00;
+		} else if (strength == 60) {
+			val = 0x01;
+		} else if (strength == 40) {
+			val = 0x03;
+		} else if (strength == 30) {
+			val = 0x07;
+		} else if (strength == 24) {
+			val = 0x0f;
+		} else if (strength == 20) {
+			val = 0x1f;
+		} else {
+			printf("Error: unsupported ADrvStrenP %d\n", strength);
+		}
+	}
+
+	return val;
+}
+
+static void prog_atx_impedance(uint16_t *phy,
+			       const struct input *input)
+{
+	int anib, c_addr;
+	int atx_impedance;
+	int adrv_stren_p;
+	int adrv_stren_n;
+	uint32_t addr;
+
+	if (input->basic.hard_macro_ver == 4 &&
+	    input->adv.atx_impedance == 20) {
+		printf("Error:ATxImpedance has to be 40 for HardMacroVer 4\n");
+		return;
+	}
+
+	adrv_stren_p = map_adrv_stren_p(input->adv.atx_impedance,
+					input->basic.hard_macro_ver);
+	adrv_stren_n = map_adrv_stren_n(input->adv.atx_impedance,
+					input->basic.hard_macro_ver);
+	atx_impedance = adrv_stren_n << csr_adrv_stren_n_lsb		|
+		       adrv_stren_p << csr_adrv_stren_p_lsb;
+	for (anib = 0; anib < input->basic.num_anib; anib++) {
+		c_addr = anib << 12;
+		addr = t_anib | c_addr | csr_atx_impedance_addr;
+		phy_io_write16(phy, addr, atx_impedance);
+	}
+}
+
+static void prog_dfi_mode(uint16_t *phy,
+			  const struct input *input)
+{
+	int dfi_mode;
+	uint32_t addr;
+
+	if (input->basic.dfi1exists == 1) {
+		dfi_mode = 0x5;	/* DFI1 exists but disabled */
+	} else {
+		dfi_mode = 0x1;	/* DFI1 does not physically exists */
+	}
+	addr = t_master | csr_dfi_mode_addr;
+	phy_io_write16(phy, addr, dfi_mode);
+}
+
+static void prog_acx4_anib_dis(uint16_t *phy, const struct input *input)
+{
+	uint32_t addr;
+
+	addr = t_master | csr_acx4_anib_dis_addr;
+	phy_io_write16(phy, addr, 0x0);
+	debug("%s 0x%x\n", __func__, phy_io_read16(phy, addr));
+}
+
+static void prog_dfi_camode(uint16_t *phy,
+			    const struct input *input)
+{
+	int dfi_camode = 2;
+	uint32_t addr = t_master | csr_dfi_camode_addr;
+
+	phy_io_write16(phy, addr, dfi_camode);
+}
+
+static void prog_cal_drv_str0(uint16_t *phy,
+			      const struct input *input)
+{
+	int cal_drv_str0;
+	int cal_drv_str_pd50;
+	int cal_drv_str_pu50;
+	uint32_t addr;
+
+	cal_drv_str_pu50 = input->adv.ext_cal_res_val;
+	cal_drv_str_pd50 = cal_drv_str_pu50;
+	cal_drv_str0 = cal_drv_str_pu50 << csr_cal_drv_str_pu50_lsb |
+			cal_drv_str_pd50;
+	addr = t_master | csr_cal_drv_str0_addr;
+	phy_io_write16(phy, addr, cal_drv_str0);
+}
+
+static void prog_cal_uclk_info(uint16_t *phy,
+			       const struct input *input)
+{
+	int cal_uclk_ticks_per1u_s;
+	uint32_t addr;
+
+	cal_uclk_ticks_per1u_s = input->basic.frequency >> 1;
+	if (cal_uclk_ticks_per1u_s < 24) {
+		cal_uclk_ticks_per1u_s = 24;
+	}
+
+	addr = t_master | csr_cal_uclk_info_addr;
+	phy_io_write16(phy, addr, cal_uclk_ticks_per1u_s);
+}
+
+static void prog_cal_rate(uint16_t *phy,
+			  const struct input *input)
+{
+	int cal_rate;
+	int cal_interval;
+	int cal_once;
+	uint32_t addr;
+
+	cal_interval = input->adv.cal_interval;
+	cal_once = input->adv.cal_once;
+	cal_rate = cal_once << csr_cal_once_lsb		|
+		  cal_interval << csr_cal_interval_lsb;
+	addr = t_master | csr_cal_rate_addr;
+	phy_io_write16(phy, addr, cal_rate);
+}
+
+static void prog_vref_in_global(uint16_t *phy,
+				const struct input *input,
+				const struct ddr4u1d *msg)
+{
+	int vref_in_global;
+	int global_vref_in_dac = 0;
+	int global_vref_in_sel = 0;
+	uint32_t addr;
+
+	/*
+	 * phy_vref_prcnt = msg->phy_vref / 128.0
+	 *  global_vref_in_dac = (phy_vref_prcnt - 0.345) / 0.005;
+	 */
+	global_vref_in_dac = (msg->phy_vref * 1000 - 345 * 128 + 320) /
+			     (5 * 128);
+
+	vref_in_global = global_vref_in_dac << csr_global_vref_in_dac_lsb |
+		       global_vref_in_sel;
+	addr = t_master | csr_vref_in_global_addr;
+	phy_io_write16(phy, addr, vref_in_global);
+}
+
+static void prog_dq_dqs_rcv_cntrl(uint16_t *phy,
+				  const struct input *input)
+{
+	int lane, byte, b_addr, c_addr;
+	int dq_dqs_rcv_cntrl;
+	int gain_curr_adj_defval = 0xb;
+	int major_mode_dbyte = 3;
+	int dfe_ctrl_defval = 0;
+	int ext_vref_range_defval = 0;
+	int sel_analog_vref = 1;
+	uint32_t addr;
+
+#ifdef ERRATA_DDR_A050958
+	gain_curr_adj_defval = 0x1f;
+#endif
+
+	dq_dqs_rcv_cntrl = gain_curr_adj_defval << csr_gain_curr_adj_lsb |
+			major_mode_dbyte << csr_major_mode_dbyte_lsb	|
+			dfe_ctrl_defval << csr_dfe_ctrl_lsb		|
+			ext_vref_range_defval << csr_ext_vref_range_lsb	|
+			sel_analog_vref << csr_sel_analog_vref_lsb;
+	for (byte = 0; byte < input->basic.num_dbyte; byte++) {
+		c_addr = byte << 12;
+		for (lane = 0; lane <= 1; lane++) {
+			b_addr = lane << 8;
+			addr = t_dbyte | c_addr | b_addr |
+					csr_dq_dqs_rcv_cntrl_addr;
+			phy_io_write16(phy, addr, dq_dqs_rcv_cntrl);
+		}
+	}
+}
+
+static void prog_mem_alert_control(uint16_t *phy,
+				   const struct input *input)
+{
+	int mem_alert_control;
+	int mem_alert_control2;
+	int malertpu_en;
+	int malertrx_en;
+	int malertvref_level;
+	int malertpu_stren;
+	int malertsync_bypass;
+	int malertdisable_val_defval = 1;
+	uint32_t addr;
+
+	if (input->basic.dram_type == DDR4 && input->adv.mem_alert_en == 1) {
+		malertpu_en = 1;
+		malertrx_en = 1;
+		malertpu_stren = input->adv.mem_alert_puimp;
+		malertvref_level = input->adv.mem_alert_vref_level;
+		malertsync_bypass = input->adv.mem_alert_sync_bypass;
+		mem_alert_control = malertdisable_val_defval << 14	|
+				  malertrx_en << 13		|
+				  malertpu_en << 12		|
+				  malertpu_stren << 8		|
+				  malertvref_level;
+		mem_alert_control2 = malertsync_bypass <<
+					csr_malertsync_bypass_lsb;
+		addr = t_master | csr_mem_alert_control_addr;
+		phy_io_write16(phy, addr, mem_alert_control);
+		addr = t_master | csr_mem_alert_control2_addr;
+		phy_io_write16(phy, addr, mem_alert_control2);
+	}
+}
+
+static void prog_dfi_freq_ratio(uint16_t *phy,
+				const struct input *input)
+{
+	int dfi_freq_ratio;
+	uint32_t addr = t_master | csr_dfi_freq_ratio_addr;
+
+	dfi_freq_ratio = input->basic.dfi_freq_ratio;
+	phy_io_write16(phy, addr, dfi_freq_ratio);
+}
+
+static void prog_tristate_mode_ca(uint16_t *phy,
+				  const struct input *input)
+{
+	int tristate_mode_ca;
+	int dis_dyn_adr_tri;
+	int ddr2tmode;
+	int ck_dis_val_def = 1;
+	uint32_t addr = t_master | csr_tristate_mode_ca_addr;
+
+	dis_dyn_adr_tri = input->adv.dis_dyn_adr_tri;
+	ddr2tmode = input->adv.is2ttiming;
+	tristate_mode_ca = ck_dis_val_def << csr_ck_dis_val_lsb	|
+			 ddr2tmode << csr_ddr2tmode_lsb		|
+			 dis_dyn_adr_tri << csr_dis_dyn_adr_tri_lsb;
+	phy_io_write16(phy, addr, tristate_mode_ca);
+}
+
+static void prog_dfi_xlat(uint16_t *phy,
+			  const struct input *input)
+{
+	uint16_t loop_vector;
+	int dfifreqxlat_dat;
+	int pllbypass_dat;
+	uint32_t addr;
+
+	/* fIXME: Shall unused P1, P2, P3 be bypassed? */
+	pllbypass_dat = input->basic.pll_bypass; /* only [0] is used */
+	for (loop_vector = 0; loop_vector < 8; loop_vector++) {
+		if (loop_vector == 0) {
+			dfifreqxlat_dat = pllbypass_dat + 0x5555;
+		} else if (loop_vector == 7) {
+			dfifreqxlat_dat = 0xf000;
+		} else {
+			dfifreqxlat_dat = 0x5555;
+		}
+		addr = t_master | (csr_dfi_freq_xlat0_addr + loop_vector);
+		phy_io_write16(phy, addr, dfifreqxlat_dat);
+	}
+}
+
+static void prog_dbyte_misc_mode(uint16_t *phy,
+				 const struct input *input,
+				 const struct ddr4u1d *msg)
+{
+	int dbyte_misc_mode;
+	int dq_dqs_rcv_cntrl1;
+	int dq_dqs_rcv_cntrl1_1;
+	int byte, c_addr;
+	uint32_t addr;
+
+	dbyte_misc_mode = 0x1 << csr_dbyte_disable_lsb;
+	dq_dqs_rcv_cntrl1 = 0x1ff << csr_power_down_rcvr_lsb		|
+			 0x1 << csr_power_down_rcvr_dqs_lsb	|
+			 0x1 << csr_rx_pad_standby_en_lsb;
+	dq_dqs_rcv_cntrl1_1 = (0x100 << csr_power_down_rcvr_lsb |
+			csr_rx_pad_standby_en_mask);
+	for (byte = 0; byte < input->basic.num_dbyte; byte++) {
+		c_addr = byte << 12;
+		if (byte <= input->basic.num_active_dbyte_dfi0 - 1) {
+			/* disable RDBI lane if not used. */
+			if ((input->basic.dram_data_width != 4) &&
+				(((msg->mr5 >> 12) & 0x1) == 0)) {
+				addr = t_dbyte
+					| c_addr
+					| csr_dq_dqs_rcv_cntrl1_addr;
+				phy_io_write16(phy, addr, dq_dqs_rcv_cntrl1_1);
+			}
+		} else {
+			addr = t_dbyte | c_addr | csr_dbyte_misc_mode_addr;
+			phy_io_write16(phy, addr, dbyte_misc_mode);
+			addr = t_dbyte | c_addr | csr_dq_dqs_rcv_cntrl1_addr;
+			phy_io_write16(phy, addr, dq_dqs_rcv_cntrl1);
+		}
+	}
+}
+
+static void prog_master_x4config(uint16_t *phy,
+				 const struct input *input)
+{
+	int master_x4config;
+	int x4tg;
+	uint32_t addr = t_master | csr_master_x4config_addr;
+
+	x4tg = input->basic.dram_data_width == 4 ? 0xf : 0;
+	master_x4config = x4tg << csr_x4tg_lsb;
+	phy_io_write16(phy, addr, master_x4config);
+}
+
+static void prog_dmipin_present(uint16_t *phy,
+				const struct input *input,
+				const struct ddr4u1d *msg)
+{
+	int dmipin_present;
+	uint32_t addr = t_master | csr_dmipin_present_addr;
+
+	dmipin_present = (msg->mr5 >> 12) & 0x1;
+	phy_io_write16(phy, addr, dmipin_present);
+}
+
+static void prog_dfi_phyupd(uint16_t *phy,
+			  const struct input *input)
+{
+	int dfiphyupd_dat;
+	uint32_t addr;
+
+	addr = t_master | (csr_dfiphyupd_addr);
+	dfiphyupd_dat = phy_io_read16(phy, addr) &
+				~csr_dfiphyupd_threshold_mask;
+
+	phy_io_write16(phy, addr, dfiphyupd_dat);
+}
+
+static void prog_cal_misc2(uint16_t *phy,
+			  const struct input *input)
+{
+	int cal_misc2_dat, cal_drv_pdth_data, cal_offsets_dat;
+	uint32_t addr;
+
+	addr = t_master | (csr_cal_misc2_addr);
+	cal_misc2_dat = phy_io_read16(phy, addr) |
+			(1 << csr_cal_misc2_err_dis);
+
+	phy_io_write16(phy, addr, cal_misc2_dat);
+
+	addr = t_master | (csr_cal_offsets_addr);
+
+	cal_drv_pdth_data = 0x9 << 6;
+	cal_offsets_dat = (phy_io_read16(phy, addr) & ~csr_cal_drv_pdth_mask)
+			| cal_drv_pdth_data;
+
+	phy_io_write16(phy, addr, cal_offsets_dat);
+}
+
+static int c_init_phy_config(uint16_t **phy_ptr,
+			     unsigned int ip_rev,
+			     const struct input *input,
+			     const void *msg)
+{
+	int i;
+	uint16_t *phy;
+	__unused const soc_info_t *soc_info;
+
+	for (i = 0; i < NUM_OF_DDRC; i++) {
+		phy = phy_ptr[i];
+		if (phy == NULL) {
+			continue;
+		}
+
+		debug("Initialize PHY %d config\n", i);
+		prog_dfi_phyupd(phy, input);
+		prog_cal_misc2(phy, input);
+		prog_tx_pre_drv_mode(phy, input);
+		prog_atx_pre_drv_mode(phy, input);
+		prog_enable_cs_multicast(phy, input);	/* rdimm and lrdimm */
+		prog_dfi_rd_data_cs_dest_map(phy, ip_rev, input, msg);
+		prog_pll_ctrl2(phy, input);
+#ifdef DDR_PLL_FIX
+		soc_info = get_soc_info();
+		debug("SOC_SI_REV = %x\n", soc_info->svr_reg.bf.maj_ver);
+		if (soc_info->svr_reg.bf.maj_ver == 1) {
+			prog_pll_pwr_dn(phy, input);
+
+			/*Enable FFE aka TxEqualizationMode for rev1 SI*/
+			phy_io_write16(phy, 0x010048, 0x1);
+		}
+#endif
+		prog_ard_ptr_init_val(phy, input);
+		prog_dqs_preamble_control(phy, input);
+		prog_dll_lck_param(phy, input);
+		prog_dll_gain_ctl(phy, input);
+		prog_proc_odt_time_ctl(phy, input);
+		prog_tx_odt_drv_stren(phy, input);
+		prog_tx_impedance_ctrl1(phy, input);
+		prog_atx_impedance(phy, input);
+		prog_dfi_mode(phy, input);
+		prog_dfi_camode(phy, input);
+		prog_cal_drv_str0(phy, input);
+		prog_cal_uclk_info(phy, input);
+		prog_cal_rate(phy, input);
+		prog_vref_in_global(phy, input, msg);
+		prog_dq_dqs_rcv_cntrl(phy, input);
+		prog_mem_alert_control(phy, input);
+		prog_dfi_freq_ratio(phy, input);
+		prog_tristate_mode_ca(phy, input);
+		prog_dfi_xlat(phy, input);
+		prog_dbyte_misc_mode(phy, input, msg);
+		prog_master_x4config(phy, input);
+		prog_dmipin_present(phy, input, msg);
+		prog_acx4_anib_dis(phy, input);
+	}
+
+	return 0;
+}
+
+static uint32_t get_mail(uint16_t *phy, int stream)
+{
+	int timeout;
+	uint32_t mail = 0U;
+
+	timeout = TIMEOUTDEFAULT;
+	while (((--timeout) != 0) &&
+	       ((phy_io_read16(phy, t_apbonly | csr_uct_shadow_regs)
+		& uct_write_prot_shadow_mask) != 0)) {
+		mdelay(10);
+	}
+	if (timeout == 0) {
+		ERROR("Timeout getting mail from PHY\n");
+		return 0xFFFF;
+	}
+
+	mail = phy_io_read16(phy, t_apbonly |
+			     csr_uct_write_only_shadow);
+	if (stream != 0) {
+		mail |= phy_io_read16(phy, t_apbonly |
+				      csr_uct_dat_write_only_shadow) << 16;
+	}
+
+	/* Ack */
+	phy_io_write16(phy, t_apbonly | csr_dct_write_prot, 0);
+
+	timeout = TIMEOUTDEFAULT;
+	while (((--timeout) != 0) &&
+	       ((phy_io_read16(phy, t_apbonly | csr_uct_shadow_regs)
+		 & uct_write_prot_shadow_mask) == 0)) {
+		mdelay(1);
+	}
+	if (timeout == 0) {
+		ERROR("Timeout ack PHY mail\n");
+	}
+
+	/* completed */
+	phy_io_write16(phy, t_apbonly | csr_dct_write_prot, 1U);
+
+	return mail;
+}
+
+#ifdef DDR_PHY_DEBUG
+static const char *lookup_msg(uint32_t index, int train2d)
+{
+	int i;
+	int size;
+	const struct phy_msg *messages;
+	const char *ptr = NULL;
+
+	if (train2d != 0) {
+		messages = messages_2d;
+		size = ARRAY_SIZE(messages_2d);
+	} else {
+		messages = messages_1d;
+		size = ARRAY_SIZE(messages_1d);
+	}
+	for (i = 0; i < size; i++) {
+		if (messages[i].index == index) {
+			ptr = messages[i].msg;
+			break;
+		}
+	}
+
+	return ptr;
+}
+#endif
+
+#define MAX_ARGS 32
+static void decode_stream_message(uint16_t *phy, int train2d)
+{
+	uint32_t index __unused;
+
+	__unused const char *format;
+	__unused uint32_t args[MAX_ARGS];
+	__unused int i;
+
+#ifdef DDR_PHY_DEBUG
+	index = get_mail(phy, 1);
+	if ((index & 0xffff) > MAX_ARGS) {	/* up to MAX_ARGS args so far */
+		printf("Program error in %s\n", __func__);
+	}
+	for (i = 0; i < (index & 0xffff) && i < MAX_ARGS; i++) {
+		args[i] = get_mail(phy, 1);
+	}
+
+	format = lookup_msg(index, train2d);
+	if (format != NULL) {
+		printf("0x%08x: ", index);
+		printf(format, args[0], args[1], args[2], args[3], args[4],
+		       args[5], args[6], args[7], args[8], args[9], args[10],
+		       args[11], args[12], args[13], args[14], args[15],
+		       args[16], args[17], args[18], args[19], args[20],
+		       args[21], args[22], args[23], args[24], args[25],
+		       args[26], args[27], args[28], args[29], args[30],
+		       args[31]);
+	}
+#endif
+}
+
+static int wait_fw_done(uint16_t *phy, int train2d)
+{
+	uint32_t mail = 0U;
+
+	while (mail == U(0x0)) {
+		mail = get_mail(phy, 0);
+		switch (mail) {
+		case U(0x7):
+			debug("%s Training completed\n", train2d ? "2D" : "1D");
+			break;
+		case U(0xff):
+			debug("%s Training failure\n", train2d ? "2D" : "1D");
+			break;
+		case U(0x0):
+			debug("End of initialization\n");
+			mail = 0U;
+			break;
+		case U(0x1):
+			debug("End of fine write leveling\n");
+			mail = 0U;
+			break;
+		case U(0x2):
+			debug("End of read enable training\n");
+			mail = 0U;
+			break;
+		case U(0x3):
+			debug("End of read delay center optimization\n");
+			mail = 0U;
+			break;
+		case U(0x4):
+			debug("End of write delay center optimization\n");
+			mail = 0U;
+			break;
+		case U(0x5):
+			debug("End of 2D read delay/voltage center optimztn\n");
+			mail = 0U;
+			break;
+		case U(0x6):
+			debug("End of 2D write delay/voltage center optmztn\n");
+			mail = 0U;
+			break;
+		case U(0x8):
+			decode_stream_message(phy, train2d);
+			mail = 0U;
+			break;
+		case U(0x9):
+			debug("End of max read latency training\n");
+			mail = 0U;
+			break;
+		case U(0xa):
+			debug("End of read dq deskew training\n");
+			mail = 0U;
+			break;
+		case U(0xc):
+			debug("End of LRDIMM Specific training, including:\n");
+			debug("/tDWL, MREP, MRD and MWD\n");
+			mail = 0U;
+			break;
+		case U(0xd):
+			debug("End of CA training\n");
+			mail = 0U;
+			break;
+		case U(0xfd):
+			debug("End of MPR read delay center optimization\n");
+			mail = 0U;
+			break;
+		case U(0xfe):
+			debug("End of Write leveling coarse delay\n");
+			mail = 0U;
+			break;
+		case U(0xffff):
+			debug("Timed out\n");
+			break;
+		default:
+			mail = 0U;
+			break;
+		}
+	}
+
+	if (mail == U(0x7)) {
+		return 0;
+	} else if (mail == U(0xff)) {
+		return -EIO;
+	} else if (mail == U(0xffff)) {
+		return -ETIMEDOUT;
+	}
+
+	debug("PHY_GEN2 FW: Unxpected mail = 0x%x\n", mail);
+
+	return -EINVAL;
+}
+
+static int g_exec_fw(uint16_t **phy_ptr, int train2d, struct input *input)
+{
+	int ret = -EINVAL;
+	int i;
+	uint16_t *phy;
+
+	for (i = 0; i < NUM_OF_DDRC; i++) {
+		phy = phy_ptr[i];
+		if (phy == NULL) {
+			continue;
+		}
+		debug("Applying PLL optimal settings\n");
+		prog_pll_ctrl2(phy, input);
+		prog_pll_ctrl(phy, input);
+		phy_io_write16(phy,
+			       t_apbonly | csr_micro_cont_mux_sel_addr,
+			       0x1);
+		phy_io_write16(phy,
+			       t_apbonly | csr_micro_reset_addr,
+			       csr_reset_to_micro_mask |
+			       csr_stall_to_micro_mask);
+		phy_io_write16(phy,
+			       t_apbonly | csr_micro_reset_addr,
+			       csr_stall_to_micro_mask);
+		phy_io_write16(phy,
+			       t_apbonly | csr_micro_reset_addr,
+			       0);
+
+		ret = wait_fw_done(phy, train2d);
+		if (ret == -ETIMEDOUT) {
+			ERROR("Wait timed out: Firmware execution on PHY %d\n",
+			      i);
+		}
+	}
+	return ret;
+}
+
+static inline int send_fw(uint16_t *phy,
+			   uint32_t dst,
+			   uint16_t *img,
+			   uint32_t size)
+{
+	uint32_t i;
+
+	if ((size % 2U) != 0U) {
+		ERROR("Wrong image size 0x%x\n", size);
+		return -EINVAL;
+	}
+
+	for (i = 0U; i < size / 2; i++) {
+		phy_io_write16(phy, dst + i, *(img + i));
+	}
+
+	return 0;
+}
+
+static int load_fw(uint16_t **phy_ptr,
+		   struct input *input,
+		   int train2d,
+		   void *msg,
+		   size_t len,
+		   uintptr_t phy_gen2_fw_img_buf,
+		   int (*img_loadr)(unsigned int, uintptr_t *, uint32_t *),
+		   uint32_t warm_boot_flag)
+{
+	uint32_t imem_id, dmem_id;
+	uintptr_t image_buf;
+	uint32_t size;
+	int ret;
+	int i;
+	uint16_t *phy;
+
+	switch (input->basic.dimm_type) {
+	case UDIMM:
+	case SODIMM:
+	case NODIMM:
+		imem_id = train2d ? DDR_IMEM_UDIMM_2D_IMAGE_ID :
+			  DDR_IMEM_UDIMM_1D_IMAGE_ID;
+		dmem_id = train2d ? DDR_DMEM_UDIMM_2D_IMAGE_ID :
+			  DDR_DMEM_UDIMM_1D_IMAGE_ID;
+		break;
+	case RDIMM:
+		imem_id = train2d ? DDR_IMEM_RDIMM_2D_IMAGE_ID :
+			  DDR_IMEM_RDIMM_1D_IMAGE_ID;
+		dmem_id = train2d ? DDR_DMEM_RDIMM_2D_IMAGE_ID :
+			  DDR_DMEM_RDIMM_1D_IMAGE_ID;
+		break;
+	default:
+		ERROR("Unsupported DIMM type\n");
+		return -EINVAL;
+	}
+
+	size = PHY_GEN2_MAX_IMAGE_SIZE;
+	image_buf = (uintptr_t)phy_gen2_fw_img_buf;
+	ret = img_loadr(imem_id, &image_buf, &size);
+	if (ret != 0) {
+		ERROR("Failed to load %d firmware.\n", imem_id);
+		return ret;
+	}
+	debug("Loaded Imaged id %d of size %x at address %lx\n",
+						imem_id, size, image_buf);
+
+	for (i = 0; i < NUM_OF_DDRC; i++) {
+		phy = phy_ptr[i];
+		if (phy == NULL) {
+			continue;
+		}
+
+		if (warm_boot_flag != DDR_WARM_BOOT) {
+			if (train2d == 0) {
+				phy_io_write16(phy, t_master |
+						csr_mem_reset_l_addr,
+						csr_protect_mem_reset_mask);
+			}
+		}
+		/* Enable access to the internal CSRs */
+		phy_io_write16(phy, t_apbonly | csr_micro_cont_mux_sel_addr, 0);
+
+		ret = send_fw(phy, PHY_GEN2_IMEM_ADDR,
+			      (uint16_t *)image_buf, size);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	size = PHY_GEN2_MAX_IMAGE_SIZE;
+	image_buf = (uintptr_t)phy_gen2_fw_img_buf;
+	ret = img_loadr(dmem_id, &image_buf, &size);
+	if (ret != 0) {
+		ERROR("Failed to load %d firmware.\n", dmem_id);
+		return ret;
+	}
+	debug("Loaded Imaged id %d of size %x at address %lx\n",
+						dmem_id, size, image_buf);
+	image_buf += len;
+	size -= len;
+
+	for (i = 0; i < NUM_OF_DDRC; i++) {
+		phy = phy_ptr[i];
+		if (phy == NULL) {
+			continue;
+		}
+
+		ret = send_fw(phy, PHY_GEN2_DMEM_ADDR, msg, len);
+		if (ret != 0) {
+			return ret;
+		}
+
+		ret = send_fw(phy, PHY_GEN2_DMEM_ADDR + len / 2,
+			      (uint16_t *)image_buf, size);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	return ret;
+}
+
+static void parse_odt(const unsigned int val,
+		       const int read,
+		       const int i,
+		       const unsigned int cs_d0,
+		       const unsigned int cs_d1,
+		       unsigned int *odt)
+{
+	int shift = read ? 4 : 0;
+	int j;
+
+	if (i < 0 || i > 3) {
+		printf("Error: invalid chip-select value\n");
+	}
+	switch (val) {
+	case DDR_ODT_CS:
+		odt[i] |= (1 << i) << shift;
+		break;
+	case DDR_ODT_ALL_OTHER_CS:
+		for (j = 0; j < DDRC_NUM_CS; j++) {
+			if (i == j) {
+				continue;
+			}
+			if (((cs_d0 | cs_d1) & (1 << j)) == 0) {
+				continue;
+			}
+			odt[j] |= (1 << i) << shift;
+		}
+		break;
+	case DDR_ODT_CS_AND_OTHER_DIMM:
+		odt[i] |= (1 << i) << 4;
+		/* fallthrough */
+	case DDR_ODT_OTHER_DIMM:
+		for (j = 0; j < DDRC_NUM_CS; j++) {
+			if ((((cs_d0 & (1 << i)) != 0) &&
+						((cs_d1 & (1 << j)) != 0)) ||
+			    (((cs_d1 & (1 << i)) != 0) &&
+						((cs_d0 & (1 << j)) != 0))) {
+				odt[j] |= (1 << i) << shift;
+			}
+		}
+		break;
+	case DDR_ODT_ALL:
+		for (j = 0; j < DDRC_NUM_CS; j++) {
+			if (((cs_d0 | cs_d1) & (1 << j)) == 0) {
+				continue;
+			}
+			odt[j] |= (1 << i) << shift;
+		}
+		break;
+	case DDR_ODT_SAME_DIMM:
+		for (j = 0; j < DDRC_NUM_CS; j++) {
+			if ((((cs_d0 & (1 << i)) != 0) &&
+						((cs_d0 & (1 << j)) != 0)) ||
+			    (((cs_d1 & (1 << i)) != 0) &&
+						((cs_d1 & (1 << j)) != 0))) {
+				odt[j] |= (1 << i) << shift;
+			}
+		}
+		break;
+	case DDR_ODT_OTHER_CS_ONSAMEDIMM:
+		for (j = 0; j < DDRC_NUM_CS; j++) {
+			if (i == j) {
+				continue;
+			}
+			if ((((cs_d0 & (1 << i)) != 0) &&
+						((cs_d0 & (1 << j)) != 0)) ||
+			    (((cs_d1 & (1 << i)) != 0) &&
+						((cs_d1 & (1 << j)) != 0))) {
+				odt[j] |= (1 << i) << shift;
+			}
+		}
+		break;
+	case DDR_ODT_NEVER:
+		break;
+	default:
+		break;
+	}
+}
+
+#ifdef DEBUG_DDR_INPUT_CONFIG
+char *dram_types_str[] = {
+		"DDR4",
+		"DDR3",
+		"LDDDR4",
+		"LPDDR3",
+		"LPDDR2",
+		"DDR5"
+};
+
+char *dimm_types_str[] = {
+		"UDIMM",
+		"SODIMM",
+		"RDIMM",
+		"LRDIMM",
+		"NODIMM",
+};
+
+
+static void print_jason_format(struct input *input,
+			       struct ddr4u1d *msg_1d,
+			       struct ddr4u2d *msg_2d)
+{
+
+	printf("\n{");
+	printf("\n    \"dram_type\": \"%s\",", dram_types_str[input->basic.dram_type]);
+	printf("\n    \"dimm_type\": \"%s\",", dimm_types_str[input->basic.dimm_type]);
+	printf("\n    \"hard_macro_ver\": \"%d\",", input->basic.hard_macro_ver);
+	printf("\n    \"num_dbyte\": \"0x%04x\",", (unsigned int)input->basic.num_dbyte);
+	printf("\n    \"num_active_dbyte_dfi0\": \"0x%04x\",", (unsigned int)input->basic.num_active_dbyte_dfi0);
+	printf("\n    \"num_anib\": \"0x%04x\",", (unsigned int)input->basic.num_anib);
+	printf("\n    \"num_rank_dfi0\": \"0x%04x\",", (unsigned int)input->basic.num_rank_dfi0);
+	printf("\n    \"num_pstates\": \"0x%04x\",", (unsigned int)input->basic.num_pstates);
+	printf("\n    \"frequency\": \"%d\",", input->basic.frequency);
+	printf("\n    \"pll_bypass\": \"0x%04x\",", (unsigned int)input->basic.dfi_freq_ratio);
+	printf("\n    \"dfi_freq_ratio\": \"0x%04x\",", (unsigned int)input->basic.dfi_freq_ratio);
+	printf("\n    \"dfi1_exists\":  \"0x%04x\",", (unsigned int)input->basic.dfi1exists);
+	printf("\n    \"dram_data_width\": \"0x%04x\",", (unsigned int)input->basic.dram_data_width);
+	printf("\n    \"dram_byte_swap\": \"0x%04x\",", (unsigned int)input->adv.dram_byte_swap);
+	printf("\n    \"ext_cal_res_val\": \"0x%04x\",", (unsigned int)input->adv.ext_cal_res_val);
+	printf("\n    \"tx_slew_rise_dq\": \"0x%04x\",", (unsigned int)input->adv.tx_slew_rise_dq);
+	printf("\n    \"tx_slew_fall_dq\": \"0x%04x\",", (unsigned int)input->adv.tx_slew_fall_dq);
+	printf("\n    \"tx_slew_rise_ac\": \"0x%04x\",", (unsigned int)input->adv.tx_slew_rise_ac);
+	printf("\n    \"tx_slew_fall_ac\": \"0x%04x\",", (unsigned int)input->adv.tx_slew_fall_ac);
+	printf("\n    \"odt_impedance\": \"%d\",", input->adv.odtimpedance);
+	printf("\n    \"tx_impedance\": \"%d\",", input->adv.tx_impedance);
+	printf("\n    \"atx_impedance\": \"%d\",", input->adv.atx_impedance);
+	printf("\n    \"mem_alert_en\": \"0x%04x\",", (unsigned int)input->adv.mem_alert_en);
+	printf("\n    \"mem_alert_pu_imp\": \"0x%04x\",", (unsigned int)input->adv.mem_alert_puimp);
+	printf("\n    \"mem_alert_vref_level\": \"0x%04x\",", (unsigned int)input->adv.mem_alert_vref_level);
+	printf("\n    \"mem_alert_sync_bypass\": \"0x%04x\",", (unsigned int)input->adv.mem_alert_sync_bypass);
+	printf("\n    \"cal_interval\": \"0x%04x\",", (unsigned int)input->adv.cal_interval);
+	printf("\n    \"cal_once\": \"0x%04x\",", (unsigned int)input->adv.cal_once);
+	printf("\n    \"dis_dyn_adr_tri\": \"0x%04x\",", (unsigned int)input->adv.dis_dyn_adr_tri);
+	printf("\n    \"is2t_timing\": \"0x%04x\",", (unsigned int)input->adv.is2ttiming);
+	printf("\n    \"d4rx_preabmle_length\": \"0x%04x\",", (unsigned int)input->adv.d4rx_preamble_length);
+	printf("\n    \"d4tx_preamble_length\": \"0x%04x\",", (unsigned int)input->adv.d4tx_preamble_length);
+	printf("\n    \"msg_misc\": \"0x%02x\",", (unsigned int)msg_1d->msg_misc);
+	printf("\n    \"reserved00\": \"0x%01x\",", (unsigned int)msg_1d->reserved00);
+	printf("\n    \"hdt_ctrl\": \"0x%02x\",", (unsigned int)msg_1d->hdt_ctrl);
+	printf("\n    \"cs_present\": \"0x%02x\",", (unsigned int)msg_1d->cs_present);
+	printf("\n    \"phy_vref\": \"0x%02x\",", (unsigned int)msg_1d->phy_vref);
+	printf("\n    \"dfi_mrl_margin\": \"0x%02x\",", (unsigned int)msg_1d->dfimrlmargin);
+	printf("\n    \"addr_mirror\": \"0x%02x\",", (unsigned int)msg_1d->addr_mirror);
+	printf("\n    \"wr_odt_pat_rank0\": \"0x%02x\",", (unsigned int)(msg_1d->acsm_odt_ctrl0 & 0x0f));
+	printf("\n    \"wr_odt_pat_rank1\": \"0x%02x\",", (unsigned int)(msg_1d->acsm_odt_ctrl1 & 0x0f));
+	printf("\n    \"wr_odt_pat_rank2\": \"0x%02x\",", (unsigned int)(msg_1d->acsm_odt_ctrl2 & 0x0f));
+	printf("\n    \"wr_odt_pat_rank3\": \"0x%02x\",", (unsigned int)(msg_1d->acsm_odt_ctrl3 & 0x0f));
+	printf("\n    \"rd_odt_pat_rank0\": \"0x%02x\",", (unsigned int)(msg_1d->acsm_odt_ctrl0 & 0xf0));
+	printf("\n    \"rd_odt_pat_rank1\": \"0x%02x\",", (unsigned int)(msg_1d->acsm_odt_ctrl1 & 0xf0));
+	printf("\n    \"rd_odt_pat_rank2\": \"0x%02x\",", (unsigned int)(msg_1d->acsm_odt_ctrl2 & 0xf0));
+	printf("\n    \"rd_odt_pat_rank3\": \"0x%02x\",", (unsigned int)(msg_1d->acsm_odt_ctrl3 & 0xf0));
+	printf("\n    \"d4_misc\": \"0x%01x\",", (unsigned int)msg_1d->d4misc);
+	printf("\n    \"share_2d_vref_results\": \"0x%01x\",", (unsigned int)msg_1d->share2dvref_result);
+	printf("\n    \"sequence_ctrl\": \"0x%04x\",", (unsigned int)msg_1d->sequence_ctrl);
+	printf("\n    \"mr0\": \"0x%04x\",", (unsigned int)msg_1d->mr0);
+	printf("\n    \"mr1\": \"0x%04x\",", (unsigned int)msg_1d->mr1);
+	printf("\n    \"mr2\": \"0x%04x\",", (unsigned int)msg_1d->mr2);
+	printf("\n    \"mr3\": \"0x%04x\",", (unsigned int)msg_1d->mr3);
+	printf("\n    \"mr4\": \"0x%04x\",", (unsigned int)msg_1d->mr4);
+	printf("\n    \"mr5\": \"0x%04x\",", (unsigned int)msg_1d->mr5);
+	printf("\n    \"mr6\": \"0x%04x\",", (unsigned int)msg_1d->mr6);
+	printf("\n    \"alt_cal_l\": \"0x%04x\",", (unsigned int)msg_1d->alt_cas_l);
+	printf("\n    \"alt_wcal_l\": \"0x%04x\",", (unsigned int)msg_1d->alt_wcas_l);
+	printf("\n    \"sequence_ctrl_2d\": \"0x%04x\",", (unsigned int)msg_2d->sequence_ctrl);
+	printf("\n    \"rtt_nom_wr_park0\": \"0x%01x\",", (unsigned int)msg_1d->rtt_nom_wr_park0);
+	printf("\n    \"rtt_nom_wr_park1\": \"0x%01x\",", (unsigned int)msg_1d->rtt_nom_wr_park1);
+	printf("\n    \"rtt_nom_wr_park2\": \"0x%01x\",", (unsigned int)msg_1d->rtt_nom_wr_park2);
+	printf("\n    \"rtt_nom_wr_park3\": \"0x%01x\",", (unsigned int)msg_1d->rtt_nom_wr_park3);
+	printf("\n    \"rtt_nom_wr_park4\": \"0x%01x\",", (unsigned int)msg_1d->rtt_nom_wr_park4);
+	printf("\n    \"rtt_nom_wr_park5\": \"0x%01x\",", (unsigned int)msg_1d->rtt_nom_wr_park5);
+	printf("\n    \"rtt_nom_wr_park6\": \"0x%01x\",", (unsigned int)msg_1d->rtt_nom_wr_park6);
+	printf("\n    \"rtt_nom_wr_park7\": \"0x%01x\"", (unsigned int)msg_1d->rtt_nom_wr_park7);
+	printf("\n}");
+	printf("\n");
+}
+#endif
+
+int compute_ddr_phy(struct ddr_info *priv)
+{
+	const unsigned long clk = priv->clk;
+	const struct memctl_opt *popts = &priv->opt;
+	const struct ddr_conf *conf = &priv->conf;
+	const struct dimm_params *dimm_param = &priv->dimm;
+	struct ddr_cfg_regs *regs = &priv->ddr_reg;
+	int ret;
+	static struct input input;
+	static struct ddr4u1d msg_1d;
+	static struct ddr4u2d msg_2d;
+	unsigned int i;
+	unsigned int odt_rd, odt_wr;
+	__unused const soc_info_t *soc_info;
+#ifdef NXP_APPLY_MAX_CDD
+	unsigned int tcfg0, tcfg4, rank;
+#endif
+
+	if (dimm_param == NULL) {
+		ERROR("Empty DIMM parameters.\n");
+		return -EINVAL;
+	}
+
+	zeromem(&input, sizeof(input));
+	zeromem(&msg_1d, sizeof(msg_1d));
+	zeromem(&msg_2d, sizeof(msg_2d));
+
+	input.basic.dram_type = DDR4;
+	/* FIXME: Add condition for LRDIMM */
+	input.basic.dimm_type = (dimm_param->rdimm != 0) ? RDIMM : UDIMM;
+	input.basic.num_dbyte = dimm_param->primary_sdram_width / 8 +
+				 dimm_param->ec_sdram_width / 8;
+	input.basic.num_active_dbyte_dfi0 = input.basic.num_dbyte;
+	input.basic.num_rank_dfi0 = dimm_param->n_ranks;
+	input.basic.dram_data_width = dimm_param->device_width;
+	input.basic.hard_macro_ver	= 0xa;
+	input.basic.num_pstates	= 1;
+	input.basic.dfi_freq_ratio	= 1;
+	input.basic.num_anib		= 0xc;
+	input.basic.train2d		= popts->skip2d ? 0 : 1;
+	input.basic.frequency = (int) (clk / 2000000ul);
+	debug("frequency = %dMHz\n", input.basic.frequency);
+	input.cs_d0 = conf->cs_on_dimm[0];
+#if DDRC_NUM_DIMM > 1
+	input.cs_d1 = conf->cs_on_dimm[1];
+#endif
+	input.mirror = dimm_param->mirrored_dimm;
+	input.mr[0] = regs->sdram_mode[0] & U(0xffff);
+	input.mr[1] = regs->sdram_mode[0] >> 16U;
+	input.mr[2] = regs->sdram_mode[1] >> 16U;
+	input.mr[3] = regs->sdram_mode[1] & U(0xffff);
+	input.mr[4] = regs->sdram_mode[8] >> 16U;
+	input.mr[5] = regs->sdram_mode[8] & U(0xffff);
+	input.mr[6] = regs->sdram_mode[9] >> 16U;
+	input.vref = popts->vref_phy;
+	debug("Vref_phy = %d percent\n", (input.vref * 100U) >> 7U);
+	for (i = 0U; i < DDRC_NUM_CS; i++) {
+		if ((regs->cs[i].config & SDRAM_CS_CONFIG_EN) == 0U) {
+			continue;
+		}
+		odt_rd = (regs->cs[i].config >> 20U) & U(0x7);
+		odt_wr = (regs->cs[i].config >> 16U) & U(0x7);
+		parse_odt(odt_rd, true, i, input.cs_d0, input.cs_d1,
+			   input.odt);
+		parse_odt(odt_wr, false, i, input.cs_d0, input.cs_d1,
+			   input.odt);
+	}
+
+	/* Do not set sdram_cfg[RD_EN] or sdram_cfg2[RCW_EN] for RDIMM */
+	if (dimm_param->rdimm != 0U) {
+		regs->sdram_cfg[0] &= ~(1 << 28U);
+		regs->sdram_cfg[1] &= ~(1 << 2U);
+		input.rcw[0] = (regs->sdram_rcw[0] >> 28U) & U(0xf);
+		input.rcw[1] = (regs->sdram_rcw[0] >> 24U) & U(0xf);
+		input.rcw[2] = (regs->sdram_rcw[0] >> 20U) & U(0xf);
+		input.rcw[3] = (regs->sdram_rcw[0] >> 16U) & U(0xf);
+		input.rcw[4] = (regs->sdram_rcw[0] >> 12U) & U(0xf);
+		input.rcw[5] = (regs->sdram_rcw[0] >> 8U) & U(0xf);
+		input.rcw[6] = (regs->sdram_rcw[0] >> 4U) & U(0xf);
+		input.rcw[7] = (regs->sdram_rcw[0] >> 0U) & U(0xf);
+		input.rcw[8] = (regs->sdram_rcw[1] >> 28U) & U(0xf);
+		input.rcw[9] = (regs->sdram_rcw[1] >> 24U) & U(0xf);
+		input.rcw[10] = (regs->sdram_rcw[1] >> 20U) & U(0xf);
+		input.rcw[11] = (regs->sdram_rcw[1] >> 16U) & U(0xf);
+		input.rcw[12] = (regs->sdram_rcw[1] >> 12U) & U(0xf);
+		input.rcw[13] = (regs->sdram_rcw[1] >> 8U) & U(0xf);
+		input.rcw[14] = (regs->sdram_rcw[1] >> 4U) & U(0xf);
+		input.rcw[15] = (regs->sdram_rcw[1] >> 0U) & U(0xf);
+		input.rcw3x = (regs->sdram_rcw[2] >> 8U) & U(0xff);
+	}
+
+	input.adv.odtimpedance = popts->odt ? popts->odt : 60;
+	input.adv.tx_impedance = popts->phy_tx_impedance ?
+					popts->phy_tx_impedance : 28;
+	input.adv.atx_impedance = popts->phy_atx_impedance ?
+					popts->phy_atx_impedance : 30;
+
+	debug("Initializing input adv data structure\n");
+	phy_gen2_init_input(&input);
+
+	debug("Initializing message block\n");
+	ret = phy_gen2_msg_init(&msg_1d, &msg_2d, &input);
+	if (ret != 0) {
+		ERROR("Init msg failed (error code %d)\n", ret);
+		return ret;
+	}
+
+	ret = c_init_phy_config(priv->phy, priv->ip_rev, &input, &msg_1d);
+	if (ret != 0) {
+		ERROR("Init PHY failed (error code %d)\n", ret);
+		return ret;
+	}
+#ifdef NXP_WARM_BOOT
+	debug("Warm boot flag value %0x\n", priv->warm_boot_flag);
+	if (priv->warm_boot_flag == DDR_WARM_BOOT) {
+		debug("Restoring the Phy training data\n");
+		// Restore the training data
+		ret = restore_phy_training_values(priv->phy,
+						  PHY_TRAINING_REGS_ON_FLASH,
+						  priv->num_ctlrs,
+						  input.basic.train2d);
+		if (ret != 0) {
+			ERROR("Restoring of training data failed %d\n", ret);
+			return ret;
+		}
+	} else {
+#endif
+		/* Mapping IMG buffer firstly */
+		ret = mmap_add_dynamic_region(priv->phy_gen2_fw_img_buf,
+			priv->phy_gen2_fw_img_buf,
+			PHY_GEN2_MAX_IMAGE_SIZE,
+			MT_MEMORY | MT_RW | MT_SECURE);
+		if (ret != 0) {
+			ERROR("Failed to add dynamic memory region.\n");
+			return ret;
+		}
+
+		debug("Load 1D firmware\n");
+		ret = load_fw(priv->phy, &input, 0, &msg_1d,
+			      sizeof(struct ddr4u1d), priv->phy_gen2_fw_img_buf,
+					priv->img_loadr, priv->warm_boot_flag);
+		if (ret != 0) {
+			ERROR("Loading firmware failed (error code %d)\n", ret);
+			return ret;
+		}
+
+		debug("Execute firmware\n");
+		ret = g_exec_fw(priv->phy, 0, &input);
+		if (ret != 0) {
+			ERROR("Execution FW failed (error code %d)\n", ret);
+		}
+
+#ifdef NXP_APPLY_MAX_CDD
+		soc_info = get_soc_info();
+		if (soc_info->svr_reg.bf.maj_ver == 2) {
+			tcfg0 = regs->timing_cfg[0];
+			tcfg4 = regs->timing_cfg[4];
+			rank = findrank(conf->cs_in_use);
+			get_cdd_val(priv->phy, rank, input.basic.frequency,
+					&tcfg0, &tcfg4);
+			regs->timing_cfg[0] = tcfg0;
+			regs->timing_cfg[4] = tcfg4;
+		}
+#endif
+
+		if ((ret == 0) && (input.basic.train2d != 0)) {
+			/* 2D training starts here */
+			debug("Load 2D firmware\n");
+			ret = load_fw(priv->phy, &input, 1, &msg_2d,
+				      sizeof(struct ddr4u2d),
+				      priv->phy_gen2_fw_img_buf,
+				      priv->img_loadr,
+				      priv->warm_boot_flag);
+			if (ret != 0) {
+				ERROR("Loading fw failed (err code %d)\n", ret);
+			} else {
+				debug("Execute 2D firmware\n");
+				ret = g_exec_fw(priv->phy, 1, &input);
+				if (ret != 0) {
+					ERROR("Execution FW failed (err %d)\n",
+					       ret);
+				}
+			}
+		}
+#ifdef NXP_WARM_BOOT
+		if (priv->warm_boot_flag != DDR_WRM_BOOT_NT_SUPPORTED &&
+		    ret == 0) {
+			debug("save the phy training data\n");
+			//Save training data TBD
+			ret = save_phy_training_values(priv->phy,
+						PHY_TRAINING_REGS_ON_FLASH,
+						priv->num_ctlrs,
+						input.basic.train2d);
+			if (ret != 0) {
+				ERROR("Saving training data failed.");
+				ERROR("Warm boot will fail. Error=%d.\n", ret);
+			}
+		}
+	} /* else */
+#endif
+
+	if (ret == 0) {
+		debug("Load PIE\n");
+		i_load_pie(priv->phy, &input, &msg_1d);
+
+		NOTICE("DDR4 %s with %d-rank %d-bit bus (x%d)\n",
+		       input.basic.dimm_type == RDIMM ? "RDIMM" :
+		       input.basic.dimm_type == LRDIMM ? "LRDIMM" :
+		       "UDIMM",
+		       dimm_param->n_ranks,
+		       dimm_param->primary_sdram_width,
+		       dimm_param->device_width);
+	}
+#ifdef DEBUG_DDR_INPUT_CONFIG
+	print_jason_format(&input, &msg_1d, &msg_2d);
+#endif
+
+	return ret;
+}
diff --git a/drivers/nxp/ddr/phy-gen2/phy.h b/drivers/nxp/ddr/phy-gen2/phy.h
new file mode 100644
index 0000000..15e80d1
--- /dev/null
+++ b/drivers/nxp/ddr/phy-gen2/phy.h
@@ -0,0 +1,334 @@
+/*
+ * Copyright 2021 NXP
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#if !defined(PHY_H) && defined(NXP_WARM_BOOT)
+#define PHY_H
+
+#include <flash_info.h>
+
+/* To store sector size to be erase on flash*/
+#define PHY_ERASE_SIZE F_SECTOR_ERASE_SZ
+
+/*Structure to implement address-data map tuples to store PHY training values*/
+struct phy_training_values {
+	uint32_t addr;
+	uint16_t data;
+};
+/* Saves PHY Training Register values after cold reset
+ *@param[in] phy_ptr array to store addresses of PHYs
+ *@param[in] address_to_store address to save PHY training register values
+ *on flash
+ *@param[in] num_of_phy the number of PHY for which training values are
+ *to be saved
+ *@param[in] train2d flag to store whether 2D training registers are to
+ *be saved or not
+ *
+ *PHY training values will be stored on flash at contigous memory in the order:
+ *1D training registers, 2D training registers
+ *for each PHY
+ *
+ *if train2d is false saving 2D training registers will be skipped
+ */
+int save_phy_training_values(uint16_t **phy_ptr, uint32_t address_to_store,
+		uint32_t num_of_phy, int train2d);
+
+/*Restores PHY Training Register values after warm reset
+ *@param[in] phy_ptr array to store addresses of PHYs
+ *@param[in] address_to_store address to retrieve PHY training register
+ *values from flash
+ *@param[in] num_of_phy the number of PHY for which training values are
+ *to be restored
+ *@param[in] train2d flag to store whether 2D training registers are
+ *to be restored or not
+ *
+ *if train2d is false saving 2D training registers will be skipped
+ */
+
+int restore_phy_training_values(uint16_t **phy_ptr, uint32_t address_to_restore,
+		uint32_t num_of_phy, int train2d);
+
+/*
+ * Address data tuples to store the PHY 1D
+ */
+
+struct phy_training_values training_1D_values[] = {
+	{0x200B2, 0},	{0x200CB, 0},	{0x10043, 0},	{0x11043, 0},
+	{0x12043, 0},	{0x13043, 0},	{0x14043, 0},	{0x15043, 0},
+	{0x16043, 0},	{0x17043, 0},	{0x18043, 0},	{0x10143, 0},
+	{0x11143, 0},	{0x12143, 0},	{0x13143, 0},	{0x14143, 0},
+	{0x15143, 0},	{0x16143, 0},	{0x17143, 0},	{0x18143, 0},
+	{0x10080, 0},	{0x11080, 0},	{0x12080, 0},	{0x13080, 0},
+	{0x14080, 0},	{0x15080, 0},	{0x16080, 0},	{0x17080, 0},
+	{0x18080, 0},	{0x10180, 0},	{0x11180, 0},	{0x12180, 0},
+	{0x13180, 0},	{0x14180, 0},	{0x15180, 0},	{0x16180, 0},
+	{0x17180, 0},	{0x18180, 0},	{0x10081, 0},	{0x11081, 0},
+	{0x12081, 0},	{0x13081, 0},	{0x14081, 0},	{0x15081, 0},
+	{0x16081, 0},	{0x17081, 0},	{0x18081, 0},	{0x10181, 0},
+	{0x11181, 0},	{0x12181, 0},	{0x13181, 0},	{0x14181, 0},
+	{0x15181, 0},	{0x16181, 0},	{0x17181, 0},	{0x18181, 0},
+	{0x10082, 0},	{0x11082, 0},	{0x12082, 0},	{0x13082, 0},
+	{0x14082, 0},	{0x15082, 0},	{0x16082, 0},	{0x17082, 0},
+	{0x18082, 0},	{0x10182, 0},	{0x11182, 0},	{0x12182, 0},
+	{0x13182, 0},	{0x14182, 0},	{0x15182, 0},	{0x16182, 0},
+	{0x17182, 0},	{0x18182, 0},	{0x10083, 0},	{0x11083, 0},
+	{0x12083, 0},	{0x13083, 0},	{0x14083, 0},	{0x15083, 0},
+	{0x16083, 0},	{0x17083, 0},	{0x18083, 0},	{0x10183, 0},
+	{0x11183, 0},	{0x12183, 0},	{0x13183, 0},	{0x14183, 0},
+	{0x15183, 0},	{0x16183, 0},	{0x17183, 0},	{0x18183, 0},
+	{0x100D0, 0},	{0x110D0, 0},	{0x120D0, 0},	{0x130D0, 0},
+	{0x140D0, 0},	{0x150D0, 0},	{0x160D0, 0},	{0x170D0, 0},
+	{0x180D0, 0},	{0x101D0, 0},	{0x111D0, 0},	{0x121D0, 0},
+	{0x131D0, 0},	{0x141D0, 0},	{0x151D0, 0},	{0x161D0, 0},
+	{0x171D0, 0},	{0x181D0, 0},	{0x100D1, 0},	{0x110D1, 0},
+	{0x120D1, 0},	{0x130D1, 0},	{0x140D1, 0},	{0x150D1, 0},
+	{0x160D1, 0},	{0x170D1, 0},	{0x180D1, 0},	{0x101D1, 0},
+	{0x111D1, 0},	{0x121D1, 0},	{0x131D1, 0},	{0x141D1, 0},
+	{0x151D1, 0},	{0x161D1, 0},	{0x171D1, 0},	{0x181D1, 0},
+	{0x100D2, 0},	{0x110D2, 0},	{0x120D2, 0},	{0x130D2, 0},
+	{0x140D2, 0},	{0x150D2, 0},	{0x160D2, 0},	{0x170D2, 0},
+	{0x180D2, 0},	{0x101D2, 0},	{0x111D2, 0},	{0x121D2, 0},
+	{0x131D2, 0},	{0x141D2, 0},	{0x151D2, 0},	{0x161D2, 0},
+	{0x171D2, 0},	{0x181D2, 0},	{0x100D3, 0},	{0x110D3, 0},
+	{0x120D3, 0},	{0x130D3, 0},	{0x140D3, 0},	{0x150D3, 0},
+	{0x160D3, 0},	{0x170D3, 0},	{0x180D3, 0},	{0x101D3, 0},
+	{0x111D3, 0},	{0x121D3, 0},	{0x131D3, 0},	{0x141D3, 0},
+	{0x151D3, 0},	{0x161D3, 0},	{0x171D3, 0},	{0x181D3, 0},
+	{0x10068, 0},	{0x11068, 0},	{0x12068, 0},	{0x13068, 0},
+	{0x14068, 0},	{0x15068, 0},	{0x16068, 0},	{0x17068, 0},
+	{0x18068, 0},	{0x10168, 0},	{0x11168, 0},	{0x12168, 0},
+	{0x13168, 0},	{0x14168, 0},	{0x15168, 0},	{0x16168, 0},
+	{0x17168, 0},	{0x18168, 0},	{0x10268, 0},	{0x11268, 0},
+	{0x12268, 0},	{0x13268, 0},	{0x14268, 0},	{0x15268, 0},
+	{0x16268, 0},	{0x17268, 0},	{0x18268, 0},	{0x10368, 0},
+	{0x11368, 0},	{0x12368, 0},	{0x13368, 0},	{0x14368, 0},
+	{0x15368, 0},	{0x16368, 0},	{0x17368, 0},	{0x18368, 0},
+	{0x10468, 0},	{0x11468, 0},	{0x12468, 0},	{0x13468, 0},
+	{0x14468, 0},	{0x15468, 0},	{0x16468, 0},	{0x17468, 0},
+	{0x18468, 0},	{0x10568, 0},	{0x11568, 0},	{0x12568, 0},
+	{0x13568, 0},	{0x14568, 0},	{0x15568, 0},	{0x16568, 0},
+	{0x17568, 0},	{0x18568, 0},	{0x10668, 0},	{0x11668, 0},
+	{0x12668, 0},	{0x13668, 0},	{0x14668, 0},	{0x15668, 0},
+	{0x16668, 0},	{0x17668, 0},	{0x18668, 0},	{0x10768, 0},
+	{0x11768, 0},	{0x12768, 0},	{0x13768, 0},	{0x14768, 0},
+	{0x15768, 0},	{0x16768, 0},	{0x17768, 0},	{0x18768, 0},
+	{0x10868, 0},	{0x11868, 0},	{0x12868, 0},	{0x13868, 0},
+	{0x14868, 0},	{0x15868, 0},	{0x16868, 0},	{0x17868, 0},
+	{0x18868, 0},	{0x10069, 0},	{0x11069, 0},	{0x12069, 0},
+	{0x13069, 0},	{0x14069, 0},	{0x15069, 0},	{0x16069, 0},
+	{0x17069, 0},	{0x18069, 0},	{0x10169, 0},	{0x11169, 0},
+	{0x12169, 0},	{0x13169, 0},	{0x14169, 0},	{0x15169, 0},
+	{0x16169, 0},	{0x17169, 0},	{0x18169, 0},	{0x10269, 0},
+	{0x11269, 0},	{0x12269, 0},	{0x13269, 0},	{0x14269, 0},
+	{0x15269, 0},	{0x16269, 0},	{0x17269, 0},	{0x18269, 0},
+	{0x10369, 0},	{0x11369, 0},	{0x12369, 0},	{0x13369, 0},
+	{0x14369, 0},	{0x15369, 0},	{0x16369, 0},	{0x17369, 0},
+	{0x18369, 0},	{0x10469, 0},	{0x11469, 0},	{0x12469, 0},
+	{0x13469, 0},	{0x14469, 0},	{0x15469, 0},	{0x16469, 0},
+	{0x17469, 0},	{0x18469, 0},	{0x10569, 0},	{0x11569, 0},
+	{0x12569, 0},	{0x13569, 0},	{0x14569, 0},	{0x15569, 0},
+	{0x16569, 0},	{0x17569, 0},	{0x18569, 0},	{0x10669, 0},
+	{0x11669, 0},	{0x12669, 0},	{0x13669, 0},	{0x14669, 0},
+	{0x15669, 0},	{0x16669, 0},	{0x17669, 0},	{0x18669, 0},
+	{0x10769, 0},	{0x11769, 0},	{0x12769, 0},	{0x13769, 0},
+	{0x14769, 0},	{0x15769, 0},	{0x16769, 0},	{0x17769, 0},
+	{0x18769, 0},	{0x10869, 0},	{0x11869, 0},	{0x12869, 0},
+	{0x13869, 0},	{0x14869, 0},	{0x15869, 0},	{0x16869, 0},
+	{0x17869, 0},	{0x18869, 0},	{0x1006A, 0},	{0x1106A, 0},
+	{0x1206A, 0},	{0x1306A, 0},	{0x1406A, 0},	{0x1506A, 0},
+	{0x1606A, 0},	{0x1706A, 0},	{0x1806A, 0},	{0x1016A, 0},
+	{0x1116A, 0},	{0x1216A, 0},	{0x1316A, 0},	{0x1416A, 0},
+	{0x1516A, 0},	{0x1616A, 0},	{0x1716A, 0},	{0x1816A, 0},
+	{0x1026A, 0},	{0x1126A, 0},	{0x1226A, 0},	{0x1326A, 0},
+	{0x1426A, 0},	{0x1526A, 0},	{0x1626A, 0},	{0x1726A, 0},
+	{0x1826A, 0},	{0x1036A, 0},	{0x1136A, 0},	{0x1236A, 0},
+	{0x1336A, 0},	{0x1436A, 0},	{0x1536A, 0},	{0x1636A, 0},
+	{0x1736A, 0},	{0x1836A, 0},	{0x1046A, 0},	{0x1146A, 0},
+	{0x1246A, 0},	{0x1346A, 0},	{0x1446A, 0},	{0x1546A, 0},
+	{0x1646A, 0},	{0x1746A, 0},	{0x1846A, 0},	{0x1056A, 0},
+	{0x1156A, 0},	{0x1256A, 0},	{0x1356A, 0},	{0x1456A, 0},
+	{0x1556A, 0},	{0x1656A, 0},	{0x1756A, 0},	{0x1856A, 0},
+	{0x1066A, 0},	{0x1166A, 0},	{0x1266A, 0},	{0x1366A, 0},
+	{0x1466A, 0},	{0x1566A, 0},	{0x1666A, 0},	{0x1766A, 0},
+	{0x1866A, 0},	{0x1076A, 0},	{0x1176A, 0},	{0x1276A, 0},
+	{0x1376A, 0},	{0x1476A, 0},	{0x1576A, 0},	{0x1676A, 0},
+	{0x1776A, 0},	{0x1876A, 0},	{0x1086A, 0},	{0x1186A, 0},
+	{0x1286A, 0},	{0x1386A, 0},	{0x1486A, 0},	{0x1586A, 0},
+	{0x1686A, 0},	{0x1786A, 0},	{0x1886A, 0},	{0x1006B, 0},
+	{0x1106B, 0},	{0x1206B, 0},	{0x1306B, 0},	{0x1406B, 0},
+	{0x1506B, 0},	{0x1606B, 0},	{0x1706B, 0},	{0x1806B, 0},
+	{0x1016B, 0},	{0x1116B, 0},	{0x1216B, 0},	{0x1316B, 0},
+	{0x1416B, 0},	{0x1516B, 0},	{0x1616B, 0},	{0x1716B, 0},
+	{0x1816B, 0},	{0x1026B, 0},	{0x1126B, 0},	{0x1226B, 0},
+	{0x1326B, 0},	{0x1426B, 0},	{0x1526B, 0},	{0x1626B, 0},
+	{0x1726B, 0},	{0x1826B, 0},	{0x1036B, 0},	{0x1136B, 0},
+	{0x1236B, 0},	{0x1336B, 0},	{0x1436B, 0},	{0x1536B, 0},
+	{0x1636B, 0},	{0x1736B, 0},	{0x1836B, 0},	{0x1046B, 0},
+	{0x1146B, 0},	{0x1246B, 0},	{0x1346B, 0},	{0x1446B, 0},
+	{0x1546B, 0},	{0x1646B, 0},	{0x1746B, 0},	{0x1846B, 0},
+	{0x1056B, 0},	{0x1156B, 0},	{0x1256B, 0},	{0x1356B, 0},
+	{0x1456B, 0},	{0x1556B, 0},	{0x1656B, 0},	{0x1756B, 0},
+	{0x1856B, 0},	{0x1066B, 0},	{0x1166B, 0},	{0x1266B, 0},
+	{0x1366B, 0},	{0x1466B, 0},	{0x1566B, 0},	{0x1666B, 0},
+	{0x1766B, 0},	{0x1866B, 0},	{0x1076B, 0},	{0x1176B, 0},
+	{0x1276B, 0},	{0x1376B, 0},	{0x1476B, 0},	{0x1576B, 0},
+	{0x1676B, 0},	{0x1776B, 0},	{0x1876B, 0},	{0x1086B, 0},
+	{0x1186B, 0},	{0x1286B, 0},	{0x1386B, 0},	{0x1486B, 0},
+	{0x1586B, 0},	{0x1686B, 0},	{0x1786B, 0},	{0x1886B, 0},
+	{0x1008C, 0},	{0x1108C, 0},	{0x1208C, 0},	{0x1308C, 0},
+	{0x1408C, 0},	{0x1508C, 0},	{0x1608C, 0},	{0x1708C, 0},
+	{0x1808C, 0},	{0x1018C, 0},	{0x1118C, 0},	{0x1218C, 0},
+	{0x1318C, 0},	{0x1418C, 0},	{0x1518C, 0},	{0x1618C, 0},
+	{0x1718C, 0},	{0x1818C, 0},	{0x1008D, 0},	{0x1108D, 0},
+	{0x1208D, 0},	{0x1308D, 0},	{0x1408D, 0},	{0x1508D, 0},
+	{0x1608D, 0},	{0x1708D, 0},	{0x1808D, 0},	{0x1018D, 0},
+	{0x1118D, 0},	{0x1218D, 0},	{0x1318D, 0},	{0x1418D, 0},
+	{0x1518D, 0},	{0x1618D, 0},	{0x1718D, 0},	{0x1818D, 0},
+	{0x1008E, 0},	{0x1108E, 0},	{0x1208E, 0},	{0x1308E, 0},
+	{0x1408E, 0},	{0x1508E, 0},	{0x1608E, 0},	{0x1708E, 0},
+	{0x1808E, 0},	{0x1018E, 0},	{0x1118E, 0},	{0x1218E, 0},
+	{0x1318E, 0},	{0x1418E, 0},	{0x1518E, 0},	{0x1618E, 0},
+	{0x1718E, 0},	{0x1818E, 0},	{0x1008F, 0},	{0x1108F, 0},
+	{0x1208F, 0},	{0x1308F, 0},	{0x1408F, 0},	{0x1508F, 0},
+	{0x1608F, 0},	{0x1708F, 0},	{0x1808F, 0},	{0x1018F, 0},
+	{0x1118F, 0},	{0x1218F, 0},	{0x1318F, 0},	{0x1418F, 0},
+	{0x1518F, 0},	{0x1618F, 0},	{0x1718F, 0},	{0x1818F, 0},
+	{0x100C0, 0},	{0x110C0, 0},	{0x120C0, 0},	{0x130C0, 0},
+	{0x140C0, 0},	{0x150C0, 0},	{0x160C0, 0},	{0x170C0, 0},
+	{0x180C0, 0},	{0x101C0, 0},	{0x111C0, 0},	{0x121C0, 0},
+	{0x131C0, 0},	{0x141C0, 0},	{0x151C0, 0},	{0x161C0, 0},
+	{0x171C0, 0},	{0x181C0, 0},	{0x102C0, 0},	{0x112C0, 0},
+	{0x122C0, 0},	{0x132C0, 0},	{0x142C0, 0},	{0x152C0, 0},
+	{0x162C0, 0},	{0x172C0, 0},	{0x182C0, 0},	{0x103C0, 0},
+	{0x113C0, 0},	{0x123C0, 0},	{0x133C0, 0},	{0x143C0, 0},
+	{0x153C0, 0},	{0x163C0, 0},	{0x173C0, 0},	{0x183C0, 0},
+	{0x104C0, 0},	{0x114C0, 0},	{0x124C0, 0},	{0x134C0, 0},
+	{0x144C0, 0},	{0x154C0, 0},	{0x164C0, 0},	{0x174C0, 0},
+	{0x184C0, 0},	{0x105C0, 0},	{0x115C0, 0},	{0x125C0, 0},
+	{0x135C0, 0},	{0x145C0, 0},	{0x155C0, 0},	{0x165C0, 0},
+	{0x175C0, 0},	{0x185C0, 0},	{0x106C0, 0},	{0x116C0, 0},
+	{0x126C0, 0},	{0x136C0, 0},	{0x146C0, 0},	{0x156C0, 0},
+	{0x166C0, 0},	{0x176C0, 0},	{0x186C0, 0},	{0x107C0, 0},
+	{0x117C0, 0},	{0x127C0, 0},	{0x137C0, 0},	{0x147C0, 0},
+	{0x157C0, 0},	{0x167C0, 0},	{0x177C0, 0},	{0x187C0, 0},
+	{0x108C0, 0},	{0x118C0, 0},	{0x128C0, 0},	{0x138C0, 0},
+	{0x148C0, 0},	{0x158C0, 0},	{0x168C0, 0},	{0x178C0, 0},
+	{0x188C0, 0},	{0x100C1, 0},	{0x110C1, 0},	{0x120C1, 0},
+	{0x130C1, 0},	{0x140C1, 0},	{0x150C1, 0},	{0x160C1, 0},
+	{0x170C1, 0},	{0x180C1, 0},	{0x101C1, 0},	{0x111C1, 0},
+	{0x121C1, 0},	{0x131C1, 0},	{0x141C1, 0},	{0x151C1, 0},
+	{0x161C1, 0},	{0x171C1, 0},	{0x181C1, 0},	{0x102C1, 0},
+	{0x112C1, 0},	{0x122C1, 0},	{0x132C1, 0},	{0x142C1, 0},
+	{0x152C1, 0},	{0x162C1, 0},	{0x172C1, 0},	{0x182C1, 0},
+	{0x103C1, 0},	{0x113C1, 0},	{0x123C1, 0},	{0x133C1, 0},
+	{0x143C1, 0},	{0x153C1, 0},	{0x163C1, 0},	{0x173C1, 0},
+	{0x183C1, 0},	{0x104C1, 0},	{0x114C1, 0},	{0x124C1, 0},
+	{0x134C1, 0},	{0x144C1, 0},	{0x154C1, 0},	{0x164C1, 0},
+	{0x174C1, 0},	{0x184C1, 0},	{0x105C1, 0},	{0x115C1, 0},
+	{0x125C1, 0},	{0x135C1, 0},	{0x145C1, 0},	{0x155C1, 0},
+	{0x165C1, 0},	{0x175C1, 0},	{0x185C1, 0},	{0x106C1, 0},
+	{0x116C1, 0},	{0x126C1, 0},	{0x136C1, 0},	{0x146C1, 0},
+	{0x156C1, 0},	{0x166C1, 0},	{0x176C1, 0},	{0x186C1, 0},
+	{0x107C1, 0},	{0x117C1, 0},	{0x127C1, 0},	{0x137C1, 0},
+	{0x147C1, 0},	{0x157C1, 0},	{0x167C1, 0},	{0x177C1, 0},
+	{0x187C1, 0},	{0x108C1, 0},	{0x118C1, 0},	{0x128C1, 0},
+	{0x138C1, 0},	{0x148C1, 0},	{0x158C1, 0},	{0x168C1, 0},
+	{0x178C1, 0},	{0x188C1, 0},	{0x100C2, 0},	{0x110C2, 0},
+	{0x120C2, 0},	{0x130C2, 0},	{0x140C2, 0},	{0x150C2, 0},
+	{0x160C2, 0},	{0x170C2, 0},	{0x180C2, 0},	{0x101C2, 0},
+	{0x111C2, 0},	{0x121C2, 0},	{0x131C2, 0},	{0x141C2, 0},
+	{0x151C2, 0},	{0x161C2, 0},	{0x171C2, 0},	{0x181C2, 0},
+	{0x102C2, 0},	{0x112C2, 0},	{0x122C2, 0},	{0x132C2, 0},
+	{0x142C2, 0},	{0x152C2, 0},	{0x162C2, 0},	{0x172C2, 0},
+	{0x182C2, 0},	{0x103C2, 0},	{0x113C2, 0},	{0x123C2, 0},
+	{0x133C2, 0},	{0x143C2, 0},	{0x153C2, 0},	{0x163C2, 0},
+	{0x173C2, 0},	{0x183C2, 0},	{0x104C2, 0},	{0x114C2, 0},
+	{0x124C2, 0},	{0x134C2, 0},	{0x144C2, 0},	{0x154C2, 0},
+	{0x164C2, 0},	{0x174C2, 0},	{0x184C2, 0},	{0x105C2, 0},
+	{0x115C2, 0},	{0x125C2, 0},	{0x135C2, 0},	{0x145C2, 0},
+	{0x155C2, 0},	{0x165C2, 0},	{0x175C2, 0},	{0x185C2, 0},
+	{0x106C2, 0},	{0x116C2, 0},	{0x126C2, 0},	{0x136C2, 0},
+	{0x146C2, 0},	{0x156C2, 0},	{0x166C2, 0},	{0x176C2, 0},
+	{0x186C2, 0},	{0x107C2, 0},	{0x117C2, 0},	{0x127C2, 0},
+	{0x137C2, 0},	{0x147C2, 0},	{0x157C2, 0},	{0x167C2, 0},
+	{0x177C2, 0},	{0x187C2, 0},	{0x108C2, 0},	{0x118C2, 0},
+	{0x128C2, 0},	{0x138C2, 0},	{0x148C2, 0},	{0x158C2, 0},
+	{0x168C2, 0},	{0x178C2, 0},	{0x188C2, 0},	{0x100C3, 0},
+	{0x110C3, 0},	{0x120C3, 0},	{0x130C3, 0},	{0x140C3, 0},
+	{0x150C3, 0},	{0x160C3, 0},	{0x170C3, 0},	{0x180C3, 0},
+	{0x101C3, 0},	{0x111C3, 0},	{0x121C3, 0},	{0x131C3, 0},
+	{0x141C3, 0},	{0x151C3, 0},	{0x161C3, 0},	{0x171C3, 0},
+	{0x181C3, 0},	{0x102C3, 0},	{0x112C3, 0},	{0x122C3, 0},
+	{0x132C3, 0},	{0x142C3, 0},	{0x152C3, 0},	{0x162C3, 0},
+	{0x172C3, 0},	{0x182C3, 0},	{0x103C3, 0},	{0x113C3, 0},
+	{0x123C3, 0},	{0x133C3, 0},	{0x143C3, 0},	{0x153C3, 0},
+	{0x163C3, 0},	{0x173C3, 0},	{0x183C3, 0},	{0x104C3, 0},
+	{0x114C3, 0},	{0x124C3, 0},	{0x134C3, 0},	{0x144C3, 0},
+	{0x154C3, 0},	{0x164C3, 0},	{0x174C3, 0},	{0x184C3, 0},
+	{0x105C3, 0},	{0x115C3, 0},	{0x125C3, 0},	{0x135C3, 0},
+	{0x145C3, 0},	{0x155C3, 0},	{0x165C3, 0},	{0x175C3, 0},
+	{0x185C3, 0},	{0x106C3, 0},	{0x116C3, 0},	{0x126C3, 0},
+	{0x136C3, 0},	{0x146C3, 0},	{0x156C3, 0},	{0x166C3, 0},
+	{0x176C3, 0},	{0x186C3, 0},	{0x107C3, 0},	{0x117C3, 0},
+	{0x127C3, 0},	{0x137C3, 0},	{0x147C3, 0},	{0x157C3, 0},
+	{0x167C3, 0},	{0x177C3, 0},	{0x187C3, 0},	{0x108C3, 0},
+	{0x118C3, 0},	{0x128C3, 0},	{0x138C3, 0},	{0x148C3, 0},
+	{0x158C3, 0},	{0x168C3, 0},	{0x178C3, 0},	{0x188C3, 0},
+	{0x10020, 0},	{0x11020, 0},	{0x12020, 0},	{0x13020, 0},
+	{0x14020, 0},	{0x15020, 0},	{0x16020, 0},	{0x17020, 0},
+	{0x18020, 0},	{0x2007D, 0},	{0x20077, 0}
+};
+
+/*
+ *Array to store the PHY 2D Training register addresses
+ */
+struct phy_training_values training_2D_values[] = {
+	{0x1008C, 0},   {0x1108C, 0},   {0x1208C, 0},   {0x1308C, 0},
+	{0x1408C, 0},   {0x1508C, 0},   {0x1608C, 0},   {0x1708C, 0},
+	{0x1808C, 0},   {0x1018C, 0},   {0x1118C, 0},   {0x1218C, 0},
+	{0x1318C, 0},   {0x1418C, 0},   {0x1518C, 0},   {0x1618C, 0},
+	{0x1718C, 0},   {0x1818C, 0},   {0x10040, 0},   {0x11040, 0},
+	{0x12040, 0},   {0x13040, 0},   {0x14040, 0},   {0x15040, 0},
+	{0x16040, 0},   {0x17040, 0},   {0x18040, 0},   {0x10140, 0},
+	{0x11140, 0},   {0x12140, 0},   {0x13140, 0},   {0x14140, 0},
+	{0x15140, 0},   {0x16140, 0},   {0x17140, 0},   {0x18140, 0},
+	{0x10240, 0},   {0x11240, 0},   {0x12240, 0},   {0x13240, 0},
+	{0x14240, 0},   {0x15240, 0},   {0x16240, 0},   {0x17240, 0},
+	{0x18240, 0},   {0x10340, 0},   {0x11340, 0},   {0x12340, 0},
+	{0x13340, 0},   {0x14340, 0},   {0x15340, 0},   {0x16340, 0},
+	{0x17340, 0},   {0x18340, 0},   {0x10440, 0},   {0x11440, 0},
+	{0x12440, 0},   {0x13440, 0},   {0x14440, 0},   {0x15440, 0},
+	{0x16440, 0},   {0x17440, 0},   {0x18440, 0},   {0x10540, 0},
+	{0x11540, 0},   {0x12540, 0},   {0x13540, 0},   {0x14540, 0},
+	{0x15540, 0},   {0x16540, 0},   {0x17540, 0},   {0x18540, 0},
+	{0x10640, 0},   {0x11640, 0},   {0x12640, 0},   {0x13640, 0},
+	{0x14640, 0},   {0x15640, 0},   {0x16640, 0},   {0x17640, 0},
+	{0x18640, 0},   {0x10740, 0},   {0x11740, 0},   {0x12740, 0},
+	{0x13740, 0},   {0x14740, 0},   {0x15740, 0},   {0x16740, 0},
+	{0x17740, 0},   {0x18740, 0},   {0x10840, 0},   {0x11840, 0},
+	{0x12840, 0},   {0x13840, 0},   {0x14840, 0},   {0x15840, 0},
+	{0x16840, 0},   {0x17840, 0},   {0x18840, 0},   {0x10030, 0},
+	{0x11030, 0},   {0x12030, 0},   {0x13030, 0},   {0x14030, 0},
+	{0x15030, 0},   {0x16030, 0},   {0x17030, 0},   {0x18030, 0},
+	{0x10130, 0},   {0x11130, 0},   {0x12130, 0},   {0x13130, 0},
+	{0x14130, 0},   {0x15130, 0},   {0x16130, 0},   {0x17130, 0},
+	{0x18130, 0},   {0x10230, 0},   {0x11230, 0},   {0x12230, 0},
+	{0x13230, 0},   {0x14230, 0},   {0x15230, 0},   {0x16230, 0},
+	{0x17230, 0},   {0x18230, 0},   {0x10330, 0},   {0x11330, 0},
+	{0x12330, 0},   {0x13330, 0},   {0x14330, 0},   {0x15330, 0},
+	{0x16330, 0},   {0x17330, 0},   {0x18330, 0},   {0x10430, 0},
+	{0x11430, 0},   {0x12430, 0},   {0x13430, 0},   {0x14430, 0},
+	{0x15430, 0},   {0x16430, 0},   {0x17430, 0},   {0x18430, 0},
+	{0x10530, 0},   {0x11530, 0},   {0x12530, 0},   {0x13530, 0},
+	{0x14530, 0},   {0x15530, 0},   {0x16530, 0},   {0x17530, 0},
+	{0x18530, 0},   {0x10630, 0},   {0x11630, 0},   {0x12630, 0},
+	{0x13630, 0},   {0x14630, 0},   {0x15630, 0},   {0x16630, 0},
+	{0x17630, 0},   {0x18630, 0},   {0x10730, 0},   {0x11730, 0},
+	{0x12730, 0},   {0x13730, 0},   {0x14730, 0},   {0x15730, 0},
+	{0x16730, 0},   {0x17730, 0},   {0x18730, 0},   {0x10830, 0},
+	{0x11830, 0},   {0x12830, 0},   {0x13830, 0},   {0x14830, 0},
+	{0x15830, 0},   {0x16830, 0},   {0x17830, 0},   {0x18830, 0}
+};
+
+#endif
diff --git a/drivers/nxp/ddr/phy-gen2/pie.h b/drivers/nxp/ddr/phy-gen2/pie.h
new file mode 100644
index 0000000..b89066a
--- /dev/null
+++ b/drivers/nxp/ddr/phy-gen2/pie.h
@@ -0,0 +1,632 @@
+/*
+ * Copyright 2021 NXP
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef PIE_H
+#define PIE_H
+
+struct pie {
+	uint32_t addr;
+	uint16_t data;
+};
+
+static const struct pie pie_udimm[] = {
+	{0x90000, 0x10},
+	{0x90001, 0x400},
+	{0x90002, 0x10e},
+	{0x90003, 0x0},
+	{0x90004, 0x0},
+	{0x90005, 0x8},
+	{0x90029, 0xb},
+	{0x9002a, 0x480},
+	{0x9002b, 0x109},
+	{0x9002c, 0x8},
+	{0x9002d, 0x448},
+	{0x9002e, 0x139},
+	{0x9002f, 0x8},
+	{0x90030, 0x478},
+	{0x90031, 0x109},
+	{0x90032, 0x2},
+	{0x90033, 0x10},
+	{0x90034, 0x139},
+	{0x90035, 0xb},
+	{0x90036, 0x7c0},
+	{0x90037, 0x139},
+	{0x90038, 0x44},
+	{0x90039, 0x633},
+	{0x9003a, 0x159},
+	{0x9003b, 0x14f},
+	{0x9003c, 0x630},
+	{0x9003d, 0x159},
+	{0x9003e, 0x47},
+	{0x9003f, 0x633},
+	{0x90040, 0x149},
+	{0x90041, 0x4f},
+	{0x90042, 0x633},
+	{0x90043, 0x179},
+	{0x90044, 0x8},
+	{0x90045, 0xe0},
+	{0x90046, 0x109},
+	{0x90047, 0x0},
+	{0x90048, 0x7c8},
+	{0x90049, 0x109},
+	{0x9004a, 0x0},
+	{0x9004b, 0x1},
+	{0x9004c, 0x8},
+	{0x9004d, 0x0},
+	{0x9004e, 0x45a},
+	{0x9004f, 0x9},
+	{0x90050, 0x0},
+	{0x90051, 0x448},
+	{0x90052, 0x109},
+	{0x90053, 0x40},
+	{0x90054, 0x633},
+	{0x90055, 0x179},
+	{0x90056, 0x1},
+	{0x90057, 0x618},
+	{0x90058, 0x109},
+	{0x90059, 0x40c0},
+	{0x9005a, 0x633},
+	{0x9005b, 0x149},
+	{0x9005c, 0x8},
+	{0x9005d, 0x4},
+	{0x9005e, 0x48},
+	{0x9005f, 0x4040},
+	{0x90060, 0x633},
+	{0x90061, 0x149},
+	{0x90062, 0x0},
+	{0x90063, 0x4},
+	{0x90064, 0x48},
+	{0x90065, 0x40},
+	{0x90066, 0x633},
+	{0x90067, 0x149},
+	{0x90068, 0x10},
+	{0x90069, 0x4},
+	{0x9006a, 0x18},
+	{0x9006b, 0x0},
+	{0x9006c, 0x4},
+	{0x9006d, 0x78},
+	{0x9006e, 0x549},
+	{0x9006f, 0x633},
+	{0x90070, 0x159},
+	{0x90071, 0xd49},
+	{0x90072, 0x633},
+	{0x90073, 0x159},
+	{0x90074, 0x94a},
+	{0x90075, 0x633},
+	{0x90076, 0x159},
+	{0x90077, 0x441},
+	{0x90078, 0x633},
+	{0x90079, 0x149},
+	{0x9007a, 0x42},
+	{0x9007b, 0x633},
+	{0x9007c, 0x149},
+	{0x9007d, 0x1},
+	{0x9007e, 0x633},
+	{0x9007f, 0x149},
+	{0x90080, 0x0},
+	{0x90081, 0xe0},
+	{0x90082, 0x109},
+	{0x90083, 0xa},
+	{0x90084, 0x10},
+	{0x90085, 0x109},
+	{0x90086, 0x9},
+	{0x90087, 0x3c0},
+	{0x90088, 0x149},
+	{0x90089, 0x9},
+	{0x9008a, 0x3c0},
+	{0x9008b, 0x159},
+	{0x9008c, 0x18},
+	{0x9008d, 0x10},
+	{0x9008e, 0x109},
+	{0x9008f, 0x0},
+	{0x90090, 0x3c0},
+	{0x90091, 0x109},
+	{0x90092, 0x18},
+	{0x90093, 0x4},
+	{0x90094, 0x48},
+	{0x90095, 0x18},
+	{0x90096, 0x4},
+	{0x90097, 0x58},
+	{0x90098, 0xb},
+	{0x90099, 0x10},
+	{0x9009a, 0x109},
+	{0x9009b, 0x1},
+	{0x9009c, 0x10},
+	{0x9009d, 0x109},
+	{0x9009e, 0x5},
+	{0x9009f, 0x7c0},
+	{0x900a0, 0x109},
+	{0x900a1, 0x0},
+	{0x900a2, 0x8140},
+	{0x900a3, 0x10c},
+	{0x900a4, 0x10},
+	{0x900a5, 0x8138},
+	{0x900a6, 0x10c},
+	{0x900a7, 0x8},
+	{0x900a8, 0x7c8},
+	{0x900a9, 0x101},
+	{0x900aa, 0x8},
+	{0x900ab, 0x448},
+	{0x900ac, 0x109},
+	{0x900ad, 0xf},
+	{0x900ae, 0x7c0},
+	{0x900af, 0x109},
+	{0x900b0, 0x47},
+	{0x900b1, 0x630},
+	{0x900b2, 0x109},
+	{0x900b3, 0x8},
+	{0x900b4, 0x618},
+	{0x900b5, 0x109},
+	{0x900b6, 0x8},
+	{0x900b7, 0xe0},
+	{0x900b8, 0x109},
+	{0x900b9, 0x0},
+	{0x900ba, 0x7c8},
+	{0x900bb, 0x109},
+	{0x900bc, 0x8},
+	{0x900bd, 0x8140},
+	{0x900be, 0x10c},
+	{0x900bf, 0x0},
+	{0x900c0, 0x478},
+	{0x900c1, 0x109},
+	{0x900c2, 0x0},
+	{0x900c3, 0x1},
+	{0x900c4, 0x8},
+	{0x900c5, 0x8},
+	{0x900c6, 0x4},
+	{0x900c7, 0x8},
+	{0x900c8, 0x8},
+	{0x900c9, 0x7c8},
+	{0x900ca, 0x101},
+	{0x90006, 0x0},
+	{0x90007, 0x0},
+	{0x90008, 0x8},
+	{0x90009, 0x0},
+	{0x9000a, 0x0},
+	{0x9000b, 0x0},
+	{0xd00e7, 0x400},
+	{0x90017, 0x0},
+	{0x90026, 0x2b},
+};
+
+static const struct pie pie_rdimm[] = {
+	{0x90000, 0x10},
+	{0x90001, 0x400},
+	{0x90002, 0x10e},
+	{0x90003, 0x0},
+	{0x90004, 0x0},
+	{0x90005, 0x8},
+	{0x40000, 0x10},
+	{0x40020, 0x0},
+	{0x40040, 0x0},
+	{0x40060, 0x0},
+	{0x40001, 0x70a},
+	{0x40021, 0x7005},
+	{0x40041, 0x0},
+	{0x40061, 0x2001},
+	{0x40002, 0x4010},
+	{0x40022, 0x0},
+	{0x40042, 0x0},
+	{0x40062, 0x0},
+	{0x90029, 0x10},
+	{0x9002a, 0x400},
+	{0x9002b, 0x16e},
+	{0x9002c, 0x8},
+	{0x9002d, 0x370},
+	{0x9002e, 0x169},
+	{0x9002f, 0x8},
+	{0x90030, 0x7aa},
+	{0x90031, 0x6a},
+	{0x90032, 0x10},
+	{0x90033, 0x7b2},
+	{0x90034, 0x6a},
+	{0x90035, 0x0},
+	{0x90036, 0x48a},
+	{0x90037, 0x6a},
+	{0x90038, 0x9},
+	{0x90039, 0x480},
+	{0x9003a, 0x16a},
+	{0x9003b, 0x4},
+	{0x9003c, 0x790},
+	{0x9003d, 0x16a},
+	{0x9003e, 0xc},
+	{0x9003f, 0x408},
+	{0x90040, 0x169},
+	{0x90041, 0xa},
+	{0x90042, 0x0},
+	{0x90043, 0x68},
+	{0x90044, 0x0},
+	{0x90045, 0x408},
+	{0x90046, 0x169},
+	{0x90047, 0x1},
+	{0x90048, 0x480},
+	{0x90049, 0x16a},
+	{0x9004a, 0xb},
+	{0x9004b, 0x480},
+	{0x9004c, 0x109},
+	{0x9004d, 0x8},
+	{0x9004e, 0x448},
+	{0x9004f, 0x139},
+	{0x90050, 0x78},
+	{0x90051, 0x8},
+	{0x90052, 0x139},
+	{0x90053, 0x2},
+	{0x90054, 0x10},
+	{0x90055, 0x139},
+	{0x90056, 0xb},
+	{0x90057, 0x7c0},
+	{0x90058, 0x139},
+	{0x90059, 0x44},
+	{0x9005a, 0x633},
+	{0x9005b, 0x159},
+	{0x9005c, 0x14f},
+	{0x9005d, 0x630},
+	{0x9005e, 0x159},
+	{0x9005f, 0x47},
+	{0x90060, 0x633},
+	{0x90061, 0x149},
+	{0x90062, 0x4f},
+	{0x90063, 0x633},
+	{0x90064, 0x179},
+	{0x90065, 0x8},
+	{0x90066, 0xe0},
+	{0x90067, 0x109},
+	{0x90068, 0x0},
+	{0x90069, 0x7c8},
+	{0x9006a, 0x109},
+	{0x9006b, 0x0},
+	{0x9006c, 0x1},
+	{0x9006d, 0x8},
+	{0x9006e, 0x0},
+	{0x9006f, 0x45a},
+	{0x90070, 0x9},
+	{0x90071, 0x0},
+	{0x90072, 0x448},
+	{0x90073, 0x109},
+	{0x90074, 0x40},
+	{0x90075, 0x633},
+	{0x90076, 0x179},
+	{0x90077, 0x1},
+	{0x90078, 0x618},
+	{0x90079, 0x109},
+	{0x9007a, 0x40c0},
+	{0x9007b, 0x633},
+	{0x9007c, 0x149},
+	{0x9007d, 0x8},
+	{0x9007e, 0x4},
+	{0x9007f, 0x48},
+	{0x90080, 0x4040},
+	{0x90081, 0x633},
+	{0x90082, 0x149},
+	{0x90083, 0x0},
+	{0x90084, 0x4},
+	{0x90085, 0x48},
+	{0x90086, 0x40},
+	{0x90087, 0x633},
+	{0x90088, 0x149},
+	{0x90089, 0x10},
+	{0x9008a, 0x4},
+	{0x9008b, 0x18},
+	{0x9008c, 0x0},
+	{0x9008d, 0x4},
+	{0x9008e, 0x78},
+	{0x9008f, 0x549},
+	{0x90090, 0x633},
+	{0x90091, 0x159},
+	{0x90092, 0xd49},
+	{0x90093, 0x633},
+	{0x90094, 0x159},
+	{0x90095, 0x94a},
+	{0x90096, 0x633},
+	{0x90097, 0x159},
+	{0x90098, 0x441},
+	{0x90099, 0x633},
+	{0x9009a, 0x149},
+	{0x9009b, 0x42},
+	{0x9009c, 0x633},
+	{0x9009d, 0x149},
+	{0x9009e, 0x1},
+	{0x9009f, 0x633},
+	{0x900a0, 0x149},
+	{0x900a1, 0x0},
+	{0x900a2, 0xe0},
+	{0x900a3, 0x109},
+	{0x900a4, 0xa},
+	{0x900a5, 0x10},
+	{0x900a6, 0x109},
+	{0x900a7, 0x9},
+	{0x900a8, 0x3c0},
+	{0x900a9, 0x149},
+	{0x900aa, 0x9},
+	{0x900ab, 0x3c0},
+	{0x900ac, 0x159},
+	{0x900ad, 0x18},
+	{0x900ae, 0x10},
+	{0x900af, 0x109},
+	{0x900b0, 0x0},
+	{0x900b1, 0x3c0},
+	{0x900b2, 0x109},
+	{0x900b3, 0x18},
+	{0x900b4, 0x4},
+	{0x900b5, 0x48},
+	{0x900b6, 0x18},
+	{0x900b7, 0x4},
+	{0x900b8, 0x58},
+	{0x900b9, 0xb},
+	{0x900ba, 0x10},
+	{0x900bb, 0x109},
+	{0x900bc, 0x1},
+	{0x900bd, 0x10},
+	{0x900be, 0x109},
+	{0x900bf, 0x5},
+	{0x900c0, 0x7c0},
+	{0x900c1, 0x109},
+	{0x900c2, 0x3},
+	{0x900c3, 0x370},
+	{0x900c4, 0x169},
+	{0x900c5, 0x3},
+	{0x900c6, 0x8},
+	{0x900c7, 0x139},
+	{0x900c8, 0x0},
+	{0x900c9, 0x400},
+	{0x900ca, 0x16e},
+	{0x900cb, 0x8},
+	{0x900cc, 0x478},
+	{0x900cd, 0x109},
+	{0x900ce, 0x0},
+	{0x900cf, 0x8140},
+	{0x900d0, 0x10c},
+	{0x900d1, 0x10},
+	{0x900d2, 0x8138},
+	{0x900d3, 0x10c},
+	{0x900d4, 0x8},
+	{0x900d5, 0x7c8},
+	{0x900d6, 0x101},
+	{0x900d7, 0x7a},
+	{0x900d8, 0x8},
+	{0x900d9, 0x109},
+	{0x900da, 0x8},
+	{0x900db, 0x448},
+	{0x900dc, 0x109},
+	{0x900dd, 0xf},
+	{0x900de, 0x7c0},
+	{0x900df, 0x109},
+	{0x900e0, 0x47},
+	{0x900e1, 0x630},
+	{0x900e2, 0x109},
+	{0x900e3, 0x8},
+	{0x900e4, 0x618},
+	{0x900e5, 0x109},
+	{0x900e6, 0x8},
+	{0x900e7, 0xe0},
+	{0x900e8, 0x109},
+	{0x900e9, 0x0},
+	{0x900ea, 0x8},
+	{0x900eb, 0x109},
+	{0x900ec, 0x0},
+	{0x900ed, 0x7c8},
+	{0x900ee, 0x109},
+	{0x900ef, 0x8},
+	{0x900f0, 0x8140},
+	{0x900f1, 0x10c},
+	{0x900f2, 0x0},
+	{0x900f3, 0x478},
+	{0x900f4, 0x109},
+	{0x900f5, 0x0},
+	{0x900f6, 0x1},
+	{0x900f7, 0x8},
+	{0x900f8, 0x8},
+	{0x900f9, 0x4},
+	{0x900fa, 0x8},
+	{0x900fb, 0x8},
+	{0x900fc, 0x7c8},
+	{0x900fd, 0x101},
+	{0x90006, 0x0},
+	{0x90007, 0x0},
+	{0x90008, 0x8},
+	{0x90009, 0x0},
+	{0x9000a, 0x0},
+	{0x9000b, 0x0},
+	{0xd00e7, 0x400},
+	{0x90017, 0x0},
+	{0x90026, 0x3a},
+};
+
+static const struct pie pie_lrdimm[] = {
+	{0x90000, 0x10},
+	{0x90001, 0x400},
+	{0x90002, 0x10e},
+	{0x90003, 0x0},
+	{0x90004, 0x0},
+	{0x90005, 0x8},
+	{0x90029, 0xb},
+	{0x9002a, 0x480},
+	{0x9002b, 0x109},
+	{0x9002c, 0x8},
+	{0x9002d, 0x448},
+	{0x9002e, 0x139},
+	{0x9002f, 0x78},
+	{0x90030, 0x8},
+	{0x90031, 0x139},
+	{0x90032, 0x2},
+	{0x90033, 0x10},
+	{0x90034, 0x139},
+	{0x90035, 0xb},
+	{0x90036, 0x7c0},
+	{0x90037, 0x139},
+	{0x90038, 0x44},
+	{0x90039, 0x633},
+	{0x9003a, 0x159},
+	{0x9003b, 0x14f},
+	{0x9003c, 0x630},
+	{0x9003d, 0x159},
+	{0x9003e, 0x47},
+	{0x9003f, 0x633},
+	{0x90040, 0x149},
+	{0x90041, 0x4f},
+	{0x90042, 0x633},
+	{0x90043, 0x179},
+	{0x90044, 0x8},
+	{0x90045, 0xe0},
+	{0x90046, 0x109},
+	{0x90047, 0x0},
+	{0x90048, 0x7c8},
+	{0x90049, 0x109},
+	{0x9004a, 0x0},
+	{0x9004b, 0x1},
+	{0x9004c, 0x8},
+	{0x9004d, 0x0},
+	{0x9004e, 0x45a},
+	{0x9004f, 0x9},
+	{0x90050, 0x0},
+	{0x90051, 0x448},
+	{0x90052, 0x109},
+	{0x90053, 0x40},
+	{0x90054, 0x633},
+	{0x90055, 0x179},
+	{0x90056, 0x1},
+	{0x90057, 0x618},
+	{0x90058, 0x109},
+	{0x90059, 0x40c0},
+	{0x9005a, 0x633},
+	{0x9005b, 0x149},
+	{0x9005c, 0x8},
+	{0x9005d, 0x4},
+	{0x9005e, 0x48},
+	{0x9005f, 0x4040},
+	{0x90060, 0x633},
+	{0x90061, 0x149},
+	{0x90062, 0x0},
+	{0x90063, 0x4},
+	{0x90064, 0x48},
+	{0x90065, 0x40},
+	{0x90066, 0x633},
+	{0x90067, 0x149},
+	{0x90068, 0x10},
+	{0x90069, 0x4},
+	{0x9006a, 0x18},
+	{0x9006b, 0x0},
+	{0x9006c, 0x4},
+	{0x9006d, 0x78},
+	{0x9006e, 0x549},
+	{0x9006f, 0x633},
+	{0x90070, 0x159},
+	{0x90071, 0xd49},
+	{0x90072, 0x633},
+	{0x90073, 0x159},
+	{0x90074, 0x94a},
+	{0x90075, 0x633},
+	{0x90076, 0x159},
+	{0x90077, 0x441},
+	{0x90078, 0x633},
+	{0x90079, 0x149},
+	{0x9007a, 0x42},
+	{0x9007b, 0x633},
+	{0x9007c, 0x149},
+	{0x9007d, 0x1},
+	{0x9007e, 0x633},
+	{0x9007f, 0x149},
+	{0x90080, 0x0},
+	{0x90081, 0xe0},
+	{0x90082, 0x109},
+	{0x90083, 0xa},
+	{0x90084, 0x10},
+	{0x90085, 0x109},
+	{0x90086, 0x9},
+	{0x90087, 0x3c0},
+	{0x90088, 0x149},
+	{0x90089, 0x9},
+	{0x9008a, 0x3c0},
+	{0x9008b, 0x159},
+	{0x9008c, 0x18},
+	{0x9008d, 0x10},
+	{0x9008e, 0x109},
+	{0x9008f, 0x0},
+	{0x90090, 0x3c0},
+	{0x90091, 0x109},
+	{0x90092, 0x18},
+	{0x90093, 0x4},
+	{0x90094, 0x48},
+	{0x90095, 0x18},
+	{0x90096, 0x4},
+	{0x90097, 0x58},
+	{0x90098, 0xb},
+	{0x90099, 0x10},
+	{0x9009a, 0x109},
+	{0x9009b, 0x1},
+	{0x9009c, 0x10},
+	{0x9009d, 0x109},
+	{0x9009e, 0x5},
+	{0x9009f, 0x7c0},
+	{0x900a0, 0x109},
+	{0x900a1, 0x3},
+	{0x900a2, 0x8},
+	{0x900a3, 0x139},
+	{0x900a4, 0x0},
+	{0x900a5, 0x400},
+	{0x900a6, 0x16e},
+	{0x900a7, 0x8},
+	{0x900a8, 0x478},
+	{0x900a9, 0x109},
+	{0x900aa, 0x0},
+	{0x900ab, 0x8140},
+	{0x900ac, 0x10c},
+	{0x900ad, 0x10},
+	{0x900ae, 0x8138},
+	{0x900af, 0x10c},
+	{0x900b0, 0x8},
+	{0x900b1, 0x7c8},
+	{0x900b2, 0x101},
+	{0x900b3, 0x7a},
+	{0x900b4, 0x8},
+	{0x900b5, 0x109},
+	{0x900b6, 0x8},
+	{0x900b7, 0x448},
+	{0x900b8, 0x109},
+	{0x900b9, 0xf},
+	{0x900ba, 0x7c0},
+	{0x900bb, 0x109},
+	{0x900bc, 0x47},
+	{0x900bd, 0x630},
+	{0x900be, 0x109},
+	{0x900bf, 0x8},
+	{0x900c0, 0x618},
+	{0x900c1, 0x109},
+	{0x900c2, 0x8},
+	{0x900c3, 0xe0},
+	{0x900c4, 0x109},
+	{0x900c5, 0x0},
+	{0x900c6, 0x8},
+	{0x900c7, 0x109},
+	{0x900c8, 0x0},
+	{0x900c9, 0x7c8},
+	{0x900ca, 0x109},
+	{0x900cb, 0x8},
+	{0x900cc, 0x8140},
+	{0x900cd, 0x10c},
+	{0x900ce, 0x0},
+	{0x900cf, 0x478},
+	{0x900d0, 0x109},
+	{0x900d1, 0x0},
+	{0x900d2, 0x1},
+	{0x900d3, 0x8},
+	{0x900d4, 0x8},
+	{0x900d5, 0x4},
+	{0x900d6, 0x8},
+	{0x900d7, 0x8},
+	{0x900d8, 0x7c8},
+	{0x900d9, 0x101},
+	{0x90006, 0x0},
+	{0x90007, 0x0},
+	{0x90008, 0x8},
+	{0x90009, 0x0},
+	{0x9000a, 0x0},
+	{0x9000b, 0x0},
+	{0xd00e7, 0x400},
+	{0x90017, 0x0},
+	{0x90026, 0x2e},
+};
+#endif
diff --git a/drivers/nxp/drivers.mk b/drivers/nxp/drivers.mk
new file mode 100644
index 0000000..d77e985
--- /dev/null
+++ b/drivers/nxp/drivers.mk
@@ -0,0 +1,99 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#
+
+###############################################################################
+
+
+PLAT_DRIVERS_PATH		:=	drivers/nxp
+PLAT_DRIVERS_INCLUDE_PATH	:=	include/drivers/nxp
+
+ifeq (${SMMU_NEEDED},yes)
+PLAT_INCLUDES	+= -Iinclude/drivers/nxp/smmu/
+endif
+
+ifeq (${DCFG_NEEDED},yes)
+include $(PLAT_DRIVERS_PATH)/dcfg/dcfg.mk
+endif
+
+ifeq (${CSU_NEEDED},yes)
+include $(PLAT_DRIVERS_PATH)/csu/csu.mk
+endif
+
+ifeq (${TIMER_NEEDED},yes)
+include $(PLAT_DRIVERS_PATH)/timer/timer.mk
+endif
+
+ifeq (${INTERCONNECT_NEEDED},yes)
+include ${PLAT_DRIVERS_PATH}/interconnect/interconnect.mk
+endif
+
+ifeq (${GIC_NEEDED},yes)
+include ${PLAT_DRIVERS_PATH}/gic/gic.mk
+endif
+
+ifeq (${SD_MMC_NEEDED},yes)
+include $(PLAT_DRIVERS_PATH)/sd/sd_mmc.mk
+endif
+
+ifeq (${CONSOLE_NEEDED},yes)
+include $(PLAT_DRIVERS_PATH)/console/console.mk
+endif
+
+ifeq (${SFP_NEEDED},yes)
+include $(PLAT_DRIVERS_PATH)/sfp/sfp.mk
+endif
+
+ifeq (${XSPI_NEEDED},yes)
+include $(PLAT_DRIVERS_PATH)/flexspi/nor/flexspi_nor.mk
+endif
+
+ifeq (${QSPI_NEEDED},yes)
+include $(PLAT_DRIVERS_PATH)/qspi/qspi.mk
+endif
+
+ifeq (${SNVS_NEEDED},yes)
+include $(PLAT_DRIVERS_PATH)/sec_mon/sec_mon.mk
+endif
+
+ifeq ($(I2C_NEEDED),yes)
+$(eval $(call add_define, I2C_INIT))
+include $(PLAT_DRIVERS_PATH)/i2c/i2c.mk
+endif
+
+ifeq ($(DDR_DRIVER_NEEDED),yes)
+$(eval $(call add_define, DDR_INIT))
+# define DDR_CNTRL_SOURCES
+ifeq ($(DDRCNTLR),MMDC)
+include $(PLAT_DRIVERS_PATH)/ddr/fsl-mmdc/ddr.mk
+else
+include $(PLAT_DRIVERS_PATH)/ddr/nxp-ddr/ddr.mk
+endif # DDR_CNTRL_SOURCES
+endif
+
+ifeq (${PMU_NEEDED},yes)
+include $(PLAT_DRIVERS_PATH)/pmu/pmu.mk
+endif
+
+ifeq (${CRYPTO_NEEDED},yes)
+include $(PLAT_DRIVERS_PATH)/crypto/caam/caam.mk
+endif
+
+ifeq (${TZASC_NEEDED},yes)
+include $(PLAT_DRIVERS_PATH)/tzc/tzc.mk
+endif
+
+ifeq (${GPIO_NEEDED},yes)
+include ${PLAT_DRIVERS_PATH}/gpio/gpio.mk
+endif
+
+ifeq (${IFC_NOR_NEEDED},yes)
+include ${PLAT_DRIVERS_PATH}/ifc/nor/ifc_nor.mk
+endif
+
+ifeq (${IFC_NAND_NEEDED},yes)
+include ${PLAT_DRIVERS_PATH}/ifc/nand/ifc_nand.mk
+endif
diff --git a/drivers/nxp/flexspi/nor/flexspi_nor.c b/drivers/nxp/flexspi/nor/flexspi_nor.c
new file mode 100644
index 0000000..748228d
--- /dev/null
+++ b/drivers/nxp/flexspi/nor/flexspi_nor.c
@@ -0,0 +1,25 @@
+/*
+ * Copyright 2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <assert.h>
+
+#include <fspi_api.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+
+int flexspi_nor_io_setup(uintptr_t nxp_flexspi_flash_addr,
+			 size_t nxp_flexspi_flash_size, uint32_t fspi_base_reg_addr)
+{
+	int ret = 0;
+
+	ret = fspi_init(fspi_base_reg_addr, nxp_flexspi_flash_addr);
+	/* Adding NOR Memory Map in XLAT Table */
+	mmap_add_region(nxp_flexspi_flash_addr, nxp_flexspi_flash_addr,
+			nxp_flexspi_flash_size, MT_MEMORY | MT_RW);
+
+	return ret;
+}
diff --git a/drivers/nxp/flexspi/nor/flexspi_nor.h b/drivers/nxp/flexspi/nor/flexspi_nor.h
new file mode 100644
index 0000000..61fc236
--- /dev/null
+++ b/drivers/nxp/flexspi/nor/flexspi_nor.h
@@ -0,0 +1,15 @@
+/*
+ * Copyright 2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef FLEXSPI_NOR_H
+#define FLEXSPI_NOR_H
+
+int flexspi_nor_io_setup(uintptr_t nxp_flexspi_flash_addr,
+			 size_t nxp_flexspi_flash_size,
+			 uint32_t fspi_base_reg_addr);
+
+#endif /*	FLEXSPI_NOR_H	*/
diff --git a/drivers/nxp/flexspi/nor/flexspi_nor.mk b/drivers/nxp/flexspi/nor/flexspi_nor.mk
new file mode 100644
index 0000000..6d9eebb
--- /dev/null
+++ b/drivers/nxp/flexspi/nor/flexspi_nor.mk
@@ -0,0 +1,35 @@
+#
+# Copyright 2020 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${XSPI_NOR},)
+XSPI_NOR	:= 1
+
+FLEXSPI_DRIVERS_PATH	:=  ${PLAT_DRIVERS_PATH}/flexspi/nor
+
+PLAT_XSPI_INCLUDES	+= -I$(FLEXSPI_DRIVERS_PATH)
+
+XSPI_BOOT_SOURCES	+= $(FLEXSPI_DRIVERS_PATH)/flexspi_nor.c	\
+			   ${FLEXSPI_DRIVERS_PATH}/fspi.c
+ifeq ($(DEBUG),1)
+XSPI_BOOT_SOURCES	+= ${FLEXSPI_DRIVERS_PATH}/test_fspi.c
+endif
+
+PLAT_XSPI_INCLUDES	+= -Iinclude/drivers/nxp/flexspi
+
+PLAT_INCLUDES		+= ${PLAT_XSPI_INCLUDES}
+
+ifeq (${BL_COMM_XSPI_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${XSPI_BOOT_SOURCES}
+else
+ifeq (${BL2_XSPI_NEEDED},yes)
+BL2_SOURCES		+= ${XSPI_BOOT_SOURCES}
+endif
+ifeq (${BL31_XSPI_NEEDED},yes)
+BL31_SOURCES		+= ${XSPI_BOOT_SOURCES}
+endif
+endif
+
+endif
diff --git a/drivers/nxp/flexspi/nor/fspi.c b/drivers/nxp/flexspi/nor/fspi.c
new file mode 100644
index 0000000..7c919b8
--- /dev/null
+++ b/drivers/nxp/flexspi/nor/fspi.c
@@ -0,0 +1,853 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * NXP FlexSpi Controller Driver.
+ * Copyright 2021 NXP
+ *
+ */
+#include <endian.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <flash_info.h>
+#include "fspi.h"
+#include <fspi_api.h>
+#include <xspi_error_codes.h>
+
+#ifdef DEBUG_FLEXSPI
+#define PR printf("In [%s][%d]\n", __func__, __LINE__)
+#define PRA(a, b) printf("In [%s][%d] %s="a"\n", __func__, __LINE__, #b, b)
+#else
+#define PR
+#define PRA(a, b)
+#endif
+
+/*
+ * This errata is valid for all NXP SoC.
+ */
+#define ERRATA_FLASH_A050272 1
+
+static uintptr_t fspi_base_reg_addr;
+static uintptr_t fspi_flash_base_addr;
+
+static void fspi_RDSR(uint32_t *, const void *, uint32_t);
+
+static void fspi_writel(uint32_t x_addr, uint32_t x_val)
+{
+	fspi_out32((uint32_t *)(fspi_base_reg_addr + x_addr),
+		 (uint32_t) x_val);
+}
+
+static uint32_t fspi_readl(uint32_t x_addr)
+{
+	return fspi_in32((uint32_t *)(fspi_base_reg_addr + x_addr));
+}
+
+static void fspi_MDIS(uint8_t x_disable)
+{
+	uint32_t ui_reg;
+
+	ui_reg = fspi_readl(FSPI_MCR0);
+	if (x_disable != 0U) {
+		ui_reg |= FSPI_MCR0_MDIS;
+	} else {
+		ui_reg &= (uint32_t) (~FSPI_MCR0_MDIS);
+	}
+
+	fspi_writel(FSPI_MCR0, ui_reg);
+}
+
+static void fspi_lock_LUT(void)
+{
+	fspi_writel(FSPI_LUTKEY, FSPI_LUTKEY_VALUE);
+	VERBOSE("%s 0x%x\n", __func__, fspi_readl(FSPI_LCKCR));
+	fspi_writel(FSPI_LCKCR, FSPI_LCKER_LOCK);
+	VERBOSE("%s 0x%x\n", __func__, fspi_readl(FSPI_LCKCR));
+}
+
+static void fspi_unlock_LUT(void)
+{
+	fspi_writel(FSPI_LUTKEY,  FSPI_LUTKEY_VALUE);
+	VERBOSE("%s 0x%x\n", __func__, fspi_readl(FSPI_LCKCR));
+	fspi_writel(FSPI_LCKCR, FSPI_LCKER_UNLOCK);
+	VERBOSE("%s 0x%x\n", __func__, fspi_readl(FSPI_LCKCR));
+}
+
+static void fspi_op_setup(uint32_t fspi_op_seq_id, bool ignore_flash_sz)
+{
+	uint32_t x_addr, x_instr0 = 0, x_instr1 = 0, x_instr2 = 0;
+	uint32_t cmd_id1, cmd_id2;
+
+	VERBOSE("In func %s\n", __func__);
+
+	switch (fspi_op_seq_id) {
+	case FSPI_READ_SEQ_ID:
+		cmd_id1 = FSPI_NOR_CMD_READ;
+		cmd_id2 = FSPI_NOR_CMD_READ_4B;
+		x_instr2 = FSPI_INSTR_OPRND0(0) | FSPI_INSTR_PAD0(FSPI_LUT_PAD1)
+				| FSPI_INSTR_OPCODE0(FSPI_LUT_READ);
+		break;
+	case FSPI_FASTREAD_SEQ_ID:
+		cmd_id1 = FSPI_NOR_CMD_FASTREAD;
+		cmd_id2 = FSPI_NOR_CMD_FASTREAD_4B;
+		x_instr2 = FSPI_INSTR_OPRND0(8) | FSPI_INSTR_PAD0(FSPI_LUT_PAD1)
+				| FSPI_INSTR_OPCODE0(FSPI_DUMMY_SDR)
+				| FSPI_INSTR_OPRND1(0)
+				| FSPI_INSTR_PAD1(FSPI_LUT_PAD1)
+				| FSPI_INSTR_OPCODE1(FSPI_LUT_READ);
+		break;
+	case FSPI_WRITE_SEQ_ID:
+		cmd_id1 = FSPI_NOR_CMD_PP;
+		cmd_id2 = FSPI_NOR_CMD_PP_4B;
+		x_instr2 = FSPI_INSTR_OPRND0(0) | FSPI_INSTR_PAD0(FSPI_LUT_PAD1)
+				| FSPI_INSTR_OPCODE0(FSPI_LUT_WRITE);
+		break;
+	case FSPI_WREN_SEQ_ID:
+		cmd_id1 = FSPI_NOR_CMD_WREN;
+		cmd_id2 = FSPI_NOR_CMD_WREN;
+		break;
+	case FSPI_SE_SEQ_ID:
+		cmd_id1 = FSPI_NOR_CMD_SE_64K;
+		cmd_id2 = FSPI_NOR_CMD_SE_64K_4B;
+		break;
+	case FSPI_4K_SEQ_ID:
+		cmd_id1 = FSPI_NOR_CMD_SE_4K;
+		cmd_id2 = FSPI_NOR_CMD_SE_4K_4B;
+		break;
+	case FSPI_BE_SEQ_ID:
+		cmd_id1 = FSPI_NOR_CMD_BE;
+		cmd_id2 = FSPI_NOR_CMD_BE;
+		break;
+	case FSPI_RDSR_SEQ_ID:
+		cmd_id1 = FSPI_NOR_CMD_RDSR;
+		cmd_id2 = FSPI_NOR_CMD_RDSR;
+		break;
+	}
+
+	x_addr = FSPI_LUTREG_OFFSET + (uint32_t)(0x10 * fspi_op_seq_id);
+	if ((F_FLASH_SIZE_BYTES <= SZ_16M_BYTES) || (ignore_flash_sz)) {
+		x_instr0 = FSPI_INSTR_OPRND0(cmd_id1);
+		x_instr1 = FSPI_INSTR_OPRND1(FSPI_LUT_ADDR24BIT);
+		VERBOSE("CMD_ID = %x offset = 0x%x\n", cmd_id1, x_addr);
+	} else {
+		x_instr0 = FSPI_INSTR_OPRND0(cmd_id2);
+		x_instr1 = FSPI_INSTR_OPRND1(FSPI_LUT_ADDR32BIT);
+		VERBOSE("CMD_ID = %x offset = 0x%x\n", cmd_id2, x_addr);
+	}
+	x_instr0 |= FSPI_INSTR_PAD0(FSPI_LUT_PAD1)
+		| FSPI_INSTR_OPCODE0(FSPI_LUT_CMD);
+
+	x_instr1 |= FSPI_INSTR_PAD1(FSPI_LUT_PAD1)
+		| FSPI_INSTR_OPCODE1(FSPI_LUT_ADDR);
+
+	if (fspi_op_seq_id == FSPI_RDSR_SEQ_ID) {
+		x_instr0 |= FSPI_INSTR_OPRND1(1) | FSPI_INSTR_PAD1(FSPI_LUT_PAD1)
+					| FSPI_INSTR_OPCODE1(FSPI_LUT_READ);
+	} else if ((fspi_op_seq_id != FSPI_BE_SEQ_ID)
+			&& (fspi_op_seq_id != FSPI_WREN_SEQ_ID)) {
+		x_instr0 |= x_instr1;
+	}
+
+	fspi_writel((x_addr), x_instr0);
+	fspi_writel((x_addr + U(0x4)), x_instr2);
+	fspi_writel((x_addr + U(0x8)), (uint32_t) 0x0);	/* STOP command */
+	fspi_writel((x_addr + U(0xc)), (uint32_t) 0x0);	/* STOP command */
+}
+
+static void fspi_setup_LUT(void)
+{
+	VERBOSE("In func %s\n", __func__);
+	fspi_unlock_LUT();
+
+	/* LUT Setup for READ Command 3-Byte low Frequency */
+	fspi_op_setup(FSPI_READ_SEQ_ID, false);
+
+	/* LUT Setup for FAST READ Command 3-Byte/4-Byte high Frequency */
+	fspi_op_setup(FSPI_FASTREAD_SEQ_ID, false);
+
+	/* LUT Setup for Page Program */
+	fspi_op_setup(FSPI_WRITE_SEQ_ID, false);
+
+	/* LUT Setup for WREN */
+	fspi_op_setup(FSPI_WREN_SEQ_ID, true);
+
+	/* LUT Setup for Sector_Erase */
+	fspi_op_setup(FSPI_SE_SEQ_ID, false);
+
+	/* LUT Setup for Sub Sector 4K Erase */
+	fspi_op_setup(FSPI_4K_SEQ_ID, false);
+
+	/* LUT Setup for Bulk_Erase */
+	fspi_op_setup(FSPI_BE_SEQ_ID, true);
+
+	/* Read Status */
+	fspi_op_setup(FSPI_RDSR_SEQ_ID, true);
+
+	fspi_lock_LUT();
+}
+
+static inline void fspi_ahb_invalidate(void)
+{
+	uint32_t reg;
+
+	VERBOSE("In func %s %d\n", __func__, __LINE__);
+	reg = fspi_readl(FSPI_MCR0);
+	reg |= FSPI_MCR0_SWRST;
+	fspi_writel(FSPI_MCR0, reg);
+	while ((fspi_readl(FSPI_MCR0) & FSPI_MCR0_SWRST) != 0)
+		;  /* FSPI_MCR0_SWRESET_MASK */
+	VERBOSE("In func %s %d\n", __func__, __LINE__);
+}
+
+#if defined(CONFIG_FSPI_AHB)
+static void fspi_init_ahb(void)
+{
+	uint32_t i, x_flash_cr2, seq_id;
+
+	x_flash_cr2 = 0;
+	/* Reset AHB RX buffer CR configuration */
+	for (i = 0; i < 8; i++) {
+		fspi_writel((FSPI_AHBRX_BUF0CR0 + 4 * i), 0U);
+	}
+
+	/* Set ADATSZ with the maximum AHB buffer size */
+	fspi_writel(FSPI_AHBRX_BUF7CR0,
+			((uint32_t) ((FSPI_RX_MAX_AHBBUF_SIZE / 8U) |
+				    FSPI_AHBRXBUF0CR7_PREF)));
+
+	/* Known limitation handling: prefetch and
+	 * no start address alignment.*/
+	fspi_writel(FSPI_AHBCR, FSPI_AHBCR_PREF_EN);
+	INFO("xAhbcr=0x%x\n", fspi_readl(FSPI_AHBCR));
+
+	// Setup AHB READ sequenceID for all flashes.
+	x_flash_cr2 = fspi_readl(FSPI_FLSHA1CR2);
+	INFO("x_flash_cr2=0x%x\n", x_flash_cr2);
+
+	seq_id = CONFIG_FSPI_FASTREAD ?
+			FSPI_FASTREAD_SEQ_ID : FSPI_READ_SEQ_ID;
+	x_flash_cr2 |= ((seq_id << FSPI_FLSHXCR2_ARDSEQI_SHIFT) & 0x1f);
+
+	INFO("x_flash_cr2=0x%x\n", x_flash_cr2);
+
+	fspi_writel(FSPI_FLSHA1CR2,  x_flash_cr2);
+	x_flash_cr2 = fspi_readl(FSPI_FLSHA1CR2);
+	INFO("x_flash_cr2=0x%x\n", x_flash_cr2);
+}
+#endif
+
+int xspi_read(uint32_t pc_rx_addr, uint32_t *pc_rx_buf, uint32_t x_size_bytes)
+{
+	if (x_size_bytes == 0) {
+		ERROR("Zero length reads are not allowed\n");
+		return XSPI_READ_FAIL;
+	}
+
+#if defined(CONFIG_FSPI_AHB)
+	return xspi_ahb_read(pc_rx_addr, pc_rx_buf, x_size_bytes);
+#else
+	return xspi_ip_read(pc_rx_addr, pc_rx_buf, x_size_bytes);
+#endif
+}
+#if defined(CONFIG_FSPI_AHB)
+int xspi_ahb_read(uint32_t pc_rx_addr, uint32_t *pc_rx_buf, uint32_t x_size_bytes)
+{
+	VERBOSE("In func %s 0x%x\n", __func__, (pc_rx_addr));
+
+	if (F_FLASH_SIZE_BYTES <= SZ_16M_BYTES) {
+		pc_rx_addr = ((uint32_t)(pcRxAddr & MASK_24BIT_ADDRESS));
+	} else {
+		pc_rx_addr = ((uint32_t)(pcRxAddr & MASK_32BIT_ADDRESS));
+	}
+
+	pc_rx_addr = ((uint32_t)(pcRxAddr + fspi_flash_base_addr));
+
+	if (((pc_rx_addr % 4) != 0) || (((uintptr_t)pc_rx_buf % 4) != 0)) {
+		WARN("%s: unaligned Start Address src=%ld dst=0x%p\n",
+		     __func__, (pc_rx_addr - fspi_flash_base_addr), pc_rx_buf);
+	}
+
+	/* Directly copy from AHB Buffer */
+	memcpy(pc_rx_buf, (void *)(uintptr_t)pc_rx_addr, x_size_bytes);
+
+	fspi_ahb_invalidate();
+	return XSPI_SUCCESS;
+}
+#endif
+
+int xspi_ip_read(uint32_t pc_rx_addr, uint32_t *pv_rx_buf, uint32_t ui_len)
+{
+
+	uint32_t i = 0U, j = 0U, x_rem = 0U;
+	uint32_t x_iteration = 0U, x_size_rx = 0U, x_size_wm, temp_size;
+	uint32_t data = 0U;
+	uint32_t x_len_bytes;
+	uint32_t x_addr, sts0, intr, seq_id;
+
+	x_addr = (uint32_t) pc_rx_addr;
+	x_len_bytes = ui_len;
+
+	/* Watermark level : 8 bytes. (BY DEFAULT) */
+	x_size_wm = 8U;
+
+	/* Clear  RX Watermark interrupt in INT register, if any existing.  */
+	fspi_writel(FSPI_INTR, FSPI_INTR_IPRXWA);
+	PRA("0x%x", fspi_readl(FSPI_INTR));
+	/* Invalid the RXFIFO, to run next IP Command */
+	/* Clears data entries in IP Rx FIFOs, Also reset R/W pointers */
+	fspi_writel(FSPI_IPRXFCR, FSPI_IPRXFCR_CLR);
+	fspi_writel(FSPI_INTR, FSPI_INTEN_IPCMDDONE);
+
+	while (x_len_bytes) {
+
+		/* FlexSPI can store no more than  FSPI_RX_IPBUF_SIZE */
+		x_size_rx = (x_len_bytes >  FSPI_RX_IPBUF_SIZE) ?
+			   FSPI_RX_IPBUF_SIZE : x_len_bytes;
+
+		/* IP Control Register0 - SF Address to be read */
+		fspi_writel(FSPI_IPCR0, x_addr);
+		PRA("0x%x", fspi_readl(FSPI_IPCR0));
+		/* IP Control Register1 - SEQID_READ operation, Size */
+
+		seq_id = CONFIG_FSPI_FASTREAD ?
+				FSPI_FASTREAD_SEQ_ID : FSPI_READ_SEQ_ID;
+
+		fspi_writel(FSPI_IPCR1,
+			    (uint32_t)(seq_id << FSPI_IPCR1_ISEQID_SHIFT) |
+			    (uint16_t) x_size_rx);
+
+		PRA("0x%x", fspi_readl(FSPI_IPCR1));
+
+		do {
+			sts0 = fspi_readl(FSPI_STS0);
+		} while (((sts0 & FSPI_STS0_ARB_IDLE) == 0) &&
+			 ((sts0 & FSPI_STS0_SEQ_IDLE) == 0));
+
+		/* Trigger IP Read Command */
+		fspi_writel(FSPI_IPCMD, FSPI_IPCMD_TRG_MASK);
+		PRA("0x%x", fspi_readl(FSPI_IPCMD));
+
+		intr = fspi_readl(FSPI_INTR);
+		if (((intr & FSPI_INTR_IPCMDGE) != 0) ||
+		    ((intr & FSPI_INTR_IPCMDERR) != 0)) {
+			ERROR("Error in IP READ INTR=0x%x\n", intr);
+			return -XSPI_IP_READ_FAIL;
+		}
+		/* Will read in n iterations of each 8 FIFO's(WM level) */
+		x_iteration = x_size_rx / x_size_wm;
+		for (i = 0U; i < x_iteration; i++) {
+			if ((fspi_readl(FSPI_INTR) & FSPI_INTR_IPRXWA_MASK) == 0) {
+				PRA("0x%x", fspi_readl(FSPI_INTR));
+			}
+			/* Wait for IP Rx Watermark Fill event */
+			while (!(fspi_readl(FSPI_INTR) & FSPI_INTR_IPRXWA_MASK)) {
+				PRA("0x%x", fspi_readl(FSPI_INTR));
+			}
+
+			/* Read RX FIFO's(upto WM level) & copy to rxbuffer */
+			for (j = 0U; j < x_size_wm; j += 4U) {
+				/* Read FIFO Data Register */
+				data = fspi_readl(FSPI_RFDR + j);
+#if FSPI_IPDATA_SWAP /* Just In case you want swap */
+				data = bswap32(data);
+#endif
+				memcpy(pv_rx_buf++, &data, 4);
+			}
+
+			/* Clear IP_RX_WATERMARK Event in INTR register */
+			/* Reset FIFO Read pointer for next iteration.*/
+			fspi_writel(FSPI_INTR, FSPI_INTR_IPRXWA);
+		}
+
+		x_rem = x_size_rx % x_size_wm;
+
+		if (x_rem != 0U) {
+			/* Wait for data filled */
+			while (!(fspi_readl(FSPI_IPRXFSTS) & FSPI_IPRXFSTS_FILL_MASK)) {
+				PRA("0x%x", fspi_readl(FSPI_IPRXFSTS));
+			}
+
+			temp_size = 0;
+			j = 0U;
+			while (x_rem > 0U) {
+				data = 0U;
+				data =  fspi_readl(FSPI_RFDR + j);
+#if FSPI_IPDATA_SWAP /* Just In case you want swap */
+				data = bswap32(data);
+#endif
+				temp_size = (x_rem < 4) ? x_rem : 4;
+				memcpy(pv_rx_buf++, &data, temp_size);
+				x_rem -= temp_size;
+			}
+		}
+
+
+		while (!(fspi_readl(FSPI_INTR) & FSPI_INTR_IPCMDDONE_MASK)) {
+			PRA("0x%x", fspi_readl(FSPI_INTR));
+		}
+
+		/* Invalid the RX FIFO, to run next IP Command */
+		fspi_writel(FSPI_IPRXFCR, FSPI_IPRXFCR_CLR);
+		/* Clear IP Command Done flag in interrupt register*/
+		fspi_writel(FSPI_INTR, FSPI_INTR_IPCMDDONE_MASK);
+
+		/* Update remaining len, Increment x_addr read pointer. */
+		x_len_bytes -= x_size_rx;
+		x_addr += x_size_rx;
+	}
+	PR;
+	return XSPI_SUCCESS;
+}
+
+void xspi_ip_write(uint32_t pc_wr_addr, uint32_t *pv_wr_buf, uint32_t ui_len)
+{
+
+	uint32_t x_iteration = 0U, x_rem = 0U;
+	uint32_t x_size_tx = 0U, x_size_wm, temp_size;
+	uint32_t i = 0U, j = 0U;
+	uint32_t ui_data = 0U;
+	uint32_t x_addr, x_len_bytes;
+
+
+	x_size_wm = 8U;	/* Default TX WaterMark level: 8 Bytes. */
+	x_addr = (uint32_t)pc_wr_addr;
+	x_len_bytes = ui_len;
+	VERBOSE("In func %s[%d] x_addr =0x%x xLen_bytes=%d\n",
+			__func__, __LINE__, x_addr, x_len_bytes);
+
+	while (x_len_bytes != 0U) {
+
+		x_size_tx = (x_len_bytes >  FSPI_TX_IPBUF_SIZE) ?
+				FSPI_TX_IPBUF_SIZE : x_len_bytes;
+
+		/* IP Control Register0 - SF Address to be read */
+		fspi_writel(FSPI_IPCR0, x_addr);
+		INFO("In func %s[%d] x_addr =0x%x xLen_bytes=%d\n",
+				__func__, __LINE__, x_addr, x_len_bytes);
+
+		/*
+		 * Fill TX FIFO's..
+		 *
+		 */
+
+		x_iteration = x_size_tx / x_size_wm;
+		for (i = 0U; i < x_iteration; i++) {
+
+			/* Ensure TX FIFO Watermark Available */
+			while ((fspi_readl(FSPI_INTR) & FSPI_INTR_IPTXWE_MASK) == 0)
+				;
+
+
+			/* Fill TxFIFO's ( upto watermark level) */
+			for (j = 0U; j < x_size_wm; j += 4U) {
+				memcpy(&ui_data, pv_wr_buf++,  4);
+				/* Write TX FIFO Data Register */
+				fspi_writel((FSPI_TFDR + j), ui_data);
+
+			}
+
+			/* Clear IP_TX_WATERMARK Event in INTR register */
+			/* Reset the FIFO Write pointer for next iteration */
+			fspi_writel(FSPI_INTR, FSPI_INTR_IPTXWE);
+		}
+
+		x_rem = x_size_tx % x_size_wm;
+
+		if (x_rem != 0U) {
+			/* Wait for TXFIFO empty */
+			while (!(fspi_readl(FSPI_INTR) & FSPI_INTR_IPTXWE))
+				;
+
+			temp_size = 0U;
+			j = 0U;
+			while (x_rem > 0U) {
+				ui_data = 0U;
+				temp_size = (x_rem < 4U) ? x_rem : 4U;
+				memcpy(&ui_data, pv_wr_buf++, temp_size);
+				INFO("%d ---> pv_wr_buf=0x%p\n", __LINE__, pv_wr_buf);
+				fspi_writel((FSPI_TFDR + j), ui_data);
+				x_rem -= temp_size;
+				j += 4U ; /* TODO: May not be needed*/
+			}
+			/* Clear IP_TX_WATERMARK Event in INTR register */
+			/* Reset FIFO's Write pointer for next iteration.*/
+			fspi_writel(FSPI_INTR, FSPI_INTR_IPTXWE);
+		}
+
+		/* IP Control Register1 - SEQID_WRITE operation, Size */
+		fspi_writel(FSPI_IPCR1, (uint32_t)(FSPI_WRITE_SEQ_ID << FSPI_IPCR1_ISEQID_SHIFT) | (uint16_t) x_size_tx);
+		/* Trigger IP Write Command */
+		fspi_writel(FSPI_IPCMD, FSPI_IPCMD_TRG_MASK);
+
+		/* Wait for IP Write command done */
+		while (!(fspi_readl(FSPI_INTR) & FSPI_INTR_IPCMDDONE_MASK))
+			;
+
+		/* Invalidate TX FIFOs & acknowledge IP_CMD_DONE event */
+		fspi_writel(FSPI_IPTXFCR, FSPI_IPTXFCR_CLR);
+		fspi_writel(FSPI_INTR, FSPI_INTR_IPCMDDONE_MASK);
+
+		/* for next iteration */
+		x_len_bytes  -=  x_size_tx;
+		x_addr += x_size_tx;
+	}
+
+}
+
+int xspi_write(uint32_t pc_wr_addr, void *pv_wr_buf, uint32_t ui_len)
+{
+
+	uint32_t x_addr;
+	uint32_t x_page1_len = 0U, x_page_l_len = 0U;
+	uint32_t i, j = 0U;
+	void *buf = pv_wr_buf;
+
+	VERBOSE("\nIn func %s\n", __func__);
+
+	x_addr = (uint32_t)(pc_wr_addr);
+	if ((ui_len <= F_PAGE_256) && ((x_addr % F_PAGE_256) == 0)) {
+		x_page1_len = ui_len;
+		INFO("%d ---> x_page1_len=0x%x x_page_l_len =0x%x j=0x%x\n", __LINE__, x_page1_len, x_page_l_len, j);
+	} else if ((ui_len <= F_PAGE_256) && ((x_addr % F_PAGE_256) != 0)) {
+		x_page1_len = (F_PAGE_256 - (x_addr % F_PAGE_256));
+		if (ui_len > x_page1_len) {
+			x_page_l_len = (ui_len - x_page1_len) % F_PAGE_256;
+		} else {
+			x_page1_len = ui_len;
+			x_page_l_len = 0;
+		}
+		j = 0U;
+		INFO("%d 0x%x 0x%x\n", x_addr % F_PAGE_256, x_addr % F_PAGE_256, F_PAGE_256);
+		INFO("%d ---> x_page1_len=0x%x x_page_l_len =0x%x j=0x%x\n", __LINE__, x_page1_len, x_page_l_len, j);
+	} else if ((ui_len > F_PAGE_256) && ((x_addr % F_PAGE_256) == 0)) {
+		j = ui_len / F_PAGE_256;
+		x_page_l_len = ui_len % F_PAGE_256;
+		INFO("%d ---> x_page1_len=0x%x x_page_l_len =0x%x j=0x%x\n", __LINE__, x_page1_len, x_page_l_len, j);
+	} else if ((ui_len > F_PAGE_256) && ((x_addr % F_PAGE_256) != 0)) {
+		x_page1_len = (F_PAGE_256 - (x_addr % F_PAGE_256));
+		j = (ui_len - x_page1_len) / F_PAGE_256;
+		x_page_l_len = (ui_len - x_page1_len) % F_PAGE_256;
+		INFO("%d ---> x_page1_len=0x%x x_page_l_len =0x%x j=0x%x\n", __LINE__, x_page1_len, x_page_l_len, j);
+	}
+
+	if (x_page1_len != 0U) {
+		xspi_wren(x_addr);
+		xspi_ip_write(x_addr, (uint32_t *)buf, x_page1_len);
+		while (is_flash_busy())
+			;
+		INFO("%d Initial pc_wr_addr=0x%x, Final x_addr=0x%x, Initial ui_len=0x%x Final ui_len=0x%x\n",
+		     __LINE__, pc_wr_addr, x_addr, ui_len, (x_addr-pc_wr_addr));
+		INFO("Initial Buf pv_wr_buf=%p, final Buf=%p\n", pv_wr_buf, buf);
+		x_addr += x_page1_len;
+		/* TODO What is buf start is not 4 aligned */
+		buf = buf + x_page1_len;
+	}
+
+	for (i = 0U; i < j; i++) {
+		INFO("In for loop Buf pv_wr_buf=%p, final Buf=%p x_addr=0x%x offset_buf %d.\n",
+				pv_wr_buf, buf, x_addr, x_page1_len/4);
+		xspi_wren(x_addr);
+		xspi_ip_write(x_addr, (uint32_t *)buf, F_PAGE_256);
+		while (is_flash_busy())
+			;
+		INFO("%d Initial pc_wr_addr=0x%x, Final x_addr=0x%x, Initial ui_len=0x%x Final ui_len=0x%x\n",
+		     __LINE__, pc_wr_addr, x_addr, ui_len, (x_addr-pc_wr_addr));
+		x_addr += F_PAGE_256;
+		/* TODO What is buf start is not 4 aligned */
+		buf = buf + F_PAGE_256;
+		INFO("Initial Buf pv_wr_buf=%p, final Buf=%p\n", pv_wr_buf, buf);
+	}
+
+	if (x_page_l_len != 0U) {
+		INFO("%d Initial Buf pv_wr_buf=%p, final Buf=%p x_page_l_len=0x%x\n", __LINE__, pv_wr_buf, buf, x_page_l_len);
+		xspi_wren(x_addr);
+		xspi_ip_write(x_addr, (uint32_t *)buf, x_page_l_len);
+		while (is_flash_busy())
+			;
+		INFO("%d Initial pc_wr_addr=0x%x, Final x_addr=0x%x, Initial ui_len=0x%x Final ui_len=0x%x\n",
+				__LINE__, pc_wr_addr, x_addr, ui_len, (x_addr-pc_wr_addr));
+	}
+
+	VERBOSE("Now calling func call Invalidate%s\n", __func__);
+	fspi_ahb_invalidate();
+	return XSPI_SUCCESS;
+}
+
+int xspi_wren(uint32_t pc_wr_addr)
+{
+	VERBOSE("In func %s Addr=0x%x\n", __func__, pc_wr_addr);
+
+	fspi_writel(FSPI_IPTXFCR, FSPI_IPTXFCR_CLR);
+
+	fspi_writel(FSPI_IPCR0, (uint32_t)pc_wr_addr);
+	fspi_writel(FSPI_IPCR1, ((FSPI_WREN_SEQ_ID << FSPI_IPCR1_ISEQID_SHIFT) |  0));
+	fspi_writel(FSPI_IPCMD, FSPI_IPCMD_TRG_MASK);
+
+	while ((fspi_readl(FSPI_INTR) & FSPI_INTR_IPCMDDONE_MASK) == 0)
+		;
+
+	fspi_writel(FSPI_INTR, FSPI_INTR_IPCMDDONE_MASK);
+	return XSPI_SUCCESS;
+}
+
+static void fspi_bbluk_er(void)
+{
+	VERBOSE("In func %s\n", __func__);
+	fspi_writel(FSPI_IPCR0, 0x0);
+	fspi_writel(FSPI_IPCR1, ((FSPI_BE_SEQ_ID << FSPI_IPCR1_ISEQID_SHIFT) | 20));
+	fspi_writel(FSPI_IPCMD, FSPI_IPCMD_TRG_MASK);
+
+	while ((fspi_readl(FSPI_INTR) & FSPI_INTR_IPCMDDONE_MASK) == 0)
+		;
+	fspi_writel(FSPI_INTR, FSPI_INTR_IPCMDDONE_MASK);
+
+}
+
+static void fspi_RDSR(uint32_t *rxbuf, const void *p_addr, uint32_t size)
+{
+	uint32_t iprxfcr = 0U;
+	uint32_t data = 0U;
+
+	iprxfcr = fspi_readl(FSPI_IPRXFCR);
+	/* IP RX FIFO would be read by processor */
+	iprxfcr = iprxfcr & (uint32_t)~FSPI_IPRXFCR_CLR;
+	/* Invalid data entries in IP RX FIFO */
+	iprxfcr = iprxfcr | FSPI_IPRXFCR_CLR;
+	fspi_writel(FSPI_IPRXFCR, iprxfcr);
+
+	fspi_writel(FSPI_IPCR0, (uintptr_t) p_addr);
+	fspi_writel(FSPI_IPCR1,
+		    (uint32_t) ((FSPI_RDSR_SEQ_ID << FSPI_IPCR1_ISEQID_SHIFT)
+		    | (uint16_t) size));
+	/* Trigger the command */
+	fspi_writel(FSPI_IPCMD, FSPI_IPCMD_TRG_MASK);
+	/* Wait for command done */
+	while ((fspi_readl(FSPI_INTR) & FSPI_INTR_IPCMDDONE_MASK) == 0)
+		;
+	fspi_writel(FSPI_INTR, FSPI_INTR_IPCMDDONE_MASK);
+
+	data = fspi_readl(FSPI_RFDR);
+	memcpy(rxbuf, &data, size);
+
+	/* Rx FIFO invalidation needs to be done prior w1c of INTR.IPRXWA bit */
+	fspi_writel(FSPI_IPRXFCR, FSPI_IPRXFCR_CLR);
+	fspi_writel(FSPI_INTR, FSPI_INTR_IPRXWA_MASK);
+	fspi_writel(FSPI_INTR, FSPI_INTR_IPCMDDONE_MASK);
+
+}
+
+bool is_flash_busy(void)
+{
+#define FSPI_ONE_BYTE 1
+	uint8_t data[4];
+
+	VERBOSE("In func %s\n\n", __func__);
+	fspi_RDSR((uint32_t *) data, 0, FSPI_ONE_BYTE);
+
+	return !!((uint32_t) data[0] & FSPI_NOR_SR_WIP_MASK);
+}
+
+int xspi_bulk_erase(void)
+{
+	VERBOSE("In func %s\n", __func__);
+	xspi_wren((uint32_t) 0x0);
+	fspi_bbluk_er();
+	while (is_flash_busy())
+		;
+	fspi_ahb_invalidate();
+	return XSPI_SUCCESS;
+}
+
+static void fspi_sec_er(uint32_t pc_wr_addr)
+{
+	uint32_t x_addr;
+
+	VERBOSE("In func %s\n", __func__);
+	x_addr = (uint32_t)(pc_wr_addr);
+
+	fspi_writel(FSPI_IPCR0, x_addr);
+	INFO("In [%s][%d] Erase address 0x%x\n", __func__, __LINE__, (x_addr));
+#if CONFIG_FSPI_ERASE_4K
+	fspi_writel(FSPI_IPCR1, ((FSPI_4K_SEQ_ID << FSPI_IPCR1_ISEQID_SHIFT) | 0));
+#else
+	fspi_writel(FSPI_IPCR1, ((FSPI_SE_SEQ_ID << FSPI_IPCR1_ISEQID_SHIFT) | 0));
+#endif
+	fspi_writel(FSPI_IPCMD, FSPI_IPCMD_TRG_MASK);
+
+	while ((fspi_readl(FSPI_INTR) & FSPI_INTR_IPCMDDONE_MASK) == 0) {
+		PRA("0x%x", fspi_readl(FSPI_INTR));
+	}
+	fspi_writel(FSPI_INTR, FSPI_INTR_IPCMDDONE_MASK);
+}
+
+int xspi_sector_erase(uint32_t pc_wr_addr, uint32_t ui_len)
+{
+	uint32_t x_addr, x_len_bytes, i, num_sector = 0U;
+
+	VERBOSE("In func %s\n", __func__);
+	x_addr = (uint32_t)(pc_wr_addr);
+	if ((x_addr % F_SECTOR_ERASE_SZ) != 0) {
+		ERROR("!!! In func %s, unalinged start address can only be in multiples of 0x%x\n",
+		      __func__, F_SECTOR_ERASE_SZ);
+		return -XSPI_ERASE_FAIL;
+	}
+
+	x_len_bytes = ui_len * 1;
+	if (x_len_bytes < F_SECTOR_ERASE_SZ) {
+		ERROR("!!! In func %s, Less than 1 sector can only be in multiples of 0x%x\n",
+				__func__, F_SECTOR_ERASE_SZ);
+		return -XSPI_ERASE_FAIL;
+	}
+
+	num_sector = x_len_bytes/F_SECTOR_ERASE_SZ;
+	num_sector += x_len_bytes % F_SECTOR_ERASE_SZ ? 1U : 0U;
+	INFO("F_SECTOR_ERASE_SZ: 0x%08x, num_sector: %d\n", F_SECTOR_ERASE_SZ, num_sector);
+
+	for (i = 0U; i < num_sector ; i++) {
+		xspi_wren(x_addr + (F_SECTOR_ERASE_SZ * i));
+		fspi_sec_er(x_addr + (F_SECTOR_ERASE_SZ * i));
+		while (is_flash_busy())
+			;
+	}
+	fspi_ahb_invalidate();
+	return XSPI_SUCCESS;
+}
+
+
+__attribute__((unused)) static void  fspi_delay_ms(uint32_t x)
+{
+	volatile unsigned long  ul_count;
+
+	for (ul_count = 0U; ul_count < (30U * x); ul_count++)
+		;
+
+}
+
+
+#if defined(DEBUG_FLEXSPI)
+static void fspi_dump_regs(void)
+{
+	uint32_t i;
+
+	VERBOSE("\nRegisters Dump:\n");
+	VERBOSE("Flexspi: Register FSPI_MCR0(0x%x) = 0x%08x\n", FSPI_MCR0, fspi_readl(FSPI_MCR0));
+	VERBOSE("Flexspi: Register FSPI_MCR2(0x%x) = 0x%08x\n", FSPI_MCR2, fspi_readl(FSPI_MCR2));
+	VERBOSE("Flexspi: Register FSPI_DLL_A_CR(0x%x) = 0x%08x\n", FSPI_DLLACR, fspi_readl(FSPI_DLLACR));
+	VERBOSE("\n");
+
+	for (i = 0U; i < 8U; i++) {
+		VERBOSE("Flexspi: Register FSPI_AHBRX_BUF0CR0(0x%x) = 0x%08x\n", FSPI_AHBRX_BUF0CR0 + i * 4, fspi_readl((FSPI_AHBRX_BUF0CR0 + i * 4)));
+	}
+	VERBOSE("\n");
+
+	VERBOSE("Flexspi: Register FSPI_AHBRX_BUF7CR0(0x%x) = 0x%08x\n", FSPI_AHBRX_BUF7CR0, fspi_readl(FSPI_AHBRX_BUF7CR0));
+	VERBOSE("Flexspi: Register FSPI_AHB_CR(0x%x) \t  = 0x%08x\n", FSPI_AHBCR, fspi_readl(FSPI_AHBCR));
+	VERBOSE("\n");
+
+	for (i = 0U; i < 4U; i++) {
+		VERBOSE("Flexspi: Register FSPI_FLSH_A1_CR2,(0x%x) = 0x%08x\n", FSPI_FLSHA1CR2 + i * 4, fspi_readl(FSPI_FLSHA1CR2 + i * 4));
+	}
+}
+#endif
+
+int fspi_init(uint32_t base_reg_addr, uint32_t flash_start_addr)
+{
+	uint32_t	mcrx;
+	uint32_t	flash_size;
+
+	if (fspi_base_reg_addr != 0U) {
+		INFO("FSPI is already initialized.\n");
+		return XSPI_SUCCESS;
+	}
+
+	fspi_base_reg_addr = base_reg_addr;
+	fspi_flash_base_addr = flash_start_addr;
+
+	INFO("Flexspi driver: Version v1.0\n");
+	INFO("Flexspi: Default MCR0 = 0x%08x, before reset\n", fspi_readl(FSPI_MCR0));
+	VERBOSE("Flexspi: Resetting controller...\n");
+
+	/* Reset FlexSpi Controller */
+	fspi_writel(FSPI_MCR0, FSPI_MCR0_SWRST);
+	while ((fspi_readl(FSPI_MCR0) & FSPI_MCR0_SWRST))
+		;  /* FSPI_MCR0_SWRESET_MASK */
+
+
+	/* Disable Controller Module before programming its registersi, especially MCR0 (Master Control Register0) */
+	fspi_MDIS(1);
+	/*
+	 * Program MCR0 with default values, AHB Timeout(0xff), IP Timeout(0xff).  {FSPI_MCR0- 0xFFFF0000}
+	 */
+
+	/* Timeout wait cycle for AHB command grant */
+	mcrx = fspi_readl(FSPI_MCR0);
+	mcrx |= (uint32_t)((FSPI_MAX_TIMEOUT_AHBCMD << FSPI_MCR0_AHBGRANTWAIT_SHIFT) & (FSPI_MCR0_AHBGRANTWAIT_MASK));
+
+	/* Time out wait cycle for IP command grant*/
+	mcrx |= (uint32_t) (FSPI_MAX_TIMEOUT_IPCMD << FSPI_MCR0_IPGRANTWAIT_SHIFT) & (FSPI_MCR0_IPGRANTWAIT_MASK);
+
+	/* TODO why BE64 set BE32*/
+	mcrx |= (uint32_t) (FSPI_ENDCFG_LE64 << FSPI_MCR0_ENDCFG_SHIFT) & FSPI_MCR0_ENDCFG_MASK;
+
+	fspi_writel(FSPI_MCR0, mcrx);
+
+	/* Reset the DLL register to default value */
+	fspi_writel(FSPI_DLLACR, FSPI_DLLACR_OVRDEN);
+	fspi_writel(FSPI_DLLBCR, FSPI_DLLBCR_OVRDEN);
+
+#if ERRATA_FLASH_A050272	/* ERRATA DLL */
+	for (uint8_t delay = 100U; delay > 0U; delay--)	{
+		__asm__ volatile ("nop");
+	}
+#endif
+
+	/* Configure flash control registers for different chip select */
+	flash_size = (F_FLASH_SIZE_BYTES * FLASH_NUM) / FSPI_BYTES_PER_KBYTES;
+	fspi_writel(FSPI_FLSHA1CR0, flash_size);
+	fspi_writel(FSPI_FLSHA2CR0, 0U);
+	fspi_writel(FSPI_FLSHB1CR0, 0U);
+	fspi_writel(FSPI_FLSHB2CR0, 0U);
+
+#if defined(CONFIG_FSPI_AHB)
+	fspi_init_ahb();
+#endif
+	/* RE-Enable Controller Module */
+	fspi_MDIS(0);
+	INFO("Flexspi: After MCR0 = 0x%08x,\n", fspi_readl(FSPI_MCR0));
+	fspi_setup_LUT();
+
+	/* Dump of all registers, ensure controller not disabled anymore*/
+#if defined(DEBUG_FLEXSPI)
+	fspi_dump_regs();
+#endif
+
+	INFO("Flexspi: Init done!!\n");
+
+#if DEBUG_FLEXSPI
+
+	uint32_t xspi_addr = SZ_57M;
+
+	/*
+	 * Second argument of fspi_test is the size of buffer(s) passed
+	 * to the function.
+	 * SIZE_BUFFER defined in test_fspi.c is kept large enough to
+	 * accommodate variety of sizes for regressive tests.
+	 */
+	fspi_test(xspi_addr, 0x40, 0);
+	fspi_test(xspi_addr, 0x15, 2);
+	fspi_test(xspi_addr, 0x80, 0);
+	fspi_test(xspi_addr, 0x81, 0);
+	fspi_test(xspi_addr, 0x79, 3);
+
+	fspi_test(xspi_addr + 0x11, 0x15, 0);
+	fspi_test(xspi_addr + 0x11, 0x40, 0);
+	fspi_test(xspi_addr + 0xff, 0x40, 1);
+	fspi_test(xspi_addr + 0x25, 0x81, 2);
+	fspi_test(xspi_addr + 0xef, 0x6f, 3);
+
+	fspi_test((xspi_addr - F_SECTOR_ERASE_SZ), 0x229, 0);
+#endif
+
+	return XSPI_SUCCESS;
+}
diff --git a/drivers/nxp/flexspi/nor/fspi.h b/drivers/nxp/flexspi/nor/fspi.h
new file mode 100644
index 0000000..da2e269
--- /dev/null
+++ b/drivers/nxp/flexspi/nor/fspi.h
@@ -0,0 +1,385 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * FlexSpi Registers & Bits definition.
+ *
+ */
+
+#ifndef FSPI_H
+#define FSPI_H
+
+#ifndef __ASSEMBLER__
+#include <lib/mmio.h>
+
+#ifdef NXP_FSPI_BE
+#define fspi_in32(a)		bswap32(mmio_read_32((uintptr_t)(a)))
+#define fspi_out32(a, v)	mmio_write_32((uintptr_t)(a), bswap32(v))
+#elif defined(NXP_FSPI_LE)
+#define fspi_in32(a)		mmio_read_32((uintptr_t)(a))
+#define fspi_out32(a, v)	mmio_write_32((uintptr_t)(a), v)
+#else
+#error Please define FSPI register endianness
+#endif
+
+#endif
+
+/* All LE so not swap needed */
+#define FSPI_IPDATA_SWAP		0U
+#define FSPI_AHBDATA_SWAP		0U
+
+#define CONFIG_FSPI_FASTREAD		1U
+
+#define FSPI_BYTES_PER_KBYTES		0x400U
+#define FLASH_NUM			1U
+
+#define FSPI_READ_SEQ_ID		0U
+#define FSPI_WREN_SEQ_ID		1U
+#define FSPI_WRITE_SEQ_ID		2U
+#define FSPI_SE_SEQ_ID			3U
+#define FSPI_RDSR_SEQ_ID		4U
+#define FSPI_BE_SEQ_ID			5U
+#define FSPI_FASTREAD_SEQ_ID		6U
+#define FSPI_4K_SEQ_ID			7U
+
+/*
+ * LUT register layout:
+ *
+ *  ---------------------------------------------------
+ *  | INSTR1 | PAD1 | OPRND1 | INSTR0 | PAD0 | OPRND0 |
+ *  ---------------------------------------------------
+ *
+ *    INSTR_SHIFT- 10, PAD_SHIFT - 8, OPRND_SHIFT -0
+ */
+#define FSPI_INSTR_OPRND0_SHIFT		0
+#define FSPI_INSTR_OPRND0(x)		(x << FSPI_INSTR_OPRND0_SHIFT)
+#define FSPI_INSTR_PAD0_SHIFT		8
+#define FSPI_INSTR_PAD0(x)		((x) << FSPI_INSTR_PAD0_SHIFT)
+#define FSPI_INSTR_OPCODE0_SHIFT	10
+#define FSPI_INSTR_OPCODE0(x)		((x) << FSPI_INSTR_OPCODE0_SHIFT)
+#define FSPI_INSTR_OPRND1_SHIFT		16
+#define FSPI_INSTR_OPRND1(x)		((x) << FSPI_INSTR_OPRND1_SHIFT)
+#define FSPI_INSTR_PAD1_SHIFT		24
+#define FSPI_INSTR_PAD1(x)		((x) << FSPI_INSTR_PAD1_SHIFT)
+#define FSPI_INSTR_OPCODE1_SHIFT	26
+#define FSPI_INSTR_OPCODE1(x)		((x) << FSPI_INSTR_OPCODE1_SHIFT)
+
+/* Instruction set for the LUT register. */
+#define LUT_STOP			0x00
+#define LUT_CMD				0x01
+#define LUT_ADDR			0x02
+#define LUT_CADDR_SDR			0x03
+#define LUT_MODE			0x04
+#define LUT_MODE2			0x05
+#define LUT_MODE4			0x06
+#define LUT_MODE8			0x07
+#define LUT_NXP_WRITE			0x08
+#define LUT_NXP_READ			0x09
+
+#define LUT_LEARN_SDR			0x0A
+#define LUT_DATSZ_SDR			0x0B
+#define LUT_DUMMY			0x0C
+#define LUT_DUMMY_RWDS_SDR		0x0D
+#define LUT_JMP_ON_CS			0x1F
+#define LUT_CMD_DDR			0x21
+#define LUT_ADDR_DDR			0x22
+#define LUT_CADDR_DDR			0x23
+#define LUT_MODE_DDR			0x24
+#define LUT_MODE2_DDR			0x25
+#define LUT_MODE4_DDR			0x26
+#define LUT_MODE8_DDR			0x27
+#define LUT_WRITE_DDR			0x28
+#define LUT_READ_DDR			0x29
+#define LUT_LEARN_DDR			0x2A
+#define LUT_DATSZ_DDR			0x2B
+#define LUT_DUMMY_DDR			0x2C
+#define LUT_DUMMY_RWDS_DDR		0x2D
+
+#define FSPI_NOR_CMD_READ		0x03
+#define FSPI_NOR_CMD_READ_4B		0x13
+#define FSPI_NOR_CMD_FASTREAD		0x0b
+#define FSPI_NOR_CMD_FASTREAD_4B	0x0c
+#define FSPI_NOR_CMD_PP			0x02
+#define FSPI_NOR_CMD_PP_4B		0x12
+#define FSPI_NOR_CMD_WREN		0x06
+#define FSPI_NOR_CMD_SE_64K		0xd8
+#define FSPI_NOR_CMD_SE_64K_4B		0xdc
+#define FSPI_NOR_CMD_SE_4K		0x20
+#define FSPI_NOR_CMD_SE_4K_4B		0x21
+#define FSPI_NOR_CMD_BE			0x60
+#define FSPI_NOR_CMD_RDSR		0x05
+#define FSPI_NOR_CMD_WREN_STOP		0x04
+
+#define FSPI_LUT_STOP			0x00
+#define FSPI_LUT_CMD			0x01
+#define FSPI_LUT_ADDR			0x02
+
+#define FSPI_LUT_PAD1			0
+#define FSPI_LUT_PAD2			1
+#define FSPI_LUT_PAD4			2
+#define FSPI_LUT_PAD8			3
+
+#define FSPI_LUT_ADDR24BIT		0x18
+#define FSPI_LUT_ADDR32BIT		0x20
+
+#define FSPI_LUT_WRITE			0x08
+#define FSPI_LUT_READ			0x09
+#define FSPI_DUMMY_SDR			0x0c
+
+/* TODO Check size if functional*/
+#define FSPI_RX_IPBUF_SIZE		0x200	/*  64*64 bits  */
+#define FSPI_TX_IPBUF_SIZE		0x400	/* 128*64 bits */
+
+#define FSPI_RX_MAX_AHBBUF_SIZE		0x800 /* 256 * 64bits */
+#define FSPI_TX_MAX_AHBBUF_SIZE		0x40  /* 8 * 64bits   */
+
+#define FSPI_LUTREG_OFFSET			0x200ul
+
+#define FSPI_MAX_TIMEOUT_AHBCMD		0xFFU
+#define FSPI_MAX_TIMEOUT_IPCMD		0xFF
+#define FSPI_SER_CLK_DIV		0x04
+#define FSPI_HSEN			0
+#define FSPI_ENDCFG_BE64		0x01
+#define FSPI_ENDCFG_BE32		0x03
+#define FSPI_ENDCFG_LE32		0x02
+#define FSPI_ENDCFG_LE64		0x0
+
+#define MASK_24BIT_ADDRESS		0x00ffffff
+#define MASK_32BIT_ADDRESS		0xffffffff
+
+/* Registers used by the driver */
+#define FSPI_MCR0			0x0ul
+#define FSPI_MCR0_AHB_TIMEOUT(x)	((x) << 24)
+#define FSPI_MCR0_IP_TIMEOUT(x)		((x) << 16)
+#define FSPI_MCR0_LEARN_EN		BIT(15)
+#define FSPI_MCR0_SCRFRUN_EN		BIT(14)
+#define FSPI_MCR0_OCTCOMB_EN		BIT(13)
+#define FSPI_MCR0_DOZE_EN		BIT(12)
+#define FSPI_MCR0_HSEN			BIT(11)
+#define FSPI_MCR0_SERCLKDIV		BIT(8)
+#define FSPI_MCR0_ATDF_EN		BIT(7)
+#define FSPI_MCR0_ARDF_EN		BIT(6)
+#define FSPI_MCR0_RXCLKSRC(x)		((x) << 4)
+#define FSPI_MCR0_END_CFG(x)		((x) << 2)
+#define FSPI_MCR0_MDIS			BIT(1)
+#define FSPI_MCR0_SWRST			BIT(0)
+
+#define FSPI_MCR0_AHBGRANTWAIT_SHIFT	24
+#define FSPI_MCR0_AHBGRANTWAIT_MASK	(0xFFU << FSPI_MCR0_AHBGRANTWAIT_SHIFT)
+#define FSPI_MCR0_IPGRANTWAIT_SHIFT	16
+#define FSPI_MCR0_IPGRANTWAIT_MASK	(0xFF << FSPI_MCR0_IPGRANTWAIT_SHIFT)
+#define FSPI_MCR0_HSEN_SHIFT		11
+#define FSPI_MCR0_HSEN_MASK		(1 << FSPI_MCR0_HSEN_SHIFT)
+#define FSPI_MCR0_SERCLKDIV_SHIFT	8
+#define FSPI_MCR0_SERCLKDIV_MASK	(7 << FSPI_MCR0_SERCLKDIV_SHIFT)
+#define FSPI_MCR0_ENDCFG_SHIFT		2
+#define FSPI_MCR0_ENDCFG_MASK		(3 << FSPI_MCR0_ENDCFG_SHIFT)
+#define FSPI_MCR0_RXCLKSRC_SHIFT	4
+#define FSPI_MCR0_RXCLKSRC_MASK		(3 << FSPI_MCR0_RXCLKSRC_SHIFT)
+
+#define FSPI_MCR1			0x04
+#define FSPI_MCR1_SEQ_TIMEOUT(x)	((x) << 16)
+#define FSPI_MCR1_AHB_TIMEOUT(x)	(x)
+
+#define FSPI_MCR2			0x08
+#define FSPI_MCR2_IDLE_WAIT(x)		((x) << 24)
+#define FSPI_MCR2_SAMEDEVICEEN		BIT(15)
+#define FSPI_MCR2_CLRLRPHS		BIT(14)
+#define FSPI_MCR2_ABRDATSZ		BIT(8)
+#define FSPI_MCR2_ABRLEARN		BIT(7)
+#define FSPI_MCR2_ABR_READ		BIT(6)
+#define FSPI_MCR2_ABRWRITE		BIT(5)
+#define FSPI_MCR2_ABRDUMMY		BIT(4)
+#define FSPI_MCR2_ABR_MODE		BIT(3)
+#define FSPI_MCR2_ABRCADDR		BIT(2)
+#define FSPI_MCR2_ABRRADDR		BIT(1)
+#define FSPI_MCR2_ABR_CMD		BIT(0)
+
+#define FSPI_AHBCR			0x0c
+#define FSPI_AHBCR_RDADDROPT		BIT(6)
+#define FSPI_AHBCR_PREF_EN		BIT(5)
+#define FSPI_AHBCR_BUFF_EN		BIT(4)
+#define FSPI_AHBCR_CACH_EN		BIT(3)
+#define FSPI_AHBCR_CLRTXBUF		BIT(2)
+#define FSPI_AHBCR_CLRRXBUF		BIT(1)
+#define FSPI_AHBCR_PAR_EN		BIT(0)
+
+#define FSPI_INTEN			0x10
+#define FSPI_INTEN_SCLKSBWR		BIT(9)
+#define FSPI_INTEN_SCLKSBRD		BIT(8)
+#define FSPI_INTEN_DATALRNFL		BIT(7)
+#define FSPI_INTEN_IPTXWE		BIT(6)
+#define FSPI_INTEN_IPRXWA		BIT(5)
+#define FSPI_INTEN_AHBCMDERR		BIT(4)
+#define FSPI_INTEN_IPCMDERR		BIT(3)
+#define FSPI_INTEN_AHBCMDGE		BIT(2)
+#define FSPI_INTEN_IPCMDGE		BIT(1)
+#define FSPI_INTEN_IPCMDDONE		BIT(0)
+
+#define FSPI_INTR			0x14
+#define FSPI_INTR_SCLKSBWR		BIT(9)
+#define FSPI_INTR_SCLKSBRD		BIT(8)
+#define FSPI_INTR_DATALRNFL		BIT(7)
+#define FSPI_INTR_IPTXWE		BIT(6)
+#define FSPI_INTR_IPRXWA		BIT(5)
+#define FSPI_INTR_AHBCMDERR		BIT(4)
+#define FSPI_INTR_IPCMDERR		BIT(3)
+#define FSPI_INTR_AHBCMDGE		BIT(2)
+#define FSPI_INTR_IPCMDGE		BIT(1)
+#define FSPI_INTR_IPCMDDONE		BIT(0)
+
+#define FSPI_LUTKEY			0x18
+#define FSPI_LUTKEY_VALUE		0x5AF05AF0
+
+#define FSPI_LCKCR			0x1C
+
+#define FSPI_LCKER_LOCK			0x1
+#define FSPI_LCKER_UNLOCK		0x2
+
+#define FSPI_BUFXCR_INVALID_MSTRID	0xE
+#define FSPI_AHBRX_BUF0CR0		0x20
+#define FSPI_AHBRX_BUF1CR0		0x24
+#define FSPI_AHBRX_BUF2CR0		0x28
+#define FSPI_AHBRX_BUF3CR0		0x2C
+#define FSPI_AHBRX_BUF4CR0		0x30
+#define FSPI_AHBRX_BUF5CR0		0x34
+#define FSPI_AHBRX_BUF6CR0		0x38
+#define FSPI_AHBRX_BUF7CR0		0x3C
+
+#define FSPI_AHBRXBUF0CR7_PREF		BIT(31)
+
+#define FSPI_AHBRX_BUF0CR1		0x40
+#define FSPI_AHBRX_BUF1CR1		0x44
+#define FSPI_AHBRX_BUF2CR1		0x48
+#define FSPI_AHBRX_BUF3CR1		0x4C
+#define FSPI_AHBRX_BUF4CR1		0x50
+#define FSPI_AHBRX_BUF5CR1		0x54
+#define FSPI_AHBRX_BUF6CR1		0x58
+#define FSPI_AHBRX_BUF7CR1		0x5C
+
+#define FSPI_FLSHA1CR0			0x60
+#define FSPI_FLSHA2CR0			0x64
+#define FSPI_FLSHB1CR0			0x68
+#define FSPI_FLSHB2CR0			0x6C
+#define FSPI_FLSHXCR0_SZ_KB		10
+#define FSPI_FLSHXCR0_SZ(x)		((x) >> FSPI_FLSHXCR0_SZ_KB)
+
+#define FSPI_FLSHA1CR1			0x70
+#define FSPI_FLSHA2CR1			0x74
+#define FSPI_FLSHB1CR1			0x78
+#define FSPI_FLSHB2CR1			0x7C
+#define FSPI_FLSHXCR1_CSINTR(x)		((x) << 16)
+#define FSPI_FLSHXCR1_CAS(x)		((x) << 11)
+#define FSPI_FLSHXCR1_WA		BIT(10)
+#define FSPI_FLSHXCR1_TCSH(x)		((x) << 5)
+#define FSPI_FLSHXCR1_TCSS(x)		(x)
+
+#define FSPI_FLSHXCR1_TCSH_SHIFT	5
+#define FSPI_FLSHXCR1_TCSH_MASK		(0x1F << FSPI_FLSHXCR1_TCSH_SHIFT)
+#define FSPI_FLSHXCR1_TCSS_SHIFT	0
+#define FSPI_FLSHXCR1_TCSS_MASK		(0x1F << FSPI_FLSHXCR1_TCSS_SHIFT)
+
+#define FSPI_FLSHA1CR2			0x80
+#define FSPI_FLSHA2CR2			0x84
+#define FSPI_FLSHB1CR2			0x88
+#define FSPI_FLSHB2CR2			0x8C
+#define FSPI_FLSHXCR2_CLRINSP		BIT(24)
+#define FSPI_FLSHXCR2_AWRWAIT		BIT(16)
+#define FSPI_FLSHXCR2_AWRSEQN_SHIFT	13
+#define FSPI_FLSHXCR2_AWRSEQI_SHIFT	8
+#define FSPI_FLSHXCR2_ARDSEQN_SHIFT	5
+#define FSPI_FLSHXCR2_ARDSEQI_SHIFT	0
+
+#define FSPI_IPCR0			0xA0
+
+#define FSPI_IPCR1			0xA4
+#define FSPI_IPCR1_IPAREN		BIT(31)
+#define FSPI_IPCR1_SEQNUM_SHIFT		24
+#define FSPI_IPCR1_SEQID_SHIFT		16
+#define FSPI_IPCR1_IDATSZ(x)		(x)
+
+#define FSPI_IPCMD			0xB0
+#define FSPI_IPCMD_TRG			BIT(0)
+
+
+/* IP Command Register */
+#define FSPI_IPCMD_TRG_SHIFT		0
+#define FSPI_IPCMD_TRG_MASK		(1 << FSPI_IPCMD_TRG_SHIFT)
+
+#define FSPI_INTR_IPRXWA_SHIFT		5
+#define FSPI_INTR_IPRXWA_MASK		(1 << FSPI_INTR_IPRXWA_SHIFT)
+
+#define FSPI_INTR_IPCMDDONE_SHIFT	0
+#define FSPI_INTR_IPCMDDONE_MASK	(1 << FSPI_INTR_IPCMDDONE_SHIFT)
+
+#define FSPI_INTR_IPTXWE_SHIFT		6
+#define FSPI_INTR_IPTXWE_MASK		(1 << FSPI_INTR_IPTXWE_SHIFT)
+
+#define FSPI_IPTXFSTS_FILL_SHIFT	0
+#define FSPI_IPTXFSTS_FILL_MASK		(0xFF << FSPI_IPTXFSTS_FILL_SHIFT)
+
+#define FSPI_IPCR1_ISEQID_SHIFT		16
+#define FSPI_IPCR1_ISEQID_MASK		(0x1F << FSPI_IPCR1_ISEQID_SHIFT)
+
+#define FSPI_IPRXFSTS_FILL_SHIFT	0
+#define FSPI_IPRXFSTS_FILL_MASK		(0xFF << FSPI_IPRXFSTS_FILL_SHIFT)
+
+#define FSPI_DLPR			0xB4
+
+#define FSPI_IPRXFCR			0xB8
+#define FSPI_IPRXFCR_CLR		BIT(0)
+#define FSPI_IPRXFCR_DMA_EN		BIT(1)
+#define FSPI_IPRXFCR_WMRK(x)		((x) << 2)
+
+#define FSPI_IPTXFCR			0xBC
+#define FSPI_IPTXFCR_CLR		BIT(0)
+#define FSPI_IPTXFCR_DMA_EN		BIT(1)
+#define FSPI_IPTXFCR_WMRK(x)		((x) << 2)
+
+#define FSPI_DLLACR			0xC0
+#define FSPI_DLLACR_OVRDEN		BIT(8)
+
+#define FSPI_DLLBCR			0xC4
+#define FSPI_DLLBCR_OVRDEN		BIT(8)
+
+#define FSPI_STS0			0xE0
+#define FSPI_STS0_DLPHB(x)		((x) << 8)
+#define FSPI_STS0_DLPHA(x)		((x) << 4)
+#define FSPI_STS0_CMD_SRC(x)		((x) << 2)
+#define FSPI_STS0_ARB_IDLE		BIT(1)
+#define FSPI_STS0_SEQ_IDLE		BIT(0)
+
+#define FSPI_STS1			0xE4
+#define FSPI_STS1_IP_ERRCD(x)		((x) << 24)
+#define FSPI_STS1_IP_ERRID(x)		((x) << 16)
+#define FSPI_STS1_AHB_ERRCD(x)		((x) << 8)
+#define FSPI_STS1_AHB_ERRID(x)		(x)
+
+#define FSPI_AHBSPNST			0xEC
+#define FSPI_AHBSPNST_DATLFT(x)		((x) << 16)
+#define FSPI_AHBSPNST_BUFID(x)		((x) << 1)
+#define FSPI_AHBSPNST_ACTIVE		BIT(0)
+
+#define FSPI_IPRXFSTS			0xF0
+#define FSPI_IPRXFSTS_RDCNTR(x)		((x) << 16)
+#define FSPI_IPRXFSTS_FILL(x)		(x)
+
+#define FSPI_IPTXFSTS			0xF4
+#define FSPI_IPTXFSTS_WRCNTR(x)		((x) << 16)
+#define FSPI_IPTXFSTS_FILL(x)		(x)
+
+#define FSPI_NOR_SR_WIP_SHIFT		(0)
+#define FSPI_NOR_SR_WIP_MASK		(1 << FSPI_NOR_SR_WIP_SHIFT)
+
+#define FSPI_RFDR			0x100
+#define FSPI_TFDR			0x180
+
+#define FSPI_LUT_BASE			0x200
+#define FSPI_LUT_OFFSET			(SEQID_LUT * 4 * 4)
+#define FSPI_LUT_REG(idx) \
+	(FSPI_LUT_BASE + FSPI_LUT_OFFSET + (idx) * 4)
+
+/* register map end */
+
+#endif
diff --git a/drivers/nxp/flexspi/nor/test_fspi.c b/drivers/nxp/flexspi/nor/test_fspi.c
new file mode 100644
index 0000000..c36c5b8
--- /dev/null
+++ b/drivers/nxp/flexspi/nor/test_fspi.c
@@ -0,0 +1,91 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+
+#include <common/debug.h>
+#include <flash_info.h>
+#include "fspi.h"
+#include <fspi_api.h>
+
+/*
+ * The macros are defined to be used as test vector for testing fspi.
+ */
+#define	SIZE_BUFFER			0x250
+
+/*
+ * You may choose fspi_swap based on core endianness and flexspi IP/AHB
+ * buffer endianness set in MCR.
+ */
+#define fspi_swap32(A)			(A)
+
+void fspi_test(uint32_t fspi_test_addr, uint32_t size, int extra)
+{
+	uint32_t buffer[SIZE_BUFFER];
+	uint32_t count = 1;
+	uint32_t failed, i;
+
+	NOTICE("-------------------------- %d----------------------------------\n", count++);
+	INFO("Sector Erase size: 0x%08x, size: %d\n", F_SECTOR_ERASE_SZ, size);
+	/* Test Sector Erase */
+	xspi_sector_erase(fspi_test_addr - fspi_test_addr % F_SECTOR_ERASE_SZ,
+			  F_SECTOR_ERASE_SZ);
+
+	/* Test Erased data using IP read */
+	xspi_ip_read((fspi_test_addr), buffer, size * 4);
+
+	failed = 0;
+	for (i = 0; i < size; i++)
+		if (fspi_swap32(0xffffffff) != buffer[i]) {
+			failed = 1;
+			break;
+		}
+
+	if (failed == 0) {
+		NOTICE("[%d]: Success Erase: data in buffer[%d] 0x%08x\n", __LINE__, i-3, buffer[i-3]);
+	} else {
+		ERROR("Erase: Failed  -->xxx with buffer[%d]=0x%08x\n", i, buffer[i]);
+	}
+
+	for (i = 0; i < SIZE_BUFFER; i++)
+		buffer[i] = 0x12345678;
+
+	/* Write data from buffer to flash */
+	xspi_write(fspi_test_addr, (void *)buffer, (size * 4 + extra));
+	/* Check written data using IP read */
+	xspi_ip_read(fspi_test_addr, buffer, (size * 4 + extra));
+	failed = 0;
+	for (i = 0; i < size; i++)
+		if (fspi_swap32(0x12345678) != buffer[i]) {
+			failed = 1;
+			break;
+		}
+
+	if (failed == 0) {
+		NOTICE("[%d]: Success IpWrite with IP READ in buffer[%d] 0x%08x\n", __LINE__, i-3, buffer[i-3]);
+	} else {
+		ERROR("Write: Failed  -->xxxx with IP READ in buffer[%d]=0x%08x\n", i, buffer[i]);
+		return;
+	}
+
+	/* xspi_read may use AHB read */
+	xspi_read((fspi_test_addr), buffer, (size * 4 + extra));
+	failed = 0;
+	for (i = 0; i < size; i++)
+		if (fspi_swap32(0x12345678) != buffer[i]) {
+			failed = 1;
+			break;
+		}
+
+	if (failed == 0) {
+		NOTICE("[%d]: Success IpWrite with AHB OR IP READ on buffer[%d] 0x%08x\n", __LINE__, i-3, buffer[i-3]);
+	} else {
+		ERROR("Write: Failed  -->xxxx with AHB READ on buffer[%d]=0x%08x\n", i, buffer[i]);
+		return;
+	}
+}
diff --git a/drivers/nxp/gic/gic.mk b/drivers/nxp/gic/gic.mk
new file mode 100644
index 0000000..d75e071
--- /dev/null
+++ b/drivers/nxp/gic/gic.mk
@@ -0,0 +1,46 @@
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#
+#------------------------------------------------------------------------------
+#
+# Select the GIC files
+#
+# -----------------------------------------------------------------------------
+
+ifeq (${ADD_GIC},)
+ADD_GIC			:= 1
+ifeq ($(GIC), GIC400)
+include drivers/arm/gic/v2/gicv2.mk
+GIC_SOURCES		+=	${GICV2_SOURCES}
+GIC_SOURCES		+=	${PLAT_DRIVERS_PATH}/gic/ls_gicv2.c	\
+				plat/common/plat_gicv2.c
+
+PLAT_INCLUDES		+=	-I${PLAT_DRIVERS_INCLUDE_PATH}/gic/gicv2
+else
+ifeq ($(GIC), GIC500)
+include drivers/arm/gic/v3/gicv3.mk
+GIC_SOURCES		+=	${GICV3_SOURCES}
+GIC_SOURCES		+=	${PLAT_DRIVERS_PATH}/gic/ls_gicv3.c	\
+				plat/common/plat_gicv3.c
+
+PLAT_INCLUDES		+=	-I${PLAT_DRIVERS_INCLUDE_PATH}/gic/gicv3
+else
+    $(error -> GIC type not set!)
+endif
+endif
+
+ifeq (${BL_COMM_GIC_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${GIC_SOURCES}
+else
+ifeq (${BL2_GIC_NEEDED},yes)
+BL2_SOURCES		+= ${GIC_SOURCES}
+endif
+ifeq (${BL31_GIC_NEEDED},yes)
+BL31_SOURCES		+= ${GIC_SOURCES}
+endif
+endif
+endif
+
+# -----------------------------------------------------------------------------
diff --git a/drivers/nxp/gic/ls_gicv2.c b/drivers/nxp/gic/ls_gicv2.c
new file mode 100644
index 0000000..62bc8db
--- /dev/null
+++ b/drivers/nxp/gic/ls_gicv2.c
@@ -0,0 +1,76 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <gicv2.h>
+#include <plat_gic.h>
+
+
+/*
+ * NXP common helper to initialize the GICv3 only driver.
+ */
+void plat_ls_gic_driver_init(uintptr_t nxp_gicd_addr,
+			     uintptr_t nxp_gicc_addr,
+			     uint8_t plat_core_count,
+			     interrupt_prop_t *ls_interrupt_props,
+			     uint8_t ls_interrupt_prop_count,
+			     uint32_t *target_mask_array)
+{
+	static struct gicv2_driver_data ls_gic_data;
+
+	ls_gic_data.gicd_base = nxp_gicd_addr;
+	ls_gic_data.gicc_base = nxp_gicc_addr;
+	ls_gic_data.target_masks = target_mask_array;
+	ls_gic_data.target_masks_num = plat_core_count;
+	ls_gic_data.interrupt_props = ls_interrupt_props;
+	ls_gic_data.interrupt_props_num = ls_interrupt_prop_count;
+
+	gicv2_driver_init(&ls_gic_data);
+}
+
+void plat_ls_gic_init(void)
+{
+	gicv2_distif_init();
+	gicv2_pcpu_distif_init();
+	gicv2_cpuif_enable();
+}
+
+/******************************************************************************
+ * ARM common helper to enable the GICv2 CPU interface
+ *****************************************************************************/
+void plat_ls_gic_cpuif_enable(void)
+{
+	gicv2_cpuif_enable();
+}
+
+/******************************************************************************
+ * ARM common helper to disable the GICv2 CPU interface
+ *****************************************************************************/
+void plat_ls_gic_cpuif_disable(void)
+{
+	gicv2_cpuif_disable();
+}
+
+/******************************************************************************
+ * NXP common helper to initialize GICv2 per cpu
+ *****************************************************************************/
+void plat_gic_pcpu_init(void)
+{
+	gicv2_pcpu_distif_init();
+	gicv2_cpuif_enable();
+}
+
+/******************************************************************************
+ * Stubs for Redistributor power management. Although GICv2 doesn't have
+ * Redistributor interface, these are provided for the sake of uniform GIC API
+ *****************************************************************************/
+void plat_ls_gic_redistif_on(void)
+{
+}
+
+void plat_ls_gic_redistif_off(void)
+{
+}
diff --git a/drivers/nxp/gic/ls_gicv3.c b/drivers/nxp/gic/ls_gicv3.c
new file mode 100644
index 0000000..9c02bd6
--- /dev/null
+++ b/drivers/nxp/gic/ls_gicv3.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <drivers/arm/gicv3.h>
+#include <plat_gic.h>
+#include <plat/common/platform.h>
+
+/*
+ * NXP common helper to initialize the GICv3 only driver.
+ */
+void plat_ls_gic_driver_init(uintptr_t nxp_gicd_addr,
+			     uintptr_t nxp_gicr_addr,
+			     uint8_t plat_core_count,
+			     interrupt_prop_t *ls_interrupt_props,
+			     uint8_t ls_interrupt_prop_count,
+			     uintptr_t *target_mask_array,
+			     mpidr_hash_fn mpidr_to_core_pos)
+{
+	static struct gicv3_driver_data ls_gic_data;
+
+	ls_gic_data.gicd_base = nxp_gicd_addr;
+	ls_gic_data.gicr_base = nxp_gicr_addr;
+	ls_gic_data.interrupt_props = ls_interrupt_props;
+	ls_gic_data.interrupt_props_num = ls_interrupt_prop_count;
+	ls_gic_data.rdistif_num = plat_core_count;
+	ls_gic_data.rdistif_base_addrs = target_mask_array;
+	ls_gic_data.mpidr_to_core_pos = mpidr_to_core_pos;
+
+	gicv3_driver_init(&ls_gic_data);
+}
+
+void plat_ls_gic_init(void)
+{
+	gicv3_distif_init();
+	gicv3_rdistif_init(plat_my_core_pos());
+	gicv3_cpuif_enable(plat_my_core_pos());
+}
+
+/*
+ * NXP common helper to enable the GICv3 CPU interface
+ */
+void plat_ls_gic_cpuif_enable(void)
+{
+	gicv3_cpuif_enable(plat_my_core_pos());
+}
+
+/*
+ * NXP common helper to disable the GICv3 CPU interface
+ */
+void plat_ls_gic_cpuif_disable(void)
+{
+	gicv3_cpuif_disable(plat_my_core_pos());
+}
+
+/*
+ * NXP common helper to initialize the per cpu distributor interface in GICv3
+ */
+void plat_gic_pcpu_init(void)
+{
+	gicv3_rdistif_init(plat_my_core_pos());
+	gicv3_cpuif_enable(plat_my_core_pos());
+}
+
+/*
+ * Stubs for Redistributor power management. Although GICv3 doesn't have
+ * Redistributor interface, these are provided for the sake of uniform GIC API
+ */
+void plat_ls_gic_redistif_on(void)
+{
+}
+
+void plat_ls_gic_redistif_off(void)
+{
+}
diff --git a/drivers/nxp/gpio/gpio.mk b/drivers/nxp/gpio/gpio.mk
new file mode 100644
index 0000000..74f0dc4
--- /dev/null
+++ b/drivers/nxp/gpio/gpio.mk
@@ -0,0 +1,28 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#-----------------------------------------------------------------------------
+
+ifeq (${GPIO_ADDED},)
+
+GPIO_ADDED		:= 1
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/gpio
+
+GPIO_SOURCES		:= $(PLAT_DRIVERS_PATH)/gpio/nxp_gpio.c
+
+ifeq (${BL_COMM_GPIO_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${GPIO_SOURCES}
+else
+ifeq (${BL2_GPIO_NEEDED},yes)
+BL2_SOURCES		+= ${GPIO_SOURCES}
+endif
+ifeq (${BL31_GPIO_NEEDED},yes)
+BL31_SOURCES		+= ${GPIO_SOURCES}
+endif
+endif
+
+endif
+#------------------------------------------------
diff --git a/drivers/nxp/gpio/nxp_gpio.c b/drivers/nxp/gpio/nxp_gpio.c
new file mode 100644
index 0000000..28c9db9
--- /dev/null
+++ b/drivers/nxp/gpio/nxp_gpio.c
@@ -0,0 +1,144 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <nxp_gpio.h>
+
+static gpio_init_info_t *gpio_init_info;
+
+void gpio_init(gpio_init_info_t *gpio_init_data)
+{
+	gpio_init_info = gpio_init_data;
+}
+
+/* This function set GPIO pin for raising POVDD. */
+int set_gpio_bit(uint32_t *gpio_base_addr,
+		 uint32_t bit_num)
+{
+	uint32_t val = 0U;
+	uint32_t *gpdir = NULL;
+	uint32_t *gpdat = NULL;
+
+	if (gpio_init_info == NULL) {
+		ERROR("GPIO is not initialized.\n");
+		return GPIO_FAILURE;
+	}
+
+	gpdir = gpio_base_addr + GPDIR_REG_OFFSET;
+	gpdat = gpio_base_addr + (GPDAT_REG_OFFSET >> 2);
+
+	/*
+	 * Set the corresponding bit in direction register
+	 * to configure the GPIO as output.
+	 */
+	val = gpio_read32(gpdir);
+	val = val | bit_num;
+	gpio_write32(gpdir, val);
+
+	/* Set the corresponding bit in GPIO data register */
+	val = gpio_read32(gpdat);
+	val = val | bit_num;
+	gpio_write32(gpdat, val);
+
+	val = gpio_read32(gpdat);
+
+	if ((val & bit_num) == 0U) {
+		return GPIO_FAILURE;
+	}
+
+	return GPIO_SUCCESS;
+}
+
+/* This function reset GPIO pin set for raising POVDD. */
+int clr_gpio_bit(uint32_t *gpio_base_addr, uint32_t bit_num)
+{
+	uint32_t val = 0U;
+	uint32_t *gpdir = NULL;
+	uint32_t *gpdat = NULL;
+
+
+	if (gpio_init_info == NULL) {
+		ERROR("GPIO is not initialized.\n");
+		return GPIO_FAILURE;
+	}
+
+	gpdir = gpio_base_addr + GPDIR_REG_OFFSET;
+	gpdat = gpio_base_addr + GPDAT_REG_OFFSET;
+
+	/*
+	 * Reset the corresponding bit in direction and data register
+	 * to configure the GPIO as input.
+	 */
+	val = gpio_read32(gpdat);
+	val = val & ~(bit_num);
+	gpio_write32(gpdat, val);
+
+	val = gpio_read32(gpdat);
+
+	val = gpio_read32(gpdir);
+	val = val & ~(bit_num);
+	gpio_write32(gpdir, val);
+
+	val = gpio_read32(gpdat);
+
+	if ((val & bit_num) != 0U) {
+		return GPIO_FAILURE;
+	}
+
+	return GPIO_SUCCESS;
+}
+
+uint32_t *select_gpio_n_bitnum(uint32_t povdd_gpio, uint32_t *bit_num)
+{
+	uint32_t *ret_gpio;
+	uint32_t povdd_gpio_val = 0U;
+	uint32_t gpio_num = 0U;
+
+	if (gpio_init_info == NULL) {
+		ERROR("GPIO is not initialized.\n");
+	}
+	/*
+	 * Subtract 1 from fuse_hdr povdd_gpio value as
+	 * for 0x1 value, bit 0 is to be set
+	 * for 0x20 value i.e 32, bit 31 i.e. 0x1f is to be set.
+	 * 0x1f - 0x00 : GPIO_1
+	 * 0x3f - 0x20 : GPIO_2
+	 * 0x5f - 0x40 : GPIO_3
+	 * 0x7f - 0x60 : GPIO_4
+	 */
+	povdd_gpio_val = (povdd_gpio - 1U) & GPIO_SEL_MASK;
+
+	/* Right shift by 5 to divide by 32 */
+	gpio_num = povdd_gpio_val >> GPIO_ID_BASE_ADDR_SHIFT;
+	*bit_num = 1U << (GPIO_BITS_PER_BASE_REG
+			  - (povdd_gpio_val & GPIO_BIT_MASK)
+			  - 1U);
+
+	switch (gpio_num) {
+	case GPIO_0:
+		ret_gpio = (uint32_t *) gpio_init_info->gpio1_base_addr;
+		break;
+	case GPIO_1:
+		ret_gpio = (uint32_t *) gpio_init_info->gpio2_base_addr;
+		break;
+	case GPIO_2:
+		ret_gpio = (uint32_t *) gpio_init_info->gpio3_base_addr;
+		break;
+	case GPIO_3:
+		ret_gpio = (uint32_t *) gpio_init_info->gpio4_base_addr;
+		break;
+	default:
+		ret_gpio = NULL;
+	}
+
+	if (ret_gpio == NULL) {
+		INFO("GPIO_NUM = %d doesn't exist.\n", gpio_num);
+	}
+
+	return ret_gpio;
+}
diff --git a/drivers/nxp/i2c/i2c.c b/drivers/nxp/i2c/i2c.c
new file mode 100644
index 0000000..9281409
--- /dev/null
+++ b/drivers/nxp/i2c/i2c.c
@@ -0,0 +1,257 @@
+/*
+ * Copyright 2016-2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <errno.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include "i2c.h"
+#include <nxp_timer.h>
+
+static uintptr_t g_nxp_i2c_addr;
+
+void i2c_init(uintptr_t nxp_i2c_addr)
+{
+	struct ls_i2c *ccsr_i2c = (void *)nxp_i2c_addr;
+
+	g_nxp_i2c_addr = nxp_i2c_addr;
+	/* Presume workaround for erratum a009203 applied */
+	i2c_out(&ccsr_i2c->cr, I2C_CR_DIS);
+	i2c_out(&ccsr_i2c->fd, I2C_FD_CONSERV);
+	i2c_out(&ccsr_i2c->sr, I2C_SR_RST);
+	i2c_out(&ccsr_i2c->cr, I2C_CR_EN);
+}
+
+static int wait_for_state(struct ls_i2c *ccsr_i2c,
+			  unsigned char state, unsigned char mask)
+{
+	unsigned char sr;
+	uint64_t start_time = get_timer_val(0);
+	uint64_t timer;
+
+	do {
+		sr = i2c_in(&ccsr_i2c->sr);
+		if (sr & I2C_SR_AL) {
+			i2c_out(&ccsr_i2c->sr, sr);
+			WARN("I2C arbitration lost\n");
+			return -EIO;
+		}
+		if ((sr & mask) == state) {
+			return (int)sr;
+		}
+
+		timer = get_timer_val(start_time);
+		if (timer > I2C_TIMEOUT)
+			break;
+		mdelay(1);
+	} while (1);
+	WARN("I2C: Timeout waiting for state 0x%x, sr = 0x%x\n", state, sr);
+
+	return -ETIMEDOUT;
+}
+
+static int tx_byte(struct ls_i2c *ccsr_i2c, unsigned char c)
+{
+	int ret;
+
+	i2c_out(&ccsr_i2c->sr, I2C_SR_IF);
+	i2c_out(&ccsr_i2c->dr, c);
+	ret = wait_for_state(ccsr_i2c, I2C_SR_IF, I2C_SR_IF);
+	if (ret < 0) {
+		WARN("%s: state error\n", __func__);
+		return ret;
+	}
+	if (ret & I2C_SR_RX_NAK) {
+		WARN("%s: nodev\n", __func__);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int gen_stop(struct ls_i2c *ccsr_i2c)
+{
+	unsigned char cr;
+	int ret;
+
+	cr = i2c_in(&ccsr_i2c->cr);
+	cr &= ~(I2C_CR_MA | I2C_CR_TX);
+	i2c_out(&ccsr_i2c->cr, cr);
+	ret = wait_for_state(ccsr_i2c, I2C_SR_IDLE, I2C_SR_BB);
+	if (ret < 0) {
+		WARN("I2C: Generating stop failed.\n");
+	}
+	return ret;
+}
+
+static int i2c_write_addr(struct ls_i2c *ccsr_i2c, unsigned char chip,
+			  int addr, int alen)
+{
+	int ret;
+	unsigned char cr;
+
+	if (alen != 1) {
+		WARN("I2C: Unsupported address len [%d]\n", alen);
+		return -EIO;
+	}
+
+	if (i2c_in(&ccsr_i2c->ad) == (chip << 1)) {
+		WARN("I2C: slave address same as self\n");
+		return -ENODEV;
+	}
+	i2c_out(&ccsr_i2c->sr, I2C_SR_IF);
+	ret = wait_for_state(ccsr_i2c, I2C_SR_IDLE, I2C_SR_BB);
+	if (ret < 0) {
+		return ret;
+	}
+
+	cr = i2c_in(&ccsr_i2c->cr);
+	cr |= I2C_CR_MA;
+	i2c_out(&ccsr_i2c->cr, cr);
+	ret = wait_for_state(ccsr_i2c, I2C_SR_BB, I2C_SR_BB);
+	if (ret < 0) {
+		return ret;
+	}
+
+	VERBOSE("Before writing chip %d\n", chip);
+	cr |= I2C_CR_TX | I2C_CR_TX_NAK;
+	i2c_out(&ccsr_i2c->cr, cr);
+	ret = tx_byte(ccsr_i2c, chip << 1);
+	if (ret < 0) {
+		gen_stop(ccsr_i2c);
+		return ret;
+	}
+
+	VERBOSE("Before writing addr\n");
+	while (alen--) {
+		ret = tx_byte(ccsr_i2c, (addr >> (alen << 3)) & 0xff);
+		if (ret < 0) {
+			gen_stop(ccsr_i2c);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int read_data(struct ls_i2c *ccsr_i2c, unsigned char chip,
+		     unsigned char *buf, int len)
+{
+	int i;
+	int ret;
+	unsigned char cr;
+
+	cr = i2c_in(&ccsr_i2c->cr);
+	cr &= ~(I2C_CR_TX | I2C_CR_TX_NAK);
+	if (len == 1) {
+		cr |= I2C_CR_TX_NAK;
+	}
+	i2c_out(&ccsr_i2c->cr, cr);
+	i2c_out(&ccsr_i2c->sr, I2C_SR_IF);
+	i2c_in(&ccsr_i2c->dr);	/* dummy read */
+	for (i = 0; i < len; i++) {
+		ret = wait_for_state(ccsr_i2c, I2C_SR_IF, I2C_SR_IF);
+		if (ret < 0) {
+			gen_stop(ccsr_i2c);
+			return ret;
+		}
+		if (i == (len - 1)) {
+			gen_stop(ccsr_i2c);
+		} else if (i == (len - 2)) {
+			/* Updating the command to send
+			 * No ACK.
+			 */
+			cr = i2c_in(&ccsr_i2c->cr);
+			cr |= I2C_CR_TX_NAK;
+			i2c_out(&ccsr_i2c->cr, cr);
+		}
+		i2c_out(&ccsr_i2c->sr, I2C_SR_IF);
+		buf[i] = i2c_in(&ccsr_i2c->dr);
+	}
+
+	return 0;
+}
+
+static int write_data(struct ls_i2c *ccsr_i2c, unsigned char chip,
+		      const unsigned char *buf, int len)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < len; i++) {
+		ret = tx_byte(ccsr_i2c, buf[i]);
+		if (ret < 0) {
+			break;
+		}
+	}
+	ret = gen_stop(ccsr_i2c);
+
+	return ret;
+}
+
+
+int i2c_read(unsigned char chip, int addr, int alen,
+	     unsigned char *buf, int len)
+{
+	int ret;
+	unsigned char cr;
+	struct ls_i2c *ccsr_i2c = (void *)g_nxp_i2c_addr;
+
+	ret = i2c_write_addr(ccsr_i2c, chip, addr, alen);
+	if (ret < 0) {
+		gen_stop(ccsr_i2c);
+		return ret;
+	}
+
+	cr = i2c_in(&ccsr_i2c->cr);
+	cr |= I2C_CR_RSTA;
+	i2c_out(&ccsr_i2c->cr, cr);
+
+	ret = tx_byte(ccsr_i2c, (chip << 1) | 1);
+	if (ret < 0) {
+		gen_stop(ccsr_i2c);
+		return ret;
+	}
+
+	return read_data(ccsr_i2c, chip, buf, len);
+}
+
+int i2c_write(unsigned char chip, int addr, int alen,
+	      const unsigned char *buf, int len)
+{
+	int ret;
+	struct ls_i2c *ccsr_i2c = (void *)g_nxp_i2c_addr;
+
+	ret = i2c_write_addr(ccsr_i2c, chip, addr, alen);
+	if (ret < 0) {
+		return ret;
+	}
+
+	return write_data(ccsr_i2c, chip, buf, len);
+}
+
+int i2c_probe_chip(unsigned char chip)
+{
+	int ret;
+	struct ls_i2c *ccsr_i2c = (void *)g_nxp_i2c_addr;
+
+	ret = i2c_write_addr(ccsr_i2c, chip, 0, 0);
+	if (ret < 0) {
+		WARN("write addr failed\n");
+		return ret;
+	}
+
+	ret = gen_stop(ccsr_i2c);
+	if (ret < 0) {
+		WARN("I2C: Probe not complete.\n");
+	}
+
+	return ret;
+}
diff --git a/drivers/nxp/i2c/i2c.mk b/drivers/nxp/i2c/i2c.mk
new file mode 100644
index 0000000..716e14a
--- /dev/null
+++ b/drivers/nxp/i2c/i2c.mk
@@ -0,0 +1,25 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${ADD_I2C},)
+
+ADD_I2C			:= 1
+
+I2C_SOURCES		+= $(PLAT_DRIVERS_PATH)/i2c/i2c.c
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/i2c
+
+ifeq (${BL_COMM_I2C_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${I2C_SOURCES}
+else
+ifeq (${BL2_I2C_NEEDED},yes)
+BL2_SOURCES		+= ${I2C_SOURCES}
+endif
+ifeq (${BL31_I2C_NEEDED},yes)
+BL31_SOURCES		+= ${I2C_SOURCES}
+endif
+endif
+endif
diff --git a/drivers/nxp/ifc/nand/ifc.h b/drivers/nxp/ifc/nand/ifc.h
new file mode 100644
index 0000000..56c5f92
--- /dev/null
+++ b/drivers/nxp/ifc/nand/ifc.h
@@ -0,0 +1,329 @@
+/*
+ * Copyright 2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IFC_H
+#define IFC_H
+
+#include <endian.h>
+
+#include <mmio.h>
+
+#define NXP_IFC_RUN_TIME_ADDR	U(0x1000)
+
+/* CPSR - Chip Select Property Register Offset */
+#define EXT_CSPR(n)		(U(0x000C) + (n * 0xC))
+#define CSPR(n)			(U(0x0010) + (n * 0xC))
+#define CSOR(n)			(U(0x0130) + (n * 0xC))
+#define EXT_CSOR(n)		(U(0x0134) + (n * 0xC))
+#define IFC_AMASK_CS0		U(0x00A0)
+
+/* NAND specific Registers Offset */
+#define NCFGR			(NXP_IFC_RUN_TIME_ADDR + U(0x0000))
+#define NAND_FCR0		(NXP_IFC_RUN_TIME_ADDR + U(0x0014))
+
+#define ROW0			(NXP_IFC_RUN_TIME_ADDR + U(0x003C))
+#define ROW1			(NXP_IFC_RUN_TIME_ADDR + U(0x004C))
+#define COL0			(NXP_IFC_RUN_TIME_ADDR + U(0x0044))
+#define COL1			(NXP_IFC_RUN_TIME_ADDR + U(0x0054))
+
+#define NAND_BC			(NXP_IFC_RUN_TIME_ADDR + U(0x0108))
+#define NAND_FIR0		(NXP_IFC_RUN_TIME_ADDR + U(0x0110))
+#define NAND_FIR1		(NXP_IFC_RUN_TIME_ADDR + U(0x0114))
+#define NAND_FIR2		(NXP_IFC_RUN_TIME_ADDR + U(0x0118))
+#define NAND_CSEL		(NXP_IFC_RUN_TIME_ADDR + U(0x015C))
+#define NANDSEQ_STRT		(NXP_IFC_RUN_TIME_ADDR + U(0x0164))
+#define NAND_EVTER_STAT		(NXP_IFC_RUN_TIME_ADDR + U(0x016C))
+#define NAND_AUTOBOOT_TRGR	(NXP_IFC_RUN_TIME_ADDR + U(0x0284))
+
+/* Size of SRAM Buffer */
+#define CSPR_PS			U(0x00000180)
+#define CSPR_PS_SHIFT		7
+#define CSPR_PS_8		0x1 // Port Size 8 bit
+#define CSPR_PS_16		0x2 // Port Size 16 bit
+#define CSPR_PS_32		0x3 // Port Size 32 bit
+
+/* Chip Select Option Register NAND Machine */
+#define CSOR_NAND_PGS		U(0x00380000)
+#define CSOR_NAND_PGS_SHIFT	19
+#define CSOR_NAND_PGS_512	U(0x00000000)
+#define CSOR_NAND_PGS_2K	U(0x00080000)
+#define CSOR_NAND_PGS_4K	U(0x00100000)
+#define CSOR_NAND_PGS_8K	U(0x00180000)
+#define CSOR_NAND_PGS_16K	U(0x00200000)
+
+
+#define CSOR_NAND_PB			U(0x00000700)
+#define CSOR_NAND_PB_32			U(0x00000000)
+#define CSOR_NAND_PB_64			U(0x00000100)
+#define CSOR_NAND_PB_128		U(0x00000200)
+#define CSOR_NAND_PB_256		U(0x00000300)
+#define CSOR_NAND_PB_512		U(0x00000400)
+#define CSOR_NAND_PB_1024		U(0x00000500)
+#define CSOR_NAND_PB_2048		U(0x00000600)
+#define CSOR_NAND_PPB_32		32
+#define CSOR_NAND_PPB_64		64
+#define CSOR_NAND_PPB_128		128
+#define CSOR_NAND_PPB_256		256
+#define CSOR_NAND_PPB_512		512
+#define CSOR_NAND_PPB_1024		1024
+#define CSOR_NAND_PPB_2048		2048
+
+/* NAND Chip select register */
+#define NAND_CSEL_SHIFT			26
+#define NAND_COL_MS_SHIFT		31
+
+/* FCR - Flash Command Register */
+#define FCR_CMD0			U(0xFF000000)
+#define FCR_CMD0_SHIFT			24
+#define FCR_CMD1			U(0x00FF0000)
+#define FCR_CMD1_SHIFT			16
+#define FCR_CMD2			U(0x0000FF00)
+#define FCR_CMD2_SHIFT			8
+#define FCR_CMD3			U(0x000000FF)
+#define FCR_CMD3_SHIFT			0
+
+/* FIR - Flash Instruction Register Opcode */
+#define FIR_OP0				U(0xFC000000)
+#define FIR_OP0_SHIFT			26
+#define FIR_OP1				U(0x03F00000)
+#define FIR_OP1_SHIFT			20
+#define FIR_OP2				U(0x000FC000)
+#define FIR_OP2_SHIFT			14
+#define FIR_OP3				U(0x00003F00)
+#define FIR_OP3_SHIFT			8
+#define FIR_OP4				U(0x000000FC)
+#define FIR_OP4_SHIFT			2
+#define FIR_OP5				U(0xFC000000)
+#define FIR_OP5_SHIFT			26
+#define FIR_OP6				U(0x03F00000)
+#define FIR_OP6_SHIFT			20
+
+/* Instruction Opcode - 6 bits */
+#define FIR_OP_NOP			0x00
+#define FIR_OP_CA0			0x01 /* Issue current column address */
+#define FIR_OP_CA1			0x02 /* Issue current column address */
+#define FIR_OP_RA0			0x05 /* Issue current column address */
+#define FIR_OP_RA1			0x06 /* Issue current column address */
+#define FIR_OP_CMD0			0x09 /* Issue command from FCR[CMD0] */
+#define FIR_OP_CMD1			0x0a /* Issue command from FCR[CMD1] */
+#define FIR_OP_CMD2			0x0b /* Issue command from FCR[CMD2] */
+#define FIR_OP_CMD3			0x0c /* Issue command from FCR[CMD3] */
+#define FIR_OP_CW0			0x11 /* Wait then issue FCR[CMD0] */
+#define FIR_OP_CW1			0x12 /* Wait then issue FCR[CMD1] */
+#define FIR_OP_CW2			0x13 /* Wait then issue FCR[CMD1] */
+#define FIR_OP_CW3			0x14 /* Wait then issue FCR[CMD1] */
+#define FIR_OP_WBCD			0x19 /* Wait then read FBCR bytes */
+#define FIR_OP_RBCD			0x1a /* Wait then read 1 or 2 bytes */
+#define FIR_OP_BTRD			0x1b /* Wait then read 1 or 2 bytes */
+#define FIR_OP_RDSTAT			0x1c /* Wait then read 1 or 2 bytes */
+#define FIR_OP_NWAIT			0x1d /* Wait then read 1 or 2 bytes */
+#define FIR_OP_WFR			0x1e /* Wait then read 1 or 2 bytes */
+
+#define NAND_SEQ_STRT_FIR_STRT		U(0x80000000)
+#define NAND_SEQ_STRT_FIR_STRT_SHIFT	31
+
+#define NAND_EVTER_STAT_FTOER		U(0x08000000)
+#define NAND_EVTER_STAT_WPER		U(0x04000000)
+#define NAND_EVTER_STAT_ECCER		U(0x02000000)
+#define NAND_EVTER_STAT_DQSER		U(0x01000000)
+#define NAND_EVTER_STAT_RCW_DN		U(0x00008000)
+#define NAND_EVTER_STAT_BOOT_DN		U(0x00004000)
+#define NAND_EVTER_STAT_RCW_DN		U(0x00008000)
+#define NAND_EVTER_STAT_OPC_DN		U(0x80000000)
+#define NAND_EVTER_STAT_BBI_SRCH_SEL	U(0x00000800)
+#define NCFGR_BOOT			U(0x80000000)
+#define NAND_AUTOBOOT_TRGR_RCW_LD	U(0x80000000)
+#define NAND_AUTOBOOT_TRGR_BOOT_LD	U(0x20000000)
+
+/* ECC ERROR STATUS Registers */
+#define NAND_RCW_LD			U(0x80000000)
+#define NAND_BOOT_LD			U(0x20000000)
+
+/*Other Temp Defines */
+/*256 bad Blocks supported */
+#define BBT_SIZE			256
+
+/*Standard NAND flash commands */
+#define NAND_CMD_READ0			0
+#define NAND_CMD_READ1			1
+#define NAND_CMD_READOOB		0x50
+
+/*Extended commands for large page devices */
+#define NAND_CMD_READSTART		0x30
+
+#define NAND_TIMEOUT_MS			40
+
+#define EMPTY_VAL_CHECK			U(0xFFFFFFFF)
+#define EMPTY_VAL			0xFF
+
+
+#define MAIN				0
+#define SPARE				1
+
+#define GOOD_BLK			1
+#define BAD_BLK				0
+#define DIV_2				2
+
+#define ATTRIBUTE_PGSZ			0xa
+#define ATTRIBUTE_PPB			0xb
+
+#define CSPR_PORT_SIZE_8		(0x1 << 7)
+#define CSPR_PORT_SIZE_16		(0x2 << 7)
+#define CSPR_PORT_SIZE_32		(0x3 << 7)
+
+/* NAND specific */
+#define RCW_SRC_NAND_PORT_MASK		U(0x00000080)
+
+#define NAND_DEFAULT_CSPR		U(0x00000053)
+#define NAND_DEFAULT_CSOR		U(0x0180C00C)
+#define NAND_DEFAULT_EXT_CSPR		U(0x00000000)
+#define NAND_DEFAULT_EXT_CSOR		U(0x00000000)
+#define NAND_DEFAULT_FTIM0		U(0x181c0c10)
+#define NAND_DEFAULT_FTIM1		U(0x5454141e)
+#define NAND_DEFAULT_FTIM2		U(0x03808034)
+#define NAND_DEFAULT_FTIM3		U(0x2c000000)
+
+#define NAND_CSOR_ECC_MODE_DISABLE	U(0x00000000)
+#define NAND_CSOR_ECC_MODE0		U(0x84000000)
+#define NAND_CSOR_ECC_MODE1		U(0x94000000)
+#define NAND_CSOR_ECC_MODE2		U(0xa4000000)
+#define NAND_CSOR_ECC_MODE3		U(0xb4000000)
+#define NAND_CSOR_PAGE_SIZE_2K		(0x1 << 19)
+#define NAND_CSOR_PAGE_SIZE_4K		(0x2 << 19)
+#define NAND_CSOR_PAGE_SIZE_8K		(0x3 << 19)
+#define NAND_CSOR_PAGE_SIZE_16K		(0x4 << 19)
+#define NAND_CSOR_PPB_64		(0x1 << 8)
+#define NAND_CSOR_PPB_128		(0x2 << 8)
+#define NAND_CSOR_PPB_256		(0x3 << 8)
+#define NAND_CSOR_PPB_512		(0x4 << 8)
+
+/* BBI INDICATOR for NAND_2K(CFG_RCW_SRC[1]) for
+ * devices greater than 2K page size(CFG_RCW_SRC[3])
+ */
+#define RCW_SRC_NAND_BBI_MASK		U(0x00000008)
+#define RCW_SRC_NAND_BBI_MASK_NAND_2K	U(0x00000002)
+#define NAND_BBI_ONFI_2K		(0x1 << 1)
+#define NAND_BBI_ONFI			(0x1 << 3)
+
+#define RCW_SRC_NAND_PAGE_MASK		U(0x00000070)
+#define RCW_SRC_NAND_PAGE_MASK_NAND_2K	U(0x0000000C)
+#define NAND_2K_XXX			0x00
+#define NAND_2K_64			0x04
+#define NAND_2K_128			0x08
+#define NAND_4K_128			0x10
+#define NAND_4K_256			0x20
+#define NAND_4K_512			0x30
+#define NAND_8K_128			0x40
+#define NAND_8K_256			0x50
+#define NAND_8K_512			0x60
+#define NAND_16K_512			0x70
+#define BLOCK_LEN_2K			2048
+
+#define RCW_SRC_NAND_ECC_MASK		U(0x00000007)
+#define RCW_SRC_NAND_ECC_MASK_NAND_2K	U(0x00000001)
+#define NAND_ECC_DISABLE		0x0
+#define NAND_ECC_4_520			0x1
+#define NAND_ECC_8_528			0x5
+#define NAND_ECC_24_1K			0x6
+#define NAND_ECC_40_1K			0x7
+
+#define NAND_SPARE_2K			U(0x00000040)
+#define NAND_SPARE_4K_ECC_M0		U(0x00000080)
+#define NAND_SPARE_4K_ECC_M1		U(0x000000D2)
+#define NAND_SPARE_4K_ECC_M2		U(0x000000B0)
+#define NAND_SPARE_4K_ECC_M3		U(0x00000120)
+#define NAND_SPARE_8K_ECC_M0		U(0x00000088)
+#define NAND_SPARE_8K_ECC_M1		U(0x00000108)
+#define NAND_SPARE_8K_ECC_M2		U(0x00000158)
+#define NAND_SPARE_8K_ECC_M3		U(0x00000238)
+#define NAND_SPARE_16K_ECC_M0		U(0x00000108)
+#define NAND_SPARE_16K_ECC_M1		U(0x00000208)
+#define NAND_SPARE_16K_ECC_M2		U(0x000002A8)
+#define NAND_SPARE_16K_ECC_M3		U(0x00000468)
+
+struct nand_info {
+	uintptr_t ifc_register_addr;
+	uintptr_t ifc_region_addr;
+	uint32_t page_size;
+	uint32_t port_size;
+	uint32_t blk_size;
+	uint32_t ppb;
+	uint32_t pi_width;	/* Bits Required to index a page in block */
+	uint32_t ral;
+	uint32_t ibr_flow;
+	uint32_t bbt[BBT_SIZE];
+	uint32_t lgb;		/* Last Good Block */
+	uint32_t bbt_max;	/* Total entries in bbt */
+	uint32_t bzero_good;
+	uint8_t bbs;
+	uint8_t bad_marker_loc;
+	uint8_t onfi_dev_flag;
+	uint8_t init_time_boot_flag;
+	uint8_t *buf;
+};
+
+struct ifc_regs {
+	uint32_t ext_cspr;
+	uint32_t cspr;
+	uint32_t csor;
+	uint32_t ext_csor;
+};
+
+struct sec_nand_info {
+	uint32_t cspr_port_size;
+	uint32_t csor_ecc_mode;
+	uint32_t csor_page_size;
+	uint32_t csor_ppb;
+	uint32_t ext_csor_spare_size;
+	uint32_t onfi_flag;
+};
+
+struct sec_nor_info {
+	uint32_t cspr_port_size;
+	uint32_t csor_nor_mode;
+	uint32_t csor_adm_shift;
+	uint32_t port_size;
+	uint32_t addr_bits;
+};
+
+enum ifc_chip_sel {
+	IFC_CS0,
+	IFC_CS1,
+	IFC_CS2,
+	IFC_CS3,
+	IFC_CS4,
+	IFC_CS5,
+	IFC_CS6,
+	IFC_CS7,
+};
+
+enum ifc_ftims {
+	IFC_FTIM0,
+	IFC_FTIM1,
+	IFC_FTIM2,
+	IFC_FTIM3,
+};
+
+#ifdef NXP_IFC_BE
+#define nand_in32(a)		bswap32(mmio_read_32((uintptr_t)a))
+#define nand_out32(a, v)	mmio_write_32((uintptr_t)a, bswap32(v))
+#else
+#define nand_in32(a)		mmio_read_32((uintptr_t)a)
+#define nand_out32(a, v)	mmio_write_32((uintptr_t)a, v)
+#endif
+
+/* Read Write on IFC registers */
+static inline void write_reg(struct nand_info *nand, uint32_t reg, uint32_t val)
+{
+	nand_out32(nand->ifc_register_addr + reg, val);
+}
+
+static inline uint32_t read_reg(struct nand_info *nand, uint32_t reg)
+{
+	return nand_in32(nand->ifc_register_addr + reg);
+}
+
+#endif /* IFC_H */
diff --git a/drivers/nxp/ifc/nand/ifc_nand.c b/drivers/nxp/ifc/nand/ifc_nand.c
new file mode 100644
index 0000000..1f7092a
--- /dev/null
+++ b/drivers/nxp/ifc/nand/ifc_nand.c
@@ -0,0 +1,658 @@
+/*
+ * Copyright 2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/io/io_block.h>
+#include "ifc.h"
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <nxp_timer.h>
+
+/* Private structure for NAND driver data */
+static struct nand_info nand_drv_data;
+
+static int update_bbt(uint32_t idx, uint32_t blk, uint32_t *updated,
+		struct nand_info *nand);
+
+static int nand_wait(struct nand_info *nand)
+{
+	int timeout = 1;
+	uint32_t  neesr;
+	unsigned long start_time;
+
+	start_time = get_timer_val(0);
+
+	while (get_timer_val(start_time) < NAND_TIMEOUT_MS) {
+		/* clear the OPC event */
+		neesr = read_reg(nand, NAND_EVTER_STAT);
+		if (neesr & NAND_EVTER_STAT_OPC_DN) {
+			write_reg(nand, NAND_EVTER_STAT, neesr);
+			timeout = 0;
+
+			/* check for other errors */
+			if (neesr & NAND_EVTER_STAT_FTOER) {
+				ERROR("%s NAND_EVTER_STAT_FTOER occurs\n",
+						__func__);
+				return -1;
+			} else if (neesr & NAND_EVTER_STAT_ECCER) {
+				ERROR("%s NAND_EVTER_STAT_ECCER occurs\n",
+						__func__);
+				return -1;
+			} else if (neesr & NAND_EVTER_STAT_DQSER) {
+				ERROR("%s NAND_EVTER_STAT_DQSER occurs\n",
+						__func__);
+				return -1;
+			}
+
+			break;
+		}
+	}
+
+	if (timeout) {
+		ERROR("%s ERROR_NAND_TIMEOUT occurs\n", __func__);
+		return -1;
+	}
+
+	return 0;
+}
+
+static uint32_t nand_get_port_size(struct nand_info *nand)
+{
+	uint32_t port_size = U(0);
+	uint32_t cs_reg;
+	uint32_t cur_cs;
+
+	cur_cs = U(0);
+	cs_reg = CSPR(cur_cs);
+	port_size = (read_reg(nand, cs_reg) & CSPR_PS) >> CSPR_PS_SHIFT;
+	switch (port_size) {
+	case CSPR_PS_8:
+		port_size = U(8);
+		break;
+	case CSPR_PS_16:
+		port_size = U(16);
+		break;
+	case CSPR_PS_32:
+		port_size = U(32);
+		break;
+	default:
+		port_size = U(8);
+	}
+
+	return port_size;
+}
+
+static uint32_t nand_get_page_size(struct nand_info *nand)
+{
+	uint32_t pg_size;
+	uint32_t cs_reg;
+	uint32_t cur_cs;
+
+	cur_cs = 0;
+	cs_reg = CSOR(cur_cs);
+	pg_size = read_reg(nand, cs_reg) & CSOR_NAND_PGS;
+	switch (pg_size) {
+	case CSOR_NAND_PGS_2K:
+		pg_size = U(2048);
+		break;
+	case CSOR_NAND_PGS_4K:
+		pg_size = U(4096);
+		break;
+	case CSOR_NAND_PGS_8K:
+		pg_size = U(8192);
+		break;
+	case CSOR_NAND_PGS_16K:
+		pg_size = U(16384);
+		break;
+	default:
+		pg_size = U(512);
+	}
+
+	return pg_size;
+}
+
+static uint32_t nand_get_pages_per_blk(struct nand_info *nand)
+{
+	uint32_t pages_per_blk;
+	uint32_t cs_reg;
+	uint32_t cur_cs;
+
+	cur_cs = 0;
+	cs_reg = CSOR(cur_cs);
+	pages_per_blk = (read_reg(nand, cs_reg) & CSOR_NAND_PB);
+	switch (pages_per_blk) {
+	case CSOR_NAND_PB_32:
+		pages_per_blk = U(32);
+		break;
+	case CSOR_NAND_PB_64:
+		pages_per_blk = U(64);
+		break;
+	case CSOR_NAND_PB_128:
+		pages_per_blk = U(128);
+		break;
+	case CSOR_NAND_PB_256:
+		pages_per_blk = U(256);
+		break;
+	case CSOR_NAND_PB_512:
+		pages_per_blk = U(512);
+		break;
+	case CSOR_NAND_PB_1024:
+		pages_per_blk = U(1024);
+		break;
+	case CSOR_NAND_PB_2048:
+		pages_per_blk = U(2048);
+		break;
+	default:
+		pages_per_blk = U(0);
+	}
+
+	return pages_per_blk;
+}
+
+static uint32_t get_page_index_width(uint32_t ppb)
+{
+	switch (ppb) {
+	case CSOR_NAND_PPB_32:
+		return U(5);
+	case CSOR_NAND_PPB_64:
+		return U(6);
+	case CSOR_NAND_PPB_128:
+		return U(7);
+	case CSOR_NAND_PPB_256:
+		return U(8);
+	case CSOR_NAND_PPB_512:
+		return U(9);
+	case CSOR_NAND_PPB_1024:
+		return U(10);
+	case CSOR_NAND_PPB_2048:
+		return U(11);
+	default:
+		return U(5);
+	}
+}
+
+static void nand_get_params(struct nand_info *nand)
+{
+	nand->port_size = nand_get_port_size(nand);
+
+	nand->page_size = nand_get_page_size(nand);
+
+	/*
+	 * Set Bad marker Location for LP / SP
+	 * Small Page : 8 Bit	 : 0x5
+	 * Small Page : 16 Bit	: 0xa
+	 * Large Page : 8 /16 Bit : 0x0
+	 */
+	nand->bad_marker_loc = (nand->page_size == 512) ?
+				((nand->port_size == 8) ? 0x5 : 0xa) : 0;
+
+	/* check for the device is ONFI compliant or not */
+	nand->onfi_dev_flag =
+	   (read_reg(nand, NAND_EVTER_STAT) & NAND_EVTER_STAT_BBI_SRCH_SEL)
+	   ? 1 : 0;
+
+	/* NAND Blk serached count for incremental Bad block search cnt */
+	nand->bbs = 0;
+
+	/* pages per Block */
+	nand->ppb = nand_get_pages_per_blk(nand);
+
+	/* Blk size */
+	nand->blk_size = nand->page_size * nand->ppb;
+
+	/* get_page_index_width */
+	nand->pi_width = get_page_index_width(nand->ppb);
+
+	/* bad block table init */
+	nand->lgb = 0;
+	nand->bbt_max = 0;
+	nand->bzero_good = 0;
+	memset(nand->bbt, EMPTY_VAL, BBT_SIZE * sizeof(nand->bbt[0]));
+}
+
+static int nand_init(struct nand_info *nand)
+{
+	uint32_t ncfgr = 0;
+
+	/* Get nand Parameters from IFC */
+	nand_get_params(nand);
+
+	/* Clear all errors */
+	write_reg(nand, NAND_EVTER_STAT, U(0xffffffff));
+
+	/*
+	 * Disable autoboot in NCFGR. Mapping will change from
+	 * physical to logical for SRAM buffer
+	 */
+	ncfgr = read_reg(nand, NCFGR);
+	write_reg(nand, NCFGR, (ncfgr & ~NCFGR_BOOT));
+
+	return 0;
+}
+
+static int nand_read_data(
+		uintptr_t ifc_region_addr,
+		uint32_t row_add,
+		uint32_t col_add,
+		uint32_t byte_cnt,
+		uint8_t *data,
+		uint32_t main_spare,
+		struct nand_info *nand)
+{
+	uint32_t page_size_add_bits = U(0);
+	uint32_t page_add_in_actual, page_add;
+	uintptr_t sram_addr_calc;
+	int ret;
+	uint32_t col_val;
+
+	/* Programming MS bit to read from spare area.*/
+	col_val = (main_spare << NAND_COL_MS_SHIFT) | col_add;
+
+	write_reg(nand, NAND_BC, byte_cnt);
+
+	write_reg(nand, ROW0, row_add);
+	write_reg(nand, COL0, col_val);
+
+	/* Program FCR for small Page */
+	if (nand->page_size == U(512)) {
+		if (byte_cnt == 0 ||
+			(byte_cnt != 0  && main_spare == 0 && col_add <= 255)) {
+			write_reg(nand, NAND_FCR0,
+				  (NAND_CMD_READ0 << FCR_CMD0_SHIFT));
+		} else if (main_spare == 0) {
+			write_reg(nand, NAND_FCR0,
+				  (NAND_CMD_READ1 << FCR_CMD0_SHIFT));
+		} else {
+			write_reg(nand, NAND_FCR0,
+				  (NAND_CMD_READOOB << FCR_CMD0_SHIFT));
+		}
+
+	} else {
+		/* Program FCR for Large Page */
+		write_reg(nand, NAND_FCR0, (NAND_CMD_READ0 << FCR_CMD0_SHIFT) |
+			  (NAND_CMD_READSTART << FCR_CMD1_SHIFT));
+	}
+	if (nand->page_size == U(512)) {
+		write_reg(nand, NAND_FIR0, ((FIR_OP_CW0 << FIR_OP0_SHIFT) |
+					  (FIR_OP_CA0 << FIR_OP1_SHIFT) |
+					  (FIR_OP_RA0 << FIR_OP2_SHIFT) |
+					  (FIR_OP_BTRD << FIR_OP3_SHIFT) |
+					  (FIR_OP_NOP << FIR_OP4_SHIFT)));
+		write_reg(nand, NAND_FIR1, U(0x00000000));
+	} else {
+		write_reg(nand, NAND_FIR0, ((FIR_OP_CW0 << FIR_OP0_SHIFT) |
+					 (FIR_OP_CA0 << FIR_OP1_SHIFT) |
+					 (FIR_OP_RA0 << FIR_OP2_SHIFT) |
+					 (FIR_OP_CMD1 << FIR_OP3_SHIFT) |
+					 (FIR_OP_BTRD << FIR_OP4_SHIFT)));
+
+		write_reg(nand, NAND_FIR1, (FIR_OP_NOP << FIR_OP5_SHIFT));
+	}
+	write_reg(nand, NANDSEQ_STRT, NAND_SEQ_STRT_FIR_STRT);
+
+	ret = nand_wait(nand);
+	if (ret != 0)
+		return ret;
+
+	/* calculate page_size_add_bits i.e bits
+	 * in sram address corresponding to area
+	 * within a page for sram
+	 */
+	if (nand->page_size == U(512))
+		page_size_add_bits = U(10);
+	else if (nand->page_size == U(2048))
+		page_size_add_bits = U(12);
+	else if (nand->page_size == U(4096))
+		page_size_add_bits = U(13);
+	else if (nand->page_size == U(8192))
+		page_size_add_bits = U(14);
+	else if (nand->page_size == U(16384))
+		page_size_add_bits = U(15);
+
+	page_add = row_add;
+
+	page_add_in_actual = (page_add << page_size_add_bits) & U(0x0000FFFF);
+
+	if (byte_cnt == 0)
+		col_add = U(0);
+
+	/* Calculate SRAM address for main and spare area */
+	if (main_spare == 0)
+		sram_addr_calc = ifc_region_addr | page_add_in_actual | col_add;
+	else
+		sram_addr_calc = ifc_region_addr | page_add_in_actual |
+				 (col_add + nand->page_size);
+
+	/* Depending Byte_count copy full page or partial page from SRAM */
+	if (byte_cnt == 0)
+		memcpy(data, (void *)sram_addr_calc,
+			nand->page_size);
+	else
+		memcpy(data, (void *)sram_addr_calc, byte_cnt);
+
+	return 0;
+}
+
+static int nand_read(struct nand_info *nand, int32_t src_addr,
+		uintptr_t dst, uint32_t size)
+{
+	uint32_t log_blk = U(0);
+	uint32_t pg_no = U(0);
+	uint32_t col_off = U(0);
+	uint32_t row_off = U(0);
+	uint32_t byte_cnt = U(0);
+	uint32_t read_cnt = U(0);
+	uint32_t i = U(0);
+	uint32_t updated = U(0);
+
+	int ret = 0;
+	uint8_t *out = (uint8_t *)dst;
+
+	uint32_t pblk;
+
+	/* loop till size */
+	while (size) {
+		log_blk = (src_addr / nand->blk_size);
+		pg_no = ((src_addr - (log_blk * nand->blk_size)) /
+					 nand->page_size);
+		pblk = log_blk;
+
+		 // iterate the bbt to find the block
+		for (i = 0; i <= nand->bbt_max; i++) {
+			if (nand->bbt[i] == EMPTY_VAL_CHECK) {
+				ret = update_bbt(i, pblk, &updated, nand);
+
+				if (ret != 0)
+					return ret;
+				 /*
+				  * if table not updated and we reached
+				  * end of table
+				  */
+				if (!updated)
+					break;
+			}
+
+			if (pblk < nand->bbt[i])
+				break;
+			else if (pblk >= nand->bbt[i])
+				pblk++;
+		}
+
+		col_off = (src_addr % nand->page_size);
+		if (col_off) {
+			if ((col_off + size) < nand->page_size)
+				byte_cnt = size;
+			else
+				byte_cnt = nand->page_size - col_off;
+
+			row_off = (pblk << nand->pi_width) | pg_no;
+
+			ret = nand_read_data(
+					nand->ifc_region_addr,
+					row_off,
+					col_off,
+					byte_cnt, out, MAIN, nand);
+
+			if (ret != 0)
+				return ret;
+		} else {
+			 /*
+			  * fullpage/Partial Page
+			  * if byte_cnt = 0 full page
+			  * else partial page
+			  */
+			if (size < nand->page_size) {
+				byte_cnt = size;
+				read_cnt = size;
+			} else	{
+				byte_cnt = nand->page_size;
+				read_cnt = 0;
+			}
+			row_off = (pblk << nand->pi_width) | pg_no;
+
+			ret = nand_read_data(
+					nand->ifc_region_addr,
+					row_off,
+					0,
+					read_cnt, out, MAIN, nand);
+
+			if (ret != 0) {
+				ERROR("Error from nand-read_data %d\n", ret);
+				return ret;
+			}
+		}
+		src_addr += byte_cnt;
+		out += byte_cnt;
+		size -= byte_cnt;
+	}
+	return 0;
+}
+
+static int isgoodblock(uint32_t blk, uint32_t *gb, struct nand_info *nand)
+{
+	uint8_t buf[2];
+	int ret;
+	uint32_t row_add;
+
+	*gb = 0;
+
+	/* read Page 0 of blk */
+	ret = nand_read_data(
+			nand->ifc_region_addr,
+			blk << nand->pi_width,
+			nand->bad_marker_loc,
+			0x2, buf, 1, nand);
+
+	if (ret != 0)
+		return ret;
+
+	/* For ONFI devices check Page 0 and Last page of block for
+	 * Bad Marker and for NON-ONFI Page 0 and 1 for Bad Marker
+	 */
+	row_add = (blk << nand->pi_width);
+	if (nand->port_size == 8) {
+		/* port size is 8 Bit */
+		/* check if page 0 has 0xff */
+		if (buf[0] == 0xff) {
+			/* check page 1 */
+			if (nand->onfi_dev_flag)
+				ret =  nand_read_data(
+						nand->ifc_region_addr,
+						row_add | (nand->ppb - 1),
+						nand->bad_marker_loc,
+						0x2, buf, SPARE, nand);
+			else
+				ret =  nand_read_data(
+						nand->ifc_region_addr,
+						row_add | 1,
+						nand->bad_marker_loc,
+						0x2, buf, SPARE, nand);
+
+			if (ret != 0)
+				return ret;
+
+			if (buf[0] == 0xff)
+				*gb = GOOD_BLK;
+			else
+				*gb = BAD_BLK;
+		} else {
+			/* no, so it is bad blk */
+			*gb = BAD_BLK;
+		}
+	} else {
+		/* Port size 16-Bit */
+		/* check if page 0 has 0xffff */
+		if ((buf[0] == 0xff) &&
+			(buf[1] == 0xff)) {
+			/* check page 1 for 0xffff */
+			if (nand->onfi_dev_flag) {
+				ret =  nand_read_data(
+						nand->ifc_region_addr,
+						row_add | (nand->ppb - 1),
+						nand->bad_marker_loc,
+						0x2, buf, SPARE, nand);
+			} else {
+				ret =  nand_read_data(
+						nand->ifc_region_addr,
+						row_add | 1,
+						nand->bad_marker_loc,
+						0x2, buf, SPARE, nand);
+			}
+
+			if (ret != 0)
+				return ret;
+
+			if ((buf[0] == 0xff) &&
+				(buf[1] == 0xff)) {
+				*gb = GOOD_BLK;
+			} else {
+				*gb = BAD_BLK;
+			}
+		} else {
+			/* no, so it is bad blk */
+			*gb = BAD_BLK;
+		}
+	}
+	return 0;
+}
+
+static int update_bbt(uint32_t idx, uint32_t blk,
+			   uint32_t *updated,  struct nand_info *nand)
+{
+	uint32_t sblk;
+	uint32_t lgb;
+	int ret;
+
+	if (nand->bzero_good && blk == 0)
+		return 0;
+
+	/* special case for lgb == 0 */
+	/* if blk <= lgb retrun */
+	if (nand->lgb != 0 && blk <= nand->lgb)
+		return 0;
+
+	*updated = 0;
+
+	/* if blk is more than lgb, iterate from lgb till a good block
+	 * is found for blk
+	 */
+
+	if (nand->lgb < blk)
+		sblk = nand->lgb;
+	else
+		/* this is when lgb = 0 */
+		sblk = blk;
+
+
+	lgb = nand->lgb;
+
+	/* loop from blk to find a good block */
+	while (1) {
+		while (lgb <= sblk) {
+			uint32_t gb = 0;
+
+			ret =  isgoodblock(lgb, &gb, nand);
+			if (ret != 0)
+				return ret;
+
+			/* special case block 0 is good then set this flag */
+			if (lgb == 0 && gb == GOOD_BLK)
+				nand->bzero_good = 1;
+
+			if (gb == BAD_BLK) {
+				if (idx >= BBT_SIZE) {
+					ERROR("NAND BBT Table full\n");
+					return -1;
+				}
+				*updated = 1;
+				nand->bbt[idx] = lgb;
+				idx++;
+				blk++;
+				sblk++;
+				if (idx > nand->bbt_max)
+					nand->bbt_max = idx;
+			}
+			lgb++;
+		}
+		/* the access block found */
+		if (sblk == blk) {
+			/* when good block found update lgb */
+			nand->lgb =  blk;
+			break;
+		}
+		sblk++;
+	}
+
+	return 0;
+}
+
+static size_t ifc_nand_read(int lba, uintptr_t buf, size_t size)
+{
+	int ret;
+	uint32_t page_size;
+	uint32_t src_addr;
+	struct nand_info *nand = &nand_drv_data;
+
+	page_size = nand_get_page_size(nand);
+	src_addr = lba * page_size;
+	ret = nand_read(nand, src_addr, buf, size);
+	return ret ? 0 : size;
+}
+
+static struct io_block_dev_spec ifc_nand_spec = {
+	.buffer = {
+		.offset = 0,
+		.length = 0,
+	},
+	.ops = {
+		.read = ifc_nand_read,
+	},
+	/*
+	 * Default block size assumed as 2K
+	 * Would be updated based on actual size
+	 */
+	.block_size = UL(2048),
+};
+
+int ifc_nand_init(uintptr_t *block_dev_spec,
+			uintptr_t ifc_region_addr,
+			uintptr_t ifc_register_addr,
+			size_t ifc_sram_size,
+			uintptr_t ifc_nand_blk_offset,
+			size_t ifc_nand_blk_size)
+{
+	struct nand_info *nand = NULL;
+	int ret;
+
+	nand = &nand_drv_data;
+	memset(nand, 0, sizeof(struct nand_info));
+
+	nand->ifc_region_addr = ifc_region_addr;
+	nand->ifc_register_addr = ifc_register_addr;
+
+	VERBOSE("nand_init\n");
+	ret = nand_init(nand);
+	if (ret) {
+		ERROR("nand init failed\n");
+		return ret;
+	}
+
+	ifc_nand_spec.buffer.offset = ifc_nand_blk_offset;
+	ifc_nand_spec.buffer.length = ifc_nand_blk_size;
+
+	ifc_nand_spec.block_size = nand_get_page_size(nand);
+
+	VERBOSE("Page size is %ld\n", ifc_nand_spec.block_size);
+
+	*block_dev_spec = (uintptr_t)&ifc_nand_spec;
+
+	/* Adding NAND SRAM< Buffer in XLAT Table */
+	mmap_add_region(ifc_region_addr, ifc_region_addr,
+			ifc_sram_size, MT_DEVICE | MT_RW);
+
+	return 0;
+}
diff --git a/drivers/nxp/ifc/nand/ifc_nand.mk b/drivers/nxp/ifc/nand/ifc_nand.mk
new file mode 100644
index 0000000..890fd23
--- /dev/null
+++ b/drivers/nxp/ifc/nand/ifc_nand.mk
@@ -0,0 +1,29 @@
+#
+# Copyright 2022 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${NAND_ADDED},)
+
+NAND_ADDED		:= 1
+
+NAND_DRIVERS_PATH	:=  ${PLAT_DRIVERS_PATH}/ifc/nand
+
+NAND_SOURCES		:=  $(NAND_DRIVERS_PATH)/ifc_nand.c \
+			    drivers/io/io_block.c
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/ifc
+
+ifeq (${BL_COMM_IFC_NAND_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${NAND_SOURCES}
+else
+ifeq (${BL2_IFC_NAND_NEEDED},yes)
+BL2_SOURCES		+= ${NAND_SOURCES}
+endif
+ifeq (${BL31_IFC_NAND_NEEDED},yes)
+BL31_SOURCES		+= ${NAND_SOURCES}
+endif
+endif
+
+endif
diff --git a/drivers/nxp/ifc/nor/ifc_nor.c b/drivers/nxp/ifc/nor/ifc_nor.c
new file mode 100644
index 0000000..24fc308
--- /dev/null
+++ b/drivers/nxp/ifc/nor/ifc_nor.c
@@ -0,0 +1,18 @@
+/*
+ * Copyright 2020-2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <lib/xlat_tables/xlat_tables_v2.h>
+
+int ifc_nor_init(uintptr_t flash_addr, size_t flash_size)
+{
+	/* Adding NOR Memory Map in XLAT Table */
+	mmap_add_region(flash_addr, flash_addr, flash_size, MT_MEMORY | MT_RW);
+
+	return 0;
+}
diff --git a/drivers/nxp/ifc/nor/ifc_nor.mk b/drivers/nxp/ifc/nor/ifc_nor.mk
new file mode 100644
index 0000000..0022a81
--- /dev/null
+++ b/drivers/nxp/ifc/nor/ifc_nor.mk
@@ -0,0 +1,28 @@
+#
+# Copyright 2020-2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${NOR_ADDED},)
+
+NOR_ADDED		:= 1
+
+NOR_DRIVERS_PATH	:=  ${PLAT_DRIVERS_PATH}/ifc/nor
+
+NOR_SOURCES		:=  $(NOR_DRIVERS_PATH)/ifc_nor.c
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/ifc
+
+ifeq (${BL_COMM_IFC_NOR_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${NOR_SOURCES}
+else
+ifeq (${BL2_IFC_NOR_NEEDED},yes)
+BL2_SOURCES		+= ${NOR_SOURCES}
+endif
+ifeq (${BL31_IFC_NOR_NEEDED},yes)
+BL31_SOURCES		+= ${NOR_SOURCES}
+endif
+endif
+
+endif
diff --git a/drivers/nxp/interconnect/interconnect.mk b/drivers/nxp/interconnect/interconnect.mk
new file mode 100644
index 0000000..aa51be4
--- /dev/null
+++ b/drivers/nxp/interconnect/interconnect.mk
@@ -0,0 +1,44 @@
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#
+#------------------------------------------------------------------------------
+#
+# Select the Interconnect files
+#
+# -----------------------------------------------------------------------------
+
+ifeq (${ADD_INTERCONNECT},)
+
+ADD_INTERCONNECT	:= 1
+PLAT_INCLUDES		+= -I${PLAT_DRIVERS_INCLUDE_PATH}/interconnect
+
+ifeq (, $(filter $(INTERCONNECT), CCI400 CCN502 CCN504 CCN508))
+    $(error -> Interconnect type not set!)
+else
+$(eval $(call add_define_val,INTERCONNECT,${INTERCONNECT}))
+ifeq ($(INTERCONNECT), $(filter $(INTERCONNECT), CCN502 CCN504 CCN508))
+INTERCONNECT_SOURCES	:= 	drivers/arm/ccn/ccn.c 		\
+				${PLAT_DRIVERS_PATH}/interconnect/ls_ccn.c
+else
+ifeq ($(INTERCONNECT), CCI400)
+INTERCONNECT_SOURCES	:= 	drivers/arm/cci/cci.c 		\
+				${PLAT_DRIVERS_PATH}/interconnect/ls_cci.c
+endif
+endif
+endif
+
+ifeq (${BL_COMM_INTERCONNECT_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${INTERCONNECT_SOURCES}
+else
+ifeq (${BL2_INTERCONNECT_NEEDED},yes)
+BL2_SOURCES		+= ${INTERCONNECT_SOURCES}
+endif
+ifeq (${BL31_INTERCONNECT_NEEDED},yes)
+BL31_SOURCES		+= ${INTERCONNECT_SOURCES}
+endif
+endif
+endif
+
+# -----------------------------------------------------------------------------
diff --git a/drivers/nxp/interconnect/ls_cci.c b/drivers/nxp/interconnect/ls_cci.c
new file mode 100644
index 0000000..72a898a
--- /dev/null
+++ b/drivers/nxp/interconnect/ls_cci.c
@@ -0,0 +1,38 @@
+/*
+ * Copyright 2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <arch.h>
+#include <cci.h>
+
+#include <plat_arm.h>
+
+/******************************************************************************
+ * The following functions are defined as weak to allow a platform to override
+ * the way ARM CCI driver is initialised and used.
+ *****************************************************************************/
+#pragma weak plat_arm_interconnect_enter_coherency
+#pragma weak plat_arm_interconnect_exit_coherency
+
+/******************************************************************************
+ * Helper function to place current master into coherency
+ *****************************************************************************/
+void plat_ls_interconnect_enter_coherency(unsigned int num_clusters)
+{
+	cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+
+	for (uint32_t index = 1U; index < num_clusters; index++) {
+		cci_enable_snoop_dvm_reqs(index);
+	}
+}
+
+/******************************************************************************
+ * Helper function to remove current master from coherency
+ *****************************************************************************/
+void plat_ls_interconnect_exit_coherency(void)
+{
+	cci_disable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+}
diff --git a/drivers/nxp/interconnect/ls_ccn.c b/drivers/nxp/interconnect/ls_ccn.c
new file mode 100644
index 0000000..8f90325
--- /dev/null
+++ b/drivers/nxp/interconnect/ls_ccn.c
@@ -0,0 +1,31 @@
+/*
+ * Copyright 2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <arch.h>
+#include <ccn.h>
+
+#include <plat_arm.h>
+
+/******************************************************************************
+ * Helper function to place current master into coherency
+ *****************************************************************************/
+void plat_ls_interconnect_enter_coherency(unsigned int num_clusters)
+{
+	ccn_enter_snoop_dvm_domain(1ULL << MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+
+	for (uint32_t index = 1U; index < num_clusters; index++) {
+		ccn_enter_snoop_dvm_domain(1ULL << index);
+	}
+}
+
+/******************************************************************************
+ * Helper function to remove current master from coherency
+ *****************************************************************************/
+void plat_ls_interconnect_exit_coherency(void)
+{
+	ccn_exit_snoop_dvm_domain(1ULL << MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+}
diff --git a/drivers/nxp/pmu/pmu.c b/drivers/nxp/pmu/pmu.c
new file mode 100644
index 0000000..2a907c8
--- /dev/null
+++ b/drivers/nxp/pmu/pmu.c
@@ -0,0 +1,45 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <dcfg.h>
+#include <lib/mmio.h>
+#include <pmu.h>
+
+void enable_timer_base_to_cluster(uintptr_t nxp_pmu_addr)
+{
+	uint32_t *cltbenr = NULL;
+	uint32_t cltbenr_val = 0U;
+
+	cltbenr = (uint32_t *)(nxp_pmu_addr
+				+ CLUST_TIMER_BASE_ENBL_OFFSET);
+
+	cltbenr_val = mmio_read_32((uintptr_t)cltbenr);
+
+	cltbenr_val = cltbenr_val
+			| (1 << MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+
+	mmio_write_32((uintptr_t)cltbenr, cltbenr_val);
+
+	VERBOSE("Enable cluster time base\n");
+}
+
+/*
+ * Enable core timebase.  In certain Layerscape SoCs, the clock for each core's
+ * has an enable bit in the PMU Physical Core Time Base Enable
+ * Register (PCTBENR), which allows the watchdog to operate.
+ */
+
+void enable_core_tb(uintptr_t nxp_pmu_addr)
+{
+	uint32_t *pctbenr = (uint32_t *) (nxp_pmu_addr +
+					  CORE_TIMEBASE_ENBL_OFFSET);
+
+	mmio_write_32((uintptr_t)pctbenr, 0xff);
+}
diff --git a/drivers/nxp/pmu/pmu.mk b/drivers/nxp/pmu/pmu.mk
new file mode 100644
index 0000000..8d2ef07
--- /dev/null
+++ b/drivers/nxp/pmu/pmu.mk
@@ -0,0 +1,26 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#-----------------------------------------------------------------------------
+ifeq (${PMU_ADDED},)
+
+PMU_ADDED		:= 1
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/pmu
+
+PMU_SOURCES		+= $(PLAT_DRIVERS_PATH)/pmu/pmu.c
+
+ifeq (${BL_COMM_PMU_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${PMU_SOURCES}
+else
+ifeq (${BL2_PMU_NEEDED},yes)
+BL2_SOURCES		+= ${PMU_SOURCES}
+endif
+ifeq (${BL31_PMU_NEEDED},yes)
+BL31_SOURCES		+= ${PMU_SOURCES}
+endif
+endif
+endif
+#------------------------------------------------
diff --git a/drivers/nxp/qspi/qspi.c b/drivers/nxp/qspi/qspi.c
new file mode 100644
index 0000000..97b2a19
--- /dev/null
+++ b/drivers/nxp/qspi/qspi.c
@@ -0,0 +1,29 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <qspi.h>
+
+int qspi_io_setup(uintptr_t nxp_qspi_flash_addr,
+		  size_t nxp_qspi_flash_size,
+		  uintptr_t fip_offset)
+{
+	uint32_t qspi_mcr_val = qspi_in32(CHS_QSPI_MCR);
+
+	/* Enable and change endianness of QSPI IP */
+	qspi_out32(CHS_QSPI_MCR, (qspi_mcr_val | CHS_QSPI_64LE));
+
+	/* Adding QSPI Memory Map in XLAT Table */
+	mmap_add_region(nxp_qspi_flash_addr, nxp_qspi_flash_addr,
+			nxp_qspi_flash_size, MT_MEMORY | MT_RW);
+
+	return 0;
+}
diff --git a/drivers/nxp/qspi/qspi.mk b/drivers/nxp/qspi/qspi.mk
new file mode 100644
index 0000000..450aeca
--- /dev/null
+++ b/drivers/nxp/qspi/qspi.mk
@@ -0,0 +1,26 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${QSPI_ADDED},)
+
+QSPI_ADDED		:= 1
+
+QSPI_SOURCES		:= $(PLAT_DRIVERS_PATH)/qspi/qspi.c
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/qspi
+
+ifeq (${BL_COMM_QSPI_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${QSPI_SOURCES}
+else
+ifeq (${BL2_QSPI_NEEDED},yes)
+BL2_SOURCES		+= ${QSPI_SOURCES}
+endif
+ifeq (${BL31_QSPI_NEEDED},yes)
+BL31_SOURCES		+= ${QSPI_SOURCES}
+endif
+endif
+
+endif
diff --git a/drivers/nxp/sd/sd_mmc.c b/drivers/nxp/sd/sd_mmc.c
new file mode 100644
index 0000000..f7f48e7
--- /dev/null
+++ b/drivers/nxp/sd/sd_mmc.c
@@ -0,0 +1,1496 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ *
+ */
+
+#include <endian.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/io/io_block.h>
+#include "nxp_timer.h"
+#include "sd_mmc.h"
+#include <utils.h>
+#include <utils_def.h>
+
+
+/* Private structure for MMC driver data */
+static struct mmc mmc_drv_data;
+
+#ifndef NXP_POLICY_OTA
+/*
+ * For NXP_POLICY_OTA, SD needs to do R/W on OCRAM. OCRAM is secure memory at
+ * default. SD can only do non-secure DMA. Configuring SD to work in PIO mode
+ * instead of DMA mode will make SD R/W on OCRAM available.
+ */
+/* To debug without dma comment this MACRO */
+#define NXP_SD_DMA_CAPABILITY
+#endif
+#define SD_TIMEOUT        1000 /* ms */
+#define SD_TIMEOUT_HIGH   20000 /* ms */
+#define SD_BLOCK_TIMEOUT  8 /* ms */
+
+#define ERROR_ESDHC_CARD_DETECT_FAIL	-1
+#define ERROR_ESDHC_UNUSABLE_CARD	-2
+#define ERROR_ESDHC_COMMUNICATION_ERROR	-3
+#define ERROR_ESDHC_BLOCK_LENGTH	-4
+#define ERROR_ESDHC_DMA_ERROR		-5
+#define ERROR_ESDHC_BUSY		-6
+
+/***************************************************************
+ * Function    :    set_speed
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  clock - Clock Value to be set
+ * Return      :    void
+ * Description :    Calculates the value of SDCLKFS and DVS to be set
+ *                  for getting the required clock assuming the base_clk
+ *                  as a fixed value (MAX_PLATFORM_CLOCK)
+ *****************************************************************/
+static void set_speed(struct mmc *mmc, uint32_t clock)
+{
+	/* sdhc_clk = (base clock) / [(SDCLKFS × 2) × (DVS +1)] */
+
+	uint32_t dvs = 1U;
+	uint32_t sdclkfs = 2U;
+	/* TBD - Change this to actual platform clock by reading via RCW */
+	uint32_t base_clk = MAX_PLATFORM_CLOCK;
+
+	if (base_clk / 16 > clock) {
+		for (sdclkfs = 2U; sdclkfs < 256U; sdclkfs *= 2U) {
+			if ((base_clk / sdclkfs) <= (clock * 16)) {
+				break;
+			}
+		}
+	}
+
+	for (dvs = 1U; dvs <= 16U; dvs++) {
+		if ((base_clk / (dvs * sdclkfs)) <= clock) {
+			break;
+		}
+	}
+
+	sdclkfs >>= 1U;
+	dvs -= 1U;
+
+	esdhc_out32(&mmc->esdhc_regs->sysctl,
+			(ESDHC_SYSCTL_DTOCV(TIMEOUT_COUNTER_SDCLK_2_27) |
+			 ESDHC_SYSCTL_SDCLKFS(sdclkfs) | ESDHC_SYSCTL_DVS(dvs) |
+			 ESDHC_SYSCTL_SDCLKEN));
+}
+
+/***************************************************************************
+ * Function    :    esdhc_init
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  card_detect - flag to indicate if card insert needs
+ *                  to be detected or not. For SDHC2 controller, Card detect
+ *                  is not present, so this field will be false
+ * Return      :    SUCCESS or Error Code
+ * Description :    1. Set Initial Clock Speed
+ *                  2. Card Detect if not eMMC
+ *                  3. Enable Controller Clock
+ *                  4. Send 80 ticks for card to power up
+ *                  5. Set LE mode and Bus Width as 1 bit.
+ ***************************************************************************/
+static int esdhc_init(struct mmc *mmc, bool card_detect)
+{
+	uint32_t val;
+	uint64_t start_time;
+
+	/* Reset the entire host controller */
+	val = esdhc_in32(&mmc->esdhc_regs->sysctl) | ESDHC_SYSCTL_RSTA;
+	esdhc_out32(&mmc->esdhc_regs->sysctl, val);
+
+	/* Wait until the controller is available */
+	start_time = get_timer_val(0);
+	while (get_timer_val(start_time) < SD_TIMEOUT_HIGH) {
+		val = esdhc_in32(&mmc->esdhc_regs->sysctl) & ESDHC_SYSCTL_RSTA;
+		if (val == 0U) {
+			break;
+		}
+	}
+
+	val = esdhc_in32(&mmc->esdhc_regs->sysctl) &
+		(ESDHC_SYSCTL_RSTA);
+	if (val != 0U) {
+		ERROR("SD Reset failed\n");
+		return ERROR_ESDHC_BUSY;
+	}
+
+	/* Set initial clock speed */
+	set_speed(mmc, CARD_IDENTIFICATION_FREQ);
+
+	if (card_detect) {
+		/* Check CINS in prsstat register */
+		val = esdhc_in32(&mmc->esdhc_regs->prsstat) &
+			ESDHC_PRSSTAT_CINS;
+		if (val == 0) {
+			ERROR("CINS not set in prsstat\n");
+			return ERROR_ESDHC_CARD_DETECT_FAIL;
+		}
+	}
+
+	/* Enable controller clock */
+	val = esdhc_in32(&mmc->esdhc_regs->sysctl) | ESDHC_SYSCTL_SDCLKEN;
+	esdhc_out32(&mmc->esdhc_regs->sysctl, val);
+
+	/* Send 80 clock ticks for the card to power up */
+	val = esdhc_in32(&mmc->esdhc_regs->sysctl) | ESDHC_SYSCTL_INITA;
+	esdhc_out32(&mmc->esdhc_regs->sysctl, val);
+
+	start_time = get_timer_val(0);
+	while (get_timer_val(start_time) < SD_TIMEOUT) {
+		val = esdhc_in32(&mmc->esdhc_regs->sysctl) & ESDHC_SYSCTL_INITA;
+		if (val != 0U) {
+			break;
+		}
+	}
+
+	val = esdhc_in32(&mmc->esdhc_regs->sysctl) & ESDHC_SYSCTL_INITA;
+	if (val == 0U) {
+		ERROR("Failed to power up the card\n");
+		return ERROR_ESDHC_CARD_DETECT_FAIL;
+	}
+
+	INFO("Card detected successfully\n");
+
+	val = esdhc_in32(&mmc->esdhc_regs->proctl);
+	val = val | (ESDHC_PROCTL_EMODE_LE | ESDHC_PROCTL_DTW_1BIT);
+
+	/* Set little endian mode, set bus width as 1-bit */
+	esdhc_out32(&mmc->esdhc_regs->proctl, val);
+
+	/* Enable cache snooping for DMA transactions */
+	val = esdhc_in32(&mmc->esdhc_regs->ctl) | ESDHC_DCR_SNOOP;
+	esdhc_out32(&mmc->esdhc_regs->ctl, val);
+
+	return 0;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_send_cmd
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  cmd - Command Number
+ *                  args - Command Args
+ * Return      :    SUCCESS is 0, or Error Code ( < 0)
+ * Description :    Updates the eSDHC registers cmdargs and xfertype
+ ***************************************************************************/
+static int esdhc_send_cmd(struct mmc *mmc, uint32_t cmd, uint32_t args)
+{
+	uint32_t val;
+	uint64_t start_time;
+	uint32_t xfertyp = 0;
+
+	esdhc_out32(&mmc->esdhc_regs->irqstat, ESDHC_IRQSTAT_CLEAR_ALL);
+
+	/* Wait for the command line & data line to be free */
+	/* (poll the CIHB,CDIHB bit of the present state register) */
+	start_time = get_timer_val(0);
+	while (get_timer_val(start_time) < SD_TIMEOUT_HIGH) {
+		val = esdhc_in32(&mmc->esdhc_regs->prsstat) &
+			(ESDHC_PRSSTAT_CIHB | ESDHC_PRSSTAT_CDIHB);
+		if (val == 0U) {
+			break;
+		}
+	}
+
+	val = esdhc_in32(&mmc->esdhc_regs->prsstat) &
+		(ESDHC_PRSSTAT_CIHB | ESDHC_PRSSTAT_CDIHB);
+	if (val != 0U) {
+		ERROR("SD send cmd: Command Line or Data Line Busy cmd = %x\n",
+				cmd);
+		return ERROR_ESDHC_BUSY;
+	}
+
+	if (cmd == CMD2 || cmd == CMD9) {
+		xfertyp |= ESDHC_XFERTYP_RSPTYP_136;
+	} else  if (cmd == CMD7 || (cmd == CMD6 && mmc->card.type == MMC_CARD)) {
+		xfertyp |= ESDHC_XFERTYP_RSPTYP_48_BUSY;
+	} else if (cmd != CMD0) {
+		xfertyp |= ESDHC_XFERTYP_RSPTYP_48;
+	}
+
+	if (cmd == CMD2 || cmd == CMD9) {
+		xfertyp |= ESDHC_XFERTYP_CCCEN; /* Command index check enable */
+	} else if ((cmd != CMD0) && (cmd != ACMD41) && (cmd != CMD1)) {
+		xfertyp = xfertyp | ESDHC_XFERTYP_CCCEN | ESDHC_XFERTYP_CICEN;
+	}
+
+	if ((cmd == CMD8 || cmd == CMD14 || cmd == CMD19) &&
+			mmc->card.type == MMC_CARD) {
+		xfertyp |=  ESDHC_XFERTYP_DPSEL;
+		if (cmd != CMD19) {
+			xfertyp |= ESDHC_XFERTYP_DTDSEL;
+		}
+	}
+
+	if (cmd == CMD6 || cmd == CMD17 || cmd == CMD18 || cmd == CMD24 ||
+	    cmd == ACMD51) {
+		if (!(mmc->card.type == MMC_CARD && cmd == CMD6)) {
+			if (cmd == CMD24) {
+				xfertyp |= ESDHC_XFERTYP_DPSEL;
+			} else {
+				xfertyp |= (ESDHC_XFERTYP_DPSEL |
+					    ESDHC_XFERTYP_DTDSEL);
+			}
+		}
+
+		if (cmd == CMD18) {
+			xfertyp |= ESDHC_XFERTYP_BCEN;
+			if (mmc->dma_support != 0) {
+				/* Set BCEN of XFERTYP */
+				xfertyp |= ESDHC_XFERTYP_DMAEN;
+			}
+		}
+
+		if ((cmd == CMD17 || cmd == CMD24) && (mmc->dma_support != 0)) {
+			xfertyp |= ESDHC_XFERTYP_DMAEN;
+		}
+	}
+
+	xfertyp |= ((cmd & 0x3F) << 24);
+	esdhc_out32(&mmc->esdhc_regs->cmdarg, args);
+	esdhc_out32(&mmc->esdhc_regs->xfertyp, xfertyp);
+
+#ifdef NXP_SD_DEBUG
+	INFO("cmd = %d\n", cmd);
+	INFO("args = %x\n", args);
+	INFO("xfertyp: = %x\n", xfertyp);
+#endif
+	return 0;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_wait_response
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  response - Value updated
+ * Return      :    SUCCESS - Response Received
+ *                  COMMUNICATION_ERROR - Command not Complete
+ *                  COMMAND_ERROR - CIE, CCE or CEBE  error
+ *                  RESP_TIMEOUT - CTOE error
+ * Description :    Checks for successful command completion.
+ *                  Clears the CC bit at the end.
+ ***************************************************************************/
+static int esdhc_wait_response(struct mmc *mmc, uint32_t *response)
+{
+	uint32_t val;
+	uint64_t start_time;
+	uint32_t status = 0U;
+
+	/* Wait for the command to complete */
+	start_time = get_timer_val(0);
+	while (get_timer_val(start_time) < SD_TIMEOUT_HIGH) {
+		val = esdhc_in32(&mmc->esdhc_regs->irqstat) & ESDHC_IRQSTAT_CC;
+		if (val != 0U) {
+			break;
+		}
+	}
+
+	val = esdhc_in32(&mmc->esdhc_regs->irqstat) & ESDHC_IRQSTAT_CC;
+	if (val == 0U) {
+		ERROR("%s:IRQSTAT Cmd not complete(CC not set)\n", __func__);
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	status = esdhc_in32(&mmc->esdhc_regs->irqstat);
+
+	/* Check whether the interrupt is a CRC, CTOE or CIE error */
+	if ((status & (ESDHC_IRQSTAT_CIE | ESDHC_IRQSTAT_CEBE |
+				ESDHC_IRQSTAT_CCE)) != 0) {
+		ERROR("%s: IRQSTAT CRC, CEBE or CIE error = %x\n",
+							__func__, status);
+		return COMMAND_ERROR;
+	}
+
+	if ((status & ESDHC_IRQSTAT_CTOE) != 0) {
+		INFO("%s: IRQSTAT CTOE set = %x\n", __func__, status);
+		return RESP_TIMEOUT;
+	}
+
+	if ((status & ESDHC_IRQSTAT_DMAE) != 0) {
+		ERROR("%s: IRQSTAT DMAE set = %x\n", __func__, status);
+		return ERROR_ESDHC_DMA_ERROR;
+	}
+
+	if (response != NULL) {
+		/* Get response values from eSDHC CMDRSPx registers. */
+		response[0] = esdhc_in32(&mmc->esdhc_regs->cmdrsp[0]);
+		response[1] = esdhc_in32(&mmc->esdhc_regs->cmdrsp[1]);
+		response[2] = esdhc_in32(&mmc->esdhc_regs->cmdrsp[2]);
+		response[3] = esdhc_in32(&mmc->esdhc_regs->cmdrsp[3]);
+#ifdef NXP_SD_DEBUG
+		INFO("Resp R1 R2 R3 R4\n");
+		INFO("Resp R1 = %x\n", response[0]);
+		INFO("R2 = %x\n", response[1]);
+		INFO("R3 = %x\n", response[2]);
+		INFO("R4 = %x\n", response[3]);
+		INFO("\n");
+#endif
+	}
+
+	/* Clear the CC bit - w1c */
+	val = esdhc_in32(&mmc->esdhc_regs->irqstat) | ESDHC_IRQSTAT_CC;
+	esdhc_out32(&mmc->esdhc_regs->irqstat, val);
+
+	return 0;
+}
+
+/***************************************************************************
+ * Function    :    mmc_switch_to_high_frquency
+ * Arguments   :    mmc - Pointer to mmc struct
+ * Return      :    SUCCESS or Error Code
+ * Description :    mmc card bellow ver 4.0 does not support high speed
+ *                  freq = 20 MHz
+ *                  Send CMD6 (CMD_SWITCH_FUNC) With args 0x03B90100
+ *                  Send CMD13 (CMD_SEND_STATUS)
+ *                  if SWITCH Error, freq = 26 MHz
+ *                  if no error, freq = 52 MHz
+ ***************************************************************************/
+static int mmc_switch_to_high_frquency(struct mmc *mmc)
+{
+	int error;
+	uint32_t response[4];
+	uint64_t start_time;
+
+	mmc->card.bus_freq = MMC_SS_20MHZ;
+	/* mmc card bellow ver 4.0 does not support high speed */
+	if (mmc->card.version < MMC_CARD_VERSION_4_X) {
+		return 0;
+	}
+
+	/* send switch cmd to change the card to High speed */
+	error = esdhc_send_cmd(mmc, CMD_SWITCH_FUNC, SET_EXT_CSD_HS_TIMING);
+	if (error != 0) {
+		return error;
+	}
+	error = esdhc_wait_response(mmc, response);
+	if (error != 0) {
+		return error;
+	}
+
+	start_time = get_timer_val(0);
+	do {
+		/* check the status for which error */
+		error = esdhc_send_cmd(mmc,
+				CMD_SEND_STATUS, mmc->card.rca << 16);
+		if (error != 0) {
+			return error;
+		}
+
+		error = esdhc_wait_response(mmc, response);
+		if (error != 0) {
+			return error;
+		}
+	} while (((response[0] & SWITCH_ERROR) != 0) &&
+			(get_timer_val(start_time) < SD_TIMEOUT));
+
+	/* Check for the present state of card */
+	if ((response[0] & SWITCH_ERROR) != 0) {
+		mmc->card.bus_freq = MMC_HS_26MHZ;
+	} else {
+		mmc->card.bus_freq = MMC_HS_52MHZ;
+	}
+
+	return 0;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_set_data_attributes
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  blkcnt
+ *                  blklen
+ * Return      :    SUCCESS or Error Code
+ * Description :    Set block attributes and watermark level register
+ ***************************************************************************/
+static int esdhc_set_data_attributes(struct mmc *mmc, uint32_t *dest_ptr,
+		uint32_t blkcnt, uint32_t blklen)
+{
+	uint32_t val;
+	uint64_t start_time;
+	uint32_t wml;
+	uint32_t wl;
+	uint32_t dst = (uint32_t)((uint64_t)(dest_ptr));
+
+	/* set blkattr when no transactions are executing */
+	start_time = get_timer_val(0);
+	while (get_timer_val(start_time) < SD_TIMEOUT_HIGH) {
+		val = esdhc_in32(&mmc->esdhc_regs->prsstat) & ESDHC_PRSSTAT_DLA;
+		if (val == 0U) {
+			break;
+		}
+	}
+
+	val = esdhc_in32(&mmc->esdhc_regs->prsstat) & ESDHC_PRSSTAT_DLA;
+	if (val != 0U) {
+		ERROR("%s: Data line active.Can't set attribute\n", __func__);
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	wml = esdhc_in32(&mmc->esdhc_regs->wml);
+	wml &= ~(ESDHC_WML_WR_BRST_MASK | ESDHC_WML_RD_BRST_MASK |
+			ESDHC_WML_RD_WML_MASK | ESDHC_WML_WR_WML_MASK);
+
+	if ((mmc->dma_support != 0) && (dest_ptr != NULL)) {
+		/* Set burst length to 128 bytes */
+		esdhc_out32(&mmc->esdhc_regs->wml,
+				wml | ESDHC_WML_WR_BRST(BURST_128_BYTES));
+		esdhc_out32(&mmc->esdhc_regs->wml,
+				wml | ESDHC_WML_RD_BRST(BURST_128_BYTES));
+
+		/* Set DMA System Destination Address */
+		esdhc_out32(&mmc->esdhc_regs->dsaddr, dst);
+	} else {
+		wl = (blklen >= BLOCK_LEN_512) ?
+			WML_512_BYTES : ((blklen + 3) / 4);
+		/* Set 'Read Water Mark Level' register */
+		esdhc_out32(&mmc->esdhc_regs->wml, wml | ESDHC_WML_RD_WML(wl));
+	}
+
+	/* Configure block Attributes register */
+	esdhc_out32(&mmc->esdhc_regs->blkattr,
+		ESDHC_BLKATTR_BLKCNT(blkcnt) | ESDHC_BLKATTR_BLKSZE(blklen));
+
+	mmc->block_len = blklen;
+
+	return 0;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_read_data_nodma
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  dest_ptr - Bufffer where read data is to be copied
+ *                  len - Length of Data to be read
+ * Return      :    SUCCESS or Error Code
+ * Description :    Read data from the sdhc buffer without using DMA
+ *                  and using polling mode
+ ***************************************************************************/
+static int esdhc_read_data_nodma(struct mmc *mmc, void *dest_ptr, uint32_t len)
+{
+	uint32_t i = 0U;
+	uint32_t status;
+	uint32_t num_blocks;
+	uint32_t *dst = (uint32_t *)dest_ptr;
+	uint32_t val;
+	uint64_t start_time;
+
+	num_blocks = len / mmc->block_len;
+
+	while ((num_blocks--) != 0U) {
+
+		start_time = get_timer_val(0);
+		while (get_timer_val(start_time) < SD_TIMEOUT_HIGH) {
+			val = esdhc_in32(&mmc->esdhc_regs->prsstat) &
+				ESDHC_PRSSTAT_BREN;
+			if (val != 0U) {
+				break;
+			}
+		}
+
+		val = esdhc_in32(&mmc->esdhc_regs->prsstat)
+			& ESDHC_PRSSTAT_BREN;
+		if (val == 0U) {
+			return ERROR_ESDHC_COMMUNICATION_ERROR;
+		}
+
+		for (i = 0U, status = esdhc_in32(&mmc->esdhc_regs->irqstat);
+				i < mmc->block_len / 4;    i++, dst++) {
+			/* get data from data port */
+			val = mmio_read_32(
+					(uintptr_t)&mmc->esdhc_regs->datport);
+			esdhc_out32(dst, val);
+			/* Increment destination pointer */
+			status = esdhc_in32(&mmc->esdhc_regs->irqstat);
+		}
+		/* Check whether the interrupt is an DTOE/DCE/DEBE */
+		if ((status & (ESDHC_IRQSTAT_DTOE | ESDHC_IRQSTAT_DCE |
+					ESDHC_IRQSTAT_DEBE)) != 0) {
+			ERROR("SD read error - DTOE, DCE, DEBE bit set = %x\n",
+									status);
+			return ERROR_ESDHC_COMMUNICATION_ERROR;
+		}
+	}
+
+	/* Wait for TC */
+
+	start_time = get_timer_val(0);
+	while (get_timer_val(start_time) < SD_TIMEOUT_HIGH) {
+		val = esdhc_in32(&mmc->esdhc_regs->irqstat) & ESDHC_IRQSTAT_TC;
+		if (val != 0U) {
+			break;
+		}
+	}
+
+	val = esdhc_in32(&mmc->esdhc_regs->irqstat) & ESDHC_IRQSTAT_TC;
+	if (val == 0U) {
+		ERROR("SD read timeout: Transfer bit not set in IRQSTAT\n");
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	return 0;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_write_data_nodma
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  src_ptr - Buffer where data is copied from
+ *                  len - Length of Data to be written
+ * Return      :    SUCCESS or Error Code
+ * Description :    Write data to the sdhc buffer without using DMA
+ *                  and using polling mode
+ ***************************************************************************/
+static int esdhc_write_data_nodma(struct mmc *mmc, void *src_ptr, uint32_t len)
+{
+	uint32_t i = 0U;
+	uint32_t status;
+	uint32_t num_blocks;
+	uint32_t *src = (uint32_t *)src_ptr;
+	uint32_t val;
+	uint64_t start_time;
+
+	num_blocks = len / mmc->block_len;
+
+	while ((num_blocks--) != 0U) {
+		start_time = get_timer_val(0);
+		while (get_timer_val(start_time) < SD_TIMEOUT_HIGH) {
+			val = esdhc_in32(&mmc->esdhc_regs->prsstat) &
+					 ESDHC_PRSSTAT_BWEN;
+			if (val != 0U) {
+				break;
+			}
+		}
+
+		val = esdhc_in32(&mmc->esdhc_regs->prsstat) &
+				 ESDHC_PRSSTAT_BWEN;
+		if (val == 0U) {
+			return ERROR_ESDHC_COMMUNICATION_ERROR;
+		}
+
+		for (i = 0U, status = esdhc_in32(&mmc->esdhc_regs->irqstat);
+		     i < mmc->block_len / 4; i++, src++) {
+			val = esdhc_in32(src);
+			/* put data to data port */
+			mmio_write_32((uintptr_t)&mmc->esdhc_regs->datport,
+				      val);
+			/* Increment source pointer */
+			status = esdhc_in32(&mmc->esdhc_regs->irqstat);
+		}
+		/* Check whether the interrupt is an DTOE/DCE/DEBE */
+		if ((status & (ESDHC_IRQSTAT_DTOE | ESDHC_IRQSTAT_DCE |
+					ESDHC_IRQSTAT_DEBE)) != 0) {
+			ERROR("SD write error - DTOE, DCE, DEBE bit set = %x\n",
+			      status);
+			return ERROR_ESDHC_COMMUNICATION_ERROR;
+		}
+	}
+
+	/* Wait for TC */
+	start_time = get_timer_val(0);
+	while (get_timer_val(start_time) < SD_TIMEOUT_HIGH) {
+		val = esdhc_in32(&mmc->esdhc_regs->irqstat) & ESDHC_IRQSTAT_TC;
+		if (val != 0U) {
+			break;
+		}
+	}
+
+	val = esdhc_in32(&mmc->esdhc_regs->irqstat) & ESDHC_IRQSTAT_TC;
+	if (val == 0U) {
+		ERROR("SD write timeout: Transfer bit not set in IRQSTAT\n");
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	return 0;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_read_data_dma
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  len - Length of Data to be read
+ * Return      :    SUCCESS or Error Code
+ * Description :    Read data from the sd card using DMA.
+ ***************************************************************************/
+static int esdhc_read_data_dma(struct mmc *mmc, uint32_t len)
+{
+	uint32_t status;
+	uint32_t tblk;
+	uint64_t start_time;
+
+	tblk = SD_BLOCK_TIMEOUT * (len / mmc->block_len);
+
+	start_time = get_timer_val(0);
+
+	/* poll till TC is set */
+	do {
+		status = esdhc_in32(&mmc->esdhc_regs->irqstat);
+
+		if ((status & (ESDHC_IRQSTAT_DEBE | ESDHC_IRQSTAT_DCE
+					| ESDHC_IRQSTAT_DTOE)) != 0) {
+			ERROR("SD read error - DTOE, DCE, DEBE bit set = %x\n",
+								 status);
+			return ERROR_ESDHC_COMMUNICATION_ERROR;
+		}
+
+		if ((status & ESDHC_IRQSTAT_DMAE) != 0) {
+			ERROR("SD read error - DMA error = %x\n", status);
+			return ERROR_ESDHC_DMA_ERROR;
+		}
+
+	} while (((status & ESDHC_IRQSTAT_TC) == 0) &&
+		((esdhc_in32(&mmc->esdhc_regs->prsstat) & ESDHC_PRSSTAT_DLA) != 0) &&
+		(get_timer_val(start_time) < SD_TIMEOUT_HIGH + tblk));
+
+	if (get_timer_val(start_time) > SD_TIMEOUT_HIGH + tblk) {
+		ERROR("SD read DMA timeout\n");
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	return 0;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_write_data_dma
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  len - Length of Data to be written
+ * Return      :    SUCCESS or Error Code
+ * Description :    Write data to the sd card using DMA.
+ ***************************************************************************/
+static int esdhc_write_data_dma(struct mmc *mmc, uint32_t len)
+{
+	uint32_t status;
+	uint32_t tblk;
+	uint64_t start_time;
+
+	tblk = SD_BLOCK_TIMEOUT * (len / mmc->block_len);
+
+	start_time = get_timer_val(0);
+
+	/* poll till TC is set */
+	do {
+		status = esdhc_in32(&mmc->esdhc_regs->irqstat);
+
+		if ((status & (ESDHC_IRQSTAT_DEBE | ESDHC_IRQSTAT_DCE
+					| ESDHC_IRQSTAT_DTOE)) != 0) {
+			ERROR("SD write error - DTOE, DCE, DEBE bit set = %x\n",
+			      status);
+			return ERROR_ESDHC_COMMUNICATION_ERROR;
+		}
+
+		if ((status & ESDHC_IRQSTAT_DMAE) != 0) {
+			ERROR("SD write error - DMA error = %x\n", status);
+			return ERROR_ESDHC_DMA_ERROR;
+		}
+	} while (((status & ESDHC_IRQSTAT_TC) == 0) &&
+		((esdhc_in32(&mmc->esdhc_regs->prsstat) & ESDHC_PRSSTAT_DLA) != 0) &&
+		(get_timer_val(start_time) < SD_TIMEOUT_HIGH + tblk));
+
+	if (get_timer_val(start_time) > SD_TIMEOUT_HIGH + tblk) {
+		ERROR("SD write DMA timeout\n");
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	return 0;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_read_data
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  dest_ptr - Bufffer where read data is to be copied
+ *                  len - Length of Data to be read
+ * Return      :    SUCCESS or Error Code
+ * Description :    Calls esdhc_read_data_nodma and clear interrupt status
+ ***************************************************************************/
+int esdhc_read_data(struct mmc *mmc, void *dest_ptr, uint32_t len)
+{
+	int ret;
+
+	if (mmc->dma_support && len > 64) {
+		ret = esdhc_read_data_dma(mmc, len);
+	} else {
+		ret = esdhc_read_data_nodma(mmc, dest_ptr, len);
+	}
+
+	/* clear interrupt status */
+	esdhc_out32(&mmc->esdhc_regs->irqstat, ESDHC_IRQSTAT_CLEAR_ALL);
+
+	return ret;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_write_data
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  src_ptr - Buffer where data is copied from
+ *                  len - Length of Data to be written
+ * Return      :    SUCCESS or Error Code
+ * Description :    Calls esdhc_write_data_nodma and clear interrupt status
+ ***************************************************************************/
+int esdhc_write_data(struct mmc *mmc, void *src_ptr, uint32_t len)
+{
+	int ret;
+
+	if (mmc->dma_support && len > 64) {
+		ret = esdhc_write_data_dma(mmc, len);
+	} else {
+		ret = esdhc_write_data_nodma(mmc, src_ptr, len);
+	}
+
+	/* clear interrupt status */
+	esdhc_out32(&mmc->esdhc_regs->irqstat, ESDHC_IRQSTAT_CLEAR_ALL);
+
+	return ret;
+}
+
+/***************************************************************************
+ * Function    :    sd_switch_to_high_freq
+ * Arguments   :    mmc - Pointer to mmc struct
+ * Return      :    SUCCESS or Error Code
+ * Description :    1. Send ACMD51 (CMD_SEND_SCR)
+ *                  2. Read the SCR to check if card supports higher freq
+ *                  3. check version from SCR
+ *                  4. If SD 1.0, return (no Switch) freq = 25 MHz.
+ *                  5. Send CMD6 (CMD_SWITCH_FUNC) with args 0x00FFFFF1 to
+ *                     check the status of switch func
+ *                  6. Send CMD6 (CMD_SWITCH_FUNC) With args 0x80FFFFF1 to
+ *                     switch to high frequency = 50 Mhz
+ ***************************************************************************/
+static int sd_switch_to_high_freq(struct mmc *mmc)
+{
+	int err;
+	uint8_t scr[8];
+	uint8_t status[64];
+	uint32_t response[4];
+	uint32_t version;
+	uint32_t count;
+	uint32_t sd_versions[] = {SD_CARD_VERSION_1_0, SD_CARD_VERSION_1_10,
+		SD_CARD_VERSION_2_0};
+
+	mmc->card.bus_freq = SD_SS_25MHZ;
+	/* Send Application command */
+	err = esdhc_send_cmd(mmc, CMD_APP_CMD, mmc->card.rca << 16);
+	if (err != 0) {
+		return err;
+	}
+
+	err = esdhc_wait_response(mmc, response);
+	if (err != 0) {
+		return err;
+	}
+
+	esdhc_set_data_attributes(mmc, NULL, 1, 8);
+	/* Read the SCR to find out if this card supports higher speeds */
+	err = esdhc_send_cmd(mmc, CMD_SEND_SCR,  mmc->card.rca << 16);
+	if (err != 0) {
+		return err;
+	}
+	err = esdhc_wait_response(mmc, response);
+	if (err != 0) {
+		return err;
+	}
+
+	/* read 8 bytes of scr data */
+	err = esdhc_read_data(mmc, scr, 8U);
+	if (err != 0) {
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	/* check version from SCR */
+	version = scr[0] & U(0xF);
+	if (version <= 2U) {
+		mmc->card.version = sd_versions[version];
+	} else {
+		mmc->card.version = SD_CARD_VERSION_2_0;
+	}
+
+	/* does not support switch func */
+	if (mmc->card.version == SD_CARD_VERSION_1_0) {
+		return 0;
+	}
+
+	/* read 64 bytes of status */
+	esdhc_set_data_attributes(mmc, NULL, 1U, 64U);
+
+	/* check the status of switch func */
+	for (count = 0U; count < 4U; count++) {
+		err = esdhc_send_cmd(mmc, CMD_SWITCH_FUNC,
+				SD_SWITCH_FUNC_CHECK_MODE);
+		if (err != 0) {
+			return err;
+		}
+		err = esdhc_wait_response(mmc, response);
+		if (err != 0) {
+			return err;
+		}
+		/* read 64 bytes of scr data */
+		err = esdhc_read_data(mmc, status, 64U);
+		if (err != 0) {
+			return ERROR_ESDHC_COMMUNICATION_ERROR;
+		}
+
+		if ((status[29] & SD_SWITCH_FUNC_HIGH_SPEED) == 0) {
+			break;
+		}
+	}
+
+	if ((status[13] & SD_SWITCH_FUNC_HIGH_SPEED) == 0) {
+		return 0;
+	}
+
+	/* SWITCH */
+	esdhc_set_data_attributes(mmc, NULL, 1, 64);
+	err = esdhc_send_cmd(mmc, CMD_SWITCH_FUNC, SD_SWITCH_FUNC_SWITCH_MODE);
+	if (err != 0) {
+		return err;
+	}
+	err = esdhc_wait_response(mmc, response);
+	if (err != 0) {
+		return err;
+	}
+
+	err = esdhc_read_data(mmc, status, 64U);
+	if (err != 0) {
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	if ((status[16]) == U(0x01)) {
+		mmc->card.bus_freq = SD_HS_50MHZ;
+	}
+
+	return 0;
+}
+
+/***************************************************************************
+ * Function    :    change_state_to_transfer_state
+ * Arguments   :    mmc - Pointer to mmc struct
+ * Return      :    SUCCESS or Error Code
+ * Description :    1. Send CMD7 (CMD_SELECT_CARD) to toggles the card
+ *                     between stand-by and transfer state
+ *                  2. Send CMD13 (CMD_SEND_STATUS) to check state as
+ *                     Transfer State
+ ***************************************************************************/
+static int change_state_to_transfer_state(struct mmc *mmc)
+{
+	int error = 0;
+	uint32_t response[4];
+	uint64_t start_time;
+
+	/* Command CMD_SELECT_CARD/CMD7 toggles the card between stand-by
+	 * and transfer states
+	 */
+	error = esdhc_send_cmd(mmc, CMD_SELECT_CARD, mmc->card.rca << 16);
+	if (error != 0) {
+		return error;
+	}
+	error = esdhc_wait_response(mmc, response);
+	if (error != 0) {
+		return error;
+	}
+
+	start_time = get_timer_val(0);
+	while (get_timer_val(start_time) < SD_TIMEOUT_HIGH) {
+		/* send CMD13 to check card status */
+		error = esdhc_send_cmd(mmc,
+					CMD_SEND_STATUS, mmc->card.rca << 16);
+		if (error != 0) {
+			return error;
+		}
+		error = esdhc_wait_response(mmc, response);
+		if ((error != 0) || ((response[0] & R1_ERROR) != 0)) {
+			return error;
+		}
+
+		/* Check for the present state of card */
+		if (((response[0] >> 9U) & U(0xF)) == STATE_TRAN) {
+			break;
+		}
+	}
+	if (((response[0] >> 9U) & U(0xF)) == STATE_TRAN) {
+		return 0;
+	} else {
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+}
+
+/***************************************************************************
+ * Function    :    get_cid_rca_csd
+ * Arguments   :    mmc - Pointer to mmc struct
+ * Return      :    SUCCESS or Error Code
+ * Description :    1. Send CMD2 (CMD_ALL_SEND_CID)
+ *                  2. get RCA for SD cards, set rca for mmc cards
+ *                     Send CMD3 (CMD_SEND_RELATIVE_ADDR)
+ *                  3. Send CMD9 (CMD_SEND_CSD)
+ *                  4. Get MMC Version from CSD
+ ***************************************************************************/
+static int get_cid_rca_csd(struct mmc *mmc)
+{
+	int err;
+	uint32_t version;
+	uint32_t response[4];
+	uint32_t mmc_version[] = {MMC_CARD_VERSION_1_2, MMC_CARD_VERSION_1_4,
+		MMC_CARD_VERSION_2_X, MMC_CARD_VERSION_3_X,
+		MMC_CARD_VERSION_4_X};
+
+	err = esdhc_send_cmd(mmc, CMD_ALL_SEND_CID, 0);
+	if (err != 0) {
+		return err;
+	}
+	err = esdhc_wait_response(mmc, response);
+	if (err != 0) {
+		return err;
+	}
+
+	/* get RCA for SD cards, set rca for mmc cards */
+	mmc->card.rca = SD_MMC_CARD_RCA;
+
+	/* send RCA cmd */
+	err = esdhc_send_cmd(mmc, CMD_SEND_RELATIVE_ADDR, mmc->card.rca << 16);
+	if (err != 0) {
+		return err;
+	}
+	err = esdhc_wait_response(mmc, response);
+	if (err != 0) {
+		return err;
+	}
+
+	/* for SD, get the the RCA */
+	if (mmc->card.type == SD_CARD) {
+		mmc->card.rca = (response[0] >> 16) & 0xFFFF;
+	}
+
+	/* Get the CSD (card specific data) from card. */
+	err = esdhc_send_cmd(mmc, CMD_SEND_CSD, mmc->card.rca << 16);
+	if (err != 0) {
+		return err;
+	}
+	err = esdhc_wait_response(mmc, response);
+	if (err != 0) {
+		return err;
+	}
+
+	version = (response[3] >> 18U) & U(0xF);
+	if (mmc->card.type == MMC_CARD) {
+		if (version <= MMC_CARD_VERSION_4_X) {
+			mmc->card.version = mmc_version[version];
+		} else {
+			mmc->card.version = MMC_CARD_VERSION_4_X;
+		}
+	}
+
+	mmc->card.block_len = 1 << ((response[2] >> 8) & 0xF);
+
+	if (mmc->card.block_len > BLOCK_LEN_512) {
+		mmc->card.block_len = BLOCK_LEN_512;
+	}
+
+	return 0;
+}
+
+/***************************************************************************
+ * Function    :    identify_mmc_card
+ * Arguments   :    mmc - Pointer to mmc struct
+ * Return      :    SUCCESS or Error Code
+ * Description :    1. Send Reset Command
+ *                  2. Send CMD1 with args to set voltage range and Sector
+ *                     Mode. (Voltage Args = 0xFF8)
+ *                  3. Check the OCR Response
+ ***************************************************************************/
+static int identify_mmc_card(struct mmc *mmc)
+{
+	uint64_t start_time;
+	uint32_t resp[4];
+	int ret;
+	uint32_t args;
+
+	/* card reset */
+	ret = esdhc_send_cmd(mmc, CMD_GO_IDLE_STATE, 0U);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = esdhc_wait_response(mmc, resp);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Send CMD1 to get the ocr value repeatedly till the card */
+	/* busy is clear. timeout = 20sec */
+
+	start_time = get_timer_val(0);
+	do {
+		/* set the bits for the voltage ranges supported by host */
+		args = mmc->voltages_caps | MMC_OCR_SECTOR_MODE;
+		ret = esdhc_send_cmd(mmc, CMD_MMC_SEND_OP_COND, args);
+		if (ret != 0) {
+			return ret;
+		}
+		ret = esdhc_wait_response(mmc, resp);
+		if (ret != 0) {
+			return ERROR_ESDHC_UNUSABLE_CARD;
+		}
+	} while (((resp[0] & MMC_OCR_BUSY) == 0U) &&
+			(get_timer_val(start_time) < SD_TIMEOUT_HIGH));
+
+	if (get_timer_val(start_time) > SD_TIMEOUT_HIGH) {
+		return ERROR_ESDHC_UNUSABLE_CARD;
+	}
+
+	if ((resp[0] & MMC_OCR_CCS) == MMC_OCR_CCS) {
+		mmc->card.is_high_capacity = 1;
+	}
+
+	return MMC_CARD;
+}
+
+/***************************************************************************
+ * Function    :    check_for_sd_card
+ * Arguments   :    mmc - Pointer to mmc struct
+ * Return      :    SUCCESS or Error Code
+ * Description :    1. Send Reset Command
+ *                  2. Send CMD8 with pattern 0xAA (to check for SD 2.0)
+ *                  3. Send ACMD41 with args to set voltage range and HCS
+ *                     HCS is set only for SD Card > 2.0
+ *                     Voltage Caps = 0xFF8
+ *                  4. Check the OCR Response
+ ***************************************************************************/
+static int check_for_sd_card(struct mmc *mmc)
+{
+	uint64_t start_time;
+	uint32_t args;
+	int  ret;
+	uint32_t resp[4];
+
+	/* Send reset command */
+	ret = esdhc_send_cmd(mmc, CMD_GO_IDLE_STATE, 0U);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = esdhc_wait_response(mmc, resp);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* send CMD8 with  pattern 0xAA */
+	args = MMC_VDD_HIGH_VOLTAGE | 0xAA;
+	ret = esdhc_send_cmd(mmc, CMD_SEND_IF_COND, args);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = esdhc_wait_response(mmc, resp);
+	if (ret == RESP_TIMEOUT) { /* sd ver 1.x or not sd */
+		mmc->card.is_high_capacity = 0;
+	} else if ((resp[0] & U(0xFF)) == U(0xAA)) { /* ver 2.0 or later */
+		mmc->card.version = SD_CARD_VERSION_2_0;
+	} else {
+		return  NOT_SD_CARD;
+	}
+	/* Send Application command-55 to get the ocr value repeatedly till
+	 * the card busy is clear. timeout = 20sec
+	 */
+
+	start_time = get_timer_val(0);
+	do {
+		ret = esdhc_send_cmd(mmc, CMD_APP_CMD, 0U);
+		if (ret != 0) {
+			return ret;
+		}
+		ret = esdhc_wait_response(mmc, resp);
+		if (ret == COMMAND_ERROR) {
+			return ERROR_ESDHC_UNUSABLE_CARD;
+		}
+
+		/* set the bits for the voltage ranges supported by host */
+		args = mmc->voltages_caps;
+		if (mmc->card.version == SD_CARD_VERSION_2_0) {
+			args |= SD_OCR_HCS;
+		}
+
+		/* Send ACMD41 to set voltage range */
+		ret = esdhc_send_cmd(mmc, CMD_SD_SEND_OP_COND, args);
+		if (ret != 0) {
+			return ret;
+		}
+		ret = esdhc_wait_response(mmc, resp);
+		if (ret == COMMAND_ERROR) {
+			return ERROR_ESDHC_UNUSABLE_CARD;
+		} else if (ret == RESP_TIMEOUT) {
+			return NOT_SD_CARD;
+		}
+	} while (((resp[0] & MMC_OCR_BUSY) == 0U) &&
+			(get_timer_val(start_time) < SD_TIMEOUT_HIGH));
+
+	if (get_timer_val(start_time) > SD_TIMEOUT_HIGH) {
+		INFO("SD_TIMEOUT_HIGH\n");
+		return ERROR_ESDHC_UNUSABLE_CARD;
+	}
+
+	/* bit set in card capacity status */
+	if ((resp[0] & MMC_OCR_CCS) == MMC_OCR_CCS) {
+		mmc->card.is_high_capacity = 1;
+	}
+
+	return SD_CARD;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_emmc_init
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  src_emmc - Flag to Indicate SRC as emmc
+ * Return      :    SUCCESS or Error Code (< 0)
+ * Description :    Base Function called from sd_mmc_init or emmc_init
+ ***************************************************************************/
+int esdhc_emmc_init(struct mmc *mmc, bool card_detect)
+{
+	int error = 0;
+	int ret = 0;
+
+	error = esdhc_init(mmc, card_detect);
+	if (error != 0) {
+		return error;
+	}
+
+	mmc->card.bus_freq = CARD_IDENTIFICATION_FREQ;
+	mmc->card.rca = 0;
+	mmc->card.is_high_capacity = 0;
+	mmc->card.type = ERROR_ESDHC_UNUSABLE_CARD;
+
+	/* Set Voltage caps as FF8 i.e all supported */
+	/* high voltage bits 2.7 - 3.6 */
+	mmc->voltages_caps = MMC_OCR_VDD_FF8;
+
+#ifdef NXP_SD_DMA_CAPABILITY
+	/* Getting host DMA capabilities. */
+	mmc->dma_support = esdhc_in32(&mmc->esdhc_regs->hostcapblt) &
+					ESDHC_HOSTCAPBLT_DMAS;
+#else
+	mmc->dma_support = 0;
+#endif
+
+	ret = NOT_SD_CARD;
+	/* If SRC is not EMMC, check for SD or MMC */
+	ret = check_for_sd_card(mmc);
+	switch (ret) {
+	case SD_CARD:
+		mmc->card.type = SD_CARD;
+		break;
+
+	case NOT_SD_CARD:
+		/* try for MMC card */
+		if (identify_mmc_card(mmc) == MMC_CARD) {
+			mmc->card.type = MMC_CARD;
+		} else {
+			return ERROR_ESDHC_UNUSABLE_CARD;
+		}
+		break;
+
+	default:
+		return ERROR_ESDHC_UNUSABLE_CARD;
+	}
+
+	/* get CID, RCA and CSD. For MMC, set the rca */
+	error = get_cid_rca_csd(mmc);
+	if (error != 0) {
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	/* change state to Transfer mode */
+	error = change_state_to_transfer_state(mmc);
+	if (error != 0) {
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	/* change to high frequency if supported */
+	if (mmc->card.type == SD_CARD) {
+		error = sd_switch_to_high_freq(mmc);
+	} else {
+		error = mmc_switch_to_high_frquency(mmc);
+	}
+	if (error != 0) {
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	/* mmc: 20000000, 26000000, 52000000 */
+	/* sd: 25000000, 50000000 */
+	set_speed(mmc, mmc->card.bus_freq);
+
+	INFO("init done:\n");
+	return 0;
+}
+
+/***************************************************************************
+ * Function    :    sd_mmc_init
+ * Arguments   :    mmc - Pointer to mmc struct
+ * Return      :    SUCCESS or Error Code
+ * Description :    Base Function called via hal_init for SD/MMC
+ *                  initialization
+ ***************************************************************************/
+int sd_mmc_init(uintptr_t nxp_esdhc_addr, bool card_detect)
+{
+	struct mmc *mmc = NULL;
+	int ret;
+
+	mmc = &mmc_drv_data;
+	memset(mmc, 0, sizeof(struct mmc));
+	mmc->esdhc_regs = (struct esdhc_regs *)nxp_esdhc_addr;
+
+	INFO("esdhc_emmc_init\n");
+	ret = esdhc_emmc_init(mmc, card_detect);
+	return ret;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_read_block
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  dst - Destination Pointer
+ *                  block - Block Number
+ * Return      :    SUCCESS or Error Code
+ * Description :    Read a Single block to Destination Pointer
+ *                  1. Send CMD16 (CMD_SET_BLOCKLEN) with args as blocklen
+ *                  2. Send CMD17 (CMD_READ_SINGLE_BLOCK) with args offset
+ ***************************************************************************/
+static int esdhc_read_block(struct mmc *mmc, void *dst, uint32_t block)
+{
+	uint32_t offset;
+	int err;
+
+	/* send cmd16 to set the block size. */
+	err = esdhc_send_cmd(mmc, CMD_SET_BLOCKLEN, mmc->card.block_len);
+	if (err != 0) {
+		return err;
+	}
+	err = esdhc_wait_response(mmc, NULL);
+	if (err != 0) {
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	if (mmc->card.is_high_capacity != 0) {
+		offset = block;
+	} else {
+		offset = block * mmc->card.block_len;
+	}
+
+	esdhc_set_data_attributes(mmc, dst, 1, mmc->card.block_len);
+	err = esdhc_send_cmd(mmc, CMD_READ_SINGLE_BLOCK, offset);
+	if (err != 0) {
+		return err;
+	}
+	err = esdhc_wait_response(mmc, NULL);
+	if (err != 0) {
+		return err;
+	}
+
+	err = esdhc_read_data(mmc, dst, mmc->card.block_len);
+
+	return err;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_write_block
+ * Arguments   :    mmc - Pointer to mmc struct
+ *                  src - Source Pointer
+ *                  block - Block Number
+ * Return      :    SUCCESS or Error Code
+ * Description :    Write a Single block from Source Pointer
+ *                  1. Send CMD16 (CMD_SET_BLOCKLEN) with args as blocklen
+ *                  2. Send CMD24 (CMD_WRITE_SINGLE_BLOCK) with args offset
+ ***************************************************************************/
+static int esdhc_write_block(struct mmc *mmc, void *src, uint32_t block)
+{
+	uint32_t offset;
+	int err;
+
+	/* send cmd16 to set the block size. */
+	err = esdhc_send_cmd(mmc, CMD_SET_BLOCKLEN, mmc->card.block_len);
+	if (err != 0) {
+		return err;
+	}
+	err = esdhc_wait_response(mmc, NULL);
+	if (err != 0) {
+		return ERROR_ESDHC_COMMUNICATION_ERROR;
+	}
+
+	if (mmc->card.is_high_capacity != 0) {
+		offset = block;
+	} else {
+		offset = block * mmc->card.block_len;
+	}
+
+	esdhc_set_data_attributes(mmc, src, 1, mmc->card.block_len);
+	err = esdhc_send_cmd(mmc, CMD_WRITE_SINGLE_BLOCK, offset);
+	if (err != 0) {
+		return err;
+	}
+	err = esdhc_wait_response(mmc, NULL);
+	if (err != 0) {
+		return err;
+	}
+
+	err = esdhc_write_data(mmc, src, mmc->card.block_len);
+
+	return err;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_read
+ * Arguments   :    src_offset - offset on sd/mmc to read from. Should be block
+ *		    size aligned
+ *                  dst - Destination Pointer
+ *                  size - Length of Data ( Multiple of block size)
+ * Return      :    SUCCESS or Error Code
+ * Description :    Calls esdhc_read_block repeatedly for reading the
+ *                  data.
+ ***************************************************************************/
+int esdhc_read(struct mmc *mmc, uint32_t src_offset, uintptr_t dst, size_t size)
+{
+	int error = 0;
+	uint32_t blk, num_blocks;
+	uint8_t *buff = (uint8_t *)dst;
+
+#ifdef NXP_SD_DEBUG
+	INFO("sd mmc read\n");
+	INFO("src = %x, dst = %lxsize = %lu\n", src_offset, dst, size);
+#endif
+
+	/* check for size */
+	if (size == 0) {
+		return 0;
+	}
+
+	if ((size % mmc->card.block_len) != 0) {
+		ERROR("Size is not block aligned\n");
+		return -1;
+	}
+
+	if ((src_offset % mmc->card.block_len) != 0) {
+		ERROR("Size is not block aligned\n");
+		return -1;
+	}
+
+	/* start block */
+	blk = src_offset / mmc->card.block_len;
+#ifdef NXP_SD_DEBUG
+	INFO("blk = %x\n", blk);
+#endif
+
+	/* Number of blocks to be read */
+	num_blocks = size / mmc->card.block_len;
+
+	while (num_blocks) {
+		error = esdhc_read_block(mmc, buff, blk);
+		if (error != 0) {
+			ERROR("Read error = %x\n", error);
+			return error;
+		}
+
+		buff = buff + mmc->card.block_len;
+		blk++;
+		num_blocks--;
+	}
+
+	INFO("sd-mmc read done.\n");
+	return error;
+}
+
+/***************************************************************************
+ * Function    :    esdhc_write
+ * Arguments   :    src - Source Pointer
+ *                  dst_offset - offset on sd/mmc to write to. Should be block
+ *		    size aligned
+ *                  size - Length of Data (Multiple of block size)
+ * Return      :    SUCCESS or Error Code
+ * Description :    Calls esdhc_write_block repeatedly for writing the
+ *                  data.
+ ***************************************************************************/
+int esdhc_write(struct mmc *mmc, uintptr_t src, uint32_t dst_offset,
+		size_t size)
+{
+	int error = 0;
+	uint32_t blk, num_blocks;
+	uint8_t *buff = (uint8_t *)src;
+
+#ifdef NXP_SD_DEBUG
+	INFO("sd mmc write\n");
+	INFO("src = %x, dst = %lxsize = %lu\n", src, dst_offset, size);
+#endif
+
+	/* check for size */
+	if (size == 0) {
+		return 0;
+	}
+
+	if ((size % mmc->card.block_len) != 0) {
+		ERROR("Size is not block aligned\n");
+		return -1;
+	}
+
+	if ((dst_offset % mmc->card.block_len) != 0) {
+		ERROR("Size is not block aligned\n");
+		return -1;
+	}
+
+	/* start block */
+	blk = dst_offset / mmc->card.block_len;
+#ifdef NXP_SD_DEBUG
+	INFO("blk = %x\n", blk);
+#endif
+
+	/* Number of blocks to be written */
+	num_blocks = size / mmc->card.block_len;
+
+	while (num_blocks != 0U) {
+		error = esdhc_write_block(mmc, buff, blk);
+		if (error != 0U) {
+			ERROR("Write error = %x\n", error);
+			return error;
+		}
+
+		buff = buff + mmc->card.block_len;
+		blk++;
+		num_blocks--;
+	}
+
+	INFO("sd-mmc write done.\n");
+	return error;
+}
+
+static size_t ls_sd_emmc_read(int lba, uintptr_t buf, size_t size)
+{
+	struct mmc *mmc = NULL;
+	int ret;
+
+	mmc = &mmc_drv_data;
+	lba *= BLOCK_LEN_512;
+	ret = esdhc_read(mmc, lba, buf, size);
+	return ret ? 0 : size;
+}
+
+static struct io_block_dev_spec ls_emmc_dev_spec = {
+	.buffer = {
+		.offset = 0,
+		.length = 0,
+	},
+	.ops = {
+		.read = ls_sd_emmc_read,
+	},
+	.block_size = BLOCK_LEN_512,
+};
+
+int sd_emmc_init(uintptr_t *block_dev_spec,
+			uintptr_t nxp_esdhc_addr,
+			size_t nxp_sd_block_offset,
+			size_t nxp_sd_block_size,
+			bool card_detect)
+{
+	int ret;
+
+	ret = sd_mmc_init(nxp_esdhc_addr, card_detect);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ls_emmc_dev_spec.buffer.offset = nxp_sd_block_offset;
+	ls_emmc_dev_spec.buffer.length = nxp_sd_block_size;
+	*block_dev_spec = (uintptr_t)&ls_emmc_dev_spec;
+
+	return 0;
+}
diff --git a/drivers/nxp/sd/sd_mmc.mk b/drivers/nxp/sd/sd_mmc.mk
new file mode 100644
index 0000000..c83b1bd
--- /dev/null
+++ b/drivers/nxp/sd/sd_mmc.mk
@@ -0,0 +1,26 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${ADD_SD_MMC},)
+
+ADD_SD_MMC	:= 1
+
+SD_MMC_BOOT_SOURCES	+= ${PLAT_DRIVERS_PATH}/sd/sd_mmc.c \
+			   drivers/io/io_block.c
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/sd
+
+ifeq (${BL_COMM_SD_MMC_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${SD_MMC_BOOT_SOURCES}
+else
+ifeq (${BL2_SD_MMC_NEEDED},yes)
+BL2_SOURCES		+= ${SD_MMC_BOOT_SOURCES}
+endif
+ifeq (${BL3_SD_MMC_NEEDED},yes)
+BL31_SOURCES		+= ${SD_MMC_BOOT_SOURCES}
+endif
+endif
+endif
diff --git a/drivers/nxp/sec_mon/sec_mon.mk b/drivers/nxp/sec_mon/sec_mon.mk
new file mode 100644
index 0000000..aaac53f
--- /dev/null
+++ b/drivers/nxp/sec_mon/sec_mon.mk
@@ -0,0 +1,25 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${ADD_SNVS},)
+
+ADD_SNVS		:= 1
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/sec_mon
+
+SNVS_SOURCES		+= $(PLAT_DRIVERS_PATH)/sec_mon/snvs.c
+
+ifeq (${BL_COMM_SNVS_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${SNVS_SOURCES}
+else
+ifeq (${BL2_SNVS_NEEDED},yes)
+BL2_SOURCES		+= ${SNVS_SOURCES}
+endif
+ifeq (${BL31_SNVS_NEEDED},yes)
+BL31_SOURCES		+= ${SNVS_SOURCES}
+endif
+endif
+endif
diff --git a/drivers/nxp/sec_mon/snvs.c b/drivers/nxp/sec_mon/snvs.c
new file mode 100644
index 0000000..6208b67
--- /dev/null
+++ b/drivers/nxp/sec_mon/snvs.c
@@ -0,0 +1,186 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <snvs.h>
+
+static uintptr_t g_nxp_snvs_addr;
+
+void snvs_init(uintptr_t nxp_snvs_addr)
+{
+	g_nxp_snvs_addr = nxp_snvs_addr;
+}
+
+uint32_t get_snvs_state(void)
+{
+	struct snvs_regs *snvs = (struct snvs_regs *) (g_nxp_snvs_addr);
+
+	return (snvs_read32(&snvs->hp_stat) & HPSTS_MASK_SSM_ST);
+}
+
+static uint32_t do_snvs_state_transition(uint32_t state_transtion_bit,
+					 uint32_t target_state)
+{
+	struct snvs_regs *snvs = (struct snvs_regs *) (g_nxp_snvs_addr);
+	uint32_t sts = get_snvs_state();
+	uint32_t fetch_cnt = 16U;
+	uint32_t val = snvs_read32(&snvs->hp_com) | state_transtion_bit;
+
+	snvs_write32(&snvs->hp_com, val);
+
+	/* polling loop till SNVS is in target state */
+	do {
+		sts = get_snvs_state();
+	} while ((sts != target_state) && ((--fetch_cnt) != 0));
+
+	return sts;
+}
+void transition_snvs_non_secure(void)
+{
+	struct snvs_regs *snvs = (struct snvs_regs *) (g_nxp_snvs_addr);
+	uint32_t sts = get_snvs_state();
+
+	switch (sts) {
+		/* If initial state is check or Non-Secure, then
+		 * set the Software Security Violation Bit and
+		 * transition to Non-Secure State.
+		 */
+	case HPSTS_CHECK_SSM_ST:
+		sts = do_snvs_state_transition(HPCOM_SW_SV, HPSTS_NON_SECURE_SSM_ST);
+		break;
+
+		/* If initial state is Trusted, Secure or Soft-Fail, then
+		 * first set the Software Security Violation Bit and
+		 * transition to Soft-Fail State.
+		 */
+	case HPSTS_TRUST_SSM_ST:
+	case HPSTS_SECURE_SSM_ST:
+	case HPSTS_SOFT_FAIL_SSM_ST:
+		sts = do_snvs_state_transition(HPCOM_SW_SV, HPSTS_NON_SECURE_SSM_ST);
+
+		/* If SSM Soft Fail to Non-Secure State Transition
+		 * Disable is not set, then set SSM_ST bit and
+		 * transition to Non-Secure State.
+		 */
+		if ((snvs_read32(&snvs->hp_com) & HPCOM_SSM_SFNS_DIS) == 0) {
+			sts = do_snvs_state_transition(HPCOM_SSM_ST, HPSTS_NON_SECURE_SSM_ST);
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+void transition_snvs_soft_fail(void)
+{
+	do_snvs_state_transition(HPCOM_SW_FSV, HPSTS_SOFT_FAIL_SSM_ST);
+}
+
+uint32_t transition_snvs_trusted(void)
+{
+	struct snvs_regs *snvs = (struct snvs_regs *) (g_nxp_snvs_addr);
+	uint32_t sts = get_snvs_state();
+
+	switch (sts) {
+		/* If initial state is check, set the SSM_ST bit to
+		 * change the state to trusted.
+		 */
+	case HPSTS_CHECK_SSM_ST:
+		sts = do_snvs_state_transition(HPCOM_SSM_ST, HPSTS_TRUST_SSM_ST);
+		break;
+		/* If SSM Secure to Trusted State Transition Disable
+		 * is not set, then set SSM_ST bit and
+		 * transition to Trusted State.
+		 */
+	case HPSTS_SECURE_SSM_ST:
+		if ((snvs_read32(&snvs->hp_com) & HPCOM_SSM_ST_DIS) == 0) {
+			sts = do_snvs_state_transition(HPCOM_SSM_ST, HPSTS_TRUST_SSM_ST);
+		}
+		break;
+		/* If initial state is Soft-Fail or Non-Secure, then
+		 * transition to Trusted is not Possible.
+		 */
+	default:
+		break;
+	}
+
+	return sts;
+}
+
+uint32_t transition_snvs_secure(void)
+{
+	uint32_t sts = get_snvs_state();
+
+	if (sts == HPSTS_SECURE_SSM_ST) {
+		return sts;
+	}
+
+	if (sts != HPSTS_TRUST_SSM_ST) {
+		sts = transition_snvs_trusted();
+		if (sts != HPSTS_TRUST_SSM_ST) {
+			return sts;
+		}
+	}
+
+	sts = do_snvs_state_transition(HPCOM_SSM_ST, HPSTS_TRUST_SSM_ST);
+
+	return sts;
+}
+
+void snvs_write_lp_gpr_bit(uint32_t offset, uint32_t bit_pos, bool flag_val)
+{
+	if (flag_val) {
+		snvs_write32(g_nxp_snvs_addr + offset,
+			     (snvs_read32(g_nxp_snvs_addr + offset))
+			     | (1 << bit_pos));
+	} else {
+		snvs_write32(g_nxp_snvs_addr + offset,
+			     (snvs_read32(g_nxp_snvs_addr + offset))
+			     & ~(1 << bit_pos));
+	}
+}
+
+uint32_t snvs_read_lp_gpr_bit(uint32_t offset, uint32_t bit_pos)
+{
+	return (snvs_read32(g_nxp_snvs_addr + offset) & (1 << bit_pos));
+}
+
+void snvs_disable_zeroize_lp_gpr(void)
+{
+	snvs_write_lp_gpr_bit(NXP_LPCR_OFFSET,
+			  NXP_GPR_Z_DIS_BIT,
+			  true);
+}
+
+#if defined(NXP_NV_SW_MAINT_LAST_EXEC_DATA) && defined(NXP_COINED_BB)
+void snvs_write_app_data_bit(uint32_t bit_pos)
+{
+	snvs_write_lp_gpr_bit(NXP_APP_DATA_LP_GPR_OFFSET,
+			      bit_pos,
+			      true);
+}
+
+uint32_t snvs_read_app_data(void)
+{
+	return snvs_read32(g_nxp_snvs_addr + NXP_APP_DATA_LP_GPR_OFFSET);
+}
+
+uint32_t snvs_read_app_data_bit(uint32_t bit_pos)
+{
+	uint8_t ret = snvs_read_lp_gpr_bit(NXP_APP_DATA_LP_GPR_OFFSET, bit_pos);
+
+	return ((ret != 0U) ? 1U : 0U);
+}
+
+void snvs_clear_app_data(void)
+{
+	snvs_write32(g_nxp_snvs_addr + NXP_APP_DATA_LP_GPR_OFFSET, 0x0);
+}
+#endif
diff --git a/drivers/nxp/sfp/fuse_prov.c b/drivers/nxp/sfp/fuse_prov.c
new file mode 100644
index 0000000..165474f
--- /dev/null
+++ b/drivers/nxp/sfp/fuse_prov.c
@@ -0,0 +1,462 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <caam.h>
+#include <common/debug.h>
+#include <dcfg.h>
+#include <drivers/delay_timer.h>
+#include <fuse_prov.h>
+#include <sfp.h>
+#include <sfp_error_codes.h>
+
+
+static int write_a_fuse(uint32_t *fuse_addr, uint32_t *fuse_hdr_val,
+			uint32_t mask)
+{
+	uint32_t last_stored_val = sfp_read32(fuse_addr);
+
+	 /* Check if fuse already blown or not */
+	if ((last_stored_val & mask) == mask) {
+		return ERROR_ALREADY_BLOWN;
+	}
+
+	 /* Write fuse in mirror registers */
+	sfp_write32(fuse_addr, last_stored_val | (*fuse_hdr_val & mask));
+
+	 /* Read back to check if write success */
+	if (sfp_read32(fuse_addr) != (last_stored_val | (*fuse_hdr_val & mask))) {
+		return ERROR_WRITE;
+	}
+
+	return 0;
+}
+
+static int write_fuses(uint32_t *fuse_addr, uint32_t *fuse_hdr_val, uint8_t len)
+{
+	int i;
+
+	 /* Check if fuse already blown or not */
+	for (i = 0; i < len; i++) {
+		if (sfp_read32(&fuse_addr[i]) != 0) {
+			return ERROR_ALREADY_BLOWN;
+		}
+	}
+
+	 /* Write fuse in mirror registers */
+	for (i = 0; i < len; i++) {
+		sfp_write32(&fuse_addr[i], fuse_hdr_val[i]);
+	}
+
+	 /* Read back to check if write success */
+	for (i = 0; i < len; i++) {
+		if (sfp_read32(&fuse_addr[i]) != fuse_hdr_val[i]) {
+			return ERROR_WRITE;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * This function program Super Root Key Hash (SRKH) in fuse
+ * registers.
+ */
+static int prog_srkh(struct fuse_hdr_t *fuse_hdr,
+		     struct sfp_ccsr_regs_t *sfp_ccsr_regs)
+{
+	int ret = 0;
+
+	ret = write_fuses(sfp_ccsr_regs->srk_hash, fuse_hdr->srkh, 8);
+
+	if (ret != 0) {
+		ret = (ret == ERROR_ALREADY_BLOWN) ?
+			ERROR_SRKH_ALREADY_BLOWN : ERROR_SRKH_WRITE;
+	}
+
+	return ret;
+}
+
+/* This function program OEMUID[0-4] in fuse registers. */
+static int prog_oemuid(struct fuse_hdr_t *fuse_hdr,
+		       struct sfp_ccsr_regs_t *sfp_ccsr_regs)
+{
+	int i, ret = 0;
+
+	for (i = 0; i < 5; i++) {
+		 /* Check OEMUIDx to be blown or not */
+		if (((fuse_hdr->flags >> (FLAG_OUID0_SHIFT + i)) & 0x1) != 0) {
+			 /* Check if OEMUID[i] already blown or not */
+			ret = write_fuses(&sfp_ccsr_regs->oem_uid[i],
+					 &fuse_hdr->oem_uid[i], 1);
+
+			if (ret != 0) {
+				ret = (ret == ERROR_ALREADY_BLOWN) ?
+					ERROR_OEMUID_ALREADY_BLOWN
+					: ERROR_OEMUID_WRITE;
+			}
+		}
+	}
+	return ret;
+}
+
+/* This function program DCV[0-1], DRV[0-1] in fuse registers. */
+static int prog_debug(struct fuse_hdr_t *fuse_hdr,
+		      struct sfp_ccsr_regs_t *sfp_ccsr_regs)
+{
+	int ret;
+
+	 /* Check DCV to be blown or not */
+	if (((fuse_hdr->flags >> (FLAG_DCV0_SHIFT)) & 0x3) != 0) {
+		 /* Check if DCV[i] already blown or not */
+		ret = write_fuses(sfp_ccsr_regs->dcv, fuse_hdr->dcv, 2);
+
+		if (ret != 0) {
+			ret = (ret == ERROR_ALREADY_BLOWN) ?
+				ERROR_DCV_ALREADY_BLOWN
+				: ERROR_DCV_WRITE;
+		}
+	}
+
+	 /* Check DRV to be blown or not */
+	if ((((fuse_hdr->flags >> (FLAG_DRV0_SHIFT)) & 0x3)) != 0) {
+		 /* Check if DRV[i] already blown or not */
+		ret = write_fuses(sfp_ccsr_regs->drv, fuse_hdr->drv, 2);
+
+		if (ret != 0) {
+			ret = (ret == ERROR_ALREADY_BLOWN) ?
+				ERROR_DRV_ALREADY_BLOWN
+				: ERROR_DRV_WRITE;
+		} else {
+			 /* Check for DRV hamming error */
+			if (sfp_read32((void *)(get_sfp_addr()
+							+ SFP_SVHESR_OFFSET))
+				& SFP_SVHESR_DRV_MASK) {
+				return ERROR_DRV_HAMMING_ERROR;
+			}
+		}
+	}
+
+	return 0;
+}
+
+ /*
+  * Turn a 256-bit random value (32 bytes) into an OTPMK code word
+  * modifying the input data array in place
+  */
+static void otpmk_make_code_word_256(uint8_t *otpmk, bool minimal_flag)
+{
+	int i;
+	uint8_t parity_bit;
+	uint8_t code_bit;
+
+	if (minimal_flag == true) {
+		 /*
+		  * Force bits 252, 253, 254 and 255 to 1
+		  * This is because these fuses may have already been blown
+		  * and the OTPMK cannot force them back to 0
+		  */
+		otpmk[252/8] |= (1 << (252%8));
+		otpmk[253/8] |= (1 << (253%8));
+		otpmk[254/8] |= (1 << (254%8));
+		otpmk[255/8] |= (1 << (255%8));
+	}
+
+	 /* Generate the hamming code for the code word */
+	parity_bit = 0;
+	code_bit = 0;
+	for (i = 0; i < 256; i += 1) {
+		if ((otpmk[i/8] & (1 << (i%8))) != 0) {
+			parity_bit ^= 1;
+			code_bit   ^= i;
+		}
+	}
+
+	 /* Inverting otpmk[code_bit] will cause the otpmk
+	  * to become a valid code word (except for overall parity)
+	  */
+	if (code_bit < 252) {
+		otpmk[code_bit/8] ^= (1 << (code_bit % 8));
+		parity_bit  ^= 1;  // account for flipping a bit changing parity
+	} else {
+		 /* Invert two bits:  (code_bit - 4) and 4
+		  * Because we invert two bits, no need to touch the parity bit
+		  */
+		otpmk[(code_bit - 4)/8] ^= (1 << ((code_bit - 4) % 8));
+		otpmk[4/8] ^= (1 << (4 % 8));
+	}
+
+	 /* Finally, adjust the overall parity of the otpmk
+	  * otpmk bit 0
+	  */
+	otpmk[0] ^= parity_bit;
+}
+
+/* This function program One Time Programmable Master Key (OTPMK)
+ *  in fuse registers.
+ */
+static int prog_otpmk(struct fuse_hdr_t *fuse_hdr,
+		      struct sfp_ccsr_regs_t *sfp_ccsr_regs)
+{
+	int ret = 0;
+	uint32_t otpmk_flags;
+	uint32_t otpmk_random[8] __aligned(CACHE_WRITEBACK_GRANULE);
+
+	otpmk_flags = (fuse_hdr->flags >> (FLAG_OTPMK_SHIFT)) & FLAG_OTPMK_MASK;
+
+	switch (otpmk_flags) {
+	case PROG_OTPMK_MIN:
+		memset(fuse_hdr->otpmk, 0, sizeof(fuse_hdr->otpmk));
+
+		 /* Minimal OTPMK value (252-255 bits set to 1) */
+		fuse_hdr->otpmk[0] |= OTPMK_MIM_BITS_MASK;
+		break;
+
+	case PROG_OTPMK_RANDOM:
+		if (is_sec_enabled() == false) {
+			ret = ERROR_OTPMK_SEC_DISABLED;
+			goto out;
+		}
+
+		 /* Generate Random number using CAAM for OTPMK */
+		memset(otpmk_random, 0, sizeof(otpmk_random));
+		if (get_rand_bytes_hw((uint8_t *)otpmk_random,
+				      sizeof(otpmk_random)) != 0) {
+			ret = ERROR_OTPMK_SEC_ERROR;
+			goto out;
+		}
+
+		 /* Run hamming over random no. to make OTPMK */
+		otpmk_make_code_word_256((uint8_t *)otpmk_random, false);
+
+		 /* Swap OTPMK */
+		fuse_hdr->otpmk[0] = otpmk_random[7];
+		fuse_hdr->otpmk[1] = otpmk_random[6];
+		fuse_hdr->otpmk[2] = otpmk_random[5];
+		fuse_hdr->otpmk[3] = otpmk_random[4];
+		fuse_hdr->otpmk[4] = otpmk_random[3];
+		fuse_hdr->otpmk[5] = otpmk_random[2];
+		fuse_hdr->otpmk[6] = otpmk_random[1];
+		fuse_hdr->otpmk[7] = otpmk_random[0];
+		break;
+
+	case PROG_OTPMK_USER:
+		break;
+
+	case PROG_OTPMK_RANDOM_MIN:
+		 /* Here assumption is that user is aware of minimal OTPMK
+		  * already blown.
+		  */
+
+		 /* Generate Random number using CAAM for OTPMK */
+		if (is_sec_enabled() == false) {
+			ret = ERROR_OTPMK_SEC_DISABLED;
+			goto out;
+		}
+
+		memset(otpmk_random, 0, sizeof(otpmk_random));
+		if (get_rand_bytes_hw((uint8_t *)otpmk_random,
+				      sizeof(otpmk_random)) != 0) {
+			ret = ERROR_OTPMK_SEC_ERROR;
+			goto out;
+		}
+
+		 /* Run hamming over random no. to make OTPMK */
+		otpmk_make_code_word_256((uint8_t *)otpmk_random, true);
+
+		 /* Swap OTPMK */
+		fuse_hdr->otpmk[0] = otpmk_random[7];
+		fuse_hdr->otpmk[1] = otpmk_random[6];
+		fuse_hdr->otpmk[2] = otpmk_random[5];
+		fuse_hdr->otpmk[3] = otpmk_random[4];
+		fuse_hdr->otpmk[4] = otpmk_random[3];
+		fuse_hdr->otpmk[5] = otpmk_random[2];
+		fuse_hdr->otpmk[6] = otpmk_random[1];
+		fuse_hdr->otpmk[7] = otpmk_random[0];
+		break;
+
+	case PROG_OTPMK_USER_MIN:
+		 /*
+		  * Here assumption is that user is aware of minimal OTPMK
+		  * already blown. Check if minimal bits are set in user
+		  * supplied OTPMK.
+		  */
+		if ((fuse_hdr->otpmk[0] & OTPMK_MIM_BITS_MASK) !=
+							OTPMK_MIM_BITS_MASK) {
+			ret = ERROR_OTPMK_USER_MIN;
+			goto out;
+		}
+		break;
+
+	default:
+		ret = 0;
+		goto out;
+	}
+
+	ret = write_fuses(sfp_ccsr_regs->otpmk, fuse_hdr->otpmk, 8);
+
+	if (ret != 0) {
+		ret = (ret == ERROR_ALREADY_BLOWN) ?
+			ERROR_OTPMK_ALREADY_BLOWN
+			: ERROR_OTPMK_WRITE;
+	} else {
+		 /* Check for DRV hamming error */
+		if ((sfp_read32((void *)(get_sfp_addr() + SFP_SVHESR_OFFSET))
+			& SFP_SVHESR_OTPMK_MASK) != 0) {
+			ret = ERROR_OTPMK_HAMMING_ERROR;
+		}
+	}
+
+out:
+	return ret;
+}
+
+/* This function program OSPR1 in fuse registers.
+ */
+static int prog_ospr1(struct fuse_hdr_t *fuse_hdr,
+		      struct sfp_ccsr_regs_t *sfp_ccsr_regs)
+{
+	int ret;
+	uint32_t mask = 0;
+
+#ifdef NXP_SFP_VER_3_4
+	if (((fuse_hdr->flags >> FLAG_MC_SHIFT) & 0x1) != 0) {
+		mask = OSPR1_MC_MASK;
+	}
+#endif
+	if (((fuse_hdr->flags >> FLAG_DBG_LVL_SHIFT) & 0x1) != 0) {
+		mask = mask | OSPR1_DBG_LVL_MASK;
+	}
+
+	ret = write_a_fuse(&sfp_ccsr_regs->ospr1, &fuse_hdr->ospr1, mask);
+
+	if (ret != 0) {
+		ret = (ret == ERROR_ALREADY_BLOWN) ?
+				ERROR_OSPR1_ALREADY_BLOWN
+				: ERROR_OSPR1_WRITE;
+	}
+
+	return ret;
+}
+
+/* This function program SYSCFG in fuse registers.
+ */
+static int prog_syscfg(struct fuse_hdr_t *fuse_hdr,
+		       struct sfp_ccsr_regs_t *sfp_ccsr_regs)
+{
+	int ret;
+
+	 /* Check if SYSCFG already blown or not */
+	ret = write_a_fuse(&sfp_ccsr_regs->ospr, &fuse_hdr->sc, OSPR0_SC_MASK);
+
+	if (ret != 0) {
+		ret = (ret == ERROR_ALREADY_BLOWN) ?
+				ERROR_SC_ALREADY_BLOWN
+				: ERROR_SC_WRITE;
+	}
+
+	return ret;
+}
+
+/* This function does fuse provisioning.
+ */
+int provision_fuses(unsigned long long fuse_scr_addr,
+		    bool en_povdd_status)
+{
+	struct fuse_hdr_t *fuse_hdr = NULL;
+	struct sfp_ccsr_regs_t *sfp_ccsr_regs = (void *)(get_sfp_addr()
+							+ SFP_FUSE_REGS_OFFSET);
+	int ret = 0;
+
+	fuse_hdr = (struct fuse_hdr_t *)fuse_scr_addr;
+
+	/*
+	 * Check for Write Protect (WP) fuse. If blown then do
+	 *  no fuse provisioning.
+	 */
+	if ((sfp_read32(&sfp_ccsr_regs->ospr) & 0x1) != 0) {
+		goto out;
+	}
+
+	 /* Check if SRKH to be blown or not */
+	if (((fuse_hdr->flags >> FLAG_SRKH_SHIFT) & 0x1) != 0) {
+		INFO("Fuse: Program SRKH\n");
+		ret = prog_srkh(fuse_hdr, sfp_ccsr_regs);
+		if (ret != 0) {
+			error_handler(ret);
+			goto out;
+		}
+	}
+
+	 /* Check if OEMUID to be blown or not */
+	if (((fuse_hdr->flags >> FLAG_OUID0_SHIFT) & FLAG_OUID_MASK) != 0) {
+		INFO("Fuse: Program OEMUIDs\n");
+		ret = prog_oemuid(fuse_hdr, sfp_ccsr_regs);
+		if (ret != 0) {
+			error_handler(ret);
+			goto out;
+		}
+	}
+
+	 /* Check if Debug values to be blown or not */
+	if (((fuse_hdr->flags >> FLAG_DCV0_SHIFT) & FLAG_DEBUG_MASK) != 0) {
+		INFO("Fuse: Program Debug values\n");
+		ret = prog_debug(fuse_hdr, sfp_ccsr_regs);
+		if (ret != 0) {
+			error_handler(ret);
+			goto out;
+		}
+	}
+
+	 /* Check if OTPMK values to be blown or not */
+	if (((fuse_hdr->flags >> FLAG_OTPMK_SHIFT) & PROG_NO_OTPMK) !=
+		PROG_NO_OTPMK) {
+		INFO("Fuse: Program OTPMK\n");
+		ret = prog_otpmk(fuse_hdr, sfp_ccsr_regs);
+		if (ret != 0) {
+			error_handler(ret);
+			goto out;
+		}
+	}
+
+
+	 /* Check if MC or DBG LVL to be blown or not */
+	if ((((fuse_hdr->flags >> FLAG_MC_SHIFT) & 0x1) != 0) ||
+		(((fuse_hdr->flags >> FLAG_DBG_LVL_SHIFT) & 0x1) != 0)) {
+		INFO("Fuse: Program OSPR1\n");
+		ret = prog_ospr1(fuse_hdr, sfp_ccsr_regs);
+		if (ret != 0) {
+			error_handler(ret);
+			goto out;
+		}
+	}
+
+	 /* Check if SYSCFG to be blown or not */
+	if (((fuse_hdr->flags >> FLAG_SYSCFG_SHIFT) & 0x1) != 0) {
+		INFO("Fuse: Program SYSCFG\n");
+		ret = prog_syscfg(fuse_hdr, sfp_ccsr_regs);
+		if (ret != 0) {
+			error_handler(ret);
+			goto out;
+		}
+	}
+
+	if (en_povdd_status) {
+		ret = sfp_program_fuses();
+		if (ret != 0) {
+			error_handler(ret);
+			goto out;
+		}
+	}
+out:
+	return ret;
+}
diff --git a/drivers/nxp/sfp/sfp.c b/drivers/nxp/sfp/sfp.c
new file mode 100644
index 0000000..e06c6b9
--- /dev/null
+++ b/drivers/nxp/sfp/sfp.c
@@ -0,0 +1,167 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <caam.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <sfp.h>
+#include <sfp_error_codes.h>
+
+static uintptr_t g_nxp_sfp_addr;
+static uint32_t srk_hash[SRK_HASH_SIZE/sizeof(uint32_t)]
+					__aligned(CACHE_WRITEBACK_GRANULE);
+
+void sfp_init(uintptr_t nxp_sfp_addr)
+{
+	g_nxp_sfp_addr = nxp_sfp_addr;
+}
+
+uintptr_t get_sfp_addr(void)
+{
+	return g_nxp_sfp_addr;
+}
+
+uint32_t *get_sfp_srk_hash(void)
+{
+	struct sfp_ccsr_regs_t *sfp_ccsr_regs =
+			(void *) (g_nxp_sfp_addr + SFP_FUSE_REGS_OFFSET);
+	int i = 0;
+
+	/* Add comparison of hash with SFP hash here */
+	for (i = 0; i < SRK_HASH_SIZE/sizeof(uint32_t); i++)
+		srk_hash[i] =
+			mmio_read_32((uintptr_t)&sfp_ccsr_regs->srk_hash[i]);
+
+	return srk_hash;
+}
+
+void set_sfp_wr_disable(void)
+{
+	/*
+	 * Mark SFP Write Disable and Write Disable Lock
+	 * Bit to prevent write to SFP fuses like
+	 * OUID's, Key Revocation fuse etc
+	 */
+	void *sfpcr = (void *)(g_nxp_sfp_addr + SFP_SFPCR_OFFSET);
+	uint32_t sfpcr_val;
+
+	sfpcr_val = sfp_read32(sfpcr);
+	sfpcr_val |= (SFP_SFPCR_WD | SFP_SFPCR_WDL);
+	sfp_write32(sfpcr, sfpcr_val);
+}
+
+int sfp_program_fuses(void)
+{
+	uint32_t ingr;
+	uint32_t sfp_cmd_status = 0U;
+	int ret = 0;
+
+	/* Program SFP fuses from mirror registers */
+	sfp_write32((void *)(g_nxp_sfp_addr + SFP_INGR_OFFSET),
+		    SFP_INGR_PROGFB_CMD);
+
+	/* Wait until fuse programming is successful */
+	do {
+		ingr = sfp_read32(g_nxp_sfp_addr + SFP_INGR_OFFSET);
+	} while (ingr & SFP_INGR_PROGFB_CMD);
+
+	/* Check for SFP fuse programming error */
+	sfp_cmd_status = sfp_read32(g_nxp_sfp_addr + SFP_INGR_OFFSET)
+			 & SFP_INGR_ERROR_MASK;
+
+	if (sfp_cmd_status != 0U) {
+		return ERROR_PROGFB_CMD;
+	}
+
+	return ret;
+}
+
+uint32_t sfp_read_oem_uid(uint8_t oem_uid)
+{
+	uint32_t val = 0U;
+	struct sfp_ccsr_regs_t *sfp_ccsr_regs = (void *)(g_nxp_sfp_addr
+							+ SFP_FUSE_REGS_OFFSET);
+
+	if (oem_uid > MAX_OEM_UID) {
+		ERROR("Invalid OEM UID received.\n");
+		return ERROR_OEMUID_WRITE;
+	}
+
+	val = sfp_read32(&sfp_ccsr_regs->oem_uid[oem_uid]);
+
+	return val;
+}
+
+/*
+ * return val:  0 - No update required.
+ *              1 - successful update done.
+ *              ERROR_OEMUID_WRITE - Invalid OEM UID
+ */
+uint32_t sfp_write_oem_uid(uint8_t oem_uid, uint32_t sfp_val)
+{
+	uint32_t val = 0U;
+	struct sfp_ccsr_regs_t *sfp_ccsr_regs = (void *)(g_nxp_sfp_addr
+							+ SFP_FUSE_REGS_OFFSET);
+
+	val = sfp_read_oem_uid(oem_uid);
+
+	if (val == ERROR_OEMUID_WRITE) {
+		return ERROR_OEMUID_WRITE;
+	}
+
+	/* Counter already set. No need to do anything */
+	if ((val & sfp_val) != 0U) {
+		return 0U;
+	}
+
+	val |= sfp_val;
+
+	INFO("SFP Value is %x for setting sfp_val = %d\n", val, sfp_val);
+
+	sfp_write32(&sfp_ccsr_regs->oem_uid[oem_uid], val);
+
+	return 1U;
+}
+
+int sfp_check_its(void)
+{
+	struct sfp_ccsr_regs_t *sfp_ccsr_regs = (void *)(g_nxp_sfp_addr
+							+ SFP_FUSE_REGS_OFFSET);
+
+	if ((sfp_read32(&sfp_ccsr_regs->ospr) & OSPR_ITS_MASK) != 0) {
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+int sfp_check_oem_wp(void)
+{
+	struct sfp_ccsr_regs_t *sfp_ccsr_regs = (void *)(g_nxp_sfp_addr
+							+ SFP_FUSE_REGS_OFFSET);
+
+	if ((sfp_read32(&sfp_ccsr_regs->ospr) & OSPR_WP_MASK) != 0) {
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+/* This function returns ospr's key_revoc values.*/
+uint32_t get_key_revoc(void)
+{
+	struct sfp_ccsr_regs_t *sfp_ccsr_regs = (void *)(g_nxp_sfp_addr
+							+ SFP_FUSE_REGS_OFFSET);
+
+	return (sfp_read32(&sfp_ccsr_regs->ospr) & OSPR_KEY_REVOC_MASK) >>
+						OSPR_KEY_REVOC_SHIFT;
+}
diff --git a/drivers/nxp/sfp/sfp.mk b/drivers/nxp/sfp/sfp.mk
new file mode 100644
index 0000000..de708c5
--- /dev/null
+++ b/drivers/nxp/sfp/sfp.mk
@@ -0,0 +1,33 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#-----------------------------------------------------------------------------
+ifeq (${SFP_ADDED},)
+
+SFP_ADDED		:= 1
+$(eval $(call add_define, NXP_SFP_ENABLED))
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/sfp
+
+SFP_SOURCES		+= $(PLAT_DRIVERS_PATH)/sfp/sfp.c
+
+ifeq (${FUSE_PROG}, 1)
+SFP_BL2_SOURCES		+= $(PLAT_DRIVERS_PATH)/sfp/fuse_prov.c
+endif
+
+ifeq (${BL_COMM_SFP_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${SFP_SOURCES}
+BL2_SOURCES		+= ${SFP_BL2_SOURCES}
+else
+ifeq (${BL2_SFP_NEEDED},yes)
+BL2_SOURCES		+= ${SFP_SOURCES}\
+			   ${SFP_BL2_SOURCES}
+endif
+ifeq (${BL31_SFP_NEEDED},yes)
+BL31_SOURCES		+= ${SFP_SOURCES}
+endif
+endif
+endif
+#------------------------------------------------
diff --git a/drivers/nxp/timer/nxp_timer.c b/drivers/nxp/timer/nxp_timer.c
new file mode 100644
index 0000000..8eecd2e
--- /dev/null
+++ b/drivers/nxp/timer/nxp_timer.c
@@ -0,0 +1,143 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <nxp_timer.h>
+#include <plat/common/platform.h>
+
+static uintptr_t g_nxp_timer_addr;
+static timer_ops_t ops;
+
+uint64_t get_timer_val(uint64_t start)
+{
+	uint64_t cntpct;
+
+	isb();
+	cntpct = read_cntpct_el0();
+	return (cntpct * 1000ULL / read_cntfrq_el0() - start);
+}
+
+static uint32_t timer_get_value(void)
+{
+	uint64_t cntpct;
+
+	isb();
+	cntpct = read_cntpct_el0();
+#ifdef ERRATA_SOC_A008585
+	uint8_t	max_fetch_count = 10U;
+	/* This erratum number needs to be confirmed to match ARM document */
+	uint64_t temp;
+
+	isb();
+	temp = read_cntpct_el0();
+
+	while (temp != cntpct && max_fetch_count) {
+		isb();
+		cntpct = read_cntpct_el0();
+		isb();
+		temp = read_cntpct_el0();
+		max_fetch_count--;
+	}
+#endif
+
+	/*
+	 * Generic delay timer implementation expects the timer to be a down
+	 * counter. We apply bitwise NOT operator to the tick values returned
+	 * by read_cntpct_el0() to simulate the down counter. The value is
+	 * clipped from 64 to 32 bits.
+	 */
+	return (uint32_t)(~cntpct);
+}
+
+static void delay_timer_init_args(uint32_t mult, uint32_t div)
+{
+	ops.get_timer_value	= timer_get_value,
+	ops.clk_mult		= mult;
+	ops.clk_div		= div;
+
+	timer_init(&ops);
+
+	VERBOSE("Generic delay timer configured with mult=%u and div=%u\n",
+		mult, div);
+}
+
+/*
+ * Initialise the nxp on-chip free rolling usec counter as the delay
+ * timer.
+ */
+void delay_timer_init(uintptr_t nxp_timer_addr)
+{
+	/* Value in ticks */
+	unsigned int mult = MHZ_TICKS_PER_SEC;
+
+	unsigned int div;
+
+	unsigned int counter_base_frequency = plat_get_syscnt_freq2();
+
+	g_nxp_timer_addr = nxp_timer_addr;
+	/* Rounding off the Counter Frequency to MHZ_TICKS_PER_SEC */
+	if (counter_base_frequency > MHZ_TICKS_PER_SEC) {
+		counter_base_frequency = (counter_base_frequency
+					/ MHZ_TICKS_PER_SEC)
+					* MHZ_TICKS_PER_SEC;
+	} else {
+		counter_base_frequency = (counter_base_frequency
+					/ KHZ_TICKS_PER_SEC)
+					* KHZ_TICKS_PER_SEC;
+	}
+
+	/* Value in ticks per second (Hz) */
+	div = counter_base_frequency;
+
+	/* Reduce multiplier and divider by dividing them repeatedly by 10 */
+	while ((mult % 10U == 0U) && (div % 10U == 0U)) {
+		mult /= 10U;
+		div /= 10U;
+	}
+
+	/* Enable and initialize the System level generic timer */
+	mmio_write_32(g_nxp_timer_addr + CNTCR_OFF,
+			CNTCR_FCREQ(0) | CNTCR_EN);
+
+	delay_timer_init_args(mult, div);
+}
+
+
+#ifdef IMAGE_BL31
+/*******************************************************************************
+ * TBD: Configures access to the system counter timer module.
+ ******************************************************************************/
+void ls_configure_sys_timer(uintptr_t ls_sys_timctl_base,
+			    uint8_t ls_config_cntacr,
+			    uint8_t plat_ls_ns_timer_frame_id)
+{
+	unsigned int reg_val;
+
+	if (ls_config_cntacr == 1U) {
+		reg_val = (1U << CNTACR_RPCT_SHIFT) | (1U << CNTACR_RVCT_SHIFT);
+		reg_val |= (1U << CNTACR_RFRQ_SHIFT) | (1U << CNTACR_RVOFF_SHIFT);
+		reg_val |= (1U << CNTACR_RWVT_SHIFT) | (1U << CNTACR_RWPT_SHIFT);
+		mmio_write_32(ls_sys_timctl_base +
+		      CNTACR_BASE(plat_ls_ns_timer_frame_id), reg_val);
+		mmio_write_32(ls_sys_timctl_base, plat_get_syscnt_freq2());
+	}
+
+	reg_val = (1U << CNTNSAR_NS_SHIFT(plat_ls_ns_timer_frame_id));
+	mmio_write_32(ls_sys_timctl_base + CNTNSAR, reg_val);
+}
+
+void enable_init_timer(void)
+{
+	/* Enable and initialize the System level generic timer */
+	mmio_write_32(g_nxp_timer_addr + CNTCR_OFF,
+			CNTCR_FCREQ(0) | CNTCR_EN);
+}
+#endif
diff --git a/drivers/nxp/timer/timer.mk b/drivers/nxp/timer/timer.mk
new file mode 100644
index 0000000..d658d19
--- /dev/null
+++ b/drivers/nxp/timer/timer.mk
@@ -0,0 +1,25 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${ADD_TIMER},)
+
+ADD_TIMER		:= 1
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/timer
+TIMER_SOURCES		+= drivers/delay_timer/delay_timer.c	\
+			   $(PLAT_DRIVERS_PATH)/timer/nxp_timer.c
+
+ifeq (${BL_COMM_TIMER_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${TIMER_SOURCES}
+else
+ifeq (${BL2_TIMER_NEEDED},yes)
+BL2_SOURCES		+= ${TIMER_SOURCES}
+endif
+ifeq (${BL31_TIMER_NEEDED},yes)
+BL31_SOURCES		+= ${TIMER_SOURCES}
+endif
+endif
+endif
diff --git a/drivers/nxp/tzc/plat_tzc380.c b/drivers/nxp/tzc/plat_tzc380.c
new file mode 100644
index 0000000..13cf3b9
--- /dev/null
+++ b/drivers/nxp/tzc/plat_tzc380.c
@@ -0,0 +1,152 @@
+/*
+ * Copyright 2018-2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <plat_tzc380.h>
+
+#pragma weak populate_tzc380_reg_list
+
+#ifdef DEFAULT_TZASC_CONFIG
+/*
+ * Typical Memory map of DRAM0
+ *    |-----------NXP_NS_DRAM_ADDR ( = NXP_DRAM0_ADDR)----------|
+ *    |								|
+ *    |								|
+ *    |			Non-SECURE REGION			|
+ *    |								|
+ *    |								|
+ *    |								|
+ *    |------- (NXP_NS_DRAM_ADDR + NXP_NS_DRAM_SIZE - 1) -------|
+ *    |-----------------NXP_SECURE_DRAM_ADDR--------------------|
+ *    |								|
+ *    |								|
+ *    |								|
+ *    |			SECURE REGION (= 64MB)			|
+ *    |								|
+ *    |								|
+ *    |								|
+ *    |--- (NXP_SECURE_DRAM_ADDR + NXP_SECURE_DRAM_SIZE - 1)----|
+ *    |-----------------NXP_SP_SHRD_DRAM_ADDR-------------------|
+ *    |								|
+ *    |	       Secure EL1 Payload SHARED REGION (= 2MB)         |
+ *    |								|
+ *    |-----------(NXP_DRAM0_ADDR + NXP_DRAM0_SIZE - 1)---------|
+ *
+ *
+ *
+ * Typical Memory map of DRAM1
+ *    |---------------------NXP_DRAM1_ADDR----------------------|
+ *    |								|
+ *    |								|
+ *    |			Non-SECURE REGION			|
+ *    |								|
+ *    |								|
+ *    |---(NXP_DRAM1_ADDR + Dynamically calculated Size - 1) ---|
+ *
+ *
+ * Typical Memory map of DRAM2
+ *    |---------------------NXP_DRAM2_ADDR----------------------|
+ *    |								|
+ *    |								|
+ *    |			Non-SECURE REGION			|
+ *    |								|
+ *    |								|
+ *    |---(NXP_DRAM2_ADDR + Dynamically calculated Size - 1) ---|
+ */
+
+/*****************************************************************************
+ * This function sets up access permissions on memory regions
+ *
+ * Input:
+ *	tzc380_reg_list	: TZC380 Region List
+ *	dram_idx	: DRAM index
+ *	list_idx	: TZC380 Region List Index
+ *	dram_start_addr	: Start address of DRAM at dram_idx.
+ *	dram_size	: Size of DRAM at dram_idx.
+ *	secure_dram_sz	: Secure DRAM Size
+ *	shrd_dram_sz	: Shared DRAM Size
+ *
+ * Out:
+ *	list_idx	: last populated index + 1
+ *
+ ****************************************************************************/
+int populate_tzc380_reg_list(struct tzc380_reg *tzc380_reg_list,
+			     int dram_idx, int list_idx,
+			     uint64_t dram_start_addr,
+			     uint64_t dram_size,
+			     uint32_t secure_dram_sz,
+			     uint32_t shrd_dram_sz)
+{
+	/* Region 0: Default region marked as Non-Secure */
+	if (list_idx == 0) {
+		tzc380_reg_list[list_idx].secure = TZC_ATTR_SP_NS_RW;
+		tzc380_reg_list[list_idx].enabled = TZC_ATTR_REGION_DISABLE;
+		tzc380_reg_list[list_idx].addr = UL(0x0);
+		tzc380_reg_list[list_idx].size = 0x0;
+		tzc380_reg_list[list_idx].sub_mask = 0x0; /* all enabled */
+		list_idx++;
+	}
+	/* Continue with list entries for index > 0 */
+	if (dram_idx == 0) {
+		/* TZC Region 1 on DRAM0 for Secure Memory*/
+		tzc380_reg_list[list_idx].secure = TZC_ATTR_SP_S_RW;
+		tzc380_reg_list[list_idx].enabled = TZC_ATTR_REGION_ENABLE;
+		tzc380_reg_list[list_idx].addr = dram_start_addr + dram_size;
+		tzc380_reg_list[list_idx].size = secure_dram_sz;
+		tzc380_reg_list[list_idx].sub_mask = 0x0; /* all enabled */
+		list_idx++;
+
+		/* TZC Region 2 on DRAM0 for Shared Memory*/
+		tzc380_reg_list[list_idx].secure = TZC_ATTR_SP_S_RW;
+		tzc380_reg_list[list_idx].enabled = TZC_ATTR_REGION_ENABLE;
+		tzc380_reg_list[list_idx].addr = dram_start_addr + dram_size + secure_dram_sz;
+		tzc380_reg_list[list_idx].size = shrd_dram_sz;
+		tzc380_reg_list[list_idx].sub_mask = 0x0; /* all enabled */
+		list_idx++;
+
+	}
+
+	return list_idx;
+}
+#else
+int populate_tzc380_reg_list(struct tzc380_reg *tzc380_reg_list,
+			     int dram_idx, int list_idx,
+			     uint64_t dram_start_addr,
+			     uint64_t dram_size,
+			     uint32_t secure_dram_sz,
+			     uint32_t shrd_dram_sz)
+{
+	ERROR("tzc380_reg_list used is not a default list\n");
+	ERROR("%s needs to be over-written.\n", __func__);
+	return 0;
+}
+#endif	/* DEFAULT_TZASC_CONFIG */
+
+
+void mem_access_setup(uintptr_t base, uint32_t total_regions,
+			struct tzc380_reg *tzc380_reg_list)
+{
+	uint32_t indx = 0;
+	unsigned int attr_value;
+
+	VERBOSE("Configuring TrustZone Controller tzc380\n");
+
+	tzc380_init(base);
+
+	tzc380_set_action(TZC_ACTION_NONE);
+
+	for (indx = 0; indx < total_regions; indx++) {
+		attr_value = tzc380_reg_list[indx].secure |
+			TZC_ATTR_SUBREG_DIS(tzc380_reg_list[indx].sub_mask) |
+			TZC_ATTR_REGION_SIZE(tzc380_reg_list[indx].size) |
+			tzc380_reg_list[indx].enabled;
+
+		tzc380_configure_region(indx, tzc380_reg_list[indx].addr,
+				attr_value);
+	}
+
+	tzc380_set_action(TZC_ACTION_ERR);
+}
diff --git a/drivers/nxp/tzc/plat_tzc400.c b/drivers/nxp/tzc/plat_tzc400.c
new file mode 100644
index 0000000..4fe5221
--- /dev/null
+++ b/drivers/nxp/tzc/plat_tzc400.c
@@ -0,0 +1,187 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <common/debug.h>
+
+#include <plat_tzc400.h>
+
+#pragma weak populate_tzc400_reg_list
+
+#ifdef DEFAULT_TZASC_CONFIG
+/*
+ * Typical Memory map of DRAM0
+ *    |-----------NXP_NS_DRAM_ADDR ( = NXP_DRAM0_ADDR)----------|
+ *    |								|
+ *    |								|
+ *    |			Non-SECURE REGION			|
+ *    |								|
+ *    |								|
+ *    |								|
+ *    |------- (NXP_NS_DRAM_ADDR + NXP_NS_DRAM_SIZE - 1) -------|
+ *    |-----------------NXP_SECURE_DRAM_ADDR--------------------|
+ *    |								|
+ *    |								|
+ *    |								|
+ *    |			SECURE REGION (= 64MB)			|
+ *    |								|
+ *    |								|
+ *    |								|
+ *    |--- (NXP_SECURE_DRAM_ADDR + NXP_SECURE_DRAM_SIZE - 1)----|
+ *    |-----------------NXP_SP_SHRD_DRAM_ADDR-------------------|
+ *    |								|
+ *    |	       Secure EL1 Payload SHARED REGION (= 2MB)         |
+ *    |								|
+ *    |-----------(NXP_DRAM0_ADDR + NXP_DRAM0_SIZE - 1)---------|
+ *
+ *
+ *
+ * Typical Memory map of DRAM1
+ *    |---------------------NXP_DRAM1_ADDR----------------------|
+ *    |								|
+ *    |								|
+ *    |			Non-SECURE REGION			|
+ *    |								|
+ *    |								|
+ *    |---(NXP_DRAM1_ADDR + Dynamically calculated Size - 1) ---|
+ *
+ *
+ * Typical Memory map of DRAM2
+ *    |---------------------NXP_DRAM2_ADDR----------------------|
+ *    |								|
+ *    |								|
+ *    |			Non-SECURE REGION			|
+ *    |								|
+ *    |								|
+ *    |---(NXP_DRAM2_ADDR + Dynamically calculated Size - 1) ---|
+ */
+
+/*****************************************************************************
+ * This function sets up access permissions on memory regions
+ *
+ * Input:
+ *	tzc400_reg_list	: TZC400 Region List
+ *	dram_idx	: DRAM index
+ *	list_idx	: TZC400 Region List Index
+ *	dram_start_addr	: Start address of DRAM at dram_idx.
+ *	dram_size	: Size of DRAM at dram_idx.
+ *	secure_dram_sz	: Secure DRAM Size
+ *	shrd_dram_sz	: Shared DRAM Size
+ *
+ * Out:
+ *	list_idx	: last populated index + 1
+ *
+ ****************************************************************************/
+int populate_tzc400_reg_list(struct tzc400_reg *tzc400_reg_list,
+			     int dram_idx, int list_idx,
+			     uint64_t dram_start_addr,
+			     uint64_t dram_size,
+			     uint32_t secure_dram_sz,
+			     uint32_t shrd_dram_sz)
+{
+	if (list_idx == 0) {
+		/* No need to configure TZC Region 0 in this list.
+		 */
+		list_idx++;
+	}
+	/* Continue with list entries for index > 0 */
+	if (dram_idx == 0) {
+		/* TZC Region 1 on DRAM0 for Secure Memory*/
+		tzc400_reg_list[list_idx].reg_filter_en = 1;
+		tzc400_reg_list[list_idx].start_addr = dram_start_addr + dram_size;
+		tzc400_reg_list[list_idx].end_addr = dram_start_addr + dram_size
+						+ secure_dram_sz - 1;
+		tzc400_reg_list[list_idx].sec_attr = TZC_REGION_S_RDWR;
+		tzc400_reg_list[list_idx].nsaid_permissions = TZC_REGION_NS_NONE;
+		list_idx++;
+
+		/* TZC Region 2 on DRAM0 for Shared Memory*/
+		tzc400_reg_list[list_idx].reg_filter_en = 1;
+		tzc400_reg_list[list_idx].start_addr = dram_start_addr + dram_size
+							+ secure_dram_sz;
+		tzc400_reg_list[list_idx].end_addr = dram_start_addr + dram_size
+							+ secure_dram_sz
+							+ shrd_dram_sz
+							- 1;
+		tzc400_reg_list[list_idx].sec_attr = TZC_REGION_S_RDWR;
+		tzc400_reg_list[list_idx].nsaid_permissions = TZC_NS_ACCESS_ID;
+		list_idx++;
+
+		/* TZC Region 3 on DRAM0 for Non-Secure Memory*/
+		tzc400_reg_list[list_idx].reg_filter_en = 1;
+		tzc400_reg_list[list_idx].start_addr = dram_start_addr;
+		tzc400_reg_list[list_idx].end_addr = dram_start_addr + dram_size
+							- 1;
+		tzc400_reg_list[list_idx].sec_attr = TZC_REGION_S_RDWR;
+		tzc400_reg_list[list_idx].nsaid_permissions = TZC_NS_ACCESS_ID;
+		list_idx++;
+	} else {
+		/* TZC Region 3+i on DRAM(> 0) for Non-Secure Memory*/
+		tzc400_reg_list[list_idx].reg_filter_en = 1;
+		tzc400_reg_list[list_idx].start_addr = dram_start_addr;
+		tzc400_reg_list[list_idx].end_addr = dram_start_addr + dram_size
+							- 1;
+		tzc400_reg_list[list_idx].sec_attr = TZC_REGION_S_RDWR;
+		tzc400_reg_list[list_idx].nsaid_permissions = TZC_NS_ACCESS_ID;
+		list_idx++;
+	}
+
+	return list_idx;
+}
+#else
+int populate_tzc400_reg_list(struct tzc400_reg *tzc400_reg_list,
+			     int dram_idx, int list_idx,
+			     uint64_t dram_start_addr,
+			     uint64_t dram_size,
+			     uint32_t secure_dram_sz,
+			     uint32_t shrd_dram_sz)
+{
+	ERROR("tzc400_reg_list used is not a default list\n");
+	ERROR("%s needs to be over-written.\n", __func__);
+	return 0;
+}
+#endif	/* DEFAULT_TZASC_CONFIG */
+
+/*******************************************************************************
+ * Configure memory access permissions
+ *   - Region 0 with no access;
+ *   - Region 1 to 4 as per the tzc400_reg_list populated by
+ *     function populate_tzc400_reg_list() with default for all the SoC.
+ ******************************************************************************/
+void mem_access_setup(uintptr_t base, uint32_t total_regions,
+		      struct tzc400_reg *tzc400_reg_list)
+{
+	uint32_t list_indx = 0U;
+
+	INFO("Configuring TrustZone Controller\n");
+
+	tzc400_init(base);
+
+	/* Disable filters. */
+	tzc400_disable_filters();
+
+	/* Region 0 set to no access by default */
+	tzc400_configure_region0(TZC_REGION_S_NONE, 0U);
+
+	for (list_indx = 1U; list_indx < total_regions; list_indx++) {
+		tzc400_configure_region(
+			tzc400_reg_list[list_indx].reg_filter_en,
+			list_indx,
+			tzc400_reg_list[list_indx].start_addr,
+			tzc400_reg_list[list_indx].end_addr,
+			tzc400_reg_list[list_indx].sec_attr,
+			tzc400_reg_list[list_indx].nsaid_permissions);
+	}
+
+	/*
+	 * Raise an exception if a NS device tries to access secure memory
+	 * TODO: Add interrupt handling support.
+	 */
+	tzc400_set_action(TZC_ACTION_ERR);
+
+	/* Enable filters. */
+	tzc400_enable_filters();
+}
diff --git a/drivers/nxp/tzc/tzc.mk b/drivers/nxp/tzc/tzc.mk
new file mode 100644
index 0000000..4418bfc
--- /dev/null
+++ b/drivers/nxp/tzc/tzc.mk
@@ -0,0 +1,40 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${ADD_TZASC},)
+
+ADD_TZASC		:= 1
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/tzc
+
+ifeq ($(TZC_ID), TZC400)
+TZASC_SOURCES		+= drivers/arm/tzc/tzc400.c\
+			   $(PLAT_DRIVERS_PATH)/tzc/plat_tzc400.c
+else
+ifeq ($(TZC_ID), TZC380)
+TZASC_SOURCES		+= drivers/arm/tzc/tzc380.c\
+			   $(PLAT_DRIVERS_PATH)/tzc/plat_tzc380.c
+else
+ifeq ($(TZC_ID), NONE)
+    $(info -> No TZC present on platform)
+else
+    $(error -> TZC type not set!)
+endif
+endif
+endif
+
+ifeq (${BL_COMM_TZASC_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${TZASC_SOURCES}
+else
+ifeq (${BL2_TZASC_NEEDED},yes)
+BL2_SOURCES		+= ${TZASC_SOURCES}
+endif
+ifeq (${BL31_TZASC_NEEDED},yes)
+BL31_SOURCES		+= ${TZASC_SOURCES}
+endif
+endif
+
+endif
diff --git a/drivers/partition/gpt.c b/drivers/partition/gpt.c
new file mode 100644
index 0000000..4fe8322
--- /dev/null
+++ b/drivers/partition/gpt.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/partition/efi.h>
+#include <drivers/partition/gpt.h>
+#include <lib/utils.h>
+
+static int unicode_to_ascii(unsigned short *str_in, unsigned char *str_out)
+{
+	uint8_t *name;
+	int i;
+
+	assert((str_in != NULL) && (str_out != NULL));
+
+	name = (uint8_t *)str_in;
+
+	assert(name[0] != '\0');
+
+	/* check whether the unicode string is valid */
+	for (i = 1; i < (EFI_NAMELEN << 1); i += 2) {
+		if (name[i] != '\0')
+			return -EINVAL;
+	}
+	/* convert the unicode string to ascii string */
+	for (i = 0; i < (EFI_NAMELEN << 1); i += 2) {
+		str_out[i >> 1] = name[i];
+		if (name[i] == '\0')
+			break;
+	}
+	return 0;
+}
+
+int parse_gpt_entry(gpt_entry_t *gpt_entry, partition_entry_t *entry)
+{
+	int result;
+
+	assert((gpt_entry != NULL) && (entry != NULL));
+
+	if ((gpt_entry->first_lba == 0) && (gpt_entry->last_lba == 0)) {
+		return -EINVAL;
+	}
+
+	zeromem(entry, sizeof(partition_entry_t));
+	result = unicode_to_ascii(gpt_entry->name, (uint8_t *)entry->name);
+	if (result != 0) {
+		return result;
+	}
+	entry->start = (uint64_t)gpt_entry->first_lba *
+		       PLAT_PARTITION_BLOCK_SIZE;
+	entry->length = (uint64_t)(gpt_entry->last_lba -
+				   gpt_entry->first_lba + 1) *
+			PLAT_PARTITION_BLOCK_SIZE;
+	guidcpy(&entry->part_guid, &gpt_entry->unique_uuid);
+	guidcpy(&entry->type_guid, &gpt_entry->type_uuid);
+
+	return 0;
+}
diff --git a/drivers/partition/partition.c b/drivers/partition/partition.c
new file mode 100644
index 0000000..1881c91
--- /dev/null
+++ b/drivers/partition/partition.c
@@ -0,0 +1,303 @@
+/*
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <inttypes.h>
+#include <stdio.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <common/tf_crc32.h>
+#include <drivers/io/io_storage.h>
+#include <drivers/partition/efi.h>
+#include <drivers/partition/partition.h>
+#include <drivers/partition/gpt.h>
+#include <drivers/partition/mbr.h>
+#include <plat/common/platform.h>
+
+static uint8_t mbr_sector[PLAT_PARTITION_BLOCK_SIZE];
+static partition_entry_list_t list;
+
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+static void dump_entries(int num)
+{
+	char name[EFI_NAMELEN];
+	int i, j, len;
+
+	VERBOSE("Partition table with %d entries:\n", num);
+	for (i = 0; i < num; i++) {
+		len = snprintf(name, EFI_NAMELEN, "%s", list.list[i].name);
+		for (j = 0; j < EFI_NAMELEN - len - 1; j++) {
+			name[len + j] = ' ';
+		}
+		name[EFI_NAMELEN - 1] = '\0';
+		VERBOSE("%d: %s %" PRIx64 "-%" PRIx64 "\n", i + 1, name, list.list[i].start,
+			list.list[i].start + list.list[i].length - 4);
+	}
+}
+#else
+#define dump_entries(num)	((void)num)
+#endif
+
+/*
+ * Load the first sector that carries MBR header.
+ * The MBR boot signature should be always valid whether it's MBR or GPT.
+ */
+static int load_mbr_header(uintptr_t image_handle, mbr_entry_t *mbr_entry)
+{
+	size_t bytes_read;
+	uintptr_t offset;
+	int result;
+
+	assert(mbr_entry != NULL);
+	/* MBR partition table is in LBA0. */
+	result = io_seek(image_handle, IO_SEEK_SET, MBR_OFFSET);
+	if (result != 0) {
+		WARN("Failed to seek (%i)\n", result);
+		return result;
+	}
+	result = io_read(image_handle, (uintptr_t)&mbr_sector,
+			 PLAT_PARTITION_BLOCK_SIZE, &bytes_read);
+	if (result != 0) {
+		WARN("Failed to read data (%i)\n", result);
+		return result;
+	}
+
+	/* Check MBR boot signature. */
+	if ((mbr_sector[LEGACY_PARTITION_BLOCK_SIZE - 2] != MBR_SIGNATURE_FIRST) ||
+	    (mbr_sector[LEGACY_PARTITION_BLOCK_SIZE - 1] != MBR_SIGNATURE_SECOND)) {
+		return -ENOENT;
+	}
+	offset = (uintptr_t)&mbr_sector + MBR_PRIMARY_ENTRY_OFFSET;
+	memcpy(mbr_entry, (void *)offset, sizeof(mbr_entry_t));
+	return 0;
+}
+
+/*
+ * Load GPT header and check the GPT signature and header CRC.
+ * If partition numbers could be found, check & update it.
+ */
+static int load_gpt_header(uintptr_t image_handle)
+{
+	gpt_header_t header;
+	size_t bytes_read;
+	int result;
+	uint32_t header_crc, calc_crc;
+
+	result = io_seek(image_handle, IO_SEEK_SET, GPT_HEADER_OFFSET);
+	if (result != 0) {
+		return result;
+	}
+	result = io_read(image_handle, (uintptr_t)&header,
+			 sizeof(gpt_header_t), &bytes_read);
+	if ((result != 0) || (sizeof(gpt_header_t) != bytes_read)) {
+		return result;
+	}
+	if (memcmp(header.signature, GPT_SIGNATURE,
+		   sizeof(header.signature)) != 0) {
+		return -EINVAL;
+	}
+
+	/*
+	 * UEFI Spec 2.8 March 2019 Page 119: HeaderCRC32 value is
+	 * computed by setting this field to 0, and computing the
+	 * 32-bit CRC for HeaderSize bytes.
+	 */
+	header_crc = header.header_crc;
+	header.header_crc = 0U;
+
+	calc_crc = tf_crc32(0U, (uint8_t *)&header, DEFAULT_GPT_HEADER_SIZE);
+	if (header_crc != calc_crc) {
+		ERROR("Invalid GPT Header CRC: Expected 0x%x but got 0x%x.\n",
+		      header_crc, calc_crc);
+		return -EINVAL;
+	}
+
+	header.header_crc = header_crc;
+
+	/* partition numbers can't exceed PLAT_PARTITION_MAX_ENTRIES */
+	list.entry_count = header.list_num;
+	if (list.entry_count > PLAT_PARTITION_MAX_ENTRIES) {
+		list.entry_count = PLAT_PARTITION_MAX_ENTRIES;
+	}
+	return 0;
+}
+
+static int load_mbr_entry(uintptr_t image_handle, mbr_entry_t *mbr_entry,
+			int part_number)
+{
+	size_t bytes_read;
+	uintptr_t offset;
+	int result;
+
+	assert(mbr_entry != NULL);
+	/* MBR partition table is in LBA0. */
+	result = io_seek(image_handle, IO_SEEK_SET, MBR_OFFSET);
+	if (result != 0) {
+		WARN("Failed to seek (%i)\n", result);
+		return result;
+	}
+	result = io_read(image_handle, (uintptr_t)&mbr_sector,
+			 PLAT_PARTITION_BLOCK_SIZE, &bytes_read);
+	if (result != 0) {
+		WARN("Failed to read data (%i)\n", result);
+		return result;
+	}
+
+	/* Check MBR boot signature. */
+	if ((mbr_sector[LEGACY_PARTITION_BLOCK_SIZE - 2] != MBR_SIGNATURE_FIRST) ||
+	    (mbr_sector[LEGACY_PARTITION_BLOCK_SIZE - 1] != MBR_SIGNATURE_SECOND)) {
+		return -ENOENT;
+	}
+	offset = (uintptr_t)&mbr_sector +
+		MBR_PRIMARY_ENTRY_OFFSET +
+		MBR_PRIMARY_ENTRY_SIZE * part_number;
+	memcpy(mbr_entry, (void *)offset, sizeof(mbr_entry_t));
+
+	return 0;
+}
+
+static int load_mbr_entries(uintptr_t image_handle)
+{
+	mbr_entry_t mbr_entry;
+	int i;
+
+	list.entry_count = MBR_PRIMARY_ENTRY_NUMBER;
+
+	for (i = 0; i < list.entry_count; i++) {
+		load_mbr_entry(image_handle, &mbr_entry, i);
+		list.list[i].start = mbr_entry.first_lba * 512;
+		list.list[i].length = mbr_entry.sector_nums * 512;
+		list.list[i].name[0] = mbr_entry.type;
+	}
+
+	return 0;
+}
+
+static int load_gpt_entry(uintptr_t image_handle, gpt_entry_t *entry)
+{
+	size_t bytes_read;
+	int result;
+
+	assert(entry != NULL);
+	result = io_read(image_handle, (uintptr_t)entry, sizeof(gpt_entry_t),
+			 &bytes_read);
+	if (sizeof(gpt_entry_t) != bytes_read)
+		return -EINVAL;
+	return result;
+}
+
+static int verify_partition_gpt(uintptr_t image_handle)
+{
+	gpt_entry_t entry;
+	int result, i;
+
+	for (i = 0; i < list.entry_count; i++) {
+		result = load_gpt_entry(image_handle, &entry);
+		assert(result == 0);
+		result = parse_gpt_entry(&entry, &list.list[i]);
+		if (result != 0) {
+			break;
+		}
+	}
+	if (i == 0) {
+		return -EINVAL;
+	}
+	/*
+	 * Only records the valid partition number that is loaded from
+	 * partition table.
+	 */
+	list.entry_count = i;
+	dump_entries(list.entry_count);
+
+	return 0;
+}
+
+int load_partition_table(unsigned int image_id)
+{
+	uintptr_t dev_handle, image_handle, image_spec = 0;
+	mbr_entry_t mbr_entry;
+	int result;
+
+	result = plat_get_image_source(image_id, &dev_handle, &image_spec);
+	if (result != 0) {
+		WARN("Failed to obtain reference to image id=%u (%i)\n",
+			image_id, result);
+		return result;
+	}
+
+	result = io_open(dev_handle, image_spec, &image_handle);
+	if (result != 0) {
+		WARN("Failed to access image id=%u (%i)\n", image_id, result);
+		return result;
+	}
+
+	result = load_mbr_header(image_handle, &mbr_entry);
+	if (result != 0) {
+		WARN("Failed to access image id=%u (%i)\n", image_id, result);
+		return result;
+	}
+	if (mbr_entry.type == PARTITION_TYPE_GPT) {
+		result = load_gpt_header(image_handle);
+		assert(result == 0);
+		result = io_seek(image_handle, IO_SEEK_SET, GPT_ENTRY_OFFSET);
+		assert(result == 0);
+		result = verify_partition_gpt(image_handle);
+	} else {
+		result = load_mbr_entries(image_handle);
+	}
+
+	io_close(image_handle);
+	return result;
+}
+
+const partition_entry_t *get_partition_entry(const char *name)
+{
+	int i;
+
+	for (i = 0; i < list.entry_count; i++) {
+		if (strcmp(name, list.list[i].name) == 0) {
+			return &list.list[i];
+		}
+	}
+	return NULL;
+}
+
+const partition_entry_t *get_partition_entry_by_type(const uuid_t *type_uuid)
+{
+	int i;
+
+	for (i = 0; i < list.entry_count; i++) {
+		if (guidcmp(type_uuid, &list.list[i].type_guid) == 0) {
+			return &list.list[i];
+		}
+	}
+
+	return NULL;
+}
+
+const partition_entry_t *get_partition_entry_by_uuid(const uuid_t *part_uuid)
+{
+	int i;
+
+	for (i = 0; i < list.entry_count; i++) {
+		if (guidcmp(part_uuid, &list.list[i].part_guid) == 0) {
+			return &list.list[i];
+		}
+	}
+
+	return NULL;
+}
+
+const partition_entry_list_t *get_partition_entry_list(void)
+{
+	return &list;
+}
+
+void partition_init(unsigned int image_id)
+{
+	load_partition_table(image_id);
+}
diff --git a/drivers/rambus/trng_ip_76.c b/drivers/rambus/trng_ip_76.c
new file mode 100644
index 0000000..8de12e9
--- /dev/null
+++ b/drivers/rambus/trng_ip_76.c
@@ -0,0 +1,249 @@
+/*
+ * Copyright (c) 2020, Marvell Technology Group Ltd. All rights reserved.
+ *
+ * Based on Linux kernel omap-rng.c - RNG driver for TI OMAP CPU family
+ *
+ * Author: Deepak Saxena <dsaxena@plexity.net>
+ *
+ * Copyright 2005 (c) MontaVista Software, Inc.
+ *
+ * Mostly based on original driver:
+ *
+ * Copyright (C) 2005 Nokia Corporation
+ * Author: Juha Yrjölä <juha.yrjola@nokia.com>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/rambus/trng_ip_76.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+#include <lib/utils.h>
+
+#define RNG_REG_STATUS_RDY			(1 << 0)
+
+#define RNG_REG_INTACK_RDY_MASK			(1 << 0)
+
+#define RNG_CONTROL_ENABLE_TRNG_MASK		(1 << 10)
+
+#define RNG_CONFIG_NOISE_BLOCKS(val)		((0xff & (val)) << 0)
+#define RNG_CONFIG_NOISE_BLK_VAL		0x5
+
+#define RNG_CONFIG_SAMPLE_CYCLES(val)		((0xff & (val)) << 16)
+#define RNG_CONFIG_SAMPLE_CYCLES_VAL		0x22
+
+#define RNG_REG_FRO_ENABLE_MASK			0xffffff
+#define RNG_REG_FRO_DETUNE_MASK			0x0
+
+#define EIP76_RNG_OUTPUT_SIZE			0x10
+#define EIP76_RNG_WAIT_ROUNDS			10
+
+#define RNG_HW_IS_EIP76(ver)			((ver) & (0xff == 0x4C))
+#define RNG_HW_VER_MAJOR(ver)			(((ver) & (0xf << 24)) >> 24)
+#define RNG_HW_VER_MINOR(ver)			(((ver) & (0xf << 20)) >> 20)
+#define RNG_HW_VER_PATCH(ver)			(((ver) & (0xf << 16)) >> 16)
+
+
+enum {
+	RNG_OUTPUT_0_REG = 0,
+	RNG_OUTPUT_1_REG,
+	RNG_OUTPUT_2_REG,
+	RNG_OUTPUT_3_REG,
+	RNG_STATUS_REG,
+	RNG_INTMASK_REG,
+	RNG_INTACK_REG,
+	RNG_CONTROL_REG,
+	RNG_CONFIG_REG,
+	RNG_ALARMCNT_REG,
+	RNG_FROENABLE_REG,
+	RNG_FRODETUNE_REG,
+	RNG_ALARMMASK_REG,
+	RNG_ALARMSTOP_REG,
+	RNG_REV_REG
+};
+
+static uint16_t reg_map_eip76[] = {
+	[RNG_OUTPUT_0_REG]	= 0x0,
+	[RNG_OUTPUT_1_REG]	= 0x4,
+	[RNG_OUTPUT_2_REG]	= 0x8,
+	[RNG_OUTPUT_3_REG]	= 0xc,
+	[RNG_STATUS_REG]	= 0x10,
+	[RNG_INTACK_REG]	= 0x10,
+	[RNG_CONTROL_REG]	= 0x14,
+	[RNG_CONFIG_REG]	= 0x18,
+	[RNG_ALARMCNT_REG]	= 0x1c,
+	[RNG_FROENABLE_REG]	= 0x20,
+	[RNG_FRODETUNE_REG]	= 0x24,
+	[RNG_ALARMMASK_REG]	= 0x28,
+	[RNG_ALARMSTOP_REG]	= 0x2c,
+	[RNG_REV_REG]		= 0x7c,
+};
+
+struct eip76_rng_dev {
+	uintptr_t	base;
+	uint16_t	*regs;
+};
+
+/* Locals */
+static struct eip76_rng_dev eip76_dev;
+static spinlock_t rng_lock;
+
+static inline uint32_t eip76_rng_read(struct eip76_rng_dev *dev, uint16_t reg)
+{
+	return mmio_read_32(dev->base + dev->regs[reg]);
+}
+
+static inline void eip76_rng_write(struct eip76_rng_dev *dev,
+				   uint16_t reg, uint32_t val)
+{
+	mmio_write_32(dev->base + dev->regs[reg], val);
+}
+
+static void eip76_rng_init(struct eip76_rng_dev *dev)
+{
+	uint32_t val;
+
+	/* Return if RNG is already running. */
+	if (eip76_rng_read(dev, RNG_CONTROL_REG) &
+			   RNG_CONTROL_ENABLE_TRNG_MASK) {
+		return;
+	}
+
+	/*  This field sets the number of 512-bit blocks of raw Noise Source
+	 * output data that must be processed by either the Conditioning
+	 * Function or the SP 800-90 DRBG ‘BC_DF’ functionality to yield
+	 * a ‘full entropy’ output value. As according to [SP 800-90B draft]
+	 * the amount of entropy input to this functionality must be twice
+	 * the amount that is output and the 8-bit samples output by the Noise
+	 * Source are supposed to have one bit of entropy each, the settings
+	 * for this field are as follows:
+	 * - SHA-1 Conditioning Function:
+	 *  generates 160 bits output, requiring 2560 sample bits,
+	 *  equivalent to 5 blocks of raw Noise Source input.
+	 * - SHA-256 Conditioning Function:
+	 *  generates 256 bits output, requiring 4096 sample bits, equivalent
+	 *  to 8 blocks of raw Noise Source input. Note that two blocks of 256
+	 *  bits are needed to start or re-seed the SP 800-90 DRBG
+	 *  (in the EIP-76d-*-SHA2 configurations)
+	 * - SP 800-90 DRBG ‘BC_DF’ functionality:
+	 *  generates 384 bits output, requiring 6144 sample bits, equivalent
+	 *  to 12 blocks of raw Noise Source input.
+	 *  This field can only be modified when ‘enable_trng’ in TRNG_CONTROL
+	 *  is ‘0’ or when either of the ‘test_known_noise’ or ‘test_cond_func’
+	 *  bits in TRNG_TEST is ‘1’. Value 0 in this field selects 256 blocks
+	 *  of 512 bits to be processed.
+	 */
+	val = RNG_CONFIG_NOISE_BLOCKS(RNG_CONFIG_NOISE_BLK_VAL);
+
+	/* This field sets the number of FRO samples that are XOR-ed together
+	 * into one bit to be shifted into the main shift register.
+	 * This value must be such that there is at least one bit of entropy
+	 * (in total) in each 8 bits that are shifted.
+	 * This field can only be modified when ‘enable_trng’ in TRNG_CONTROL
+	 * is ‘0’ or when either of the ‘test_known_noise’ or ‘test_cond_func’
+	 * bits in TRNG_TEST is ‘1’. Value 0 in this field selects 65536 FRO
+	 * samples to be XOR-ed together
+	 */
+	val |= RNG_CONFIG_SAMPLE_CYCLES(RNG_CONFIG_SAMPLE_CYCLES_VAL);
+	eip76_rng_write(dev, RNG_CONFIG_REG, val);
+
+	/* Enable all available FROs */
+	eip76_rng_write(dev, RNG_FRODETUNE_REG, RNG_REG_FRO_DETUNE_MASK);
+	eip76_rng_write(dev, RNG_FROENABLE_REG, RNG_REG_FRO_ENABLE_MASK);
+
+	/* Enable TRNG */
+	eip76_rng_write(dev, RNG_CONTROL_REG, RNG_CONTROL_ENABLE_TRNG_MASK);
+}
+
+int32_t eip76_rng_read_rand_buf(void *data, bool wait)
+{
+	uint32_t i, present;
+
+	if (!eip76_dev.base) /* not initialized */
+		return -1;
+
+	for (i = 0; i < EIP76_RNG_WAIT_ROUNDS; i++) {
+		present = eip76_rng_read(&eip76_dev, RNG_STATUS_REG) &
+					 RNG_REG_STATUS_RDY;
+		if (present || !wait) {
+			break;
+		}
+
+		udelay(10);
+	}
+
+	if (present != 0U) {
+		return 0;
+	}
+
+	memcpy(data,
+	       (void *)(eip76_dev.base + eip76_dev.regs[RNG_OUTPUT_0_REG]),
+	       EIP76_RNG_OUTPUT_SIZE);
+
+	eip76_rng_write(&eip76_dev, RNG_INTACK_REG, RNG_REG_INTACK_RDY_MASK);
+
+	return EIP76_RNG_OUTPUT_SIZE;
+}
+
+int32_t eip76_rng_probe(uintptr_t base_addr)
+{
+	uint32_t ver;
+
+	eip76_dev.base = base_addr;
+	eip76_dev.regs = reg_map_eip76;
+
+	eip76_rng_init(&eip76_dev);
+
+	ver = eip76_rng_read(&eip76_dev, RNG_REV_REG);
+
+	INFO("%s Random Number Generator HW ver. %01x.%01x.%01x\n",
+	     RNG_HW_IS_EIP76(ver) ? "TRNG-IP-76" : "Unknown",
+	     RNG_HW_VER_MAJOR(ver), RNG_HW_VER_MINOR(ver),
+	     RNG_HW_VER_PATCH(ver));
+
+	return 0;
+}
+
+int32_t eip76_rng_get_random(uint8_t *data, uint32_t len)
+{
+	static uint8_t rand[EIP76_RNG_OUTPUT_SIZE];
+	static uint8_t pos;
+	uint32_t i;
+	int32_t ret = 0;
+
+	if (!data)
+		return -1;
+
+	spin_lock(&rng_lock);
+
+	for (i = 0; i < len; i++) {
+		if (pos >= EIP76_RNG_OUTPUT_SIZE) {
+			pos = 0;
+		}
+
+		if (pos != 0U) {
+			ret = eip76_rng_read_rand_buf(rand, true);
+		}
+
+		/* Only advance FIFO index if it is non zero or
+		 * the update from TRNG HW was successful
+		 */
+		if (pos || ret > 0) {
+			data[i] = rand[pos++];
+			ret = 0;
+		} else {
+			ret = -1;
+			break;
+		}
+	}
+
+	spin_unlock(&rng_lock);
+
+	return ret;
+}
diff --git a/drivers/renesas/common/auth/auth_mod.c b/drivers/renesas/common/auth/auth_mod.c
new file mode 100644
index 0000000..4aa86e2
--- /dev/null
+++ b/drivers/renesas/common/auth/auth_mod.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights
+ * reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+#include "rom_api.h"
+
+typedef int32_t(*secure_boot_api_f) (uint32_t a, uint32_t b, void *c);
+extern int32_t rcar_get_certificate(const int32_t name, uint32_t *cert_addr);
+
+#define RCAR_IMAGE_ID_MAX	(10)
+#define RCAR_CERT_MAGIC_NUM	(0xE291F358U)
+#define RCAR_BOOT_KEY_CERT	(0xE6300C00U)
+#define RCAR_BOOT_KEY_CERT_NEW	(0xE6300F00U)
+#define RST_BASE		(0xE6160000U)
+#define RST_MODEMR		(RST_BASE + 0x0060U)
+#define MFISOFTMDR		(0xE6260600U)
+#define MODEMR_MD5_MASK		(0x00000020U)
+#define MODEMR_MD5_SHIFT	(5U)
+#define SOFTMD_BOOTMODE_MASK	(0x00000001U)
+#define SOFTMD_NORMALBOOT	(0x1U)
+
+static secure_boot_api_f secure_boot_api;
+
+int auth_mod_get_parent_id(unsigned int img_id, unsigned int *parent_id)
+{
+	return 1;
+}
+
+int auth_mod_verify_img(unsigned int img_id, void *ptr, unsigned int len)
+{
+	int32_t ret = 0, index = 0;
+	uint32_t cert_addr = 0U;
+	static const struct img_to_cert_t {
+		uint32_t id;
+		int32_t cert;
+		const char *name;
+	} image[RCAR_IMAGE_ID_MAX] = {
+		{ BL31_IMAGE_ID, SOC_FW_CONTENT_CERT_ID, "BL31" },
+		{ BL32_IMAGE_ID, TRUSTED_OS_FW_CONTENT_CERT_ID, "BL32" },
+		{ BL33_IMAGE_ID, NON_TRUSTED_FW_CONTENT_CERT_ID, "BL33" },
+		{ BL332_IMAGE_ID, BL332_CERT_ID, "BL332" },
+		{ BL333_IMAGE_ID, BL333_CERT_ID, "BL333" },
+		{ BL334_IMAGE_ID, BL334_CERT_ID, "BL334" },
+		{ BL335_IMAGE_ID, BL335_CERT_ID, "BL335" },
+		{ BL336_IMAGE_ID, BL336_CERT_ID, "BL336" },
+		{ BL337_IMAGE_ID, BL337_CERT_ID, "BL337" },
+		{ BL338_IMAGE_ID, BL338_CERT_ID, "BL338" },
+	};
+
+#if IMAGE_BL2
+	switch (img_id) {
+	case TRUSTED_KEY_CERT_ID:
+	case SOC_FW_KEY_CERT_ID:
+	case TRUSTED_OS_FW_KEY_CERT_ID:
+	case NON_TRUSTED_FW_KEY_CERT_ID:
+	case BL332_KEY_CERT_ID:
+	case BL333_KEY_CERT_ID:
+	case BL334_KEY_CERT_ID:
+	case BL335_KEY_CERT_ID:
+	case BL336_KEY_CERT_ID:
+	case BL337_KEY_CERT_ID:
+	case BL338_KEY_CERT_ID:
+	case SOC_FW_CONTENT_CERT_ID:
+	case TRUSTED_OS_FW_CONTENT_CERT_ID:
+	case NON_TRUSTED_FW_CONTENT_CERT_ID:
+	case BL332_CERT_ID:
+	case BL333_CERT_ID:
+	case BL334_CERT_ID:
+	case BL335_CERT_ID:
+	case BL336_CERT_ID:
+	case BL337_CERT_ID:
+	case BL338_CERT_ID:
+		return ret;
+	case BL31_IMAGE_ID:
+	case BL32_IMAGE_ID:
+	case BL33_IMAGE_ID:
+	case BL332_IMAGE_ID:
+	case BL333_IMAGE_ID:
+	case BL334_IMAGE_ID:
+	case BL335_IMAGE_ID:
+	case BL336_IMAGE_ID:
+	case BL337_IMAGE_ID:
+	case BL338_IMAGE_ID:
+		goto verify_image;
+	default:
+		return -1;
+	}
+
+verify_image:
+	for (index = 0; index < RCAR_IMAGE_ID_MAX; index++) {
+		if (img_id != image[index].id)
+			continue;
+
+		ret = rcar_get_certificate(image[index].cert, &cert_addr);
+		break;
+	}
+
+	if (ret || (index == RCAR_IMAGE_ID_MAX)) {
+		ERROR("Verification Failed for image id = %d\n", img_id);
+		return ret;
+	}
+#if RCAR_BL2_DCACHE == 1
+	/* clean and disable */
+	write_sctlr_el3(read_sctlr_el3() & ~SCTLR_C_BIT);
+	dcsw_op_all(DCCISW);
+#endif
+	ret = (mmio_read_32(RCAR_BOOT_KEY_CERT_NEW) == RCAR_CERT_MAGIC_NUM) ?
+	    secure_boot_api(RCAR_BOOT_KEY_CERT_NEW, cert_addr, NULL) :
+	    secure_boot_api(RCAR_BOOT_KEY_CERT, cert_addr, NULL);
+	if (ret)
+		ERROR("Verification Failed 0x%x, %s\n", ret, image[index].name);
+
+#if RCAR_BL2_DCACHE == 1
+	/* enable */
+	write_sctlr_el3(read_sctlr_el3() | SCTLR_C_BIT);
+#endif /* RCAR_BL2_DCACHE */
+
+#endif /* IMAGE_BL2 */
+	return ret;
+}
+
+static int32_t normal_boot_verify(uint32_t a, uint32_t b, void *c)
+{
+	return 0;
+}
+
+void auth_mod_init(void)
+{
+#if RCAR_SECURE_BOOT
+	uint32_t soft_md = mmio_read_32(MFISOFTMDR) & SOFTMD_BOOTMODE_MASK;
+	uint32_t md = mmio_read_32(RST_MODEMR) & MODEMR_MD5_MASK;
+	uint32_t lcs, ret;
+
+	secure_boot_api = (secure_boot_api_f) &rcar_rom_secure_boot_api;
+
+	ret = rcar_rom_get_lcs(&lcs);
+	if (ret) {
+		ERROR("BL2: Failed to get the LCS. (%d)\n", ret);
+		panic();
+	}
+
+	switch (lcs) {
+	case LCS_SE:
+		if (soft_md == SOFTMD_NORMALBOOT)
+			secure_boot_api = &normal_boot_verify;
+		break;
+	case LCS_SD:
+		secure_boot_api = &normal_boot_verify;
+		break;
+	default:
+		if (md >> MODEMR_MD5_SHIFT)
+			secure_boot_api = &normal_boot_verify;
+	}
+
+	NOTICE("BL2: %s boot\n",
+	       secure_boot_api == &normal_boot_verify ? "Normal" : "Secure");
+#else
+	NOTICE("BL2: Normal boot\n");
+	secure_boot_api = &normal_boot_verify;
+#endif
+}
diff --git a/drivers/renesas/common/avs/avs_driver.c b/drivers/renesas/common/avs/avs_driver.c
new file mode 100644
index 0000000..2c939cd
--- /dev/null
+++ b/drivers/renesas/common/avs/avs_driver.c
@@ -0,0 +1,630 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+
+#include "avs_driver.h"
+#include "cpg_registers.h"
+#include "rcar_def.h"
+#include "rcar_private.h"
+
+#if (AVS_SETTING_ENABLE == 1)
+#if PMIC_ROHM_BD9571
+/* Read PMIC register for debug. 1:enable / 0:disable */
+#define AVS_READ_PMIC_REG_ENABLE	0
+/* The re-try number of times of the AVS setting. */
+#define AVS_RETRY_NUM			(1U)
+#endif /* PMIC_ROHM_BD9571 */
+
+/* Base address of Adaptive Voltage Scaling module registers*/
+#define AVS_BASE			(0xE60A0000U)
+/* Adaptive Dynamic Voltage ADJust Parameter2 registers */
+#define ADVADJP2			(AVS_BASE + 0x013CU)
+
+/* Mask VOLCOND bit in ADVADJP2 registers */
+#define ADVADJP2_VOLCOND_MASK		(0x000001FFU)	/* VOLCOND[8:0] */
+
+#if PMIC_ROHM_BD9571
+/* I2C for DVFS bit in CPG registers for module standby and software reset*/
+#define CPG_SYS_DVFS_BIT		(0x04000000U)
+#endif /* PMIC_ROHM_BD9571 */
+/* ADVFS Module bit in CPG registers for module standby and software reset*/
+#define CPG_SYS_ADVFS_BIT		(0x02000000U)
+
+#if PMIC_ROHM_BD9571
+/* Base address of IICDVFS registers*/
+#define IIC_DVFS_BASE			(0xE60B0000U)
+/* IIC bus data register */
+#define IIC_ICDR			(IIC_DVFS_BASE + 0x0000U)
+/* IIC bus control register */
+#define IIC_ICCR			(IIC_DVFS_BASE + 0x0004U)
+/* IIC bus status register */
+#define IIC_ICSR			(IIC_DVFS_BASE + 0x0008U)
+/* IIC interrupt control register */
+#define IIC_ICIC			(IIC_DVFS_BASE + 0x000CU)
+/* IIC clock control register low */
+#define IIC_ICCL			(IIC_DVFS_BASE + 0x0010U)
+/* IIC clock control register high */
+#define IIC_ICCH			(IIC_DVFS_BASE + 0x0014U)
+
+/* Bit in ICSR register */
+#define ICSR_BUSY			(0x10U)
+#define ICSR_AL				(0x08U)
+#define ICSR_TACK			(0x04U)
+#define ICSR_WAIT			(0x02U)
+#define ICSR_DTE			(0x01U)
+
+/* Bit in ICIC register */
+#define ICIC_TACKE			(0x04U)
+#define ICIC_WAITE			(0x02U)
+#define ICIC_DTEE			(0x01U)
+
+/* I2C bus interface enable */
+#define ICCR_ENABLE			(0x80U)
+/* Start condition */
+#define ICCR_START			(0x94U)
+/* Stop condition */
+#define ICCR_STOP			(0x90U)
+/* Restart condition with change to receive mode change */
+#define ICCR_START_RECV			(0x81U)
+/* Stop condition for receive mode */
+#define ICCR_STOP_RECV			(0xC0U)
+
+/* Low-level period of SCL */
+#define ICCL_FREQ_8p33M			(0x07U)	/* for CP Phy 8.3333MHz */
+#define ICCL_FREQ_10M			(0x09U)	/* for CP Phy 10MHz */
+#define ICCL_FREQ_12p5M			(0x0BU)	/* for CP Phy 12.5MHz */
+#define ICCL_FREQ_16p66M		(0x0EU)	/* for CP Phy 16.6666MHz */
+/* High-level period of SCL */
+#define ICCH_FREQ_8p33M			(0x01U)	/* for CP Phy 8.3333MHz */
+#define ICCH_FREQ_10M			(0x02U)	/* for CP Phy 10MHz */
+#define ICCH_FREQ_12p5M			(0x03U)	/* for CP Phy 12.5MHz */
+#define ICCH_FREQ_16p66M		(0x05U)	/* for CP Phy 16.6666MHz */
+
+/* PMIC */
+/* ROHM BD9571 slave address + (W) */
+#define PMIC_W_SLAVE_ADDRESS		(0x60U)
+/* ROHM BD9571 slave address + (R) */
+#define PMIC_R_SLAVE_ADDRESS		(0x61U)
+/* ROHM BD9571 DVFS SetVID register */
+#define PMIC_DVFS_SETVID		(0x54U)
+#endif /* PMIC_ROHM_BD9571  */
+
+/* Individual information */
+#define EFUSE_AVS0			(0U)
+#define EFUSE_AVS_NUM			ARRAY_SIZE(init_vol_tbl)
+
+typedef struct {
+	uint32_t avs;		/* AVS code */
+	uint8_t vol;		/* Voltage */
+} initial_voltage_t;
+
+static const initial_voltage_t init_vol_tbl[] = {
+	/* AVS code, ROHM BD9571 DVFS SetVID register */
+	{0x00U, 0x53U},		/* AVS0, 0.83V */
+	{0x01U, 0x52U},		/* AVS1, 0.82V */
+	{0x02U, 0x51U},		/* AVS2, 0.81V */
+	{0x04U, 0x50U},		/* AVS3, 0.80V */
+	{0x08U, 0x4FU},		/* AVS4, 0.79V */
+	{0x10U, 0x4EU},		/* AVS5, 0.78V */
+	{0x20U, 0x4DU},		/* AVS6, 0.77V */
+	{0x40U, 0x4CU}		/* AVS7, 0.76V */
+};
+
+#if PMIC_ROHM_BD9571
+/* Kind of AVS settings status */
+typedef enum {
+	avs_status_none = 0,
+	avs_status_init,
+	avs_status_start_condition,
+	avs_status_set_slave_addr,
+	avs_status_write_reg_addr,
+	avs_status_write_reg_data,
+	avs_status_stop_condition,
+	avs_status_end,
+	avs_status_complete,
+	avs_status_al_start,
+	avs_status_al_transfer,
+	avs_status_nack,
+	avs_status_error_stop,
+	ave_status_error_end
+} avs_status_t;
+
+/* Kind of AVS error */
+typedef enum {
+	avs_error_none = 0,
+	avs_error_al,
+	avs_error_nack
+} avs_error_t;
+
+static avs_status_t avs_status;
+static uint32_t avs_retry;
+#endif /* PMIC_ROHM_BD9571  */
+static uint32_t efuse_avs = EFUSE_AVS0;
+
+#if PMIC_ROHM_BD9571
+/* prototype */
+static avs_error_t avs_check_error(void);
+static void avs_set_iic_clock(void);
+#if AVS_READ_PMIC_REG_ENABLE == 1
+static uint8_t avs_read_pmic_reg(uint8_t addr);
+static void avs_poll(uint8_t bit_pos, uint8_t val);
+#endif
+#endif /* PMIC_ROHM_BD9571 */
+#endif /* (AVS_SETTING_ENABLE==1) */
+
+/*
+ * Initialize to enable the AVS setting.
+ */
+void rcar_avs_init(void)
+{
+#if (AVS_SETTING_ENABLE == 1)
+	uint32_t val;
+
+#if PMIC_ROHM_BD9571
+	/* Initialize AVS status */
+	avs_status = avs_status_init;
+#endif /* PMIC_ROHM_BD9571 */
+
+	/* Enable clock supply to ADVFS. */
+	mstpcr_write(CPG_SMSTPCR9, CPG_MSTPSR9, CPG_SYS_ADVFS_BIT);
+
+	/* Read AVS code (Initial values are derived from eFuse) */
+	val = mmio_read_32(ADVADJP2) & ADVADJP2_VOLCOND_MASK;
+
+	for (efuse_avs = 0U; efuse_avs < EFUSE_AVS_NUM; efuse_avs++) {
+		if (val == init_vol_tbl[efuse_avs].avs)
+			break;
+	}
+
+	if (efuse_avs >= EFUSE_AVS_NUM)
+		efuse_avs = EFUSE_AVS0;	/* Not applicable */
+#if PMIC_ROHM_BD9571
+	/* Enable clock supply to DVFS. */
+	mstpcr_write(CPG_SMSTPCR9, CPG_MSTPSR9, CPG_SYS_DVFS_BIT);
+
+	/* Disable I2C module and All internal registers initialized. */
+	mmio_write_8(IIC_ICCR, 0x00U);
+	while ((mmio_read_8(IIC_ICCR) & ICCR_ENABLE) != 0U) {
+		/* Disable I2C module and all internal registers initialized. */
+		mmio_write_8(IIC_ICCR, 0x00U);
+	}
+
+	/* Set next status */
+	avs_status = avs_status_start_condition;
+
+#endif /* PMIC_ROHM_BD9571 */
+#endif /* (AVS_SETTING_ENABLE==1) */
+}
+
+/*
+ * Set the value of register corresponding to the voltage
+ * by transfer of I2C to PIMC.
+ */
+void rcar_avs_setting(void)
+{
+#if (AVS_SETTING_ENABLE == 1)
+#if PMIC_ROHM_BD9571
+	avs_error_t err;
+
+	switch (avs_status) {
+	case avs_status_start_condition:
+		/* Set ICCR.ICE=1 to activate the I2C module. */
+		mmio_write_8(IIC_ICCR, mmio_read_8(IIC_ICCR) | ICCR_ENABLE);
+		/* Set frequency of 400kHz */
+		avs_set_iic_clock();
+		/* Set ICIC.TACKE=1, ICIC.WAITE=1, ICIC.DTEE=1 to */
+		/* enable interrupt control.                      */
+		mmio_write_8(IIC_ICIC, mmio_read_8(IIC_ICIC)
+			     | ICIC_TACKE | ICIC_WAITE | ICIC_DTEE);
+		/* Write H'94 in ICCR to issue start condition */
+		mmio_write_8(IIC_ICCR, ICCR_START);
+		/* Set next status */
+		avs_status = avs_status_set_slave_addr;
+		break;
+	case avs_status_set_slave_addr:
+		/* Check error. */
+		err = avs_check_error();
+		if (err == avs_error_al) {
+			/* Recovery sequence of just after start. */
+			avs_status = avs_status_al_start;
+		} else if (err == avs_error_nack) {
+			/* Recovery sequence of detected NACK */
+			avs_status = avs_status_nack;
+		} else {
+			/* Was data transmission enabled ? */
+			if ((mmio_read_8(IIC_ICSR) & ICSR_DTE) == ICSR_DTE) {
+				/* Clear ICIC.DTEE to disable a DTE interrupt */
+				mmio_write_8(IIC_ICIC, mmio_read_8(IIC_ICIC)
+					     & (uint8_t) (~ICIC_DTEE));
+				/* Send PMIC slave address + (W) */
+				mmio_write_8(IIC_ICDR, PMIC_W_SLAVE_ADDRESS);
+				/* Set next status */
+				avs_status = avs_status_write_reg_addr;
+			}
+		}
+		break;
+	case avs_status_write_reg_addr:
+		/* Check error. */
+		err = avs_check_error();
+		if (err == avs_error_al) {
+			/* Recovery sequence of during data transfer. */
+			avs_status = avs_status_al_transfer;
+		} else if (err == avs_error_nack) {
+			/* Recovery sequence of detected NACK */
+			avs_status = avs_status_nack;
+		} else {
+			/* If wait state after data transmission. */
+			if ((mmio_read_8(IIC_ICSR) & ICSR_WAIT) == ICSR_WAIT) {
+				/* Write PMIC DVFS_SetVID address */
+				mmio_write_8(IIC_ICDR, PMIC_DVFS_SETVID);
+				/* Clear ICSR.WAIT to exit from wait state. */
+				mmio_write_8(IIC_ICSR, mmio_read_8(IIC_ICSR)
+					     & (uint8_t) (~ICSR_WAIT));
+				/* Set next status */
+				avs_status = avs_status_write_reg_data;
+			}
+		}
+		break;
+	case avs_status_write_reg_data:
+		/* Check error. */
+		err = avs_check_error();
+		if (err == avs_error_al) {
+			/* Recovery sequence of during data transfer. */
+			avs_status = avs_status_al_transfer;
+		} else if (err == avs_error_nack) {
+			/* Recovery sequence of detected NACK */
+			avs_status = avs_status_nack;
+		} else {
+			/* If wait state after data transmission. */
+			if ((mmio_read_8(IIC_ICSR) & ICSR_WAIT) == ICSR_WAIT) {
+				/* Dose efuse_avs exceed the number of */
+				/* the tables? */
+				if (efuse_avs >= EFUSE_AVS_NUM) {
+					ERROR("%s%s=%u\n", "AVS number of ",
+					      "eFuse is out of range. number",
+					      efuse_avs);
+					/* Infinite loop */
+					panic();
+				}
+				/* Write PMIC DVFS_SetVID value */
+				mmio_write_8(IIC_ICDR,
+					     init_vol_tbl[efuse_avs].vol);
+				/* Clear ICSR.WAIT to exit from wait state. */
+				mmio_write_8(IIC_ICSR, mmio_read_8(IIC_ICSR)
+					     & (uint8_t) (~ICSR_WAIT));
+				/* Set next status */
+				avs_status = avs_status_stop_condition;
+			}
+		}
+		break;
+	case avs_status_stop_condition:
+		err = avs_check_error();
+		if (err == avs_error_al) {
+			/* Recovery sequence of during data transfer. */
+			avs_status = avs_status_al_transfer;
+		} else if (err == avs_error_nack) {
+			/* Recovery sequence of detected NACK */
+			avs_status = avs_status_nack;
+		} else {
+			/* If wait state after data transmission. */
+			if ((mmio_read_8(IIC_ICSR) & ICSR_WAIT) == ICSR_WAIT) {
+				/* Write H'90 in ICCR to issue stop condition */
+				mmio_write_8(IIC_ICCR, ICCR_STOP);
+				/* Clear ICSR.WAIT to exit from wait state. */
+				mmio_write_8(IIC_ICSR, mmio_read_8(IIC_ICSR)
+					     & (uint8_t) (~ICSR_WAIT));
+				/* Set next status */
+				avs_status = avs_status_end;
+			}
+		}
+		break;
+	case avs_status_end:
+		/* Is this module not busy?. */
+		if ((mmio_read_8(IIC_ICSR) & ICSR_BUSY) == 0U) {
+			/* Set ICCR=H'00 to disable the I2C module. */
+			mmio_write_8(IIC_ICCR, 0x00U);
+			/* Set next status */
+			avs_status = avs_status_complete;
+		}
+		break;
+	case avs_status_al_start:
+		/* Clear ICSR.AL bit */
+		mmio_write_8(IIC_ICSR, (mmio_read_8(IIC_ICSR)
+					& (uint8_t) (~ICSR_AL)));
+		/* Transmit a clock pulse */
+		mmio_write_8(IIC_ICDR, init_vol_tbl[EFUSE_AVS0].vol);
+		/* Set next status */
+		avs_status = avs_status_error_stop;
+		break;
+	case avs_status_al_transfer:
+		/* Clear ICSR.AL bit */
+		mmio_write_8(IIC_ICSR, (mmio_read_8(IIC_ICSR)
+					& (uint8_t) (~ICSR_AL)));
+		/* Set next status */
+		avs_status = avs_status_error_stop;
+		break;
+	case avs_status_nack:
+		/* Write H'90 in ICCR to issue stop condition */
+		mmio_write_8(IIC_ICCR, ICCR_STOP);
+		/* Disable a WAIT and DTEE interrupt. */
+		mmio_write_8(IIC_ICIC, mmio_read_8(IIC_ICIC)
+			     & (uint8_t) (~(ICIC_WAITE | ICIC_DTEE)));
+		/* Clear ICSR.TACK bit */
+		mmio_write_8(IIC_ICSR, mmio_read_8(IIC_ICSR)
+			     & (uint8_t) (~ICSR_TACK));
+		/* Set next status */
+		avs_status = ave_status_error_end;
+		break;
+	case avs_status_error_stop:
+		/* If wait state after data transmission. */
+		if ((mmio_read_8(IIC_ICSR) & ICSR_WAIT) == ICSR_WAIT) {
+			/* Write H'90 in ICCR to issue stop condition */
+			mmio_write_8(IIC_ICCR, ICCR_STOP);
+			/* Clear ICSR.WAIT to exit from wait state. */
+			mmio_write_8(IIC_ICSR, mmio_read_8(IIC_ICSR)
+				     & (uint8_t) (~ICSR_WAIT));
+			/* Set next status */
+			avs_status = ave_status_error_end;
+		}
+		break;
+	case ave_status_error_end:
+		/* Is this module not busy?. */
+		if ((mmio_read_8(IIC_ICSR) & ICSR_BUSY) == 0U) {
+			/* Set ICCR=H'00 to disable the I2C module. */
+			mmio_write_8(IIC_ICCR, 0x00U);
+			/* Increment the re-try number of times. */
+			avs_retry++;
+			/* Set start a re-try to status. */
+			avs_status = avs_status_start_condition;
+		}
+		break;
+	case avs_status_complete:
+		/* After "avs_status" became the "avs_status_complete", */
+		/* "avs_setting()" function may be called. */
+		break;
+	default:
+		/* This case is not possible. */
+		ERROR("AVS setting is in invalid status. status=%u\n",
+		      avs_status);
+		/* Infinite loop */
+		panic();
+		break;
+	}
+#endif /* PMIC_ROHM_BD9571 */
+#endif /* (AVS_SETTING_ENABLE==1) */
+}
+
+/*
+ * Finish the AVS setting.
+ */
+void rcar_avs_end(void)
+{
+#if (AVS_SETTING_ENABLE == 1)
+	uint32_t mstp;
+
+#if PMIC_ROHM_BD9571
+	/* While status is not completion, be repeated. */
+	while (avs_status != avs_status_complete)
+		rcar_avs_setting();
+
+	NOTICE("AVS setting succeeded. DVFS_SetVID=0x%x\n",
+	       init_vol_tbl[efuse_avs].vol);
+
+#if AVS_READ_PMIC_REG_ENABLE == 1
+	{
+		uint8_t addr = PMIC_DVFS_SETVID;
+		uint8_t value = avs_read_pmic_reg(addr);
+
+		NOTICE("Read PMIC register. address=0x%x value=0x%x\n",
+		       addr, value);
+	}
+#endif
+
+	/* Bit of the module which wants to disable clock supply. */
+	mstp = CPG_SYS_DVFS_BIT;
+	/* Disables the supply of clock signal to a module. */
+	cpg_write(CPG_SMSTPCR9, mmio_read_32(CPG_SMSTPCR9) | mstp);
+#endif /* PMIC_ROHM_BD9571 */
+
+	/* Bit of the module which wants to disable clock supply. */
+	mstp = CPG_SYS_ADVFS_BIT;
+	/* Disables the supply of clock signal to a module. */
+	cpg_write(CPG_SMSTPCR9, mmio_read_32(CPG_SMSTPCR9) | mstp);
+
+#endif /* (AVS_SETTING_ENABLE==1) */
+}
+
+#if (AVS_SETTING_ENABLE == 1)
+#if PMIC_ROHM_BD9571
+/*
+ * Check error and judge re-try.
+ */
+static avs_error_t avs_check_error(void)
+{
+	avs_error_t ret;
+
+	if ((mmio_read_8(IIC_ICSR) & ICSR_AL) == ICSR_AL) {
+		NOTICE("%s AVS status=%d Retry=%u\n",
+		       "Loss of arbitration is detected.", avs_status, avs_retry);
+		/* Check of retry number of times */
+		if (avs_retry >= AVS_RETRY_NUM) {
+			ERROR("AVS setting failed in retry. max=%u\n",
+			      AVS_RETRY_NUM);
+			/* Infinite loop */
+			panic();
+		}
+		/* Set the error detected to error status. */
+		ret = avs_error_al;
+	} else if ((mmio_read_8(IIC_ICSR) & ICSR_TACK) == ICSR_TACK) {
+		NOTICE("%s AVS status=%d Retry=%u\n",
+		       "Non-acknowledge is detected.", avs_status, avs_retry);
+		/* Check of retry number of times */
+		if (avs_retry >= AVS_RETRY_NUM) {
+			ERROR("AVS setting failed in retry. max=%u\n",
+			      AVS_RETRY_NUM);
+			/* Infinite loop */
+			panic();
+		}
+		/* Set the error detected to error status. */
+		ret = avs_error_nack;
+	} else {
+		/* Not error. */
+		ret = avs_error_none;
+	}
+	return ret;
+}
+
+/*
+ * Set I2C for DVFS clock.
+ */
+static void avs_set_iic_clock(void)
+{
+	uint32_t md_pin;
+
+	/* Read Mode pin register. */
+	md_pin = mmio_read_32(RCAR_MODEMR) & CHECK_MD13_MD14;
+	/* Set the module clock (CP phy) for the IIC-DVFS. */
+	/* CP phy is EXTAL / 2.                            */
+	switch (md_pin) {
+	case MD14_MD13_TYPE_0:	/* EXTAL = 16.6666MHz */
+		mmio_write_8(IIC_ICCL, ICCL_FREQ_8p33M);
+		mmio_write_8(IIC_ICCH, ICCH_FREQ_8p33M);
+		break;
+	case MD14_MD13_TYPE_1:	/* EXTAL = 20MHz */
+		mmio_write_8(IIC_ICCL, ICCL_FREQ_10M);
+		mmio_write_8(IIC_ICCH, ICCH_FREQ_10M);
+		break;
+	case MD14_MD13_TYPE_2:	/* EXTAL = 25MHz (H3/M3) */
+		mmio_write_8(IIC_ICCL, ICCL_FREQ_12p5M);
+		mmio_write_8(IIC_ICCH, ICCH_FREQ_12p5M);
+		break;
+	case MD14_MD13_TYPE_3:	/* EXTAL = 33.3333MHz */
+		mmio_write_8(IIC_ICCL, ICCL_FREQ_16p66M);
+		mmio_write_8(IIC_ICCH, ICCH_FREQ_16p66M);
+		break;
+	default:		/* This case is not possible. */
+		/* CP Phy frequency is to be set for the 16.66MHz */
+		mmio_write_8(IIC_ICCL, ICCL_FREQ_16p66M);
+		mmio_write_8(IIC_ICCH, ICCH_FREQ_16p66M);
+		break;
+	}
+}
+
+#if AVS_READ_PMIC_REG_ENABLE == 1
+/*
+ * Read the value of the register of PMIC.
+ */
+static uint8_t avs_read_pmic_reg(uint8_t addr)
+{
+	uint8_t reg;
+
+	/* Set ICCR.ICE=1 to activate the I2C module. */
+	mmio_write_8(IIC_ICCR, mmio_read_8(IIC_ICCR) | ICCR_ENABLE);
+
+	/* Set frequency of 400kHz */
+	avs_set_iic_clock();
+
+	/*
+	 * Set ICIC.WAITE=1, ICIC.DTEE=1 to enable data transmission
+	 * interrupt and wait interrupt.
+	 */
+	mmio_write_8(IIC_ICIC, mmio_read_8(IIC_ICIC) | ICIC_WAITE | ICIC_DTEE);
+
+	/* Write H'94 in ICCR to issue start condition */
+	mmio_write_8(IIC_ICCR, ICCR_START);
+
+	/* Wait for a until ICSR.DTE becomes 1. */
+	avs_poll(ICSR_DTE, 1U);
+
+	/* Clear ICIC.DTEE to disable a DTE interrupt. */
+	mmio_write_8(IIC_ICIC, mmio_read_8(IIC_ICIC) & (uint8_t) (~ICIC_DTEE));
+	/* Send slave address of PMIC */
+	mmio_write_8(IIC_ICDR, PMIC_W_SLAVE_ADDRESS);
+
+	/* Wait for a until ICSR.WAIT becomes 1. */
+	avs_poll(ICSR_WAIT, 1U);
+
+	/* write PMIC address */
+	mmio_write_8(IIC_ICDR, addr);
+	/* Clear ICSR.WAIT to exit from WAIT status. */
+	mmio_write_8(IIC_ICSR, mmio_read_8(IIC_ICSR) & (uint8_t) (~ICSR_WAIT));
+
+	/* Wait for a until ICSR.WAIT becomes 1. */
+	avs_poll(ICSR_WAIT, 1U);
+
+	/* Write H'94 in ICCR to issue restart condition */
+	mmio_write_8(IIC_ICCR, ICCR_START);
+	/* Clear ICSR.WAIT to exit from WAIT status. */
+	mmio_write_8(IIC_ICSR, mmio_read_8(IIC_ICSR) & (uint8_t) (~ICSR_WAIT));
+	/* Set ICIC.DTEE=1 to enable data transmission interrupt. */
+	mmio_write_8(IIC_ICIC, mmio_read_8(IIC_ICIC) | ICIC_DTEE);
+
+	/* Wait for a until ICSR.DTE becomes 1. */
+	avs_poll(ICSR_DTE, 1U);
+
+	/* Clear ICIC.DTEE to disable a DTE interrupt. */
+	mmio_write_8(IIC_ICIC, mmio_read_8(IIC_ICIC) & (uint8_t) (~ICIC_DTEE));
+	/* Send slave address of PMIC */
+	mmio_write_8(IIC_ICDR, PMIC_R_SLAVE_ADDRESS);
+
+	/* Wait for a until ICSR.WAIT becomes 1. */
+	avs_poll(ICSR_WAIT, 1U);
+
+	/* Write H'81 to ICCR to issue the repeated START condition     */
+	/* for changing the transmission mode to the receive mode.      */
+	mmio_write_8(IIC_ICCR, ICCR_START_RECV);
+	/* Clear ICSR.WAIT to exit from WAIT status. */
+	mmio_write_8(IIC_ICSR, mmio_read_8(IIC_ICSR) & (uint8_t) (~ICSR_WAIT));
+
+	/* Wait for a until ICSR.WAIT becomes 1. */
+	avs_poll(ICSR_WAIT, 1U);
+
+	/* Set ICCR to H'C0 for the STOP condition */
+	mmio_write_8(IIC_ICCR, ICCR_STOP_RECV);
+	/* Clear ICSR.WAIT to exit from WAIT status. */
+	mmio_write_8(IIC_ICSR, mmio_read_8(IIC_ICSR) & (uint8_t) (~ICSR_WAIT));
+	/* Set ICIC.DTEE=1 to enable data transmission interrupt. */
+	mmio_write_8(IIC_ICIC, mmio_read_8(IIC_ICIC) | ICIC_DTEE);
+
+	/* Wait for a until ICSR.DTE becomes 1. */
+	avs_poll(ICSR_DTE, 1U);
+
+	/* Receive DVFS SetVID register */
+	/* Clear ICIC.DTEE to disable a DTE interrupt. */
+	mmio_write_8(IIC_ICIC, mmio_read_8(IIC_ICIC) & (uint8_t) (~ICIC_DTEE));
+	/* Receive DVFS SetVID register */
+	reg = mmio_read_8(IIC_ICDR);
+
+	/* Wait until ICSR.BUSY is cleared. */
+	avs_poll(ICSR_BUSY, 0U);
+
+	/* Set ICCR=H'00 to disable the I2C module. */
+	mmio_write_8(IIC_ICCR, 0x00U);
+
+	return reg;
+}
+
+/*
+ * Wait processing by the polling.
+ */
+static void avs_poll(uint8_t bit_pos, uint8_t val)
+{
+	uint8_t bit_val = 0U;
+
+	if (val != 0U)
+		bit_val = bit_pos;
+
+	while (1) {
+		if ((mmio_read_8(IIC_ICSR) & bit_pos) == bit_val)
+			break;
+	}
+}
+#endif /* AVS_READ_PMIC_REG_ENABLE */
+#endif /* PMIC_ROHM_BD9571 */
+#endif /* (AVS_SETTING_ENABLE==1) */
diff --git a/drivers/renesas/common/avs/avs_driver.h b/drivers/renesas/common/avs/avs_driver.h
new file mode 100644
index 0000000..aa773b6
--- /dev/null
+++ b/drivers/renesas/common/avs/avs_driver.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights
+ * reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef AVS_DRIVER_H
+#define AVS_DRIVER_H
+
+/* AVS Setting. 1:enable / 0:disable */
+#ifndef AVS_SETTING_ENABLE
+#define AVS_SETTING_ENABLE	1
+#endif /* AVS_SETTING_ENABLE */
+
+void rcar_avs_init(void);
+void rcar_avs_setting(void);
+void rcar_avs_end(void);
+
+#endif /* AVS_DRIVER_H */
diff --git a/drivers/renesas/common/common.c b/drivers/renesas/common/common.c
new file mode 100644
index 0000000..a0aa480
--- /dev/null
+++ b/drivers/renesas/common/common.c
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2018-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+
+#include "cpg_registers.h"
+#include "rcar_private.h"
+
+#if IMAGE_BL31
+void __attribute__ ((section(".system_ram"))) cpg_write(uintptr_t regadr, uint32_t regval)
+#else
+void cpg_write(uintptr_t regadr, uint32_t regval)
+#endif
+{
+	uint32_t value = regval;
+
+	mmio_write_32(CPG_CPGWPR, ~value);
+	mmio_write_32(regadr, value);
+}
+
+#if IMAGE_BL31
+void __attribute__ ((section(".system_ram"))) mstpcr_write(uint32_t mstpcr, uint32_t mstpsr,
+							   uint32_t target_bit)
+#else
+void mstpcr_write(uint32_t mstpcr, uint32_t mstpsr, uint32_t target_bit)
+#endif
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(mstpcr);
+	reg &= ~target_bit;
+	cpg_write(mstpcr, reg);
+	while ((mmio_read_32(mstpsr) & target_bit) != 0U) {
+	}
+}
diff --git a/drivers/renesas/common/console/rcar_console.S b/drivers/renesas/common/console/rcar_console.S
new file mode 100644
index 0000000..b683d7b
--- /dev/null
+++ b/drivers/renesas/common/console/rcar_console.S
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2018-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <console_macros.S>
+#include <drivers/renesas/rcar/console/console.h>
+
+	.globl	console_rcar_register
+	.globl	console_rcar_init
+	.globl	console_rcar_putc
+	.globl	console_rcar_flush
+
+	.extern	rcar_log_init
+	.extern	rcar_set_log_data
+
+	/* -----------------------------------------------
+	 * int console_rcar_register(
+	 *      uintptr_t base, uint32_t clk, uint32_t baud,
+	 *      console_t *console)
+	 * Function to initialize and register a new rcar
+	 * 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_rcar_register
+	mov	x7, x30
+	mov	x6, x3
+	cbz	x6, register_fail
+	str	x0, [x6, #CONSOLE_T_BASE]
+
+	bl	rcar_log_init
+	cbz	x0, register_fail
+
+	mov	x0, x6
+	mov	x30, x7
+	finish_console_register rcar, putc=1, getc=0, flush=1
+
+register_fail:
+	ret	x7
+endfunc console_rcar_register
+
+	/* ---------------------------------------------
+	 * int console_rcar_init(unsigned long 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 crash reporting.
+	 * In: x0 - console base address
+	 *     w1 - Uart clock in Hz
+	 *     w2 - Baud rate
+	 * Out: return 1 on success
+	 * Clobber list : x1, x2
+	 * ---------------------------------------------
+	 */
+func console_rcar_init
+	mov	w0, #1
+	ret
+endfunc console_rcar_init
+
+	/* --------------------------------------------------------
+	 * int console_rcar_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_rcar_putc
+	b	rcar_set_log_data
+endfunc console_rcar_putc
+
+	/* ---------------------------------------------
+	 * void console_rcar_flush(void)
+	 * Function to force a write of all buffered
+	 * data that hasn't been output. It returns void
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func console_rcar_flush
+	ret
+endfunc console_rcar_flush
diff --git a/drivers/renesas/common/console/rcar_printf.c b/drivers/renesas/common/console/rcar_printf.c
new file mode 100644
index 0000000..ad074fe
--- /dev/null
+++ b/drivers/renesas/common/console/rcar_printf.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdarg.h>
+#include <stdint.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/bakery_lock.h>
+
+#include "rcar_def.h"
+#include "rcar_private.h"
+#include "rcar_printf.h"
+
+#define INDEX_TIMER_COUNT	(4U)
+
+#define RCAR_LOG_HEAD	(('T' << 0) | ('L' << 8) | ('O' << 16) | ('G' << 24))
+
+/*
+ * The log is initialized and used before BL31 xlat tables are initialized,
+ * therefore the log memory is a device memory at that point. Make sure the
+ * memory is correclty aligned and accessed only with up-to 32bit, aligned,
+ * writes.
+ */
+CASSERT((RCAR_BL31_LOG_BASE & 0x7) == 0, assert_bl31_log_base_unaligned);
+CASSERT((RCAR_BL31_LOG_MAX & 0x7) == 0, assert_bl31_log_max_unaligned);
+
+extern RCAR_INSTANTIATE_LOCK typedef struct log_head {
+	uint32_t head;
+	uint32_t index;
+	uint32_t size;
+	uint32_t res;
+} loghead_t;
+
+typedef struct log_map {
+	loghead_t header;
+	uint8_t log_data[RCAR_BL31_LOG_MAX];
+	uint8_t res_data[RCAR_LOG_RES_SIZE];
+} logmap_t;
+
+int32_t rcar_set_log_data(int32_t c)
+{
+	logmap_t *t_log;
+
+	t_log = (logmap_t *) RCAR_BL31_LOG_BASE;
+
+	rcar_lock_get();
+
+	/*
+	 * If index is broken, then index and size initialize
+	 */
+	if (t_log->header.index >= (uint32_t) RCAR_BL31_LOG_MAX) {
+		t_log->header.index = 0U;
+		t_log->header.size = 0U;
+	}
+	/*
+	 * data store to log area then index and size renewal
+	 */
+	t_log->log_data[t_log->header.index] = (uint8_t) c;
+	t_log->header.index++;
+	if (t_log->header.size < t_log->header.index) {
+		t_log->header.size = t_log->header.index;
+	}
+	if (t_log->header.index >= (uint32_t) RCAR_BL31_LOG_MAX) {
+		t_log->header.index = 0U;
+	}
+
+	rcar_lock_release();
+
+	return 1;
+}
+
+int32_t rcar_log_init(void)
+{
+	logmap_t *t_log = (logmap_t *)RCAR_BL31_LOG_BASE;
+	uint32_t *log_data = (uint32_t *)t_log->log_data;
+	int16_t init_flag = 0;
+	int i;
+
+	if (t_log->header.head != RCAR_LOG_HEAD) {
+		/*
+		 * Log header is not "TLOG", then log area initialize
+		 */
+		init_flag = 1;
+	}
+	if (t_log->header.index >= (uint32_t) RCAR_BL31_LOG_MAX) {
+		/*
+		 * index is broken, then log area initialize
+		 */
+		init_flag = 1;
+	}
+	if (init_flag == 1) {
+		for (i = 0; i < RCAR_BL31_LOG_MAX; i += 4)
+			*log_data++ = 0;
+
+		t_log->header.head = RCAR_LOG_HEAD;
+		t_log->header.index = 0U;
+		t_log->header.size = 0U;
+	}
+	rcar_lock_init();
+
+	return 1;
+}
diff --git a/drivers/renesas/common/console/rcar_printf.h b/drivers/renesas/common/console/rcar_printf.h
new file mode 100644
index 0000000..5da70e6
--- /dev/null
+++ b/drivers/renesas/common/console/rcar_printf.h
@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RCAR_PRINTF_H
+#define RCAR_PRINTF_H
+
+#include <string.h>
+
+int32_t rcar_set_log_data(int32_t c);
+int32_t rcar_log_init(void);
+
+#endif /* RCAR_PRINTF_H */
diff --git a/drivers/renesas/common/ddr/boot_init_dram.h b/drivers/renesas/common/ddr/boot_init_dram.h
new file mode 100644
index 0000000..ac237b2
--- /dev/null
+++ b/drivers/renesas/common/ddr/boot_init_dram.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (c) 2018-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef BOOT_INIT_DRAM_H
+#define BOOT_INIT_DRAM_H
+
+extern int32_t rcar_dram_init(void);
+
+#define INITDRAM_OK		0
+#define INITDRAM_NG		0xffffffff
+#define INITDRAM_ERR_I		0xffffffff
+#define INITDRAM_ERR_O		0xfffffffe
+#define INITDRAM_ERR_T		0xfffffff0
+
+#endif /* BOOT_INIT_DRAM_H */
diff --git a/drivers/renesas/common/ddr/ddr.mk b/drivers/renesas/common/ddr/ddr.mk
new file mode 100644
index 0000000..9483686
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr.mk
@@ -0,0 +1,17 @@
+#
+# Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq ($(RCAR_LSI),$(filter $(RCAR_LSI),${RCAR_E3} ${RZ_G2E}))
+    include drivers/renesas/common/ddr/ddr_a/ddr_a.mk
+    BL2_SOURCES += drivers/renesas/common/ddr/dram_sub_func.c
+else ifeq (${RCAR_LSI},${RCAR_D3})
+    include drivers/renesas/common/ddr/ddr_a/ddr_a.mk
+else ifeq (${RCAR_LSI},${RCAR_V3M})
+    include drivers/renesas/common/ddr/ddr_a/ddr_a.mk
+else
+    include drivers/renesas/common/ddr/ddr_b/ddr_b.mk
+    BL2_SOURCES += drivers/renesas/common/ddr/dram_sub_func.c
+endif
diff --git a/drivers/renesas/common/ddr/ddr_a/boot_init_dram_regdef.h b/drivers/renesas/common/ddr/ddr_a/boot_init_dram_regdef.h
new file mode 100644
index 0000000..0f89b43
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_a/boot_init_dram_regdef.h
@@ -0,0 +1,8 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "../ddr_regs.h"
diff --git a/drivers/renesas/common/ddr/ddr_a/ddr_a.mk b/drivers/renesas/common/ddr/ddr_a/ddr_a.mk
new file mode 100644
index 0000000..cd6433d
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_a/ddr_a.mk
@@ -0,0 +1,13 @@
+#
+# Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq ($(RCAR_LSI),$(filter $(RCAR_LSI),${RCAR_E3} ${RZ_G2E}))
+BL2_SOURCES += drivers/renesas/common/ddr/ddr_a/ddr_init_e3.c
+else ifeq (${RCAR_LSI},${RCAR_D3})
+BL2_SOURCES += drivers/renesas/common/ddr/ddr_a/ddr_init_d3.c
+else
+BL2_SOURCES += drivers/renesas/common/ddr/ddr_a/ddr_init_v3m.c
+endif
diff --git a/drivers/renesas/common/ddr/ddr_a/ddr_init_d3.c b/drivers/renesas/common/ddr/ddr_a/ddr_init_d3.c
new file mode 100644
index 0000000..f0113f1
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_a/ddr_init_d3.c
@@ -0,0 +1,735 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation.
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <lib/mmio.h>
+#include <common/debug.h>
+#include "rcar_def.h"
+#include "../ddr_regs.h"
+
+#define RCAR_DDR_VERSION	"rev.0.02"
+
+/* Average periodic refresh interval[ns]. Support 3900,7800 */
+#define REFRESH_RATE  3900
+
+
+#if RCAR_LSI != RCAR_D3
+#error "Don't have DDR initialize routine."
+#endif
+
+static void init_ddr_d3_1866(void)
+{
+	uint32_t i, r2, r3, r5, r6, r7, r12;
+
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00001234);
+	mmio_write_32(DBSC_DBKIND, 0x00000007);
+	mmio_write_32(DBSC_DBMEMCONF_0_0, 0x0f030a01);
+	mmio_write_32(DBSC_DBPHYCONF0, 0x00000001);
+	mmio_write_32(DBSC_DBTR0, 0x0000000D);
+	mmio_write_32(DBSC_DBTR1, 0x00000009);
+	mmio_write_32(DBSC_DBTR2, 0x00000000);
+	mmio_write_32(DBSC_DBTR3, 0x0000000D);
+	mmio_write_32(DBSC_DBTR4, 0x000D000D);
+	mmio_write_32(DBSC_DBTR5, 0x0000002D);
+	mmio_write_32(DBSC_DBTR6, 0x00000020);
+	mmio_write_32(DBSC_DBTR7, 0x00060006);
+	mmio_write_32(DBSC_DBTR8, 0x00000021);
+	mmio_write_32(DBSC_DBTR9, 0x00000007);
+	mmio_write_32(DBSC_DBTR10, 0x0000000E);
+	mmio_write_32(DBSC_DBTR11, 0x0000000C);
+	mmio_write_32(DBSC_DBTR12, 0x00140014);
+	mmio_write_32(DBSC_DBTR13, 0x000000F2);
+	mmio_write_32(DBSC_DBTR14, 0x00170006);
+	mmio_write_32(DBSC_DBTR15, 0x00060005);
+	mmio_write_32(DBSC_DBTR16, 0x09210507);
+	mmio_write_32(DBSC_DBTR17, 0x040E0000);
+	mmio_write_32(DBSC_DBTR18, 0x00000200);
+	mmio_write_32(DBSC_DBTR19, 0x0129004B);
+	mmio_write_32(DBSC_DBTR20, 0x020000FB);
+	mmio_write_32(DBSC_DBTR21, 0x00040004);
+	mmio_write_32(DBSC_DBBL, 0x00000000);
+	mmio_write_32(DBSC_DBODT0, 0x00000001);
+	mmio_write_32(DBSC_DBADJ0, 0x00000001);
+	mmio_write_32(DBSC_DBSYSCONF1, 0x00000002);
+	mmio_write_32(DBSC_DBDFICNT_0, 0x00000010);
+	mmio_write_32(DBSC_DBBCAMDIS, 0x00000001);
+	mmio_write_32(DBSC_DBSCHRW1, 0x00000046);
+	mmio_write_32(DBSC_SCFCTST0, 0x0C050B03);
+	mmio_write_32(DBSC_SCFCTST1, 0x0305030C);
+
+	mmio_write_32(DBSC_DBPDLK_0, 0x0000A55A);
+	mmio_write_32(DBSC_DBCMD, 0x01000001);
+	mmio_write_32(DBSC_DBCMD, 0x08000000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x80010000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000008);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000B8000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x04058A04);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000091);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BB6B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000095);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BBAD);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000099);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BB6B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x04058A00);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000021);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0024641E);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010073);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0C058A00);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x04058A00);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000003);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0780C700);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000007);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(30)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000004);
+	mmio_write_32(DBSC_DBPDRGD_0,
+		(uint32_t) (REFRESH_RATE * 928 / 125) - 400
+			+ 0x0A300000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000022);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x1000040B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000023);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x35A00D77);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000024);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x2A8A2C28);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000025);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x30005E00);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000026);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0014CB49);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000027);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00000F14);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000028);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00000046);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000029);
+	if (REFRESH_RATE > 3900) {
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000020);
+	} else {
+		mmio_write_32(DBSC_DBPDRGD_0, 0x000000A0);
+	}
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000002C);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x81003047);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000020);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00181884);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000001A);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x33C03C10);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000000E);
+	r2 = (mmio_read_32(DBSC_DBPDRGD_0) & 0x0000FF00) >> 0x9;
+	r3 = (r2 << 16) + (r2 << 8) + r2;
+	r6 = (r2 << 24) + (r2 << 16) + (r2 << 8) + r2;
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000011);
+	mmio_write_32(DBSC_DBPDRGD_0, r3);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000012);
+	mmio_write_32(DBSC_DBPDRGD_0, r3);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000016);
+	mmio_write_32(DBSC_DBPDRGD_0, r6);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000017);
+	mmio_write_32(DBSC_DBPDRGD_0, r6);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000018);
+	mmio_write_32(DBSC_DBPDRGD_0, r6);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000019);
+	mmio_write_32(DBSC_DBPDRGD_0, r6);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010181);
+	mmio_write_32(DBSC_DBCMD, 0x08000001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010601);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	for (i = 0; i < 2; i++) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB1 + i * 0x20);
+		r5 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFF00) >> 0x8;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB4 + i * 0x20);
+		r6 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFF;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB3 + i * 0x20);
+		r7 = mmio_read_32(DBSC_DBPDRGD_0) & 0x7;
+
+		if (r6 > 0) {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r7 + 0x1) & 0x7));
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | r6);
+		} else {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | r7);
+
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 |
+						     ((r6 + (r5 << 1)) & 0xFF));
+		}
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000005);
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC1AA00C0);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010801);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000005);
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC1AA00D8);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0001F001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000AF);
+	r2 = mmio_read_32(DBSC_DBPDRGD_0);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000AF);
+	mmio_write_32(DBSC_DBPDRGD_0, ((r2 + 0x1) & 0xFF) | (r2 & 0xFFFFFF00));
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000CF);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000CF);
+	r2 = mmio_read_32(DBSC_DBPDRGD_0);
+	mmio_write_32(DBSC_DBPDRGD_0, ((r2 + 0x1) & 0xFF) | (r2 & 0xFFFFFF00));
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000002C);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x81003087);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010401);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	for (i = 0; i < 2; i++) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB1 + i * 0x20);
+		r5 = ((mmio_read_32(DBSC_DBPDRGD_0) & 0xFF00) >> 0x8);
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB4 + i * 0x20);
+		r6 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFF;
+
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB3 + i * 0x20);
+		r7 = mmio_read_32(DBSC_DBPDRGD_0) & 0x7;
+		r12 = (r5 >> 0x2);
+
+		if (r12 < r6) {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r7 + 0x1) & 0x7));
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r6 - r12) & 0xFF));
+		} else {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | (r7 & 0x7));
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 |
+						     ((r6 + r5 +
+						      (r5 >> 1) + r12) & 0xFF));
+		}
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00015001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000003);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0380C700);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000007);
+	while (mmio_read_32(DBSC_DBPDRGD_0) & BIT(30))
+		;
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000021);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0024643E);
+
+	mmio_write_32(DBSC_DBBUS0CNF1, 0x00000010);
+	mmio_write_32(DBSC_DBCALCNF,
+		(uint32_t) (64000000 / REFRESH_RATE) + 0x01000000);
+	mmio_write_32(DBSC_DBRFCNF1,
+		(uint32_t) (REFRESH_RATE * 116 / 125) + 0x00080000);
+	mmio_write_32(DBSC_DBRFCNF2, 0x00010000);
+	mmio_write_32(DBSC_DBDFICUPDCNF, 0x40100001);
+	mmio_write_32(DBSC_DBRFEN, 0x00000001);
+	mmio_write_32(DBSC_DBACEN, 0x00000001);
+	mmio_write_32(DBSC_DBPDLK_0, 0x00000000);
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00000000);
+
+#ifdef ddr_qos_init_setting // only for non qos_init
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00001234);
+	mmio_write_32(DBSC_DBCAM0CNF1, 0x00043218);
+	mmio_write_32(DBSC_DBCAM0CNF2, 0x000000F4);
+	mmio_write_32(DBSC_DBSCHCNT0, 0x000f0037);
+	mmio_write_32(DBSC_DBSCHSZ0, 0x00000001);
+	mmio_write_32(DBSC_DBSCHRW0, 0x22421111);
+	mmio_write_32(DBSC_SCFCTST2, 0x012F1123);
+	mmio_write_32(DBSC_DBSCHQOS00, 0x00000F00);
+	mmio_write_32(DBSC_DBSCHQOS01, 0x00000B00);
+	mmio_write_32(DBSC_DBSCHQOS02, 0x00000000);
+	mmio_write_32(DBSC_DBSCHQOS03, 0x00000000);
+	mmio_write_32(DBSC_DBSCHQOS40, 0x00000300);
+	mmio_write_32(DBSC_DBSCHQOS41, 0x000002F0);
+	mmio_write_32(DBSC_DBSCHQOS42, 0x00000200);
+	mmio_write_32(DBSC_DBSCHQOS43, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS90, 0x00000300);
+	mmio_write_32(DBSC_DBSCHQOS91, 0x000002F0);
+	mmio_write_32(DBSC_DBSCHQOS92, 0x00000200);
+	mmio_write_32(DBSC_DBSCHQOS93, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS130, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS131, 0x000000F0);
+	mmio_write_32(DBSC_DBSCHQOS132, 0x000000A0);
+	mmio_write_32(DBSC_DBSCHQOS133, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS140, 0x000000C0);
+	mmio_write_32(DBSC_DBSCHQOS141, 0x000000B0);
+	mmio_write_32(DBSC_DBSCHQOS142, 0x00000080);
+	mmio_write_32(DBSC_DBSCHQOS143, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS150, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS151, 0x00000030);
+	mmio_write_32(DBSC_DBSCHQOS152, 0x00000020);
+	mmio_write_32(DBSC_DBSCHQOS153, 0x00000010);
+	mmio_write_32(0xE67F0018, 0x00000001);
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00000000);
+#endif
+}
+
+static void init_ddr_d3_1600(void)
+{
+	uint32_t i, r2, r3, r5, r6, r7, r12;
+
+	mmio_write_32(CPG_CPGWPR, 0x5A5AFFFF);
+	mmio_write_32(CPG_CPGWPCR, 0xA5A50000);
+
+	mmio_write_32(CPG_SRCR4, 0x20000000);
+
+	mmio_write_32(0xE61500DC, 0xe2200000);
+	while (!(mmio_read_32(CPG_PLLECR) & BIT(11)))
+		;
+
+	mmio_write_32(CPG_SRSTCLR4, 0x20000000);
+
+	mmio_write_32(CPG_CPGWPCR, 0xA5A50001);
+
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00001234);
+	mmio_write_32(DBSC_DBKIND, 0x00000007);
+	mmio_write_32(DBSC_DBMEMCONF_0_0, 0x0f030a01);
+	mmio_write_32(DBSC_DBPHYCONF0, 0x00000001);
+	mmio_write_32(DBSC_DBTR0, 0x0000000B);
+	mmio_write_32(DBSC_DBTR1, 0x00000008);
+	mmio_write_32(DBSC_DBTR2, 0x00000000);
+	mmio_write_32(DBSC_DBTR3, 0x0000000B);
+	mmio_write_32(DBSC_DBTR4, 0x000B000B);
+	mmio_write_32(DBSC_DBTR5, 0x00000027);
+	mmio_write_32(DBSC_DBTR6, 0x0000001C);
+	mmio_write_32(DBSC_DBTR7, 0x00060006);
+	mmio_write_32(DBSC_DBTR8, 0x00000020);
+	mmio_write_32(DBSC_DBTR9, 0x00000006);
+	mmio_write_32(DBSC_DBTR10, 0x0000000C);
+	mmio_write_32(DBSC_DBTR11, 0x0000000A);
+	mmio_write_32(DBSC_DBTR12, 0x00120012);
+	mmio_write_32(DBSC_DBTR13, 0x000000CE);
+	mmio_write_32(DBSC_DBTR14, 0x00140005);
+	mmio_write_32(DBSC_DBTR15, 0x00050004);
+	mmio_write_32(DBSC_DBTR16, 0x071F0305);
+	mmio_write_32(DBSC_DBTR17, 0x040C0000);
+	mmio_write_32(DBSC_DBTR18, 0x00000200);
+	mmio_write_32(DBSC_DBTR19, 0x01000040);
+	mmio_write_32(DBSC_DBTR20, 0x020000D6);
+	mmio_write_32(DBSC_DBTR21, 0x00040004);
+	mmio_write_32(DBSC_DBBL, 0x00000000);
+	mmio_write_32(DBSC_DBODT0, 0x00000001);
+	mmio_write_32(DBSC_DBADJ0, 0x00000001);
+	mmio_write_32(DBSC_DBSYSCONF1, 0x00000002);
+	mmio_write_32(DBSC_DBDFICNT_0, 0x00000010);
+	mmio_write_32(DBSC_DBBCAMDIS, 0x00000001);
+	mmio_write_32(DBSC_DBSCHRW1, 0x00000046);
+	mmio_write_32(DBSC_SCFCTST0, 0x0D050B03);
+	mmio_write_32(DBSC_SCFCTST1, 0x0306030C);
+
+	mmio_write_32(DBSC_DBPDLK_0, 0x0000A55A);
+	mmio_write_32(DBSC_DBCMD, 0x01000001);
+	mmio_write_32(DBSC_DBCMD, 0x08000000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x80010000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000008);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000B8000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x04058904);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000091);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BB6B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000095);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BBAD);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000099);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BB6B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x04058900);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000021);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0024641E);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010073);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0C058900);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x04058900);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000003);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0780C700);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000007);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(30)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000004);
+	mmio_write_32(DBSC_DBPDRGD_0,
+		(uint32_t) (REFRESH_RATE * 792 / 125) - 400 + 0x08B00000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000022);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x1000040B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000023);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x2D9C0B66);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000024);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x2A88B400);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000025);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x30005200);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000026);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0014A9C9);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000027);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00000D70);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000028);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00000046);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000029);
+	if (REFRESH_RATE > 3900) {
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000018);
+	} else {
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000098);
+	}
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000002C);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x81003047);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000020);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00181884);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000001A);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x33C03C10);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000000E);
+	r2 = (mmio_read_32(DBSC_DBPDRGD_0) & 0x0000FF00) >> 0x9;
+	r3 = (r2 << 16) + (r2 << 8) + r2;
+	r6 = (r2 << 24) + (r2 << 16) + (r2 << 8) + r2;
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000011);
+	mmio_write_32(DBSC_DBPDRGD_0, r3);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000012);
+	mmio_write_32(DBSC_DBPDRGD_0, r3);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000016);
+	mmio_write_32(DBSC_DBPDRGD_0, r6);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000017);
+	mmio_write_32(DBSC_DBPDRGD_0, r6);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000018);
+	mmio_write_32(DBSC_DBPDRGD_0, r6);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000019);
+	mmio_write_32(DBSC_DBPDRGD_0, r6);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010181);
+	mmio_write_32(DBSC_DBCMD, 0x08000001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010601);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	for (i = 0; i < 2; i++) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB1 + i * 0x20);
+		r5 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFF00) >> 0x8;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB4 + i * 0x20);
+		r6 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFF;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB3 + i * 0x20);
+		r7 = mmio_read_32(DBSC_DBPDRGD_0) & 0x7;
+		if (r6 > 0) {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r7 + 0x1) & 0x7));
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | r6);
+		} else {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | r7);
+
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 |
+						     ((r6 + (r5 << 1)) & 0xFF));
+		}
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000005);
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC1AA00C0);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010801);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000005);
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC1AA00D8);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0001F001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000AF);
+	r2 = mmio_read_32(DBSC_DBPDRGD_0);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000AF);
+	mmio_write_32(DBSC_DBPDRGD_0, ((r2 + 0x1) & 0xFF) | (r2 & 0xFFFFFF00));
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000CF);
+	r2 = mmio_read_32(DBSC_DBPDRGD_0);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000CF);
+	mmio_write_32(DBSC_DBPDRGD_0, ((r2 + 0x1) & 0xFF) | (r2 & 0xFFFFFF00));
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000002C);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x81003087);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010401);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	for (i = 0; i < 2; i++) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB1 + i * 0x20);
+		r5 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFF00) >> 0x8;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB4 + i * 0x20);
+		r6 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFF;
+
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB3 + i * 0x20);
+		r7 = mmio_read_32(DBSC_DBPDRGD_0) & 0x7;
+		r12 = (r5 >> 0x2);
+
+		if (r12 < r6) {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r7 + 0x1) & 0x7));
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r6 - r12) & 0xFF));
+		} else {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | (r7 & 0x7));
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 |
+						     ((r6 + r5 +
+						      (r5 >> 1) + r12) & 0xFF));
+		}
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00015001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000003);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0380C700);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000007);
+	while (mmio_read_32(DBSC_DBPDRGD_0) & BIT(30))
+		;
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000021);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0024643E);
+
+	mmio_write_32(DBSC_DBBUS0CNF1, 0x00000010);
+	mmio_write_32(DBSC_DBCALCNF,
+		(uint32_t) (64000000 / REFRESH_RATE) + 0x01000000);
+	mmio_write_32(DBSC_DBRFCNF1,
+		(uint32_t) (REFRESH_RATE * 99 / 125) + 0x00080000);
+	mmio_write_32(DBSC_DBRFCNF2, 0x00010000);
+	mmio_write_32(DBSC_DBDFICUPDCNF, 0x40100001);
+	mmio_write_32(DBSC_DBRFEN, 0x00000001);
+	mmio_write_32(DBSC_DBACEN, 0x00000001);
+	mmio_write_32(DBSC_DBPDLK_0, 0x00000000);
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00000000);
+
+#ifdef ddr_qos_init_setting // only for non qos_init
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00001234);
+	mmio_write_32(DBSC_DBCAM0CNF1, 0x00043218);
+	mmio_write_32(DBSC_DBCAM0CNF2, 0x000000F4);
+	mmio_write_32(DBSC_DBSCHCNT0, 0x000f0037);
+	mmio_write_32(DBSC_DBSCHSZ0, 0x00000001);
+	mmio_write_32(DBSC_DBSCHRW0, 0x22421111);
+	mmio_write_32(DBSC_SCFCTST2, 0x012F1123);
+	mmio_write_32(DBSC_DBSCHQOS00, 0x00000F00);
+	mmio_write_32(DBSC_DBSCHQOS01, 0x00000B00);
+	mmio_write_32(DBSC_DBSCHQOS02, 0x00000000);
+	mmio_write_32(DBSC_DBSCHQOS03, 0x00000000);
+	mmio_write_32(DBSC_DBSCHQOS40, 0x00000300);
+	mmio_write_32(DBSC_DBSCHQOS41, 0x000002F0);
+	mmio_write_32(DBSC_DBSCHQOS42, 0x00000200);
+	mmio_write_32(DBSC_DBSCHQOS43, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS90, 0x00000300);
+	mmio_write_32(DBSC_DBSCHQOS91, 0x000002F0);
+	mmio_write_32(DBSC_DBSCHQOS92, 0x00000200);
+	mmio_write_32(DBSC_DBSCHQOS93, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS130, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS131, 0x000000F0);
+	mmio_write_32(DBSC_DBSCHQOS132, 0x000000A0);
+	mmio_write_32(DBSC_DBSCHQOS133, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS140, 0x000000C0);
+	mmio_write_32(DBSC_DBSCHQOS141, 0x000000B0);
+	mmio_write_32(DBSC_DBSCHQOS142, 0x00000080);
+	mmio_write_32(DBSC_DBSCHQOS143, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS150, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS151, 0x00000030);
+	mmio_write_32(DBSC_DBSCHQOS152, 0x00000020);
+	mmio_write_32(DBSC_DBSCHQOS153, 0x00000010);
+	mmio_write_32(0xE67F0018, 0x00000001);
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00000000);
+#endif
+}
+
+#define PRR			0xFFF00044U
+#define PRR_PRODUCT_MASK	0x00007F00U
+#define PRR_PRODUCT_D3		0x00005800U
+
+#define	MODEMR_MD19		BIT(19)
+
+int32_t rcar_dram_init(void)
+{
+	uint32_t reg;
+	uint32_t ddr_mbps;
+
+	reg = mmio_read_32(PRR);
+	if ((reg & PRR_PRODUCT_MASK) != PRR_PRODUCT_D3) {
+		ERROR("LSI Product ID (PRR=0x%x) DDR initialize not supported.\n",
+		      reg);
+		panic();
+	}
+
+	reg = mmio_read_32(RST_MODEMR);
+	if (reg & MODEMR_MD19) {
+		init_ddr_d3_1866();
+		ddr_mbps = 1866;
+	} else {
+		init_ddr_d3_1600();
+		ddr_mbps = 1600;
+	}
+
+	NOTICE("BL2: DDR%d(%s)\n", ddr_mbps, RCAR_DDR_VERSION);
+
+	return 0;
+}
diff --git a/drivers/renesas/common/ddr/ddr_a/ddr_init_e3.c b/drivers/renesas/common/ddr/ddr_a/ddr_init_e3.c
new file mode 100644
index 0000000..fc278ef
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_a/ddr_init_e3.c
@@ -0,0 +1,1712 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation.
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "boot_init_dram.h"
+#include "rcar_def.h"
+#include "../ddr_regs.h"
+
+#include "../dram_sub_func.h"
+
+#define RCAR_E3_DDR_VERSION    "rev.0.12"
+
+/* Average periodic refresh interval[ns]. Support 3900,7800 */
+#ifdef ddr_qos_init_setting
+#define REFRESH_RATE	3900U
+#else
+#if RCAR_REF_INT == 1
+#define REFRESH_RATE	7800U
+#else
+#define REFRESH_RATE	3900U
+#endif
+#endif
+
+/*
+ *  Initialize ddr
+ */
+uint32_t init_ddr(void)
+{
+	uint32_t i, r2, r5, r6, r7, r12;
+	uint32_t ddr_md;
+	uint32_t regval, j;
+	uint32_t dqsgd_0c, bdlcount_0c, bdlcount_0c_div2, bdlcount_0c_div4;
+	uint32_t bdlcount_0c_div8, bdlcount_0c_div16;
+	uint32_t gatesl_0c, rdqsd_0c, rdqsnd_0c, rbd_0c[4];
+	uint32_t pdqsr_ctl, lcdl_ctl, lcdl_judge1, lcdl_judge2;
+	uint32_t pdr_ctl;
+	uint32_t byp_ctl;
+
+	if ((mmio_read_32(0xFFF00044) & 0x000000FF) == 0x00000000) {
+		pdqsr_ctl = 1;
+		lcdl_ctl = 1;
+		pdr_ctl = 1;
+		byp_ctl = 1;
+	} else {
+		pdqsr_ctl = 0;
+		lcdl_ctl = 0;
+		pdr_ctl = 0;
+		byp_ctl = 0;
+	}
+
+	/* Judge the DDR bit rate (ddr_md : 0 = 1584Mbps, 1 = 1856Mbps) */
+	ddr_md = (mmio_read_32(RST_MODEMR) >> 19) & BIT(0);
+
+	/* 1584Mbps setting */
+	if (ddr_md == 0) {
+		mmio_write_32(CPG_CPGWPR, 0x5A5AFFFF);
+		mmio_write_32(CPG_CPGWPCR, 0xA5A50000);
+
+		mmio_write_32(CPG_SRCR4, 0x20000000);
+
+		mmio_write_32(0xE61500DC, 0xe2200000);	/* Change to 1584Mbps */
+		while (!(mmio_read_32(CPG_PLLECR) & BIT(11)))
+			;
+
+		mmio_write_32(CPG_SRSTCLR4, 0x20000000);
+
+		mmio_write_32(CPG_CPGWPCR, 0xA5A50001);
+	}
+
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00001234);
+	mmio_write_32(DBSC_DBKIND, 0x00000007);
+
+#if RCAR_DRAM_DDR3L_MEMCONF == 0
+	mmio_write_32(DBSC_DBMEMCONF_0_0, 0x0f030a02);	/* 1GB */
+#else
+	mmio_write_32(DBSC_DBMEMCONF_0_0, 0x10030a02);	/* 2GB(default) */
+#endif
+
+#if RCAR_DRAM_DDR3L_MEMDUAL == 1
+	r2 = mmio_read_32(0xE6790614);
+	mmio_write_32(0xE6790614, r2 | 0x3); /* MCS1_N/MODT1 are activated. */
+#endif
+
+	mmio_write_32(DBSC_DBPHYCONF0, 0x1);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_DBTR0, 0xB);
+		mmio_write_32(DBSC_DBTR1, 0x8);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_DBTR0, 0xD);
+		mmio_write_32(DBSC_DBTR1, 0x9);
+	}
+
+	mmio_write_32(DBSC_DBTR2, 0x00000000);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_DBTR3, 0x0000000B);
+		mmio_write_32(DBSC_DBTR4, 0x000B000B);
+		mmio_write_32(DBSC_DBTR5, 0x00000027);
+		mmio_write_32(DBSC_DBTR6, 0x0000001C);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_DBTR3, 0x0000000D);
+		mmio_write_32(DBSC_DBTR4, 0x000D000D);
+		mmio_write_32(DBSC_DBTR5, 0x0000002D);
+		mmio_write_32(DBSC_DBTR6, 0x00000020);
+	}
+
+	mmio_write_32(DBSC_DBTR7, 0x00060006);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_DBTR8, 0x00000020);
+		mmio_write_32(DBSC_DBTR9, 0x00000006);
+		mmio_write_32(DBSC_DBTR10, 0x0000000C);
+		mmio_write_32(DBSC_DBTR11, 0x0000000A);
+		mmio_write_32(DBSC_DBTR12, 0x00120012);
+		mmio_write_32(DBSC_DBTR13, 0x000000CE);
+		mmio_write_32(DBSC_DBTR14, 0x00140005);
+		mmio_write_32(DBSC_DBTR15, 0x00050004);
+		mmio_write_32(DBSC_DBTR16, 0x071F0305);
+		mmio_write_32(DBSC_DBTR17, 0x040C0000);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_DBTR8, 0x00000021);
+		mmio_write_32(DBSC_DBTR9, 0x00000007);
+		mmio_write_32(DBSC_DBTR10, 0x0000000E);
+		mmio_write_32(DBSC_DBTR11, 0x0000000C);
+		mmio_write_32(DBSC_DBTR12, 0x00140014);
+		mmio_write_32(DBSC_DBTR13, 0x000000F2);
+		mmio_write_32(DBSC_DBTR14, 0x00170006);
+		mmio_write_32(DBSC_DBTR15, 0x00060005);
+		mmio_write_32(DBSC_DBTR16, 0x09210507);
+		mmio_write_32(DBSC_DBTR17, 0x040E0000);
+	}
+
+	mmio_write_32(DBSC_DBTR18, 0x00000200);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_DBTR19, 0x01000040);
+		mmio_write_32(DBSC_DBTR20, 0x020000D6);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_DBTR19, 0x0129004B);
+		mmio_write_32(DBSC_DBTR20, 0x020000FB);
+	}
+
+	mmio_write_32(DBSC_DBTR21, 0x00040004);
+	mmio_write_32(DBSC_DBBL, 0x00000000);
+	mmio_write_32(DBSC_DBODT0, 0x00000001);
+	mmio_write_32(DBSC_DBADJ0, 0x00000001);
+	mmio_write_32(DBSC_DBSYSCONF1, 0x00000002);
+	mmio_write_32(DBSC_DBDFICNT_0, 0x00000010);
+	mmio_write_32(DBSC_DBBCAMDIS, 0x00000001);
+	mmio_write_32(DBSC_DBSCHRW1, 0x00000046);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_SCFCTST0, 0x0D050B03);
+		mmio_write_32(DBSC_SCFCTST1, 0x0306030C);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_SCFCTST0, 0x0C050B03);
+		mmio_write_32(DBSC_SCFCTST1, 0x0305030C);
+	}
+
+	/*
+	 * Initial_Step0( INITBYP )
+	 */
+	mmio_write_32(DBSC_DBPDLK_0, 0x0000A55A);
+	mmio_write_32(DBSC_DBCMD, 0x01840001);
+	mmio_write_32(DBSC_DBCMD, 0x08840000);
+	NOTICE("BL2: [COLD_BOOT]\n");
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x80010000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	/*
+	 * Initial_Step1( ZCAL,PLLINIT,DCAL,PHYRST training )
+	 */
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000008);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000B8000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x04058904);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x04058A04);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000091);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BB6B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000095);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BBAD);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000099);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BB6B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x04058900);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x04058A00);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000021);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0024641E);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010073);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	/*
+	 * Initial_Step2( DRAMRST/DRAMINT training )
+	 */
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0C058900);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0C058A00);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x04058900);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x04058A00);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000003);
+	if (byp_ctl == 1)
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0780C720);
+	else
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0780C700);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000007);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(30)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000004);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, (REFRESH_RATE * 792 / 125) -
+					     400 + 0x08B00000);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, (REFRESH_RATE * 928 / 125) -
+					     400 + 0x0A300000);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000022);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x1000040B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000023);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x2D9C0B66);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x35A00D77);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000024);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x2A88B400);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x2A8A2C28);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000025);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x30005200);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x30005E00);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000026);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0014A9C9);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0014CB49);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000027);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000D70);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000F14);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000028);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00000046);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000029);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		if (REFRESH_RATE > 3900)	/* [7]SRT=0 */
+			mmio_write_32(DBSC_DBPDRGD_0, 0x18);
+		else				/* [7]SRT=1 */
+			mmio_write_32(DBSC_DBPDRGD_0, 0x98);
+	} else {		/* 1856Mbps */
+		if (REFRESH_RATE > 3900)	/* [7]SRT=0 */
+			mmio_write_32(DBSC_DBPDRGD_0, 0x20);
+		else				/* [7]SRT=1 */
+			mmio_write_32(DBSC_DBPDRGD_0, 0xA0);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000002C);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x81003047);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000020);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00181884);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000001A);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x33C03C10);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000107);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000108);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000109);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010181);
+	mmio_write_32(DBSC_DBCMD, 0x08840001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	/*
+	 * Initial_Step3( WL/QSG training )
+	 */
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010601);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	for (i = 0; i < 4; i++) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB1 + i * 0x20);
+		r5 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFF00) >> 0x8;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB4 + i * 0x20);
+		r6 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFF;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB3 + i * 0x20);
+		r7 = mmio_read_32(DBSC_DBPDRGD_0) & 0x7;
+
+		if (r6 > 0) {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r7 + 0x1) & 0x7));
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | r6);
+		} else {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | r7);
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 |
+						     ((r6 + ((r5) << 1)) &
+						     0xFF));
+		}
+	}
+
+	/*
+	 * Initial_Step4( WLADJ training )
+	 */
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000005);
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC1AA00C0);
+
+	if (pdqsr_ctl == 0) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	}
+
+	/* PDR always off */
+	if (pdr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000103);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010801);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	/*
+	 * Initial_Step5(Read Data Bit Deskew)
+	 */
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000005);
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC1AA00D8);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00011001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	if (pdqsr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	}
+
+	/* PDR dynamic */
+	if (pdr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000103);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+	}
+
+	/*
+	 * Initial_Step6(Write Data Bit Deskew)
+	 */
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00012001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	/*
+	 * Initial_Step7(Read Data Eye Training)
+	 */
+	if (pdqsr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	}
+
+	/* PDR always off */
+	if (pdr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000103);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00014001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	if (pdqsr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	}
+
+	/* PDR dynamic */
+	if (pdr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000103);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+	}
+
+	/*
+	 * Initial_Step8(Write Data Eye Training)
+	 */
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00018001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	/*
+	 * Initial_Step3_2( DQS Gate Training )
+	 */
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000002C);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x81003087);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010401);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	for (i = 0; i < 4; i++) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB1 + i * 0x20);
+		r5 = ((mmio_read_32(DBSC_DBPDRGD_0) & 0xFF00) >> 0x8);
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB4 + i * 0x20);
+		r6 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFF;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB3 + i * 0x20);
+		r7 = mmio_read_32(DBSC_DBPDRGD_0) & 0x7;
+		r12 = (r5 >> 0x2);
+		if (r12 < r6) {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r7 + 0x1) & 0x7));
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r6 - r12) & 0xFF));
+		} else {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | (r7 & 0x7));
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r6 + r5 +
+						     (r5 >> 1) + r12) & 0xFF));
+		}
+	}
+
+	/*
+	 * Initial_Step5-2_7-2( Rd bit Rd eye )
+	 */
+	if (pdqsr_ctl == 0) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	}
+
+	/* PDR always off */
+	if (pdr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000103);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00015001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	if (lcdl_ctl == 1) {
+		for (i = 0; i < 4; i++) {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			dqsgd_0c = mmio_read_32(DBSC_DBPDRGD_0) & 0xFF;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB1 + i * 0x20);
+			bdlcount_0c = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFF00) >>
+					8;
+			bdlcount_0c_div2 = bdlcount_0c >> 1;
+			bdlcount_0c_div4 = bdlcount_0c >> 2;
+			bdlcount_0c_div8 = bdlcount_0c >> 3;
+			bdlcount_0c_div16 = bdlcount_0c >> 4;
+
+			if (ddr_md == 0) {	/* 1584Mbps */
+				lcdl_judge1 = bdlcount_0c_div2 +
+					      bdlcount_0c_div4 +
+					      bdlcount_0c_div8;
+				lcdl_judge2 = bdlcount_0c +
+					      bdlcount_0c_div4 +
+					      bdlcount_0c_div16;
+			} else {		/* 1856Mbps */
+				lcdl_judge1 = bdlcount_0c_div2 +
+					      bdlcount_0c_div4;
+				lcdl_judge2 = bdlcount_0c +
+					      bdlcount_0c_div4;
+			}
+
+			if (dqsgd_0c <= lcdl_judge1)
+				continue;
+
+			if (dqsgd_0c <= lcdl_judge2) {
+				mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+				regval = mmio_read_32(DBSC_DBPDRGD_0) &
+						0xFFFFFF00;
+				mmio_write_32(DBSC_DBPDRGD_0,
+					      (dqsgd_0c - bdlcount_0c_div8) |
+					      regval);
+			} else {
+				mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+				regval = mmio_read_32(DBSC_DBPDRGD_0) &
+						0xFFFFFF00;
+				mmio_write_32(DBSC_DBPDRGD_0, regval);
+				mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+				gatesl_0c = mmio_read_32(DBSC_DBPDRGD_0) & 0x7;
+				mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+				regval = mmio_read_32(DBSC_DBPDRGD_0) &
+						0xFFFFFFF8;
+				mmio_write_32(DBSC_DBPDRGD_0, regval |
+							     (gatesl_0c + 1));
+				mmio_write_32(DBSC_DBPDRGA_0, 0xAF + i * 0x20);
+				regval = (mmio_read_32(DBSC_DBPDRGD_0));
+				rdqsd_0c = (regval & 0xFF00) >> 8;
+				rdqsnd_0c = (regval & 0xFF0000) >> 16;
+				mmio_write_32(DBSC_DBPDRGA_0, 0xAF + i * 0x20);
+				mmio_write_32(DBSC_DBPDRGD_0,
+					      (regval & 0xFF0000FF) |
+					      ((rdqsd_0c +
+						bdlcount_0c_div4) << 8) |
+					      ((rdqsnd_0c +
+						bdlcount_0c_div4) << 16));
+				mmio_write_32(DBSC_DBPDRGA_0, 0xAA + i * 0x20);
+				regval = (mmio_read_32(DBSC_DBPDRGD_0));
+				rbd_0c[0] = (regval) & 0x1f;
+				rbd_0c[1] = (regval >> 8) & 0x1f;
+				rbd_0c[2] = (regval >> 16) & 0x1f;
+				rbd_0c[3] = (regval >> 24) & 0x1f;
+				mmio_write_32(DBSC_DBPDRGA_0, 0xAA + i * 0x20);
+				regval = mmio_read_32(DBSC_DBPDRGD_0) &
+					0xE0E0E0E0;
+				for (j = 0; j < 4; j++) {
+					rbd_0c[j] = rbd_0c[j] +
+						    bdlcount_0c_div4;
+					if (rbd_0c[j] > 0x1F)
+						rbd_0c[j] = 0x1F;
+					regval = regval | (rbd_0c[j] << 8 * j);
+				}
+				mmio_write_32(DBSC_DBPDRGD_0, regval);
+				mmio_write_32(DBSC_DBPDRGA_0, 0xAB + i * 0x20);
+				regval = (mmio_read_32(DBSC_DBPDRGD_0));
+				rbd_0c[0] = (regval) & 0x1f;
+				rbd_0c[1] = (regval >> 8) & 0x1f;
+				rbd_0c[2] = (regval >> 16) & 0x1f;
+				rbd_0c[3] = (regval >> 24) & 0x1f;
+				mmio_write_32(DBSC_DBPDRGA_0, 0xAB + i * 0x20);
+				regval = mmio_read_32(DBSC_DBPDRGD_0) &
+					0xE0E0E0E0;
+				for (j = 0; j < 4; j++) {
+					rbd_0c[j] = rbd_0c[j] +
+						    bdlcount_0c_div4;
+					if (rbd_0c[j] > 0x1F)
+						rbd_0c[j] = 0x1F;
+					regval = regval | (rbd_0c[j] << 8 * j);
+				}
+				mmio_write_32(DBSC_DBPDRGD_0, regval);
+			}
+		}
+		mmio_write_32(DBSC_DBPDRGA_0, 0x2);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7D81E37);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000003);
+	if (byp_ctl == 1)
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0380C720);
+	else
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0380C700);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000007);
+	while (mmio_read_32(DBSC_DBPDRGD_0) & BIT(30))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000021);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0024643E);
+
+	mmio_write_32(DBSC_DBBUS0CNF1, 0x00000010);
+	mmio_write_32(DBSC_DBCALCNF, (64000000 / REFRESH_RATE) + 0x01000000);
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_DBRFCNF1,
+			      (REFRESH_RATE * 99 / 125) + 0x00080000);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_DBRFCNF1,
+			      (REFRESH_RATE * 116 / 125) + 0x00080000);
+	}
+
+	mmio_write_32(DBSC_DBRFCNF2, 0x00010000);
+	mmio_write_32(DBSC_DBDFICUPDCNF, 0x40100001);
+	mmio_write_32(DBSC_DBRFEN, 0x00000001);
+	mmio_write_32(DBSC_DBACEN, 0x00000001);
+
+	if (pdqsr_ctl == 1) {
+		mmio_write_32(0xE67F0018, 0x00000001);
+		regval = mmio_read_32(0x40000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGD_0, regval);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	}
+
+	/* PDR dynamic */
+	if (pdr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000103);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+	}
+
+	/*
+	 * Initial_Step9( Initial End )
+	 */
+	mmio_write_32(DBSC_DBPDLK_0, 0x00000000);
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00000000);
+
+#ifdef ddr_qos_init_setting /* only for non qos_init */
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00001234);
+	mmio_write_32(DBSC_DBCAM0CNF1, 0x00043218);
+	mmio_write_32(DBSC_DBCAM0CNF2, 0x000000F4);
+	mmio_write_32(DBSC_DBSCHCNT0, 0x000f0037);
+	mmio_write_32(DBSC_DBSCHSZ0, 0x00000001);
+	mmio_write_32(DBSC_DBSCHRW0, 0x22421111);
+	mmio_write_32(DBSC_SCFCTST2, 0x012F1123);
+	mmio_write_32(DBSC_DBSCHQOS00, 0x00000F00);
+	mmio_write_32(DBSC_DBSCHQOS01, 0x00000B00);
+	mmio_write_32(DBSC_DBSCHQOS02, 0x00000000);
+	mmio_write_32(DBSC_DBSCHQOS03, 0x00000000);
+	mmio_write_32(DBSC_DBSCHQOS40, 0x00000300);
+	mmio_write_32(DBSC_DBSCHQOS41, 0x000002F0);
+	mmio_write_32(DBSC_DBSCHQOS42, 0x00000200);
+	mmio_write_32(DBSC_DBSCHQOS43, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS90, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS91, 0x000000F0);
+	mmio_write_32(DBSC_DBSCHQOS92, 0x000000A0);
+	mmio_write_32(DBSC_DBSCHQOS93, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS130, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS131, 0x000000F0);
+	mmio_write_32(DBSC_DBSCHQOS132, 0x000000A0);
+	mmio_write_32(DBSC_DBSCHQOS133, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS140, 0x000000C0);
+	mmio_write_32(DBSC_DBSCHQOS141, 0x000000B0);
+	mmio_write_32(DBSC_DBSCHQOS142, 0x00000080);
+	mmio_write_32(DBSC_DBSCHQOS143, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS150, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS151, 0x00000030);
+	mmio_write_32(DBSC_DBSCHQOS152, 0x00000020);
+	mmio_write_32(DBSC_DBSCHQOS153, 0x00000010);
+
+	if (pdqsr_ctl == 0)
+		mmio_write_32(0xE67F0018, 0x00000001);
+
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00000000);
+#endif
+
+	return 1;
+}
+
+static uint32_t recovery_from_backup_mode(uint32_t ddr_backup)
+{
+	/*
+	 * recovery_Step0(DBSC Setting 1) / same "init_ddr"
+	 */
+	uint32_t r2, r5, r6, r7, r12, i;
+	uint32_t ddr_md;
+	uint32_t err;
+	uint32_t regval, j;
+	uint32_t dqsgd_0c, bdlcount_0c, bdlcount_0c_div2, bdlcount_0c_div4;
+	uint32_t bdlcount_0c_div8, bdlcount_0c_div16;
+	uint32_t gatesl_0c, rdqsd_0c, rdqsnd_0c, rbd_0c[4];
+	uint32_t pdqsr_ctl, lcdl_ctl, lcdl_judge1, lcdl_judge2;
+	uint32_t pdr_ctl;
+	uint32_t byp_ctl;
+
+	if ((mmio_read_32(0xFFF00044) & 0x000000FF) == 0x00000000) {
+		pdqsr_ctl = 1;
+		lcdl_ctl = 1;
+		pdr_ctl = 1;
+		byp_ctl = 1;
+	} else {
+		pdqsr_ctl = 0;
+		lcdl_ctl = 0;
+		pdr_ctl = 0;
+		byp_ctl = 0;
+	}
+
+	/* Judge the DDR bit rate (ddr_md : 0 = 1584Mbps, 1 = 1856Mbps) */
+	ddr_md = (mmio_read_32(RST_MODEMR) >> 19) & BIT(0);
+
+	/* 1584Mbps setting */
+	if (ddr_md == 0) {
+		mmio_write_32(CPG_CPGWPR, 0x5A5AFFFF);
+		mmio_write_32(CPG_CPGWPCR, 0xA5A50000);
+
+		mmio_write_32(CPG_SRCR4, 0x20000000);
+
+		mmio_write_32(0xE61500DC, 0xe2200000);	/* Change to 1584Mbps */
+		while (!(mmio_read_32(CPG_PLLECR) & BIT(11)))
+			;
+
+		mmio_write_32(CPG_SRSTCLR4, 0x20000000);
+
+		mmio_write_32(CPG_CPGWPCR, 0xA5A50001);
+	}
+
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00001234);
+	mmio_write_32(DBSC_DBKIND, 0x00000007);
+
+#if RCAR_DRAM_DDR3L_MEMCONF == 0
+	mmio_write_32(DBSC_DBMEMCONF_0_0, 0x0f030a02);
+#else
+	mmio_write_32(DBSC_DBMEMCONF_0_0, 0x10030a02);
+#endif
+
+#if RCAR_DRAM_DDR3L_MEMDUAL == 1
+	r2 = mmio_read_32(0xE6790614);
+	mmio_write_32(0xE6790614, r2 | 0x3); /* MCS1_N/MODT1 are activated. */
+#endif
+
+	mmio_write_32(DBSC_DBPHYCONF0, 0x00000001);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_DBTR0, 0x0000000B);
+		mmio_write_32(DBSC_DBTR1, 0x00000008);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_DBTR0, 0x0000000D);
+		mmio_write_32(DBSC_DBTR1, 0x00000009);
+	}
+
+	mmio_write_32(DBSC_DBTR2, 0x00000000);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_DBTR3, 0x0000000B);
+		mmio_write_32(DBSC_DBTR4, 0x000B000B);
+		mmio_write_32(DBSC_DBTR5, 0x00000027);
+		mmio_write_32(DBSC_DBTR6, 0x0000001C);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_DBTR3, 0x0000000D);
+		mmio_write_32(DBSC_DBTR4, 0x000D000D);
+		mmio_write_32(DBSC_DBTR5, 0x0000002D);
+		mmio_write_32(DBSC_DBTR6, 0x00000020);
+	}
+
+	mmio_write_32(DBSC_DBTR7, 0x00060006);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_DBTR8, 0x00000020);
+		mmio_write_32(DBSC_DBTR9, 0x00000006);
+		mmio_write_32(DBSC_DBTR10, 0x0000000C);
+		mmio_write_32(DBSC_DBTR11, 0x0000000A);
+		mmio_write_32(DBSC_DBTR12, 0x00120012);
+		mmio_write_32(DBSC_DBTR13, 0x000000CE);
+		mmio_write_32(DBSC_DBTR14, 0x00140005);
+		mmio_write_32(DBSC_DBTR15, 0x00050004);
+		mmio_write_32(DBSC_DBTR16, 0x071F0305);
+		mmio_write_32(DBSC_DBTR17, 0x040C0000);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_DBTR8, 0x00000021);
+		mmio_write_32(DBSC_DBTR9, 0x00000007);
+		mmio_write_32(DBSC_DBTR10, 0x0000000E);
+		mmio_write_32(DBSC_DBTR11, 0x0000000C);
+		mmio_write_32(DBSC_DBTR12, 0x00140014);
+		mmio_write_32(DBSC_DBTR13, 0x000000F2);
+		mmio_write_32(DBSC_DBTR14, 0x00170006);
+		mmio_write_32(DBSC_DBTR15, 0x00060005);
+		mmio_write_32(DBSC_DBTR16, 0x09210507);
+		mmio_write_32(DBSC_DBTR17, 0x040E0000);
+	}
+
+	mmio_write_32(DBSC_DBTR18, 0x00000200);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_DBTR19, 0x01000040);
+		mmio_write_32(DBSC_DBTR20, 0x020000D6);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_DBTR19, 0x0129004B);
+		mmio_write_32(DBSC_DBTR20, 0x020000FB);
+	}
+
+	mmio_write_32(DBSC_DBTR21, 0x00040004);
+	mmio_write_32(DBSC_DBBL, 0x00000000);
+	mmio_write_32(DBSC_DBODT0, 0x00000001);
+	mmio_write_32(DBSC_DBADJ0, 0x00000001);
+	mmio_write_32(DBSC_DBSYSCONF1, 0x00000002);
+	mmio_write_32(DBSC_DBDFICNT_0, 0x00000010);
+	mmio_write_32(DBSC_DBBCAMDIS, 0x00000001);
+	mmio_write_32(DBSC_DBSCHRW1, 0x00000046);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_SCFCTST0, 0x0D050B03);
+		mmio_write_32(DBSC_SCFCTST1, 0x0306030C);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_SCFCTST0, 0x0C050B03);
+		mmio_write_32(DBSC_SCFCTST1, 0x0305030C);
+	}
+
+	/*
+	 * recovery_Step1(PHY setting 1)
+	 */
+	mmio_write_32(DBSC_DBPDLK_0, 0x0000A55A);
+	mmio_write_32(DBSC_DBCMD, 0x01840001);
+	mmio_write_32(DBSC_DBCMD, 0x0A840000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000008);	/* DDR_PLLCR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000B8000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000003);	/* DDR_PGCR1 */
+	if (byp_ctl == 1)
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0780C720);
+	else
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0780C700);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000020);	/* DDR_DXCCR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00181884);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000001A);	/* DDR_ACIOCR0 */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x33C03C10);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000007);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(30)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000004);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, (REFRESH_RATE * 792 / 125) -
+					     400 + 0x08B00000);
+	} else {		/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, (REFRESH_RATE * 928 / 125) -
+					     400 + 0x0A300000);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000022);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x1000040B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000023);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x2D9C0B66);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x35A00D77);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000024);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x2A88B400);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x2A8A2C28);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000025);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x30005200);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x30005E00);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000026);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0014A9C9);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0014CB49);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000027);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000D70);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000F14);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000028);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00000046);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000029);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		if (REFRESH_RATE > 3900)
+			mmio_write_32(DBSC_DBPDRGD_0, 0x18);	/* [7]SRT=0 */
+		else
+			mmio_write_32(DBSC_DBPDRGD_0, 0x98);	/* [7]SRT=1 */
+	} else {	/* 1856Mbps */
+		if (REFRESH_RATE > 3900)
+			mmio_write_32(DBSC_DBPDRGD_0, 0x20);	/* [7]SRT=0 */
+		else
+			mmio_write_32(DBSC_DBPDRGD_0, 0xA0);	/* [7]SRT=1 */
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000002C);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x81003047);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000091);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BB6B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000095);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BBAD);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000099);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BB6B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000021);	/* DDR_DSGCR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0024641E);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);	/* DDR_PGSR0 */
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);	/* DDR_PIR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x40010000);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);	/* DDR_PGSR0 */
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000092);	/* DDR_ZQ0DR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC2C59AB5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000096);	/* DDR_ZQ1DR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC4285FBF);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000009A);	/* DDR_ZQ2DR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC2C59AB5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);	/* DDR_ZQCR */
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0C058900);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0C058A00);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);	/* DDR_ZQCR */
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x04058900);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x04058A00);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);	/* DDR_PIR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00050001);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);	/* DDR_PGSR0 */
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	/* ddr backupmode end */
+	if (ddr_backup)
+		NOTICE("BL2: [WARM_BOOT]\n");
+	else
+		NOTICE("BL2: [COLD_BOOT]\n");
+
+	err = rcar_dram_update_boot_status(ddr_backup);
+	if (err) {
+		NOTICE("BL2: [BOOT_STATUS_UPDATE_ERROR]\n");
+		return INITDRAM_ERR_I;
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000092);	/* DDR_ZQ0DR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x02C59AB5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000096);	/* DDR_ZQ1DR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x04285FBF);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000009A);	/* DDR_ZQ2DR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x02C59AB5);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);	/* DDR_PIR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x08000000);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);	/* DDR_PIR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00000003);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);	/* DDR_PGSR0 */
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);	/* DDR_PIR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x80010000);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);	/* DDR_PGSR0 */
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);	/* DDR_PIR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010073);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);	/* DDR_PGSR0 */
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);	/* DDR_ZQCR */
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0C058900);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0C058A00);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);	/* DDR_ZQCR */
+
+	/* Select setting value in bps */
+	if (ddr_md == 0)	/* 1584Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x04058900);
+	else			/* 1856Mbps */
+		mmio_write_32(DBSC_DBPDRGD_0, 0x04058A00);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000000C);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x18000040);
+
+	/*
+	 * recovery_Step2(PHY setting 2)
+	 */
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000107);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000108);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000109);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+
+	mmio_write_32(DBSC_DBCALCNF, (64000000 / REFRESH_RATE) + 0x01000000);
+	mmio_write_32(DBSC_DBBUS0CNF1, 0x00000010);
+
+	/* Select setting value in bps */
+	if (ddr_md == 0) {	/* 1584Mbps */
+		mmio_write_32(DBSC_DBRFCNF1,
+			      (REFRESH_RATE * 99 / 125) + 0x00080000);
+	} else {			/* 1856Mbps */
+		mmio_write_32(DBSC_DBRFCNF1,
+			      (REFRESH_RATE * 116 / 125) + 0x00080000);
+	}
+
+	mmio_write_32(DBSC_DBRFCNF2, 0x00010000);
+	mmio_write_32(DBSC_DBRFEN, 0x00000001);
+	mmio_write_32(DBSC_DBCMD, 0x0A840001);
+	while (mmio_read_32(DBSC_DBWAIT) & BIT(0))
+		;
+
+	mmio_write_32(DBSC_DBCMD, 0x00000000);
+
+	mmio_write_32(DBSC_DBCMD, 0x04840010);
+	while (mmio_read_32(DBSC_DBWAIT) & BIT(0))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);	/* DDR_PGSR0 */
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);	/* DDR_PIR */
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010701);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);	/* DDR_PGSR0 */
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	for (i = 0; i < 4; i++) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB1 + i * 0x20);
+		r5 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFF00) >> 0x8;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB4 + i * 0x20);
+		r6 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFF;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB3 + i * 0x20);
+		r7 = mmio_read_32(DBSC_DBPDRGD_0) & 0x7;
+
+		if (r6 > 0) {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r7 + 0x1) & 0x7));
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | r6);
+		} else {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | r7);
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0,
+				      r2 | ((r6 + (r5 << 1)) & 0xFF));
+		}
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000005);
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC1AA00C0);
+
+	if (pdqsr_ctl == 0) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	}
+
+	/* PDR always off */
+	if (pdr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000103);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010801);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000005);
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC1AA00D8);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00011001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	if (pdqsr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	}
+
+	/* PDR dynamic */
+	if (pdr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000103);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00012001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	if (pdqsr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	}
+
+	/* PDR always off */
+	if (pdr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000103);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00014001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	if (pdqsr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	}
+
+	/* PDR dynamic */
+	if (pdr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000103);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00018001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000002C);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x81003087);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010401);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	for (i = 0; i < 4; i++) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB1 + i * 0x20);
+		r5 = ((mmio_read_32(DBSC_DBPDRGD_0) & 0xFF00) >> 0x8);
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB4 + i * 0x20);
+		r6 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFF;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB3 + i * 0x20);
+		r7 = mmio_read_32(DBSC_DBPDRGD_0) & 0x7;
+		r12 = r5 >> 0x2;
+
+		if (r12 < r6) {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r7 + 0x1) & 0x7));
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | ((r6 - r12) & 0xFF));
+		} else {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | (r7 & 0x7));
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00;
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0,
+				      r2 |
+				      ((r6 + r5 + (r5 >> 1) + r12) & 0xFF));
+		}
+	}
+
+	if (pdqsr_ctl == 0) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	}
+
+	/* PDR always off */
+	if (pdr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000103);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000008);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00015001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	if (lcdl_ctl == 1) {
+		for (i = 0; i < 4; i++) {
+			mmio_write_32(DBSC_DBPDRGA_0, 0x000000B0 + i * 0x20);
+			dqsgd_0c = mmio_read_32(DBSC_DBPDRGD_0) & 0x000000FF;
+			mmio_write_32(DBSC_DBPDRGA_0, 0x000000B1 + i * 0x20);
+			bdlcount_0c = (mmio_read_32(DBSC_DBPDRGD_0) &
+					0x0000FF00) >> 8;
+			bdlcount_0c_div2 = (bdlcount_0c >> 1);
+			bdlcount_0c_div4 = (bdlcount_0c >> 2);
+			bdlcount_0c_div8 = (bdlcount_0c >> 3);
+			bdlcount_0c_div16 = (bdlcount_0c >> 4);
+
+			if (ddr_md == 0) {	/* 1584Mbps */
+				lcdl_judge1 = bdlcount_0c_div2 +
+					      bdlcount_0c_div4 +
+					      bdlcount_0c_div8;
+				lcdl_judge2 = bdlcount_0c +
+					      bdlcount_0c_div4 +
+					      bdlcount_0c_div16;
+			} else {	/* 1856Mbps */
+				lcdl_judge1 = bdlcount_0c_div2 +
+					      bdlcount_0c_div4;
+				lcdl_judge2 = bdlcount_0c +
+					      bdlcount_0c_div4;
+			}
+
+			if (dqsgd_0c <= lcdl_judge1)
+				continue;
+
+			if (dqsgd_0c <= lcdl_judge2) {
+				mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+				regval = mmio_read_32(DBSC_DBPDRGD_0) &
+						0xFFFFFF00;
+				mmio_write_32(DBSC_DBPDRGD_0,
+					      (dqsgd_0c - bdlcount_0c_div8) |
+					      regval);
+			} else {
+				mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+				regval = mmio_read_32(DBSC_DBPDRGD_0) &
+						0xFFFFFF00;
+				mmio_write_32(DBSC_DBPDRGD_0, regval);
+				mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+				gatesl_0c = mmio_read_32(DBSC_DBPDRGD_0) & 0x7;
+				mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+				regval = mmio_read_32(DBSC_DBPDRGD_0) &
+						0xFFFFFFF8;
+				mmio_write_32(DBSC_DBPDRGD_0,
+					      regval | (gatesl_0c + 1));
+				mmio_write_32(DBSC_DBPDRGA_0, 0xAF + i * 0x20);
+				regval = mmio_read_32(DBSC_DBPDRGD_0);
+				rdqsd_0c = (regval & 0xFF00) >> 8;
+				rdqsnd_0c = (regval & 0xFF0000) >> 16;
+				mmio_write_32(DBSC_DBPDRGA_0, 0xAF + i * 0x20);
+				mmio_write_32(DBSC_DBPDRGD_0,
+					      (regval & 0xFF0000FF) |
+					      ((rdqsd_0c +
+						bdlcount_0c_div4) << 8) |
+					      ((rdqsnd_0c +
+						bdlcount_0c_div4) << 16));
+				mmio_write_32(DBSC_DBPDRGA_0, 0xAA + i * 0x20);
+				regval = (mmio_read_32(DBSC_DBPDRGD_0));
+				rbd_0c[0] = (regval) & 0x1f;
+				rbd_0c[1] = (regval >>  8) & 0x1f;
+				rbd_0c[2] = (regval >> 16) & 0x1f;
+				rbd_0c[3] = (regval >> 24) & 0x1f;
+				mmio_write_32(DBSC_DBPDRGA_0, 0xAA + i * 0x20);
+				regval = mmio_read_32(DBSC_DBPDRGD_0) &
+						0xE0E0E0E0;
+				for (j = 0; j < 4; j++) {
+					rbd_0c[j] = rbd_0c[j] +
+						    bdlcount_0c_div4;
+					if (rbd_0c[j] > 0x1F)
+						rbd_0c[j] = 0x1F;
+					regval = regval | (rbd_0c[j] << 8 * j);
+				}
+				mmio_write_32(DBSC_DBPDRGD_0, regval);
+				mmio_write_32(DBSC_DBPDRGA_0, 0xAB + i * 0x20);
+				regval = (mmio_read_32(DBSC_DBPDRGD_0));
+				rbd_0c[0] = regval & 0x1f;
+				rbd_0c[1] = (regval >> 8) & 0x1f;
+				rbd_0c[2] = (regval >> 16) & 0x1f;
+				rbd_0c[3] = (regval >> 24) & 0x1f;
+				mmio_write_32(DBSC_DBPDRGA_0, 0xAB + i * 0x20);
+				regval = mmio_read_32(DBSC_DBPDRGD_0) &
+						0xE0E0E0E0;
+				for (j = 0; j < 4; j++) {
+					rbd_0c[j] = rbd_0c[j] +
+						    bdlcount_0c_div4;
+					if (rbd_0c[j] > 0x1F)
+						rbd_0c[j] = 0x1F;
+					regval = regval | (rbd_0c[j] << 8 * j);
+				}
+				mmio_write_32(DBSC_DBPDRGD_0, regval);
+			}
+		}
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000002);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x07D81E37);
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000003);
+	if (byp_ctl == 1)
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0380C720);
+	else
+		mmio_write_32(DBSC_DBPDRGD_0, 0x0380C700);
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000007);
+	while (mmio_read_32(DBSC_DBPDRGD_0) & BIT(30))
+		;
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000021);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0024643E);
+
+	/*
+	 * recovery_Step3(DBSC Setting 2)
+	 */
+	mmio_write_32(DBSC_DBDFICUPDCNF, 0x40100001);
+	mmio_write_32(DBSC_DBACEN, 0x00000001);
+
+	if (pdqsr_ctl == 1) {
+		mmio_write_32(0xE67F0018, 0x00000001);
+		regval = mmio_read_32(0x40000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGD_0, regval);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	}
+
+	/* PDR dynamic */
+	if (pdr_ctl == 1) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000A3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000C3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x000000E3);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+		mmio_write_32(DBSC_DBPDRGA_0, 0x00000103);
+		mmio_write_32(DBSC_DBPDRGD_0, 0x00000000);
+	}
+
+	mmio_write_32(DBSC_DBPDLK_0, 0x00000000);
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00000000);
+
+#ifdef ddr_qos_init_setting /* only for non qos_init */
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00001234);
+	mmio_write_32(DBSC_DBCAM0CNF1, 0x00043218);
+	mmio_write_32(DBSC_DBCAM0CNF2, 0x000000F4);
+	mmio_write_32(DBSC_DBSCHCNT0, 0x000f0037);
+	mmio_write_32(DBSC_DBSCHSZ0, 0x00000001);
+	mmio_write_32(DBSC_DBSCHRW0, 0x22421111);
+	mmio_write_32(DBSC_SCFCTST2, 0x012F1123);
+	mmio_write_32(DBSC_DBSCHQOS00, 0x00000F00);
+	mmio_write_32(DBSC_DBSCHQOS01, 0x00000B00);
+	mmio_write_32(DBSC_DBSCHQOS02, 0x00000000);
+	mmio_write_32(DBSC_DBSCHQOS03, 0x00000000);
+	mmio_write_32(DBSC_DBSCHQOS40, 0x00000300);
+	mmio_write_32(DBSC_DBSCHQOS41, 0x000002F0);
+	mmio_write_32(DBSC_DBSCHQOS42, 0x00000200);
+	mmio_write_32(DBSC_DBSCHQOS43, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS90, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS91, 0x000000F0);
+	mmio_write_32(DBSC_DBSCHQOS92, 0x000000A0);
+	mmio_write_32(DBSC_DBSCHQOS93, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS130, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS131, 0x000000F0);
+	mmio_write_32(DBSC_DBSCHQOS132, 0x000000A0);
+	mmio_write_32(DBSC_DBSCHQOS133, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS140, 0x000000C0);
+	mmio_write_32(DBSC_DBSCHQOS141, 0x000000B0);
+	mmio_write_32(DBSC_DBSCHQOS142, 0x00000080);
+	mmio_write_32(DBSC_DBSCHQOS143, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS150, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS151, 0x00000030);
+	mmio_write_32(DBSC_DBSCHQOS152, 0x00000020);
+	mmio_write_32(DBSC_DBSCHQOS153, 0x00000010);
+
+	if (pdqsr_ctl == 0)
+		mmio_write_32(0xE67F0018, 0x00000001);
+
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00000000);
+#endif
+
+	return 1;
+
+} /* recovery_from_backup_mode */
+
+/*
+ * init_ddr : MD19=0,DDR3L,1584Mbps / MD19=1,DDR3L,1856Mbps
+ */
+
+/*
+ * DDR Initialize entry for IPL
+ */
+int32_t rcar_dram_init(void)
+{
+	uint32_t dataL;
+	uint32_t failcount;
+	uint32_t md = 0;
+	uint32_t ddr = 0;
+	uint32_t ddr_backup;
+
+	md = *((volatile uint32_t*)RST_MODEMR);
+	ddr = (md & 0x00080000) >> 19;
+	if (ddr == 0x0)
+		NOTICE("BL2: DDR1584(%s)\n", RCAR_E3_DDR_VERSION);
+	else if (ddr == 0x1)
+		NOTICE("BL2: DDR1856(%s)\n", RCAR_E3_DDR_VERSION);
+
+	rcar_dram_get_boot_status(&ddr_backup);
+
+	if (ddr_backup == DRAM_BOOT_STATUS_WARM)
+		dataL = recovery_from_backup_mode(ddr_backup);	/* WARM boot */
+	else
+		dataL = init_ddr();				/* COLD boot */
+
+	if (dataL == 1)
+		failcount = 0;
+	else
+		failcount = 1;
+
+	if (failcount == 0)
+		return INITDRAM_OK;
+	else
+		return INITDRAM_NG;
+
+}
diff --git a/drivers/renesas/common/ddr/ddr_a/ddr_init_v3m.c b/drivers/renesas/common/ddr/ddr_a/ddr_init_v3m.c
new file mode 100644
index 0000000..5410771
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_a/ddr_init_v3m.c
@@ -0,0 +1,339 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <stdint.h>
+#include "boot_init_dram.h"
+#include "rcar_def.h"
+#include "../ddr_regs.h"
+
+static uint32_t init_ddr_v3m_1600(void)
+{
+	uint32_t i, r2, r5, r6, r7, r12;
+
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00001234);
+	mmio_write_32(DBSC_DBKIND, 0x00000007);
+#if RCAR_DRAM_DDR3L_MEMCONF == 0
+	mmio_write_32(DBSC_DBMEMCONF_0_0, 0x0f030a02); // 1GB: Eagle
+#else
+	mmio_write_32(DBSC_DBMEMCONF_0_0, 0x10030a02); // 2GB: V3MSK
+#endif
+	mmio_write_32(DBSC_DBPHYCONF0, 0x00000001);
+	mmio_write_32(DBSC_DBTR0, 0x0000000B);
+	mmio_write_32(DBSC_DBTR1, 0x00000008);
+	mmio_write_32(DBSC_DBTR3, 0x0000000B);
+	mmio_write_32(DBSC_DBTR4, 0x000B000B);
+	mmio_write_32(DBSC_DBTR5, 0x00000027);
+	mmio_write_32(DBSC_DBTR6, 0x0000001C);
+	mmio_write_32(DBSC_DBTR7, 0x00060006);
+	mmio_write_32(DBSC_DBTR8, 0x00000020);
+	mmio_write_32(DBSC_DBTR9, 0x00000006);
+	mmio_write_32(DBSC_DBTR10, 0x0000000C);
+	mmio_write_32(DBSC_DBTR11, 0x0000000B);
+	mmio_write_32(DBSC_DBTR12, 0x00120012);
+	mmio_write_32(DBSC_DBTR13, 0x01180118);
+	mmio_write_32(DBSC_DBTR14, 0x00140005);
+	mmio_write_32(DBSC_DBTR15, 0x00050004);
+	mmio_write_32(DBSC_DBTR16, 0x071D0305);
+	mmio_write_32(DBSC_DBTR17, 0x040C0010);
+	mmio_write_32(DBSC_DBTR18, 0x00000200);
+	mmio_write_32(DBSC_DBTR19, 0x01000040);
+	mmio_write_32(DBSC_DBTR20, 0x02000120);
+	mmio_write_32(DBSC_DBTR21, 0x00040004);
+	mmio_write_32(DBSC_DBBL, 0x00000000);
+	mmio_write_32(DBSC_DBODT0, 0x00000001);
+	mmio_write_32(DBSC_DBADJ0, 0x00000001);
+	mmio_write_32(DBSC_DBCAM0CNF1, 0x00082010);
+	mmio_write_32(DBSC_DBCAM0CNF2, 0x00002000);
+	mmio_write_32(DBSC_DBSCHCNT0, 0x080f003f);
+	mmio_write_32(DBSC_DBSCHCNT1, 0x00001010);
+	mmio_write_32(DBSC_DBSCHSZ0, 0x00000001);
+	mmio_write_32(DBSC_DBSCHRW0, 0x00000200);
+	mmio_write_32(DBSC_DBSCHRW1, 0x00000040);
+	mmio_write_32(DBSC_DBSCHQOS40, 0x00000600);
+	mmio_write_32(DBSC_DBSCHQOS41, 0x00000480);
+	mmio_write_32(DBSC_DBSCHQOS42, 0x00000300);
+	mmio_write_32(DBSC_DBSCHQOS43, 0x00000180);
+	mmio_write_32(DBSC_DBSCHQOS90, 0x00000400);
+	mmio_write_32(DBSC_DBSCHQOS91, 0x00000300);
+	mmio_write_32(DBSC_DBSCHQOS92, 0x00000200);
+	mmio_write_32(DBSC_DBSCHQOS93, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS130, 0x00000300);
+	mmio_write_32(DBSC_DBSCHQOS131, 0x00000240);
+	mmio_write_32(DBSC_DBSCHQOS132, 0x00000180);
+	mmio_write_32(DBSC_DBSCHQOS133, 0x000000c0);
+	mmio_write_32(DBSC_DBSCHQOS140, 0x00000200);
+	mmio_write_32(DBSC_DBSCHQOS141, 0x00000180);
+	mmio_write_32(DBSC_DBSCHQOS142, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS143, 0x00000080);
+	mmio_write_32(DBSC_DBSCHQOS150, 0x00000100);
+	mmio_write_32(DBSC_DBSCHQOS151, 0x000000c0);
+	mmio_write_32(DBSC_DBSCHQOS152, 0x00000080);
+	mmio_write_32(DBSC_DBSCHQOS153, 0x00000040);
+	mmio_write_32(DBSC_DBSYSCONF1, 0x00000002);
+	mmio_write_32(DBSC_DBCAM0CNF1, 0x00040C04);
+	mmio_write_32(DBSC_DBCAM0CNF2, 0x000001c4);
+	mmio_write_32(DBSC_DBSCHSZ0, 0x00000003);
+	mmio_write_32(DBSC_DBSCHRW1, 0x001a0080);
+	mmio_write_32(DBSC_DBDFICNT_0, 0x00000010);
+
+	mmio_write_32(DBSC_DBPDLK_0, 0x0000A55A);
+	mmio_write_32(DBSC_DBCMD, 0x01000001);
+	mmio_write_32(DBSC_DBCMD, 0x08000000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x80010000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000008);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000B8000);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x04058904);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000091);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BB6D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000095);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BB6B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000099);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0007BB6D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x04058900);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000021);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0024641E);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010073);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0C058900);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000090);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x04058900);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000003);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0780C700);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000007);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(30)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000004);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x08C0C170);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000022);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x1000040B);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000023);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x2D9C0B66);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000024);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x2A88C400);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000025);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x30005200);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000026);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0014A9C9);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000027);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00000D70);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000028);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00000004);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000029);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00000018);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000002C);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x81003047);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000020);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00181884);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000001A);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x13C03C10);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E7);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E8);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E9);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000107);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000108);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0D0D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000109);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x000D0D0D);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010181);
+	mmio_write_32(DBSC_DBCMD, 0x08000001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010601);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	for (i = 0; i < 4; i++) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB1 + i * 0x20);
+		r5 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFF00) >> 8;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB4 + i * 0x20);
+		r6 = mmio_read_32(DBSC_DBPDRGD_0) & 0xFF;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB3 + i * 0x20);
+		r7 = mmio_read_32(DBSC_DBPDRGD_0) & 0x7;
+
+		if (r6 > 0) {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8);
+
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, ((r7 + 1) & 0x7) | r2);
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00);
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | r6);
+		} else {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8);
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 | r7);
+
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00);
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 |
+						     (((r5 << 1) + r6) & 0xFF));
+		}
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000005);
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC1AA00A0);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010801);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000005);
+	mmio_write_32(DBSC_DBPDRGD_0, 0xC1AA00B8);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0001F001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C000285);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x0000002C);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x81003087);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00010401);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	for (i = 0; i < 4; i++) {
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB1 + i * 0x20);
+		r5 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFF00) >> 8;
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB4 + i * 0x20);
+		r6 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFF);
+
+		mmio_write_32(DBSC_DBPDRGA_0, 0xB3 + i * 0x20);
+		r7 = (mmio_read_32(DBSC_DBPDRGD_0) & 0x7);
+		r12 = (r5 >> 2);
+		if (r6 - r12 > 0) {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8);
+
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, ((r7 + 1) & 0x7) | r2);
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00);
+
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, ((r6 - r12) & 0xFF) | r2);
+		} else {
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			r2 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFFF8);
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB2 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, (r7 & 0x7) | r2);
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			r2 = (mmio_read_32(DBSC_DBPDRGD_0) & 0xFFFFFF00);
+			mmio_write_32(DBSC_DBPDRGA_0, 0xB0 + i * 0x20);
+			mmio_write_32(DBSC_DBPDRGD_0, r2 |
+						     ((r6 + r5 +
+						      (r5 >> 1) + r12) & 0xFF));
+		}
+	}
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000A0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000C0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x000000E0);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000100);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x7C0002C5);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000001);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x00015001);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000006);
+	while (!(mmio_read_32(DBSC_DBPDRGD_0) & BIT(0)))
+		;
+
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000003);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0380C700);
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000007);
+	while (mmio_read_32(DBSC_DBPDRGD_0) & BIT(30))
+		;
+	mmio_write_32(DBSC_DBPDRGA_0, 0x00000021);
+	mmio_write_32(DBSC_DBPDRGD_0, 0x0024643E);
+
+	mmio_write_32(DBSC_DBBUS0CNF1, 0x00000000);
+	mmio_write_32(DBSC_DBBUS0CNF0, 0x00010001);
+	mmio_write_32(DBSC_DBCALCNF, 0x0100200E);
+	mmio_write_32(DBSC_DBRFCNF1, 0x00081860);
+	mmio_write_32(DBSC_DBRFCNF2, 0x00010000);
+	mmio_write_32(DBSC_DBDFICUPDCNF, 0x40100001);
+	mmio_write_32(DBSC_DBRFEN, 0x00000001);
+	mmio_write_32(DBSC_DBACEN, 0x00000001);
+	mmio_write_32(DBSC_DBPDLK_0, 0x00000000);
+	mmio_write_32(0xE67F0024, 0x00000001);
+	mmio_write_32(DBSC_DBSYSCNT0, 0x00000000);
+
+	return INITDRAM_OK;
+}
+
+int32_t rcar_dram_init(void)
+{
+	return init_ddr_v3m_1600();
+}
diff --git a/drivers/renesas/common/ddr/ddr_b/boot_init_dram.c b/drivers/renesas/common/ddr/ddr_b/boot_init_dram.c
new file mode 100644
index 0000000..8d002de
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_b/boot_init_dram.c
@@ -0,0 +1,4484 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation.
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "ddr_regdef.h"
+#include "init_dram_tbl_h3.h"
+#include "init_dram_tbl_m3.h"
+#include "init_dram_tbl_h3ver2.h"
+#include "init_dram_tbl_m3n.h"
+#include "boot_init_dram_regdef.h"
+#include "boot_init_dram.h"
+#include "dram_sub_func.h"
+#include "micro_delay.h"
+#include "rcar_def.h"
+
+#define DDR_BACKUPMODE
+#define FATAL_MSG(x) NOTICE(x)
+
+/* variables */
+#ifdef RCAR_DDR_FIXED_LSI_TYPE
+#ifndef RCAR_AUTO
+#define RCAR_AUTO	99
+#define RCAR_H3		0
+#define RCAR_M3		1
+#define RCAR_M3N	2
+#define RCAR_E3		3	/* NON */
+#define RCAR_H3N	4
+
+#define RZ_G2M		100U
+#define RZ_G2H		101U
+#define RZ_G2N		102U
+
+#define RCAR_CUT_10	0
+#define RCAR_CUT_11	1
+#define RCAR_CUT_20	10
+#define RCAR_CUT_30	20
+#endif
+#ifndef RCAR_LSI
+#define RCAR_LSI	RCAR_AUTO
+#endif
+
+#if (RCAR_LSI == RCAR_AUTO)
+static uint32_t prr_product;
+static uint32_t prr_cut;
+#else
+#if (RCAR_LSI == RCAR_H3)
+static const uint32_t prr_product = PRR_PRODUCT_H3;
+#elif(RCAR_LSI == RCAR_M3 || RCAR_LSI == RZ_G2M)
+static const uint32_t prr_product = PRR_PRODUCT_M3;
+#elif(RCAR_LSI == RCAR_M3N || RCAR_LSI == RZ_G2N)
+static const uint32_t prr_product = PRR_PRODUCT_M3N;
+#elif(RCAR_LSI == RCAR_H3N || RCAR_LSI == RZ_G2H)
+static const uint32_t prr_product = PRR_PRODUCT_H3;
+#endif /* RCAR_LSI */
+
+#ifndef RCAR_LSI_CUT
+static uint32_t prr_cut;
+#else /* RCAR_LSI_CUT */
+#if (RCAR_LSI_CUT == RCAR_CUT_10)
+static const uint32_t prr_cut = PRR_PRODUCT_10;
+#elif(RCAR_LSI_CUT == RCAR_CUT_11)
+static const uint32_t prr_cut = PRR_PRODUCT_11;
+#elif(RCAR_LSI_CUT == RCAR_CUT_20)
+static const uint32_t prr_cut = PRR_PRODUCT_20;
+#elif(RCAR_LSI_CUT == RCAR_CUT_30)
+static const uint32_t prr_cut = PRR_PRODUCT_30;
+#endif /* RCAR_LSI_CUT */
+#endif /* RCAR_LSI_CUT */
+#endif /* RCAR_AUTO_NON */
+#else /* RCAR_DDR_FIXED_LSI_TYPE */
+static uint32_t prr_product;
+static uint32_t prr_cut;
+#endif /* RCAR_DDR_FIXED_LSI_TYPE */
+
+static const uint32_t *p_ddr_regdef_tbl;
+static uint32_t brd_clk;
+static uint32_t brd_clkdiv;
+static uint32_t brd_clkdiva;
+static uint32_t ddr_mbps;
+static uint32_t ddr_mbpsdiv;
+static uint32_t ddr_tccd;
+static uint32_t ddr_phycaslice;
+static const struct _boardcnf *board_cnf;
+static uint32_t ddr_phyvalid;
+static uint32_t ddr_density[DRAM_CH_CNT][CS_CNT];
+static uint32_t ch_have_this_cs[CS_CNT] __aligned(64);
+static uint32_t rdqdm_dly[DRAM_CH_CNT][CSAB_CNT][SLICE_CNT * 2][9];
+static uint32_t max_density;
+static uint32_t ddr0800_mul;
+static uint32_t ddr_mul;
+static uint32_t DDR_PHY_SLICE_REGSET_OFS;
+static uint32_t DDR_PHY_ADR_V_REGSET_OFS;
+static uint32_t DDR_PHY_ADR_I_REGSET_OFS;
+static uint32_t DDR_PHY_ADR_G_REGSET_OFS;
+static uint32_t DDR_PI_REGSET_OFS;
+static uint32_t DDR_PHY_SLICE_REGSET_SIZE;
+static uint32_t DDR_PHY_ADR_V_REGSET_SIZE;
+static uint32_t DDR_PHY_ADR_I_REGSET_SIZE;
+static uint32_t DDR_PHY_ADR_G_REGSET_SIZE;
+static uint32_t DDR_PI_REGSET_SIZE;
+static uint32_t DDR_PHY_SLICE_REGSET_NUM;
+static uint32_t DDR_PHY_ADR_V_REGSET_NUM;
+static uint32_t DDR_PHY_ADR_I_REGSET_NUM;
+static uint32_t DDR_PHY_ADR_G_REGSET_NUM;
+static uint32_t DDR_PI_REGSET_NUM;
+static uint32_t DDR_PHY_ADR_I_NUM;
+#define DDR_PHY_REGSET_MAX 128
+#define DDR_PI_REGSET_MAX 320
+static uint32_t _cnf_DDR_PHY_SLICE_REGSET[DDR_PHY_REGSET_MAX];
+static uint32_t _cnf_DDR_PHY_ADR_V_REGSET[DDR_PHY_REGSET_MAX];
+static uint32_t _cnf_DDR_PHY_ADR_I_REGSET[DDR_PHY_REGSET_MAX];
+static uint32_t _cnf_DDR_PHY_ADR_G_REGSET[DDR_PHY_REGSET_MAX];
+static uint32_t _cnf_DDR_PI_REGSET[DDR_PI_REGSET_MAX];
+static uint32_t pll3_mode;
+static uint32_t loop_max;
+#ifdef DDR_BACKUPMODE
+uint32_t ddr_backup;
+/* #define DDR_BACKUPMODE_HALF           //for Half channel(ch0,1 only) */
+#endif
+
+#ifdef ddr_qos_init_setting	/*  only for non qos_init */
+#define OPERATING_FREQ			(400U)	/* Mhz */
+#define BASE_SUB_SLOT_NUM		(0x6U)
+#define SUB_SLOT_CYCLE			(0x7EU)	/* 126 */
+#define QOSWT_WTSET0_CYCLE		\
+	((SUB_SLOT_CYCLE * BASE_SUB_SLOT_NUM * 1000U) / \
+	OPERATING_FREQ)	/* unit:ns */
+
+uint32_t get_refperiod(void)
+{
+	return QOSWT_WTSET0_CYCLE;
+}
+#else /*  ddr_qos_init_setting // only for non qos_init */
+extern uint32_t get_refperiod(void);
+#endif /* ddr_qos_init_setting // only for non qos_init */
+
+#define _reg_PHY_RX_CAL_X_NUM 11
+static const uint32_t _reg_PHY_RX_CAL_X[_reg_PHY_RX_CAL_X_NUM] = {
+	_reg_PHY_RX_CAL_DQ0,
+	_reg_PHY_RX_CAL_DQ1,
+	_reg_PHY_RX_CAL_DQ2,
+	_reg_PHY_RX_CAL_DQ3,
+	_reg_PHY_RX_CAL_DQ4,
+	_reg_PHY_RX_CAL_DQ5,
+	_reg_PHY_RX_CAL_DQ6,
+	_reg_PHY_RX_CAL_DQ7,
+	_reg_PHY_RX_CAL_DM,
+	_reg_PHY_RX_CAL_DQS,
+	_reg_PHY_RX_CAL_FDBK
+};
+
+#define _reg_PHY_CLK_WRX_SLAVE_DELAY_NUM 10
+static const uint32_t _reg_PHY_CLK_WRX_SLAVE_DELAY
+	[_reg_PHY_CLK_WRX_SLAVE_DELAY_NUM] = {
+	_reg_PHY_CLK_WRDQ0_SLAVE_DELAY,
+	_reg_PHY_CLK_WRDQ1_SLAVE_DELAY,
+	_reg_PHY_CLK_WRDQ2_SLAVE_DELAY,
+	_reg_PHY_CLK_WRDQ3_SLAVE_DELAY,
+	_reg_PHY_CLK_WRDQ4_SLAVE_DELAY,
+	_reg_PHY_CLK_WRDQ5_SLAVE_DELAY,
+	_reg_PHY_CLK_WRDQ6_SLAVE_DELAY,
+	_reg_PHY_CLK_WRDQ7_SLAVE_DELAY,
+	_reg_PHY_CLK_WRDM_SLAVE_DELAY,
+	_reg_PHY_CLK_WRDQS_SLAVE_DELAY
+};
+
+#define _reg_PHY_RDDQS_X_FALL_SLAVE_DELAY_NUM 9
+static const uint32_t _reg_PHY_RDDQS_X_FALL_SLAVE_DELAY
+	[_reg_PHY_RDDQS_X_FALL_SLAVE_DELAY_NUM] = {
+	_reg_PHY_RDDQS_DQ0_FALL_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ1_FALL_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ2_FALL_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ3_FALL_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ4_FALL_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ5_FALL_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ6_FALL_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ7_FALL_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DM_FALL_SLAVE_DELAY
+};
+
+#define _reg_PHY_RDDQS_X_RISE_SLAVE_DELAY_NUM 9
+static const uint32_t _reg_PHY_RDDQS_X_RISE_SLAVE_DELAY
+	[_reg_PHY_RDDQS_X_RISE_SLAVE_DELAY_NUM] = {
+	_reg_PHY_RDDQS_DQ0_RISE_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ1_RISE_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ2_RISE_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ3_RISE_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ4_RISE_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ5_RISE_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ6_RISE_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DQ7_RISE_SLAVE_DELAY,
+	_reg_PHY_RDDQS_DM_RISE_SLAVE_DELAY
+};
+
+#define _reg_PHY_PAD_TERM_X_NUM 8
+static const uint32_t _reg_PHY_PAD_TERM_X[_reg_PHY_PAD_TERM_X_NUM] = {
+	_reg_PHY_PAD_FDBK_TERM,
+	_reg_PHY_PAD_DATA_TERM,
+	_reg_PHY_PAD_DQS_TERM,
+	_reg_PHY_PAD_ADDR_TERM,
+	_reg_PHY_PAD_CLK_TERM,
+	_reg_PHY_PAD_CKE_TERM,
+	_reg_PHY_PAD_RST_TERM,
+	_reg_PHY_PAD_CS_TERM
+};
+
+#define _reg_PHY_CLK_CACS_SLAVE_DELAY_X_NUM 10
+static const uint32_t _reg_PHY_CLK_CACS_SLAVE_DELAY_X
+	[_reg_PHY_CLK_CACS_SLAVE_DELAY_X_NUM] = {
+	_reg_PHY_ADR0_CLK_WR_SLAVE_DELAY,
+	_reg_PHY_ADR1_CLK_WR_SLAVE_DELAY,
+	_reg_PHY_ADR2_CLK_WR_SLAVE_DELAY,
+	_reg_PHY_ADR3_CLK_WR_SLAVE_DELAY,
+	_reg_PHY_ADR4_CLK_WR_SLAVE_DELAY,
+	_reg_PHY_ADR5_CLK_WR_SLAVE_DELAY,
+
+	_reg_PHY_GRP_SLAVE_DELAY_0,
+	_reg_PHY_GRP_SLAVE_DELAY_1,
+	_reg_PHY_GRP_SLAVE_DELAY_2,
+	_reg_PHY_GRP_SLAVE_DELAY_3
+};
+
+/* Prototypes */
+static inline uint32_t vch_nxt(uint32_t pos);
+static void cpg_write_32(uint32_t a, uint32_t v);
+static void pll3_control(uint32_t high);
+static inline void dsb_sev(void);
+static void wait_dbcmd(void);
+static void send_dbcmd(uint32_t cmd);
+static uint32_t reg_ddrphy_read(uint32_t phyno, uint32_t regadd);
+static void reg_ddrphy_write(uint32_t phyno, uint32_t regadd, uint32_t regdata);
+static void reg_ddrphy_write_a(uint32_t regadd, uint32_t regdata);
+static inline uint32_t ddr_regdef(uint32_t _regdef);
+static inline uint32_t ddr_regdef_adr(uint32_t _regdef);
+static inline uint32_t ddr_regdef_lsb(uint32_t _regdef);
+static void ddr_setval_s(uint32_t ch, uint32_t slice, uint32_t _regdef,
+			 uint32_t val);
+static uint32_t ddr_getval_s(uint32_t ch, uint32_t slice, uint32_t _regdef);
+static void ddr_setval(uint32_t ch, uint32_t regdef, uint32_t val);
+static void ddr_setval_ach_s(uint32_t slice, uint32_t regdef, uint32_t val);
+static void ddr_setval_ach(uint32_t regdef, uint32_t val);
+static void ddr_setval_ach_as(uint32_t regdef, uint32_t val);
+static uint32_t ddr_getval(uint32_t ch, uint32_t regdef);
+static uint32_t ddr_getval_ach(uint32_t regdef, uint32_t *p);
+static uint32_t ddr_getval_ach_as(uint32_t regdef, uint32_t *p);
+static void _tblcopy(uint32_t *to, const uint32_t *from, uint32_t size);
+static void ddrtbl_setval(uint32_t *tbl, uint32_t _regdef, uint32_t val);
+static uint32_t ddrtbl_getval(uint32_t *tbl, uint32_t _regdef);
+static uint32_t ddrphy_regif_chk(void);
+static inline void ddrphy_regif_idle(void);
+static uint16_t _f_scale(uint32_t _ddr_mbps, uint32_t _ddr_mbpsdiv, uint32_t ps,
+			 uint16_t cyc);
+static void _f_scale_js2(uint32_t _ddr_mbps, uint32_t _ddr_mbpsdiv,
+			 uint16_t *_js2);
+static int16_t _f_scale_adj(int16_t ps);
+static void ddrtbl_load(void);
+static void ddr_config_sub(void);
+static void get_ca_swizzle(uint32_t ch, uint32_t ddr_csn, uint32_t *p_swz);
+static void ddr_config_sub_h3v1x(void);
+static void ddr_config(void);
+static void dbsc_regset(void);
+static void dbsc_regset_post(void);
+static uint32_t dfi_init_start(void);
+static void change_lpddr4_en(uint32_t mode);
+static uint32_t set_term_code(void);
+static void ddr_register_set(void);
+static inline uint32_t wait_freqchgreq(uint32_t assert);
+static inline void set_freqchgack(uint32_t assert);
+static inline void set_dfifrequency(uint32_t freq);
+static uint32_t pll3_freq(uint32_t on);
+static void update_dly(void);
+static uint32_t pi_training_go(void);
+static uint32_t init_ddr(void);
+static uint32_t swlvl1(uint32_t ddr_csn, uint32_t reg_cs, uint32_t reg_kick);
+static uint32_t wdqdm_man1(void);
+static uint32_t wdqdm_man(void);
+static uint32_t rdqdm_man1(void);
+static uint32_t rdqdm_man(void);
+
+static int32_t _find_change(uint64_t val, uint32_t dir);
+static uint32_t _rx_offset_cal_updn(uint32_t code);
+static uint32_t rx_offset_cal(void);
+static uint32_t rx_offset_cal_hw(void);
+static void adjust_rddqs_latency(void);
+static void adjust_wpath_latency(void);
+
+struct ddrt_data {
+	int32_t init_temp;	/* Initial Temperature (do) */
+	uint32_t init_cal[4];	/* Initial io-code (4 is for H3) */
+	uint32_t tcomp_cal[4];	/* Temp. compensated io-code (4 is for H3) */
+};
+
+static struct ddrt_data tcal;
+
+static void pvtcode_update(void);
+static void pvtcode_update2(void);
+static void ddr_padcal_tcompensate_getinit(uint32_t override);
+
+/* load board configuration */
+#include "boot_init_dram_config.c"
+
+#ifndef DDR_FAST_INIT
+static uint32_t rdqdm_le[DRAM_CH_CNT][CS_CNT][SLICE_CNT * 2][9];
+static uint32_t rdqdm_te[DRAM_CH_CNT][CS_CNT][SLICE_CNT * 2][9];
+static uint32_t rdqdm_nw[DRAM_CH_CNT][CS_CNT][SLICE_CNT * 2][9];
+static uint32_t rdqdm_win[DRAM_CH_CNT][CS_CNT][SLICE_CNT];
+static uint32_t rdqdm_st[DRAM_CH_CNT][CS_CNT][SLICE_CNT * 2];
+static void rdqdm_clr1(uint32_t ch, uint32_t ddr_csn);
+static uint32_t rdqdm_ana1(uint32_t ch, uint32_t ddr_csn);
+
+static uint32_t wdqdm_le[DRAM_CH_CNT][CS_CNT][SLICE_CNT][9];
+static uint32_t wdqdm_te[DRAM_CH_CNT][CS_CNT][SLICE_CNT][9];
+static uint32_t wdqdm_dly[DRAM_CH_CNT][CS_CNT][SLICE_CNT][9];
+static uint32_t wdqdm_st[DRAM_CH_CNT][CS_CNT][SLICE_CNT];
+static uint32_t wdqdm_win[DRAM_CH_CNT][CS_CNT][SLICE_CNT];
+static void wdqdm_clr1(uint32_t ch, uint32_t ddr_csn);
+static uint32_t wdqdm_ana1(uint32_t ch, uint32_t ddr_csn);
+#endif/* DDR_FAST_INIT */
+
+/* macro for channel selection loop */
+static inline uint32_t vch_nxt(uint32_t pos)
+{
+	uint32_t posn;
+
+	for (posn = pos; posn < DRAM_CH_CNT; posn++) {
+		if (ddr_phyvalid & (1U << posn))
+			break;
+	}
+	return posn;
+}
+
+#define foreach_vch(ch) \
+for (ch = vch_nxt(0); ch < DRAM_CH_CNT; ch = vch_nxt(ch + 1))
+
+#define foreach_ech(ch) \
+for (ch = 0; ch < DRAM_CH_CNT; ch++)
+
+/* Printing functions */
+#define MSG_LF(...)
+
+/* clock settings, reset control */
+static void cpg_write_32(uint32_t a, uint32_t v)
+{
+	mmio_write_32(CPG_CPGWPR, ~v);
+	mmio_write_32(a, v);
+}
+
+static void pll3_control(uint32_t high)
+{
+	uint32_t data_l, data_div, data_mul, tmp_div;
+
+	if (high) {
+		tmp_div = 3999 * brd_clkdiv * (brd_clkdiva + 1) /
+			(brd_clk * ddr_mul) / 2;
+		data_mul = ((ddr_mul * tmp_div) - 1) << 24;
+		pll3_mode = 1;
+		loop_max = 2;
+	} else {
+		tmp_div = 3999 * brd_clkdiv * (brd_clkdiva + 1) /
+			(brd_clk * ddr0800_mul) / 2;
+		data_mul = ((ddr0800_mul * tmp_div) - 1) << 24;
+		pll3_mode = 0;
+		loop_max = 8;
+	}
+
+	switch (tmp_div) {
+	case 1:
+		data_div = 0;
+		break;
+	case 2:
+	case 3:
+	case 4:
+		data_div = tmp_div;
+		break;
+	default:
+		data_div = 6;
+		data_mul = (data_mul * tmp_div) / 3;
+		break;
+	}
+	data_mul = data_mul | (brd_clkdiva << 7);
+
+	/* PLL3 disable */
+	data_l = mmio_read_32(CPG_PLLECR) & ~CPG_PLLECR_PLL3E_BIT;
+	cpg_write_32(CPG_PLLECR, data_l);
+	dsb_sev();
+
+	if ((prr_product == PRR_PRODUCT_M3) ||
+	    ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_20))) {
+		/* PLL3 DIV resetting(Lowest value:3) */
+		data_l = 0x00030003 | (0xFF80FF80 & mmio_read_32(CPG_FRQCRD));
+		cpg_write_32(CPG_FRQCRD, data_l);
+		dsb_sev();
+
+		/* zb3 clk stop */
+		data_l = CPG_ZB3CKCR_ZB3ST_BIT | mmio_read_32(CPG_ZB3CKCR);
+		cpg_write_32(CPG_ZB3CKCR, data_l);
+		dsb_sev();
+
+		/* PLL3 enable */
+		data_l = CPG_PLLECR_PLL3E_BIT | mmio_read_32(CPG_PLLECR);
+		cpg_write_32(CPG_PLLECR, data_l);
+		dsb_sev();
+
+		do {
+			data_l = mmio_read_32(CPG_PLLECR);
+		} while ((data_l & CPG_PLLECR_PLL3ST_BIT) == 0);
+		dsb_sev();
+
+		/* PLL3 DIV resetting (Highest value:0) */
+		data_l = (0xFF80FF80 & mmio_read_32(CPG_FRQCRD));
+		cpg_write_32(CPG_FRQCRD, data_l);
+		dsb_sev();
+
+		/* DIV SET KICK */
+		data_l = CPG_FRQCRB_KICK_BIT | mmio_read_32(CPG_FRQCRB);
+		cpg_write_32(CPG_FRQCRB, data_l);
+		dsb_sev();
+
+		/* PLL3 multiplie set */
+		cpg_write_32(CPG_PLL3CR, data_mul);
+		dsb_sev();
+
+		do {
+			data_l = mmio_read_32(CPG_PLLECR);
+		} while ((data_l & CPG_PLLECR_PLL3ST_BIT) == 0);
+		dsb_sev();
+
+		/* PLL3 DIV resetting(Target value) */
+		data_l = (data_div << 16) | data_div |
+			 (mmio_read_32(CPG_FRQCRD) & 0xFF80FF80);
+		cpg_write_32(CPG_FRQCRD, data_l);
+		dsb_sev();
+
+		/* DIV SET KICK */
+		data_l = CPG_FRQCRB_KICK_BIT | mmio_read_32(CPG_FRQCRB);
+		cpg_write_32(CPG_FRQCRB, data_l);
+		dsb_sev();
+
+		do {
+			data_l = mmio_read_32(CPG_PLLECR);
+		} while ((data_l & CPG_PLLECR_PLL3ST_BIT) == 0);
+		dsb_sev();
+
+		/* zb3 clk start */
+		data_l = (~CPG_ZB3CKCR_ZB3ST_BIT) & mmio_read_32(CPG_ZB3CKCR);
+		cpg_write_32(CPG_ZB3CKCR, data_l);
+		dsb_sev();
+
+	} else { /*  H3Ver.3.0/M3N/V3H */
+
+		/* PLL3 multiplie set */
+		cpg_write_32(CPG_PLL3CR, data_mul);
+		dsb_sev();
+
+		/* PLL3 DIV set(Target value) */
+		data_l = (data_div << 16) | data_div |
+			 (mmio_read_32(CPG_FRQCRD) & 0xFF80FF80);
+		cpg_write_32(CPG_FRQCRD, data_l);
+
+		/* DIV SET KICK */
+		data_l = CPG_FRQCRB_KICK_BIT | mmio_read_32(CPG_FRQCRB);
+		cpg_write_32(CPG_FRQCRB, data_l);
+		dsb_sev();
+
+		/* PLL3 enable */
+		data_l = CPG_PLLECR_PLL3E_BIT | mmio_read_32(CPG_PLLECR);
+		cpg_write_32(CPG_PLLECR, data_l);
+		dsb_sev();
+
+		do {
+			data_l = mmio_read_32(CPG_PLLECR);
+		} while ((data_l & CPG_PLLECR_PLL3ST_BIT) == 0);
+		dsb_sev();
+	}
+}
+
+/* barrier */
+static inline void dsb_sev(void)
+{
+	__asm__ __volatile__("dsb sy");
+}
+
+/* DDR memory register access */
+static void wait_dbcmd(void)
+{
+	uint32_t data_l;
+	/* dummy read */
+	data_l = mmio_read_32(DBSC_DBCMD);
+	dsb_sev();
+	while (1) {
+		/* wait DBCMD 1=busy, 0=ready */
+		data_l = mmio_read_32(DBSC_DBWAIT);
+		dsb_sev();
+		if ((data_l & 0x00000001) == 0x00)
+			break;
+	}
+}
+
+static void send_dbcmd(uint32_t cmd)
+{
+	/* dummy read */
+	wait_dbcmd();
+	mmio_write_32(DBSC_DBCMD, cmd);
+	dsb_sev();
+}
+
+static void dbwait_loop(uint32_t wait_loop)
+{
+	uint32_t i;
+
+	for (i = 0; i < wait_loop; i++)
+		wait_dbcmd();
+}
+
+/* DDRPHY register access (raw) */
+static uint32_t reg_ddrphy_read(uint32_t phyno, uint32_t regadd)
+{
+	uint32_t val;
+	uint32_t loop;
+
+	val = 0;
+	if ((prr_product != PRR_PRODUCT_M3N) &&
+	    (prr_product != PRR_PRODUCT_V3H)) {
+		mmio_write_32(DBSC_DBPDRGA(phyno), regadd);
+		dsb_sev();
+
+		while (mmio_read_32(DBSC_DBPDRGA(phyno)) != regadd) {
+			dsb_sev();
+		}
+		dsb_sev();
+
+		for (loop = 0; loop < loop_max; loop++) {
+			val = mmio_read_32(DBSC_DBPDRGD(phyno));
+			dsb_sev();
+		}
+		(void)val;
+	} else {
+		mmio_write_32(DBSC_DBPDRGA(phyno), regadd | 0x00004000);
+		dsb_sev();
+		while (mmio_read_32(DBSC_DBPDRGA(phyno)) !=
+		       (regadd | 0x0000C000)) {
+			dsb_sev();
+		};
+		val = mmio_read_32(DBSC_DBPDRGA(phyno));
+		mmio_write_32(DBSC_DBPDRGA(phyno), regadd | 0x00008000);
+		while (mmio_read_32(DBSC_DBPDRGA(phyno)) != regadd) {
+			dsb_sev();
+		};
+		dsb_sev();
+
+		mmio_write_32(DBSC_DBPDRGA(phyno), regadd | 0x00008000);
+		while (mmio_read_32(DBSC_DBPDRGA(phyno)) != regadd) {
+			dsb_sev();
+		};
+
+		dsb_sev();
+		val = mmio_read_32(DBSC_DBPDRGD(phyno));
+		dsb_sev();
+		(void)val;
+	}
+	return val;
+}
+
+static void reg_ddrphy_write(uint32_t phyno, uint32_t regadd, uint32_t regdata)
+{
+	uint32_t val;
+	uint32_t loop;
+
+	if ((prr_product != PRR_PRODUCT_M3N) &&
+	    (prr_product != PRR_PRODUCT_V3H)) {
+		mmio_write_32(DBSC_DBPDRGA(phyno), regadd);
+		dsb_sev();
+		for (loop = 0; loop < loop_max; loop++) {
+			val = mmio_read_32(DBSC_DBPDRGA(phyno));
+			dsb_sev();
+		}
+		mmio_write_32(DBSC_DBPDRGD(phyno), regdata);
+		dsb_sev();
+
+		for (loop = 0; loop < loop_max; loop++) {
+			val = mmio_read_32(DBSC_DBPDRGD(phyno));
+			dsb_sev();
+		}
+	} else {
+		mmio_write_32(DBSC_DBPDRGA(phyno), regadd);
+		dsb_sev();
+
+		while (mmio_read_32(DBSC_DBPDRGA(phyno)) != regadd) {
+			dsb_sev();
+		};
+		dsb_sev();
+
+		mmio_write_32(DBSC_DBPDRGD(phyno), regdata);
+		dsb_sev();
+
+		while (mmio_read_32(DBSC_DBPDRGA(phyno)) !=
+		       (regadd | 0x00008000)) {
+			dsb_sev();
+		};
+		mmio_write_32(DBSC_DBPDRGA(phyno), regadd | 0x00008000);
+
+		while (mmio_read_32(DBSC_DBPDRGA(phyno)) != regadd) {
+			dsb_sev();
+		};
+		dsb_sev();
+
+		mmio_write_32(DBSC_DBPDRGA(phyno), regadd);
+	}
+	(void)val;
+}
+
+static void reg_ddrphy_write_a(uint32_t regadd, uint32_t regdata)
+{
+	uint32_t ch;
+	uint32_t val;
+	uint32_t loop;
+
+	if ((prr_product != PRR_PRODUCT_M3N) &&
+	    (prr_product != PRR_PRODUCT_V3H)) {
+		foreach_vch(ch) {
+			mmio_write_32(DBSC_DBPDRGA(ch), regadd);
+			dsb_sev();
+		}
+
+		foreach_vch(ch) {
+			mmio_write_32(DBSC_DBPDRGD(ch), regdata);
+			dsb_sev();
+		}
+
+		for (loop = 0; loop < loop_max; loop++) {
+			val = mmio_read_32(DBSC_DBPDRGD(0));
+			dsb_sev();
+		}
+		(void)val;
+	} else {
+		foreach_vch(ch) {
+			reg_ddrphy_write(ch, regadd, regdata);
+			dsb_sev();
+		}
+	}
+}
+
+static inline void ddrphy_regif_idle(void)
+{
+	uint32_t val;
+
+	val = reg_ddrphy_read(0, ddr_regdef_adr(_reg_PI_INT_STATUS));
+	dsb_sev();
+	(void)val;
+}
+
+/* DDRPHY register access (field modify) */
+static inline uint32_t ddr_regdef(uint32_t _regdef)
+{
+	return p_ddr_regdef_tbl[_regdef];
+}
+
+static inline uint32_t ddr_regdef_adr(uint32_t _regdef)
+{
+	return DDR_REGDEF_ADR(p_ddr_regdef_tbl[_regdef]);
+}
+
+static inline uint32_t ddr_regdef_lsb(uint32_t _regdef)
+{
+	return DDR_REGDEF_LSB(p_ddr_regdef_tbl[_regdef]);
+}
+
+static void ddr_setval_s(uint32_t ch, uint32_t slice, uint32_t _regdef,
+			 uint32_t val)
+{
+	uint32_t adr;
+	uint32_t lsb;
+	uint32_t len;
+	uint32_t msk;
+	uint32_t tmp;
+	uint32_t regdef;
+
+	regdef = ddr_regdef(_regdef);
+	adr = DDR_REGDEF_ADR(regdef) + 0x80 * slice;
+	len = DDR_REGDEF_LEN(regdef);
+	lsb = DDR_REGDEF_LSB(regdef);
+	if (len == 0x20)
+		msk = 0xffffffff;
+	else
+		msk = ((1U << len) - 1) << lsb;
+
+	tmp = reg_ddrphy_read(ch, adr);
+	tmp = (tmp & (~msk)) | ((val << lsb) & msk);
+	reg_ddrphy_write(ch, adr, tmp);
+}
+
+static uint32_t ddr_getval_s(uint32_t ch, uint32_t slice, uint32_t _regdef)
+{
+	uint32_t adr;
+	uint32_t lsb;
+	uint32_t len;
+	uint32_t msk;
+	uint32_t tmp;
+	uint32_t regdef;
+
+	regdef = ddr_regdef(_regdef);
+	adr = DDR_REGDEF_ADR(regdef) + 0x80 * slice;
+	len = DDR_REGDEF_LEN(regdef);
+	lsb = DDR_REGDEF_LSB(regdef);
+	if (len == 0x20)
+		msk = 0xffffffff;
+	else
+		msk = ((1U << len) - 1);
+
+	tmp = reg_ddrphy_read(ch, adr);
+	tmp = (tmp >> lsb) & msk;
+
+	return tmp;
+}
+
+static void ddr_setval(uint32_t ch, uint32_t regdef, uint32_t val)
+{
+	ddr_setval_s(ch, 0, regdef, val);
+}
+
+static void ddr_setval_ach_s(uint32_t slice, uint32_t regdef, uint32_t val)
+{
+	uint32_t ch;
+
+	foreach_vch(ch)
+	    ddr_setval_s(ch, slice, regdef, val);
+}
+
+static void ddr_setval_ach(uint32_t regdef, uint32_t val)
+{
+	ddr_setval_ach_s(0, regdef, val);
+}
+
+static void ddr_setval_ach_as(uint32_t regdef, uint32_t val)
+{
+	uint32_t slice;
+
+	for (slice = 0; slice < SLICE_CNT; slice++)
+		ddr_setval_ach_s(slice, regdef, val);
+}
+
+static uint32_t ddr_getval(uint32_t ch, uint32_t regdef)
+{
+	return ddr_getval_s(ch, 0, regdef);
+}
+
+static uint32_t ddr_getval_ach(uint32_t regdef, uint32_t *p)
+{
+	uint32_t ch;
+
+	foreach_vch(ch)
+	    p[ch] = ddr_getval_s(ch, 0, regdef);
+	return p[0];
+}
+
+static uint32_t ddr_getval_ach_as(uint32_t regdef, uint32_t *p)
+{
+	uint32_t ch, slice;
+	uint32_t *pp;
+
+	pp = p;
+	foreach_vch(ch)
+		for (slice = 0; slice < SLICE_CNT; slice++)
+			*pp++ = ddr_getval_s(ch, slice, regdef);
+	return p[0];
+}
+
+/* handling functions for setteing ddrphy value table */
+static void _tblcopy(uint32_t *to, const uint32_t *from, uint32_t size)
+{
+	uint32_t i;
+
+	for (i = 0; i < size; i++) {
+		to[i] = from[i];
+	}
+}
+
+static void ddrtbl_setval(uint32_t *tbl, uint32_t _regdef, uint32_t val)
+{
+	uint32_t adr;
+	uint32_t lsb;
+	uint32_t len;
+	uint32_t msk;
+	uint32_t tmp;
+	uint32_t adrmsk;
+	uint32_t regdef;
+
+	regdef = ddr_regdef(_regdef);
+	adr = DDR_REGDEF_ADR(regdef);
+	len = DDR_REGDEF_LEN(regdef);
+	lsb = DDR_REGDEF_LSB(regdef);
+	if (len == 0x20)
+		msk = 0xffffffff;
+	else
+		msk = ((1U << len) - 1) << lsb;
+
+	if (adr < 0x400) {
+		adrmsk = 0xff;
+	} else {
+		adrmsk = 0x7f;
+	}
+
+	tmp = tbl[adr & adrmsk];
+	tmp = (tmp & (~msk)) | ((val << lsb) & msk);
+	tbl[adr & adrmsk] = tmp;
+}
+
+static uint32_t ddrtbl_getval(uint32_t *tbl, uint32_t _regdef)
+{
+	uint32_t adr;
+	uint32_t lsb;
+	uint32_t len;
+	uint32_t msk;
+	uint32_t tmp;
+	uint32_t adrmsk;
+	uint32_t regdef;
+
+	regdef = ddr_regdef(_regdef);
+	adr = DDR_REGDEF_ADR(regdef);
+	len = DDR_REGDEF_LEN(regdef);
+	lsb = DDR_REGDEF_LSB(regdef);
+	if (len == 0x20)
+		msk = 0xffffffff;
+	else
+		msk = ((1U << len) - 1);
+
+	if (adr < 0x400) {
+		adrmsk = 0xff;
+	} else {
+		adrmsk = 0x7f;
+	}
+
+	tmp = tbl[adr & adrmsk];
+	tmp = (tmp >> lsb) & msk;
+
+	return tmp;
+}
+
+/* DDRPHY register access handling */
+static uint32_t ddrphy_regif_chk(void)
+{
+	uint32_t tmp_ach[DRAM_CH_CNT];
+	uint32_t ch;
+	uint32_t err;
+	uint32_t PI_VERSION_CODE;
+
+	if (((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) ||
+	    (prr_product == PRR_PRODUCT_M3)) {
+		PI_VERSION_CODE = 0x2041; /* H3 Ver.1.x/M3-W */
+	} else {
+		PI_VERSION_CODE = 0x2040; /* H3 Ver.2.0 or later/M3-N/V3H */
+	}
+
+	ddr_getval_ach(_reg_PI_VERSION, (uint32_t *)tmp_ach);
+	err = 0;
+	foreach_vch(ch) {
+		if (tmp_ach[ch] != PI_VERSION_CODE)
+			err = 1;
+	}
+	return err;
+}
+
+/* functions and parameters for timing setting */
+struct _jedec_spec1 {
+	uint16_t fx3;
+	uint8_t rlwodbi;
+	uint8_t rlwdbi;
+	uint8_t WL;
+	uint8_t nwr;
+	uint8_t nrtp;
+	uint8_t odtlon;
+	uint8_t MR1;
+	uint8_t MR2;
+};
+
+#define JS1_USABLEC_SPEC_LO 2
+#define JS1_USABLEC_SPEC_HI 5
+#define JS1_FREQ_TBL_NUM 8
+#define JS1_MR1(f) (0x04 | ((f) << 4))
+#define JS1_MR2(f) (0x00 | ((f) << 3) | (f))
+const struct _jedec_spec1 js1[JS1_FREQ_TBL_NUM] = {
+	/* 533.333Mbps */
+	{  800,  6,  6,  4,  6,  8, 0, JS1_MR1(0), JS1_MR2(0) | 0x40 },
+	/* 1066.666Mbps */
+	{ 1600, 10, 12,  8, 10,  8, 0, JS1_MR1(1), JS1_MR2(1) | 0x40 },
+	/* 1600.000Mbps */
+	{ 2400, 14, 16, 12, 16,  8, 6, JS1_MR1(2), JS1_MR2(2) | 0x40 },
+	/* 2133.333Mbps */
+	{ 3200, 20, 22, 10, 20,  8, 4, JS1_MR1(3), JS1_MR2(3) },
+	/* 2666.666Mbps */
+	{ 4000, 24, 28, 12, 24, 10, 4, JS1_MR1(4), JS1_MR2(4) },
+	/* 3200.000Mbps */
+	{ 4800, 28, 32, 14, 30, 12, 6, JS1_MR1(5), JS1_MR2(5) },
+	/* 3733.333Mbps */
+	{ 5600, 32, 36, 16, 34, 14, 6, JS1_MR1(6), JS1_MR2(6) },
+	/* 4266.666Mbps */
+	{ 6400, 36, 40, 18, 40, 16, 8, JS1_MR1(7), JS1_MR2(7) }
+};
+
+struct _jedec_spec2 {
+	uint16_t ps;
+	uint16_t cyc;
+};
+
+#define js2_tsr 0
+#define js2_txp 1
+#define js2_trtp 2
+#define js2_trcd 3
+#define js2_trppb 4
+#define js2_trpab 5
+#define js2_tras 6
+#define js2_twr 7
+#define js2_twtr 8
+#define js2_trrd 9
+#define js2_tppd 10
+#define js2_tfaw 11
+#define js2_tdqsck 12
+#define js2_tckehcmd 13
+#define js2_tckelcmd 14
+#define js2_tckelpd 15
+#define js2_tmrr 16
+#define js2_tmrw 17
+#define js2_tmrd 18
+#define js2_tzqcalns 19
+#define js2_tzqlat 20
+#define js2_tiedly 21
+#define js2_tODTon_min 22
+#define JS2_TBLCNT 23
+
+#define js2_trcpb (JS2_TBLCNT)
+#define js2_trcab (JS2_TBLCNT + 1)
+#define js2_trfcab (JS2_TBLCNT + 2)
+#define JS2_CNT (JS2_TBLCNT + 3)
+
+#ifndef JS2_DERATE
+#define JS2_DERATE 0
+#endif
+const struct _jedec_spec2 jedec_spec2[2][JS2_TBLCNT] = {
+	{
+/*tSR   */ {15000, 3},
+/*tXP   */ {7500, 3},
+/*tRTP  */ {7500, 8},
+/*tRCD  */ {18000, 4},
+/*tRPpb */ {18000, 3},
+/*tRPab */ {21000, 3},
+/*tRAS  */ {42000, 3},
+/*tWR   */ {18000, 4},
+/*tWTR  */ {10000, 8},
+/*tRRD  */ {10000, 4},
+/*tPPD  */ {0, 0},
+/*tFAW  */ {40000, 0},
+/*tDQSCK*/ {3500, 0},
+/*tCKEHCMD*/ {7500, 3},
+/*tCKELCMD*/ {7500, 3},
+/*tCKELPD*/ {7500, 3},
+/*tMRR*/ {0, 8},
+/*tMRW*/ {10000, 10},
+/*tMRD*/ {14000, 10},
+/*tZQCALns*/ {1000 * 10, 0},
+/*tZQLAT*/ {30000, 10},
+/*tIEdly*/ {12500, 0},
+/*tODTon_min*/ {1500, 0}
+	 }, {
+/*tSR   */ {15000, 3},
+/*tXP   */ {7500, 3},
+/*tRTP  */ {7500, 8},
+/*tRCD  */ {19875, 4},
+/*tRPpb */ {19875, 3},
+/*tRPab */ {22875, 3},
+/*tRAS  */ {43875, 3},
+/*tWR   */ {18000, 4},
+/*tWTR  */ {10000, 8},
+/*tRRD  */ {11875, 4},
+/*tPPD  */ {0, 0},
+/*tFAW  */ {40000, 0},
+/*tDQSCK*/ {3600, 0},
+/*tCKEHCMD*/ {7500, 3},
+/*tCKELCMD*/ {7500, 3},
+/*tCKELPD*/ {7500, 3},
+/*tMRR*/ {0, 8},
+/*tMRW*/ {10000, 10},
+/*tMRD*/ {14000, 10},
+/*tZQCALns*/ {1000 * 10, 0},
+/*tZQLAT*/ {30000, 10},
+/*tIEdly*/ {12500, 0},
+/*tODTon_min*/ {1500, 0}
+	}
+};
+
+const uint16_t jedec_spec2_trfc_ab[7] = {
+/*	4Gb, 6Gb, 8Gb,12Gb, 16Gb, 24Gb(non), 32Gb(non)	*/
+	 130, 180, 180, 280, 280, 560, 560
+};
+
+static uint32_t js1_ind;
+static uint16_t js2[JS2_CNT];
+static uint8_t RL;
+static uint8_t WL;
+
+static uint16_t _f_scale(uint32_t _ddr_mbps, uint32_t _ddr_mbpsdiv, uint32_t ps,
+			 uint16_t cyc)
+{
+	uint32_t tmp;
+	uint32_t div;
+
+	tmp = (((uint32_t)(ps) + 9) / 10) * _ddr_mbps;
+	div = tmp / (200000 * _ddr_mbpsdiv);
+	if (tmp != (div * 200000 * _ddr_mbpsdiv))
+		div = div + 1;
+
+	if (div > cyc)
+		return (uint16_t)div;
+	return cyc;
+}
+
+static void _f_scale_js2(uint32_t _ddr_mbps, uint32_t _ddr_mbpsdiv,
+			 uint16_t *_js2)
+{
+	int i;
+
+	for (i = 0; i < JS2_TBLCNT; i++) {
+		_js2[i] = _f_scale(_ddr_mbps, _ddr_mbpsdiv,
+				  1UL * jedec_spec2[JS2_DERATE][i].ps,
+				  jedec_spec2[JS2_DERATE][i].cyc);
+	}
+
+	_js2[js2_trcpb] = _js2[js2_tras] + _js2[js2_trppb];
+	_js2[js2_trcab] = _js2[js2_tras] + _js2[js2_trpab];
+}
+
+/* scaler for DELAY value */
+static int16_t _f_scale_adj(int16_t ps)
+{
+	int32_t tmp;
+	/*
+	 * tmp = (int32_t)512 * ps * ddr_mbps /2 / ddr_mbpsdiv / 1000 / 1000;
+	 *     = ps * ddr_mbps /2 / ddr_mbpsdiv *512 / 8 / 8 / 125 / 125
+	 *     = ps * ddr_mbps / ddr_mbpsdiv *4 / 125 / 125
+	 */
+	tmp =
+	    (int32_t)4 * (int32_t)ps * (int32_t)ddr_mbps /
+	    (int32_t)ddr_mbpsdiv;
+	tmp = (int32_t)tmp / (int32_t)15625;
+
+	return (int16_t)tmp;
+}
+
+static const uint32_t reg_pi_mr1_data_fx_csx[2][CSAB_CNT] = {
+	{
+	 _reg_PI_MR1_DATA_F0_0,
+	 _reg_PI_MR1_DATA_F0_1,
+	 _reg_PI_MR1_DATA_F0_2,
+	 _reg_PI_MR1_DATA_F0_3},
+	{
+	 _reg_PI_MR1_DATA_F1_0,
+	 _reg_PI_MR1_DATA_F1_1,
+	 _reg_PI_MR1_DATA_F1_2,
+	 _reg_PI_MR1_DATA_F1_3}
+};
+
+static const uint32_t reg_pi_mr2_data_fx_csx[2][CSAB_CNT] = {
+	{
+	 _reg_PI_MR2_DATA_F0_0,
+	 _reg_PI_MR2_DATA_F0_1,
+	 _reg_PI_MR2_DATA_F0_2,
+	 _reg_PI_MR2_DATA_F0_3},
+	{
+	 _reg_PI_MR2_DATA_F1_0,
+	 _reg_PI_MR2_DATA_F1_1,
+	 _reg_PI_MR2_DATA_F1_2,
+	 _reg_PI_MR2_DATA_F1_3}
+};
+
+static const uint32_t reg_pi_mr3_data_fx_csx[2][CSAB_CNT] = {
+	{
+	 _reg_PI_MR3_DATA_F0_0,
+	 _reg_PI_MR3_DATA_F0_1,
+	 _reg_PI_MR3_DATA_F0_2,
+	 _reg_PI_MR3_DATA_F0_3},
+	{
+	 _reg_PI_MR3_DATA_F1_0,
+	 _reg_PI_MR3_DATA_F1_1,
+	 _reg_PI_MR3_DATA_F1_2,
+	 _reg_PI_MR3_DATA_F1_3}
+};
+
+const uint32_t reg_pi_mr11_data_fx_csx[2][CSAB_CNT] = {
+	{
+	 _reg_PI_MR11_DATA_F0_0,
+	 _reg_PI_MR11_DATA_F0_1,
+	 _reg_PI_MR11_DATA_F0_2,
+	 _reg_PI_MR11_DATA_F0_3},
+	{
+	 _reg_PI_MR11_DATA_F1_0,
+	 _reg_PI_MR11_DATA_F1_1,
+	 _reg_PI_MR11_DATA_F1_2,
+	 _reg_PI_MR11_DATA_F1_3}
+};
+
+const uint32_t reg_pi_mr12_data_fx_csx[2][CSAB_CNT] = {
+	{
+	 _reg_PI_MR12_DATA_F0_0,
+	 _reg_PI_MR12_DATA_F0_1,
+	 _reg_PI_MR12_DATA_F0_2,
+	 _reg_PI_MR12_DATA_F0_3},
+	{
+	 _reg_PI_MR12_DATA_F1_0,
+	 _reg_PI_MR12_DATA_F1_1,
+	 _reg_PI_MR12_DATA_F1_2,
+	 _reg_PI_MR12_DATA_F1_3}
+};
+
+const uint32_t reg_pi_mr14_data_fx_csx[2][CSAB_CNT] = {
+	{
+	 _reg_PI_MR14_DATA_F0_0,
+	 _reg_PI_MR14_DATA_F0_1,
+	 _reg_PI_MR14_DATA_F0_2,
+	 _reg_PI_MR14_DATA_F0_3},
+	{
+	 _reg_PI_MR14_DATA_F1_0,
+	 _reg_PI_MR14_DATA_F1_1,
+	 _reg_PI_MR14_DATA_F1_2,
+	 _reg_PI_MR14_DATA_F1_3}
+};
+
+/*
+ * regif pll w/a   ( REGIF H3 Ver.2.0 or later/M3-N/V3H WA )
+ */
+static void regif_pll_wa(void)
+{
+	uint32_t ch;
+
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) {
+		// PLL setting for PHY : H3 Ver.1.x
+		reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_PLL_WAIT),
+				   (0x0064U <<
+				    ddr_regdef_lsb(_reg_PHY_PLL_WAIT)));
+		reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_PLL_CTRL),
+				   ddrtbl_getval(_cnf_DDR_PHY_ADR_G_REGSET,
+						 _reg_PHY_PLL_CTRL));
+
+		reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_LP4_BOOT_PLL_CTRL),
+				   ddrtbl_getval(_cnf_DDR_PHY_ADR_G_REGSET,
+						 _reg_PHY_LP4_BOOT_PLL_CTRL));
+
+	} else {
+		/*  PLL setting for PHY : M3-W/M3-N/V3H/H3 Ver.2.0 or later */
+		reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_PLL_WAIT),
+				   (0x5064U <<
+				    ddr_regdef_lsb(_reg_PHY_PLL_WAIT)));
+
+		reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_PLL_CTRL),
+				   (ddrtbl_getval
+				    (_cnf_DDR_PHY_ADR_G_REGSET,
+				     _reg_PHY_PLL_CTRL_TOP) << 16) |
+				   ddrtbl_getval(_cnf_DDR_PHY_ADR_G_REGSET,
+						 _reg_PHY_PLL_CTRL));
+		reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_PLL_CTRL_CA),
+				   ddrtbl_getval(_cnf_DDR_PHY_ADR_G_REGSET,
+						 _reg_PHY_PLL_CTRL_CA));
+
+		reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_LP4_BOOT_PLL_CTRL),
+				   (ddrtbl_getval
+				    (_cnf_DDR_PHY_ADR_G_REGSET,
+				     _reg_PHY_LP4_BOOT_PLL_CTRL_CA) << 16) |
+				   ddrtbl_getval(_cnf_DDR_PHY_ADR_G_REGSET,
+						 _reg_PHY_LP4_BOOT_PLL_CTRL));
+		reg_ddrphy_write_a(ddr_regdef_adr
+				   (_reg_PHY_LP4_BOOT_TOP_PLL_CTRL),
+				   ddrtbl_getval(_cnf_DDR_PHY_ADR_G_REGSET,
+						 _reg_PHY_LP4_BOOT_TOP_PLL_CTRL
+						 ));
+	}
+
+	reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_LPDDR3_CS),
+			   _cnf_DDR_PHY_ADR_G_REGSET
+			   [ddr_regdef_adr(_reg_PHY_LPDDR3_CS) -
+			   DDR_PHY_ADR_G_REGSET_OFS]);
+
+	/* protect register interface */
+	ddrphy_regif_idle();
+	pll3_control(0);
+
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) {
+		/*  non */
+	} else {
+		reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_DLL_RST_EN),
+				   (0x01U <<
+				    ddr_regdef_lsb(_reg_PHY_DLL_RST_EN)));
+		ddrphy_regif_idle();
+	}
+
+	/* init start */
+	/* dbdficnt0:
+	 * dfi_dram_clk_disable=1
+	 * dfi_frequency = 0
+	 * freq_ratio = 01 (2:1)
+	 * init_start =0
+	 */
+	foreach_vch(ch)
+	    mmio_write_32(DBSC_DBDFICNT(ch), 0x00000F10);
+	dsb_sev();
+
+	/* dbdficnt0:
+	 * dfi_dram_clk_disable=1
+	 * dfi_frequency = 0
+	 * freq_ratio = 01 (2:1)
+	 * init_start =1
+	 */
+	foreach_vch(ch)
+	    mmio_write_32(DBSC_DBDFICNT(ch), 0x00000F11);
+	dsb_sev();
+
+	foreach_ech(ch)
+	if ((board_cnf->phyvalid) & BIT(ch))
+		while ((mmio_read_32(DBSC_PLL_LOCK(ch)) & 0x1f) != 0x1f)
+			;
+	dsb_sev();
+}
+
+/* load table data into DDR registers */
+static void ddrtbl_load(void)
+{
+	uint32_t i;
+	uint32_t slice;
+	uint32_t csab;
+	uint32_t adr;
+	uint32_t data_l;
+	uint32_t tmp[3];
+	uint16_t dataS;
+
+	/* TIMING REGISTERS */
+	/* search jedec_spec1 index */
+	for (i = JS1_USABLEC_SPEC_LO; i < JS1_FREQ_TBL_NUM - 1; i++) {
+		if (js1[i].fx3 * 2U * ddr_mbpsdiv >= ddr_mbps * 3U)
+			break;
+	}
+	if (i > JS1_USABLEC_SPEC_HI)
+		js1_ind = JS1_USABLEC_SPEC_HI;
+	else
+		js1_ind = i;
+
+	if (board_cnf->dbi_en)
+		RL = js1[js1_ind].rlwdbi;
+	else
+		RL = js1[js1_ind].rlwodbi;
+
+	WL = js1[js1_ind].WL;
+
+	/* calculate jedec_spec2 */
+	_f_scale_js2(ddr_mbps, ddr_mbpsdiv, js2);
+
+	/* PREPARE TBL */
+	if (prr_product == PRR_PRODUCT_H3) {
+		if (prr_cut <= PRR_PRODUCT_11) {
+			/*  H3 Ver.1.x */
+			_tblcopy(_cnf_DDR_PHY_SLICE_REGSET,
+				 DDR_PHY_SLICE_REGSET_H3,
+				 DDR_PHY_SLICE_REGSET_NUM_H3);
+			_tblcopy(_cnf_DDR_PHY_ADR_V_REGSET,
+				 DDR_PHY_ADR_V_REGSET_H3,
+				 DDR_PHY_ADR_V_REGSET_NUM_H3);
+			_tblcopy(_cnf_DDR_PHY_ADR_I_REGSET,
+				 DDR_PHY_ADR_I_REGSET_H3,
+				 DDR_PHY_ADR_I_REGSET_NUM_H3);
+			_tblcopy(_cnf_DDR_PHY_ADR_G_REGSET,
+				 DDR_PHY_ADR_G_REGSET_H3,
+				 DDR_PHY_ADR_G_REGSET_NUM_H3);
+			_tblcopy(_cnf_DDR_PI_REGSET, DDR_PI_REGSET_H3,
+				 DDR_PI_REGSET_NUM_H3);
+
+			DDR_PHY_SLICE_REGSET_OFS = DDR_PHY_SLICE_REGSET_OFS_H3;
+			DDR_PHY_ADR_V_REGSET_OFS = DDR_PHY_ADR_V_REGSET_OFS_H3;
+			DDR_PHY_ADR_I_REGSET_OFS = DDR_PHY_ADR_I_REGSET_OFS_H3;
+			DDR_PHY_ADR_G_REGSET_OFS = DDR_PHY_ADR_G_REGSET_OFS_H3;
+			DDR_PI_REGSET_OFS = DDR_PI_REGSET_OFS_H3;
+			DDR_PHY_SLICE_REGSET_SIZE =
+			    DDR_PHY_SLICE_REGSET_SIZE_H3;
+			DDR_PHY_ADR_V_REGSET_SIZE =
+			    DDR_PHY_ADR_V_REGSET_SIZE_H3;
+			DDR_PHY_ADR_I_REGSET_SIZE =
+			    DDR_PHY_ADR_I_REGSET_SIZE_H3;
+			DDR_PHY_ADR_G_REGSET_SIZE =
+			    DDR_PHY_ADR_G_REGSET_SIZE_H3;
+			DDR_PI_REGSET_SIZE = DDR_PI_REGSET_SIZE_H3;
+			DDR_PHY_SLICE_REGSET_NUM = DDR_PHY_SLICE_REGSET_NUM_H3;
+			DDR_PHY_ADR_V_REGSET_NUM = DDR_PHY_ADR_V_REGSET_NUM_H3;
+			DDR_PHY_ADR_I_REGSET_NUM = DDR_PHY_ADR_I_REGSET_NUM_H3;
+			DDR_PHY_ADR_G_REGSET_NUM = DDR_PHY_ADR_G_REGSET_NUM_H3;
+			DDR_PI_REGSET_NUM = DDR_PI_REGSET_NUM_H3;
+
+			DDR_PHY_ADR_I_NUM = 1;
+		} else {
+			/*  H3 Ver.2.0 or later */
+			_tblcopy(_cnf_DDR_PHY_SLICE_REGSET,
+				 DDR_PHY_SLICE_REGSET_H3VER2,
+				 DDR_PHY_SLICE_REGSET_NUM_H3VER2);
+			_tblcopy(_cnf_DDR_PHY_ADR_V_REGSET,
+				 DDR_PHY_ADR_V_REGSET_H3VER2,
+				 DDR_PHY_ADR_V_REGSET_NUM_H3VER2);
+			_tblcopy(_cnf_DDR_PHY_ADR_G_REGSET,
+				 DDR_PHY_ADR_G_REGSET_H3VER2,
+				 DDR_PHY_ADR_G_REGSET_NUM_H3VER2);
+			_tblcopy(_cnf_DDR_PI_REGSET, DDR_PI_REGSET_H3VER2,
+				 DDR_PI_REGSET_NUM_H3VER2);
+
+			DDR_PHY_SLICE_REGSET_OFS =
+			    DDR_PHY_SLICE_REGSET_OFS_H3VER2;
+			DDR_PHY_ADR_V_REGSET_OFS =
+			    DDR_PHY_ADR_V_REGSET_OFS_H3VER2;
+			DDR_PHY_ADR_G_REGSET_OFS =
+			    DDR_PHY_ADR_G_REGSET_OFS_H3VER2;
+			DDR_PI_REGSET_OFS = DDR_PI_REGSET_OFS_H3VER2;
+			DDR_PHY_SLICE_REGSET_SIZE =
+			    DDR_PHY_SLICE_REGSET_SIZE_H3VER2;
+			DDR_PHY_ADR_V_REGSET_SIZE =
+			    DDR_PHY_ADR_V_REGSET_SIZE_H3VER2;
+			DDR_PHY_ADR_G_REGSET_SIZE =
+			    DDR_PHY_ADR_G_REGSET_SIZE_H3VER2;
+			DDR_PI_REGSET_SIZE = DDR_PI_REGSET_SIZE_H3VER2;
+			DDR_PHY_SLICE_REGSET_NUM =
+			    DDR_PHY_SLICE_REGSET_NUM_H3VER2;
+			DDR_PHY_ADR_V_REGSET_NUM =
+			    DDR_PHY_ADR_V_REGSET_NUM_H3VER2;
+			DDR_PHY_ADR_G_REGSET_NUM =
+			    DDR_PHY_ADR_G_REGSET_NUM_H3VER2;
+			DDR_PI_REGSET_NUM = DDR_PI_REGSET_NUM_H3VER2;
+
+			DDR_PHY_ADR_I_NUM = 0;
+		}
+	} else if (prr_product == PRR_PRODUCT_M3) {
+		/*  M3-W */
+		_tblcopy(_cnf_DDR_PHY_SLICE_REGSET,
+			 DDR_PHY_SLICE_REGSET_M3, DDR_PHY_SLICE_REGSET_NUM_M3);
+		_tblcopy(_cnf_DDR_PHY_ADR_V_REGSET,
+			 DDR_PHY_ADR_V_REGSET_M3, DDR_PHY_ADR_V_REGSET_NUM_M3);
+		_tblcopy(_cnf_DDR_PHY_ADR_I_REGSET,
+			 DDR_PHY_ADR_I_REGSET_M3, DDR_PHY_ADR_I_REGSET_NUM_M3);
+		_tblcopy(_cnf_DDR_PHY_ADR_G_REGSET,
+			 DDR_PHY_ADR_G_REGSET_M3, DDR_PHY_ADR_G_REGSET_NUM_M3);
+		_tblcopy(_cnf_DDR_PI_REGSET,
+			 DDR_PI_REGSET_M3, DDR_PI_REGSET_NUM_M3);
+
+		DDR_PHY_SLICE_REGSET_OFS = DDR_PHY_SLICE_REGSET_OFS_M3;
+		DDR_PHY_ADR_V_REGSET_OFS = DDR_PHY_ADR_V_REGSET_OFS_M3;
+		DDR_PHY_ADR_I_REGSET_OFS = DDR_PHY_ADR_I_REGSET_OFS_M3;
+		DDR_PHY_ADR_G_REGSET_OFS = DDR_PHY_ADR_G_REGSET_OFS_M3;
+		DDR_PI_REGSET_OFS = DDR_PI_REGSET_OFS_M3;
+		DDR_PHY_SLICE_REGSET_SIZE = DDR_PHY_SLICE_REGSET_SIZE_M3;
+		DDR_PHY_ADR_V_REGSET_SIZE = DDR_PHY_ADR_V_REGSET_SIZE_M3;
+		DDR_PHY_ADR_I_REGSET_SIZE = DDR_PHY_ADR_I_REGSET_SIZE_M3;
+		DDR_PHY_ADR_G_REGSET_SIZE = DDR_PHY_ADR_G_REGSET_SIZE_M3;
+		DDR_PI_REGSET_SIZE = DDR_PI_REGSET_SIZE_M3;
+		DDR_PHY_SLICE_REGSET_NUM = DDR_PHY_SLICE_REGSET_NUM_M3;
+		DDR_PHY_ADR_V_REGSET_NUM = DDR_PHY_ADR_V_REGSET_NUM_M3;
+		DDR_PHY_ADR_I_REGSET_NUM = DDR_PHY_ADR_I_REGSET_NUM_M3;
+		DDR_PHY_ADR_G_REGSET_NUM = DDR_PHY_ADR_G_REGSET_NUM_M3;
+		DDR_PI_REGSET_NUM = DDR_PI_REGSET_NUM_M3;
+
+		DDR_PHY_ADR_I_NUM = 2;
+	} else {
+		/*  M3-N/V3H */
+		_tblcopy(_cnf_DDR_PHY_SLICE_REGSET,
+			 DDR_PHY_SLICE_REGSET_M3N,
+			 DDR_PHY_SLICE_REGSET_NUM_M3N);
+		_tblcopy(_cnf_DDR_PHY_ADR_V_REGSET, DDR_PHY_ADR_V_REGSET_M3N,
+			 DDR_PHY_ADR_V_REGSET_NUM_M3N);
+		_tblcopy(_cnf_DDR_PHY_ADR_I_REGSET, DDR_PHY_ADR_I_REGSET_M3N,
+			 DDR_PHY_ADR_I_REGSET_NUM_M3N);
+		_tblcopy(_cnf_DDR_PHY_ADR_G_REGSET, DDR_PHY_ADR_G_REGSET_M3N,
+			 DDR_PHY_ADR_G_REGSET_NUM_M3N);
+		_tblcopy(_cnf_DDR_PI_REGSET, DDR_PI_REGSET_M3N,
+			 DDR_PI_REGSET_NUM_M3N);
+
+		DDR_PHY_SLICE_REGSET_OFS = DDR_PHY_SLICE_REGSET_OFS_M3N;
+		DDR_PHY_ADR_V_REGSET_OFS = DDR_PHY_ADR_V_REGSET_OFS_M3N;
+		DDR_PHY_ADR_I_REGSET_OFS = DDR_PHY_ADR_I_REGSET_OFS_M3N;
+		DDR_PHY_ADR_G_REGSET_OFS = DDR_PHY_ADR_G_REGSET_OFS_M3N;
+		DDR_PI_REGSET_OFS = DDR_PI_REGSET_OFS_M3N;
+		DDR_PHY_SLICE_REGSET_SIZE = DDR_PHY_SLICE_REGSET_SIZE_M3N;
+		DDR_PHY_ADR_V_REGSET_SIZE = DDR_PHY_ADR_V_REGSET_SIZE_M3N;
+		DDR_PHY_ADR_I_REGSET_SIZE = DDR_PHY_ADR_I_REGSET_SIZE_M3N;
+		DDR_PHY_ADR_G_REGSET_SIZE = DDR_PHY_ADR_G_REGSET_SIZE_M3N;
+		DDR_PI_REGSET_SIZE = DDR_PI_REGSET_SIZE_M3N;
+		DDR_PHY_SLICE_REGSET_NUM = DDR_PHY_SLICE_REGSET_NUM_M3N;
+		DDR_PHY_ADR_V_REGSET_NUM = DDR_PHY_ADR_V_REGSET_NUM_M3N;
+		DDR_PHY_ADR_I_REGSET_NUM = DDR_PHY_ADR_I_REGSET_NUM_M3N;
+		DDR_PHY_ADR_G_REGSET_NUM = DDR_PHY_ADR_G_REGSET_NUM_M3N;
+		DDR_PI_REGSET_NUM = DDR_PI_REGSET_NUM_M3N;
+
+		DDR_PHY_ADR_I_NUM = 2;
+	}
+
+	/* PLL CODE CHANGE */
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut == PRR_PRODUCT_11)) {
+		ddrtbl_setval(_cnf_DDR_PHY_ADR_G_REGSET, _reg_PHY_PLL_CTRL,
+			      0x1142);
+		ddrtbl_setval(_cnf_DDR_PHY_ADR_G_REGSET,
+			      _reg_PHY_LP4_BOOT_PLL_CTRL, 0x1142);
+	}
+
+	/* on fly gate adjust */
+	if ((prr_product == PRR_PRODUCT_M3) && (prr_cut == PRR_PRODUCT_10)) {
+		ddrtbl_setval(_cnf_DDR_PHY_SLICE_REGSET,
+			      _reg_ON_FLY_GATE_ADJUST_EN, 0x00);
+	}
+
+	/* Adjust PI parameters */
+#ifdef _def_LPDDR4_ODT
+	for (i = 0; i < 2; i++) {
+		for (csab = 0; csab < CSAB_CNT; csab++) {
+			ddrtbl_setval(_cnf_DDR_PI_REGSET,
+				      reg_pi_mr11_data_fx_csx[i][csab],
+				      _def_LPDDR4_ODT);
+		}
+	}
+#endif /* _def_LPDDR4_ODT */
+
+#ifdef _def_LPDDR4_VREFCA
+	for (i = 0; i < 2; i++) {
+		for (csab = 0; csab < CSAB_CNT; csab++) {
+			ddrtbl_setval(_cnf_DDR_PI_REGSET,
+				      reg_pi_mr12_data_fx_csx[i][csab],
+				      _def_LPDDR4_VREFCA);
+		}
+	}
+#endif /* _def_LPDDR4_VREFCA */
+	if ((prr_product == PRR_PRODUCT_M3N) ||
+	    (prr_product == PRR_PRODUCT_V3H)) {
+		js2[js2_tiedly] = _f_scale(ddr_mbps, ddr_mbpsdiv, 7000, 0) + 7U;
+		if (js2[js2_tiedly] > (RL))
+			js2[js2_tiedly] = RL;
+	} else if ((prr_product == PRR_PRODUCT_H3) &&
+		   (prr_cut > PRR_PRODUCT_11)) {
+		js2[js2_tiedly] = _f_scale(ddr_mbps, ddr_mbpsdiv, 9000, 0) + 4U;
+	} else if ((prr_product == PRR_PRODUCT_H3) &&
+		   (prr_cut <= PRR_PRODUCT_11)) {
+		js2[js2_tiedly] = _f_scale(ddr_mbps, ddr_mbpsdiv, 10000, 0);
+	}
+
+	if (((prr_product == PRR_PRODUCT_H3) && (prr_cut > PRR_PRODUCT_11)) ||
+	    (prr_product == PRR_PRODUCT_M3N) ||
+	    (prr_product == PRR_PRODUCT_V3H)) {
+		if ((js2[js2_tiedly]) >= 0x1e)
+			dataS = 0x1e;
+		else
+			dataS = js2[js2_tiedly];
+	} else {
+		if ((js2[js2_tiedly]) >= 0x0e)
+			dataS = 0x0e;
+		else
+			dataS = js2[js2_tiedly];
+	}
+
+	ddrtbl_setval(_cnf_DDR_PHY_SLICE_REGSET, _reg_PHY_RDDATA_EN_DLY, dataS);
+	ddrtbl_setval(_cnf_DDR_PHY_SLICE_REGSET, _reg_PHY_RDDATA_EN_TSEL_DLY,
+		      (dataS - 2));
+	if ((prr_product == PRR_PRODUCT_M3N) ||
+	    (prr_product == PRR_PRODUCT_V3H)) {
+		ddrtbl_setval(_cnf_DDR_PHY_SLICE_REGSET,
+			      _reg_PHY_RDDATA_EN_OE_DLY, dataS - 2);
+	}
+	ddrtbl_setval(_cnf_DDR_PI_REGSET, _reg_PI_RDLAT_ADJ_F1, RL - dataS);
+
+	if (ddrtbl_getval
+	    (_cnf_DDR_PHY_SLICE_REGSET, _reg_PHY_WRITE_PATH_LAT_ADD)) {
+		data_l = WL - 1;
+	} else {
+		data_l = WL;
+	}
+	ddrtbl_setval(_cnf_DDR_PI_REGSET, _reg_PI_WRLAT_ADJ_F1, data_l - 2);
+	ddrtbl_setval(_cnf_DDR_PI_REGSET, _reg_PI_WRLAT_F1, data_l);
+
+	if (board_cnf->dbi_en) {
+		ddrtbl_setval(_cnf_DDR_PHY_SLICE_REGSET, _reg_PHY_DBI_MODE,
+			      0x01);
+		ddrtbl_setval(_cnf_DDR_PHY_SLICE_REGSET,
+			      _reg_PHY_WDQLVL_DATADM_MASK, 0x000);
+	} else {
+		ddrtbl_setval(_cnf_DDR_PHY_SLICE_REGSET, _reg_PHY_DBI_MODE,
+			      0x00);
+		ddrtbl_setval(_cnf_DDR_PHY_SLICE_REGSET,
+			      _reg_PHY_WDQLVL_DATADM_MASK, 0x100);
+	}
+
+	tmp[0] = js1[js1_ind].MR1;
+	tmp[1] = js1[js1_ind].MR2;
+	data_l = ddrtbl_getval(_cnf_DDR_PI_REGSET, _reg_PI_MR3_DATA_F1_0);
+	if (board_cnf->dbi_en)
+		tmp[2] = data_l | 0xc0;
+	else
+		tmp[2] = data_l & (~0xc0);
+
+	for (i = 0; i < 2; i++) {
+		for (csab = 0; csab < CSAB_CNT; csab++) {
+			ddrtbl_setval(_cnf_DDR_PI_REGSET,
+				      reg_pi_mr1_data_fx_csx[i][csab], tmp[0]);
+			ddrtbl_setval(_cnf_DDR_PI_REGSET,
+				      reg_pi_mr2_data_fx_csx[i][csab], tmp[1]);
+			ddrtbl_setval(_cnf_DDR_PI_REGSET,
+				      reg_pi_mr3_data_fx_csx[i][csab], tmp[2]);
+		}
+	}
+
+	/* DDRPHY INT START */
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) {
+		/* non */
+	} else {
+		regif_pll_wa();
+		dbwait_loop(5);
+	}
+
+	/* FREQ_SEL_MULTICAST & PER_CS_TRAINING_MULTICAST SET (for safety) */
+	reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_FREQ_SEL_MULTICAST_EN),
+			   BIT(ddr_regdef_lsb(_reg_PHY_FREQ_SEL_MULTICAST_EN)));
+	ddr_setval_ach_as(_reg_PHY_PER_CS_TRAINING_MULTICAST_EN, 0x01);
+
+	/* SET DATA SLICE TABLE */
+	for (slice = 0; slice < SLICE_CNT; slice++) {
+		adr =
+		    DDR_PHY_SLICE_REGSET_OFS +
+		    DDR_PHY_SLICE_REGSET_SIZE * slice;
+		for (i = 0; i < DDR_PHY_SLICE_REGSET_NUM; i++) {
+			reg_ddrphy_write_a(adr + i,
+					   _cnf_DDR_PHY_SLICE_REGSET[i]);
+		}
+	}
+
+	/* SET ADR SLICE TABLE */
+	adr = DDR_PHY_ADR_V_REGSET_OFS;
+	for (i = 0; i < DDR_PHY_ADR_V_REGSET_NUM; i++) {
+		reg_ddrphy_write_a(adr + i, _cnf_DDR_PHY_ADR_V_REGSET[i]);
+	}
+
+	if (((prr_product == PRR_PRODUCT_M3) ||
+	     (prr_product == PRR_PRODUCT_M3N)) &&
+	    ((0x00ffffff & (uint32_t)((board_cnf->ch[0].ca_swap) >> 40))
+	    != 0x00)) {
+		adr = DDR_PHY_ADR_I_REGSET_OFS + DDR_PHY_ADR_I_REGSET_SIZE;
+		for (i = 0; i < DDR_PHY_ADR_V_REGSET_NUM; i++) {
+			reg_ddrphy_write_a(adr + i,
+					   _cnf_DDR_PHY_ADR_V_REGSET[i]);
+		}
+		ddrtbl_setval(_cnf_DDR_PHY_ADR_G_REGSET,
+			      _reg_PHY_ADR_DISABLE, 0x02);
+		DDR_PHY_ADR_I_NUM -= 1;
+		ddr_phycaslice = 1;
+
+#ifndef _def_LPDDR4_ODT
+		for (i = 0; i < 2; i++) {
+			for (csab = 0; csab < CSAB_CNT; csab++) {
+				ddrtbl_setval(_cnf_DDR_PI_REGSET,
+					      reg_pi_mr11_data_fx_csx[i][csab],
+					      0x66);
+			}
+		}
+#endif/* _def_LPDDR4_ODT */
+	} else {
+		ddr_phycaslice = 0;
+	}
+
+	if (DDR_PHY_ADR_I_NUM > 0) {
+		for (slice = 0; slice < DDR_PHY_ADR_I_NUM; slice++) {
+			adr =
+			    DDR_PHY_ADR_I_REGSET_OFS +
+			    DDR_PHY_ADR_I_REGSET_SIZE * slice;
+			for (i = 0; i < DDR_PHY_ADR_I_REGSET_NUM; i++) {
+				reg_ddrphy_write_a(adr + i,
+						   _cnf_DDR_PHY_ADR_I_REGSET
+						   [i]);
+			}
+		}
+	}
+
+	/* SET ADRCTRL SLICE TABLE */
+	adr = DDR_PHY_ADR_G_REGSET_OFS;
+	for (i = 0; i < DDR_PHY_ADR_G_REGSET_NUM; i++) {
+		reg_ddrphy_write_a(adr + i, _cnf_DDR_PHY_ADR_G_REGSET[i]);
+	}
+
+	/* SET PI REGISTERS */
+	adr = DDR_PI_REGSET_OFS;
+	for (i = 0; i < DDR_PI_REGSET_NUM; i++) {
+		reg_ddrphy_write_a(adr + i, _cnf_DDR_PI_REGSET[i]);
+	}
+}
+
+/* CONFIGURE DDR REGISTERS */
+static void ddr_config_sub(void)
+{
+	uint32_t i;
+	uint32_t ch, slice;
+	uint32_t data_l;
+	uint32_t tmp;
+	uint8_t high_byte[SLICE_CNT];
+	const uint32_t _par_CALVL_DEVICE_MAP = 1;
+
+	foreach_vch(ch) {
+	/* BOARD SETTINGS (DQ,DM,VREF_DRIVING) */
+		for (slice = 0; slice < SLICE_CNT; slice++) {
+			high_byte[slice] =
+			    (board_cnf->ch[ch].dqs_swap >> (4 * slice)) % 2;
+			ddr_setval_s(ch, slice, _reg_PHY_DQ_DM_SWIZZLE0,
+				     board_cnf->ch[ch].dq_swap[slice]);
+			ddr_setval_s(ch, slice, _reg_PHY_DQ_DM_SWIZZLE1,
+				     board_cnf->ch[ch].dm_swap[slice]);
+			if (high_byte[slice]) {
+				/* HIGHER 16 BYTE */
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_CALVL_VREF_DRIVING_SLICE,
+					     0x00);
+			} else {
+				/* LOWER 16 BYTE */
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_CALVL_VREF_DRIVING_SLICE,
+					     0x01);
+			}
+		}
+
+	/* BOARD SETTINGS (CA,ADDR_SEL) */
+		data_l = (0x00ffffff & (uint32_t)(board_cnf->ch[ch].ca_swap)) |
+			0x00888888;
+
+		/* --- ADR_CALVL_SWIZZLE --- */
+		if (prr_product == PRR_PRODUCT_M3) {
+			ddr_setval(ch, _reg_PHY_ADR_CALVL_SWIZZLE0_0, data_l);
+			ddr_setval(ch, _reg_PHY_ADR_CALVL_SWIZZLE1_0,
+				   0x00000000);
+			ddr_setval(ch, _reg_PHY_ADR_CALVL_SWIZZLE0_1, data_l);
+			ddr_setval(ch, _reg_PHY_ADR_CALVL_SWIZZLE1_1,
+				   0x00000000);
+			ddr_setval(ch, _reg_PHY_ADR_CALVL_DEVICE_MAP,
+				   _par_CALVL_DEVICE_MAP);
+		} else {
+			ddr_setval(ch, _reg_PHY_ADR_CALVL_SWIZZLE0, data_l);
+			ddr_setval(ch, _reg_PHY_ADR_CALVL_SWIZZLE1, 0x00000000);
+			ddr_setval(ch, _reg_PHY_CALVL_DEVICE_MAP,
+				   _par_CALVL_DEVICE_MAP);
+		}
+
+		/* --- ADR_ADDR_SEL --- */
+		if ((prr_product == PRR_PRODUCT_H3) &&
+		    (prr_cut > PRR_PRODUCT_11)) {
+			data_l = 0x00FFFFFF & board_cnf->ch[ch].ca_swap;
+		} else {
+			data_l = 0;
+			tmp = board_cnf->ch[ch].ca_swap;
+			for (i = 0; i < 6; i++) {
+				data_l |= ((tmp & 0x0f) << (i * 5));
+				tmp = tmp >> 4;
+			}
+		}
+		ddr_setval(ch, _reg_PHY_ADR_ADDR_SEL, data_l);
+		if (ddr_phycaslice == 1) {
+			/* ----------- adr slice2 swap ----------- */
+			tmp  = (uint32_t)((board_cnf->ch[ch].ca_swap) >> 40);
+			data_l = (tmp & 0x00ffffff) | 0x00888888;
+
+			/* --- ADR_CALVL_SWIZZLE --- */
+			if (prr_product == PRR_PRODUCT_M3) {
+				ddr_setval_s(ch, 2,
+					     _reg_PHY_ADR_CALVL_SWIZZLE0_0,
+					     data_l);
+				ddr_setval_s(ch, 2,
+					     _reg_PHY_ADR_CALVL_SWIZZLE1_0,
+					     0x00000000);
+				ddr_setval_s(ch, 2,
+					     _reg_PHY_ADR_CALVL_SWIZZLE0_1,
+					     data_l);
+				ddr_setval_s(ch, 2,
+					     _reg_PHY_ADR_CALVL_SWIZZLE1_1,
+					     0x00000000);
+				ddr_setval_s(ch, 2,
+					     _reg_PHY_ADR_CALVL_DEVICE_MAP,
+					     _par_CALVL_DEVICE_MAP);
+			} else {
+				ddr_setval_s(ch, 2,
+					     _reg_PHY_ADR_CALVL_SWIZZLE0,
+					     data_l);
+				ddr_setval_s(ch, 2,
+					     _reg_PHY_ADR_CALVL_SWIZZLE1,
+					     0x00000000);
+				ddr_setval_s(ch, 2,
+					     _reg_PHY_CALVL_DEVICE_MAP,
+					     _par_CALVL_DEVICE_MAP);
+			}
+
+			/* --- ADR_ADDR_SEL --- */
+			data_l = 0;
+			for (i = 0; i < 6; i++) {
+				data_l |= ((tmp & 0x0f) << (i * 5));
+				tmp = tmp >> 4;
+			}
+
+			ddr_setval_s(ch, 2, _reg_PHY_ADR_ADDR_SEL, data_l);
+		}
+
+	/* BOARD SETTINGS (BYTE_ORDER_SEL) */
+		if (prr_product == PRR_PRODUCT_M3) {
+			/* --- DATA_BYTE_SWAP --- */
+			data_l = 0;
+			tmp = board_cnf->ch[ch].dqs_swap;
+			for (i = 0; i < 4; i++) {
+				data_l |= ((tmp & 0x03) << (i * 2));
+				tmp = tmp >> 4;
+			}
+		} else {
+			/* --- DATA_BYTE_SWAP --- */
+			data_l = board_cnf->ch[ch].dqs_swap;
+			ddr_setval(ch, _reg_PI_DATA_BYTE_SWAP_EN, 0x01);
+			ddr_setval(ch, _reg_PI_DATA_BYTE_SWAP_SLICE0,
+				   (data_l) & 0x0f);
+			ddr_setval(ch, _reg_PI_DATA_BYTE_SWAP_SLICE1,
+				   (data_l >> 4 * 1) & 0x0f);
+			ddr_setval(ch, _reg_PI_DATA_BYTE_SWAP_SLICE2,
+				   (data_l >> 4 * 2) & 0x0f);
+			ddr_setval(ch, _reg_PI_DATA_BYTE_SWAP_SLICE3,
+				   (data_l >> 4 * 3) & 0x0f);
+
+			ddr_setval(ch, _reg_PHY_DATA_BYTE_ORDER_SEL_HIGH, 0x00);
+		}
+		ddr_setval(ch, _reg_PHY_DATA_BYTE_ORDER_SEL, data_l);
+	}
+}
+
+static void get_ca_swizzle(uint32_t ch, uint32_t ddr_csn, uint32_t *p_swz)
+{
+	uint32_t slice;
+	uint32_t tmp;
+	uint32_t tgt;
+
+	if (ddr_csn / 2) {
+		tgt = 3;
+	} else {
+		tgt = 1;
+	}
+
+	for (slice = 0; slice < SLICE_CNT; slice++) {
+		tmp = (board_cnf->ch[ch].dqs_swap >> (4 * slice)) & 0x0f;
+		if (tgt == tmp)
+			break;
+	}
+	tmp = 0x00FFFFFF & board_cnf->ch[ch].ca_swap;
+	if (slice % 2)
+		tmp |= 0x00888888;
+	*p_swz = tmp;
+}
+
+static void ddr_config_sub_h3v1x(void)
+{
+	uint32_t ch, slice;
+	uint32_t data_l;
+	uint32_t tmp;
+	uint8_t high_byte[SLICE_CNT];
+	uint32_t ca_swizzle;
+	uint32_t ca;
+	uint32_t csmap;
+	uint32_t o_inv;
+	uint32_t inv;
+	uint32_t bit_soc;
+	uint32_t bit_mem;
+	uint32_t j;
+
+	const uint8_t o_mr15 = 0x55;
+	const uint8_t o_mr20 = 0x55;
+	const uint16_t o_mr32_mr40 = 0x5a3c;
+
+	foreach_vch(ch) {
+	/* BOARD SETTINGS (DQ,DM,VREF_DRIVING) */
+		csmap = 0;
+		for (slice = 0; slice < SLICE_CNT; slice++) {
+			tmp = (board_cnf->ch[ch].dqs_swap >> (4 * slice)) &
+			      0x0f;
+			high_byte[slice] = tmp % 2;
+			if (tmp == 1 && (slice >= 2))
+				csmap |= 0x05;
+			if (tmp == 3 && (slice >= 2))
+				csmap |= 0x50;
+			ddr_setval_s(ch, slice, _reg_PHY_DQ_SWIZZLING,
+				     board_cnf->ch[ch].dq_swap[slice]);
+			if (high_byte[slice]) {
+				/* HIGHER 16 BYTE */
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_CALVL_VREF_DRIVING_SLICE,
+					     0x00);
+			} else {
+				/* LOWER 16 BYTE */
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_CALVL_VREF_DRIVING_SLICE,
+					     0x01);
+			}
+		}
+	/* BOARD SETTINGS (CA,ADDR_SEL) */
+		ca = 0x00FFFFFF & board_cnf->ch[ch].ca_swap;
+		ddr_setval(ch, _reg_PHY_ADR_ADDR_SEL, ca);
+		ddr_setval(ch, _reg_PHY_CALVL_CS_MAP, csmap);
+
+		get_ca_swizzle(ch, 0, &ca_swizzle);
+
+		ddr_setval(ch, _reg_PHY_ADR_CALVL_SWIZZLE0_0, ca_swizzle);
+		ddr_setval(ch, _reg_PHY_ADR_CALVL_SWIZZLE1_0, 0x00000000);
+		ddr_setval(ch, _reg_PHY_ADR_CALVL_SWIZZLE0_1, 0x00000000);
+		ddr_setval(ch, _reg_PHY_ADR_CALVL_SWIZZLE1_1, 0x00000000);
+		ddr_setval(ch, _reg_PHY_ADR_CALVL_DEVICE_MAP, 0x01);
+
+		for (slice = 0; slice < SLICE_CNT; slice++) {
+			ddr_setval_s(ch, slice, _reg_PI_RDLVL_PATTERN_NUM,
+				     0x01);
+			ddr_setval_s(ch, slice, _reg_PI_RDLVL_PATTERN_START,
+				     0x08);
+
+			if (high_byte[slice])
+				o_inv = o_mr20;
+			else
+				o_inv = o_mr15;
+
+			tmp = board_cnf->ch[ch].dq_swap[slice];
+			inv = 0;
+			j = 0;
+			for (bit_soc = 0; bit_soc < 8; bit_soc++) {
+				bit_mem = (tmp >> (4 * bit_soc)) & 0x0f;
+				j |= (1U << bit_mem);
+				if (o_inv & (1U << bit_mem))
+					inv |= (1U << bit_soc);
+			}
+			data_l = o_mr32_mr40;
+			if (!high_byte[slice])
+				data_l |= (inv << 24);
+			if (high_byte[slice])
+				data_l |= (inv << 16);
+			ddr_setval_s(ch, slice, _reg_PHY_LP4_RDLVL_PATT8,
+				     data_l);
+		}
+	}
+}
+
+static void ddr_config(void)
+{
+	int32_t i;
+	uint32_t ch, slice;
+	uint32_t data_l;
+	uint32_t tmp;
+	int8_t _adj;
+	int16_t adj;
+	uint32_t dq;
+	union {
+		uint32_t ui32[4];
+		uint8_t ui8[16];
+	} patt;
+	uint16_t patm;
+
+	/* configure ddrphy registers */
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) {
+		ddr_config_sub_h3v1x();
+	} else {	/*  H3 Ver.2.0 or later/M3-N/V3H is same as M3-W */
+		ddr_config_sub();
+	}
+
+	/* WDQ_USER_PATT */
+	foreach_vch(ch) {
+		for (slice = 0; slice < SLICE_CNT; slice++) {
+			patm = 0;
+			for (i = 0; i < 16; i++) {
+				tmp = board_cnf->ch[ch].wdqlvl_patt[i];
+				patt.ui8[i] = tmp & 0xff;
+				if (tmp & 0x100)
+					patm |= (1U << i);
+			}
+			ddr_setval_s(ch, slice, _reg_PHY_USER_PATT0,
+				     patt.ui32[0]);
+			ddr_setval_s(ch, slice, _reg_PHY_USER_PATT1,
+				     patt.ui32[1]);
+			ddr_setval_s(ch, slice, _reg_PHY_USER_PATT2,
+				     patt.ui32[2]);
+			ddr_setval_s(ch, slice, _reg_PHY_USER_PATT3,
+				     patt.ui32[3]);
+			ddr_setval_s(ch, slice, _reg_PHY_USER_PATT4, patm);
+		}
+	}
+
+	/* CACS DLY */
+	data_l = board_cnf->cacs_dly + _f_scale_adj(board_cnf->cacs_dly_adj);
+	reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_FREQ_SEL_MULTICAST_EN),
+			   0x00U);
+	foreach_vch(ch) {
+		for (i = 0; i < _reg_PHY_CLK_CACS_SLAVE_DELAY_X_NUM - 4; i++) {
+			adj = _f_scale_adj(board_cnf->ch[ch].cacs_adj[i]);
+			ddrtbl_setval(_cnf_DDR_PHY_ADR_V_REGSET,
+				      _reg_PHY_CLK_CACS_SLAVE_DELAY_X[i],
+				      data_l + adj);
+			reg_ddrphy_write(ch,
+					 ddr_regdef_adr
+					 (_reg_PHY_CLK_CACS_SLAVE_DELAY_X[i]),
+					_cnf_DDR_PHY_ADR_V_REGSET
+					[ddr_regdef_adr
+					(_reg_PHY_CLK_CACS_SLAVE_DELAY_X[i]) -
+					DDR_PHY_ADR_V_REGSET_OFS]);
+		}
+
+		for (i = (_reg_PHY_CLK_CACS_SLAVE_DELAY_X_NUM - 4);
+		     i < _reg_PHY_CLK_CACS_SLAVE_DELAY_X_NUM; i++) {
+			adj = _f_scale_adj(board_cnf->ch[ch].cacs_adj[i]);
+			ddrtbl_setval(_cnf_DDR_PHY_ADR_G_REGSET,
+				      _reg_PHY_CLK_CACS_SLAVE_DELAY_X[i],
+				      data_l + adj);
+			reg_ddrphy_write(ch,
+					 ddr_regdef_adr
+					 (_reg_PHY_CLK_CACS_SLAVE_DELAY_X[i]),
+					_cnf_DDR_PHY_ADR_G_REGSET
+					[ddr_regdef_adr
+					(_reg_PHY_CLK_CACS_SLAVE_DELAY_X[i]) -
+					DDR_PHY_ADR_G_REGSET_OFS]);
+		}
+
+		if (ddr_phycaslice == 1) {
+			for (i = 0; i < 6; i++) {
+				adj = _f_scale_adj
+					(board_cnf->ch[ch].cacs_adj
+					[i +
+					_reg_PHY_CLK_CACS_SLAVE_DELAY_X_NUM]);
+				ddrtbl_setval(_cnf_DDR_PHY_ADR_V_REGSET,
+					      _reg_PHY_CLK_CACS_SLAVE_DELAY_X
+					      [i],
+					      data_l + adj);
+				reg_ddrphy_write(ch,
+						 ddr_regdef_adr
+					(_reg_PHY_CLK_CACS_SLAVE_DELAY_X[i]) +
+					0x0100,
+					_cnf_DDR_PHY_ADR_V_REGSET
+					[ddr_regdef_adr
+					(_reg_PHY_CLK_CACS_SLAVE_DELAY_X[i]) -
+					DDR_PHY_ADR_V_REGSET_OFS]);
+			}
+		}
+	}
+
+	reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_FREQ_SEL_MULTICAST_EN),
+			   BIT(ddr_regdef_lsb(_reg_PHY_FREQ_SEL_MULTICAST_EN)));
+
+	/* WDQDM DLY */
+	data_l = board_cnf->dqdm_dly_w;
+	foreach_vch(ch) {
+		for (slice = 0; slice < SLICE_CNT; slice++) {
+			for (i = 0; i <= 8; i++) {
+				dq = slice * 8 + i;
+				if (i == 8)
+					_adj = board_cnf->ch[ch].dm_adj_w[slice];
+				else
+					_adj = board_cnf->ch[ch].dq_adj_w[dq];
+				adj = _f_scale_adj(_adj);
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_CLK_WRX_SLAVE_DELAY[i],
+					     data_l + adj);
+			}
+		}
+	}
+
+	/* RDQDM DLY */
+	data_l = board_cnf->dqdm_dly_r;
+	foreach_vch(ch) {
+		for (slice = 0; slice < SLICE_CNT; slice++) {
+			for (i = 0; i <= 8; i++) {
+				dq = slice * 8 + i;
+				if (i == 8)
+					_adj = board_cnf->ch[ch].dm_adj_r[slice];
+				else
+					_adj = board_cnf->ch[ch].dq_adj_r[dq];
+				adj = _f_scale_adj(_adj);
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_RDDQS_X_FALL_SLAVE_DELAY
+					     [i], data_l + adj);
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_RDDQS_X_RISE_SLAVE_DELAY
+					     [i], data_l + adj);
+			}
+		}
+	}
+}
+
+/* DBSC register setting functions */
+static void dbsc_regset_pre(void)
+{
+	uint32_t ch, csab;
+	uint32_t data_l;
+
+	/* PRIMARY SETTINGS */
+	/* LPDDR4, BL=16, DFI interface */
+	mmio_write_32(DBSC_DBKIND, 0x0000000a);
+	mmio_write_32(DBSC_DBBL, 0x00000002);
+	mmio_write_32(DBSC_DBPHYCONF0, 0x00000001);
+
+	/* FREQRATIO=2 */
+	mmio_write_32(DBSC_DBSYSCONF1, 0x00000002);
+
+	/* Chanel map (H3 Ver.1.x) */
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11))
+		mmio_write_32(DBSC_DBSCHCNT1, 0x00001010);
+
+	/* DRAM SIZE REGISTER:
+	 * set all ranks as density=0(4Gb) for PHY initialization
+	 */
+	foreach_vch(ch) {
+		for (csab = 0; csab < 4; csab++) {
+			mmio_write_32(DBSC_DBMEMCONF(ch, csab),
+				      DBMEMCONF_REGD(0));
+		}
+	}
+
+	if (prr_product == PRR_PRODUCT_M3) {
+		data_l = 0xe4e4e4e4;
+		foreach_ech(ch) {
+			if ((ddr_phyvalid & (1U << ch)))
+				data_l = (data_l & (~(0x000000FF << (ch * 8))))
+				    | (((board_cnf->ch[ch].dqs_swap & 0x0003)
+					| ((board_cnf->ch[ch].dqs_swap & 0x0030)
+					   >> 2)
+					| ((board_cnf->ch[ch].dqs_swap & 0x0300)
+					   >> 4)
+					| ((board_cnf->ch[ch].dqs_swap & 0x3000)
+					   >> 6)) << (ch * 8));
+		}
+		mmio_write_32(DBSC_DBBSWAP, data_l);
+	}
+}
+
+static void dbsc_regset(void)
+{
+	int32_t i;
+	uint32_t ch;
+	uint32_t data_l;
+	uint32_t data_l2;
+	uint32_t tmp[4];
+
+	/* RFC */
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut == PRR_PRODUCT_20) &&
+	    (max_density == 0)) {
+		js2[js2_trfcab] =
+		    _f_scale(ddr_mbps, ddr_mbpsdiv,
+			     1UL * jedec_spec2_trfc_ab[1] * 1000, 0);
+	} else {
+		js2[js2_trfcab] =
+		    _f_scale(ddr_mbps, ddr_mbpsdiv,
+			     1UL * jedec_spec2_trfc_ab[max_density] *
+			     1000, 0);
+	}
+
+	/* DBTR0.CL  : RL */
+	mmio_write_32(DBSC_DBTR(0), RL);
+
+	/* DBTR1.CWL : WL */
+	mmio_write_32(DBSC_DBTR(1), WL);
+
+	/* DBTR2.AL  : 0 */
+	mmio_write_32(DBSC_DBTR(2), 0);
+
+	/* DBTR3.TRCD: tRCD */
+	mmio_write_32(DBSC_DBTR(3), js2[js2_trcd]);
+
+	/* DBTR4.TRPA,TRP: tRPab,tRPpb */
+	mmio_write_32(DBSC_DBTR(4), (js2[js2_trpab] << 16) | js2[js2_trppb]);
+
+	/* DBTR5.TRC : use tRCpb */
+	mmio_write_32(DBSC_DBTR(5), js2[js2_trcpb]);
+
+	/* DBTR6.TRAS : tRAS */
+	mmio_write_32(DBSC_DBTR(6), js2[js2_tras]);
+
+	/* DBTR7.TRRD : tRRD */
+	mmio_write_32(DBSC_DBTR(7), (js2[js2_trrd] << 16) | js2[js2_trrd]);
+
+	/* DBTR8.TFAW : tFAW */
+	mmio_write_32(DBSC_DBTR(8), js2[js2_tfaw]);
+
+	/* DBTR9.TRDPR : tRTP */
+	mmio_write_32(DBSC_DBTR(9), js2[js2_trtp]);
+
+	/* DBTR10.TWR : nWR */
+	mmio_write_32(DBSC_DBTR(10), js1[js1_ind].nwr);
+
+	/*
+	 * DBTR11.TRDWR : RL +  BL / 2 + Rounddown(tRPST) + PHY_ODTLoff -
+	 * 		  odtlon + tDQSCK - tODTon,min +
+	 * 		  PCB delay (out+in) + tPHY_ODToff
+	 */
+	mmio_write_32(DBSC_DBTR(11),
+		      RL + (16 / 2) + 1 + 2 - js1[js1_ind].odtlon +
+		      js2[js2_tdqsck] - js2[js2_tODTon_min] +
+		      _f_scale(ddr_mbps, ddr_mbpsdiv, 1300, 0));
+
+	/* DBTR12.TWRRD : WL + 1 + BL/2 + tWTR */
+	data_l = WL + 1 + (16 / 2) + js2[js2_twtr];
+	mmio_write_32(DBSC_DBTR(12), (data_l << 16) | data_l);
+
+	/* DBTR13.TRFCAB : tRFCab */
+	mmio_write_32(DBSC_DBTR(13), (js2[js2_trfcab]));
+
+	/* DBTR14.TCKEHDLL,tCKEH : tCKEHCMD,tCKEHCMD */
+	mmio_write_32(DBSC_DBTR(14),
+		      (js2[js2_tckehcmd] << 16) | (js2[js2_tckehcmd]));
+
+	/* DBTR15.TCKESR,TCKEL : tSR,tCKELPD */
+	mmio_write_32(DBSC_DBTR(15), (js2[js2_tsr] << 16) | (js2[js2_tckelpd]));
+
+	/* DBTR16 */
+	/* WDQL : tphy_wrlat + tphy_wrdata */
+	tmp[0] = ddrtbl_getval(_cnf_DDR_PI_REGSET, _reg_PI_WRLAT_F1);
+	/* DQENLTNCY : tphy_wrlat = WL-2 : PHY_WRITE_PATH_LAT_ADD == 0
+	 *             tphy_wrlat = WL-3 : PHY_WRITE_PATH_LAT_ADD != 0
+	 */
+	tmp[1] = ddrtbl_getval(_cnf_DDR_PI_REGSET, _reg_PI_WRLAT_ADJ_F1);
+	/* DQL : tphy_rdlat + trdata_en */
+	/* it is not important for dbsc */
+	tmp[2] = RL + 16;
+	/* DQIENLTNCY : trdata_en */
+	tmp[3] = ddrtbl_getval(_cnf_DDR_PI_REGSET, _reg_PI_RDLAT_ADJ_F1) - 1;
+	mmio_write_32(DBSC_DBTR(16),
+		      (tmp[3] << 24) | (tmp[2] << 16) | (tmp[1] << 8) | tmp[0]);
+
+	/* DBTR24 */
+	/* WRCSLAT = WRLAT -5 */
+	tmp[0] -= 5;
+	/* WRCSGAP = 5 */
+	tmp[1] = 5;
+	/* RDCSLAT = RDLAT_ADJ +2 */
+	if (prr_product == PRR_PRODUCT_M3) {
+		tmp[2] = tmp[3];
+	} else {
+		tmp[2] = tmp[3] + 2;
+	}
+	/* RDCSGAP = 6 */
+	if (prr_product == PRR_PRODUCT_M3) {
+		tmp[3] = 4;
+	} else {
+		tmp[3] = 6;
+	}
+	mmio_write_32(DBSC_DBTR(24),
+		      (tmp[3] << 24) | (tmp[2] << 16) | (tmp[1] << 8) | tmp[0]);
+
+	/* DBTR17.TMODRD,TMOD,TRDMR: tMRR,tMRD,(0) */
+	mmio_write_32(DBSC_DBTR(17),
+		      (js2[js2_tmrr] << 24) | (js2[js2_tmrd] << 16));
+
+	/* DBTR18.RODTL, RODTA, WODTL, WODTA : do not use in LPDDR4 */
+	mmio_write_32(DBSC_DBTR(18), 0);
+
+	/* DBTR19.TZQCL, TZQCS : do not use in LPDDR4 */
+	mmio_write_32(DBSC_DBTR(19), 0);
+
+	/* DBTR20.TXSDLL, TXS : tRFCab+tCKEHCMD */
+	data_l = js2[js2_trfcab] + js2[js2_tckehcmd];
+	mmio_write_32(DBSC_DBTR(20), (data_l << 16) | data_l);
+
+	/* DBTR21.TCCD */
+	/* DBTR23.TCCD */
+	/* H3 Ver.1.0 cannot use TBTR23 feature */
+	if (ddr_tccd == 8 &&
+	    !((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_10))
+	    ) {
+		data_l = 8;
+		mmio_write_32(DBSC_DBTR(21), (data_l << 16) | data_l);
+		mmio_write_32(DBSC_DBTR(23), 0x00000002);
+	} else if (ddr_tccd <= 11) {
+		data_l = 11;
+		mmio_write_32(DBSC_DBTR(21), (data_l << 16) | data_l);
+		mmio_write_32(DBSC_DBTR(23), 0x00000000);
+	} else {
+		data_l = ddr_tccd;
+		mmio_write_32(DBSC_DBTR(21), (data_l << 16) | data_l);
+		mmio_write_32(DBSC_DBTR(23), 0x00000000);
+	}
+
+	/* DBTR22.ZQLAT : */
+	data_l = js2[js2_tzqcalns] * 100;	/*  1000 * 1000 ps */
+	data_l = (data_l << 16) | (js2[js2_tzqlat] + 24 + 20);
+	mmio_write_32(DBSC_DBTR(22), data_l);
+
+	/* DBTR25 : do not use in LPDDR4 */
+	mmio_write_32(DBSC_DBTR(25), 0);
+
+	/* DBRNK : */
+	/*
+	 * DBSC_DBRNK2 rkrr
+	 * DBSC_DBRNK3 rkrw
+	 * DBSC_DBRNK4 rkwr
+	 * DBSC_DBRNK5 rkww
+	 */
+#define _par_DBRNK_VAL		(0x7007)
+
+	for (i = 0; i < 4; i++) {
+		data_l = (_par_DBRNK_VAL >> (i * 4)) & 0x0f;
+		if ((prr_product == PRR_PRODUCT_H3) &&
+		    (prr_cut > PRR_PRODUCT_11) && (i == 0)) {
+			data_l += 1;
+		}
+		data_l2 = 0;
+		foreach_vch(ch) {
+			data_l2 = data_l2 | (data_l << (4 * ch));
+		}
+		mmio_write_32(DBSC_DBRNK(2 + i), data_l2);
+	}
+	mmio_write_32(DBSC_DBADJ0, 0x00000000);
+
+	/* timing registers for Scheduler */
+	/* SCFCTST0 */
+	/* SCFCTST0 ACT-ACT */
+	tmp[3] = 1UL * js2[js2_trcpb] * 800 * ddr_mbpsdiv / ddr_mbps;
+	/* SCFCTST0 RDA-ACT */
+	tmp[2] =
+	    1UL * ((16 / 2) + js2[js2_trtp] - 8 +
+		   js2[js2_trppb]) * 800 * ddr_mbpsdiv / ddr_mbps;
+	/* SCFCTST0 WRA-ACT */
+	tmp[1] =
+	    1UL * (WL + 1 + (16 / 2) +
+		   js1[js1_ind].nwr) * 800 * ddr_mbpsdiv / ddr_mbps;
+	/* SCFCTST0 PRE-ACT */
+	tmp[0] = 1UL * js2[js2_trppb];
+	mmio_write_32(DBSC_SCFCTST0,
+		      (tmp[3] << 24) | (tmp[2] << 16) | (tmp[1] << 8) | tmp[0]);
+
+	/* SCFCTST1 */
+	/* SCFCTST1 RD-WR */
+	tmp[3] =
+	    1UL * (mmio_read_32(DBSC_DBTR(11)) & 0xff) * 800 * ddr_mbpsdiv /
+	    ddr_mbps;
+	/* SCFCTST1 WR-RD */
+	tmp[2] =
+	    1UL * (mmio_read_32(DBSC_DBTR(12)) & 0xff) * 800 * ddr_mbpsdiv /
+	    ddr_mbps;
+	/* SCFCTST1 ACT-RD/WR */
+	tmp[1] = 1UL * js2[js2_trcd] * 800 * ddr_mbpsdiv / ddr_mbps;
+	/* SCFCTST1 ASYNCOFS */
+	tmp[0] = 12;
+	mmio_write_32(DBSC_SCFCTST1,
+		      (tmp[3] << 24) | (tmp[2] << 16) | (tmp[1] << 8) | tmp[0]);
+
+	/* DBSCHRW1 */
+	/* DBSCHRW1 SCTRFCAB */
+	tmp[0] = 1UL * js2[js2_trfcab] * 800 * ddr_mbpsdiv / ddr_mbps;
+	data_l = (((mmio_read_32(DBSC_DBTR(16)) & 0x00FF0000) >> 16)
+		 + (mmio_read_32(DBSC_DBTR(22)) & 0x0000FFFF)
+		 + (0x28 * 2)) * 400 * 2 * ddr_mbpsdiv / ddr_mbps + 7;
+	if (tmp[0] < data_l)
+		tmp[0] = data_l;
+
+	if ((prr_product == PRR_PRODUCT_M3) && (prr_cut < PRR_PRODUCT_30)) {
+		mmio_write_32(DBSC_DBSCHRW1, tmp[0]
+			+ ((mmio_read_32(DBSC_DBTR(22)) & 0x0000FFFF)
+			* 400 * 2 * ddr_mbpsdiv + (ddr_mbps - 1)) /
+			ddr_mbps - 3);
+	} else {
+		mmio_write_32(DBSC_DBSCHRW1, tmp[0]
+			+ ((mmio_read_32(DBSC_DBTR(22)) & 0x0000FFFF)
+			* 400 * 2 * ddr_mbpsdiv + (ddr_mbps - 1)) /
+			ddr_mbps);
+	}
+
+	/* QOS and CAM */
+#ifdef ddr_qos_init_setting	/*  only for non qos_init */
+	/*wbkwait(0004), wbkmdhi(4,2),wbkmdlo(1,8) */
+	mmio_write_32(DBSC_DBCAM0CNF1, 0x00043218);
+	/*0(fillunit),8(dirtymax),4(dirtymin) */
+	mmio_write_32(DBSC_DBCAM0CNF2, 0x000000F4);
+	/*stop_tolerance */
+	mmio_write_32(DBSC_DBSCHRW0, 0x22421111);
+	/*rd-wr/wr-rd toggle priority */
+	mmio_write_32(DBSC_SCFCTST2, 0x012F1123);
+	mmio_write_32(DBSC_DBSCHSZ0, 0x00000001);
+	mmio_write_32(DBSC_DBSCHCNT0, 0x000F0037);
+
+	/* QoS Settings */
+	mmio_write_32(DBSC_DBSCHQOS00, 0x00000F00U);
+	mmio_write_32(DBSC_DBSCHQOS01, 0x00000B00U);
+	mmio_write_32(DBSC_DBSCHQOS02, 0x00000000U);
+	mmio_write_32(DBSC_DBSCHQOS03, 0x00000000U);
+	mmio_write_32(DBSC_DBSCHQOS40, 0x00000300U);
+	mmio_write_32(DBSC_DBSCHQOS41, 0x000002F0U);
+	mmio_write_32(DBSC_DBSCHQOS42, 0x00000200U);
+	mmio_write_32(DBSC_DBSCHQOS43, 0x00000100U);
+	mmio_write_32(DBSC_DBSCHQOS90, 0x00000100U);
+	mmio_write_32(DBSC_DBSCHQOS91, 0x000000F0U);
+	mmio_write_32(DBSC_DBSCHQOS92, 0x000000A0U);
+	mmio_write_32(DBSC_DBSCHQOS93, 0x00000040U);
+	mmio_write_32(DBSC_DBSCHQOS120, 0x00000040U);
+	mmio_write_32(DBSC_DBSCHQOS121, 0x00000030U);
+	mmio_write_32(DBSC_DBSCHQOS122, 0x00000020U);
+	mmio_write_32(DBSC_DBSCHQOS123, 0x00000010U);
+	mmio_write_32(DBSC_DBSCHQOS130, 0x00000100U);
+	mmio_write_32(DBSC_DBSCHQOS131, 0x000000F0U);
+	mmio_write_32(DBSC_DBSCHQOS132, 0x000000A0U);
+	mmio_write_32(DBSC_DBSCHQOS133, 0x00000040U);
+	mmio_write_32(DBSC_DBSCHQOS140, 0x000000C0U);
+	mmio_write_32(DBSC_DBSCHQOS141, 0x000000B0U);
+	mmio_write_32(DBSC_DBSCHQOS142, 0x00000080U);
+	mmio_write_32(DBSC_DBSCHQOS143, 0x00000040U);
+	mmio_write_32(DBSC_DBSCHQOS150, 0x00000040U);
+	mmio_write_32(DBSC_DBSCHQOS151, 0x00000030U);
+	mmio_write_32(DBSC_DBSCHQOS152, 0x00000020U);
+	mmio_write_32(DBSC_DBSCHQOS153, 0x00000010U);
+
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif /* ddr_qos_init_setting */
+	/* H3 Ver.1.1 need to set monitor function */
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut == PRR_PRODUCT_11)) {
+		mmio_write_32(DBSC_DBMONCONF4, 0x00700000);
+	}
+
+	if (prr_product == PRR_PRODUCT_H3) {
+		if (prr_cut == PRR_PRODUCT_10) {
+			/* resrdis, simple mode, sc off */
+			mmio_write_32(DBSC_DBBCAMDIS, 0x00000007);
+		} else if (prr_cut == PRR_PRODUCT_11) {
+			/* resrdis, simple mode         */
+			mmio_write_32(DBSC_DBBCAMDIS, 0x00000005);
+		} else if (prr_cut < PRR_PRODUCT_30) {
+			/* H3 Ver.2.0                   */
+			/* resrdis                      */
+			mmio_write_32(DBSC_DBBCAMDIS, 0x00000001);
+		} else {	/* H3 Ver.3.0(include H3N)      */
+			/* exprespque                   */
+			mmio_write_32(DBSC_DBBCAMDIS, 0x00000010);
+		}
+	} else {		/* M3-W/M3-N/V3H                */
+		/* resrdis                      */
+		mmio_write_32(DBSC_DBBCAMDIS, 0x00000001);
+	}
+}
+
+static void dbsc_regset_post(void)
+{
+	uint32_t ch, cs;
+	uint32_t data_l;
+	uint32_t slice, rdlat_max, rdlat_min;
+
+	rdlat_max = 0;
+	rdlat_min = 0xffff;
+	foreach_vch(ch) {
+		for (cs = 0; cs < CS_CNT; cs++) {
+			if ((ch_have_this_cs[cs] & (1U << ch)) != 0) {
+				for (slice = 0; slice < SLICE_CNT; slice++) {
+					ddr_setval_s(ch, slice,
+						     _reg_PHY_PER_CS_TRAINING_INDEX,
+						     cs);
+					data_l = ddr_getval_s(ch, slice,
+							      _reg_PHY_RDDQS_LATENCY_ADJUST);
+					if (data_l > rdlat_max)
+						rdlat_max = data_l;
+					if (data_l < rdlat_min)
+						rdlat_min = data_l;
+				}
+			}
+		}
+	}
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut > PRR_PRODUCT_11)) {
+#if RCAR_DRAM_SPLIT == 2
+		if (board_cnf->phyvalid == 0x05) {
+			mmio_write_32(DBSC_DBTR(24),
+				      (rdlat_max << 24) + (rdlat_min << 16) +
+				      mmio_read_32(DBSC_DBTR(24)));
+		} else {
+			mmio_write_32(DBSC_DBTR(24),
+				      ((rdlat_max * 2 - rdlat_min + 4) << 24) +
+				      ((rdlat_min + 2) << 16) +
+				      mmio_read_32(DBSC_DBTR(24)));
+		}
+#else /*RCAR_DRAM_SPLIT == 2 */
+		mmio_write_32(DBSC_DBTR(24),
+			      ((rdlat_max * 2 - rdlat_min + 4) << 24) +
+			      ((rdlat_min + 2) << 16) +
+			      mmio_read_32(DBSC_DBTR(24)));
+#endif /*RCAR_DRAM_SPLIT == 2 */
+	} else {
+		mmio_write_32(DBSC_DBTR(24),
+			      ((rdlat_max + 2) << 24) +
+			      ((rdlat_max + 2) << 16) +
+			      mmio_read_32(DBSC_DBTR(24)));
+	}
+
+	/* set ddr density information */
+	foreach_ech(ch) {
+		for (cs = 0; cs < CS_CNT; cs++) {
+			if (ddr_density[ch][cs] == 0xff) {
+				mmio_write_32(DBSC_DBMEMCONF(ch, cs), 0x00);
+			} else {
+				mmio_write_32(DBSC_DBMEMCONF(ch, cs),
+					      DBMEMCONF_REGD(ddr_density[ch]
+							     [cs]));
+			}
+		}
+		mmio_write_32(DBSC_DBMEMCONF(ch, 2), 0x00000000);
+		mmio_write_32(DBSC_DBMEMCONF(ch, 3), 0x00000000);
+	}
+
+	mmio_write_32(DBSC_DBBUS0CNF1, 0x00000010);
+
+	/*set DBI */
+	if (board_cnf->dbi_en)
+		mmio_write_32(DBSC_DBDBICNT, 0x00000003);
+
+	/* H3 Ver.2.0 or later/M3-N/V3H DBI wa */
+	if ((((prr_product == PRR_PRODUCT_H3) &&
+	      (prr_cut > PRR_PRODUCT_11)) ||
+	     (prr_product == PRR_PRODUCT_M3N) ||
+	     (prr_product == PRR_PRODUCT_V3H)) &&
+	    board_cnf->dbi_en)
+		reg_ddrphy_write_a(0x00001010, 0x01000000);
+
+	/*set REFCYCLE */
+	data_l = (get_refperiod()) * ddr_mbps / 2000 / ddr_mbpsdiv;
+	mmio_write_32(DBSC_DBRFCNF1, 0x00080000 | (data_l & 0x0000ffff));
+	mmio_write_32(DBSC_DBRFCNF2, 0x00010000 | DBSC_REFINTS);
+
+#if RCAR_REWT_TRAINING != 0
+	/* Periodic-WriteDQ Training seeting */
+	if (((prr_product == PRR_PRODUCT_H3) &&
+	     (prr_cut <= PRR_PRODUCT_11)) ||
+	    ((prr_product == PRR_PRODUCT_M3) &&
+	     (prr_cut == PRR_PRODUCT_10))) {
+		/* non : H3 Ver.1.x/M3-W Ver.1.0 not support */
+	} else {
+		/* H3 Ver.2.0 or later/M3-W Ver.1.1 or later/M3-N/V3H */
+		mmio_write_32(DBSC_DBDFIPMSTRCNF, 0x00000000);
+
+		ddr_setval_ach_as(_reg_PHY_WDQLVL_PATT, 0x04);
+		ddr_setval_ach_as(_reg_PHY_WDQLVL_QTR_DLY_STEP, 0x0F);
+		ddr_setval_ach_as(_reg_PHY_WDQLVL_DLY_STEP, 0x50);
+		ddr_setval_ach_as(_reg_PHY_WDQLVL_DQDM_SLV_DLY_START, 0x0300);
+
+		ddr_setval_ach(_reg_PI_WDQLVL_CS_MAP,
+			       ddrtbl_getval(_cnf_DDR_PI_REGSET,
+					     _reg_PI_WDQLVL_CS_MAP));
+		ddr_setval_ach(_reg_PI_LONG_COUNT_MASK, 0x1f);
+		ddr_setval_ach(_reg_PI_WDQLVL_VREF_EN, 0x00);
+		ddr_setval_ach(_reg_PI_WDQLVL_ROTATE, 0x01);
+		ddr_setval_ach(_reg_PI_TREF_F0, 0x0000);
+		ddr_setval_ach(_reg_PI_TREF_F1, 0x0000);
+		ddr_setval_ach(_reg_PI_TREF_F2, 0x0000);
+
+		if (prr_product == PRR_PRODUCT_M3) {
+			ddr_setval_ach(_reg_PI_WDQLVL_EN, 0x02);
+		} else {
+			ddr_setval_ach(_reg_PI_WDQLVL_EN_F1, 0x02);
+		}
+		ddr_setval_ach(_reg_PI_WDQLVL_PERIODIC, 0x01);
+
+		/* DFI_PHYMSTR_ACK , WTmode setting */
+		/* DFI_PHYMSTR_ACK: WTmode =b'01 */
+		mmio_write_32(DBSC_DBDFIPMSTRCNF, 0x00000011);
+	}
+#endif /* RCAR_REWT_TRAINING */
+	/* periodic dram zqcal enable */
+	mmio_write_32(DBSC_DBCALCNF, 0x01000010);
+
+	/* periodic phy ctrl update enable */
+	if (((prr_product == PRR_PRODUCT_H3) &&
+	     (prr_cut <= PRR_PRODUCT_11)) ||
+	    ((prr_product == PRR_PRODUCT_M3) &&
+	     (prr_cut < PRR_PRODUCT_30))) {
+		/* non : H3 Ver.1.x/M3-W Ver.1.x not support */
+	} else {
+#if RCAR_DRAM_SPLIT == 2
+		if ((prr_product == PRR_PRODUCT_H3) &&
+		    (board_cnf->phyvalid == 0x05))
+			mmio_write_32(DBSC_DBDFICUPDCNF, 0x2a240001);
+		else
+			mmio_write_32(DBSC_DBDFICUPDCNF, 0x28240001);
+#else /* RCAR_DRAM_SPLIT == 2 */
+		mmio_write_32(DBSC_DBDFICUPDCNF, 0x28240001);
+#endif /* RCAR_DRAM_SPLIT == 2 */
+	}
+
+#ifdef DDR_BACKUPMODE
+	/* SRX */
+	if (ddr_backup == DRAM_BOOT_STATUS_WARM) {
+#ifdef DDR_BACKUPMODE_HALF		/* for Half channel(ch0, 1 only) */
+		NOTICE("BL2: [DEBUG_MESS] DDR_BACKUPMODE_HALF\n");
+		send_dbcmd(0x0A040001);
+		if (Prr_Product == PRR_PRODUCT_H3)
+			send_dbcmd(0x0A140001);
+#else /* DDR_BACKUPMODE_HALF */		/* for All channels */
+		send_dbcmd(0x0A840001);
+#endif /* DDR_BACKUPMODE_HALF */
+	}
+#endif /* DDR_BACKUPMODE */
+
+	/* set Auto Refresh */
+	mmio_write_32(DBSC_DBRFEN, 0x00000001);
+
+#if RCAR_REWT_TRAINING != 0
+	/* Periodic WriteDQ Traning */
+	if (((prr_product == PRR_PRODUCT_H3) &&
+	     (prr_cut <= PRR_PRODUCT_11)) ||
+	    ((prr_product == PRR_PRODUCT_M3) &&
+	     (prr_cut == PRR_PRODUCT_10))) {
+		/* non : H3 Ver.1.x/M3-W Ver.1.0 not support */
+	} else {
+		/* H3 Ver.2.0 or later/M3-W Ver.1.1 or later/M3-N/V3H */
+		ddr_setval_ach(_reg_PI_WDQLVL_INTERVAL, 0x0100);
+	}
+#endif /* RCAR_REWT_TRAINING */
+
+	/* dram access enable */
+	mmio_write_32(DBSC_DBACEN, 0x00000001);
+
+	MSG_LF(__func__ "(done)");
+}
+
+/* DFI_INIT_START */
+static uint32_t dfi_init_start(void)
+{
+	uint32_t ch;
+	uint32_t phytrainingok;
+	uint32_t retry;
+	uint32_t data_l;
+	const uint32_t RETRY_MAX = 0x10000;
+
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) {
+		/* PLL3 Disable */
+		/* protect register interface */
+		ddrphy_regif_idle();
+
+		pll3_control(0);
+
+		/* init start */
+		/* dbdficnt0:
+		 * dfi_dram_clk_disable=1
+		 * dfi_frequency = 0
+		 * freq_ratio = 01 (2:1)
+		 * init_start =0
+		 */
+		foreach_vch(ch)
+		    mmio_write_32(DBSC_DBDFICNT(ch), 0x00000F10);
+		dsb_sev();
+
+		/* dbdficnt0:
+		 * dfi_dram_clk_disable=1
+		 * dfi_frequency = 0
+		 * freq_ratio = 01 (2:1)
+		 * init_start =1
+		 */
+		foreach_vch(ch)
+		    mmio_write_32(DBSC_DBDFICNT(ch), 0x00000F11);
+		dsb_sev();
+
+	} else {
+		ddr_setval_ach_as(_reg_PHY_DLL_RST_EN, 0x02);
+		dsb_sev();
+		ddrphy_regif_idle();
+	}
+
+	/* dll_rst negate */
+	foreach_vch(ch)
+	    mmio_write_32(DBSC_DBPDCNT3(ch), 0x0000CF01);
+	dsb_sev();
+
+	/* wait init_complete */
+	phytrainingok = 0;
+	retry = 0;
+	while (retry++ < RETRY_MAX) {
+		foreach_vch(ch) {
+			data_l = mmio_read_32(DBSC_DBDFISTAT(ch));
+			if (data_l & 0x00000001)
+				phytrainingok |= (1U << ch);
+		}
+		dsb_sev();
+		if (phytrainingok == ddr_phyvalid)
+			break;
+		if (retry % 256 == 0)
+			ddr_setval_ach_as(_reg_SC_PHY_RX_CAL_START, 0x01);
+	}
+
+	/* all ch ok? */
+	if ((phytrainingok & ddr_phyvalid) != ddr_phyvalid)
+		return 0xff;
+
+	/* dbdficnt0:
+	 * dfi_dram_clk_disable=0
+	 * dfi_frequency = 0
+	 * freq_ratio = 01 (2:1)
+	 * init_start =0
+	 */
+	foreach_vch(ch)
+	    mmio_write_32(DBSC_DBDFICNT(ch), 0x00000010);
+	dsb_sev();
+
+	return 0;
+}
+
+/* drivablity setting : CMOS MODE ON/OFF */
+static void change_lpddr4_en(uint32_t mode)
+{
+	uint32_t ch;
+	uint32_t i;
+	uint32_t data_l;
+	const uint32_t _reg_PHY_PAD_DRIVE_X[3] = {
+		_reg_PHY_PAD_ADDR_DRIVE,
+		_reg_PHY_PAD_CLK_DRIVE,
+		_reg_PHY_PAD_CS_DRIVE
+	};
+
+	foreach_vch(ch) {
+		for (i = 0; i < 3; i++) {
+			data_l = ddr_getval(ch, _reg_PHY_PAD_DRIVE_X[i]);
+			if (mode) {
+				data_l |= (1U << 14);
+			} else {
+				data_l &= ~(1U << 14);
+			}
+			ddr_setval(ch, _reg_PHY_PAD_DRIVE_X[i], data_l);
+		}
+	}
+}
+
+/* drivablity setting */
+static uint32_t set_term_code(void)
+{
+	int32_t i;
+	uint32_t ch, index;
+	uint32_t data_l;
+	uint32_t chip_id[2];
+	uint32_t term_code;
+	uint32_t override;
+	uint32_t pvtr;
+	uint32_t pvtp;
+	uint32_t pvtn;
+
+	term_code = ddrtbl_getval(_cnf_DDR_PHY_ADR_G_REGSET,
+				  _reg_PHY_PAD_DATA_TERM);
+	override = 0;
+	for (i = 0; i < 2; i++)
+		chip_id[i] = mmio_read_32(LIFEC_CHIPID(i));
+
+	index = 0;
+	while (1) {
+		if (termcode_by_sample[index][0] == 0xffffffff) {
+			break;
+		}
+		if ((termcode_by_sample[index][0] == chip_id[0]) &&
+		    (termcode_by_sample[index][1] == chip_id[1])) {
+			term_code = termcode_by_sample[index][2];
+			override = 1;
+			break;
+		}
+		index++;
+	}
+
+	if (override) {
+		for (index = 0; index < _reg_PHY_PAD_TERM_X_NUM; index++) {
+			data_l =
+			    ddrtbl_getval(_cnf_DDR_PHY_ADR_G_REGSET,
+					  _reg_PHY_PAD_TERM_X[index]);
+			data_l = (data_l & 0xfffe0000) | term_code;
+			ddr_setval_ach(_reg_PHY_PAD_TERM_X[index], data_l);
+		}
+	} else if ((prr_product == PRR_PRODUCT_M3) &&
+		   (prr_cut == PRR_PRODUCT_10)) {
+		/*  non */
+	} else {
+		ddr_setval_ach(_reg_PHY_PAD_TERM_X[0],
+			       (ddrtbl_getval
+				(_cnf_DDR_PHY_ADR_G_REGSET,
+				 _reg_PHY_PAD_TERM_X[0]) & 0xFFFE0000));
+		ddr_setval_ach(_reg_PHY_CAL_CLEAR_0, 0x01);
+		ddr_setval_ach(_reg_PHY_CAL_START_0, 0x01);
+		foreach_vch(ch) {
+			do {
+				data_l =
+				    ddr_getval(ch, _reg_PHY_CAL_RESULT2_OBS_0);
+			} while (!(data_l & 0x00800000));
+		}
+		if ((prr_product == PRR_PRODUCT_H3) &&
+		    (prr_cut <= PRR_PRODUCT_11)) {
+			foreach_vch(ch) {
+				data_l = ddr_getval(ch, _reg_PHY_PAD_TERM_X[0]);
+				pvtr = (data_l >> 12) & 0x1f;
+				pvtr += 8;
+				if (pvtr > 0x1f)
+					pvtr = 0x1f;
+				data_l =
+				    ddr_getval(ch, _reg_PHY_CAL_RESULT2_OBS_0);
+				pvtn = (data_l >> 6) & 0x03f;
+				pvtp = (data_l >> 0) & 0x03f;
+
+				for (index = 0; index < _reg_PHY_PAD_TERM_X_NUM;
+				     index++) {
+					data_l =
+					    ddrtbl_getval
+					    (_cnf_DDR_PHY_ADR_G_REGSET,
+					     _reg_PHY_PAD_TERM_X[index]);
+					data_l = (data_l & 0xfffe0000)
+					    | (pvtr << 12)
+					    | (pvtn << 6)
+					    | (pvtp);
+					ddr_setval(ch,
+						   _reg_PHY_PAD_TERM_X[index],
+						   data_l);
+				}
+			}
+		} else {
+			/* M3-W Ver.1.1 or later/H3 Ver.2.0 or later/M3-N/V3H */
+			foreach_vch(ch) {
+				for (index = 0; index < _reg_PHY_PAD_TERM_X_NUM;
+				     index++) {
+					data_l =
+					    ddr_getval(ch,
+						       _reg_PHY_PAD_TERM_X
+						       [index]);
+					ddr_setval(ch,
+						   _reg_PHY_PAD_TERM_X[index],
+						   (data_l & 0xFFFE0FFF) |
+						   0x00015000);
+				}
+			}
+		}
+	}
+
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) {
+		/* non */
+	} else {
+		ddr_padcal_tcompensate_getinit(override);
+	}
+
+	return 0;
+}
+
+/* DDR mode register setting */
+static void ddr_register_set(void)
+{
+	int32_t fspwp;
+	uint32_t tmp;
+
+	for (fspwp = 1; fspwp >= 0; fspwp--) {
+		/*MR13, fspwp */
+		send_dbcmd(0x0e840d08 | ((2 - fspwp) << 6));
+
+		tmp =
+		    ddrtbl_getval(_cnf_DDR_PI_REGSET,
+				  reg_pi_mr1_data_fx_csx[fspwp][0]);
+		send_dbcmd(0x0e840100 | tmp);
+
+		tmp =
+		    ddrtbl_getval(_cnf_DDR_PI_REGSET,
+				  reg_pi_mr2_data_fx_csx[fspwp][0]);
+		send_dbcmd(0x0e840200 | tmp);
+
+		tmp =
+		    ddrtbl_getval(_cnf_DDR_PI_REGSET,
+				  reg_pi_mr3_data_fx_csx[fspwp][0]);
+		send_dbcmd(0x0e840300 | tmp);
+
+		tmp =
+		    ddrtbl_getval(_cnf_DDR_PI_REGSET,
+				  reg_pi_mr11_data_fx_csx[fspwp][0]);
+		send_dbcmd(0x0e840b00 | tmp);
+
+		tmp =
+		    ddrtbl_getval(_cnf_DDR_PI_REGSET,
+				  reg_pi_mr12_data_fx_csx[fspwp][0]);
+		send_dbcmd(0x0e840c00 | tmp);
+
+		tmp =
+		    ddrtbl_getval(_cnf_DDR_PI_REGSET,
+				  reg_pi_mr14_data_fx_csx[fspwp][0]);
+		send_dbcmd(0x0e840e00 | tmp);
+		/* MR22 */
+		send_dbcmd(0x0e841616);
+
+		/* ZQCAL start */
+		send_dbcmd(0x0d84004F);
+
+		/* ZQLAT */
+		send_dbcmd(0x0d840051);
+	}
+
+	/* MR13, fspwp */
+	send_dbcmd(0x0e840d08);
+}
+
+/* Training handshake functions */
+static inline uint32_t wait_freqchgreq(uint32_t assert)
+{
+	uint32_t data_l;
+	uint32_t count;
+	uint32_t ch;
+
+	count = 100000;
+
+	/* H3 Ver.1.x cannot see frqchg_req */
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) {
+		return 0;
+	}
+
+	if (assert) {
+		do {
+			data_l = 1;
+			foreach_vch(ch) {
+				data_l &= mmio_read_32(DBSC_DBPDSTAT(ch));
+			}
+			count = count - 1;
+		} while (((data_l & 0x01) != 0x01) & (count != 0));
+	} else {
+		do {
+			data_l = 0;
+			foreach_vch(ch) {
+				data_l |= mmio_read_32(DBSC_DBPDSTAT(ch));
+			}
+			count = count - 1;
+		} while (((data_l & 0x01) != 0x00) & (count != 0));
+	}
+
+	return (count == 0);
+}
+
+static inline void set_freqchgack(uint32_t assert)
+{
+	uint32_t ch;
+	uint32_t data_l;
+
+	if (assert)
+		data_l = 0x0CF20000;
+	else
+		data_l = 0x00000000;
+
+	foreach_vch(ch)
+	    mmio_write_32(DBSC_DBPDCNT2(ch), data_l);
+}
+
+static inline void set_dfifrequency(uint32_t freq)
+{
+	uint32_t ch;
+
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) {
+		foreach_vch(ch)
+		    mmio_clrsetbits_32(DBSC_DBPDCNT1(ch), 0x1fU, freq);
+	} else {
+		foreach_vch(ch) {
+			mmio_clrsetbits_32(DBSC_DBDFICNT(ch), 0x1fU << 24,
+					   (freq << 24));
+		}
+	}
+	dsb_sev();
+}
+
+static uint32_t pll3_freq(uint32_t on)
+{
+	uint32_t timeout;
+
+	timeout = wait_freqchgreq(1);
+
+	if (timeout) {
+		return 1;
+	}
+
+	pll3_control(on);
+	set_dfifrequency(on);
+
+	set_freqchgack(1);
+	timeout = wait_freqchgreq(0);
+	set_freqchgack(0);
+
+	if (timeout) {
+		FATAL_MSG("BL2: Time out[2]\n");
+		return 1;
+	}
+	return 0;
+}
+
+/* update dly */
+static void update_dly(void)
+{
+	ddr_setval_ach(_reg_SC_PHY_MANUAL_UPDATE, 0x01);
+	ddr_setval_ach(_reg_PHY_ADRCTL_MANUAL_UPDATE, 0x01);
+}
+
+/* training by pi */
+static uint32_t pi_training_go(void)
+{
+	uint32_t flag;
+	uint32_t data_l;
+	uint32_t retry;
+	const uint32_t RETRY_MAX = 4096 * 16;
+	uint32_t ch;
+
+	uint32_t mst_ch;
+	uint32_t cur_frq;
+	uint32_t complete;
+	uint32_t frqchg_req;
+
+	/* pi_start */
+	ddr_setval_ach(_reg_PI_START, 0x01);
+	foreach_vch(ch)
+	    ddr_getval(ch, _reg_PI_INT_STATUS);
+
+	/* set dfi_phymstr_ack = 1 */
+	mmio_write_32(DBSC_DBDFIPMSTRCNF, 0x00000001);
+	dsb_sev();
+
+	/* wait pi_int_status[0] */
+	mst_ch = 0;
+	flag = 0;
+	complete = 0;
+	cur_frq = 0;
+	retry = RETRY_MAX;
+	do {
+		frqchg_req = mmio_read_32(DBSC_DBPDSTAT(mst_ch)) & 0x01;
+
+		/* H3 Ver.1.x cannot see frqchg_req */
+		if ((prr_product == PRR_PRODUCT_H3) &&
+		    (prr_cut <= PRR_PRODUCT_11)) {
+			if ((retry % 4096) == 1) {
+				frqchg_req = 1;
+			} else {
+				frqchg_req = 0;
+			}
+		}
+
+		if (frqchg_req) {
+			if (cur_frq) {
+				/* Low frequency */
+				flag = pll3_freq(0);
+				cur_frq = 0;
+			} else {
+				/* High frequency */
+				flag = pll3_freq(1);
+				cur_frq = 1;
+			}
+			if (flag)
+				break;
+		} else {
+			if (cur_frq) {
+				foreach_vch(ch) {
+					if (complete & (1U << ch))
+						continue;
+					data_l =
+					    ddr_getval(ch, _reg_PI_INT_STATUS);
+					if (data_l & 0x01) {
+						complete |= (1U << ch);
+					}
+				}
+				if (complete == ddr_phyvalid)
+					break;
+			}
+		}
+	} while (--retry);
+	foreach_vch(ch) {
+		/* dummy read */
+		data_l = ddr_getval_s(ch, 0, _reg_PHY_CAL_RESULT2_OBS_0);
+		data_l = ddr_getval(ch, _reg_PI_INT_STATUS);
+		ddr_setval(ch, _reg_PI_INT_ACK, data_l);
+	}
+	if (ddrphy_regif_chk()) {
+		return 0xfd;
+	}
+	return complete;
+}
+
+/* Initialize DDR */
+static uint32_t init_ddr(void)
+{
+	int32_t i;
+	uint32_t data_l;
+	uint32_t phytrainingok;
+	uint32_t ch, slice;
+	uint32_t err;
+	int16_t adj;
+
+	MSG_LF(__func__ ":0\n");
+
+#ifdef DDR_BACKUPMODE
+	rcar_dram_get_boot_status(&ddr_backup);
+#endif
+
+	/* unlock phy */
+	/* Unlock DDRPHY register(AGAIN) */
+	foreach_vch(ch)
+	    mmio_write_32(DBSC_DBPDLK(ch), 0x0000A55A);
+	dsb_sev();
+
+	if ((((prr_product == PRR_PRODUCT_H3) &&
+	      (prr_cut > PRR_PRODUCT_11)) ||
+	     (prr_product == PRR_PRODUCT_M3N) ||
+	     (prr_product == PRR_PRODUCT_V3H)) && board_cnf->dbi_en)
+		reg_ddrphy_write_a(0x00001010, 0x01000001);
+	else
+		reg_ddrphy_write_a(0x00001010, 0x00000001);
+	/* DBSC register pre-setting */
+	dbsc_regset_pre();
+
+	/* load ddrphy registers */
+
+	ddrtbl_load();
+
+	/* configure ddrphy registers */
+	ddr_config();
+
+	/* dfi_reset assert */
+	foreach_vch(ch)
+	    mmio_write_32(DBSC_DBPDCNT0(ch), 0x01);
+	dsb_sev();
+
+	/* dbsc register set */
+	dbsc_regset();
+	MSG_LF(__func__ ":1\n");
+
+	/* dfi_reset negate */
+	foreach_vch(ch)
+	    mmio_write_32(DBSC_DBPDCNT0(ch), 0x00);
+	dsb_sev();
+
+	/* dfi_init_start (start ddrphy) */
+	err = dfi_init_start();
+	if (err) {
+		return INITDRAM_ERR_I;
+	}
+	MSG_LF(__func__ ":2\n");
+
+	/* ddr backupmode end */
+#ifdef DDR_BACKUPMODE
+	if (ddr_backup) {
+		NOTICE("BL2: [WARM_BOOT]\n");
+	} else {
+		NOTICE("BL2: [COLD_BOOT]\n");
+	}
+	err = rcar_dram_update_boot_status(ddr_backup);
+	if (err) {
+		NOTICE("BL2: [BOOT_STATUS_UPDATE_ERROR]\n");
+		return INITDRAM_ERR_I;
+	}
+#endif
+	MSG_LF(__func__ ":3\n");
+
+	/* override term code after dfi_init_complete */
+	err = set_term_code();
+	if (err) {
+		return INITDRAM_ERR_I;
+	}
+	MSG_LF(__func__ ":4\n");
+
+	/* rx offset calibration */
+	if ((prr_cut > PRR_PRODUCT_11) || (prr_product == PRR_PRODUCT_M3N) ||
+	    (prr_product == PRR_PRODUCT_V3H)) {
+		err = rx_offset_cal_hw();
+	} else {
+		err = rx_offset_cal();
+	}
+	if (err)
+		return INITDRAM_ERR_O;
+	MSG_LF(__func__ ":5\n");
+
+	/* Dummy PDE */
+	send_dbcmd(0x08840000);
+
+	/* PDX */
+	send_dbcmd(0x08840001);
+
+	/* check register i/f is alive */
+	err = ddrphy_regif_chk();
+	if (err) {
+		return INITDRAM_ERR_O;
+	}
+	MSG_LF(__func__ ":6\n");
+
+	/* phy initialize end */
+
+	/* setup DDR mode registers */
+	/* CMOS MODE */
+	change_lpddr4_en(0);
+
+	/* MRS */
+	ddr_register_set();
+
+	/* Thermal sensor setting */
+	/* THCTR Bit6: PONM=0 , Bit0: THSST=1  */
+	data_l = (mmio_read_32(THS1_THCTR) & 0xFFFFFFBF) | 0x00000001;
+	mmio_write_32(THS1_THCTR, data_l);
+
+	/* LPDDR4 MODE */
+	change_lpddr4_en(1);
+
+	MSG_LF(__func__ ":7\n");
+
+	/* mask CS_MAP if RANKx is not found */
+	foreach_vch(ch) {
+		data_l = ddr_getval(ch, _reg_PI_CS_MAP);
+		if (!(ch_have_this_cs[1] & (1U << ch)))
+			data_l = data_l & 0x05;
+		ddr_setval(ch, _reg_PI_CS_MAP, data_l);
+	}
+
+	/* exec pi_training */
+	reg_ddrphy_write_a(ddr_regdef_adr(_reg_PHY_FREQ_SEL_MULTICAST_EN),
+			   BIT(ddr_regdef_lsb(_reg_PHY_FREQ_SEL_MULTICAST_EN)));
+	ddr_setval_ach_as(_reg_PHY_PER_CS_TRAINING_MULTICAST_EN, 0x00);
+
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) {
+		ddr_setval_ach_as(_reg_PHY_PER_CS_TRAINING_EN, 0x01);
+	} else {
+		foreach_vch(ch) {
+			for (slice = 0; slice < SLICE_CNT; slice++) {
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_PER_CS_TRAINING_EN,
+					     ((ch_have_this_cs[1]) >> ch)
+					     & 0x01);
+			}
+		}
+	}
+
+	phytrainingok = pi_training_go();
+
+	if (ddr_phyvalid != (phytrainingok & ddr_phyvalid)) {
+		return INITDRAM_ERR_T | phytrainingok;
+	}
+
+	MSG_LF(__func__ ":8\n");
+
+	/* CACS DLY ADJUST */
+	data_l = board_cnf->cacs_dly + _f_scale_adj(board_cnf->cacs_dly_adj);
+	foreach_vch(ch) {
+		for (i = 0; i < _reg_PHY_CLK_CACS_SLAVE_DELAY_X_NUM; i++) {
+			adj = _f_scale_adj(board_cnf->ch[ch].cacs_adj[i]);
+			ddr_setval(ch, _reg_PHY_CLK_CACS_SLAVE_DELAY_X[i],
+				   data_l + adj);
+		}
+
+		if (ddr_phycaslice == 1) {
+			for (i = 0; i < 6; i++) {
+				adj = _f_scale_adj(board_cnf->ch[ch].cacs_adj
+					[i +
+					_reg_PHY_CLK_CACS_SLAVE_DELAY_X_NUM]);
+				ddr_setval_s(ch, 2,
+					     _reg_PHY_CLK_CACS_SLAVE_DELAY_X
+					     [i],
+					     data_l + adj
+				);
+			}
+		}
+	}
+
+	update_dly();
+	MSG_LF(__func__ ":9\n");
+
+	/* H3 fix rd latency to avoid bug in elasitic buffer */
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11))
+		adjust_rddqs_latency();
+
+	/* Adjust Write path latency */
+	if (ddrtbl_getval
+	    (_cnf_DDR_PHY_SLICE_REGSET, _reg_PHY_WRITE_PATH_LAT_ADD))
+		adjust_wpath_latency();
+
+	/* RDQLVL Training */
+	if (!ddrtbl_getval(_cnf_DDR_PHY_SLICE_REGSET, _reg_PHY_IE_MODE))
+		ddr_setval_ach_as(_reg_PHY_IE_MODE, 0x01);
+
+	err = rdqdm_man();
+
+	if (!ddrtbl_getval(_cnf_DDR_PHY_SLICE_REGSET, _reg_PHY_IE_MODE))
+		ddr_setval_ach_as(_reg_PHY_IE_MODE, 0x00);
+
+	if (err) {
+		return INITDRAM_ERR_T;
+	}
+	update_dly();
+	MSG_LF(__func__ ":10\n");
+
+	/* WDQLVL Training */
+	err = wdqdm_man();
+	if (err) {
+		return INITDRAM_ERR_T;
+	}
+	update_dly();
+	MSG_LF(__func__ ":11\n");
+
+	/* training complete, setup DBSC */
+	if (((prr_product == PRR_PRODUCT_H3) && (prr_cut > PRR_PRODUCT_11)) ||
+	    (prr_product == PRR_PRODUCT_M3N) ||
+	    (prr_product == PRR_PRODUCT_V3H)) {
+		ddr_setval_ach_as(_reg_PHY_DFI40_POLARITY, 0x00);
+		ddr_setval_ach(_reg_PI_DFI40_POLARITY, 0x00);
+	}
+
+	dbsc_regset_post();
+	MSG_LF(__func__ ":12\n");
+
+	return phytrainingok;
+}
+
+/* SW LEVELING COMMON */
+static uint32_t swlvl1(uint32_t ddr_csn, uint32_t reg_cs, uint32_t reg_kick)
+{
+	uint32_t ch;
+	uint32_t data_l;
+	uint32_t retry;
+	uint32_t waiting;
+	uint32_t err;
+
+	const uint32_t RETRY_MAX = 0x1000;
+
+	err = 0;
+	/* set EXIT -> OP_DONE is cleared */
+	ddr_setval_ach(_reg_PI_SWLVL_EXIT, 0x01);
+
+	/* kick */
+	foreach_vch(ch) {
+		if (ch_have_this_cs[ddr_csn % 2] & (1U << ch)) {
+			ddr_setval(ch, reg_cs, ddr_csn);
+			ddr_setval(ch, reg_kick, 0x01);
+		}
+	}
+	foreach_vch(ch) {
+		/*PREPARE ADDR REGISTER (for SWLVL_OP_DONE) */
+		ddr_getval(ch, _reg_PI_SWLVL_OP_DONE);
+	}
+	waiting = ch_have_this_cs[ddr_csn % 2];
+	dsb_sev();
+	retry = RETRY_MAX;
+	do {
+		foreach_vch(ch) {
+			if (!(waiting & (1U << ch)))
+				continue;
+			data_l = ddr_getval(ch, _reg_PI_SWLVL_OP_DONE);
+			if (data_l & 0x01)
+				waiting &= ~(1U << ch);
+		}
+		retry--;
+	} while (waiting && (retry > 0));
+	if (retry == 0) {
+		err = 1;
+	}
+
+	dsb_sev();
+	/* set EXIT -> OP_DONE is cleared */
+	ddr_setval_ach(_reg_PI_SWLVL_EXIT, 0x01);
+	dsb_sev();
+
+	return err;
+}
+
+/* WDQ TRAINING */
+#ifndef DDR_FAST_INIT
+static void wdqdm_clr1(uint32_t ch, uint32_t ddr_csn)
+{
+	int32_t i, k;
+	uint32_t cs, slice;
+	uint32_t data_l;
+
+	/* clr of training results buffer */
+	cs = ddr_csn % 2;
+	data_l = board_cnf->dqdm_dly_w;
+	for (slice = 0; slice < SLICE_CNT; slice++) {
+		k = (board_cnf->ch[ch].dqs_swap >> (4 * slice)) & 0x0f;
+		if (((k >= 2) && (ddr_csn < 2)) || ((k < 2) && (ddr_csn >= 2)))
+			continue;
+
+		for (i = 0; i <= 8; i++) {
+			if (ch_have_this_cs[CS_CNT - 1 - cs] & (1U << ch))
+				wdqdm_dly[ch][cs][slice][i] =
+				    wdqdm_dly[ch][CS_CNT - 1 - cs][slice][i];
+			else
+				wdqdm_dly[ch][cs][slice][i] = data_l;
+			wdqdm_le[ch][cs][slice][i] = 0;
+			wdqdm_te[ch][cs][slice][i] = 0;
+		}
+		wdqdm_st[ch][cs][slice] = 0;
+		wdqdm_win[ch][cs][slice] = 0;
+	}
+}
+
+static uint32_t wdqdm_ana1(uint32_t ch, uint32_t ddr_csn)
+{
+	int32_t i, k;
+	uint32_t cs, slice;
+	uint32_t data_l;
+	uint32_t err;
+	const uint32_t _par_WDQLVL_RETRY_THRES = 0x7c0;
+
+	int32_t min_win;
+	int32_t win;
+	int8_t _adj;
+	int16_t adj;
+	uint32_t dq;
+
+	/* analysis of training results */
+	err = 0;
+	for (slice = 0; slice < SLICE_CNT; slice += 1) {
+		k = (board_cnf->ch[ch].dqs_swap >> (4 * slice)) & 0x0f;
+		if (((k >= 2) && (ddr_csn < 2)) || ((k < 2) && (ddr_csn >= 2)))
+			continue;
+
+		cs = ddr_csn % 2;
+		ddr_setval_s(ch, slice, _reg_PHY_PER_CS_TRAINING_INDEX, cs);
+		for (i = 0; i < 9; i++) {
+			dq = slice * 8 + i;
+			if (i == 8)
+				_adj = board_cnf->ch[ch].dm_adj_w[slice];
+			else
+				_adj = board_cnf->ch[ch].dq_adj_w[dq];
+			adj = _f_scale_adj(_adj);
+
+			data_l =
+			    ddr_getval_s(ch, slice,
+					 _reg_PHY_CLK_WRX_SLAVE_DELAY[i]) + adj;
+			ddr_setval_s(ch, slice, _reg_PHY_CLK_WRX_SLAVE_DELAY[i],
+				     data_l);
+			wdqdm_dly[ch][cs][slice][i] = data_l;
+		}
+		ddr_setval_s(ch, slice, _reg_PHY_PER_CS_TRAINING_EN, 0x00);
+		data_l = ddr_getval_s(ch, slice, _reg_PHY_WDQLVL_STATUS_OBS);
+		wdqdm_st[ch][cs][slice] = data_l;
+		min_win = INT_LEAST32_MAX;
+		for (i = 0; i <= 8; i++) {
+			ddr_setval_s(ch, slice, _reg_PHY_WDQLVL_DQDM_OBS_SELECT,
+				     i);
+
+			data_l =
+			    ddr_getval_s(ch, slice,
+					 _reg_PHY_WDQLVL_DQDM_TE_DLY_OBS);
+			wdqdm_te[ch][cs][slice][i] = data_l;
+			data_l =
+			    ddr_getval_s(ch, slice,
+					 _reg_PHY_WDQLVL_DQDM_LE_DLY_OBS);
+			wdqdm_le[ch][cs][slice][i] = data_l;
+			win =
+			    (int32_t)wdqdm_te[ch][cs][slice][i] -
+			    wdqdm_le[ch][cs][slice][i];
+			if (min_win > win)
+				min_win = win;
+			if (data_l >= _par_WDQLVL_RETRY_THRES)
+				err = 2;
+		}
+		wdqdm_win[ch][cs][slice] = min_win;
+		if ((prr_product == PRR_PRODUCT_H3) &&
+		    (prr_cut <= PRR_PRODUCT_11)) {
+			ddr_setval_s(ch, slice, _reg_PHY_PER_CS_TRAINING_EN,
+				     0x01);
+		} else {
+			ddr_setval_s(ch, slice, _reg_PHY_PER_CS_TRAINING_EN,
+				     ((ch_have_this_cs[1]) >> ch) & 0x01);
+		}
+	}
+	return err;
+}
+#endif/* DDR_FAST_INIT */
+
+static void wdqdm_cp(uint32_t ddr_csn, uint32_t restore)
+{
+	uint32_t i;
+	uint32_t ch, slice;
+	uint32_t tgt_cs, src_cs;
+	uint32_t tmp_r;
+
+	/* copy of training results */
+	foreach_vch(ch) {
+		for (tgt_cs = 0; tgt_cs < CS_CNT; tgt_cs++) {
+			for (slice = 0; slice < SLICE_CNT; slice++) {
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_PER_CS_TRAINING_INDEX,
+					     tgt_cs);
+				src_cs = ddr_csn % 2;
+				if (!(ch_have_this_cs[1] & (1U << ch)))
+					src_cs = 0;
+				for (i = 0; i <= 4; i += 4) {
+					if (restore)
+						tmp_r =
+						    rdqdm_dly[ch][tgt_cs][slice]
+						    [i];
+					else
+						tmp_r =
+						    rdqdm_dly[ch][src_cs][slice]
+						    [i];
+
+					ddr_setval_s(ch, slice,
+						     _reg_PHY_RDDQS_X_RISE_SLAVE_DELAY
+						     [i], tmp_r);
+				}
+			}
+		}
+	}
+}
+
+static uint32_t wdqdm_man1(void)
+{
+	int32_t k;
+	uint32_t ch, cs, slice;
+	uint32_t ddr_csn;
+	uint32_t data_l;
+	uint32_t err;
+	uint32_t high_dq[DRAM_CH_CNT];
+	uint32_t mr14_csab0_bak[DRAM_CH_CNT];
+#ifndef DDR_FAST_INIT
+	uint32_t err_flg;
+#endif/* DDR_FAST_INIT */
+
+	/* manual execution of training */
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) {
+		foreach_vch(ch) {
+			high_dq[ch] = 0;
+			for (slice = 0; slice < SLICE_CNT; slice++) {
+				k = (board_cnf->ch[ch].dqs_swap >>
+				    (4 * slice)) & 0x0f;
+				if (k >= 2)
+					high_dq[ch] |= (1U << slice);
+			}
+			ddr_setval(ch, _reg_PI_16BIT_DRAM_CONNECT, 0x00);
+		}
+	}
+	err = 0;
+	/* CLEAR PREV RESULT */
+	for (cs = 0; cs < CS_CNT; cs++) {
+		ddr_setval_ach_as(_reg_PHY_PER_CS_TRAINING_INDEX, cs);
+		if (((prr_product == PRR_PRODUCT_H3) &&
+		     (prr_cut > PRR_PRODUCT_11)) ||
+		    (prr_product == PRR_PRODUCT_M3N) ||
+		    (prr_product == PRR_PRODUCT_V3H)) {
+			ddr_setval_ach_as(_reg_SC_PHY_WDQLVL_CLR_PREV_RESULTS,
+					  0x01);
+		} else {
+			ddr_setval_ach_as(_reg_PHY_WDQLVL_CLR_PREV_RESULTS,
+					  0x01);
+		}
+	}
+	ddrphy_regif_idle();
+
+#ifndef DDR_FAST_INIT
+	err_flg = 0;
+#endif/* DDR_FAST_INIT */
+	for (ddr_csn = 0; ddr_csn < CSAB_CNT; ddr_csn++) {
+		if ((prr_product == PRR_PRODUCT_H3) &&
+		    (prr_cut <= PRR_PRODUCT_11)) {
+			foreach_vch(ch) {
+				data_l = mmio_read_32(DBSC_DBDFICNT(ch));
+				data_l &= ~(0x00ffU << 16);
+
+				if (ddr_csn >= 2)
+					k = (high_dq[ch] ^ 0x0f);
+				else
+					k = high_dq[ch];
+				data_l |= (k << 16);
+				mmio_write_32(DBSC_DBDFICNT(ch), data_l);
+				ddr_setval(ch, _reg_PI_WDQLVL_RESP_MASK, k);
+			}
+		}
+		if (((prr_product == PRR_PRODUCT_H3) &&
+		     (prr_cut <= PRR_PRODUCT_11)) ||
+		    ((prr_product == PRR_PRODUCT_M3) &&
+		     (prr_cut == PRR_PRODUCT_10))) {
+			wdqdm_cp(ddr_csn, 0);
+		}
+
+		foreach_vch(ch) {
+			data_l =
+			    ddr_getval(ch,
+				       reg_pi_mr14_data_fx_csx[1][ddr_csn]);
+			ddr_setval(ch, reg_pi_mr14_data_fx_csx[1][0], data_l);
+		}
+
+		/* KICK WDQLVL */
+		err = swlvl1(ddr_csn, _reg_PI_WDQLVL_CS, _reg_PI_WDQLVL_REQ);
+		if (err)
+			goto err_exit;
+
+		if (ddr_csn == 0)
+			foreach_vch(ch) {
+			mr14_csab0_bak[ch] =
+			    ddr_getval(ch, reg_pi_mr14_data_fx_csx[1][0]);
+		} else
+			foreach_vch(ch) {
+			ddr_setval(ch, reg_pi_mr14_data_fx_csx[1][0],
+				   mr14_csab0_bak[ch]);
+			}
+#ifndef DDR_FAST_INIT
+		foreach_vch(ch) {
+			if (!(ch_have_this_cs[ddr_csn % 2] & (1U << ch))) {
+				wdqdm_clr1(ch, ddr_csn);
+				continue;
+			}
+			err = wdqdm_ana1(ch, ddr_csn);
+			if (err)
+				err_flg |= (1U << (ddr_csn * 4 + ch));
+			ddrphy_regif_idle();
+		}
+#endif/* DDR_FAST_INIT */
+	}
+err_exit:
+#ifndef DDR_FAST_INIT
+	err |= err_flg;
+#endif/* DDR_FAST_INIT */
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) {
+		ddr_setval_ach(_reg_PI_16BIT_DRAM_CONNECT, 0x01);
+		foreach_vch(ch) {
+			data_l = mmio_read_32(DBSC_DBDFICNT(ch));
+			data_l &= ~(0x00ffU << 16);
+			mmio_write_32(DBSC_DBDFICNT(ch), data_l);
+			ddr_setval(ch, _reg_PI_WDQLVL_RESP_MASK, 0x00);
+		}
+	}
+	return err;
+}
+
+static uint32_t wdqdm_man(void)
+{
+	uint32_t err, retry_cnt;
+	const uint32_t retry_max = 0x10;
+	uint32_t datal, ch, ddr_csn, mr14_bkup[4][4];
+
+	datal = RL + js2[js2_tdqsck] + (16 / 2) + 1 - WL + 2 + 2 + 19;
+	if ((mmio_read_32(DBSC_DBTR(11)) & 0xFF) > datal)
+		datal = mmio_read_32(DBSC_DBTR(11)) & 0xFF;
+	ddr_setval_ach(_reg_PI_TDFI_WDQLVL_RW, datal);
+
+	if (((prr_product == PRR_PRODUCT_H3) && (prr_cut > PRR_PRODUCT_11)) ||
+	    (prr_product == PRR_PRODUCT_M3N) ||
+	    (prr_product == PRR_PRODUCT_V3H)) {
+		ddr_setval_ach(_reg_PI_TDFI_WDQLVL_WR_F0,
+			       (mmio_read_32(DBSC_DBTR(12)) & 0xFF) + 10);
+		ddr_setval_ach(_reg_PI_TDFI_WDQLVL_WR_F1,
+			       (mmio_read_32(DBSC_DBTR(12)) & 0xFF) + 10);
+	} else {
+		ddr_setval_ach(_reg_PI_TDFI_WDQLVL_WR,
+			       (mmio_read_32(DBSC_DBTR(12)) & 0xFF) + 10);
+	}
+	ddr_setval_ach(_reg_PI_TRFC_F0, mmio_read_32(DBSC_DBTR(13)) & 0x1FF);
+	ddr_setval_ach(_reg_PI_TRFC_F1, mmio_read_32(DBSC_DBTR(13)) & 0x1FF);
+
+	retry_cnt = 0;
+	err = 0;
+	do {
+		if ((prr_product == PRR_PRODUCT_H3) &&
+		    (prr_cut <= PRR_PRODUCT_11)) {
+			err = wdqdm_man1();
+		} else {
+			ddr_setval_ach(_reg_PI_WDQLVL_VREF_EN, 0x01);
+			ddr_setval_ach(_reg_PI_WDQLVL_VREF_NORMAL_STEPSIZE,
+				       0x01);
+			if ((prr_product == PRR_PRODUCT_M3N) ||
+			    (prr_product == PRR_PRODUCT_V3H)) {
+				ddr_setval_ach(_reg_PI_WDQLVL_VREF_DELTA_F1,
+					       0x0C);
+			} else {
+				ddr_setval_ach(_reg_PI_WDQLVL_VREF_DELTA, 0x0C);
+			}
+			dsb_sev();
+			err = wdqdm_man1();
+			foreach_vch(ch) {
+				for (ddr_csn = 0; ddr_csn < CSAB_CNT; ddr_csn++) {
+					mr14_bkup[ch][ddr_csn] =
+					    ddr_getval(ch,
+						       reg_pi_mr14_data_fx_csx
+						       [1][ddr_csn]);
+					dsb_sev();
+				}
+			}
+
+			if ((prr_product == PRR_PRODUCT_M3N) ||
+			    (prr_product == PRR_PRODUCT_V3H)) {
+				ddr_setval_ach(_reg_PI_WDQLVL_VREF_DELTA_F1,
+					       0x04);
+			} else {
+				ddr_setval_ach(_reg_PI_WDQLVL_VREF_DELTA, 0x04);
+			}
+			pvtcode_update();
+			err = wdqdm_man1();
+			foreach_vch(ch) {
+				for (ddr_csn = 0; ddr_csn < CSAB_CNT; ddr_csn++) {
+					mr14_bkup[ch][ddr_csn] =
+					    (mr14_bkup[ch][ddr_csn] +
+					     ddr_getval(ch,
+							reg_pi_mr14_data_fx_csx
+							[1][ddr_csn])) / 2;
+					ddr_setval(ch,
+						   reg_pi_mr14_data_fx_csx[1]
+						   [ddr_csn],
+						   mr14_bkup[ch][ddr_csn]);
+				}
+			}
+
+			ddr_setval_ach(_reg_PI_WDQLVL_VREF_NORMAL_STEPSIZE,
+				       0x00);
+			if ((prr_product == PRR_PRODUCT_M3N) ||
+			    (prr_product == PRR_PRODUCT_V3H)) {
+				ddr_setval_ach(_reg_PI_WDQLVL_VREF_DELTA_F1,
+					       0x00);
+				ddr_setval_ach
+				    (_reg_PI_WDQLVL_VREF_INITIAL_START_POINT_F1,
+				     0x00);
+				ddr_setval_ach
+				    (_reg_PI_WDQLVL_VREF_INITIAL_STOP_POINT_F1,
+				     0x00);
+			} else {
+				ddr_setval_ach(_reg_PI_WDQLVL_VREF_DELTA, 0x00);
+				ddr_setval_ach
+				    (_reg_PI_WDQLVL_VREF_INITIAL_START_POINT,
+				     0x00);
+				ddr_setval_ach
+				    (_reg_PI_WDQLVL_VREF_INITIAL_STOP_POINT,
+				     0x00);
+			}
+			ddr_setval_ach(_reg_PI_WDQLVL_VREF_INITIAL_STEPSIZE,
+				       0x00);
+
+			pvtcode_update2();
+			err = wdqdm_man1();
+			ddr_setval_ach(_reg_PI_WDQLVL_VREF_EN, 0x00);
+		}
+	} while (err && (++retry_cnt < retry_max));
+
+	if (((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) ||
+	    ((prr_product == PRR_PRODUCT_M3) && (prr_cut <= PRR_PRODUCT_10))) {
+		wdqdm_cp(0, 1);
+	}
+
+	return (retry_cnt >= retry_max);
+}
+
+/* RDQ TRAINING */
+#ifndef DDR_FAST_INIT
+static void rdqdm_clr1(uint32_t ch, uint32_t ddr_csn)
+{
+	int32_t i, k;
+	uint32_t cs, slice;
+	uint32_t data_l;
+
+	/* clr of training results buffer */
+	cs = ddr_csn % 2;
+	data_l = board_cnf->dqdm_dly_r;
+	for (slice = 0; slice < SLICE_CNT; slice++) {
+		k = (board_cnf->ch[ch].dqs_swap >> (4 * slice)) & 0x0f;
+		if (((k >= 2) && (ddr_csn < 2)) || ((k < 2) && (ddr_csn >= 2)))
+			continue;
+
+		for (i = 0; i <= 8; i++) {
+			if (ch_have_this_cs[CS_CNT - 1 - cs] & (1U << ch)) {
+				rdqdm_dly[ch][cs][slice][i] =
+				    rdqdm_dly[ch][CS_CNT - 1 - cs][slice][i];
+				rdqdm_dly[ch][cs][slice + SLICE_CNT][i] =
+				    rdqdm_dly[ch][CS_CNT - 1 - cs][slice +
+								   SLICE_CNT]
+				    [i];
+			} else {
+				rdqdm_dly[ch][cs][slice][i] = data_l;
+				rdqdm_dly[ch][cs][slice + SLICE_CNT][i] =
+					data_l;
+			}
+			rdqdm_le[ch][cs][slice][i] = 0;
+			rdqdm_le[ch][cs][slice + SLICE_CNT][i] = 0;
+			rdqdm_te[ch][cs][slice][i] = 0;
+			rdqdm_te[ch][cs][slice + SLICE_CNT][i] = 0;
+			rdqdm_nw[ch][cs][slice][i] = 0;
+			rdqdm_nw[ch][cs][slice + SLICE_CNT][i] = 0;
+		}
+		rdqdm_st[ch][cs][slice] = 0;
+		rdqdm_win[ch][cs][slice] = 0;
+	}
+}
+
+static uint32_t rdqdm_ana1(uint32_t ch, uint32_t ddr_csn)
+{
+	int32_t i, k;
+	uint32_t cs, slice;
+	uint32_t data_l;
+	uint32_t err;
+	int8_t _adj;
+	int16_t adj;
+	uint32_t dq;
+	int32_t min_win;
+	int32_t win;
+	uint32_t rdq_status_obs_select;
+
+	/* analysis of training results */
+	err = 0;
+	for (slice = 0; slice < SLICE_CNT; slice++) {
+		k = (board_cnf->ch[ch].dqs_swap >> (4 * slice)) & 0x0f;
+		if (((k >= 2) && (ddr_csn < 2)) || ((k < 2) && (ddr_csn >= 2)))
+			continue;
+
+		cs = ddr_csn % 2;
+		ddr_setval_s(ch, slice, _reg_PHY_PER_CS_TRAINING_INDEX, cs);
+		ddrphy_regif_idle();
+
+		ddr_getval_s(ch, slice, _reg_PHY_PER_CS_TRAINING_INDEX);
+		ddrphy_regif_idle();
+
+		for (i = 0; i <= 8; i++) {
+			dq = slice * 8 + i;
+			if (i == 8)
+				_adj = board_cnf->ch[ch].dm_adj_r[slice];
+			else
+				_adj = board_cnf->ch[ch].dq_adj_r[dq];
+
+			adj = _f_scale_adj(_adj);
+
+			data_l =
+			    ddr_getval_s(ch, slice,
+					 _reg_PHY_RDDQS_X_RISE_SLAVE_DELAY[i]) +
+			    adj;
+			ddr_setval_s(ch, slice,
+				     _reg_PHY_RDDQS_X_RISE_SLAVE_DELAY[i],
+				     data_l);
+			rdqdm_dly[ch][cs][slice][i] = data_l;
+
+			data_l =
+			    ddr_getval_s(ch, slice,
+					 _reg_PHY_RDDQS_X_FALL_SLAVE_DELAY[i]) +
+			    adj;
+			ddr_setval_s(ch, slice,
+				     _reg_PHY_RDDQS_X_FALL_SLAVE_DELAY[i],
+				     data_l);
+			rdqdm_dly[ch][cs][slice + SLICE_CNT][i] = data_l;
+		}
+		min_win = INT_LEAST32_MAX;
+		for (i = 0; i <= 8; i++) {
+			data_l =
+			    ddr_getval_s(ch, slice, _reg_PHY_RDLVL_STATUS_OBS);
+			rdqdm_st[ch][cs][slice] = data_l;
+			rdqdm_st[ch][cs][slice + SLICE_CNT] = data_l;
+			/* k : rise/fall */
+			for (k = 0; k < 2; k++) {
+				if (i == 8) {
+					rdq_status_obs_select = 16 + 8 * k;
+				} else {
+					rdq_status_obs_select = i + k * 8;
+				}
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_RDLVL_RDDQS_DQ_OBS_SELECT,
+					     rdq_status_obs_select);
+
+				data_l =
+				    ddr_getval_s(ch, slice,
+						 _reg_PHY_RDLVL_RDDQS_DQ_LE_DLY_OBS);
+				rdqdm_le[ch][cs][slice + SLICE_CNT * k][i] =
+				    data_l;
+
+				data_l =
+				    ddr_getval_s(ch, slice,
+						 _reg_PHY_RDLVL_RDDQS_DQ_TE_DLY_OBS);
+				rdqdm_te[ch][cs][slice + SLICE_CNT * k][i] =
+				    data_l;
+
+				data_l =
+				    ddr_getval_s(ch, slice,
+						 _reg_PHY_RDLVL_RDDQS_DQ_NUM_WINDOWS_OBS);
+				rdqdm_nw[ch][cs][slice + SLICE_CNT * k][i] =
+				    data_l;
+
+				win =
+				    (int32_t)rdqdm_te[ch][cs][slice +
+							      SLICE_CNT *
+							      k][i] -
+				    rdqdm_le[ch][cs][slice + SLICE_CNT * k][i];
+				if (i != 8) {
+					if (min_win > win)
+						min_win = win;
+				}
+			}
+		}
+		rdqdm_win[ch][cs][slice] = min_win;
+		if (min_win <= 0) {
+			err = 2;
+		}
+	}
+	return err;
+}
+#endif/* DDR_FAST_INIT */
+
+static uint32_t rdqdm_man1(void)
+{
+	uint32_t ch;
+	uint32_t ddr_csn;
+#ifdef DDR_FAST_INIT
+	uint32_t slice;
+	uint32_t i, adj, data_l;
+#endif/* DDR_FAST_INIT */
+	uint32_t err;
+
+	/* manual execution of training */
+	err = 0;
+
+	for (ddr_csn = 0; ddr_csn < CSAB_CNT; ddr_csn++) {
+		/* KICK RDQLVL */
+		err = swlvl1(ddr_csn, _reg_PI_RDLVL_CS, _reg_PI_RDLVL_REQ);
+		if (err)
+			goto err_exit;
+#ifndef DDR_FAST_INIT
+		foreach_vch(ch) {
+			if (!(ch_have_this_cs[ddr_csn % 2] & (1U << ch))) {
+				rdqdm_clr1(ch, ddr_csn);
+				ddrphy_regif_idle();
+				continue;
+			}
+			err = rdqdm_ana1(ch, ddr_csn);
+			ddrphy_regif_idle();
+			if (err)
+				goto err_exit;
+		}
+#else/* DDR_FAST_INIT */
+		foreach_vch(ch) {
+			if (ch_have_this_cs[ddr_csn] & (1U << ch)) {
+				for (slice = 0; slice < SLICE_CNT; slice++) {
+					if (ddr_getval_s(ch, slice,
+							 _reg_PHY_RDLVL_STATUS_OBS) !=
+					    0x0D00FFFF) {
+						err = (1U << ch) |
+							(0x10U << slice);
+						goto err_exit;
+					}
+				}
+			}
+			if (((prr_product == PRR_PRODUCT_H3) &&
+			     (prr_cut <= PRR_PRODUCT_11)) ||
+			    ((prr_product == PRR_PRODUCT_M3) &&
+			     (prr_cut <= PRR_PRODUCT_10))) {
+				for (slice = 0; slice < SLICE_CNT; slice++) {
+					for (i = 0; i <= 8; i++) {
+						if (i == 8)
+							adj = _f_scale_adj(board_cnf->ch[ch].dm_adj_r[slice]);
+						else
+							adj = _f_scale_adj(board_cnf->ch[ch].dq_adj_r[slice * 8 + i]);
+						ddr_setval_s(ch, slice, _reg_PHY_PER_CS_TRAINING_INDEX, ddr_csn);
+						data_l = ddr_getval_s(ch, slice, _reg_PHY_RDDQS_X_RISE_SLAVE_DELAY[i]) + adj;
+						ddr_setval_s(ch, slice, _reg_PHY_RDDQS_X_RISE_SLAVE_DELAY[i], data_l);
+						rdqdm_dly[ch][ddr_csn][slice][i] = data_l;
+						rdqdm_dly[ch][ddr_csn | 1][slice][i] = data_l;
+
+						data_l = ddr_getval_s(ch, slice, _reg_PHY_RDDQS_X_FALL_SLAVE_DELAY[i]) + adj;
+						ddr_setval_s(ch, slice, _reg_PHY_RDDQS_X_FALL_SLAVE_DELAY[i], data_l);
+						rdqdm_dly[ch][ddr_csn][slice + SLICE_CNT][i] = data_l;
+						rdqdm_dly[ch][ddr_csn | 1][slice + SLICE_CNT][i] = data_l;
+					}
+				}
+			}
+		}
+		ddrphy_regif_idle();
+
+#endif/* DDR_FAST_INIT */
+	}
+
+err_exit:
+	return err;
+}
+
+static uint32_t rdqdm_man(void)
+{
+	uint32_t err, retry_cnt;
+	const uint32_t retry_max = 0x01;
+
+	ddr_setval_ach_as(_reg_PHY_DQ_TSEL_ENABLE,
+			  0x00000004 | ddrtbl_getval(_cnf_DDR_PHY_SLICE_REGSET,
+						     _reg_PHY_DQ_TSEL_ENABLE));
+	ddr_setval_ach_as(_reg_PHY_DQS_TSEL_ENABLE,
+			  0x00000004 | ddrtbl_getval(_cnf_DDR_PHY_SLICE_REGSET,
+						     _reg_PHY_DQS_TSEL_ENABLE));
+	ddr_setval_ach_as(_reg_PHY_DQ_TSEL_SELECT,
+			  0xFF0FFFFF & ddrtbl_getval(_cnf_DDR_PHY_SLICE_REGSET,
+						     _reg_PHY_DQ_TSEL_SELECT));
+	ddr_setval_ach_as(_reg_PHY_DQS_TSEL_SELECT,
+			  0xFF0FFFFF & ddrtbl_getval(_cnf_DDR_PHY_SLICE_REGSET,
+						     _reg_PHY_DQS_TSEL_SELECT));
+
+	retry_cnt = 0;
+	do {
+		err = rdqdm_man1();
+		ddrphy_regif_idle();
+	} while (err && (++retry_cnt < retry_max));
+	ddr_setval_ach_as(_reg_PHY_DQ_TSEL_ENABLE,
+			  ddrtbl_getval(_cnf_DDR_PHY_SLICE_REGSET,
+					_reg_PHY_DQ_TSEL_ENABLE));
+	ddr_setval_ach_as(_reg_PHY_DQS_TSEL_ENABLE,
+			  ddrtbl_getval(_cnf_DDR_PHY_SLICE_REGSET,
+					_reg_PHY_DQS_TSEL_ENABLE));
+	ddr_setval_ach_as(_reg_PHY_DQ_TSEL_SELECT,
+			  ddrtbl_getval(_cnf_DDR_PHY_SLICE_REGSET,
+					_reg_PHY_DQ_TSEL_SELECT));
+	ddr_setval_ach_as(_reg_PHY_DQS_TSEL_SELECT,
+			  ddrtbl_getval(_cnf_DDR_PHY_SLICE_REGSET,
+					_reg_PHY_DQS_TSEL_SELECT));
+
+	return (retry_cnt >= retry_max);
+}
+
+/* rx offset calibration */
+static int32_t _find_change(uint64_t val, uint32_t dir)
+{
+	int32_t i;
+	uint32_t startval;
+	uint32_t curval;
+	const int32_t VAL_END = 0x3f;
+
+	if (dir == 0) {
+		startval = (val & 0x01);
+		for (i = 1; i <= VAL_END; i++) {
+			curval = (val >> i) & 0x01;
+			if (curval != startval)
+				return i;
+		}
+		return VAL_END;
+	}
+
+	startval = (val >> dir) & 0x01;
+	for (i = dir - 1; i >= 0; i--) {
+		curval = (val >> i) & 0x01;
+		if (curval != startval)
+			return i;
+	}
+	return 0;
+}
+
+static uint32_t _rx_offset_cal_updn(uint32_t code)
+{
+	const uint32_t CODE_MAX = 0x40;
+	uint32_t tmp;
+
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) {
+		if (code == 0)
+			tmp = (1U << 6) | (CODE_MAX - 1);
+		else if (code <= 0x20)
+			tmp =
+			    ((CODE_MAX - 1 -
+			      (0x20 - code) * 2) << 6) | (CODE_MAX - 1);
+		else
+			tmp =
+			    ((CODE_MAX - 1) << 6) | (CODE_MAX - 1 -
+						     (code - 0x20) * 2);
+	} else {
+		if (code == 0)
+			tmp = (1U << 6) | (CODE_MAX - 1);
+		else
+			tmp = (code << 6) | (CODE_MAX - code);
+	}
+	return tmp;
+}
+
+static uint32_t rx_offset_cal(void)
+{
+	uint32_t index;
+	uint32_t code;
+	const uint32_t CODE_MAX = 0x40;
+	const uint32_t CODE_STEP = 2;
+	uint32_t ch, slice;
+	uint32_t tmp;
+	uint32_t tmp_ach_as[DRAM_CH_CNT][SLICE_CNT];
+	uint64_t val[DRAM_CH_CNT][SLICE_CNT][_reg_PHY_RX_CAL_X_NUM];
+	uint64_t tmpval;
+	int32_t lsb, msb;
+
+	ddr_setval_ach_as(_reg_PHY_RX_CAL_OVERRIDE, 0x01);
+	foreach_vch(ch) {
+		for (slice = 0; slice < SLICE_CNT; slice++) {
+			for (index = 0; index < _reg_PHY_RX_CAL_X_NUM; index++)
+				val[ch][slice][index] = 0;
+		}
+	}
+
+	for (code = 0; code < CODE_MAX / CODE_STEP; code++) {
+		tmp = _rx_offset_cal_updn(code * CODE_STEP);
+		for (index = 0; index < _reg_PHY_RX_CAL_X_NUM; index++) {
+			ddr_setval_ach_as(_reg_PHY_RX_CAL_X[index], tmp);
+		}
+		dsb_sev();
+		ddr_getval_ach_as(_reg_PHY_RX_CAL_OBS, (uint32_t *)tmp_ach_as);
+
+		foreach_vch(ch) {
+			for (slice = 0; slice < SLICE_CNT; slice++) {
+				tmp = tmp_ach_as[ch][slice];
+				for (index = 0; index < _reg_PHY_RX_CAL_X_NUM;
+				     index++) {
+					if (tmp & (1U << index)) {
+						val[ch][slice][index] |=
+						    (1ULL << code);
+					} else {
+						val[ch][slice][index] &=
+						    ~(1ULL << code);
+					}
+				}
+			}
+		}
+	}
+	foreach_vch(ch) {
+		for (slice = 0; slice < SLICE_CNT; slice++) {
+			for (index = 0; index < _reg_PHY_RX_CAL_X_NUM;
+			     index++) {
+				tmpval = val[ch][slice][index];
+				lsb = _find_change(tmpval, 0);
+				msb =
+				    _find_change(tmpval,
+						 (CODE_MAX / CODE_STEP) - 1);
+				tmp = (lsb + msb) >> 1;
+
+				tmp = _rx_offset_cal_updn(tmp * CODE_STEP);
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_RX_CAL_X[index], tmp);
+			}
+		}
+	}
+	ddr_setval_ach_as(_reg_PHY_RX_CAL_OVERRIDE, 0x00);
+
+	return 0;
+}
+
+static uint32_t rx_offset_cal_hw(void)
+{
+	uint32_t ch, slice;
+	uint32_t retry;
+	uint32_t complete;
+	uint32_t tmp;
+	uint32_t tmp_ach_as[DRAM_CH_CNT][SLICE_CNT];
+
+	ddr_setval_ach_as(_reg_PHY_RX_CAL_X[9], 0x00);
+	ddr_setval_ach_as(_reg_PHY_RX_CAL_OVERRIDE, 0x00);
+	ddr_setval_ach_as(_reg_PHY_RX_CAL_SAMPLE_WAIT, 0x0f);
+
+	retry = 0;
+	while (retry < 4096) {
+		if ((retry & 0xff) == 0) {
+			ddr_setval_ach_as(_reg_SC_PHY_RX_CAL_START, 0x01);
+		}
+		foreach_vch(ch)
+		for (slice = 0; slice < SLICE_CNT; slice++)
+			tmp_ach_as[ch][slice] =
+			    ddr_getval_s(ch, slice, _reg_PHY_RX_CAL_X[9]);
+
+		complete = 1;
+		foreach_vch(ch) {
+			for (slice = 0; slice < SLICE_CNT; slice++) {
+				tmp = tmp_ach_as[ch][slice];
+				tmp = (tmp & 0x3f) + ((tmp >> 6) & 0x3f);
+				if (((prr_product == PRR_PRODUCT_H3) &&
+				     (prr_cut > PRR_PRODUCT_11)) ||
+				    (prr_product == PRR_PRODUCT_M3N) ||
+				    (prr_product == PRR_PRODUCT_V3H)) {
+					if (tmp != 0x3E)
+						complete = 0;
+				} else {
+					if (tmp != 0x40)
+						complete = 0;
+				}
+			}
+		}
+		if (complete)
+			break;
+
+		retry++;
+	}
+
+	return (complete == 0);
+}
+
+/* adjust rddqs latency */
+static void adjust_rddqs_latency(void)
+{
+	uint32_t ch, slice;
+	uint32_t dly;
+	uint32_t maxlatx2;
+	uint32_t tmp;
+	uint32_t rdlat_adjx2[SLICE_CNT];
+
+	foreach_vch(ch) {
+		maxlatx2 = 0;
+		for (slice = 0; slice < SLICE_CNT; slice++) {
+			ddr_setval_s(ch, slice, _reg_PHY_PER_CS_TRAINING_INDEX,
+				     0x00);
+
+			dly =
+			    ddr_getval_s(ch, slice,
+					 _reg_PHY_RDDQS_GATE_SLAVE_DELAY);
+			tmp =
+			    ddr_getval_s(ch, slice,
+					 _reg_PHY_RDDQS_LATENCY_ADJUST);
+			/* note gate_slave_delay[9] is always 0 */
+			tmp = (tmp << 1) + (dly >> 8);
+			rdlat_adjx2[slice] = tmp;
+			if (maxlatx2 < tmp)
+				maxlatx2 = tmp;
+		}
+		maxlatx2 = ((maxlatx2 + 1) >> 1) << 1;
+		for (slice = 0; slice < SLICE_CNT; slice++) {
+			tmp = maxlatx2 - rdlat_adjx2[slice];
+			tmp = (tmp >> 1);
+			if (tmp) {
+				ddr_setval_s(ch, slice, _reg_PHY_RPTR_UPDATE,
+					     ddr_getval_s(ch, slice,
+							  _reg_PHY_RPTR_UPDATE)
+					     + 1);
+			}
+		}
+	}
+}
+
+/* adjust wpath latency */
+static void adjust_wpath_latency(void)
+{
+	uint32_t ch, cs, slice;
+	uint32_t dly;
+	uint32_t wpath_add;
+	const uint32_t _par_EARLY_THRESHOLD_VAL = 0x180;
+
+	foreach_vch(ch) {
+		for (slice = 0; slice < SLICE_CNT; slice += 1) {
+			for (cs = 0; cs < CS_CNT; cs++) {
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_PER_CS_TRAINING_INDEX,
+					     cs);
+				ddr_getval_s(ch, slice,
+					     _reg_PHY_PER_CS_TRAINING_INDEX);
+				dly =
+				    ddr_getval_s(ch, slice,
+						 _reg_PHY_CLK_WRDQS_SLAVE_DELAY);
+				if (dly <= _par_EARLY_THRESHOLD_VAL)
+					continue;
+
+				wpath_add =
+				    ddr_getval_s(ch, slice,
+						 _reg_PHY_WRITE_PATH_LAT_ADD);
+				ddr_setval_s(ch, slice,
+					     _reg_PHY_WRITE_PATH_LAT_ADD,
+					     wpath_add - 1);
+			}
+		}
+	}
+}
+
+/* DDR Initialize entry */
+int32_t rcar_dram_init(void)
+{
+	uint32_t ch, cs;
+	uint32_t data_l;
+	uint32_t bus_mbps, bus_mbpsdiv;
+	uint32_t tmp_tccd;
+	uint32_t failcount;
+	uint32_t cnf_boardtype;
+
+	/* Thermal sensor setting */
+	data_l = mmio_read_32(CPG_MSTPSR5);
+	if (data_l & BIT(22)) {	/*  case THS/TSC Standby */
+		data_l &= ~BIT(22);
+		cpg_write_32(CPG_SMSTPCR5, data_l);
+		while (mmio_read_32(CPG_MSTPSR5) & BIT(22))
+			;  /*  wait bit=0 */
+	}
+
+	/* THCTR Bit6: PONM=0 , Bit0: THSST=0   */
+	data_l = mmio_read_32(THS1_THCTR);
+	if (data_l & 0x00000040U) {
+		data_l = data_l & 0xFFFFFFBEU;
+	} else {
+		data_l = data_l | BIT(1);
+	}
+
+	mmio_write_32(THS1_THCTR, data_l);
+
+	/* Judge product and cut */
+#ifdef RCAR_DDR_FIXED_LSI_TYPE
+#if (RCAR_LSI == RCAR_AUTO)
+	prr_product = mmio_read_32(PRR) & PRR_PRODUCT_MASK;
+	prr_cut = mmio_read_32(PRR) & PRR_CUT_MASK;
+#else /* RCAR_LSI */
+#ifndef RCAR_LSI_CUT
+	prr_cut = mmio_read_32(PRR) & PRR_CUT_MASK;
+#endif /* RCAR_LSI_CUT */
+#endif /* RCAR_LSI */
+#else /* RCAR_DDR_FIXED_LSI_TYPE */
+	prr_product = mmio_read_32(PRR) & PRR_PRODUCT_MASK;
+	prr_cut = mmio_read_32(PRR) & PRR_CUT_MASK;
+#endif /* RCAR_DDR_FIXED_LSI_TYPE */
+
+	if (prr_product == PRR_PRODUCT_H3) {
+		if (prr_cut <= PRR_PRODUCT_11) {
+			p_ddr_regdef_tbl =
+				(const uint32_t *)&DDR_REGDEF_TBL[0][0];
+		} else {
+			p_ddr_regdef_tbl =
+				(const uint32_t *)&DDR_REGDEF_TBL[2][0];
+		}
+	} else if (prr_product == PRR_PRODUCT_M3) {
+		p_ddr_regdef_tbl =
+			(const uint32_t *)&DDR_REGDEF_TBL[1][0];
+	} else if ((prr_product == PRR_PRODUCT_M3N) ||
+		   (prr_product == PRR_PRODUCT_V3H)) {
+		p_ddr_regdef_tbl =
+			(const uint32_t *)&DDR_REGDEF_TBL[3][0];
+	} else {
+		FATAL_MSG("BL2: DDR:Unknown Product\n");
+		return 0xff;
+	}
+
+	if (((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) ||
+	    ((prr_product == PRR_PRODUCT_M3) && (prr_cut < PRR_PRODUCT_30))) {
+		/* non : H3 Ver.1.x/M3-W Ver.1.x not support */
+	} else {
+		mmio_write_32(DBSC_DBSYSCNT0, 0x00001234);
+	}
+
+	/* Judge board type */
+	cnf_boardtype = boardcnf_get_brd_type();
+	if (cnf_boardtype >= BOARDNUM) {
+		FATAL_MSG("BL2: DDR:Unknown Board\n");
+		return 0xff;
+	}
+	board_cnf = (const struct _boardcnf *)&boardcnfs[cnf_boardtype];
+
+/* RCAR_DRAM_SPLIT_2CH           (2U) */
+#if RCAR_DRAM_SPLIT == 2
+	/* H3(Test for future H3-N): Swap ch2 and ch1 for 2ch-split */
+	if ((prr_product == PRR_PRODUCT_H3) && (board_cnf->phyvalid == 0x05)) {
+		mmio_write_32(DBSC_DBMEMSWAPCONF0, 0x00000006);
+		ddr_phyvalid = 0x03;
+	} else {
+		ddr_phyvalid = board_cnf->phyvalid;
+	}
+#else /* RCAR_DRAM_SPLIT_2CH */
+	ddr_phyvalid = board_cnf->phyvalid;
+#endif /* RCAR_DRAM_SPLIT_2CH */
+
+	max_density = 0;
+
+	for (cs = 0; cs < CS_CNT; cs++) {
+		ch_have_this_cs[cs] = 0;
+	}
+
+	foreach_ech(ch)
+	for (cs = 0; cs < CS_CNT; cs++)
+		ddr_density[ch][cs] = 0xff;
+
+	foreach_vch(ch) {
+		for (cs = 0; cs < CS_CNT; cs++) {
+			data_l = board_cnf->ch[ch].ddr_density[cs];
+			ddr_density[ch][cs] = data_l;
+
+			if (data_l == 0xff)
+				continue;
+			if (data_l > max_density)
+				max_density = data_l;
+			if ((cs == 1) && (prr_product == PRR_PRODUCT_H3) &&
+			    (prr_cut <= PRR_PRODUCT_11))
+				continue;
+			ch_have_this_cs[cs] |= (1U << ch);
+		}
+	}
+
+	/* Judge board clock frequency (in MHz) */
+	boardcnf_get_brd_clk(cnf_boardtype, &brd_clk, &brd_clkdiv);
+	if ((brd_clk / brd_clkdiv) > 25) {
+		brd_clkdiva = 1;
+	} else {
+		brd_clkdiva = 0;
+	}
+
+	/* Judge ddr operating frequency clock(in Mbps) */
+	boardcnf_get_ddr_mbps(cnf_boardtype, &ddr_mbps, &ddr_mbpsdiv);
+
+	ddr0800_mul = CLK_DIV(800, 2, brd_clk, brd_clkdiv * (brd_clkdiva + 1));
+
+	ddr_mul = CLK_DIV(ddr_mbps, ddr_mbpsdiv * 2, brd_clk,
+			  brd_clkdiv * (brd_clkdiva + 1));
+
+	/* Adjust tccd */
+	data_l = (0x00006000 & mmio_read_32(RST_MODEMR)) >> 13;
+	bus_mbps = 0;
+	bus_mbpsdiv = 0;
+	switch (data_l) {
+	case 0:
+		bus_mbps = brd_clk * 0x60 * 2;
+		bus_mbpsdiv = brd_clkdiv * 1;
+		break;
+	case 1:
+		bus_mbps = brd_clk * 0x50 * 2;
+		bus_mbpsdiv = brd_clkdiv * 1;
+		break;
+	case 2:
+		bus_mbps = brd_clk * 0x40 * 2;
+		bus_mbpsdiv = brd_clkdiv * 1;
+		break;
+	case 3:
+		bus_mbps = brd_clk * 0x60 * 2;
+		bus_mbpsdiv = brd_clkdiv * 2;
+		break;
+	default:
+		bus_mbps = brd_clk * 0x60 * 2;
+		bus_mbpsdiv = brd_clkdiv * 2;
+		break;
+	}
+	tmp_tccd = CLK_DIV(ddr_mbps * 8, ddr_mbpsdiv, bus_mbps, bus_mbpsdiv);
+	if (8 * ddr_mbps * bus_mbpsdiv != tmp_tccd * bus_mbps * ddr_mbpsdiv)
+		tmp_tccd = tmp_tccd + 1;
+
+	if (tmp_tccd < 8)
+		ddr_tccd = 8;
+	else
+		ddr_tccd = tmp_tccd;
+
+	NOTICE("BL2: DDR%d(%s)\n", ddr_mbps / ddr_mbpsdiv, RCAR_DDR_VERSION);
+
+	MSG_LF("Start\n");
+
+	/* PLL Setting */
+	pll3_control(1);
+
+	/* initialize DDR */
+	data_l = init_ddr();
+	if (data_l == ddr_phyvalid) {
+		failcount = 0;
+	} else {
+		failcount = 1;
+	}
+
+	foreach_vch(ch)
+	    mmio_write_32(DBSC_DBPDLK(ch), 0x00000000);
+	if (((prr_product == PRR_PRODUCT_H3) && (prr_cut <= PRR_PRODUCT_11)) ||
+	    ((prr_product == PRR_PRODUCT_M3) && (prr_cut < PRR_PRODUCT_30))) {
+		/* non : H3 Ver.1.x/M3-W Ver.1.x not support */
+	} else {
+		mmio_write_32(DBSC_DBSYSCNT0, 0x00000000);
+	}
+
+	if (failcount == 0) {
+		return INITDRAM_OK;
+	} else {
+		return INITDRAM_NG;
+	}
+}
+
+void pvtcode_update(void)
+{
+	uint32_t ch;
+	uint32_t data_l;
+	uint32_t pvtp[4], pvtn[4], pvtp_init, pvtn_init;
+	int32_t pvtp_tmp, pvtn_tmp;
+
+	foreach_vch(ch) {
+		pvtn_init = (tcal.tcomp_cal[ch] & 0xFC0) >> 6;
+		pvtp_init = (tcal.tcomp_cal[ch] & 0x03F) >> 0;
+
+		if (8912 * pvtp_init > 44230) {
+			pvtp_tmp = (5000 + 8912 * pvtp_init - 44230) / 10000;
+		} else {
+			pvtp_tmp =
+			    -((-(5000 + 8912 * pvtp_init - 44230)) / 10000);
+		}
+		pvtn_tmp = (5000 + 5776 * pvtn_init + 30280) / 10000;
+
+		pvtn[ch] = pvtn_tmp + pvtn_init;
+		pvtp[ch] = pvtp_tmp + pvtp_init;
+
+		if (pvtn[ch] > 63) {
+			pvtn[ch] = 63;
+			pvtp[ch] =
+			    (pvtp_tmp) * (63 - 6 * pvtn_tmp -
+					  pvtn_init) / (pvtn_tmp) +
+			    6 * pvtp_tmp + pvtp_init;
+		}
+		if ((prr_product == PRR_PRODUCT_H3) &&
+		    (prr_cut <= PRR_PRODUCT_11)) {
+			data_l = pvtp[ch] | (pvtn[ch] << 6) |
+				 (tcal.tcomp_cal[ch] & 0xfffff000);
+			reg_ddrphy_write(ch,
+					 ddr_regdef_adr(_reg_PHY_PAD_FDBK_TERM),
+					 data_l | 0x00020000);
+			reg_ddrphy_write(ch,
+					 ddr_regdef_adr(_reg_PHY_PAD_DATA_TERM),
+					 data_l);
+			reg_ddrphy_write(ch,
+					 ddr_regdef_adr(_reg_PHY_PAD_DQS_TERM),
+					 data_l);
+			reg_ddrphy_write(ch,
+					 ddr_regdef_adr(_reg_PHY_PAD_ADDR_TERM),
+					 data_l);
+			reg_ddrphy_write(ch,
+					 ddr_regdef_adr(_reg_PHY_PAD_CS_TERM),
+					 data_l);
+		} else {
+			data_l = pvtp[ch] | (pvtn[ch] << 6) | 0x00015000;
+			reg_ddrphy_write(ch,
+					 ddr_regdef_adr(_reg_PHY_PAD_FDBK_TERM),
+					 data_l | 0x00020000);
+			reg_ddrphy_write(ch,
+					 ddr_regdef_adr(_reg_PHY_PAD_DATA_TERM),
+					 data_l);
+			reg_ddrphy_write(ch,
+					 ddr_regdef_adr(_reg_PHY_PAD_DQS_TERM),
+					 data_l);
+			reg_ddrphy_write(ch,
+					 ddr_regdef_adr(_reg_PHY_PAD_ADDR_TERM),
+					 data_l);
+			reg_ddrphy_write(ch,
+					 ddr_regdef_adr(_reg_PHY_PAD_CS_TERM),
+					 data_l);
+		}
+	}
+}
+
+void pvtcode_update2(void)
+{
+	uint32_t ch;
+
+	foreach_vch(ch) {
+		reg_ddrphy_write(ch, ddr_regdef_adr(_reg_PHY_PAD_FDBK_TERM),
+				 tcal.init_cal[ch] | 0x00020000);
+		reg_ddrphy_write(ch, ddr_regdef_adr(_reg_PHY_PAD_DATA_TERM),
+				 tcal.init_cal[ch]);
+		reg_ddrphy_write(ch, ddr_regdef_adr(_reg_PHY_PAD_DQS_TERM),
+				 tcal.init_cal[ch]);
+		reg_ddrphy_write(ch, ddr_regdef_adr(_reg_PHY_PAD_ADDR_TERM),
+				 tcal.init_cal[ch]);
+		reg_ddrphy_write(ch, ddr_regdef_adr(_reg_PHY_PAD_CS_TERM),
+				 tcal.init_cal[ch]);
+	}
+}
+
+void ddr_padcal_tcompensate_getinit(uint32_t override)
+{
+	uint32_t ch;
+	uint32_t data_l;
+	uint32_t pvtp, pvtn;
+
+	tcal.init_temp = 0;
+	for (ch = 0; ch < 4; ch++) {
+		tcal.init_cal[ch] = 0;
+		tcal.tcomp_cal[ch] = 0;
+	}
+
+	foreach_vch(ch) {
+		tcal.init_cal[ch] = ddr_getval(ch, _reg_PHY_PAD_TERM_X[1]);
+		tcal.tcomp_cal[ch] = ddr_getval(ch, _reg_PHY_PAD_TERM_X[1]);
+	}
+
+	if (!override) {
+		data_l = mmio_read_32(THS1_TEMP);
+		if (data_l < 2800) {
+			tcal.init_temp =
+			    (143 * (int32_t)data_l - 359000) / 1000;
+		} else {
+			tcal.init_temp =
+			    (121 * (int32_t)data_l - 296300) / 1000;
+		}
+
+		foreach_vch(ch) {
+			pvtp = (tcal.init_cal[ch] >> 0) & 0x000003F;
+			pvtn = (tcal.init_cal[ch] >> 6) & 0x000003F;
+			if ((int32_t)pvtp >
+			    ((tcal.init_temp * 29 - 3625) / 1000))
+				pvtp =
+				    (int32_t)pvtp +
+				    ((3625 - tcal.init_temp * 29) / 1000);
+			else
+				pvtp = 0;
+
+			if ((int32_t)pvtn >
+			    ((tcal.init_temp * 54 - 6750) / 1000))
+				pvtn =
+				    (int32_t)pvtn +
+				    ((6750 - tcal.init_temp * 54) / 1000);
+			else
+				pvtn = 0;
+
+			if ((prr_product == PRR_PRODUCT_H3) &&
+			    (prr_cut <= PRR_PRODUCT_11)) {
+				tcal.init_cal[ch] =
+				    (tcal.init_cal[ch] & 0xfffff000) |
+				    (pvtn << 6) |
+				    pvtp;
+			} else {
+				tcal.init_cal[ch] =
+				    0x00015000 | (pvtn << 6) | pvtp;
+			}
+		}
+		tcal.init_temp = 125;
+	}
+}
+
+#ifndef ddr_qos_init_setting
+/* For QoS init */
+uint8_t get_boardcnf_phyvalid(void)
+{
+	return ddr_phyvalid;
+}
+#endif /* ddr_qos_init_setting */
diff --git a/drivers/renesas/common/ddr/ddr_b/boot_init_dram_config.c b/drivers/renesas/common/ddr/ddr_b/boot_init_dram_config.c
new file mode 100644
index 0000000..bbb0200
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_b/boot_init_dram_config.c
@@ -0,0 +1,2108 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation.
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RZG_SOC
+#define RZG_SOC		0
+#endif
+
+#if (RZG_SOC == 1)
+#define BOARDNUM 4
+#else
+
+#include <board.h>
+
+#define BOARDNUM 22
+#endif /* RZG_SOC == 1 */
+#define BOARD_JUDGE_AUTO
+
+#ifdef BOARD_JUDGE_AUTO
+static uint32_t _board_judge(void);
+
+static uint32_t boardcnf_get_brd_type(void)
+{
+	return _board_judge();
+}
+#else
+static uint32_t boardcnf_get_brd_type(void)
+{
+	return 1;
+}
+#endif
+
+#define DDR_FAST_INIT
+
+struct _boardcnf_ch {
+	uint8_t ddr_density[CS_CNT];
+	uint64_t ca_swap;
+	uint16_t dqs_swap;
+	uint32_t dq_swap[SLICE_CNT];
+	uint8_t dm_swap[SLICE_CNT];
+	uint16_t wdqlvl_patt[16];
+	int8_t cacs_adj[16];
+	int8_t dm_adj_w[SLICE_CNT];
+	int8_t dq_adj_w[SLICE_CNT * 8];
+	int8_t dm_adj_r[SLICE_CNT];
+	int8_t dq_adj_r[SLICE_CNT * 8];
+};
+
+struct _boardcnf {
+	uint8_t phyvalid;
+	uint8_t dbi_en;
+	uint16_t cacs_dly;
+	int16_t cacs_dly_adj;
+	uint16_t dqdm_dly_w;
+	uint16_t dqdm_dly_r;
+	struct _boardcnf_ch ch[DRAM_CH_CNT];
+};
+
+#define WDQLVL_PAT {\
+	0x00AA,\
+	0x0055,\
+	0x00AA,\
+	0x0155,\
+	0x01CC,\
+	0x0133,\
+	0x00CC,\
+	0x0033,\
+	0x00F0,\
+	0x010F,\
+	0x01F0,\
+	0x010F,\
+	0x00F0,\
+	0x00F0,\
+	0x000F,\
+	0x010F}
+
+#if (RZG_SOC == 1)
+static const struct _boardcnf boardcnfs[BOARDNUM] = {
+	{
+/* boardcnf[0] HopeRun HiHope RZ/G2M 16Gbit/1rank/2ch board with G2M SoC */
+	 .phyvalid = 0x03U,
+	 .dbi_en = 0x01U,
+	 .cacs_dly = 0x02c0U,
+	 .cacs_dly_adj = 0x0U,
+	 .dqdm_dly_w = 0x0300U,
+	 .dqdm_dly_r = 0x00a0U,
+	 .ch = {
+		{
+		 { 0x04U, 0xffU },
+		 0x00345201UL,
+		 0x3201U,
+		 { 0x01672543U, 0x45361207U, 0x45632107U, 0x60715234U },
+		 { 0x08U, 0x08U, 0x08U, 0x08U },
+		 WDQLVL_PAT,
+		 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+		 { 0, 0, 0, 0 },
+		 { 0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0 },
+		 { 0, 0, 0, 0 },
+		 { 0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0 }
+		},
+		{
+		 { 0x04U, 0xffU },
+		 0x00302154UL,
+		 0x2310U,
+		 { 0x01672543U, 0x45361207U, 0x45632107U, 0x60715234U },
+		 { 0x08U, 0x08U, 0x08U, 0x08U },
+		 WDQLVL_PAT,
+		 { 0, 0, 0, 0, 0, 0, 0, 0,  0, 0 },
+		 { 0, 0, 0, 0 },
+		 { 0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0 },
+		 { 0, 0, 0, 0 },
+		 { 0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0 }
+		}
+		}
+	},
+/* boardcnf[1] HopeRun HiHope RZ/G2M 8Gbit/2rank/2ch board with G2M SoC */
+	{
+	 0x03U,
+	 0x01U,
+	 0x02c0U,
+	 0x0U,
+	 0x0300U,
+	 0x00a0U,
+	{
+		{
+		 { 0x02U, 0x02U },
+		 0x00345201UL,
+		 0x3201U,
+		 { 0x01672543U, 0x45361207U, 0x45632107U, 0x60715234U },
+		 { 0x08U, 0x08U, 0x08U, 0x08U },
+		 WDQLVL_PAT,
+		 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+		 { 0, 0, 0, 0 },
+		 { 0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0 },
+		 { 0, 0, 0, 0 },
+		 { 0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0 }
+		},
+		{
+		 { 0x02U, 0x02U },
+		 0x00302154UL,
+		 0x2310,
+		 { 0x01672543U, 0x45361207U, 0x45632107U, 0x60715234U },
+		 { 0x08U, 0x08U, 0x08U, 0x08U },
+		 WDQLVL_PAT,
+		 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+		 { 0, 0, 0, 0 },
+		 { 0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0,
+		   0, 0, 0, 0, 0, 0, 0, 0 },
+		{ 0, 0, 0, 0 },
+		{ 0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0 }
+		}
+	}
+	},
+/* boardcnf[2] HopeRun HiHope RZ/G2H board 16Gbit/1rank/2ch */
+	{
+		0x05U,
+		0x01U,
+		0x0300U,
+		0,
+		0x0300U,
+		0x00a0U,
+		{
+			{
+				{ 0x04U, 0xffU },
+				0x00345201UL,
+				0x3201U,
+				{ 0x01672543U, 0x45367012U, 0x45632107U, 0x60715234U },
+				{ 0x08U, 0x08U, 0x08U, 0x08U },
+				WDQLVL_PAT,
+				{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+				{ 0, 0, 0, 0 },
+				{ 0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0 },
+				{ 0, 0, 0, 0 },
+				{ 0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0 }
+			},
+			{
+				{ 0x04U, 0xffU },
+				0x00302154UL,
+				0x2310U,
+				{ 0x01672543U, 0x45361207U, 0x45632107U, 0x60715234U },
+				{ 0x08U, 0x08U, 0x08U, 0x08U },
+				WDQLVL_PAT,
+				{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+				{ 0, 0, 0, 0 },
+				{ 0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0 },
+				{ 0, 0, 0, 0 },
+				{ 0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0 }
+			},
+			{
+				{ 0x04U, 0xffU },
+				0x00302154UL,
+				0x2310U,
+				{ 0x01672543U, 0x45361207U, 0x45632107U, 0x60715234U },
+				{ 0x08U, 0x08U, 0x08U, 0x08U },
+				WDQLVL_PAT,
+				{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+				{ 0, 0, 0, 0 },
+				{ 0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0 },
+				{ 0, 0, 0, 0 },
+				{ 0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0 }
+			},
+			{
+				{ 0xffU, 0xffU },
+				0UL,
+				0U,
+				{ 0U, 0U, 0U, 0U },
+				{ 0U, 0U, 0U, 0U },
+				WDQLVL_PAT,
+				{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+				{ 0, 0, 0, 0 },
+				{ 0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0 },
+				{ 0, 0, 0, 0 },
+				{ 0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0 }
+			}
+		}
+	},
+/* boardcnf[3] HopeRun HiHope RZ/G2N board 16Gbit/2rank/1ch */
+	{
+		0x01U,
+		0x01U,
+		0x0300U,
+		0,
+		0x0300U,
+		0x00a0U,
+		{
+			{
+				{ 0x04U, 0x04U },
+				0x00345201UL,
+				0x3201U,
+				{ 0x01672543U, 0x45361207U, 0x45632107U, 0x60715234U },
+				{ 0x08U, 0x08U, 0x08U, 0x08U },
+				WDQLVL_PAT,
+				{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+				{ 0, 0, 0, 0 },
+				{ 0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0 },
+				{ 0, 0, 0, 0 },
+				{ 0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0,
+				  0, 0, 0, 0, 0, 0, 0, 0 }
+			}
+		}
+	},
+};
+#else
+static const struct _boardcnf boardcnfs[BOARDNUM] = {
+	{
+/* boardcnf[0] RENESAS SALVATOR-X board with M3-W/SIP */
+	 .phyvalid = 0x03,
+	 .dbi_en = 0x01,
+	 .cacs_dly = 0x02c0,
+	 .cacs_dly_adj = 0,
+	 .dqdm_dly_w = 0x0300,
+	 .dqdm_dly_r = 0x00a0,
+	 .ch = {
+		{
+		 {0x02, 0x02},
+		 0x00543210U,
+		 0x3201U,
+		 {0x70612543, 0x43251670, 0x45326170, 0x10672534},
+		 {0x08, 0x08, 0x08, 0x08},
+		 WDQLVL_PAT,
+		 {0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0},
+		 {0, 0, 0, 0},
+		 {0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0},
+		 {0, 0, 0, 0},
+		 {0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0}
+		 },
+
+		{
+		 {0x02, 0x02},
+		 0x00543210,
+		 0x2310,
+		 {0x01327654, 0x34526107, 0x35421670, 0x70615324},
+		 {0x08, 0x08, 0x08, 0x08},
+		 WDQLVL_PAT,
+		 {0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0},
+		 {0, 0, 0, 0},
+		 {0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0},
+		 {0, 0, 0, 0},
+		 {0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0,
+		  0, 0, 0, 0, 0, 0, 0, 0}
+		}
+		}
+	 },
+/* boardcnf[1] RENESAS KRIEK board with M3-W/SoC */
+	{
+	 0x03,
+	 0x01,
+	 0x2c0,
+	 0,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x02, 0x02},
+	   0x00345201,
+	   0x3201,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0x02},
+	   0x00302154,
+	   0x2310,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[2] RENESAS SALVATOR-X board with H3 Ver.1.x/SIP(8Gbit 1rank) */
+	{
+	 0x0f,
+	 0x00,
+	 0x300,
+	 -320,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x02, 0xff},
+	   0x00543210,
+	   0x3210,
+	   {0x20741365, 0x34256107, 0x57460321, 0x70614532},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00543210,
+	   0x3102,
+	   {0x23547610, 0x34526107, 0x67452310, 0x32106754},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00543210,
+	   0x0213,
+	   {0x30216754, 0x67453210, 0x70165243, 0x07162345},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00543210,
+	   0x0213,
+	   {0x01327654, 0x70615432, 0x54760123, 0x07162345},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[3] RENESAS Starter Kit board with M3-W/SIP(8Gbit 1rank) */
+	{
+	 0x03,
+	 0x01,
+	 0x02c0,
+	 0,
+	 0x0300,
+	 0x00a0,
+	{
+	{
+	   {0x02, 0xFF},
+	   0x00543210U,
+	   0x3201,
+	   {0x70612543, 0x43251670, 0x45326170, 0x10672534},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xFF},
+	   0x00543210,
+	   0x2310,
+	   {0x01327654, 0x34526107, 0x35421670, 0x70615324},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[4] RENESAS SALVATOR-M(1rank) board with H3 Ver.1.x/SoC */
+	{
+	 0x0f,
+	 0x00,
+	 0x2c0,
+	 -320,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x02, 0xff},
+	   0x00315024,
+	   0x3120,
+	   {0x30671254, 0x26541037, 0x17054623, 0x12307645},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00025143,
+	   0x3210,
+	   {0x70613542, 0x16245307, 0x30712645, 0x21706354},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00523104,
+	   0x2301,
+	   {0x70613542, 0x16245307, 0x30712645, 0x21706354},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00153402,
+	   0x2031,
+	   {0x30671254, 0x26541037, 0x17054623, 0x12307645},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[5] RENESAS KRIEK-1rank board with M3-W/SoC */
+	{
+	 0x03,
+	 0x01,
+	 0x2c0,
+	 0,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x02, 0xff},
+	   0x00345201,
+	   0x3201,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00302154,
+	   0x2310,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[6] RENESAS SALVATOR-X board with H3 Ver.1.x/SIP(8Gbit 2rank) */
+	{
+	 0x0f,
+	 0x00,
+	 0x300,
+	 -320,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x02, 0x02},
+	   0x00543210,
+	   0x3210,
+	   {0x20741365, 0x34256107, 0x57460321, 0x70614532},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0x02},
+	   0x00543210,
+	   0x3102,
+	   {0x23547610, 0x34526107, 0x67452310, 0x32106754},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0x02},
+	   0x00543210,
+	   0x0213,
+	   {0x30216754, 0x67453210, 0x70165243, 0x07162345},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0x02},
+	   0x00543210,
+	   0x0213,
+	   {0x01327654, 0x70615432, 0x54760123, 0x07162345},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/*
+ * boardcnf[7] RENESAS SALVATOR-X board with
+ * H3 Ver.2.0 or later/SIP(8Gbit 1rank)
+ */
+	{
+	 0x0f,
+	 0x01,
+	 0x300,
+	 0,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x02, 0xff},
+	   0x00543210,
+	   0x2310,
+	   {0x70631425, 0x34527016, 0x43527610, 0x32104567},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00105432,
+	   0x3210,
+	   {0x43256107, 0x07162354, 0x10234567, 0x01235467},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00543210,
+	   0x2301,
+	   {0x01327654, 0x02316457, 0x10234567, 0x01325467},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00543210,
+	   0x2301,
+	   {0x12034765, 0x23105467, 0x23017645, 0x32106745},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/*
+ * boardcnf[8] RENESAS SALVATOR-X board with
+ * H3 Ver.2.0 or later/SIP(8Gbit 2rank)
+ */
+	{
+#if RCAR_DRAM_CHANNEL == 5
+	 0x05,
+#else
+	 0x0f,
+#endif
+	 0x01,
+	 0x300,
+	 0,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x02, 0x02},
+	   0x00543210,
+	   0x2310,
+	   {0x70631425, 0x34527016, 0x43527610, 0x32104567},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+#if ((RCAR_DRAM_CHANNEL == 5) && (RCAR_DRAM_SPLIT == 2))
+	{
+	   {0x02, 0x02},
+	   0x00543210,
+	   0x2301,
+	   {0x01327654, 0x02316457, 0x10234567, 0x01325467},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+#else
+	{
+	   {0x02, 0x02},
+	   0x00105432,
+	   0x3210,
+	   {0x43256107, 0x07162354, 0x10234567, 0x01235467},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+#endif
+	{
+	   {0x02, 0x02},
+	   0x00543210,
+	   0x2301,
+	   {0x01327654, 0x02316457, 0x10234567, 0x01325467},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0x02},
+	   0x00543210,
+	   0x2301,
+	   {0x12034765, 0x23105467, 0x23017645, 0x32106745},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[9] RENESAS SALVATOR-MS(1rank) board with H3 Ver.2.0 or later/SoC */
+	{
+	 0x0f,
+	 0x01,
+	 0x300,
+	 0,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x02, 0xff},
+	   0x00543210,
+	   0x3210,
+	   {0x27645310, 0x75346210, 0x53467210, 0x23674510},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00543210,
+	   0x2301,
+	   {0x23764510, 0x43257610, 0x43752610, 0x37652401},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {-128, -128, -128, -128, -128, -128, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00452103,
+	   0x3210,
+	   {0x32764510, 0x43257610, 0x43752610, 0x26573401},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0xff},
+	   0x00520413,
+	   0x2301,
+	   {0x47652301, 0x75346210, 0x53467210, 0x32674501},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {30, 30, 30, 30, 30, 30, 30, 30,
+	    30, 30},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[10] RENESAS Kriek(2rank) board with M3-N/SoC */
+	{
+	 0x01,
+	 0x01,
+	 0x300,
+	 0,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x02, 0x02},
+	   0x00345201,
+	   0x3201,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[11] RENESAS SALVATOR-X board with M3-N/SIP(8Gbit 2rank) */
+	{
+	 0x01,
+	 0x01,
+	 0x300,
+	 0,
+	 0x300,
+	 0x0a0,
+	{
+	{
+#if (RCAR_DRAM_LPDDR4_MEMCONF == 2)
+	   {0x04, 0x04},
+#else
+	   {0x02, 0x02},
+#endif
+	   0x00342501,
+	   0x3201,
+	   {0x10672534, 0x43257106, 0x34527601, 0x71605243},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[12] RENESAS CONDOR board with V3H/SoC */
+	{
+	 0x01,
+	 0x1,
+	 0x300,
+	 0,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x02, 0x02},
+	   0x00501342,
+	   0x3201,
+	   {0x70562134, 0x34526071, 0x23147506, 0x12430567},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[13] RENESAS KRIEK board with PM3/SoC */
+	{
+	 0x05,
+	 0x00,
+	 0x2c0,
+	 -320,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x02, 0x02},
+	   0x00345201,
+	   0x3201,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0x02},
+	   0x00302154,
+	   0x2310,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0x02},
+	   0x00302154,
+	   0x2310,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0xff, 0xff},
+	   0,
+	   0,
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[14] SALVATOR-X board with H3 Ver.2.0 or later/SIP(16Gbit 1rank) */
+	{
+#if RCAR_DRAM_CHANNEL == 5
+	 0x05,
+#else
+	 0x0f,
+#endif
+	 0x01,
+	 0x300,
+	 0,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x04, 0xff},
+	   0x00543210,
+	   0x2310,
+	   {0x70631425, 0x34527016, 0x43527610, 0x32104567},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+#if ((RCAR_DRAM_CHANNEL == 5) && (RCAR_DRAM_SPLIT == 2))
+	{
+	   {0x04, 0xff},
+	   0x00543210,
+	   0x2301,
+	   {0x01327654, 0x02316457, 0x10234567, 0x01325467},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+#else
+	{
+	   {0x04, 0xff},
+	   0x00105432,
+	   0x3210,
+	   {0x43256107, 0x07162354, 0x10234567, 0x01235467},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+#endif
+	{
+	   {0x04, 0xff},
+	   0x00543210,
+	   0x2301,
+	   {0x01327654, 0x02316457, 0x10234567, 0x01325467},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x04, 0xff},
+	   0x00543210,
+	   0x2301,
+	   {0x12034765, 0x23105467, 0x23017645, 0x32106745},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[15] RENESAS KRIEK board with H3N */
+	{
+	 0x05,
+	 0x01,
+	 0x300,
+	 0,
+	 0x300,
+	 0x0a0,
+	{
+	{
+	   {0x02, 0x02},
+	   0x00345201,
+	   0x3201,
+	   {0x01672543, 0x45367012, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0x02},
+	   0x00302154,
+	   0x2310,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x02, 0x02},
+	   0x00302154,
+	   0x2310,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0xff, 0xff},
+	   0,
+	   0,
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[16] RENESAS KRIEK-P2P board with M3-W/SoC */
+	{
+	 0x03,
+	 0x01,
+	 0x0320,
+	 0,
+	 0x0300,
+	 0x00a0,
+	{
+	{
+	   {0x04, 0x04},
+	    0x520314FFFF523041,
+	    0x3201,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	    WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x04, 0x04},
+	    0x314250FFFF312405,
+	    0x2310,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	    WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	 },
+/* boardcnf[17] RENESAS KRIEK-P2P board with M3-N/SoC */
+	{
+	 0x01,
+	 0x01,
+	 0x0300,
+	 0,
+	 0x0300,
+	 0x00a0,
+	{
+	{
+	   {0x04, 0x04},
+	    0x520314FFFF523041,
+	    0x3201,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	    WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	},
+/* boardcnf[18] RENESAS SALVATOR-X board with M3-W/SIP(16Gbit 2rank) */
+	{
+	 0x03,
+	 0x01,
+	 0x02c0,
+	 0,
+	 0x0300,
+	 0x00a0,
+	{
+	{
+	   {0x04, 0x04},
+	    0x00543210,
+	    0x3201,
+	   {0x70612543, 0x43251670, 0x45326170, 0x10672534},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x04, 0x04},
+	    0x00543210,
+	    0x2310,
+	   {0x01327654, 0x34526107, 0x35421670, 0x70615324},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	},
+/* boardcnf[19] RENESAS SALVATOR-X board with M3-W/SIP(16Gbit 1rank) */
+	{
+	 0x03,
+	 0x01,
+	 0x02c0,
+	 0,
+	 0x0300,
+	 0x00a0,
+	{
+	{
+	   {0x04, 0xff},
+	    0x00543210,
+	    0x3201,
+	   {0x70612543, 0x43251670, 0x45326170, 0x10672534},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x04, 0xff},
+	    0x00543210,
+	    0x2310,
+	   {0x01327654, 0x34526107, 0x35421670, 0x70615324},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	},
+/* boardcnf[20] RENESAS KRIEK 16Gbit/2rank/2ch board with M3-W/SoC */
+	{
+	 0x03,
+	 0x01,
+	 0x02c0,
+	 0,
+	 0x0300,
+	 0x00a0,
+	{
+	{
+	   {0x04, 0x04},
+	    0x00345201,
+	    0x3201,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	    WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x04, 0x04},
+	    0x00302154,
+	    0x2310,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	},
+/* boardcnf[21] RENESAS KRIEK 16Gbit/1rank/2ch board with M3-W/SoC */
+	{
+	 0x03,
+	 0x01,
+	 0x02c0,
+	 0,
+	 0x0300,
+	 0x00a0,
+	{
+	{
+	   {0x04, 0xff},
+	    0x00345201,
+	    0x3201,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	   },
+	{
+	   {0x04, 0xff},
+	    0x00302154,
+	    0x2310,
+	   {0x01672543, 0x45361207, 0x45632107, 0x60715234},
+	   {0x08, 0x08, 0x08, 0x08},
+	   WDQLVL_PAT,
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0},
+	   {0, 0, 0, 0},
+	   {0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0}
+	}
+	}
+	}
+};
+#endif /* RZG_SOC == 1 */
+
+void boardcnf_get_brd_clk(uint32_t brd, uint32_t *clk, uint32_t *div)
+{
+	uint32_t md;
+
+	if ((prr_product == PRR_PRODUCT_H3) && (prr_cut == PRR_PRODUCT_10)) {
+		*clk = 50;
+		*div = 3;
+	} else {
+		md = (mmio_read_32(RST_MODEMR) >> 13) & 0x3;
+		switch (md) {
+		case 0x0:
+			*clk = 50;
+			*div = 3;
+			break;
+		case 0x1:
+			*clk = 60;
+			*div = 3;
+			break;
+		case 0x2:
+			*clk = 75;
+			*div = 3;
+			break;
+		case 0x3:
+			*clk = 100;
+			*div = 3;
+			break;
+		}
+	}
+	(void)brd;
+}
+
+void boardcnf_get_ddr_mbps(uint32_t brd, uint32_t *mbps, uint32_t *div)
+{
+	uint32_t md;
+
+	if (prr_product == PRR_PRODUCT_V3H) {
+		md = (mmio_read_32(RST_MODEMR) >> 19) & 0x1;
+		md = (md | (md << 1)) & 0x3; /* 0 or 3 */
+	} else {
+		md = (mmio_read_32(RST_MODEMR) >> 17) & 0x5;
+		md = (md | (md >> 1)) & 0x3;
+	}
+	switch (md) {
+	case 0x0:
+		*mbps = 3200;
+		*div = 1;
+		break;
+	case 0x1:
+		*mbps = 2800;
+		*div = 1;
+		break;
+	case 0x2:
+		*mbps = 2400;
+		*div = 1;
+		break;
+	case 0x3:
+		*mbps = 1600;
+		*div = 1;
+		break;
+	}
+	(void)brd;
+}
+
+#define _def_REFPERIOD  1890
+
+#define M3_SAMPLE_TT_A84        0xB866CC10, 0x3B250421
+#define M3_SAMPLE_TT_A85        0xB866CC10, 0x3AA50421
+#define M3_SAMPLE_TT_A86        0xB866CC10, 0x3AA48421
+#define M3_SAMPLE_FF_B45        0xB866CC10, 0x3AB00C21
+#define M3_SAMPLE_FF_B49        0xB866CC10, 0x39B10C21
+#define M3_SAMPLE_FF_B56        0xB866CC10, 0x3AAF8C21
+#define M3_SAMPLE_SS_E24        0xB866CC10, 0x3BA39421
+#define M3_SAMPLE_SS_E28        0xB866CC10, 0x3C231421
+#define M3_SAMPLE_SS_E32        0xB866CC10, 0x3C241421
+
+static const uint32_t termcode_by_sample[20][3] = {
+	{M3_SAMPLE_TT_A84, 0x000158D5},
+	{M3_SAMPLE_TT_A85, 0x00015955},
+	{M3_SAMPLE_TT_A86, 0x00015955},
+	{M3_SAMPLE_FF_B45, 0x00015690},
+	{M3_SAMPLE_FF_B49, 0x00015753},
+	{M3_SAMPLE_FF_B56, 0x00015793},
+	{M3_SAMPLE_SS_E24, 0x00015996},
+	{M3_SAMPLE_SS_E28, 0x000159D7},
+	{M3_SAMPLE_SS_E32, 0x00015997},
+	{0xFFFFFFFF, 0xFFFFFFFF, 0x0001554F}
+};
+
+#ifdef BOARD_JUDGE_AUTO
+/*
+ * SAMPLE board detect function
+ */
+#define PFC_PMMR	0xE6060000U
+#define PFC_PUEN5	0xE6060414U
+#define PFC_PUEN6	0xE6060418U
+#define PFC_PUD5	0xE6060454U
+#define PFC_PUD6	0xE6060458U
+#define GPIO_INDT5	0xE605500CU
+#define GPIO_GPSR6	0xE6060118U
+
+#if (RCAR_GEN3_ULCB == 0) && (RZG_SOC == 0)
+static void pfc_write_and_poll(uint32_t a, uint32_t v)
+{
+	mmio_write_32(PFC_PMMR, ~v);
+	v = ~mmio_read_32(PFC_PMMR);
+	mmio_write_32(a, v);
+	while (v != mmio_read_32(a))
+		;
+	dsb_sev();
+}
+#endif
+
+#ifndef RCAR_GEN3_ULCB
+#define RCAR_GEN3_ULCB		0
+#endif
+
+#if (RCAR_GEN3_ULCB == 0) && (RZG_SOC == 0)	/* non Starter Kit */
+
+static uint32_t opencheck_SSI_WS6(void)
+{
+	uint32_t dataL, down, up;
+	uint32_t gpsr6_bak;
+	uint32_t puen5_bak;
+	uint32_t pud5_bak;
+
+	gpsr6_bak = mmio_read_32(GPIO_GPSR6);
+	puen5_bak = mmio_read_32(PFC_PUEN5);
+	pud5_bak = mmio_read_32(PFC_PUD5);
+	dsb_sev();
+
+	dataL = (gpsr6_bak & ~BIT(15));
+	pfc_write_and_poll(GPIO_GPSR6, dataL);
+
+	/* Pull-Up/Down Enable (PUEN5[22]=1) */
+	dataL = puen5_bak;
+	dataL |= (BIT(22));
+	pfc_write_and_poll(PFC_PUEN5, dataL);
+
+	/* Pull-Down-Enable (PUD5[22]=0, PUEN5[22]=1) */
+	dataL = pud5_bak;
+	dataL &= ~(BIT(22));
+	pfc_write_and_poll(PFC_PUD5, dataL);
+	/* GPSR6[15]=SSI_WS6 */
+	rcar_micro_delay(10);
+	down = (mmio_read_32(GPIO_INDT6) >> 15) & 0x1;
+	dsb_sev();
+
+	/* Pull-Up-Enable (PUD5[22]=1, PUEN5[22]=1) */
+	dataL = pud5_bak;
+	dataL |= (BIT(22));
+	pfc_write_and_poll(PFC_PUD5, dataL);
+
+	/* GPSR6[15]=SSI_WS6 */
+	rcar_micro_delay(10);
+	up = (mmio_read_32(GPIO_INDT6) >> 15) & 0x1;
+
+	dsb_sev();
+
+	pfc_write_and_poll(GPIO_GPSR6, gpsr6_bak);
+	pfc_write_and_poll(PFC_PUEN5, puen5_bak);
+	pfc_write_and_poll(PFC_PUD5, pud5_bak);
+
+	if (down == up) {
+		/* Same = Connect */
+		return 0;
+	}
+
+	/* Diff = Open */
+	return 1;
+}
+
+#endif
+
+#if (RZG_SOC == 1)
+#define LPDDR4_2RANK   (0x01U << 25U)
+
+static uint32_t rzg2_board_judge(void)
+{
+	uint32_t brd;
+
+	switch (prr_product) {
+	case PRR_PRODUCT_M3:
+		brd = 1U;
+		if ((mmio_read_32(PRR) & PRR_CUT_MASK) != RCAR_M3_CUT_VER11) {
+			if ((mmio_read_32(GPIO_INDT5) & LPDDR4_2RANK) == 0U) {
+				brd = 0U;
+			}
+		}
+		break;
+	case PRR_PRODUCT_H3:
+		brd = 2U;
+		break;
+	case PRR_PRODUCT_M3N:
+		brd = 3U;
+		break;
+	default:
+		brd = 99U;
+	}
+
+	return brd;
+}
+#endif /* RZG_SOC == 1 */
+
+#if (RZG_SOC == 0) && (RCAR_DRAM_LPDDR4_MEMCONF != 0)
+static uint32_t ddr_rank_judge(void)
+{
+	uint32_t brd;
+
+#if (RCAR_DRAM_MEMRANK == 0)
+	int32_t ret;
+	uint32_t type = 0U;
+	uint32_t rev = 0U;
+
+	brd = 99U;
+	ret = rcar_get_board_type(&type, &rev);
+	if ((ret == 0) && (rev != 0xFFU)) {
+		if (type == (uint32_t)BOARD_SALVATOR_XS) {
+			if (rev == 0x11U) {
+				brd = 14U;
+			} else {
+				brd = 8U;
+			}
+		} else if (type == (uint32_t)BOARD_STARTER_KIT_PRE) {
+			if (rev == 0x21U) {
+				brd = 14U;
+			} else {
+				brd = 8U;
+			}
+		}
+	}
+#elif (RCAR_DRAM_MEMRANK == 1)
+	brd = 14U;
+#elif (RCAR_DRAM_MEMRANK == 2)
+	brd = 8U;
+#else
+#error Invalid value was set to RCAR_DRAM_MEMRANK
+#endif /* (RCAR_DRAM_MEMRANK == 0) */
+	return brd;
+}
+#endif /* (RCAR_DRAM_LPDDR4_MEMCONF != 0) */
+
+static uint32_t _board_judge(void)
+{
+	uint32_t brd;
+
+#if (RZG_SOC == 1)
+	brd = rzg2_board_judge();
+#else
+#if (RCAR_GEN3_ULCB == 1)
+	/* Starter Kit */
+	if (prr_product == PRR_PRODUCT_H3) {
+		if (prr_cut <= PRR_PRODUCT_11) {
+			/* RENESAS Starter Kit(H3 Ver.1.x/SIP) board */
+			brd = 2;
+		} else {
+			/* RENESAS Starter Kit(H3 Ver.2.0 or later/SIP) board */
+#if (RCAR_DRAM_LPDDR4_MEMCONF == 0)
+			brd = 7;
+#else
+			brd = ddr_rank_judge();
+#endif
+		}
+	} else if (prr_product == PRR_PRODUCT_M3) {
+		if (prr_cut >= PRR_PRODUCT_30) {
+			/* RENESAS Starter Kit (M3-W Ver.3.0/SIP) */
+			brd = 18;
+		} else {
+			/* RENESAS Starter Kit(M3-W/SIP 8Gbit 1rank) board */
+			brd = 3;
+		}
+	} else {
+		/* RENESAS Starter Kit(M3-N/SIP) board */
+		brd = 11;
+	}
+#else
+	uint32_t usb2_ovc_open;
+
+	usb2_ovc_open = opencheck_SSI_WS6();
+
+	/* RENESAS Eva-board */
+	brd = 99;
+	if (prr_product == PRR_PRODUCT_V3H) {
+		/* RENESAS Condor board */
+		brd = 12;
+	} else if (usb2_ovc_open) {
+		if (prr_product == PRR_PRODUCT_M3N) {
+			/* RENESAS Kriek board with M3-N */
+			brd = 10;
+		} else if (prr_product == PRR_PRODUCT_M3) {
+			/* RENESAS Kriek board with M3-W */
+			brd = 1;
+		} else if ((prr_product == PRR_PRODUCT_H3) &&
+			   (prr_cut <= PRR_PRODUCT_11)) {
+			/* RENESAS Kriek board with PM3 */
+			brd = 13;
+		} else if ((prr_product == PRR_PRODUCT_H3) &&
+			   (prr_cut > PRR_PRODUCT_20)) {
+			/* RENESAS Kriek board with H3N */
+			brd = 15;
+		}
+	} else {
+		if (prr_product == PRR_PRODUCT_H3) {
+			if (prr_cut <= PRR_PRODUCT_11) {
+				/* RENESAS SALVATOR-X (H3 Ver.1.x/SIP) */
+				brd = 2;
+			} else if (prr_cut < PRR_PRODUCT_30) {
+				/* RENESAS SALVATOR-X (H3 Ver.2.0/SIP) */
+				brd = 7;	//  8Gbit/1rank
+			} else {
+				/* RENESAS SALVATOR-X (H3 Ver.3.0/SIP) */
+#if (RCAR_DRAM_LPDDR4_MEMCONF == 0)
+				brd = 7;
+#else
+				brd = ddr_rank_judge();
+#endif
+			}
+		} else if (prr_product == PRR_PRODUCT_M3N) {
+			/* RENESAS SALVATOR-X (M3-N/SIP) */
+			brd = 11;
+		} else if ((prr_product == PRR_PRODUCT_M3) &&
+			   (prr_cut <= PRR_PRODUCT_20)) {
+			/* RENESAS SALVATOR-X (M3-W/SIP) */
+			brd = 0;
+		} else if ((prr_product == PRR_PRODUCT_M3) &&
+			   (prr_cut < PRR_PRODUCT_30)) {
+			/* RENESAS SALVATOR-X (M3-W Ver.1.x/SIP) */
+			brd = 19;
+		} else if ((prr_product == PRR_PRODUCT_M3) &&
+			   (prr_cut >= PRR_PRODUCT_30)) {
+			/* RENESAS SALVATOR-X (M3-W ver.3.0/SIP) */
+			brd = 18;
+		}
+	}
+#endif
+#endif /* RZG_SOC == 1 */
+
+	return brd;
+}
+#endif
diff --git a/drivers/renesas/common/ddr/ddr_b/boot_init_dram_regdef.h b/drivers/renesas/common/ddr/ddr_b/boot_init_dram_regdef.h
new file mode 100644
index 0000000..3cb1975
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_b/boot_init_dram_regdef.h
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation.
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#define RCAR_DDR_VERSION	"rev.0.41"
+#define DRAM_CH_CNT		0x04
+#define SLICE_CNT		0x04
+#define CS_CNT			0x02
+
+/* order : CS0A, CS0B, CS1A, CS1B */
+#define CSAB_CNT		(CS_CNT * 2)
+
+/* order : CH0A, CH0B, CH1A, CH1B, CH2A, CH2B, CH3A, CH3B */
+#define CHAB_CNT		(DRAM_CH_CNT * 2)
+
+/* pll setting */
+#define CLK_DIV(a, diva, b, divb) (((a) * (divb)) / ((b) * (diva)))
+#define CLK_MUL(a, diva, b, divb) (((a) * (b)) / ((diva) * (divb)))
+
+/* for ddr deisity setting */
+#define DBMEMCONF_REG(d3, row, bank, col, dw)	\
+	(((d3) << 30) | ((row) << 24) | ((bank) << 16) | ((col) << 8) | (dw))
+
+#define DBMEMCONF_REGD(density)		\
+	(DBMEMCONF_REG((density) % 2, ((density) + 1) / \
+	2 + (29 - 3 - 10 - 2), 3, 10, 2))
+
+#define DBMEMCONF_VAL(ch, cs) (DBMEMCONF_REGD(DBMEMCONF_DENS(ch, cs)))
+
+/* refresh mode */
+#define DBSC_REFINTS		(0x0)
+
+/* system registers */
+#define CPG_FRQCRB		(CPG_BASE + 0x0004U)
+
+#define CPG_PLLECR		(CPG_BASE + 0x00D0U)
+#define CPG_MSTPSR5		(CPG_BASE + 0x003CU)
+#define CPG_SRCR4		(CPG_BASE + 0x00BCU)
+#define CPG_PLL3CR		(CPG_BASE + 0x00DCU)
+#define CPG_ZB3CKCR		(CPG_BASE + 0x0380U)
+#define CPG_FRQCRD		(CPG_BASE + 0x00E4U)
+#define CPG_SMSTPCR5		(CPG_BASE + 0x0144U)
+#define CPG_CPGWPR		(CPG_BASE + 0x0900U)
+#define CPG_SRSTCLR4		(CPG_BASE + 0x0950U)
+
+#define CPG_FRQCRB_KICK_BIT	BIT(31)
+#define CPG_PLLECR_PLL3E_BIT	BIT(3)
+#define CPG_PLLECR_PLL3ST_BIT	BIT(11)
+#define CPG_ZB3CKCR_ZB3ST_BIT	BIT(11)
+
+#define RST_BASE		(0xE6160000U)
+#define RST_MODEMR		(RST_BASE + 0x0060U)
+
+#define LIFEC_CHIPID(x)		(0xE6110040U + 0x04U * (x))
+
+/* DBSC registers */
+#include "../ddr_regs.h"
+
+#define DBSC_DBMONCONF4		0xE6793010U
+
+#define DBSC_PLL_LOCK(ch)	(0xE6794054U + 0x100U * (ch))
+#define DBSC_PLL_LOCK_0		0xE6794054U
+#define DBSC_PLL_LOCK_1		0xE6794154U
+#define DBSC_PLL_LOCK_2		0xE6794254U
+#define DBSC_PLL_LOCK_3		0xE6794354U
+
+/* STAT registers */
+#define MSTAT_SL_INIT		0xE67E8000U
+#define MSTAT_REF_ARS		0xE67E8004U
+#define MSTATQ_STATQC		0xE67E8008U
+#define MSTATQ_WTENABLE		0xE67E8030U
+#define MSTATQ_WTREFRESH	0xE67E8034U
+#define MSTATQ_WTSETTING0	0xE67E8038U
+#define MSTATQ_WTSETTING1	0xE67E803CU
+
+#define QOS_BASE1		(0xE67F0000U)
+#define QOSCTRL_RAS		(QOS_BASE1 + 0x0000U)
+#define QOSCTRL_FIXTH		(QOS_BASE1 + 0x0004U)
+#define QOSCTRL_RAEN		(QOS_BASE1 + 0x0018U)
+#define QOSCTRL_REGGD		(QOS_BASE1 + 0x0020U)
+#define QOSCTRL_DANN		(QOS_BASE1 + 0x0030U)
+#define QOSCTRL_DANT		(QOS_BASE1 + 0x0038U)
+#define QOSCTRL_EC		(QOS_BASE1 + 0x003CU)
+#define QOSCTRL_EMS		(QOS_BASE1 + 0x0040U)
+#define QOSCTRL_INSFC		(QOS_BASE1 + 0x0050U)
+#define QOSCTRL_BERR		(QOS_BASE1 + 0x0054U)
+#define QOSCTRL_RACNT0		(QOS_BASE1 + 0x0080U)
+#define QOSCTRL_STATGEN0	(QOS_BASE1 + 0x0088U)
+
+/* other module */
+#define THS1_THCTR		0xE6198020U
+#define THS1_TEMP		0xE6198028U
diff --git a/drivers/renesas/common/ddr/ddr_b/ddr_b.mk b/drivers/renesas/common/ddr/ddr_b/ddr_b.mk
new file mode 100644
index 0000000..0334780
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_b/ddr_b.mk
@@ -0,0 +1,7 @@
+#
+# Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+BL2_SOURCES += drivers/renesas/common/ddr/ddr_b/boot_init_dram.c
diff --git a/drivers/renesas/common/ddr/ddr_b/ddr_regdef.h b/drivers/renesas/common/ddr/ddr_b/ddr_regdef.h
new file mode 100644
index 0000000..adf8dab
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_b/ddr_regdef.h
@@ -0,0 +1,5887 @@
+/*
+ * Copyright (c) 2018-2019, Renesas Electronics Corporation.
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#define _reg_PHY_DQ_DM_SWIZZLE0                            0x00000000U
+#define _reg_PHY_DQ_DM_SWIZZLE1                            0x00000001U
+#define _reg_PHY_CLK_WR_BYPASS_SLAVE_DELAY                 0x00000002U
+#define _reg_PHY_RDDQS_GATE_BYPASS_SLAVE_DELAY             0x00000003U
+#define _reg_PHY_BYPASS_TWO_CYC_PREAMBLE                   0x00000004U
+#define _reg_PHY_CLK_BYPASS_OVERRIDE                       0x00000005U
+#define _reg_PHY_SW_WRDQ0_SHIFT                            0x00000006U
+#define _reg_PHY_SW_WRDQ1_SHIFT                            0x00000007U
+#define _reg_PHY_SW_WRDQ2_SHIFT                            0x00000008U
+#define _reg_PHY_SW_WRDQ3_SHIFT                            0x00000009U
+#define _reg_PHY_SW_WRDQ4_SHIFT                            0x0000000aU
+#define _reg_PHY_SW_WRDQ5_SHIFT                            0x0000000bU
+#define _reg_PHY_SW_WRDQ6_SHIFT                            0x0000000cU
+#define _reg_PHY_SW_WRDQ7_SHIFT                            0x0000000dU
+#define _reg_PHY_SW_WRDM_SHIFT                             0x0000000eU
+#define _reg_PHY_SW_WRDQS_SHIFT                            0x0000000fU
+#define _reg_PHY_DQ_TSEL_ENABLE                            0x00000010U
+#define _reg_PHY_DQ_TSEL_SELECT                            0x00000011U
+#define _reg_PHY_DQS_TSEL_ENABLE                           0x00000012U
+#define _reg_PHY_DQS_TSEL_SELECT                           0x00000013U
+#define _reg_PHY_TWO_CYC_PREAMBLE                          0x00000014U
+#define _reg_PHY_DBI_MODE                                  0x00000015U
+#define _reg_PHY_PER_RANK_CS_MAP                           0x00000016U
+#define _reg_PHY_PER_CS_TRAINING_MULTICAST_EN              0x00000017U
+#define _reg_PHY_PER_CS_TRAINING_INDEX                     0x00000018U
+#define _reg_PHY_LP4_BOOT_RDDATA_EN_IE_DLY                 0x00000019U
+#define _reg_PHY_LP4_BOOT_RDDATA_EN_DLY                    0x0000001aU
+#define _reg_PHY_LP4_BOOT_RDDATA_EN_TSEL_DLY               0x0000001bU
+#define _reg_PHY_LP4_BOOT_RPTR_UPDATE                      0x0000001cU
+#define _reg_PHY_LP4_BOOT_RDDQS_GATE_SLAVE_DELAY           0x0000001dU
+#define _reg_PHY_LP4_BOOT_RDDQS_LATENCY_ADJUST             0x0000001eU
+#define _reg_PHY_LP4_BOOT_WRPATH_GATE_DISABLE              0x0000001fU
+#define _reg_PHY_LP4_BOOT_RDDATA_EN_OE_DLY                 0x00000020U
+#define _reg_PHY_LPBK_CONTROL                              0x00000021U
+#define _reg_PHY_LPBK_DFX_TIMEOUT_EN                       0x00000022U
+#define _reg_PHY_AUTO_TIMING_MARGIN_CONTROL                0x00000023U
+#define _reg_PHY_AUTO_TIMING_MARGIN_OBS                    0x00000024U
+#define _reg_PHY_SLICE_PWR_RDC_DISABLE                     0x00000025U
+#define _reg_PHY_PRBS_PATTERN_START                        0x00000026U
+#define _reg_PHY_PRBS_PATTERN_MASK                         0x00000027U
+#define _reg_PHY_RDDQS_DQ_BYPASS_SLAVE_DELAY               0x00000028U
+#define _reg_PHY_GATE_ERROR_DELAY_SELECT                   0x00000029U
+#define _reg_SC_PHY_SNAP_OBS_REGS                          0x0000002aU
+#define _reg_PHY_LPDDR                                     0x0000002bU
+#define _reg_PHY_LPDDR_TYPE                                0x0000002cU
+#define _reg_PHY_GATE_SMPL1_SLAVE_DELAY                    0x0000002dU
+#define _reg_PHY_GATE_SMPL2_SLAVE_DELAY                    0x0000002eU
+#define _reg_ON_FLY_GATE_ADJUST_EN                         0x0000002fU
+#define _reg_PHY_GATE_TRACKING_OBS                         0x00000030U
+#define _reg_PHY_DFI40_POLARITY                            0x00000031U
+#define _reg_PHY_LP4_PST_AMBLE                             0x00000032U
+#define _reg_PHY_RDLVL_PATT8                               0x00000033U
+#define _reg_PHY_RDLVL_PATT9                               0x00000034U
+#define _reg_PHY_RDLVL_PATT10                              0x00000035U
+#define _reg_PHY_RDLVL_PATT11                              0x00000036U
+#define _reg_PHY_LP4_RDLVL_PATT8                           0x00000037U
+#define _reg_PHY_LP4_RDLVL_PATT9                           0x00000038U
+#define _reg_PHY_LP4_RDLVL_PATT10                          0x00000039U
+#define _reg_PHY_LP4_RDLVL_PATT11                          0x0000003aU
+#define _reg_PHY_SLAVE_LOOP_CNT_UPDATE                     0x0000003bU
+#define _reg_PHY_SW_FIFO_PTR_RST_DISABLE                   0x0000003cU
+#define _reg_PHY_MASTER_DLY_LOCK_OBS_SELECT                0x0000003dU
+#define _reg_PHY_RDDQ_ENC_OBS_SELECT                       0x0000003eU
+#define _reg_PHY_RDDQS_DQ_ENC_OBS_SELECT                   0x0000003fU
+#define _reg_PHY_WR_ENC_OBS_SELECT                         0x00000040U
+#define _reg_PHY_WR_SHIFT_OBS_SELECT                       0x00000041U
+#define _reg_PHY_FIFO_PTR_OBS_SELECT                       0x00000042U
+#define _reg_PHY_LVL_DEBUG_MODE                            0x00000043U
+#define _reg_SC_PHY_LVL_DEBUG_CONT                         0x00000044U
+#define _reg_PHY_WRLVL_CAPTURE_CNT                         0x00000045U
+#define _reg_PHY_WRLVL_UPDT_WAIT_CNT                       0x00000046U
+#define _reg_PHY_WRLVL_DQ_MASK                             0x00000047U
+#define _reg_PHY_GTLVL_CAPTURE_CNT                         0x00000048U
+#define _reg_PHY_GTLVL_UPDT_WAIT_CNT                       0x00000049U
+#define _reg_PHY_RDLVL_CAPTURE_CNT                         0x0000004aU
+#define _reg_PHY_RDLVL_UPDT_WAIT_CNT                       0x0000004bU
+#define _reg_PHY_RDLVL_OP_MODE                             0x0000004cU
+#define _reg_PHY_RDLVL_RDDQS_DQ_OBS_SELECT                 0x0000004dU
+#define _reg_PHY_RDLVL_DATA_MASK                           0x0000004eU
+#define _reg_PHY_RDLVL_DATA_SWIZZLE                        0x0000004fU
+#define _reg_PHY_WDQLVL_BURST_CNT                          0x00000050U
+#define _reg_PHY_WDQLVL_PATT                               0x00000051U
+#define _reg_PHY_WDQLVL_DQDM_SLV_DLY_JUMP_OFFSET           0x00000052U
+#define _reg_PHY_WDQLVL_UPDT_WAIT_CNT                      0x00000053U
+#define _reg_PHY_WDQLVL_DQDM_OBS_SELECT                    0x00000054U
+#define _reg_PHY_WDQLVL_QTR_DLY_STEP                       0x00000055U
+#define _reg_SC_PHY_WDQLVL_CLR_PREV_RESULTS                0x00000056U
+#define _reg_PHY_WDQLVL_CLR_PREV_RESULTS                   0x00000057U
+#define _reg_PHY_WDQLVL_DATADM_MASK                        0x00000058U
+#define _reg_PHY_USER_PATT0                                0x00000059U
+#define _reg_PHY_USER_PATT1                                0x0000005aU
+#define _reg_PHY_USER_PATT2                                0x0000005bU
+#define _reg_PHY_USER_PATT3                                0x0000005cU
+#define _reg_PHY_USER_PATT4                                0x0000005dU
+#define _reg_PHY_DQ_SWIZZLING                              0x0000005eU
+#define _reg_PHY_CALVL_VREF_DRIVING_SLICE                  0x0000005fU
+#define _reg_SC_PHY_MANUAL_CLEAR                           0x00000060U
+#define _reg_PHY_FIFO_PTR_OBS                              0x00000061U
+#define _reg_PHY_LPBK_RESULT_OBS                           0x00000062U
+#define _reg_PHY_LPBK_ERROR_COUNT_OBS                      0x00000063U
+#define _reg_PHY_MASTER_DLY_LOCK_OBS                       0x00000064U
+#define _reg_PHY_RDDQ_SLV_DLY_ENC_OBS                      0x00000065U
+#define _reg_PHY_RDDQS_BASE_SLV_DLY_ENC_OBS                0x00000066U
+#define _reg_PHY_RDDQS_DQ_RISE_ADDER_SLV_DLY_ENC_OBS       0x00000067U
+#define _reg_PHY_RDDQS_DQ_FALL_ADDER_SLV_DLY_ENC_OBS       0x00000068U
+#define _reg_PHY_RDDQS_GATE_SLV_DLY_ENC_OBS                0x00000069U
+#define _reg_PHY_WRDQS_BASE_SLV_DLY_ENC_OBS                0x0000006aU
+#define _reg_PHY_WRDQ_BASE_SLV_DLY_ENC_OBS                 0x0000006bU
+#define _reg_PHY_WR_ADDER_SLV_DLY_ENC_OBS                  0x0000006cU
+#define _reg_PHY_WR_SHIFT_OBS                              0x0000006dU
+#define _reg_PHY_WRLVL_HARD0_DELAY_OBS                     0x0000006eU
+#define _reg_PHY_WRLVL_HARD1_DELAY_OBS                     0x0000006fU
+#define _reg_PHY_WRLVL_STATUS_OBS                          0x00000070U
+#define _reg_PHY_GATE_SMPL1_SLV_DLY_ENC_OBS                0x00000071U
+#define _reg_PHY_GATE_SMPL2_SLV_DLY_ENC_OBS                0x00000072U
+#define _reg_PHY_WRLVL_ERROR_OBS                           0x00000073U
+#define _reg_PHY_GTLVL_HARD0_DELAY_OBS                     0x00000074U
+#define _reg_PHY_GTLVL_HARD1_DELAY_OBS                     0x00000075U
+#define _reg_PHY_GTLVL_STATUS_OBS                          0x00000076U
+#define _reg_PHY_RDLVL_RDDQS_DQ_LE_DLY_OBS                 0x00000077U
+#define _reg_PHY_RDLVL_RDDQS_DQ_TE_DLY_OBS                 0x00000078U
+#define _reg_PHY_RDLVL_RDDQS_DQ_NUM_WINDOWS_OBS            0x00000079U
+#define _reg_PHY_RDLVL_STATUS_OBS                          0x0000007aU
+#define _reg_PHY_WDQLVL_DQDM_LE_DLY_OBS                    0x0000007bU
+#define _reg_PHY_WDQLVL_DQDM_TE_DLY_OBS                    0x0000007cU
+#define _reg_PHY_WDQLVL_STATUS_OBS                         0x0000007dU
+#define _reg_PHY_DDL_MODE                                  0x0000007eU
+#define _reg_PHY_DDL_TEST_OBS                              0x0000007fU
+#define _reg_PHY_DDL_TEST_MSTR_DLY_OBS                     0x00000080U
+#define _reg_PHY_DDL_TRACK_UPD_THRESHOLD                   0x00000081U
+#define _reg_PHY_LP4_WDQS_OE_EXTEND                        0x00000082U
+#define _reg_SC_PHY_RX_CAL_START                           0x00000083U
+#define _reg_PHY_RX_CAL_OVERRIDE                           0x00000084U
+#define _reg_PHY_RX_CAL_SAMPLE_WAIT                        0x00000085U
+#define _reg_PHY_RX_CAL_DQ0                                0x00000086U
+#define _reg_PHY_RX_CAL_DQ1                                0x00000087U
+#define _reg_PHY_RX_CAL_DQ2                                0x00000088U
+#define _reg_PHY_RX_CAL_DQ3                                0x00000089U
+#define _reg_PHY_RX_CAL_DQ4                                0x0000008aU
+#define _reg_PHY_RX_CAL_DQ5                                0x0000008bU
+#define _reg_PHY_RX_CAL_DQ6                                0x0000008cU
+#define _reg_PHY_RX_CAL_DQ7                                0x0000008dU
+#define _reg_PHY_RX_CAL_DM                                 0x0000008eU
+#define _reg_PHY_RX_CAL_DQS                                0x0000008fU
+#define _reg_PHY_RX_CAL_FDBK                               0x00000090U
+#define _reg_PHY_RX_CAL_OBS                                0x00000091U
+#define _reg_PHY_RX_CAL_LOCK_OBS                           0x00000092U
+#define _reg_PHY_RX_CAL_DISABLE                            0x00000093U
+#define _reg_PHY_CLK_WRDQ0_SLAVE_DELAY                     0x00000094U
+#define _reg_PHY_CLK_WRDQ1_SLAVE_DELAY                     0x00000095U
+#define _reg_PHY_CLK_WRDQ2_SLAVE_DELAY                     0x00000096U
+#define _reg_PHY_CLK_WRDQ3_SLAVE_DELAY                     0x00000097U
+#define _reg_PHY_CLK_WRDQ4_SLAVE_DELAY                     0x00000098U
+#define _reg_PHY_CLK_WRDQ5_SLAVE_DELAY                     0x00000099U
+#define _reg_PHY_CLK_WRDQ6_SLAVE_DELAY                     0x0000009aU
+#define _reg_PHY_CLK_WRDQ7_SLAVE_DELAY                     0x0000009bU
+#define _reg_PHY_CLK_WRDM_SLAVE_DELAY                      0x0000009cU
+#define _reg_PHY_CLK_WRDQS_SLAVE_DELAY                     0x0000009dU
+#define _reg_PHY_WRLVL_THRESHOLD_ADJUST                    0x0000009eU
+#define _reg_PHY_RDDQ0_SLAVE_DELAY                         0x0000009fU
+#define _reg_PHY_RDDQ1_SLAVE_DELAY                         0x000000a0U
+#define _reg_PHY_RDDQ2_SLAVE_DELAY                         0x000000a1U
+#define _reg_PHY_RDDQ3_SLAVE_DELAY                         0x000000a2U
+#define _reg_PHY_RDDQ4_SLAVE_DELAY                         0x000000a3U
+#define _reg_PHY_RDDQ5_SLAVE_DELAY                         0x000000a4U
+#define _reg_PHY_RDDQ6_SLAVE_DELAY                         0x000000a5U
+#define _reg_PHY_RDDQ7_SLAVE_DELAY                         0x000000a6U
+#define _reg_PHY_RDDM_SLAVE_DELAY                          0x000000a7U
+#define _reg_PHY_RDDQS_DQ0_RISE_SLAVE_DELAY                0x000000a8U
+#define _reg_PHY_RDDQS_DQ0_FALL_SLAVE_DELAY                0x000000a9U
+#define _reg_PHY_RDDQS_DQ1_RISE_SLAVE_DELAY                0x000000aaU
+#define _reg_PHY_RDDQS_DQ1_FALL_SLAVE_DELAY                0x000000abU
+#define _reg_PHY_RDDQS_DQ2_RISE_SLAVE_DELAY                0x000000acU
+#define _reg_PHY_RDDQS_DQ2_FALL_SLAVE_DELAY                0x000000adU
+#define _reg_PHY_RDDQS_DQ3_RISE_SLAVE_DELAY                0x000000aeU
+#define _reg_PHY_RDDQS_DQ3_FALL_SLAVE_DELAY                0x000000afU
+#define _reg_PHY_RDDQS_DQ4_RISE_SLAVE_DELAY                0x000000b0U
+#define _reg_PHY_RDDQS_DQ4_FALL_SLAVE_DELAY                0x000000b1U
+#define _reg_PHY_RDDQS_DQ5_RISE_SLAVE_DELAY                0x000000b2U
+#define _reg_PHY_RDDQS_DQ5_FALL_SLAVE_DELAY                0x000000b3U
+#define _reg_PHY_RDDQS_DQ6_RISE_SLAVE_DELAY                0x000000b4U
+#define _reg_PHY_RDDQS_DQ6_FALL_SLAVE_DELAY                0x000000b5U
+#define _reg_PHY_RDDQS_DQ7_RISE_SLAVE_DELAY                0x000000b6U
+#define _reg_PHY_RDDQS_DQ7_FALL_SLAVE_DELAY                0x000000b7U
+#define _reg_PHY_RDDQS_DM_RISE_SLAVE_DELAY                 0x000000b8U
+#define _reg_PHY_RDDQS_DM_FALL_SLAVE_DELAY                 0x000000b9U
+#define _reg_PHY_RDDQS_GATE_SLAVE_DELAY                    0x000000baU
+#define _reg_PHY_RDDQS_LATENCY_ADJUST                      0x000000bbU
+#define _reg_PHY_WRITE_PATH_LAT_ADD                        0x000000bcU
+#define _reg_PHY_WRLVL_DELAY_EARLY_THRESHOLD               0x000000bdU
+#define _reg_PHY_WRLVL_DELAY_PERIOD_THRESHOLD              0x000000beU
+#define _reg_PHY_WRLVL_EARLY_FORCE_ZERO                    0x000000bfU
+#define _reg_PHY_GTLVL_RDDQS_SLV_DLY_START                 0x000000c0U
+#define _reg_PHY_GTLVL_LAT_ADJ_START                       0x000000c1U
+#define _reg_PHY_WDQLVL_DQDM_SLV_DLY_START                 0x000000c2U
+#define _reg_PHY_RDLVL_RDDQS_DQ_SLV_DLY_START              0x000000c3U
+#define _reg_PHY_FDBK_PWR_CTRL                             0x000000c4U
+#define _reg_PHY_DQ_OE_TIMING                              0x000000c5U
+#define _reg_PHY_DQ_TSEL_RD_TIMING                         0x000000c6U
+#define _reg_PHY_DQ_TSEL_WR_TIMING                         0x000000c7U
+#define _reg_PHY_DQS_OE_TIMING                             0x000000c8U
+#define _reg_PHY_DQS_TSEL_RD_TIMING                        0x000000c9U
+#define _reg_PHY_DQS_OE_RD_TIMING                          0x000000caU
+#define _reg_PHY_DQS_TSEL_WR_TIMING                        0x000000cbU
+#define _reg_PHY_PER_CS_TRAINING_EN                        0x000000ccU
+#define _reg_PHY_DQ_IE_TIMING                              0x000000cdU
+#define _reg_PHY_DQS_IE_TIMING                             0x000000ceU
+#define _reg_PHY_RDDATA_EN_IE_DLY                          0x000000cfU
+#define _reg_PHY_IE_MODE                                   0x000000d0U
+#define _reg_PHY_RDDATA_EN_DLY                             0x000000d1U
+#define _reg_PHY_RDDATA_EN_TSEL_DLY                        0x000000d2U
+#define _reg_PHY_RDDATA_EN_OE_DLY                          0x000000d3U
+#define _reg_PHY_SW_MASTER_MODE                            0x000000d4U
+#define _reg_PHY_MASTER_DELAY_START                        0x000000d5U
+#define _reg_PHY_MASTER_DELAY_STEP                         0x000000d6U
+#define _reg_PHY_MASTER_DELAY_WAIT                         0x000000d7U
+#define _reg_PHY_MASTER_DELAY_HALF_MEASURE                 0x000000d8U
+#define _reg_PHY_RPTR_UPDATE                               0x000000d9U
+#define _reg_PHY_WRLVL_DLY_STEP                            0x000000daU
+#define _reg_PHY_WRLVL_RESP_WAIT_CNT                       0x000000dbU
+#define _reg_PHY_GTLVL_DLY_STEP                            0x000000dcU
+#define _reg_PHY_GTLVL_RESP_WAIT_CNT                       0x000000ddU
+#define _reg_PHY_GTLVL_BACK_STEP                           0x000000deU
+#define _reg_PHY_GTLVL_FINAL_STEP                          0x000000dfU
+#define _reg_PHY_WDQLVL_DLY_STEP                           0x000000e0U
+#define _reg_PHY_TOGGLE_PRE_SUPPORT                        0x000000e1U
+#define _reg_PHY_RDLVL_DLY_STEP                            0x000000e2U
+#define _reg_PHY_WRPATH_GATE_DISABLE                       0x000000e3U
+#define _reg_PHY_WRPATH_GATE_TIMING                        0x000000e4U
+#define _reg_PHY_ADR0_SW_WRADDR_SHIFT                      0x000000e5U
+#define _reg_PHY_ADR1_SW_WRADDR_SHIFT                      0x000000e6U
+#define _reg_PHY_ADR2_SW_WRADDR_SHIFT                      0x000000e7U
+#define _reg_PHY_ADR3_SW_WRADDR_SHIFT                      0x000000e8U
+#define _reg_PHY_ADR4_SW_WRADDR_SHIFT                      0x000000e9U
+#define _reg_PHY_ADR5_SW_WRADDR_SHIFT                      0x000000eaU
+#define _reg_PHY_ADR_CLK_WR_BYPASS_SLAVE_DELAY             0x000000ebU
+#define _reg_PHY_ADR_CLK_BYPASS_OVERRIDE                   0x000000ecU
+#define _reg_SC_PHY_ADR_MANUAL_CLEAR                       0x000000edU
+#define _reg_PHY_ADR_LPBK_RESULT_OBS                       0x000000eeU
+#define _reg_PHY_ADR_LPBK_ERROR_COUNT_OBS                  0x000000efU
+#define _reg_PHY_ADR_MASTER_DLY_LOCK_OBS_SELECT            0x000000f0U
+#define _reg_PHY_ADR_MASTER_DLY_LOCK_OBS                   0x000000f1U
+#define _reg_PHY_ADR_BASE_SLV_DLY_ENC_OBS                  0x000000f2U
+#define _reg_PHY_ADR_ADDER_SLV_DLY_ENC_OBS                 0x000000f3U
+#define _reg_PHY_ADR_SLAVE_LOOP_CNT_UPDATE                 0x000000f4U
+#define _reg_PHY_ADR_SLV_DLY_ENC_OBS_SELECT                0x000000f5U
+#define _reg_SC_PHY_ADR_SNAP_OBS_REGS                      0x000000f6U
+#define _reg_PHY_ADR_TSEL_ENABLE                           0x000000f7U
+#define _reg_PHY_ADR_LPBK_CONTROL                          0x000000f8U
+#define _reg_PHY_ADR_PRBS_PATTERN_START                    0x000000f9U
+#define _reg_PHY_ADR_PRBS_PATTERN_MASK                     0x000000faU
+#define _reg_PHY_ADR_PWR_RDC_DISABLE                       0x000000fbU
+#define _reg_PHY_ADR_TYPE                                  0x000000fcU
+#define _reg_PHY_ADR_WRADDR_SHIFT_OBS                      0x000000fdU
+#define _reg_PHY_ADR_IE_MODE                               0x000000feU
+#define _reg_PHY_ADR_DDL_MODE                              0x000000ffU
+#define _reg_PHY_ADR_DDL_TEST_OBS                          0x00000100U
+#define _reg_PHY_ADR_DDL_TEST_MSTR_DLY_OBS                 0x00000101U
+#define _reg_PHY_ADR_CALVL_START                           0x00000102U
+#define _reg_PHY_ADR_CALVL_COARSE_DLY                      0x00000103U
+#define _reg_PHY_ADR_CALVL_QTR                             0x00000104U
+#define _reg_PHY_ADR_CALVL_SWIZZLE0                        0x00000105U
+#define _reg_PHY_ADR_CALVL_SWIZZLE1                        0x00000106U
+#define _reg_PHY_ADR_CALVL_SWIZZLE0_0                      0x00000107U
+#define _reg_PHY_ADR_CALVL_SWIZZLE1_0                      0x00000108U
+#define _reg_PHY_ADR_CALVL_SWIZZLE0_1                      0x00000109U
+#define _reg_PHY_ADR_CALVL_SWIZZLE1_1                      0x0000010aU
+#define _reg_PHY_ADR_CALVL_DEVICE_MAP                      0x0000010bU
+#define _reg_PHY_ADR_CALVL_RANK_CTRL                       0x0000010cU
+#define _reg_PHY_ADR_CALVL_NUM_PATTERNS                    0x0000010dU
+#define _reg_PHY_ADR_CALVL_CAPTURE_CNT                     0x0000010eU
+#define _reg_PHY_ADR_CALVL_RESP_WAIT_CNT                   0x0000010fU
+#define _reg_PHY_ADR_CALVL_DEBUG_MODE                      0x00000110U
+#define _reg_SC_PHY_ADR_CALVL_DEBUG_CONT                   0x00000111U
+#define _reg_SC_PHY_ADR_CALVL_ERROR_CLR                    0x00000112U
+#define _reg_PHY_ADR_CALVL_OBS_SELECT                      0x00000113U
+#define _reg_PHY_ADR_CALVL_OBS0                            0x00000114U
+#define _reg_PHY_ADR_CALVL_OBS1                            0x00000115U
+#define _reg_PHY_ADR_CALVL_RESULT                          0x00000116U
+#define _reg_PHY_ADR_CALVL_FG_0                            0x00000117U
+#define _reg_PHY_ADR_CALVL_BG_0                            0x00000118U
+#define _reg_PHY_ADR_CALVL_FG_1                            0x00000119U
+#define _reg_PHY_ADR_CALVL_BG_1                            0x0000011aU
+#define _reg_PHY_ADR_CALVL_FG_2                            0x0000011bU
+#define _reg_PHY_ADR_CALVL_BG_2                            0x0000011cU
+#define _reg_PHY_ADR_CALVL_FG_3                            0x0000011dU
+#define _reg_PHY_ADR_CALVL_BG_3                            0x0000011eU
+#define _reg_PHY_ADR_ADDR_SEL                              0x0000011fU
+#define _reg_PHY_ADR_LP4_BOOT_SLV_DELAY                    0x00000120U
+#define _reg_PHY_ADR_BIT_MASK                              0x00000121U
+#define _reg_PHY_ADR_SEG_MASK                              0x00000122U
+#define _reg_PHY_ADR_CALVL_TRAIN_MASK                      0x00000123U
+#define _reg_PHY_ADR_CSLVL_TRAIN_MASK                      0x00000124U
+#define _reg_PHY_ADR_SW_TXIO_CTRL                          0x00000125U
+#define _reg_PHY_ADR_TSEL_SELECT                           0x00000126U
+#define _reg_PHY_ADR0_CLK_WR_SLAVE_DELAY                   0x00000127U
+#define _reg_PHY_ADR1_CLK_WR_SLAVE_DELAY                   0x00000128U
+#define _reg_PHY_ADR2_CLK_WR_SLAVE_DELAY                   0x00000129U
+#define _reg_PHY_ADR3_CLK_WR_SLAVE_DELAY                   0x0000012aU
+#define _reg_PHY_ADR4_CLK_WR_SLAVE_DELAY                   0x0000012bU
+#define _reg_PHY_ADR5_CLK_WR_SLAVE_DELAY                   0x0000012cU
+#define _reg_PHY_ADR_SW_MASTER_MODE                        0x0000012dU
+#define _reg_PHY_ADR_MASTER_DELAY_START                    0x0000012eU
+#define _reg_PHY_ADR_MASTER_DELAY_STEP                     0x0000012fU
+#define _reg_PHY_ADR_MASTER_DELAY_WAIT                     0x00000130U
+#define _reg_PHY_ADR_MASTER_DELAY_HALF_MEASURE             0x00000131U
+#define _reg_PHY_ADR_CALVL_DLY_STEP                        0x00000132U
+#define _reg_PHY_FREQ_SEL                                  0x00000133U
+#define _reg_PHY_FREQ_SEL_FROM_REGIF                       0x00000134U
+#define _reg_PHY_FREQ_SEL_MULTICAST_EN                     0x00000135U
+#define _reg_PHY_FREQ_SEL_INDEX                            0x00000136U
+#define _reg_PHY_SW_GRP_SHIFT_0                            0x00000137U
+#define _reg_PHY_SW_GRP_SHIFT_1                            0x00000138U
+#define _reg_PHY_SW_GRP_SHIFT_2                            0x00000139U
+#define _reg_PHY_SW_GRP_SHIFT_3                            0x0000013aU
+#define _reg_PHY_GRP_BYPASS_SLAVE_DELAY                    0x0000013bU
+#define _reg_PHY_SW_GRP_BYPASS_SHIFT                       0x0000013cU
+#define _reg_PHY_GRP_BYPASS_OVERRIDE                       0x0000013dU
+#define _reg_SC_PHY_MANUAL_UPDATE                          0x0000013eU
+#define _reg_SC_PHY_MANUAL_UPDATE_PHYUPD_ENABLE            0x0000013fU
+#define _reg_PHY_LP4_BOOT_DISABLE                          0x00000140U
+#define _reg_PHY_CSLVL_ENABLE                              0x00000141U
+#define _reg_PHY_CSLVL_CS_MAP                              0x00000142U
+#define _reg_PHY_CSLVL_START                               0x00000143U
+#define _reg_PHY_CSLVL_QTR                                 0x00000144U
+#define _reg_PHY_CSLVL_COARSE_CHK                          0x00000145U
+#define _reg_PHY_CSLVL_CAPTURE_CNT                         0x00000146U
+#define _reg_PHY_CSLVL_COARSE_DLY                          0x00000147U
+#define _reg_PHY_CSLVL_COARSE_CAPTURE_CNT                  0x00000148U
+#define _reg_PHY_CSLVL_DEBUG_MODE                          0x00000149U
+#define _reg_SC_PHY_CSLVL_DEBUG_CONT                       0x0000014aU
+#define _reg_SC_PHY_CSLVL_ERROR_CLR                        0x0000014bU
+#define _reg_PHY_CSLVL_OBS0                                0x0000014cU
+#define _reg_PHY_CSLVL_OBS1                                0x0000014dU
+#define _reg_PHY_CALVL_CS_MAP                              0x0000014eU
+#define _reg_PHY_GRP_SLV_DLY_ENC_OBS_SELECT                0x0000014fU
+#define _reg_PHY_GRP_SHIFT_OBS_SELECT                      0x00000150U
+#define _reg_PHY_GRP_SLV_DLY_ENC_OBS                       0x00000151U
+#define _reg_PHY_GRP_SHIFT_OBS                             0x00000152U
+#define _reg_PHY_ADRCTL_SLAVE_LOOP_CNT_UPDATE              0x00000153U
+#define _reg_PHY_ADRCTL_SNAP_OBS_REGS                      0x00000154U
+#define _reg_PHY_DFI_PHYUPD_TYPE                           0x00000155U
+#define _reg_PHY_ADRCTL_LPDDR                              0x00000156U
+#define _reg_PHY_LP4_ACTIVE                                0x00000157U
+#define _reg_PHY_LPDDR3_CS                                 0x00000158U
+#define _reg_PHY_CALVL_RESULT_MASK                         0x00000159U
+#define _reg_SC_PHY_UPDATE_CLK_CAL_VALUES                  0x0000015aU
+#define _reg_PHY_SW_TXIO_CTRL_0                            0x0000015bU
+#define _reg_PHY_SW_TXIO_CTRL_1                            0x0000015cU
+#define _reg_PHY_SW_TXIO_CTRL_2                            0x0000015dU
+#define _reg_PHY_SW_TXIO_CTRL_3                            0x0000015eU
+#define _reg_PHY_MEMCLK_SW_TXIO_CTRL                       0x0000015fU
+#define _reg_PHY_CA_SW_TXPWR_CTRL                          0x00000160U
+#define _reg_PHY_MEMCLK_SW_TXPWR_CTRL                      0x00000161U
+#define _reg_PHY_USER_DEF_REG_AC_0                         0x00000162U
+#define _reg_PHY_USER_DEF_REG_AC_1                         0x00000163U
+#define _reg_PHY_USER_DEF_REG_AC_2                         0x00000164U
+#define _reg_PHY_USER_DEF_REG_AC_3                         0x00000165U
+#define _reg_PHY_UPDATE_CLK_CAL_VALUES                     0x00000166U
+#define _reg_PHY_CONTINUOUS_CLK_CAL_UPDATE                 0x00000167U
+#define _reg_PHY_PLL_CTRL                                  0x00000168U
+#define _reg_PHY_PLL_CTRL_TOP                              0x00000169U
+#define _reg_PHY_PLL_CTRL_CA                               0x0000016aU
+#define _reg_PHY_PLL_BYPASS                                0x0000016bU
+#define _reg_PHY_LOW_FREQ_SEL                              0x0000016cU
+#define _reg_PHY_PAD_VREF_CTRL_DQ_0                        0x0000016dU
+#define _reg_PHY_PAD_VREF_CTRL_DQ_1                        0x0000016eU
+#define _reg_PHY_PAD_VREF_CTRL_DQ_2                        0x0000016fU
+#define _reg_PHY_PAD_VREF_CTRL_DQ_3                        0x00000170U
+#define _reg_PHY_PAD_VREF_CTRL_AC                          0x00000171U
+#define _reg_PHY_CSLVL_DLY_STEP                            0x00000172U
+#define _reg_PHY_SET_DFI_INPUT_0                           0x00000173U
+#define _reg_PHY_SET_DFI_INPUT_1                           0x00000174U
+#define _reg_PHY_SET_DFI_INPUT_2                           0x00000175U
+#define _reg_PHY_SET_DFI_INPUT_3                           0x00000176U
+#define _reg_PHY_GRP_SLAVE_DELAY_0                         0x00000177U
+#define _reg_PHY_GRP_SLAVE_DELAY_1                         0x00000178U
+#define _reg_PHY_GRP_SLAVE_DELAY_2                         0x00000179U
+#define _reg_PHY_GRP_SLAVE_DELAY_3                         0x0000017aU
+#define _reg_PHY_CS_ACS_ALLOCATION_0                       0x0000017bU
+#define _reg_PHY_CS_ACS_ALLOCATION_1                       0x0000017cU
+#define _reg_PHY_CS_ACS_ALLOCATION_2                       0x0000017dU
+#define _reg_PHY_CS_ACS_ALLOCATION_3                       0x0000017eU
+#define _reg_PHY_LP4_BOOT_PLL_CTRL                         0x0000017fU
+#define _reg_PHY_LP4_BOOT_PLL_CTRL_CA                      0x00000180U
+#define _reg_PHY_LP4_BOOT_TOP_PLL_CTRL                     0x00000181U
+#define _reg_PHY_PLL_CTRL_OVERRIDE                         0x00000182U
+#define _reg_PHY_PLL_WAIT                                  0x00000183U
+#define _reg_PHY_PLL_WAIT_TOP                              0x00000184U
+#define _reg_PHY_PLL_OBS_0                                 0x00000185U
+#define _reg_PHY_PLL_OBS_1                                 0x00000186U
+#define _reg_PHY_PLL_OBS_2                                 0x00000187U
+#define _reg_PHY_PLL_OBS_3                                 0x00000188U
+#define _reg_PHY_PLL_OBS_4                                 0x00000189U
+#define _reg_PHY_PLL_TESTOUT_SEL                           0x0000018aU
+#define _reg_PHY_TCKSRE_WAIT                               0x0000018bU
+#define _reg_PHY_LP4_BOOT_LOW_FREQ_SEL                     0x0000018cU
+#define _reg_PHY_LP_WAKEUP                                 0x0000018dU
+#define _reg_PHY_LS_IDLE_EN                                0x0000018eU
+#define _reg_PHY_LP_CTRLUPD_CNTR_CFG                       0x0000018fU
+#define _reg_PHY_TDFI_PHY_WRDELAY                          0x00000190U
+#define _reg_PHY_PAD_FDBK_DRIVE                            0x00000191U
+#define _reg_PHY_PAD_DATA_DRIVE                            0x00000192U
+#define _reg_PHY_PAD_DQS_DRIVE                             0x00000193U
+#define _reg_PHY_PAD_ADDR_DRIVE                            0x00000194U
+#define _reg_PHY_PAD_CLK_DRIVE                             0x00000195U
+#define _reg_PHY_PAD_FDBK_TERM                             0x00000196U
+#define _reg_PHY_PAD_DATA_TERM                             0x00000197U
+#define _reg_PHY_PAD_DQS_TERM                              0x00000198U
+#define _reg_PHY_PAD_ADDR_TERM                             0x00000199U
+#define _reg_PHY_PAD_CLK_TERM                              0x0000019aU
+#define _reg_PHY_PAD_CKE_DRIVE                             0x0000019bU
+#define _reg_PHY_PAD_CKE_TERM                              0x0000019cU
+#define _reg_PHY_PAD_RST_DRIVE                             0x0000019dU
+#define _reg_PHY_PAD_RST_TERM                              0x0000019eU
+#define _reg_PHY_PAD_CS_DRIVE                              0x0000019fU
+#define _reg_PHY_PAD_CS_TERM                               0x000001a0U
+#define _reg_PHY_PAD_ODT_DRIVE                             0x000001a1U
+#define _reg_PHY_PAD_ODT_TERM                              0x000001a2U
+#define _reg_PHY_ADRCTL_RX_CAL                             0x000001a3U
+#define _reg_PHY_ADRCTL_LP3_RX_CAL                         0x000001a4U
+#define _reg_PHY_TST_CLK_PAD_CTRL                          0x000001a5U
+#define _reg_PHY_TST_CLK_PAD_CTRL2                         0x000001a6U
+#define _reg_PHY_CAL_MODE_0                                0x000001a7U
+#define _reg_PHY_CAL_CLEAR_0                               0x000001a8U
+#define _reg_PHY_CAL_START_0                               0x000001a9U
+#define _reg_PHY_CAL_INTERVAL_COUNT_0                      0x000001aaU
+#define _reg_PHY_CAL_SAMPLE_WAIT_0                         0x000001abU
+#define _reg_PHY_LP4_BOOT_CAL_CLK_SELECT_0                 0x000001acU
+#define _reg_PHY_CAL_CLK_SELECT_0                          0x000001adU
+#define _reg_PHY_CAL_RESULT_OBS_0                          0x000001aeU
+#define _reg_PHY_CAL_RESULT2_OBS_0                         0x000001afU
+#define _reg_PHY_CAL_CPTR_CNT_0                            0x000001b0U
+#define _reg_PHY_CAL_SETTLING_PRD_0                        0x000001b1U
+#define _reg_PHY_CAL_PU_FINE_ADJ_0                         0x000001b2U
+#define _reg_PHY_CAL_PD_FINE_ADJ_0                         0x000001b3U
+#define _reg_PHY_CAL_RCV_FINE_ADJ_0                        0x000001b4U
+#define _reg_PHY_CAL_DBG_CFG_0                             0x000001b5U
+#define _reg_SC_PHY_PAD_DBG_CONT_0                         0x000001b6U
+#define _reg_PHY_CAL_RESULT3_OBS_0                         0x000001b7U
+#define _reg_PHY_ADRCTL_PVT_MAP_0                          0x000001b8U
+#define _reg_PHY_CAL_SLOPE_ADJ_0                           0x000001b9U
+#define _reg_PHY_CAL_SLOPE_ADJ_PASS2_0                     0x000001baU
+#define _reg_PHY_CAL_TWO_PASS_CFG_0                        0x000001bbU
+#define _reg_PHY_CAL_SW_CAL_CFG_0                          0x000001bcU
+#define _reg_PHY_CAL_RANGE_MIN_0                           0x000001bdU
+#define _reg_PHY_CAL_RANGE_MAX_0                           0x000001beU
+#define _reg_PHY_PAD_ATB_CTRL                              0x000001bfU
+#define _reg_PHY_ADRCTL_MANUAL_UPDATE                      0x000001c0U
+#define _reg_PHY_AC_LPBK_ERR_CLEAR                         0x000001c1U
+#define _reg_PHY_AC_LPBK_OBS_SELECT                        0x000001c2U
+#define _reg_PHY_AC_LPBK_ENABLE                            0x000001c3U
+#define _reg_PHY_AC_LPBK_CONTROL                           0x000001c4U
+#define _reg_PHY_AC_PRBS_PATTERN_START                     0x000001c5U
+#define _reg_PHY_AC_PRBS_PATTERN_MASK                      0x000001c6U
+#define _reg_PHY_AC_LPBK_RESULT_OBS                        0x000001c7U
+#define _reg_PHY_AC_CLK_LPBK_OBS_SELECT                    0x000001c8U
+#define _reg_PHY_AC_CLK_LPBK_ENABLE                        0x000001c9U
+#define _reg_PHY_AC_CLK_LPBK_CONTROL                       0x000001caU
+#define _reg_PHY_AC_CLK_LPBK_RESULT_OBS                    0x000001cbU
+#define _reg_PHY_AC_PWR_RDC_DISABLE                        0x000001ccU
+#define _reg_PHY_DATA_BYTE_ORDER_SEL                       0x000001cdU
+#define _reg_PHY_DATA_BYTE_ORDER_SEL_HIGH                  0x000001ceU
+#define _reg_PHY_LPDDR4_CONNECT                            0x000001cfU
+#define _reg_PHY_CALVL_DEVICE_MAP                          0x000001d0U
+#define _reg_PHY_ADR_DISABLE                               0x000001d1U
+#define _reg_PHY_ADRCTL_MSTR_DLY_ENC_SEL                   0x000001d2U
+#define _reg_PHY_CS_DLY_UPT_PER_AC_SLICE                   0x000001d3U
+#define _reg_PHY_DDL_AC_ENABLE                             0x000001d4U
+#define _reg_PHY_DDL_AC_MODE                               0x000001d5U
+#define _reg_PHY_PAD_BACKGROUND_CAL                        0x000001d6U
+#define _reg_PHY_INIT_UPDATE_CONFIG                        0x000001d7U
+#define _reg_PHY_DDL_TRACK_UPD_THRESHOLD_AC                0x000001d8U
+#define _reg_PHY_DLL_RST_EN                                0x000001d9U
+#define _reg_PHY_AC_INIT_COMPLETE_OBS                      0x000001daU
+#define _reg_PHY_DS_INIT_COMPLETE_OBS                      0x000001dbU
+#define _reg_PHY_UPDATE_MASK                               0x000001dcU
+#define _reg_PHY_PLL_SWITCH_CNT                            0x000001ddU
+#define _reg_PI_START                                      0x000001deU
+#define _reg_PI_DRAM_CLASS                                 0x000001dfU
+#define _reg_PI_VERSION                                    0x000001e0U
+#define _reg_PI_NORMAL_LVL_SEQ                             0x000001e1U
+#define _reg_PI_INIT_LVL_EN                                0x000001e2U
+#define _reg_PI_NOTCARE_PHYUPD                             0x000001e3U
+#define _reg_PI_ONBUS_MBIST                                0x000001e4U
+#define _reg_PI_TCMD_GAP                                   0x000001e5U
+#define _reg_PI_MASTER_ACK_DURATION_MIN                    0x000001e6U
+#define _reg_PI_DFI_VERSION                                0x000001e7U
+#define _reg_PI_TDFI_PHYMSTR_TYPE0                         0x000001e8U
+#define _reg_PI_TDFI_PHYMSTR_TYPE1                         0x000001e9U
+#define _reg_PI_TDFI_PHYMSTR_TYPE2                         0x000001eaU
+#define _reg_PI_TDFI_PHYMSTR_TYPE3                         0x000001ebU
+#define _reg_PI_DFI_PHYMSTR_TYPE                           0x000001ecU
+#define _reg_PI_DFI_PHYMSTR_CS_STATE_R                     0x000001edU
+#define _reg_PI_DFI_PHYMSTR_STATE_SEL_R                    0x000001eeU
+#define _reg_PI_TDFI_PHYMSTR_MAX_F0                        0x000001efU
+#define _reg_PI_TDFI_PHYMSTR_RESP_F0                       0x000001f0U
+#define _reg_PI_TDFI_PHYMSTR_MAX_F1                        0x000001f1U
+#define _reg_PI_TDFI_PHYMSTR_RESP_F1                       0x000001f2U
+#define _reg_PI_TDFI_PHYMSTR_MAX_F2                        0x000001f3U
+#define _reg_PI_TDFI_PHYMSTR_RESP_F2                       0x000001f4U
+#define _reg_PI_TDFI_PHYUPD_RESP_F0                        0x000001f5U
+#define _reg_PI_TDFI_PHYUPD_TYPE0_F0                       0x000001f6U
+#define _reg_PI_TDFI_PHYUPD_TYPE1_F0                       0x000001f7U
+#define _reg_PI_TDFI_PHYUPD_TYPE2_F0                       0x000001f8U
+#define _reg_PI_TDFI_PHYUPD_TYPE3_F0                       0x000001f9U
+#define _reg_PI_TDFI_PHYUPD_RESP_F1                        0x000001faU
+#define _reg_PI_TDFI_PHYUPD_TYPE0_F1                       0x000001fbU
+#define _reg_PI_TDFI_PHYUPD_TYPE1_F1                       0x000001fcU
+#define _reg_PI_TDFI_PHYUPD_TYPE2_F1                       0x000001fdU
+#define _reg_PI_TDFI_PHYUPD_TYPE3_F1                       0x000001feU
+#define _reg_PI_TDFI_PHYUPD_RESP_F2                        0x000001ffU
+#define _reg_PI_TDFI_PHYUPD_TYPE0_F2                       0x00000200U
+#define _reg_PI_TDFI_PHYUPD_TYPE1_F2                       0x00000201U
+#define _reg_PI_TDFI_PHYUPD_TYPE2_F2                       0x00000202U
+#define _reg_PI_TDFI_PHYUPD_TYPE3_F2                       0x00000203U
+#define _reg_PI_CONTROL_ERROR_STATUS                       0x00000204U
+#define _reg_PI_EXIT_AFTER_INIT_CALVL                      0x00000205U
+#define _reg_PI_FREQ_MAP                                   0x00000206U
+#define _reg_PI_INIT_WORK_FREQ                             0x00000207U
+#define _reg_PI_INIT_DFS_CALVL_ONLY                        0x00000208U
+#define _reg_PI_POWER_ON_SEQ_BYPASS_ARRAY                  0x00000209U
+#define _reg_PI_POWER_ON_SEQ_END_ARRAY                     0x0000020aU
+#define _reg_PI_SEQ1_PAT                                   0x0000020bU
+#define _reg_PI_SEQ1_PAT_MASK                              0x0000020cU
+#define _reg_PI_SEQ2_PAT                                   0x0000020dU
+#define _reg_PI_SEQ2_PAT_MASK                              0x0000020eU
+#define _reg_PI_SEQ3_PAT                                   0x0000020fU
+#define _reg_PI_SEQ3_PAT_MASK                              0x00000210U
+#define _reg_PI_SEQ4_PAT                                   0x00000211U
+#define _reg_PI_SEQ4_PAT_MASK                              0x00000212U
+#define _reg_PI_SEQ5_PAT                                   0x00000213U
+#define _reg_PI_SEQ5_PAT_MASK                              0x00000214U
+#define _reg_PI_SEQ6_PAT                                   0x00000215U
+#define _reg_PI_SEQ6_PAT_MASK                              0x00000216U
+#define _reg_PI_SEQ7_PAT                                   0x00000217U
+#define _reg_PI_SEQ7_PAT_MASK                              0x00000218U
+#define _reg_PI_SEQ8_PAT                                   0x00000219U
+#define _reg_PI_SEQ8_PAT_MASK                              0x0000021aU
+#define _reg_PI_WDT_DISABLE                                0x0000021bU
+#define _reg_PI_SW_RST_N                                   0x0000021cU
+#define _reg_RESERVED_R0                                   0x0000021dU
+#define _reg_PI_CS_MAP                                     0x0000021eU
+#define _reg_PI_TDELAY_RDWR_2_BUS_IDLE_F0                  0x0000021fU
+#define _reg_PI_TDELAY_RDWR_2_BUS_IDLE_F1                  0x00000220U
+#define _reg_PI_TDELAY_RDWR_2_BUS_IDLE_F2                  0x00000221U
+#define _reg_PI_TMRR                                       0x00000222U
+#define _reg_PI_WRLAT_F0                                   0x00000223U
+#define _reg_PI_ADDITIVE_LAT_F0                            0x00000224U
+#define _reg_PI_CASLAT_LIN_F0                              0x00000225U
+#define _reg_PI_WRLAT_F1                                   0x00000226U
+#define _reg_PI_ADDITIVE_LAT_F1                            0x00000227U
+#define _reg_PI_CASLAT_LIN_F1                              0x00000228U
+#define _reg_PI_WRLAT_F2                                   0x00000229U
+#define _reg_PI_ADDITIVE_LAT_F2                            0x0000022aU
+#define _reg_PI_CASLAT_LIN_F2                              0x0000022bU
+#define _reg_PI_PREAMBLE_SUPPORT                           0x0000022cU
+#define _reg_PI_AREFRESH                                   0x0000022dU
+#define _reg_PI_MCAREF_FORWARD_ONLY                        0x0000022eU
+#define _reg_PI_TRFC_F0                                    0x0000022fU
+#define _reg_PI_TREF_F0                                    0x00000230U
+#define _reg_PI_TRFC_F1                                    0x00000231U
+#define _reg_PI_TREF_F1                                    0x00000232U
+#define _reg_PI_TRFC_F2                                    0x00000233U
+#define _reg_PI_TREF_F2                                    0x00000234U
+#define _reg_RESERVED_H3VER2                               0x00000235U
+#define _reg_PI_TREF_INTERVAL                              0x00000236U
+#define _reg_PI_FREQ_CHANGE_REG_COPY                       0x00000237U
+#define _reg_PI_FREQ_SEL_FROM_REGIF                        0x00000238U
+#define _reg_PI_SWLVL_LOAD                                 0x00000239U
+#define _reg_PI_SWLVL_OP_DONE                              0x0000023aU
+#define _reg_PI_SW_WRLVL_RESP_0                            0x0000023bU
+#define _reg_PI_SW_WRLVL_RESP_1                            0x0000023cU
+#define _reg_PI_SW_WRLVL_RESP_2                            0x0000023dU
+#define _reg_PI_SW_WRLVL_RESP_3                            0x0000023eU
+#define _reg_PI_SW_RDLVL_RESP_0                            0x0000023fU
+#define _reg_PI_SW_RDLVL_RESP_1                            0x00000240U
+#define _reg_PI_SW_RDLVL_RESP_2                            0x00000241U
+#define _reg_PI_SW_RDLVL_RESP_3                            0x00000242U
+#define _reg_PI_SW_CALVL_RESP_0                            0x00000243U
+#define _reg_PI_SW_LEVELING_MODE                           0x00000244U
+#define _reg_PI_SWLVL_START                                0x00000245U
+#define _reg_PI_SWLVL_EXIT                                 0x00000246U
+#define _reg_PI_SWLVL_WR_SLICE_0                           0x00000247U
+#define _reg_PI_SWLVL_RD_SLICE_0                           0x00000248U
+#define _reg_PI_SWLVL_VREF_UPDATE_SLICE_0                  0x00000249U
+#define _reg_PI_SW_WDQLVL_RESP_0                           0x0000024aU
+#define _reg_PI_SWLVL_WR_SLICE_1                           0x0000024bU
+#define _reg_PI_SWLVL_RD_SLICE_1                           0x0000024cU
+#define _reg_PI_SWLVL_VREF_UPDATE_SLICE_1                  0x0000024dU
+#define _reg_PI_SW_WDQLVL_RESP_1                           0x0000024eU
+#define _reg_PI_SWLVL_WR_SLICE_2                           0x0000024fU
+#define _reg_PI_SWLVL_RD_SLICE_2                           0x00000250U
+#define _reg_PI_SWLVL_VREF_UPDATE_SLICE_2                  0x00000251U
+#define _reg_PI_SW_WDQLVL_RESP_2                           0x00000252U
+#define _reg_PI_SWLVL_WR_SLICE_3                           0x00000253U
+#define _reg_PI_SWLVL_RD_SLICE_3                           0x00000254U
+#define _reg_PI_SWLVL_VREF_UPDATE_SLICE_3                  0x00000255U
+#define _reg_PI_SW_WDQLVL_RESP_3                           0x00000256U
+#define _reg_PI_SW_WDQLVL_VREF                             0x00000257U
+#define _reg_PI_SWLVL_SM2_START                            0x00000258U
+#define _reg_PI_SWLVL_SM2_WR                               0x00000259U
+#define _reg_PI_SWLVL_SM2_RD                               0x0000025aU
+#define _reg_PI_SEQUENTIAL_LVL_REQ                         0x0000025bU
+#define _reg_PI_DFS_PERIOD_EN                              0x0000025cU
+#define _reg_PI_SRE_PERIOD_EN                              0x0000025dU
+#define _reg_PI_DFI40_POLARITY                             0x0000025eU
+#define _reg_PI_16BIT_DRAM_CONNECT                         0x0000025fU
+#define _reg_PI_TDFI_CTRL_DELAY_F0                         0x00000260U
+#define _reg_PI_TDFI_CTRL_DELAY_F1                         0x00000261U
+#define _reg_PI_TDFI_CTRL_DELAY_F2                         0x00000262U
+#define _reg_PI_WRLVL_REQ                                  0x00000263U
+#define _reg_PI_WRLVL_CS                                   0x00000264U
+#define _reg_PI_WLDQSEN                                    0x00000265U
+#define _reg_PI_WLMRD                                      0x00000266U
+#define _reg_PI_WRLVL_EN_F0                                0x00000267U
+#define _reg_PI_WRLVL_EN_F1                                0x00000268U
+#define _reg_PI_WRLVL_EN_F2                                0x00000269U
+#define _reg_PI_WRLVL_EN                                   0x0000026aU
+#define _reg_PI_WRLVL_INTERVAL                             0x0000026bU
+#define _reg_PI_WRLVL_PERIODIC                             0x0000026cU
+#define _reg_PI_WRLVL_ON_SREF_EXIT                         0x0000026dU
+#define _reg_PI_WRLVL_DISABLE_DFS                          0x0000026eU
+#define _reg_PI_WRLVL_RESP_MASK                            0x0000026fU
+#define _reg_PI_WRLVL_ROTATE                               0x00000270U
+#define _reg_PI_WRLVL_CS_MAP                               0x00000271U
+#define _reg_PI_WRLVL_ERROR_STATUS                         0x00000272U
+#define _reg_PI_TDFI_WRLVL_EN                              0x00000273U
+#define _reg_PI_TDFI_WRLVL_WW_F0                           0x00000274U
+#define _reg_PI_TDFI_WRLVL_WW_F1                           0x00000275U
+#define _reg_PI_TDFI_WRLVL_WW_F2                           0x00000276U
+#define _reg_PI_TDFI_WRLVL_WW                              0x00000277U
+#define _reg_PI_TDFI_WRLVL_RESP                            0x00000278U
+#define _reg_PI_TDFI_WRLVL_MAX                             0x00000279U
+#define _reg_PI_WRLVL_STROBE_NUM                           0x0000027aU
+#define _reg_PI_WRLVL_MRR_DQ_RETURN_HIZ                    0x0000027bU
+#define _reg_PI_WRLVL_EN_DEASSERT_2_MRR                    0x0000027cU
+#define _reg_PI_TODTL_2CMD_F0                              0x0000027dU
+#define _reg_PI_ODT_EN_F0                                  0x0000027eU
+#define _reg_PI_TODTL_2CMD_F1                              0x0000027fU
+#define _reg_PI_ODT_EN_F1                                  0x00000280U
+#define _reg_PI_TODTL_2CMD_F2                              0x00000281U
+#define _reg_PI_ODT_EN_F2                                  0x00000282U
+#define _reg_PI_TODTH_WR                                   0x00000283U
+#define _reg_PI_TODTH_RD                                   0x00000284U
+#define _reg_PI_ODT_RD_MAP_CS0                             0x00000285U
+#define _reg_PI_ODT_WR_MAP_CS0                             0x00000286U
+#define _reg_PI_ODT_RD_MAP_CS1                             0x00000287U
+#define _reg_PI_ODT_WR_MAP_CS1                             0x00000288U
+#define _reg_PI_ODT_RD_MAP_CS2                             0x00000289U
+#define _reg_PI_ODT_WR_MAP_CS2                             0x0000028aU
+#define _reg_PI_ODT_RD_MAP_CS3                             0x0000028bU
+#define _reg_PI_ODT_WR_MAP_CS3                             0x0000028cU
+#define _reg_PI_EN_ODT_ASSERT_EXCEPT_RD                    0x0000028dU
+#define _reg_PI_ODTLON_F0                                  0x0000028eU
+#define _reg_PI_TODTON_MIN_F0                              0x0000028fU
+#define _reg_PI_ODTLON_F1                                  0x00000290U
+#define _reg_PI_TODTON_MIN_F1                              0x00000291U
+#define _reg_PI_ODTLON_F2                                  0x00000292U
+#define _reg_PI_TODTON_MIN_F2                              0x00000293U
+#define _reg_PI_WR_TO_ODTH_F0                              0x00000294U
+#define _reg_PI_WR_TO_ODTH_F1                              0x00000295U
+#define _reg_PI_WR_TO_ODTH_F2                              0x00000296U
+#define _reg_PI_RD_TO_ODTH_F0                              0x00000297U
+#define _reg_PI_RD_TO_ODTH_F1                              0x00000298U
+#define _reg_PI_RD_TO_ODTH_F2                              0x00000299U
+#define _reg_PI_ADDRESS_MIRRORING                          0x0000029aU
+#define _reg_PI_RDLVL_REQ                                  0x0000029bU
+#define _reg_PI_RDLVL_GATE_REQ                             0x0000029cU
+#define _reg_PI_RDLVL_CS                                   0x0000029dU
+#define _reg_PI_RDLVL_PAT_0                                0x0000029eU
+#define _reg_PI_RDLVL_PAT_1                                0x0000029fU
+#define _reg_PI_RDLVL_PAT_2                                0x000002a0U
+#define _reg_PI_RDLVL_PAT_3                                0x000002a1U
+#define _reg_PI_RDLVL_PAT_4                                0x000002a2U
+#define _reg_PI_RDLVL_PAT_5                                0x000002a3U
+#define _reg_PI_RDLVL_PAT_6                                0x000002a4U
+#define _reg_PI_RDLVL_PAT_7                                0x000002a5U
+#define _reg_PI_RDLVL_SEQ_EN                               0x000002a6U
+#define _reg_PI_RDLVL_GATE_SEQ_EN                          0x000002a7U
+#define _reg_PI_RDLVL_PERIODIC                             0x000002a8U
+#define _reg_PI_RDLVL_ON_SREF_EXIT                         0x000002a9U
+#define _reg_PI_RDLVL_DISABLE_DFS                          0x000002aaU
+#define _reg_PI_RDLVL_GATE_PERIODIC                        0x000002abU
+#define _reg_PI_RDLVL_GATE_ON_SREF_EXIT                    0x000002acU
+#define _reg_PI_RDLVL_GATE_DISABLE_DFS                     0x000002adU
+#define _reg_RESERVED_R1                                   0x000002aeU
+#define _reg_PI_RDLVL_ROTATE                               0x000002afU
+#define _reg_PI_RDLVL_GATE_ROTATE                          0x000002b0U
+#define _reg_PI_RDLVL_CS_MAP                               0x000002b1U
+#define _reg_PI_RDLVL_GATE_CS_MAP                          0x000002b2U
+#define _reg_PI_TDFI_RDLVL_RR                              0x000002b3U
+#define _reg_PI_TDFI_RDLVL_RESP                            0x000002b4U
+#define _reg_PI_RDLVL_RESP_MASK                            0x000002b5U
+#define _reg_PI_TDFI_RDLVL_EN                              0x000002b6U
+#define _reg_PI_RDLVL_EN_F0                                0x000002b7U
+#define _reg_PI_RDLVL_GATE_EN_F0                           0x000002b8U
+#define _reg_PI_RDLVL_EN_F1                                0x000002b9U
+#define _reg_PI_RDLVL_GATE_EN_F1                           0x000002baU
+#define _reg_PI_RDLVL_EN_F2                                0x000002bbU
+#define _reg_PI_RDLVL_GATE_EN_F2                           0x000002bcU
+#define _reg_PI_RDLVL_EN                                   0x000002bdU
+#define _reg_PI_RDLVL_GATE_EN                              0x000002beU
+#define _reg_PI_TDFI_RDLVL_MAX                             0x000002bfU
+#define _reg_PI_RDLVL_ERROR_STATUS                         0x000002c0U
+#define _reg_PI_RDLVL_INTERVAL                             0x000002c1U
+#define _reg_PI_RDLVL_GATE_INTERVAL                        0x000002c2U
+#define _reg_PI_RDLVL_PATTERN_START                        0x000002c3U
+#define _reg_PI_RDLVL_PATTERN_NUM                          0x000002c4U
+#define _reg_PI_RDLVL_STROBE_NUM                           0x000002c5U
+#define _reg_PI_RDLVL_GATE_STROBE_NUM                      0x000002c6U
+#define _reg_PI_LPDDR4_RDLVL_PATTERN_8                     0x000002c7U
+#define _reg_PI_LPDDR4_RDLVL_PATTERN_9                     0x000002c8U
+#define _reg_PI_LPDDR4_RDLVL_PATTERN_10                    0x000002c9U
+#define _reg_PI_LPDDR4_RDLVL_PATTERN_11                    0x000002caU
+#define _reg_PI_RD_PREAMBLE_TRAINING_EN                    0x000002cbU
+#define _reg_PI_REG_DIMM_ENABLE                            0x000002ccU
+#define _reg_PI_RDLAT_ADJ_F0                               0x000002cdU
+#define _reg_PI_RDLAT_ADJ_F1                               0x000002ceU
+#define _reg_PI_RDLAT_ADJ_F2                               0x000002cfU
+#define _reg_PI_TDFI_RDDATA_EN                             0x000002d0U
+#define _reg_PI_WRLAT_ADJ_F0                               0x000002d1U
+#define _reg_PI_WRLAT_ADJ_F1                               0x000002d2U
+#define _reg_PI_WRLAT_ADJ_F2                               0x000002d3U
+#define _reg_PI_TDFI_PHY_WRLAT                             0x000002d4U
+#define _reg_PI_TDFI_WRCSLAT_F0                            0x000002d5U
+#define _reg_PI_TDFI_WRCSLAT_F1                            0x000002d6U
+#define _reg_PI_TDFI_WRCSLAT_F2                            0x000002d7U
+#define _reg_PI_TDFI_RDCSLAT_F0                            0x000002d8U
+#define _reg_PI_TDFI_RDCSLAT_F1                            0x000002d9U
+#define _reg_PI_TDFI_RDCSLAT_F2                            0x000002daU
+#define _reg_PI_TDFI_PHY_WRDATA_F0                         0x000002dbU
+#define _reg_PI_TDFI_PHY_WRDATA_F1                         0x000002dcU
+#define _reg_PI_TDFI_PHY_WRDATA_F2                         0x000002ddU
+#define _reg_PI_TDFI_PHY_WRDATA                            0x000002deU
+#define _reg_PI_CALVL_REQ                                  0x000002dfU
+#define _reg_PI_CALVL_CS                                   0x000002e0U
+#define _reg_RESERVED_R2                                   0x000002e1U
+#define _reg_RESERVED_R3                                   0x000002e2U
+#define _reg_PI_CALVL_SEQ_EN                               0x000002e3U
+#define _reg_PI_CALVL_PERIODIC                             0x000002e4U
+#define _reg_PI_CALVL_ON_SREF_EXIT                         0x000002e5U
+#define _reg_PI_CALVL_DISABLE_DFS                          0x000002e6U
+#define _reg_PI_CALVL_ROTATE                               0x000002e7U
+#define _reg_PI_CALVL_CS_MAP                               0x000002e8U
+#define _reg_PI_TDFI_CALVL_EN                              0x000002e9U
+#define _reg_PI_TDFI_CALVL_CC_F0                           0x000002eaU
+#define _reg_PI_TDFI_CALVL_CAPTURE_F0                      0x000002ebU
+#define _reg_PI_TDFI_CALVL_CC_F1                           0x000002ecU
+#define _reg_PI_TDFI_CALVL_CAPTURE_F1                      0x000002edU
+#define _reg_PI_TDFI_CALVL_CC_F2                           0x000002eeU
+#define _reg_PI_TDFI_CALVL_CAPTURE_F2                      0x000002efU
+#define _reg_PI_TDFI_CALVL_RESP                            0x000002f0U
+#define _reg_PI_TDFI_CALVL_MAX                             0x000002f1U
+#define _reg_PI_CALVL_RESP_MASK                            0x000002f2U
+#define _reg_PI_CALVL_EN_F0                                0x000002f3U
+#define _reg_PI_CALVL_EN_F1                                0x000002f4U
+#define _reg_PI_CALVL_EN_F2                                0x000002f5U
+#define _reg_PI_CALVL_EN                                   0x000002f6U
+#define _reg_PI_CALVL_ERROR_STATUS                         0x000002f7U
+#define _reg_PI_CALVL_INTERVAL                             0x000002f8U
+#define _reg_PI_TCACKEL                                    0x000002f9U
+#define _reg_PI_TCAMRD                                     0x000002faU
+#define _reg_PI_TCACKEH                                    0x000002fbU
+#define _reg_PI_TMRZ_F0                                    0x000002fcU
+#define _reg_PI_TCAENT_F0                                  0x000002fdU
+#define _reg_PI_TMRZ_F1                                    0x000002feU
+#define _reg_PI_TCAENT_F1                                  0x000002ffU
+#define _reg_PI_TMRZ_F2                                    0x00000300U
+#define _reg_PI_TCAENT_F2                                  0x00000301U
+#define _reg_PI_TCAEXT                                     0x00000302U
+#define _reg_PI_CA_TRAIN_VREF_EN                           0x00000303U
+#define _reg_PI_TDFI_CACSCA_F0                             0x00000304U
+#define _reg_PI_TDFI_CASEL_F0                              0x00000305U
+#define _reg_PI_TVREF_SHORT_F0                             0x00000306U
+#define _reg_PI_TVREF_LONG_F0                              0x00000307U
+#define _reg_PI_TDFI_CACSCA_F1                             0x00000308U
+#define _reg_PI_TDFI_CASEL_F1                              0x00000309U
+#define _reg_PI_TVREF_SHORT_F1                             0x0000030aU
+#define _reg_PI_TVREF_LONG_F1                              0x0000030bU
+#define _reg_PI_TDFI_CACSCA_F2                             0x0000030cU
+#define _reg_PI_TDFI_CASEL_F2                              0x0000030dU
+#define _reg_PI_TVREF_SHORT_F2                             0x0000030eU
+#define _reg_PI_TVREF_LONG_F2                              0x0000030fU
+#define _reg_PI_CALVL_VREF_INITIAL_START_POINT_F0          0x00000310U
+#define _reg_PI_CALVL_VREF_INITIAL_STOP_POINT_F0           0x00000311U
+#define _reg_PI_CALVL_VREF_INITIAL_START_POINT_F1          0x00000312U
+#define _reg_PI_CALVL_VREF_INITIAL_STOP_POINT_F1           0x00000313U
+#define _reg_PI_CALVL_VREF_INITIAL_START_POINT_F2          0x00000314U
+#define _reg_PI_CALVL_VREF_INITIAL_STOP_POINT_F2           0x00000315U
+#define _reg_PI_CALVL_VREF_INITIAL_START_POINT             0x00000316U
+#define _reg_PI_CALVL_VREF_INITIAL_STOP_POINT              0x00000317U
+#define _reg_PI_CALVL_VREF_INITIAL_STEPSIZE                0x00000318U
+#define _reg_PI_CALVL_VREF_NORMAL_STEPSIZE                 0x00000319U
+#define _reg_PI_CALVL_VREF_DELTA_F0                        0x0000031aU
+#define _reg_PI_CALVL_VREF_DELTA_F1                        0x0000031bU
+#define _reg_PI_CALVL_VREF_DELTA_F2                        0x0000031cU
+#define _reg_PI_CALVL_VREF_DELTA                           0x0000031dU
+#define _reg_PI_TDFI_INIT_START_MIN                        0x0000031eU
+#define _reg_PI_TDFI_INIT_COMPLETE_MIN                     0x0000031fU
+#define _reg_PI_TDFI_CALVL_STROBE_F0                       0x00000320U
+#define _reg_PI_TXP_F0                                     0x00000321U
+#define _reg_PI_TMRWCKEL_F0                                0x00000322U
+#define _reg_PI_TCKELCK_F0                                 0x00000323U
+#define _reg_PI_TDFI_CALVL_STROBE_F1                       0x00000324U
+#define _reg_PI_TXP_F1                                     0x00000325U
+#define _reg_PI_TMRWCKEL_F1                                0x00000326U
+#define _reg_PI_TCKELCK_F1                                 0x00000327U
+#define _reg_PI_TDFI_CALVL_STROBE_F2                       0x00000328U
+#define _reg_PI_TXP_F2                                     0x00000329U
+#define _reg_PI_TMRWCKEL_F2                                0x0000032aU
+#define _reg_PI_TCKELCK_F2                                 0x0000032bU
+#define _reg_PI_TCKCKEH                                    0x0000032cU
+#define _reg_PI_CALVL_STROBE_NUM                           0x0000032dU
+#define _reg_PI_SW_CA_TRAIN_VREF                           0x0000032eU
+#define _reg_PI_TDFI_INIT_START_F0                         0x0000032fU
+#define _reg_PI_TDFI_INIT_COMPLETE_F0                      0x00000330U
+#define _reg_PI_TDFI_INIT_START_F1                         0x00000331U
+#define _reg_PI_TDFI_INIT_COMPLETE_F1                      0x00000332U
+#define _reg_PI_TDFI_INIT_START_F2                         0x00000333U
+#define _reg_PI_TDFI_INIT_COMPLETE_F2                      0x00000334U
+#define _reg_PI_CLKDISABLE_2_INIT_START                    0x00000335U
+#define _reg_PI_INIT_STARTORCOMPLETE_2_CLKDISABLE          0x00000336U
+#define _reg_PI_DRAM_CLK_DISABLE_DEASSERT_SEL              0x00000337U
+#define _reg_PI_REFRESH_BETWEEN_SEGMENT_DISABLE            0x00000338U
+#define _reg_PI_TCKEHDQS_F0                                0x00000339U
+#define _reg_PI_TCKEHDQS_F1                                0x0000033aU
+#define _reg_PI_TCKEHDQS_F2                                0x0000033bU
+#define _reg_PI_MC_DFS_PI_SET_VREF_ENABLE                  0x0000033cU
+#define _reg_PI_WDQLVL_VREF_EN                             0x0000033dU
+#define _reg_PI_WDQLVL_BST_NUM                             0x0000033eU
+#define _reg_PI_TDFI_WDQLVL_WR_F0                          0x0000033fU
+#define _reg_PI_TDFI_WDQLVL_WR_F1                          0x00000340U
+#define _reg_PI_TDFI_WDQLVL_WR_F2                          0x00000341U
+#define _reg_PI_TDFI_WDQLVL_WR                             0x00000342U
+#define _reg_PI_TDFI_WDQLVL_RW                             0x00000343U
+#define _reg_PI_WDQLVL_RESP_MASK                           0x00000344U
+#define _reg_PI_WDQLVL_ROTATE                              0x00000345U
+#define _reg_PI_WDQLVL_CS_MAP                              0x00000346U
+#define _reg_PI_WDQLVL_VREF_INITIAL_START_POINT_F0         0x00000347U
+#define _reg_PI_WDQLVL_VREF_INITIAL_STOP_POINT_F0          0x00000348U
+#define _reg_PI_WDQLVL_VREF_INITIAL_START_POINT_F1         0x00000349U
+#define _reg_PI_WDQLVL_VREF_INITIAL_STOP_POINT_F1          0x0000034aU
+#define _reg_PI_WDQLVL_VREF_INITIAL_START_POINT_F2         0x0000034bU
+#define _reg_PI_WDQLVL_VREF_INITIAL_STOP_POINT_F2          0x0000034cU
+#define _reg_PI_WDQLVL_VREF_INITIAL_START_POINT            0x0000034dU
+#define _reg_PI_WDQLVL_VREF_INITIAL_STOP_POINT             0x0000034eU
+#define _reg_PI_WDQLVL_VREF_INITIAL_STEPSIZE               0x0000034fU
+#define _reg_PI_WDQLVL_VREF_NORMAL_STEPSIZE                0x00000350U
+#define _reg_PI_WDQLVL_VREF_DELTA_F0                       0x00000351U
+#define _reg_PI_WDQLVL_VREF_DELTA_F1                       0x00000352U
+#define _reg_PI_WDQLVL_VREF_DELTA_F2                       0x00000353U
+#define _reg_PI_WDQLVL_VREF_DELTA                          0x00000354U
+#define _reg_PI_WDQLVL_PERIODIC                            0x00000355U
+#define _reg_PI_WDQLVL_REQ                                 0x00000356U
+#define _reg_PI_WDQLVL_CS                                  0x00000357U
+#define _reg_PI_TDFI_WDQLVL_EN                             0x00000358U
+#define _reg_PI_TDFI_WDQLVL_RESP                           0x00000359U
+#define _reg_PI_TDFI_WDQLVL_MAX                            0x0000035aU
+#define _reg_PI_WDQLVL_INTERVAL                            0x0000035bU
+#define _reg_PI_WDQLVL_EN_F0                               0x0000035cU
+#define _reg_PI_WDQLVL_EN_F1                               0x0000035dU
+#define _reg_PI_WDQLVL_EN_F2                               0x0000035eU
+#define _reg_PI_WDQLVL_EN                                  0x0000035fU
+#define _reg_PI_WDQLVL_ON_SREF_EXIT                        0x00000360U
+#define _reg_PI_WDQLVL_DISABLE_DFS                         0x00000361U
+#define _reg_PI_WDQLVL_ERROR_STATUS                        0x00000362U
+#define _reg_PI_MR1_DATA_F0_0                              0x00000363U
+#define _reg_PI_MR2_DATA_F0_0                              0x00000364U
+#define _reg_PI_MR3_DATA_F0_0                              0x00000365U
+#define _reg_PI_MR11_DATA_F0_0                             0x00000366U
+#define _reg_PI_MR12_DATA_F0_0                             0x00000367U
+#define _reg_PI_MR14_DATA_F0_0                             0x00000368U
+#define _reg_PI_MR22_DATA_F0_0                             0x00000369U
+#define _reg_PI_MR1_DATA_F1_0                              0x0000036aU
+#define _reg_PI_MR2_DATA_F1_0                              0x0000036bU
+#define _reg_PI_MR3_DATA_F1_0                              0x0000036cU
+#define _reg_PI_MR11_DATA_F1_0                             0x0000036dU
+#define _reg_PI_MR12_DATA_F1_0                             0x0000036eU
+#define _reg_PI_MR14_DATA_F1_0                             0x0000036fU
+#define _reg_PI_MR22_DATA_F1_0                             0x00000370U
+#define _reg_PI_MR1_DATA_F2_0                              0x00000371U
+#define _reg_PI_MR2_DATA_F2_0                              0x00000372U
+#define _reg_PI_MR3_DATA_F2_0                              0x00000373U
+#define _reg_PI_MR11_DATA_F2_0                             0x00000374U
+#define _reg_PI_MR12_DATA_F2_0                             0x00000375U
+#define _reg_PI_MR14_DATA_F2_0                             0x00000376U
+#define _reg_PI_MR22_DATA_F2_0                             0x00000377U
+#define _reg_PI_MR13_DATA_0                                0x00000378U
+#define _reg_PI_MR1_DATA_F0_1                              0x00000379U
+#define _reg_PI_MR2_DATA_F0_1                              0x0000037aU
+#define _reg_PI_MR3_DATA_F0_1                              0x0000037bU
+#define _reg_PI_MR11_DATA_F0_1                             0x0000037cU
+#define _reg_PI_MR12_DATA_F0_1                             0x0000037dU
+#define _reg_PI_MR14_DATA_F0_1                             0x0000037eU
+#define _reg_PI_MR22_DATA_F0_1                             0x0000037fU
+#define _reg_PI_MR1_DATA_F1_1                              0x00000380U
+#define _reg_PI_MR2_DATA_F1_1                              0x00000381U
+#define _reg_PI_MR3_DATA_F1_1                              0x00000382U
+#define _reg_PI_MR11_DATA_F1_1                             0x00000383U
+#define _reg_PI_MR12_DATA_F1_1                             0x00000384U
+#define _reg_PI_MR14_DATA_F1_1                             0x00000385U
+#define _reg_PI_MR22_DATA_F1_1                             0x00000386U
+#define _reg_PI_MR1_DATA_F2_1                              0x00000387U
+#define _reg_PI_MR2_DATA_F2_1                              0x00000388U
+#define _reg_PI_MR3_DATA_F2_1                              0x00000389U
+#define _reg_PI_MR11_DATA_F2_1                             0x0000038aU
+#define _reg_PI_MR12_DATA_F2_1                             0x0000038bU
+#define _reg_PI_MR14_DATA_F2_1                             0x0000038cU
+#define _reg_PI_MR22_DATA_F2_1                             0x0000038dU
+#define _reg_PI_MR13_DATA_1                                0x0000038eU
+#define _reg_PI_MR1_DATA_F0_2                              0x0000038fU
+#define _reg_PI_MR2_DATA_F0_2                              0x00000390U
+#define _reg_PI_MR3_DATA_F0_2                              0x00000391U
+#define _reg_PI_MR11_DATA_F0_2                             0x00000392U
+#define _reg_PI_MR12_DATA_F0_2                             0x00000393U
+#define _reg_PI_MR14_DATA_F0_2                             0x00000394U
+#define _reg_PI_MR22_DATA_F0_2                             0x00000395U
+#define _reg_PI_MR1_DATA_F1_2                              0x00000396U
+#define _reg_PI_MR2_DATA_F1_2                              0x00000397U
+#define _reg_PI_MR3_DATA_F1_2                              0x00000398U
+#define _reg_PI_MR11_DATA_F1_2                             0x00000399U
+#define _reg_PI_MR12_DATA_F1_2                             0x0000039aU
+#define _reg_PI_MR14_DATA_F1_2                             0x0000039bU
+#define _reg_PI_MR22_DATA_F1_2                             0x0000039cU
+#define _reg_PI_MR1_DATA_F2_2                              0x0000039dU
+#define _reg_PI_MR2_DATA_F2_2                              0x0000039eU
+#define _reg_PI_MR3_DATA_F2_2                              0x0000039fU
+#define _reg_PI_MR11_DATA_F2_2                             0x000003a0U
+#define _reg_PI_MR12_DATA_F2_2                             0x000003a1U
+#define _reg_PI_MR14_DATA_F2_2                             0x000003a2U
+#define _reg_PI_MR22_DATA_F2_2                             0x000003a3U
+#define _reg_PI_MR13_DATA_2                                0x000003a4U
+#define _reg_PI_MR1_DATA_F0_3                              0x000003a5U
+#define _reg_PI_MR2_DATA_F0_3                              0x000003a6U
+#define _reg_PI_MR3_DATA_F0_3                              0x000003a7U
+#define _reg_PI_MR11_DATA_F0_3                             0x000003a8U
+#define _reg_PI_MR12_DATA_F0_3                             0x000003a9U
+#define _reg_PI_MR14_DATA_F0_3                             0x000003aaU
+#define _reg_PI_MR22_DATA_F0_3                             0x000003abU
+#define _reg_PI_MR1_DATA_F1_3                              0x000003acU
+#define _reg_PI_MR2_DATA_F1_3                              0x000003adU
+#define _reg_PI_MR3_DATA_F1_3                              0x000003aeU
+#define _reg_PI_MR11_DATA_F1_3                             0x000003afU
+#define _reg_PI_MR12_DATA_F1_3                             0x000003b0U
+#define _reg_PI_MR14_DATA_F1_3                             0x000003b1U
+#define _reg_PI_MR22_DATA_F1_3                             0x000003b2U
+#define _reg_PI_MR1_DATA_F2_3                              0x000003b3U
+#define _reg_PI_MR2_DATA_F2_3                              0x000003b4U
+#define _reg_PI_MR3_DATA_F2_3                              0x000003b5U
+#define _reg_PI_MR11_DATA_F2_3                             0x000003b6U
+#define _reg_PI_MR12_DATA_F2_3                             0x000003b7U
+#define _reg_PI_MR14_DATA_F2_3                             0x000003b8U
+#define _reg_PI_MR22_DATA_F2_3                             0x000003b9U
+#define _reg_PI_MR13_DATA_3                                0x000003baU
+#define _reg_PI_BANK_DIFF                                  0x000003bbU
+#define _reg_PI_ROW_DIFF                                   0x000003bcU
+#define _reg_PI_TFC_F0                                     0x000003bdU
+#define _reg_PI_TFC_F1                                     0x000003beU
+#define _reg_PI_TFC_F2                                     0x000003bfU
+#define _reg_PI_TCCD                                       0x000003c0U
+#define _reg_PI_TRTP_F0                                    0x000003c1U
+#define _reg_PI_TRP_F0                                     0x000003c2U
+#define _reg_PI_TRCD_F0                                    0x000003c3U
+#define _reg_PI_TWTR_F0                                    0x000003c4U
+#define _reg_PI_TWR_F0                                     0x000003c5U
+#define _reg_PI_TRAS_MAX_F0                                0x000003c6U
+#define _reg_PI_TRAS_MIN_F0                                0x000003c7U
+#define _reg_PI_TDQSCK_MAX_F0                              0x000003c8U
+#define _reg_PI_TCCDMW_F0                                  0x000003c9U
+#define _reg_PI_TSR_F0                                     0x000003caU
+#define _reg_PI_TMRD_F0                                    0x000003cbU
+#define _reg_PI_TMRW_F0                                    0x000003ccU
+#define _reg_PI_TMOD_F0                                    0x000003cdU
+#define _reg_PI_TRTP_F1                                    0x000003ceU
+#define _reg_PI_TRP_F1                                     0x000003cfU
+#define _reg_PI_TRCD_F1                                    0x000003d0U
+#define _reg_PI_TWTR_F1                                    0x000003d1U
+#define _reg_PI_TWR_F1                                     0x000003d2U
+#define _reg_PI_TRAS_MAX_F1                                0x000003d3U
+#define _reg_PI_TRAS_MIN_F1                                0x000003d4U
+#define _reg_PI_TDQSCK_MAX_F1                              0x000003d5U
+#define _reg_PI_TCCDMW_F1                                  0x000003d6U
+#define _reg_PI_TSR_F1                                     0x000003d7U
+#define _reg_PI_TMRD_F1                                    0x000003d8U
+#define _reg_PI_TMRW_F1                                    0x000003d9U
+#define _reg_PI_TMOD_F1                                    0x000003daU
+#define _reg_PI_TRTP_F2                                    0x000003dbU
+#define _reg_PI_TRP_F2                                     0x000003dcU
+#define _reg_PI_TRCD_F2                                    0x000003ddU
+#define _reg_PI_TWTR_F2                                    0x000003deU
+#define _reg_PI_TWR_F2                                     0x000003dfU
+#define _reg_PI_TRAS_MAX_F2                                0x000003e0U
+#define _reg_PI_TRAS_MIN_F2                                0x000003e1U
+#define _reg_PI_TDQSCK_MAX_F2                              0x000003e2U
+#define _reg_PI_TCCDMW_F2                                  0x000003e3U
+#define _reg_PI_TSR_F2                                     0x000003e4U
+#define _reg_PI_TMRD_F2                                    0x000003e5U
+#define _reg_PI_TMRW_F2                                    0x000003e6U
+#define _reg_PI_TMOD_F2                                    0x000003e7U
+#define _reg_RESERVED_R4                                   0x000003e8U
+#define _reg_RESERVED_R5                                   0x000003e9U
+#define _reg_RESERVED_R6                                   0x000003eaU
+#define _reg_RESERVED_R7                                   0x000003ebU
+#define _reg_RESERVED_R8                                   0x000003ecU
+#define _reg_RESERVED_R9                                   0x000003edU
+#define _reg_RESERVED_R10                                  0x000003eeU
+#define _reg_RESERVED_R11                                  0x000003efU
+#define _reg_RESERVED_R12                                  0x000003f0U
+#define _reg_RESERVED_R13                                  0x000003f1U
+#define _reg_RESERVED_R14                                  0x000003f2U
+#define _reg_RESERVED_R15                                  0x000003f3U
+#define _reg_RESERVED_R16                                  0x000003f4U
+#define _reg_RESERVED_R17                                  0x000003f5U
+#define _reg_RESERVED_R18                                  0x000003f6U
+#define _reg_RESERVED_R19                                  0x000003f7U
+#define _reg_RESERVED_R20                                  0x000003f8U
+#define _reg_RESERVED_R21                                  0x000003f9U
+#define _reg_RESERVED_R22                                  0x000003faU
+#define _reg_RESERVED_R23                                  0x000003fbU
+#define _reg_PI_INT_STATUS                                 0x000003fcU
+#define _reg_PI_INT_ACK                                    0x000003fdU
+#define _reg_PI_INT_MASK                                   0x000003feU
+#define _reg_PI_BIST_EXP_DATA_P0                           0x000003ffU
+#define _reg_PI_BIST_EXP_DATA_P1                           0x00000400U
+#define _reg_PI_BIST_EXP_DATA_P2                           0x00000401U
+#define _reg_PI_BIST_EXP_DATA_P3                           0x00000402U
+#define _reg_PI_BIST_FAIL_DATA_P0                          0x00000403U
+#define _reg_PI_BIST_FAIL_DATA_P1                          0x00000404U
+#define _reg_PI_BIST_FAIL_DATA_P2                          0x00000405U
+#define _reg_PI_BIST_FAIL_DATA_P3                          0x00000406U
+#define _reg_PI_BIST_FAIL_ADDR_P0                          0x00000407U
+#define _reg_PI_BIST_FAIL_ADDR_P1                          0x00000408U
+#define _reg_PI_BSTLEN                                     0x00000409U
+#define _reg_PI_LONG_COUNT_MASK                            0x0000040aU
+#define _reg_PI_CMD_SWAP_EN                                0x0000040bU
+#define _reg_PI_CKE_MUX_0                                  0x0000040cU
+#define _reg_PI_CKE_MUX_1                                  0x0000040dU
+#define _reg_PI_CKE_MUX_2                                  0x0000040eU
+#define _reg_PI_CKE_MUX_3                                  0x0000040fU
+#define _reg_PI_CS_MUX_0                                   0x00000410U
+#define _reg_PI_CS_MUX_1                                   0x00000411U
+#define _reg_PI_CS_MUX_2                                   0x00000412U
+#define _reg_PI_CS_MUX_3                                   0x00000413U
+#define _reg_PI_RAS_N_MUX                                  0x00000414U
+#define _reg_PI_CAS_N_MUX                                  0x00000415U
+#define _reg_PI_WE_N_MUX                                   0x00000416U
+#define _reg_PI_BANK_MUX_0                                 0x00000417U
+#define _reg_PI_BANK_MUX_1                                 0x00000418U
+#define _reg_PI_BANK_MUX_2                                 0x00000419U
+#define _reg_PI_ODT_MUX_0                                  0x0000041aU
+#define _reg_PI_ODT_MUX_1                                  0x0000041bU
+#define _reg_PI_ODT_MUX_2                                  0x0000041cU
+#define _reg_PI_ODT_MUX_3                                  0x0000041dU
+#define _reg_PI_RESET_N_MUX_0                              0x0000041eU
+#define _reg_PI_RESET_N_MUX_1                              0x0000041fU
+#define _reg_PI_RESET_N_MUX_2                              0x00000420U
+#define _reg_PI_RESET_N_MUX_3                              0x00000421U
+#define _reg_PI_DATA_BYTE_SWAP_EN                          0x00000422U
+#define _reg_PI_DATA_BYTE_SWAP_SLICE0                      0x00000423U
+#define _reg_PI_DATA_BYTE_SWAP_SLICE1                      0x00000424U
+#define _reg_PI_DATA_BYTE_SWAP_SLICE2                      0x00000425U
+#define _reg_PI_DATA_BYTE_SWAP_SLICE3                      0x00000426U
+#define _reg_PI_CTRLUPD_REQ_PER_AREF_EN                    0x00000427U
+#define _reg_PI_TDFI_CTRLUPD_MIN                           0x00000428U
+#define _reg_PI_TDFI_CTRLUPD_MAX_F0                        0x00000429U
+#define _reg_PI_TDFI_CTRLUPD_INTERVAL_F0                   0x0000042aU
+#define _reg_PI_TDFI_CTRLUPD_MAX_F1                        0x0000042bU
+#define _reg_PI_TDFI_CTRLUPD_INTERVAL_F1                   0x0000042cU
+#define _reg_PI_TDFI_CTRLUPD_MAX_F2                        0x0000042dU
+#define _reg_PI_TDFI_CTRLUPD_INTERVAL_F2                   0x0000042eU
+#define _reg_PI_UPDATE_ERROR_STATUS                        0x0000042fU
+#define _reg_PI_BIST_GO                                    0x00000430U
+#define _reg_PI_BIST_RESULT                                0x00000431U
+#define _reg_PI_ADDR_SPACE                                 0x00000432U
+#define _reg_PI_BIST_DATA_CHECK                            0x00000433U
+#define _reg_PI_BIST_ADDR_CHECK                            0x00000434U
+#define _reg_PI_BIST_START_ADDRESS_P0                      0x00000435U
+#define _reg_PI_BIST_START_ADDRESS_P1                      0x00000436U
+#define _reg_PI_BIST_DATA_MASK_P0                          0x00000437U
+#define _reg_PI_BIST_DATA_MASK_P1                          0x00000438U
+#define _reg_PI_BIST_ERR_COUNT                             0x00000439U
+#define _reg_PI_BIST_ERR_STOP                              0x0000043aU
+#define _reg_PI_BIST_ADDR_MASK_0_P0                        0x0000043bU
+#define _reg_PI_BIST_ADDR_MASK_0_P1                        0x0000043cU
+#define _reg_PI_BIST_ADDR_MASK_1_P0                        0x0000043dU
+#define _reg_PI_BIST_ADDR_MASK_1_P1                        0x0000043eU
+#define _reg_PI_BIST_ADDR_MASK_2_P0                        0x0000043fU
+#define _reg_PI_BIST_ADDR_MASK_2_P1                        0x00000440U
+#define _reg_PI_BIST_ADDR_MASK_3_P0                        0x00000441U
+#define _reg_PI_BIST_ADDR_MASK_3_P1                        0x00000442U
+#define _reg_PI_BIST_ADDR_MASK_4_P0                        0x00000443U
+#define _reg_PI_BIST_ADDR_MASK_4_P1                        0x00000444U
+#define _reg_PI_BIST_ADDR_MASK_5_P0                        0x00000445U
+#define _reg_PI_BIST_ADDR_MASK_5_P1                        0x00000446U
+#define _reg_PI_BIST_ADDR_MASK_6_P0                        0x00000447U
+#define _reg_PI_BIST_ADDR_MASK_6_P1                        0x00000448U
+#define _reg_PI_BIST_ADDR_MASK_7_P0                        0x00000449U
+#define _reg_PI_BIST_ADDR_MASK_7_P1                        0x0000044aU
+#define _reg_PI_BIST_ADDR_MASK_8_P0                        0x0000044bU
+#define _reg_PI_BIST_ADDR_MASK_8_P1                        0x0000044cU
+#define _reg_PI_BIST_ADDR_MASK_9_P0                        0x0000044dU
+#define _reg_PI_BIST_ADDR_MASK_9_P1                        0x0000044eU
+#define _reg_PI_BIST_MODE                                  0x0000044fU
+#define _reg_PI_BIST_ADDR_MODE                             0x00000450U
+#define _reg_PI_BIST_PAT_MODE                              0x00000451U
+#define _reg_PI_BIST_USER_PAT_P0                           0x00000452U
+#define _reg_PI_BIST_USER_PAT_P1                           0x00000453U
+#define _reg_PI_BIST_USER_PAT_P2                           0x00000454U
+#define _reg_PI_BIST_USER_PAT_P3                           0x00000455U
+#define _reg_PI_BIST_PAT_NUM                               0x00000456U
+#define _reg_PI_BIST_STAGE_0                               0x00000457U
+#define _reg_PI_BIST_STAGE_1                               0x00000458U
+#define _reg_PI_BIST_STAGE_2                               0x00000459U
+#define _reg_PI_BIST_STAGE_3                               0x0000045aU
+#define _reg_PI_BIST_STAGE_4                               0x0000045bU
+#define _reg_PI_BIST_STAGE_5                               0x0000045cU
+#define _reg_PI_BIST_STAGE_6                               0x0000045dU
+#define _reg_PI_BIST_STAGE_7                               0x0000045eU
+#define _reg_PI_COL_DIFF                                   0x0000045fU
+#define _reg_PI_SELF_REFRESH_EN                            0x00000460U
+#define _reg_PI_TXSR_F0                                    0x00000461U
+#define _reg_PI_TXSR_F1                                    0x00000462U
+#define _reg_PI_TXSR_F2                                    0x00000463U
+#define _reg_PI_MONITOR_SRC_SEL_0                          0x00000464U
+#define _reg_PI_MONITOR_CAP_SEL_0                          0x00000465U
+#define _reg_PI_MONITOR_0                                  0x00000466U
+#define _reg_PI_MONITOR_SRC_SEL_1                          0x00000467U
+#define _reg_PI_MONITOR_CAP_SEL_1                          0x00000468U
+#define _reg_PI_MONITOR_1                                  0x00000469U
+#define _reg_PI_MONITOR_SRC_SEL_2                          0x0000046aU
+#define _reg_PI_MONITOR_CAP_SEL_2                          0x0000046bU
+#define _reg_PI_MONITOR_2                                  0x0000046cU
+#define _reg_PI_MONITOR_SRC_SEL_3                          0x0000046dU
+#define _reg_PI_MONITOR_CAP_SEL_3                          0x0000046eU
+#define _reg_PI_MONITOR_3                                  0x0000046fU
+#define _reg_PI_MONITOR_SRC_SEL_4                          0x00000470U
+#define _reg_PI_MONITOR_CAP_SEL_4                          0x00000471U
+#define _reg_PI_MONITOR_4                                  0x00000472U
+#define _reg_PI_MONITOR_SRC_SEL_5                          0x00000473U
+#define _reg_PI_MONITOR_CAP_SEL_5                          0x00000474U
+#define _reg_PI_MONITOR_5                                  0x00000475U
+#define _reg_PI_MONITOR_SRC_SEL_6                          0x00000476U
+#define _reg_PI_MONITOR_CAP_SEL_6                          0x00000477U
+#define _reg_PI_MONITOR_6                                  0x00000478U
+#define _reg_PI_MONITOR_SRC_SEL_7                          0x00000479U
+#define _reg_PI_MONITOR_CAP_SEL_7                          0x0000047aU
+#define _reg_PI_MONITOR_7                                  0x0000047bU
+#define _reg_PI_MONITOR_STROBE                             0x0000047cU
+#define _reg_PI_DLL_LOCK                                   0x0000047dU
+#define _reg_PI_FREQ_NUMBER_STATUS                         0x0000047eU
+#define _reg_RESERVED_R24                                  0x0000047fU
+#define _reg_PI_PHYMSTR_TYPE                               0x00000480U
+#define _reg_PI_POWER_REDUC_EN                             0x00000481U
+#define _reg_RESERVED_R25                                  0x00000482U
+#define _reg_RESERVED_R26                                  0x00000483U
+#define _reg_RESERVED_R27                                  0x00000484U
+#define _reg_RESERVED_R28                                  0x00000485U
+#define _reg_RESERVED_R29                                  0x00000486U
+#define _reg_RESERVED_R30                                  0x00000487U
+#define _reg_RESERVED_R31                                  0x00000488U
+#define _reg_RESERVED_R32                                  0x00000489U
+#define _reg_RESERVED_R33                                  0x0000048aU
+#define _reg_RESERVED_R34                                  0x0000048bU
+#define _reg_RESERVED_R35                                  0x0000048cU
+#define _reg_RESERVED_R36                                  0x0000048dU
+#define _reg_RESERVED_R37                                  0x0000048eU
+#define _reg_RESERVED_R38                                  0x0000048fU
+#define _reg_RESERVED_R39                                  0x00000490U
+#define _reg_PI_WRLVL_MAX_STROBE_PEND                      0x00000491U
+#define _reg_PI_TSDO_F0                                    0x00000492U
+#define _reg_PI_TSDO_F1                                    0x00000493U
+#define _reg_PI_TSDO_F2                                    0x00000494U
+
+#define DDR_REGDEF_ADR(regdef) ((regdef) & 0xffff)
+#define DDR_REGDEF_LEN(regdef) (((regdef) >> 16) & 0xff)
+#define DDR_REGDEF_LSB(regdef) (((regdef) >> 24) & 0xff)
+
+static const uint32_t DDR_REGDEF_TBL[4][1173] = {
+	{
+/*0000*/ 0xffffffffU,
+/*0001*/ 0xffffffffU,
+/*0002*/ 0x000b0400U,
+/*0003*/ 0xffffffffU,
+/*0004*/ 0xffffffffU,
+/*0005*/ 0x10010400U,
+/*0006*/ 0x18050400U,
+/*0007*/ 0x00050401U,
+/*0008*/ 0x08050401U,
+/*0009*/ 0x10050401U,
+/*000a*/ 0x18050401U,
+/*000b*/ 0x00050402U,
+/*000c*/ 0x08050402U,
+/*000d*/ 0x10050402U,
+/*000e*/ 0x18050402U,
+/*000f*/ 0x00040403U,
+/*0010*/ 0x08030403U,
+/*0011*/ 0x00180404U,
+/*0012*/ 0x18030404U,
+/*0013*/ 0x00180405U,
+/*0014*/ 0x18020405U,
+/*0015*/ 0x00010406U,
+/*0016*/ 0x08020406U,
+/*0017*/ 0x10010406U,
+/*0018*/ 0x18010406U,
+/*0019*/ 0x00020407U,
+/*001a*/ 0x08040407U,
+/*001b*/ 0x10040407U,
+/*001c*/ 0x18040407U,
+/*001d*/ 0x000a0408U,
+/*001e*/ 0x10040408U,
+/*001f*/ 0xffffffffU,
+/*0020*/ 0xffffffffU,
+/*0021*/ 0x18070408U,
+/*0022*/ 0xffffffffU,
+/*0023*/ 0xffffffffU,
+/*0024*/ 0xffffffffU,
+/*0025*/ 0xffffffffU,
+/*0026*/ 0xffffffffU,
+/*0027*/ 0xffffffffU,
+/*0028*/ 0x000a0409U,
+/*0029*/ 0x10040409U,
+/*002a*/ 0x18010409U,
+/*002b*/ 0x0001040aU,
+/*002c*/ 0x0802040aU,
+/*002d*/ 0x1009040aU,
+/*002e*/ 0x0009040bU,
+/*002f*/ 0x1002040bU,
+/*0030*/ 0x0020040cU,
+/*0031*/ 0xffffffffU,
+/*0032*/ 0x0001040dU,
+/*0033*/ 0xffffffffU,
+/*0034*/ 0xffffffffU,
+/*0035*/ 0xffffffffU,
+/*0036*/ 0xffffffffU,
+/*0037*/ 0x0020040eU,
+/*0038*/ 0x0020040fU,
+/*0039*/ 0x00200410U,
+/*003a*/ 0x00200411U,
+/*003b*/ 0x00030412U,
+/*003c*/ 0x08010412U,
+/*003d*/ 0x10030412U,
+/*003e*/ 0x18030412U,
+/*003f*/ 0x00040413U,
+/*0040*/ 0x08040413U,
+/*0041*/ 0x10040413U,
+/*0042*/ 0x18040413U,
+/*0043*/ 0x00010414U,
+/*0044*/ 0x08010414U,
+/*0045*/ 0x10060414U,
+/*0046*/ 0x18040414U,
+/*0047*/ 0xffffffffU,
+/*0048*/ 0x00060415U,
+/*0049*/ 0x08040415U,
+/*004a*/ 0x10060415U,
+/*004b*/ 0x18040415U,
+/*004c*/ 0x00020416U,
+/*004d*/ 0x08050416U,
+/*004e*/ 0x10080416U,
+/*004f*/ 0x00200417U,
+/*0050*/ 0x00060418U,
+/*0051*/ 0x08030418U,
+/*0052*/ 0x100b0418U,
+/*0053*/ 0x00040419U,
+/*0054*/ 0x08040419U,
+/*0055*/ 0x10040419U,
+/*0056*/ 0xffffffffU,
+/*0057*/ 0x18010419U,
+/*0058*/ 0x0009041aU,
+/*0059*/ 0x0020041bU,
+/*005a*/ 0x0020041cU,
+/*005b*/ 0x0020041dU,
+/*005c*/ 0x0020041eU,
+/*005d*/ 0x0010041fU,
+/*005e*/ 0x00200420U,
+/*005f*/ 0x00010421U,
+/*0060*/ 0x08060421U,
+/*0061*/ 0x10080421U,
+/*0062*/ 0x00200422U,
+/*0063*/ 0xffffffffU,
+/*0064*/ 0x000a0423U,
+/*0065*/ 0x10060423U,
+/*0066*/ 0x18070423U,
+/*0067*/ 0x00080424U,
+/*0068*/ 0x08080424U,
+/*0069*/ 0x100a0424U,
+/*006a*/ 0x00070425U,
+/*006b*/ 0x08080425U,
+/*006c*/ 0x10080425U,
+/*006d*/ 0x18030425U,
+/*006e*/ 0x000a0426U,
+/*006f*/ 0x100a0426U,
+/*0070*/ 0x00110427U,
+/*0071*/ 0x00090428U,
+/*0072*/ 0x10090428U,
+/*0073*/ 0x00100429U,
+/*0074*/ 0x100e0429U,
+/*0075*/ 0x000e042aU,
+/*0076*/ 0x100c042aU,
+/*0077*/ 0x000a042bU,
+/*0078*/ 0x100a042bU,
+/*0079*/ 0x0002042cU,
+/*007a*/ 0x0020042dU,
+/*007b*/ 0x000b042eU,
+/*007c*/ 0x100b042eU,
+/*007d*/ 0x0020042fU,
+/*007e*/ 0x00120430U,
+/*007f*/ 0x00200431U,
+/*0080*/ 0x00200432U,
+/*0081*/ 0xffffffffU,
+/*0082*/ 0xffffffffU,
+/*0083*/ 0x00010433U,
+/*0084*/ 0x08010433U,
+/*0085*/ 0x10080433U,
+/*0086*/ 0x000c0434U,
+/*0087*/ 0x100c0434U,
+/*0088*/ 0x000c0435U,
+/*0089*/ 0x100c0435U,
+/*008a*/ 0x000c0436U,
+/*008b*/ 0x100c0436U,
+/*008c*/ 0x000c0437U,
+/*008d*/ 0x100c0437U,
+/*008e*/ 0x000c0438U,
+/*008f*/ 0x100c0438U,
+/*0090*/ 0x000c0439U,
+/*0091*/ 0x100b0439U,
+/*0092*/ 0xffffffffU,
+/*0093*/ 0xffffffffU,
+/*0094*/ 0x000b043aU,
+/*0095*/ 0x100b043aU,
+/*0096*/ 0x000b043bU,
+/*0097*/ 0x100b043bU,
+/*0098*/ 0x000b043cU,
+/*0099*/ 0x100b043cU,
+/*009a*/ 0x000b043dU,
+/*009b*/ 0x100b043dU,
+/*009c*/ 0x000b043eU,
+/*009d*/ 0x100a043eU,
+/*009e*/ 0xffffffffU,
+/*009f*/ 0x000a043fU,
+/*00a0*/ 0x100a043fU,
+/*00a1*/ 0x000a0440U,
+/*00a2*/ 0x100a0440U,
+/*00a3*/ 0x000a0441U,
+/*00a4*/ 0x100a0441U,
+/*00a5*/ 0x000a0442U,
+/*00a6*/ 0x100a0442U,
+/*00a7*/ 0xffffffffU,
+/*00a8*/ 0x000a0443U,
+/*00a9*/ 0x100a0443U,
+/*00aa*/ 0x000a0444U,
+/*00ab*/ 0x100a0444U,
+/*00ac*/ 0x000a0445U,
+/*00ad*/ 0x100a0445U,
+/*00ae*/ 0x000a0446U,
+/*00af*/ 0x100a0446U,
+/*00b0*/ 0x000a0447U,
+/*00b1*/ 0x100a0447U,
+/*00b2*/ 0x000a0448U,
+/*00b3*/ 0x100a0448U,
+/*00b4*/ 0x000a0449U,
+/*00b5*/ 0x100a0449U,
+/*00b6*/ 0x000a044aU,
+/*00b7*/ 0x100a044aU,
+/*00b8*/ 0x000a044bU,
+/*00b9*/ 0x100a044bU,
+/*00ba*/ 0x000a044cU,
+/*00bb*/ 0x1004044cU,
+/*00bc*/ 0x1803044cU,
+/*00bd*/ 0x000a044dU,
+/*00be*/ 0x100a044dU,
+/*00bf*/ 0x0001044eU,
+/*00c0*/ 0x080a044eU,
+/*00c1*/ 0x1804044eU,
+/*00c2*/ 0x000b044fU,
+/*00c3*/ 0x100a044fU,
+/*00c4*/ 0xffffffffU,
+/*00c5*/ 0x00080450U,
+/*00c6*/ 0x08080450U,
+/*00c7*/ 0x10080450U,
+/*00c8*/ 0x18080450U,
+/*00c9*/ 0x00080451U,
+/*00ca*/ 0xffffffffU,
+/*00cb*/ 0x08080451U,
+/*00cc*/ 0x10010451U,
+/*00cd*/ 0x18080451U,
+/*00ce*/ 0x00080452U,
+/*00cf*/ 0x08020452U,
+/*00d0*/ 0x10020452U,
+/*00d1*/ 0x18040452U,
+/*00d2*/ 0x00040453U,
+/*00d3*/ 0xffffffffU,
+/*00d4*/ 0x08040453U,
+/*00d5*/ 0x100a0453U,
+/*00d6*/ 0x00060454U,
+/*00d7*/ 0x08080454U,
+/*00d8*/ 0xffffffffU,
+/*00d9*/ 0x10040454U,
+/*00da*/ 0x18040454U,
+/*00db*/ 0x00050455U,
+/*00dc*/ 0x08040455U,
+/*00dd*/ 0x10050455U,
+/*00de*/ 0x000a0456U,
+/*00df*/ 0x100a0456U,
+/*00e0*/ 0x00080457U,
+/*00e1*/ 0xffffffffU,
+/*00e2*/ 0x08040457U,
+/*00e3*/ 0xffffffffU,
+/*00e4*/ 0xffffffffU,
+/*00e5*/ 0x00050600U,
+/*00e6*/ 0x08050600U,
+/*00e7*/ 0x10050600U,
+/*00e8*/ 0x18050600U,
+/*00e9*/ 0x00050601U,
+/*00ea*/ 0x08050601U,
+/*00eb*/ 0x100b0601U,
+/*00ec*/ 0x00010602U,
+/*00ed*/ 0x08030602U,
+/*00ee*/ 0x00200603U,
+/*00ef*/ 0xffffffffU,
+/*00f0*/ 0x00030604U,
+/*00f1*/ 0x080a0604U,
+/*00f2*/ 0xffffffffU,
+/*00f3*/ 0xffffffffU,
+/*00f4*/ 0x18030604U,
+/*00f5*/ 0x00030605U,
+/*00f6*/ 0x08010605U,
+/*00f7*/ 0x10010605U,
+/*00f8*/ 0x18060605U,
+/*00f9*/ 0xffffffffU,
+/*00fa*/ 0xffffffffU,
+/*00fb*/ 0xffffffffU,
+/*00fc*/ 0x00020606U,
+/*00fd*/ 0x08030606U,
+/*00fe*/ 0x10010606U,
+/*00ff*/ 0x000f0607U,
+/*0100*/ 0x00200608U,
+/*0101*/ 0x00200609U,
+/*0102*/ 0x000b060aU,
+/*0103*/ 0x100b060aU,
+/*0104*/ 0x000b060bU,
+/*0105*/ 0xffffffffU,
+/*0106*/ 0xffffffffU,
+/*0107*/ 0x0018060cU,
+/*0108*/ 0x0018060dU,
+/*0109*/ 0x0018060eU,
+/*010a*/ 0x0018060fU,
+/*010b*/ 0x1804060fU,
+/*010c*/ 0x00050610U,
+/*010d*/ 0x08020610U,
+/*010e*/ 0x10040610U,
+/*010f*/ 0x18040610U,
+/*0110*/ 0x00010611U,
+/*0111*/ 0x08010611U,
+/*0112*/ 0x10010611U,
+/*0113*/ 0x18030611U,
+/*0114*/ 0x00200612U,
+/*0115*/ 0x00200613U,
+/*0116*/ 0x00010614U,
+/*0117*/ 0x08140614U,
+/*0118*/ 0x00140615U,
+/*0119*/ 0x00140616U,
+/*011a*/ 0x00140617U,
+/*011b*/ 0x00140618U,
+/*011c*/ 0x00140619U,
+/*011d*/ 0x0014061aU,
+/*011e*/ 0x0014061bU,
+/*011f*/ 0x0018061cU,
+/*0120*/ 0x000a061dU,
+/*0121*/ 0x1006061dU,
+/*0122*/ 0x1806061dU,
+/*0123*/ 0x0006061eU,
+/*0124*/ 0xffffffffU,
+/*0125*/ 0xffffffffU,
+/*0126*/ 0x0008061fU,
+/*0127*/ 0x080b061fU,
+/*0128*/ 0x000b0620U,
+/*0129*/ 0x100b0620U,
+/*012a*/ 0x000b0621U,
+/*012b*/ 0x100b0621U,
+/*012c*/ 0x000b0622U,
+/*012d*/ 0x10040622U,
+/*012e*/ 0x000a0623U,
+/*012f*/ 0x10060623U,
+/*0130*/ 0x18080623U,
+/*0131*/ 0xffffffffU,
+/*0132*/ 0x00040624U,
+/*0133*/ 0xffffffffU,
+/*0134*/ 0xffffffffU,
+/*0135*/ 0x00010700U,
+/*0136*/ 0x08020700U,
+/*0137*/ 0x10050700U,
+/*0138*/ 0x18050700U,
+/*0139*/ 0x00050701U,
+/*013a*/ 0x08050701U,
+/*013b*/ 0x100b0701U,
+/*013c*/ 0x00050702U,
+/*013d*/ 0x08010702U,
+/*013e*/ 0x10010702U,
+/*013f*/ 0xffffffffU,
+/*0140*/ 0x18010702U,
+/*0141*/ 0x00010703U,
+/*0142*/ 0x08040703U,
+/*0143*/ 0x100b0703U,
+/*0144*/ 0x000b0704U,
+/*0145*/ 0xffffffffU,
+/*0146*/ 0x10040704U,
+/*0147*/ 0x000b0705U,
+/*0148*/ 0x10040705U,
+/*0149*/ 0x18010705U,
+/*014a*/ 0x00010706U,
+/*014b*/ 0x08010706U,
+/*014c*/ 0x00200707U,
+/*014d*/ 0x00200708U,
+/*014e*/ 0x00080709U,
+/*014f*/ 0x080a0709U,
+/*0150*/ 0x18050709U,
+/*0151*/ 0x000a070aU,
+/*0152*/ 0x1003070aU,
+/*0153*/ 0x1803070aU,
+/*0154*/ 0x0001070bU,
+/*0155*/ 0x0802070bU,
+/*0156*/ 0x1001070bU,
+/*0157*/ 0x1801070bU,
+/*0158*/ 0x0001070cU,
+/*0159*/ 0x0802070cU,
+/*015a*/ 0xffffffffU,
+/*015b*/ 0xffffffffU,
+/*015c*/ 0xffffffffU,
+/*015d*/ 0xffffffffU,
+/*015e*/ 0xffffffffU,
+/*015f*/ 0xffffffffU,
+/*0160*/ 0xffffffffU,
+/*0161*/ 0xffffffffU,
+/*0162*/ 0xffffffffU,
+/*0163*/ 0xffffffffU,
+/*0164*/ 0xffffffffU,
+/*0165*/ 0xffffffffU,
+/*0166*/ 0x1001070cU,
+/*0167*/ 0x1801070cU,
+/*0168*/ 0x000d070dU,
+/*0169*/ 0xffffffffU,
+/*016a*/ 0xffffffffU,
+/*016b*/ 0x0005070eU,
+/*016c*/ 0x0001070fU,
+/*016d*/ 0x080e070fU,
+/*016e*/ 0x000e0710U,
+/*016f*/ 0x100e0710U,
+/*0170*/ 0x000e0711U,
+/*0171*/ 0x100e0711U,
+/*0172*/ 0x00040712U,
+/*0173*/ 0xffffffffU,
+/*0174*/ 0xffffffffU,
+/*0175*/ 0xffffffffU,
+/*0176*/ 0xffffffffU,
+/*0177*/ 0x080b0712U,
+/*0178*/ 0x000b0713U,
+/*0179*/ 0x100b0713U,
+/*017a*/ 0x000b0714U,
+/*017b*/ 0xffffffffU,
+/*017c*/ 0xffffffffU,
+/*017d*/ 0xffffffffU,
+/*017e*/ 0xffffffffU,
+/*017f*/ 0x000d0715U,
+/*0180*/ 0xffffffffU,
+/*0181*/ 0xffffffffU,
+/*0182*/ 0x10100715U,
+/*0183*/ 0x00080716U,
+/*0184*/ 0xffffffffU,
+/*0185*/ 0x08100716U,
+/*0186*/ 0x00100717U,
+/*0187*/ 0x10100717U,
+/*0188*/ 0x00100718U,
+/*0189*/ 0x10100718U,
+/*018a*/ 0x00030719U,
+/*018b*/ 0x08040719U,
+/*018c*/ 0x10010719U,
+/*018d*/ 0x18040719U,
+/*018e*/ 0xffffffffU,
+/*018f*/ 0xffffffffU,
+/*0190*/ 0x0001071aU,
+/*0191*/ 0x0812071aU,
+/*0192*/ 0x000a071bU,
+/*0193*/ 0x100c071bU,
+/*0194*/ 0x0012071cU,
+/*0195*/ 0x0014071dU,
+/*0196*/ 0x0012071eU,
+/*0197*/ 0x0011071fU,
+/*0198*/ 0x00110720U,
+/*0199*/ 0x00120721U,
+/*019a*/ 0x00120722U,
+/*019b*/ 0x00120723U,
+/*019c*/ 0x00120724U,
+/*019d*/ 0x00120725U,
+/*019e*/ 0x00120726U,
+/*019f*/ 0x00120727U,
+/*01a0*/ 0x00120728U,
+/*01a1*/ 0xffffffffU,
+/*01a2*/ 0xffffffffU,
+/*01a3*/ 0x00190729U,
+/*01a4*/ 0x0019072aU,
+/*01a5*/ 0x0020072bU,
+/*01a6*/ 0x0017072cU,
+/*01a7*/ 0x1808072cU,
+/*01a8*/ 0x0001072dU,
+/*01a9*/ 0x0801072dU,
+/*01aa*/ 0x0020072eU,
+/*01ab*/ 0x0008072fU,
+/*01ac*/ 0xffffffffU,
+/*01ad*/ 0x0803072fU,
+/*01ae*/ 0x00180730U,
+/*01af*/ 0x00180731U,
+/*01b0*/ 0xffffffffU,
+/*01b1*/ 0xffffffffU,
+/*01b2*/ 0xffffffffU,
+/*01b3*/ 0xffffffffU,
+/*01b4*/ 0xffffffffU,
+/*01b5*/ 0xffffffffU,
+/*01b6*/ 0xffffffffU,
+/*01b7*/ 0xffffffffU,
+/*01b8*/ 0xffffffffU,
+/*01b9*/ 0xffffffffU,
+/*01ba*/ 0xffffffffU,
+/*01bb*/ 0xffffffffU,
+/*01bc*/ 0xffffffffU,
+/*01bd*/ 0xffffffffU,
+/*01be*/ 0xffffffffU,
+/*01bf*/ 0x00100732U,
+/*01c0*/ 0x10010732U,
+/*01c1*/ 0x18010732U,
+/*01c2*/ 0x00050733U,
+/*01c3*/ 0x00200734U,
+/*01c4*/ 0x00090735U,
+/*01c5*/ 0xffffffffU,
+/*01c6*/ 0xffffffffU,
+/*01c7*/ 0x00200736U,
+/*01c8*/ 0x00040737U,
+/*01c9*/ 0x08100737U,
+/*01ca*/ 0x18060737U,
+/*01cb*/ 0x00100738U,
+/*01cc*/ 0xffffffffU,
+/*01cd*/ 0xffffffffU,
+/*01ce*/ 0xffffffffU,
+/*01cf*/ 0xffffffffU,
+/*01d0*/ 0xffffffffU,
+/*01d1*/ 0xffffffffU,
+/*01d2*/ 0xffffffffU,
+/*01d3*/ 0xffffffffU,
+/*01d4*/ 0x00200739U,
+/*01d5*/ 0x000b073aU,
+/*01d6*/ 0xffffffffU,
+/*01d7*/ 0xffffffffU,
+/*01d8*/ 0xffffffffU,
+/*01d9*/ 0xffffffffU,
+/*01da*/ 0xffffffffU,
+/*01db*/ 0xffffffffU,
+/*01dc*/ 0xffffffffU,
+/*01dd*/ 0xffffffffU,
+/*01de*/ 0x00010200U,
+/*01df*/ 0x08040200U,
+/*01e0*/ 0x10100200U,
+/*01e1*/ 0x00010201U,
+/*01e2*/ 0x08010201U,
+/*01e3*/ 0xffffffffU,
+/*01e4*/ 0xffffffffU,
+/*01e5*/ 0x10100201U,
+/*01e6*/ 0xffffffffU,
+/*01e7*/ 0xffffffffU,
+/*01e8*/ 0xffffffffU,
+/*01e9*/ 0xffffffffU,
+/*01ea*/ 0xffffffffU,
+/*01eb*/ 0xffffffffU,
+/*01ec*/ 0xffffffffU,
+/*01ed*/ 0xffffffffU,
+/*01ee*/ 0xffffffffU,
+/*01ef*/ 0x00200202U,
+/*01f0*/ 0x00100203U,
+/*01f1*/ 0x00200204U,
+/*01f2*/ 0x00100205U,
+/*01f3*/ 0x00200206U,
+/*01f4*/ 0x00100207U,
+/*01f5*/ 0x10100207U,
+/*01f6*/ 0x00200208U,
+/*01f7*/ 0x00200209U,
+/*01f8*/ 0x0020020aU,
+/*01f9*/ 0x0020020bU,
+/*01fa*/ 0x0010020cU,
+/*01fb*/ 0x0020020dU,
+/*01fc*/ 0x0020020eU,
+/*01fd*/ 0x0020020fU,
+/*01fe*/ 0x00200210U,
+/*01ff*/ 0x00100211U,
+/*0200*/ 0x00200212U,
+/*0201*/ 0x00200213U,
+/*0202*/ 0x00200214U,
+/*0203*/ 0x00200215U,
+/*0204*/ 0x00090216U,
+/*0205*/ 0x10010216U,
+/*0206*/ 0x00200217U,
+/*0207*/ 0x00050218U,
+/*0208*/ 0x08010218U,
+/*0209*/ 0x10080218U,
+/*020a*/ 0x18080218U,
+/*020b*/ 0x001c0219U,
+/*020c*/ 0x001c021aU,
+/*020d*/ 0x001c021bU,
+/*020e*/ 0x001c021cU,
+/*020f*/ 0x001c021dU,
+/*0210*/ 0x001c021eU,
+/*0211*/ 0x001c021fU,
+/*0212*/ 0x001c0220U,
+/*0213*/ 0x001c0221U,
+/*0214*/ 0x001c0222U,
+/*0215*/ 0x001c0223U,
+/*0216*/ 0x001c0224U,
+/*0217*/ 0x001c0225U,
+/*0218*/ 0x001c0226U,
+/*0219*/ 0x001c0227U,
+/*021a*/ 0x001c0228U,
+/*021b*/ 0x00010229U,
+/*021c*/ 0x08010229U,
+/*021d*/ 0x10010229U,
+/*021e*/ 0x18040229U,
+/*021f*/ 0x0008022aU,
+/*0220*/ 0x0808022aU,
+/*0221*/ 0x1008022aU,
+/*0222*/ 0x1804022aU,
+/*0223*/ 0x0006022bU,
+/*0224*/ 0xffffffffU,
+/*0225*/ 0x0807022bU,
+/*0226*/ 0x1006022bU,
+/*0227*/ 0xffffffffU,
+/*0228*/ 0x1807022bU,
+/*0229*/ 0x0006022cU,
+/*022a*/ 0xffffffffU,
+/*022b*/ 0x0807022cU,
+/*022c*/ 0x1002022cU,
+/*022d*/ 0x1801022cU,
+/*022e*/ 0xffffffffU,
+/*022f*/ 0x000a022dU,
+/*0230*/ 0x1010022dU,
+/*0231*/ 0x000a022eU,
+/*0232*/ 0x1010022eU,
+/*0233*/ 0x000a022fU,
+/*0234*/ 0x1010022fU,
+/*0235*/ 0xffffffffU,
+/*0236*/ 0x00100230U,
+/*0237*/ 0xffffffffU,
+/*0238*/ 0xffffffffU,
+/*0239*/ 0x10010230U,
+/*023a*/ 0x18010230U,
+/*023b*/ 0x00010231U,
+/*023c*/ 0x08010231U,
+/*023d*/ 0x10010231U,
+/*023e*/ 0x18010231U,
+/*023f*/ 0x00020232U,
+/*0240*/ 0x08020232U,
+/*0241*/ 0x10020232U,
+/*0242*/ 0x18020232U,
+/*0243*/ 0x00020233U,
+/*0244*/ 0x08030233U,
+/*0245*/ 0x10010233U,
+/*0246*/ 0x18010233U,
+/*0247*/ 0x00010234U,
+/*0248*/ 0x08010234U,
+/*0249*/ 0xffffffffU,
+/*024a*/ 0x10020234U,
+/*024b*/ 0x18010234U,
+/*024c*/ 0x00010235U,
+/*024d*/ 0xffffffffU,
+/*024e*/ 0x08020235U,
+/*024f*/ 0x10010235U,
+/*0250*/ 0x18010235U,
+/*0251*/ 0xffffffffU,
+/*0252*/ 0x00020236U,
+/*0253*/ 0x08010236U,
+/*0254*/ 0x10010236U,
+/*0255*/ 0xffffffffU,
+/*0256*/ 0x18020236U,
+/*0257*/ 0x00070237U,
+/*0258*/ 0x08010237U,
+/*0259*/ 0x10010237U,
+/*025a*/ 0x18010237U,
+/*025b*/ 0x00010238U,
+/*025c*/ 0x08010238U,
+/*025d*/ 0x10010238U,
+/*025e*/ 0xffffffffU,
+/*025f*/ 0x18010238U,
+/*0260*/ 0x00040239U,
+/*0261*/ 0x08040239U,
+/*0262*/ 0x10040239U,
+/*0263*/ 0x18010239U,
+/*0264*/ 0x0002023aU,
+/*0265*/ 0x0806023aU,
+/*0266*/ 0x1006023aU,
+/*0267*/ 0xffffffffU,
+/*0268*/ 0xffffffffU,
+/*0269*/ 0xffffffffU,
+/*026a*/ 0x1802023aU,
+/*026b*/ 0x0010023bU,
+/*026c*/ 0x1001023bU,
+/*026d*/ 0x1801023bU,
+/*026e*/ 0xffffffffU,
+/*026f*/ 0x0004023cU,
+/*0270*/ 0x0801023cU,
+/*0271*/ 0x1004023cU,
+/*0272*/ 0x1802023cU,
+/*0273*/ 0x0008023dU,
+/*0274*/ 0xffffffffU,
+/*0275*/ 0xffffffffU,
+/*0276*/ 0xffffffffU,
+/*0277*/ 0x080a023dU,
+/*0278*/ 0x0020023eU,
+/*0279*/ 0x0020023fU,
+/*027a*/ 0x00050240U,
+/*027b*/ 0x08010240U,
+/*027c*/ 0x10050240U,
+/*027d*/ 0x18080240U,
+/*027e*/ 0x00010241U,
+/*027f*/ 0x08080241U,
+/*0280*/ 0x10010241U,
+/*0281*/ 0x18080241U,
+/*0282*/ 0x00010242U,
+/*0283*/ 0x08040242U,
+/*0284*/ 0x10040242U,
+/*0285*/ 0x18040242U,
+/*0286*/ 0x00040243U,
+/*0287*/ 0x08040243U,
+/*0288*/ 0x10040243U,
+/*0289*/ 0x18040243U,
+/*028a*/ 0x00040244U,
+/*028b*/ 0x08040244U,
+/*028c*/ 0x10040244U,
+/*028d*/ 0x18010244U,
+/*028e*/ 0x00040245U,
+/*028f*/ 0x08040245U,
+/*0290*/ 0x10040245U,
+/*0291*/ 0x18040245U,
+/*0292*/ 0x00040246U,
+/*0293*/ 0x08040246U,
+/*0294*/ 0x10060246U,
+/*0295*/ 0x18060246U,
+/*0296*/ 0x00060247U,
+/*0297*/ 0x08060247U,
+/*0298*/ 0x10060247U,
+/*0299*/ 0x18060247U,
+/*029a*/ 0xffffffffU,
+/*029b*/ 0x00010248U,
+/*029c*/ 0x08010248U,
+/*029d*/ 0x10020248U,
+/*029e*/ 0xffffffffU,
+/*029f*/ 0xffffffffU,
+/*02a0*/ 0xffffffffU,
+/*02a1*/ 0xffffffffU,
+/*02a2*/ 0xffffffffU,
+/*02a3*/ 0xffffffffU,
+/*02a4*/ 0xffffffffU,
+/*02a5*/ 0xffffffffU,
+/*02a6*/ 0x18040248U,
+/*02a7*/ 0x00040249U,
+/*02a8*/ 0x08010249U,
+/*02a9*/ 0x10010249U,
+/*02aa*/ 0xffffffffU,
+/*02ab*/ 0x18010249U,
+/*02ac*/ 0x0001024aU,
+/*02ad*/ 0xffffffffU,
+/*02ae*/ 0x0801024aU,
+/*02af*/ 0x1001024aU,
+/*02b0*/ 0x1801024aU,
+/*02b1*/ 0x0004024bU,
+/*02b2*/ 0x0804024bU,
+/*02b3*/ 0x100a024bU,
+/*02b4*/ 0x0020024cU,
+/*02b5*/ 0x0004024dU,
+/*02b6*/ 0x0808024dU,
+/*02b7*/ 0xffffffffU,
+/*02b8*/ 0xffffffffU,
+/*02b9*/ 0xffffffffU,
+/*02ba*/ 0xffffffffU,
+/*02bb*/ 0xffffffffU,
+/*02bc*/ 0xffffffffU,
+/*02bd*/ 0x1002024dU,
+/*02be*/ 0x1802024dU,
+/*02bf*/ 0x0020024eU,
+/*02c0*/ 0x0002024fU,
+/*02c1*/ 0x0810024fU,
+/*02c2*/ 0x00100250U,
+/*02c3*/ 0x10040250U,
+/*02c4*/ 0x18040250U,
+/*02c5*/ 0x00050251U,
+/*02c6*/ 0x08050251U,
+/*02c7*/ 0xffffffffU,
+/*02c8*/ 0xffffffffU,
+/*02c9*/ 0xffffffffU,
+/*02ca*/ 0xffffffffU,
+/*02cb*/ 0x10010251U,
+/*02cc*/ 0x18010251U,
+/*02cd*/ 0x00070252U,
+/*02ce*/ 0x08070252U,
+/*02cf*/ 0x10070252U,
+/*02d0*/ 0x18070252U,
+/*02d1*/ 0x00070253U,
+/*02d2*/ 0x08070253U,
+/*02d3*/ 0x10070253U,
+/*02d4*/ 0x18070253U,
+/*02d5*/ 0x00070254U,
+/*02d6*/ 0x08070254U,
+/*02d7*/ 0x10070254U,
+/*02d8*/ 0xffffffffU,
+/*02d9*/ 0xffffffffU,
+/*02da*/ 0xffffffffU,
+/*02db*/ 0xffffffffU,
+/*02dc*/ 0xffffffffU,
+/*02dd*/ 0xffffffffU,
+/*02de*/ 0x18030254U,
+/*02df*/ 0x00010255U,
+/*02e0*/ 0x08020255U,
+/*02e1*/ 0x10010255U,
+/*02e2*/ 0x18040255U,
+/*02e3*/ 0x00020256U,
+/*02e4*/ 0x08010256U,
+/*02e5*/ 0x10010256U,
+/*02e6*/ 0xffffffffU,
+/*02e7*/ 0x18010256U,
+/*02e8*/ 0x00040257U,
+/*02e9*/ 0x08080257U,
+/*02ea*/ 0x100a0257U,
+/*02eb*/ 0x000a0258U,
+/*02ec*/ 0x100a0258U,
+/*02ed*/ 0x000a0259U,
+/*02ee*/ 0x100a0259U,
+/*02ef*/ 0x000a025aU,
+/*02f0*/ 0x0020025bU,
+/*02f1*/ 0x0020025cU,
+/*02f2*/ 0x0001025dU,
+/*02f3*/ 0xffffffffU,
+/*02f4*/ 0xffffffffU,
+/*02f5*/ 0xffffffffU,
+/*02f6*/ 0x0802025dU,
+/*02f7*/ 0x1002025dU,
+/*02f8*/ 0x0010025eU,
+/*02f9*/ 0x1005025eU,
+/*02fa*/ 0x1806025eU,
+/*02fb*/ 0x0005025fU,
+/*02fc*/ 0x0805025fU,
+/*02fd*/ 0x100e025fU,
+/*02fe*/ 0x00050260U,
+/*02ff*/ 0x080e0260U,
+/*0300*/ 0x18050260U,
+/*0301*/ 0x000e0261U,
+/*0302*/ 0x10050261U,
+/*0303*/ 0x18010261U,
+/*0304*/ 0x00050262U,
+/*0305*/ 0x08050262U,
+/*0306*/ 0x100a0262U,
+/*0307*/ 0x000a0263U,
+/*0308*/ 0x10050263U,
+/*0309*/ 0x18050263U,
+/*030a*/ 0x000a0264U,
+/*030b*/ 0x100a0264U,
+/*030c*/ 0x00050265U,
+/*030d*/ 0x08050265U,
+/*030e*/ 0x100a0265U,
+/*030f*/ 0x000a0266U,
+/*0310*/ 0xffffffffU,
+/*0311*/ 0xffffffffU,
+/*0312*/ 0xffffffffU,
+/*0313*/ 0xffffffffU,
+/*0314*/ 0xffffffffU,
+/*0315*/ 0xffffffffU,
+/*0316*/ 0x10070266U,
+/*0317*/ 0x18070266U,
+/*0318*/ 0x00040267U,
+/*0319*/ 0x08040267U,
+/*031a*/ 0xffffffffU,
+/*031b*/ 0xffffffffU,
+/*031c*/ 0xffffffffU,
+/*031d*/ 0x10040267U,
+/*031e*/ 0x18080267U,
+/*031f*/ 0x00080268U,
+/*0320*/ 0x08040268U,
+/*0321*/ 0xffffffffU,
+/*0322*/ 0xffffffffU,
+/*0323*/ 0xffffffffU,
+/*0324*/ 0x10040268U,
+/*0325*/ 0xffffffffU,
+/*0326*/ 0xffffffffU,
+/*0327*/ 0xffffffffU,
+/*0328*/ 0x18040268U,
+/*0329*/ 0xffffffffU,
+/*032a*/ 0xffffffffU,
+/*032b*/ 0xffffffffU,
+/*032c*/ 0x00040269U,
+/*032d*/ 0x08050269U,
+/*032e*/ 0x10070269U,
+/*032f*/ 0x18080269U,
+/*0330*/ 0x0010026aU,
+/*0331*/ 0x1008026aU,
+/*0332*/ 0x0010026bU,
+/*0333*/ 0x1008026bU,
+/*0334*/ 0x0010026cU,
+/*0335*/ 0x1008026cU,
+/*0336*/ 0x1808026cU,
+/*0337*/ 0x0001026dU,
+/*0338*/ 0x0801026dU,
+/*0339*/ 0x1006026dU,
+/*033a*/ 0x1806026dU,
+/*033b*/ 0x0006026eU,
+/*033c*/ 0xffffffffU,
+/*033d*/ 0x0801026eU,
+/*033e*/ 0x1003026eU,
+/*033f*/ 0xffffffffU,
+/*0340*/ 0xffffffffU,
+/*0341*/ 0xffffffffU,
+/*0342*/ 0x000a026fU,
+/*0343*/ 0x100a026fU,
+/*0344*/ 0x00040270U,
+/*0345*/ 0x08010270U,
+/*0346*/ 0x10040270U,
+/*0347*/ 0xffffffffU,
+/*0348*/ 0xffffffffU,
+/*0349*/ 0xffffffffU,
+/*034a*/ 0xffffffffU,
+/*034b*/ 0xffffffffU,
+/*034c*/ 0xffffffffU,
+/*034d*/ 0x18070270U,
+/*034e*/ 0x00070271U,
+/*034f*/ 0x08050271U,
+/*0350*/ 0x10050271U,
+/*0351*/ 0xffffffffU,
+/*0352*/ 0xffffffffU,
+/*0353*/ 0xffffffffU,
+/*0354*/ 0x18040271U,
+/*0355*/ 0x00010272U,
+/*0356*/ 0x08010272U,
+/*0357*/ 0x10020272U,
+/*0358*/ 0x18080272U,
+/*0359*/ 0x00200273U,
+/*035a*/ 0x00200274U,
+/*035b*/ 0x00100275U,
+/*035c*/ 0xffffffffU,
+/*035d*/ 0xffffffffU,
+/*035e*/ 0xffffffffU,
+/*035f*/ 0x10020275U,
+/*0360*/ 0x18010275U,
+/*0361*/ 0xffffffffU,
+/*0362*/ 0x00020276U,
+/*0363*/ 0x08080276U,
+/*0364*/ 0x10080276U,
+/*0365*/ 0x18080276U,
+/*0366*/ 0x00080277U,
+/*0367*/ 0x08080277U,
+/*0368*/ 0x10080277U,
+/*0369*/ 0xffffffffU,
+/*036a*/ 0x18080277U,
+/*036b*/ 0x00080278U,
+/*036c*/ 0x08080278U,
+/*036d*/ 0x10080278U,
+/*036e*/ 0x18080278U,
+/*036f*/ 0x00080279U,
+/*0370*/ 0xffffffffU,
+/*0371*/ 0x08080279U,
+/*0372*/ 0x10080279U,
+/*0373*/ 0x18080279U,
+/*0374*/ 0x0008027aU,
+/*0375*/ 0x0808027aU,
+/*0376*/ 0x1008027aU,
+/*0377*/ 0xffffffffU,
+/*0378*/ 0x1808027aU,
+/*0379*/ 0x0008027bU,
+/*037a*/ 0x0808027bU,
+/*037b*/ 0x1008027bU,
+/*037c*/ 0x1808027bU,
+/*037d*/ 0x0008027cU,
+/*037e*/ 0x0808027cU,
+/*037f*/ 0xffffffffU,
+/*0380*/ 0x1008027cU,
+/*0381*/ 0x1808027cU,
+/*0382*/ 0x0008027dU,
+/*0383*/ 0x0808027dU,
+/*0384*/ 0x1008027dU,
+/*0385*/ 0x1808027dU,
+/*0386*/ 0xffffffffU,
+/*0387*/ 0x0008027eU,
+/*0388*/ 0x0808027eU,
+/*0389*/ 0x1008027eU,
+/*038a*/ 0x1808027eU,
+/*038b*/ 0x0008027fU,
+/*038c*/ 0x0808027fU,
+/*038d*/ 0xffffffffU,
+/*038e*/ 0x1008027fU,
+/*038f*/ 0x1808027fU,
+/*0390*/ 0x00080280U,
+/*0391*/ 0x08080280U,
+/*0392*/ 0x10080280U,
+/*0393*/ 0x18080280U,
+/*0394*/ 0x00080281U,
+/*0395*/ 0xffffffffU,
+/*0396*/ 0x08080281U,
+/*0397*/ 0x10080281U,
+/*0398*/ 0x18080281U,
+/*0399*/ 0x00080282U,
+/*039a*/ 0x08080282U,
+/*039b*/ 0x10080282U,
+/*039c*/ 0xffffffffU,
+/*039d*/ 0x18080282U,
+/*039e*/ 0x00080283U,
+/*039f*/ 0x08080283U,
+/*03a0*/ 0x10080283U,
+/*03a1*/ 0x18080283U,
+/*03a2*/ 0x00080284U,
+/*03a3*/ 0xffffffffU,
+/*03a4*/ 0x08080284U,
+/*03a5*/ 0x10080284U,
+/*03a6*/ 0x18080284U,
+/*03a7*/ 0x00080285U,
+/*03a8*/ 0x08080285U,
+/*03a9*/ 0x10080285U,
+/*03aa*/ 0x18080285U,
+/*03ab*/ 0xffffffffU,
+/*03ac*/ 0x00080286U,
+/*03ad*/ 0x08080286U,
+/*03ae*/ 0x10080286U,
+/*03af*/ 0x18080286U,
+/*03b0*/ 0x00080287U,
+/*03b1*/ 0x08080287U,
+/*03b2*/ 0xffffffffU,
+/*03b3*/ 0x10080287U,
+/*03b4*/ 0x18080287U,
+/*03b5*/ 0x00080288U,
+/*03b6*/ 0x08080288U,
+/*03b7*/ 0x10080288U,
+/*03b8*/ 0x18080288U,
+/*03b9*/ 0xffffffffU,
+/*03ba*/ 0x00080289U,
+/*03bb*/ 0x08020289U,
+/*03bc*/ 0x10030289U,
+/*03bd*/ 0x000a028aU,
+/*03be*/ 0x100a028aU,
+/*03bf*/ 0x000a028bU,
+/*03c0*/ 0x1005028bU,
+/*03c1*/ 0x1804028bU,
+/*03c2*/ 0x0008028cU,
+/*03c3*/ 0x0808028cU,
+/*03c4*/ 0x1006028cU,
+/*03c5*/ 0x1806028cU,
+/*03c6*/ 0x0011028dU,
+/*03c7*/ 0x1808028dU,
+/*03c8*/ 0x0004028eU,
+/*03c9*/ 0x0806028eU,
+/*03ca*/ 0xffffffffU,
+/*03cb*/ 0x1006028eU,
+/*03cc*/ 0x1808028eU,
+/*03cd*/ 0xffffffffU,
+/*03ce*/ 0x0004028fU,
+/*03cf*/ 0x0808028fU,
+/*03d0*/ 0x1008028fU,
+/*03d1*/ 0x1806028fU,
+/*03d2*/ 0x00060290U,
+/*03d3*/ 0x08110290U,
+/*03d4*/ 0x00080291U,
+/*03d5*/ 0x08040291U,
+/*03d6*/ 0x10060291U,
+/*03d7*/ 0xffffffffU,
+/*03d8*/ 0x18060291U,
+/*03d9*/ 0x00080292U,
+/*03da*/ 0xffffffffU,
+/*03db*/ 0x08040292U,
+/*03dc*/ 0x10080292U,
+/*03dd*/ 0x18080292U,
+/*03de*/ 0x00060293U,
+/*03df*/ 0x08060293U,
+/*03e0*/ 0x00110294U,
+/*03e1*/ 0x18080294U,
+/*03e2*/ 0x00040295U,
+/*03e3*/ 0x08060295U,
+/*03e4*/ 0xffffffffU,
+/*03e5*/ 0x10060295U,
+/*03e6*/ 0x18080295U,
+/*03e7*/ 0xffffffffU,
+/*03e8*/ 0x00040296U,
+/*03e9*/ 0x08040296U,
+/*03ea*/ 0x10040296U,
+/*03eb*/ 0x18040296U,
+/*03ec*/ 0x00040297U,
+/*03ed*/ 0x08040297U,
+/*03ee*/ 0x10040297U,
+/*03ef*/ 0x18040297U,
+/*03f0*/ 0x00040298U,
+/*03f1*/ 0x08040298U,
+/*03f2*/ 0x10040298U,
+/*03f3*/ 0x18040298U,
+/*03f4*/ 0x00040299U,
+/*03f5*/ 0x08040299U,
+/*03f6*/ 0x10040299U,
+/*03f7*/ 0x18040299U,
+/*03f8*/ 0x0004029aU,
+/*03f9*/ 0x0804029aU,
+/*03fa*/ 0x1004029aU,
+/*03fb*/ 0x1804029aU,
+/*03fc*/ 0x0011029bU,
+/*03fd*/ 0x0010029cU,
+/*03fe*/ 0x0011029dU,
+/*03ff*/ 0x0020029eU,
+/*0400*/ 0x0020029fU,
+/*0401*/ 0x002002a0U,
+/*0402*/ 0x002002a1U,
+/*0403*/ 0x002002a2U,
+/*0404*/ 0x002002a3U,
+/*0405*/ 0x002002a4U,
+/*0406*/ 0x002002a5U,
+/*0407*/ 0x002002a6U,
+/*0408*/ 0x000202a7U,
+/*0409*/ 0x080502a7U,
+/*040a*/ 0x100502a7U,
+/*040b*/ 0xffffffffU,
+/*040c*/ 0xffffffffU,
+/*040d*/ 0xffffffffU,
+/*040e*/ 0xffffffffU,
+/*040f*/ 0xffffffffU,
+/*0410*/ 0xffffffffU,
+/*0411*/ 0xffffffffU,
+/*0412*/ 0xffffffffU,
+/*0413*/ 0xffffffffU,
+/*0414*/ 0xffffffffU,
+/*0415*/ 0xffffffffU,
+/*0416*/ 0xffffffffU,
+/*0417*/ 0xffffffffU,
+/*0418*/ 0xffffffffU,
+/*0419*/ 0xffffffffU,
+/*041a*/ 0xffffffffU,
+/*041b*/ 0xffffffffU,
+/*041c*/ 0xffffffffU,
+/*041d*/ 0xffffffffU,
+/*041e*/ 0xffffffffU,
+/*041f*/ 0xffffffffU,
+/*0420*/ 0xffffffffU,
+/*0421*/ 0xffffffffU,
+/*0422*/ 0xffffffffU,
+/*0423*/ 0xffffffffU,
+/*0424*/ 0xffffffffU,
+/*0425*/ 0xffffffffU,
+/*0426*/ 0xffffffffU,
+/*0427*/ 0x180102a7U,
+/*0428*/ 0x000402a8U,
+/*0429*/ 0x081002a8U,
+/*042a*/ 0x002002a9U,
+/*042b*/ 0x001002aaU,
+/*042c*/ 0x002002abU,
+/*042d*/ 0x001002acU,
+/*042e*/ 0x002002adU,
+/*042f*/ 0x000702aeU,
+/*0430*/ 0x080102aeU,
+/*0431*/ 0x100202aeU,
+/*0432*/ 0x180602aeU,
+/*0433*/ 0x000102afU,
+/*0434*/ 0x080102afU,
+/*0435*/ 0x002002b0U,
+/*0436*/ 0x000202b1U,
+/*0437*/ 0x002002b2U,
+/*0438*/ 0x002002b3U,
+/*0439*/ 0xffffffffU,
+/*043a*/ 0xffffffffU,
+/*043b*/ 0xffffffffU,
+/*043c*/ 0xffffffffU,
+/*043d*/ 0xffffffffU,
+/*043e*/ 0xffffffffU,
+/*043f*/ 0xffffffffU,
+/*0440*/ 0xffffffffU,
+/*0441*/ 0xffffffffU,
+/*0442*/ 0xffffffffU,
+/*0443*/ 0xffffffffU,
+/*0444*/ 0xffffffffU,
+/*0445*/ 0xffffffffU,
+/*0446*/ 0xffffffffU,
+/*0447*/ 0xffffffffU,
+/*0448*/ 0xffffffffU,
+/*0449*/ 0xffffffffU,
+/*044a*/ 0xffffffffU,
+/*044b*/ 0xffffffffU,
+/*044c*/ 0xffffffffU,
+/*044d*/ 0xffffffffU,
+/*044e*/ 0xffffffffU,
+/*044f*/ 0xffffffffU,
+/*0450*/ 0xffffffffU,
+/*0451*/ 0xffffffffU,
+/*0452*/ 0xffffffffU,
+/*0453*/ 0xffffffffU,
+/*0454*/ 0xffffffffU,
+/*0455*/ 0xffffffffU,
+/*0456*/ 0xffffffffU,
+/*0457*/ 0xffffffffU,
+/*0458*/ 0xffffffffU,
+/*0459*/ 0xffffffffU,
+/*045a*/ 0xffffffffU,
+/*045b*/ 0xffffffffU,
+/*045c*/ 0xffffffffU,
+/*045d*/ 0xffffffffU,
+/*045e*/ 0xffffffffU,
+/*045f*/ 0x000402b4U,
+/*0460*/ 0xffffffffU,
+/*0461*/ 0xffffffffU,
+/*0462*/ 0xffffffffU,
+/*0463*/ 0xffffffffU,
+/*0464*/ 0xffffffffU,
+/*0465*/ 0xffffffffU,
+/*0466*/ 0xffffffffU,
+/*0467*/ 0xffffffffU,
+/*0468*/ 0xffffffffU,
+/*0469*/ 0xffffffffU,
+/*046a*/ 0xffffffffU,
+/*046b*/ 0xffffffffU,
+/*046c*/ 0xffffffffU,
+/*046d*/ 0xffffffffU,
+/*046e*/ 0xffffffffU,
+/*046f*/ 0xffffffffU,
+/*0470*/ 0xffffffffU,
+/*0471*/ 0xffffffffU,
+/*0472*/ 0xffffffffU,
+/*0473*/ 0xffffffffU,
+/*0474*/ 0xffffffffU,
+/*0475*/ 0xffffffffU,
+/*0476*/ 0xffffffffU,
+/*0477*/ 0xffffffffU,
+/*0478*/ 0xffffffffU,
+/*0479*/ 0xffffffffU,
+/*047a*/ 0xffffffffU,
+/*047b*/ 0xffffffffU,
+/*047c*/ 0xffffffffU,
+/*047d*/ 0xffffffffU,
+/*047e*/ 0xffffffffU,
+/*047f*/ 0xffffffffU,
+/*0480*/ 0xffffffffU,
+/*0481*/ 0xffffffffU,
+/*0482*/ 0xffffffffU,
+/*0483*/ 0xffffffffU,
+/*0484*/ 0xffffffffU,
+/*0485*/ 0xffffffffU,
+/*0486*/ 0xffffffffU,
+/*0487*/ 0xffffffffU,
+/*0488*/ 0xffffffffU,
+/*0489*/ 0xffffffffU,
+/*048a*/ 0xffffffffU,
+/*048b*/ 0xffffffffU,
+/*048c*/ 0xffffffffU,
+/*048d*/ 0xffffffffU,
+/*048e*/ 0xffffffffU,
+/*048f*/ 0xffffffffU,
+/*0490*/ 0xffffffffU,
+/*0491*/ 0xffffffffU,
+/*0492*/ 0xffffffffU,
+/*0493*/ 0xffffffffU,
+/*0494*/ 0xffffffffU,
+	 },
+	{
+/*0000*/ 0x00200800U,
+/*0001*/ 0x00040801U,
+/*0002*/ 0x080b0801U,
+/*0003*/ 0xffffffffU,
+/*0004*/ 0xffffffffU,
+/*0005*/ 0x18010801U,
+/*0006*/ 0x00050802U,
+/*0007*/ 0x08050802U,
+/*0008*/ 0x10050802U,
+/*0009*/ 0x18050802U,
+/*000a*/ 0x00050803U,
+/*000b*/ 0x08050803U,
+/*000c*/ 0x10050803U,
+/*000d*/ 0x18050803U,
+/*000e*/ 0x00050804U,
+/*000f*/ 0x08040804U,
+/*0010*/ 0x10030804U,
+/*0011*/ 0x00180805U,
+/*0012*/ 0x18030805U,
+/*0013*/ 0x00180806U,
+/*0014*/ 0x18020806U,
+/*0015*/ 0x00010807U,
+/*0016*/ 0x08020807U,
+/*0017*/ 0x10010807U,
+/*0018*/ 0x18010807U,
+/*0019*/ 0x00020808U,
+/*001a*/ 0x08040808U,
+/*001b*/ 0x10040808U,
+/*001c*/ 0x18040808U,
+/*001d*/ 0x000a0809U,
+/*001e*/ 0x10040809U,
+/*001f*/ 0xffffffffU,
+/*0020*/ 0xffffffffU,
+/*0021*/ 0x18070809U,
+/*0022*/ 0xffffffffU,
+/*0023*/ 0xffffffffU,
+/*0024*/ 0xffffffffU,
+/*0025*/ 0xffffffffU,
+/*0026*/ 0xffffffffU,
+/*0027*/ 0xffffffffU,
+/*0028*/ 0x000a080aU,
+/*0029*/ 0x1005080aU,
+/*002a*/ 0x1801080aU,
+/*002b*/ 0x0001080bU,
+/*002c*/ 0x0802080bU,
+/*002d*/ 0x1009080bU,
+/*002e*/ 0x0009080cU,
+/*002f*/ 0x1002080cU,
+/*0030*/ 0x0020080dU,
+/*0031*/ 0xffffffffU,
+/*0032*/ 0x0001080eU,
+/*0033*/ 0xffffffffU,
+/*0034*/ 0xffffffffU,
+/*0035*/ 0xffffffffU,
+/*0036*/ 0xffffffffU,
+/*0037*/ 0x0020080fU,
+/*0038*/ 0x00200810U,
+/*0039*/ 0x00200811U,
+/*003a*/ 0x00200812U,
+/*003b*/ 0x00030813U,
+/*003c*/ 0x08010813U,
+/*003d*/ 0x10030813U,
+/*003e*/ 0x18030813U,
+/*003f*/ 0x00040814U,
+/*0040*/ 0x08040814U,
+/*0041*/ 0x10040814U,
+/*0042*/ 0x18040814U,
+/*0043*/ 0x00010815U,
+/*0044*/ 0x08010815U,
+/*0045*/ 0x10060815U,
+/*0046*/ 0x18040815U,
+/*0047*/ 0xffffffffU,
+/*0048*/ 0x00060816U,
+/*0049*/ 0x08040816U,
+/*004a*/ 0x10060816U,
+/*004b*/ 0x18040816U,
+/*004c*/ 0x00020817U,
+/*004d*/ 0x08050817U,
+/*004e*/ 0x10080817U,
+/*004f*/ 0x00200818U,
+/*0050*/ 0x00060819U,
+/*0051*/ 0x08030819U,
+/*0052*/ 0x100b0819U,
+/*0053*/ 0x0004081aU,
+/*0054*/ 0x0804081aU,
+/*0055*/ 0x1004081aU,
+/*0056*/ 0xffffffffU,
+/*0057*/ 0x1801081aU,
+/*0058*/ 0x0009081bU,
+/*0059*/ 0x0020081cU,
+/*005a*/ 0x0020081dU,
+/*005b*/ 0x0020081eU,
+/*005c*/ 0x0020081fU,
+/*005d*/ 0x00100820U,
+/*005e*/ 0xffffffffU,
+/*005f*/ 0x10010820U,
+/*0060*/ 0x18060820U,
+/*0061*/ 0x00080821U,
+/*0062*/ 0x00200822U,
+/*0063*/ 0xffffffffU,
+/*0064*/ 0x000a0823U,
+/*0065*/ 0x10060823U,
+/*0066*/ 0x18070823U,
+/*0067*/ 0x00080824U,
+/*0068*/ 0x08080824U,
+/*0069*/ 0x100a0824U,
+/*006a*/ 0x00070825U,
+/*006b*/ 0x08080825U,
+/*006c*/ 0x10080825U,
+/*006d*/ 0x18030825U,
+/*006e*/ 0x000a0826U,
+/*006f*/ 0x100a0826U,
+/*0070*/ 0x00110827U,
+/*0071*/ 0x00090828U,
+/*0072*/ 0x10090828U,
+/*0073*/ 0x00100829U,
+/*0074*/ 0x100e0829U,
+/*0075*/ 0x000e082aU,
+/*0076*/ 0x100c082aU,
+/*0077*/ 0x000a082bU,
+/*0078*/ 0x100a082bU,
+/*0079*/ 0x0002082cU,
+/*007a*/ 0x0020082dU,
+/*007b*/ 0x000b082eU,
+/*007c*/ 0x100b082eU,
+/*007d*/ 0x0020082fU,
+/*007e*/ 0x00120830U,
+/*007f*/ 0x00200831U,
+/*0080*/ 0x00200832U,
+/*0081*/ 0xffffffffU,
+/*0082*/ 0xffffffffU,
+/*0083*/ 0x00010833U,
+/*0084*/ 0x08010833U,
+/*0085*/ 0x10080833U,
+/*0086*/ 0x000c0834U,
+/*0087*/ 0x100c0834U,
+/*0088*/ 0x000c0835U,
+/*0089*/ 0x100c0835U,
+/*008a*/ 0x000c0836U,
+/*008b*/ 0x100c0836U,
+/*008c*/ 0x000c0837U,
+/*008d*/ 0x100c0837U,
+/*008e*/ 0x000c0838U,
+/*008f*/ 0x100c0838U,
+/*0090*/ 0x000c0839U,
+/*0091*/ 0x100b0839U,
+/*0092*/ 0xffffffffU,
+/*0093*/ 0xffffffffU,
+/*0094*/ 0x000b083aU,
+/*0095*/ 0x100b083aU,
+/*0096*/ 0x000b083bU,
+/*0097*/ 0x100b083bU,
+/*0098*/ 0x000b083cU,
+/*0099*/ 0x100b083cU,
+/*009a*/ 0x000b083dU,
+/*009b*/ 0x100b083dU,
+/*009c*/ 0x000b083eU,
+/*009d*/ 0x100a083eU,
+/*009e*/ 0xffffffffU,
+/*009f*/ 0x000a083fU,
+/*00a0*/ 0x100a083fU,
+/*00a1*/ 0x000a0840U,
+/*00a2*/ 0x100a0840U,
+/*00a3*/ 0x000a0841U,
+/*00a4*/ 0x100a0841U,
+/*00a5*/ 0x000a0842U,
+/*00a6*/ 0x100a0842U,
+/*00a7*/ 0x000a0843U,
+/*00a8*/ 0x100a0843U,
+/*00a9*/ 0x000a0844U,
+/*00aa*/ 0x100a0844U,
+/*00ab*/ 0x000a0845U,
+/*00ac*/ 0x100a0845U,
+/*00ad*/ 0x000a0846U,
+/*00ae*/ 0x100a0846U,
+/*00af*/ 0x000a0847U,
+/*00b0*/ 0x100a0847U,
+/*00b1*/ 0x000a0848U,
+/*00b2*/ 0x100a0848U,
+/*00b3*/ 0x000a0849U,
+/*00b4*/ 0x100a0849U,
+/*00b5*/ 0x000a084aU,
+/*00b6*/ 0x100a084aU,
+/*00b7*/ 0x000a084bU,
+/*00b8*/ 0x100a084bU,
+/*00b9*/ 0x000a084cU,
+/*00ba*/ 0x100a084cU,
+/*00bb*/ 0x0004084dU,
+/*00bc*/ 0x0803084dU,
+/*00bd*/ 0x100a084dU,
+/*00be*/ 0x000a084eU,
+/*00bf*/ 0x1001084eU,
+/*00c0*/ 0x000a084fU,
+/*00c1*/ 0x1004084fU,
+/*00c2*/ 0x000b0850U,
+/*00c3*/ 0x100a0850U,
+/*00c4*/ 0xffffffffU,
+/*00c5*/ 0x00080851U,
+/*00c6*/ 0x08080851U,
+/*00c7*/ 0x10080851U,
+/*00c8*/ 0x18080851U,
+/*00c9*/ 0x00080852U,
+/*00ca*/ 0xffffffffU,
+/*00cb*/ 0x08080852U,
+/*00cc*/ 0x10010852U,
+/*00cd*/ 0x18080852U,
+/*00ce*/ 0x00080853U,
+/*00cf*/ 0x08020853U,
+/*00d0*/ 0x10020853U,
+/*00d1*/ 0x18040853U,
+/*00d2*/ 0x00040854U,
+/*00d3*/ 0xffffffffU,
+/*00d4*/ 0x08040854U,
+/*00d5*/ 0x100a0854U,
+/*00d6*/ 0x00060855U,
+/*00d7*/ 0x08080855U,
+/*00d8*/ 0xffffffffU,
+/*00d9*/ 0x10040855U,
+/*00da*/ 0x18040855U,
+/*00db*/ 0x00050856U,
+/*00dc*/ 0x08040856U,
+/*00dd*/ 0x10050856U,
+/*00de*/ 0x000a0857U,
+/*00df*/ 0x100a0857U,
+/*00e0*/ 0x00080858U,
+/*00e1*/ 0xffffffffU,
+/*00e2*/ 0x08040858U,
+/*00e3*/ 0xffffffffU,
+/*00e4*/ 0xffffffffU,
+/*00e5*/ 0x00050a00U,
+/*00e6*/ 0x08050a00U,
+/*00e7*/ 0x10050a00U,
+/*00e8*/ 0x18050a00U,
+/*00e9*/ 0x00050a01U,
+/*00ea*/ 0x08050a01U,
+/*00eb*/ 0x100b0a01U,
+/*00ec*/ 0x00010a02U,
+/*00ed*/ 0x08030a02U,
+/*00ee*/ 0x00200a03U,
+/*00ef*/ 0xffffffffU,
+/*00f0*/ 0x00030a04U,
+/*00f1*/ 0x080a0a04U,
+/*00f2*/ 0xffffffffU,
+/*00f3*/ 0xffffffffU,
+/*00f4*/ 0x18030a04U,
+/*00f5*/ 0x00030a05U,
+/*00f6*/ 0x08010a05U,
+/*00f7*/ 0x10010a05U,
+/*00f8*/ 0x18060a05U,
+/*00f9*/ 0xffffffffU,
+/*00fa*/ 0xffffffffU,
+/*00fb*/ 0xffffffffU,
+/*00fc*/ 0x00020a06U,
+/*00fd*/ 0x08030a06U,
+/*00fe*/ 0x10010a06U,
+/*00ff*/ 0x000f0a07U,
+/*0100*/ 0x00200a08U,
+/*0101*/ 0x00200a09U,
+/*0102*/ 0x000b0a0aU,
+/*0103*/ 0x100b0a0aU,
+/*0104*/ 0x000b0a0bU,
+/*0105*/ 0xffffffffU,
+/*0106*/ 0xffffffffU,
+/*0107*/ 0x00180a0cU,
+/*0108*/ 0x00180a0dU,
+/*0109*/ 0x00180a0eU,
+/*010a*/ 0x00180a0fU,
+/*010b*/ 0x18040a0fU,
+/*010c*/ 0x00020a10U,
+/*010d*/ 0x08020a10U,
+/*010e*/ 0x10040a10U,
+/*010f*/ 0x18040a10U,
+/*0110*/ 0x00010a11U,
+/*0111*/ 0x08010a11U,
+/*0112*/ 0x10010a11U,
+/*0113*/ 0x18030a11U,
+/*0114*/ 0x00200a12U,
+/*0115*/ 0x00200a13U,
+/*0116*/ 0xffffffffU,
+/*0117*/ 0x00140a14U,
+/*0118*/ 0x00140a15U,
+/*0119*/ 0x00140a16U,
+/*011a*/ 0x00140a17U,
+/*011b*/ 0x00140a18U,
+/*011c*/ 0x00140a19U,
+/*011d*/ 0x00140a1aU,
+/*011e*/ 0x00140a1bU,
+/*011f*/ 0x001e0a1cU,
+/*0120*/ 0x000a0a1dU,
+/*0121*/ 0x10060a1dU,
+/*0122*/ 0x18060a1dU,
+/*0123*/ 0x00060a1eU,
+/*0124*/ 0xffffffffU,
+/*0125*/ 0x08060a1eU,
+/*0126*/ 0x00080a1fU,
+/*0127*/ 0x080b0a1fU,
+/*0128*/ 0x000b0a20U,
+/*0129*/ 0x100b0a20U,
+/*012a*/ 0x000b0a21U,
+/*012b*/ 0x100b0a21U,
+/*012c*/ 0x000b0a22U,
+/*012d*/ 0x10040a22U,
+/*012e*/ 0x000a0a23U,
+/*012f*/ 0x10060a23U,
+/*0130*/ 0x18080a23U,
+/*0131*/ 0xffffffffU,
+/*0132*/ 0x00040a24U,
+/*0133*/ 0xffffffffU,
+/*0134*/ 0xffffffffU,
+/*0135*/ 0x00010b80U,
+/*0136*/ 0x08020b80U,
+/*0137*/ 0x10050b80U,
+/*0138*/ 0x18050b80U,
+/*0139*/ 0x00050b81U,
+/*013a*/ 0x08050b81U,
+/*013b*/ 0x100b0b81U,
+/*013c*/ 0x00050b82U,
+/*013d*/ 0x08010b82U,
+/*013e*/ 0x10010b82U,
+/*013f*/ 0xffffffffU,
+/*0140*/ 0x18010b82U,
+/*0141*/ 0x00010b83U,
+/*0142*/ 0x08040b83U,
+/*0143*/ 0x100b0b83U,
+/*0144*/ 0x000b0b84U,
+/*0145*/ 0xffffffffU,
+/*0146*/ 0x10040b84U,
+/*0147*/ 0x000b0b85U,
+/*0148*/ 0x10040b85U,
+/*0149*/ 0x18010b85U,
+/*014a*/ 0x00010b86U,
+/*014b*/ 0x08010b86U,
+/*014c*/ 0x00200b87U,
+/*014d*/ 0x00200b88U,
+/*014e*/ 0x00080b89U,
+/*014f*/ 0x080a0b89U,
+/*0150*/ 0x18050b89U,
+/*0151*/ 0x000a0b8aU,
+/*0152*/ 0x10030b8aU,
+/*0153*/ 0x18030b8aU,
+/*0154*/ 0x00010b8bU,
+/*0155*/ 0x08020b8bU,
+/*0156*/ 0x10010b8bU,
+/*0157*/ 0x18010b8bU,
+/*0158*/ 0x00010b8cU,
+/*0159*/ 0x08030b8cU,
+/*015a*/ 0xffffffffU,
+/*015b*/ 0x10040b8cU,
+/*015c*/ 0x18040b8cU,
+/*015d*/ 0x00040b8dU,
+/*015e*/ 0x08040b8dU,
+/*015f*/ 0xffffffffU,
+/*0160*/ 0xffffffffU,
+/*0161*/ 0xffffffffU,
+/*0162*/ 0xffffffffU,
+/*0163*/ 0xffffffffU,
+/*0164*/ 0xffffffffU,
+/*0165*/ 0xffffffffU,
+/*0166*/ 0xffffffffU,
+/*0167*/ 0xffffffffU,
+/*0168*/ 0x000d0b8eU,
+/*0169*/ 0x100d0b8eU,
+/*016a*/ 0x000d0b8fU,
+/*016b*/ 0x00050b90U,
+/*016c*/ 0x00010b91U,
+/*016d*/ 0x080e0b91U,
+/*016e*/ 0x000e0b92U,
+/*016f*/ 0x100e0b92U,
+/*0170*/ 0x000e0b93U,
+/*0171*/ 0x100e0b93U,
+/*0172*/ 0x00040b94U,
+/*0173*/ 0x08040b94U,
+/*0174*/ 0x10040b94U,
+/*0175*/ 0x18040b94U,
+/*0176*/ 0x00040b95U,
+/*0177*/ 0x080b0b95U,
+/*0178*/ 0x000b0b96U,
+/*0179*/ 0x100b0b96U,
+/*017a*/ 0x000b0b97U,
+/*017b*/ 0xffffffffU,
+/*017c*/ 0xffffffffU,
+/*017d*/ 0xffffffffU,
+/*017e*/ 0xffffffffU,
+/*017f*/ 0x000d0b98U,
+/*0180*/ 0x100d0b98U,
+/*0181*/ 0x000d0b99U,
+/*0182*/ 0x10100b99U,
+/*0183*/ 0x10080b8dU,
+/*0184*/ 0x18080b8dU,
+/*0185*/ 0x00100b9aU,
+/*0186*/ 0x10100b9aU,
+/*0187*/ 0x00100b9bU,
+/*0188*/ 0x10100b9bU,
+/*0189*/ 0x00100b9cU,
+/*018a*/ 0x10030b9cU,
+/*018b*/ 0x18040b9cU,
+/*018c*/ 0x00010b9dU,
+/*018d*/ 0x08040b9dU,
+/*018e*/ 0xffffffffU,
+/*018f*/ 0xffffffffU,
+/*0190*/ 0x10010b9dU,
+/*0191*/ 0x00140b9eU,
+/*0192*/ 0x000a0b9fU,
+/*0193*/ 0x100c0b9fU,
+/*0194*/ 0x00120ba0U,
+/*0195*/ 0x00140ba1U,
+/*0196*/ 0x00120ba2U,
+/*0197*/ 0x00110ba3U,
+/*0198*/ 0x00110ba4U,
+/*0199*/ 0x00120ba5U,
+/*019a*/ 0x00120ba6U,
+/*019b*/ 0x00120ba7U,
+/*019c*/ 0x00120ba8U,
+/*019d*/ 0x00120ba9U,
+/*019e*/ 0x00120baaU,
+/*019f*/ 0x00120babU,
+/*01a0*/ 0x00120bacU,
+/*01a1*/ 0xffffffffU,
+/*01a2*/ 0xffffffffU,
+/*01a3*/ 0x00190badU,
+/*01a4*/ 0x00190baeU,
+/*01a5*/ 0x00200bafU,
+/*01a6*/ 0x00170bb0U,
+/*01a7*/ 0x18080bb0U,
+/*01a8*/ 0x00010bb1U,
+/*01a9*/ 0x08010bb1U,
+/*01aa*/ 0x00200bb2U,
+/*01ab*/ 0x00080bb3U,
+/*01ac*/ 0xffffffffU,
+/*01ad*/ 0x08030bb3U,
+/*01ae*/ 0x00180bb4U,
+/*01af*/ 0x00180bb5U,
+/*01b0*/ 0xffffffffU,
+/*01b1*/ 0xffffffffU,
+/*01b2*/ 0xffffffffU,
+/*01b3*/ 0xffffffffU,
+/*01b4*/ 0xffffffffU,
+/*01b5*/ 0xffffffffU,
+/*01b6*/ 0xffffffffU,
+/*01b7*/ 0xffffffffU,
+/*01b8*/ 0xffffffffU,
+/*01b9*/ 0xffffffffU,
+/*01ba*/ 0xffffffffU,
+/*01bb*/ 0xffffffffU,
+/*01bc*/ 0xffffffffU,
+/*01bd*/ 0xffffffffU,
+/*01be*/ 0xffffffffU,
+/*01bf*/ 0x00100bb6U,
+/*01c0*/ 0x10010bb6U,
+/*01c1*/ 0x18010bb6U,
+/*01c2*/ 0x00050bb7U,
+/*01c3*/ 0x00200bb8U,
+/*01c4*/ 0x00090bb9U,
+/*01c5*/ 0xffffffffU,
+/*01c6*/ 0xffffffffU,
+/*01c7*/ 0x00200bbaU,
+/*01c8*/ 0x00040bbbU,
+/*01c9*/ 0x08100bbbU,
+/*01ca*/ 0x18060bbbU,
+/*01cb*/ 0x00100bbcU,
+/*01cc*/ 0xffffffffU,
+/*01cd*/ 0x10080bbcU,
+/*01ce*/ 0xffffffffU,
+/*01cf*/ 0xffffffffU,
+/*01d0*/ 0xffffffffU,
+/*01d1*/ 0x18030bbcU,
+/*01d2*/ 0x00020bbdU,
+/*01d3*/ 0xffffffffU,
+/*01d4*/ 0x00200bbeU,
+/*01d5*/ 0x000b0bbfU,
+/*01d6*/ 0xffffffffU,
+/*01d7*/ 0xffffffffU,
+/*01d8*/ 0xffffffffU,
+/*01d9*/ 0x10020bbfU,
+/*01da*/ 0xffffffffU,
+/*01db*/ 0xffffffffU,
+/*01dc*/ 0xffffffffU,
+/*01dd*/ 0xffffffffU,
+/*01de*/ 0x00010200U,
+/*01df*/ 0x08040200U,
+/*01e0*/ 0x10100200U,
+/*01e1*/ 0x00010201U,
+/*01e2*/ 0x08010201U,
+/*01e3*/ 0xffffffffU,
+/*01e4*/ 0xffffffffU,
+/*01e5*/ 0x10100201U,
+/*01e6*/ 0xffffffffU,
+/*01e7*/ 0xffffffffU,
+/*01e8*/ 0xffffffffU,
+/*01e9*/ 0xffffffffU,
+/*01ea*/ 0xffffffffU,
+/*01eb*/ 0xffffffffU,
+/*01ec*/ 0xffffffffU,
+/*01ed*/ 0xffffffffU,
+/*01ee*/ 0xffffffffU,
+/*01ef*/ 0x00200202U,
+/*01f0*/ 0x00100203U,
+/*01f1*/ 0x00200204U,
+/*01f2*/ 0x00100205U,
+/*01f3*/ 0x00200206U,
+/*01f4*/ 0x00100207U,
+/*01f5*/ 0x10100207U,
+/*01f6*/ 0x00200208U,
+/*01f7*/ 0x00200209U,
+/*01f8*/ 0x0020020aU,
+/*01f9*/ 0x0020020bU,
+/*01fa*/ 0x0010020cU,
+/*01fb*/ 0x0020020dU,
+/*01fc*/ 0x0020020eU,
+/*01fd*/ 0x0020020fU,
+/*01fe*/ 0x00200210U,
+/*01ff*/ 0x00100211U,
+/*0200*/ 0x00200212U,
+/*0201*/ 0x00200213U,
+/*0202*/ 0x00200214U,
+/*0203*/ 0x00200215U,
+/*0204*/ 0x00090216U,
+/*0205*/ 0x10010216U,
+/*0206*/ 0x00200217U,
+/*0207*/ 0x00050218U,
+/*0208*/ 0x08010218U,
+/*0209*/ 0x10080218U,
+/*020a*/ 0x18080218U,
+/*020b*/ 0x001e0219U,
+/*020c*/ 0x001e021aU,
+/*020d*/ 0x001e021bU,
+/*020e*/ 0x001e021cU,
+/*020f*/ 0x001e021dU,
+/*0210*/ 0x001e021eU,
+/*0211*/ 0x001e021fU,
+/*0212*/ 0x001e0220U,
+/*0213*/ 0x001e0221U,
+/*0214*/ 0x001e0222U,
+/*0215*/ 0x001e0223U,
+/*0216*/ 0x001e0224U,
+/*0217*/ 0x001e0225U,
+/*0218*/ 0x001e0226U,
+/*0219*/ 0x001e0227U,
+/*021a*/ 0x001e0228U,
+/*021b*/ 0x00010229U,
+/*021c*/ 0x08010229U,
+/*021d*/ 0x10010229U,
+/*021e*/ 0x18040229U,
+/*021f*/ 0x0008022aU,
+/*0220*/ 0x0808022aU,
+/*0221*/ 0x1008022aU,
+/*0222*/ 0x1804022aU,
+/*0223*/ 0x0005022bU,
+/*0224*/ 0x0806022bU,
+/*0225*/ 0x1007022bU,
+/*0226*/ 0x1805022bU,
+/*0227*/ 0x0006022cU,
+/*0228*/ 0x0807022cU,
+/*0229*/ 0x1005022cU,
+/*022a*/ 0x1806022cU,
+/*022b*/ 0x0007022dU,
+/*022c*/ 0x0802022dU,
+/*022d*/ 0x1001022dU,
+/*022e*/ 0xffffffffU,
+/*022f*/ 0x000a022eU,
+/*0230*/ 0x1010022eU,
+/*0231*/ 0x000a022fU,
+/*0232*/ 0x1010022fU,
+/*0233*/ 0x000a0230U,
+/*0234*/ 0x10100230U,
+/*0235*/ 0xffffffffU,
+/*0236*/ 0x00100231U,
+/*0237*/ 0xffffffffU,
+/*0238*/ 0xffffffffU,
+/*0239*/ 0x10010231U,
+/*023a*/ 0x18010231U,
+/*023b*/ 0x00010232U,
+/*023c*/ 0x08010232U,
+/*023d*/ 0x10010232U,
+/*023e*/ 0x18010232U,
+/*023f*/ 0x00020233U,
+/*0240*/ 0x08020233U,
+/*0241*/ 0x10020233U,
+/*0242*/ 0x18020233U,
+/*0243*/ 0x00020234U,
+/*0244*/ 0x08030234U,
+/*0245*/ 0x10010234U,
+/*0246*/ 0x18010234U,
+/*0247*/ 0x00010235U,
+/*0248*/ 0x08010235U,
+/*0249*/ 0xffffffffU,
+/*024a*/ 0x10020235U,
+/*024b*/ 0x18010235U,
+/*024c*/ 0x00010236U,
+/*024d*/ 0xffffffffU,
+/*024e*/ 0x08020236U,
+/*024f*/ 0x10010236U,
+/*0250*/ 0x18010236U,
+/*0251*/ 0xffffffffU,
+/*0252*/ 0x00020237U,
+/*0253*/ 0x08010237U,
+/*0254*/ 0x10010237U,
+/*0255*/ 0xffffffffU,
+/*0256*/ 0x18020237U,
+/*0257*/ 0x00070238U,
+/*0258*/ 0x08010238U,
+/*0259*/ 0x10010238U,
+/*025a*/ 0x18010238U,
+/*025b*/ 0x00010239U,
+/*025c*/ 0x08010239U,
+/*025d*/ 0x10010239U,
+/*025e*/ 0xffffffffU,
+/*025f*/ 0x18010239U,
+/*0260*/ 0x0004023aU,
+/*0261*/ 0x0804023aU,
+/*0262*/ 0x1004023aU,
+/*0263*/ 0x1801023aU,
+/*0264*/ 0x0002023bU,
+/*0265*/ 0x0806023bU,
+/*0266*/ 0x1006023bU,
+/*0267*/ 0xffffffffU,
+/*0268*/ 0xffffffffU,
+/*0269*/ 0xffffffffU,
+/*026a*/ 0x1802023bU,
+/*026b*/ 0x0010023cU,
+/*026c*/ 0x1001023cU,
+/*026d*/ 0x1801023cU,
+/*026e*/ 0xffffffffU,
+/*026f*/ 0x0004023dU,
+/*0270*/ 0x0801023dU,
+/*0271*/ 0x1004023dU,
+/*0272*/ 0x1802023dU,
+/*0273*/ 0x0008023eU,
+/*0274*/ 0xffffffffU,
+/*0275*/ 0xffffffffU,
+/*0276*/ 0xffffffffU,
+/*0277*/ 0x080a023eU,
+/*0278*/ 0x0020023fU,
+/*0279*/ 0x00200240U,
+/*027a*/ 0x00050241U,
+/*027b*/ 0x08010241U,
+/*027c*/ 0x10050241U,
+/*027d*/ 0x18080241U,
+/*027e*/ 0x00010242U,
+/*027f*/ 0x08080242U,
+/*0280*/ 0x10010242U,
+/*0281*/ 0x18080242U,
+/*0282*/ 0x00010243U,
+/*0283*/ 0x08040243U,
+/*0284*/ 0x10040243U,
+/*0285*/ 0x18040243U,
+/*0286*/ 0x00040244U,
+/*0287*/ 0x08040244U,
+/*0288*/ 0x10040244U,
+/*0289*/ 0x18040244U,
+/*028a*/ 0x00040245U,
+/*028b*/ 0x08040245U,
+/*028c*/ 0x10040245U,
+/*028d*/ 0x18010245U,
+/*028e*/ 0x00040246U,
+/*028f*/ 0x08040246U,
+/*0290*/ 0x10040246U,
+/*0291*/ 0x18040246U,
+/*0292*/ 0x00040247U,
+/*0293*/ 0x08040247U,
+/*0294*/ 0x10060247U,
+/*0295*/ 0x18060247U,
+/*0296*/ 0x00060248U,
+/*0297*/ 0x08060248U,
+/*0298*/ 0x10060248U,
+/*0299*/ 0x18060248U,
+/*029a*/ 0x00040249U,
+/*029b*/ 0x08010249U,
+/*029c*/ 0x10010249U,
+/*029d*/ 0x18020249U,
+/*029e*/ 0xffffffffU,
+/*029f*/ 0xffffffffU,
+/*02a0*/ 0xffffffffU,
+/*02a1*/ 0xffffffffU,
+/*02a2*/ 0xffffffffU,
+/*02a3*/ 0xffffffffU,
+/*02a4*/ 0xffffffffU,
+/*02a5*/ 0xffffffffU,
+/*02a6*/ 0x0004024aU,
+/*02a7*/ 0x0804024aU,
+/*02a8*/ 0x1001024aU,
+/*02a9*/ 0x1801024aU,
+/*02aa*/ 0xffffffffU,
+/*02ab*/ 0x0001024bU,
+/*02ac*/ 0x0801024bU,
+/*02ad*/ 0xffffffffU,
+/*02ae*/ 0x1001024bU,
+/*02af*/ 0x1801024bU,
+/*02b0*/ 0x0001024cU,
+/*02b1*/ 0x0804024cU,
+/*02b2*/ 0x1004024cU,
+/*02b3*/ 0x000a024dU,
+/*02b4*/ 0x0020024eU,
+/*02b5*/ 0x0004024fU,
+/*02b6*/ 0x0808024fU,
+/*02b7*/ 0xffffffffU,
+/*02b8*/ 0xffffffffU,
+/*02b9*/ 0xffffffffU,
+/*02ba*/ 0xffffffffU,
+/*02bb*/ 0xffffffffU,
+/*02bc*/ 0xffffffffU,
+/*02bd*/ 0x1002024fU,
+/*02be*/ 0x1802024fU,
+/*02bf*/ 0x00200250U,
+/*02c0*/ 0x00020251U,
+/*02c1*/ 0x08100251U,
+/*02c2*/ 0x00100252U,
+/*02c3*/ 0x10040252U,
+/*02c4*/ 0x18040252U,
+/*02c5*/ 0x00050253U,
+/*02c6*/ 0x08050253U,
+/*02c7*/ 0xffffffffU,
+/*02c8*/ 0xffffffffU,
+/*02c9*/ 0xffffffffU,
+/*02ca*/ 0xffffffffU,
+/*02cb*/ 0x10010253U,
+/*02cc*/ 0x18010253U,
+/*02cd*/ 0x00080254U,
+/*02ce*/ 0x08080254U,
+/*02cf*/ 0x10080254U,
+/*02d0*/ 0x18080254U,
+/*02d1*/ 0x00080255U,
+/*02d2*/ 0x08080255U,
+/*02d3*/ 0x10080255U,
+/*02d4*/ 0x18080255U,
+/*02d5*/ 0x00080256U,
+/*02d6*/ 0x08080256U,
+/*02d7*/ 0x10080256U,
+/*02d8*/ 0xffffffffU,
+/*02d9*/ 0xffffffffU,
+/*02da*/ 0xffffffffU,
+/*02db*/ 0xffffffffU,
+/*02dc*/ 0xffffffffU,
+/*02dd*/ 0xffffffffU,
+/*02de*/ 0x18030256U,
+/*02df*/ 0x00010257U,
+/*02e0*/ 0x08020257U,
+/*02e1*/ 0x10010257U,
+/*02e2*/ 0x18040257U,
+/*02e3*/ 0x00020258U,
+/*02e4*/ 0x08010258U,
+/*02e5*/ 0x10010258U,
+/*02e6*/ 0xffffffffU,
+/*02e7*/ 0x18010258U,
+/*02e8*/ 0x00040259U,
+/*02e9*/ 0x08080259U,
+/*02ea*/ 0x100a0259U,
+/*02eb*/ 0x000a025aU,
+/*02ec*/ 0x100a025aU,
+/*02ed*/ 0x000a025bU,
+/*02ee*/ 0x100a025bU,
+/*02ef*/ 0x000a025cU,
+/*02f0*/ 0x0020025dU,
+/*02f1*/ 0x0020025eU,
+/*02f2*/ 0x0001025fU,
+/*02f3*/ 0xffffffffU,
+/*02f4*/ 0xffffffffU,
+/*02f5*/ 0xffffffffU,
+/*02f6*/ 0x0802025fU,
+/*02f7*/ 0x1002025fU,
+/*02f8*/ 0x00100260U,
+/*02f9*/ 0x10050260U,
+/*02fa*/ 0x18060260U,
+/*02fb*/ 0x00050261U,
+/*02fc*/ 0x08050261U,
+/*02fd*/ 0x100e0261U,
+/*02fe*/ 0x00050262U,
+/*02ff*/ 0x080e0262U,
+/*0300*/ 0x18050262U,
+/*0301*/ 0x000e0263U,
+/*0302*/ 0x10050263U,
+/*0303*/ 0x18010263U,
+/*0304*/ 0x00050264U,
+/*0305*/ 0x08050264U,
+/*0306*/ 0x100a0264U,
+/*0307*/ 0x000a0265U,
+/*0308*/ 0x10050265U,
+/*0309*/ 0x18050265U,
+/*030a*/ 0x000a0266U,
+/*030b*/ 0x100a0266U,
+/*030c*/ 0x00050267U,
+/*030d*/ 0x08050267U,
+/*030e*/ 0x100a0267U,
+/*030f*/ 0x000a0268U,
+/*0310*/ 0xffffffffU,
+/*0311*/ 0xffffffffU,
+/*0312*/ 0xffffffffU,
+/*0313*/ 0xffffffffU,
+/*0314*/ 0xffffffffU,
+/*0315*/ 0xffffffffU,
+/*0316*/ 0x10070268U,
+/*0317*/ 0x18070268U,
+/*0318*/ 0x00040269U,
+/*0319*/ 0x08040269U,
+/*031a*/ 0xffffffffU,
+/*031b*/ 0xffffffffU,
+/*031c*/ 0xffffffffU,
+/*031d*/ 0x10040269U,
+/*031e*/ 0x18080269U,
+/*031f*/ 0x0008026aU,
+/*0320*/ 0x0804026aU,
+/*0321*/ 0xffffffffU,
+/*0322*/ 0xffffffffU,
+/*0323*/ 0xffffffffU,
+/*0324*/ 0x1004026aU,
+/*0325*/ 0xffffffffU,
+/*0326*/ 0xffffffffU,
+/*0327*/ 0xffffffffU,
+/*0328*/ 0x1804026aU,
+/*0329*/ 0xffffffffU,
+/*032a*/ 0xffffffffU,
+/*032b*/ 0xffffffffU,
+/*032c*/ 0x0004026bU,
+/*032d*/ 0x0805026bU,
+/*032e*/ 0x1007026bU,
+/*032f*/ 0x1808026bU,
+/*0330*/ 0x0010026cU,
+/*0331*/ 0x1008026cU,
+/*0332*/ 0x0010026dU,
+/*0333*/ 0x1008026dU,
+/*0334*/ 0x0010026eU,
+/*0335*/ 0x1008026eU,
+/*0336*/ 0x1808026eU,
+/*0337*/ 0x0001026fU,
+/*0338*/ 0x0801026fU,
+/*0339*/ 0x1006026fU,
+/*033a*/ 0x1806026fU,
+/*033b*/ 0x00060270U,
+/*033c*/ 0xffffffffU,
+/*033d*/ 0x08010270U,
+/*033e*/ 0x10030270U,
+/*033f*/ 0xffffffffU,
+/*0340*/ 0xffffffffU,
+/*0341*/ 0xffffffffU,
+/*0342*/ 0x000a0271U,
+/*0343*/ 0x100a0271U,
+/*0344*/ 0x00040272U,
+/*0345*/ 0x08010272U,
+/*0346*/ 0x10040272U,
+/*0347*/ 0xffffffffU,
+/*0348*/ 0xffffffffU,
+/*0349*/ 0xffffffffU,
+/*034a*/ 0xffffffffU,
+/*034b*/ 0xffffffffU,
+/*034c*/ 0xffffffffU,
+/*034d*/ 0x18070272U,
+/*034e*/ 0x00070273U,
+/*034f*/ 0x08050273U,
+/*0350*/ 0x10050273U,
+/*0351*/ 0xffffffffU,
+/*0352*/ 0xffffffffU,
+/*0353*/ 0xffffffffU,
+/*0354*/ 0x18040273U,
+/*0355*/ 0x00010274U,
+/*0356*/ 0x08010274U,
+/*0357*/ 0x10020274U,
+/*0358*/ 0x18080274U,
+/*0359*/ 0x00200275U,
+/*035a*/ 0x00200276U,
+/*035b*/ 0x00100277U,
+/*035c*/ 0xffffffffU,
+/*035d*/ 0xffffffffU,
+/*035e*/ 0xffffffffU,
+/*035f*/ 0x10020277U,
+/*0360*/ 0x18010277U,
+/*0361*/ 0xffffffffU,
+/*0362*/ 0x00020278U,
+/*0363*/ 0x08100278U,
+/*0364*/ 0x00100279U,
+/*0365*/ 0x10100279U,
+/*0366*/ 0x0008027aU,
+/*0367*/ 0x0808027aU,
+/*0368*/ 0x1008027aU,
+/*0369*/ 0xffffffffU,
+/*036a*/ 0x0010027bU,
+/*036b*/ 0x1010027bU,
+/*036c*/ 0x0010027cU,
+/*036d*/ 0x1008027cU,
+/*036e*/ 0x1808027cU,
+/*036f*/ 0x0008027dU,
+/*0370*/ 0xffffffffU,
+/*0371*/ 0x0810027dU,
+/*0372*/ 0x0010027eU,
+/*0373*/ 0x1010027eU,
+/*0374*/ 0x0008027fU,
+/*0375*/ 0x0808027fU,
+/*0376*/ 0x1008027fU,
+/*0377*/ 0xffffffffU,
+/*0378*/ 0x1808027fU,
+/*0379*/ 0x00100280U,
+/*037a*/ 0x10100280U,
+/*037b*/ 0x00100281U,
+/*037c*/ 0x10080281U,
+/*037d*/ 0x18080281U,
+/*037e*/ 0x00080282U,
+/*037f*/ 0xffffffffU,
+/*0380*/ 0x08100282U,
+/*0381*/ 0x00100283U,
+/*0382*/ 0x10100283U,
+/*0383*/ 0x00080284U,
+/*0384*/ 0x08080284U,
+/*0385*/ 0x10080284U,
+/*0386*/ 0xffffffffU,
+/*0387*/ 0x00100285U,
+/*0388*/ 0x10100285U,
+/*0389*/ 0x00100286U,
+/*038a*/ 0x10080286U,
+/*038b*/ 0x18080286U,
+/*038c*/ 0x00080287U,
+/*038d*/ 0xffffffffU,
+/*038e*/ 0x08080287U,
+/*038f*/ 0x10100287U,
+/*0390*/ 0x00100288U,
+/*0391*/ 0x10100288U,
+/*0392*/ 0x00080289U,
+/*0393*/ 0x08080289U,
+/*0394*/ 0x10080289U,
+/*0395*/ 0xffffffffU,
+/*0396*/ 0x0010028aU,
+/*0397*/ 0x1010028aU,
+/*0398*/ 0x0010028bU,
+/*0399*/ 0x1008028bU,
+/*039a*/ 0x1808028bU,
+/*039b*/ 0x0008028cU,
+/*039c*/ 0xffffffffU,
+/*039d*/ 0x0810028cU,
+/*039e*/ 0x0010028dU,
+/*039f*/ 0x1010028dU,
+/*03a0*/ 0x0008028eU,
+/*03a1*/ 0x0808028eU,
+/*03a2*/ 0x1008028eU,
+/*03a3*/ 0xffffffffU,
+/*03a4*/ 0x1808028eU,
+/*03a5*/ 0x0010028fU,
+/*03a6*/ 0x1010028fU,
+/*03a7*/ 0x00100290U,
+/*03a8*/ 0x10080290U,
+/*03a9*/ 0x18080290U,
+/*03aa*/ 0x00080291U,
+/*03ab*/ 0xffffffffU,
+/*03ac*/ 0x08100291U,
+/*03ad*/ 0x00100292U,
+/*03ae*/ 0x10100292U,
+/*03af*/ 0x00080293U,
+/*03b0*/ 0x08080293U,
+/*03b1*/ 0x10080293U,
+/*03b2*/ 0xffffffffU,
+/*03b3*/ 0x00100294U,
+/*03b4*/ 0x10100294U,
+/*03b5*/ 0x00100295U,
+/*03b6*/ 0x10080295U,
+/*03b7*/ 0x18080295U,
+/*03b8*/ 0x00080296U,
+/*03b9*/ 0xffffffffU,
+/*03ba*/ 0x08080296U,
+/*03bb*/ 0x10020296U,
+/*03bc*/ 0x18030296U,
+/*03bd*/ 0x000a0297U,
+/*03be*/ 0x100a0297U,
+/*03bf*/ 0x000a0298U,
+/*03c0*/ 0x10050298U,
+/*03c1*/ 0x18040298U,
+/*03c2*/ 0x00080299U,
+/*03c3*/ 0x08080299U,
+/*03c4*/ 0x10060299U,
+/*03c5*/ 0x18060299U,
+/*03c6*/ 0x0011029aU,
+/*03c7*/ 0x1808029aU,
+/*03c8*/ 0x0004029bU,
+/*03c9*/ 0x0806029bU,
+/*03ca*/ 0xffffffffU,
+/*03cb*/ 0x1006029bU,
+/*03cc*/ 0x1808029bU,
+/*03cd*/ 0x0008029cU,
+/*03ce*/ 0x0804029cU,
+/*03cf*/ 0x1008029cU,
+/*03d0*/ 0x1808029cU,
+/*03d1*/ 0x0006029dU,
+/*03d2*/ 0x0806029dU,
+/*03d3*/ 0x0011029eU,
+/*03d4*/ 0x1808029eU,
+/*03d5*/ 0x0004029fU,
+/*03d6*/ 0x0806029fU,
+/*03d7*/ 0xffffffffU,
+/*03d8*/ 0x1006029fU,
+/*03d9*/ 0x1808029fU,
+/*03da*/ 0x000802a0U,
+/*03db*/ 0x080402a0U,
+/*03dc*/ 0x100802a0U,
+/*03dd*/ 0x180802a0U,
+/*03de*/ 0x000602a1U,
+/*03df*/ 0x080602a1U,
+/*03e0*/ 0x001102a2U,
+/*03e1*/ 0x180802a2U,
+/*03e2*/ 0x000402a3U,
+/*03e3*/ 0x080602a3U,
+/*03e4*/ 0xffffffffU,
+/*03e5*/ 0x100602a3U,
+/*03e6*/ 0x180802a3U,
+/*03e7*/ 0x000802a4U,
+/*03e8*/ 0x080402a4U,
+/*03e9*/ 0x100402a4U,
+/*03ea*/ 0x180402a4U,
+/*03eb*/ 0x000402a5U,
+/*03ec*/ 0x080402a5U,
+/*03ed*/ 0x100402a5U,
+/*03ee*/ 0x180402a5U,
+/*03ef*/ 0x000402a6U,
+/*03f0*/ 0x080402a6U,
+/*03f1*/ 0x100402a6U,
+/*03f2*/ 0x180402a6U,
+/*03f3*/ 0x000402a7U,
+/*03f4*/ 0x080402a7U,
+/*03f5*/ 0x100402a7U,
+/*03f6*/ 0x180402a7U,
+/*03f7*/ 0x000402a8U,
+/*03f8*/ 0x080402a8U,
+/*03f9*/ 0x100402a8U,
+/*03fa*/ 0x180402a8U,
+/*03fb*/ 0x000402a9U,
+/*03fc*/ 0x081202a9U,
+/*03fd*/ 0x001102aaU,
+/*03fe*/ 0x001202abU,
+/*03ff*/ 0x002002acU,
+/*0400*/ 0x002002adU,
+/*0401*/ 0x002002aeU,
+/*0402*/ 0x002002afU,
+/*0403*/ 0x002002b0U,
+/*0404*/ 0x002002b1U,
+/*0405*/ 0x002002b2U,
+/*0406*/ 0x002002b3U,
+/*0407*/ 0x002002b4U,
+/*0408*/ 0x000302b5U,
+/*0409*/ 0x080502b5U,
+/*040a*/ 0x100502b5U,
+/*040b*/ 0x180102b5U,
+/*040c*/ 0x000502b6U,
+/*040d*/ 0x080502b6U,
+/*040e*/ 0x100502b6U,
+/*040f*/ 0x180502b6U,
+/*0410*/ 0x000502b7U,
+/*0411*/ 0x080502b7U,
+/*0412*/ 0x100502b7U,
+/*0413*/ 0x180502b7U,
+/*0414*/ 0x000502b8U,
+/*0415*/ 0x080502b8U,
+/*0416*/ 0x100502b8U,
+/*0417*/ 0x180502b8U,
+/*0418*/ 0x000502b9U,
+/*0419*/ 0x080502b9U,
+/*041a*/ 0x100502b9U,
+/*041b*/ 0x180502b9U,
+/*041c*/ 0x000502baU,
+/*041d*/ 0x080502baU,
+/*041e*/ 0x100502baU,
+/*041f*/ 0x180502baU,
+/*0420*/ 0x000502bbU,
+/*0421*/ 0x080502bbU,
+/*0422*/ 0x100102bbU,
+/*0423*/ 0x180202bbU,
+/*0424*/ 0x000202bcU,
+/*0425*/ 0x080202bcU,
+/*0426*/ 0x100202bcU,
+/*0427*/ 0x180102bcU,
+/*0428*/ 0x000402bdU,
+/*0429*/ 0x081002bdU,
+/*042a*/ 0x002002beU,
+/*042b*/ 0x001002bfU,
+/*042c*/ 0x002002c0U,
+/*042d*/ 0x001002c1U,
+/*042e*/ 0x002002c2U,
+/*042f*/ 0x000702c3U,
+/*0430*/ 0x080102c3U,
+/*0431*/ 0x100202c3U,
+/*0432*/ 0x180602c3U,
+/*0433*/ 0x000102c4U,
+/*0434*/ 0x080102c4U,
+/*0435*/ 0x002002c5U,
+/*0436*/ 0x000302c6U,
+/*0437*/ 0x002002c7U,
+/*0438*/ 0x002002c8U,
+/*0439*/ 0xffffffffU,
+/*043a*/ 0xffffffffU,
+/*043b*/ 0xffffffffU,
+/*043c*/ 0xffffffffU,
+/*043d*/ 0xffffffffU,
+/*043e*/ 0xffffffffU,
+/*043f*/ 0xffffffffU,
+/*0440*/ 0xffffffffU,
+/*0441*/ 0xffffffffU,
+/*0442*/ 0xffffffffU,
+/*0443*/ 0xffffffffU,
+/*0444*/ 0xffffffffU,
+/*0445*/ 0xffffffffU,
+/*0446*/ 0xffffffffU,
+/*0447*/ 0xffffffffU,
+/*0448*/ 0xffffffffU,
+/*0449*/ 0xffffffffU,
+/*044a*/ 0xffffffffU,
+/*044b*/ 0xffffffffU,
+/*044c*/ 0xffffffffU,
+/*044d*/ 0xffffffffU,
+/*044e*/ 0xffffffffU,
+/*044f*/ 0xffffffffU,
+/*0450*/ 0xffffffffU,
+/*0451*/ 0xffffffffU,
+/*0452*/ 0xffffffffU,
+/*0453*/ 0xffffffffU,
+/*0454*/ 0xffffffffU,
+/*0455*/ 0xffffffffU,
+/*0456*/ 0xffffffffU,
+/*0457*/ 0xffffffffU,
+/*0458*/ 0xffffffffU,
+/*0459*/ 0xffffffffU,
+/*045a*/ 0xffffffffU,
+/*045b*/ 0xffffffffU,
+/*045c*/ 0xffffffffU,
+/*045d*/ 0xffffffffU,
+/*045e*/ 0xffffffffU,
+/*045f*/ 0x000402c9U,
+/*0460*/ 0xffffffffU,
+/*0461*/ 0xffffffffU,
+/*0462*/ 0xffffffffU,
+/*0463*/ 0xffffffffU,
+/*0464*/ 0xffffffffU,
+/*0465*/ 0xffffffffU,
+/*0466*/ 0xffffffffU,
+/*0467*/ 0xffffffffU,
+/*0468*/ 0xffffffffU,
+/*0469*/ 0xffffffffU,
+/*046a*/ 0xffffffffU,
+/*046b*/ 0xffffffffU,
+/*046c*/ 0xffffffffU,
+/*046d*/ 0xffffffffU,
+/*046e*/ 0xffffffffU,
+/*046f*/ 0xffffffffU,
+/*0470*/ 0xffffffffU,
+/*0471*/ 0xffffffffU,
+/*0472*/ 0xffffffffU,
+/*0473*/ 0xffffffffU,
+/*0474*/ 0xffffffffU,
+/*0475*/ 0xffffffffU,
+/*0476*/ 0xffffffffU,
+/*0477*/ 0xffffffffU,
+/*0478*/ 0xffffffffU,
+/*0479*/ 0xffffffffU,
+/*047a*/ 0xffffffffU,
+/*047b*/ 0xffffffffU,
+/*047c*/ 0xffffffffU,
+/*047d*/ 0xffffffffU,
+/*047e*/ 0xffffffffU,
+/*047f*/ 0xffffffffU,
+/*0480*/ 0xffffffffU,
+/*0481*/ 0xffffffffU,
+/*0482*/ 0xffffffffU,
+/*0483*/ 0xffffffffU,
+/*0484*/ 0xffffffffU,
+/*0485*/ 0xffffffffU,
+/*0486*/ 0xffffffffU,
+/*0487*/ 0xffffffffU,
+/*0488*/ 0xffffffffU,
+/*0489*/ 0xffffffffU,
+/*048a*/ 0xffffffffU,
+/*048b*/ 0xffffffffU,
+/*048c*/ 0xffffffffU,
+/*048d*/ 0xffffffffU,
+/*048e*/ 0xffffffffU,
+/*048f*/ 0xffffffffU,
+/*0490*/ 0xffffffffU,
+/*0491*/ 0xffffffffU,
+/*0492*/ 0xffffffffU,
+/*0493*/ 0xffffffffU,
+/*0494*/ 0xffffffffU,
+	 },
+	{
+/*0000*/ 0x00200400U,
+/*0001*/ 0x00040401U,
+/*0002*/ 0x080b0401U,
+/*0003*/ 0x000a0402U,
+/*0004*/ 0x10020402U,
+/*0005*/ 0x18010402U,
+/*0006*/ 0x00050403U,
+/*0007*/ 0x08050403U,
+/*0008*/ 0x10050403U,
+/*0009*/ 0x18050403U,
+/*000a*/ 0x00050404U,
+/*000b*/ 0x08050404U,
+/*000c*/ 0x10050404U,
+/*000d*/ 0x18050404U,
+/*000e*/ 0x00050405U,
+/*000f*/ 0x08040405U,
+/*0010*/ 0x10030405U,
+/*0011*/ 0x00180406U,
+/*0012*/ 0x18030406U,
+/*0013*/ 0x00180407U,
+/*0014*/ 0x18020407U,
+/*0015*/ 0x00010408U,
+/*0016*/ 0x08020408U,
+/*0017*/ 0x10010408U,
+/*0018*/ 0x18010408U,
+/*0019*/ 0x00020409U,
+/*001a*/ 0x08040409U,
+/*001b*/ 0x10040409U,
+/*001c*/ 0x18040409U,
+/*001d*/ 0xffffffffU,
+/*001e*/ 0x0004040aU,
+/*001f*/ 0xffffffffU,
+/*0020*/ 0xffffffffU,
+/*0021*/ 0x0809040aU,
+/*0022*/ 0x1801040aU,
+/*0023*/ 0x0020040bU,
+/*0024*/ 0x001c040cU,
+/*0025*/ 0x0001040dU,
+/*0026*/ 0x0807040dU,
+/*0027*/ 0x1009040dU,
+/*0028*/ 0x000a040eU,
+/*0029*/ 0x1005040eU,
+/*002a*/ 0x1801040eU,
+/*002b*/ 0x1001040fU,
+/*002c*/ 0x1802040fU,
+/*002d*/ 0x0009040fU,
+/*002e*/ 0x00090410U,
+/*002f*/ 0x10020410U,
+/*0030*/ 0x00200411U,
+/*0031*/ 0x00010412U,
+/*0032*/ 0x08020412U,
+/*0033*/ 0xffffffffU,
+/*0034*/ 0xffffffffU,
+/*0035*/ 0xffffffffU,
+/*0036*/ 0xffffffffU,
+/*0037*/ 0x00200413U,
+/*0038*/ 0x00200414U,
+/*0039*/ 0x00200415U,
+/*003a*/ 0x00200416U,
+/*003b*/ 0x00030417U,
+/*003c*/ 0x08010417U,
+/*003d*/ 0x10040417U,
+/*003e*/ 0x18030417U,
+/*003f*/ 0x00040418U,
+/*0040*/ 0x08040418U,
+/*0041*/ 0x10040418U,
+/*0042*/ 0x18040418U,
+/*0043*/ 0x00010419U,
+/*0044*/ 0x08010419U,
+/*0045*/ 0x10060419U,
+/*0046*/ 0x18040419U,
+/*0047*/ 0xffffffffU,
+/*0048*/ 0x0006041aU,
+/*0049*/ 0x0804041aU,
+/*004a*/ 0x1006041aU,
+/*004b*/ 0x1804041aU,
+/*004c*/ 0x0002041bU,
+/*004d*/ 0x0805041bU,
+/*004e*/ 0x1008041bU,
+/*004f*/ 0xffffffffU,
+/*0050*/ 0x1806041bU,
+/*0051*/ 0x0003041cU,
+/*0052*/ 0x080b041cU,
+/*0053*/ 0x1804041cU,
+/*0054*/ 0x0004041dU,
+/*0055*/ 0x0804041dU,
+/*0056*/ 0x1001041dU,
+/*0057*/ 0xffffffffU,
+/*0058*/ 0x0009041eU,
+/*0059*/ 0x0020041fU,
+/*005a*/ 0x00200420U,
+/*005b*/ 0x00200421U,
+/*005c*/ 0x00200422U,
+/*005d*/ 0x00100423U,
+/*005e*/ 0xffffffffU,
+/*005f*/ 0x10010423U,
+/*0060*/ 0x18060423U,
+/*0061*/ 0x00080424U,
+/*0062*/ 0x00200425U,
+/*0063*/ 0x00100426U,
+/*0064*/ 0x100a0426U,
+/*0065*/ 0x00060427U,
+/*0066*/ 0x08070427U,
+/*0067*/ 0x10080427U,
+/*0068*/ 0x18080427U,
+/*0069*/ 0x000a0428U,
+/*006a*/ 0x10070428U,
+/*006b*/ 0x18080428U,
+/*006c*/ 0x00080429U,
+/*006d*/ 0x08030429U,
+/*006e*/ 0x100a0429U,
+/*006f*/ 0x000a042aU,
+/*0070*/ 0x0011042bU,
+/*0071*/ 0x0009042cU,
+/*0072*/ 0x1009042cU,
+/*0073*/ 0x0010042dU,
+/*0074*/ 0x100e042dU,
+/*0075*/ 0x000e042eU,
+/*0076*/ 0x0012042fU,
+/*0077*/ 0x000a0430U,
+/*0078*/ 0x100a0430U,
+/*0079*/ 0x00020431U,
+/*007a*/ 0x00200432U,
+/*007b*/ 0x000b0433U,
+/*007c*/ 0x100b0433U,
+/*007d*/ 0x00200434U,
+/*007e*/ 0x00120435U,
+/*007f*/ 0x00200436U,
+/*0080*/ 0x00200437U,
+/*0081*/ 0x00080438U,
+/*0082*/ 0x08010438U,
+/*0083*/ 0x10010438U,
+/*0084*/ 0x18010438U,
+/*0085*/ 0x00080439U,
+/*0086*/ 0x080c0439U,
+/*0087*/ 0x000c043aU,
+/*0088*/ 0x100c043aU,
+/*0089*/ 0x000c043bU,
+/*008a*/ 0x100c043bU,
+/*008b*/ 0x000c043cU,
+/*008c*/ 0x100c043cU,
+/*008d*/ 0x000c043dU,
+/*008e*/ 0x100c043dU,
+/*008f*/ 0x000c043eU,
+/*0090*/ 0x100c043eU,
+/*0091*/ 0x000b043fU,
+/*0092*/ 0x1009043fU,
+/*0093*/ 0x00010440U,
+/*0094*/ 0x000b0441U,
+/*0095*/ 0x100b0441U,
+/*0096*/ 0x000b0442U,
+/*0097*/ 0x100b0442U,
+/*0098*/ 0x000b0443U,
+/*0099*/ 0x100b0443U,
+/*009a*/ 0x000b0444U,
+/*009b*/ 0x100b0444U,
+/*009c*/ 0x000b0445U,
+/*009d*/ 0x100a0445U,
+/*009e*/ 0x00020446U,
+/*009f*/ 0x080a0446U,
+/*00a0*/ 0x000a0447U,
+/*00a1*/ 0x100a0447U,
+/*00a2*/ 0x000a0448U,
+/*00a3*/ 0x100a0448U,
+/*00a4*/ 0x000a0449U,
+/*00a5*/ 0x100a0449U,
+/*00a6*/ 0x000a044aU,
+/*00a7*/ 0x100a044aU,
+/*00a8*/ 0x000a044bU,
+/*00a9*/ 0x100a044bU,
+/*00aa*/ 0x000a044cU,
+/*00ab*/ 0x100a044cU,
+/*00ac*/ 0x000a044dU,
+/*00ad*/ 0x100a044dU,
+/*00ae*/ 0x000a044eU,
+/*00af*/ 0x100a044eU,
+/*00b0*/ 0x000a044fU,
+/*00b1*/ 0x100a044fU,
+/*00b2*/ 0x000a0450U,
+/*00b3*/ 0x100a0450U,
+/*00b4*/ 0x000a0451U,
+/*00b5*/ 0x100a0451U,
+/*00b6*/ 0x000a0452U,
+/*00b7*/ 0x100a0452U,
+/*00b8*/ 0x000a0453U,
+/*00b9*/ 0x100a0453U,
+/*00ba*/ 0x000a0454U,
+/*00bb*/ 0x10040454U,
+/*00bc*/ 0x18030454U,
+/*00bd*/ 0x000a0455U,
+/*00be*/ 0x100a0455U,
+/*00bf*/ 0x00010456U,
+/*00c0*/ 0x080a0456U,
+/*00c1*/ 0x18040456U,
+/*00c2*/ 0x000b0457U,
+/*00c3*/ 0x100a0457U,
+/*00c4*/ 0x00030458U,
+/*00c5*/ 0x00080459U,
+/*00c6*/ 0x08080459U,
+/*00c7*/ 0x10080459U,
+/*00c8*/ 0x18080459U,
+/*00c9*/ 0x0008045aU,
+/*00ca*/ 0xffffffffU,
+/*00cb*/ 0x0808045aU,
+/*00cc*/ 0x1001045aU,
+/*00cd*/ 0x1808045aU,
+/*00ce*/ 0x0008045bU,
+/*00cf*/ 0x0802045bU,
+/*00d0*/ 0x1002045bU,
+/*00d1*/ 0x1805045bU,
+/*00d2*/ 0x0005045cU,
+/*00d3*/ 0xffffffffU,
+/*00d4*/ 0x0804045cU,
+/*00d5*/ 0x100a045cU,
+/*00d6*/ 0x0006045dU,
+/*00d7*/ 0x0808045dU,
+/*00d8*/ 0x1008045dU,
+/*00d9*/ 0x1804045dU,
+/*00da*/ 0x0004045eU,
+/*00db*/ 0x0805045eU,
+/*00dc*/ 0x1004045eU,
+/*00dd*/ 0x1805045eU,
+/*00de*/ 0x000a045fU,
+/*00df*/ 0x100a045fU,
+/*00e0*/ 0x00080460U,
+/*00e1*/ 0xffffffffU,
+/*00e2*/ 0x08040460U,
+/*00e3*/ 0xffffffffU,
+/*00e4*/ 0xffffffffU,
+/*00e5*/ 0x00050600U,
+/*00e6*/ 0x08050600U,
+/*00e7*/ 0x10050600U,
+/*00e8*/ 0x18050600U,
+/*00e9*/ 0x00050601U,
+/*00ea*/ 0x08050601U,
+/*00eb*/ 0x100b0601U,
+/*00ec*/ 0x00010602U,
+/*00ed*/ 0x08030602U,
+/*00ee*/ 0x00200603U,
+/*00ef*/ 0x00100604U,
+/*00f0*/ 0x10040604U,
+/*00f1*/ 0x000a0605U,
+/*00f2*/ 0x10090605U,
+/*00f3*/ 0x00080606U,
+/*00f4*/ 0x08030606U,
+/*00f5*/ 0x10030606U,
+/*00f6*/ 0x18010606U,
+/*00f7*/ 0x00010607U,
+/*00f8*/ 0x08070607U,
+/*00f9*/ 0x10070607U,
+/*00fa*/ 0x18050607U,
+/*00fb*/ 0x00010608U,
+/*00fc*/ 0x08020608U,
+/*00fd*/ 0x10030608U,
+/*00fe*/ 0x18010608U,
+/*00ff*/ 0x000f0609U,
+/*0100*/ 0x0020060aU,
+/*0101*/ 0x0020060bU,
+/*0102*/ 0x000b060cU,
+/*0103*/ 0x100b060cU,
+/*0104*/ 0x000b060dU,
+/*0105*/ 0x0018060eU,
+/*0106*/ 0x0018060fU,
+/*0107*/ 0xffffffffU,
+/*0108*/ 0xffffffffU,
+/*0109*/ 0xffffffffU,
+/*010a*/ 0xffffffffU,
+/*010b*/ 0xffffffffU,
+/*010c*/ 0x1802060fU,
+/*010d*/ 0x00020610U,
+/*010e*/ 0x08040610U,
+/*010f*/ 0x10040610U,
+/*0110*/ 0x18010610U,
+/*0111*/ 0x00010611U,
+/*0112*/ 0x08010611U,
+/*0113*/ 0x10030611U,
+/*0114*/ 0x00200612U,
+/*0115*/ 0x00200613U,
+/*0116*/ 0xffffffffU,
+/*0117*/ 0x00140614U,
+/*0118*/ 0x00140615U,
+/*0119*/ 0x00140616U,
+/*011a*/ 0x00140617U,
+/*011b*/ 0x00140618U,
+/*011c*/ 0x00140619U,
+/*011d*/ 0x0014061aU,
+/*011e*/ 0x0014061bU,
+/*011f*/ 0x0018061cU,
+/*0120*/ 0x000a061dU,
+/*0121*/ 0x1006061dU,
+/*0122*/ 0x1806061dU,
+/*0123*/ 0x0006061eU,
+/*0124*/ 0xffffffffU,
+/*0125*/ 0x0806061eU,
+/*0126*/ 0x0008061fU,
+/*0127*/ 0x080b061fU,
+/*0128*/ 0x000b0620U,
+/*0129*/ 0x100b0620U,
+/*012a*/ 0x000b0621U,
+/*012b*/ 0x100b0621U,
+/*012c*/ 0x000b0622U,
+/*012d*/ 0x10040622U,
+/*012e*/ 0x000a0623U,
+/*012f*/ 0x10060623U,
+/*0130*/ 0x18080623U,
+/*0131*/ 0x00080624U,
+/*0132*/ 0x08040624U,
+/*0133*/ 0x00020680U,
+/*0134*/ 0x00010681U,
+/*0135*/ 0x08010681U,
+/*0136*/ 0x10020681U,
+/*0137*/ 0x18050681U,
+/*0138*/ 0x00050682U,
+/*0139*/ 0x08050682U,
+/*013a*/ 0x10050682U,
+/*013b*/ 0x000b0683U,
+/*013c*/ 0x10050683U,
+/*013d*/ 0x18010683U,
+/*013e*/ 0x00010684U,
+/*013f*/ 0xffffffffU,
+/*0140*/ 0x08010684U,
+/*0141*/ 0x10010684U,
+/*0142*/ 0x18040684U,
+/*0143*/ 0x000b0685U,
+/*0144*/ 0x100b0685U,
+/*0145*/ 0x000b0686U,
+/*0146*/ 0x10040686U,
+/*0147*/ 0x000b0687U,
+/*0148*/ 0x10040687U,
+/*0149*/ 0x18010687U,
+/*014a*/ 0x00010688U,
+/*014b*/ 0x08010688U,
+/*014c*/ 0x00200689U,
+/*014d*/ 0x0020068aU,
+/*014e*/ 0x0008068bU,
+/*014f*/ 0x080a068bU,
+/*0150*/ 0x1805068bU,
+/*0151*/ 0x000a068cU,
+/*0152*/ 0x1003068cU,
+/*0153*/ 0x1803068cU,
+/*0154*/ 0x0001068dU,
+/*0155*/ 0x0802068dU,
+/*0156*/ 0x1001068dU,
+/*0157*/ 0x1801068dU,
+/*0158*/ 0x0001068eU,
+/*0159*/ 0x0802068eU,
+/*015a*/ 0x1001068eU,
+/*015b*/ 0x0004068fU,
+/*015c*/ 0x0804068fU,
+/*015d*/ 0x1004068fU,
+/*015e*/ 0x1804068fU,
+/*015f*/ 0x00010690U,
+/*0160*/ 0x08010690U,
+/*0161*/ 0x10010690U,
+/*0162*/ 0x00200691U,
+/*0163*/ 0x00200692U,
+/*0164*/ 0x00200693U,
+/*0165*/ 0x00200694U,
+/*0166*/ 0xffffffffU,
+/*0167*/ 0x1801068eU,
+/*0168*/ 0x000d0696U,
+/*0169*/ 0x100d0696U,
+/*016a*/ 0x000d0697U,
+/*016b*/ 0x00050698U,
+/*016c*/ 0x00010699U,
+/*016d*/ 0x080e0699U,
+/*016e*/ 0x000e069aU,
+/*016f*/ 0x100e069aU,
+/*0170*/ 0x000e069bU,
+/*0171*/ 0x100e069bU,
+/*0172*/ 0x0004069cU,
+/*0173*/ 0x0804069cU,
+/*0174*/ 0x1004069cU,
+/*0175*/ 0x1804069cU,
+/*0176*/ 0x0004069dU,
+/*0177*/ 0x080b069dU,
+/*0178*/ 0x000b069eU,
+/*0179*/ 0x100b069eU,
+/*017a*/ 0x000b069fU,
+/*017b*/ 0xffffffffU,
+/*017c*/ 0xffffffffU,
+/*017d*/ 0xffffffffU,
+/*017e*/ 0xffffffffU,
+/*017f*/ 0x000d06a0U,
+/*0180*/ 0x100d06a0U,
+/*0181*/ 0x000d06a1U,
+/*0182*/ 0x101006a1U,
+/*0183*/ 0x00080695U,
+/*0184*/ 0x08080695U,
+/*0185*/ 0x001006a2U,
+/*0186*/ 0x101006a2U,
+/*0187*/ 0x001006a3U,
+/*0188*/ 0x101006a3U,
+/*0189*/ 0x001006a4U,
+/*018a*/ 0x100306a4U,
+/*018b*/ 0x180406a4U,
+/*018c*/ 0x000106a5U,
+/*018d*/ 0x080806a5U,
+/*018e*/ 0x100106a5U,
+/*018f*/ 0x180506a5U,
+/*0190*/ 0x000106a6U,
+/*0191*/ 0x081406a6U,
+/*0192*/ 0x000a06a7U,
+/*0193*/ 0x100c06a7U,
+/*0194*/ 0x001206a8U,
+/*0195*/ 0x001406a9U,
+/*0196*/ 0x001206aaU,
+/*0197*/ 0x001106abU,
+/*0198*/ 0x001106acU,
+/*0199*/ 0x001206adU,
+/*019a*/ 0x001206aeU,
+/*019b*/ 0x001206afU,
+/*019c*/ 0x001206b0U,
+/*019d*/ 0x001206b1U,
+/*019e*/ 0x001206b2U,
+/*019f*/ 0x001206b3U,
+/*01a0*/ 0x001206b4U,
+/*01a1*/ 0x001206b5U,
+/*01a2*/ 0x001206b6U,
+/*01a3*/ 0x000e06b7U,
+/*01a4*/ 0x100d06b7U,
+/*01a5*/ 0x002006b8U,
+/*01a6*/ 0x001706b9U,
+/*01a7*/ 0x000906baU,
+/*01a8*/ 0x100106baU,
+/*01a9*/ 0x180106baU,
+/*01aa*/ 0x002006bbU,
+/*01ab*/ 0x000806bcU,
+/*01ac*/ 0x080306bcU,
+/*01ad*/ 0x100306bcU,
+/*01ae*/ 0x001806bdU,
+/*01af*/ 0x001806beU,
+/*01b0*/ 0x180706beU,
+/*01b1*/ 0x000506bfU,
+/*01b2*/ 0x080806bfU,
+/*01b3*/ 0x100806bfU,
+/*01b4*/ 0x180806bfU,
+/*01b5*/ 0x000106c0U,
+/*01b6*/ 0x080106c0U,
+/*01b7*/ 0x002006c1U,
+/*01b8*/ 0xffffffffU,
+/*01b9*/ 0xffffffffU,
+/*01ba*/ 0xffffffffU,
+/*01bb*/ 0xffffffffU,
+/*01bc*/ 0xffffffffU,
+/*01bd*/ 0xffffffffU,
+/*01be*/ 0xffffffffU,
+/*01bf*/ 0x001006c2U,
+/*01c0*/ 0x100106c2U,
+/*01c1*/ 0x180106c2U,
+/*01c2*/ 0x000206c3U,
+/*01c3*/ 0x080406c3U,
+/*01c4*/ 0x100906c3U,
+/*01c5*/ 0x000706c4U,
+/*01c6*/ 0x080406c4U,
+/*01c7*/ 0x002006c5U,
+/*01c8*/ 0x000106c6U,
+/*01c9*/ 0x080206c6U,
+/*01ca*/ 0x100606c6U,
+/*01cb*/ 0x001006c7U,
+/*01cc*/ 0x100106c7U,
+/*01cd*/ 0x002006c8U,
+/*01ce*/ 0x000806c9U,
+/*01cf*/ 0x080106c9U,
+/*01d0*/ 0x100506c9U,
+/*01d1*/ 0xffffffffU,
+/*01d2*/ 0x180206c9U,
+/*01d3*/ 0x000106caU,
+/*01d4*/ 0x002006cbU,
+/*01d5*/ 0x000b06ccU,
+/*01d6*/ 0x100106ccU,
+/*01d7*/ 0x180306ccU,
+/*01d8*/ 0x000806cdU,
+/*01d9*/ 0x080206cdU,
+/*01da*/ 0x100c06cdU,
+/*01db*/ 0x000406ceU,
+/*01dc*/ 0x080106ceU,
+/*01dd*/ 0xffffffffU,
+/*01de*/ 0x00010200U,
+/*01df*/ 0x08040200U,
+/*01e0*/ 0x10100200U,
+/*01e1*/ 0x00010201U,
+/*01e2*/ 0x08010201U,
+/*01e3*/ 0x10010201U,
+/*01e4*/ 0xffffffffU,
+/*01e5*/ 0x00100202U,
+/*01e6*/ 0x10080202U,
+/*01e7*/ 0xffffffffU,
+/*01e8*/ 0xffffffffU,
+/*01e9*/ 0xffffffffU,
+/*01ea*/ 0xffffffffU,
+/*01eb*/ 0xffffffffU,
+/*01ec*/ 0xffffffffU,
+/*01ed*/ 0xffffffffU,
+/*01ee*/ 0xffffffffU,
+/*01ef*/ 0x00200203U,
+/*01f0*/ 0x00100204U,
+/*01f1*/ 0x00200205U,
+/*01f2*/ 0x00100206U,
+/*01f3*/ 0x00200207U,
+/*01f4*/ 0x00100208U,
+/*01f5*/ 0x00140209U,
+/*01f6*/ 0x0020020aU,
+/*01f7*/ 0x0020020bU,
+/*01f8*/ 0x0020020cU,
+/*01f9*/ 0x0020020dU,
+/*01fa*/ 0x0014020eU,
+/*01fb*/ 0x0020020fU,
+/*01fc*/ 0x00200210U,
+/*01fd*/ 0x00200211U,
+/*01fe*/ 0x00200212U,
+/*01ff*/ 0x00140213U,
+/*0200*/ 0x00200214U,
+/*0201*/ 0x00200215U,
+/*0202*/ 0x00200216U,
+/*0203*/ 0x00200217U,
+/*0204*/ 0x00090218U,
+/*0205*/ 0x10010218U,
+/*0206*/ 0x00200219U,
+/*0207*/ 0x0005021aU,
+/*0208*/ 0x0801021aU,
+/*0209*/ 0x1008021aU,
+/*020a*/ 0x1808021aU,
+/*020b*/ 0x001c021bU,
+/*020c*/ 0x001c021cU,
+/*020d*/ 0x001c021dU,
+/*020e*/ 0x001c021eU,
+/*020f*/ 0x001c021fU,
+/*0210*/ 0x001c0220U,
+/*0211*/ 0x001c0221U,
+/*0212*/ 0x001c0222U,
+/*0213*/ 0x001c0223U,
+/*0214*/ 0x001c0224U,
+/*0215*/ 0x001c0225U,
+/*0216*/ 0x001c0226U,
+/*0217*/ 0x001c0227U,
+/*0218*/ 0x001c0228U,
+/*0219*/ 0x001c0229U,
+/*021a*/ 0x001c022aU,
+/*021b*/ 0x0001022bU,
+/*021c*/ 0x0801022bU,
+/*021d*/ 0x1001022bU,
+/*021e*/ 0x1804022bU,
+/*021f*/ 0x0008022cU,
+/*0220*/ 0x0808022cU,
+/*0221*/ 0x1008022cU,
+/*0222*/ 0x1804022cU,
+/*0223*/ 0x0007022dU,
+/*0224*/ 0xffffffffU,
+/*0225*/ 0x0807022dU,
+/*0226*/ 0x1007022dU,
+/*0227*/ 0xffffffffU,
+/*0228*/ 0x1807022dU,
+/*0229*/ 0x0007022eU,
+/*022a*/ 0xffffffffU,
+/*022b*/ 0x0807022eU,
+/*022c*/ 0x1002022eU,
+/*022d*/ 0x1801022eU,
+/*022e*/ 0x0001022fU,
+/*022f*/ 0x080a022fU,
+/*0230*/ 0x00140230U,
+/*0231*/ 0x000a0231U,
+/*0232*/ 0x00140232U,
+/*0233*/ 0x000a0233U,
+/*0234*/ 0x00140234U,
+/*0235*/ 0x18010234U,
+/*0236*/ 0x00100235U,
+/*0237*/ 0x10050235U,
+/*0238*/ 0x18010235U,
+/*0239*/ 0x00010236U,
+/*023a*/ 0x08010236U,
+/*023b*/ 0x10010236U,
+/*023c*/ 0x18010236U,
+/*023d*/ 0x00010237U,
+/*023e*/ 0x08010237U,
+/*023f*/ 0x10020237U,
+/*0240*/ 0x18020237U,
+/*0241*/ 0x00020238U,
+/*0242*/ 0x08020238U,
+/*0243*/ 0x10020238U,
+/*0244*/ 0x18030238U,
+/*0245*/ 0x00010239U,
+/*0246*/ 0x08010239U,
+/*0247*/ 0x10010239U,
+/*0248*/ 0x18010239U,
+/*0249*/ 0xffffffffU,
+/*024a*/ 0x0002023aU,
+/*024b*/ 0x0801023aU,
+/*024c*/ 0x1001023aU,
+/*024d*/ 0xffffffffU,
+/*024e*/ 0x1802023aU,
+/*024f*/ 0x0001023bU,
+/*0250*/ 0x0801023bU,
+/*0251*/ 0xffffffffU,
+/*0252*/ 0x1002023bU,
+/*0253*/ 0x1801023bU,
+/*0254*/ 0x0001023cU,
+/*0255*/ 0xffffffffU,
+/*0256*/ 0x0802023cU,
+/*0257*/ 0x1007023cU,
+/*0258*/ 0x1801023cU,
+/*0259*/ 0x0001023dU,
+/*025a*/ 0x0801023dU,
+/*025b*/ 0x1001023dU,
+/*025c*/ 0x1801023dU,
+/*025d*/ 0x0001023eU,
+/*025e*/ 0x0801023eU,
+/*025f*/ 0x1001023eU,
+/*0260*/ 0x1804023eU,
+/*0261*/ 0x0004023fU,
+/*0262*/ 0x0804023fU,
+/*0263*/ 0x1001023fU,
+/*0264*/ 0x1802023fU,
+/*0265*/ 0x00060240U,
+/*0266*/ 0x08060240U,
+/*0267*/ 0x10020240U,
+/*0268*/ 0x18020240U,
+/*0269*/ 0x00020241U,
+/*026a*/ 0xffffffffU,
+/*026b*/ 0x08100241U,
+/*026c*/ 0x18010241U,
+/*026d*/ 0x00010242U,
+/*026e*/ 0x08010242U,
+/*026f*/ 0x10040242U,
+/*0270*/ 0x18010242U,
+/*0271*/ 0x00040243U,
+/*0272*/ 0x08020243U,
+/*0273*/ 0x10080243U,
+/*0274*/ 0xffffffffU,
+/*0275*/ 0xffffffffU,
+/*0276*/ 0xffffffffU,
+/*0277*/ 0x000a0244U,
+/*0278*/ 0x00200245U,
+/*0279*/ 0x00200246U,
+/*027a*/ 0x00050247U,
+/*027b*/ 0x08010247U,
+/*027c*/ 0x10050247U,
+/*027d*/ 0x18080247U,
+/*027e*/ 0x00010248U,
+/*027f*/ 0x08080248U,
+/*0280*/ 0x10010248U,
+/*0281*/ 0x18080248U,
+/*0282*/ 0x00010249U,
+/*0283*/ 0x08040249U,
+/*0284*/ 0x10040249U,
+/*0285*/ 0x18040249U,
+/*0286*/ 0x0004024aU,
+/*0287*/ 0x0804024aU,
+/*0288*/ 0x1004024aU,
+/*0289*/ 0x1804024aU,
+/*028a*/ 0x0004024bU,
+/*028b*/ 0x0804024bU,
+/*028c*/ 0x1004024bU,
+/*028d*/ 0x1801024bU,
+/*028e*/ 0x0004024cU,
+/*028f*/ 0x0804024cU,
+/*0290*/ 0x1004024cU,
+/*0291*/ 0x1804024cU,
+/*0292*/ 0x0004024dU,
+/*0293*/ 0x0804024dU,
+/*0294*/ 0x1006024dU,
+/*0295*/ 0x1806024dU,
+/*0296*/ 0x0006024eU,
+/*0297*/ 0x0806024eU,
+/*0298*/ 0x1006024eU,
+/*0299*/ 0x1806024eU,
+/*029a*/ 0xffffffffU,
+/*029b*/ 0x0001024fU,
+/*029c*/ 0x0801024fU,
+/*029d*/ 0x1002024fU,
+/*029e*/ 0xffffffffU,
+/*029f*/ 0xffffffffU,
+/*02a0*/ 0xffffffffU,
+/*02a1*/ 0xffffffffU,
+/*02a2*/ 0xffffffffU,
+/*02a3*/ 0xffffffffU,
+/*02a4*/ 0xffffffffU,
+/*02a5*/ 0xffffffffU,
+/*02a6*/ 0x1804024fU,
+/*02a7*/ 0x00040250U,
+/*02a8*/ 0x08010250U,
+/*02a9*/ 0x10010250U,
+/*02aa*/ 0x18010250U,
+/*02ab*/ 0x00010251U,
+/*02ac*/ 0x08010251U,
+/*02ad*/ 0x10010251U,
+/*02ae*/ 0x18010251U,
+/*02af*/ 0x00010252U,
+/*02b0*/ 0x08010252U,
+/*02b1*/ 0x10040252U,
+/*02b2*/ 0x18040252U,
+/*02b3*/ 0x000a0253U,
+/*02b4*/ 0x00200254U,
+/*02b5*/ 0x00040255U,
+/*02b6*/ 0x08080255U,
+/*02b7*/ 0x10020255U,
+/*02b8*/ 0x18020255U,
+/*02b9*/ 0x00020256U,
+/*02ba*/ 0x08020256U,
+/*02bb*/ 0x10020256U,
+/*02bc*/ 0x18020256U,
+/*02bd*/ 0xffffffffU,
+/*02be*/ 0xffffffffU,
+/*02bf*/ 0x00200257U,
+/*02c0*/ 0x00020258U,
+/*02c1*/ 0x08100258U,
+/*02c2*/ 0x00100259U,
+/*02c3*/ 0x10040259U,
+/*02c4*/ 0x18040259U,
+/*02c5*/ 0x0005025aU,
+/*02c6*/ 0x0805025aU,
+/*02c7*/ 0x0020025bU,
+/*02c8*/ 0x0020025cU,
+/*02c9*/ 0x0020025dU,
+/*02ca*/ 0x0020025eU,
+/*02cb*/ 0x0001025fU,
+/*02cc*/ 0x0801025fU,
+/*02cd*/ 0x1007025fU,
+/*02ce*/ 0x1807025fU,
+/*02cf*/ 0x00070260U,
+/*02d0*/ 0x08070260U,
+/*02d1*/ 0x10070260U,
+/*02d2*/ 0x18070260U,
+/*02d3*/ 0x00070261U,
+/*02d4*/ 0x08070261U,
+/*02d5*/ 0x10070261U,
+/*02d6*/ 0x18070261U,
+/*02d7*/ 0x00070262U,
+/*02d8*/ 0x08070262U,
+/*02d9*/ 0x10070262U,
+/*02da*/ 0x18070262U,
+/*02db*/ 0x00030263U,
+/*02dc*/ 0x08030263U,
+/*02dd*/ 0x10030263U,
+/*02de*/ 0xffffffffU,
+/*02df*/ 0x18010263U,
+/*02e0*/ 0x00020264U,
+/*02e1*/ 0x08010264U,
+/*02e2*/ 0x10040264U,
+/*02e3*/ 0x18020264U,
+/*02e4*/ 0x00010265U,
+/*02e5*/ 0x08010265U,
+/*02e6*/ 0x10010265U,
+/*02e7*/ 0x18010265U,
+/*02e8*/ 0x00040266U,
+/*02e9*/ 0x08080266U,
+/*02ea*/ 0x100a0266U,
+/*02eb*/ 0x000a0267U,
+/*02ec*/ 0x100a0267U,
+/*02ed*/ 0x000a0268U,
+/*02ee*/ 0x100a0268U,
+/*02ef*/ 0x000a0269U,
+/*02f0*/ 0x0020026aU,
+/*02f1*/ 0x0020026bU,
+/*02f2*/ 0x0001026cU,
+/*02f3*/ 0x0802026cU,
+/*02f4*/ 0x1002026cU,
+/*02f5*/ 0x1802026cU,
+/*02f6*/ 0xffffffffU,
+/*02f7*/ 0x0002026dU,
+/*02f8*/ 0x0810026dU,
+/*02f9*/ 0x1805026dU,
+/*02fa*/ 0x0006026eU,
+/*02fb*/ 0x0805026eU,
+/*02fc*/ 0x1005026eU,
+/*02fd*/ 0x000e026fU,
+/*02fe*/ 0x1005026fU,
+/*02ff*/ 0x000e0270U,
+/*0300*/ 0x10050270U,
+/*0301*/ 0x000e0271U,
+/*0302*/ 0x10050271U,
+/*0303*/ 0x18010271U,
+/*0304*/ 0x00050272U,
+/*0305*/ 0x08050272U,
+/*0306*/ 0x100a0272U,
+/*0307*/ 0x000a0273U,
+/*0308*/ 0x10050273U,
+/*0309*/ 0x18050273U,
+/*030a*/ 0x000a0274U,
+/*030b*/ 0x100a0274U,
+/*030c*/ 0x00050275U,
+/*030d*/ 0x08050275U,
+/*030e*/ 0x100a0275U,
+/*030f*/ 0x000a0276U,
+/*0310*/ 0xffffffffU,
+/*0311*/ 0xffffffffU,
+/*0312*/ 0xffffffffU,
+/*0313*/ 0xffffffffU,
+/*0314*/ 0xffffffffU,
+/*0315*/ 0xffffffffU,
+/*0316*/ 0x10070276U,
+/*0317*/ 0x18070276U,
+/*0318*/ 0x00040277U,
+/*0319*/ 0x08040277U,
+/*031a*/ 0xffffffffU,
+/*031b*/ 0xffffffffU,
+/*031c*/ 0xffffffffU,
+/*031d*/ 0x10040277U,
+/*031e*/ 0x18080277U,
+/*031f*/ 0x00080278U,
+/*0320*/ 0x08040278U,
+/*0321*/ 0xffffffffU,
+/*0322*/ 0xffffffffU,
+/*0323*/ 0xffffffffU,
+/*0324*/ 0x10040278U,
+/*0325*/ 0xffffffffU,
+/*0326*/ 0xffffffffU,
+/*0327*/ 0xffffffffU,
+/*0328*/ 0x18040278U,
+/*0329*/ 0xffffffffU,
+/*032a*/ 0xffffffffU,
+/*032b*/ 0xffffffffU,
+/*032c*/ 0x00040279U,
+/*032d*/ 0x08050279U,
+/*032e*/ 0x10070279U,
+/*032f*/ 0x18080279U,
+/*0330*/ 0x0010027aU,
+/*0331*/ 0x1008027aU,
+/*0332*/ 0x0010027bU,
+/*0333*/ 0x1008027bU,
+/*0334*/ 0x0010027cU,
+/*0335*/ 0x1008027cU,
+/*0336*/ 0x1808027cU,
+/*0337*/ 0x0001027dU,
+/*0338*/ 0x0801027dU,
+/*0339*/ 0x1006027dU,
+/*033a*/ 0x1806027dU,
+/*033b*/ 0x0006027eU,
+/*033c*/ 0x0801027eU,
+/*033d*/ 0x1001027eU,
+/*033e*/ 0x1803027eU,
+/*033f*/ 0x000a027fU,
+/*0340*/ 0x100a027fU,
+/*0341*/ 0x000a0280U,
+/*0342*/ 0xffffffffU,
+/*0343*/ 0x100a0280U,
+/*0344*/ 0x00040281U,
+/*0345*/ 0x08010281U,
+/*0346*/ 0x10040281U,
+/*0347*/ 0xffffffffU,
+/*0348*/ 0xffffffffU,
+/*0349*/ 0xffffffffU,
+/*034a*/ 0xffffffffU,
+/*034b*/ 0xffffffffU,
+/*034c*/ 0xffffffffU,
+/*034d*/ 0x18070281U,
+/*034e*/ 0x00070282U,
+/*034f*/ 0x08050282U,
+/*0350*/ 0x10050282U,
+/*0351*/ 0xffffffffU,
+/*0352*/ 0xffffffffU,
+/*0353*/ 0xffffffffU,
+/*0354*/ 0x18040282U,
+/*0355*/ 0x00010283U,
+/*0356*/ 0x08010283U,
+/*0357*/ 0x10020283U,
+/*0358*/ 0x18080283U,
+/*0359*/ 0x00200284U,
+/*035a*/ 0x00200285U,
+/*035b*/ 0x00100286U,
+/*035c*/ 0x10020286U,
+/*035d*/ 0x18020286U,
+/*035e*/ 0x00020287U,
+/*035f*/ 0xffffffffU,
+/*0360*/ 0x08010287U,
+/*0361*/ 0x10010287U,
+/*0362*/ 0x18020287U,
+/*0363*/ 0x00080288U,
+/*0364*/ 0x08080288U,
+/*0365*/ 0x10080288U,
+/*0366*/ 0x18080288U,
+/*0367*/ 0x00080289U,
+/*0368*/ 0x08080289U,
+/*0369*/ 0xffffffffU,
+/*036a*/ 0x10080289U,
+/*036b*/ 0x18080289U,
+/*036c*/ 0x0008028aU,
+/*036d*/ 0x0808028aU,
+/*036e*/ 0x1008028aU,
+/*036f*/ 0x1808028aU,
+/*0370*/ 0xffffffffU,
+/*0371*/ 0x0008028bU,
+/*0372*/ 0x0808028bU,
+/*0373*/ 0x1008028bU,
+/*0374*/ 0x1808028bU,
+/*0375*/ 0x0008028cU,
+/*0376*/ 0x0808028cU,
+/*0377*/ 0xffffffffU,
+/*0378*/ 0x1008028cU,
+/*0379*/ 0x1808028cU,
+/*037a*/ 0x0008028dU,
+/*037b*/ 0x0808028dU,
+/*037c*/ 0x1008028dU,
+/*037d*/ 0x1808028dU,
+/*037e*/ 0x0008028eU,
+/*037f*/ 0xffffffffU,
+/*0380*/ 0x0808028eU,
+/*0381*/ 0x1008028eU,
+/*0382*/ 0x1808028eU,
+/*0383*/ 0x0008028fU,
+/*0384*/ 0x0808028fU,
+/*0385*/ 0x1008028fU,
+/*0386*/ 0xffffffffU,
+/*0387*/ 0x1808028fU,
+/*0388*/ 0x00080290U,
+/*0389*/ 0x08080290U,
+/*038a*/ 0x10080290U,
+/*038b*/ 0x18080290U,
+/*038c*/ 0x00080291U,
+/*038d*/ 0xffffffffU,
+/*038e*/ 0x08080291U,
+/*038f*/ 0x10080291U,
+/*0390*/ 0x18080291U,
+/*0391*/ 0x00080292U,
+/*0392*/ 0x08080292U,
+/*0393*/ 0x10080292U,
+/*0394*/ 0x18080292U,
+/*0395*/ 0xffffffffU,
+/*0396*/ 0x00080293U,
+/*0397*/ 0x08080293U,
+/*0398*/ 0x10080293U,
+/*0399*/ 0x18080293U,
+/*039a*/ 0x00080294U,
+/*039b*/ 0x08080294U,
+/*039c*/ 0xffffffffU,
+/*039d*/ 0x10080294U,
+/*039e*/ 0x18080294U,
+/*039f*/ 0x00080295U,
+/*03a0*/ 0x08080295U,
+/*03a1*/ 0x10080295U,
+/*03a2*/ 0x18080295U,
+/*03a3*/ 0xffffffffU,
+/*03a4*/ 0x00080296U,
+/*03a5*/ 0x08080296U,
+/*03a6*/ 0x10080296U,
+/*03a7*/ 0x18080296U,
+/*03a8*/ 0x00080297U,
+/*03a9*/ 0x08080297U,
+/*03aa*/ 0x10080297U,
+/*03ab*/ 0xffffffffU,
+/*03ac*/ 0x18080297U,
+/*03ad*/ 0x00080298U,
+/*03ae*/ 0x08080298U,
+/*03af*/ 0x10080298U,
+/*03b0*/ 0x18080298U,
+/*03b1*/ 0x00080299U,
+/*03b2*/ 0xffffffffU,
+/*03b3*/ 0x08080299U,
+/*03b4*/ 0x10080299U,
+/*03b5*/ 0x18080299U,
+/*03b6*/ 0x0008029aU,
+/*03b7*/ 0x0808029aU,
+/*03b8*/ 0x1008029aU,
+/*03b9*/ 0xffffffffU,
+/*03ba*/ 0x1808029aU,
+/*03bb*/ 0x0002029bU,
+/*03bc*/ 0x0803029bU,
+/*03bd*/ 0x100a029bU,
+/*03be*/ 0x000a029cU,
+/*03bf*/ 0x100a029cU,
+/*03c0*/ 0x0005029dU,
+/*03c1*/ 0x0808029dU,
+/*03c2*/ 0x1008029dU,
+/*03c3*/ 0x1808029dU,
+/*03c4*/ 0x0006029eU,
+/*03c5*/ 0x0806029eU,
+/*03c6*/ 0x0011029fU,
+/*03c7*/ 0x1808029fU,
+/*03c8*/ 0x000402a0U,
+/*03c9*/ 0x080602a0U,
+/*03ca*/ 0xffffffffU,
+/*03cb*/ 0x100602a0U,
+/*03cc*/ 0x180802a0U,
+/*03cd*/ 0xffffffffU,
+/*03ce*/ 0x000802a1U,
+/*03cf*/ 0x080802a1U,
+/*03d0*/ 0x100802a1U,
+/*03d1*/ 0x180602a1U,
+/*03d2*/ 0x000602a2U,
+/*03d3*/ 0x081102a2U,
+/*03d4*/ 0x000802a3U,
+/*03d5*/ 0x080402a3U,
+/*03d6*/ 0x100602a3U,
+/*03d7*/ 0xffffffffU,
+/*03d8*/ 0x180602a3U,
+/*03d9*/ 0x000802a4U,
+/*03da*/ 0xffffffffU,
+/*03db*/ 0x080802a4U,
+/*03dc*/ 0x100802a4U,
+/*03dd*/ 0x180802a4U,
+/*03de*/ 0x000602a5U,
+/*03df*/ 0x080602a5U,
+/*03e0*/ 0x001102a6U,
+/*03e1*/ 0x180802a6U,
+/*03e2*/ 0x000402a7U,
+/*03e3*/ 0x080602a7U,
+/*03e4*/ 0xffffffffU,
+/*03e5*/ 0x100602a7U,
+/*03e6*/ 0x180802a7U,
+/*03e7*/ 0xffffffffU,
+/*03e8*/ 0x000402a8U,
+/*03e9*/ 0x080402a8U,
+/*03ea*/ 0x100402a8U,
+/*03eb*/ 0x180402a8U,
+/*03ec*/ 0x000402a9U,
+/*03ed*/ 0x080402a9U,
+/*03ee*/ 0x100402a9U,
+/*03ef*/ 0x180402a9U,
+/*03f0*/ 0x000402aaU,
+/*03f1*/ 0x080402aaU,
+/*03f2*/ 0x100402aaU,
+/*03f3*/ 0x180402aaU,
+/*03f4*/ 0x000402abU,
+/*03f5*/ 0x080402abU,
+/*03f6*/ 0x100402abU,
+/*03f7*/ 0x180402abU,
+/*03f8*/ 0x000402acU,
+/*03f9*/ 0x080402acU,
+/*03fa*/ 0x100402acU,
+/*03fb*/ 0x180402acU,
+/*03fc*/ 0x001202adU,
+/*03fd*/ 0x001102aeU,
+/*03fe*/ 0x001202afU,
+/*03ff*/ 0x002002b0U,
+/*0400*/ 0x002002b1U,
+/*0401*/ 0x002002b2U,
+/*0402*/ 0x002002b3U,
+/*0403*/ 0x002002b4U,
+/*0404*/ 0x002002b5U,
+/*0405*/ 0x002002b6U,
+/*0406*/ 0x002002b7U,
+/*0407*/ 0x002002b8U,
+/*0408*/ 0x000202b9U,
+/*0409*/ 0x080502b9U,
+/*040a*/ 0x100502b9U,
+/*040b*/ 0x180102b9U,
+/*040c*/ 0x000402baU,
+/*040d*/ 0x080402baU,
+/*040e*/ 0x100402baU,
+/*040f*/ 0x180402baU,
+/*0410*/ 0x000402bbU,
+/*0411*/ 0x080402bbU,
+/*0412*/ 0x100402bbU,
+/*0413*/ 0x180402bbU,
+/*0414*/ 0xffffffffU,
+/*0415*/ 0xffffffffU,
+/*0416*/ 0xffffffffU,
+/*0417*/ 0xffffffffU,
+/*0418*/ 0xffffffffU,
+/*0419*/ 0xffffffffU,
+/*041a*/ 0x000402bcU,
+/*041b*/ 0x080402bcU,
+/*041c*/ 0x100402bcU,
+/*041d*/ 0x180402bcU,
+/*041e*/ 0x000402bdU,
+/*041f*/ 0x080402bdU,
+/*0420*/ 0x100402bdU,
+/*0421*/ 0x180402bdU,
+/*0422*/ 0x000102beU,
+/*0423*/ 0x080202beU,
+/*0424*/ 0x100202beU,
+/*0425*/ 0x180202beU,
+/*0426*/ 0x000202bfU,
+/*0427*/ 0x080102bfU,
+/*0428*/ 0x100402bfU,
+/*0429*/ 0x001002c0U,
+/*042a*/ 0x002002c1U,
+/*042b*/ 0x001002c2U,
+/*042c*/ 0x002002c3U,
+/*042d*/ 0x001002c4U,
+/*042e*/ 0x002002c5U,
+/*042f*/ 0x000702c6U,
+/*0430*/ 0x080102c6U,
+/*0431*/ 0x100202c6U,
+/*0432*/ 0x180602c6U,
+/*0433*/ 0x000102c7U,
+/*0434*/ 0x080102c7U,
+/*0435*/ 0x002002c8U,
+/*0436*/ 0x000202c9U,
+/*0437*/ 0x002002caU,
+/*0438*/ 0x002002cbU,
+/*0439*/ 0x000c02ccU,
+/*043a*/ 0x100c02ccU,
+/*043b*/ 0x002002cdU,
+/*043c*/ 0x000302ceU,
+/*043d*/ 0x002002cfU,
+/*043e*/ 0x000302d0U,
+/*043f*/ 0x002002d1U,
+/*0440*/ 0x000302d2U,
+/*0441*/ 0x002002d3U,
+/*0442*/ 0x000302d4U,
+/*0443*/ 0x002002d5U,
+/*0444*/ 0x000302d6U,
+/*0445*/ 0x002002d7U,
+/*0446*/ 0x000302d8U,
+/*0447*/ 0x002002d9U,
+/*0448*/ 0x000302daU,
+/*0449*/ 0x002002dbU,
+/*044a*/ 0x000302dcU,
+/*044b*/ 0x002002ddU,
+/*044c*/ 0x000302deU,
+/*044d*/ 0x002002dfU,
+/*044e*/ 0x000302e0U,
+/*044f*/ 0x080302e0U,
+/*0450*/ 0x100202e0U,
+/*0451*/ 0x180202e0U,
+/*0452*/ 0x002002e1U,
+/*0453*/ 0x002002e2U,
+/*0454*/ 0x002002e3U,
+/*0455*/ 0x002002e4U,
+/*0456*/ 0x000402e5U,
+/*0457*/ 0x001e02e6U,
+/*0458*/ 0x001e02e7U,
+/*0459*/ 0x001e02e8U,
+/*045a*/ 0x001e02e9U,
+/*045b*/ 0x001e02eaU,
+/*045c*/ 0x001e02ebU,
+/*045d*/ 0x001e02ecU,
+/*045e*/ 0x001e02edU,
+/*045f*/ 0x000402eeU,
+/*0460*/ 0xffffffffU,
+/*0461*/ 0xffffffffU,
+/*0462*/ 0xffffffffU,
+/*0463*/ 0xffffffffU,
+/*0464*/ 0x080402eeU,
+/*0465*/ 0x100102eeU,
+/*0466*/ 0x180802eeU,
+/*0467*/ 0x000402efU,
+/*0468*/ 0x080102efU,
+/*0469*/ 0x100802efU,
+/*046a*/ 0x180402efU,
+/*046b*/ 0x000102f0U,
+/*046c*/ 0x080802f0U,
+/*046d*/ 0x100402f0U,
+/*046e*/ 0x180102f0U,
+/*046f*/ 0x000802f1U,
+/*0470*/ 0x080402f1U,
+/*0471*/ 0x100102f1U,
+/*0472*/ 0x180802f1U,
+/*0473*/ 0x000402f2U,
+/*0474*/ 0x080102f2U,
+/*0475*/ 0x100802f2U,
+/*0476*/ 0x180402f2U,
+/*0477*/ 0x000102f3U,
+/*0478*/ 0x080802f3U,
+/*0479*/ 0x100402f3U,
+/*047a*/ 0x180102f3U,
+/*047b*/ 0x000802f4U,
+/*047c*/ 0x080802f4U,
+/*047d*/ 0x100102f4U,
+/*047e*/ 0x180502f4U,
+/*047f*/ 0xffffffffU,
+/*0480*/ 0xffffffffU,
+/*0481*/ 0xffffffffU,
+/*0482*/ 0xffffffffU,
+/*0483*/ 0xffffffffU,
+/*0484*/ 0xffffffffU,
+/*0485*/ 0xffffffffU,
+/*0486*/ 0xffffffffU,
+/*0487*/ 0xffffffffU,
+/*0488*/ 0xffffffffU,
+/*0489*/ 0xffffffffU,
+/*048a*/ 0xffffffffU,
+/*048b*/ 0xffffffffU,
+/*048c*/ 0xffffffffU,
+/*048d*/ 0xffffffffU,
+/*048e*/ 0xffffffffU,
+/*048f*/ 0xffffffffU,
+/*0490*/ 0xffffffffU,
+/*0491*/ 0xffffffffU,
+/*0492*/ 0xffffffffU,
+/*0493*/ 0xffffffffU,
+/*0494*/ 0xffffffffU,
+	 },
+	{
+/*0000*/ 0x00200800U,
+/*0001*/ 0x00040801U,
+/*0002*/ 0x080b0801U,
+/*0003*/ 0x000a0802U,
+/*0004*/ 0x10020802U,
+/*0005*/ 0x18010802U,
+/*0006*/ 0x00060803U,
+/*0007*/ 0x08060803U,
+/*0008*/ 0x10060803U,
+/*0009*/ 0x18060803U,
+/*000a*/ 0x00060804U,
+/*000b*/ 0x08060804U,
+/*000c*/ 0x10050804U,
+/*000d*/ 0x18060804U,
+/*000e*/ 0x00060805U,
+/*000f*/ 0x08040805U,
+/*0010*/ 0x10030805U,
+/*0011*/ 0x00180806U,
+/*0012*/ 0x18030806U,
+/*0013*/ 0x00180807U,
+/*0014*/ 0x18020807U,
+/*0015*/ 0x0801085eU,
+/*0016*/ 0x00020808U,
+/*0017*/ 0x08010808U,
+/*0018*/ 0x10010808U,
+/*0019*/ 0x18020808U,
+/*001a*/ 0x00050809U,
+/*001b*/ 0x08050809U,
+/*001c*/ 0x10040809U,
+/*001d*/ 0xffffffffU,
+/*001e*/ 0x18040809U,
+/*001f*/ 0x0002080aU,
+/*0020*/ 0x0805080aU,
+/*0021*/ 0x1009080aU,
+/*0022*/ 0x0001080bU,
+/*0023*/ 0x0020080cU,
+/*0024*/ 0x001c080dU,
+/*0025*/ 0x0001080eU,
+/*0026*/ 0x0807080eU,
+/*0027*/ 0x1009080eU,
+/*0028*/ 0x000a080fU,
+/*0029*/ 0x1005080fU,
+/*002a*/ 0x1801080fU,
+/*002b*/ 0x10010810U,
+/*002c*/ 0x18020810U,
+/*002d*/ 0x00090810U,
+/*002e*/ 0x00090811U,
+/*002f*/ 0x10020811U,
+/*0030*/ 0x00200812U,
+/*0031*/ 0x00010813U,
+/*0032*/ 0x08020813U,
+/*0033*/ 0x00200814U,
+/*0034*/ 0x00200815U,
+/*0035*/ 0x00200816U,
+/*0036*/ 0x00200817U,
+/*0037*/ 0xffffffffU,
+/*0038*/ 0xffffffffU,
+/*0039*/ 0xffffffffU,
+/*003a*/ 0xffffffffU,
+/*003b*/ 0x00030818U,
+/*003c*/ 0x08010818U,
+/*003d*/ 0x10040818U,
+/*003e*/ 0x18030818U,
+/*003f*/ 0x00040819U,
+/*0040*/ 0x08040819U,
+/*0041*/ 0x10040819U,
+/*0042*/ 0x18040819U,
+/*0043*/ 0x0001081aU,
+/*0044*/ 0x0801081aU,
+/*0045*/ 0x1006081aU,
+/*0046*/ 0x1804081aU,
+/*0047*/ 0x0008081bU,
+/*0048*/ 0x0806081bU,
+/*0049*/ 0x1004081bU,
+/*004a*/ 0x1806081bU,
+/*004b*/ 0x0004081cU,
+/*004c*/ 0x0802081cU,
+/*004d*/ 0x1005081cU,
+/*004e*/ 0x1808081cU,
+/*004f*/ 0xffffffffU,
+/*0050*/ 0x0006081dU,
+/*0051*/ 0x0803081dU,
+/*0052*/ 0x100b081dU,
+/*0053*/ 0x0004081eU,
+/*0054*/ 0x0804081eU,
+/*0055*/ 0x1004081eU,
+/*0056*/ 0x1801081eU,
+/*0057*/ 0xffffffffU,
+/*0058*/ 0x0009081fU,
+/*0059*/ 0x00200820U,
+/*005a*/ 0x00200821U,
+/*005b*/ 0x00200822U,
+/*005c*/ 0x00200823U,
+/*005d*/ 0x00100824U,
+/*005e*/ 0xffffffffU,
+/*005f*/ 0x10010824U,
+/*0060*/ 0x18060824U,
+/*0061*/ 0x00080825U,
+/*0062*/ 0x00200826U,
+/*0063*/ 0x00100827U,
+/*0064*/ 0x100b0827U,
+/*0065*/ 0x00070828U,
+/*0066*/ 0x08070828U,
+/*0067*/ 0x10090828U,
+/*0068*/ 0x00090829U,
+/*0069*/ 0x100b0829U,
+/*006a*/ 0x0007082aU,
+/*006b*/ 0x0808082aU,
+/*006c*/ 0x1009082aU,
+/*006d*/ 0x0003082bU,
+/*006e*/ 0x080a082bU,
+/*006f*/ 0x000a082cU,
+/*0070*/ 0x0011082dU,
+/*0071*/ 0x000a082eU,
+/*0072*/ 0x100a082eU,
+/*0073*/ 0x0010082fU,
+/*0074*/ 0x100e082fU,
+/*0075*/ 0x000e0830U,
+/*0076*/ 0x00120831U,
+/*0077*/ 0x000a0832U,
+/*0078*/ 0x100a0832U,
+/*0079*/ 0x00020833U,
+/*007a*/ 0x00200834U,
+/*007b*/ 0x000b0835U,
+/*007c*/ 0x100b0835U,
+/*007d*/ 0x00200836U,
+/*007e*/ 0x00130837U,
+/*007f*/ 0x00200838U,
+/*0080*/ 0x00200839U,
+/*0081*/ 0x0008083aU,
+/*0082*/ 0x0801083aU,
+/*0083*/ 0x1001083aU,
+/*0084*/ 0x1801083aU,
+/*0085*/ 0x0008083bU,
+/*0086*/ 0x080c083bU,
+/*0087*/ 0x000c083cU,
+/*0088*/ 0x100c083cU,
+/*0089*/ 0x000c083dU,
+/*008a*/ 0x100c083dU,
+/*008b*/ 0x000c083eU,
+/*008c*/ 0x100c083eU,
+/*008d*/ 0x000c083fU,
+/*008e*/ 0x100c083fU,
+/*008f*/ 0x000c0840U,
+/*0090*/ 0x100c0840U,
+/*0091*/ 0x000b0841U,
+/*0092*/ 0x10090841U,
+/*0093*/ 0x00010842U,
+/*0094*/ 0x000b0843U,
+/*0095*/ 0x100b0843U,
+/*0096*/ 0x000b0844U,
+/*0097*/ 0x100b0844U,
+/*0098*/ 0x000b0845U,
+/*0099*/ 0x100b0845U,
+/*009a*/ 0x000b0846U,
+/*009b*/ 0x100b0846U,
+/*009c*/ 0x000b0847U,
+/*009d*/ 0x100a0847U,
+/*009e*/ 0x00020848U,
+/*009f*/ 0x080a0848U,
+/*00a0*/ 0x000a0849U,
+/*00a1*/ 0x100a0849U,
+/*00a2*/ 0x000a084aU,
+/*00a3*/ 0x100a084aU,
+/*00a4*/ 0x000a084bU,
+/*00a5*/ 0x100a084bU,
+/*00a6*/ 0x000a084cU,
+/*00a7*/ 0x100a084cU,
+/*00a8*/ 0x000a084dU,
+/*00a9*/ 0x100a084dU,
+/*00aa*/ 0x000a084eU,
+/*00ab*/ 0x100a084eU,
+/*00ac*/ 0x000a084fU,
+/*00ad*/ 0x100a084fU,
+/*00ae*/ 0x000a0850U,
+/*00af*/ 0x100a0850U,
+/*00b0*/ 0x000a0851U,
+/*00b1*/ 0x100a0851U,
+/*00b2*/ 0x000a0852U,
+/*00b3*/ 0x100a0852U,
+/*00b4*/ 0x000a0853U,
+/*00b5*/ 0x100a0853U,
+/*00b6*/ 0x000a0854U,
+/*00b7*/ 0x100a0854U,
+/*00b8*/ 0x000a0855U,
+/*00b9*/ 0x100a0855U,
+/*00ba*/ 0x000a0856U,
+/*00bb*/ 0x10040856U,
+/*00bc*/ 0x18030856U,
+/*00bd*/ 0x000a0857U,
+/*00be*/ 0x100a0857U,
+/*00bf*/ 0x00010858U,
+/*00c0*/ 0x080a0858U,
+/*00c1*/ 0x18040858U,
+/*00c2*/ 0x000b0859U,
+/*00c3*/ 0x100a0859U,
+/*00c4*/ 0x0003085aU,
+/*00c5*/ 0x0008085bU,
+/*00c6*/ 0x0808085bU,
+/*00c7*/ 0x1008085bU,
+/*00c8*/ 0x1808085bU,
+/*00c9*/ 0x0008085cU,
+/*00ca*/ 0x0808085cU,
+/*00cb*/ 0x1008085cU,
+/*00cc*/ 0x1801085cU,
+/*00cd*/ 0x0008085dU,
+/*00ce*/ 0x0808085dU,
+/*00cf*/ 0x1002085dU,
+/*00d0*/ 0x1802085dU,
+/*00d1*/ 0x0005085eU,
+/*00d2*/ 0x1005085eU,
+/*00d3*/ 0x1805085eU,
+/*00d4*/ 0x0004085fU,
+/*00d5*/ 0x080b085fU,
+/*00d6*/ 0x1806085fU,
+/*00d7*/ 0x00080860U,
+/*00d8*/ 0x08080860U,
+/*00d9*/ 0x10040860U,
+/*00da*/ 0x18040860U,
+/*00db*/ 0x00060861U,
+/*00dc*/ 0x08040861U,
+/*00dd*/ 0x10050861U,
+/*00de*/ 0x000a0862U,
+/*00df*/ 0x100a0862U,
+/*00e0*/ 0x00080863U,
+/*00e1*/ 0x08010863U,
+/*00e2*/ 0x10040863U,
+/*00e3*/ 0x00020864U,
+/*00e4*/ 0x08030864U,
+/*00e5*/ 0x00050a00U,
+/*00e6*/ 0x08050a00U,
+/*00e7*/ 0x10050a00U,
+/*00e8*/ 0x18050a00U,
+/*00e9*/ 0x00050a01U,
+/*00ea*/ 0x08050a01U,
+/*00eb*/ 0x100b0a01U,
+/*00ec*/ 0x00010a02U,
+/*00ed*/ 0x08030a02U,
+/*00ee*/ 0x00200a03U,
+/*00ef*/ 0x00100a04U,
+/*00f0*/ 0x10040a04U,
+/*00f1*/ 0x000b0a05U,
+/*00f2*/ 0x10070a05U,
+/*00f3*/ 0x00090a06U,
+/*00f4*/ 0x10030a06U,
+/*00f5*/ 0x18030a06U,
+/*00f6*/ 0x00010a07U,
+/*00f7*/ 0x08010a07U,
+/*00f8*/ 0x10070a07U,
+/*00f9*/ 0x18070a07U,
+/*00fa*/ 0x00050a08U,
+/*00fb*/ 0x08010a08U,
+/*00fc*/ 0x10020a08U,
+/*00fd*/ 0x18030a08U,
+/*00fe*/ 0x00010a09U,
+/*00ff*/ 0x080f0a09U,
+/*0100*/ 0x00200a0aU,
+/*0101*/ 0x00200a0bU,
+/*0102*/ 0x000b0a0cU,
+/*0103*/ 0x100b0a0cU,
+/*0104*/ 0x000b0a0dU,
+/*0105*/ 0x00180a0eU,
+/*0106*/ 0x00180a0fU,
+/*0107*/ 0xffffffffU,
+/*0108*/ 0xffffffffU,
+/*0109*/ 0xffffffffU,
+/*010a*/ 0xffffffffU,
+/*010b*/ 0xffffffffU,
+/*010c*/ 0x18020a0fU,
+/*010d*/ 0x00020a10U,
+/*010e*/ 0x08040a10U,
+/*010f*/ 0x10040a10U,
+/*0110*/ 0x18010a10U,
+/*0111*/ 0x00010a11U,
+/*0112*/ 0x08010a11U,
+/*0113*/ 0x10030a11U,
+/*0114*/ 0x00200a12U,
+/*0115*/ 0x00200a13U,
+/*0116*/ 0xffffffffU,
+/*0117*/ 0x00140a14U,
+/*0118*/ 0x00140a15U,
+/*0119*/ 0x00140a16U,
+/*011a*/ 0x00140a17U,
+/*011b*/ 0x00140a18U,
+/*011c*/ 0x00140a19U,
+/*011d*/ 0x00140a1aU,
+/*011e*/ 0x00140a1bU,
+/*011f*/ 0x001e0a1cU,
+/*0120*/ 0x000a0a1dU,
+/*0121*/ 0x10060a1dU,
+/*0122*/ 0x18060a1dU,
+/*0123*/ 0x00060a1eU,
+/*0124*/ 0x08060a1eU,
+/*0125*/ 0x10060a1eU,
+/*0126*/ 0x00080a1fU,
+/*0127*/ 0x080b0a1fU,
+/*0128*/ 0x000b0a20U,
+/*0129*/ 0x100b0a20U,
+/*012a*/ 0x000b0a21U,
+/*012b*/ 0x100b0a21U,
+/*012c*/ 0x000b0a22U,
+/*012d*/ 0x10040a22U,
+/*012e*/ 0x000b0a23U,
+/*012f*/ 0x10060a23U,
+/*0130*/ 0x18080a23U,
+/*0131*/ 0x00080a24U,
+/*0132*/ 0x08040a24U,
+/*0133*/ 0x00020b80U,
+/*0134*/ 0x00010b81U,
+/*0135*/ 0x08010b81U,
+/*0136*/ 0x10020b81U,
+/*0137*/ 0x18050b81U,
+/*0138*/ 0x00050b82U,
+/*0139*/ 0x08050b82U,
+/*013a*/ 0x10050b82U,
+/*013b*/ 0x000b0b83U,
+/*013c*/ 0x10050b83U,
+/*013d*/ 0x18010b83U,
+/*013e*/ 0x00010b84U,
+/*013f*/ 0x08010b84U,
+/*0140*/ 0x10010b84U,
+/*0141*/ 0x18010b84U,
+/*0142*/ 0x00040b85U,
+/*0143*/ 0x080b0b85U,
+/*0144*/ 0x000b0b86U,
+/*0145*/ 0x100b0b86U,
+/*0146*/ 0x00040b87U,
+/*0147*/ 0x080b0b87U,
+/*0148*/ 0x18040b87U,
+/*0149*/ 0x00010b88U,
+/*014a*/ 0x08010b88U,
+/*014b*/ 0x10010b88U,
+/*014c*/ 0x00200b89U,
+/*014d*/ 0x00200b8aU,
+/*014e*/ 0x00080b8bU,
+/*014f*/ 0x080a0b8bU,
+/*0150*/ 0x18050b8bU,
+/*0151*/ 0x000b0b8cU,
+/*0152*/ 0x10030b8cU,
+/*0153*/ 0x18030b8cU,
+/*0154*/ 0x00010b8dU,
+/*0155*/ 0x08020b8dU,
+/*0156*/ 0x10010b8dU,
+/*0157*/ 0x18010b8dU,
+/*0158*/ 0x00010b8eU,
+/*0159*/ 0xffffffffU,
+/*015a*/ 0x08010b8eU,
+/*015b*/ 0x18040b8eU,
+/*015c*/ 0x00040b8fU,
+/*015d*/ 0x08040b8fU,
+/*015e*/ 0x10040b8fU,
+/*015f*/ 0x18010b8fU,
+/*0160*/ 0x00010b90U,
+/*0161*/ 0x08010b90U,
+/*0162*/ 0x00200b91U,
+/*0163*/ 0x00200b92U,
+/*0164*/ 0x00200b93U,
+/*0165*/ 0x00200b94U,
+/*0166*/ 0xffffffffU,
+/*0167*/ 0x10010b8eU,
+/*0168*/ 0x000d0b96U,
+/*0169*/ 0x100d0b96U,
+/*016a*/ 0x000d0b97U,
+/*016b*/ 0x00050b98U,
+/*016c*/ 0x00010b99U,
+/*016d*/ 0x080e0b99U,
+/*016e*/ 0x000e0b9aU,
+/*016f*/ 0x100e0b9aU,
+/*0170*/ 0x000e0b9bU,
+/*0171*/ 0x100e0b9bU,
+/*0172*/ 0x00040b9cU,
+/*0173*/ 0x08040b9cU,
+/*0174*/ 0x10040b9cU,
+/*0175*/ 0x18040b9cU,
+/*0176*/ 0x00040b9dU,
+/*0177*/ 0x080b0b9dU,
+/*0178*/ 0x000b0b9eU,
+/*0179*/ 0x100b0b9eU,
+/*017a*/ 0x000b0b9fU,
+/*017b*/ 0x00040ba0U,
+/*017c*/ 0x08040ba0U,
+/*017d*/ 0x10040ba0U,
+/*017e*/ 0x18040ba0U,
+/*017f*/ 0x000d0ba1U,
+/*0180*/ 0x100d0ba1U,
+/*0181*/ 0x000d0ba2U,
+/*0182*/ 0x10100ba2U,
+/*0183*/ 0x00080b95U,
+/*0184*/ 0x08080b95U,
+/*0185*/ 0x00100ba3U,
+/*0186*/ 0x10100ba3U,
+/*0187*/ 0x00100ba4U,
+/*0188*/ 0x10100ba4U,
+/*0189*/ 0x00100ba5U,
+/*018a*/ 0x10030ba5U,
+/*018b*/ 0x18040ba5U,
+/*018c*/ 0x00010ba6U,
+/*018d*/ 0x08080ba6U,
+/*018e*/ 0x10010ba6U,
+/*018f*/ 0x000a0ba7U,
+/*0190*/ 0x10010ba7U,
+/*0191*/ 0x00140ba8U,
+/*0192*/ 0x000b0ba9U,
+/*0193*/ 0x100c0ba9U,
+/*0194*/ 0x00120baaU,
+/*0195*/ 0x00140babU,
+/*0196*/ 0x00120bacU,
+/*0197*/ 0x00110badU,
+/*0198*/ 0x00110baeU,
+/*0199*/ 0x00120bafU,
+/*019a*/ 0x00120bb0U,
+/*019b*/ 0x00120bb1U,
+/*019c*/ 0x00120bb2U,
+/*019d*/ 0x00120bb3U,
+/*019e*/ 0x00120bb4U,
+/*019f*/ 0x00120bb5U,
+/*01a0*/ 0x00120bb6U,
+/*01a1*/ 0x00120bb7U,
+/*01a2*/ 0x00120bb8U,
+/*01a3*/ 0x000e0bb9U,
+/*01a4*/ 0x100d0bb9U,
+/*01a5*/ 0x00200bbaU,
+/*01a6*/ 0x00170bbbU,
+/*01a7*/ 0x000d0bbcU,
+/*01a8*/ 0x10010bbcU,
+/*01a9*/ 0x18010bbcU,
+/*01aa*/ 0x00200bbdU,
+/*01ab*/ 0x00080bbeU,
+/*01ac*/ 0x08030bbeU,
+/*01ad*/ 0x10030bbeU,
+/*01ae*/ 0x00180bbfU,
+/*01af*/ 0x00180bc0U,
+/*01b0*/ 0x18070bc0U,
+/*01b1*/ 0x00070bc1U,
+/*01b2*/ 0x08080bc1U,
+/*01b3*/ 0x10080bc1U,
+/*01b4*/ 0x18080bc1U,
+/*01b5*/ 0x00010bc2U,
+/*01b6*/ 0x08010bc2U,
+/*01b7*/ 0x00200bc3U,
+/*01b8*/ 0x00070bc4U,
+/*01b9*/ 0x08140bc4U,
+/*01ba*/ 0x00140bc5U,
+/*01bb*/ 0x00190bc6U,
+/*01bc*/ 0x00170bc7U,
+/*01bd*/ 0x00110bc8U,
+/*01be*/ 0x00110bc9U,
+/*01bf*/ 0x00100bcaU,
+/*01c0*/ 0x10010bcaU,
+/*01c1*/ 0x18010bcaU,
+/*01c2*/ 0x00020bcbU,
+/*01c3*/ 0x08040bcbU,
+/*01c4*/ 0x10090bcbU,
+/*01c5*/ 0x00070bccU,
+/*01c6*/ 0x08040bccU,
+/*01c7*/ 0x00200bcdU,
+/*01c8*/ 0x00010bceU,
+/*01c9*/ 0x08020bceU,
+/*01ca*/ 0x10060bceU,
+/*01cb*/ 0x00100bcfU,
+/*01cc*/ 0x10010bcfU,
+/*01cd*/ 0x00200bd0U,
+/*01ce*/ 0x00080bd1U,
+/*01cf*/ 0x08010bd1U,
+/*01d0*/ 0x10050bd1U,
+/*01d1*/ 0x18030bd1U,
+/*01d2*/ 0x00020bd2U,
+/*01d3*/ 0xffffffffU,
+/*01d4*/ 0x00200bd3U,
+/*01d5*/ 0x000b0bd4U,
+/*01d6*/ 0xffffffffU,
+/*01d7*/ 0x10030bd4U,
+/*01d8*/ 0x18080bd4U,
+/*01d9*/ 0x00020bd5U,
+/*01da*/ 0x080c0bd5U,
+/*01db*/ 0x18040bd5U,
+/*01dc*/ 0x00010bd6U,
+/*01dd*/ 0x08050bd6U,
+/*01de*/ 0x00010200U,
+/*01df*/ 0x08040200U,
+/*01e0*/ 0x10100200U,
+/*01e1*/ 0x00010201U,
+/*01e2*/ 0x08010201U,
+/*01e3*/ 0x10010201U,
+/*01e4*/ 0x18010201U,
+/*01e5*/ 0x00100202U,
+/*01e6*/ 0x10080202U,
+/*01e7*/ 0x18010202U,
+/*01e8*/ 0x00200203U,
+/*01e9*/ 0x00200204U,
+/*01ea*/ 0x00200205U,
+/*01eb*/ 0x00200206U,
+/*01ec*/ 0x00020207U,
+/*01ed*/ 0x08010207U,
+/*01ee*/ 0x10010207U,
+/*01ef*/ 0x00200208U,
+/*01f0*/ 0x00140209U,
+/*01f1*/ 0x0020020aU,
+/*01f2*/ 0x0014020bU,
+/*01f3*/ 0x0020020cU,
+/*01f4*/ 0x0014020dU,
+/*01f5*/ 0x0014020eU,
+/*01f6*/ 0x0020020fU,
+/*01f7*/ 0x00200210U,
+/*01f8*/ 0x00200211U,
+/*01f9*/ 0x00200212U,
+/*01fa*/ 0x00140213U,
+/*01fb*/ 0x00200214U,
+/*01fc*/ 0x00200215U,
+/*01fd*/ 0x00200216U,
+/*01fe*/ 0x00200217U,
+/*01ff*/ 0x00140218U,
+/*0200*/ 0x00200219U,
+/*0201*/ 0x0020021aU,
+/*0202*/ 0x0020021bU,
+/*0203*/ 0x0020021cU,
+/*0204*/ 0x0009021dU,
+/*0205*/ 0x1001021dU,
+/*0206*/ 0x0020021eU,
+/*0207*/ 0x0005021fU,
+/*0208*/ 0x0801021fU,
+/*0209*/ 0x1008021fU,
+/*020a*/ 0x1808021fU,
+/*020b*/ 0x001e0220U,
+/*020c*/ 0x001e0221U,
+/*020d*/ 0x001e0222U,
+/*020e*/ 0x001e0223U,
+/*020f*/ 0x001e0224U,
+/*0210*/ 0x001e0225U,
+/*0211*/ 0x001e0226U,
+/*0212*/ 0x001e0227U,
+/*0213*/ 0x001e0228U,
+/*0214*/ 0x001e0229U,
+/*0215*/ 0x001e022aU,
+/*0216*/ 0x001e022bU,
+/*0217*/ 0x001e022cU,
+/*0218*/ 0x001e022dU,
+/*0219*/ 0x001e022eU,
+/*021a*/ 0x001e022fU,
+/*021b*/ 0x00010230U,
+/*021c*/ 0x08010230U,
+/*021d*/ 0x10010230U,
+/*021e*/ 0x18040230U,
+/*021f*/ 0x00080231U,
+/*0220*/ 0x08080231U,
+/*0221*/ 0x10080231U,
+/*0222*/ 0x18040231U,
+/*0223*/ 0x00070232U,
+/*0224*/ 0x08060232U,
+/*0225*/ 0x10070232U,
+/*0226*/ 0x18070232U,
+/*0227*/ 0x00060233U,
+/*0228*/ 0x08070233U,
+/*0229*/ 0x10070233U,
+/*022a*/ 0x18060233U,
+/*022b*/ 0x00070234U,
+/*022c*/ 0x08020234U,
+/*022d*/ 0x10010234U,
+/*022e*/ 0x18010234U,
+/*022f*/ 0x000a0235U,
+/*0230*/ 0x00140236U,
+/*0231*/ 0x000a0237U,
+/*0232*/ 0x00140238U,
+/*0233*/ 0x000a0239U,
+/*0234*/ 0x0014023aU,
+/*0235*/ 0xffffffffU,
+/*0236*/ 0xffffffffU,
+/*0237*/ 0x0005023bU,
+/*0238*/ 0x0001023cU,
+/*0239*/ 0x1001023cU,
+/*023a*/ 0x1801023cU,
+/*023b*/ 0x0001023dU,
+/*023c*/ 0x0801023dU,
+/*023d*/ 0x1001023dU,
+/*023e*/ 0x1801023dU,
+/*023f*/ 0x0002023eU,
+/*0240*/ 0x0802023eU,
+/*0241*/ 0x1002023eU,
+/*0242*/ 0x1802023eU,
+/*0243*/ 0x0002023fU,
+/*0244*/ 0x0803023fU,
+/*0245*/ 0x1001023fU,
+/*0246*/ 0x1801023fU,
+/*0247*/ 0x00010240U,
+/*0248*/ 0x08010240U,
+/*0249*/ 0x10010240U,
+/*024a*/ 0x18020240U,
+/*024b*/ 0x00010241U,
+/*024c*/ 0x08010241U,
+/*024d*/ 0x10010241U,
+/*024e*/ 0x18020241U,
+/*024f*/ 0x00010242U,
+/*0250*/ 0x08010242U,
+/*0251*/ 0x10010242U,
+/*0252*/ 0x18020242U,
+/*0253*/ 0x00010243U,
+/*0254*/ 0x08010243U,
+/*0255*/ 0x10010243U,
+/*0256*/ 0x18020243U,
+/*0257*/ 0xffffffffU,
+/*0258*/ 0x00010244U,
+/*0259*/ 0x08010244U,
+/*025a*/ 0x10010244U,
+/*025b*/ 0x18010244U,
+/*025c*/ 0x00010245U,
+/*025d*/ 0x08010245U,
+/*025e*/ 0x10010245U,
+/*025f*/ 0x18010245U,
+/*0260*/ 0x00040246U,
+/*0261*/ 0x08040246U,
+/*0262*/ 0x10040246U,
+/*0263*/ 0x18010246U,
+/*0264*/ 0x00020247U,
+/*0265*/ 0x08060247U,
+/*0266*/ 0x10060247U,
+/*0267*/ 0x18020247U,
+/*0268*/ 0x00020248U,
+/*0269*/ 0x08020248U,
+/*026a*/ 0xffffffffU,
+/*026b*/ 0x10100248U,
+/*026c*/ 0x00010249U,
+/*026d*/ 0x08010249U,
+/*026e*/ 0x10010249U,
+/*026f*/ 0x18040249U,
+/*0270*/ 0x0001024aU,
+/*0271*/ 0x0804024aU,
+/*0272*/ 0x1003024aU,
+/*0273*/ 0x1808024aU,
+/*0274*/ 0x000a024bU,
+/*0275*/ 0x100a024bU,
+/*0276*/ 0x000a024cU,
+/*0277*/ 0xffffffffU,
+/*0278*/ 0x0020024dU,
+/*0279*/ 0x0020024eU,
+/*027a*/ 0x0005024fU,
+/*027b*/ 0x1801023aU,
+/*027c*/ 0x0805023cU,
+/*027d*/ 0x0808024fU,
+/*027e*/ 0x1001024fU,
+/*027f*/ 0x1808024fU,
+/*0280*/ 0x00010250U,
+/*0281*/ 0x08080250U,
+/*0282*/ 0x10010250U,
+/*0283*/ 0x18040250U,
+/*0284*/ 0x00040251U,
+/*0285*/ 0x08040251U,
+/*0286*/ 0x10040251U,
+/*0287*/ 0x18040251U,
+/*0288*/ 0x00040252U,
+/*0289*/ 0x08040252U,
+/*028a*/ 0x10040252U,
+/*028b*/ 0x18040252U,
+/*028c*/ 0x00040253U,
+/*028d*/ 0x08010253U,
+/*028e*/ 0x10040253U,
+/*028f*/ 0x18040253U,
+/*0290*/ 0x00040254U,
+/*0291*/ 0x08040254U,
+/*0292*/ 0x10040254U,
+/*0293*/ 0x18040254U,
+/*0294*/ 0x00060255U,
+/*0295*/ 0x08060255U,
+/*0296*/ 0x10060255U,
+/*0297*/ 0x18060255U,
+/*0298*/ 0x00060256U,
+/*0299*/ 0x08060256U,
+/*029a*/ 0x10040256U,
+/*029b*/ 0x18010256U,
+/*029c*/ 0x00010257U,
+/*029d*/ 0x08020257U,
+/*029e*/ 0x00200258U,
+/*029f*/ 0x00200259U,
+/*02a0*/ 0x0020025aU,
+/*02a1*/ 0x0020025bU,
+/*02a2*/ 0x0020025cU,
+/*02a3*/ 0x0020025dU,
+/*02a4*/ 0x0020025eU,
+/*02a5*/ 0x0020025fU,
+/*02a6*/ 0x00040260U,
+/*02a7*/ 0x08040260U,
+/*02a8*/ 0x10010260U,
+/*02a9*/ 0x18010260U,
+/*02aa*/ 0x00010261U,
+/*02ab*/ 0x08010261U,
+/*02ac*/ 0x10010261U,
+/*02ad*/ 0x18010261U,
+/*02ae*/ 0x00010262U,
+/*02af*/ 0x08010262U,
+/*02b0*/ 0x10010262U,
+/*02b1*/ 0x18040262U,
+/*02b2*/ 0x00040263U,
+/*02b3*/ 0x080a0263U,
+/*02b4*/ 0x00200264U,
+/*02b5*/ 0x00040265U,
+/*02b6*/ 0x08080265U,
+/*02b7*/ 0x10020265U,
+/*02b8*/ 0x18020265U,
+/*02b9*/ 0x00020266U,
+/*02ba*/ 0x08020266U,
+/*02bb*/ 0x10020266U,
+/*02bc*/ 0x18020266U,
+/*02bd*/ 0xffffffffU,
+/*02be*/ 0xffffffffU,
+/*02bf*/ 0x00200267U,
+/*02c0*/ 0x00030268U,
+/*02c1*/ 0x08100268U,
+/*02c2*/ 0x00100269U,
+/*02c3*/ 0x10040269U,
+/*02c4*/ 0x18040269U,
+/*02c5*/ 0x0005026aU,
+/*02c6*/ 0x0805026aU,
+/*02c7*/ 0xffffffffU,
+/*02c8*/ 0xffffffffU,
+/*02c9*/ 0xffffffffU,
+/*02ca*/ 0xffffffffU,
+/*02cb*/ 0x1001026aU,
+/*02cc*/ 0x1801026aU,
+/*02cd*/ 0x0008026bU,
+/*02ce*/ 0x0808026bU,
+/*02cf*/ 0x1008026bU,
+/*02d0*/ 0x1808026bU,
+/*02d1*/ 0x0008026cU,
+/*02d2*/ 0x0808026cU,
+/*02d3*/ 0x1008026cU,
+/*02d4*/ 0x1808026cU,
+/*02d5*/ 0x0008026dU,
+/*02d6*/ 0x0808026dU,
+/*02d7*/ 0x1008026dU,
+/*02d8*/ 0x1808026dU,
+/*02d9*/ 0x0008026eU,
+/*02da*/ 0x0808026eU,
+/*02db*/ 0x1003026eU,
+/*02dc*/ 0x1803026eU,
+/*02dd*/ 0x0003026fU,
+/*02de*/ 0xffffffffU,
+/*02df*/ 0x0801026fU,
+/*02e0*/ 0x1002026fU,
+/*02e1*/ 0x1801026fU,
+/*02e2*/ 0x00040270U,
+/*02e3*/ 0x08020270U,
+/*02e4*/ 0x10010270U,
+/*02e5*/ 0x18010270U,
+/*02e6*/ 0x00010271U,
+/*02e7*/ 0x08010271U,
+/*02e8*/ 0x10040271U,
+/*02e9*/ 0x18080271U,
+/*02ea*/ 0x000a0272U,
+/*02eb*/ 0x100a0272U,
+/*02ec*/ 0x000a0273U,
+/*02ed*/ 0x100a0273U,
+/*02ee*/ 0x000a0274U,
+/*02ef*/ 0x100a0274U,
+/*02f0*/ 0x00200275U,
+/*02f1*/ 0x00200276U,
+/*02f2*/ 0x00010277U,
+/*02f3*/ 0x08020277U,
+/*02f4*/ 0x10020277U,
+/*02f5*/ 0x18020277U,
+/*02f6*/ 0xffffffffU,
+/*02f7*/ 0x00020278U,
+/*02f8*/ 0x08100278U,
+/*02f9*/ 0x18050278U,
+/*02fa*/ 0x00060279U,
+/*02fb*/ 0x08050279U,
+/*02fc*/ 0x10050279U,
+/*02fd*/ 0x000e027aU,
+/*02fe*/ 0x1005027aU,
+/*02ff*/ 0x000e027bU,
+/*0300*/ 0x1005027bU,
+/*0301*/ 0x000e027cU,
+/*0302*/ 0x1005027cU,
+/*0303*/ 0x1801027cU,
+/*0304*/ 0x0005027dU,
+/*0305*/ 0x0805027dU,
+/*0306*/ 0x100a027dU,
+/*0307*/ 0x000a027eU,
+/*0308*/ 0x1005027eU,
+/*0309*/ 0x1805027eU,
+/*030a*/ 0x000a027fU,
+/*030b*/ 0x100a027fU,
+/*030c*/ 0x00050280U,
+/*030d*/ 0x08050280U,
+/*030e*/ 0x100a0280U,
+/*030f*/ 0x000a0281U,
+/*0310*/ 0x10070281U,
+/*0311*/ 0x18070281U,
+/*0312*/ 0x00070282U,
+/*0313*/ 0x08070282U,
+/*0314*/ 0x10070282U,
+/*0315*/ 0x18070282U,
+/*0316*/ 0xffffffffU,
+/*0317*/ 0xffffffffU,
+/*0318*/ 0x00040283U,
+/*0319*/ 0x08040283U,
+/*031a*/ 0x10040283U,
+/*031b*/ 0x18040283U,
+/*031c*/ 0x00040284U,
+/*031d*/ 0xffffffffU,
+/*031e*/ 0x08080284U,
+/*031f*/ 0x10080284U,
+/*0320*/ 0x18040284U,
+/*0321*/ 0x00050285U,
+/*0322*/ 0x08080285U,
+/*0323*/ 0x10050285U,
+/*0324*/ 0x18040285U,
+/*0325*/ 0x00050286U,
+/*0326*/ 0x08080286U,
+/*0327*/ 0x10050286U,
+/*0328*/ 0x18040286U,
+/*0329*/ 0x00050287U,
+/*032a*/ 0x08080287U,
+/*032b*/ 0x10050287U,
+/*032c*/ 0x18040287U,
+/*032d*/ 0x00050288U,
+/*032e*/ 0x08070288U,
+/*032f*/ 0x10080288U,
+/*0330*/ 0x00100289U,
+/*0331*/ 0x10080289U,
+/*0332*/ 0x0010028aU,
+/*0333*/ 0x1008028aU,
+/*0334*/ 0x0010028bU,
+/*0335*/ 0x1008028bU,
+/*0336*/ 0x1808028bU,
+/*0337*/ 0x0001028cU,
+/*0338*/ 0x0801028cU,
+/*0339*/ 0x1006028cU,
+/*033a*/ 0x1806028cU,
+/*033b*/ 0x0006028dU,
+/*033c*/ 0x0801028dU,
+/*033d*/ 0x1001028dU,
+/*033e*/ 0x1803028dU,
+/*033f*/ 0x000a028eU,
+/*0340*/ 0x100a028eU,
+/*0341*/ 0x000a028fU,
+/*0342*/ 0xffffffffU,
+/*0343*/ 0x100a028fU,
+/*0344*/ 0x00040290U,
+/*0345*/ 0x08010290U,
+/*0346*/ 0x10040290U,
+/*0347*/ 0x18070290U,
+/*0348*/ 0x00070291U,
+/*0349*/ 0x08070291U,
+/*034a*/ 0x10070291U,
+/*034b*/ 0x18070291U,
+/*034c*/ 0x00070292U,
+/*034d*/ 0xffffffffU,
+/*034e*/ 0xffffffffU,
+/*034f*/ 0x08050292U,
+/*0350*/ 0x10050292U,
+/*0351*/ 0x18040292U,
+/*0352*/ 0x00040293U,
+/*0353*/ 0x08040293U,
+/*0354*/ 0xffffffffU,
+/*0355*/ 0x10010293U,
+/*0356*/ 0x18010293U,
+/*0357*/ 0x00020294U,
+/*0358*/ 0x08080294U,
+/*0359*/ 0x00200295U,
+/*035a*/ 0x00200296U,
+/*035b*/ 0x00100297U,
+/*035c*/ 0x10020297U,
+/*035d*/ 0x18020297U,
+/*035e*/ 0x00020298U,
+/*035f*/ 0xffffffffU,
+/*0360*/ 0x08010298U,
+/*0361*/ 0x10010298U,
+/*0362*/ 0x18020298U,
+/*0363*/ 0x00100299U,
+/*0364*/ 0x10100299U,
+/*0365*/ 0x0010029aU,
+/*0366*/ 0x1008029aU,
+/*0367*/ 0x1808029aU,
+/*0368*/ 0x0008029bU,
+/*0369*/ 0x0808029bU,
+/*036a*/ 0x1010029bU,
+/*036b*/ 0x0010029cU,
+/*036c*/ 0x1010029cU,
+/*036d*/ 0x0008029dU,
+/*036e*/ 0x0808029dU,
+/*036f*/ 0x1008029dU,
+/*0370*/ 0x1808029dU,
+/*0371*/ 0x0010029eU,
+/*0372*/ 0x1010029eU,
+/*0373*/ 0x0010029fU,
+/*0374*/ 0x1008029fU,
+/*0375*/ 0x1808029fU,
+/*0376*/ 0x000802a0U,
+/*0377*/ 0x080802a0U,
+/*0378*/ 0x100802a0U,
+/*0379*/ 0x001002a1U,
+/*037a*/ 0x101002a1U,
+/*037b*/ 0x001002a2U,
+/*037c*/ 0x100802a2U,
+/*037d*/ 0x180802a2U,
+/*037e*/ 0x000802a3U,
+/*037f*/ 0x080802a3U,
+/*0380*/ 0x101002a3U,
+/*0381*/ 0x001002a4U,
+/*0382*/ 0x101002a4U,
+/*0383*/ 0x000802a5U,
+/*0384*/ 0x080802a5U,
+/*0385*/ 0x100802a5U,
+/*0386*/ 0x180802a5U,
+/*0387*/ 0x001002a6U,
+/*0388*/ 0x101002a6U,
+/*0389*/ 0x001002a7U,
+/*038a*/ 0x100802a7U,
+/*038b*/ 0x180802a7U,
+/*038c*/ 0x000802a8U,
+/*038d*/ 0x080802a8U,
+/*038e*/ 0x100802a8U,
+/*038f*/ 0x001002a9U,
+/*0390*/ 0x101002a9U,
+/*0391*/ 0x001002aaU,
+/*0392*/ 0x100802aaU,
+/*0393*/ 0x180802aaU,
+/*0394*/ 0x000802abU,
+/*0395*/ 0x080802abU,
+/*0396*/ 0x101002abU,
+/*0397*/ 0x001002acU,
+/*0398*/ 0x101002acU,
+/*0399*/ 0x000802adU,
+/*039a*/ 0x080802adU,
+/*039b*/ 0x100802adU,
+/*039c*/ 0x180802adU,
+/*039d*/ 0x001002aeU,
+/*039e*/ 0x101002aeU,
+/*039f*/ 0x001002afU,
+/*03a0*/ 0x100802afU,
+/*03a1*/ 0x180802afU,
+/*03a2*/ 0x000802b0U,
+/*03a3*/ 0x080802b0U,
+/*03a4*/ 0x100802b0U,
+/*03a5*/ 0x001002b1U,
+/*03a6*/ 0x101002b1U,
+/*03a7*/ 0x001002b2U,
+/*03a8*/ 0x100802b2U,
+/*03a9*/ 0x180802b2U,
+/*03aa*/ 0x000802b3U,
+/*03ab*/ 0x080802b3U,
+/*03ac*/ 0x101002b3U,
+/*03ad*/ 0x001002b4U,
+/*03ae*/ 0x101002b4U,
+/*03af*/ 0x000802b5U,
+/*03b0*/ 0x080802b5U,
+/*03b1*/ 0x100802b5U,
+/*03b2*/ 0x180802b5U,
+/*03b3*/ 0x001002b6U,
+/*03b4*/ 0x101002b6U,
+/*03b5*/ 0x001002b7U,
+/*03b6*/ 0x100802b7U,
+/*03b7*/ 0x180802b7U,
+/*03b8*/ 0x000802b8U,
+/*03b9*/ 0x080802b8U,
+/*03ba*/ 0x100802b8U,
+/*03bb*/ 0x180202b8U,
+/*03bc*/ 0x000302b9U,
+/*03bd*/ 0x080a02b9U,
+/*03be*/ 0x000a02baU,
+/*03bf*/ 0x100a02baU,
+/*03c0*/ 0x000502bbU,
+/*03c1*/ 0x080802bbU,
+/*03c2*/ 0x100802bbU,
+/*03c3*/ 0x180802bbU,
+/*03c4*/ 0x000602bcU,
+/*03c5*/ 0x080602bcU,
+/*03c6*/ 0x001102bdU,
+/*03c7*/ 0x180802bdU,
+/*03c8*/ 0x000402beU,
+/*03c9*/ 0x080602beU,
+/*03ca*/ 0x100802beU,
+/*03cb*/ 0x180802beU,
+/*03cc*/ 0x000802bfU,
+/*03cd*/ 0x080802bfU,
+/*03ce*/ 0x100802bfU,
+/*03cf*/ 0x180802bfU,
+/*03d0*/ 0x000802c0U,
+/*03d1*/ 0x080602c0U,
+/*03d2*/ 0x100602c0U,
+/*03d3*/ 0x001102c1U,
+/*03d4*/ 0x180802c1U,
+/*03d5*/ 0x000402c2U,
+/*03d6*/ 0x080602c2U,
+/*03d7*/ 0x100802c2U,
+/*03d8*/ 0x180802c2U,
+/*03d9*/ 0x000802c3U,
+/*03da*/ 0x080802c3U,
+/*03db*/ 0x100802c3U,
+/*03dc*/ 0x180802c3U,
+/*03dd*/ 0x000802c4U,
+/*03de*/ 0x080602c4U,
+/*03df*/ 0x100602c4U,
+/*03e0*/ 0x001102c5U,
+/*03e1*/ 0x180802c5U,
+/*03e2*/ 0x000402c6U,
+/*03e3*/ 0x080602c6U,
+/*03e4*/ 0x100802c6U,
+/*03e5*/ 0x180802c6U,
+/*03e6*/ 0x000802c7U,
+/*03e7*/ 0x080802c7U,
+/*03e8*/ 0x100402c7U,
+/*03e9*/ 0x180402c7U,
+/*03ea*/ 0x000402c8U,
+/*03eb*/ 0x080402c8U,
+/*03ec*/ 0x100402c8U,
+/*03ed*/ 0x180402c8U,
+/*03ee*/ 0x000402c9U,
+/*03ef*/ 0x080402c9U,
+/*03f0*/ 0x100402c9U,
+/*03f1*/ 0x180402c9U,
+/*03f2*/ 0x000402caU,
+/*03f3*/ 0x080402caU,
+/*03f4*/ 0x100402caU,
+/*03f5*/ 0x180402caU,
+/*03f6*/ 0x000402cbU,
+/*03f7*/ 0x080402cbU,
+/*03f8*/ 0x100402cbU,
+/*03f9*/ 0x180402cbU,
+/*03fa*/ 0x000402ccU,
+/*03fb*/ 0x080402ccU,
+/*03fc*/ 0x001702cdU,
+/*03fd*/ 0x001602ceU,
+/*03fe*/ 0x001702cfU,
+/*03ff*/ 0x002002d0U,
+/*0400*/ 0x002002d1U,
+/*0401*/ 0x002002d2U,
+/*0402*/ 0x002002d3U,
+/*0403*/ 0x002002d4U,
+/*0404*/ 0x002002d5U,
+/*0405*/ 0x002002d6U,
+/*0406*/ 0x002002d7U,
+/*0407*/ 0x002002d8U,
+/*0408*/ 0x000202d9U,
+/*0409*/ 0x080502d9U,
+/*040a*/ 0x100502d9U,
+/*040b*/ 0x180102d9U,
+/*040c*/ 0x000502daU,
+/*040d*/ 0x080502daU,
+/*040e*/ 0x100502daU,
+/*040f*/ 0x180502daU,
+/*0410*/ 0x000502dbU,
+/*0411*/ 0x080502dbU,
+/*0412*/ 0x100502dbU,
+/*0413*/ 0x180502dbU,
+/*0414*/ 0x000502dcU,
+/*0415*/ 0x080502dcU,
+/*0416*/ 0x100502dcU,
+/*0417*/ 0x180502dcU,
+/*0418*/ 0x000502ddU,
+/*0419*/ 0x080502ddU,
+/*041a*/ 0x100502ddU,
+/*041b*/ 0x180502ddU,
+/*041c*/ 0x000502deU,
+/*041d*/ 0x080502deU,
+/*041e*/ 0x100502deU,
+/*041f*/ 0x180502deU,
+/*0420*/ 0x000502dfU,
+/*0421*/ 0x080502dfU,
+/*0422*/ 0x100102dfU,
+/*0423*/ 0x180202dfU,
+/*0424*/ 0x000202e0U,
+/*0425*/ 0x080202e0U,
+/*0426*/ 0x100202e0U,
+/*0427*/ 0x180102e0U,
+/*0428*/ 0x000802e1U,
+/*0429*/ 0x081502e1U,
+/*042a*/ 0x002002e2U,
+/*042b*/ 0x001502e3U,
+/*042c*/ 0x002002e4U,
+/*042d*/ 0x001502e5U,
+/*042e*/ 0x002002e6U,
+/*042f*/ 0x000702e7U,
+/*0430*/ 0x080102e7U,
+/*0431*/ 0x100202e7U,
+/*0432*/ 0x180602e7U,
+/*0433*/ 0x000102e8U,
+/*0434*/ 0x080102e8U,
+/*0435*/ 0x002002e9U,
+/*0436*/ 0x000202eaU,
+/*0437*/ 0x002002ebU,
+/*0438*/ 0x002002ecU,
+/*0439*/ 0x000c02edU,
+/*043a*/ 0x100c02edU,
+/*043b*/ 0x002002eeU,
+/*043c*/ 0x000302efU,
+/*043d*/ 0x002002f0U,
+/*043e*/ 0x000302f1U,
+/*043f*/ 0x002002f2U,
+/*0440*/ 0x000302f3U,
+/*0441*/ 0x002002f4U,
+/*0442*/ 0x000302f5U,
+/*0443*/ 0x002002f6U,
+/*0444*/ 0x000302f7U,
+/*0445*/ 0x002002f8U,
+/*0446*/ 0x000302f9U,
+/*0447*/ 0x002002faU,
+/*0448*/ 0x000302fbU,
+/*0449*/ 0x002002fcU,
+/*044a*/ 0x000302fdU,
+/*044b*/ 0x002002feU,
+/*044c*/ 0x000302ffU,
+/*044d*/ 0x00200300U,
+/*044e*/ 0x00030301U,
+/*044f*/ 0x08030301U,
+/*0450*/ 0x10020301U,
+/*0451*/ 0x18020301U,
+/*0452*/ 0x00200302U,
+/*0453*/ 0x00200303U,
+/*0454*/ 0x00200304U,
+/*0455*/ 0x00200305U,
+/*0456*/ 0x00040306U,
+/*0457*/ 0x001e0307U,
+/*0458*/ 0x001e0308U,
+/*0459*/ 0x001e0309U,
+/*045a*/ 0x001e030aU,
+/*045b*/ 0x001e030bU,
+/*045c*/ 0x001e030cU,
+/*045d*/ 0x001e030dU,
+/*045e*/ 0x001e030eU,
+/*045f*/ 0x0004030fU,
+/*0460*/ 0x0801030fU,
+/*0461*/ 0x1010030fU,
+/*0462*/ 0x00100310U,
+/*0463*/ 0x10100310U,
+/*0464*/ 0x00040311U,
+/*0465*/ 0x08010311U,
+/*0466*/ 0x10080311U,
+/*0467*/ 0x18040311U,
+/*0468*/ 0x00010312U,
+/*0469*/ 0x08080312U,
+/*046a*/ 0x10040312U,
+/*046b*/ 0x18010312U,
+/*046c*/ 0x00080313U,
+/*046d*/ 0x08040313U,
+/*046e*/ 0x10010313U,
+/*046f*/ 0x18080313U,
+/*0470*/ 0x00040314U,
+/*0471*/ 0x08010314U,
+/*0472*/ 0x10080314U,
+/*0473*/ 0x18040314U,
+/*0474*/ 0x00010315U,
+/*0475*/ 0x08080315U,
+/*0476*/ 0x10040315U,
+/*0477*/ 0x18010315U,
+/*0478*/ 0x00080316U,
+/*0479*/ 0x08040316U,
+/*047a*/ 0x10010316U,
+/*047b*/ 0x18080316U,
+/*047c*/ 0x00080317U,
+/*047d*/ 0x00010318U,
+/*047e*/ 0x08050318U,
+/*047f*/ 0x10010318U,
+/*0480*/ 0x18020318U,
+/*0481*/ 0x00010319U,
+/*0482*/ 0x08010319U,
+/*0483*/ 0x10010319U,
+/*0484*/ 0x18010319U,
+/*0485*/ 0x0001031aU,
+/*0486*/ 0x0801031aU,
+/*0487*/ 0x1001031aU,
+/*0488*/ 0x1801031aU,
+/*0489*/ 0x0001031bU,
+/*048a*/ 0x0801031bU,
+/*048b*/ 0x1001031bU,
+/*048c*/ 0x1801031bU,
+/*048d*/ 0x0001031cU,
+/*048e*/ 0x0801031cU,
+/*048f*/ 0x1001031cU,
+/*0490*/ 0x1801031cU,
+/*0491*/ 0x0008031dU,
+/*0492*/ 0x0808031dU,
+/*0493*/ 0x1008031dU,
+/*0494*/ 0x1808031dU,
+	}
+};
diff --git a/drivers/renesas/common/ddr/ddr_b/init_dram_tbl_h3.h b/drivers/renesas/common/ddr/ddr_b/init_dram_tbl_h3.h
new file mode 100644
index 0000000..357f8ba
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_b/init_dram_tbl_h3.h
@@ -0,0 +1,441 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation.
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#define DDR_PHY_SLICE_REGSET_OFS_H3  0x0400
+#define DDR_PHY_ADR_V_REGSET_OFS_H3  0x0600
+#define DDR_PHY_ADR_I_REGSET_OFS_H3  0x0680
+#define DDR_PHY_ADR_G_REGSET_OFS_H3  0x0700
+#define DDR_PI_REGSET_OFS_H3         0x0200
+
+#define DDR_PHY_SLICE_REGSET_SIZE_H3 0x80
+#define DDR_PHY_ADR_V_REGSET_SIZE_H3 0x80
+#define DDR_PHY_ADR_I_REGSET_SIZE_H3 0x80
+#define DDR_PHY_ADR_G_REGSET_SIZE_H3 0x80
+#define DDR_PI_REGSET_SIZE_H3        0x100
+
+#define DDR_PHY_SLICE_REGSET_NUM_H3  88
+#define DDR_PHY_ADR_V_REGSET_NUM_H3  37
+#define DDR_PHY_ADR_I_REGSET_NUM_H3  37
+#define DDR_PHY_ADR_G_REGSET_NUM_H3  59
+#define DDR_PI_REGSET_NUM_H3         181
+
+static const uint32_t DDR_PHY_SLICE_REGSET_H3[DDR_PHY_SLICE_REGSET_NUM_H3] = {
+	/*0400*/ 0x000004f0,
+	/*0401*/ 0x00000000,
+	/*0402*/ 0x00000000,
+	/*0403*/ 0x00000100,
+	/*0404*/ 0x01003c0c,
+	/*0405*/ 0x02003c0c,
+	/*0406*/ 0x00010300,
+	/*0407*/ 0x04000100,
+	/*0408*/ 0x00000300,
+	/*0409*/ 0x000700c0,
+	/*040a*/ 0x00b00201,
+	/*040b*/ 0x00000020,
+	/*040c*/ 0x00000000,
+	/*040d*/ 0x00000000,
+	/*040e*/ 0x00000000,
+	/*040f*/ 0x00000000,
+	/*0410*/ 0x00000000,
+	/*0411*/ 0x00000000,
+	/*0412*/ 0x00000000,
+	/*0413*/ 0x09000000,
+	/*0414*/ 0x04080000,
+	/*0415*/ 0x04080400,
+	/*0416*/ 0x00000000,
+	/*0417*/ 0x32103210,
+	/*0418*/ 0x00800708,
+	/*0419*/ 0x000f000c,
+	/*041a*/ 0x00000100,
+	/*041b*/ 0x55aa55aa,
+	/*041c*/ 0x33cc33cc,
+	/*041d*/ 0x0ff00ff0,
+	/*041e*/ 0x0f0ff0f0,
+	/*041f*/ 0x00008e38,
+	/*0420*/ 0x76543210,
+	/*0421*/ 0x00000001,
+	/*0422*/ 0x00000000,
+	/*0423*/ 0x00000000,
+	/*0424*/ 0x00000000,
+	/*0425*/ 0x00000000,
+	/*0426*/ 0x00000000,
+	/*0427*/ 0x00000000,
+	/*0428*/ 0x00000000,
+	/*0429*/ 0x00000000,
+	/*042a*/ 0x00000000,
+	/*042b*/ 0x00000000,
+	/*042c*/ 0x00000000,
+	/*042d*/ 0x00000000,
+	/*042e*/ 0x00000000,
+	/*042f*/ 0x00000000,
+	/*0430*/ 0x00000000,
+	/*0431*/ 0x00000000,
+	/*0432*/ 0x00000000,
+	/*0433*/ 0x00200000,
+	/*0434*/ 0x08200820,
+	/*0435*/ 0x08200820,
+	/*0436*/ 0x08200820,
+	/*0437*/ 0x08200820,
+	/*0438*/ 0x08200820,
+	/*0439*/ 0x00000820,
+	/*043a*/ 0x03000300,
+	/*043b*/ 0x03000300,
+	/*043c*/ 0x03000300,
+	/*043d*/ 0x03000300,
+	/*043e*/ 0x00000300,
+	/*043f*/ 0x00000000,
+	/*0440*/ 0x00000000,
+	/*0441*/ 0x00000000,
+	/*0442*/ 0x00000000,
+	/*0443*/ 0x00a000a0,
+	/*0444*/ 0x00a000a0,
+	/*0445*/ 0x00a000a0,
+	/*0446*/ 0x00a000a0,
+	/*0447*/ 0x00a000a0,
+	/*0448*/ 0x00a000a0,
+	/*0449*/ 0x00a000a0,
+	/*044a*/ 0x00a000a0,
+	/*044b*/ 0x00a000a0,
+	/*044c*/ 0x01040109,
+	/*044d*/ 0x00000200,
+	/*044e*/ 0x01000000,
+	/*044f*/ 0x00000200,
+	/*0450*/ 0x4041a151,
+	/*0451*/ 0xc00141a0,
+	/*0452*/ 0x0e0100c0,
+	/*0453*/ 0x0010000c,
+	/*0454*/ 0x0c064208,
+	/*0455*/ 0x000f0c18,
+	/*0456*/ 0x00e00140,
+	/*0457*/ 0x00000c20
+};
+
+static const uint32_t DDR_PHY_ADR_V_REGSET_H3[DDR_PHY_ADR_V_REGSET_NUM_H3] = {
+	/*0600*/ 0x00000000,
+	/*0601*/ 0x00000000,
+	/*0602*/ 0x00000000,
+	/*0603*/ 0x00000000,
+	/*0604*/ 0x00000000,
+	/*0605*/ 0x00000000,
+	/*0606*/ 0x00000002,
+	/*0607*/ 0x00000000,
+	/*0608*/ 0x00000000,
+	/*0609*/ 0x00000000,
+	/*060a*/ 0x00400320,
+	/*060b*/ 0x00000040,
+	/*060c*/ 0x00dcba98,
+	/*060d*/ 0x00000000,
+	/*060e*/ 0x00dcba98,
+	/*060f*/ 0x01000000,
+	/*0610*/ 0x00020003,
+	/*0611*/ 0x00000000,
+	/*0612*/ 0x00000000,
+	/*0613*/ 0x00000000,
+	/*0614*/ 0x00002a01,
+	/*0615*/ 0x00000015,
+	/*0616*/ 0x00000015,
+	/*0617*/ 0x0000002a,
+	/*0618*/ 0x00000033,
+	/*0619*/ 0x0000000c,
+	/*061a*/ 0x0000000c,
+	/*061b*/ 0x00000033,
+	/*061c*/ 0x00418820,
+	/*061d*/ 0x003f0000,
+	/*061e*/ 0x0000003f,
+	/*061f*/ 0x0002006e,
+	/*0620*/ 0x02000200,
+	/*0621*/ 0x02000200,
+	/*0622*/ 0x00000200,
+	/*0623*/ 0x42080010,
+	/*0624*/ 0x00000003
+};
+
+static const uint32_t DDR_PHY_ADR_I_REGSET_H3[DDR_PHY_ADR_I_REGSET_NUM_H3] = {
+	/*0680*/ 0x04040404,
+	/*0681*/ 0x00000404,
+	/*0682*/ 0x00000000,
+	/*0683*/ 0x00000000,
+	/*0684*/ 0x00000000,
+	/*0685*/ 0x00000000,
+	/*0686*/ 0x00000002,
+	/*0687*/ 0x00000000,
+	/*0688*/ 0x00000000,
+	/*0689*/ 0x00000000,
+	/*068a*/ 0x00400320,
+	/*068b*/ 0x00000040,
+	/*068c*/ 0x00000000,
+	/*068d*/ 0x00000000,
+	/*068e*/ 0x00000000,
+	/*068f*/ 0x01000000,
+	/*0690*/ 0x00020003,
+	/*0691*/ 0x00000000,
+	/*0692*/ 0x00000000,
+	/*0693*/ 0x00000000,
+	/*0694*/ 0x00002a01,
+	/*0695*/ 0x00000015,
+	/*0696*/ 0x00000015,
+	/*0697*/ 0x0000002a,
+	/*0698*/ 0x00000033,
+	/*0699*/ 0x0000000c,
+	/*069a*/ 0x0000000c,
+	/*069b*/ 0x00000033,
+	/*069c*/ 0x00000000,
+	/*069d*/ 0x00000000,
+	/*069e*/ 0x00000000,
+	/*069f*/ 0x0002006e,
+	/*06a0*/ 0x02000200,
+	/*06a1*/ 0x02000200,
+	/*06a2*/ 0x00000200,
+	/*06a3*/ 0x42080010,
+	/*06a4*/ 0x00000003
+};
+
+static const uint32_t DDR_PHY_ADR_G_REGSET_H3[DDR_PHY_ADR_G_REGSET_NUM_H3] = {
+	/*0700*/ 0x00000001,
+	/*0701*/ 0x00000000,
+	/*0702*/ 0x00000005,
+	/*0703*/ 0x04000f00,
+	/*0704*/ 0x00020080,
+	/*0705*/ 0x00020055,
+	/*0706*/ 0x00000000,
+	/*0707*/ 0x00000000,
+	/*0708*/ 0x00000000,
+	/*0709*/ 0x00000050,
+	/*070a*/ 0x00000000,
+	/*070b*/ 0x01010100,
+	/*070c*/ 0x00000200,
+	/*070d*/ 0x00001102,
+	/*070e*/ 0x00000000,
+	/*070f*/ 0x000f1f00,
+	/*0710*/ 0x0f1f0f1f,
+	/*0711*/ 0x0f1f0f1f,
+	/*0712*/ 0x00020003,
+	/*0713*/ 0x02000200,
+	/*0714*/ 0x00000200,
+	/*0715*/ 0x00001102,
+	/*0716*/ 0x00000064,
+	/*0717*/ 0x00000000,
+	/*0718*/ 0x00000000,
+	/*0719*/ 0x00000502,
+	/*071a*/ 0x027f6e00,
+	/*071b*/ 0x007f007f,
+	/*071c*/ 0x00007f3c,
+	/*071d*/ 0x00047f6e,
+	/*071e*/ 0x0003154f,
+	/*071f*/ 0x0001154f,
+	/*0720*/ 0x0001154f,
+	/*0721*/ 0x0001154f,
+	/*0722*/ 0x0001154f,
+	/*0723*/ 0x00003fee,
+	/*0724*/ 0x0001154f,
+	/*0725*/ 0x00003fee,
+	/*0726*/ 0x0001154f,
+	/*0727*/ 0x00007f3c,
+	/*0728*/ 0x0001154f,
+	/*0729*/ 0x00000000,
+	/*072a*/ 0x00000000,
+	/*072b*/ 0x00000000,
+	/*072c*/ 0x65000000,
+	/*072d*/ 0x00000000,
+	/*072e*/ 0x00000000,
+	/*072f*/ 0x00000201,
+	/*0730*/ 0x00000000,
+	/*0731*/ 0x00000000,
+	/*0732*/ 0x00000000,
+	/*0733*/ 0x00000000,
+	/*0734*/ 0x00000000,
+	/*0735*/ 0x00000000,
+	/*0736*/ 0x00000000,
+	/*0737*/ 0x00000000,
+	/*0738*/ 0x00000000,
+	/*0739*/ 0x00000000,
+	/*073a*/ 0x00000000
+};
+
+static const uint32_t DDR_PI_REGSET_H3[DDR_PI_REGSET_NUM_H3] = {
+	/*0200*/ 0x00000b00,
+	/*0201*/ 0x00000100,
+	/*0202*/ 0x00000000,
+	/*0203*/ 0x0000ffff,
+	/*0204*/ 0x00000000,
+	/*0205*/ 0x0000ffff,
+	/*0206*/ 0x00000000,
+	/*0207*/ 0x304cffff,
+	/*0208*/ 0x00000200,
+	/*0209*/ 0x00000200,
+	/*020a*/ 0x00000200,
+	/*020b*/ 0x00000200,
+	/*020c*/ 0x0000304c,
+	/*020d*/ 0x00000200,
+	/*020e*/ 0x00000200,
+	/*020f*/ 0x00000200,
+	/*0210*/ 0x00000200,
+	/*0211*/ 0x0000304c,
+	/*0212*/ 0x00000200,
+	/*0213*/ 0x00000200,
+	/*0214*/ 0x00000200,
+	/*0215*/ 0x00000200,
+	/*0216*/ 0x00010000,
+	/*0217*/ 0x00000003,
+	/*0218*/ 0x01000001,
+	/*0219*/ 0x00000000,
+	/*021a*/ 0x00000000,
+	/*021b*/ 0x00000000,
+	/*021c*/ 0x00000000,
+	/*021d*/ 0x00000000,
+	/*021e*/ 0x00000000,
+	/*021f*/ 0x00000000,
+	/*0220*/ 0x00000000,
+	/*0221*/ 0x00000000,
+	/*0222*/ 0x00000000,
+	/*0223*/ 0x00000000,
+	/*0224*/ 0x00000000,
+	/*0225*/ 0x00000000,
+	/*0226*/ 0x00000000,
+	/*0227*/ 0x00000000,
+	/*0228*/ 0x00000000,
+	/*0229*/ 0x0f000101,
+	/*022a*/ 0x08492d25,
+	/*022b*/ 0x500e0c04,
+	/*022c*/ 0x0002500e,
+	/*022d*/ 0x00460003,
+	/*022e*/ 0x182600cf,
+	/*022f*/ 0x182600cf,
+	/*0230*/ 0x00000005,
+	/*0231*/ 0x00000000,
+	/*0232*/ 0x00000000,
+	/*0233*/ 0x00000000,
+	/*0234*/ 0x00000000,
+	/*0235*/ 0x00000000,
+	/*0236*/ 0x00000000,
+	/*0237*/ 0x00000000,
+	/*0238*/ 0x01000000,
+	/*0239*/ 0x00040404,
+	/*023a*/ 0x01280a00,
+	/*023b*/ 0x00000000,
+	/*023c*/ 0x000f0000,
+	/*023d*/ 0x00001803,
+	/*023e*/ 0x00000000,
+	/*023f*/ 0x00000000,
+	/*0240*/ 0x00060002,
+	/*0241*/ 0x00010001,
+	/*0242*/ 0x01000101,
+	/*0243*/ 0x04020201,
+	/*0244*/ 0x00080804,
+	/*0245*/ 0x00000000,
+	/*0246*/ 0x08030000,
+	/*0247*/ 0x15150408,
+	/*0248*/ 0x00000000,
+	/*0249*/ 0x00000000,
+	/*024a*/ 0x00000000,
+	/*024b*/ 0x001e0f0f,
+	/*024c*/ 0x00000000,
+	/*024d*/ 0x01000300,
+	/*024e*/ 0x00000000,
+	/*024f*/ 0x00000000,
+	/*0250*/ 0x01000000,
+	/*0251*/ 0x00010101,
+	/*0252*/ 0x000e0e0e,
+	/*0253*/ 0x000c0c0c,
+	/*0254*/ 0x02060601,
+	/*0255*/ 0x00000000,
+	/*0256*/ 0x00000003,
+	/*0257*/ 0x00181703,
+	/*0258*/ 0x00280006,
+	/*0259*/ 0x00280016,
+	/*025a*/ 0x00000016,
+	/*025b*/ 0x00000000,
+	/*025c*/ 0x00000000,
+	/*025d*/ 0x00000000,
+	/*025e*/ 0x140a0000,
+	/*025f*/ 0x0005010a,
+	/*0260*/ 0x03018d03,
+	/*0261*/ 0x000a018d,
+	/*0262*/ 0x00060100,
+	/*0263*/ 0x01000006,
+	/*0264*/ 0x018e018e,
+	/*0265*/ 0x018e0100,
+	/*0266*/ 0x1111018e,
+	/*0267*/ 0x10010204,
+	/*0268*/ 0x09090650,
+	/*0269*/ 0x20110202,
+	/*026a*/ 0x00201000,
+	/*026b*/ 0x00201000,
+	/*026c*/ 0x04041000,
+	/*026d*/ 0x18020100,
+	/*026e*/ 0x00010118,
+	/*026f*/ 0x004b004a,
+	/*0270*/ 0x050f0000,
+	/*0271*/ 0x0c01021e,
+	/*0272*/ 0x34000000,
+	/*0273*/ 0x00000000,
+	/*0274*/ 0x00000000,
+	/*0275*/ 0x00000000,
+	/*0276*/ 0x312ed400,
+	/*0277*/ 0xd4111132,
+	/*0278*/ 0x1132312e,
+	/*0279*/ 0x312ed411,
+	/*027a*/ 0x00111132,
+	/*027b*/ 0x32312ed4,
+	/*027c*/ 0x2ed41111,
+	/*027d*/ 0x11113231,
+	/*027e*/ 0x32312ed4,
+	/*027f*/ 0xd4001111,
+	/*0280*/ 0x1132312e,
+	/*0281*/ 0x312ed411,
+	/*0282*/ 0xd4111132,
+	/*0283*/ 0x1132312e,
+	/*0284*/ 0x2ed40011,
+	/*0285*/ 0x11113231,
+	/*0286*/ 0x32312ed4,
+	/*0287*/ 0x2ed41111,
+	/*0288*/ 0x11113231,
+	/*0289*/ 0x00020000,
+	/*028a*/ 0x018d018d,
+	/*028b*/ 0x0c08018d,
+	/*028c*/ 0x1f121d22,
+	/*028d*/ 0x4301b344,
+	/*028e*/ 0x10172006,
+	/*028f*/ 0x121d220c,
+	/*0290*/ 0x01b3441f,
+	/*0291*/ 0x17200643,
+	/*0292*/ 0x1d220c10,
+	/*0293*/ 0x00001f12,
+	/*0294*/ 0x4301b344,
+	/*0295*/ 0x10172006,
+	/*0296*/ 0x00020002,
+	/*0297*/ 0x00020002,
+	/*0298*/ 0x00020002,
+	/*0299*/ 0x00020002,
+	/*029a*/ 0x00020002,
+	/*029b*/ 0x00000000,
+	/*029c*/ 0x00000000,
+	/*029d*/ 0x00000000,
+	/*029e*/ 0x00000000,
+	/*029f*/ 0x00000000,
+	/*02a0*/ 0x00000000,
+	/*02a1*/ 0x00000000,
+	/*02a2*/ 0x00000000,
+	/*02a3*/ 0x00000000,
+	/*02a4*/ 0x00000000,
+	/*02a5*/ 0x00000000,
+	/*02a6*/ 0x00000000,
+	/*02a7*/ 0x01000400,
+	/*02a8*/ 0x00304c00,
+	/*02a9*/ 0x0001e2f8,
+	/*02aa*/ 0x0000304c,
+	/*02ab*/ 0x0001e2f8,
+	/*02ac*/ 0x0000304c,
+	/*02ad*/ 0x0001e2f8,
+	/*02ae*/ 0x08000000,
+	/*02af*/ 0x00000100,
+	/*02b0*/ 0x00000000,
+	/*02b1*/ 0x00000000,
+	/*02b2*/ 0x00000000,
+	/*02b3*/ 0x00000000,
+	/*02b4*/ 0x00000002
+};
diff --git a/drivers/renesas/common/ddr/ddr_b/init_dram_tbl_h3ver2.h b/drivers/renesas/common/ddr/ddr_b/init_dram_tbl_h3ver2.h
new file mode 100644
index 0000000..e5258af
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_b/init_dram_tbl_h3ver2.h
@@ -0,0 +1,538 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation.
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#define DDR_PHY_SLICE_REGSET_OFS_H3VER2  0x0400
+#define DDR_PHY_ADR_V_REGSET_OFS_H3VER2  0x0600
+#define DDR_PHY_ADR_I_REGSET_OFS_H3VER2  0x0640
+#define DDR_PHY_ADR_G_REGSET_OFS_H3VER2  0x0680
+#define DDR_PI_REGSET_OFS_H3VER2         0x0200
+
+#define DDR_PHY_SLICE_REGSET_SIZE_H3VER2 0x80
+#define DDR_PHY_ADR_V_REGSET_SIZE_H3VER2 0x40
+#define DDR_PHY_ADR_I_REGSET_SIZE_H3VER2 0x40
+#define DDR_PHY_ADR_G_REGSET_SIZE_H3VER2 0x80
+#define DDR_PI_REGSET_SIZE_H3VER2        0x100
+
+#define DDR_PHY_SLICE_REGSET_NUM_H3VER2  97
+#define DDR_PHY_ADR_V_REGSET_NUM_H3VER2  37
+#define DDR_PHY_ADR_I_REGSET_NUM_H3VER2  37
+#define DDR_PHY_ADR_G_REGSET_NUM_H3VER2  79
+#define DDR_PI_REGSET_NUM_H3VER2         245
+
+static const uint32_t DDR_PHY_SLICE_REGSET_H3VER2
+	[DDR_PHY_SLICE_REGSET_NUM_H3VER2] = {
+	/*0400*/ 0x76543210,
+	/*0401*/ 0x0004f008,
+	/*0402*/ 0x00020133,
+	/*0403*/ 0x00000000,
+	/*0404*/ 0x00000000,
+	/*0405*/ 0x00010000,
+	/*0406*/ 0x016e6e0e,
+	/*0407*/ 0x026e6e0e,
+	/*0408*/ 0x00010300,
+	/*0409*/ 0x04000100,
+	/*040a*/ 0x01000000,
+	/*040b*/ 0x00000000,
+	/*040c*/ 0x00000000,
+	/*040d*/ 0x00000100,
+	/*040e*/ 0x001700c0,
+	/*040f*/ 0x020100b0,
+	/*0410*/ 0x00030020,
+	/*0411*/ 0x00000000,
+	/*0412*/ 0x00000000,
+	/*0413*/ 0x00000000,
+	/*0414*/ 0x00000000,
+	/*0415*/ 0x00000000,
+	/*0416*/ 0x00000000,
+	/*0417*/ 0x00000000,
+	/*0418*/ 0x09000000,
+	/*0419*/ 0x04080000,
+	/*041a*/ 0x04080400,
+	/*041b*/ 0x08000000,
+	/*041c*/ 0x0c008007,
+	/*041d*/ 0x00000f00,
+	/*041e*/ 0x00000100,
+	/*041f*/ 0x55aa55aa,
+	/*0420*/ 0x33cc33cc,
+	/*0421*/ 0x0ff00ff0,
+	/*0422*/ 0x0f0ff0f0,
+	/*0423*/ 0x00018e38,
+	/*0424*/ 0x00000000,
+	/*0425*/ 0x00000000,
+	/*0426*/ 0x00000000,
+	/*0427*/ 0x00000000,
+	/*0428*/ 0x00000000,
+	/*0429*/ 0x00000000,
+	/*042a*/ 0x00000000,
+	/*042b*/ 0x00000000,
+	/*042c*/ 0x00000000,
+	/*042d*/ 0x00000000,
+	/*042e*/ 0x00000000,
+	/*042f*/ 0x00000000,
+	/*0430*/ 0x00000000,
+	/*0431*/ 0x00000000,
+	/*0432*/ 0x00000000,
+	/*0433*/ 0x00000000,
+	/*0434*/ 0x00000000,
+	/*0435*/ 0x00000000,
+	/*0436*/ 0x00000000,
+	/*0437*/ 0x00000000,
+	/*0438*/ 0x00000104,
+	/*0439*/ 0x00082020,
+	/*043a*/ 0x08200820,
+	/*043b*/ 0x08200820,
+	/*043c*/ 0x08200820,
+	/*043d*/ 0x08200820,
+	/*043e*/ 0x08200820,
+	/*043f*/ 0x00000000,
+	/*0440*/ 0x00000000,
+	/*0441*/ 0x03000300,
+	/*0442*/ 0x03000300,
+	/*0443*/ 0x03000300,
+	/*0444*/ 0x03000300,
+	/*0445*/ 0x00000300,
+	/*0446*/ 0x00000000,
+	/*0447*/ 0x00000000,
+	/*0448*/ 0x00000000,
+	/*0449*/ 0x00000000,
+	/*044a*/ 0x00000000,
+	/*044b*/ 0x00a000a0,
+	/*044c*/ 0x00a000a0,
+	/*044d*/ 0x00a000a0,
+	/*044e*/ 0x00a000a0,
+	/*044f*/ 0x00a000a0,
+	/*0450*/ 0x00a000a0,
+	/*0451*/ 0x00a000a0,
+	/*0452*/ 0x00a000a0,
+	/*0453*/ 0x00a000a0,
+	/*0454*/ 0x01040109,
+	/*0455*/ 0x00000200,
+	/*0456*/ 0x01000000,
+	/*0457*/ 0x00000200,
+	/*0458*/ 0x00000004,
+	/*0459*/ 0x4041a151,
+	/*045a*/ 0xc00141a0,
+	/*045b*/ 0x0e0000c0,
+	/*045c*/ 0x0010000c,
+	/*045d*/ 0x063e4208,
+	/*045e*/ 0x0f0c180c,
+	/*045f*/ 0x00e00140,
+	/*0460*/ 0x00000c20
+};
+
+static const uint32_t
+	DDR_PHY_ADR_V_REGSET_H3VER2[DDR_PHY_ADR_V_REGSET_NUM_H3VER2] = {
+	/*0600*/ 0x00000000,
+	/*0601*/ 0x00000000,
+	/*0602*/ 0x00000000,
+	/*0603*/ 0x00000000,
+	/*0604*/ 0x00000000,
+	/*0605*/ 0x00000000,
+	/*0606*/ 0x00000000,
+	/*0607*/ 0x00010000,
+	/*0608*/ 0x00000200,
+	/*0609*/ 0x00000000,
+	/*060a*/ 0x00000000,
+	/*060b*/ 0x00000000,
+	/*060c*/ 0x00400320,
+	/*060d*/ 0x00000040,
+	/*060e*/ 0x00dcba98,
+	/*060f*/ 0x03000000,
+	/*0610*/ 0x00000200,
+	/*0611*/ 0x00000000,
+	/*0612*/ 0x00000000,
+	/*0613*/ 0x00000000,
+	/*0614*/ 0x0000002a,
+	/*0615*/ 0x00000015,
+	/*0616*/ 0x00000015,
+	/*0617*/ 0x0000002a,
+	/*0618*/ 0x00000033,
+	/*0619*/ 0x0000000c,
+	/*061a*/ 0x0000000c,
+	/*061b*/ 0x00000033,
+	/*061c*/ 0x00418820,
+	/*061d*/ 0x003f0000,
+	/*061e*/ 0x0000003f,
+	/*061f*/ 0x0002c06e,
+	/*0620*/ 0x02c002c0,
+	/*0621*/ 0x02c002c0,
+	/*0622*/ 0x000002c0,
+	/*0623*/ 0x42080010,
+	/*0624*/ 0x0000033e
+};
+
+static const uint32_t
+	DDR_PHY_ADR_I_REGSET_H3VER2[DDR_PHY_ADR_I_REGSET_NUM_H3VER2] = {
+	/*0640*/ 0x00000000,
+	/*0641*/ 0x00000000,
+	/*0642*/ 0x00000000,
+	/*0643*/ 0x00000000,
+	/*0644*/ 0x00000000,
+	/*0645*/ 0x00000000,
+	/*0646*/ 0x00000000,
+	/*0647*/ 0x00000000,
+	/*0648*/ 0x00000000,
+	/*0649*/ 0x00000000,
+	/*064a*/ 0x00000000,
+	/*064b*/ 0x00000000,
+	/*064c*/ 0x00000000,
+	/*064d*/ 0x00000000,
+	/*064e*/ 0x00000000,
+	/*064f*/ 0x00000000,
+	/*0650*/ 0x00000000,
+	/*0651*/ 0x00000000,
+	/*0652*/ 0x00000000,
+	/*0653*/ 0x00000000,
+	/*0654*/ 0x00000000,
+	/*0655*/ 0x00000000,
+	/*0656*/ 0x00000000,
+	/*0657*/ 0x00000000,
+	/*0658*/ 0x00000000,
+	/*0659*/ 0x00000000,
+	/*065a*/ 0x00000000,
+	/*065b*/ 0x00000000,
+	/*065c*/ 0x00000000,
+	/*065d*/ 0x00000000,
+	/*065e*/ 0x00000000,
+	/*065f*/ 0x00000000,
+	/*0660*/ 0x00000000,
+	/*0661*/ 0x00000000,
+	/*0662*/ 0x00000000,
+	/*0663*/ 0x00000000,
+	/*0664*/ 0x00000000
+};
+
+static const uint32_t
+	DDR_PHY_ADR_G_REGSET_H3VER2[DDR_PHY_ADR_G_REGSET_NUM_H3VER2] = {
+	/*0680*/ 0x00000000,
+	/*0681*/ 0x00000100,
+	/*0682*/ 0x00000000,
+	/*0683*/ 0x00050000,
+	/*0684*/ 0x0f000000,
+	/*0685*/ 0x00800400,
+	/*0686*/ 0x00020032,
+	/*0687*/ 0x00020055,
+	/*0688*/ 0x00000000,
+	/*0689*/ 0x00000000,
+	/*068a*/ 0x00000000,
+	/*068b*/ 0x00000050,
+	/*068c*/ 0x00000000,
+	/*068d*/ 0x01010100,
+	/*068e*/ 0x01000200,
+	/*068f*/ 0x00000000,
+	/*0690*/ 0x00010100,
+	/*0691*/ 0x00000000,
+	/*0692*/ 0x00000000,
+	/*0693*/ 0x00000000,
+	/*0694*/ 0x00000000,
+	/*0695*/ 0x00005064,
+	/*0696*/ 0x01421142,
+	/*0697*/ 0x00000142,
+	/*0698*/ 0x00000000,
+	/*0699*/ 0x000f1100,
+	/*069a*/ 0x0f110f11,
+	/*069b*/ 0x09000f11,
+	/*069c*/ 0x00000003,
+	/*069d*/ 0x0002c000,
+	/*069e*/ 0x02c002c0,
+	/*069f*/ 0x000002c0,
+	/*06a0*/ 0x03421342,
+	/*06a1*/ 0x00000342,
+	/*06a2*/ 0x00000000,
+	/*06a3*/ 0x00000000,
+	/*06a4*/ 0x05020000,
+	/*06a5*/ 0x14000000,
+	/*06a6*/ 0x027f6e00,
+	/*06a7*/ 0x047f027f,
+	/*06a8*/ 0x00027f6e,
+	/*06a9*/ 0x00047f6e,
+	/*06aa*/ 0x0003554f,
+	/*06ab*/ 0x0001554f,
+	/*06ac*/ 0x0001554f,
+	/*06ad*/ 0x0001554f,
+	/*06ae*/ 0x0001554f,
+	/*06af*/ 0x00003fee,
+	/*06b0*/ 0x0001554f,
+	/*06b1*/ 0x00003fee,
+	/*06b2*/ 0x0001554f,
+	/*06b3*/ 0x00027f6e,
+	/*06b4*/ 0x0001554f,
+	/*06b5*/ 0x00004011,
+	/*06b6*/ 0x00004410,
+	/*06b7*/ 0x00000000,
+	/*06b8*/ 0x00000000,
+	/*06b9*/ 0x00000000,
+	/*06ba*/ 0x00000065,
+	/*06bb*/ 0x00000000,
+	/*06bc*/ 0x00020201,
+	/*06bd*/ 0x00000000,
+	/*06be*/ 0x03000000,
+	/*06bf*/ 0x00000008,
+	/*06c0*/ 0x00000000,
+	/*06c1*/ 0x00000000,
+	/*06c2*/ 0x00000000,
+	/*06c3*/ 0x00000000,
+	/*06c4*/ 0x00000001,
+	/*06c5*/ 0x00000000,
+	/*06c6*/ 0x00000000,
+	/*06c7*/ 0x00000000,
+	/*06c8*/ 0x000000e4,
+	/*06c9*/ 0x00010198,
+	/*06ca*/ 0x00000000,
+	/*06cb*/ 0x00000000,
+	/*06cc*/ 0x07010000,
+	/*06cd*/ 0x00000104,
+	/*06ce*/ 0x00000000
+};
+
+static const uint32_t DDR_PI_REGSET_H3VER2[DDR_PI_REGSET_NUM_H3VER2] = {
+	/*0200*/ 0x00000b00,
+	/*0201*/ 0x00000100,
+	/*0202*/ 0x00640000,
+	/*0203*/ 0x00000000,
+	/*0204*/ 0x0000ffff,
+	/*0205*/ 0x00000000,
+	/*0206*/ 0x0000ffff,
+	/*0207*/ 0x00000000,
+	/*0208*/ 0x0000ffff,
+	/*0209*/ 0x0000304c,
+	/*020a*/ 0x00000200,
+	/*020b*/ 0x00000200,
+	/*020c*/ 0x00000200,
+	/*020d*/ 0x00000200,
+	/*020e*/ 0x0000304c,
+	/*020f*/ 0x00000200,
+	/*0210*/ 0x00000200,
+	/*0211*/ 0x00000200,
+	/*0212*/ 0x00000200,
+	/*0213*/ 0x0000304c,
+	/*0214*/ 0x00000200,
+	/*0215*/ 0x00000200,
+	/*0216*/ 0x00000200,
+	/*0217*/ 0x00000200,
+	/*0218*/ 0x00010000,
+	/*0219*/ 0x00000003,
+	/*021a*/ 0x01000001,
+	/*021b*/ 0x00000000,
+	/*021c*/ 0x00000000,
+	/*021d*/ 0x00000000,
+	/*021e*/ 0x00000000,
+	/*021f*/ 0x00000000,
+	/*0220*/ 0x00000000,
+	/*0221*/ 0x00000000,
+	/*0222*/ 0x00000000,
+	/*0223*/ 0x00000000,
+	/*0224*/ 0x00000000,
+	/*0225*/ 0x00000000,
+	/*0226*/ 0x00000000,
+	/*0227*/ 0x00000000,
+	/*0228*/ 0x00000000,
+	/*0229*/ 0x00000000,
+	/*022a*/ 0x00000000,
+	/*022b*/ 0x0f000101,
+	/*022c*/ 0x08492d25,
+	/*022d*/ 0x500e0c04,
+	/*022e*/ 0x0002500e,
+	/*022f*/ 0x00000301,
+	/*0230*/ 0x00000046,
+	/*0231*/ 0x000000cf,
+	/*0232*/ 0x00001826,
+	/*0233*/ 0x000000cf,
+	/*0234*/ 0x00001826,
+	/*0235*/ 0x00000005,
+	/*0236*/ 0x00000000,
+	/*0237*/ 0x00000000,
+	/*0238*/ 0x00000000,
+	/*0239*/ 0x00000000,
+	/*023a*/ 0x00000000,
+	/*023b*/ 0x00000000,
+	/*023c*/ 0x00000000,
+	/*023d*/ 0x00000000,
+	/*023e*/ 0x04010000,
+	/*023f*/ 0x00000404,
+	/*0240*/ 0x0101280a,
+	/*0241*/ 0x00000000,
+	/*0242*/ 0x00000000,
+	/*0243*/ 0x0003000f,
+	/*0244*/ 0x00000018,
+	/*0245*/ 0x00000000,
+	/*0246*/ 0x00000000,
+	/*0247*/ 0x00060002,
+	/*0248*/ 0x00010001,
+	/*0249*/ 0x01000101,
+	/*024a*/ 0x04020201,
+	/*024b*/ 0x00080804,
+	/*024c*/ 0x00000000,
+	/*024d*/ 0x08030000,
+	/*024e*/ 0x15150408,
+	/*024f*/ 0x00000000,
+	/*0250*/ 0x00000000,
+	/*0251*/ 0x00000000,
+	/*0252*/ 0x0f0f0000,
+	/*0253*/ 0x0000001e,
+	/*0254*/ 0x00000000,
+	/*0255*/ 0x01000300,
+	/*0256*/ 0x00000100,
+	/*0257*/ 0x00000000,
+	/*0258*/ 0x00000000,
+	/*0259*/ 0x01000000,
+	/*025a*/ 0x00000101,
+	/*025b*/ 0x55555a5a,
+	/*025c*/ 0x55555a5a,
+	/*025d*/ 0x55555a5a,
+	/*025e*/ 0x55555a5a,
+	/*025f*/ 0x0e0e0001,
+	/*0260*/ 0x0c0c000e,
+	/*0261*/ 0x0601000c,
+	/*0262*/ 0x17170106,
+	/*0263*/ 0x00020202,
+	/*0264*/ 0x03000000,
+	/*0265*/ 0x00000000,
+	/*0266*/ 0x00181703,
+	/*0267*/ 0x00280006,
+	/*0268*/ 0x00280016,
+	/*0269*/ 0x00000016,
+	/*026a*/ 0x00000000,
+	/*026b*/ 0x00000000,
+	/*026c*/ 0x00000000,
+	/*026d*/ 0x0a000000,
+	/*026e*/ 0x00010a14,
+	/*026f*/ 0x00030005,
+	/*0270*/ 0x0003018d,
+	/*0271*/ 0x000a018d,
+	/*0272*/ 0x00060100,
+	/*0273*/ 0x01000006,
+	/*0274*/ 0x018e018e,
+	/*0275*/ 0x018e0100,
+	/*0276*/ 0x1111018e,
+	/*0277*/ 0x10010204,
+	/*0278*/ 0x09090650,
+	/*0279*/ 0xff110202,
+	/*027a*/ 0x00ff1000,
+	/*027b*/ 0x00ff1000,
+	/*027c*/ 0x04041000,
+	/*027d*/ 0x18020100,
+	/*027e*/ 0x01010018,
+	/*027f*/ 0x004a004a,
+	/*0280*/ 0x004b004a,
+	/*0281*/ 0x050f0000,
+	/*0282*/ 0x0c01021e,
+	/*0283*/ 0x34000000,
+	/*0284*/ 0x00000000,
+	/*0285*/ 0x00000000,
+	/*0286*/ 0x00000000,
+	/*0287*/ 0x00000000,
+	/*0288*/ 0x36312ed4,
+	/*0289*/ 0x2ed41111,
+	/*028a*/ 0x11113631,
+	/*028b*/ 0x36312ed4,
+	/*028c*/ 0xd4001111,
+	/*028d*/ 0x1136312e,
+	/*028e*/ 0x312ed411,
+	/*028f*/ 0xd4111136,
+	/*0290*/ 0x1136312e,
+	/*0291*/ 0x2ed40011,
+	/*0292*/ 0x11113631,
+	/*0293*/ 0x36312ed4,
+	/*0294*/ 0x2ed41111,
+	/*0295*/ 0x11113631,
+	/*0296*/ 0x312ed400,
+	/*0297*/ 0xd4111136,
+	/*0298*/ 0x1136312e,
+	/*0299*/ 0x312ed411,
+	/*029a*/ 0x00111136,
+	/*029b*/ 0x018d0200,
+	/*029c*/ 0x018d018d,
+	/*029d*/ 0x1d220c08,
+	/*029e*/ 0x00001f12,
+	/*029f*/ 0x4301b344,
+	/*02a0*/ 0x10172006,
+	/*02a1*/ 0x121d220c,
+	/*02a2*/ 0x01b3441f,
+	/*02a3*/ 0x17200643,
+	/*02a4*/ 0x1d220c10,
+	/*02a5*/ 0x00001f12,
+	/*02a6*/ 0x4301b344,
+	/*02a7*/ 0x10172006,
+	/*02a8*/ 0x00020002,
+	/*02a9*/ 0x00020002,
+	/*02aa*/ 0x00020002,
+	/*02ab*/ 0x00020002,
+	/*02ac*/ 0x00020002,
+	/*02ad*/ 0x00000000,
+	/*02ae*/ 0x00000000,
+	/*02af*/ 0x00000000,
+	/*02b0*/ 0x00000000,
+	/*02b1*/ 0x00000000,
+	/*02b2*/ 0x00000000,
+	/*02b3*/ 0x00000000,
+	/*02b4*/ 0x00000000,
+	/*02b5*/ 0x00000000,
+	/*02b6*/ 0x00000000,
+	/*02b7*/ 0x00000000,
+	/*02b8*/ 0x00000000,
+	/*02b9*/ 0x00000400,
+	/*02ba*/ 0x05040302,
+	/*02bb*/ 0x01000f0e,
+	/*02bc*/ 0x07060504,
+	/*02bd*/ 0x03020100,
+	/*02be*/ 0x02010000,
+	/*02bf*/ 0x00000103,
+	/*02c0*/ 0x0000304c,
+	/*02c1*/ 0x0001e2f8,
+	/*02c2*/ 0x0000304c,
+	/*02c3*/ 0x0001e2f8,
+	/*02c4*/ 0x0000304c,
+	/*02c5*/ 0x0001e2f8,
+	/*02c6*/ 0x08000000,
+	/*02c7*/ 0x00000100,
+	/*02c8*/ 0x00000000,
+	/*02c9*/ 0x00000000,
+	/*02ca*/ 0x00000000,
+	/*02cb*/ 0x00000000,
+	/*02cc*/ 0x00010000,
+	/*02cd*/ 0x00000000,
+	/*02ce*/ 0x00000000,
+	/*02cf*/ 0x00000000,
+	/*02d0*/ 0x00000000,
+	/*02d1*/ 0x00000000,
+	/*02d2*/ 0x00000000,
+	/*02d3*/ 0x00000000,
+	/*02d4*/ 0x00000000,
+	/*02d5*/ 0x00000000,
+	/*02d6*/ 0x00000000,
+	/*02d7*/ 0x00000000,
+	/*02d8*/ 0x00000000,
+	/*02d9*/ 0x00000000,
+	/*02da*/ 0x00000000,
+	/*02db*/ 0x00000000,
+	/*02dc*/ 0x00000000,
+	/*02dd*/ 0x00000000,
+	/*02de*/ 0x00000000,
+	/*02df*/ 0x00000000,
+	/*02e0*/ 0x00000000,
+	/*02e1*/ 0x00000000,
+	/*02e2*/ 0x00000000,
+	/*02e3*/ 0x00000000,
+	/*02e4*/ 0x00000000,
+	/*02e5*/ 0x00000000,
+	/*02e6*/ 0x00000000,
+	/*02e7*/ 0x00000000,
+	/*02e8*/ 0x00000000,
+	/*02e9*/ 0x00000000,
+	/*02ea*/ 0x00000000,
+	/*02eb*/ 0x00000000,
+	/*02ec*/ 0x00000000,
+	/*02ed*/ 0x00000000,
+	/*02ee*/ 0x00000002,
+	/*02ef*/ 0x00000000,
+	/*02f0*/ 0x00000000,
+	/*02f1*/ 0x00000000,
+	/*02f2*/ 0x00000000,
+	/*02f3*/ 0x00000000,
+	/*02f4*/ 0x00000000
+};
diff --git a/drivers/renesas/common/ddr/ddr_b/init_dram_tbl_m3.h b/drivers/renesas/common/ddr/ddr_b/init_dram_tbl_m3.h
new file mode 100644
index 0000000..b491f0e
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_b/init_dram_tbl_m3.h
@@ -0,0 +1,468 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation.
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#define DDR_PHY_SLICE_REGSET_OFS_M3  0x0800
+#define DDR_PHY_ADR_V_REGSET_OFS_M3  0x0a00
+#define DDR_PHY_ADR_I_REGSET_OFS_M3  0x0a80
+#define DDR_PHY_ADR_G_REGSET_OFS_M3  0x0b80
+#define DDR_PI_REGSET_OFS_M3         0x0200
+
+#define DDR_PHY_SLICE_REGSET_SIZE_M3 0x80
+#define DDR_PHY_ADR_V_REGSET_SIZE_M3 0x80
+#define DDR_PHY_ADR_I_REGSET_SIZE_M3 0x80
+#define DDR_PHY_ADR_G_REGSET_SIZE_M3 0x80
+#define DDR_PI_REGSET_SIZE_M3        0x100
+
+#define DDR_PHY_SLICE_REGSET_NUM_M3  89
+#define DDR_PHY_ADR_V_REGSET_NUM_M3  37
+#define DDR_PHY_ADR_I_REGSET_NUM_M3  37
+#define DDR_PHY_ADR_G_REGSET_NUM_M3  64
+#define DDR_PI_REGSET_NUM_M3         202
+
+static const uint32_t DDR_PHY_SLICE_REGSET_M3[DDR_PHY_SLICE_REGSET_NUM_M3] = {
+	/*0800*/ 0x76543210,
+	/*0801*/ 0x0004f008,
+	/*0802*/ 0x00000000,
+	/*0803*/ 0x00000000,
+	/*0804*/ 0x00010000,
+	/*0805*/ 0x036e6e0e,
+	/*0806*/ 0x026e6e0e,
+	/*0807*/ 0x00010300,
+	/*0808*/ 0x04000100,
+	/*0809*/ 0x00000300,
+	/*080a*/ 0x001700c0,
+	/*080b*/ 0x00b00201,
+	/*080c*/ 0x00030020,
+	/*080d*/ 0x00000000,
+	/*080e*/ 0x00000000,
+	/*080f*/ 0x00000000,
+	/*0810*/ 0x00000000,
+	/*0811*/ 0x00000000,
+	/*0812*/ 0x00000000,
+	/*0813*/ 0x00000000,
+	/*0814*/ 0x09000000,
+	/*0815*/ 0x04080000,
+	/*0816*/ 0x04080400,
+	/*0817*/ 0x00000000,
+	/*0818*/ 0x32103210,
+	/*0819*/ 0x00800708,
+	/*081a*/ 0x000f000c,
+	/*081b*/ 0x00000100,
+	/*081c*/ 0x55aa55aa,
+	/*081d*/ 0x33cc33cc,
+	/*081e*/ 0x0ff00ff0,
+	/*081f*/ 0x0f0ff0f0,
+	/*0820*/ 0x00018e38,
+	/*0821*/ 0x00000000,
+	/*0822*/ 0x00000000,
+	/*0823*/ 0x00000000,
+	/*0824*/ 0x00000000,
+	/*0825*/ 0x00000000,
+	/*0826*/ 0x00000000,
+	/*0827*/ 0x00000000,
+	/*0828*/ 0x00000000,
+	/*0829*/ 0x00000000,
+	/*082a*/ 0x00000000,
+	/*082b*/ 0x00000000,
+	/*082c*/ 0x00000000,
+	/*082d*/ 0x00000000,
+	/*082e*/ 0x00000000,
+	/*082f*/ 0x00000000,
+	/*0830*/ 0x00000000,
+	/*0831*/ 0x00000000,
+	/*0832*/ 0x00000000,
+	/*0833*/ 0x00200000,
+	/*0834*/ 0x08200820,
+	/*0835*/ 0x08200820,
+	/*0836*/ 0x08200820,
+	/*0837*/ 0x08200820,
+	/*0838*/ 0x08200820,
+	/*0839*/ 0x00000820,
+	/*083a*/ 0x03000300,
+	/*083b*/ 0x03000300,
+	/*083c*/ 0x03000300,
+	/*083d*/ 0x03000300,
+	/*083e*/ 0x00000300,
+	/*083f*/ 0x00000000,
+	/*0840*/ 0x00000000,
+	/*0841*/ 0x00000000,
+	/*0842*/ 0x00000000,
+	/*0843*/ 0x00a00000,
+	/*0844*/ 0x00a000a0,
+	/*0845*/ 0x00a000a0,
+	/*0846*/ 0x00a000a0,
+	/*0847*/ 0x00a000a0,
+	/*0848*/ 0x00a000a0,
+	/*0849*/ 0x00a000a0,
+	/*084a*/ 0x00a000a0,
+	/*084b*/ 0x00a000a0,
+	/*084c*/ 0x010900a0,
+	/*084d*/ 0x02000104,
+	/*084e*/ 0x00000000,
+	/*084f*/ 0x00010000,
+	/*0850*/ 0x00000200,
+	/*0851*/ 0x4041a151,
+	/*0852*/ 0xc00141a0,
+	/*0853*/ 0x0e0100c0,
+	/*0854*/ 0x0010000c,
+	/*0855*/ 0x0c064208,
+	/*0856*/ 0x000f0c18,
+	/*0857*/ 0x00e00140,
+	/*0858*/ 0x00000c20
+};
+
+static const uint32_t DDR_PHY_ADR_V_REGSET_M3[DDR_PHY_ADR_V_REGSET_NUM_M3] = {
+	/*0a00*/ 0x00000000,
+	/*0a01*/ 0x00000000,
+	/*0a02*/ 0x00000000,
+	/*0a03*/ 0x00000000,
+	/*0a04*/ 0x00000000,
+	/*0a05*/ 0x00000000,
+	/*0a06*/ 0x00000002,
+	/*0a07*/ 0x00000000,
+	/*0a08*/ 0x00000000,
+	/*0a09*/ 0x00000000,
+	/*0a0a*/ 0x00400320,
+	/*0a0b*/ 0x00000040,
+	/*0a0c*/ 0x00dcba98,
+	/*0a0d*/ 0x00000000,
+	/*0a0e*/ 0x00dcba98,
+	/*0a0f*/ 0x01000000,
+	/*0a10*/ 0x00020003,
+	/*0a11*/ 0x00000000,
+	/*0a12*/ 0x00000000,
+	/*0a13*/ 0x00000000,
+	/*0a14*/ 0x0000002a,
+	/*0a15*/ 0x00000015,
+	/*0a16*/ 0x00000015,
+	/*0a17*/ 0x0000002a,
+	/*0a18*/ 0x00000033,
+	/*0a19*/ 0x0000000c,
+	/*0a1a*/ 0x0000000c,
+	/*0a1b*/ 0x00000033,
+	/*0a1c*/ 0x0a418820,
+	/*0a1d*/ 0x003f0000,
+	/*0a1e*/ 0x0000003f,
+	/*0a1f*/ 0x0002c06e,
+	/*0a20*/ 0x02c002c0,
+	/*0a21*/ 0x02c002c0,
+	/*0a22*/ 0x000002c0,
+	/*0a23*/ 0x42080010,
+	/*0a24*/ 0x00000003
+};
+
+static const uint32_t DDR_PHY_ADR_I_REGSET_M3[DDR_PHY_ADR_I_REGSET_NUM_M3] = {
+	/*0a80*/ 0x04040404,
+	/*0a81*/ 0x00000404,
+	/*0a82*/ 0x00000000,
+	/*0a83*/ 0x00000000,
+	/*0a84*/ 0x00000000,
+	/*0a85*/ 0x00000000,
+	/*0a86*/ 0x00000002,
+	/*0a87*/ 0x00000000,
+	/*0a88*/ 0x00000000,
+	/*0a89*/ 0x00000000,
+	/*0a8a*/ 0x00400320,
+	/*0a8b*/ 0x00000040,
+	/*0a8c*/ 0x00000000,
+	/*0a8d*/ 0x00000000,
+	/*0a8e*/ 0x00000000,
+	/*0a8f*/ 0x01000000,
+	/*0a90*/ 0x00020003,
+	/*0a91*/ 0x00000000,
+	/*0a92*/ 0x00000000,
+	/*0a93*/ 0x00000000,
+	/*0a94*/ 0x0000002a,
+	/*0a95*/ 0x00000015,
+	/*0a96*/ 0x00000015,
+	/*0a97*/ 0x0000002a,
+	/*0a98*/ 0x00000033,
+	/*0a99*/ 0x0000000c,
+	/*0a9a*/ 0x0000000c,
+	/*0a9b*/ 0x00000033,
+	/*0a9c*/ 0x00000000,
+	/*0a9d*/ 0x00000000,
+	/*0a9e*/ 0x00000000,
+	/*0a9f*/ 0x0002c06e,
+	/*0aa0*/ 0x02c002c0,
+	/*0aa1*/ 0x02c002c0,
+	/*0aa2*/ 0x000002c0,
+	/*0aa3*/ 0x42080010,
+	/*0aa4*/ 0x00000003
+};
+
+static const uint32_t DDR_PHY_ADR_G_REGSET_M3[DDR_PHY_ADR_G_REGSET_NUM_M3] = {
+	/*0b80*/ 0x00000001,
+	/*0b81*/ 0x00000000,
+	/*0b82*/ 0x00000005,
+	/*0b83*/ 0x04000f00,
+	/*0b84*/ 0x00020080,
+	/*0b85*/ 0x00020055,
+	/*0b86*/ 0x00000000,
+	/*0b87*/ 0x00000000,
+	/*0b88*/ 0x00000000,
+	/*0b89*/ 0x00000050,
+	/*0b8a*/ 0x00000000,
+	/*0b8b*/ 0x01010100,
+	/*0b8c*/ 0x00000600,
+	/*0b8d*/ 0x50640000,
+	/*0b8e*/ 0x01421142,
+	/*0b8f*/ 0x00000142,
+	/*0b90*/ 0x00000000,
+	/*0b91*/ 0x000f1600,
+	/*0b92*/ 0x0f160f16,
+	/*0b93*/ 0x0f160f16,
+	/*0b94*/ 0x00000003,
+	/*0b95*/ 0x0002c000,
+	/*0b96*/ 0x02c002c0,
+	/*0b97*/ 0x000002c0,
+	/*0b98*/ 0x03421342,
+	/*0b99*/ 0x00000342,
+	/*0b9a*/ 0x00000000,
+	/*0b9b*/ 0x00000000,
+	/*0b9c*/ 0x05020000,
+	/*0b9d*/ 0x00000000,
+	/*0b9e*/ 0x00027f6e,
+	/*0b9f*/ 0x047f027f,
+	/*0ba0*/ 0x00027f6e,
+	/*0ba1*/ 0x00047f6e,
+	/*0ba2*/ 0x0003554f,
+	/*0ba3*/ 0x0001554f,
+	/*0ba4*/ 0x0001554f,
+	/*0ba5*/ 0x0001554f,
+	/*0ba6*/ 0x0001554f,
+	/*0ba7*/ 0x00003fee,
+	/*0ba8*/ 0x0001554f,
+	/*0ba9*/ 0x00003fee,
+	/*0baa*/ 0x0001554f,
+	/*0bab*/ 0x00027f6e,
+	/*0bac*/ 0x0001554f,
+	/*0bad*/ 0x00000000,
+	/*0bae*/ 0x00000000,
+	/*0baf*/ 0x00000000,
+	/*0bb0*/ 0x65000000,
+	/*0bb1*/ 0x00000000,
+	/*0bb2*/ 0x00000000,
+	/*0bb3*/ 0x00000201,
+	/*0bb4*/ 0x00000000,
+	/*0bb5*/ 0x00000000,
+	/*0bb6*/ 0x00000000,
+	/*0bb7*/ 0x00000000,
+	/*0bb8*/ 0x00000000,
+	/*0bb9*/ 0x00000000,
+	/*0bba*/ 0x00000000,
+	/*0bbb*/ 0x00000000,
+	/*0bbc*/ 0x06e40000,
+	/*0bbd*/ 0x00000000,
+	/*0bbe*/ 0x00000000,
+	/*0bbf*/ 0x00010000
+};
+
+static const uint32_t DDR_PI_REGSET_M3[DDR_PI_REGSET_NUM_M3] = {
+	/*0200*/ 0x00000b00,
+	/*0201*/ 0x00000100,
+	/*0202*/ 0x00000000,
+	/*0203*/ 0x0000ffff,
+	/*0204*/ 0x00000000,
+	/*0205*/ 0x0000ffff,
+	/*0206*/ 0x00000000,
+	/*0207*/ 0x304cffff,
+	/*0208*/ 0x00000200,
+	/*0209*/ 0x00000200,
+	/*020a*/ 0x00000200,
+	/*020b*/ 0x00000200,
+	/*020c*/ 0x0000304c,
+	/*020d*/ 0x00000200,
+	/*020e*/ 0x00000200,
+	/*020f*/ 0x00000200,
+	/*0210*/ 0x00000200,
+	/*0211*/ 0x0000304c,
+	/*0212*/ 0x00000200,
+	/*0213*/ 0x00000200,
+	/*0214*/ 0x00000200,
+	/*0215*/ 0x00000200,
+	/*0216*/ 0x00010000,
+	/*0217*/ 0x00000003,
+	/*0218*/ 0x01000001,
+	/*0219*/ 0x00000000,
+	/*021a*/ 0x00000000,
+	/*021b*/ 0x00000000,
+	/*021c*/ 0x00000000,
+	/*021d*/ 0x00000000,
+	/*021e*/ 0x00000000,
+	/*021f*/ 0x00000000,
+	/*0220*/ 0x00000000,
+	/*0221*/ 0x00000000,
+	/*0222*/ 0x00000000,
+	/*0223*/ 0x00000000,
+	/*0224*/ 0x00000000,
+	/*0225*/ 0x00000000,
+	/*0226*/ 0x00000000,
+	/*0227*/ 0x00000000,
+	/*0228*/ 0x00000000,
+	/*0229*/ 0x0f000101,
+	/*022a*/ 0x08492d25,
+	/*022b*/ 0x0e0c0004,
+	/*022c*/ 0x000e5000,
+	/*022d*/ 0x00000250,
+	/*022e*/ 0x00460003,
+	/*022f*/ 0x182600cf,
+	/*0230*/ 0x182600cf,
+	/*0231*/ 0x00000005,
+	/*0232*/ 0x00000000,
+	/*0233*/ 0x00000000,
+	/*0234*/ 0x00000000,
+	/*0235*/ 0x00000000,
+	/*0236*/ 0x00000000,
+	/*0237*/ 0x00000000,
+	/*0238*/ 0x00000000,
+	/*0239*/ 0x01000000,
+	/*023a*/ 0x00040404,
+	/*023b*/ 0x01280a00,
+	/*023c*/ 0x00000000,
+	/*023d*/ 0x000f0000,
+	/*023e*/ 0x00001803,
+	/*023f*/ 0x00000000,
+	/*0240*/ 0x00000000,
+	/*0241*/ 0x00060002,
+	/*0242*/ 0x00010001,
+	/*0243*/ 0x01000101,
+	/*0244*/ 0x04020201,
+	/*0245*/ 0x00080804,
+	/*0246*/ 0x00000000,
+	/*0247*/ 0x08030000,
+	/*0248*/ 0x15150408,
+	/*0249*/ 0x00000000,
+	/*024a*/ 0x00000000,
+	/*024b*/ 0x00000000,
+	/*024c*/ 0x000f0f00,
+	/*024d*/ 0x0000001e,
+	/*024e*/ 0x00000000,
+	/*024f*/ 0x01000300,
+	/*0250*/ 0x00000000,
+	/*0251*/ 0x00000000,
+	/*0252*/ 0x01000000,
+	/*0253*/ 0x00010101,
+	/*0254*/ 0x000e0e0e,
+	/*0255*/ 0x000c0c0c,
+	/*0256*/ 0x02060601,
+	/*0257*/ 0x00000000,
+	/*0258*/ 0x00000003,
+	/*0259*/ 0x00181703,
+	/*025a*/ 0x00280006,
+	/*025b*/ 0x00280016,
+	/*025c*/ 0x00000016,
+	/*025d*/ 0x00000000,
+	/*025e*/ 0x00000000,
+	/*025f*/ 0x00000000,
+	/*0260*/ 0x140a0000,
+	/*0261*/ 0x0005010a,
+	/*0262*/ 0x03018d03,
+	/*0263*/ 0x000a018d,
+	/*0264*/ 0x00060100,
+	/*0265*/ 0x01000006,
+	/*0266*/ 0x018e018e,
+	/*0267*/ 0x018e0100,
+	/*0268*/ 0x1111018e,
+	/*0269*/ 0x10010204,
+	/*026a*/ 0x09090650,
+	/*026b*/ 0x20110202,
+	/*026c*/ 0x00201000,
+	/*026d*/ 0x00201000,
+	/*026e*/ 0x04041000,
+	/*026f*/ 0x18020100,
+	/*0270*/ 0x00010118,
+	/*0271*/ 0x004b004a,
+	/*0272*/ 0x050f0000,
+	/*0273*/ 0x0c01021e,
+	/*0274*/ 0x34000000,
+	/*0275*/ 0x00000000,
+	/*0276*/ 0x00000000,
+	/*0277*/ 0x00000000,
+	/*0278*/ 0x0000d400,
+	/*0279*/ 0x0031002e,
+	/*027a*/ 0x00111136,
+	/*027b*/ 0x002e00d4,
+	/*027c*/ 0x11360031,
+	/*027d*/ 0x0000d411,
+	/*027e*/ 0x0031002e,
+	/*027f*/ 0x00111136,
+	/*0280*/ 0x002e00d4,
+	/*0281*/ 0x11360031,
+	/*0282*/ 0x0000d411,
+	/*0283*/ 0x0031002e,
+	/*0284*/ 0x00111136,
+	/*0285*/ 0x002e00d4,
+	/*0286*/ 0x11360031,
+	/*0287*/ 0x00d40011,
+	/*0288*/ 0x0031002e,
+	/*0289*/ 0x00111136,
+	/*028a*/ 0x002e00d4,
+	/*028b*/ 0x11360031,
+	/*028c*/ 0x0000d411,
+	/*028d*/ 0x0031002e,
+	/*028e*/ 0x00111136,
+	/*028f*/ 0x002e00d4,
+	/*0290*/ 0x11360031,
+	/*0291*/ 0x0000d411,
+	/*0292*/ 0x0031002e,
+	/*0293*/ 0x00111136,
+	/*0294*/ 0x002e00d4,
+	/*0295*/ 0x11360031,
+	/*0296*/ 0x02000011,
+	/*0297*/ 0x018d018d,
+	/*0298*/ 0x0c08018d,
+	/*0299*/ 0x1f121d22,
+	/*029a*/ 0x4301b344,
+	/*029b*/ 0x10172006,
+	/*029c*/ 0x1d220c10,
+	/*029d*/ 0x00001f12,
+	/*029e*/ 0x4301b344,
+	/*029f*/ 0x10172006,
+	/*02a0*/ 0x1d220c10,
+	/*02a1*/ 0x00001f12,
+	/*02a2*/ 0x4301b344,
+	/*02a3*/ 0x10172006,
+	/*02a4*/ 0x02000210,
+	/*02a5*/ 0x02000200,
+	/*02a6*/ 0x02000200,
+	/*02a7*/ 0x02000200,
+	/*02a8*/ 0x02000200,
+	/*02a9*/ 0x00000000,
+	/*02aa*/ 0x00000000,
+	/*02ab*/ 0x00000000,
+	/*02ac*/ 0x00000000,
+	/*02ad*/ 0x00000000,
+	/*02ae*/ 0x00000000,
+	/*02af*/ 0x00000000,
+	/*02b0*/ 0x00000000,
+	/*02b1*/ 0x00000000,
+	/*02b2*/ 0x00000000,
+	/*02b3*/ 0x00000000,
+	/*02b4*/ 0x00000000,
+	/*02b5*/ 0x00000400,
+	/*02b6*/ 0x15141312,
+	/*02b7*/ 0x11100f0e,
+	/*02b8*/ 0x080b0c0d,
+	/*02b9*/ 0x05040a09,
+	/*02ba*/ 0x01000706,
+	/*02bb*/ 0x00000302,
+	/*02bc*/ 0x01030201,
+	/*02bd*/ 0x00304c00,
+	/*02be*/ 0x0001e2f8,
+	/*02bf*/ 0x0000304c,
+	/*02c0*/ 0x0001e2f8,
+	/*02c1*/ 0x0000304c,
+	/*02c2*/ 0x0001e2f8,
+	/*02c3*/ 0x08000000,
+	/*02c4*/ 0x00000100,
+	/*02c5*/ 0x00000000,
+	/*02c6*/ 0x00000000,
+	/*02c7*/ 0x00000000,
+	/*02c8*/ 0x00000000,
+	/*02c9*/ 0x00000002
+};
diff --git a/drivers/renesas/common/ddr/ddr_b/init_dram_tbl_m3n.h b/drivers/renesas/common/ddr/ddr_b/init_dram_tbl_m3n.h
new file mode 100644
index 0000000..fb3032d
--- /dev/null
+++ b/drivers/renesas/common/ddr/ddr_b/init_dram_tbl_m3n.h
@@ -0,0 +1,587 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation.
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#define DDR_PHY_SLICE_REGSET_OFS_M3N  0x0800
+#define DDR_PHY_ADR_V_REGSET_OFS_M3N  0x0a00
+#define DDR_PHY_ADR_I_REGSET_OFS_M3N  0x0a80
+#define DDR_PHY_ADR_G_REGSET_OFS_M3N  0x0b80
+#define DDR_PI_REGSET_OFS_M3N         0x0200
+
+#define DDR_PHY_SLICE_REGSET_SIZE_M3N 0x80
+#define DDR_PHY_ADR_V_REGSET_SIZE_M3N 0x80
+#define DDR_PHY_ADR_I_REGSET_SIZE_M3N 0x80
+#define DDR_PHY_ADR_G_REGSET_SIZE_M3N 0x80
+#define DDR_PI_REGSET_SIZE_M3N        0x100
+
+#define DDR_PHY_SLICE_REGSET_NUM_M3N  101
+#define DDR_PHY_ADR_V_REGSET_NUM_M3N  37
+#define DDR_PHY_ADR_I_REGSET_NUM_M3N  37
+#define DDR_PHY_ADR_G_REGSET_NUM_M3N  87
+#define DDR_PI_REGSET_NUM_M3N         286
+
+static const uint32_t DDR_PHY_SLICE_REGSET_M3N[DDR_PHY_SLICE_REGSET_NUM_M3N] = {
+	/*0800*/ 0x76543210,
+	/*0801*/ 0x0004f008,
+	/*0802*/ 0x00020200,
+	/*0803*/ 0x00000000,
+	/*0804*/ 0x00000000,
+	/*0805*/ 0x00010000,
+	/*0806*/ 0x036e6e0e,
+	/*0807*/ 0x026e6e0e,
+	/*0808*/ 0x00000103,
+	/*0809*/ 0x00040001,
+	/*080a*/ 0x00000103,
+	/*080b*/ 0x00000001,
+	/*080c*/ 0x00000000,
+	/*080d*/ 0x00000000,
+	/*080e*/ 0x00000100,
+	/*080f*/ 0x001800c0,
+	/*0810*/ 0x020100b0,
+	/*0811*/ 0x00030020,
+	/*0812*/ 0x00000000,
+	/*0813*/ 0x00000000,
+	/*0814*/ 0x0000aaaa,
+	/*0815*/ 0x00005555,
+	/*0816*/ 0x0000b5b5,
+	/*0817*/ 0x00004a4a,
+	/*0818*/ 0x00000000,
+	/*0819*/ 0x09000000,
+	/*081a*/ 0x04080000,
+	/*081b*/ 0x08040000,
+	/*081c*/ 0x00000004,
+	/*081d*/ 0x00800710,
+	/*081e*/ 0x000f000c,
+	/*081f*/ 0x00000100,
+	/*0820*/ 0x55aa55aa,
+	/*0821*/ 0x33cc33cc,
+	/*0822*/ 0x0ff00ff0,
+	/*0823*/ 0x0f0ff0f0,
+	/*0824*/ 0x00018e38,
+	/*0825*/ 0x00000000,
+	/*0826*/ 0x00000000,
+	/*0827*/ 0x00000000,
+	/*0828*/ 0x00000000,
+	/*0829*/ 0x00000000,
+	/*082a*/ 0x00000000,
+	/*082b*/ 0x00000000,
+	/*082c*/ 0x00000000,
+	/*082d*/ 0x00000000,
+	/*082e*/ 0x00000000,
+	/*082f*/ 0x00000000,
+	/*0830*/ 0x00000000,
+	/*0831*/ 0x00000000,
+	/*0832*/ 0x00000000,
+	/*0833*/ 0x00000000,
+	/*0834*/ 0x00000000,
+	/*0835*/ 0x00000000,
+	/*0836*/ 0x00000000,
+	/*0837*/ 0x00000000,
+	/*0838*/ 0x00000000,
+	/*0839*/ 0x00000000,
+	/*083a*/ 0x00000104,
+	/*083b*/ 0x00082020,
+	/*083c*/ 0x08200820,
+	/*083d*/ 0x08200820,
+	/*083e*/ 0x08200820,
+	/*083f*/ 0x08200820,
+	/*0840*/ 0x08200820,
+	/*0841*/ 0x00000000,
+	/*0842*/ 0x00000000,
+	/*0843*/ 0x03000300,
+	/*0844*/ 0x03000300,
+	/*0845*/ 0x03000300,
+	/*0846*/ 0x03000300,
+	/*0847*/ 0x00000300,
+	/*0848*/ 0x00000000,
+	/*0849*/ 0x00000000,
+	/*084a*/ 0x00000000,
+	/*084b*/ 0x00000000,
+	/*084c*/ 0x00000000,
+	/*084d*/ 0x00a000a0,
+	/*084e*/ 0x00a000a0,
+	/*084f*/ 0x00a000a0,
+	/*0850*/ 0x00a000a0,
+	/*0851*/ 0x00a000a0,
+	/*0852*/ 0x00a000a0,
+	/*0853*/ 0x00a000a0,
+	/*0854*/ 0x00a000a0,
+	/*0855*/ 0x00a000a0,
+	/*0856*/ 0x01040119,
+	/*0857*/ 0x00000200,
+	/*0858*/ 0x01000000,
+	/*0859*/ 0x00000200,
+	/*085a*/ 0x00000004,
+	/*085b*/ 0x4041a151,
+	/*085c*/ 0x0141a0a0,
+	/*085d*/ 0x0000c0c0,
+	/*085e*/ 0x0e0c000e,
+	/*085f*/ 0x10001000,
+	/*0860*/ 0x0c073e42,
+	/*0861*/ 0x000f0c28,
+	/*0862*/ 0x00e00140,
+	/*0863*/ 0x000c0020,
+	/*0864*/ 0x00000203
+};
+
+static const uint32_t DDR_PHY_ADR_V_REGSET_M3N[DDR_PHY_ADR_V_REGSET_NUM_M3N] = {
+	/*0a00*/ 0x00000000,
+	/*0a01*/ 0x00000000,
+	/*0a02*/ 0x00000000,
+	/*0a03*/ 0x00000000,
+	/*0a04*/ 0x00000000,
+	/*0a05*/ 0x00000000,
+	/*0a06*/ 0x00000000,
+	/*0a07*/ 0x01000000,
+	/*0a08*/ 0x00020000,
+	/*0a09*/ 0x00000000,
+	/*0a0a*/ 0x00000000,
+	/*0a0b*/ 0x00000000,
+	/*0a0c*/ 0x00400000,
+	/*0a0d*/ 0x00000080,
+	/*0a0e*/ 0x00dcba98,
+	/*0a0f*/ 0x03000000,
+	/*0a10*/ 0x00000200,
+	/*0a11*/ 0x00000000,
+	/*0a12*/ 0x00000000,
+	/*0a13*/ 0x00000000,
+	/*0a14*/ 0x0000002a,
+	/*0a15*/ 0x00000015,
+	/*0a16*/ 0x00000015,
+	/*0a17*/ 0x0000002a,
+	/*0a18*/ 0x00000033,
+	/*0a19*/ 0x0000000c,
+	/*0a1a*/ 0x0000000c,
+	/*0a1b*/ 0x00000033,
+	/*0a1c*/ 0x0a418820,
+	/*0a1d*/ 0x003f0000,
+	/*0a1e*/ 0x0000013f,
+	/*0a1f*/ 0x0002c06e,
+	/*0a20*/ 0x02c002c0,
+	/*0a21*/ 0x02c002c0,
+	/*0a22*/ 0x000002c0,
+	/*0a23*/ 0x42080010,
+	/*0a24*/ 0x0000033e
+};
+
+static const uint32_t DDR_PHY_ADR_I_REGSET_M3N[DDR_PHY_ADR_I_REGSET_NUM_M3N] = {
+	/*0a80*/ 0x00000000,
+	/*0a81*/ 0x00000000,
+	/*0a82*/ 0x00000000,
+	/*0a83*/ 0x00000000,
+	/*0a84*/ 0x00000000,
+	/*0a85*/ 0x00000000,
+	/*0a86*/ 0x00000000,
+	/*0a87*/ 0x01000000,
+	/*0a88*/ 0x00020000,
+	/*0a89*/ 0x00000000,
+	/*0a8a*/ 0x00000000,
+	/*0a8b*/ 0x00000000,
+	/*0a8c*/ 0x00400000,
+	/*0a8d*/ 0x00000080,
+	/*0a8e*/ 0x00000000,
+	/*0a8f*/ 0x03000000,
+	/*0a90*/ 0x00000200,
+	/*0a91*/ 0x00000000,
+	/*0a92*/ 0x00000000,
+	/*0a93*/ 0x00000000,
+	/*0a94*/ 0x0000002a,
+	/*0a95*/ 0x00000015,
+	/*0a96*/ 0x00000015,
+	/*0a97*/ 0x0000002a,
+	/*0a98*/ 0x00000033,
+	/*0a99*/ 0x0000000c,
+	/*0a9a*/ 0x0000000c,
+	/*0a9b*/ 0x00000033,
+	/*0a9c*/ 0x00000000,
+	/*0a9d*/ 0x00000000,
+	/*0a9e*/ 0x00000000,
+	/*0a9f*/ 0x0002c06e,
+	/*0aa0*/ 0x02c002c0,
+	/*0aa1*/ 0x02c002c0,
+	/*0aa2*/ 0x000002c0,
+	/*0aa3*/ 0x42080010,
+	/*0aa4*/ 0x0000033e
+};
+
+static const uint32_t DDR_PHY_ADR_G_REGSET_M3N[DDR_PHY_ADR_G_REGSET_NUM_M3N] = {
+	/*0b80*/ 0x00000000,
+	/*0b81*/ 0x00000100,
+	/*0b82*/ 0x00000000,
+	/*0b83*/ 0x00050000,
+	/*0b84*/ 0x00000000,
+	/*0b85*/ 0x0004000f,
+	/*0b86*/ 0x00280080,
+	/*0b87*/ 0x02005502,
+	/*0b88*/ 0x00000000,
+	/*0b89*/ 0x00000000,
+	/*0b8a*/ 0x00000000,
+	/*0b8b*/ 0x00000050,
+	/*0b8c*/ 0x00000000,
+	/*0b8d*/ 0x01010100,
+	/*0b8e*/ 0x00010000,
+	/*0b8f*/ 0x00000000,
+	/*0b90*/ 0x00000101,
+	/*0b91*/ 0x00000000,
+	/*0b92*/ 0x00000000,
+	/*0b93*/ 0x00000000,
+	/*0b94*/ 0x00000000,
+	/*0b95*/ 0x00005064,
+	/*0b96*/ 0x01421142,
+	/*0b97*/ 0x00000142,
+	/*0b98*/ 0x00000000,
+	/*0b99*/ 0x000f1600,
+	/*0b9a*/ 0x0f160f16,
+	/*0b9b*/ 0x0f160f16,
+	/*0b9c*/ 0x00000003,
+	/*0b9d*/ 0x0002c000,
+	/*0b9e*/ 0x02c002c0,
+	/*0b9f*/ 0x000002c0,
+	/*0ba0*/ 0x08040201,
+	/*0ba1*/ 0x03421342,
+	/*0ba2*/ 0x00000342,
+	/*0ba3*/ 0x00000000,
+	/*0ba4*/ 0x00000000,
+	/*0ba5*/ 0x05030000,
+	/*0ba6*/ 0x00010700,
+	/*0ba7*/ 0x00000014,
+	/*0ba8*/ 0x00027f6e,
+	/*0ba9*/ 0x047f027f,
+	/*0baa*/ 0x00027f6e,
+	/*0bab*/ 0x00047f6e,
+	/*0bac*/ 0x0003554f,
+	/*0bad*/ 0x0001554f,
+	/*0bae*/ 0x0001554f,
+	/*0baf*/ 0x0001554f,
+	/*0bb0*/ 0x0001554f,
+	/*0bb1*/ 0x00003fee,
+	/*0bb2*/ 0x0001554f,
+	/*0bb3*/ 0x00003fee,
+	/*0bb4*/ 0x0001554f,
+	/*0bb5*/ 0x00027f6e,
+	/*0bb6*/ 0x0001554f,
+	/*0bb7*/ 0x00004011,
+	/*0bb8*/ 0x00004410,
+	/*0bb9*/ 0x00000000,
+	/*0bba*/ 0x00000000,
+	/*0bbb*/ 0x00000000,
+	/*0bbc*/ 0x00000265,
+	/*0bbd*/ 0x00000000,
+	/*0bbe*/ 0x00040401,
+	/*0bbf*/ 0x00000000,
+	/*0bc0*/ 0x03000000,
+	/*0bc1*/ 0x00000020,
+	/*0bc2*/ 0x00000000,
+	/*0bc3*/ 0x00000000,
+	/*0bc4*/ 0x04102006,
+	/*0bc5*/ 0x00041020,
+	/*0bc6*/ 0x01c98c98,
+	/*0bc7*/ 0x00400000,
+	/*0bc8*/ 0x00000000,
+	/*0bc9*/ 0x0001ffff,
+	/*0bca*/ 0x00000000,
+	/*0bcb*/ 0x00000000,
+	/*0bcc*/ 0x00000001,
+	/*0bcd*/ 0x00000000,
+	/*0bce*/ 0x00000000,
+	/*0bcf*/ 0x00000000,
+	/*0bd0*/ 0x76543210,
+	/*0bd1*/ 0x06010198,
+	/*0bd2*/ 0x00000000,
+	/*0bd3*/ 0x00000000,
+	/*0bd4*/ 0x04070000,
+	/*0bd5*/ 0x00000001,
+	/*0bd6*/ 0x00000f00
+};
+
+static const uint32_t DDR_PI_REGSET_M3N[DDR_PI_REGSET_NUM_M3N] = {
+	/*0200*/ 0x00000b00,
+	/*0201*/ 0x00000101,
+	/*0202*/ 0x01640000,
+	/*0203*/ 0x00000014,
+	/*0204*/ 0x00000014,
+	/*0205*/ 0x00000014,
+	/*0206*/ 0x00000014,
+	/*0207*/ 0x00000000,
+	/*0208*/ 0x00000000,
+	/*0209*/ 0x0000ffff,
+	/*020a*/ 0x00000000,
+	/*020b*/ 0x0000ffff,
+	/*020c*/ 0x00000000,
+	/*020d*/ 0x0000ffff,
+	/*020e*/ 0x0000304c,
+	/*020f*/ 0x00000200,
+	/*0210*/ 0x00000200,
+	/*0211*/ 0x00000200,
+	/*0212*/ 0x00000200,
+	/*0213*/ 0x0000304c,
+	/*0214*/ 0x00000200,
+	/*0215*/ 0x00000200,
+	/*0216*/ 0x00000200,
+	/*0217*/ 0x00000200,
+	/*0218*/ 0x0000304c,
+	/*0219*/ 0x00000200,
+	/*021a*/ 0x00000200,
+	/*021b*/ 0x00000200,
+	/*021c*/ 0x00000200,
+	/*021d*/ 0x00010000,
+	/*021e*/ 0x00000003,
+	/*021f*/ 0x01000001,
+	/*0220*/ 0x00000000,
+	/*0221*/ 0x00000000,
+	/*0222*/ 0x00000000,
+	/*0223*/ 0x00000000,
+	/*0224*/ 0x00000000,
+	/*0225*/ 0x00000000,
+	/*0226*/ 0x00000000,
+	/*0227*/ 0x00000000,
+	/*0228*/ 0x00000000,
+	/*0229*/ 0x00000000,
+	/*022a*/ 0x00000000,
+	/*022b*/ 0x00000000,
+	/*022c*/ 0x00000000,
+	/*022d*/ 0x00000000,
+	/*022e*/ 0x00000000,
+	/*022f*/ 0x00000000,
+	/*0230*/ 0x0f000101,
+	/*0231*/ 0x084d3129,
+	/*0232*/ 0x0e0c0004,
+	/*0233*/ 0x000e5000,
+	/*0234*/ 0x01000250,
+	/*0235*/ 0x00000003,
+	/*0236*/ 0x00000046,
+	/*0237*/ 0x000000cf,
+	/*0238*/ 0x00001826,
+	/*0239*/ 0x000000cf,
+	/*023a*/ 0x00001826,
+	/*023b*/ 0x00000000,
+	/*023c*/ 0x00000000,
+	/*023d*/ 0x00000000,
+	/*023e*/ 0x00000000,
+	/*023f*/ 0x00000000,
+	/*0240*/ 0x00000000,
+	/*0241*/ 0x00000000,
+	/*0242*/ 0x00000000,
+	/*0243*/ 0x00000000,
+	/*0244*/ 0x00000000,
+	/*0245*/ 0x01000000,
+	/*0246*/ 0x00040404,
+	/*0247*/ 0x01280a00,
+	/*0248*/ 0x00000001,
+	/*0249*/ 0x00000000,
+	/*024a*/ 0x03000f00,
+	/*024b*/ 0x00200020,
+	/*024c*/ 0x00000020,
+	/*024d*/ 0x00000000,
+	/*024e*/ 0x00000000,
+	/*024f*/ 0x00010002,
+	/*0250*/ 0x01010001,
+	/*0251*/ 0x02010100,
+	/*0252*/ 0x08040402,
+	/*0253*/ 0x00000008,
+	/*0254*/ 0x00000000,
+	/*0255*/ 0x04080803,
+	/*0256*/ 0x00001515,
+	/*0257*/ 0x00000000,
+	/*0258*/ 0x000000aa,
+	/*0259*/ 0x00000055,
+	/*025a*/ 0x000000b5,
+	/*025b*/ 0x0000004a,
+	/*025c*/ 0x00000056,
+	/*025d*/ 0x000000a9,
+	/*025e*/ 0x000000a9,
+	/*025f*/ 0x000000b5,
+	/*0260*/ 0x00000000,
+	/*0261*/ 0x00000000,
+	/*0262*/ 0x0f000000,
+	/*0263*/ 0x00001e0f,
+	/*0264*/ 0x000007d0,
+	/*0265*/ 0x01000300,
+	/*0266*/ 0x00000100,
+	/*0267*/ 0x00000000,
+	/*0268*/ 0x00000000,
+	/*0269*/ 0x01000000,
+	/*026a*/ 0x00010101,
+	/*026b*/ 0x000e0e0e,
+	/*026c*/ 0x000c0c0c,
+	/*026d*/ 0x01060601,
+	/*026e*/ 0x04041717,
+	/*026f*/ 0x00000004,
+	/*0270*/ 0x00000300,
+	/*0271*/ 0x17030000,
+	/*0272*/ 0x00060018,
+	/*0273*/ 0x00160028,
+	/*0274*/ 0x00160028,
+	/*0275*/ 0x00000000,
+	/*0276*/ 0x00000000,
+	/*0277*/ 0x00000000,
+	/*0278*/ 0x0a000000,
+	/*0279*/ 0x00010a14,
+	/*027a*/ 0x00030005,
+	/*027b*/ 0x0003018d,
+	/*027c*/ 0x000a018d,
+	/*027d*/ 0x00060100,
+	/*027e*/ 0x01000006,
+	/*027f*/ 0x018e018e,
+	/*0280*/ 0x018e0100,
+	/*0281*/ 0x1e1a018e,
+	/*0282*/ 0x1e1a1e1a,
+	/*0283*/ 0x01010204,
+	/*0284*/ 0x06501001,
+	/*0285*/ 0x090d0a07,
+	/*0286*/ 0x090d0a07,
+	/*0287*/ 0x0811180f,
+	/*0288*/ 0x00ff1102,
+	/*0289*/ 0x00ff1000,
+	/*028a*/ 0x00ff1000,
+	/*028b*/ 0x04041000,
+	/*028c*/ 0x18020100,
+	/*028d*/ 0x01010018,
+	/*028e*/ 0x005f005f,
+	/*028f*/ 0x005f005f,
+	/*0290*/ 0x050f0000,
+	/*0291*/ 0x051e051e,
+	/*0292*/ 0x0c01021e,
+	/*0293*/ 0x00000c0c,
+	/*0294*/ 0x00003400,
+	/*0295*/ 0x00000000,
+	/*0296*/ 0x00000000,
+	/*0297*/ 0x00000000,
+	/*0298*/ 0x00000000,
+	/*0299*/ 0x002e00d4,
+	/*029a*/ 0x11360031,
+	/*029b*/ 0x00d41611,
+	/*029c*/ 0x0031002e,
+	/*029d*/ 0x16111136,
+	/*029e*/ 0x002e00d4,
+	/*029f*/ 0x11360031,
+	/*02a0*/ 0x00001611,
+	/*02a1*/ 0x002e00d4,
+	/*02a2*/ 0x11360031,
+	/*02a3*/ 0x00d41611,
+	/*02a4*/ 0x0031002e,
+	/*02a5*/ 0x16111136,
+	/*02a6*/ 0x002e00d4,
+	/*02a7*/ 0x11360031,
+	/*02a8*/ 0x00001611,
+	/*02a9*/ 0x002e00d4,
+	/*02aa*/ 0x11360031,
+	/*02ab*/ 0x00d41611,
+	/*02ac*/ 0x0031002e,
+	/*02ad*/ 0x16111136,
+	/*02ae*/ 0x002e00d4,
+	/*02af*/ 0x11360031,
+	/*02b0*/ 0x00001611,
+	/*02b1*/ 0x002e00d4,
+	/*02b2*/ 0x11360031,
+	/*02b3*/ 0x00d41611,
+	/*02b4*/ 0x0031002e,
+	/*02b5*/ 0x16111136,
+	/*02b6*/ 0x002e00d4,
+	/*02b7*/ 0x11360031,
+	/*02b8*/ 0x00001611,
+	/*02b9*/ 0x00018d00,
+	/*02ba*/ 0x018d018d,
+	/*02bb*/ 0x1d220c08,
+	/*02bc*/ 0x00001f12,
+	/*02bd*/ 0x4301b344,
+	/*02be*/ 0x17032006,
+	/*02bf*/ 0x220c1010,
+	/*02c0*/ 0x001f121d,
+	/*02c1*/ 0x4301b344,
+	/*02c2*/ 0x17062006,
+	/*02c3*/ 0x220c1010,
+	/*02c4*/ 0x001f121d,
+	/*02c5*/ 0x4301b344,
+	/*02c6*/ 0x17182006,
+	/*02c7*/ 0x00021010,
+	/*02c8*/ 0x00020002,
+	/*02c9*/ 0x00020002,
+	/*02ca*/ 0x00020002,
+	/*02cb*/ 0x00020002,
+	/*02cc*/ 0x00000002,
+	/*02cd*/ 0x00000000,
+	/*02ce*/ 0x00000000,
+	/*02cf*/ 0x00000000,
+	/*02d0*/ 0x00000000,
+	/*02d1*/ 0x00000000,
+	/*02d2*/ 0x00000000,
+	/*02d3*/ 0x00000000,
+	/*02d4*/ 0x00000000,
+	/*02d5*/ 0x00000000,
+	/*02d6*/ 0x00000000,
+	/*02d7*/ 0x00000000,
+	/*02d8*/ 0x00000000,
+	/*02d9*/ 0x00000400,
+	/*02da*/ 0x15141312,
+	/*02db*/ 0x11100f0e,
+	/*02dc*/ 0x080b0c0d,
+	/*02dd*/ 0x05040a09,
+	/*02de*/ 0x01000706,
+	/*02df*/ 0x00000302,
+	/*02e0*/ 0x01030201,
+	/*02e1*/ 0x00304c08,
+	/*02e2*/ 0x0001e2f8,
+	/*02e3*/ 0x0000304c,
+	/*02e4*/ 0x0001e2f8,
+	/*02e5*/ 0x0000304c,
+	/*02e6*/ 0x0001e2f8,
+	/*02e7*/ 0x08000000,
+	/*02e8*/ 0x00000100,
+	/*02e9*/ 0x00000000,
+	/*02ea*/ 0x00000000,
+	/*02eb*/ 0x00000000,
+	/*02ec*/ 0x00000000,
+	/*02ed*/ 0x00010000,
+	/*02ee*/ 0x00000000,
+	/*02ef*/ 0x00000000,
+	/*02f0*/ 0x00000000,
+	/*02f1*/ 0x00000000,
+	/*02f2*/ 0x00000000,
+	/*02f3*/ 0x00000000,
+	/*02f4*/ 0x00000000,
+	/*02f5*/ 0x00000000,
+	/*02f6*/ 0x00000000,
+	/*02f7*/ 0x00000000,
+	/*02f8*/ 0x00000000,
+	/*02f9*/ 0x00000000,
+	/*02fa*/ 0x00000000,
+	/*02fb*/ 0x00000000,
+	/*02fc*/ 0x00000000,
+	/*02fd*/ 0x00000000,
+	/*02fe*/ 0x00000000,
+	/*02ff*/ 0x00000000,
+	/*0300*/ 0x00000000,
+	/*0301*/ 0x00000000,
+	/*0302*/ 0x00000000,
+	/*0303*/ 0x00000000,
+	/*0304*/ 0x00000000,
+	/*0305*/ 0x00000000,
+	/*0306*/ 0x00000000,
+	/*0307*/ 0x00000000,
+	/*0308*/ 0x00000000,
+	/*0309*/ 0x00000000,
+	/*030a*/ 0x00000000,
+	/*030b*/ 0x00000000,
+	/*030c*/ 0x00000000,
+	/*030d*/ 0x00000000,
+	/*030e*/ 0x00000000,
+	/*030f*/ 0x00050002,
+	/*0310*/ 0x015c0057,
+	/*0311*/ 0x01000100,
+	/*0312*/ 0x01020001,
+	/*0313*/ 0x00010300,
+	/*0314*/ 0x05000104,
+	/*0315*/ 0x01060001,
+	/*0316*/ 0x00010700,
+	/*0317*/ 0x00000000,
+	/*0318*/ 0x00000000,
+	/*0319*/ 0x00000001,
+	/*031a*/ 0x00000000,
+	/*031b*/ 0x00000000,
+	/*031c*/ 0x00000000,
+	/*031d*/ 0x20080101
+};
diff --git a/drivers/renesas/common/ddr/dram_sub_func.c b/drivers/renesas/common/ddr/dram_sub_func.c
new file mode 100644
index 0000000..ab8eabb
--- /dev/null
+++ b/drivers/renesas/common/ddr/dram_sub_func.c
@@ -0,0 +1,165 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "dram_sub_func.h"
+#include "rcar_def.h"
+
+#if RCAR_SYSTEM_SUSPEND
+/* Local defines */
+#define DRAM_BACKUP_GPIO_USE		0
+#include "iic_dvfs.h"
+#if PMIC_ROHM_BD9571
+#define	PMIC_SLAVE_ADDR			0x30U
+#define	PMIC_BKUP_MODE_CNT		0x20U
+#define	PMIC_QLLM_CNT			0x27U
+#define	BIT_BKUP_CTRL_OUT		BIT(4)
+#define	BIT_QLLM_DDR0_EN		BIT(0)
+#define	BIT_QLLM_DDR1_EN		BIT(1)
+#endif
+
+#define GPIO_BKUP_REQB_SHIFT_SALVATOR	9U	/* GP1_9 (BKUP_REQB) */
+#define GPIO_BKUP_TRG_SHIFT_SALVATOR	8U	/* GP1_8 (BKUP_TRG) */
+#define GPIO_BKUP_REQB_SHIFT_EBISU	14U	/* GP6_14(BKUP_REQB) */
+#define GPIO_BKUP_TRG_SHIFT_EBISU	13U	/* GP6_13(BKUP_TRG) */
+#define GPIO_BKUP_REQB_SHIFT_CONDOR	1U	/* GP3_1 (BKUP_REQB) */
+#define GPIO_BKUP_TRG_SHIFT_CONDOR	0U	/* GP3_0 (BKUP_TRG) */
+
+#define DRAM_BKUP_TRG_LOOP_CNT		1000U
+#endif
+
+void rcar_dram_get_boot_status(uint32_t *status)
+{
+#if RCAR_SYSTEM_SUSPEND
+	uint32_t reg_data;
+	uint32_t product;
+	uint32_t shift;
+	uint32_t gpio;
+
+	product = mmio_read_32(PRR) & PRR_PRODUCT_MASK;
+	if (product == PRR_PRODUCT_V3H) {
+		shift = GPIO_BKUP_TRG_SHIFT_CONDOR;
+		gpio = GPIO_INDT3;
+	} else if (product == PRR_PRODUCT_E3) {
+		shift = GPIO_BKUP_TRG_SHIFT_EBISU;
+		gpio = GPIO_INDT6;
+	} else {
+		shift = GPIO_BKUP_TRG_SHIFT_SALVATOR;
+		gpio = GPIO_INDT1;
+	}
+
+	reg_data = mmio_read_32(gpio);
+	if (reg_data & BIT(shift))
+		*status = DRAM_BOOT_STATUS_WARM;
+	else
+		*status = DRAM_BOOT_STATUS_COLD;
+#else	/* RCAR_SYSTEM_SUSPEND */
+	*status = DRAM_BOOT_STATUS_COLD;
+#endif	/* RCAR_SYSTEM_SUSPEND */
+}
+
+int32_t rcar_dram_update_boot_status(uint32_t status)
+{
+	int32_t ret = 0;
+#if RCAR_SYSTEM_SUSPEND
+	uint32_t reg_data;
+#if PMIC_ROHM_BD9571
+#if DRAM_BACKUP_GPIO_USE == 0
+	uint8_t bkup_mode_cnt = 0U;
+#else
+	uint32_t reqb, outd;
+#endif
+	uint8_t qllm_cnt = 0U;
+	int32_t i2c_dvfs_ret = -1;
+#endif
+	uint32_t loop_count;
+	uint32_t product;
+	uint32_t trg;
+	uint32_t gpio;
+
+	product = mmio_read_32(PRR) & PRR_PRODUCT_MASK;
+	if (product == PRR_PRODUCT_V3H) {
+#if DRAM_BACKUP_GPIO_USE == 1
+		reqb = GPIO_BKUP_REQB_SHIFT_CONDOR;
+		outd = GPIO_OUTDT3;
+#endif
+		trg = GPIO_BKUP_TRG_SHIFT_CONDOR;
+		gpio = GPIO_INDT3;
+	} else if (product == PRR_PRODUCT_E3) {
+#if DRAM_BACKUP_GPIO_USE == 1
+		reqb = GPIO_BKUP_REQB_SHIFT_EBISU;
+		outd = GPIO_OUTDT6;
+#endif
+		trg = GPIO_BKUP_TRG_SHIFT_EBISU;
+		gpio = GPIO_INDT6;
+	} else {
+#if DRAM_BACKUP_GPIO_USE == 1
+		reqb = GPIO_BKUP_REQB_SHIFT_SALVATOR;
+		outd = GPIO_OUTDT1;
+#endif
+		trg = GPIO_BKUP_TRG_SHIFT_SALVATOR;
+		gpio = GPIO_INDT1;
+	}
+
+	if (status == DRAM_BOOT_STATUS_WARM) {
+#if DRAM_BACKUP_GPIO_USE == 1
+		mmio_setbits_32(outd, BIT(reqb));
+#else
+#if PMIC_ROHM_BD9571
+		/* Set BKUP_CRTL_OUT=High (BKUP mode cnt register) */
+		i2c_dvfs_ret = rcar_iic_dvfs_receive(PMIC_SLAVE_ADDR,
+						     PMIC_BKUP_MODE_CNT,
+						     &bkup_mode_cnt);
+		if (i2c_dvfs_ret) {
+			ERROR("BKUP mode cnt READ ERROR.\n");
+			ret = DRAM_UPDATE_STATUS_ERR;
+		} else {
+			bkup_mode_cnt &= (uint8_t)~BIT_BKUP_CTRL_OUT;
+			i2c_dvfs_ret = rcar_iic_dvfs_send(PMIC_SLAVE_ADDR,
+							  PMIC_BKUP_MODE_CNT,
+							  bkup_mode_cnt);
+			if (i2c_dvfs_ret) {
+				ERROR("BKUP mode cnt WRITE ERROR. value = %d\n",
+				      bkup_mode_cnt);
+				ret = DRAM_UPDATE_STATUS_ERR;
+			}
+		}
+#endif /* PMIC_ROHM_BD9571 */
+#endif /* DRAM_BACKUP_GPIO_USE == 1 */
+		/* Wait BKUP_TRG=Low */
+		loop_count = DRAM_BKUP_TRG_LOOP_CNT;
+		while (loop_count > 0) {
+			reg_data = mmio_read_32(gpio);
+			if (!(reg_data & BIT(trg)))
+				break;
+			loop_count--;
+		}
+
+		if (!loop_count) {
+			ERROR("\nWarm booting...\n"
+			      " The potential of BKUP_TRG did not switch to Low.\n"
+			      " If you expect the operation of cold boot,\n"
+			      " check the board configuration (ex, Dip-SW) and/or the H/W failure.\n");
+			ret = DRAM_UPDATE_STATUS_ERR;
+		}
+	}
+#if PMIC_ROHM_BD9571
+	if (!ret) {
+		qllm_cnt = BIT_QLLM_DDR0_EN | BIT_QLLM_DDR1_EN;
+		i2c_dvfs_ret = rcar_iic_dvfs_send(PMIC_SLAVE_ADDR,
+						  PMIC_QLLM_CNT,
+						  qllm_cnt);
+		if (i2c_dvfs_ret) {
+			ERROR("QLLM cnt WRITE ERROR. value = %d\n", qllm_cnt);
+			ret = DRAM_UPDATE_STATUS_ERR;
+		}
+	}
+#endif
+#endif
+	return ret;
+}
diff --git a/drivers/renesas/common/ddr/dram_sub_func.h b/drivers/renesas/common/ddr/dram_sub_func.h
new file mode 100644
index 0000000..69c4d86
--- /dev/null
+++ b/drivers/renesas/common/ddr/dram_sub_func.h
@@ -0,0 +1,17 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef DRAM_SUB_FUNC_H
+#define DRAM_SUB_FUNC_H
+
+#define DRAM_UPDATE_STATUS_ERR	-1
+#define DRAM_BOOT_STATUS_COLD	0
+#define DRAM_BOOT_STATUS_WARM	1
+
+int32_t rcar_dram_update_boot_status(uint32_t status);
+void rcar_dram_get_boot_status(uint32_t *status);
+
+#endif /* DRAM_SUB_FUNC_H */
diff --git a/drivers/renesas/common/ddr_regs.h b/drivers/renesas/common/ddr_regs.h
new file mode 100644
index 0000000..ba26c69
--- /dev/null
+++ b/drivers/renesas/common/ddr_regs.h
@@ -0,0 +1,257 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef BOOT_INIT_DRAM_REGDEF_H_
+#define BOOT_INIT_DRAM_REGDEF_H_
+
+/* DBSC registers */
+#define DBSC_DBSYSCONF0		0xE6790000U
+#define DBSC_DBSYSCONF1		0xE6790004U
+#define DBSC_DBPHYCONF0		0xE6790010U
+#define DBSC_DBKIND		0xE6790020U
+#define DBSC_DBMEMCONF(ch, cs)	(0xE6790030U + 0x10U * (ch) + 0x04U * (cs))
+#define DBSC_DBMEMCONF_0_0	0xE6790030U
+#define DBSC_DBMEMCONF_0_1	0xE6790034U
+#define DBSC_DBMEMCONF_0_2	0xE6790038U
+#define DBSC_DBMEMCONF_0_3	0xE679003CU
+#define DBSC_DBMEMCONF_1_2	0xE6790048U
+#define DBSC_DBMEMCONF_1_3	0xE679004CU
+#define DBSC_DBMEMCONF_1_0	0xE6790040U
+#define DBSC_DBMEMCONF_1_1	0xE6790044U
+#define DBSC_DBMEMCONF_2_0	0xE6790050U
+#define DBSC_DBMEMCONF_2_1	0xE6790054U
+#define DBSC_DBMEMCONF_2_2	0xE6790058U
+#define DBSC_DBMEMCONF_2_3	0xE679005CU
+#define DBSC_DBMEMCONF_3_0	0xE6790060U
+#define DBSC_DBMEMCONF_3_1	0xE6790064U
+#define DBSC_DBMEMCONF_3_2	0xE6790068U
+#define DBSC_DBMEMCONF_3_3	0xE679006CU
+#define DBSC_DBSYSCNT0		0xE6790100U
+#define DBSC_DBSVCR1		0xE6790104U
+#define DBSC_DBSTATE0		0xE6790108U
+#define DBSC_DBSTATE1		0xE679010CU
+#define DBSC_DBINTEN		0xE6790180U
+#define DBSC_DBINTSTAT0		0xE6790184U
+#define DBSC_DBACEN		0xE6790200U
+#define DBSC_DBRFEN		0xE6790204U
+#define DBSC_DBCMD		0xE6790208U
+#define DBSC_DBWAIT		0xE6790210U
+#define DBSC_DBSYSCTRL0		0xE6790280U
+#define DBSC_DBTR(x)		(0xE6790300U + 0x04U * (x))
+#define DBSC_DBTR0		0xE6790300U
+#define DBSC_DBTR1		0xE6790304U
+#define DBSC_DBTR2		0xE6790308U
+#define DBSC_DBTR3		0xE679030CU
+#define DBSC_DBTR4		0xE6790310U
+#define DBSC_DBTR5		0xE6790314U
+#define DBSC_DBTR6		0xE6790318U
+#define DBSC_DBTR7		0xE679031CU
+#define DBSC_DBTR8		0xE6790320U
+#define DBSC_DBTR9		0xE6790324U
+#define DBSC_DBTR10		0xE6790328U
+#define DBSC_DBTR11		0xE679032CU
+#define DBSC_DBTR12		0xE6790330U
+#define DBSC_DBTR13		0xE6790334U
+#define DBSC_DBTR14		0xE6790338U
+#define DBSC_DBTR15		0xE679033CU
+#define DBSC_DBTR16		0xE6790340U
+#define DBSC_DBTR17		0xE6790344U
+#define DBSC_DBTR18		0xE6790348U
+#define DBSC_DBTR19		0xE679034CU
+#define DBSC_DBTR20		0xE6790350U
+#define DBSC_DBTR21		0xE6790354U
+#define DBSC_DBTR22		0xE6790358U
+#define DBSC_DBTR23		0xE679035CU
+#define DBSC_DBTR24		0xE6790360U
+#define DBSC_DBTR25		0xE6790364U
+#define DBSC_DBTR26		0xE6790368U
+#define DBSC_DBBL		0xE6790400U
+#define DBSC_DBRFCNF1		0xE6790414U
+#define DBSC_DBRFCNF2		0xE6790418U
+#define DBSC_DBTSPCNF		0xE6790420U
+#define DBSC_DBCALCNF		0xE6790424U
+#define DBSC_DBRNK(x)		(0xE6790430U + 0x04U * (x))
+#define DBSC_DBRNK2		0xE6790438U
+#define DBSC_DBRNK3		0xE679043CU
+#define DBSC_DBRNK4		0xE6790440U
+#define DBSC_DBRNK5		0xE6790444U
+#define DBSC_DBPDNCNF		0xE6790450U
+#define DBSC_DBODT(x)		(0xE6790460U + 0x04U * (x))
+#define DBSC_DBODT0		0xE6790460U
+#define DBSC_DBODT1		0xE6790464U
+#define DBSC_DBODT2		0xE6790468U
+#define DBSC_DBODT3		0xE679046CU
+#define DBSC_DBODT4		0xE6790470U
+#define DBSC_DBODT5		0xE6790474U
+#define DBSC_DBODT6		0xE6790478U
+#define DBSC_DBODT7		0xE679047CU
+#define DBSC_DBADJ0		0xE6790500U
+#define DBSC_DBDBICNT		0xE6790518U
+#define DBSC_DBDFIPMSTRCNF	0xE6790520U
+#define DBSC_DBDFICUPDCNF	0xE679052CU
+#define DBSC_DBDFISTAT(ch)	(0xE6790600U + 0x40U * (ch))
+#define DBSC_DBDFISTAT_0	0xE6790600U
+#define DBSC_DBDFISTAT_1	0xE6790640U
+#define DBSC_DBDFISTAT_2	0xE6790680U
+#define DBSC_DBDFISTAT_3	0xE67906C0U
+#define DBSC_DBDFICNT(ch)	(0xE6790604U + 0x40U * (ch))
+#define DBSC_DBDFICNT_0		0xE6790604U
+#define DBSC_DBDFICNT_1		0xE6790644U
+#define DBSC_DBDFICNT_2		0xE6790684U
+#define DBSC_DBDFICNT_3		0xE67906C4U
+#define DBSC_DBPDCNT0(ch)	(0xE6790610U + 0x40U * (ch))
+#define DBSC_DBPDCNT0_0		0xE6790610U
+#define DBSC_DBPDCNT0_1		0xE6790650U
+#define DBSC_DBPDCNT0_2		0xE6790690U
+#define DBSC_DBPDCNT0_3		0xE67906D0U
+#define DBSC_DBPDCNT1(ch)	(0xE6790614U + 0x40U * (ch))
+#define DBSC_DBPDCNT1_0		0xE6790614U
+#define DBSC_DBPDCNT1_1		0xE6790654U
+#define DBSC_DBPDCNT1_2		0xE6790694U
+#define DBSC_DBPDCNT1_3		0xE67906D4U
+#define DBSC_DBPDCNT2(ch)	(0xE6790618U + 0x40U * (ch))
+#define DBSC_DBPDCNT2_0		0xE6790618U
+#define DBSC_DBPDCNT2_1		0xE6790658U
+#define DBSC_DBPDCNT2_2		0xE6790698U
+#define DBSC_DBPDCNT2_3		0xE67906D8U
+#define DBSC_DBPDCNT3(ch)	(0xE679061CU + 0x40U * (ch))
+#define DBSC_DBPDCNT3_0		0xE679061CU
+#define DBSC_DBPDCNT3_1		0xE679065CU
+#define DBSC_DBPDCNT3_2		0xE679069CU
+#define DBSC_DBPDCNT3_3		0xE67906DCU
+#define DBSC_DBPDLK(ch)		(0xE6790620U + 0x40U * (ch))
+#define DBSC_DBPDLK_0		0xE6790620U
+#define DBSC_DBPDLK_1		0xE6790660U
+#define DBSC_DBPDLK_2		0xE67906a0U
+#define DBSC_DBPDLK_3		0xE67906e0U
+#define DBSC_DBPDRGA(ch)	(0xE6790624U + 0x40U * (ch))
+#define DBSC_DBPDRGD(ch)	(0xE6790628U + 0x40U * (ch))
+#define DBSC_DBPDRGA_0		0xE6790624U
+#define DBSC_DBPDRGD_0		0xE6790628U
+#define DBSC_DBPDRGA_1		0xE6790664U
+#define DBSC_DBPDRGD_1		0xE6790668U
+#define DBSC_DBPDRGA_2		0xE67906A4U
+#define DBSC_DBPDRGD_2		0xE67906A8U
+#define DBSC_DBPDRGA_3		0xE67906E4U
+#define DBSC_DBPDRGD_3		0xE67906E8U
+#define DBSC_DBPDSTAT(ch)	(0xE6790630U + 0x40U * (ch))
+#define DBSC_DBPDSTAT_0		0xE6790630U
+#define DBSC_DBPDSTAT_1		0xE6790670U
+#define DBSC_DBPDSTAT_2		0xE67906B0U
+#define DBSC_DBPDSTAT_3		0xE67906F0U
+#define DBSC_DBBUS0CNF0		0xE6790800U
+#define DBSC_DBBUS0CNF1		0xE6790804U
+#define DBSC_DBCAM0CNF1		0xE6790904U
+#define DBSC_DBCAM0CNF2		0xE6790908U
+#define DBSC_DBCAM0CNF3		0xE679090CU
+#define DBSC_DBBSWAP		0xE67909F0U
+#define DBSC_DBBCAMDIS		0xE67909FCU
+#define DBSC_DBSCHCNT0		0xE6791000U
+#define DBSC_DBSCHCNT1		0xE6791004U
+#define DBSC_DBSCHSZ0		0xE6791010U
+#define DBSC_DBSCHRW0		0xE6791020U
+#define DBSC_DBSCHRW1		0xE6791024U
+#define DBSC_DBSCHQOS_0(x)	(0xE6791030U + 0x10U * (x))
+#define DBSC_DBSCHQOS_1(x)	(0xE6791034U + 0x10U * (x))
+#define DBSC_DBSCHQOS_2(x)	(0xE6791038U + 0x10U * (x))
+#define DBSC_DBSCHQOS_3(x)	(0xE679103CU + 0x10U * (x))
+#define DBSC_DBSCHQOS00		0xE6791030U
+#define DBSC_DBSCHQOS01		0xE6791034U
+#define DBSC_DBSCHQOS02		0xE6791038U
+#define DBSC_DBSCHQOS03		0xE679103CU
+#define DBSC_DBSCHQOS10		0xE6791040U
+#define DBSC_DBSCHQOS11		0xE6791044U
+#define DBSC_DBSCHQOS12		0xE6791048U
+#define DBSC_DBSCHQOS13		0xE679104CU
+#define DBSC_DBSCHQOS20		0xE6791050U
+#define DBSC_DBSCHQOS21		0xE6791054U
+#define DBSC_DBSCHQOS22		0xE6791058U
+#define DBSC_DBSCHQOS23		0xE679105CU
+#define DBSC_DBSCHQOS30		0xE6791060U
+#define DBSC_DBSCHQOS31		0xE6791064U
+#define DBSC_DBSCHQOS32		0xE6791068U
+#define DBSC_DBSCHQOS33		0xE679106CU
+#define DBSC_DBSCHQOS40		0xE6791070U
+#define DBSC_DBSCHQOS41		0xE6791074U
+#define DBSC_DBSCHQOS42		0xE6791078U
+#define DBSC_DBSCHQOS43		0xE679107CU
+#define DBSC_DBSCHQOS50		0xE6791080U
+#define DBSC_DBSCHQOS51		0xE6791084U
+#define DBSC_DBSCHQOS52		0xE6791088U
+#define DBSC_DBSCHQOS53		0xE679108CU
+#define DBSC_DBSCHQOS60		0xE6791090U
+#define DBSC_DBSCHQOS61		0xE6791094U
+#define DBSC_DBSCHQOS62		0xE6791098U
+#define DBSC_DBSCHQOS63		0xE679109CU
+#define DBSC_DBSCHQOS70		0xE67910A0U
+#define DBSC_DBSCHQOS71		0xE67910A4U
+#define DBSC_DBSCHQOS72		0xE67910A8U
+#define DBSC_DBSCHQOS73		0xE67910ACU
+#define DBSC_DBSCHQOS80		0xE67910B0U
+#define DBSC_DBSCHQOS81		0xE67910B4U
+#define DBSC_DBSCHQOS82		0xE67910B8U
+#define DBSC_DBSCHQOS83		0xE67910BCU
+#define DBSC_DBSCHQOS90		0xE67910C0U
+#define DBSC_DBSCHQOS91		0xE67910C4U
+#define DBSC_DBSCHQOS92		0xE67910C8U
+#define DBSC_DBSCHQOS93		0xE67910CCU
+#define DBSC_DBSCHQOS100	0xE67910D0U
+#define DBSC_DBSCHQOS101	0xE67910D4U
+#define DBSC_DBSCHQOS102	0xE67910D8U
+#define DBSC_DBSCHQOS103	0xE67910DCU
+#define DBSC_DBSCHQOS110	0xE67910E0U
+#define DBSC_DBSCHQOS111	0xE67910E4U
+#define DBSC_DBSCHQOS112	0xE67910E8U
+#define DBSC_DBSCHQOS113	0xE67910ECU
+#define DBSC_DBSCHQOS120	0xE67910F0U
+#define DBSC_DBSCHQOS121	0xE67910F4U
+#define DBSC_DBSCHQOS122	0xE67910F8U
+#define DBSC_DBSCHQOS123	0xE67910FCU
+#define DBSC_DBSCHQOS130	0xE6791100U
+#define DBSC_DBSCHQOS131	0xE6791104U
+#define DBSC_DBSCHQOS132	0xE6791108U
+#define DBSC_DBSCHQOS133	0xE679110CU
+#define DBSC_DBSCHQOS140	0xE6791110U
+#define DBSC_DBSCHQOS141	0xE6791114U
+#define DBSC_DBSCHQOS142	0xE6791118U
+#define DBSC_DBSCHQOS143	0xE679111CU
+#define DBSC_DBSCHQOS150	0xE6791120U
+#define DBSC_DBSCHQOS151	0xE6791124U
+#define DBSC_DBSCHQOS152	0xE6791128U
+#define DBSC_DBSCHQOS153	0xE679112CU
+#define DBSC_DBSCTR0		0xE6791700U
+#define DBSC_DBSCTR1		0xE6791708U
+#define DBSC_DBSCHRW2		0xE679170CU
+#define DBSC_SCFCTST01(x)	(0xE6791700U + 0x08U * (x))
+#define DBSC_SCFCTST0		0xE6791700U
+#define DBSC_SCFCTST1		0xE6791708U
+#define DBSC_SCFCTST2		0xE679170CU
+#define DBSC_DBMRRDR(chab)	(0xE6791800U + 0x04U * (chab))
+#define DBSC_DBMRRDR_0		0xE6791800U
+#define DBSC_DBMRRDR_1		0xE6791804U
+#define DBSC_DBMRRDR_2		0xE6791808U
+#define DBSC_DBMRRDR_3		0xE679180CU
+#define DBSC_DBMRRDR_4		0xE6791810U
+#define DBSC_DBMRRDR_5		0xE6791814U
+#define DBSC_DBMRRDR_6		0xE6791818U
+#define DBSC_DBMRRDR_7		0xE679181CU
+#define DBSC_DBMEMSWAPCONF0	0xE6792000U
+
+/* CPG registers */
+#define CPG_BASE		0xE6150000U
+#define CPG_FRQCRB		(CPG_BASE + 0x0004U)
+#define CPG_PLLECR		(CPG_BASE + 0x00D0U)
+#define CPG_MSTPSR5		(CPG_BASE + 0x003CU)
+#define CPG_SRCR4		(CPG_BASE + 0x00BCU)
+#define CPG_PLL3CR		(CPG_BASE + 0x00DCU)
+#define CPG_ZB3CKCR		(CPG_BASE + 0x0380U)
+#define CPG_FRQCRD		(CPG_BASE + 0x00E4U)
+#define CPG_SMSTPCR5		(CPG_BASE + 0x0144U)
+#define CPG_CPGWPR		(CPG_BASE + 0x0900U)
+#define CPG_SRSTCLR4		(CPG_BASE + 0x0950U)
+
+#endif /* BOOT_INIT_DRAM_REGDEF_H_*/
diff --git a/drivers/renesas/common/delay/micro_delay.c b/drivers/renesas/common/delay/micro_delay.c
new file mode 100644
index 0000000..a5e2a69
--- /dev/null
+++ b/drivers/renesas/common/delay/micro_delay.c
@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2018-2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+
+#include "micro_delay.h"
+
+#define RCAR_CONV_MICROSEC		1000000U
+
+void
+#if IMAGE_BL31
+	__attribute__ ((section(".system_ram")))
+#endif
+	rcar_micro_delay(uint64_t micro_sec)
+{
+	uint64_t freq;
+	uint64_t base_count;
+	uint64_t get_count;
+	uint64_t wait_time = 0U;
+
+	freq = read_cntfrq_el0();
+	base_count = read_cntpct_el0();
+	while (micro_sec > wait_time) {
+		get_count = read_cntpct_el0();
+		wait_time = ((get_count - base_count) * RCAR_CONV_MICROSEC) / freq;
+	}
+}
diff --git a/drivers/renesas/common/delay/micro_delay.h b/drivers/renesas/common/delay/micro_delay.h
new file mode 100644
index 0000000..37b71f8
--- /dev/null
+++ b/drivers/renesas/common/delay/micro_delay.h
@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MICRO_DELAY_H
+#define MICRO_DELAY_H
+
+#ifndef __ASSEMBLER__
+#include <stdint.h>
+void rcar_micro_delay(uint64_t micro_sec);
+#endif
+
+#endif /* MICRO_DELAY_H */
diff --git a/drivers/renesas/common/dma/dma_driver.c b/drivers/renesas/common/dma/dma_driver.c
new file mode 100644
index 0000000..44ee985
--- /dev/null
+++ b/drivers/renesas/common/dma/dma_driver.c
@@ -0,0 +1,153 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "cpg_registers.h"
+#include "rcar_def.h"
+#include "rcar_private.h"
+
+/* DMA CHANNEL setting (0/16/32) */
+#if RCAR_LSI == RCAR_V3M
+#define DMA_CH		16
+#else
+#define DMA_CH		0
+#endif
+
+#if (DMA_CH == 0)
+#define SYS_DMAC_BIT	((uint32_t)1U << 19U)
+#define DMA_BASE	(0xE6700000U)
+#elif (DMA_CH == 16)
+#define SYS_DMAC_BIT	((uint32_t)1U << 18U)
+#define DMA_BASE	(0xE7300000U)
+#elif (DMA_CH == 32)
+#define SYS_DMAC_BIT	((uint32_t)1U << 17U)
+#define DMA_BASE	(0xE7320000U)
+#else
+#define SYS_DMAC_BIT	((uint32_t)1U << 19U)
+#define DMA_BASE	(0xE6700000U)
+#endif
+
+/* DMA operation */
+#define DMA_DMAOR	(DMA_BASE + 0x0060U)
+/* DMA secure control */
+#define DMA_DMASEC	(DMA_BASE + 0x0030U)
+/* DMA channel clear */
+#define DMA_DMACHCLR	(DMA_BASE + 0x0080U)
+/* DMA source address */
+#define DMA_DMASAR	(DMA_BASE + 0x8000U)
+/* DMA destination address */
+#define DMA_DMADAR	(DMA_BASE + 0x8004U)
+/* DMA transfer count */
+#define DMA_DMATCR	(DMA_BASE + 0x8008U)
+/* DMA channel control */
+#define DMA_DMACHCR	(DMA_BASE + 0x800CU)
+/* DMA fixed destination address */
+#define DMA_DMAFIXDAR	(DMA_BASE + 0x8014U)
+
+#define DMA_USE_CHANNEL		(0x00000001U)
+#define DMAOR_INITIAL		(0x0301U)
+#define DMACHCLR_CH_ALL		(0x0000FFFFU)
+#define DMAFIXDAR_32BIT_SHIFT	(32U)
+#define DMAFIXDAR_DAR_MASK	(0x000000FFU)
+#define DMADAR_BOUNDARY_ADDR	(0x100000000ULL)
+#define DMATCR_CNT_SHIFT	(6U)
+#define DMATCR_MAX		(0x00FFFFFFU)
+#define DMACHCR_TRN_MODE	(0x00105409U)
+#define DMACHCR_DE_BIT		(0x00000001U)
+#define DMACHCR_TE_BIT		(0x00000002U)
+#define DMACHCR_CHE_BIT		(0x80000000U)
+
+#define DMA_SIZE_UNIT		FLASH_TRANS_SIZE_UNIT
+#define DMA_FRACTION_MASK	(0xFFU)
+#define DMA_DST_LIMIT		(0x10000000000ULL)
+
+/* transfer length limit */
+#define DMA_LENGTH_LIMIT	((DMATCR_MAX * (1U << DMATCR_CNT_SHIFT)) \
+				& ~DMA_FRACTION_MASK)
+
+static void dma_enable(void)
+{
+	mstpcr_write(CPG_SMSTPCR2, CPG_MSTPSR2, SYS_DMAC_BIT);
+}
+
+static void dma_setup(void)
+{
+	mmio_write_16(DMA_DMAOR, 0);
+	mmio_write_32(DMA_DMACHCLR, DMACHCLR_CH_ALL);
+}
+
+static void dma_start(uintptr_t dst, uint32_t src, uint32_t len)
+{
+	mmio_write_16(DMA_DMAOR, DMAOR_INITIAL);
+	mmio_write_32(DMA_DMAFIXDAR, (dst >> DMAFIXDAR_32BIT_SHIFT) &
+		      DMAFIXDAR_DAR_MASK);
+	mmio_write_32(DMA_DMADAR, dst & UINT32_MAX);
+	mmio_write_32(DMA_DMASAR, src);
+	mmio_write_32(DMA_DMATCR, len >> DMATCR_CNT_SHIFT);
+	mmio_write_32(DMA_DMASEC, DMA_USE_CHANNEL);
+	mmio_write_32(DMA_DMACHCR, DMACHCR_TRN_MODE);
+}
+
+static void dma_end(void)
+{
+	while ((mmio_read_32(DMA_DMACHCR) & DMACHCR_TE_BIT) == 0) {
+		if ((mmio_read_32(DMA_DMACHCR) & DMACHCR_CHE_BIT) != 0U) {
+			ERROR("BL2: DMA - Channel Address Error\n");
+			panic();
+			break;
+		}
+	}
+	/* DMA transfer Disable */
+	mmio_clrbits_32(DMA_DMACHCR, DMACHCR_DE_BIT);
+	while ((mmio_read_32(DMA_DMACHCR) & DMACHCR_DE_BIT) != 0)
+		;
+
+	mmio_write_32(DMA_DMASEC, 0);
+	mmio_write_16(DMA_DMAOR, 0);
+	mmio_write_32(DMA_DMACHCLR, DMA_USE_CHANNEL);
+}
+
+void rcar_dma_exec(uintptr_t dst, uint32_t src, uint32_t len)
+{
+	uint32_t dma_len = len;
+
+	if (len & DMA_FRACTION_MASK)
+		dma_len = (len + DMA_SIZE_UNIT) & ~DMA_FRACTION_MASK;
+
+	if (!dma_len || dma_len > DMA_LENGTH_LIMIT) {
+		ERROR("BL2: DMA - size invalid, length (0x%x)\n", dma_len);
+		panic();
+	}
+
+	if (src & DMA_FRACTION_MASK) {
+		ERROR("BL2: DMA - src address invalid (0x%x), len=(0x%x)\n",
+		      src, dma_len);
+		panic();
+	}
+
+	if ((dst & UINT32_MAX) + dma_len > DMADAR_BOUNDARY_ADDR	||
+	    (dst + dma_len > DMA_DST_LIMIT) ||
+	    (dst & DMA_FRACTION_MASK)) {
+		ERROR("BL2: DMA - dest address invalid (0x%lx), len=(0x%x)\n",
+		      dst, dma_len);
+		panic();
+	}
+
+	dma_start(dst, src, dma_len);
+	dma_end();
+}
+
+void rcar_dma_init(void)
+{
+	dma_enable();
+	dma_setup();
+}
diff --git a/drivers/renesas/common/emmc/emmc_cmd.c b/drivers/renesas/common/emmc/emmc_cmd.c
new file mode 100644
index 0000000..d255bff
--- /dev/null
+++ b/drivers/renesas/common/emmc/emmc_cmd.c
@@ -0,0 +1,493 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+
+#include "emmc_config.h"
+#include "emmc_def.h"
+#include "emmc_hal.h"
+#include "emmc_registers.h"
+#include "emmc_std.h"
+#include "micro_delay.h"
+
+static void emmc_little_to_big(uint8_t *p, uint32_t value)
+{
+	if (p == NULL)
+		return;
+
+	p[0] = (uint8_t) (value >> 24);
+	p[1] = (uint8_t) (value >> 16);
+	p[2] = (uint8_t) (value >> 8);
+	p[3] = (uint8_t) value;
+
+}
+
+static void emmc_softreset(void)
+{
+	int32_t loop = 10000;
+	int32_t retry = 1000;
+
+	/* flag clear */
+	mmc_drv_obj.during_cmd_processing = FALSE;
+	mmc_drv_obj.during_transfer = FALSE;
+	mmc_drv_obj.during_dma_transfer = FALSE;
+	mmc_drv_obj.state_machine_blocking = FALSE;
+	mmc_drv_obj.force_terminate = FALSE;
+	mmc_drv_obj.dma_error_flag = FALSE;
+
+	/* during operation ? */
+	if ((GETR_32(SD_INFO2) & SD_INFO2_CBSY) == 0)
+		goto reset;
+
+	/* wait CMDSEQ = 0 */
+	while (loop > 0) {
+		if ((GETR_32(SD_INFO2) & SD_INFO2_CBSY) == 0)
+			break;	/* ready */
+
+		loop--;
+		if ((loop == 0) && (retry > 0)) {
+			rcar_micro_delay(1000U);	/* wait 1ms */
+			loop = 10000;
+			retry--;
+		}
+	}
+
+reset:
+	/* reset */
+	SETR_32(SOFT_RST, (GETR_32(SOFT_RST) & (~SOFT_RST_SDRST)));
+	SETR_32(SOFT_RST, (GETR_32(SOFT_RST) | SOFT_RST_SDRST));
+
+	/* initialize */
+	SETR_32(SD_INFO1, 0x00000000U);
+	SETR_32(SD_INFO2, SD_INFO2_CLEAR);
+	SETR_32(SD_INFO1_MASK, 0x00000000U);	/* all interrupt disable */
+	SETR_32(SD_INFO2_MASK, SD_INFO2_CLEAR);	/* all interrupt disable */
+}
+
+static void emmc_read_response(uint32_t *response)
+{
+	uint8_t *p;
+
+	if (response == NULL)
+		return;
+
+	/* read response */
+	if (mmc_drv_obj.response_length != EMMC_MAX_RESPONSE_LENGTH) {
+		*response = GETR_32(SD_RSP10);	/* [39:8] */
+		return;
+	}
+
+	/* CSD or CID */
+	p = (uint8_t *) (response);
+	emmc_little_to_big(p, ((GETR_32(SD_RSP76) << 8)
+			| (GETR_32(SD_RSP54) >> 24)));	/* [127:96]     */
+	emmc_little_to_big(p + 4, ((GETR_32(SD_RSP54) << 8)
+			| (GETR_32(SD_RSP32) >> 24)));	/* [95:64]      */
+	emmc_little_to_big(p + 8, ((GETR_32(SD_RSP32) << 8)
+			| (GETR_32(SD_RSP10) >> 24)));	/* [63:32]      */
+	emmc_little_to_big(p + 12, (GETR_32(SD_RSP10) << 8));
+}
+
+static EMMC_ERROR_CODE emmc_response_check(uint32_t *response,
+					   uint32_t error_mask)
+{
+
+	HAL_MEMCARD_RESPONSE_TYPE response_type =
+	    ((HAL_MEMCARD_RESPONSE_TYPE)mmc_drv_obj.cmd_info.cmd & HAL_MEMCARD_RESPONSE_TYPE_MASK);
+
+	if (response == NULL)
+		return EMMC_ERR_PARAM;
+
+	if (response_type == HAL_MEMCARD_RESPONSE_NONE)
+		return EMMC_SUCCESS;
+
+
+	if (response_type <= HAL_MEMCARD_RESPONSE_R1b) {
+		/* R1 or R1b */
+		mmc_drv_obj.current_state =
+		    (EMMC_R1_STATE) ((*response & EMMC_R1_STATE_MASK) >>
+				     EMMC_R1_STATE_SHIFT);
+		if ((*response & error_mask) != 0) {
+			if ((0x80 & *response) != 0) {
+				ERROR("BL2: emmc SWITCH_ERROR\n");
+			}
+			return EMMC_ERR_CARD_STATUS_BIT;
+		}
+		return EMMC_SUCCESS;
+	}
+
+	if (response_type == HAL_MEMCARD_RESPONSE_R4) {
+		if ((*response & EMMC_R4_STATUS) != 0)
+			return EMMC_ERR_CARD_STATUS_BIT;
+	}
+
+	return EMMC_SUCCESS;
+}
+
+static void emmc_WaitCmd2Cmd_8Cycle(void)
+{
+	uint32_t dataL, wait = 0;
+
+	dataL = GETR_32(SD_CLK_CTRL);
+	dataL &= 0x000000FF;
+
+	switch (dataL) {
+	case 0xFF:
+	case 0x00:
+	case 0x01:
+	case 0x02:
+	case 0x04:
+	case 0x08:
+	case 0x10:
+	case 0x20:
+		wait = 10U;
+		break;
+	case 0x40:
+		wait = 20U;
+		break;
+	case 0x80:
+		wait = 30U;
+		break;
+	}
+
+	rcar_micro_delay(wait);
+}
+
+static void cmdErrSdInfo2Log(void)
+{
+	ERROR("BL2: emmc ERR SD_INFO2 = 0x%x\n", mmc_drv_obj.error_info.info2);
+}
+
+static void emmc_data_transfer_dma(void)
+{
+	mmc_drv_obj.during_dma_transfer = TRUE;
+	mmc_drv_obj.dma_error_flag = FALSE;
+
+	SETR_32(SD_INFO1_MASK, 0x00000000U);
+	SETR_32(SD_INFO2_MASK, (SD_INFO2_ALL_ERR | SD_INFO2_CLEAR));
+
+	/* DMAC setting */
+	if (mmc_drv_obj.cmd_info.dir == HAL_MEMCARD_WRITE) {
+		/* transfer complete interrupt enable */
+		SETR_32(DM_CM_INFO1_MASK,
+			(DM_CM_INFO_MASK_CLEAR | DM_CM_INFO_CH0_ENABLE));
+		SETR_32(DM_CM_INFO2_MASK,
+			(DM_CM_INFO_MASK_CLEAR | DM_CM_INFO_CH0_ENABLE));
+		/* BUFF --> FIFO */
+		SETR_32(DM_CM_DTRAN_MODE, (DM_CM_DTRAN_MODE_CH0 |
+					   DM_CM_DTRAN_MODE_BIT_WIDTH));
+	} else {
+		/* transfer complete interrupt enable */
+		SETR_32(DM_CM_INFO1_MASK,
+			(DM_CM_INFO_MASK_CLEAR | DM_CM_INFO_CH1_ENABLE));
+		SETR_32(DM_CM_INFO2_MASK,
+			(DM_CM_INFO_MASK_CLEAR | DM_CM_INFO_CH1_ENABLE));
+		/* FIFO --> BUFF */
+		SETR_32(DM_CM_DTRAN_MODE, (DM_CM_DTRAN_MODE_CH1
+					   | DM_CM_DTRAN_MODE_BIT_WIDTH));
+	}
+	SETR_32(DM_DTRAN_ADDR, (((uintptr_t) mmc_drv_obj.buff_address_virtual &
+				 DM_DTRAN_ADDR_WRITE_MASK)));
+
+	SETR_32(DM_CM_DTRAN_CTRL, DM_CM_DTRAN_CTRL_START);
+}
+
+EMMC_ERROR_CODE emmc_exec_cmd(uint32_t error_mask, uint32_t *response)
+{
+	EMMC_ERROR_CODE rtn_code = EMMC_SUCCESS;
+	HAL_MEMCARD_RESPONSE_TYPE response_type;
+	HAL_MEMCARD_COMMAND_TYPE cmd_type;
+	EMMC_INT_STATE state;
+	uint32_t err_not_care_flag = FALSE;
+
+	/* parameter check */
+	if (response == NULL) {
+		emmc_write_error_info(EMMC_FUNCNO_EXEC_CMD, EMMC_ERR_PARAM);
+		return EMMC_ERR_PARAM;
+	}
+
+	/* state check */
+	if (mmc_drv_obj.clock_enable != TRUE) {
+		emmc_write_error_info(EMMC_FUNCNO_EXEC_CMD, EMMC_ERR_STATE);
+		return EMMC_ERR_STATE;
+	}
+
+	if (mmc_drv_obj.state_machine_blocking == TRUE) {
+		emmc_write_error_info(EMMC_FUNCNO_EXEC_CMD, EMMC_ERR);
+		return EMMC_ERR;
+	}
+
+	state = ESTATE_BEGIN;
+	response_type =
+	    ((HAL_MEMCARD_RESPONSE_TYPE)mmc_drv_obj.cmd_info.cmd &
+					HAL_MEMCARD_RESPONSE_TYPE_MASK);
+	cmd_type =
+	    ((HAL_MEMCARD_COMMAND_TYPE) mmc_drv_obj.cmd_info.cmd &
+					HAL_MEMCARD_COMMAND_TYPE_MASK);
+
+	/* state machine */
+	while ((mmc_drv_obj.force_terminate != TRUE) && (state != ESTATE_END)) {
+		/* The interrupt factor flag is observed. */
+		emmc_interrupt();
+
+		/* wait interrupt */
+		if (mmc_drv_obj.state_machine_blocking == TRUE)
+			continue;
+
+		switch (state) {
+		case ESTATE_BEGIN:
+			/* Busy check */
+			if ((mmc_drv_obj.error_info.info2 & SD_INFO2_CBSY) != 0) {
+				emmc_write_error_info(EMMC_FUNCNO_EXEC_CMD,
+						      EMMC_ERR_CARD_BUSY);
+				return EMMC_ERR_CARD_BUSY;
+			}
+
+			/* clear register */
+			SETR_32(SD_INFO1, 0x00000000U);
+			SETR_32(SD_INFO2, SD_INFO2_CLEAR);
+			SETR_32(SD_INFO1_MASK, SD_INFO1_INFO0);
+			SETR_32(SD_INFO2_MASK,
+				(SD_INFO2_ALL_ERR | SD_INFO2_CLEAR));
+
+			state = ESTATE_ISSUE_CMD;
+			/* through */
+
+		case ESTATE_ISSUE_CMD:
+			/* ARG */
+			SETR_32(SD_ARG, mmc_drv_obj.cmd_info.arg);
+			/* issue cmd */
+			SETR_32(SD_CMD, mmc_drv_obj.cmd_info.hw);
+			/* Set driver flag */
+			mmc_drv_obj.during_cmd_processing = TRUE;
+			mmc_drv_obj.state_machine_blocking = TRUE;
+
+			if (response_type == HAL_MEMCARD_RESPONSE_NONE) {
+				state = ESTATE_NON_RESP_CMD;
+			} else {
+				state = ESTATE_RCV_RESP;
+			}
+
+			break;
+
+		case ESTATE_NON_RESP_CMD:
+			/* interrupt disable */
+			SETR_32(SD_INFO1_MASK, 0x00000000U);
+			SETR_32(SD_INFO2_MASK, SD_INFO2_CLEAR);
+
+			/* check interrupt */
+			if ((mmc_drv_obj.int_event2 & SD_INFO2_ALL_ERR) != 0) {
+				/* error interrupt */
+				cmdErrSdInfo2Log();
+				rtn_code = EMMC_ERR_INFO2;
+				state = ESTATE_ERROR;
+			} else if ((mmc_drv_obj.int_event1 & SD_INFO1_INFO0) ==
+				   0) {
+				/* not receive expected interrupt */
+				rtn_code = EMMC_ERR_RESPONSE;
+				state = ESTATE_ERROR;
+			} else {
+				emmc_WaitCmd2Cmd_8Cycle();
+				state = ESTATE_END;
+			}
+			break;
+
+		case ESTATE_RCV_RESP:
+			/* interrupt disable */
+			SETR_32(SD_INFO1_MASK, 0x00000000U);
+			SETR_32(SD_INFO2_MASK, SD_INFO2_CLEAR);
+
+			/* check interrupt */
+			if ((mmc_drv_obj.int_event2 & SD_INFO2_ALL_ERR) != 0) {
+				if ((mmc_drv_obj.get_partition_access_flag ==
+				     TRUE)
+				    && ((mmc_drv_obj.int_event2 & SD_INFO2_ERR6)
+					!= 0U)) {
+					err_not_care_flag = TRUE;
+					rtn_code = EMMC_ERR_CMD_TIMEOUT;
+				} else {
+					/* error interrupt */
+					cmdErrSdInfo2Log();
+					rtn_code = EMMC_ERR_INFO2;
+				}
+				state = ESTATE_ERROR;
+				break;
+			} else if ((mmc_drv_obj.int_event1 & SD_INFO1_INFO0) ==
+				   0) {
+				/* not receive expected interrupt */
+				rtn_code = EMMC_ERR_RESPONSE;
+				state = ESTATE_ERROR;
+				break;
+			}
+
+			/* read response */
+			emmc_read_response(response);
+
+			/* check response */
+			rtn_code = emmc_response_check(response, error_mask);
+			if (rtn_code != EMMC_SUCCESS) {
+				state = ESTATE_ERROR;
+				break;
+			}
+
+			if (response_type == HAL_MEMCARD_RESPONSE_R1b) {
+				/* R1b */
+				SETR_32(SD_INFO2_MASK,
+					(SD_INFO2_ALL_ERR | SD_INFO2_CLEAR));
+				state = ESTATE_RCV_RESPONSE_BUSY;
+			} else {
+				state = ESTATE_CHECK_RESPONSE_COMPLETE;
+			}
+			break;
+
+		case ESTATE_RCV_RESPONSE_BUSY:
+			/* check interrupt */
+			if ((mmc_drv_obj.int_event2 & SD_INFO2_ALL_ERR) != 0) {
+				/* error interrupt */
+				cmdErrSdInfo2Log();
+				rtn_code = EMMC_ERR_INFO2;
+				state = ESTATE_ERROR;
+				break;
+			}
+			/* DAT0 not Busy */
+			if ((SD_INFO2_DAT0 & mmc_drv_obj.error_info.info2) != 0) {
+				state = ESTATE_CHECK_RESPONSE_COMPLETE;
+				break;
+			}
+			break;
+
+		case ESTATE_CHECK_RESPONSE_COMPLETE:
+			if (cmd_type >= HAL_MEMCARD_COMMAND_TYPE_ADTC_WRITE) {
+				state = ESTATE_DATA_TRANSFER;
+			} else {
+				emmc_WaitCmd2Cmd_8Cycle();
+				state = ESTATE_END;
+			}
+			break;
+
+		case ESTATE_DATA_TRANSFER:
+			/* ADTC command  */
+			mmc_drv_obj.during_transfer = TRUE;
+			mmc_drv_obj.state_machine_blocking = TRUE;
+
+			if (mmc_drv_obj.transfer_mode == HAL_MEMCARD_DMA) {
+				/* DMA */
+				emmc_data_transfer_dma();
+			} else {
+				/* PIO */
+				/* interrupt enable (FIFO read/write enable) */
+				if (mmc_drv_obj.cmd_info.dir ==
+				    HAL_MEMCARD_WRITE) {
+					SETR_32(SD_INFO2_MASK,
+						(SD_INFO2_BWE | SD_INFO2_ALL_ERR
+						 | SD_INFO2_CLEAR));
+				} else {
+					SETR_32(SD_INFO2_MASK,
+						(SD_INFO2_BRE | SD_INFO2_ALL_ERR
+						 | SD_INFO2_CLEAR));
+				}
+			}
+			state = ESTATE_DATA_TRANSFER_COMPLETE;
+			break;
+
+		case ESTATE_DATA_TRANSFER_COMPLETE:
+			/* check interrupt */
+			if ((mmc_drv_obj.int_event2 & SD_INFO2_ALL_ERR) != 0) {
+				/* error interrupt */
+				cmdErrSdInfo2Log();
+				rtn_code = EMMC_ERR_INFO2;
+				state = ESTATE_TRANSFER_ERROR;
+				break;
+			}
+
+			/* DMAC error ? */
+			if (mmc_drv_obj.dma_error_flag == TRUE) {
+				/* Error occurred in DMAC driver. */
+				rtn_code = EMMC_ERR_FROM_DMAC_TRANSFER;
+				state = ESTATE_TRANSFER_ERROR;
+			} else if (mmc_drv_obj.during_dma_transfer == TRUE) {
+				/* DMAC not finished. unknown error */
+				rtn_code = EMMC_ERR;
+				state = ESTATE_TRANSFER_ERROR;
+			} else {
+				SETR_32(SD_INFO1_MASK, SD_INFO1_INFO2);
+				SETR_32(SD_INFO2_MASK,
+					(SD_INFO2_ALL_ERR | SD_INFO2_CLEAR));
+
+				mmc_drv_obj.state_machine_blocking = TRUE;
+
+				state = ESTATE_ACCESS_END;
+			}
+			break;
+
+		case ESTATE_ACCESS_END:
+
+			/* clear flag */
+			if (mmc_drv_obj.transfer_mode == HAL_MEMCARD_DMA) {
+				/* W (CC_EXT_MODE, H'0000_1010) SD_BUF DMA transfer disabled */
+				SETR_32(CC_EXT_MODE, CC_EXT_MODE_CLEAR);
+				SETR_32(SD_STOP, 0x00000000U);
+				mmc_drv_obj.during_dma_transfer = FALSE;
+			}
+
+			SETR_32(SD_INFO1_MASK, 0x00000000U);
+			SETR_32(SD_INFO2_MASK, SD_INFO2_CLEAR);
+			SETR_32(SD_INFO1, 0x00000000U);
+			SETR_32(SD_INFO2, SD_INFO2_CLEAR);
+
+			if ((mmc_drv_obj.int_event1 & SD_INFO1_INFO2) != 0) {
+				emmc_WaitCmd2Cmd_8Cycle();
+				state = ESTATE_END;
+			} else {
+				state = ESTATE_ERROR;
+			}
+			break;
+
+		case ESTATE_TRANSFER_ERROR:
+			/* The error occurred in the Data transfer.  */
+			if (mmc_drv_obj.transfer_mode == HAL_MEMCARD_DMA) {
+				/* W (CC_EXT_MODE, H'0000_1010) SD_BUF DMA transfer disabled */
+				SETR_32(CC_EXT_MODE, CC_EXT_MODE_CLEAR);
+				SETR_32(SD_STOP, 0x00000000U);
+				mmc_drv_obj.during_dma_transfer = FALSE;
+			}
+			/* through */
+
+		case ESTATE_ERROR:
+			if (err_not_care_flag == TRUE) {
+				mmc_drv_obj.during_cmd_processing = FALSE;
+			} else {
+				emmc_softreset();
+				emmc_write_error_info(EMMC_FUNCNO_EXEC_CMD,
+						      rtn_code);
+			}
+			return rtn_code;
+
+		default:
+			state = ESTATE_END;
+			break;
+		} /* switch (state) */
+	} /* while ( (mmc_drv_obj.force_terminate != TRUE) && (state != ESTATE_END) ) */
+
+	/* force terminate */
+	if (mmc_drv_obj.force_terminate == TRUE) {
+		/* timeout timer is expired. Or, PIO data transfer error. */
+		/* Timeout occurred in the DMA transfer. */
+		if (mmc_drv_obj.during_dma_transfer == TRUE) {
+			mmc_drv_obj.during_dma_transfer = FALSE;
+		}
+		ERROR("BL2: emmc exec_cmd:EMMC_ERR_FORCE_TERMINATE\n");
+		emmc_softreset();
+
+		return EMMC_ERR_FORCE_TERMINATE; /* error information has already been written. */
+	}
+
+	/* success */
+	mmc_drv_obj.during_cmd_processing = FALSE;
+	mmc_drv_obj.during_transfer = FALSE;
+
+	return EMMC_SUCCESS;
+}
diff --git a/drivers/renesas/common/emmc/emmc_config.h b/drivers/renesas/common/emmc/emmc_config.h
new file mode 100644
index 0000000..16b6b8a
--- /dev/null
+++ b/drivers/renesas/common/emmc/emmc_config.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef EMMC_CONFIG_H
+#define EMMC_CONFIG_H
+
+/* RCA */
+#define EMMC_RCA		1UL
+/* 314ms (freq = 400KHz, timeout Counter = 0x04(SDCLK * 2^17)  */
+#define EMMC_RW_DATA_TIMEOUT	0x40UL
+/* how many times to try after fail. Don't change. */
+#define EMMC_RETRY_COUNT	0
+#define EMMC_CMD_MAX		60UL	/* Don't change. */
+
+#define LOADIMAGE_FLAGS_DMA_ENABLE	0x00000001UL
+
+#endif /* EMMC_CONFIG_H */
diff --git a/drivers/renesas/common/emmc/emmc_def.h b/drivers/renesas/common/emmc/emmc_def.h
new file mode 100644
index 0000000..178c795
--- /dev/null
+++ b/drivers/renesas/common/emmc/emmc_def.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/**
+ * @file  emmc_def.h
+ * @brief eMMC boot is expecting this header file
+ *
+ */
+
+#ifndef EMMC_DEF_H
+#define EMMC_DEF_H
+
+#include "emmc_std.h"
+
+/* ************************ HEADER (INCLUDE) SECTION *********************** */
+
+/* ***************** MACROS, CONSTANTS, COMPILATION FLAGS ****************** */
+#define EMMC_POWER_ON		(1U)
+
+/* ********************** STRUCTURES, TYPE DEFINITIONS ********************* */
+
+/* ********************** DECLARATION OF EXTERNAL DATA ********************* */
+extern st_mmc_base mmc_drv_obj;
+
+/* ************************** FUNCTION PROTOTYPES ************************** */
+
+/** @brief for assembler program
+ */
+uint32_t _rom_emmc_finalize(void);
+
+/** @brief eMMC driver API
+ */
+EMMC_ERROR_CODE rcar_emmc_init(void);
+EMMC_ERROR_CODE emmc_terminate(void);
+EMMC_ERROR_CODE rcar_emmc_memcard_power(uint8_t mode);
+EMMC_ERROR_CODE rcar_emmc_mount(void);
+EMMC_ERROR_CODE emmc_set_request_mmc_clock(uint32_t *freq);
+EMMC_ERROR_CODE emmc_send_idle_cmd(uint32_t arg);
+EMMC_ERROR_CODE emmc_select_partition(EMMC_PARTITION_ID id);
+EMMC_ERROR_CODE emmc_read_sector(uint32_t *buff_address_virtual,
+				 uint32_t sector_number, uint32_t count,
+				 uint32_t feature_flags);
+EMMC_ERROR_CODE emmc_write_sector(uint32_t *buff_address_virtual,
+				  uint32_t sector_number, uint32_t count,
+				  uint32_t feature_flags);
+EMMC_ERROR_CODE emmc_erase_sector(uint32_t *start_address,
+				  uint32_t *end_address);
+uint32_t emmc_bit_field(uint8_t *data, uint32_t top, uint32_t bottom);
+
+/** @brief interrupt service
+ */
+uint32_t emmc_interrupt(void);
+
+/** @brief DMA
+ */
+
+/** @brief send command API
+ */
+EMMC_ERROR_CODE emmc_exec_cmd(uint32_t error_mask, uint32_t *response);
+void emmc_make_nontrans_cmd(HAL_MEMCARD_COMMAND cmd, uint32_t arg);
+void emmc_make_trans_cmd(HAL_MEMCARD_COMMAND cmd, uint32_t arg,
+			 uint32_t *buff_address_virtual, uint32_t len,
+			 HAL_MEMCARD_OPERATION dir,
+			 HAL_MEMCARD_DATA_TRANSFER_MODE transfer_mode);
+EMMC_ERROR_CODE emmc_set_ext_csd(uint32_t arg);
+
+/** @brief for error information
+ */
+void emmc_write_error_info(uint16_t func_no, EMMC_ERROR_CODE error_code);
+void emmc_write_error_info_func_no(uint16_t func_no);
+
+/* ********************************* CODE ********************************** */
+
+#endif /* EMMC_DEF_H */
+/* ******************************** END ************************************ */
diff --git a/drivers/renesas/common/emmc/emmc_hal.h b/drivers/renesas/common/emmc/emmc_hal.h
new file mode 100644
index 0000000..0a85517
--- /dev/null
+++ b/drivers/renesas/common/emmc/emmc_hal.h
@@ -0,0 +1,535 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef EMMC_HAL_H
+#define EMMC_HAL_H
+
+/* memory card error/status types */
+#define HAL_MEMCARD_OUT_OF_RANGE		0x80000000L
+#define HAL_MEMCARD_ADDRESS_ERROR		0x40000000L
+#define HAL_MEMCARD_BLOCK_LEN_ERROR		0x20000000L
+#define HAL_MEMCARD_ERASE_SEQ_ERROR		0x10000000L
+#define HAL_MEMCARD_ERASE_PARAM			0x08000000L
+#define HAL_MEMCARD_WP_VIOLATION		0x04000000L
+#define HAL_MEMCARD_CARD_IS_LOCKED		0x02000000L
+#define HAL_MEMCARD_LOCK_UNLOCK_FAILED		0x01000000L
+#define HAL_MEMCARD_COM_CRC_ERROR		0x00800000L
+#define HAL_MEMCARD_ILEGAL_COMMAND		0x00400000L
+#define HAL_MEMCARD_CARD_ECC_FAILED		0x00200000L
+#define HAL_MEMCARD_CC_ERROR			0x00100000L
+#define HAL_MEMCARD_ERROR			0x00080000L
+#define HAL_MEMCARD_UNDERRUN			0x00040000L
+#define HAL_MEMCARD_OVERRUN			0x00020000L
+#define HAL_MEMCARD_CIDCSD_OVERWRITE		0x00010000L
+#define HAL_MEMCARD_WP_ERASE_SKIP		0x00008000L
+#define HAL_MEMCARD_CARD_ECC_DISABLED		0x00004000L
+#define HAL_MEMCARD_ERASE_RESET			0x00002000L
+#define HAL_MEMCARD_CARD_STATE			0x00001E00L
+#define HAL_MEMCARD_CARD_READY_FOR_DATA		0x00000100L
+#define HAL_MEMCARD_APP_CMD			0x00000020L
+#define HAL_MEMCARD_SWITCH_ERROR		0x00000080L
+#define HAL_MEMCARD_AKE_SEQ_ERROR		0x00000008L
+#define HAL_MEMCARD_NO_ERRORS			0x00000000L
+
+/* Memory card response types */
+#define HAL_MEMCARD_COMMAND_INDEX_MASK		0x0003f
+
+/* Type of the return value. */
+typedef enum {
+	HAL_MEMCARD_FAIL = 0U,
+	HAL_MEMCARD_OK = 1U,
+	HAL_MEMCARD_DMA_ALLOC_FAIL = 2U,     /* DMA channel allocation failed */
+	HAL_MEMCARD_DMA_TRANSFER_FAIL = 3U,  /* DMA transfer failed */
+	HAL_MEMCARD_CARD_STATUS_ERROR = 4U,  /* card status non-masked error */
+	HAL_MEMCARD_CMD_TIMEOUT = 5U,	     /* Command timeout occurred */
+	HAL_MEMCARD_DATA_TIMEOUT = 6U,	     /* Data timeout occurred */
+	HAL_MEMCARD_CMD_CRC_ERROR = 7U,	     /* Command CRC error occurred */
+	HAL_MEMCARD_DATA_CRC_ERROR = 8U	     /* Data CRC error occurred */
+} HAL_MEMCARD_RETURN;
+
+/* memory access operation */
+typedef enum {
+	HAL_MEMCARD_READ = 0U,	 /* read */
+	HAL_MEMCARD_WRITE = 1U	 /* write */
+} HAL_MEMCARD_OPERATION;
+
+/* Type of data width on memorycard bus */
+typedef enum {
+	HAL_MEMCARD_DATA_WIDTH_1_BIT = 0U,
+	HAL_MEMCARD_DATA_WIDTH_4_BIT = 1U,
+	HAL_MEMCARD_DATA_WIDTH_8_BIT = 2U
+} HAL_MEMCARD_DATA_WIDTH; /* data (bus) width types */
+
+/* Presence of the memory card */
+typedef enum {
+	HAL_MEMCARD_CARD_IS_IN = 0U,
+	HAL_MEMCARD_CARD_IS_OUT = 1U
+} HAL_MEMCARD_PRESENCE_STATUS;	/* presence status of the memory card */
+
+/* mode of data transfer */
+typedef enum {
+	HAL_MEMCARD_DMA = 0U,
+	HAL_MEMCARD_NOT_DMA = 1U
+} HAL_MEMCARD_DATA_TRANSFER_MODE;
+
+/* Memory card response types. */
+typedef enum hal_memcard_response_type {
+	HAL_MEMCARD_RESPONSE_NONE = 0x00000U,
+	HAL_MEMCARD_RESPONSE_R1 = 0x00100U,
+	HAL_MEMCARD_RESPONSE_R1b = 0x00200U,
+	HAL_MEMCARD_RESPONSE_R2 = 0x00300U,
+	HAL_MEMCARD_RESPONSE_R3 = 0x00400U,
+	HAL_MEMCARD_RESPONSE_R4 = 0x00500U,
+	HAL_MEMCARD_RESPONSE_R5 = 0x00600U,
+	HAL_MEMCARD_RESPONSE_R6 = 0x00700U,
+	HAL_MEMCARD_RESPONSE_R7 = 0x00800U,
+	HAL_MEMCARD_RESPONSE_TYPE_MASK = 0x00f00U
+} HAL_MEMCARD_RESPONSE_TYPE;
+
+/* Memory card command types. */
+typedef enum hal_memcard_command_type {
+	HAL_MEMCARD_COMMAND_TYPE_BC = 0x00000U,
+	HAL_MEMCARD_COMMAND_TYPE_BCR = 0x01000U,
+	HAL_MEMCARD_COMMAND_TYPE_AC = 0x02000U,
+	HAL_MEMCARD_COMMAND_TYPE_ADTC_WRITE = 0x03000U,
+	HAL_MEMCARD_COMMAND_TYPE_ADTC_READ = 0x04000U,
+	HAL_MEMCARD_COMMAND_TYPE_MASK = 0x07000U
+} HAL_MEMCARD_COMMAND_TYPE;
+
+/* Type of memory card */
+typedef enum hal_memcard_command_card_type {
+	HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON = 0x00000U,
+	HAL_MEMCARD_COMMAND_CARD_TYPE_MMC = 0x08000U,
+	HAL_MEMCARD_COMMAND_CARD_TYPE_SD = 0x10000U,
+	HAL_MEMCARD_COMMAND_CARD_TYPE_MASK = 0x18000U
+} HAL_MEMCARD_COMMAND_CARD_TYPE;
+
+/* Memory card application command. */
+typedef enum hal_memcard_command_app_norm {
+	HAL_MEMCARD_COMMAND_NORMAL = 0x00000U,
+	HAL_MEMCARD_COMMAND_APP = 0x20000U,
+	HAL_MEMCARD_COMMAND_APP_NORM_MASK = 0x20000U
+} HAL_MEMCARD_COMMAND_APP_NORM;
+
+/* Memory card command codes. */
+typedef enum {
+/* class 0 and class 1 */
+	/* CMD0 */
+	CMD0_GO_IDLE_STATE =
+	    0U | (uint32_t)HAL_MEMCARD_RESPONSE_NONE |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_BC |
+	    (uint32_t) HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD1 */
+	CMD1_SEND_OP_COND =
+	    1U | (uint32_t)HAL_MEMCARD_RESPONSE_R3 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_BCR |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD2 */
+	CMD2_ALL_SEND_CID_MMC =
+	    2U | (uint32_t)HAL_MEMCARD_RESPONSE_R2 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_BCR |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	CMD2_ALL_SEND_CID_SD =
+	    2U | (uint32_t)HAL_MEMCARD_RESPONSE_R2 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_BCR |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD3 */
+	CMD3_SET_RELATIVE_ADDR =
+	    3U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	CMD3_SEND_RELATIVE_ADDR =
+	    3U | (uint32_t)HAL_MEMCARD_RESPONSE_R6 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD4 */
+	CMD4_SET_DSR =
+	    4U | (uint32_t)HAL_MEMCARD_RESPONSE_NONE |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_BC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD5 */
+	CMD5_SLEEP_AWAKE =
+	    5U | (uint32_t)HAL_MEMCARD_RESPONSE_R1b |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD6 */
+	CMD6_SWITCH =
+	    6U | (uint32_t)HAL_MEMCARD_RESPONSE_R1b |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	CMD6_SWITCH_FUNC =
+	    6U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	ACMD6_SET_BUS_WIDTH =
+	    6U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_APP,
+	/* CMD7 */
+	CMD7_SELECT_CARD =
+	    7U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD7(from Disconnected State to Programming State) */
+	CMD7_SELECT_CARD_PROG =
+	    7U | (uint32_t)HAL_MEMCARD_RESPONSE_R1b |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	CMD7_DESELECT_CARD =
+	    7U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD8 */
+	CMD8_SEND_EXT_CSD =
+	    8U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_READ |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	CMD8_SEND_IF_COND =
+	    8U | (uint32_t)HAL_MEMCARD_RESPONSE_R7 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_BCR |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD9 */
+	CMD9_SEND_CSD =
+	    9U | (uint32_t)HAL_MEMCARD_RESPONSE_R2 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD10 */
+	CMD10_SEND_CID =
+	    10U | (uint32_t)HAL_MEMCARD_RESPONSE_R2 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD11 */
+	CMD11_READ_DAT_UNTIL_STOP =
+	    11U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_READ |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD12 */
+	CMD12_STOP_TRANSMISSION =
+	    12U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD12(R1b : write case) */
+	CMD12_STOP_TRANSMISSION_WRITE =
+	    12U | (uint32_t)HAL_MEMCARD_RESPONSE_R1b |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD13 */
+	CMD13_SEND_STATUS =
+	    13U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	ACMD13_SD_STATUS =
+	    13U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_READ |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_APP,
+	/* CMD14 */
+	CMD14_BUSTEST_R =
+	    14U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_READ |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD15 */
+	CMD15_GO_INACTIVE_STATE =
+	    15U | (uint32_t)HAL_MEMCARD_RESPONSE_NONE |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+
+/* class 2 */
+	/* CMD16 */
+	CMD16_SET_BLOCKLEN =
+	    16U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD17 */
+	CMD17_READ_SINGLE_BLOCK =
+	     17U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	     (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_READ |
+	     (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	     (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD18 */
+	CMD18_READ_MULTIPLE_BLOCK =
+	    18U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_READ |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD19 */
+	CMD19_BUS_TEST_W =
+	    19U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_WRITE |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+
+/* class 3 */
+	/* CMD20 */
+	CMD20_WRITE_DAT_UNTIL_STOP =
+	    20U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_WRITE |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD21 */
+	CMD21 = 21U,
+	/* CMD22 */
+	CMD22 = 22U,
+	ACMD22_SEND_NUM_WR_BLOCKS =
+	    22U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_APP,
+
+/* class 4 */
+	/* CMD23 */
+	CMD23_SET_BLOCK_COUNT =
+	    23U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	ACMD23_SET_WR_BLK_ERASE_COUNT =
+	    23U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_APP,
+	/* CMD24 */
+	CMD24_WRITE_BLOCK =
+	    24U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_WRITE |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD25 */
+	CMD25_WRITE_MULTIPLE_BLOCK =
+	    25U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_WRITE |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD26 */
+	CMD26_PROGRAM_CID =
+	    26U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_WRITE |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD27 */
+	CMD27_PROGRAM_CSD =
+	    27U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_WRITE |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+
+/* class 6 */
+	/* CMD28 */
+	CMD28_SET_WRITE_PROT =
+	    28U | (uint32_t)HAL_MEMCARD_RESPONSE_R1b |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD29 */
+	CMD29_CLR_WRITE_PROT =
+	    29U | (uint32_t)HAL_MEMCARD_RESPONSE_R1b |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD30 */
+	CMD30_SEND_WRITE_PROT =
+	    30U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_READ |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD31 */
+	CMD30_SEND_WRITE_PROT_TYPE =
+	    31U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_READ |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+
+/* class 5 */
+	/* CMD32 */
+	CMD32_ERASE_WR_BLK_START =
+	    32U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD33 */
+	CMD33_ERASE_WR_BLK_END =
+	    33U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD34 */
+	CMD34 = 34U,
+	/* CMD35 */
+	CMD35_ERASE_GROUP_START =
+	    35U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD36 */
+	CMD36_ERASE_GROUP_END =
+	    36U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD37 */
+	CMD37 = 37U,
+	/* CMD38 */
+	CMD38_ERASE =
+	    38U | (uint32_t)HAL_MEMCARD_RESPONSE_R1b |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+
+/* class 9 */
+	/* CMD39 */
+	CMD39_FASTIO =
+	    39U | (uint32_t)HAL_MEMCARD_RESPONSE_R4 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD40 */
+	CMD40_GO_IRQSTATE =
+	    40U | (uint32_t)HAL_MEMCARD_RESPONSE_R5 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_BCR |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_MMC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD41 */
+	CMD41 = 41,
+	ACMD41_SD_SEND_OP_COND =
+	     41U | (uint32_t)HAL_MEMCARD_RESPONSE_R3 |
+	     (uint32_t)HAL_MEMCARD_COMMAND_TYPE_BCR |
+	     (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	     (uint32_t)HAL_MEMCARD_COMMAND_APP,
+
+/* class 7 */
+	/* CMD42 */
+	CMD42_LOCK_UNLOCK =
+	    42U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_WRITE |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	    (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	ACMD42_SET_CLR_CARD_DETECT =
+	    42U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_APP,
+	CMD43 = 43U,		/* CMD43 */
+	CMD44 = 44U,		/* CMD44 */
+	CMD45 = 45U,		/* CMD45 */
+	CMD46 = 46U,		/* CMD46 */
+	CMD47 = 47U,		/* CMD47 */
+	CMD48 = 48U,		/* CMD48 */
+	CMD49 = 49U,		/* CMD49 */
+	CMD50 = 50U,		/* CMD50 */
+	CMD51 = 51U,		/* CMD51 */
+	ACMD51_SEND_SCR =
+	    51U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	    (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_READ |
+	    (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_SD |
+	    (uint32_t)HAL_MEMCARD_COMMAND_APP,
+	CMD52 = 52U,		/* CMD52 */
+	CMD53 = 53U,		/* CMD53 */
+	CMD54 = 54U,		/* CMD54 */
+
+/* class 8 */
+	/* CMD55 */
+	CMD55_APP_CMD =
+	   55U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	   (uint32_t)HAL_MEMCARD_COMMAND_TYPE_AC |
+	   (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	   (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	/* CMD56 */
+	CMD56_GEN_CMD =
+	   56U | (uint32_t)HAL_MEMCARD_RESPONSE_R1 |
+	   (uint32_t)HAL_MEMCARD_COMMAND_TYPE_ADTC_WRITE |
+	   (uint32_t)HAL_MEMCARD_COMMAND_CARD_TYPE_COMMON |
+	   (uint32_t)HAL_MEMCARD_COMMAND_NORMAL,
+	CMD57 = 57U,		/* CMD57 */
+	CMD58 = 58U,		/* CMD58 */
+	CMD59 = 59U,		/* CMD59 */
+	CMD60 = 60U,		/* CMD60 */
+	CMD61 = 61U,		/* CMD61 */
+	CMD62 = 62U,		/* CMD62 */
+	CMD63 = 63U		/* CMD63 */
+} HAL_MEMCARD_COMMAND;
+
+/*
+ * Configuration structure from HAL layer.
+ *
+ * If some field is not available it should be filled with 0xFF.
+ * The API version is 32-bit unsigned integer telling the version of the API.
+ * The integer is divided to four sections which each can be treated as a 8-bit
+ * unsigned number:
+ * Bits 31-24 make the most significant part of the version number. This number
+ * starts from 1 i.e. the second version of the API will be 0x02xxxxxx. This
+ * number changes only, if the API itself changes so much that it is not
+ * compatible anymore with older releases.
+ * Bits 23-16 API minor version number. For example API version 2.1 would be
+ * 0x0201xxxx.
+ * Bits 15-8 are the number of the year when release is done. The 0 is year
+ * 2000, 1 is year 2001 and so on
+ * Bits 7- are the week number when release is done. First full week of the
+ * year is 1
+ *
+ * Example: let's assume that release 2.1 is done on week 10 year 2008
+ * the version will get the value 0x0201080A
+ */
+typedef struct {
+	/*
+	 * Version of the chipset API implementation
+	 *
+	 * bits [31:24] API specification major version number.<br>
+	 * bits [23:16] API specification minor version number.<br>
+	 * bits [15:8] API implementation year. (2000 = 0, 2001 = 1, ...)
+	 * bits [7:0] API implementation week.
+	 * Example: API spec version 4.0, implementation w46 2008 => 0x0400082E
+	 */
+	uint32_t api_version;
+
+	/* maximum block count which can be transferred at once */
+	uint32_t max_block_count;
+
+	/* maximum clock frequence in Hz supported by HW */
+	uint32_t max_clock_freq;
+
+	/* maximum data bus width supported by HW */
+	uint16_t max_data_width;
+
+	/* Is high-speed mode supported by HW (yes=1, no=0) */
+	uint8_t hs_mode_supported;
+
+	/* Is memory card removable (yes=1, no=0) */
+	uint8_t card_removable;
+
+} HAL_MEMCARD_HW_CONF;
+
+/* Configuration structure to HAL layer. */
+typedef struct {
+	/* how many times to try after fail, for instance sending command */
+	uint32_t retries_after_fail;
+} HAL_MEMCARD_INIT_CONF;
+
+#endif /* EMMC_HAL_H */
diff --git a/drivers/renesas/common/emmc/emmc_init.c b/drivers/renesas/common/emmc/emmc_init.c
new file mode 100644
index 0000000..c0ec600
--- /dev/null
+++ b/drivers/renesas/common/emmc/emmc_init.c
@@ -0,0 +1,163 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+
+#include <lib/mmio.h>
+
+#include "emmc_config.h"
+#include "emmc_hal.h"
+#include "emmc_std.h"
+#include "emmc_registers.h"
+#include "emmc_def.h"
+#include "rcar_private.h"
+#include "cpg_registers.h"
+
+st_mmc_base mmc_drv_obj;
+
+EMMC_ERROR_CODE rcar_emmc_memcard_power(uint8_t mode)
+{
+
+	if (mode == TRUE) {
+		/* power on (Vcc&Vccq is always power on) */
+		mmc_drv_obj.card_power_enable = TRUE;
+	} else {
+		/* power off (Vcc&Vccq is always power on) */
+		mmc_drv_obj.card_power_enable = FALSE;
+		mmc_drv_obj.mount = FALSE;
+		mmc_drv_obj.selected = FALSE;
+	}
+
+	return EMMC_SUCCESS;
+}
+static inline void emmc_set_retry_count(uint32_t retry)
+{
+	mmc_drv_obj.retries_after_fail = retry;
+}
+
+static inline void emmc_set_data_timeout(uint32_t data_timeout)
+{
+	mmc_drv_obj.data_timeout = data_timeout;
+}
+
+static void emmc_memset(uint8_t *buff, uint8_t data, uint32_t cnt)
+{
+	if (buff == NULL) {
+		return;
+	}
+
+	while (cnt > 0) {
+		*buff++ = data;
+		cnt--;
+	}
+}
+
+static void emmc_driver_config(void)
+{
+	emmc_set_retry_count(EMMC_RETRY_COUNT);
+	emmc_set_data_timeout(EMMC_RW_DATA_TIMEOUT);
+}
+
+static void emmc_drv_init(void)
+{
+	emmc_memset((uint8_t *) (&mmc_drv_obj), 0, sizeof(st_mmc_base));
+	mmc_drv_obj.card_present = HAL_MEMCARD_CARD_IS_IN;
+	mmc_drv_obj.data_timeout = EMMC_RW_DATA_TIMEOUT;
+	mmc_drv_obj.bus_width = HAL_MEMCARD_DATA_WIDTH_1_BIT;
+}
+
+static EMMC_ERROR_CODE emmc_dev_finalize(void)
+{
+	EMMC_ERROR_CODE result;
+	uint32_t dataL;
+
+	/*
+	 * MMC power off
+	 * the power supply of eMMC device is always turning on.
+	 * RST_n : Hi --> Low level.
+	 */
+	result = rcar_emmc_memcard_power(FALSE);
+
+	/* host controller reset */
+	SETR_32(SD_INFO1, 0x00000000U);		/* all interrupt clear */
+	SETR_32(SD_INFO2, SD_INFO2_CLEAR);	/* all interrupt clear */
+	SETR_32(SD_INFO1_MASK, 0x00000000U);	/* all interrupt disable */
+	SETR_32(SD_INFO2_MASK, SD_INFO2_CLEAR);	/* all interrupt disable */
+	SETR_32(SD_CLK_CTRL, 0x00000000U);	/* MMC clock stop */
+
+	dataL = mmio_read_32(SMSTPCR3);
+	if ((dataL & CPG_MSTP_MMC) == 0U) {
+		dataL |= (CPG_MSTP_MMC);
+		mmio_write_32(CPG_CPGWPR, (~dataL));
+		mmio_write_32(SMSTPCR3, dataL);
+	}
+
+	return result;
+}
+
+static EMMC_ERROR_CODE emmc_dev_init(void)
+{
+	/* Enable clock supply to eMMC. */
+	mstpcr_write(SMSTPCR3, CPG_MSTPSR3, CPG_MSTP_MMC);
+
+	/* Set SD clock */
+	mmio_write_32(CPG_CPGWPR, ~((uint32_t) (BIT9 | BIT0)));	/* SD phy 200MHz */
+
+	/* Stop SDnH clock & SDn=200MHz */
+	mmio_write_32(CPG_SDxCKCR, (BIT9 | BIT0));
+
+	/* MMCIF initialize */
+	SETR_32(SD_INFO1, 0x00000000U);		/* all interrupt clear */
+	SETR_32(SD_INFO2, SD_INFO2_CLEAR);	/* all interrupt clear */
+	SETR_32(SD_INFO1_MASK, 0x00000000U);	/* all interrupt disable */
+	SETR_32(SD_INFO2_MASK, SD_INFO2_CLEAR);	/* all interrupt disable */
+
+	SETR_32(HOST_MODE, 0x00000000U);	/* SD_BUF access width = 64-bit */
+	SETR_32(SD_OPTION, 0x0000C0EEU);	/* Bus width = 1bit, timeout=MAX */
+	SETR_32(SD_CLK_CTRL, 0x00000000U);	/* Disable Automatic Control & Clock Output */
+
+	return EMMC_SUCCESS;
+}
+
+static EMMC_ERROR_CODE emmc_reset_controller(void)
+{
+	EMMC_ERROR_CODE result;
+
+	/* initialize mmc driver */
+	emmc_drv_init();
+
+	/* initialize H/W */
+	result = emmc_dev_init();
+	if (result == EMMC_SUCCESS) {
+		mmc_drv_obj.initialize = TRUE;
+	}
+
+	return result;
+
+}
+
+EMMC_ERROR_CODE emmc_terminate(void)
+{
+	EMMC_ERROR_CODE result;
+
+	result = emmc_dev_finalize();
+
+	emmc_memset((uint8_t *) (&mmc_drv_obj), 0, sizeof(st_mmc_base));
+
+	return result;
+}
+
+EMMC_ERROR_CODE rcar_emmc_init(void)
+{
+	EMMC_ERROR_CODE result;
+
+	result = emmc_reset_controller();
+	if (result == EMMC_SUCCESS) {
+		emmc_driver_config();
+	}
+
+	return result;
+}
diff --git a/drivers/renesas/common/emmc/emmc_interrupt.c b/drivers/renesas/common/emmc/emmc_interrupt.c
new file mode 100644
index 0000000..092fdfb
--- /dev/null
+++ b/drivers/renesas/common/emmc/emmc_interrupt.c
@@ -0,0 +1,217 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights
+ * reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+
+#include <lib/mmio.h>
+
+#include "emmc_config.h"
+#include "emmc_def.h"
+#include "emmc_hal.h"
+#include "emmc_registers.h"
+#include "emmc_std.h"
+#include "rcar_def.h"
+
+static EMMC_ERROR_CODE emmc_trans_sector(uint32_t *buff_address_virtual);
+
+uint32_t emmc_interrupt(void)
+{
+	EMMC_ERROR_CODE result;
+	uint32_t prr_data;
+	uint32_t cut_ver;
+	uint32_t end_bit;
+
+	prr_data = mmio_read_32((uintptr_t) RCAR_PRR);
+	cut_ver = prr_data & PRR_CUT_MASK;
+	if ((prr_data & PRR_PRODUCT_MASK) == PRR_PRODUCT_H3) {
+		if (cut_ver == PRR_PRODUCT_10) {
+			end_bit = BIT17;
+		} else if (cut_ver == PRR_PRODUCT_11) {
+			end_bit = BIT17;
+		} else {
+			end_bit = BIT20;
+		}
+	} else if ((prr_data & PRR_PRODUCT_MASK) == PRR_PRODUCT_M3) {
+		if (cut_ver == PRR_PRODUCT_10) {
+			end_bit = BIT17;
+		} else {
+			end_bit = BIT20;
+		}
+	} else {
+		end_bit = BIT20;
+	}
+
+	/* SD_INFO */
+	mmc_drv_obj.error_info.info1 = GETR_32(SD_INFO1);
+	mmc_drv_obj.error_info.info2 = GETR_32(SD_INFO2);
+
+	/* SD_INFO EVENT */
+	mmc_drv_obj.int_event1 =
+	    mmc_drv_obj.error_info.info1 & GETR_32(SD_INFO1_MASK);
+	mmc_drv_obj.int_event2 =
+	    mmc_drv_obj.error_info.info2 & GETR_32(SD_INFO2_MASK);
+
+	/* ERR_STS */
+	mmc_drv_obj.error_info.status1 = GETR_32(SD_ERR_STS1);
+	mmc_drv_obj.error_info.status2 = GETR_32(SD_ERR_STS2);
+
+	/* DM_CM_INFO */
+	mmc_drv_obj.error_info.dm_info1 = GETR_32(DM_CM_INFO1);
+	mmc_drv_obj.error_info.dm_info2 = GETR_32(DM_CM_INFO2);
+
+	/* DM_CM_INFO EVENT */
+	mmc_drv_obj.dm_event1 =
+	    mmc_drv_obj.error_info.dm_info1 & GETR_32(DM_CM_INFO1_MASK);
+	mmc_drv_obj.dm_event2 =
+	    mmc_drv_obj.error_info.dm_info2 & GETR_32(DM_CM_INFO2_MASK);
+
+	/* ERR SD_INFO2 */
+	if ((SD_INFO2_ALL_ERR & mmc_drv_obj.int_event2) != 0) {
+		SETR_32(SD_INFO1_MASK, 0x00000000U);	/* interrupt disable */
+		SETR_32(SD_INFO2_MASK, SD_INFO2_CLEAR);	/* interrupt disable */
+		SETR_32(SD_INFO1, 0x00000000U);	/* interrupt clear */
+		SETR_32(SD_INFO2, SD_INFO2_CLEAR);	/* interrupt clear */
+		mmc_drv_obj.state_machine_blocking = FALSE;
+	}
+
+	/* PIO Transfer */
+	/* BWE/BRE */
+	else if (((SD_INFO2_BWE | SD_INFO2_BRE) & mmc_drv_obj.int_event2)) {
+		/* BWE */
+		if (SD_INFO2_BWE & mmc_drv_obj.int_event2) {
+			SETR_32(SD_INFO2, (GETR_32(SD_INFO2) & ~SD_INFO2_BWE));
+		}
+		/* BRE */
+		else {
+			SETR_32(SD_INFO2, (GETR_32(SD_INFO2) & ~SD_INFO2_BRE));
+		}
+
+		result = emmc_trans_sector(mmc_drv_obj.buff_address_virtual);
+		mmc_drv_obj.buff_address_virtual += EMMC_BLOCK_LENGTH;
+		mmc_drv_obj.remain_size -= EMMC_BLOCK_LENGTH;
+
+		if (result != EMMC_SUCCESS) {
+			/* data transfer error */
+			emmc_write_error_info(EMMC_FUNCNO_NONE, result);
+
+			/* Panic */
+			SETR_32(SD_INFO1_MASK, 0x00000000U);
+			SETR_32(SD_INFO2_MASK, SD_INFO2_CLEAR);
+			SETR_32(SD_INFO1, 0x00000000U);
+			/* interrupt clear */
+			SETR_32(SD_INFO2, SD_INFO2_CLEAR);
+			mmc_drv_obj.force_terminate = TRUE;
+		} else {
+			mmc_drv_obj.during_transfer = FALSE;
+		}
+		mmc_drv_obj.state_machine_blocking = FALSE;
+	}
+
+	/* DMA_TRANSFER */
+	/* DM_CM_INFO1: DMA-ch0 transfer complete or error occurred */
+	else if ((BIT16 & mmc_drv_obj.dm_event1) != 0) {
+		SETR_32(DM_CM_INFO1, 0x00000000U);
+		SETR_32(DM_CM_INFO2, 0x00000000U);
+		/* interrupt clear */
+		SETR_32(SD_INFO2, (GETR_32(SD_INFO2) & ~SD_INFO2_BWE));
+		/* DM_CM_INFO2:  DMA-ch0 error occurred */
+		if ((BIT16 & mmc_drv_obj.dm_event2) != 0) {
+			mmc_drv_obj.dma_error_flag = TRUE;
+		} else {
+			mmc_drv_obj.during_dma_transfer = FALSE;
+			mmc_drv_obj.during_transfer = FALSE;
+		}
+		/* wait next interrupt */
+		mmc_drv_obj.state_machine_blocking = FALSE;
+	}
+	/* DM_CM_INFO1: DMA-ch1 transfer complete or error occurred */
+	else if ((end_bit & mmc_drv_obj.dm_event1) != 0U) {
+		SETR_32(DM_CM_INFO1, 0x00000000U);
+		SETR_32(DM_CM_INFO2, 0x00000000U);
+		/* interrupt clear */
+		SETR_32(SD_INFO2, (GETR_32(SD_INFO2) & ~SD_INFO2_BRE));
+		/* DM_CM_INFO2: DMA-ch1 error occurred */
+		if ((BIT17 & mmc_drv_obj.dm_event2) != 0) {
+			mmc_drv_obj.dma_error_flag = TRUE;
+		} else {
+			mmc_drv_obj.during_dma_transfer = FALSE;
+			mmc_drv_obj.during_transfer = FALSE;
+		}
+		/* wait next interrupt */
+		mmc_drv_obj.state_machine_blocking = FALSE;
+	}
+
+	/* Response end  */
+	else if ((SD_INFO1_INFO0 & mmc_drv_obj.int_event1) != 0) {
+		/* interrupt clear */
+		SETR_32(SD_INFO1, (GETR_32(SD_INFO1) & ~SD_INFO1_INFO0));
+		mmc_drv_obj.state_machine_blocking = FALSE;
+	}
+	/* Access end  */
+	else if ((SD_INFO1_INFO2 & mmc_drv_obj.int_event1) != 0) {
+		/* interrupt clear */
+		SETR_32(SD_INFO1, (GETR_32(SD_INFO1) & ~SD_INFO1_INFO2));
+		mmc_drv_obj.state_machine_blocking = FALSE;
+	} else {
+		/* nothing to do. */
+	}
+
+	return (uint32_t) 0;
+}
+
+static EMMC_ERROR_CODE emmc_trans_sector(uint32_t *buff_address_virtual)
+{
+	uint32_t length, i;
+	uint64_t *bufPtrLL;
+
+	if (buff_address_virtual == NULL) {
+		return EMMC_ERR_PARAM;
+	}
+
+	if ((mmc_drv_obj.during_transfer != TRUE)
+	    || (mmc_drv_obj.remain_size == 0)) {
+		return EMMC_ERR_STATE;
+	}
+
+	bufPtrLL = (uint64_t *) buff_address_virtual;
+	length = mmc_drv_obj.remain_size;
+
+	/* data transefer */
+	for (i = 0; i < (length >> 3); i++) {
+		/* Write */
+		if (mmc_drv_obj.cmd_info.dir == HAL_MEMCARD_WRITE) {
+			SETR_64(SD_BUF0, *bufPtrLL);	/* buffer --> FIFO */
+		}
+		/* Read */
+		else {
+			/* Checks when the read data reaches SD_SIZE. */
+			/* The BRE bit is cleared at emmc_interrupt function. */
+			if (((i %
+			      (uint32_t) (EMMC_BLOCK_LENGTH >>
+					  EMMC_BUF_SIZE_SHIFT)) == 0U)
+			    && (i != 0U)) {
+				/* BRE check */
+				while (((GETR_32(SD_INFO2)) & SD_INFO2_BRE) ==
+				       0U) {
+					/* ERROR check */
+					if (((GETR_32(SD_INFO2)) &
+					     SD_INFO2_ALL_ERR) != 0U) {
+						return EMMC_ERR_TRANSFER;
+					}
+				}
+				/* BRE clear */
+				SETR_32(SD_INFO2,
+					(uint32_t) (GETR_32(SD_INFO2) &
+						    ~SD_INFO2_BRE));
+			}
+			*bufPtrLL = GETR_64(SD_BUF0);	/* FIFO --> buffer */
+		}
+		bufPtrLL++;
+	}
+
+	return EMMC_SUCCESS;
+}
diff --git a/drivers/renesas/common/emmc/emmc_mount.c b/drivers/renesas/common/emmc/emmc_mount.c
new file mode 100644
index 0000000..e04afd4
--- /dev/null
+++ b/drivers/renesas/common/emmc/emmc_mount.c
@@ -0,0 +1,686 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "emmc_config.h"
+#include "emmc_def.h"
+#include "emmc_hal.h"
+#include "emmc_registers.h"
+#include "emmc_std.h"
+#include "micro_delay.h"
+#include "rcar_def.h"
+
+static EMMC_ERROR_CODE emmc_clock_ctrl(uint8_t mode);
+static EMMC_ERROR_CODE emmc_card_init(void);
+static EMMC_ERROR_CODE emmc_high_speed(void);
+static EMMC_ERROR_CODE emmc_bus_width(uint32_t width);
+static uint32_t emmc_set_timeout_register_value(uint32_t freq);
+static void set_sd_clk(uint32_t clkDiv);
+static uint32_t emmc_calc_tran_speed(uint32_t *freq);
+static void emmc_get_partition_access(void);
+static void emmc_set_bootpartition(void);
+
+static void emmc_set_bootpartition(void)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(RCAR_PRR) & (PRR_PRODUCT_MASK | PRR_CUT_MASK);
+	if (reg == PRR_PRODUCT_M3_CUT10) {
+		mmc_drv_obj.boot_partition_en =
+		    (EMMC_PARTITION_ID) ((mmc_drv_obj.ext_csd_data[179] &
+					  EMMC_BOOT_PARTITION_EN_MASK) >>
+					 EMMC_BOOT_PARTITION_EN_SHIFT);
+	} else if ((reg == PRR_PRODUCT_H3_CUT20)
+		   || (reg == PRR_PRODUCT_M3_CUT11)) {
+		mmc_drv_obj.boot_partition_en = mmc_drv_obj.partition_access;
+	} else {
+		if ((mmio_read_32(MFISBTSTSR) & MFISBTSTSR_BOOT_PARTITION) !=
+		    0U) {
+			mmc_drv_obj.boot_partition_en = PARTITION_ID_BOOT_2;
+		} else {
+			mmc_drv_obj.boot_partition_en = PARTITION_ID_BOOT_1;
+		}
+	}
+}
+
+static EMMC_ERROR_CODE emmc_card_init(void)
+{
+	int32_t retry;
+	uint32_t freq = MMC_400KHZ;	/* 390KHz */
+	EMMC_ERROR_CODE result;
+	uint32_t result_calc;
+
+	/* state check */
+	if ((mmc_drv_obj.initialize != TRUE)
+	    || (mmc_drv_obj.card_power_enable != TRUE)
+	    || ((GETR_32(SD_INFO2) & SD_INFO2_CBSY) != 0)
+	    ) {
+		emmc_write_error_info(EMMC_FUNCNO_CARD_INIT, EMMC_ERR_STATE);
+		return EMMC_ERR_STATE;
+	}
+
+	/* clock on (force change) */
+	mmc_drv_obj.current_freq = 0;
+	mmc_drv_obj.max_freq = MMC_20MHZ;
+	result = emmc_set_request_mmc_clock(&freq);
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_CARD_INIT);
+		return EMMC_ERR;
+	}
+
+	rcar_micro_delay(1000U);	/* wait 1ms */
+
+	/* Get current access partition */
+	emmc_get_partition_access();
+
+	/* CMD0, arg=0x00000000 */
+	result = emmc_send_idle_cmd(0x00000000);
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_CARD_INIT);
+		return result;
+	}
+
+	rcar_micro_delay(200U);	/* wait 74clock 390kHz(189.74us) */
+
+	/* CMD1 */
+	emmc_make_nontrans_cmd(CMD1_SEND_OP_COND, EMMC_HOST_OCR_VALUE);
+	for (retry = 300; retry > 0; retry--) {
+		result =
+		    emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+		if (result != EMMC_SUCCESS) {
+			emmc_write_error_info_func_no(EMMC_FUNCNO_CARD_INIT);
+			return result;
+		}
+
+		if ((mmc_drv_obj.r3_ocr & EMMC_OCR_STATUS_BIT) != 0) {
+			break;	/* card is ready. exit loop */
+		}
+		rcar_micro_delay(1000U);	/* wait 1ms */
+	}
+
+	if (retry == 0) {
+		emmc_write_error_info(EMMC_FUNCNO_CARD_INIT, EMMC_ERR_TIMEOUT);
+		return EMMC_ERR_TIMEOUT;
+	}
+
+	switch (mmc_drv_obj.r3_ocr & EMMC_OCR_ACCESS_MODE_MASK) {
+	case EMMC_OCR_ACCESS_MODE_SECT:
+		mmc_drv_obj.access_mode = TRUE;	/* sector mode */
+		break;
+	default:
+		/* unknown value */
+		emmc_write_error_info(EMMC_FUNCNO_CARD_INIT, EMMC_ERR);
+		return EMMC_ERR;
+	}
+
+	/* CMD2 */
+	emmc_make_nontrans_cmd(CMD2_ALL_SEND_CID_MMC, 0x00000000);
+	mmc_drv_obj.response = (uint32_t *) (&mmc_drv_obj.cid_data[0]);	/* use CID special buffer */
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_CARD_INIT);
+		return result;
+	}
+
+	/* CMD3 */
+	emmc_make_nontrans_cmd(CMD3_SET_RELATIVE_ADDR, EMMC_RCA << 16);
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_CARD_INIT);
+		return result;
+	}
+
+	/* CMD9 (CSD) */
+	emmc_make_nontrans_cmd(CMD9_SEND_CSD, EMMC_RCA << 16);
+	mmc_drv_obj.response = (uint32_t *) (&mmc_drv_obj.csd_data[0]);	/* use CSD special buffer */
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_CARD_INIT);
+		return result;
+	}
+
+	/* card version check */
+	if (EMMC_CSD_SPEC_VARS() < 4) {
+		emmc_write_error_info(EMMC_FUNCNO_CARD_INIT,
+				      EMMC_ERR_ILLEGAL_CARD);
+		return EMMC_ERR_ILLEGAL_CARD;
+	}
+
+	/* CMD7 (select card) */
+	emmc_make_nontrans_cmd(CMD7_SELECT_CARD, EMMC_RCA << 16);
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_CARD_INIT);
+		return result;
+	}
+
+	mmc_drv_obj.selected = TRUE;
+
+	/*
+	 * card speed check
+	 * Card spec is calculated from TRAN_SPEED(CSD)
+	 */
+	result_calc = emmc_calc_tran_speed(&freq);
+	if (result_calc == 0) {
+		emmc_write_error_info(EMMC_FUNCNO_CARD_INIT,
+				      EMMC_ERR_ILLEGAL_CARD);
+		return EMMC_ERR_ILLEGAL_CARD;
+	}
+	mmc_drv_obj.max_freq = freq;	/* max frequency (card spec) */
+
+	result = emmc_set_request_mmc_clock(&freq);
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_CARD_INIT);
+		return EMMC_ERR;
+	}
+
+	/* set read/write timeout */
+	mmc_drv_obj.data_timeout = emmc_set_timeout_register_value(freq);
+	SETR_32(SD_OPTION,
+		((GETR_32(SD_OPTION) & ~(SD_OPTION_TIMEOUT_CNT_MASK)) |
+		 mmc_drv_obj.data_timeout));
+
+	/* SET_BLOCKLEN(512byte) */
+	/* CMD16 */
+	emmc_make_nontrans_cmd(CMD16_SET_BLOCKLEN, EMMC_BLOCK_LENGTH);
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_CARD_INIT);
+		return result;
+	}
+
+	/* Transfer Data Length */
+	SETR_32(SD_SIZE, EMMC_BLOCK_LENGTH);
+
+	/* CMD8 (EXT_CSD) */
+	emmc_make_trans_cmd(CMD8_SEND_EXT_CSD, 0x00000000,
+			    (uint32_t *) (&mmc_drv_obj.ext_csd_data[0]),
+			    EMMC_MAX_EXT_CSD_LENGTH, HAL_MEMCARD_READ,
+			    HAL_MEMCARD_NOT_DMA);
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		/*
+		 * CMD12 is not send.
+		 * If BUS initialization is failed, user must be execute Bus initialization again.
+		 * Bus initialization is start CMD0(soft reset command).
+		 */
+		emmc_write_error_info_func_no(EMMC_FUNCNO_CARD_INIT);
+		return result;
+	}
+
+	/* Set boot partition */
+	emmc_set_bootpartition();
+
+	return EMMC_SUCCESS;
+}
+
+static EMMC_ERROR_CODE emmc_high_speed(void)
+{
+	uint32_t freq;	      /* High speed mode clock frequency */
+	EMMC_ERROR_CODE result;
+	uint8_t cardType;
+
+	/* state check */
+	if (mmc_drv_obj.selected != TRUE) {
+		emmc_write_error_info(EMMC_FUNCNO_HIGH_SPEED, EMMC_ERR_STATE);
+		return EMMC_ERR_STATE;
+	}
+
+	/* max frequency */
+	cardType = (uint8_t) mmc_drv_obj.ext_csd_data[EMMC_EXT_CSD_CARD_TYPE];
+	if ((cardType & EMMC_EXT_CSD_CARD_TYPE_52MHZ) != 0)
+		freq = MMC_52MHZ;
+	else if ((cardType & EMMC_EXT_CSD_CARD_TYPE_26MHZ) != 0)
+		freq = MMC_26MHZ;
+	else
+		freq = MMC_20MHZ;
+
+	/* Hi-Speed-mode selection */
+	if ((freq == MMC_52MHZ) || (freq == MMC_26MHZ)) {
+		/* CMD6 */
+		emmc_make_nontrans_cmd(CMD6_SWITCH, EMMC_SWITCH_HS_TIMING);
+		result =
+		    emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+		if (result != EMMC_SUCCESS) {
+			emmc_write_error_info_func_no(EMMC_FUNCNO_HIGH_SPEED);
+			return result;
+		}
+
+		mmc_drv_obj.hs_timing = TIMING_HIGH_SPEED;	/* High-Speed */
+	}
+
+	/* set mmc clock */
+	mmc_drv_obj.max_freq = freq;
+	result = emmc_set_request_mmc_clock(&freq);
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_HIGH_SPEED);
+		return EMMC_ERR;
+	}
+
+	/* set read/write timeout */
+	mmc_drv_obj.data_timeout = emmc_set_timeout_register_value(freq);
+	SETR_32(SD_OPTION,
+		((GETR_32(SD_OPTION) & ~(SD_OPTION_TIMEOUT_CNT_MASK)) |
+		 mmc_drv_obj.data_timeout));
+
+	/* CMD13 */
+	emmc_make_nontrans_cmd(CMD13_SEND_STATUS, EMMC_RCA << 16);
+	result =
+	    emmc_exec_cmd(EMMC_R1_ERROR_MASK_WITHOUT_CRC, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_HIGH_SPEED);
+		return result;
+	}
+
+	return EMMC_SUCCESS;
+}
+
+static EMMC_ERROR_CODE emmc_clock_ctrl(uint8_t mode)
+{
+	uint32_t value;
+
+	/* busy check */
+	if ((GETR_32(SD_INFO2) & SD_INFO2_CBSY) != 0) {
+		emmc_write_error_info(EMMC_FUNCNO_SET_CLOCK,
+				      EMMC_ERR_CARD_BUSY);
+		return EMMC_ERR;
+	}
+
+	if (mode == TRUE) {
+		/* clock ON */
+		value =
+		    ((GETR_32(SD_CLK_CTRL) | MMC_SD_CLK_START) &
+		     SD_CLK_WRITE_MASK);
+		SETR_32(SD_CLK_CTRL, value);	/* on  */
+		mmc_drv_obj.clock_enable = TRUE;
+	} else {
+		/* clock OFF */
+		value =
+		    ((GETR_32(SD_CLK_CTRL) & MMC_SD_CLK_STOP) &
+		     SD_CLK_WRITE_MASK);
+		SETR_32(SD_CLK_CTRL, value);	/* off */
+		mmc_drv_obj.clock_enable = FALSE;
+	}
+
+	return EMMC_SUCCESS;
+}
+
+static EMMC_ERROR_CODE emmc_bus_width(uint32_t width)
+{
+	EMMC_ERROR_CODE result = EMMC_ERR;
+
+	/* parameter check */
+	if ((width != 8) && (width != 4) && (width != 1)) {
+		emmc_write_error_info(EMMC_FUNCNO_BUS_WIDTH, EMMC_ERR_PARAM);
+		return EMMC_ERR_PARAM;
+	}
+
+	/* state check */
+	if (mmc_drv_obj.selected != TRUE) {
+		emmc_write_error_info(EMMC_FUNCNO_BUS_WIDTH, EMMC_ERR_STATE);
+		return EMMC_ERR_STATE;
+	}
+
+	/* 2 = 8bit, 1 = 4bit, 0 =1bit */
+	mmc_drv_obj.bus_width = (HAL_MEMCARD_DATA_WIDTH) (width >> 2);
+
+	/* CMD6 */
+	emmc_make_nontrans_cmd(CMD6_SWITCH,
+			       (EMMC_SWITCH_BUS_WIDTH_1 |
+				(mmc_drv_obj.bus_width << 8)));
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		/* occurred error */
+		mmc_drv_obj.bus_width = HAL_MEMCARD_DATA_WIDTH_1_BIT;
+		goto EXIT;
+	}
+
+	switch (mmc_drv_obj.bus_width) {
+	case HAL_MEMCARD_DATA_WIDTH_1_BIT:
+		SETR_32(SD_OPTION,
+			((GETR_32(SD_OPTION) & ~(BIT15 | BIT13)) | BIT15));
+		break;
+	case HAL_MEMCARD_DATA_WIDTH_4_BIT:
+		SETR_32(SD_OPTION, (GETR_32(SD_OPTION) & ~(BIT15 | BIT13)));
+		break;
+	case HAL_MEMCARD_DATA_WIDTH_8_BIT:
+		SETR_32(SD_OPTION,
+			((GETR_32(SD_OPTION) & ~(BIT15 | BIT13)) | BIT13));
+		break;
+	default:
+		goto EXIT;
+	}
+
+	/* CMD13 */
+	emmc_make_nontrans_cmd(CMD13_SEND_STATUS, EMMC_RCA << 16);
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		goto EXIT;
+	}
+
+	/* CMD8 (EXT_CSD) */
+	emmc_make_trans_cmd(CMD8_SEND_EXT_CSD, 0x00000000,
+			    (uint32_t *) (&mmc_drv_obj.ext_csd_data[0]),
+			    EMMC_MAX_EXT_CSD_LENGTH, HAL_MEMCARD_READ,
+			    HAL_MEMCARD_NOT_DMA);
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		goto EXIT;
+	}
+
+	return EMMC_SUCCESS;
+
+EXIT:
+	emmc_write_error_info(EMMC_FUNCNO_BUS_WIDTH, result);
+	ERROR("BL2: emmc bus_width error end\n");
+	return result;
+}
+
+EMMC_ERROR_CODE emmc_select_partition(EMMC_PARTITION_ID id)
+{
+	EMMC_ERROR_CODE result;
+	uint32_t arg;
+	uint32_t partition_config;
+
+	/* state check */
+	if (mmc_drv_obj.mount != TRUE) {
+		emmc_write_error_info(EMMC_FUNCNO_NONE, EMMC_ERR_STATE);
+		return EMMC_ERR_STATE;
+	}
+
+	/* id = PARTITION_ACCESS(Bit[2:0]) */
+	if ((id & ~PARTITION_ID_MASK) != 0) {
+		emmc_write_error_info(EMMC_FUNCNO_NONE, EMMC_ERR_PARAM);
+		return EMMC_ERR_PARAM;
+	}
+
+	/* EXT_CSD[179] value */
+	partition_config =
+	    (uint32_t) mmc_drv_obj.ext_csd_data[EMMC_EXT_CSD_PARTITION_CONFIG];
+	if ((partition_config & PARTITION_ID_MASK) == id) {
+		result = EMMC_SUCCESS;
+	} else {
+
+		partition_config =
+		    (uint32_t) ((partition_config & ~PARTITION_ID_MASK) | id);
+		arg = EMMC_SWITCH_PARTITION_CONFIG | (partition_config << 8);
+
+		result = emmc_set_ext_csd(arg);
+	}
+
+	return result;
+}
+
+static void set_sd_clk(uint32_t clkDiv)
+{
+	uint32_t dataL;
+
+	dataL = (GETR_32(SD_CLK_CTRL) & (~SD_CLK_CTRL_CLKDIV_MASK));
+
+	switch (clkDiv) {
+	case 1:
+		dataL |= 0x000000FFU;
+		break;		/* 1/1   */
+	case 2:
+		dataL |= 0x00000000U;
+		break;		/* 1/2   */
+	case 4:
+		dataL |= 0x00000001U;
+		break;		/* 1/4   */
+	case 8:
+		dataL |= 0x00000002U;
+		break;		/* 1/8   */
+	case 16:
+		dataL |= 0x00000004U;
+		break;		/* 1/16  */
+	case 32:
+		dataL |= 0x00000008U;
+		break;		/* 1/32  */
+	case 64:
+		dataL |= 0x00000010U;
+		break;		/* 1/64  */
+	case 128:
+		dataL |= 0x00000020U;
+		break;		/* 1/128 */
+	case 256:
+		dataL |= 0x00000040U;
+		break;		/* 1/256 */
+	case 512:
+		dataL |= 0x00000080U;
+		break;		/* 1/512 */
+	}
+
+	SETR_32(SD_CLK_CTRL, dataL);
+	mmc_drv_obj.current_freq = (uint32_t) clkDiv;
+}
+
+static void emmc_get_partition_access(void)
+{
+	uint32_t reg;
+	EMMC_ERROR_CODE result;
+
+	reg = mmio_read_32(RCAR_PRR) & (PRR_PRODUCT_MASK | PRR_CUT_MASK);
+	if ((reg == PRR_PRODUCT_H3_CUT20) || (reg == PRR_PRODUCT_M3_CUT11)) {
+		SETR_32(SD_OPTION, 0x000060EEU);	/* 8 bits width */
+		/* CMD8 (EXT_CSD) */
+		emmc_make_trans_cmd(CMD8_SEND_EXT_CSD, 0x00000000U,
+				    (uint32_t *) (&mmc_drv_obj.ext_csd_data[0]),
+				    EMMC_MAX_EXT_CSD_LENGTH,
+				    HAL_MEMCARD_READ, HAL_MEMCARD_NOT_DMA);
+		mmc_drv_obj.get_partition_access_flag = TRUE;
+		result =
+		    emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+		mmc_drv_obj.get_partition_access_flag = FALSE;
+		if (result == EMMC_SUCCESS) {
+			mmc_drv_obj.partition_access =
+			    (EMMC_PARTITION_ID) (mmc_drv_obj.ext_csd_data[179]
+						 & PARTITION_ID_MASK);
+		} else if (result == EMMC_ERR_CMD_TIMEOUT) {
+			mmc_drv_obj.partition_access = PARTITION_ID_BOOT_1;
+		} else {
+			emmc_write_error_info(EMMC_FUNCNO_GET_PERTITION_ACCESS,
+					      result);
+			panic();
+		}
+		SETR_32(SD_OPTION, 0x0000C0EEU);	/* Initialize */
+	}
+}
+
+static uint32_t emmc_calc_tran_speed(uint32_t *freq)
+{
+	const uint32_t unit[8] = { 10000U, 100000U, 1000000U, 10000000U,
+				   0U, 0U, 0U, 0U }; /* frequency unit (1/10) */
+	const uint32_t mult[16] = { 0U, 10U, 12U, 13U, 15U, 20U, 26U, 30U, 35U,
+				    40U, 45U, 52U, 55U, 60U, 70U, 80U };
+	uint32_t tran_speed = EMMC_CSD_TRAN_SPEED();
+	uint32_t max_freq;
+	uint32_t result;
+
+	/*
+	 * tran_speed = 0x32
+	 * unit[tran_speed&0x7] = uint[0x2] = 1000000
+	 * mult[(tran_speed&0x78)>>3] = mult[0x30>>3] = mult[6] = 26
+	 * 1000000 * 26 = 26000000 (26MHz)
+	 */
+
+	result = 1;
+	max_freq =
+	    unit[tran_speed & EMMC_TRANSPEED_FREQ_UNIT_MASK] *
+	    mult[(tran_speed & EMMC_TRANSPEED_MULT_MASK) >>
+		 EMMC_TRANSPEED_MULT_SHIFT];
+
+	if (max_freq == 0) {
+		result = 0;
+	} else if (max_freq >= MMC_FREQ_52MHZ) {
+		*freq = MMC_52MHZ;
+	} else if (max_freq >= MMC_FREQ_26MHZ) {
+		*freq = MMC_26MHZ;
+	} else if (max_freq >= MMC_FREQ_20MHZ) {
+		*freq = MMC_20MHZ;
+	} else {
+		*freq = MMC_400KHZ;
+	}
+
+	return result;
+}
+
+static uint32_t emmc_set_timeout_register_value(uint32_t freq)
+{
+	uint32_t timeout_cnt;	/* SD_OPTION   - Timeout Counter  */
+
+	switch (freq) {
+	case 1U:
+		timeout_cnt = 0xE0U;
+		break;		/* SDCLK * 2^27 */
+	case 2U:
+		timeout_cnt = 0xE0U;
+		break;		/* SDCLK * 2^27 */
+	case 4U:
+		timeout_cnt = 0xD0U;
+		break;		/* SDCLK * 2^26 */
+	case 8U:
+		timeout_cnt = 0xC0U;
+		break;		/* SDCLK * 2^25 */
+	case 16U:
+		timeout_cnt = 0xB0U;
+		break;		/* SDCLK * 2^24 */
+	case 32U:
+		timeout_cnt = 0xA0U;
+		break;		/* SDCLK * 2^23 */
+	case 64U:
+		timeout_cnt = 0x90U;
+		break;		/* SDCLK * 2^22 */
+	case 128U:
+		timeout_cnt = 0x80U;
+		break;		/* SDCLK * 2^21 */
+	case 256U:
+		timeout_cnt = 0x70U;
+		break;		/* SDCLK * 2^20 */
+	case 512U:
+		timeout_cnt = 0x70U;
+		break;		/* SDCLK * 2^20 */
+	default:
+		timeout_cnt = 0xE0U;
+		break;		/* SDCLK * 2^27 */
+	}
+
+	return timeout_cnt;
+}
+
+EMMC_ERROR_CODE emmc_set_ext_csd(uint32_t arg)
+{
+	EMMC_ERROR_CODE result;
+
+	/* CMD6 */
+	emmc_make_nontrans_cmd(CMD6_SWITCH, arg);
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		return result;
+	}
+
+	/* CMD13 */
+	emmc_make_nontrans_cmd(CMD13_SEND_STATUS, EMMC_RCA << 16);
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		return result;
+	}
+
+	/* CMD8 (EXT_CSD) */
+	emmc_make_trans_cmd(CMD8_SEND_EXT_CSD, 0x00000000,
+			    (uint32_t *) (&mmc_drv_obj.ext_csd_data[0]),
+			    EMMC_MAX_EXT_CSD_LENGTH, HAL_MEMCARD_READ,
+			    HAL_MEMCARD_NOT_DMA);
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		return result;
+	}
+	return EMMC_SUCCESS;
+}
+
+EMMC_ERROR_CODE emmc_set_request_mmc_clock(uint32_t *freq)
+{
+	/* parameter check */
+	if (freq == NULL) {
+		emmc_write_error_info(EMMC_FUNCNO_SET_CLOCK, EMMC_ERR_PARAM);
+		return EMMC_ERR_PARAM;
+	}
+
+	/* state check */
+	if ((mmc_drv_obj.initialize != TRUE)
+	    || (mmc_drv_obj.card_power_enable != TRUE)) {
+		emmc_write_error_info(EMMC_FUNCNO_SET_CLOCK, EMMC_ERR_STATE);
+		return EMMC_ERR_STATE;
+	}
+
+	/* clock is already running in the desired frequency. */
+	if ((mmc_drv_obj.clock_enable == TRUE)
+	    && (mmc_drv_obj.current_freq == *freq)) {
+		return EMMC_SUCCESS;
+	}
+
+	/* busy check */
+	if ((GETR_32(SD_INFO2) & SD_INFO2_CBSY) != 0) {
+		emmc_write_error_info(EMMC_FUNCNO_SET_CLOCK,
+				      EMMC_ERR_CARD_BUSY);
+		return EMMC_ERR;
+	}
+
+	set_sd_clk(*freq);
+	mmc_drv_obj.clock_enable = FALSE;
+
+	return emmc_clock_ctrl(TRUE);	/* clock on */
+}
+
+EMMC_ERROR_CODE rcar_emmc_mount(void)
+{
+	EMMC_ERROR_CODE result;
+
+	/* state check */
+	if ((mmc_drv_obj.initialize != TRUE)
+	    || (mmc_drv_obj.card_power_enable != TRUE)
+	    || ((GETR_32(SD_INFO2) & SD_INFO2_CBSY) != 0)
+	    ) {
+		emmc_write_error_info(EMMC_FUNCNO_MOUNT, EMMC_ERR_STATE);
+		return EMMC_ERR_STATE;
+	}
+
+	/* initialize card (IDLE state --> Transfer state) */
+	result = emmc_card_init();
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_CARD_INIT);
+		if (emmc_clock_ctrl(FALSE) != EMMC_SUCCESS) {
+			/* nothing to do. */
+		}
+		return result;
+	}
+
+	/* Switching high speed mode */
+	result = emmc_high_speed();
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_HIGH_SPEED);
+		if (emmc_clock_ctrl(FALSE) != EMMC_SUCCESS) {
+			/* nothing to do. */
+		}
+		return result;
+	}
+
+	/* Changing the data bus width */
+	result = emmc_bus_width(8);
+	if (result != EMMC_SUCCESS) {
+		emmc_write_error_info_func_no(EMMC_FUNCNO_BUS_WIDTH);
+		if (emmc_clock_ctrl(FALSE) != EMMC_SUCCESS) {
+			/* nothing to do. */
+		}
+		return result;
+	}
+
+	/* mount complete */
+	mmc_drv_obj.mount = TRUE;
+
+	return EMMC_SUCCESS;
+}
diff --git a/drivers/renesas/common/emmc/emmc_read.c b/drivers/renesas/common/emmc/emmc_read.c
new file mode 100644
index 0000000..96e73ca
--- /dev/null
+++ b/drivers/renesas/common/emmc/emmc_read.c
@@ -0,0 +1,130 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+
+#include "emmc_config.h"
+#include "emmc_def.h"
+#include "emmc_hal.h"
+#include "emmc_registers.h"
+#include "emmc_std.h"
+
+#define MIN_EMMC(a, b)	(((a) < (b)) ? (a) : (b))
+#define EMMC_RW_SECTOR_COUNT_MAX	0x0000ffffU
+
+static EMMC_ERROR_CODE emmc_multiple_block_read(uint32_t *buff_address_virtual,
+		uint32_t sector_number, uint32_t count,
+		HAL_MEMCARD_DATA_TRANSFER_MODE transfer_mode)
+{
+	EMMC_ERROR_CODE result;
+
+	/* parameter check */
+	if ((count > EMMC_RW_SECTOR_COUNT_MAX)
+	    || (count == 0)
+	    || ((transfer_mode != HAL_MEMCARD_DMA)
+		&& (transfer_mode != HAL_MEMCARD_NOT_DMA))
+	    ) {
+		emmc_write_error_info(EMMC_FUNCNO_READ_SECTOR, EMMC_ERR_PARAM);
+		return EMMC_ERR_PARAM;
+	}
+
+	/* CMD23 */
+	emmc_make_nontrans_cmd(CMD23_SET_BLOCK_COUNT, count);
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		return result;
+	}
+	SETR_32(SD_SECCNT, count);
+	SETR_32(SD_STOP, 0x00000100);
+	/* SD_BUF Read/Write DMA Transfer enable */
+	SETR_32(CC_EXT_MODE, (CC_EXT_MODE_CLEAR | CC_EXT_MODE_DMASDRW_ENABLE));
+
+	/* CMD18 */
+	emmc_make_trans_cmd(CMD18_READ_MULTIPLE_BLOCK, sector_number,
+			    buff_address_virtual,
+			    count << EMMC_SECTOR_SIZE_SHIFT, HAL_MEMCARD_READ,
+			    transfer_mode);
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		return result;	/* CMD18 error code */
+	}
+
+	/* CMD13 */
+	emmc_make_nontrans_cmd(CMD13_SEND_STATUS, EMMC_RCA << 16);
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		return result;
+	}
+#if RCAR_BL2_DCACHE == 1
+	if (transfer_mode == HAL_MEMCARD_NOT_DMA) {
+		flush_dcache_range((uint64_t) buff_address_virtual,
+				   ((size_t) count << EMMC_SECTOR_SIZE_SHIFT));
+	}
+#endif /* RCAR_BL2_DCACHE == 1 */
+
+	/* ready status check */
+	if ((mmc_drv_obj.r1_card_status & EMMC_R1_READY) == 0) {
+		emmc_write_error_info(EMMC_FUNCNO_READ_SECTOR,
+				      EMMC_ERR_CARD_BUSY);
+		return EMMC_ERR_CARD_BUSY;
+	}
+
+	/* state check */
+	if (mmc_drv_obj.current_state != EMMC_R1_STATE_TRAN) {
+		emmc_write_error_info(EMMC_FUNCNO_READ_SECTOR,
+				      EMMC_ERR_CARD_STATE);
+		return EMMC_ERR_CARD_STATE;
+	}
+
+	return EMMC_SUCCESS;
+}
+
+EMMC_ERROR_CODE emmc_read_sector(uint32_t *buff_address_virtual,
+				 uint32_t sector_number,
+				 uint32_t count, uint32_t feature_flags)
+{
+	uint32_t trans_count;
+	uint32_t remain;
+	EMMC_ERROR_CODE result;
+	HAL_MEMCARD_DATA_TRANSFER_MODE transfer_mode;
+
+	/* parameter check */
+	if (count == 0) {
+		emmc_write_error_info(EMMC_FUNCNO_READ_SECTOR, EMMC_ERR_PARAM);
+		return EMMC_ERR_PARAM;
+	}
+
+	/* state check */
+	if (mmc_drv_obj.mount != TRUE) {
+		emmc_write_error_info(EMMC_FUNCNO_READ_SECTOR, EMMC_ERR_STATE);
+		return EMMC_ERR_STATE;
+	}
+
+	/* DMA? */
+	if ((feature_flags & LOADIMAGE_FLAGS_DMA_ENABLE) != 0) {
+		transfer_mode = HAL_MEMCARD_DMA;
+	} else {
+		transfer_mode = HAL_MEMCARD_NOT_DMA;
+	}
+
+	remain = count;
+	while (remain != 0) {
+		trans_count = MIN_EMMC(remain, EMMC_RW_SECTOR_COUNT_MAX);
+		result =
+		    emmc_multiple_block_read(buff_address_virtual,
+					     sector_number, trans_count,
+					     transfer_mode);
+		if (result != EMMC_SUCCESS) {
+			return result;
+		}
+
+		buff_address_virtual += (EMMC_BLOCK_LENGTH_DW * trans_count);
+		sector_number += trans_count;
+		remain -= trans_count;
+	}
+
+	return EMMC_SUCCESS;
+}
diff --git a/drivers/renesas/common/emmc/emmc_registers.h b/drivers/renesas/common/emmc/emmc_registers.h
new file mode 100644
index 0000000..67d285d
--- /dev/null
+++ b/drivers/renesas/common/emmc/emmc_registers.h
@@ -0,0 +1,215 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef EMMC_REGISTERS_H
+#define EMMC_REGISTERS_H
+
+/* MMC channel select */
+#define MMC_CH0		(0U)	/* SDHI2/MMC0 */
+#define MMC_CH1		(1U)	/* SDHI3/MMC1 */
+
+#if (RCAR_LSI == RCAR_E3)  || (RCAR_LSI == RZ_G2M) || (RCAR_LSI == RZ_G2H) || (RCAR_LSI == RZ_G2N)
+#define USE_MMC_CH	(MMC_CH1)	/* R-Car E3 or RZ/G2{H,M,N} */
+#else /* RCAR_LSI == RCAR_E3 || RCAR_LSI == RZ_G2{H,M,N} */
+#define USE_MMC_CH	(MMC_CH0)	/* R-Car H3/M3/M3N */
+#endif /* RCAR_LSI == RCAR_E3 || RCAR_LSI == RZ_G2{H,M,N} */
+
+#define BIT0	(0x00000001U)
+#define BIT1	(0x00000002U)
+#define BIT2	(0x00000004U)
+#define BIT3	(0x00000008U)
+#define BIT4	(0x00000010U)
+#define BIT5	(0x00000020U)
+#define BIT6	(0x00000040U)
+#define BIT7	(0x00000080U)
+#define BIT8	(0x00000100U)
+#define BIT9	(0x00000200U)
+#define BIT10	(0x00000400U)
+#define BIT11	(0x00000800U)
+#define BIT12	(0x00001000U)
+#define BIT13	(0x00002000U)
+#define BIT14	(0x00004000U)
+#define BIT15	(0x00008000U)
+#define BIT16	(0x00010000U)
+#define BIT17	(0x00020000U)
+#define BIT18	(0x00040000U)
+#define BIT19	(0x00080000U)
+#define BIT20	(0x00100000U)
+#define BIT21	(0x00200000U)
+#define BIT22	(0x00400000U)
+#define BIT23	(0x00800000U)
+#define BIT24	(0x01000000U)
+#define BIT25	(0x02000000U)
+#define BIT26	(0x04000000U)
+#define BIT27	(0x08000000U)
+#define BIT28	(0x10000000U)
+#define BIT29	(0x20000000U)
+#define BIT30	(0x40000000U)
+#define BIT31	(0x80000000U)
+
+#if USE_MMC_CH == MMC_CH0
+#define CPG_SDxCKCR		(CPG_SD2CKCR)	/* SDHI2/MMC0 */
+#else /* USE_MMC_CH == MMC_CH0 */
+#define CPG_SDxCKCR		(CPG_SD3CKCR)	/* SDHI3/MMC1 */
+#endif /* USE_MMC_CH == MMC_CH0 */
+
+/* Boot Status register */
+#define  MFISBTSTSR			(0xE6260604U)
+
+#define  MFISBTSTSR_BOOT_PARTITION	(0x00000010U)
+
+/* eMMC registers */
+#define MMC0_SD_BASE		(0xEE140000U)
+#define MMC1_SD_BASE		(0xEE160000U)
+
+#if USE_MMC_CH == MMC_CH0
+#define MMC_SD_BASE		(MMC0_SD_BASE)
+#else /* USE_MMC_CH == MMC_CH0 */
+#define MMC_SD_BASE		(MMC1_SD_BASE)
+#endif /* USE_MMC_CH == MMC_CH0 */
+
+#define SD_CMD			(MMC_SD_BASE + 0x0000U)
+#define SD_PORTSEL		(MMC_SD_BASE + 0x0008U)
+#define SD_ARG			(MMC_SD_BASE + 0x0010U)
+#define SD_ARG1			(MMC_SD_BASE + 0x0018U)
+#define SD_STOP			(MMC_SD_BASE + 0x0020U)
+#define SD_SECCNT		(MMC_SD_BASE + 0x0028U)
+#define SD_RSP10		(MMC_SD_BASE + 0x0030U)
+#define SD_RSP1			(MMC_SD_BASE + 0x0038U)
+#define SD_RSP32		(MMC_SD_BASE + 0x0040U)
+#define SD_RSP3			(MMC_SD_BASE + 0x0048U)
+#define SD_RSP54		(MMC_SD_BASE + 0x0050U)
+#define SD_RSP5			(MMC_SD_BASE + 0x0058U)
+#define SD_RSP76		(MMC_SD_BASE + 0x0060U)
+#define SD_RSP7			(MMC_SD_BASE + 0x0068U)
+#define SD_INFO1		(MMC_SD_BASE + 0x0070U)
+#define SD_INFO2		(MMC_SD_BASE + 0x0078U)
+#define SD_INFO1_MASK		(MMC_SD_BASE + 0x0080U)
+#define SD_INFO2_MASK		(MMC_SD_BASE + 0x0088U)
+#define SD_CLK_CTRL		(MMC_SD_BASE + 0x0090U)
+#define SD_SIZE			(MMC_SD_BASE + 0x0098U)
+#define SD_OPTION		(MMC_SD_BASE + 0x00A0U)
+#define SD_ERR_STS1		(MMC_SD_BASE + 0x00B0U)
+#define SD_ERR_STS2		(MMC_SD_BASE + 0x00B8U)
+#define SD_BUF0			(MMC_SD_BASE + 0x00C0U)
+#define SDIO_MODE		(MMC_SD_BASE + 0x00D0U)
+#define SDIO_INFO1		(MMC_SD_BASE + 0x00D8U)
+#define SDIO_INFO1_MASK		(MMC_SD_BASE + 0x00E0U)
+#define CC_EXT_MODE		(MMC_SD_BASE + 0x0360U)
+#define SOFT_RST		(MMC_SD_BASE + 0x0380U)
+#define VERSION			(MMC_SD_BASE + 0x0388U)
+#define HOST_MODE		(MMC_SD_BASE + 0x0390U)
+#define DM_CM_DTRAN_MODE	(MMC_SD_BASE + 0x0820U)
+#define DM_CM_DTRAN_CTRL	(MMC_SD_BASE + 0x0828U)
+#define DM_CM_RST		(MMC_SD_BASE + 0x0830U)
+#define DM_CM_INFO1		(MMC_SD_BASE + 0x0840U)
+#define DM_CM_INFO1_MASK	(MMC_SD_BASE + 0x0848U)
+#define DM_CM_INFO2		(MMC_SD_BASE + 0x0850U)
+#define DM_CM_INFO2_MASK	(MMC_SD_BASE + 0x0858U)
+#define DM_DTRAN_ADDR		(MMC_SD_BASE + 0x0880U)
+
+/* SD_INFO1 Registers */
+#define SD_INFO1_HPIRES		0x00010000UL /* Response Reception Completion */
+#define SD_INFO1_INFO10		0x00000400UL /* Indicates the SDDAT3 state */
+#define SD_INFO1_INFO9		0x00000200UL /* SDDAT3 Card Insertion */
+#define SD_INFO1_INFO8		0x00000100UL /* SDDAT3 Card Removal */
+#define SD_INFO1_INFO7		0x00000080UL /* Write Protect */
+#define SD_INFO1_INFO5		0x00000020UL /* Indicates the ISDCD state */
+#define SD_INFO1_INFO4		0x00000010UL /* ISDCD Card Insertion */
+#define SD_INFO1_INFO3		0x00000008UL /* ISDCD Card Removal */
+#define SD_INFO1_INFO2		0x00000004UL /* Access end */
+#define SD_INFO1_INFO0		0x00000001UL /* Response end */
+
+/* SD_INFO2 Registers */
+#define SD_INFO2_ILA		0x00008000UL /* Illegal Access Error */
+#define SD_INFO2_CBSY		0x00004000UL /* Command Type Register Busy */
+#define SD_INFO2_SCLKDIVEN	0x00002000UL
+#define SD_INFO2_BWE		0x00000200UL /* SD_BUF Write Enable */
+#define SD_INFO2_BRE		0x00000100UL /* SD_BUF Read Enable */
+#define SD_INFO2_DAT0		0x00000080UL /* SDDAT0 */
+#define SD_INFO2_ERR6		0x00000040UL /* Response Timeout */
+#define SD_INFO2_ERR5		0x00000020UL /* SD_BUF Illegal Read Access */
+#define SD_INFO2_ERR4		0x00000010UL /* SD_BUF Illegal Write Access */
+#define SD_INFO2_ERR3		0x00000008UL /* Data Timeout */
+#define SD_INFO2_ERR2		0x00000004UL /* END Error */
+#define SD_INFO2_ERR1		0x00000002UL /* CRC Error */
+#define SD_INFO2_ERR0		0x00000001UL /* CMD Error */
+#define SD_INFO2_ALL_ERR	0x0000807FUL
+#define SD_INFO2_CLEAR		0x00000800UL /* BIT11 write value should always be 1. HWM_0003 */
+
+/* SOFT_RST */
+#define SOFT_RST_SDRST		0x00000001UL
+
+/* SD_CLK_CTRL */
+#define SD_CLK_CTRL_SDCLKOFFEN		0x00000200UL
+#define SD_CLK_CTRL_SCLKEN		0x00000100UL
+#define SD_CLK_CTRL_CLKDIV_MASK		0x000000FFUL
+#define SD_CLOCK_ENABLE			0x00000100UL
+#define SD_CLOCK_DISABLE		0x00000000UL
+#define SD_CLK_WRITE_MASK		0x000003FFUL
+#define SD_CLK_CLKDIV_CLEAR_MASK	0xFFFFFF0FUL
+
+/* SD_OPTION */
+#define SD_OPTION_TIMEOUT_CNT_MASK	0x000000F0UL
+
+/*
+ * MMC Clock Frequency
+ * 200MHz * 1/x = output clock
+ */
+#define MMC_CLK_OFF		0UL   /* Clock output is disabled */
+#define MMC_400KHZ		512UL /* 200MHz * 1/512 = 390 KHz */
+#define MMC_20MHZ		16UL  /* 200MHz * 1/16 = 12.5 MHz Normal speed mode */
+#define MMC_26MHZ		8UL   /* 200MHz * 1/8 = 25 MHz HS mode 26Mhz */
+#define MMC_52MHZ		4UL   /* 200MHz * 1/4 = 50 MHz HS mode 52Mhz */
+#define MMC_100MHZ		2UL   /* 200MHz * 1/2 = 100 MHz */
+#define MMC_200MHZ		1UL   /* 200MHz * 1/1 = 200 MHz */
+
+#define MMC_FREQ_52MHZ		52000000UL
+#define MMC_FREQ_26MHZ		26000000UL
+#define MMC_FREQ_20MHZ		20000000UL
+
+/* MMC Clock DIV */
+#define MMC_SD_CLK_START	0x00000100UL	/* CLOCK On */
+#define MMC_SD_CLK_STOP		(~0x00000100UL)	/* CLOCK stop */
+#define MMC_SD_CLK_DIV1		0x000000FFUL	/* 1/1 */
+#define MMC_SD_CLK_DIV2		0x00000000UL	/* 1/2 */
+#define MMC_SD_CLK_DIV4		0x00000001UL	/* 1/4 */
+#define MMC_SD_CLK_DIV8		0x00000002UL	/* 1/8 */
+#define MMC_SD_CLK_DIV16	0x00000004UL	/* 1/16 */
+#define MMC_SD_CLK_DIV32	0x00000008UL	/* 1/32 */
+#define MMC_SD_CLK_DIV64	0x00000010UL	/* 1/64 */
+#define MMC_SD_CLK_DIV128	0x00000020UL	/* 1/128 */
+#define MMC_SD_CLK_DIV256	0x00000040UL	/* 1/256 */
+#define MMC_SD_CLK_DIV512	0x00000080UL	/* 1/512 */
+
+/* DM_CM_DTRAN_MODE */
+#define DM_CM_DTRAN_MODE_CH0		0x00000000UL	/* CH0(downstream) */
+#define DM_CM_DTRAN_MODE_CH1		0x00010000UL	/* CH1(upstream)   */
+#define DM_CM_DTRAN_MODE_BIT_WIDTH	0x00000030UL
+
+/* CC_EXT_MODE */
+#define CC_EXT_MODE_DMASDRW_ENABLE	0x00000002UL	/* SD_BUF Read/Write DMA Transfer */
+#define CC_EXT_MODE_CLEAR		0x00001010UL	/* BIT 12 & 4 always 1. */
+
+/* DM_CM_INFO_MASK */
+#define DM_CM_INFO_MASK_CLEAR		0xFFFCFFFEUL
+#define DM_CM_INFO_CH0_ENABLE		0x00010001UL
+#define DM_CM_INFO_CH1_ENABLE		0x00020001UL
+
+/* DM_DTRAN_ADDR */
+#define DM_DTRAN_ADDR_WRITE_MASK	0xFFFFFFF8UL
+
+/* DM_CM_DTRAN_CTRL */
+#define DM_CM_DTRAN_CTRL_START		0x00000001UL
+
+/* SYSC Registers */
+#if USE_MMC_CH == MMC_CH0
+#define CPG_MSTP_MMC		(BIT12)	/* SDHI2/MMC0 */
+#else /* USE_MMC_CH == MMC_CH0 */
+#define CPG_MSTP_MMC		(BIT11)	/* SDHI3/MMC1 */
+#endif /* USE_MMC_CH == MMC_CH0 */
+
+#endif /* EMMC_REGISTERS_H */
diff --git a/drivers/renesas/common/emmc/emmc_std.h b/drivers/renesas/common/emmc/emmc_std.h
new file mode 100644
index 0000000..087c6e9
--- /dev/null
+++ b/drivers/renesas/common/emmc/emmc_std.h
@@ -0,0 +1,475 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef EMMC_STD_H
+#define EMMC_STD_H
+
+#include "emmc_hal.h"
+
+#ifndef FALSE
+#define FALSE	0U
+#endif
+#ifndef TRUE
+#define TRUE	1U
+#endif
+
+/* 64bit registers */
+#define SETR_64(r, v)			(*(volatile uint64_t *)(r) = (v))
+#define GETR_64(r)			(*(volatile uint64_t *)(r))
+
+/* 32bit registers */
+#define SETR_32(r, v)			(*(volatile uint32_t *)(r) = (v))
+#define GETR_32(r)			(*(volatile uint32_t *)(r))
+
+/* 16bit registers */
+#define SETR_16(r, v)			(*(volatile uint16_t *)(r) = (v))
+#define GETR_16(r)			(*(volatile uint16_t *)(r))
+
+/* 8bit registers */
+#define SETR_8(r, v)			(*(volatile uint8_t *)(r) = (v))
+#define GETR_8(r)			(*(volatile uint8_t *)(r))
+
+/* CSD register Macros */
+#define EMMC_GET_CID(x, y)	(emmc_bit_field(mmc_drv_obj.cid_data, (x), (y)))
+
+#define EMMC_CID_MID()			(EMMC_GET_CID(127, 120))
+#define EMMC_CID_CBX()			(EMMC_GET_CID(113, 112))
+#define EMMC_CID_OID()			(EMMC_GET_CID(111, 104))
+#define EMMC_CID_PNM1()			(EMMC_GET_CID(103, 88))
+#define EMMC_CID_PNM2()			(EMMC_GET_CID(87, 56))
+#define EMMC_CID_PRV()			(EMMC_GET_CID(55, 48))
+#define EMMC_CID_PSN()			(EMMC_GET_CID(47, 16))
+#define EMMC_CID_MDT()			(EMMC_GET_CID(15, 8))
+#define EMMC_CID_CRC()			(EMMC_GET_CID(7, 1))
+
+/* CSD register Macros */
+#define EMMC_GET_CSD(x, y)	(emmc_bit_field(mmc_drv_obj.csd_data, (x), (y)))
+
+#define EMMC_CSD_CSD_STRUCTURE()	(EMMC_GET_CSD(127, 126))
+#define EMMC_CSD_SPEC_VARS()		(EMMC_GET_CSD(125, 122))
+#define EMMC_CSD_TAAC()			(EMMC_GET_CSD(119, 112))
+#define EMMC_CSD_NSAC()			(EMMC_GET_CSD(111, 104))
+#define EMMC_CSD_TRAN_SPEED()		(EMMC_GET_CSD(103, 96))
+#define EMMC_CSD_CCC()			(EMMC_GET_CSD(95, 84))
+#define EMMC_CSD_READ_BL_LEN()		(EMMC_GET_CSD(83, 80))
+#define EMMC_CSD_READ_BL_PARTIAL()	(EMMC_GET_CSD(79, 79))
+#define EMMC_CSD_WRITE_BLK_MISALIGN()	(EMMC_GET_CSD(78, 78))
+#define EMMC_CSD_READ_BLK_MISALIGN()	(EMMC_GET_CSD(77, 77))
+#define EMMC_CSD_DSR_IMP()		(EMMC_GET_CSD(76, 76))
+#define EMMC_CSD_C_SIZE()		(EMMC_GET_CSD(73, 62))
+#define EMMC_CSD_VDD_R_CURR_MIN()	(EMMC_GET_CSD(61, 59))
+#define EMMC_CSD_VDD_R_CURR_MAX()	(EMMC_GET_CSD(58, 56))
+#define EMMC_CSD_VDD_W_CURR_MIN()	(EMMC_GET_CSD(55, 53))
+#define EMMC_CSD_VDD_W_CURR_MAX()	(EMMC_GET_CSD(52, 50))
+#define EMMC_CSD_C_SIZE_MULT()		(EMMC_GET_CSD(49, 47))
+#define EMMC_CSD_ERASE_GRP_SIZE()	(EMMC_GET_CSD(46, 42))
+#define EMMC_CSD_ERASE_GRP_MULT()	(EMMC_GET_CSD(41, 37))
+#define EMMC_CSD_WP_GRP_SIZE()		(EMMC_GET_CSD(36, 32))
+#define EMMC_CSD_WP_GRP_ENABLE()	(EMMC_GET_CSD(31, 31))
+#define EMMC_CSD_DEFALT_ECC()		(EMMC_GET_CSD(30, 29))
+#define EMMC_CSD_R2W_FACTOR()		(EMMC_GET_CSD(28, 26))
+#define EMMC_CSD_WRITE_BL_LEN()		(EMMC_GET_CSD(25, 22))
+#define EMMC_CSD_WRITE_BL_PARTIAL()	(EMMC_GET_CSD(21, 21))
+#define EMMC_CSD_CONTENT_PROT_APP()	(EMMC_GET_CSD(16, 16))
+#define EMMC_CSD_FILE_FORMAT_GRP()	(EMMC_GET_CSD(15, 15))
+#define EMMC_CSD_COPY()			(EMMC_GET_CSD(14, 14))
+#define EMMC_CSD_PERM_WRITE_PROTECT()	(EMMC_GET_CSD(13, 13))
+#define EMMC_CSD_TMP_WRITE_PROTECT()	(EMMC_GET_CSD(12, 12))
+#define EMMC_CSD_FILE_FORMAT()		(EMMC_GET_CSD(11, 10))
+#define EMMC_CSD_ECC()			(EMMC_GET_CSD(9, 8))
+#define EMMC_CSD_CRC()			(EMMC_GET_CSD(7, 1))
+
+/* sector access */
+#define EMMC_4B_BOUNDARY_CHECK_MASK	0x00000003
+#define EMMC_SECTOR_SIZE_SHIFT		9U	/* 512 = 2^9 */
+#define EMMC_SECTOR_SIZE		512
+#define EMMC_BLOCK_LENGTH		512
+#define EMMC_BLOCK_LENGTH_DW		128
+#define EMMC_BUF_SIZE_SHIFT		3U	/* 8byte = 2^3 */
+
+/* eMMC specification clock */
+#define EMMC_CLOCK_SPEC_400K		400000UL	 /* initialize clock 400KHz */
+#define EMMC_CLOCK_SPEC_20M		20000000UL	 /* normal speed 20MHz */
+#define EMMC_CLOCK_SPEC_26M		26000000UL	 /* high speed 26MHz */
+#define EMMC_CLOCK_SPEC_52M		52000000UL	 /* high speed 52MHz */
+#define EMMC_CLOCK_SPEC_100M		100000000UL	 /* high speed 100MHz */
+
+/* EMMC driver error code. (extended HAL_MEMCARD_RETURN) */
+typedef enum {
+	EMMC_ERR = 0,				/* unknown error */
+	EMMC_SUCCESS,				/* OK */
+	EMMC_ERR_FROM_DMAC,			/* DMAC allocation error */
+	EMMC_ERR_FROM_DMAC_TRANSFER,		/* DMAC transfer error */
+	EMMC_ERR_CARD_STATUS_BIT,		/* card status error */
+	EMMC_ERR_CMD_TIMEOUT,			/* command timeout error */
+	EMMC_ERR_DATA_TIMEOUT,			/* data timeout error */
+	EMMC_ERR_CMD_CRC,			/* command CRC error */
+	EMMC_ERR_DATA_CRC,			/* data CRC error */
+	EMMC_ERR_PARAM,				/* parameter error */
+	EMMC_ERR_RESPONSE,			/* response error */
+	EMMC_ERR_RESPONSE_BUSY,			/* response busy error */
+	EMMC_ERR_TRANSFER,			/* data transfer error */
+	EMMC_ERR_READ_SECTOR,			/* read sector error */
+	EMMC_ERR_WRITE_SECTOR,			/* write sector error */
+	EMMC_ERR_STATE,				/* state error */
+	EMMC_ERR_TIMEOUT,			/* timeout error */
+	EMMC_ERR_ILLEGAL_CARD,			/* illegal card */
+	EMMC_ERR_CARD_BUSY,			/* Busy state */
+	EMMC_ERR_CARD_STATE,			/* card state error */
+	EMMC_ERR_SET_TRACE,			/* trace information error */
+	EMMC_ERR_FROM_TIMER,			/* Timer error */
+	EMMC_ERR_FORCE_TERMINATE,		/* Force terminate */
+	EMMC_ERR_CARD_POWER,			/* card power fail */
+	EMMC_ERR_ERASE_SECTOR,			/* erase sector error */
+	EMMC_ERR_INFO2				/* exec cmd error info2 */
+} EMMC_ERROR_CODE;
+
+/* Function number */
+#define EMMC_FUNCNO_NONE				0U
+#define EMMC_FUNCNO_DRIVER_INIT				1U
+#define EMMC_FUNCNO_CARD_POWER_ON			2U
+#define EMMC_FUNCNO_MOUNT				3U
+#define EMMC_FUNCNO_CARD_INIT				4U
+#define EMMC_FUNCNO_HIGH_SPEED				5U
+#define EMMC_FUNCNO_BUS_WIDTH				6U
+#define EMMC_FUNCNO_MULTI_BOOT_SELECT_PARTITION		7U
+#define EMMC_FUNCNO_MULTI_BOOT_READ_SECTOR		8U
+#define EMMC_FUNCNO_TRANS_DATA_READ_SECTOR		9U
+#define EMMC_FUNCNO_UBOOT_IMAGE_SELECT_PARTITION	10U
+#define EMMC_FUNCNO_UBOOT_IMAGE_READ_SECTOR		11U
+#define EMMC_FUNCNO_SET_CLOCK				12U
+#define EMMC_FUNCNO_EXEC_CMD				13U
+#define EMMC_FUNCNO_READ_SECTOR				14U
+#define EMMC_FUNCNO_WRITE_SECTOR			15U
+#define EMMC_FUNCNO_ERASE_SECTOR			16U
+#define EMMC_FUNCNO_GET_PERTITION_ACCESS		17U
+/*
+ * Response
+ * R1
+ * Type 'E' bit and bit14(must be 0). ignore bit22
+ */
+#define EMMC_R1_ERROR_MASK			0xFDBFE080U
+/* Ignore bit23 (Not check CRC error) */
+#define EMMC_R1_ERROR_MASK_WITHOUT_CRC		(0xFD3FE080U)
+#define EMMC_R1_STATE_MASK			0x00001E00U	/* [12:9] */
+#define EMMC_R1_READY				0x00000100U	/* bit8 */
+#define EMMC_R1_STATE_SHIFT			9
+
+/* R4 */
+#define EMMC_R4_RCA_MASK			0xFFFF0000UL
+#define EMMC_R4_STATUS				0x00008000UL
+
+/* CSD */
+#define EMMC_TRANSPEED_FREQ_UNIT_MASK		0x07	/* bit[2:0] */
+#define EMMC_TRANSPEED_FREQ_UNIT_SHIFT		0
+#define EMMC_TRANSPEED_MULT_MASK		0x78	/* bit[6:3] */
+#define EMMC_TRANSPEED_MULT_SHIFT		3
+
+/* OCR */
+#define EMMC_HOST_OCR_VALUE			0x40FF8080
+#define EMMC_OCR_STATUS_BIT			0x80000000L	/* Card power up status bit */
+#define EMMC_OCR_ACCESS_MODE_MASK		0x60000000L	/* bit[30:29] */
+#define EMMC_OCR_ACCESS_MODE_SECT		0x40000000L
+#define EMMC_OCR_ACCESS_MODE_BYTE		0x00000000L
+
+/* EXT_CSD */
+#define EMMC_EXT_CSD_S_CMD_SET				504
+#define EMMC_EXT_CSD_INI_TIMEOUT_AP			241
+#define EMMC_EXT_CSD_PWR_CL_DDR_52_360			239
+#define EMMC_EXT_CSD_PWR_CL_DDR_52_195			238
+#define EMMC_EXT_CSD_MIN_PERF_DDR_W_8_52		235
+#define EMMC_EXT_CSD_MIN_PERF_DDR_R_8_52		234
+#define EMMC_EXT_CSD_TRIM_MULT				232
+#define EMMC_EXT_CSD_SEC_FEATURE_SUPPORT		231
+#define EMMC_EXT_CSD_SEC_ERASE_MULT			229
+#define EMMC_EXT_CSD_BOOT_INFO				228
+#define EMMC_EXT_CSD_BOOT_SIZE_MULTI			226
+#define EMMC_EXT_CSD_ACC_SIZE				225
+#define EMMC_EXT_CSD_HC_ERASE_GRP_SIZE			224
+#define EMMC_EXT_CSD_ERASE_TIMEOUT_MULT			223
+#define EMMC_EXT_CSD_PEL_WR_SEC_C			222
+#define EMMC_EXT_CSD_HC_WP_GRP_SIZE			221
+#define EMMC_EXT_CSD_S_C_VCC				220
+#define EMMC_EXT_CSD_S_C_VCCQ				219
+#define EMMC_EXT_CSD_S_A_TIMEOUT			217
+#define EMMC_EXT_CSD_SEC_COUNT				215
+#define EMMC_EXT_CSD_MIN_PERF_W_8_52			210
+#define EMMC_EXT_CSD_MIN_PERF_R_8_52			209
+#define EMMC_EXT_CSD_MIN_PERF_W_8_26_4_52		208
+#define EMMC_EXT_CSD_MIN_PERF_R_8_26_4_52		207
+#define EMMC_EXT_CSD_MIN_PERF_W_4_26			206
+#define EMMC_EXT_CSD_MIN_PERF_R_4_26			205
+#define EMMC_EXT_CSD_PWR_CL_26_360			203
+#define EMMC_EXT_CSD_PWR_CL_52_360			202
+#define EMMC_EXT_CSD_PWR_CL_26_195			201
+#define EMMC_EXT_CSD_PWR_CL_52_195			200
+#define EMMC_EXT_CSD_CARD_TYPE				196
+#define EMMC_EXT_CSD_CSD_STRUCTURE			194
+#define EMMC_EXT_CSD_EXT_CSD_REV			192
+#define EMMC_EXT_CSD_CMD_SET				191
+#define EMMC_EXT_CSD_CMD_SET_REV			189
+#define EMMC_EXT_CSD_POWER_CLASS			187
+#define EMMC_EXT_CSD_HS_TIMING				185
+#define EMMC_EXT_CSD_BUS_WIDTH				183
+#define EMMC_EXT_CSD_ERASED_MEM_CONT			181
+#define EMMC_EXT_CSD_PARTITION_CONFIG			179
+#define EMMC_EXT_CSD_BOOT_CONFIG_PROT			178
+#define EMMC_EXT_CSD_BOOT_BUS_WIDTH			177
+#define EMMC_EXT_CSD_ERASE_GROUP_DEF			175
+#define EMMC_EXT_CSD_BOOT_WP				173
+#define EMMC_EXT_CSD_USER_WP				171
+#define EMMC_EXT_CSD_FW_CONFIG				169
+#define EMMC_EXT_CSD_RPMB_SIZE_MULT			168
+#define EMMC_EXT_CSD_RST_n_FUNCTION			162
+#define EMMC_EXT_CSD_PARTITIONING_SUPPORT		160
+#define EMMC_EXT_CSD_MAX_ENH_SIZE_MULT			159
+#define EMMC_EXT_CSD_PARTITIONS_ATTRIBUTE		156
+#define EMMC_EXT_CSD_PARTITION_SETTING_COMPLETED	155
+#define EMMC_EXT_CSD_GP_SIZE_MULT			154
+#define EMMC_EXT_CSD_ENH_SIZE_MULT			142
+#define EMMC_EXT_CSD_ENH_START_ADDR			139
+#define EMMC_EXT_CSD_SEC_BAD_BLK_MGMNT			134
+
+#define EMMC_EXT_CSD_CARD_TYPE_26MHZ			0x01
+#define EMMC_EXT_CSD_CARD_TYPE_52MHZ			0x02
+#define EMMC_EXT_CSD_CARD_TYPE_DDR_52MHZ_12V		0x04
+#define EMMC_EXT_CSD_CARD_TYPE_DDR_52MHZ_18V		0x08
+#define EMMC_EXT_CSD_CARD_TYPE_52MHZ_MASK		0x0e
+
+/* SWITCH (CMD6) argument */
+#define EXTCSD_ACCESS_BYTE	(BIT25 | BIT24)
+#define EXTCSD_SET_BITS		BIT24
+
+#define HS_TIMING_ADD		(185 << 16)	/* H'b9 */
+#define HS_TIMING_1		(1 << 8)
+#define HS_TIMING_HS200		(2 << 8)
+#define HS_TIMING_HS400		(3 << 8)
+
+#define BUS_WIDTH_ADD		(183 << 16)	/* H'b7 */
+#define BUS_WIDTH_1		(0 << 8)
+#define BUS_WIDTH_4		(1 << 8)
+#define BUS_WIDTH_8		(2 << 8)
+#define BUS_WIDTH_4DDR		(5 << 8)
+#define BUS_WIDTH_8DDR		(6 << 8)
+
+#define EMMC_SWITCH_HS_TIMING		(EXTCSD_ACCESS_BYTE | HS_TIMING_ADD |\
+					 HS_TIMING_1)		/* H'03b90100 */
+#define EMMC_SWITCH_HS_TIMING_OFF	(EXTCSD_ACCESS_BYTE |\
+					 HS_TIMING_ADD)		/* H'03b90000 */
+
+#define EMMC_SWITCH_BUS_WIDTH_1		(EXTCSD_ACCESS_BYTE | BUS_WIDTH_ADD |\
+					 BUS_WIDTH_1)		/* H'03b70000 */
+#define EMMC_SWITCH_BUS_WIDTH_4		(EXTCSD_ACCESS_BYTE | BUS_WIDTH_ADD |\
+					 BUS_WIDTH_4)		/* H'03b70100 */
+#define EMMC_SWITCH_BUS_WIDTH_8		(EXTCSD_ACCESS_BYTE | BUS_WIDTH_ADD |\
+					 BUS_WIDTH_8)		/* H'03b70200 */
+#define EMMC_SWITCH_BUS_WIDTH_4DDR	(EXTCSD_ACCESS_BYTE | BUS_WIDTH_ADD |\
+					 BUS_WIDTH_4DDR)	/* H'03b70500 */
+#define EMMC_SWITCH_BUS_WIDTH_8DDR	(EXTCSD_ACCESS_BYTE | BUS_WIDTH_ADD |\
+					 BUS_WIDTH_8DDR)	/* H'03b70600 */
+/* Partition config = 0x00 */
+#define EMMC_SWITCH_PARTITION_CONFIG	0x03B30000UL
+
+#define TIMING_HIGH_SPEED		1UL
+#define EMMC_BOOT_PARTITION_EN_MASK	0x38U
+#define EMMC_BOOT_PARTITION_EN_SHIFT	3U
+
+/* Bus width */
+#define EMMC_BUSWIDTH_1BIT		CE_CMD_SET_DATW_1BIT
+#define EMMC_BUSWIDTH_4BIT		CE_CMD_SET_DATW_4BIT
+#define EMMC_BUSWIDTH_8BIT		CE_CMD_SET_DATW_8BIT
+
+/* for st_mmc_base */
+#define EMMC_MAX_RESPONSE_LENGTH	17
+#define EMMC_MAX_CID_LENGTH		16
+#define EMMC_MAX_CSD_LENGTH		16
+#define EMMC_MAX_EXT_CSD_LENGTH		512U
+#define EMMC_RES_REG_ALIGNED		4U
+#define EMMC_BUF_REG_ALIGNED		8U
+
+/* TAAC mask */
+#define TAAC_TIME_UNIT_MASK		(0x07)
+#define TAAC_MULTIPLIER_FACTOR_MASK	(0x0F)
+
+/* Partition id */
+typedef enum {
+	PARTITION_ID_USER = 0x0,	/* User Area */
+	PARTITION_ID_BOOT_1 = 0x1,	/* boot partition 1 */
+	PARTITION_ID_BOOT_2 = 0x2,	/* boot partition 2 */
+	PARTITION_ID_RPMB = 0x3,	/* Replay Protected Memory Block */
+	PARTITION_ID_GP_1 = 0x4,	/* General Purpose partition 1 */
+	PARTITION_ID_GP_2 = 0x5,	/* General Purpose partition 2 */
+	PARTITION_ID_GP_3 = 0x6,	/* General Purpose partition 3 */
+	PARTITION_ID_GP_4 = 0x7,	/* General Purpose partition 4 */
+	PARTITION_ID_MASK = 0x7		/* [2:0] */
+} EMMC_PARTITION_ID;
+
+/* card state in R1 response [12:9] */
+typedef enum {
+	EMMC_R1_STATE_IDLE = 0,
+	EMMC_R1_STATE_READY,
+	EMMC_R1_STATE_IDENT,
+	EMMC_R1_STATE_STBY,
+	EMMC_R1_STATE_TRAN,
+	EMMC_R1_STATE_DATA,
+	EMMC_R1_STATE_RCV,
+	EMMC_R1_STATE_PRG,
+	EMMC_R1_STATE_DIS,
+	EMMC_R1_STATE_BTST,
+	EMMC_R1_STATE_SLEP
+} EMMC_R1_STATE;
+
+typedef enum {
+	ESTATE_BEGIN = 0,
+	ESTATE_ISSUE_CMD,
+	ESTATE_NON_RESP_CMD,
+	ESTATE_RCV_RESP,
+	ESTATE_RCV_RESPONSE_BUSY,
+	ESTATE_CHECK_RESPONSE_COMPLETE,
+	ESTATE_DATA_TRANSFER,
+	ESTATE_DATA_TRANSFER_COMPLETE,
+	ESTATE_ACCESS_END,
+	ESTATE_TRANSFER_ERROR,
+	ESTATE_ERROR,
+	ESTATE_END
+} EMMC_INT_STATE;
+
+/* eMMC boot driver error information */
+typedef struct {
+	uint16_t num;			/* error no */
+	uint16_t code;			/* error code */
+
+	volatile uint32_t info1;	/* SD_INFO1. (hw dependent) */
+	volatile uint32_t info2;	/* SD_INFO2. (hw dependent) */
+	volatile uint32_t status1;	/* SD_ERR_STS1. (hw dependent) */
+	volatile uint32_t status2;	/* SD_ERR_STS2. (hw dependent) */
+	volatile uint32_t dm_info1;	/* DM_CM_INFO1. (hw dependent) */
+	volatile uint32_t dm_info2;	/* DM_CM_INFO2. (hw dependent) */
+} st_error_info;
+
+/* Command information */
+typedef struct {
+	HAL_MEMCARD_COMMAND cmd;	/* Command information */
+	uint32_t arg;			/* argument */
+	HAL_MEMCARD_OPERATION dir;	/* direction */
+	uint32_t hw;			/* SD_CMD register value. */
+} st_command_info;
+
+/* MMC driver base */
+typedef struct {
+	st_error_info error_info;	/* error information */
+	st_command_info cmd_info;	/* command information */
+
+	/* for data transfer */
+	uint32_t *buff_address_virtual;		/* Dest or Src buff */
+	uint32_t *buff_address_physical;	/* Dest or Src buff */
+	HAL_MEMCARD_DATA_WIDTH bus_width;	/* bus width */
+
+	uint32_t trans_size;		/* transfer size for this command */
+	uint32_t remain_size;		/* remain size for this command */
+	uint32_t response_length;	/* response length for this command */
+	uint32_t sector_size;		/* sector_size */
+
+	/* clock */
+	uint32_t base_clock;		/* MMC host controller clock */
+	/*
+	 * Max freq (Card Spec)[Hz]. It changes dynamically by CSD and
+	 * EXT_CSD.
+	 */
+	uint32_t max_freq;
+	/* request freq [Hz] (400K, 26MHz, 52MHz, etc) */
+	uint32_t request_freq;
+	/* current MMC clock[Hz] (the closest frequency supported by HW) */
+	uint32_t current_freq;
+
+	/* state flag */
+	/* presence status of the memory card */
+	HAL_MEMCARD_PRESENCE_STATUS card_present;
+
+	uint32_t card_power_enable;
+	uint32_t clock_enable;
+	/* True : initialize complete. */
+	uint32_t initialize;
+	/* True : sector access, FALSE : byte access */
+	uint32_t access_mode;
+	/* True : mount complete. */
+	uint32_t mount;
+	/* True : selected card. */
+	uint32_t selected;
+	/* 0: DMA, 1:PIO */
+	HAL_MEMCARD_DATA_TRANSFER_MODE transfer_mode;
+
+	/* loaded ISSW image No. ISSW have copy image. */
+	uint32_t image_num;
+	/* card state */
+	EMMC_R1_STATE current_state;
+	/* True : during command processing */
+	volatile uint32_t during_cmd_processing;
+	/* True : during transfer */
+	volatile uint32_t during_transfer;
+	/* True : during transfer (DMA) */
+	volatile uint32_t during_dma_transfer;
+	/* True : occurred DMAC error */
+	volatile uint32_t dma_error_flag;
+	/* force terminate flag */
+	volatile uint32_t force_terminate;
+	/* state machine blocking flag : True or False */
+	volatile uint32_t state_machine_blocking;
+	/* True : get partition access processing */
+	volatile uint32_t get_partition_access_flag;
+
+	EMMC_PARTITION_ID boot_partition_en;	/* Boot partition */
+	EMMC_PARTITION_ID partition_access;	/* Current access partition */
+
+	/* timeout */
+	uint32_t hs_timing;
+
+	/* read and write data timeout */
+	uint32_t data_timeout;
+
+	/* retry */
+	uint32_t retries_after_fail;
+
+	/* interrupt */
+	volatile uint32_t int_event1;	/* interrupt SD_INFO1 Event */
+	volatile uint32_t int_event2;	/* interrupt SD_INFO2 Event */
+	volatile uint32_t dm_event1;	/* interrupt DM_CM_INFO1 Event */
+	volatile uint32_t dm_event2;	/* interrupt DM_CM_INFO2 Event */
+
+	/* response */
+	uint32_t *response;		/* buffer ptr for executing command. */
+	uint32_t r1_card_status;	/* R1 response data */
+	uint32_t r3_ocr;		/* R3 response data */
+	uint32_t r4_resp;		/* R4 response data */
+	uint32_t r5_resp;		/* R5 response data */
+
+	/* True : clock mode is low. (MMC clock = Max26MHz) */
+	uint32_t low_clock_mode_enable;
+
+	uint32_t reserved2;
+	uint32_t reserved3;
+	uint32_t reserved4;
+
+	/* CSD registers (4byte align) */
+	uint8_t csd_data[EMMC_MAX_CSD_LENGTH]			/* CSD */
+	    __attribute__ ((aligned(EMMC_RES_REG_ALIGNED)));
+	/* CID registers (4byte align) */
+	uint8_t cid_data[EMMC_MAX_CID_LENGTH]			/* CID */
+	    __attribute__ ((aligned(EMMC_RES_REG_ALIGNED)));
+	/* EXT CSD registers (8byte align) */
+	uint8_t ext_csd_data[EMMC_MAX_EXT_CSD_LENGTH]		/* EXT_CSD */
+	    __attribute__ ((aligned(EMMC_BUF_REG_ALIGNED)));
+	/* Response registers (4byte align) */
+	uint8_t response_data[EMMC_MAX_RESPONSE_LENGTH]		/* other response */
+	    __attribute__ ((aligned(EMMC_RES_REG_ALIGNED)));
+} st_mmc_base;
+
+typedef int (*func) (void);
+
+uint32_t emmc_get_csd_time(void);
+
+#define MMC_DEBUG
+#endif /* EMMC_STD_H */
diff --git a/drivers/renesas/common/emmc/emmc_utility.c b/drivers/renesas/common/emmc/emmc_utility.c
new file mode 100644
index 0000000..2e88abc
--- /dev/null
+++ b/drivers/renesas/common/emmc/emmc_utility.c
@@ -0,0 +1,226 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+
+#include "emmc_config.h"
+#include "emmc_def.h"
+#include "emmc_hal.h"
+#include "emmc_registers.h"
+#include "emmc_std.h"
+
+static const uint32_t cmd_reg_hw[EMMC_CMD_MAX + 1] = {
+	0x00000000,		/* CMD0 */
+	0x00000701,		/* CMD1 */
+	0x00000002,		/* CMD2 */
+	0x00000003,		/* CMD3 */
+	0x00000004,		/* CMD4 */
+	0x00000505,		/* CMD5 */
+	0x00000406,		/* CMD6 */
+	0x00000007,		/* CMD7 */
+	0x00001C08,		/* CMD8 */
+	0x00000009,		/* CMD9 */
+	0x0000000A,		/* CMD10 */
+	0x00000000,		/* reserved */
+	0x0000000C,		/* CMD12 */
+	0x0000000D,		/* CMD13 */
+	0x00001C0E,		/* CMD14 */
+	0x0000000F,		/* CMD15 */
+	0x00000010,		/* CMD16 */
+	0x00000011,		/* CMD17 */
+	0x00007C12,		/* CMD18 */
+	0x00000C13,		/* CMD19 */
+	0x00000000,
+	0x00001C15,		/* CMD21 */
+	0x00000000,
+	0x00000017,		/* CMD23 */
+	0x00000018,		/* CMD24 */
+	0x00006C19,		/* CMD25 */
+	0x00000C1A,		/* CMD26 */
+	0x0000001B,		/* CMD27 */
+	0x0000001C,		/* CMD28 */
+	0x0000001D,		/* CMD29 */
+	0x0000001E,		/* CMD30 */
+	0x00001C1F,		/* CMD31 */
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000423,		/* CMD35 */
+	0x00000424,		/* CMD36 */
+	0x00000000,
+	0x00000026,		/* CMD38 */
+	0x00000427,		/* CMD39 */
+	0x00000428,		/* CMD40(send cmd) */
+	0x00000000,
+	0x0000002A,		/* CMD42 */
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000C31,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00007C35,
+	0x00006C36,
+	0x00000037,		/* CMD55 */
+	0x00000038,		/* CMD56(Read) */
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000
+};
+
+uint32_t emmc_bit_field(uint8_t *data, uint32_t top, uint32_t bottom)
+{
+	uint32_t value;
+
+	uint32_t index_top = (uint32_t) (15 - (top >> 3));
+	uint32_t index_bottom = (uint32_t) (15 - (bottom >> 3));
+
+	if (index_top == index_bottom) {
+		value = data[index_top];
+	} else if ((index_top + 1) == index_bottom) {
+		value =
+		    (uint32_t) ((data[index_top] << 8) | data[index_bottom]);
+	} else if ((index_top + 2) == index_bottom) {
+		value =
+		    (uint32_t) ((data[index_top] << 16) |
+				(data[index_top + 1] << 8) | data[index_top +
+								  2]);
+	} else {
+		value =
+		    (uint32_t) ((data[index_top] << 24) |
+				(data[index_top + 1] << 16) |
+				(data[index_top + 2] << 8) |
+				data[index_top + 3]);
+	}
+
+	value = ((value >> (bottom & 0x07)) & ((1 << (top - bottom + 1)) - 1));
+
+	return value;
+}
+
+void emmc_write_error_info(uint16_t func_no, EMMC_ERROR_CODE error_code)
+{
+
+	mmc_drv_obj.error_info.num = func_no;
+	mmc_drv_obj.error_info.code = (uint16_t) error_code;
+
+	ERROR("BL2: emmc err:func_no=0x%x code=0x%x\n", func_no, error_code);
+}
+
+void emmc_write_error_info_func_no(uint16_t func_no)
+{
+
+	mmc_drv_obj.error_info.num = func_no;
+
+	ERROR("BL2: emmc err:func_no=0x%x\n", func_no);
+}
+
+void emmc_make_nontrans_cmd(HAL_MEMCARD_COMMAND cmd, uint32_t arg)
+{
+	/* command information */
+	mmc_drv_obj.cmd_info.cmd = cmd;
+	mmc_drv_obj.cmd_info.arg = arg;
+	mmc_drv_obj.cmd_info.dir = HAL_MEMCARD_READ;
+	mmc_drv_obj.cmd_info.hw =
+	    cmd_reg_hw[cmd & HAL_MEMCARD_COMMAND_INDEX_MASK];
+
+	/* clear data transfer information */
+	mmc_drv_obj.trans_size = 0;
+	mmc_drv_obj.remain_size = 0;
+	mmc_drv_obj.buff_address_virtual = NULL;
+	mmc_drv_obj.buff_address_physical = NULL;
+
+	/* response information */
+	mmc_drv_obj.response_length = 6;
+
+	switch (mmc_drv_obj.cmd_info.cmd & HAL_MEMCARD_RESPONSE_TYPE_MASK) {
+	case HAL_MEMCARD_RESPONSE_NONE:
+		mmc_drv_obj.response = (uint32_t *) mmc_drv_obj.response_data;
+		mmc_drv_obj.response_length = 0;
+		break;
+	case HAL_MEMCARD_RESPONSE_R1:
+		mmc_drv_obj.response = &mmc_drv_obj.r1_card_status;
+		break;
+	case HAL_MEMCARD_RESPONSE_R1b:
+		mmc_drv_obj.cmd_info.hw |= BIT10; /* bit10 = R1 busy bit */
+		mmc_drv_obj.response = &mmc_drv_obj.r1_card_status;
+		break;
+	case HAL_MEMCARD_RESPONSE_R2:
+		mmc_drv_obj.response = (uint32_t *) mmc_drv_obj.response_data;
+		mmc_drv_obj.response_length = 17;
+		break;
+	case HAL_MEMCARD_RESPONSE_R3:
+		mmc_drv_obj.response = &mmc_drv_obj.r3_ocr;
+		break;
+	case HAL_MEMCARD_RESPONSE_R4:
+		mmc_drv_obj.response = &mmc_drv_obj.r4_resp;
+		break;
+	case HAL_MEMCARD_RESPONSE_R5:
+		mmc_drv_obj.response = &mmc_drv_obj.r5_resp;
+		break;
+	default:
+		mmc_drv_obj.response = (uint32_t *) mmc_drv_obj.response_data;
+		break;
+	}
+}
+
+void emmc_make_trans_cmd(HAL_MEMCARD_COMMAND cmd, uint32_t arg,
+			 uint32_t *buff_address_virtual,
+			 uint32_t len,
+			 HAL_MEMCARD_OPERATION dir,
+			 HAL_MEMCARD_DATA_TRANSFER_MODE transfer_mode)
+{
+	emmc_make_nontrans_cmd(cmd, arg);	/* update common information */
+
+	/* for data transfer command */
+	mmc_drv_obj.cmd_info.dir = dir;
+	mmc_drv_obj.buff_address_virtual = buff_address_virtual;
+	mmc_drv_obj.buff_address_physical = buff_address_virtual;
+	mmc_drv_obj.trans_size = len;
+	mmc_drv_obj.remain_size = len;
+	mmc_drv_obj.transfer_mode = transfer_mode;
+}
+
+EMMC_ERROR_CODE emmc_send_idle_cmd(uint32_t arg)
+{
+	EMMC_ERROR_CODE result;
+	uint32_t freq;
+
+	/* initialize state */
+	mmc_drv_obj.mount = FALSE;
+	mmc_drv_obj.selected = FALSE;
+	mmc_drv_obj.during_transfer = FALSE;
+	mmc_drv_obj.during_cmd_processing = FALSE;
+	mmc_drv_obj.during_dma_transfer = FALSE;
+	mmc_drv_obj.dma_error_flag = FALSE;
+	mmc_drv_obj.force_terminate = FALSE;
+	mmc_drv_obj.state_machine_blocking = FALSE;
+
+	mmc_drv_obj.bus_width = HAL_MEMCARD_DATA_WIDTH_1_BIT;
+	mmc_drv_obj.max_freq = MMC_20MHZ;	/* 20MHz */
+	mmc_drv_obj.current_state = EMMC_R1_STATE_IDLE;
+
+	/* CMD0 (MMC clock is current frequency. if Data transfer mode, 20MHz or higher.) */
+	emmc_make_nontrans_cmd(CMD0_GO_IDLE_STATE, arg);	/* CMD0 */
+	result = emmc_exec_cmd(EMMC_R1_ERROR_MASK, mmc_drv_obj.response);
+	if (result != EMMC_SUCCESS) {
+		return result;
+	}
+
+	/* change MMC clock(400KHz) */
+	freq = MMC_400KHZ;
+	result = emmc_set_request_mmc_clock(&freq);
+	if (result != EMMC_SUCCESS) {
+		return result;
+	}
+
+	return EMMC_SUCCESS;
+}
diff --git a/drivers/renesas/common/iic_dvfs/iic_dvfs.c b/drivers/renesas/common/iic_dvfs/iic_dvfs.c
new file mode 100644
index 0000000..bf80697
--- /dev/null
+++ b/drivers/renesas/common/iic_dvfs/iic_dvfs.c
@@ -0,0 +1,600 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "cpg_registers.h"
+#include "iic_dvfs.h"
+#include "rcar_def.h"
+#include "rcar_private.h"
+
+#define DVFS_RETRY_MAX				(2U)
+
+#define IIC_DVFS_SET_ICCL_EXTAL_TYPE_0		(0x07U)
+#define IIC_DVFS_SET_ICCL_EXTAL_TYPE_1		(0x09U)
+#define IIC_DVFS_SET_ICCL_EXTAL_TYPE_2		(0x0BU)
+#define IIC_DVFS_SET_ICCL_EXTAL_TYPE_3		(0x0EU)
+#define IIC_DVFS_SET_ICCL_EXTAL_TYPE_E		(0x15U)
+
+#define IIC_DVFS_SET_ICCH_EXTAL_TYPE_0		(0x01U)
+#define IIC_DVFS_SET_ICCH_EXTAL_TYPE_1		(0x02U)
+#define IIC_DVFS_SET_ICCH_EXTAL_TYPE_2		(0x03U)
+#define IIC_DVFS_SET_ICCH_EXTAL_TYPE_3		(0x05U)
+#define IIC_DVFS_SET_ICCH_EXTAL_TYPE_E		(0x07U)
+
+#define CPG_BIT_SMSTPCR9_DVFS			(0x04000000U)
+
+#define IIC_DVFS_REG_BASE			(0xE60B0000U)
+#define IIC_DVFS_REG_ICDR			(IIC_DVFS_REG_BASE + 0x0000U)
+#define IIC_DVFS_REG_ICCR			(IIC_DVFS_REG_BASE + 0x0004U)
+#define IIC_DVFS_REG_ICSR			(IIC_DVFS_REG_BASE + 0x0008U)
+#define IIC_DVFS_REG_ICIC			(IIC_DVFS_REG_BASE + 0x000CU)
+#define IIC_DVFS_REG_ICCL			(IIC_DVFS_REG_BASE + 0x0010U)
+#define IIC_DVFS_REG_ICCH			(IIC_DVFS_REG_BASE + 0x0014U)
+
+#define IIC_DVFS_BIT_ICSR_BUSY			(0x10U)
+#define IIC_DVFS_BIT_ICSR_AL			(0x08U)
+#define IIC_DVFS_BIT_ICSR_TACK			(0x04U)
+#define IIC_DVFS_BIT_ICSR_WAIT			(0x02U)
+#define IIC_DVFS_BIT_ICSR_DTE			(0x01U)
+
+#define IIC_DVFS_BIT_ICCR_ENABLE		(0x80U)
+#define IIC_DVFS_SET_ICCR_START			(0x94U)
+#define IIC_DVFS_SET_ICCR_STOP			(0x90U)
+#define IIC_DVFS_SET_ICCR_RETRANSMISSION	(0x94U)
+#define IIC_DVFS_SET_ICCR_CHANGE		(0x81U)
+#define IIC_DVFS_SET_ICCR_STOP_READ		(0xC0U)
+
+#define IIC_DVFS_BIT_ICIC_TACKE			(0x04U)
+#define IIC_DVFS_BIT_ICIC_WAITE			(0x02U)
+#define IIC_DVFS_BIT_ICIC_DTEE			(0x01U)
+
+#define DVFS_READ_MODE				(0x01U)
+#define DVFS_WRITE_MODE				(0x00U)
+
+#define IIC_DVFS_SET_DUMMY			(0x52U)
+#define IIC_DVFS_SET_BUSY_LOOP			(500000000U)
+
+enum dvfs_state_t {
+	DVFS_START = 0,
+	DVFS_STOP,
+	DVFS_RETRANSMIT,
+	DVFS_READ,
+	DVFS_STOP_READ,
+	DVFS_SET_SLAVE_READ,
+	DVFS_SET_SLAVE,
+	DVFS_WRITE_ADDR,
+	DVFS_WRITE_DATA,
+	DVFS_CHANGE_SEND_TO_RECEIVE,
+	DVFS_DONE,
+};
+
+#define DVFS_PROCESS			(1)
+#define DVFS_COMPLETE			(0)
+#define DVFS_ERROR			(-1)
+
+#if IMAGE_BL31
+#define IIC_DVFS_FUNC(__name, ...)					\
+static int32_t	__attribute__ ((section(".system_ram")))		\
+dvfs_ ##__name(__VA_ARGS__)
+
+#define RCAR_DVFS_API(__name, ...)					\
+int32_t __attribute__ ((section(".system_ram")))			\
+rcar_iic_dvfs_ ##__name(__VA_ARGS__)
+
+#else
+#define IIC_DVFS_FUNC(__name, ...)					\
+static int32_t dvfs_ ##__name(__VA_ARGS__)
+
+#define RCAR_DVFS_API(__name, ...)					\
+int32_t rcar_iic_dvfs_ ##__name(__VA_ARGS__)
+#endif
+
+IIC_DVFS_FUNC(check_error, enum dvfs_state_t *state, uint32_t *err, uint8_t mode)
+{
+	uint8_t icsr_al = 0U, icsr_tack = 0U;
+	uint8_t reg, stop;
+	uint32_t i = 0U;
+
+	stop = mode == DVFS_READ_MODE ? IIC_DVFS_SET_ICCR_STOP_READ :
+	    IIC_DVFS_SET_ICCR_STOP;
+
+	reg = mmio_read_8(IIC_DVFS_REG_ICSR);
+	icsr_al = (reg & IIC_DVFS_BIT_ICSR_AL) == IIC_DVFS_BIT_ICSR_AL;
+	icsr_tack = (reg & IIC_DVFS_BIT_ICSR_TACK) == IIC_DVFS_BIT_ICSR_TACK;
+
+	if (icsr_al == 0U && icsr_tack == 0U) {
+		return DVFS_PROCESS;
+	}
+
+	if (icsr_al) {
+		reg = mmio_read_8(IIC_DVFS_REG_ICSR) & ~IIC_DVFS_BIT_ICSR_AL;
+		mmio_write_8(IIC_DVFS_REG_ICSR, reg);
+
+		if (*state == DVFS_SET_SLAVE) {
+			mmio_write_8(IIC_DVFS_REG_ICDR, IIC_DVFS_SET_DUMMY);
+		}
+
+		do {
+			reg = mmio_read_8(IIC_DVFS_REG_ICSR) &
+			    IIC_DVFS_BIT_ICSR_WAIT;
+		} while (reg == 0U);
+
+		mmio_write_8(IIC_DVFS_REG_ICCR, stop);
+
+		reg = mmio_read_8(IIC_DVFS_REG_ICSR) & ~IIC_DVFS_BIT_ICSR_WAIT;
+		mmio_write_8(IIC_DVFS_REG_ICSR, reg);
+
+		i = 0U;
+		do {
+			reg = mmio_read_8(IIC_DVFS_REG_ICSR) &
+			    IIC_DVFS_BIT_ICSR_BUSY;
+			if (reg == 0U) {
+				break;
+			}
+
+			if (i++ > IIC_DVFS_SET_BUSY_LOOP) {
+				panic();
+			}
+
+		} while (true);
+
+		mmio_write_8(IIC_DVFS_REG_ICCR, 0x00U);
+
+		(*err)++;
+		if (*err > DVFS_RETRY_MAX) {
+			return DVFS_ERROR;
+		}
+
+		*state = DVFS_START;
+
+		return DVFS_PROCESS;
+
+	}
+
+	/* icsr_tack */
+	mmio_write_8(IIC_DVFS_REG_ICCR, stop);
+
+	reg = mmio_read_8(IIC_DVFS_REG_ICIC);
+	reg &= ~(IIC_DVFS_BIT_ICIC_WAITE | IIC_DVFS_BIT_ICIC_DTEE);
+	mmio_write_8(IIC_DVFS_REG_ICIC, reg);
+
+	reg = mmio_read_8(IIC_DVFS_REG_ICSR) & ~IIC_DVFS_BIT_ICSR_TACK;
+	mmio_write_8(IIC_DVFS_REG_ICSR, reg);
+
+	i = 0U;
+	while ((mmio_read_8(IIC_DVFS_REG_ICSR) & IIC_DVFS_BIT_ICSR_BUSY) != 0U) {
+		if (i++ > IIC_DVFS_SET_BUSY_LOOP) {
+			panic();
+		}
+	}
+
+	mmio_write_8(IIC_DVFS_REG_ICCR, 0U);
+	(*err)++;
+
+	if (*err > DVFS_RETRY_MAX) {
+		return DVFS_ERROR;
+	}
+
+	*state = DVFS_START;
+
+	return DVFS_PROCESS;
+}
+
+IIC_DVFS_FUNC(start, enum dvfs_state_t *state)
+{
+	uint8_t iccl = IIC_DVFS_SET_ICCL_EXTAL_TYPE_E;
+	uint8_t icch = IIC_DVFS_SET_ICCH_EXTAL_TYPE_E;
+	int32_t result = DVFS_PROCESS;
+	uint32_t reg, lsi_product;
+	uint8_t mode;
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICCR) | IIC_DVFS_BIT_ICCR_ENABLE;
+	mmio_write_8(IIC_DVFS_REG_ICCR, mode);
+
+	lsi_product = mmio_read_32(RCAR_PRR) & PRR_PRODUCT_MASK;
+	if (lsi_product == PRR_PRODUCT_E3) {
+		goto start;
+	}
+
+	reg = mmio_read_32(RCAR_MODEMR) & CHECK_MD13_MD14;
+	switch (reg) {
+	case MD14_MD13_TYPE_0:
+		iccl = IIC_DVFS_SET_ICCL_EXTAL_TYPE_0;
+		icch = IIC_DVFS_SET_ICCH_EXTAL_TYPE_0;
+		break;
+	case MD14_MD13_TYPE_1:
+		iccl = IIC_DVFS_SET_ICCL_EXTAL_TYPE_1;
+		icch = IIC_DVFS_SET_ICCH_EXTAL_TYPE_1;
+		break;
+	case MD14_MD13_TYPE_2:
+		iccl = IIC_DVFS_SET_ICCL_EXTAL_TYPE_2;
+		icch = IIC_DVFS_SET_ICCH_EXTAL_TYPE_2;
+		break;
+	default:
+		iccl = IIC_DVFS_SET_ICCL_EXTAL_TYPE_3;
+		icch = IIC_DVFS_SET_ICCH_EXTAL_TYPE_3;
+		break;
+	}
+start:
+	mmio_write_8(IIC_DVFS_REG_ICCL, iccl);
+	mmio_write_8(IIC_DVFS_REG_ICCH, icch);
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICIC)
+	    | IIC_DVFS_BIT_ICIC_TACKE
+	    | IIC_DVFS_BIT_ICIC_WAITE | IIC_DVFS_BIT_ICIC_DTEE;
+
+	mmio_write_8(IIC_DVFS_REG_ICIC, mode);
+	mmio_write_8(IIC_DVFS_REG_ICCR, IIC_DVFS_SET_ICCR_START);
+
+	*state = DVFS_SET_SLAVE;
+
+	return result;
+}
+
+IIC_DVFS_FUNC(set_slave, enum dvfs_state_t *state, uint32_t *err, uint8_t slave)
+{
+	uint8_t mode;
+	int32_t result;
+	uint8_t address;
+
+	result = dvfs_check_error(state, err, DVFS_WRITE_MODE);
+	if (result == DVFS_ERROR) {
+		return result;
+	}
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & IIC_DVFS_BIT_ICSR_DTE;
+	if (mode != IIC_DVFS_BIT_ICSR_DTE) {
+		return result;
+	}
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICIC) & ~IIC_DVFS_BIT_ICIC_DTEE;
+	mmio_write_8(IIC_DVFS_REG_ICIC, mode);
+
+	address = slave << 1;
+	mmio_write_8(IIC_DVFS_REG_ICDR, address);
+
+	*state = DVFS_WRITE_ADDR;
+
+	return result;
+}
+
+IIC_DVFS_FUNC(write_addr, enum dvfs_state_t *state, uint32_t *err, uint8_t reg_addr)
+{
+	uint8_t mode;
+	int32_t result;
+
+	result = dvfs_check_error(state, err, DVFS_WRITE_MODE);
+	if (result == DVFS_ERROR) {
+		return result;
+	}
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & IIC_DVFS_BIT_ICSR_WAIT;
+	if (mode != IIC_DVFS_BIT_ICSR_WAIT) {
+		return result;
+	}
+
+	mmio_write_8(IIC_DVFS_REG_ICDR, reg_addr);
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & ~IIC_DVFS_BIT_ICSR_WAIT;
+	mmio_write_8(IIC_DVFS_REG_ICSR, mode);
+
+	*state = DVFS_WRITE_DATA;
+
+	return result;
+}
+
+IIC_DVFS_FUNC(write_data, enum dvfs_state_t *state, uint32_t *err,
+	      uint8_t reg_data)
+{
+	int32_t result;
+	uint8_t mode;
+
+	result = dvfs_check_error(state, err, DVFS_WRITE_MODE);
+	if (result == DVFS_ERROR) {
+		return result;
+	}
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & IIC_DVFS_BIT_ICSR_WAIT;
+	if (mode != IIC_DVFS_BIT_ICSR_WAIT) {
+		return result;
+	}
+
+	mmio_write_8(IIC_DVFS_REG_ICDR, reg_data);
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & ~IIC_DVFS_BIT_ICSR_WAIT;
+	mmio_write_8(IIC_DVFS_REG_ICSR, mode);
+
+	*state = DVFS_STOP;
+
+	return result;
+}
+
+IIC_DVFS_FUNC(stop, enum dvfs_state_t *state, uint32_t *err)
+{
+	int32_t result;
+	uint8_t mode;
+
+	result = dvfs_check_error(state, err, DVFS_WRITE_MODE);
+	if (result == DVFS_ERROR) {
+		return result;
+	}
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & IIC_DVFS_BIT_ICSR_WAIT;
+	if (mode != IIC_DVFS_BIT_ICSR_WAIT) {
+		return result;
+	}
+
+	mmio_write_8(IIC_DVFS_REG_ICCR, IIC_DVFS_SET_ICCR_STOP);
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & ~IIC_DVFS_BIT_ICSR_WAIT;
+	mmio_write_8(IIC_DVFS_REG_ICSR, mode);
+
+	*state = DVFS_DONE;
+
+	return result;
+}
+
+IIC_DVFS_FUNC(done, void)
+{
+	uint32_t i;
+
+	for (i = 0U; i < IIC_DVFS_SET_BUSY_LOOP; i++) {
+		if ((mmio_read_8(IIC_DVFS_REG_ICSR) & IIC_DVFS_BIT_ICSR_BUSY) != 0U) {
+			continue;
+		}
+		goto done;
+	}
+
+	panic();
+done:
+	mmio_write_8(IIC_DVFS_REG_ICCR, 0U);
+
+	return DVFS_COMPLETE;
+}
+
+IIC_DVFS_FUNC(write_reg_addr_read, enum dvfs_state_t *state, uint32_t *err,
+	      uint8_t reg_addr)
+{
+	int32_t result;
+	uint8_t mode;
+
+	result = dvfs_check_error(state, err, DVFS_WRITE_MODE);
+	if (result == DVFS_ERROR) {
+		return result;
+	}
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & IIC_DVFS_BIT_ICSR_WAIT;
+	if (mode != IIC_DVFS_BIT_ICSR_WAIT) {
+		return result;
+	}
+
+	mmio_write_8(IIC_DVFS_REG_ICDR, reg_addr);
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & ~IIC_DVFS_BIT_ICSR_WAIT;
+	mmio_write_8(IIC_DVFS_REG_ICSR, mode);
+
+	*state = DVFS_RETRANSMIT;
+
+	return result;
+}
+
+IIC_DVFS_FUNC(retransmit, enum dvfs_state_t *state, uint32_t *err)
+{
+	int32_t result;
+	uint8_t mode;
+
+	result = dvfs_check_error(state, err, DVFS_WRITE_MODE);
+	if (result == DVFS_ERROR) {
+		return result;
+	}
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & IIC_DVFS_BIT_ICSR_WAIT;
+	if (mode != IIC_DVFS_BIT_ICSR_WAIT) {
+		return result;
+	}
+
+	mmio_write_8(IIC_DVFS_REG_ICCR, IIC_DVFS_SET_ICCR_RETRANSMISSION);
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & ~IIC_DVFS_BIT_ICSR_WAIT;
+	mmio_write_8(IIC_DVFS_REG_ICSR, mode);
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICIC) | IIC_DVFS_BIT_ICIC_DTEE;
+	mmio_write_8(IIC_DVFS_REG_ICIC, mode);
+
+	*state = DVFS_SET_SLAVE_READ;
+
+	return result;
+}
+
+IIC_DVFS_FUNC(set_slave_read, enum dvfs_state_t *state, uint32_t *err,
+	      uint8_t slave)
+{
+	uint8_t address;
+	int32_t result;
+	uint8_t mode;
+
+	result = dvfs_check_error(state, err, DVFS_WRITE_MODE);
+	if (result == DVFS_ERROR) {
+		return result;
+	}
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & IIC_DVFS_BIT_ICSR_DTE;
+	if (mode != IIC_DVFS_BIT_ICSR_DTE) {
+		return result;
+	}
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICIC) & ~IIC_DVFS_BIT_ICIC_DTEE;
+	mmio_write_8(IIC_DVFS_REG_ICIC, mode);
+
+	address = ((uint8_t) (slave << 1) + DVFS_READ_MODE);
+	mmio_write_8(IIC_DVFS_REG_ICDR, address);
+
+	*state = DVFS_CHANGE_SEND_TO_RECEIVE;
+
+	return result;
+}
+
+IIC_DVFS_FUNC(change_send_to_receive, enum dvfs_state_t *state, uint32_t *err)
+{
+	int32_t result;
+	uint8_t mode;
+
+	result = dvfs_check_error(state, err, DVFS_WRITE_MODE);
+	if (result == DVFS_ERROR) {
+		return result;
+	}
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & IIC_DVFS_BIT_ICSR_WAIT;
+	if (mode != IIC_DVFS_BIT_ICSR_WAIT) {
+		return result;
+	}
+
+	mmio_write_8(IIC_DVFS_REG_ICCR, IIC_DVFS_SET_ICCR_CHANGE);
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & ~IIC_DVFS_BIT_ICSR_WAIT;
+	mmio_write_8(IIC_DVFS_REG_ICSR, mode);
+
+	*state = DVFS_STOP_READ;
+
+	return result;
+}
+
+IIC_DVFS_FUNC(stop_read, enum dvfs_state_t *state, uint32_t *err)
+{
+	int32_t result;
+	uint8_t mode;
+
+	result = dvfs_check_error(state, err, DVFS_READ_MODE);
+	if (result == DVFS_ERROR) {
+		return result;
+	}
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & IIC_DVFS_BIT_ICSR_WAIT;
+	if (mode != IIC_DVFS_BIT_ICSR_WAIT) {
+		return result;
+	}
+
+	mmio_write_8(IIC_DVFS_REG_ICCR, IIC_DVFS_SET_ICCR_STOP_READ);
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & ~IIC_DVFS_BIT_ICSR_WAIT;
+	mmio_write_8(IIC_DVFS_REG_ICSR, mode);
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICIC) | IIC_DVFS_BIT_ICIC_DTEE;
+	mmio_write_8(IIC_DVFS_REG_ICIC, mode);
+
+	*state = DVFS_READ;
+
+	return result;
+}
+
+IIC_DVFS_FUNC(read, enum dvfs_state_t *state, uint8_t *reg_data)
+{
+	uint8_t mode;
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICSR) & IIC_DVFS_BIT_ICSR_DTE;
+	if (mode != IIC_DVFS_BIT_ICSR_DTE) {
+		return DVFS_PROCESS;
+	}
+
+	mode = mmio_read_8(IIC_DVFS_REG_ICIC) & ~IIC_DVFS_BIT_ICIC_DTEE;
+	mmio_write_8(IIC_DVFS_REG_ICIC, mode);
+
+	*reg_data = mmio_read_8(IIC_DVFS_REG_ICDR);
+	*state = DVFS_DONE;
+
+	return DVFS_PROCESS;
+}
+
+RCAR_DVFS_API(send, uint8_t slave, uint8_t reg_addr, uint8_t reg_data)
+{
+	enum dvfs_state_t state = DVFS_START;
+	int32_t result = DVFS_PROCESS;
+	uint32_t err = 0U;
+
+	mstpcr_write(SCMSTPCR9, CPG_MSTPSR9, CPG_BIT_SMSTPCR9_DVFS);
+	mmio_write_8(IIC_DVFS_REG_ICCR, 1U);
+again:
+	switch (state) {
+	case DVFS_START:
+		result = dvfs_start(&state);
+		break;
+	case DVFS_SET_SLAVE:
+		result = dvfs_set_slave(&state, &err, slave);
+		break;
+	case DVFS_WRITE_ADDR:
+		result = dvfs_write_addr(&state, &err, reg_addr);
+		break;
+	case DVFS_WRITE_DATA:
+		result = dvfs_write_data(&state, &err, reg_data);
+		break;
+	case DVFS_STOP:
+		result = dvfs_stop(&state, &err);
+		break;
+	case DVFS_DONE:
+		result = dvfs_done();
+		break;
+	default:
+		panic();
+		break;
+	}
+
+	if (result == DVFS_PROCESS) {
+		goto again;
+	}
+
+	return result;
+}
+
+RCAR_DVFS_API(receive, uint8_t slave, uint8_t reg, uint8_t *data)
+{
+	enum dvfs_state_t state = DVFS_START;
+	int32_t result = DVFS_PROCESS;
+	uint32_t err = 0U;
+
+	mstpcr_write(SCMSTPCR9, CPG_MSTPSR9, CPG_BIT_SMSTPCR9_DVFS);
+	mmio_write_8(IIC_DVFS_REG_ICCR, 1U);
+again:
+	switch (state) {
+	case DVFS_START:
+		result = dvfs_start(&state);
+		break;
+	case DVFS_SET_SLAVE:
+		result = dvfs_set_slave(&state, &err, slave);
+		break;
+	case DVFS_WRITE_ADDR:
+		result = dvfs_write_reg_addr_read(&state, &err, reg);
+		break;
+	case DVFS_RETRANSMIT:
+		result = dvfs_retransmit(&state, &err);
+		break;
+	case DVFS_SET_SLAVE_READ:
+		result = dvfs_set_slave_read(&state, &err, slave);
+		break;
+	case DVFS_CHANGE_SEND_TO_RECEIVE:
+		result = dvfs_change_send_to_receive(&state, &err);
+		break;
+	case DVFS_STOP_READ:
+		result = dvfs_stop_read(&state, &err);
+		break;
+	case DVFS_READ:
+		result = dvfs_read(&state, data);
+		break;
+	case DVFS_DONE:
+		result = dvfs_done();
+		break;
+	default:
+		panic();
+		break;
+	}
+
+	if (result == DVFS_PROCESS) {
+		goto again;
+	}
+
+	return result;
+}
diff --git a/drivers/renesas/common/iic_dvfs/iic_dvfs.h b/drivers/renesas/common/iic_dvfs/iic_dvfs.h
new file mode 100644
index 0000000..244c06c
--- /dev/null
+++ b/drivers/renesas/common/iic_dvfs/iic_dvfs.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IIC_DVFS_H
+#define IIC_DVFS_H
+
+/* PMIC slave */
+#define PMIC			(0x30U)
+#define BKUP_MODE_CNT		(0x20U)
+#define DVFS_SET_VID		(0x54U)
+#define REG_KEEP10		(0x79U)
+
+/* EEPROM slave */
+#define EEPROM			(0x50U)
+#define BOARD_ID		(0x70U)
+
+int32_t rcar_iic_dvfs_receive(uint8_t slave, uint8_t reg, uint8_t *data);
+int32_t rcar_iic_dvfs_send(uint8_t slave, uint8_t regr, uint8_t data);
+
+#endif /* IIC_DVFS_H */
diff --git a/drivers/renesas/common/io/io_common.h b/drivers/renesas/common/io/io_common.h
new file mode 100644
index 0000000..6eb7777
--- /dev/null
+++ b/drivers/renesas/common/io/io_common.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IO_COMMON_H
+#define IO_COMMON_H
+
+typedef struct io_drv_spec {
+	size_t offset;
+	size_t length;
+	uint32_t partition;
+} io_drv_spec_t;
+
+#endif /* IO_COMMON_H */
diff --git a/drivers/renesas/common/io/io_emmcdrv.c b/drivers/renesas/common/io/io_emmcdrv.c
new file mode 100644
index 0000000..c2b5f7c
--- /dev/null
+++ b/drivers/renesas/common/io/io_emmcdrv.c
@@ -0,0 +1,179 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_storage.h>
+
+#include "emmc_config.h"
+#include "emmc_def.h"
+#include "emmc_hal.h"
+#include "emmc_std.h"
+#include "io_common.h"
+#include "io_emmcdrv.h"
+#include "io_private.h"
+
+static int32_t emmcdrv_dev_open(const uintptr_t spec __attribute__ ((unused)),
+				io_dev_info_t **dev_info);
+static int32_t emmcdrv_dev_close(io_dev_info_t *dev_info);
+
+typedef struct {
+	uint32_t in_use;
+	uintptr_t base;
+	signed long long file_pos;
+	EMMC_PARTITION_ID partition;
+} file_state_t;
+
+static file_state_t current_file = { 0 };
+
+static EMMC_PARTITION_ID emmcdrv_bootpartition = PARTITION_ID_USER;
+
+static io_type_t device_type_emmcdrv(void)
+{
+	return IO_TYPE_MEMMAP;
+}
+
+static int32_t emmcdrv_block_seek(io_entity_t *entity, int32_t mode,
+				  signed long long offset)
+{
+	if (mode != IO_SEEK_SET) {
+		return IO_FAIL;
+	}
+
+	((file_state_t *) entity->info)->file_pos = offset;
+
+	return IO_SUCCESS;
+}
+
+static int32_t emmcdrv_block_read(io_entity_t *entity, uintptr_t buffer,
+				  size_t length, size_t *length_read)
+{
+	file_state_t *fp = (file_state_t *) entity->info;
+	uint32_t sector_add, sector_num, emmc_dma = 0;
+	int32_t result = IO_SUCCESS;
+
+	sector_add = current_file.file_pos >> EMMC_SECTOR_SIZE_SHIFT;
+	sector_num = (length + EMMC_SECTOR_SIZE - 1U) >> EMMC_SECTOR_SIZE_SHIFT;
+
+	NOTICE("BL2: Load dst=0x%lx src=(p:%d)0x%llx(%d) len=0x%lx(%d)\n",
+	       buffer,
+	       current_file.partition, current_file.file_pos,
+	       sector_add, length, sector_num);
+
+	if ((buffer + length - 1U) <= (uintptr_t)UINT32_MAX) {
+		emmc_dma = LOADIMAGE_FLAGS_DMA_ENABLE;
+	}
+
+	if (emmc_read_sector((uint32_t *) buffer, sector_add, sector_num,
+			     emmc_dma) != EMMC_SUCCESS) {
+		result = IO_FAIL;
+	}
+
+	*length_read = length;
+	fp->file_pos += (signed long long)length;
+
+	return result;
+}
+
+static int32_t emmcdrv_block_open(io_dev_info_t *dev_info,
+				  const uintptr_t spec, io_entity_t *entity)
+{
+	const io_drv_spec_t *block_spec = (io_drv_spec_t *) spec;
+
+	if (current_file.in_use != 0U) {
+		WARN("mmc_block: Only one open spec at a time\n");
+		return IO_RESOURCES_EXHAUSTED;
+	}
+
+	current_file.file_pos = 0;
+	current_file.in_use = 1;
+
+	if (emmcdrv_bootpartition == PARTITION_ID_USER) {
+		emmcdrv_bootpartition = mmc_drv_obj.boot_partition_en;
+		if ((emmcdrv_bootpartition == PARTITION_ID_BOOT_1) ||
+		    (emmcdrv_bootpartition == PARTITION_ID_BOOT_2)) {
+			current_file.partition = emmcdrv_bootpartition;
+
+			NOTICE("BL2: eMMC boot from partition %d\n",
+			       emmcdrv_bootpartition);
+			goto done;
+		}
+		return IO_FAIL;
+	}
+
+	if ((block_spec->partition == PARTITION_ID_USER) ||
+	    (block_spec->partition == PARTITION_ID_BOOT_1) ||
+	    (block_spec->partition == PARTITION_ID_BOOT_2)) {
+		current_file.partition = block_spec->partition;
+	} else {
+		current_file.partition = emmcdrv_bootpartition;
+	}
+
+done:
+	if (emmc_select_partition(current_file.partition) != EMMC_SUCCESS) {
+		return IO_FAIL;
+	}
+
+	entity->info = (uintptr_t) &current_file;
+
+	return IO_SUCCESS;
+}
+
+static int32_t emmcdrv_block_close(io_entity_t *entity)
+{
+	memset((void *)&current_file, 0, sizeof(current_file));
+	entity->info = 0U;
+
+	return IO_SUCCESS;
+}
+
+static const io_dev_funcs_t emmcdrv_dev_funcs = {
+	.type = &device_type_emmcdrv,
+	.open = &emmcdrv_block_open,
+	.seek = &emmcdrv_block_seek,
+	.size = NULL,
+	.read = &emmcdrv_block_read,
+	.write = NULL,
+	.close = &emmcdrv_block_close,
+	.dev_init = NULL,
+	.dev_close = &emmcdrv_dev_close
+};
+
+static const io_dev_info_t emmcdrv_dev_info = {
+	.funcs = &emmcdrv_dev_funcs,
+	.info = (uintptr_t) 0
+};
+
+static const io_dev_connector_t emmcdrv_dev_connector = {
+	&emmcdrv_dev_open,
+};
+
+static int32_t emmcdrv_dev_open(const uintptr_t spec __attribute__ ((unused)),
+				io_dev_info_t **dev_info)
+{
+	*dev_info = (io_dev_info_t *) &emmcdrv_dev_info;
+
+	return IO_SUCCESS;
+}
+
+static int32_t emmcdrv_dev_close(io_dev_info_t *dev_info)
+{
+	return IO_SUCCESS;
+}
+
+int32_t rcar_register_io_dev_emmcdrv(const io_dev_connector_t **dev_con)
+{
+	int32_t rc;
+
+	rc = io_register_device(&emmcdrv_dev_info);
+	if (rc == IO_SUCCESS) {
+		*dev_con = &emmcdrv_dev_connector;
+	}
+
+	return rc;
+}
diff --git a/drivers/renesas/common/io/io_emmcdrv.h b/drivers/renesas/common/io/io_emmcdrv.h
new file mode 100644
index 0000000..95070f2
--- /dev/null
+++ b/drivers/renesas/common/io/io_emmcdrv.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IO_EMMCDRV_H
+#define IO_EMMCDRV_H
+
+struct io_dev_connector;
+int32_t rcar_register_io_dev_emmcdrv(const io_dev_connector_t **connector);
+
+#endif /* IO_EMMCDRV_H */
diff --git a/drivers/renesas/common/io/io_memdrv.c b/drivers/renesas/common/io/io_memdrv.c
new file mode 100644
index 0000000..1f31c0f
--- /dev/null
+++ b/drivers/renesas/common/io/io_memdrv.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_storage.h>
+
+#include "io_common.h"
+#include "io_memdrv.h"
+#include "io_private.h"
+#include "rcar_def.h"
+
+extern void rcar_dma_exec(uintptr_t dst, uint32_t src, uint32_t len);
+
+static int32_t memdrv_dev_open(const uintptr_t dev __attribute__ ((unused)),
+			       io_dev_info_t **dev_info);
+static int32_t memdrv_dev_close(io_dev_info_t *dev_info);
+
+/*
+ * As we need to be able to keep state for seek, only one file can be open
+ * at a time. Make this a structure and point to the entity->info. When we
+ * can malloc memory we can change this to support more open files.
+ */
+typedef struct {
+	uint32_t in_use;
+	uintptr_t base;
+	signed long long file_pos;
+} file_state_t;
+
+static file_state_t current_file = { 0 };
+
+static io_type_t device_type_memdrv(void)
+{
+	return IO_TYPE_MEMMAP;
+}
+
+static int32_t memdrv_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+				 io_entity_t *entity)
+{
+	const io_drv_spec_t *block_spec = (io_drv_spec_t *) spec;
+
+	/*
+	 * Since we need to track open state for seek() we only allow one open
+	 * spec at a time. When we have dynamic memory we can malloc and set
+	 * entity->info.
+	 */
+	if (current_file.in_use != 0U) {
+		return IO_RESOURCES_EXHAUSTED;
+	}
+
+	/* File cursor offset for seek and incremental reads etc. */
+	current_file.base = block_spec->offset;
+	current_file.file_pos = 0;
+	current_file.in_use = 1;
+
+	entity->info = (uintptr_t) &current_file;
+
+	return IO_SUCCESS;
+}
+
+static int32_t memdrv_block_seek(io_entity_t *entity, int32_t mode,
+				 signed long long offset)
+{
+	if (mode != IO_SEEK_SET) {
+		return IO_FAIL;
+	}
+
+	((file_state_t *) entity->info)->file_pos = offset;
+
+	return IO_SUCCESS;
+}
+
+static int32_t memdrv_block_read(io_entity_t *entity, uintptr_t buffer,
+				 size_t length, size_t *cnt)
+{
+	file_state_t *fp;
+
+	fp = (file_state_t *) entity->info;
+
+	NOTICE("BL2: dst=0x%lx src=0x%llx len=%ld(0x%lx)\n",
+	       buffer, (unsigned long long)fp->base +
+	       (unsigned long long)fp->file_pos, length, length);
+
+	if (FLASH_MEMORY_SIZE < (fp->file_pos + (signed long long)length)) {
+		ERROR("BL2: check load image (source address)\n");
+		return IO_FAIL;
+	}
+
+	rcar_dma_exec(buffer, fp->base + (uintptr_t)fp->file_pos, length);
+	fp->file_pos += (signed long long)length;
+	*cnt = length;
+
+	return IO_SUCCESS;
+}
+
+static int32_t memdrv_block_close(io_entity_t *entity)
+{
+	entity->info = 0U;
+
+	memset((void *)&current_file, 0, sizeof(current_file));
+
+	return IO_SUCCESS;
+}
+
+static const io_dev_funcs_t memdrv_dev_funcs = {
+	.type = &device_type_memdrv,
+	.open = &memdrv_block_open,
+	.seek = &memdrv_block_seek,
+	.size = NULL,
+	.read = &memdrv_block_read,
+	.write = NULL,
+	.close = &memdrv_block_close,
+	.dev_init = NULL,
+	.dev_close = &memdrv_dev_close,
+};
+
+static const io_dev_info_t memdrv_dev_info = {
+	.funcs = &memdrv_dev_funcs,
+	.info = 0,
+};
+
+static const io_dev_connector_t memdrv_dev_connector = {
+	.dev_open = &memdrv_dev_open
+};
+
+static int32_t memdrv_dev_open(const uintptr_t dev __attribute__ ((unused)),
+			       io_dev_info_t **dev_info)
+{
+	*dev_info = (io_dev_info_t *) &memdrv_dev_info;
+
+	return IO_SUCCESS;
+}
+
+static int32_t memdrv_dev_close(io_dev_info_t *dev_info)
+{
+	return IO_SUCCESS;
+}
+
+int32_t rcar_register_io_dev_memdrv(const io_dev_connector_t **dev_con)
+{
+	int32_t result;
+
+	result = io_register_device(&memdrv_dev_info);
+	if (result == IO_SUCCESS) {
+		*dev_con = &memdrv_dev_connector;
+	}
+
+	return result;
+}
diff --git a/drivers/renesas/common/io/io_memdrv.h b/drivers/renesas/common/io/io_memdrv.h
new file mode 100644
index 0000000..90e6812
--- /dev/null
+++ b/drivers/renesas/common/io/io_memdrv.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IO_MEMDRV_H
+#define IO_MEMDRV_H
+
+struct io_dev_connector;
+int32_t rcar_register_io_dev_memdrv(const io_dev_connector_t **connector);
+
+#endif /* IO_MEMDRV_H */
diff --git a/drivers/renesas/common/io/io_private.h b/drivers/renesas/common/io/io_private.h
new file mode 100644
index 0000000..207523a
--- /dev/null
+++ b/drivers/renesas/common/io/io_private.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IO_PRIVATE_H
+#define IO_PRIVATE_H
+
+/*
+ * Return codes reported by 'io_*' APIs
+ * The value of fail should not overlap with define of the errno.
+ * The errno is in "include/lib/stdlib/sys/errno.h".
+ */
+#define IO_SUCCESS		(0)
+#define IO_FAIL			(-0x81)
+#define IO_NOT_SUPPORTED	(-0x82)
+#define IO_RESOURCES_EXHAUSTED	(-0x83)
+
+#endif /* IO_PRIVATE_H */
diff --git a/drivers/renesas/common/io/io_rcar.c b/drivers/renesas/common/io/io_rcar.c
new file mode 100644
index 0000000..45ef386
--- /dev/null
+++ b/drivers/renesas/common/io/io_rcar.c
@@ -0,0 +1,665 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/auth/auth_mod.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_storage.h>
+#include <lib/mmio.h>
+#include <plat/common/platform.h>
+#include <tools_share/firmware_image_package.h>
+#include <tools_share/uuid.h>
+
+#include "io_rcar.h"
+#include "io_common.h"
+#include "io_private.h"
+#include <platform_def.h>
+
+extern int32_t plat_get_drv_source(uint32_t id, uintptr_t *dev,
+				   uintptr_t *image_spec);
+
+static int32_t rcar_dev_open(const uintptr_t dev_spec __attribute__ ((unused)),
+			     io_dev_info_t **dev_info);
+static int32_t rcar_dev_close(io_dev_info_t *dev_info);
+
+typedef struct {
+	const int32_t name;
+	const uint32_t offset;
+	const uint32_t attr;
+} plat_rcar_name_offset_t;
+
+typedef struct {
+	/*
+	 * Put position above the struct to allow {0} on static init.
+	 * It is a workaround for a known bug in GCC
+	 * http://gcc.gnu.org/bugzilla/show_bug.cgi?id=53119
+	 */
+	uint32_t position;
+	uint32_t no_load;
+	uintptr_t offset;
+	uint32_t size;
+	uintptr_t dst;
+	uintptr_t partition;	/* for eMMC */
+	/* RCAR_EMMC_PARTITION_BOOT_0 */
+	/* RCAR_EMMC_PARTITION_BOOT_1 */
+	/* RCAR_EMMC_PARTITION_USER   */
+} file_state_t;
+
+#define RCAR_GET_FLASH_ADR(a, b)	((uint32_t)((0x40000U * (a)) + (b)))
+#define RCAR_ATTR_SET_CALCADDR(a)	((a) & 0xF)
+#define RCAR_ATTR_SET_ISNOLOAD(a)	(((a) & 0x1) << 16U)
+#define RCAR_ATTR_SET_CERTOFF(a)	(((a) & 0xF) << 8U)
+#define RCAR_ATTR_SET_ALL(a, b, c)	((uint32_t)(RCAR_ATTR_SET_CALCADDR(a) |\
+					RCAR_ATTR_SET_ISNOLOAD(b) |\
+					RCAR_ATTR_SET_CERTOFF(c)))
+
+#define RCAR_ATTR_GET_CALCADDR(a)	((a) & 0xFU)
+#define RCAR_ATTR_GET_ISNOLOAD(a)	(((a) >> 16) & 0x1U)
+#define RCAR_ATTR_GET_CERTOFF(a)	((uint32_t)(((a) >> 8) & 0xFU))
+
+#define RCAR_MAX_BL3X_IMAGE		(8U)
+#define RCAR_SECTOR6_CERT_OFFSET	(0x400U)
+#define RCAR_SDRAM_certESS		(0x43F00000U)
+#define RCAR_CERT_SIZE			(0x800U)
+#define RCAR_CERT_INFO_SIZE_OFFSET	(0x264U)
+#define RCAR_CERT_INFO_DST_OFFSET	(0x154U)
+#define RCAR_CERT_INFO_SIZE_OFFSET1	(0x364U)
+#define RCAR_CERT_INFO_DST_OFFSET1	(0x1D4U)
+#define RCAR_CERT_INFO_SIZE_OFFSET2	(0x464U)
+#define RCAR_CERT_INFO_DST_OFFSET2	(0x254U)
+#define RCAR_CERT_LOAD			(1U)
+
+#define RCAR_FLASH_CERT_HEADER		RCAR_GET_FLASH_ADR(6U, 0U)
+#define RCAR_EMMC_CERT_HEADER		(0x00030000U)
+
+#define RCAR_COUNT_LOAD_BL33		(2U)
+#define RCAR_COUNT_LOAD_BL33X		(3U)
+
+static const plat_rcar_name_offset_t name_offset[] = {
+	{BL31_IMAGE_ID, 0U, RCAR_ATTR_SET_ALL(0, 0, 0)},
+
+	/* BL3-2 is optional in the platform */
+	{BL32_IMAGE_ID, 0U, RCAR_ATTR_SET_ALL(1, 0, 1)},
+	{BL33_IMAGE_ID, 0U, RCAR_ATTR_SET_ALL(2, 0, 2)},
+	{BL332_IMAGE_ID, 0U, RCAR_ATTR_SET_ALL(3, 0, 3)},
+	{BL333_IMAGE_ID, 0U, RCAR_ATTR_SET_ALL(4, 0, 4)},
+	{BL334_IMAGE_ID, 0U, RCAR_ATTR_SET_ALL(5, 0, 5)},
+	{BL335_IMAGE_ID, 0U, RCAR_ATTR_SET_ALL(6, 0, 6)},
+	{BL336_IMAGE_ID, 0U, RCAR_ATTR_SET_ALL(7, 0, 7)},
+	{BL337_IMAGE_ID, 0U, RCAR_ATTR_SET_ALL(8, 0, 8)},
+	{BL338_IMAGE_ID, 0U, RCAR_ATTR_SET_ALL(9, 0, 9)},
+};
+
+#if TRUSTED_BOARD_BOOT
+static const plat_rcar_name_offset_t cert_offset[] = {
+	/* Certificates */
+	{TRUSTED_KEY_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 0)},
+	{SOC_FW_KEY_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 0)},
+	{TRUSTED_OS_FW_KEY_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 0)},
+	{NON_TRUSTED_FW_KEY_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 0)},
+	{SOC_FW_CONTENT_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 0)},
+	{TRUSTED_OS_FW_CONTENT_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 1)},
+	{NON_TRUSTED_FW_CONTENT_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 2)},
+	{BL332_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 3)},
+	{BL333_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 4)},
+	{BL334_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 5)},
+	{BL335_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 6)},
+	{BL336_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 7)},
+	{BL337_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 8)},
+	{BL338_CERT_ID, 0U, RCAR_ATTR_SET_ALL(0, 1, 9)},
+};
+#endif /* TRUSTED_BOARD_BOOT */
+
+static file_state_t current_file = { 0 };
+
+static uintptr_t rcar_handle, rcar_spec;
+static uint64_t rcar_image_header[RCAR_MAX_BL3X_IMAGE + 2U] = { 0U };
+static uint64_t rcar_image_header_prttn[RCAR_MAX_BL3X_IMAGE + 2U] = { 0U };
+static uint64_t rcar_image_number = { 0U };
+static uint32_t rcar_cert_load = { 0U };
+
+static io_type_t device_type_rcar(void)
+{
+	return IO_TYPE_FIRMWARE_IMAGE_PACKAGE;
+}
+
+int32_t rcar_get_certificate(const int32_t name, uint32_t *cert)
+{
+#if TRUSTED_BOARD_BOOT
+	int32_t i;
+
+	for (i = 0; i < ARRAY_SIZE(cert_offset); i++) {
+		if (name != cert_offset[i].name) {
+			continue;
+		}
+
+		*cert = RCAR_CERT_SIZE;
+		*cert *= RCAR_ATTR_GET_CERTOFF(cert_offset[i].attr);
+		*cert += RCAR_SDRAM_certESS;
+		return 0;
+	}
+#endif
+	return -EINVAL;
+}
+
+#define MFISBTSTSR			(0xE6260604U)
+#define MFISBTSTSR_BOOT_PARTITION	(0x00000010U)
+
+static int32_t file_to_offset(const int32_t name, uintptr_t *offset,
+			      uint32_t *cert, uint32_t *no_load,
+			      uintptr_t *partition)
+{
+	uint32_t addr;
+	int32_t i;
+
+	for (i = 0; i < ARRAY_SIZE(name_offset); i++) {
+		if (name != name_offset[i].name) {
+			continue;
+		}
+
+		addr = RCAR_ATTR_GET_CALCADDR(name_offset[i].attr);
+		if (rcar_image_number + 2U < addr) {
+			continue;
+		}
+
+		*offset = rcar_image_header[addr];
+
+		if (mmio_read_32(MFISBTSTSR) & MFISBTSTSR_BOOT_PARTITION)
+			*offset += 0x800000;
+		*cert = RCAR_CERT_SIZE;
+		*cert *= RCAR_ATTR_GET_CERTOFF(name_offset[i].attr);
+		*cert += RCAR_SDRAM_certESS;
+		*no_load = RCAR_ATTR_GET_ISNOLOAD(name_offset[i].attr);
+		*partition = rcar_image_header_prttn[addr];
+		return IO_SUCCESS;
+	}
+
+#if TRUSTED_BOARD_BOOT
+	for (i = 0; i < ARRAY_SIZE(cert_offset); i++) {
+		if (name != cert_offset[i].name) {
+			continue;
+		}
+
+		*no_load = RCAR_ATTR_GET_ISNOLOAD(cert_offset[i].attr);
+		*partition = 0U;
+		*offset = 0U;
+		*cert = 0U;
+		return IO_SUCCESS;
+	}
+#endif
+	return -EINVAL;
+}
+
+#define RCAR_BOOT_KEY_CERT_NEW	(0xE6300F00U)
+#define	RCAR_CERT_MAGIC_NUM	(0xE291F358U)
+
+void rcar_read_certificate(uint64_t cert, uint32_t *len, uintptr_t *dst)
+{
+	uint32_t seed, val, info_1, info_2;
+	uintptr_t size, dsth, dstl;
+
+	cert &= 0xFFFFFFFFU;
+
+	seed = mmio_read_32(RCAR_BOOT_KEY_CERT_NEW);
+	val = mmio_read_32(RCAR_BOOT_KEY_CERT_NEW + 0xC);
+	info_1 = (val >> 18) & 0x3U;
+	val = mmio_read_32(cert + 0xC);
+	info_2 = (val >> 21) & 0x3;
+
+	if (seed == RCAR_CERT_MAGIC_NUM) {
+		if (info_1 != 1) {
+			ERROR("BL2: Cert is invalid.\n");
+			*dst = 0;
+			*len = 0;
+			return;
+		}
+
+		if (info_2 > 2) {
+			ERROR("BL2: Cert is invalid.\n");
+			*dst = 0;
+			*len = 0;
+			return;
+		}
+
+		switch (info_2) {
+		case 2:
+			size = cert + RCAR_CERT_INFO_SIZE_OFFSET2;
+			dstl = cert + RCAR_CERT_INFO_DST_OFFSET2;
+			break;
+		case 1:
+			size = cert + RCAR_CERT_INFO_SIZE_OFFSET1;
+			dstl = cert + RCAR_CERT_INFO_DST_OFFSET1;
+			break;
+		case 0:
+			size = cert + RCAR_CERT_INFO_SIZE_OFFSET;
+			dstl = cert + RCAR_CERT_INFO_DST_OFFSET;
+			break;
+		}
+
+		*len = mmio_read_32(size) * 4U;
+		dsth = dstl + 4U;
+		*dst = ((uintptr_t) mmio_read_32(dsth) << 32) +
+		    ((uintptr_t) mmio_read_32(dstl));
+		return;
+	}
+
+	size = cert + RCAR_CERT_INFO_SIZE_OFFSET;
+	*len = mmio_read_32(size) * 4U;
+	dstl = cert + RCAR_CERT_INFO_DST_OFFSET;
+	dsth = dstl + 4U;
+	*dst = ((uintptr_t) mmio_read_32(dsth) << 32) +
+	    ((uintptr_t) mmio_read_32(dstl));
+}
+
+static int32_t check_load_area(uintptr_t dst, uintptr_t len)
+{
+	uint32_t legacy = dst + len <= UINT32_MAX - 1 ? 1 : 0;
+	uintptr_t dram_start, dram_end;
+	uintptr_t prot_start, prot_end;
+	int32_t result = IO_SUCCESS;
+
+	dram_start = legacy ? DRAM1_BASE : DRAM_40BIT_BASE;
+
+	dram_end = legacy ? DRAM1_BASE + DRAM1_SIZE :
+	    DRAM_40BIT_BASE + DRAM_40BIT_SIZE;
+
+	prot_start = legacy ? DRAM_PROTECTED_BASE : DRAM_40BIT_PROTECTED_BASE;
+
+	prot_end = prot_start + DRAM_PROTECTED_SIZE;
+
+	if (dst < dram_start || dst > dram_end - len) {
+		ERROR("BL2: dst address is on the protected area.\n");
+		result = IO_FAIL;
+		goto done;
+	}
+
+	/* load image is within SDRAM protected area */
+	if (dst >= prot_start && dst < prot_end) {
+		ERROR("BL2: dst address is on the protected area.\n");
+		result = IO_FAIL;
+	}
+
+	if (dst < prot_start && dst > prot_start - len) {
+		ERROR("BL2: loaded data is on the protected area.\n");
+		result = IO_FAIL;
+	}
+done:
+	if (result == IO_FAIL) {
+		ERROR("BL2: Out of range : dst=0x%lx len=0x%lx\n", dst, len);
+	}
+
+	return result;
+}
+
+static int32_t load_bl33x(void)
+{
+	static int32_t loaded = IO_NOT_SUPPORTED;
+	uintptr_t dst, partition, handle;
+	uint32_t noload, cert, len, i;
+	uintptr_t offset;
+	int32_t rc;
+	size_t cnt;
+	const int32_t img[] = {
+		BL33_IMAGE_ID,
+		BL332_IMAGE_ID,
+		BL333_IMAGE_ID,
+		BL334_IMAGE_ID,
+		BL335_IMAGE_ID,
+		BL336_IMAGE_ID,
+		BL337_IMAGE_ID,
+		BL338_IMAGE_ID
+	};
+
+	if (loaded != IO_NOT_SUPPORTED) {
+		return loaded;
+	}
+
+	for (i = 1; i < rcar_image_number; i++) {
+		rc = file_to_offset(img[i], &offset, &cert, &noload,
+				    &partition);
+		if (rc != IO_SUCCESS) {
+			WARN("%s: failed to get offset\n", __func__);
+			loaded = IO_FAIL;
+			return loaded;
+		}
+
+		rcar_read_certificate((uint64_t) cert, &len, &dst);
+		((io_drv_spec_t *) rcar_spec)->partition = partition;
+
+		rc = io_open(rcar_handle, rcar_spec, &handle);
+		if (rc != IO_SUCCESS) {
+			WARN("%s: Failed to open FIP (%i)\n", __func__, rc);
+			loaded = IO_FAIL;
+			return loaded;
+		}
+
+		rc = io_seek(handle, IO_SEEK_SET, offset);
+		if (rc != IO_SUCCESS) {
+			WARN("%s: failed to seek\n", __func__);
+			loaded = IO_FAIL;
+			return loaded;
+		}
+
+		rc = check_load_area(dst, len);
+		if (rc != IO_SUCCESS) {
+			WARN("%s: check load area\n", __func__);
+			loaded = IO_FAIL;
+			return loaded;
+		}
+
+		rc = io_read(handle, dst, len, &cnt);
+		if (rc != IO_SUCCESS) {
+			WARN("%s: failed to read\n", __func__);
+			loaded = IO_FAIL;
+			return loaded;
+		}
+#if TRUSTED_BOARD_BOOT
+		rc = auth_mod_verify_img(img[i], (void *)dst, len);
+		if (rc != 0) {
+			memset((void *)dst, 0x00, len);
+			loaded = IO_FAIL;
+			return loaded;
+		}
+#endif
+		io_close(handle);
+	}
+
+	loaded = IO_SUCCESS;
+
+	return loaded;
+}
+
+static int32_t rcar_dev_init(io_dev_info_t *dev_info, const uintptr_t name)
+{
+	static uint64_t header[64] __aligned(FLASH_TRANS_SIZE_UNIT) = {0UL};
+	uintptr_t handle;
+	ssize_t offset;
+	uint32_t i;
+	int32_t rc;
+	size_t cnt;
+
+	/* Obtain a reference to the image by querying the platform layer */
+	rc = plat_get_drv_source(name, &rcar_handle, &rcar_spec);
+	if (rc != IO_SUCCESS) {
+		WARN("Failed to obtain reference to img %ld (%i)\n", name, rc);
+		return IO_FAIL;
+	}
+
+	if (rcar_cert_load == RCAR_CERT_LOAD) {
+		return IO_SUCCESS;
+	}
+
+	rc = io_open(rcar_handle, rcar_spec, &handle);
+	if (rc != IO_SUCCESS) {
+		WARN("Failed to access img %ld (%i)\n", name, rc);
+		return IO_FAIL;
+	}
+
+	/*
+	 * get start address list
+	 * [0] address num
+	 * [1] BL33-1 image address
+	 * [2] BL33-2 image address
+	 * [3] BL33-3 image address
+	 * [4] BL33-4 image address
+	 * [5] BL33-5 image address
+	 * [6] BL33-6 image address
+	 * [7] BL33-7 image address
+	 * [8] BL33-8 image address
+	 */
+	offset = name == EMMC_DEV_ID ? RCAR_EMMC_CERT_HEADER :
+	    RCAR_FLASH_CERT_HEADER;
+	rc = io_seek(handle, IO_SEEK_SET, offset);
+	if (rc != IO_SUCCESS) {
+		WARN("Firmware Image Package header failed to seek\n");
+		goto error;
+	}
+
+	rc = io_read(handle, (uintptr_t) &header, sizeof(header), &cnt);
+	if (rc != IO_SUCCESS) {
+		WARN("Firmware Image Package header failed to read\n");
+		goto error;
+	}
+
+#if RCAR_BL2_DCACHE == 1
+	inv_dcache_range((uint64_t) header, sizeof(header));
+#endif
+
+	rcar_image_number = header[0];
+	for (i = 0; i < rcar_image_number + 2; i++) {
+		rcar_image_header[i] = header[i * 2 + 1];
+		rcar_image_header_prttn[i] = header[i * 2 + 2];
+	}
+
+	if (rcar_image_number == 0 || rcar_image_number > RCAR_MAX_BL3X_IMAGE) {
+		WARN("Firmware Image Package header check failed.\n");
+		rc = IO_FAIL;
+		goto error;
+	}
+
+	rc = io_seek(handle, IO_SEEK_SET, offset + RCAR_SECTOR6_CERT_OFFSET);
+	if (rc != IO_SUCCESS) {
+		WARN("Firmware Image Package header failed to seek cert\n");
+		goto error;
+	}
+
+	rc = io_read(handle, RCAR_SDRAM_certESS,
+		     RCAR_CERT_SIZE * (2 + rcar_image_number), &cnt);
+	if (rc != IO_SUCCESS) {
+		WARN("cert file read error.\n");
+		goto error;
+	}
+
+#if RCAR_BL2_DCACHE == 1
+	inv_dcache_range(RCAR_SDRAM_certESS,
+			 RCAR_CERT_SIZE * (2 + rcar_image_number));
+#endif
+
+	rcar_cert_load = RCAR_CERT_LOAD;
+error:
+
+	if (rc != IO_SUCCESS) {
+		rc = IO_FAIL;
+	}
+
+	io_close(handle);
+
+	return rc;
+
+}
+
+static int32_t rcar_file_open(io_dev_info_t *info, const uintptr_t file_spec,
+			      io_entity_t *entity)
+{
+	const io_drv_spec_t *spec = (io_drv_spec_t *) file_spec;
+	uintptr_t partition, offset, dst;
+	uint32_t noload, cert, len;
+	int32_t rc;
+
+	/*
+	 * Only one file open at a time. We need to  track state (ie, file
+	 * cursor position). Since the header lives at offset zero, this entry
+	 * should never be zero in an active file.
+	 * Once the system supports dynamic memory allocation we will allow more
+	 * than one open file at a time.
+	 */
+	if (current_file.offset != 0U) {
+		WARN("%s: Only one open file at a time.\n", __func__);
+		return IO_RESOURCES_EXHAUSTED;
+	}
+
+	rc = file_to_offset(spec->offset, &offset, &cert, &noload, &partition);
+	if (rc != IO_SUCCESS) {
+		WARN("Failed to open file name %ld (%i)\n", spec->offset, rc);
+		return IO_FAIL;
+	}
+
+	if (noload != 0U) {
+		current_file.offset = 1;
+		current_file.dst = 0;
+		current_file.size = 1;
+		current_file.position = 0;
+		current_file.no_load = noload;
+		current_file.partition = 0;
+		entity->info = (uintptr_t) &current_file;
+
+		return IO_SUCCESS;
+	}
+
+	rcar_read_certificate((uint64_t) cert, &len, &dst);
+
+	/* Baylibre: HACK */
+	if (spec->offset == BL31_IMAGE_ID && len < RCAR_TRUSTED_SRAM_SIZE) {
+		WARN("%s,%s\n", "r-car ignoring the BL31 size from certificate",
+		     "using RCAR_TRUSTED_SRAM_SIZE instead");
+		len = RCAR_TRUSTED_SRAM_SIZE;
+	}
+
+	current_file.partition = partition;
+	current_file.no_load = noload;
+	current_file.offset = offset;
+	current_file.position = 0;
+	current_file.size = len;
+	current_file.dst = dst;
+	entity->info = (uintptr_t) &current_file;
+
+	return IO_SUCCESS;
+}
+
+static int32_t rcar_file_len(io_entity_t *entity, size_t *length)
+{
+	*length = ((file_state_t *) entity->info)->size;
+
+	NOTICE("%s: len: 0x%08lx\n", __func__, *length);
+
+	return IO_SUCCESS;
+}
+
+static int32_t rcar_file_read(io_entity_t *entity, uintptr_t buffer,
+			      size_t length, size_t *cnt)
+{
+	file_state_t *fp = (file_state_t *) entity->info;
+	ssize_t offset = fp->offset + fp->position;
+	uintptr_t handle;
+	int32_t rc;
+
+#ifdef SPD_NONE
+	static uint32_t load_bl33x_counter = 1;
+#else
+	static uint32_t load_bl33x_counter;
+#endif
+	if (current_file.no_load != 0U) {
+		*cnt = length;
+		return IO_SUCCESS;
+	}
+
+	((io_drv_spec_t *) rcar_spec)->partition = fp->partition;
+
+	rc = io_open(rcar_handle, rcar_spec, &handle);
+	if (rc != IO_SUCCESS) {
+		WARN("Failed to open FIP (%i)\n", rc);
+		return IO_FAIL;
+	}
+
+	rc = io_seek(handle, IO_SEEK_SET, offset);
+	if (rc != IO_SUCCESS) {
+		WARN("%s: failed to seek\n", __func__);
+		goto error;
+	}
+
+	if (load_bl33x_counter == RCAR_COUNT_LOAD_BL33) {
+		rc = check_load_area(buffer, length);
+		if (rc != IO_SUCCESS) {
+			WARN("%s: load area err\n", __func__);
+			goto error;
+		}
+	}
+
+	rc = io_read(handle, buffer, length, cnt);
+	if (rc != IO_SUCCESS) {
+		WARN("Failed to read payload (%i)\n", rc);
+		goto error;
+	}
+
+	fp->position += *cnt;
+	io_close(handle);
+
+	load_bl33x_counter += 1;
+	if (load_bl33x_counter == RCAR_COUNT_LOAD_BL33X) {
+		return load_bl33x();
+	}
+
+	return IO_SUCCESS;
+error:
+	io_close(handle);
+	return IO_FAIL;
+}
+
+static int32_t rcar_file_close(io_entity_t *entity)
+{
+	if (current_file.offset != 0U) {
+		memset(&current_file, 0, sizeof(current_file));
+	}
+
+	entity->info = 0U;
+
+	return IO_SUCCESS;
+}
+
+static const io_dev_funcs_t rcar_dev_funcs = {
+	.type = &device_type_rcar,
+	.open = &rcar_file_open,
+	.seek = NULL,
+	.size = &rcar_file_len,
+	.read = &rcar_file_read,
+	.write = NULL,
+	.close = &rcar_file_close,
+	.dev_init = &rcar_dev_init,
+	.dev_close = &rcar_dev_close,
+};
+
+static const io_dev_info_t rcar_dev_info = {
+	.funcs = &rcar_dev_funcs,
+	.info = (uintptr_t) 0
+};
+
+static const io_dev_connector_t rcar_dev_connector = {
+	.dev_open = &rcar_dev_open
+};
+
+static int32_t rcar_dev_open(const uintptr_t dev_spec __attribute__ ((unused)),
+			     io_dev_info_t **dev_info)
+{
+	*dev_info = (io_dev_info_t *) &rcar_dev_info;
+
+	return IO_SUCCESS;
+}
+
+static int32_t rcar_dev_close(io_dev_info_t *dev_info)
+{
+	rcar_handle = 0;
+	rcar_spec = 0;
+
+	return IO_SUCCESS;
+}
+
+int32_t rcar_register_io_dev(const io_dev_connector_t **dev_con)
+{
+	int32_t result;
+
+	result = io_register_device(&rcar_dev_info);
+	if (result == IO_SUCCESS) {
+		*dev_con = &rcar_dev_connector;
+	}
+
+	return result;
+}
diff --git a/drivers/renesas/common/io/io_rcar.h b/drivers/renesas/common/io/io_rcar.h
new file mode 100644
index 0000000..c26a617
--- /dev/null
+++ b/drivers/renesas/common/io/io_rcar.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IO_RCAR_H
+#define IO_RCAR_H
+
+int32_t rcar_register_io_dev(const io_dev_connector_t **dev_con);
+int32_t rcar_get_certificate(const int32_t name, uint32_t *cert);
+void rcar_read_certificate(uint64_t cert, uint32_t *size, uintptr_t *dest);
+
+#endif /* IO_RCAR_H */
diff --git a/drivers/renesas/common/pfc_regs.h b/drivers/renesas/common/pfc_regs.h
new file mode 100644
index 0000000..4187733
--- /dev/null
+++ b/drivers/renesas/common/pfc_regs.h
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef PFC_REGS_H
+#define PFC_REGS_H
+
+/* GPIO base address */
+#define GPIO_BASE		(0xE6050000U)
+
+/* GPIO registers */
+#define GPIO_IOINTSEL0		(GPIO_BASE + 0x0000U)
+#define GPIO_INOUTSEL0		(GPIO_BASE + 0x0004U)
+#define GPIO_OUTDT0		(GPIO_BASE + 0x0008U)
+#define GPIO_INDT0		(GPIO_BASE + 0x000CU)
+#define GPIO_INTDT0		(GPIO_BASE + 0x0010U)
+#define GPIO_INTCLR0		(GPIO_BASE + 0x0014U)
+#define GPIO_INTMSK0		(GPIO_BASE + 0x0018U)
+#define GPIO_MSKCLR0		(GPIO_BASE + 0x001CU)
+#define GPIO_POSNEG0		(GPIO_BASE + 0x0020U)
+#define GPIO_EDGLEVEL0		(GPIO_BASE + 0x0024U)
+#define GPIO_FILONOFF0		(GPIO_BASE + 0x0028U)
+#define GPIO_INTMSKS0		(GPIO_BASE + 0x0038U)
+#define GPIO_MSKCLRS0		(GPIO_BASE + 0x003CU)
+#define GPIO_OUTDTSEL0		(GPIO_BASE + 0x0040U)
+#define GPIO_OUTDTH0		(GPIO_BASE + 0x0044U)
+#define GPIO_OUTDTL0		(GPIO_BASE + 0x0048U)
+#define GPIO_BOTHEDGE0		(GPIO_BASE + 0x004CU)
+#define GPIO_IOINTSEL1		(GPIO_BASE + 0x1000U)
+#define GPIO_INOUTSEL1		(GPIO_BASE + 0x1004U)
+#define GPIO_OUTDT1		(GPIO_BASE + 0x1008U)
+#define GPIO_INDT1		(GPIO_BASE + 0x100CU)
+#define GPIO_INTDT1		(GPIO_BASE + 0x1010U)
+#define GPIO_INTCLR1		(GPIO_BASE + 0x1014U)
+#define GPIO_INTMSK1		(GPIO_BASE + 0x1018U)
+#define GPIO_MSKCLR1		(GPIO_BASE + 0x101CU)
+#define GPIO_POSNEG1		(GPIO_BASE + 0x1020U)
+#define GPIO_EDGLEVEL1		(GPIO_BASE + 0x1024U)
+#define GPIO_FILONOFF1		(GPIO_BASE + 0x1028U)
+#define GPIO_INTMSKS1		(GPIO_BASE + 0x1038U)
+#define GPIO_MSKCLRS1		(GPIO_BASE + 0x103CU)
+#define GPIO_OUTDTSEL1		(GPIO_BASE + 0x1040U)
+#define GPIO_OUTDTH1		(GPIO_BASE + 0x1044U)
+#define GPIO_OUTDTL1		(GPIO_BASE + 0x1048U)
+#define GPIO_BOTHEDGE1		(GPIO_BASE + 0x104CU)
+#define GPIO_IOINTSEL2		(GPIO_BASE + 0x2000U)
+#define GPIO_INOUTSEL2		(GPIO_BASE + 0x2004U)
+#define GPIO_OUTDT2		(GPIO_BASE + 0x2008U)
+#define GPIO_INDT2		(GPIO_BASE + 0x200CU)
+#define GPIO_INTDT2		(GPIO_BASE + 0x2010U)
+#define GPIO_INTCLR2		(GPIO_BASE + 0x2014U)
+#define GPIO_INTMSK2		(GPIO_BASE + 0x2018U)
+#define GPIO_MSKCLR2		(GPIO_BASE + 0x201CU)
+#define GPIO_POSNEG2		(GPIO_BASE + 0x2020U)
+#define GPIO_EDGLEVEL2		(GPIO_BASE + 0x2024U)
+#define GPIO_FILONOFF2		(GPIO_BASE + 0x2028U)
+#define GPIO_INTMSKS2		(GPIO_BASE + 0x2038U)
+#define GPIO_MSKCLRS2		(GPIO_BASE + 0x203CU)
+#define GPIO_OUTDTSEL2		(GPIO_BASE + 0x2040U)
+#define GPIO_OUTDTH2		(GPIO_BASE + 0x2044U)
+#define GPIO_OUTDTL2		(GPIO_BASE + 0x2048U)
+#define GPIO_BOTHEDGE2		(GPIO_BASE + 0x204CU)
+#define GPIO_IOINTSEL3		(GPIO_BASE + 0x3000U)
+#define GPIO_INOUTSEL3		(GPIO_BASE + 0x3004U)
+#define GPIO_OUTDT3		(GPIO_BASE + 0x3008U)
+#define GPIO_INDT3		(GPIO_BASE + 0x300CU)
+#define GPIO_INTDT3		(GPIO_BASE + 0x3010U)
+#define GPIO_INTCLR3		(GPIO_BASE + 0x3014U)
+#define GPIO_INTMSK3		(GPIO_BASE + 0x3018U)
+#define GPIO_MSKCLR3		(GPIO_BASE + 0x301CU)
+#define GPIO_POSNEG3		(GPIO_BASE + 0x3020U)
+#define GPIO_EDGLEVEL3		(GPIO_BASE + 0x3024U)
+#define GPIO_FILONOFF3		(GPIO_BASE + 0x3028U)
+#define GPIO_INTMSKS3		(GPIO_BASE + 0x3038U)
+#define GPIO_MSKCLRS3		(GPIO_BASE + 0x303CU)
+#define GPIO_OUTDTSEL3		(GPIO_BASE + 0x3040U)
+#define GPIO_OUTDTH3		(GPIO_BASE + 0x3044U)
+#define GPIO_OUTDTL3		(GPIO_BASE + 0x3048U)
+#define GPIO_BOTHEDGE3		(GPIO_BASE + 0x304CU)
+#define GPIO_IOINTSEL4		(GPIO_BASE + 0x4000U)
+#define GPIO_INOUTSEL4		(GPIO_BASE + 0x4004U)
+#define GPIO_OUTDT4		(GPIO_BASE + 0x4008U)
+#define GPIO_INDT4		(GPIO_BASE + 0x400CU)
+#define GPIO_INTDT4		(GPIO_BASE + 0x4010U)
+#define GPIO_INTCLR4		(GPIO_BASE + 0x4014U)
+#define GPIO_INTMSK4		(GPIO_BASE + 0x4018U)
+#define GPIO_MSKCLR4		(GPIO_BASE + 0x401CU)
+#define GPIO_POSNEG4		(GPIO_BASE + 0x4020U)
+#define GPIO_EDGLEVEL4		(GPIO_BASE + 0x4024U)
+#define GPIO_FILONOFF4		(GPIO_BASE + 0x4028U)
+#define GPIO_INTMSKS4		(GPIO_BASE + 0x4038U)
+#define GPIO_MSKCLRS4		(GPIO_BASE + 0x403CU)
+#define GPIO_OUTDTSEL4		(GPIO_BASE + 0x4040U)
+#define GPIO_OUTDTH4		(GPIO_BASE + 0x4044U)
+#define GPIO_OUTDTL4		(GPIO_BASE + 0x4048U)
+#define GPIO_BOTHEDGE4		(GPIO_BASE + 0x404CU)
+#define GPIO_IOINTSEL5		(GPIO_BASE + 0x5000U)
+#define GPIO_INOUTSEL5		(GPIO_BASE + 0x5004U)
+#define GPIO_OUTDT5		(GPIO_BASE + 0x5008U)
+#define GPIO_INDT5		(GPIO_BASE + 0x500CU)
+#define GPIO_INTDT5		(GPIO_BASE + 0x5010U)
+#define GPIO_INTCLR5		(GPIO_BASE + 0x5014U)
+#define GPIO_INTMSK5		(GPIO_BASE + 0x5018U)
+#define GPIO_MSKCLR5		(GPIO_BASE + 0x501CU)
+#define GPIO_POSNEG5		(GPIO_BASE + 0x5020U)
+#define GPIO_EDGLEVEL5		(GPIO_BASE + 0x5024U)
+#define GPIO_FILONOFF5		(GPIO_BASE + 0x5028U)
+#define GPIO_INTMSKS5		(GPIO_BASE + 0x5038U)
+#define GPIO_MSKCLRS5		(GPIO_BASE + 0x503CU)
+#define GPIO_OUTDTSEL5		(GPIO_BASE + 0x5040U)
+#define GPIO_OUTDTH5		(GPIO_BASE + 0x5044U)
+#define GPIO_OUTDTL5		(GPIO_BASE + 0x5048U)
+#define GPIO_BOTHEDGE5		(GPIO_BASE + 0x504CU)
+#define GPIO_IOINTSEL6		(GPIO_BASE + 0x5400U)
+#define GPIO_INOUTSEL6		(GPIO_BASE + 0x5404U)
+#define GPIO_OUTDT6		(GPIO_BASE + 0x5408U)
+#define GPIO_INTDT6		(GPIO_BASE + 0x5410U)
+#define GPIO_INTCLR6		(GPIO_BASE + 0x5414U)
+#define GPIO_INTMSK6		(GPIO_BASE + 0x5418U)
+#define GPIO_MSKCLR6		(GPIO_BASE + 0x541CU)
+#define GPIO_POSNEG6		(GPIO_BASE + 0x5420U)
+#define GPIO_EDGLEVEL6		(GPIO_BASE + 0x5424U)
+#define GPIO_FILONOFF6		(GPIO_BASE + 0x5428U)
+#define GPIO_INTMSKS6		(GPIO_BASE + 0x5438U)
+#define GPIO_MSKCLRS6		(GPIO_BASE + 0x543CU)
+#define GPIO_OUTDTSEL6		(GPIO_BASE + 0x5440U)
+#define GPIO_OUTDTH6		(GPIO_BASE + 0x5444U)
+#define GPIO_OUTDTL6		(GPIO_BASE + 0x5448U)
+#define GPIO_BOTHEDGE6		(GPIO_BASE + 0x544CU)
+#define GPIO_IOINTSEL7		(GPIO_BASE + 0x5800U)
+#define GPIO_INOUTSEL7		(GPIO_BASE + 0x5804U)
+#define GPIO_OUTDT7		(GPIO_BASE + 0x5808U)
+#define GPIO_INDT7		(GPIO_BASE + 0x580CU)
+#define GPIO_INTDT7		(GPIO_BASE + 0x5810U)
+#define GPIO_INTCLR7		(GPIO_BASE + 0x5814U)
+#define GPIO_INTMSK7		(GPIO_BASE + 0x5818U)
+#define GPIO_MSKCLR7		(GPIO_BASE + 0x581CU)
+#define GPIO_POSNEG7		(GPIO_BASE + 0x5820U)
+#define GPIO_EDGLEVEL7		(GPIO_BASE + 0x5824U)
+#define GPIO_FILONOFF7		(GPIO_BASE + 0x5828U)
+#define GPIO_INTMSKS7		(GPIO_BASE + 0x5838U)
+#define GPIO_MSKCLRS7		(GPIO_BASE + 0x583CU)
+#define GPIO_OUTDTSEL7		(GPIO_BASE + 0x5840U)
+#define GPIO_OUTDTH7		(GPIO_BASE + 0x5844U)
+#define GPIO_OUTDTL7		(GPIO_BASE + 0x5848U)
+#define GPIO_BOTHEDGE7		(GPIO_BASE + 0x584CU)
+
+/* Pin functon base address */
+#define PFC_BASE		(0xE6060000U)
+
+/* Pin functon registers */
+#define PFC_PMMR		(PFC_BASE + 0x0000U)
+#define PFC_GPSR0		(PFC_BASE + 0x0100U)
+#define PFC_GPSR1		(PFC_BASE + 0x0104U)
+#define PFC_GPSR2		(PFC_BASE + 0x0108U)
+#define PFC_GPSR3		(PFC_BASE + 0x010CU)
+#define PFC_GPSR4		(PFC_BASE + 0x0110U)
+#define PFC_GPSR5		(PFC_BASE + 0x0114U)
+#define PFC_GPSR6		(PFC_BASE + 0x0118U)
+#define PFC_GPSR7		(PFC_BASE + 0x011CU)
+#define PFC_IPSR0		(PFC_BASE + 0x0200U)
+#define PFC_IPSR1		(PFC_BASE + 0x0204U)
+#define PFC_IPSR2		(PFC_BASE + 0x0208U)
+#define PFC_IPSR3		(PFC_BASE + 0x020CU)
+#define PFC_IPSR4		(PFC_BASE + 0x0210U)
+#define PFC_IPSR5		(PFC_BASE + 0x0214U)
+#define PFC_IPSR6		(PFC_BASE + 0x0218U)
+#define PFC_IPSR7		(PFC_BASE + 0x021CU)
+#define PFC_IPSR8		(PFC_BASE + 0x0220U)
+#define PFC_IPSR9		(PFC_BASE + 0x0224U)
+#define PFC_IPSR10		(PFC_BASE + 0x0228U)
+#define PFC_IPSR11		(PFC_BASE + 0x022CU)
+#define PFC_IPSR12		(PFC_BASE + 0x0230U)
+#define PFC_IPSR13		(PFC_BASE + 0x0234U)
+#define PFC_IPSR14		(PFC_BASE + 0x0238U)
+#define PFC_IPSR15		(PFC_BASE + 0x023CU)
+#define PFC_IPSR16		(PFC_BASE + 0x0240U)
+#define PFC_IPSR17		(PFC_BASE + 0x0244U)
+#define PFC_IPSR18		(PFC_BASE + 0x0248U)
+#define PFC_DRVCTRL0		(PFC_BASE + 0x0300U)
+#define PFC_DRVCTRL1		(PFC_BASE + 0x0304U)
+#define PFC_DRVCTRL2		(PFC_BASE + 0x0308U)
+#define PFC_DRVCTRL3		(PFC_BASE + 0x030CU)
+#define PFC_DRVCTRL4		(PFC_BASE + 0x0310U)
+#define PFC_DRVCTRL5		(PFC_BASE + 0x0314U)
+#define PFC_DRVCTRL6		(PFC_BASE + 0x0318U)
+#define PFC_DRVCTRL7		(PFC_BASE + 0x031CU)
+#define PFC_DRVCTRL8		(PFC_BASE + 0x0320U)
+#define PFC_DRVCTRL9		(PFC_BASE + 0x0324U)
+#define PFC_DRVCTRL10		(PFC_BASE + 0x0328U)
+#define PFC_DRVCTRL11		(PFC_BASE + 0x032CU)
+#define PFC_DRVCTRL12		(PFC_BASE + 0x0330U)
+#define PFC_DRVCTRL13		(PFC_BASE + 0x0334U)
+#define PFC_DRVCTRL14		(PFC_BASE + 0x0338U)
+#define PFC_DRVCTRL15		(PFC_BASE + 0x033CU)
+#define PFC_DRVCTRL16		(PFC_BASE + 0x0340U)
+#define PFC_DRVCTRL17		(PFC_BASE + 0x0344U)
+#define PFC_DRVCTRL18		(PFC_BASE + 0x0348U)
+#define PFC_DRVCTRL19		(PFC_BASE + 0x034CU)
+#define PFC_DRVCTRL20		(PFC_BASE + 0x0350U)
+#define PFC_DRVCTRL21		(PFC_BASE + 0x0354U)
+#define PFC_DRVCTRL22		(PFC_BASE + 0x0358U)
+#define PFC_DRVCTRL23		(PFC_BASE + 0x035CU)
+#define PFC_DRVCTRL24		(PFC_BASE + 0x0360U)
+#define PFC_POCCTRL0		(PFC_BASE + 0x0380U)
+#define PFC_IOCTRL31		(PFC_BASE + 0x0384U)
+#define PFC_POCCTRL2		(PFC_BASE + 0x0388U)
+#define PFC_TDSELCTRL0		(PFC_BASE + 0x03C0U)
+#define PFC_IOCTRL		(PFC_BASE + 0x03E0U)
+#define PFC_TSREG		(PFC_BASE + 0x03E4U)
+#define PFC_PUEN0		(PFC_BASE + 0x0400U)
+#define PFC_PUEN1		(PFC_BASE + 0x0404U)
+#define PFC_PUEN2		(PFC_BASE + 0x0408U)
+#define PFC_PUEN3		(PFC_BASE + 0x040CU)
+#define PFC_PUEN4		(PFC_BASE + 0x0410U)
+#define PFC_PUEN5		(PFC_BASE + 0x0414U)
+#define PFC_PUEN6		(PFC_BASE + 0x0418U)
+#define PFC_PUD0		(PFC_BASE + 0x0440U)
+#define PFC_PUD1		(PFC_BASE + 0x0444U)
+#define PFC_PUD2		(PFC_BASE + 0x0448U)
+#define PFC_PUD3		(PFC_BASE + 0x044CU)
+#define PFC_PUD4		(PFC_BASE + 0x0450U)
+#define PFC_PUD5		(PFC_BASE + 0x0454U)
+#define PFC_PUD6		(PFC_BASE + 0x0458U)
+#define PFC_MOD_SEL0		(PFC_BASE + 0x0500U)
+#define PFC_MOD_SEL1		(PFC_BASE + 0x0504U)
+#define PFC_MOD_SEL2		(PFC_BASE + 0x0508U)
+
+#endif /* PFC_REGS_H */
diff --git a/drivers/renesas/common/pwrc/call_sram.S b/drivers/renesas/common/pwrc/call_sram.S
new file mode 100644
index 0000000..aa8644c
--- /dev/null
+++ b/drivers/renesas/common/pwrc/call_sram.S
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+
+.global rcar_pwrc_switch_stack
+
+/*
+ * x0 : jump address,
+ * x1 : stack address,
+ * x2 : arg,
+ * x3 : stack address (temporary)
+ */
+func rcar_pwrc_switch_stack
+
+	/* lr to stack */
+	stp	x29, x30, [sp,#-16]
+
+	/* change stack pointer */
+	mov	x3, sp
+	mov	sp, x1
+
+	/* save stack pointer */
+	sub	sp, sp, #16
+	stp	x0, x3, [sp]
+
+	/* data synchronization barrier */
+	dsb	sy
+
+	/* jump to code */
+	mov	x1, x0
+	mov	x0, x2
+	blr	x1
+
+	/* load stack pointer */
+	ldp 	x0, x2, [sp,#0]
+
+	/* change stack pointer */
+	mov	sp, x2
+
+	/* return */
+	ldp	x29, x30, [sp,#-16]
+	ret
+endfunc rcar_pwrc_switch_stack
diff --git a/drivers/renesas/common/pwrc/pwrc.c b/drivers/renesas/common/pwrc/pwrc.c
new file mode 100644
index 0000000..b60ccab
--- /dev/null
+++ b/drivers/renesas/common/pwrc/pwrc.c
@@ -0,0 +1,917 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/bakery_lock.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <plat/common/platform.h>
+
+#include "iic_dvfs.h"
+#include "micro_delay.h"
+#include "pwrc.h"
+#include "rcar_def.h"
+#include "rcar_private.h"
+#include "cpg_registers.h"
+
+/*
+ * Someday there will be a generic power controller api. At the moment each
+ * platform has its own pwrc so just exporting functions should be acceptable.
+ */
+RCAR_INSTANTIATE_LOCK
+
+#define WUP_IRQ_SHIFT				(0U)
+#define WUP_FIQ_SHIFT				(8U)
+#define WUP_CSD_SHIFT				(16U)
+#define BIT_SOFTRESET				(1U << 15)
+#define BIT_CA53_SCU				(1U << 21)
+#define BIT_CA57_SCU				(1U << 12)
+#define REQ_RESUME				(1U << 1)
+#define REQ_OFF					(1U << 0)
+#define STATUS_PWRUP				(1U << 4)
+#define STATUS_PWRDOWN				(1U << 0)
+#define STATE_CA57_CPU				(27U)
+#define STATE_CA53_CPU				(22U)
+#define MODE_L2_DOWN				(0x00000002U)
+#define CPU_PWR_OFF				(0x00000003U)
+#define RCAR_PSTR_MASK				(0x00000003U)
+#define ST_ALL_STANDBY				(0x00003333U)
+#define SYSCEXTMASK_EXTMSK0			(0x00000001U)
+/* Suspend to ram	*/
+#define DBSC4_REG_BASE				(0xE6790000U)
+#define DBSC4_REG_DBSYSCNT0			(DBSC4_REG_BASE + 0x0100U)
+#define DBSC4_REG_DBACEN			(DBSC4_REG_BASE + 0x0200U)
+#define DBSC4_REG_DBCMD				(DBSC4_REG_BASE + 0x0208U)
+#define DBSC4_REG_DBRFEN			(DBSC4_REG_BASE + 0x0204U)
+#define DBSC4_REG_DBWAIT			(DBSC4_REG_BASE + 0x0210U)
+#define DBSC4_REG_DBCALCNF			(DBSC4_REG_BASE + 0x0424U)
+#define DBSC4_REG_DBDFIPMSTRCNF			(DBSC4_REG_BASE + 0x0520U)
+#define DBSC4_REG_DBPDLK0			(DBSC4_REG_BASE + 0x0620U)
+#define DBSC4_REG_DBPDRGA0			(DBSC4_REG_BASE + 0x0624U)
+#define DBSC4_REG_DBPDRGD0			(DBSC4_REG_BASE + 0x0628U)
+#define DBSC4_REG_DBCAM0CTRL0			(DBSC4_REG_BASE + 0x0940U)
+#define DBSC4_REG_DBCAM0STAT0			(DBSC4_REG_BASE + 0x0980U)
+#define DBSC4_REG_DBCAM1STAT0			(DBSC4_REG_BASE + 0x0990U)
+#define DBSC4_REG_DBCAM2STAT0			(DBSC4_REG_BASE + 0x09A0U)
+#define DBSC4_REG_DBCAM3STAT0			(DBSC4_REG_BASE + 0x09B0U)
+#define DBSC4_BIT_DBACEN_ACCEN			((uint32_t)(1U << 0))
+#define DBSC4_BIT_DBRFEN_ARFEN			((uint32_t)(1U << 0))
+#define DBSC4_BIT_DBCAMxSTAT0			(0x00000001U)
+#define DBSC4_BIT_DBDFIPMSTRCNF_PMSTREN		(0x00000001U)
+#define DBSC4_SET_DBCMD_OPC_PRE			(0x04000000U)
+#define DBSC4_SET_DBCMD_OPC_SR			(0x0A000000U)
+#define DBSC4_SET_DBCMD_OPC_PD			(0x08000000U)
+#define DBSC4_SET_DBCMD_OPC_MRW			(0x0E000000U)
+#define DBSC4_SET_DBCMD_CH_ALL			(0x00800000U)
+#define DBSC4_SET_DBCMD_RANK_ALL		(0x00040000U)
+#define DBSC4_SET_DBCMD_ARG_ALL			(0x00000010U)
+#define DBSC4_SET_DBCMD_ARG_ENTER		(0x00000000U)
+#define DBSC4_SET_DBCMD_ARG_MRW_ODTC		(0x00000B00U)
+#define DBSC4_SET_DBSYSCNT0_WRITE_ENABLE	(0x00001234U)
+#define DBSC4_SET_DBSYSCNT0_WRITE_DISABLE	(0x00000000U)
+#define DBSC4_SET_DBPDLK0_PHY_ACCESS		(0x0000A55AU)
+#define DBSC4_SET_DBPDRGA0_ACIOCR0		(0x0000001AU)
+#define DBSC4_SET_DBPDRGD0_ACIOCR0		(0x33C03C11U)
+#define DBSC4_SET_DBPDRGA0_DXCCR		(0x00000020U)
+#define DBSC4_SET_DBPDRGD0_DXCCR		(0x00181006U)
+#define DBSC4_SET_DBPDRGA0_PGCR1		(0x00000003U)
+#define DBSC4_SET_DBPDRGD0_PGCR1		(0x0380C600U)
+#define DBSC4_SET_DBPDRGA0_ACIOCR1		(0x0000001BU)
+#define DBSC4_SET_DBPDRGD0_ACIOCR1		(0xAAAAAAAAU)
+#define DBSC4_SET_DBPDRGA0_ACIOCR3		(0x0000001DU)
+#define DBSC4_SET_DBPDRGD0_ACIOCR3		(0xAAAAAAAAU)
+#define DBSC4_SET_DBPDRGA0_ACIOCR5		(0x0000001FU)
+#define DBSC4_SET_DBPDRGD0_ACIOCR5		(0x000000AAU)
+#define DBSC4_SET_DBPDRGA0_DX0GCR2		(0x000000A2U)
+#define DBSC4_SET_DBPDRGD0_DX0GCR2		(0xAAAA0000U)
+#define DBSC4_SET_DBPDRGA0_DX1GCR2		(0x000000C2U)
+#define DBSC4_SET_DBPDRGD0_DX1GCR2		(0xAAAA0000U)
+#define DBSC4_SET_DBPDRGA0_DX2GCR2		(0x000000E2U)
+#define DBSC4_SET_DBPDRGD0_DX2GCR2		(0xAAAA0000U)
+#define DBSC4_SET_DBPDRGA0_DX3GCR2		(0x00000102U)
+#define DBSC4_SET_DBPDRGD0_DX3GCR2		(0xAAAA0000U)
+#define DBSC4_SET_DBPDRGA0_ZQCR			(0x00000090U)
+#define DBSC4_SET_DBPDRGD0_ZQCR_MD19_0		(0x04058904U)
+#define DBSC4_SET_DBPDRGD0_ZQCR_MD19_1		(0x04058A04U)
+#define DBSC4_SET_DBPDRGA0_DX0GCR0		(0x000000A0U)
+#define DBSC4_SET_DBPDRGD0_DX0GCR0		(0x7C0002E5U)
+#define DBSC4_SET_DBPDRGA0_DX1GCR0		(0x000000C0U)
+#define DBSC4_SET_DBPDRGD0_DX1GCR0		(0x7C0002E5U)
+#define DBSC4_SET_DBPDRGA0_DX2GCR0		(0x000000E0U)
+#define DBSC4_SET_DBPDRGD0_DX2GCR0		(0x7C0002E5U)
+#define DBSC4_SET_DBPDRGA0_DX3GCR0		(0x00000100U)
+#define DBSC4_SET_DBPDRGD0_DX3GCR0		(0x7C0002E5U)
+#define DBSC4_SET_DBPDRGA0_DX0GCR1		(0x000000A1U)
+#define DBSC4_SET_DBPDRGD0_DX0GCR1		(0x55550000U)
+#define DBSC4_SET_DBPDRGA0_DX1GCR1		(0x000000C1U)
+#define DBSC4_SET_DBPDRGD0_DX1GCR1		(0x55550000U)
+#define DBSC4_SET_DBPDRGA0_DX2GCR1		(0x000000E1U)
+#define DBSC4_SET_DBPDRGD0_DX2GCR1		(0x55550000U)
+#define DBSC4_SET_DBPDRGA0_DX3GCR1		(0x00000101U)
+#define DBSC4_SET_DBPDRGD0_DX3GCR1		(0x55550000U)
+#define DBSC4_SET_DBPDRGA0_DX0GCR3		(0x000000A3U)
+#define DBSC4_SET_DBPDRGD0_DX0GCR3		(0x00008484U)
+#define DBSC4_SET_DBPDRGA0_DX1GCR3		(0x000000C3U)
+#define DBSC4_SET_DBPDRGD0_DX1GCR3		(0x00008484U)
+#define DBSC4_SET_DBPDRGA0_DX2GCR3		(0x000000E3U)
+#define DBSC4_SET_DBPDRGD0_DX2GCR3		(0x00008484U)
+#define DBSC4_SET_DBPDRGA0_DX3GCR3		(0x00000103U)
+#define DBSC4_SET_DBPDRGD0_DX3GCR3		(0x00008484U)
+#define RST_BASE				(0xE6160000U)
+#define RST_MODEMR				(RST_BASE + 0x0060U)
+#define RST_MODEMR_BIT0				(0x00000001U)
+
+#define RCAR_CNTCR_OFF				(0x00U)
+#define RCAR_CNTCVL_OFF				(0x08U)
+#define RCAR_CNTCVU_OFF				(0x0CU)
+#define RCAR_CNTFID_OFF				(0x20U)
+
+#define RCAR_CNTCR_EN				((uint32_t)1U << 0U)
+#define RCAR_CNTCR_FCREQ(x)			((uint32_t)(x) << 8U)
+
+#if PMIC_ROHM_BD9571
+#define BIT_BKUP_CTRL_OUT			((uint8_t)(1U << 4))
+#define PMIC_BKUP_MODE_CNT			(0x20U)
+#define PMIC_QLLM_CNT				(0x27U)
+#define PMIC_RETRY_MAX				(100U)
+#endif /* PMIC_ROHM_BD9571 */
+#define SCTLR_EL3_M_BIT				((uint32_t)1U << 0)
+#define RCAR_CA53CPU_NUM_MAX			(4U)
+#define RCAR_CA57CPU_NUM_MAX			(4U)
+#define IS_A53A57(c)	((c) == RCAR_CLUSTER_A53A57)
+#define IS_CA57(c)	((c) == RCAR_CLUSTER_CA57)
+#define IS_CA53(c)	((c) == RCAR_CLUSTER_CA53)
+
+#ifndef __ASSEMBLER__
+IMPORT_SYM(unsigned long, __system_ram_start__, SYSTEM_RAM_START);
+IMPORT_SYM(unsigned long, __system_ram_end__, SYSTEM_RAM_END);
+IMPORT_SYM(unsigned long, __SRAM_COPY_START__, SRAM_COPY_START);
+#endif
+
+uint32_t rcar_pwrc_status(u_register_t mpidr)
+{
+	uint32_t ret = 0;
+	uint64_t cm, cpu;
+	uint32_t reg;
+	uint32_t c;
+
+	rcar_lock_get();
+
+	c = rcar_pwrc_get_cluster();
+	cm = mpidr & MPIDR_CLUSTER_MASK;
+
+	if (!IS_A53A57(c) && cm != 0) {
+		ret = RCAR_INVALID;
+		goto done;
+	}
+
+	reg = mmio_read_32(RCAR_PRR);
+	cpu = mpidr & MPIDR_CPU_MASK;
+
+	if (IS_CA53(c))
+		if (reg & (1 << (STATE_CA53_CPU + cpu)))
+			ret = RCAR_INVALID;
+	if (IS_CA57(c))
+		if (reg & (1 << (STATE_CA57_CPU + cpu)))
+			ret = RCAR_INVALID;
+done:
+	rcar_lock_release();
+
+	return ret;
+}
+
+static void scu_power_up(u_register_t mpidr)
+{
+	uintptr_t reg_pwrsr, reg_cpumcr, reg_pwron, reg_pwrer;
+	uint32_t c, sysc_reg_bit;
+	uint32_t lsi_product;
+	uint32_t lsi_cut;
+
+	c = rcar_pwrc_get_mpidr_cluster(mpidr);
+	reg_cpumcr = IS_CA57(c) ? RCAR_CA57CPUCMCR : RCAR_CA53CPUCMCR;
+	sysc_reg_bit = IS_CA57(c) ? BIT_CA57_SCU : BIT_CA53_SCU;
+	reg_pwron = IS_CA57(c) ? RCAR_PWRONCR5 : RCAR_PWRONCR3;
+	reg_pwrer = IS_CA57(c) ? RCAR_PWRER5 : RCAR_PWRER3;
+	reg_pwrsr = IS_CA57(c) ? RCAR_PWRSR5 : RCAR_PWRSR3;
+
+	if ((mmio_read_32(reg_pwrsr) & STATUS_PWRDOWN) == 0)
+		return;
+
+	if (mmio_read_32(reg_cpumcr) != 0)
+		mmio_write_32(reg_cpumcr, 0);
+
+	lsi_product = mmio_read_32((uintptr_t)RCAR_PRR);
+	lsi_cut = lsi_product & PRR_CUT_MASK;
+	lsi_product &= PRR_PRODUCT_MASK;
+
+	if ((lsi_product == PRR_PRODUCT_M3 && lsi_cut >= PRR_PRODUCT_30) ||
+	    lsi_product == PRR_PRODUCT_H3 ||
+	    lsi_product == PRR_PRODUCT_M3N ||
+	    lsi_product == PRR_PRODUCT_E3) {
+		mmio_setbits_32(RCAR_SYSCEXTMASK, SYSCEXTMASK_EXTMSK0);
+	}
+
+	mmio_setbits_32(RCAR_SYSCIER, sysc_reg_bit);
+	mmio_setbits_32(RCAR_SYSCIMR, sysc_reg_bit);
+
+	do {
+		while ((mmio_read_32(RCAR_SYSCSR) & REQ_RESUME) == 0)
+			;
+		mmio_write_32(reg_pwron, 1);
+	} while (mmio_read_32(reg_pwrer) & 1);
+
+	while ((mmio_read_32(RCAR_SYSCISR) & sysc_reg_bit) == 0)
+		;
+	mmio_write_32(RCAR_SYSCISCR, sysc_reg_bit);
+
+	if ((lsi_product == PRR_PRODUCT_M3 && lsi_cut >= PRR_PRODUCT_30) ||
+	    lsi_product == PRR_PRODUCT_H3 ||
+	    lsi_product == PRR_PRODUCT_M3N ||
+	    lsi_product == PRR_PRODUCT_E3) {
+		mmio_clrbits_32(RCAR_SYSCEXTMASK, SYSCEXTMASK_EXTMSK0);
+	}
+
+	while ((mmio_read_32(reg_pwrsr) & STATUS_PWRUP) == 0)
+		;
+}
+
+void rcar_pwrc_cpuon(u_register_t mpidr)
+{
+	uint32_t res_data, on_data;
+	uintptr_t res_reg, on_reg;
+	uint32_t limit, c;
+	uint64_t cpu;
+
+	rcar_lock_get();
+
+	c = rcar_pwrc_get_mpidr_cluster(mpidr);
+	res_reg = IS_CA53(c) ? RCAR_CA53RESCNT : RCAR_CA57RESCNT;
+	on_reg = IS_CA53(c) ? RCAR_CA53WUPCR : RCAR_CA57WUPCR;
+	limit = IS_CA53(c) ? 0x5A5A0000 : 0xA5A50000;
+
+	res_data = mmio_read_32(res_reg) | limit;
+	scu_power_up(mpidr);
+	cpu = mpidr & MPIDR_CPU_MASK;
+	on_data = 1 << cpu;
+	mmio_write_32(CPG_CPGWPR, ~on_data);
+	mmio_write_32(on_reg, on_data);
+	mmio_write_32(res_reg, res_data & (~(1 << (3 - cpu))));
+
+	rcar_lock_release();
+}
+
+void rcar_pwrc_cpuoff(u_register_t mpidr)
+{
+	uint32_t c;
+	uintptr_t reg;
+	uint64_t cpu;
+
+	rcar_lock_get();
+
+	cpu = mpidr & MPIDR_CPU_MASK;
+	c = rcar_pwrc_get_mpidr_cluster(mpidr);
+	reg = IS_CA53(c) ? RCAR_CA53CPU0CR : RCAR_CA57CPU0CR;
+
+	if (read_mpidr_el1() != mpidr)
+		panic();
+
+	mmio_write_32(CPG_CPGWPR, ~CPU_PWR_OFF);
+	mmio_write_32(reg + cpu * 0x0010, CPU_PWR_OFF);
+
+	rcar_lock_release();
+}
+
+void rcar_pwrc_enable_interrupt_wakeup(u_register_t mpidr)
+{
+	uint32_t c, shift_irq, shift_fiq;
+	uintptr_t reg;
+	uint64_t cpu;
+
+	rcar_lock_get();
+
+	cpu = mpidr & MPIDR_CPU_MASK;
+	c = rcar_pwrc_get_mpidr_cluster(mpidr);
+	reg = IS_CA53(c) ? RCAR_WUPMSKCA53 : RCAR_WUPMSKCA57;
+
+	shift_irq = WUP_IRQ_SHIFT + cpu;
+	shift_fiq = WUP_FIQ_SHIFT + cpu;
+
+	mmio_clrbits_32(reg, ((uint32_t) 1 << shift_irq) |
+		      ((uint32_t) 1 << shift_fiq));
+	rcar_lock_release();
+}
+
+void rcar_pwrc_disable_interrupt_wakeup(u_register_t mpidr)
+{
+	uint32_t c, shift_irq, shift_fiq;
+	uintptr_t reg;
+	uint64_t cpu;
+
+	rcar_lock_get();
+
+	cpu = mpidr & MPIDR_CPU_MASK;
+	c = rcar_pwrc_get_mpidr_cluster(mpidr);
+	reg = IS_CA53(c) ? RCAR_WUPMSKCA53 : RCAR_WUPMSKCA57;
+
+	shift_irq = WUP_IRQ_SHIFT + cpu;
+	shift_fiq = WUP_FIQ_SHIFT + cpu;
+
+	mmio_setbits_32(reg, ((uint32_t) 1 << shift_irq) |
+		      ((uint32_t) 1 << shift_fiq));
+	rcar_lock_release();
+}
+
+void rcar_pwrc_all_disable_interrupt_wakeup(void)
+{
+	uint32_t cpu_num;
+	u_register_t cl, cpu, mpidr;
+
+	const uint32_t cluster[PLATFORM_CLUSTER_COUNT] = {
+		RCAR_CLUSTER_CA57,
+		RCAR_CLUSTER_CA53
+	};
+
+	for (cl = 0; cl < PLATFORM_CLUSTER_COUNT; cl++) {
+		cpu_num = rcar_pwrc_get_cpu_num(cluster[cl]);
+		for (cpu = 0; cpu < cpu_num; cpu++) {
+			mpidr = ((cl << MPIDR_AFFINITY_BITS) | cpu);
+			if (mpidr == rcar_boot_mpidr) {
+				rcar_pwrc_enable_interrupt_wakeup(mpidr);
+			} else {
+				rcar_pwrc_disable_interrupt_wakeup(mpidr);
+			}
+		}
+	}
+}
+
+void rcar_pwrc_clusteroff(u_register_t mpidr)
+{
+	uint32_t c, product, cut, reg;
+	uintptr_t dst;
+
+	rcar_lock_get();
+
+	reg = mmio_read_32(RCAR_PRR);
+	product = reg & PRR_PRODUCT_MASK;
+	cut = reg & PRR_CUT_MASK;
+
+	c = rcar_pwrc_get_mpidr_cluster(mpidr);
+	dst = IS_CA53(c) ? RCAR_CA53CPUCMCR : RCAR_CA57CPUCMCR;
+
+	if (product == PRR_PRODUCT_M3 && cut < PRR_PRODUCT_30) {
+		goto done;
+	}
+
+	if (product == PRR_PRODUCT_H3 && cut <= PRR_PRODUCT_20) {
+		goto done;
+	}
+
+	/* all of the CPUs in the cluster is in the CoreStandby mode */
+	mmio_write_32(dst, MODE_L2_DOWN);
+done:
+	rcar_lock_release();
+}
+
+static uint64_t rcar_pwrc_saved_cntpct_el0;
+static uint32_t rcar_pwrc_saved_cntfid;
+
+#if RCAR_SYSTEM_SUSPEND
+static void rcar_pwrc_save_timer_state(void)
+{
+	rcar_pwrc_saved_cntpct_el0 = read_cntpct_el0();
+
+	rcar_pwrc_saved_cntfid =
+		mmio_read_32((uintptr_t)(RCAR_CNTC_BASE + RCAR_CNTFID_OFF));
+}
+#endif /* RCAR_SYSTEM_SUSPEND */
+
+void rcar_pwrc_restore_timer_state(void)
+{
+	/* Stop timer before restoring counter value */
+	mmio_write_32((uintptr_t)(RCAR_CNTC_BASE + RCAR_CNTCR_OFF), 0U);
+
+	mmio_write_32((uintptr_t)(RCAR_CNTC_BASE + RCAR_CNTCVL_OFF),
+		(uint32_t)(rcar_pwrc_saved_cntpct_el0 & 0xFFFFFFFFU));
+	mmio_write_32((uintptr_t)(RCAR_CNTC_BASE + RCAR_CNTCVU_OFF),
+		(uint32_t)(rcar_pwrc_saved_cntpct_el0 >> 32U));
+
+	mmio_write_32((uintptr_t)(RCAR_CNTC_BASE + RCAR_CNTFID_OFF),
+		rcar_pwrc_saved_cntfid);
+
+	/* Start generic timer back */
+	write_cntfrq_el0((u_register_t)plat_get_syscnt_freq2());
+
+	mmio_write_32((uintptr_t)(RCAR_CNTC_BASE + RCAR_CNTCR_OFF),
+		(RCAR_CNTCR_FCREQ(0U) | RCAR_CNTCR_EN));
+}
+
+#if !PMIC_ROHM_BD9571
+void rcar_pwrc_system_reset(void)
+{
+	mmio_write_32(RCAR_SRESCR, 0x5AA50000U | BIT_SOFTRESET);
+}
+#endif /* PMIC_ROHM_BD9571 */
+
+#define RST_CA53_CPU0_BARH		(0xE6160080U)
+#define RST_CA53_CPU0_BARL		(0xE6160084U)
+#define RST_CA57_CPU0_BARH		(0xE61600C0U)
+#define RST_CA57_CPU0_BARL		(0xE61600C4U)
+
+void rcar_pwrc_setup(void)
+{
+	uintptr_t rst_barh;
+	uintptr_t rst_barl;
+	uint32_t i, j;
+	uint64_t reset = (uint64_t) (&plat_secondary_reset) & 0xFFFFFFFF;
+
+	const uint32_t cluster[PLATFORM_CLUSTER_COUNT] = {
+		RCAR_CLUSTER_CA53,
+		RCAR_CLUSTER_CA57
+	};
+	const uintptr_t reg_barh[PLATFORM_CLUSTER_COUNT] = {
+		RST_CA53_CPU0_BARH,
+		RST_CA57_CPU0_BARH
+	};
+	const uintptr_t reg_barl[PLATFORM_CLUSTER_COUNT] = {
+		RST_CA53_CPU0_BARL,
+		RST_CA57_CPU0_BARL
+	};
+
+	for (i = 0; i < PLATFORM_CLUSTER_COUNT; i++) {
+		rst_barh = reg_barh[i];
+		rst_barl = reg_barl[i];
+		for (j = 0; j < rcar_pwrc_get_cpu_num(cluster[i]); j++) {
+			mmio_write_32(rst_barh, 0);
+			mmio_write_32(rst_barl, (uint32_t) reset);
+			rst_barh += 0x10;
+			rst_barl += 0x10;
+		}
+	}
+
+	rcar_lock_init();
+}
+
+#if RCAR_SYSTEM_SUSPEND
+#define DBCAM_FLUSH(__bit)		\
+do {					\
+	;				\
+} while (!(mmio_read_32(DBSC4_REG_DBCAM##__bit##STAT0) & DBSC4_BIT_DBCAMxSTAT0))
+
+
+static void __attribute__ ((section(".system_ram")))
+	rcar_pwrc_set_self_refresh(void)
+{
+	uint32_t reg = mmio_read_32(RCAR_PRR);
+	uint32_t cut, product;
+
+	product = reg & PRR_PRODUCT_MASK;
+	cut = reg & PRR_CUT_MASK;
+
+	if (product == PRR_PRODUCT_M3 && cut < PRR_PRODUCT_30) {
+		goto self_refresh;
+	}
+
+	if (product == PRR_PRODUCT_H3 && cut < PRR_PRODUCT_20) {
+		goto self_refresh;
+	}
+
+	mmio_write_32(DBSC4_REG_DBSYSCNT0, DBSC4_SET_DBSYSCNT0_WRITE_ENABLE);
+
+self_refresh:
+
+	/* DFI_PHYMSTR_ACK setting */
+	mmio_write_32(DBSC4_REG_DBDFIPMSTRCNF,
+			mmio_read_32(DBSC4_REG_DBDFIPMSTRCNF) &
+			(~DBSC4_BIT_DBDFIPMSTRCNF_PMSTREN));
+
+	/* Set the Self-Refresh mode */
+	mmio_write_32(DBSC4_REG_DBACEN, 0);
+
+	if (product == PRR_PRODUCT_H3 && cut < PRR_PRODUCT_20)
+		rcar_micro_delay(100);
+	else if (product == PRR_PRODUCT_H3) {
+		mmio_write_32(DBSC4_REG_DBCAM0CTRL0, 1);
+		DBCAM_FLUSH(0);
+		DBCAM_FLUSH(1);
+		DBCAM_FLUSH(2);
+		DBCAM_FLUSH(3);
+		mmio_write_32(DBSC4_REG_DBCAM0CTRL0, 0);
+	} else if (product == PRR_PRODUCT_M3) {
+		mmio_write_32(DBSC4_REG_DBCAM0CTRL0, 1);
+		DBCAM_FLUSH(0);
+		DBCAM_FLUSH(1);
+		mmio_write_32(DBSC4_REG_DBCAM0CTRL0, 0);
+	} else {
+		mmio_write_32(DBSC4_REG_DBCAM0CTRL0, 1);
+		DBCAM_FLUSH(0);
+		mmio_write_32(DBSC4_REG_DBCAM0CTRL0, 0);
+	}
+
+	/* Set the SDRAM calibration configuration register */
+	mmio_write_32(DBSC4_REG_DBCALCNF, 0);
+
+	reg = DBSC4_SET_DBCMD_OPC_PRE | DBSC4_SET_DBCMD_CH_ALL |
+	    DBSC4_SET_DBCMD_RANK_ALL | DBSC4_SET_DBCMD_ARG_ALL;
+	mmio_write_32(DBSC4_REG_DBCMD, reg);
+	while (mmio_read_32(DBSC4_REG_DBWAIT))
+		;
+
+	/* Self-Refresh entry command   */
+	reg = DBSC4_SET_DBCMD_OPC_SR | DBSC4_SET_DBCMD_CH_ALL |
+	    DBSC4_SET_DBCMD_RANK_ALL | DBSC4_SET_DBCMD_ARG_ENTER;
+	mmio_write_32(DBSC4_REG_DBCMD, reg);
+	while (mmio_read_32(DBSC4_REG_DBWAIT))
+		;
+
+	/* Mode Register Write command. (ODT disabled)  */
+	reg = DBSC4_SET_DBCMD_OPC_MRW | DBSC4_SET_DBCMD_CH_ALL |
+	    DBSC4_SET_DBCMD_RANK_ALL | DBSC4_SET_DBCMD_ARG_MRW_ODTC;
+	mmio_write_32(DBSC4_REG_DBCMD, reg);
+	while (mmio_read_32(DBSC4_REG_DBWAIT))
+		;
+
+	/* Power Down entry command     */
+	reg = DBSC4_SET_DBCMD_OPC_PD | DBSC4_SET_DBCMD_CH_ALL |
+	    DBSC4_SET_DBCMD_RANK_ALL | DBSC4_SET_DBCMD_ARG_ENTER;
+	mmio_write_32(DBSC4_REG_DBCMD, reg);
+	while (mmio_read_32(DBSC4_REG_DBWAIT))
+		;
+
+	/* Set the auto-refresh enable register */
+	mmio_write_32(DBSC4_REG_DBRFEN, 0U);
+	rcar_micro_delay(1U);
+
+	if (product == PRR_PRODUCT_M3 && cut < PRR_PRODUCT_30)
+		return;
+
+	if (product == PRR_PRODUCT_H3 && cut < PRR_PRODUCT_20)
+		return;
+
+	mmio_write_32(DBSC4_REG_DBSYSCNT0, DBSC4_SET_DBSYSCNT0_WRITE_DISABLE);
+}
+
+static void __attribute__ ((section(".system_ram")))
+rcar_pwrc_set_self_refresh_e3(void)
+{
+	uint32_t ddr_md;
+	uint32_t reg;
+
+	ddr_md = (mmio_read_32(RST_MODEMR) >> 19) & RST_MODEMR_BIT0;
+
+	/* Write enable */
+	mmio_write_32(DBSC4_REG_DBSYSCNT0, DBSC4_SET_DBSYSCNT0_WRITE_ENABLE);
+	mmio_write_32(DBSC4_REG_DBACEN, 0);
+	DBCAM_FLUSH(0);
+
+	reg = DBSC4_SET_DBCMD_OPC_PRE | DBSC4_SET_DBCMD_CH_ALL |
+	    DBSC4_SET_DBCMD_RANK_ALL | DBSC4_SET_DBCMD_ARG_ALL;
+	mmio_write_32(DBSC4_REG_DBCMD, reg);
+	while (mmio_read_32(DBSC4_REG_DBWAIT))
+		;
+
+	reg = DBSC4_SET_DBCMD_OPC_SR | DBSC4_SET_DBCMD_CH_ALL |
+	    DBSC4_SET_DBCMD_RANK_ALL | DBSC4_SET_DBCMD_ARG_ENTER;
+	mmio_write_32(DBSC4_REG_DBCMD, reg);
+	while (mmio_read_32(DBSC4_REG_DBWAIT))
+		;
+
+	/*
+	 * Set the auto-refresh enable register
+	 * Set the ARFEN bit to 0 in the DBRFEN
+	 */
+	mmio_write_32(DBSC4_REG_DBRFEN, 0);
+
+	mmio_write_32(DBSC4_REG_DBPDLK0, DBSC4_SET_DBPDLK0_PHY_ACCESS);
+
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_ACIOCR0);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_ACIOCR0);
+
+	/* DDR_DXCCR */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DXCCR);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DXCCR);
+
+	/* DDR_PGCR1 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_PGCR1);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_PGCR1);
+
+	/* DDR_ACIOCR1 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_ACIOCR1);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_ACIOCR1);
+
+	/* DDR_ACIOCR3 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_ACIOCR3);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_ACIOCR3);
+
+	/* DDR_ACIOCR5 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_ACIOCR5);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_ACIOCR5);
+
+	/* DDR_DX0GCR2 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX0GCR2);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX0GCR2);
+
+	/* DDR_DX1GCR2 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX1GCR2);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX1GCR2);
+
+	/* DDR_DX2GCR2 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX2GCR2);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX2GCR2);
+
+	/* DDR_DX3GCR2 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX3GCR2);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX3GCR2);
+
+	/* DDR_ZQCR */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_ZQCR);
+
+	mmio_write_32(DBSC4_REG_DBPDRGD0, ddr_md == 0 ?
+		      DBSC4_SET_DBPDRGD0_ZQCR_MD19_0 :
+		      DBSC4_SET_DBPDRGD0_ZQCR_MD19_1);
+
+	/* DDR_DX0GCR0 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX0GCR0);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX0GCR0);
+
+	/* DDR_DX1GCR0 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX1GCR0);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX1GCR0);
+
+	/* DDR_DX2GCR0 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX2GCR0);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX2GCR0);
+
+	/* DDR_DX3GCR0 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX3GCR0);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX3GCR0);
+
+	/* DDR_DX0GCR1 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX0GCR1);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX0GCR1);
+
+	/* DDR_DX1GCR1 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX1GCR1);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX1GCR1);
+
+	/* DDR_DX2GCR1 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX2GCR1);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX2GCR1);
+
+	/* DDR_DX3GCR1 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX3GCR1);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX3GCR1);
+
+	/* DDR_DX0GCR3 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX0GCR3);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX0GCR3);
+
+	/* DDR_DX1GCR3 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX1GCR3);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX1GCR3);
+
+	/* DDR_DX2GCR3 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX2GCR3);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX2GCR3);
+
+	/* DDR_DX3GCR3 */
+	mmio_write_32(DBSC4_REG_DBPDRGA0, DBSC4_SET_DBPDRGA0_DX3GCR3);
+	mmio_write_32(DBSC4_REG_DBPDRGD0, DBSC4_SET_DBPDRGD0_DX3GCR3);
+
+	/* Write disable */
+	mmio_write_32(DBSC4_REG_DBSYSCNT0, DBSC4_SET_DBSYSCNT0_WRITE_DISABLE);
+}
+
+void __attribute__ ((section(".system_ram"))) __attribute__ ((noinline))
+rcar_pwrc_go_suspend_to_ram(void)
+{
+#if PMIC_ROHM_BD9571
+	int32_t rc = -1, qllm = -1;
+	uint8_t mode;
+	uint32_t i;
+#endif
+	uint32_t reg, product;
+
+	reg = mmio_read_32(RCAR_PRR);
+	product = reg & PRR_PRODUCT_MASK;
+
+	if (product != PRR_PRODUCT_E3)
+		rcar_pwrc_set_self_refresh();
+	else
+		rcar_pwrc_set_self_refresh_e3();
+
+#if PMIC_ROHM_BD9571
+	/* Set QLLM Cnt Disable */
+	for (i = 0; (i < PMIC_RETRY_MAX) && (qllm != 0); i++)
+		qllm = rcar_iic_dvfs_send(PMIC, PMIC_QLLM_CNT, 0);
+
+	/* Set trigger of power down to PMIV */
+	for (i = 0; (i < PMIC_RETRY_MAX) && (rc != 0) && (qllm == 0); i++) {
+		rc = rcar_iic_dvfs_receive(PMIC, PMIC_BKUP_MODE_CNT, &mode);
+		if (rc == 0) {
+			mode |= BIT_BKUP_CTRL_OUT;
+			rc = rcar_iic_dvfs_send(PMIC, PMIC_BKUP_MODE_CNT, mode);
+		}
+	}
+#endif
+	wfi();
+
+	while (1)
+		;
+}
+
+void rcar_pwrc_set_suspend_to_ram(void)
+{
+	uintptr_t jump = (uintptr_t) &rcar_pwrc_go_suspend_to_ram;
+	uintptr_t stack = (uintptr_t) (DEVICE_SRAM_STACK_BASE +
+				       DEVICE_SRAM_STACK_SIZE);
+	uint32_t sctlr;
+
+	rcar_pwrc_save_timer_state();
+
+	/* disable MMU */
+	sctlr = (uint32_t) read_sctlr_el3();
+	sctlr &= (uint32_t) ~SCTLR_EL3_M_BIT;
+	write_sctlr_el3((uint64_t) sctlr);
+
+	rcar_pwrc_switch_stack(jump, stack, NULL);
+}
+
+void rcar_pwrc_init_suspend_to_ram(void)
+{
+#if PMIC_ROHM_BD9571
+	uint8_t mode;
+
+	if (rcar_iic_dvfs_receive(PMIC, PMIC_BKUP_MODE_CNT, &mode))
+		panic();
+
+	mode &= (uint8_t) (~BIT_BKUP_CTRL_OUT);
+	if (rcar_iic_dvfs_send(PMIC, PMIC_BKUP_MODE_CNT, mode))
+		panic();
+#endif
+}
+
+void rcar_pwrc_suspend_to_ram(void)
+{
+#if RCAR_SYSTEM_RESET_KEEPON_DDR
+	int32_t error;
+
+	error = rcar_iic_dvfs_send(PMIC, REG_KEEP10, 0);
+	if (error) {
+		ERROR("Failed send KEEP10 init ret=%d\n", error);
+		return;
+	}
+#endif
+	rcar_pwrc_set_suspend_to_ram();
+}
+#endif
+
+void rcar_pwrc_code_copy_to_system_ram(void)
+{
+	int ret __attribute__ ((unused));	/* in assert */
+	uint32_t attr;
+	struct device_sram_t {
+		uintptr_t base;
+		size_t len;
+	} sram = {
+		.base = (uintptr_t) DEVICE_SRAM_BASE,
+		.len = DEVICE_SRAM_SIZE,
+	};
+	struct ddr_code_t {
+		void *base;
+		size_t len;
+	} code = {
+		.base = (void *) SRAM_COPY_START,
+		.len = SYSTEM_RAM_END - SYSTEM_RAM_START,
+	};
+
+	attr = MT_MEMORY | MT_RW | MT_SECURE | MT_EXECUTE_NEVER;
+	ret = xlat_change_mem_attributes(sram.base, sram.len, attr);
+	assert(ret == 0);
+
+	memcpy((void *)sram.base, code.base, code.len);
+	flush_dcache_range((uint64_t) sram.base, code.len);
+
+	attr = MT_MEMORY | MT_RO | MT_SECURE | MT_EXECUTE;
+	ret = xlat_change_mem_attributes(sram.base, sram.len, attr);
+	assert(ret == 0);
+
+	/* Invalidate instruction cache */
+	plat_invalidate_icache();
+	dsb();
+	isb();
+}
+
+uint32_t rcar_pwrc_get_cluster(void)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(RCAR_PRR);
+
+	if (reg & (1U << (STATE_CA53_CPU + RCAR_CA53CPU_NUM_MAX)))
+		return RCAR_CLUSTER_CA57;
+
+	if (reg & (1U << (STATE_CA57_CPU + RCAR_CA57CPU_NUM_MAX)))
+		return RCAR_CLUSTER_CA53;
+
+	return RCAR_CLUSTER_A53A57;
+}
+
+uint32_t rcar_pwrc_get_mpidr_cluster(u_register_t mpidr)
+{
+	uint32_t c = rcar_pwrc_get_cluster();
+
+	if (IS_A53A57(c)) {
+		if (mpidr & MPIDR_CLUSTER_MASK)
+			return RCAR_CLUSTER_CA53;
+
+		return RCAR_CLUSTER_CA57;
+	}
+
+	return c;
+}
+
+#if RCAR_LSI == RCAR_D3
+uint32_t rcar_pwrc_get_cpu_num(uint32_t c)
+{
+	return 1;
+}
+#else
+uint32_t rcar_pwrc_get_cpu_num(uint32_t c)
+{
+	uint32_t reg = mmio_read_32(RCAR_PRR);
+	uint32_t count = 0, i;
+
+	if (IS_A53A57(c) || IS_CA53(c)) {
+		if (reg & (1 << (STATE_CA53_CPU + RCAR_CA53CPU_NUM_MAX)))
+			goto count_ca57;
+
+		for (i = 0; i < RCAR_CA53CPU_NUM_MAX; i++) {
+			if (reg & (1 << (STATE_CA53_CPU + i)))
+				continue;
+			count++;
+		}
+	}
+
+count_ca57:
+	if (IS_A53A57(c) || IS_CA57(c)) {
+		if (reg & (1U << (STATE_CA57_CPU + RCAR_CA57CPU_NUM_MAX)))
+			goto done;
+
+		for (i = 0; i < RCAR_CA57CPU_NUM_MAX; i++) {
+			if (reg & (1 << (STATE_CA57_CPU + i)))
+				continue;
+			count++;
+		}
+	}
+
+done:
+	return count;
+}
+#endif
+
+int32_t rcar_pwrc_cpu_on_check(u_register_t mpidr)
+{
+	uint64_t i;
+	uint64_t j;
+	uint64_t cpu_count;
+	uintptr_t reg_PSTR;
+	uint32_t status;
+	uint64_t my_cpu;
+	int32_t rtn;
+	uint32_t my_cluster_type;
+	const uint32_t cluster_type[PLATFORM_CLUSTER_COUNT] = {
+			RCAR_CLUSTER_CA53,
+			RCAR_CLUSTER_CA57
+	};
+	const uintptr_t registerPSTR[PLATFORM_CLUSTER_COUNT] = {
+			RCAR_CA53PSTR,
+			RCAR_CA57PSTR
+	};
+
+	my_cluster_type = rcar_pwrc_get_cluster();
+
+	rtn = 0;
+	my_cpu = mpidr & ((uint64_t)(MPIDR_CPU_MASK));
+	for (i = 0U; i < ((uint64_t)(PLATFORM_CLUSTER_COUNT)); i++) {
+		cpu_count = rcar_pwrc_get_cpu_num(cluster_type[i]);
+		reg_PSTR = registerPSTR[i];
+		for (j = 0U; j < cpu_count; j++) {
+			if ((my_cluster_type != cluster_type[i]) || (my_cpu != j)) {
+				status = mmio_read_32(reg_PSTR) >> (j * 4U);
+				if ((status & 0x00000003U) == 0U) {
+					rtn--;
+				}
+			}
+		}
+	}
+
+	return rtn;
+}
diff --git a/drivers/renesas/common/pwrc/pwrc.h b/drivers/renesas/common/pwrc/pwrc.h
new file mode 100644
index 0000000..eefa62f
--- /dev/null
+++ b/drivers/renesas/common/pwrc/pwrc.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PWRC_H
+#define PWRC_H
+
+#define PPOFFR_OFF		0x0
+#define PPONR_OFF		0x4
+#define PCOFFR_OFF		0x8
+#define PWKUPR_OFF		0xc
+#define PSYSR_OFF		0x10
+
+#define PWKUPR_WEN		(1ull << 31)
+
+#define PSYSR_AFF_L2		(1U << 31)
+#define PSYSR_AFF_L1		(1 << 30)
+#define PSYSR_AFF_L0		(1 << 29)
+#define PSYSR_WEN		(1 << 28)
+#define PSYSR_PC		(1 << 27)
+#define PSYSR_PP		(1 << 26)
+
+#define PSYSR_WK_SHIFT		(24)
+#define PSYSR_WK_MASK		(0x3)
+#define PSYSR_WK(x)		(((x) >> PSYSR_WK_SHIFT) & PSYSR_WK_MASK)
+
+#define WKUP_COLD		0x0
+#define WKUP_RESET		0x1
+#define WKUP_PPONR		0x2
+#define WKUP_GICREQ		0x3
+
+#define RCAR_INVALID		(0xffffffffU)
+#define PSYSR_INVALID		0xffffffff
+
+#define RCAR_CLUSTER_A53A57	(0U)
+#define RCAR_CLUSTER_CA53	(1U)
+#define RCAR_CLUSTER_CA57	(2U)
+
+extern u_register_t rcar_boot_mpidr;
+
+#ifndef __ASSEMBLER__
+void rcar_pwrc_disable_interrupt_wakeup(u_register_t mpidr);
+void rcar_pwrc_enable_interrupt_wakeup(u_register_t mpidr);
+void rcar_pwrc_all_disable_interrupt_wakeup(void);
+void rcar_pwrc_clusteroff(u_register_t mpidr);
+void rcar_pwrc_cpuoff(u_register_t mpidr);
+void rcar_pwrc_cpuon(u_register_t mpidr);
+int32_t rcar_pwrc_cpu_on_check(u_register_t mpidr);
+void rcar_pwrc_setup(void);
+
+uint32_t rcar_pwrc_get_cpu_wkr(u_register_t mpidr);
+uint32_t rcar_pwrc_status(u_register_t mpidr);
+uint32_t rcar_pwrc_get_cluster(void);
+uint32_t rcar_pwrc_get_mpidr_cluster(u_register_t mpidr);
+uint32_t rcar_pwrc_get_cpu_num(uint32_t cluster_type);
+void rcar_pwrc_restore_timer_state(void);
+void plat_secondary_reset(void);
+
+void rcar_pwrc_code_copy_to_system_ram(void);
+
+#if !PMIC_ROHM_BD9571
+void rcar_pwrc_system_reset(void);
+#endif
+
+#if RCAR_SYSTEM_SUSPEND
+void rcar_pwrc_go_suspend_to_ram(void);
+void rcar_pwrc_set_suspend_to_ram(void);
+void rcar_pwrc_init_suspend_to_ram(void);
+void rcar_pwrc_suspend_to_ram(void);
+#endif
+
+extern uint32_t rcar_pwrc_switch_stack(uintptr_t jump, uintptr_t stack,
+				       void *arg);
+#endif
+
+#endif /* PWRC_H */
diff --git a/drivers/renesas/common/qos_reg.h b/drivers/renesas/common/qos_reg.h
new file mode 100644
index 0000000..f2012fa
--- /dev/null
+++ b/drivers/renesas/common/qos_reg.h
@@ -0,0 +1,133 @@
+/*
+ * Copyright (c) 2017-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_REG_H
+#define QOS_REG_H
+
+#define	RCAR_QOS_NONE			3U
+#define	RCAR_QOS_TYPE_DEFAULT		0U
+
+#define	RCAR_DRAM_SPLIT_LINEAR		0U
+#define	RCAR_DRAM_SPLIT_4CH		1U
+#define	RCAR_DRAM_SPLIT_2CH		2U
+#define	RCAR_DRAM_SPLIT_AUTO		3U
+#define	RST_BASE			(0xE6160000U)
+#define	RST_MODEMR			(RST_BASE + 0x0060U)
+
+#define	DBSC_BASE			0xE6790000U
+#define DBSC_DBSYSCNT0			(DBSC_BASE + 0x0100U)
+#define DBSC_AXARB			(DBSC_BASE + 0x0800U)
+#define DBSC_DBCAM0CNF1			(DBSC_BASE + 0x0904U)
+#define DBSC_DBCAM0CNF2			(DBSC_BASE + 0x0908U)
+#define DBSC_DBCAM0CNF3			(DBSC_BASE + 0x090CU)
+#define DBSC_DBSCHCNT0			(DBSC_BASE + 0x1000U)
+#define DBSC_DBSCHCNT1			(DBSC_BASE + 0x1004U)
+#define DBSC_DBSCHSZ0			(DBSC_BASE + 0x1010U)
+#define DBSC_DBSCHRW0			(DBSC_BASE + 0x1020U)
+#define DBSC_DBSCHRW1			(DBSC_BASE + 0x1024U)
+#define DBSC_DBSCHQOS00			(DBSC_BASE + 0x1030U)
+#define DBSC_DBSCHQOS01			(DBSC_BASE + 0x1034U)
+#define DBSC_DBSCHQOS02			(DBSC_BASE + 0x1038U)
+#define DBSC_DBSCHQOS03			(DBSC_BASE + 0x103CU)
+#define DBSC_DBSCHQOS40			(DBSC_BASE + 0x1070U)
+#define DBSC_DBSCHQOS41			(DBSC_BASE + 0x1074U)
+#define DBSC_DBSCHQOS42			(DBSC_BASE + 0x1078U)
+#define DBSC_DBSCHQOS43			(DBSC_BASE + 0x107CU)
+#define DBSC_DBSCHQOS90			(DBSC_BASE + 0x10C0U)
+#define DBSC_DBSCHQOS91			(DBSC_BASE + 0x10C4U)
+#define DBSC_DBSCHQOS92			(DBSC_BASE + 0x10C8U)
+#define DBSC_DBSCHQOS93			(DBSC_BASE + 0x10CCU)
+#define DBSC_DBSCHQOS120		(DBSC_BASE + 0x10F0U)
+#define DBSC_DBSCHQOS121		(DBSC_BASE + 0x10F4U)
+#define DBSC_DBSCHQOS122		(DBSC_BASE + 0x10F8U)
+#define DBSC_DBSCHQOS123		(DBSC_BASE + 0x10FCU)
+#define DBSC_DBSCHQOS130		(DBSC_BASE + 0x1100U)
+#define DBSC_DBSCHQOS131		(DBSC_BASE + 0x1104U)
+#define DBSC_DBSCHQOS132		(DBSC_BASE + 0x1108U)
+#define DBSC_DBSCHQOS133		(DBSC_BASE + 0x110CU)
+#define DBSC_DBSCHQOS140		(DBSC_BASE + 0x1110U)
+#define DBSC_DBSCHQOS141		(DBSC_BASE + 0x1114U)
+#define DBSC_DBSCHQOS142		(DBSC_BASE + 0x1118U)
+#define DBSC_DBSCHQOS143		(DBSC_BASE + 0x111CU)
+#define DBSC_DBSCHQOS150		(DBSC_BASE + 0x1120U)
+#define DBSC_DBSCHQOS151		(DBSC_BASE + 0x1124U)
+#define DBSC_DBSCHQOS152		(DBSC_BASE + 0x1128U)
+#define DBSC_DBSCHQOS153		(DBSC_BASE + 0x112CU)
+#define DBSC_SCFCTST0			(DBSC_BASE + 0x1700U)
+#define DBSC_SCFCTST1			(DBSC_BASE + 0x1708U)
+#define DBSC_SCFCTST2			(DBSC_BASE + 0x170CU)
+
+#define	AXI_BASE			0xE6784000U
+#define	AXI_ADSPLCR0			(AXI_BASE + 0x0008U)
+#define	AXI_ADSPLCR1			(AXI_BASE + 0x000CU)
+#define	AXI_ADSPLCR2			(AXI_BASE + 0x0010U)
+#define	AXI_ADSPLCR3			(AXI_BASE + 0x0014U)
+#define	AXI_MMCR			(AXI_BASE + 0x0300U)
+#define	ADSPLCR0_ADRMODE_DEFAULT	((uint32_t)0U << 31U)
+#define	ADSPLCR0_ADRMODE_GEN2		((uint32_t)1U << 31U)
+#define	ADSPLCR0_SPLITSEL(x)		((uint32_t)(x) << 16U)
+#define	ADSPLCR0_AREA(x)		((uint32_t)(x) <<  8U)
+#define	ADSPLCR0_SWP			0x0CU
+
+#define	AXI_TR3CR			0xE67D100CU
+#define	AXI_TR4CR			0xE67D1014U
+
+#define	QOS_BASE0			0xE67E0000U
+#define	QOSBW_FIX_QOS_BANK0		(QOS_BASE0 + 0x0000U)
+#define	QOSBW_FIX_QOS_BANK1		(QOS_BASE0 + 0x1000U)
+#define	QOSBW_BE_QOS_BANK0		(QOS_BASE0 + 0x2000U)
+#define	QOSBW_BE_QOS_BANK1		(QOS_BASE0 + 0x3000U)
+#define	QOSCTRL_SL_INIT			(QOS_BASE0 + 0x8000U)
+#define	QOSCTRL_REF_ARS			(QOS_BASE0 + 0x8004U)
+#define	QOSCTRL_STATQC			(QOS_BASE0 + 0x8008U)
+
+#define	QOS_BASE1			0xE67F0000U
+#define	QOSCTRL_RAS			(QOS_BASE1 + 0x0000U)
+#define	QOSCTRL_FIXTH			(QOS_BASE1 + 0x0004U)
+#define	QOSCTRL_RAEN			(QOS_BASE1 + 0x0018U)
+#define	QOSCTRL_REGGD			(QOS_BASE1 + 0x0020U)
+#define	QOSCTRL_DANN			(QOS_BASE1 + 0x0030U)
+#define	QOSCTRL_DANT			(QOS_BASE1 + 0x0038U)
+#define	QOSCTRL_EC			(QOS_BASE1 + 0x003CU)
+#define	QOSCTRL_EMS			(QOS_BASE1 + 0x0040U)
+#define	QOSCTRL_FSS			(QOS_BASE1 + 0x0048U)
+#define	QOSCTRL_INSFC			(QOS_BASE1 + 0x0050U)
+#define	QOSCTRL_BERR			(QOS_BASE1 + 0x0054U)
+#define	QOSCTRL_EARLYR			(QOS_BASE1 + 0x0060U)
+#define	QOSCTRL_RACNT0			(QOS_BASE1 + 0x0080U)
+#define	QOSCTRL_STATGEN0		(QOS_BASE1 + 0x0088U)
+
+#define	GPU_ACT_GRD			0xFD820808U
+#define	GPU_ACT0			0xFD820800U
+#define	GPU_ACT1			0xFD821800U
+#define	GPU_ACT2			0xFD822800U
+#define	GPU_ACT3			0xFD823800U
+#define	GPU_ACT4			0xFD824800U
+#define	GPU_ACT5			0xFD825800U
+#define	GPU_ACT6			0xFD826800U
+#define	GPU_ACT7			0xFD827800U
+
+#define	RT_ACT0				0xFFC50800U
+#define	RT_ACT1				0xFFC51800U
+
+#define	CPU_ACT0			0xF1300800U
+#define	CPU_ACT1			0xF1340800U
+#define	CPU_ACT2			0xF1380800U
+#define	CPU_ACT3			0xF13C0800U
+
+#define	RCAR_REWT_TRAINING_DISABLE	0U
+#define	RCAR_REWT_TRAINING_ENABLE	1U
+
+#define QOSWT_FIX_WTQOS_BANK0		(QOSBW_FIX_QOS_BANK0 + 0x0800U)
+#define QOSWT_FIX_WTQOS_BANK1		(QOSBW_FIX_QOS_BANK1 + 0x0800U)
+#define QOSWT_BE_WTQOS_BANK0		(QOSBW_BE_QOS_BANK0  + 0x0800U)
+#define QOSWT_BE_WTQOS_BANK1		(QOSBW_BE_QOS_BANK1  + 0x0800U)
+#define	QOSWT_WTEN			(QOS_BASE0 + 0x8030U)
+#define	QOSWT_WTREF			(QOS_BASE0 + 0x8034U)
+#define	QOSWT_WTSET0			(QOS_BASE0 + 0x8038U)
+#define	QOSWT_WTSET1			(QOS_BASE0 + 0x803CU)
+
+#endif /* QOS_REG_H */
diff --git a/drivers/renesas/common/rom/rom_api.c b/drivers/renesas/common/rom/rom_api.c
new file mode 100644
index 0000000..fda2815
--- /dev/null
+++ b/drivers/renesas/common/rom/rom_api.c
@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <lib/mmio.h>
+
+#include "rcar_def.h"
+#include "rom_api.h"
+
+typedef uint32_t(*rom_secure_boot_api_f) (uint32_t *key, uint32_t *cert,
+					  rom_read_flash_f pFuncReadFlash);
+
+typedef uint32_t(*rom_get_lcs_api_f) (uint32_t *lcs);
+
+#define OLD_API_TABLE1	(0U)	/* H3 Ver.1.0/Ver.1.1 */
+#define OLD_API_TABLE2	(1U)	/* H3 Ver.2.0 */
+#define OLD_API_TABLE3	(2U)	/* M3 Ver.1.0 */
+#define NEW_API_TABLE	(3U)	/* H3 Ver.3.0, M3 Ver.1.1 or later, M3N, E3, D3, V3M WS2.0 */
+#define NEW_API_TABLE2	(4U)	/* V3M WS1.0 */
+#define API_TABLE_MAX	(5U)	/* table max */
+				/* Later than H3 Ver.2.0 */
+
+static uint32_t get_table_index(void)
+{
+	uint32_t product;
+	uint32_t cut_ver;
+	uint32_t index;
+
+	product = mmio_read_32(RCAR_PRR) & PRR_PRODUCT_MASK;
+	cut_ver = mmio_read_32(RCAR_PRR) & PRR_CUT_MASK;
+
+	switch (product) {
+	case PRR_PRODUCT_H3:
+		if (cut_ver == PRR_PRODUCT_10)
+			index = OLD_API_TABLE1;
+		else if (cut_ver == PRR_PRODUCT_11)
+			index = OLD_API_TABLE1;
+		else if (cut_ver == PRR_PRODUCT_20)
+			index = OLD_API_TABLE2;
+		else
+			/* Later than H3 Ver.2.0 */
+			index = NEW_API_TABLE;
+		break;
+	case PRR_PRODUCT_M3:
+		if (cut_ver == PRR_PRODUCT_10)
+			index = OLD_API_TABLE3;
+		else
+			/* M3 Ver.1.1 or later */
+			index = NEW_API_TABLE;
+		break;
+	case PRR_PRODUCT_V3M:
+		if (cut_ver == PRR_PRODUCT_10)
+			/* V3M WS1.0 */
+			index = NEW_API_TABLE2;
+		else
+			/* V3M WS2.0 or later */
+			index = NEW_API_TABLE;
+		break;
+	default:
+		index = NEW_API_TABLE;
+		break;
+	}
+
+	return index;
+}
+
+uint32_t rcar_rom_secure_boot_api(uint32_t *key, uint32_t *cert,
+			     rom_read_flash_f read_flash)
+{
+	static const uintptr_t rom_api_table[API_TABLE_MAX] = {
+		0xEB10DD64U,	/* H3 Ver.1.0/Ver.1.1 */
+		0xEB116ED4U,	/* H3 Ver.2.0 */
+		0xEB1102FCU,	/* M3 Ver.1.0 */
+		0xEB100180U,	/* H3 Ver.3.0, M3 Ver.1.1 or later, M3N, E3, D3, V3M WS2.0 */
+		0xEB110128U,	/* V3M WS1.0 */
+	};
+	rom_secure_boot_api_f secure_boot;
+	uint32_t index;
+
+	index = get_table_index();
+	secure_boot = (rom_secure_boot_api_f) rom_api_table[index];
+
+	return secure_boot(key, cert, read_flash);
+}
+
+uint32_t rcar_rom_get_lcs(uint32_t *lcs)
+{
+	static const uintptr_t rom_get_lcs_table[API_TABLE_MAX] = {
+		0xEB10DFE0U,	/* H3 Ver.1.0/Ver.1.1 */
+		0xEB117150U,	/* H3 Ver.2.0 */
+		0xEB110578U,	/* M3 Ver.1.0 */
+		0xEB10018CU,	/* H3 Ver.3.0, M3 Ver.1.1 or later, M3N, E3, D3, V3M WS2.0 */
+		0xEB1103A4U,	/* V3M WS1.0 */
+	};
+	rom_get_lcs_api_f get_lcs;
+	uint32_t index;
+
+	index = get_table_index();
+	get_lcs = (rom_get_lcs_api_f) rom_get_lcs_table[index];
+
+	return get_lcs(lcs);
+}
diff --git a/drivers/renesas/common/rom/rom_api.h b/drivers/renesas/common/rom/rom_api.h
new file mode 100644
index 0000000..1d5b03d
--- /dev/null
+++ b/drivers/renesas/common/rom/rom_api.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef ROM_API_H
+#define ROM_API_H
+
+#include <stdint.h>
+
+#define SBROM_OK				(0x00000000U)
+#define SBROM_ILLEGAL_INPUT_PARAM_ERR		(0x0B000001U)
+#define SBROM_ILLEGAL_OEM_HASH_VALUE_ERR	(0x0B000008U)
+#define SBROM_ILLEGAL_LCS_FOR_OPERATION_ERR	(0x0B000010U)
+#define SBROM_HASH_NOT_PROGRAMMED_ERR		(0x0B000100U)
+#define SBROM_PUB_KEY_HASH_VALIDATION_FAILURE	(0xF1000006U)
+#define SBROM_RSA_SIG_VERIFICATION_FAILED	(0xF1000007U)
+
+#define LCS_CM					(0x0U)
+#define LCS_DM					(0x1U)
+#define LCS_SD					(0x3U)
+#define LCS_SE					(0x5U)
+#define LCS_FA					(0x7U)
+
+typedef uint32_t(*rom_read_flash_f) (uint64_t src, uint8_t *dst, uint32_t len);
+uint32_t rcar_rom_secure_boot_api(uint32_t *key, uint32_t *cert,
+				  rom_read_flash_f f);
+uint32_t rcar_rom_get_lcs(uint32_t *lcs);
+
+#endif /* ROM_API_H */
diff --git a/drivers/renesas/common/rpc/rpc_driver.c b/drivers/renesas/common/rpc/rpc_driver.c
new file mode 100644
index 0000000..63de5b8
--- /dev/null
+++ b/drivers/renesas/common/rpc/rpc_driver.c
@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "cpg_registers.h"
+#include "rcar_def.h"
+#include "rcar_private.h"
+#include "rpc_registers.h"
+
+#define MSTPSR9_RPC_BIT		(0x00020000U)
+#define RPC_CMNCR_MD_BIT	(0x80000000U)
+#define RPC_PHYCNT_CAL		BIT(31)
+#define RPC_PHYCNT_STRTIM_M3V1	(0x6 << 15UL)
+#define RPC_PHYCNT_STRTIM	(0x7 << 15UL)
+
+static void rpc_enable(void)
+{
+	/* Enable clock supply to RPC. */
+	mstpcr_write(CPG_SMSTPCR9, CPG_MSTPSR9, MSTPSR9_RPC_BIT);
+}
+
+static void rpc_setup(void)
+{
+	uint32_t product, cut, reg, phy_strtim;
+
+	if (mmio_read_32(RPC_CMNCR) & RPC_CMNCR_MD_BIT)
+		mmio_clrbits_32(RPC_CMNCR, RPC_CMNCR_MD_BIT);
+
+	product = mmio_read_32(RCAR_PRR) & PRR_PRODUCT_MASK;
+	cut = mmio_read_32(RCAR_PRR) & PRR_CUT_MASK;
+
+	if ((product ==  PRR_PRODUCT_M3) && (cut < PRR_PRODUCT_30))
+		phy_strtim = RPC_PHYCNT_STRTIM_M3V1;
+	else
+		phy_strtim = RPC_PHYCNT_STRTIM;
+
+	reg = mmio_read_32(RPC_PHYCNT);
+	reg &= ~RPC_PHYCNT_STRTIM;
+	reg |= phy_strtim;
+	mmio_write_32(RPC_PHYCNT, reg);
+	reg |= RPC_PHYCNT_CAL;
+	mmio_write_32(RPC_PHYCNT, reg);
+}
+
+void rcar_rpc_init(void)
+{
+	rpc_enable();
+	rpc_setup();
+}
diff --git a/drivers/renesas/common/rpc/rpc_registers.h b/drivers/renesas/common/rpc/rpc_registers.h
new file mode 100644
index 0000000..79aea85
--- /dev/null
+++ b/drivers/renesas/common/rpc/rpc_registers.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RPC_REGISTERS_H
+#define RPC_REGISTERS_H
+
+#define RPC_BASE	(0xEE200000U)
+#define RPC_CMNCR	(RPC_BASE + 0x0000U)
+#define RPC_SSLDR	(RPC_BASE + 0x0004U)
+#define RPC_DRCR	(RPC_BASE + 0x000CU)
+#define RPC_DRCMR	(RPC_BASE + 0x0010U)
+#define RPC_DRENR	(RPC_BASE + 0x001CU)
+#define RPC_SMCR	(RPC_BASE + 0x0020U)
+#define RPC_SMCMR	(RPC_BASE + 0x0024U)
+#define RPC_SMENR	(RPC_BASE + 0x0030U)
+#define RPC_CMNSR	(RPC_BASE + 0x0048U)
+#define RPC_DRDMCR	(RPC_BASE + 0x0058U)
+#define RPC_DRDRENR	(RPC_BASE + 0x005CU)
+#define RPC_PHYCNT	(RPC_BASE + 0x007CU)
+#define RPC_PHYINT	(RPC_BASE + 0x0088U)
+
+#endif /* RPC_REGISTERS_H */
diff --git a/drivers/renesas/common/scif/scif.S b/drivers/renesas/common/scif/scif.S
new file mode 100644
index 0000000..72b5b4b
--- /dev/null
+++ b/drivers/renesas/common/scif/scif.S
@@ -0,0 +1,341 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <console_macros.S>
+#include <drivers/renesas/rcar/console/console.h>
+
+#define SCIF_INTERNAL_CLK	0
+#define SCIF_EXTARNAL_CLK	1
+#define SCIF_CLK		SCIF_INTERNAL_CLK
+
+/* product register */
+#define PRR			(0xFFF00044)
+#define PRR_PRODUCT_MASK	(0x00007F00)
+#define PRR_CUT_MASK		(0x000000FF)
+#define PRR_PRODUCT_H3_VER_10	(0x00004F00)
+#define PRR_PRODUCT_E3		(0x00005700)
+#define PRR_PRODUCT_D3		(0x00005800)
+
+/* module stop */
+#define CPG_BASE		(0xE6150000)
+#define CPG_SMSTPCR2		(0x0138)
+#define CPG_SMSTPCR3		(0x013C)
+#define CPG_MSTPSR2		(0x0040)
+#define CPG_MSTPSR3		(0x0048)
+#define MSTP207			(1 << 7)
+#define MSTP310			(1 << 10)
+#define CPG_CPGWPR		(0x0900)
+
+/* scif */
+#define SCIF0_BASE		(0xE6E60000)
+#define SCIF2_BASE		(0xE6E88000)
+#define SCIF_SCSMR		(0x00)
+#define SCIF_SCBRR		(0x04)
+#define SCIF_SCSCR		(0x08)
+#define SCIF_SCFTDR		(0x0C)
+#define SCIF_SCFSR		(0x10)
+#define SCIF_SCFRDR		(0x14)
+#define SCIF_SCFCR		(0x18)
+#define SCIF_SCFDR		(0x1C)
+#define SCIF_SCSPTR		(0x20)
+#define SCIF_SCLSR		(0x24)
+#define SCIF_DL			(0x30)
+#define SCIF_CKS		(0x34)
+
+#if RCAR_LSI == RCAR_V3M
+#define SCIF_BASE		SCIF0_BASE
+#define CPG_SMSTPCR		CPG_SMSTPCR2
+#define CPG_MSTPSR		CPG_MSTPSR2
+#define MSTP			MSTP207
+#else
+#define SCIF_BASE		SCIF2_BASE
+#define CPG_SMSTPCR		CPG_SMSTPCR3
+#define CPG_MSTPSR		CPG_MSTPSR3
+#define MSTP			MSTP310
+#endif
+
+/* mode pin */
+#define RST_MODEMR		(0xE6160060)
+#define MODEMR_MD12		(0x00001000)
+
+#define SCSMR_CA_MASK		(1 << 7)
+#define SCSMR_CA_ASYNC		(0x0000)
+#define SCSMR_CHR_MASK		(1 << 6)
+#define SCSMR_CHR_8		(0x0000)
+#define SCSMR_PE_MASK		(1 << 5)
+#define SCSMR_PE_DIS		(0x0000)
+#define SCSMR_STOP_MASK		(1 << 3)
+#define SCSMR_STOP_1		(0x0000)
+#define SCSMR_CKS_MASK		(3 << 0)
+#define SCSMR_CKS_DIV1		(0x0000)
+#define SCSMR_INIT_DATA		(SCSMR_CA_ASYNC +	\
+					 SCSMR_CHR_8 +		\
+					 SCSMR_PE_DIS +		\
+					 SCSMR_STOP_1 +		\
+					 SCSMR_CKS_DIV1)
+#define SCBRR_115200BPS		(17)
+#define SCBRR_115200BPS_D3_SSCG	(16)
+#define SCBRR_115200BPS_E3_SSCG	(15)
+#define SCBRR_230400BPS		(8)
+
+#define SCSCR_TE_MASK		(1 << 5)
+#define SCSCR_TE_DIS		(0x0000)
+#define SCSCR_TE_EN		(0x0020)
+#define SCSCR_RE_MASK		(1 << 4)
+#define SCSCR_RE_DIS		(0x0000)
+#define SCSCR_RE_EN		(0x0010)
+#define SCSCR_CKE_MASK		(3 << 0)
+#define SCSCR_CKE_INT		(0x0000)
+#define SCSCR_CKE_BRG		(0x0002)
+#if SCIF_CLK == SCIF_EXTARNAL_CLK
+#define SCSCR_CKE_INT_CLK	(SCSCR_CKE_BRG)
+#else
+#define SCFSR_TEND_MASK		(1 << 6)
+#define SCFSR_TEND_TRANS_END	(0x0040)
+#define SCSCR_CKE_INT_CLK	(SCSCR_CKE_INT)
+#endif
+#define SCFSR_INIT_DATA		(0x0000)
+#define SCFCR_TTRG_MASK		(3 << 4)
+#define SCFCR_TTRG_8		(0x0000)
+#define SCFCR_TTRG_0		(0x0030)
+#define SCFCR_TFRST_MASK	(1 << 2)
+#define SCFCR_TFRST_DIS		(0x0000)
+#define SCFCR_TFRST_EN		(0x0004)
+#define SCFCR_RFRS_MASK		(1 << 1)
+#define SCFCR_RFRS_DIS		(0x0000)
+#define SCFCR_RFRS_EN		(0x0002)
+#define SCFCR_INIT_DATA		(SCFCR_TTRG_8)
+#define SCFDR_T_MASK		(0x1f << 8)
+#define DL_INIT_DATA		(8)
+#define CKS_CKS_DIV_MASK	(1 << 15)
+#define CKS_CKS_DIV_CLK		(0x0000)
+#define CKS_XIN_MASK		(1 << 14)
+#define CKS_XIN_SCIF_CLK	(0x0000)
+#define CKS_INIT_DATA		(CKS_CKS_DIV_CLK + CKS_XIN_SCIF_CLK)
+
+	.globl	console_rcar_register
+	.globl	console_rcar_init
+	.globl	console_rcar_putc
+	.globl	console_rcar_flush
+
+	/*
+	 * -----------------------------------------------
+	 * int console_rcar_register(
+	 *      uintptr_t base, uint32_t clk, uint32_t baud,
+	 *      console_t *console)
+	 * Function to initialize and register a new rcar
+	 * 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_rcar_register
+	mov	x7, x30
+	mov	x6, x3
+	cbz	x6, register_fail
+	str	x0, [x6, #CONSOLE_T_BASE]
+
+	bl	console_rcar_init
+
+	mov	x0, x6
+	mov	x30, x7
+	finish_console_register rcar, putc=1, getc=0, flush=1
+
+register_fail:
+	ret	x7
+endfunc console_rcar_register
+
+	/*
+	 * int console_rcar_init(unsigned long 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_rcar_register
+	 * and crash reporting.
+	 * In: x0 - console base address
+	 *     w1 - Uart clock in Hz
+	 *     w2 - Baud rate
+	 * Out: return 1 on success
+	 * Clobber list : x1, x2
+	 */
+func console_rcar_init
+	ldr	x0, =CPG_BASE
+	ldr	w1, [x0, #CPG_SMSTPCR]
+	and	w1, w1, #~MSTP
+	mvn	w2, w1
+	str	w2, [x0, #CPG_CPGWPR]
+	str	w1, [x0, #CPG_SMSTPCR]
+5:
+	ldr w1, [x0, #CPG_MSTPSR]
+	and w1, w1, #MSTP
+	cbnz w1, 5b
+
+	ldr	x0, =SCIF_BASE
+	/* Clear bits TE and RE in SCSCR to 0 */
+	mov	w1, #(SCSCR_TE_DIS + SCSCR_RE_DIS)
+	strh	w1, [x0, #SCIF_SCSCR]
+	/* Set bits TFRST and RFRST in SCFCR to 1 */
+	ldrh	w1, [x0, #SCIF_SCFCR]
+	orr	w1, w1, #(SCFCR_TFRST_EN + SCFCR_RFRS_EN)
+	strh	w1, [x0, #SCIF_SCFCR]
+	/*
+	 * Read flags of ER, DR, BRK, and RDF in SCFSR and those of TO and ORER
+	 * in SCLSR, then clear them to 0
+	 */
+	mov	w1, #SCFSR_INIT_DATA
+	strh	w1, [x0, #SCIF_SCFSR]
+	mov	w1, #0
+	strh	w1, [x0, #SCIF_SCLSR]
+	/* Set bits CKE[1:0] in SCSCR */
+	ldrh	w1, [x0, #SCIF_SCSCR]
+	and	w1, w1, #~SCSCR_CKE_MASK
+	mov	w2, #SCSCR_CKE_INT_CLK
+	orr	w1, w1, w2
+	strh	w1, [x0, #SCIF_SCSCR]
+	/* Set data transfer format in SCSMR */
+	mov	w1, #SCSMR_INIT_DATA
+	strh	w1, [x0, #SCIF_SCSMR]
+	/* Set value in SCBRR */
+#if SCIF_CLK == SCIF_INTERNAL_CLK
+	ldr	x1, =PRR
+	ldr	w1, [x1]
+	and	w1, w1, #(PRR_PRODUCT_MASK | PRR_CUT_MASK)
+	mov	w2, #PRR_PRODUCT_H3_VER_10
+	cmp	w1, w2
+	beq	3f
+	and	w1, w1, #PRR_PRODUCT_MASK
+	mov	w2, #PRR_PRODUCT_D3
+	cmp	w1, w2
+	beq	5f
+	and	w1, w1, #PRR_PRODUCT_MASK
+	mov	w2, #PRR_PRODUCT_E3
+	cmp	w1, w2
+	bne	4f
+
+	/* When SSCG(MD12) on (E3) */
+	ldr	x1, =RST_MODEMR
+	ldr	w1, [x1]
+	and	w1, w1, #MODEMR_MD12
+	mov	w2, #MODEMR_MD12
+	cmp	w1, w2
+	bne	4f
+
+	/* When SSCG(MD12) on (E3) */
+	mov	w1, #SCBRR_115200BPS_E3_SSCG
+	b	2f
+5:
+	/* In case of D3 */
+	ldr	x1, =RST_MODEMR
+	ldr	w1, [x1]
+	and	w1, w1, #MODEMR_MD12
+	mov	w2, #MODEMR_MD12
+	cmp	w1, w2
+	bne	4f
+
+	/* When SSCG(MD12) on (D3) */
+	mov	w1, #SCBRR_115200BPS_D3_SSCG
+	b	2f
+4:
+	/* In case of H3/M3/M3N or when SSCG(MD12) is off in E3/D3 */
+	mov	w1, #SCBRR_115200BPS
+	b	2f
+3:
+	mov	w1, #SCBRR_230400BPS
+2:
+	strb	w1, [x0, SCIF_SCBRR]
+#else
+	mov	w1, #DL_INIT_DATA
+	strh	w1, [x0, #SCIF_DL]
+	mov	w1, #CKS_INIT_DATA
+	strh	w1, [x0, #SCIF_CKS]
+#endif
+	/* 1-bit interval elapsed */
+	mov	w1, #100
+1:
+	subs	w1, w1, #1
+	cbnz	w1, 1b
+	/*
+	 * Set bits RTRG[1:0], TTRG[1:0], and MCE in SCFCR
+	 * Clear bits FRST and RFRST to 0
+	 */
+	mov	w1, #SCFCR_INIT_DATA
+	strh	w1, [x0, #SCIF_SCFCR]
+	/* Set bits TE and RE in SCSCR to 1 */
+	ldrh	w1, [x0, #SCIF_SCSCR]
+	orr	w1, w1, #(SCSCR_TE_EN + SCSCR_RE_EN)
+	strh	w1, [x0, #SCIF_SCSCR]
+	mov	x0, #1
+
+	ret
+endfunc console_rcar_init
+
+	/*
+	 * int console_rcar_putc(int c, unsigned int 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 - pointer to console_t structure
+	 * Out : return -1 on error else return character.
+	 * Clobber list : x2
+	 */
+func console_rcar_putc
+	ldr	x1, =SCIF_BASE
+	cmp	w0, #0xA
+	/* Prepend '\r' to '\n' */
+	bne	2f
+1:
+	/* Check if the transmit FIFO is full */
+	ldrh	w2, [x1, #SCIF_SCFDR]
+	ubfx	w2, w2, #8, #5
+	cmp	w2, #16
+	bcs	1b
+	mov	w2, #0x0D
+	strb	w2, [x1, #SCIF_SCFTDR]
+2:
+	/* Check if the transmit FIFO is full */
+	ldrh	w2, [x1, #SCIF_SCFDR]
+	ubfx	w2, w2, #8, #5
+	cmp	w2, #16
+	bcs	2b
+	strb	w0, [x1, #SCIF_SCFTDR]
+
+	/* Clear TEND flag */
+	ldrh	w2, [x1, #SCIF_SCFSR]
+	and	w2, w2, #~SCFSR_TEND_MASK
+	strh	w2, [x1, #SCIF_SCFSR]
+
+	ret
+endfunc console_rcar_putc
+
+	/*
+	 * void console_rcar_flush(void)
+	 * Function to force a write of all buffered
+	 * data that hasn't been output. It returns void
+	 * Clobber list : x0, x1
+	 */
+func console_rcar_flush
+	ldr	x0, =SCIF_BASE
+1:
+	/* Check TEND flag */
+	ldrh	w1, [x0, #SCIF_SCFSR]
+	and	w1, w1, #SCFSR_TEND_MASK
+	cmp	w1, #SCFSR_TEND_TRANS_END
+	bne	1b
+
+	ldr	x0, =SCIF_BASE
+	ldrh	w1, [x0, #SCIF_SCSCR]
+	and	w1, w1, #~(SCSCR_TE_EN + SCSCR_RE_EN)
+	strh	w1, [x0, #SCIF_SCSCR]
+
+	ret
+endfunc console_rcar_flush
diff --git a/drivers/renesas/common/watchdog/swdt.c b/drivers/renesas/common/watchdog/swdt.c
new file mode 100644
index 0000000..29ef6f4
--- /dev/null
+++ b/drivers/renesas/common/watchdog/swdt.c
@@ -0,0 +1,169 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/gicv2.h>
+#include <lib/mmio.h>
+
+#include "rcar_def.h"
+
+extern void gicd_set_icenabler(uintptr_t base, unsigned int id);
+
+#define RST_BASE			(0xE6160000U)
+#define RST_WDTRSTCR			(RST_BASE + 0x0054U)
+#define SWDT_BASE			(0xE6030000U)
+#define SWDT_WTCNT			(SWDT_BASE + 0x0000U)
+#define SWDT_WTCSRA			(SWDT_BASE + 0x0004U)
+#define SWDT_WTCSRB			(SWDT_BASE + 0x0008U)
+#define SWDT_GICD_BASE			(0xF1010000U)
+#define SWDT_GICC_BASE			(0xF1020000U)
+#define SWDT_GICD_CTLR			(SWDT_GICD_BASE + 0x0000U)
+#define SWDT_GICD_IGROUPR		(SWDT_GICD_BASE + 0x0080U)
+#define SWDT_GICD_ISPRIORITYR		(SWDT_GICD_BASE + 0x0400U)
+#define SWDT_GICC_CTLR			(SWDT_GICC_BASE + 0x0000U)
+#define SWDT_GICC_PMR			(SWDT_GICC_BASE + 0x0004U)
+#define SWDT_GICD_ITARGETSR		(SWDT_GICD_BASE + 0x0800U)
+#define IGROUPR_NUM			(16U)
+#define ISPRIORITY_NUM			(128U)
+#define ITARGET_MASK			(0x03U)
+
+#define WDTRSTCR_UPPER_BYTE		(0xA55A0000U)
+#define WTCSRA_UPPER_BYTE		(0xA5A5A500U)
+#define WTCSRB_UPPER_BYTE		(0xA5A5A500U)
+#define WTCNT_UPPER_BYTE		(0x5A5A0000U)
+#define WTCNT_RESET_VALUE		(0xF488U)
+#define WTCSRA_BIT_CKS			(0x0007U)
+#define WTCSRB_BIT_CKS			(0x003FU)
+#define SWDT_RSTMSK			(1U << 1U)
+#define WTCSRA_WOVFE			(1U << 3U)
+#define WTCSRA_WRFLG			(1U << 5U)
+#define SWDT_ENABLE			(1U << 7U)
+
+#define WDTRSTCR_MASK_ALL		(0x0000FFFFU)
+#define WTCSRA_MASK_ALL			(0x000000FFU)
+#define WTCNT_INIT_DATA			(WTCNT_UPPER_BYTE + WTCNT_RESET_VALUE)
+#define WTCSRA_INIT_DATA		(WTCSRA_UPPER_BYTE + 0x0FU)
+#define WTCSRB_INIT_DATA		(WTCSRB_UPPER_BYTE + 0x21U)
+
+#if RCAR_LSI == RCAR_D3
+#define WTCNT_COUNT_8p13k		(0x10000U - 40760U)
+#else
+#define WTCNT_COUNT_8p13k		(0x10000U - 40687U)
+#endif
+#define WTCNT_COUNT_8p13k_H3VER10	(0x10000U - 20343U)
+#define WTCNT_COUNT_8p22k		(0x10000U - 41115U)
+#define WTCNT_COUNT_7p81k		(0x10000U - 39062U)
+#define WTCSRA_CKS_DIV16		(0x00000002U)
+
+static void swdt_disable(void)
+{
+	uint32_t rmsk;
+
+	rmsk = mmio_read_32(RST_WDTRSTCR) & WDTRSTCR_MASK_ALL;
+	rmsk |= SWDT_RSTMSK;
+	mmio_write_32(RST_WDTRSTCR, WDTRSTCR_UPPER_BYTE | rmsk);
+
+	mmio_write_32(SWDT_WTCNT, WTCNT_INIT_DATA);
+	mmio_write_32(SWDT_WTCSRA, WTCSRA_INIT_DATA);
+	mmio_write_32(SWDT_WTCSRB, WTCSRB_INIT_DATA);
+
+	/* Set the interrupt clear enable register */
+	gicd_set_icenabler(RCAR_GICD_BASE, ARM_IRQ_SEC_WDT);
+}
+
+void rcar_swdt_init(void)
+{
+	uint32_t rmsk, sr;
+#if (RCAR_LSI != RCAR_E3) && (RCAR_LSI != RCAR_D3) && (RCAR_LSI != RZ_G2E)
+	uint32_t reg, val, product_cut, chk_data;
+
+	reg = mmio_read_32(RCAR_PRR);
+	product_cut = reg & (PRR_PRODUCT_MASK | PRR_CUT_MASK);
+
+	reg = mmio_read_32(RCAR_MODEMR);
+	chk_data = reg & CHECK_MD13_MD14;
+#endif
+	/* stop watchdog */
+	if (mmio_read_32(SWDT_WTCSRA) & SWDT_ENABLE)
+		mmio_write_32(SWDT_WTCSRA, WTCSRA_UPPER_BYTE);
+
+	mmio_write_32(SWDT_WTCSRA, WTCSRA_UPPER_BYTE |
+		      WTCSRA_WOVFE | WTCSRA_CKS_DIV16);
+
+#if (RCAR_LSI == RCAR_E3) || (RCAR_LSI == RZ_G2E)
+	mmio_write_32(SWDT_WTCNT, WTCNT_UPPER_BYTE | WTCNT_COUNT_7p81k);
+#elif (RCAR_LSI == RCAR_D3)
+	mmio_write_32(SWDT_WTCNT, WTCNT_UPPER_BYTE | WTCNT_COUNT_8p13k);
+#else
+	val = WTCNT_UPPER_BYTE;
+
+	switch (chk_data) {
+	case MD14_MD13_TYPE_0:
+	case MD14_MD13_TYPE_2:
+		val |= WTCNT_COUNT_8p13k;
+		break;
+	case MD14_MD13_TYPE_1:
+		val |= WTCNT_COUNT_8p22k;
+		break;
+	case MD14_MD13_TYPE_3:
+		val |= product_cut == (PRR_PRODUCT_H3 | PRR_PRODUCT_10) ?
+		    WTCNT_COUNT_8p13k_H3VER10 : WTCNT_COUNT_8p13k;
+		break;
+	default:
+		ERROR("MODEMR ERROR value = %x\n", chk_data);
+		panic();
+		break;
+	}
+
+	mmio_write_32(SWDT_WTCNT, val);
+#endif
+	rmsk = mmio_read_32(RST_WDTRSTCR) & WDTRSTCR_MASK_ALL;
+	rmsk |= SWDT_RSTMSK | WDTRSTCR_UPPER_BYTE;
+	mmio_write_32(RST_WDTRSTCR, rmsk);
+
+	while ((mmio_read_8(SWDT_WTCSRA) & WTCSRA_WRFLG) != 0U)
+		;
+
+	/* Start the System WatchDog Timer */
+	sr = mmio_read_32(SWDT_WTCSRA) & WTCSRA_MASK_ALL;
+	mmio_write_32(SWDT_WTCSRA, (WTCSRA_UPPER_BYTE | sr | SWDT_ENABLE));
+}
+
+void rcar_swdt_release(void)
+{
+	uintptr_t itarget = SWDT_GICD_ITARGETSR +
+	    (ARM_IRQ_SEC_WDT & ~ITARGET_MASK);
+	uint32_t i;
+
+	/* Disable FIQ interrupt */
+	write_daifset(DAIF_FIQ_BIT);
+	/* FIQ interrupts are not taken to EL3 */
+	write_scr_el3(read_scr_el3() & ~SCR_FIQ_BIT);
+
+	swdt_disable();
+	gicv2_cpuif_disable();
+
+	for (i = 0; i < IGROUPR_NUM; i++)
+		mmio_write_32(SWDT_GICD_IGROUPR + i * 4, 0U);
+
+	for (i = 0; i < ISPRIORITY_NUM; i++)
+		mmio_write_32(SWDT_GICD_ISPRIORITYR + i * 4, 0U);
+
+	mmio_write_32(itarget, 0U);
+	mmio_write_32(SWDT_GICD_CTLR, 0U);
+	mmio_write_32(SWDT_GICC_CTLR, 0U);
+	mmio_write_32(SWDT_GICC_PMR, 0U);
+}
+
+void rcar_swdt_exec(uint64_t p)
+{
+	gicv2_end_of_interrupt(ARM_IRQ_SEC_WDT);
+	rcar_swdt_release();
+	ERROR("\n");
+	ERROR("System WDT overflow, occurred address is %p\n", (void *)p);
+	panic();
+}
diff --git a/drivers/renesas/rcar/board/board.c b/drivers/renesas/rcar/board/board.c
new file mode 100644
index 0000000..dbbaed6
--- /dev/null
+++ b/drivers/renesas/rcar/board/board.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights
+ * reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <lib/utils_def.h>
+
+#include <iic_dvfs.h>
+
+#include "board.h"
+
+#ifndef BOARD_DEFAULT
+#if (RCAR_LSI == RCAR_D3)
+#define BOARD_DEFAULT		(BOARD_DRAAK << BOARD_CODE_SHIFT)
+#elif (RCAR_LSI == RCAR_E3)
+#define BOARD_DEFAULT		(BOARD_EBISU << BOARD_CODE_SHIFT)
+#elif (RCAR_LSI == RCAR_V3M)
+#define BOARD_DEFAULT		(BOARD_EAGLE << BOARD_CODE_SHIFT)
+#else
+#define BOARD_DEFAULT		(BOARD_SALVATOR_X << BOARD_CODE_SHIFT)
+#endif
+#endif
+
+#define BOARD_CODE_MASK		(0xF8)
+#define BOARD_REV_MASK		(0x07)
+#define BOARD_CODE_SHIFT	(0x03)
+#define BOARD_ID_UNKNOWN	(0xFF)
+
+#define SXS_ID	{ 0x10U, 0x11U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU }
+#define SX_ID	{ 0x10U, 0x11U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU }
+#define SKP_ID	{ 0x10U, 0x10U, 0x20U, 0x21U, 0xFFU, 0xFFU, 0xFFU, 0xFFU }
+#define SK_ID	{ 0x10U, 0x30U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU }
+#define EB4_ID	{ 0x10U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU }
+#define EB_ID	{ 0x10U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU }
+#define DR_ID	{ 0x10U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU }
+#define EA_ID	{ 0x10U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU }
+#define KK_ID	{ 0x10U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU }
+
+const char *g_board_tbl[] = {
+	[BOARD_STARTER_KIT_PRE] = "Starter Kit Premier",
+	[BOARD_STARTER_KIT] = "Starter Kit",
+	[BOARD_SALVATOR_XS] = "Salvator-XS",
+	[BOARD_SALVATOR_X] = "Salvator-X",
+	[BOARD_EBISU_4D] = "Ebisu-4D",
+	[BOARD_KRIEK] = "Kriek",
+	[BOARD_EBISU] = "Ebisu",
+	[BOARD_DRAAK] = "Draak",
+	[BOARD_EAGLE] = "Eagle",
+	[BOARD_UNKNOWN] = "unknown"
+};
+
+int32_t rcar_get_board_type(uint32_t *type, uint32_t *rev)
+{
+	int32_t ret = 0;
+	const uint8_t board_tbl[][8] = {
+		[BOARD_STARTER_KIT_PRE] = SKP_ID,
+		[BOARD_SALVATOR_XS] = SXS_ID,
+		[BOARD_STARTER_KIT] = SK_ID,
+		[BOARD_SALVATOR_X] = SX_ID,
+		[BOARD_EBISU_4D] = EB4_ID,
+		[BOARD_EBISU] = EB_ID,
+		[BOARD_DRAAK] = DR_ID,
+		[BOARD_EAGLE] = EA_ID,
+		[BOARD_KRIEK] = KK_ID,
+	};
+	static uint8_t board_id = BOARD_ID_UNKNOWN;
+
+	if (board_id != BOARD_ID_UNKNOWN)
+		goto get_type;
+
+#if PMIC_ROHM_BD9571
+	/* Board ID detection from EEPROM */
+	ret = rcar_iic_dvfs_receive(EEPROM, BOARD_ID, &board_id);
+	if (ret) {
+		board_id = BOARD_ID_UNKNOWN;
+		goto get_type;
+	}
+
+	if (board_id == BOARD_ID_UNKNOWN)
+		board_id = BOARD_DEFAULT;
+#else
+	board_id = BOARD_DEFAULT;
+#endif
+
+get_type:
+	*type = ((uint32_t) board_id & BOARD_CODE_MASK) >> BOARD_CODE_SHIFT;
+
+	if (*type >= ARRAY_SIZE(board_tbl)) {
+		/* no revision information, set Rev0.0. */
+		*rev = 0;
+		return ret;
+	}
+
+	*rev = board_tbl[*type][(uint8_t) (board_id & BOARD_REV_MASK)];
+
+	return ret;
+}
diff --git a/drivers/renesas/rcar/board/board.h b/drivers/renesas/rcar/board/board.h
new file mode 100644
index 0000000..51a8e30
--- /dev/null
+++ b/drivers/renesas/rcar/board/board.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2015-2018, Renesas Electronics Corporation. All rights
+ * reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef BOARD_H
+#define BOARD_H
+
+#define BOARD_SALVATOR_X		(0x00)
+#define BOARD_KRIEK			(0x01)
+#define BOARD_STARTER_KIT		(0x02)
+#define BOARD_SALVATOR_XS		(0x04)
+#define BOARD_EBISU			(0x08)
+#define BOARD_STARTER_KIT_PRE		(0x0B)
+#define BOARD_EBISU_4D			(0x0DU)
+#define BOARD_DRAAK			(0x0EU)
+#define BOARD_EAGLE			(0x0FU)
+#define BOARD_UNKNOWN			(BOARD_EAGLE + 1U)
+
+#define BOARD_REV_UNKNOWN		(0xFF)
+
+extern const char *g_board_tbl[];
+
+/************************************************************************
+ * Revisions are expressed in 8 bits.
+ *  The upper 4 bits are major version.
+ *  The lower 4 bits are minor version.
+ ************************************************************************/
+#define GET_BOARD_MAJOR(a)	((uint32_t)(a) >> 0x4)
+#define GET_BOARD_MINOR(a)	((uint32_t)(a) &  0xF)
+#define GET_BOARD_NAME(a)	(g_board_tbl[(a)])
+
+int32_t rcar_get_board_type(uint32_t *type, uint32_t *rev);
+
+#endif /* BOARD_H */
diff --git a/drivers/renesas/rcar/cpld/ulcb_cpld.c b/drivers/renesas/rcar/cpld/ulcb_cpld.c
new file mode 100644
index 0000000..5ffb2e1
--- /dev/null
+++ b/drivers/renesas/rcar/cpld/ulcb_cpld.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+#include "ulcb_cpld.h"
+
+#define SCLK			8	/* GP_6_8 */
+#define SSTBZ			3	/* GP_2_3 */
+#define MOSI			7	/* GP_6_7 */
+
+#define CPLD_ADDR_RESET		0x80	/* RW */
+
+/* LSI Multiplexed Pin Setting Mask Register */
+#define PFC_PMMR		0xE6060000
+
+/* General output registers */
+#define GPIO_OUTDT2		0xE6052008
+#define GPIO_OUTDT6		0xE6055408
+
+/* General input/output switching registers */
+#define GPIO_INOUTSEL2		0xE6052004
+#define GPIO_INOUTSEL6		0xE6055404
+
+/* General IO/Interrupt Switching Register */
+#define GPIO_IOINTSEL6		0xE6055400
+
+/* GPIO/perihperal function select */
+#define PFC_GPSR2		0xE6060108
+#define PFC_GPSR6		0xE6060118
+
+static void gpio_set_value(uint32_t addr, uint8_t gpio, uint32_t val)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(addr);
+	if (val)
+		reg |= (1 << gpio);
+	else
+		reg &= ~(1 << gpio);
+	mmio_write_32(addr, reg);
+}
+
+static void gpio_direction_output(uint32_t addr, uint8_t gpio)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(addr);
+	reg |= (1 << gpio);
+	mmio_write_32(addr, reg);
+}
+
+static void gpio_pfc(uint32_t addr, uint8_t gpio)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(addr);
+	reg &= ~(1 << gpio);
+	mmio_write_32(PFC_PMMR, ~reg);
+	mmio_write_32(addr, reg);
+}
+
+static void cpld_write(uint8_t addr, uint32_t data)
+{
+	int i;
+
+	for (i = 0; i < 32; i++) {
+		/* MSB first */
+		gpio_set_value(GPIO_OUTDT6, MOSI, data & (1U << 31));
+		gpio_set_value(GPIO_OUTDT6, SCLK, 1);
+		data <<= 1;
+		gpio_set_value(GPIO_OUTDT6, SCLK, 0);
+	}
+
+	for (i = 0; i < 8; i++) {
+		/* MSB first */
+		gpio_set_value(GPIO_OUTDT6, MOSI, addr & 0x80);
+		gpio_set_value(GPIO_OUTDT6, SCLK, 1);
+		addr <<= 1;
+		gpio_set_value(GPIO_OUTDT6, SCLK, 0);
+	}
+
+	/* WRITE */
+	gpio_set_value(GPIO_OUTDT6, MOSI, 1);
+	gpio_set_value(GPIO_OUTDT2, SSTBZ, 0);
+	gpio_set_value(GPIO_OUTDT6, SCLK, 1);
+	gpio_set_value(GPIO_OUTDT6, SCLK, 0);
+	gpio_set_value(GPIO_OUTDT2, SSTBZ, 1);
+}
+
+static void cpld_init(void)
+{
+	gpio_pfc(PFC_GPSR6, SCLK);
+	gpio_pfc(PFC_GPSR2, SSTBZ);
+	gpio_pfc(PFC_GPSR6, MOSI);
+
+	gpio_set_value(GPIO_IOINTSEL6, SCLK, 0);
+	gpio_set_value(GPIO_OUTDT6, SCLK, 0);
+	gpio_set_value(GPIO_OUTDT2, SSTBZ, 1);
+	gpio_set_value(GPIO_OUTDT6, MOSI, 0);
+
+	gpio_direction_output(GPIO_INOUTSEL6, SCLK);
+	gpio_direction_output(GPIO_INOUTSEL2, SSTBZ);
+	gpio_direction_output(GPIO_INOUTSEL6, MOSI);
+}
+
+void rcar_cpld_reset_cpu(void)
+{
+	cpld_init();
+
+	cpld_write(CPLD_ADDR_RESET, 1);
+}
diff --git a/drivers/renesas/rcar/cpld/ulcb_cpld.h b/drivers/renesas/rcar/cpld/ulcb_cpld.h
new file mode 100644
index 0000000..1616d71
--- /dev/null
+++ b/drivers/renesas/rcar/cpld/ulcb_cpld.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RCAR_ULCB_CPLD_H__
+#define RCAR_ULCB_CPLD_H__
+
+extern void rcar_cpld_reset_cpu(void);
+
+#endif /* RCAR_ULCB_CPLD_H__ */
diff --git a/drivers/renesas/rcar/pfc/D3/pfc_init_d3.c b/drivers/renesas/rcar/pfc/D3/pfc_init_d3.c
new file mode 100644
index 0000000..aaa3b43
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/D3/pfc_init_d3.c
@@ -0,0 +1,667 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <lib/mmio.h>
+#include "pfc_init_d3.h"
+#include "rcar_def.h"
+#include "../pfc_regs.h"
+
+/* PFC */
+#define GPSR0_D15		BIT(15)
+#define GPSR0_D14		BIT(14)
+#define GPSR0_D13		BIT(13)
+#define GPSR0_D12		BIT(12)
+#define GPSR0_D11		BIT(11)
+#define GPSR0_D10		BIT(10)
+#define GPSR0_D9		BIT(9)
+#define GPSR0_D8		BIT(8)
+#define GPSR0_D7		BIT(7)
+#define GPSR0_D6		BIT(6)
+#define GPSR0_D5		BIT(5)
+#define GPSR0_D4		BIT(4)
+#define GPSR0_D3		BIT(3)
+#define GPSR0_D2		BIT(2)
+#define GPSR0_D1		BIT(1)
+#define GPSR0_D0		BIT(0)
+#define GPSR1_CLKOUT		BIT(28)
+#define GPSR1_EX_WAIT0_A	BIT(27)
+#define GPSR1_WE1		BIT(26)
+#define GPSR1_WE0		BIT(25)
+#define GPSR1_RD_WR		BIT(24)
+#define GPSR1_RD		BIT(23)
+#define GPSR1_BS		BIT(22)
+#define GPSR1_CS1_A26		BIT(21)
+#define GPSR1_CS0		BIT(20)
+#define GPSR1_A19		BIT(19)
+#define GPSR1_A18		BIT(18)
+#define GPSR1_A17		BIT(17)
+#define GPSR1_A16		BIT(16)
+#define GPSR1_A15		BIT(15)
+#define GPSR1_A14		BIT(14)
+#define GPSR1_A13		BIT(13)
+#define GPSR1_A12		BIT(12)
+#define GPSR1_A11		BIT(11)
+#define GPSR1_A10		BIT(10)
+#define GPSR1_A9		BIT(9)
+#define GPSR1_A8		BIT(8)
+#define GPSR1_A7		BIT(7)
+#define GPSR1_A6		BIT(6)
+#define GPSR1_A5		BIT(5)
+#define GPSR1_A4		BIT(4)
+#define GPSR1_A3		BIT(3)
+#define GPSR1_A2		BIT(2)
+#define GPSR1_A1		BIT(1)
+#define GPSR1_A0		BIT(0)
+#define GPSR2_AVB_AVTP_CAPTURE_A	BIT(14)
+#define GPSR2_AVB_AVTP_MATCH_A	BIT(13)
+#define GPSR2_AVB_LINK		BIT(12)
+#define GPSR2_AVB_PHY_INT	BIT(11)
+#define GPSR2_AVB_MAGIC		BIT(10)
+#define GPSR2_AVB_MDC		BIT(9)
+#define GPSR2_PWM2_A		BIT(8)
+#define GPSR2_PWM1_A		BIT(7)
+#define GPSR2_PWM0		BIT(6)
+#define GPSR2_IRQ5		BIT(5)
+#define GPSR2_IRQ4		BIT(4)
+#define GPSR2_IRQ3		BIT(3)
+#define GPSR2_IRQ2		BIT(2)
+#define GPSR2_IRQ1		BIT(1)
+#define GPSR2_IRQ0		BIT(0)
+#define GPSR3_SD1_WP		BIT(15)
+#define GPSR3_SD1_CD		BIT(14)
+#define GPSR3_SD0_WP		BIT(13)
+#define GPSR3_SD0_CD		BIT(12)
+#define GPSR3_SD1_DAT3		BIT(11)
+#define GPSR3_SD1_DAT2		BIT(10)
+#define GPSR3_SD1_DAT1		BIT(9)
+#define GPSR3_SD1_DAT0		BIT(8)
+#define GPSR3_SD1_CMD		BIT(7)
+#define GPSR3_SD1_CLK		BIT(6)
+#define GPSR3_SD0_DAT3		BIT(5)
+#define GPSR3_SD0_DAT2		BIT(4)
+#define GPSR3_SD0_DAT1		BIT(3)
+#define GPSR3_SD0_DAT0		BIT(2)
+#define GPSR3_SD0_CMD		BIT(1)
+#define GPSR3_SD0_CLK		BIT(0)
+#define GPSR4_SD3_DS		BIT(17)
+#define GPSR4_SD3_DAT7		BIT(16)
+#define GPSR4_SD3_DAT6		BIT(15)
+#define GPSR4_SD3_DAT5		BIT(14)
+#define GPSR4_SD3_DAT4		BIT(13)
+#define GPSR4_SD3_DAT3		BIT(12)
+#define GPSR4_SD3_DAT2		BIT(11)
+#define GPSR4_SD3_DAT1		BIT(10)
+#define GPSR4_SD3_DAT0		BIT(9)
+#define GPSR4_SD3_CMD		BIT(8)
+#define GPSR4_SD3_CLK		BIT(7)
+#define GPSR4_SD2_DS		BIT(6)
+#define GPSR4_SD2_DAT3		BIT(5)
+#define GPSR4_SD2_DAT2		BIT(4)
+#define GPSR4_SD2_DAT1		BIT(3)
+#define GPSR4_SD2_DAT0		BIT(2)
+#define GPSR4_SD2_CMD		BIT(1)
+#define GPSR4_SD2_CLK		BIT(0)
+#define GPSR5_MLB_DAT		BIT(25)
+#define GPSR5_MLB_SIG		BIT(24)
+#define GPSR5_MLB_CLK		BIT(23)
+#define GPSR5_MSIOF0_RXD	BIT(22)
+#define GPSR5_MSIOF0_SS2	BIT(21)
+#define GPSR5_MSIOF0_TXD	BIT(20)
+#define GPSR5_MSIOF0_SS1	BIT(19)
+#define GPSR5_MSIOF0_SYNC	BIT(18)
+#define GPSR5_MSIOF0_SCK	BIT(17)
+#define GPSR5_HRTS0		BIT(16)
+#define GPSR5_HCTS0		BIT(15)
+#define GPSR5_HTX0		BIT(14)
+#define GPSR5_HRX0		BIT(13)
+#define GPSR5_HSCK0		BIT(12)
+#define GPSR5_RX2_A		BIT(11)
+#define GPSR5_TX2_A		BIT(10)
+#define GPSR5_SCK2		BIT(9)
+#define GPSR5_RTS1_TANS		BIT(8)
+#define GPSR5_CTS1		BIT(7)
+#define GPSR5_TX1_A		BIT(6)
+#define GPSR5_RX1_A		BIT(5)
+#define GPSR5_RTS0_TANS		BIT(4)
+#define GPSR5_CTS0		BIT(3)
+#define GPSR5_TX0		BIT(2)
+#define GPSR5_RX0		BIT(1)
+#define GPSR5_SCK0		BIT(0)
+#define GPSR6_USB31_OVC		BIT(31)
+#define GPSR6_USB31_PWEN	BIT(30)
+#define GPSR6_USB30_OVC		BIT(29)
+#define GPSR6_USB30_PWEN	BIT(28)
+#define GPSR6_USB1_OVC		BIT(27)
+#define GPSR6_USB1_PWEN		BIT(26)
+#define GPSR6_USB0_OVC		BIT(25)
+#define GPSR6_USB0_PWEN		BIT(24)
+#define GPSR6_AUDIO_CLKB_B	BIT(23)
+#define GPSR6_AUDIO_CLKA_A	BIT(22)
+#define GPSR6_SSI_SDATA9_A	BIT(21)
+#define GPSR6_SSI_SDATA8	BIT(20)
+#define GPSR6_SSI_SDATA7	BIT(19)
+#define GPSR6_SSI_WS78		BIT(18)
+#define GPSR6_SSI_SCK78		BIT(17)
+#define GPSR6_SSI_SDATA6	BIT(16)
+#define GPSR6_SSI_WS6		BIT(15)
+#define GPSR6_SSI_SCK6		BIT(14)
+#define GPSR6_SSI_SDATA5	BIT(13)
+#define GPSR6_SSI_WS5		BIT(12)
+#define GPSR6_SSI_SCK5		BIT(11)
+#define GPSR6_SSI_SDATA4	BIT(10)
+#define GPSR6_SSI_WS4		BIT(9)
+#define GPSR6_SSI_SCK4		BIT(8)
+#define GPSR6_SSI_SDATA3	BIT(7)
+#define GPSR6_SSI_WS34		BIT(6)
+#define GPSR6_SSI_SCK34		BIT(5)
+#define GPSR6_SSI_SDATA2_A	BIT(4)
+#define GPSR6_SSI_SDATA1_A	BIT(3)
+#define GPSR6_SSI_SDATA0	BIT(2)
+#define GPSR6_SSI_WS0129	BIT(1)
+#define GPSR6_SSI_SCK0129	BIT(0)
+#define GPSR7_HDMI1_CEC		BIT(3)
+#define GPSR7_HDMI0_CEC		BIT(2)
+#define GPSR7_AVS2		BIT(1)
+#define GPSR7_AVS1		BIT(0)
+
+#define IPSR_28_FUNC(x)		((uint32_t)(x) << 28U)
+#define IPSR_24_FUNC(x)		((uint32_t)(x) << 24U)
+#define IPSR_20_FUNC(x)		((uint32_t)(x) << 20U)
+#define IPSR_16_FUNC(x)		((uint32_t)(x) << 16U)
+#define IPSR_12_FUNC(x)		((uint32_t)(x) << 12U)
+#define IPSR_8_FUNC(x)		((uint32_t)(x) << 8U)
+#define IPSR_4_FUNC(x)		((uint32_t)(x) << 4U)
+#define IPSR_0_FUNC(x)		((uint32_t)(x) << 0U)
+
+#define POC_SD3_DS_33V		BIT(29)
+#define POC_SD3_DAT7_33V	BIT(28)
+#define POC_SD3_DAT6_33V	BIT(27)
+#define POC_SD3_DAT5_33V	BIT(26)
+#define POC_SD3_DAT4_33V	BIT(25)
+#define POC_SD3_DAT3_33V	BIT(24)
+#define POC_SD3_DAT2_33V	BIT(23)
+#define POC_SD3_DAT1_33V	BIT(22)
+#define POC_SD3_DAT0_33V	BIT(21)
+#define POC_SD3_CMD_33V		BIT(20)
+#define POC_SD3_CLK_33V		BIT(19)
+#define POC_SD2_DS_33V		BIT(18)
+#define POC_SD2_DAT3_33V	BIT(17)
+#define POC_SD2_DAT2_33V	BIT(16)
+#define POC_SD2_DAT1_33V	BIT(15)
+#define POC_SD2_DAT0_33V	BIT(14)
+#define POC_SD2_CMD_33V		BIT(13)
+#define POC_SD2_CLK_33V		BIT(12)
+#define POC_SD1_DAT3_33V	BIT(11)
+#define POC_SD1_DAT2_33V	BIT(10)
+#define POC_SD1_DAT1_33V	BIT(9)
+#define POC_SD1_DAT0_33V	BIT(8)
+#define POC_SD1_CMD_33V		BIT(7)
+#define POC_SD1_CLK_33V		BIT(6)
+#define POC_SD0_DAT3_33V	BIT(5)
+#define POC_SD0_DAT2_33V	BIT(4)
+#define POC_SD0_DAT1_33V	BIT(3)
+#define POC_SD0_DAT0_33V	BIT(2)
+#define POC_SD0_CMD_33V		BIT(1)
+#define POC_SD0_CLK_33V		BIT(0)
+
+#define DRVCTRL0_MASK		(0xCCCCCCCCU)
+#define DRVCTRL1_MASK		(0xCCCCCCC8U)
+#define DRVCTRL2_MASK		(0x88888888U)
+#define DRVCTRL3_MASK		(0x88888888U)
+#define DRVCTRL4_MASK		(0x88888888U)
+#define DRVCTRL5_MASK		(0x88888888U)
+#define DRVCTRL6_MASK		(0x88888888U)
+#define DRVCTRL7_MASK		(0x88888888U)
+#define DRVCTRL8_MASK		(0x88888888U)
+#define DRVCTRL9_MASK		(0x88888888U)
+#define DRVCTRL10_MASK		(0x88888888U)
+#define DRVCTRL11_MASK		(0x888888CCU)
+#define DRVCTRL12_MASK		(0xCCCFFFCFU)
+#define DRVCTRL13_MASK		(0xCC888888U)
+#define DRVCTRL14_MASK		(0x88888888U)
+#define DRVCTRL15_MASK		(0x88888888U)
+#define DRVCTRL16_MASK		(0x88888888U)
+#define DRVCTRL17_MASK		(0x88888888U)
+#define DRVCTRL18_MASK		(0x88888888U)
+#define DRVCTRL19_MASK		(0x88888888U)
+#define DRVCTRL20_MASK		(0x88888888U)
+#define DRVCTRL21_MASK		(0x88888888U)
+#define DRVCTRL22_MASK		(0x88888888U)
+#define DRVCTRL23_MASK		(0x88888888U)
+#define DRVCTRL24_MASK		(0x8888888FU)
+
+#define DRVCTRL0_QSPI0_SPCLK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL0_QSPI0_MOSI_IO0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL0_QSPI0_MISO_IO1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL0_QSPI0_IO2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL0_QSPI0_IO3(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL0_QSPI0_SSL(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL0_QSPI1_SPCLK(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL0_QSPI1_MOSI_IO0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL1_QSPI1_MISO_IO1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL1_QSPI1_IO2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL1_QSPI1_IO3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL1_QSPI1_SS(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL1_RPC_INT(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL1_RPC_WP(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL1_RPC_RESET(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL1_AVB_RX_CTL(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL2_AVB_RXC(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL2_AVB_RD0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL2_AVB_RD1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL2_AVB_RD2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL2_AVB_RD3(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL2_AVB_TX_CTL(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL2_AVB_TXC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL2_AVB_TD0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL3_AVB_TD1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL3_AVB_TD2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL3_AVB_TD3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL3_AVB_TXCREFCLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL3_AVB_MDIO(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL3_AVB_MDC(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL3_AVB_MAGIC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL3_AVB_PHY_INT(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL4_AVB_LINK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL4_AVB_AVTP_MATCH(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL4_AVB_AVTP_CAPTURE(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL4_IRQ0(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL4_IRQ1(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL4_IRQ2(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL4_IRQ3(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL4_IRQ4(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL5_IRQ5(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL5_PWM0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL5_PWM1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL5_PWM2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL5_A0(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL5_A1(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL5_A2(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL5_A3(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL6_A4(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL6_A5(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL6_A6(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL6_A7(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL6_A8(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL6_A9(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL6_A10(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL6_A11(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL7_A12(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL7_A13(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL7_A14(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL7_A15(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL7_A16(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL7_A17(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL7_A18(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL7_A19(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL8_CLKOUT(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL8_CS0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL8_CS1_A2(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL8_BS(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL8_RD(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL8_RD_W(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL8_WE0(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL8_WE1(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL9_EX_WAIT0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL9_PRESETOU(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL9_D0(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL9_D1(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL9_D2(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL9_D3(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL9_D4(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL9_D5(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL10_D6(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL10_D7(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL10_D8(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL10_D9(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL10_D10(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL10_D11(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL10_D12(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL10_D13(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL11_D14(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL11_D15(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL11_AVS1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL11_AVS2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL11_HDMI0_CEC(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL11_HDMI1_CEC(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL11_DU_DOTCLKIN0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL11_DU_DOTCLKIN1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL12_DU_DOTCLKIN2(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL12_DU_DOTCLKIN3(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL12_DU_FSCLKST(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL12_DU_TMS(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL13_TDO(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL13_ASEBRK(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL13_SD0_CLK(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL13_SD0_CMD(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL13_SD0_DAT0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL13_SD0_DAT1(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL13_SD0_DAT2(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL13_SD0_DAT3(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL14_SD1_CLK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL14_SD1_CMD(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL14_SD1_DAT0(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL14_SD1_DAT1(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL14_SD1_DAT2(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL14_SD1_DAT3(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL14_SD2_CLK(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL14_SD2_CMD(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL15_SD2_DAT0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL15_SD2_DAT1(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL15_SD2_DAT2(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL15_SD2_DAT3(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL15_SD2_DS(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL15_SD3_CLK(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL15_SD3_CMD(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL15_SD3_DAT0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL16_SD3_DAT1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL16_SD3_DAT2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL16_SD3_DAT3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL16_SD3_DAT4(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL16_SD3_DAT5(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL16_SD3_DAT6(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL16_SD3_DAT7(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL16_SD3_DS(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL17_SD0_CD(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL17_SD0_WP(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL17_SD1_CD(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL17_SD1_WP(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL17_SCK0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL17_RX0(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL17_TX0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL17_CTS0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL18_RTS0_TANS(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL18_RX1(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL18_TX1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL18_CTS1(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL18_RTS1_TANS(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL18_SCK2(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL18_TX2(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL18_RX2(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL19_HSCK0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL19_HRX0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL19_HTX0(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL19_HCTS0(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL19_HRTS0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL19_MSIOF0_SCK(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL19_MSIOF0_SYNC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL19_MSIOF0_SS1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL20_MSIOF0_TXD(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL20_MSIOF0_SS2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL20_MSIOF0_RXD(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL20_MLB_CLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL20_MLB_SIG(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL20_MLB_DAT(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL20_MLB_REF(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL20_SSI_SCK0129(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL21_SSI_WS0129(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL21_SSI_SDATA0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL21_SSI_SDATA1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL21_SSI_SDATA2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL21_SSI_SCK34(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL21_SSI_WS34(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL21_SSI_SDATA3(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL21_SSI_SCK4(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL22_SSI_WS4(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL22_SSI_SDATA4(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL22_SSI_SCK5(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL22_SSI_WS5(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL22_SSI_SDATA5(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL22_SSI_SCK6(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL22_SSI_WS6(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL22_SSI_SDATA6(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL23_SSI_SCK78(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL23_SSI_WS78(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL23_SSI_SDATA7(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL23_SSI_SDATA8(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL23_SSI_SDATA9(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL23_AUDIO_CLKA(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL23_AUDIO_CLKB(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL23_USB0_PWEN(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL24_USB0_OVC(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL24_USB1_PWEN(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL24_USB1_OVC(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL24_USB30_PWEN(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL24_USB30_OVC(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL24_USB31_PWEN(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL24_USB31_OVC(x)	((uint32_t)(x) << 4U)
+
+#define MOD_SEL0_MSIOF3_A	((uint32_t)0U << 29U)
+#define MOD_SEL0_MSIOF3_B	((uint32_t)1U << 29U)
+#define MOD_SEL0_MSIOF3_C	((uint32_t)2U << 29U)
+#define MOD_SEL0_MSIOF3_D	((uint32_t)3U << 29U)
+#define MOD_SEL0_MSIOF3_E	((uint32_t)4U << 29U)
+#define MOD_SEL0_MSIOF2_A	((uint32_t)0U << 27U)
+#define MOD_SEL0_MSIOF2_B	((uint32_t)1U << 27U)
+#define MOD_SEL0_MSIOF2_C	((uint32_t)2U << 27U)
+#define MOD_SEL0_MSIOF2_D	((uint32_t)3U << 27U)
+#define MOD_SEL0_MSIOF1_A	((uint32_t)0U << 24U)
+#define MOD_SEL0_MSIOF1_B	((uint32_t)1U << 24U)
+#define MOD_SEL0_MSIOF1_C	((uint32_t)2U << 24U)
+#define MOD_SEL0_MSIOF1_D	((uint32_t)3U << 24U)
+#define MOD_SEL0_MSIOF1_E	((uint32_t)4U << 24U)
+#define MOD_SEL0_MSIOF1_F	((uint32_t)5U << 24U)
+#define MOD_SEL0_MSIOF1_G	((uint32_t)6U << 24U)
+#define MOD_SEL0_LBSC_A		((uint32_t)0U << 23U)
+#define MOD_SEL0_LBSC_B		((uint32_t)1U << 23U)
+#define MOD_SEL0_IEBUS_A	((uint32_t)0U << 22U)
+#define MOD_SEL0_IEBUS_B	((uint32_t)1U << 22U)
+#define MOD_SEL0_I2C2_A		((uint32_t)0U << 21U)
+#define MOD_SEL0_I2C2_B		((uint32_t)1U << 21U)
+#define MOD_SEL0_I2C1_A		((uint32_t)0U << 20U)
+#define MOD_SEL0_I2C1_B		((uint32_t)1U << 20U)
+#define MOD_SEL0_HSCIF4_A	((uint32_t)0U << 19U)
+#define MOD_SEL0_HSCIF4_B	((uint32_t)1U << 19U)
+#define MOD_SEL0_HSCIF3_A	((uint32_t)0U << 17U)
+#define MOD_SEL0_HSCIF3_B	((uint32_t)1U << 17U)
+#define MOD_SEL0_HSCIF3_C	((uint32_t)2U << 17U)
+#define MOD_SEL0_HSCIF3_D	((uint32_t)3U << 17U)
+#define MOD_SEL0_HSCIF1_A	((uint32_t)0U << 16U)
+#define MOD_SEL0_HSCIF1_B	((uint32_t)1U << 16U)
+#define MOD_SEL0_FSO_A		((uint32_t)0U << 15U)
+#define MOD_SEL0_FSO_B		((uint32_t)1U << 15U)
+#define MOD_SEL0_HSCIF2_A	((uint32_t)0U << 13U)
+#define MOD_SEL0_HSCIF2_B	((uint32_t)1U << 13U)
+#define MOD_SEL0_HSCIF2_C	((uint32_t)2U << 13U)
+#define MOD_SEL0_ETHERAVB_A	((uint32_t)0U << 12U)
+#define MOD_SEL0_ETHERAVB_B	((uint32_t)1U << 12U)
+#define MOD_SEL0_DRIF3_A	((uint32_t)0U << 11U)
+#define MOD_SEL0_DRIF3_B	((uint32_t)1U << 11U)
+#define MOD_SEL0_DRIF2_A	((uint32_t)0U << 10U)
+#define MOD_SEL0_DRIF2_B	((uint32_t)1U << 10U)
+#define MOD_SEL0_DRIF1_A	((uint32_t)0U << 8U)
+#define MOD_SEL0_DRIF1_B	((uint32_t)1U << 8U)
+#define MOD_SEL0_DRIF1_C	((uint32_t)2U << 8U)
+#define MOD_SEL0_DRIF0_A	((uint32_t)0U << 6U)
+#define MOD_SEL0_DRIF0_B	((uint32_t)1U << 6U)
+#define MOD_SEL0_DRIF0_C	((uint32_t)2U << 6U)
+#define MOD_SEL0_CANFD0_A	((uint32_t)0U << 5U)
+#define MOD_SEL0_CANFD0_B	((uint32_t)1U << 5U)
+#define MOD_SEL0_ADG_A_A	((uint32_t)0U << 3U)
+#define MOD_SEL0_ADG_A_B	((uint32_t)1U << 3U)
+#define MOD_SEL0_ADG_A_C	((uint32_t)2U << 3U)
+#define MOD_SEL1_TSIF1_A	((uint32_t)0U << 30U)
+#define MOD_SEL1_TSIF1_B	((uint32_t)1U << 30U)
+#define MOD_SEL1_TSIF1_C	((uint32_t)2U << 30U)
+#define MOD_SEL1_TSIF1_D	((uint32_t)3U << 30U)
+#define MOD_SEL1_TSIF0_A	((uint32_t)0U << 27U)
+#define MOD_SEL1_TSIF0_B	((uint32_t)1U << 27U)
+#define MOD_SEL1_TSIF0_C	((uint32_t)2U << 27U)
+#define MOD_SEL1_TSIF0_D	((uint32_t)3U << 27U)
+#define MOD_SEL1_TSIF0_E	((uint32_t)4U << 27U)
+#define MOD_SEL1_TIMER_TMU_A	((uint32_t)0U << 26U)
+#define MOD_SEL1_TIMER_TMU_B	((uint32_t)1U << 26U)
+#define MOD_SEL1_SSP1_1_A	((uint32_t)0U << 24U)
+#define MOD_SEL1_SSP1_1_B	((uint32_t)1U << 24U)
+#define MOD_SEL1_SSP1_1_C	((uint32_t)2U << 24U)
+#define MOD_SEL1_SSP1_1_D	((uint32_t)3U << 24U)
+#define MOD_SEL1_SSP1_0_A	((uint32_t)0U << 21U)
+#define MOD_SEL1_SSP1_0_B	((uint32_t)1U << 21U)
+#define MOD_SEL1_SSP1_0_C	((uint32_t)2U << 21U)
+#define MOD_SEL1_SSP1_0_D	((uint32_t)3U << 21U)
+#define MOD_SEL1_SSP1_0_E	((uint32_t)4U << 21U)
+#define MOD_SEL1_SSI_A		((uint32_t)0U << 20U)
+#define MOD_SEL1_SSI_B		((uint32_t)1U << 20U)
+#define MOD_SEL1_SPEED_PULSE_IF_A	((uint32_t)0U << 19U)
+#define MOD_SEL1_SPEED_PULSE_IF_B	((uint32_t)1U << 19U)
+#define MOD_SEL1_SIMCARD_A	((uint32_t)0U << 17U)
+#define MOD_SEL1_SIMCARD_B	((uint32_t)1U << 17U)
+#define MOD_SEL1_SIMCARD_C	((uint32_t)2U << 17U)
+#define MOD_SEL1_SIMCARD_D	((uint32_t)3U << 17U)
+#define MOD_SEL1_SDHI2_A	((uint32_t)0U << 16U)
+#define MOD_SEL1_SDHI2_B	((uint32_t)1U << 16U)
+#define MOD_SEL1_SCIF4_A	((uint32_t)0U << 14U)
+#define MOD_SEL1_SCIF4_B	((uint32_t)1U << 14U)
+#define MOD_SEL1_SCIF4_C	((uint32_t)2U << 14U)
+#define MOD_SEL1_SCIF3_A	((uint32_t)0U << 13U)
+#define MOD_SEL1_SCIF3_B	((uint32_t)1U << 13U)
+#define MOD_SEL1_SCIF2_A	((uint32_t)0U << 12U)
+#define MOD_SEL1_SCIF2_B	((uint32_t)1U << 12U)
+#define MOD_SEL1_SCIF1_A	((uint32_t)0U << 11U)
+#define MOD_SEL1_SCIF1_B	((uint32_t)1U << 11U)
+#define MOD_SEL1_SCIF_A		((uint32_t)0U << 10U)
+#define MOD_SEL1_SCIF_B		((uint32_t)1U << 10U)
+#define MOD_SEL1_REMOCON_A	((uint32_t)0U << 9U)
+#define MOD_SEL1_REMOCON_B	((uint32_t)1U << 9U)
+#define MOD_SEL1_RCAN0_A	((uint32_t)0U << 6U)
+#define MOD_SEL1_RCAN0_B	((uint32_t)1U << 6U)
+#define MOD_SEL1_PWM6_A		((uint32_t)0U << 5U)
+#define MOD_SEL1_PWM6_B		((uint32_t)1U << 5U)
+#define MOD_SEL1_PWM5_A		((uint32_t)0U << 4U)
+#define MOD_SEL1_PWM5_B		((uint32_t)1U << 4U)
+#define MOD_SEL1_PWM4_A		((uint32_t)0U << 3U)
+#define MOD_SEL1_PWM4_B		((uint32_t)1U << 3U)
+#define MOD_SEL1_PWM3_A		((uint32_t)0U << 2U)
+#define MOD_SEL1_PWM3_B		((uint32_t)1U << 2U)
+#define MOD_SEL1_PWM2_A		((uint32_t)0U << 1U)
+#define MOD_SEL1_PWM2_B		((uint32_t)1U << 1U)
+#define MOD_SEL1_PWM1_A		((uint32_t)0U << 0U)
+#define MOD_SEL1_PWM1_B		((uint32_t)1U << 0U)
+#define MOD_SEL2_I2C_5_A	((uint32_t)0U << 31U)
+#define MOD_SEL2_I2C_5_B	((uint32_t)1U << 31U)
+#define MOD_SEL2_I2C_3_A	((uint32_t)0U << 30U)
+#define MOD_SEL2_I2C_3_B	((uint32_t)1U << 30U)
+#define MOD_SEL2_I2C_0_A	((uint32_t)0U << 29U)
+#define MOD_SEL2_I2C_0_B	((uint32_t)1U << 29U)
+#define MOD_SEL2_FM_A		((uint32_t)0U << 27U)
+#define MOD_SEL2_FM_B		((uint32_t)1U << 27U)
+#define MOD_SEL2_FM_C		((uint32_t)2U << 27U)
+#define MOD_SEL2_FM_D		((uint32_t)3U << 27U)
+#define MOD_SEL2_SCIF5_A	((uint32_t)0U << 26U)
+#define MOD_SEL2_SCIF5_B	((uint32_t)1U << 26U)
+#define MOD_SEL2_I2C6_A		((uint32_t)0U << 23U)
+#define MOD_SEL2_I2C6_B		((uint32_t)1U << 23U)
+#define MOD_SEL2_I2C6_C		((uint32_t)2U << 23U)
+#define MOD_SEL2_NDF_A		((uint32_t)0U << 22U)
+#define MOD_SEL2_NDF_B		((uint32_t)1U << 22U)
+#define MOD_SEL2_SSI2_A		((uint32_t)0U << 21U)
+#define MOD_SEL2_SSI2_B		((uint32_t)1U << 21U)
+#define MOD_SEL2_SSI9_A		((uint32_t)0U << 20U)
+#define MOD_SEL2_SSI9_B		((uint32_t)1U << 20U)
+#define MOD_SEL2_TIMER_TMU2_A	((uint32_t)0U << 19U)
+#define MOD_SEL2_TIMER_TMU2_B	((uint32_t)1U << 19U)
+#define MOD_SEL2_ADG_B_A	((uint32_t)0U << 18U)
+#define MOD_SEL2_ADG_B_B	((uint32_t)1U << 18U)
+#define MOD_SEL2_ADG_C_A	((uint32_t)0U << 17U)
+#define MOD_SEL2_ADG_C_B	((uint32_t)1U << 17U)
+#define MOD_SEL2_VIN4_A		((uint32_t)0U << 0U)
+#define MOD_SEL2_VIN4_B		((uint32_t)1U << 0U)
+
+static void pfc_reg_write(uint32_t addr, uint32_t data)
+{
+	mmio_write_32(PFC_PMMR, ~data);
+	mmio_write_32((uintptr_t)addr, data);
+}
+
+void pfc_init_d3(void)
+{
+	/* initialize module select */
+	pfc_reg_write(PFC_MOD_SEL0, 0x00000000U);
+	pfc_reg_write(PFC_MOD_SEL1, 0x00000000U);
+
+	/* initialize peripheral function select */
+	pfc_reg_write(PFC_IPSR0,  0x00000001U);
+	pfc_reg_write(PFC_IPSR1,  0x00000000U);
+	pfc_reg_write(PFC_IPSR2,  0x00000000U);
+	pfc_reg_write(PFC_IPSR3,  0x00000000U);
+	pfc_reg_write(PFC_IPSR4,  0x00002000U);
+	pfc_reg_write(PFC_IPSR5,  0x00000000U);
+	pfc_reg_write(PFC_IPSR6,  0x00000000U);
+	pfc_reg_write(PFC_IPSR7,  0x00000000U);
+	pfc_reg_write(PFC_IPSR8,  0x11003301U);
+	pfc_reg_write(PFC_IPSR9,  0x11111111U);
+	pfc_reg_write(PFC_IPSR10, 0x00020000U);
+	pfc_reg_write(PFC_IPSR11, 0x40001110U);
+	pfc_reg_write(PFC_IPSR12, 0x00000000U);
+	pfc_reg_write(PFC_IPSR13, 0x00000000U);
+
+	/* initialize GPIO/perihperal function select */
+	pfc_reg_write(PFC_GPSR0, 0x0000001FU);
+	pfc_reg_write(PFC_GPSR1, 0x3FFFFFFFU);
+	pfc_reg_write(PFC_GPSR2, 0xFFFFFFFFU);
+	pfc_reg_write(PFC_GPSR3, 0x000003FFU);
+	pfc_reg_write(PFC_GPSR4, 0xFC7F0F7EU);
+	pfc_reg_write(PFC_GPSR5, 0x001BFFFBU);
+	pfc_reg_write(PFC_GPSR6, 0x00003FFFU);
+
+	/* initialize POC control register */
+	pfc_reg_write(PFC_POCCTRL0,   0xC00FFFFFU);
+	pfc_reg_write(PFC_POCCTRL2,   0XFFFFFFFEU);
+	pfc_reg_write(PFC_TDSELCTRL0, 0x00000000U);
+
+	/* initialize LSI pin pull-up/down control */
+	pfc_reg_write(PFC_PUD0, 0x0047C1A2U);
+	pfc_reg_write(PFC_PUD1, 0x4E13ABFFU);
+	pfc_reg_write(PFC_PUD2, 0xFFFFFFFFU);
+	pfc_reg_write(PFC_PUD3, 0xFF0FFFFFU);
+	pfc_reg_write(PFC_PUD4, 0xE0000000U);
+	pfc_reg_write(PFC_PUD5, 0x60000000U);
+
+	/* initialize LSI pin pull-enable register */
+	pfc_reg_write(PFC_PUEN0, 0x00000000U);
+	pfc_reg_write(PFC_PUEN1, 0x00000000U);
+	pfc_reg_write(PFC_PUEN2, 0x00000000U);
+	pfc_reg_write(PFC_PUEN3, 0x000F008CU);
+	pfc_reg_write(PFC_PUEN4, 0x00000000U);
+	pfc_reg_write(PFC_PUEN5, 0x00000000U);
+
+	/* initialize positive/negative logic select */
+	mmio_write_32(GPIO_POSNEG0, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG1, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG2, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG3, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG4, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG5, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG6, 0x00000000U);
+
+	/* initialize general IO/interrupt switching */
+	mmio_write_32(GPIO_IOINTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL5, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL6, 0x00000000U);
+
+	/* initialize general output register */
+	mmio_write_32(GPIO_OUTDT0, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT1, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT2, 0x00000400U);
+	mmio_write_32(GPIO_OUTDT3, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT4, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT5, 0x00000006U);
+	mmio_write_32(GPIO_OUTDT6, 0x00003880U);
+
+	/* initialize general input/output switching */
+	mmio_write_32(GPIO_INOUTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL4, 0x00802000U);
+	mmio_write_32(GPIO_INOUTSEL5, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL6, 0x00000000U);
+}
diff --git a/drivers/renesas/rcar/pfc/D3/pfc_init_d3.h b/drivers/renesas/rcar/pfc/D3/pfc_init_d3.h
new file mode 100644
index 0000000..b7b1754
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/D3/pfc_init_d3.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PFC_INIT_D3_H
+#define PFC_INIT_D3_H
+
+void pfc_init_d3(void);
+
+#endif	/* PFC_INIT_D3_H */
diff --git a/drivers/renesas/rcar/pfc/E3/pfc_init_e3.c b/drivers/renesas/rcar/pfc/E3/pfc_init_e3.c
new file mode 100644
index 0000000..bd0048e
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/E3/pfc_init_e3.c
@@ -0,0 +1,651 @@
+/*
+ * Copyright (c) 2018-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>		/* for uint32_t */
+#include <lib/mmio.h>
+#include "pfc_init_e3.h"
+#include "rcar_def.h"
+#include "../pfc_regs.h"
+
+/* PFC */
+#define GPSR0_SDA4		BIT(17)
+#define GPSR0_SCL4		BIT(16)
+#define GPSR0_D15		BIT(15)
+#define GPSR0_D14		BIT(14)
+#define GPSR0_D13		BIT(13)
+#define GPSR0_D12		BIT(12)
+#define GPSR0_D11		BIT(11)
+#define GPSR0_D10		BIT(10)
+#define GPSR0_D9		BIT(9)
+#define GPSR0_D8		BIT(8)
+#define GPSR0_D7		BIT(7)
+#define GPSR0_D6		BIT(6)
+#define GPSR0_D5		BIT(5)
+#define GPSR0_D4		BIT(4)
+#define GPSR0_D3		BIT(3)
+#define GPSR0_D2		BIT(2)
+#define GPSR0_D1		BIT(1)
+#define GPSR0_D0		BIT(0)
+#define GPSR1_WE0		BIT(22)
+#define GPSR1_CS0		BIT(21)
+#define GPSR1_CLKOUT		BIT(20)
+#define GPSR1_A19		BIT(19)
+#define GPSR1_A18		BIT(18)
+#define GPSR1_A17		BIT(17)
+#define GPSR1_A16		BIT(16)
+#define GPSR1_A15		BIT(15)
+#define GPSR1_A14		BIT(14)
+#define GPSR1_A13		BIT(13)
+#define GPSR1_A12		BIT(12)
+#define GPSR1_A11		BIT(11)
+#define GPSR1_A10		BIT(10)
+#define GPSR1_A9		BIT(9)
+#define GPSR1_A8		BIT(8)
+#define GPSR1_A7		BIT(7)
+#define GPSR1_A6		BIT(6)
+#define GPSR1_A5		BIT(5)
+#define GPSR1_A4		BIT(4)
+#define GPSR1_A3		BIT(3)
+#define GPSR1_A2		BIT(2)
+#define GPSR1_A1		BIT(1)
+#define GPSR1_A0		BIT(0)
+#define GPSR2_BIT27_REVERSED	BIT(27)
+#define GPSR2_BIT26_REVERSED	BIT(26)
+#define GPSR2_EX_WAIT0		BIT(25)
+#define GPSR2_RD_WR		BIT(24)
+#define GPSR2_RD		BIT(23)
+#define GPSR2_BS		BIT(22)
+#define GPSR2_AVB_PHY_INT	BIT(21)
+#define GPSR2_AVB_TXCREFCLK	BIT(20)
+#define GPSR2_AVB_RD3		BIT(19)
+#define GPSR2_AVB_RD2		BIT(18)
+#define GPSR2_AVB_RD1		BIT(17)
+#define GPSR2_AVB_RD0		BIT(16)
+#define GPSR2_AVB_RXC		BIT(15)
+#define GPSR2_AVB_RX_CTL	BIT(14)
+#define GPSR2_RPC_RESET		BIT(13)
+#define GPSR2_RPC_RPC_INT	BIT(12)
+#define GPSR2_QSPI1_SSL		BIT(11)
+#define GPSR2_QSPI1_IO3		BIT(10)
+#define GPSR2_QSPI1_IO2		BIT(9)
+#define GPSR2_QSPI1_MISO_IO1	BIT(8)
+#define GPSR2_QSPI1_MOSI_IO0	BIT(7)
+#define GPSR2_QSPI1_SPCLK	BIT(6)
+#define GPSR2_QSPI0_SSL		BIT(5)
+#define GPSR2_QSPI0_IO3		BIT(4)
+#define GPSR2_QSPI0_IO2		BIT(3)
+#define GPSR2_QSPI0_MISO_IO1	BIT(2)
+#define GPSR2_QSPI0_MOSI_IO0	BIT(1)
+#define GPSR2_QSPI0_SPCLK	BIT(0)
+#define GPSR3_SD1_WP		BIT(15)
+#define GPSR3_SD1_CD		BIT(14)
+#define GPSR3_SD0_WP		BIT(13)
+#define GPSR3_SD0_CD		BIT(12)
+#define GPSR3_SD1_DAT3		BIT(11)
+#define GPSR3_SD1_DAT2		BIT(10)
+#define GPSR3_SD1_DAT1		BIT(9)
+#define GPSR3_SD1_DAT0		BIT(8)
+#define GPSR3_SD1_CMD		BIT(7)
+#define GPSR3_SD1_CLK		BIT(6)
+#define GPSR3_SD0_DAT3		BIT(5)
+#define GPSR3_SD0_DAT2		BIT(4)
+#define GPSR3_SD0_DAT1		BIT(3)
+#define GPSR3_SD0_DAT0		BIT(2)
+#define GPSR3_SD0_CMD		BIT(1)
+#define GPSR3_SD0_CLK		BIT(0)
+#define GPSR4_SD3_DS		BIT(10)
+#define GPSR4_SD3_DAT7		BIT(9)
+#define GPSR4_SD3_DAT6		BIT(8)
+#define GPSR4_SD3_DAT5		BIT(7)
+#define GPSR4_SD3_DAT4		BIT(6)
+#define GPSR4_SD3_DAT3		BIT(5)
+#define GPSR4_SD3_DAT2		BIT(4)
+#define GPSR4_SD3_DAT1		BIT(3)
+#define GPSR4_SD3_DAT0		BIT(2)
+#define GPSR4_SD3_CMD		BIT(1)
+#define GPSR4_SD3_CLK		BIT(0)
+#define GPSR5_MLB_DAT		BIT(19)
+#define GPSR5_MLB_SIG		BIT(18)
+#define GPSR5_MLB_CLK		BIT(17)
+#define GPSR5_SSI_SDATA9	BIT(16)
+#define GPSR5_MSIOF0_SS2	BIT(15)
+#define GPSR5_MSIOF0_SS1	BIT(14)
+#define GPSR5_MSIOF0_SYNC	BIT(13)
+#define GPSR5_MSIOF0_TXD	BIT(12)
+#define GPSR5_MSIOF0_RXD	BIT(11)
+#define GPSR5_MSIOF0_SCK	BIT(10)
+#define GPSR5_RX2_A		BIT(9)
+#define GPSR5_TX2_A		BIT(8)
+#define GPSR5_SCK2_A		BIT(7)
+#define GPSR5_TX1		BIT(6)
+#define GPSR5_RX1		BIT(5)
+#define GPSR5_RTS0_A		BIT(4)
+#define GPSR5_CTS0_A		BIT(3)
+#define GPSR5_TX0_A		BIT(2)
+#define GPSR5_RX0_A		BIT(1)
+#define GPSR5_SCK0_A		BIT(0)
+#define GPSR6_USB30_PWEN	BIT(17)
+#define GPSR6_SSI_SDATA6	BIT(16)
+#define GPSR6_SSI_WS6		BIT(15)
+#define GPSR6_SSI_SCK6		BIT(14)
+#define GPSR6_SSI_SDATA5	BIT(13)
+#define GPSR6_SSI_WS5		BIT(12)
+#define GPSR6_SSI_SCK5		BIT(11)
+#define GPSR6_SSI_SDATA4	BIT(10)
+#define GPSR6_USB30_OVC		BIT(9)
+#define GPSR6_AUDIO_CLKA	BIT(8)
+#define GPSR6_SSI_SDATA3	BIT(7)
+#define GPSR6_SSI_WS349		BIT(6)
+#define GPSR6_SSI_SCK349	BIT(5)
+#define GPSR6_SSI_SDATA2	BIT(4)
+#define GPSR6_SSI_SDATA1	BIT(3)
+#define GPSR6_SSI_SDATA0	BIT(2)
+#define GPSR6_SSI_WS01239	BIT(1)
+#define GPSR6_SSI_SCK01239	BIT(0)
+
+#define IPSR_28_FUNC(x)		((uint32_t)(x) << 28U)
+#define IPSR_24_FUNC(x)		((uint32_t)(x) << 24U)
+#define IPSR_20_FUNC(x)		((uint32_t)(x) << 20U)
+#define IPSR_16_FUNC(x)		((uint32_t)(x) << 16U)
+#define IPSR_12_FUNC(x)		((uint32_t)(x) << 12U)
+#define IPSR_8_FUNC(x)		((uint32_t)(x) << 8U)
+#define IPSR_4_FUNC(x)		((uint32_t)(x) << 4U)
+#define IPSR_0_FUNC(x)		((uint32_t)(x) << 0U)
+
+#define POCCTRL0_MASK		(0x0007F000U)
+#define POC_SD3_DS_33V		BIT(29)
+#define POC_SD3_DAT7_33V	BIT(28)
+#define POC_SD3_DAT6_33V	BIT(27)
+#define POC_SD3_DAT5_33V	BIT(26)
+#define POC_SD3_DAT4_33V	BIT(25)
+#define POC_SD3_DAT3_33V	BIT(24)
+#define POC_SD3_DAT2_33V	BIT(23)
+#define POC_SD3_DAT1_33V	BIT(22)
+#define POC_SD3_DAT0_33V	BIT(21)
+#define POC_SD3_CMD_33V		BIT(20)
+#define POC_SD3_CLK_33V		BIT(19)
+#define POC_SD1_DAT3_33V	BIT(11)
+#define POC_SD1_DAT2_33V	BIT(10)
+#define POC_SD1_DAT1_33V	BIT(9)
+#define POC_SD1_DAT0_33V	BIT(8)
+#define POC_SD1_CMD_33V		BIT(7)
+#define POC_SD1_CLK_33V		BIT(6)
+#define POC_SD0_DAT3_33V	BIT(5)
+#define POC_SD0_DAT2_33V	BIT(4)
+#define POC_SD0_DAT1_33V	BIT(3)
+#define POC_SD0_DAT0_33V	BIT(2)
+#define POC_SD0_CMD_33V		BIT(1)
+#define POC_SD0_CLK_33V		BIT(0)
+
+#define POCCTRL2_MASK		(0xFFFFFFFEU)
+#define POC2_VREF_33V		BIT(0)
+
+#define MOD_SEL0_ADGB_A		((uint32_t)0U << 29U)
+#define MOD_SEL0_ADGB_B		((uint32_t)1U << 29U)
+#define MOD_SEL0_ADGB_C		((uint32_t)2U << 29U)
+#define MOD_SEL0_DRIF0_A	((uint32_t)0U << 28U)
+#define MOD_SEL0_DRIF0_B	((uint32_t)1U << 28U)
+#define MOD_SEL0_FM_A		((uint32_t)0U << 26U)
+#define MOD_SEL0_FM_B		((uint32_t)1U << 26U)
+#define MOD_SEL0_FM_C		((uint32_t)2U << 26U)
+#define MOD_SEL0_FSO_A		((uint32_t)0U << 25U)
+#define MOD_SEL0_FSO_B		((uint32_t)1U << 25U)
+#define MOD_SEL0_HSCIF0_A	((uint32_t)0U << 24U)
+#define MOD_SEL0_HSCIF0_B	((uint32_t)1U << 24U)
+#define MOD_SEL0_HSCIF1_A	((uint32_t)0U << 23U)
+#define MOD_SEL0_HSCIF1_B	((uint32_t)1U << 23U)
+#define MOD_SEL0_HSCIF2_A	((uint32_t)0U << 22U)
+#define MOD_SEL0_HSCIF2_B	((uint32_t)1U << 22U)
+#define MOD_SEL0_I2C1_A		((uint32_t)0U << 20U)
+#define MOD_SEL0_I2C1_B		((uint32_t)1U << 20U)
+#define MOD_SEL0_I2C1_C		((uint32_t)2U << 20U)
+#define MOD_SEL0_I2C1_D		((uint32_t)3U << 20U)
+#define MOD_SEL0_I2C2_A		((uint32_t)0U << 17U)
+#define MOD_SEL0_I2C2_B		((uint32_t)1U << 17U)
+#define MOD_SEL0_I2C2_C		((uint32_t)2U << 17U)
+#define MOD_SEL0_I2C2_D		((uint32_t)3U << 17U)
+#define MOD_SEL0_I2C2_E		((uint32_t)4U << 17U)
+#define MOD_SEL0_NDFC_A		((uint32_t)0U << 16U)
+#define MOD_SEL0_NDFC_B		((uint32_t)1U << 16U)
+#define MOD_SEL0_PWM0_A		((uint32_t)0U << 15U)
+#define MOD_SEL0_PWM0_B		((uint32_t)1U << 15U)
+#define MOD_SEL0_PWM1_A		((uint32_t)0U << 14U)
+#define MOD_SEL0_PWM1_B		((uint32_t)1U << 14U)
+#define MOD_SEL0_PWM2_A		((uint32_t)0U << 12U)
+#define MOD_SEL0_PWM2_B		((uint32_t)1U << 12U)
+#define MOD_SEL0_PWM2_C		((uint32_t)2U << 12U)
+#define MOD_SEL0_PWM3_A		((uint32_t)0U << 10U)
+#define MOD_SEL0_PWM3_B		((uint32_t)1U << 10U)
+#define MOD_SEL0_PWM3_C		((uint32_t)2U << 10U)
+#define MOD_SEL0_PWM4_A		((uint32_t)0U << 9U)
+#define MOD_SEL0_PWM4_B		((uint32_t)1U << 9U)
+#define MOD_SEL0_PWM5_A		((uint32_t)0U << 8U)
+#define MOD_SEL0_PWM5_B		((uint32_t)1U << 8U)
+#define MOD_SEL0_PWM6_A		((uint32_t)0U << 7U)
+#define MOD_SEL0_PWM6_B		((uint32_t)1U << 7U)
+#define MOD_SEL0_REMOCON_A	((uint32_t)0U << 5U)
+#define MOD_SEL0_REMOCON_B	((uint32_t)1U << 5U)
+#define MOD_SEL0_REMOCON_C	((uint32_t)2U << 5U)
+#define MOD_SEL0_SCIF_A		((uint32_t)0U << 4U)
+#define MOD_SEL0_SCIF_B		((uint32_t)1U << 4U)
+#define MOD_SEL0_SCIF0_A	((uint32_t)0U << 3U)
+#define MOD_SEL0_SCIF0_B	((uint32_t)1U << 3U)
+#define MOD_SEL0_SCIF2_A	((uint32_t)0U << 2U)
+#define MOD_SEL0_SCIF2_B	((uint32_t)1U << 2U)
+#define MOD_SEL0_SPEED_PULSE_IF_A	((uint32_t)0U << 0U)
+#define MOD_SEL0_SPEED_PULSE_IF_B	((uint32_t)1U << 0U)
+#define MOD_SEL0_SPEED_PULSE_IF_C	((uint32_t)2U << 0U)
+#define MOD_SEL1_SIMCARD_A	((uint32_t)0U << 31U)
+#define MOD_SEL1_SIMCARD_B	((uint32_t)1U << 31U)
+#define MOD_SEL1_SSI2_A		((uint32_t)0U << 30U)
+#define MOD_SEL1_SSI2_B		((uint32_t)1U << 30U)
+#define MOD_SEL1_TIMER_TMU_A	((uint32_t)0U << 29U)
+#define MOD_SEL1_TIMER_TMU_B	((uint32_t)1U << 29U)
+#define MOD_SEL1_USB20_CH0_A	((uint32_t)0U << 28U)
+#define MOD_SEL1_USB20_CH0_B	((uint32_t)1U << 28U)
+#define MOD_SEL1_DRIF2_A	((uint32_t)0U << 26U)
+#define MOD_SEL1_DRIF2_B	((uint32_t)1U << 26U)
+#define MOD_SEL1_DRIF3_A	((uint32_t)0U << 25U)
+#define MOD_SEL1_DRIF3_B	((uint32_t)1U << 25U)
+#define MOD_SEL1_HSCIF3_A	((uint32_t)0U << 22U)
+#define MOD_SEL1_HSCIF3_B	((uint32_t)1U << 22U)
+#define MOD_SEL1_HSCIF3_C	((uint32_t)2U << 22U)
+#define MOD_SEL1_HSCIF3_D	((uint32_t)3U << 22U)
+#define MOD_SEL1_HSCIF3_E	((uint32_t)4U << 22U)
+#define MOD_SEL1_HSCIF4_A	((uint32_t)0U << 19U)
+#define MOD_SEL1_HSCIF4_B	((uint32_t)1U << 19U)
+#define MOD_SEL1_HSCIF4_C	((uint32_t)2U << 19U)
+#define MOD_SEL1_HSCIF4_D	((uint32_t)3U << 19U)
+#define MOD_SEL1_HSCIF4_E	((uint32_t)4U << 19U)
+#define MOD_SEL1_I2C6_A		((uint32_t)0U << 18U)
+#define MOD_SEL1_I2C6_B		((uint32_t)1U << 18U)
+#define MOD_SEL1_I2C7_A		((uint32_t)0U << 17U)
+#define MOD_SEL1_I2C7_B		((uint32_t)1U << 17U)
+#define MOD_SEL1_MSIOF2_A	((uint32_t)0U << 16U)
+#define MOD_SEL1_MSIOF2_B	((uint32_t)1U << 16U)
+#define MOD_SEL1_MSIOF3_A	((uint32_t)0U << 15U)
+#define MOD_SEL1_MSIOF3_B	((uint32_t)1U << 15U)
+#define MOD_SEL1_SCIF3_A	((uint32_t)0U << 13U)
+#define MOD_SEL1_SCIF3_B	((uint32_t)1U << 13U)
+#define MOD_SEL1_SCIF3_C	((uint32_t)2U << 13U)
+#define MOD_SEL1_SCIF4_A	((uint32_t)0U << 11U)
+#define MOD_SEL1_SCIF4_B	((uint32_t)1U << 11U)
+#define MOD_SEL1_SCIF4_C	((uint32_t)2U << 11U)
+#define MOD_SEL1_SCIF5_A	((uint32_t)0U << 9U)
+#define MOD_SEL1_SCIF5_B	((uint32_t)1U << 9U)
+#define MOD_SEL1_SCIF5_C	((uint32_t)2U << 9U)
+#define MOD_SEL1_VIN4_A		((uint32_t)0U << 8U)
+#define MOD_SEL1_VIN4_B		((uint32_t)1U << 8U)
+#define MOD_SEL1_VIN5_A		((uint32_t)0U << 7U)
+#define MOD_SEL1_VIN5_B		((uint32_t)1U << 7U)
+#define MOD_SEL1_ADGC_A		((uint32_t)0U << 5U)
+#define MOD_SEL1_ADGC_B		((uint32_t)1U << 5U)
+#define MOD_SEL1_ADGC_C		((uint32_t)2U << 5U)
+#define MOD_SEL1_SSI9_A		((uint32_t)0U << 4U)
+#define MOD_SEL1_SSI9_B		((uint32_t)1U << 4U)
+
+static void pfc_reg_write(uint32_t addr, uint32_t data)
+{
+	mmio_write_32(PFC_PMMR, ~data);
+	mmio_write_32((uintptr_t)addr, data);
+}
+
+void pfc_init_e3(void)
+{
+	uint32_t reg;
+
+	/* initialize module select */
+	pfc_reg_write(PFC_MOD_SEL0, MOD_SEL0_ADGB_A
+		      | MOD_SEL0_DRIF0_A
+		      | MOD_SEL0_FM_A
+		      | MOD_SEL0_FSO_A
+		      | MOD_SEL0_HSCIF0_A
+		      | MOD_SEL0_HSCIF1_A
+		      | MOD_SEL0_HSCIF2_A
+		      | MOD_SEL0_I2C1_A
+		      | MOD_SEL0_I2C2_A
+		      | MOD_SEL0_NDFC_A
+		      | MOD_SEL0_PWM0_A
+		      | MOD_SEL0_PWM1_A
+		      | MOD_SEL0_PWM2_A
+		      | MOD_SEL0_PWM3_A
+		      | MOD_SEL0_PWM4_A
+		      | MOD_SEL0_PWM5_A
+		      | MOD_SEL0_PWM6_A
+		      | MOD_SEL0_REMOCON_A
+		      | MOD_SEL0_SCIF_A
+		      | MOD_SEL0_SCIF0_A
+		      | MOD_SEL0_SCIF2_A
+		      | MOD_SEL0_SPEED_PULSE_IF_A);
+	pfc_reg_write(PFC_MOD_SEL1, MOD_SEL1_SIMCARD_A
+		      | MOD_SEL1_SSI2_A
+		      | MOD_SEL1_TIMER_TMU_A
+		      | MOD_SEL1_USB20_CH0_B
+		      | MOD_SEL1_DRIF2_A
+		      | MOD_SEL1_DRIF3_A
+		      | MOD_SEL1_HSCIF3_A
+		      | MOD_SEL1_HSCIF4_A
+		      | MOD_SEL1_I2C6_A
+		      | MOD_SEL1_I2C7_A
+		      | MOD_SEL1_MSIOF2_A
+		      | MOD_SEL1_MSIOF3_A
+		      | MOD_SEL1_SCIF3_A
+		      | MOD_SEL1_SCIF4_A
+		      | MOD_SEL1_SCIF5_A
+		      | MOD_SEL1_VIN4_A
+		      | MOD_SEL1_VIN5_A
+		      | MOD_SEL1_ADGC_A
+		      | MOD_SEL1_SSI9_A);
+
+	/* initialize peripheral function select */
+	pfc_reg_write(PFC_IPSR0, IPSR_28_FUNC(0)	/* QSPI1_MISO/IO1 */
+		      | IPSR_24_FUNC(0)	/* QSPI1_MOSI/IO0 */
+		      | IPSR_20_FUNC(0)	/* QSPI1_SPCLK */
+		      | IPSR_16_FUNC(0)	/* QSPI0_IO3 */
+		      | IPSR_12_FUNC(0)	/* QSPI0_IO2 */
+		      | IPSR_8_FUNC(0)	/* QSPI0_MISO/IO1 */
+		      | IPSR_4_FUNC(0)	/* QSPI0_MOSI/IO0 */
+		      | IPSR_0_FUNC(0));	/* QSPI0_SPCLK */
+	pfc_reg_write(PFC_IPSR1, IPSR_28_FUNC(0)	/* AVB_RD2 */
+		      | IPSR_24_FUNC(0)	/* AVB_RD1 */
+		      | IPSR_20_FUNC(0)	/* AVB_RD0 */
+		      | IPSR_16_FUNC(0)	/* RPC_RESET# */
+		      | IPSR_12_FUNC(0)	/* RPC_INT# */
+		      | IPSR_8_FUNC(0)	/* QSPI1_SSL */
+		      | IPSR_4_FUNC(0)	/* QSPI1_IO3 */
+		      | IPSR_0_FUNC(0));	/* QSPI1_IO2 */
+	pfc_reg_write(PFC_IPSR2, IPSR_28_FUNC(1)	/* IRQ0 */
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(2)	/* AVB_LINK */
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)	/* AVB_MDC */
+		      | IPSR_4_FUNC(0)	/* AVB_MDIO */
+		      | IPSR_0_FUNC(0));	/* AVB_TXCREFCLK */
+	pfc_reg_write(PFC_IPSR3, IPSR_28_FUNC(5)	/* DU_HSYNC */
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(5)	/* DU_DG4 */
+		      | IPSR_8_FUNC(5)	/* DU_DOTCLKOUT0 */
+		      | IPSR_4_FUNC(5)	/* DU_DISP */
+		      | IPSR_0_FUNC(1));	/* IRQ1 */
+	pfc_reg_write(PFC_IPSR4, IPSR_28_FUNC(5)	/* DU_DB5 */
+		      | IPSR_24_FUNC(5)	/* DU_DB4 */
+		      | IPSR_20_FUNC(5)	/* DU_DB3 */
+		      | IPSR_16_FUNC(5)	/* DU_DB2 */
+		      | IPSR_12_FUNC(5)	/* DU_DG6 */
+		      | IPSR_8_FUNC(5)	/* DU_VSYNC */
+		      | IPSR_4_FUNC(5)	/* DU_DG5 */
+		      | IPSR_0_FUNC(5));	/* DU_DG7 */
+	pfc_reg_write(PFC_IPSR5, IPSR_28_FUNC(5)	/* DU_DR3 */
+		      | IPSR_24_FUNC(5)	/* DU_DB7 */
+		      | IPSR_20_FUNC(5)	/* DU_DR2 */
+		      | IPSR_16_FUNC(5)	/* DU_DR1 */
+		      | IPSR_12_FUNC(5)	/* DU_DR0 */
+		      | IPSR_8_FUNC(5)	/* DU_DB1 */
+		      | IPSR_4_FUNC(5)	/* DU_DB0 */
+		      | IPSR_0_FUNC(5));	/* DU_DB6 */
+	pfc_reg_write(PFC_IPSR6, IPSR_28_FUNC(5)	/* DU_DG1 */
+		      | IPSR_24_FUNC(5)	/* DU_DG0 */
+		      | IPSR_20_FUNC(5)	/* DU_DR7 */
+		      | IPSR_16_FUNC(2)	/* IRQ5 */
+		      | IPSR_12_FUNC(5)	/* DU_DR6 */
+		      | IPSR_8_FUNC(5)	/* DU_DR5 */
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(5));	/* DU_DR4 */
+	pfc_reg_write(PFC_IPSR7, IPSR_28_FUNC(0)	/* SD0_CLK */
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(5)	/* DU_DOTCLKIN0 */
+		      | IPSR_16_FUNC(5)	/* DU_DG3 */
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(5));	/* DU_DG2 */
+	pfc_reg_write(PFC_IPSR8, IPSR_28_FUNC(0)	/* SD1_DAT0 */
+		      | IPSR_24_FUNC(0)	/* SD1_CMD */
+		      | IPSR_20_FUNC(0)	/* SD1_CLK */
+		      | IPSR_16_FUNC(0)	/* SD0_DAT3 */
+		      | IPSR_12_FUNC(0)	/* SD0_DAT2 */
+		      | IPSR_8_FUNC(0)	/* SD0_DAT1 */
+		      | IPSR_4_FUNC(0)	/* SD0_DAT0 */
+		      | IPSR_0_FUNC(0));	/* SD0_CMD */
+	pfc_reg_write(PFC_IPSR9, IPSR_28_FUNC(0)	/* SD3_DAT2 */
+		      | IPSR_24_FUNC(0)	/* SD3_DAT1 */
+		      | IPSR_20_FUNC(0)	/* SD3_DAT0 */
+		      | IPSR_16_FUNC(0)	/* SD3_CMD */
+		      | IPSR_12_FUNC(0)	/* SD3_CLK */
+		      | IPSR_8_FUNC(0)	/* SD1_DAT3 */
+		      | IPSR_4_FUNC(0)	/* SD1_DAT2 */
+		      | IPSR_0_FUNC(0));	/* SD1_DAT1 */
+	pfc_reg_write(PFC_IPSR10, IPSR_28_FUNC(0)	/* SD0_WP */
+		      | IPSR_24_FUNC(0)	/* SD0_CD */
+		      | IPSR_20_FUNC(0)	/* SD3_DS */
+		      | IPSR_16_FUNC(0)	/* SD3_DAT7 */
+		      | IPSR_12_FUNC(0)	/* SD3_DAT6 */
+		      | IPSR_8_FUNC(0)	/* SD3_DAT5 */
+		      | IPSR_4_FUNC(0)	/* SD3_DAT4 */
+		      | IPSR_0_FUNC(0));	/* SD3_DAT3 */
+	pfc_reg_write(PFC_IPSR11, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(2)	/* AUDIO_CLKOUT1_A */
+		      | IPSR_16_FUNC(2)	/* AUDIO_CLKOUT_A */
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)	/* SD1_WP */
+		      | IPSR_0_FUNC(0));	/* SD1_CD */
+	pfc_reg_write(PFC_IPSR12, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)	/* RX2_A */
+		      | IPSR_8_FUNC(0)	/* TX2_A */
+		      | IPSR_4_FUNC(2)	/* AUDIO_CLKB_A */
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR13, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(2)	/* AUDIO_CLKC_A */
+		      | IPSR_4_FUNC(1)	/* HTX2_A */
+		      | IPSR_0_FUNC(1));	/* HRX2_A */
+	pfc_reg_write(PFC_IPSR14, IPSR_28_FUNC(3)	/* USB0_PWEN_B */
+		      | IPSR_24_FUNC(0)	/* SSI_SDATA4 */
+		      | IPSR_20_FUNC(0)	/* SSI_SDATA3 */
+		      | IPSR_16_FUNC(0)	/* SSI_WS349 */
+		      | IPSR_12_FUNC(0)	/* SSI_SCK349 */
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)	/* SSI_SDATA1 */
+		      | IPSR_0_FUNC(0));	/* SSI_SDATA0 */
+	pfc_reg_write(PFC_IPSR15, IPSR_28_FUNC(0)	/* USB30_OVC */
+		      | IPSR_24_FUNC(0)	/* USB30_PWEN */
+		      | IPSR_20_FUNC(0)	/* AUDIO_CLKA */
+		      | IPSR_16_FUNC(1)	/* HRTS2#_A */
+		      | IPSR_12_FUNC(1)	/* HCTS2#_A */
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(3));	/* USB0_OVC_B */
+
+	/* initialize GPIO/perihperal function select */
+	pfc_reg_write(PFC_GPSR0, GPSR0_SCL4
+		      | GPSR0_D15
+		      | GPSR0_D11
+		      | GPSR0_D10
+		      | GPSR0_D9
+		      | GPSR0_D8
+		      | GPSR0_D7
+		      | GPSR0_D6
+		      | GPSR0_D5
+		      | GPSR0_D3
+		      | GPSR0_D2
+		      | GPSR0_D1
+		      | GPSR0_D0);
+	pfc_reg_write(PFC_GPSR1, GPSR1_WE0
+		      | GPSR1_CS0
+		      | GPSR1_A19
+		      | GPSR1_A18
+		      | GPSR1_A17
+		      | GPSR1_A16
+		      | GPSR1_A15
+		      | GPSR1_A14
+		      | GPSR1_A13
+		      | GPSR1_A12
+		      | GPSR1_A11
+		      | GPSR1_A10
+		      | GPSR1_A9
+		      | GPSR1_A8
+		      | GPSR1_A4
+		      | GPSR1_A3
+		      | GPSR1_A2
+		      | GPSR1_A1
+		      | GPSR1_A0);
+	pfc_reg_write(PFC_GPSR2, GPSR2_BIT27_REVERSED
+		      | GPSR2_BIT26_REVERSED
+		      | GPSR2_RD
+		      | GPSR2_AVB_PHY_INT
+		      | GPSR2_AVB_TXCREFCLK
+		      | GPSR2_AVB_RD3
+		      | GPSR2_AVB_RD2
+		      | GPSR2_AVB_RD1
+		      | GPSR2_AVB_RD0
+		      | GPSR2_AVB_RXC
+		      | GPSR2_AVB_RX_CTL
+		      | GPSR2_RPC_RESET
+		      | GPSR2_RPC_RPC_INT
+		      | GPSR2_QSPI1_SSL
+		      | GPSR2_QSPI1_IO3
+		      | GPSR2_QSPI1_IO2
+		      | GPSR2_QSPI1_MISO_IO1
+		      | GPSR2_QSPI1_MOSI_IO0
+		      | GPSR2_QSPI1_SPCLK
+		      | GPSR2_QSPI0_SSL
+		      | GPSR2_QSPI0_IO3
+		      | GPSR2_QSPI0_IO2
+		      | GPSR2_QSPI0_MISO_IO1
+		      | GPSR2_QSPI0_MOSI_IO0
+		      | GPSR2_QSPI0_SPCLK);
+	pfc_reg_write(PFC_GPSR3, GPSR3_SD1_WP
+		      | GPSR3_SD1_CD
+		      | GPSR3_SD0_WP
+		      | GPSR3_SD0_CD
+		      | GPSR3_SD1_DAT3
+		      | GPSR3_SD1_DAT2
+		      | GPSR3_SD1_DAT1
+		      | GPSR3_SD1_DAT0
+		      | GPSR3_SD1_CMD
+		      | GPSR3_SD1_CLK
+		      | GPSR3_SD0_DAT3
+		      | GPSR3_SD0_DAT2
+		      | GPSR3_SD0_DAT1
+		      | GPSR3_SD0_DAT0
+		      | GPSR3_SD0_CMD
+		      | GPSR3_SD0_CLK);
+	pfc_reg_write(PFC_GPSR4, GPSR4_SD3_DS
+		      | GPSR4_SD3_DAT7
+		      | GPSR4_SD3_DAT6
+		      | GPSR4_SD3_DAT5
+		      | GPSR4_SD3_DAT4
+		      | GPSR4_SD3_DAT3
+		      | GPSR4_SD3_DAT2
+		      | GPSR4_SD3_DAT1
+		      | GPSR4_SD3_DAT0
+		      | GPSR4_SD3_CMD
+		      | GPSR4_SD3_CLK);
+	pfc_reg_write(PFC_GPSR5, GPSR5_SSI_SDATA9
+		      | GPSR5_MSIOF0_SS2
+		      | GPSR5_MSIOF0_SS1
+		      | GPSR5_RX2_A
+		      | GPSR5_TX2_A
+		      | GPSR5_SCK2_A
+		      | GPSR5_RTS0_A
+		      | GPSR5_CTS0_A);
+	pfc_reg_write(PFC_GPSR6, GPSR6_USB30_PWEN
+		      | GPSR6_SSI_SDATA6
+		      | GPSR6_SSI_WS6
+		      | GPSR6_SSI_WS5
+		      | GPSR6_SSI_SCK5
+		      | GPSR6_SSI_SDATA4
+		      | GPSR6_USB30_OVC
+		      | GPSR6_AUDIO_CLKA
+		      | GPSR6_SSI_SDATA3
+		      | GPSR6_SSI_WS349
+		      | GPSR6_SSI_SCK349
+		      | GPSR6_SSI_SDATA1
+		      | GPSR6_SSI_SDATA0
+		      | GPSR6_SSI_WS01239
+		      | GPSR6_SSI_SCK01239);
+
+	/* initialize POC control */
+	reg = mmio_read_32(PFC_POCCTRL0);
+	reg = ((reg & POCCTRL0_MASK) | POC_SD1_DAT3_33V
+	       | POC_SD1_DAT2_33V
+	       | POC_SD1_DAT1_33V
+	       | POC_SD1_DAT0_33V
+	       | POC_SD1_CMD_33V
+	       | POC_SD1_CLK_33V
+	       | POC_SD0_DAT3_33V
+	       | POC_SD0_DAT2_33V
+	       | POC_SD0_DAT1_33V
+	       | POC_SD0_DAT0_33V
+	       | POC_SD0_CMD_33V
+	       | POC_SD0_CLK_33V);
+	pfc_reg_write(PFC_POCCTRL0, reg);
+	reg = mmio_read_32(PFC_POCCTRL2);
+	reg = (reg & POCCTRL2_MASK);
+	pfc_reg_write(PFC_POCCTRL2, reg);
+
+	/* initialize LSI pin pull-up/down control */
+	pfc_reg_write(PFC_PUD0, 0xFDF80000U);
+	pfc_reg_write(PFC_PUD1, 0xCE298464U);
+	pfc_reg_write(PFC_PUD2, 0xA4C380F4U);
+	pfc_reg_write(PFC_PUD3, 0x0000079FU);
+	pfc_reg_write(PFC_PUD4, 0xFFF0FFFFU);
+	pfc_reg_write(PFC_PUD5, 0x40000000U);
+
+	/* initialize LSI pin pull-enable register */
+	pfc_reg_write(PFC_PUEN0, 0xFFF00000U);
+	pfc_reg_write(PFC_PUEN1, 0x00000000U);
+	pfc_reg_write(PFC_PUEN2, 0x00000004U);
+	pfc_reg_write(PFC_PUEN3, 0x00000000U);
+	pfc_reg_write(PFC_PUEN4, 0x07800010U);
+	pfc_reg_write(PFC_PUEN5, 0x00000000U);
+
+	/* initialize positive/negative logic select */
+	mmio_write_32(GPIO_POSNEG0, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG1, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG2, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG3, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG4, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG5, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG6, 0x00000000U);
+
+	/* initialize general IO/interrupt switching */
+	mmio_write_32(GPIO_IOINTSEL0, 0x00020000U);
+	mmio_write_32(GPIO_IOINTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL5, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL6, 0x00000000U);
+
+	/* initialize general output register */
+	mmio_write_32(GPIO_OUTDT0, 0x00000010U);
+	mmio_write_32(GPIO_OUTDT1, 0x00100000U);
+	mmio_write_32(GPIO_OUTDT2, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT3, 0x00008000U);
+	mmio_write_32(GPIO_OUTDT5, 0x00060000U);
+	mmio_write_32(GPIO_OUTDT6, 0x00000000U);
+
+	/* initialize general input/output switching */
+	mmio_write_32(GPIO_INOUTSEL0, 0x00000010U);
+	mmio_write_32(GPIO_INOUTSEL1, 0x00100020U);
+	mmio_write_32(GPIO_INOUTSEL2, 0x03000000U);
+	mmio_write_32(GPIO_INOUTSEL3, 0x00008000U);
+	mmio_write_32(GPIO_INOUTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL5, 0x00060000U);
+	mmio_write_32(GPIO_INOUTSEL6, 0x00004000U);
+}
diff --git a/drivers/renesas/rcar/pfc/E3/pfc_init_e3.h b/drivers/renesas/rcar/pfc/E3/pfc_init_e3.h
new file mode 100644
index 0000000..647a937
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/E3/pfc_init_e3.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PFC_INIT_E3_H
+#define PFC_INIT_E3_H
+
+void pfc_init_e3(void);
+
+#endif /* PFC_INIT_E3_H */
diff --git a/drivers/renesas/rcar/pfc/H3/pfc_init_h3_v1.c b/drivers/renesas/rcar/pfc/H3/pfc_init_h3_v1.c
new file mode 100644
index 0000000..effdc76
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/H3/pfc_init_h3_v1.c
@@ -0,0 +1,1183 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <lib/mmio.h>
+#include "rcar_def.h"
+#include "../pfc_regs.h"
+
+#define GPSR0_D15		BIT(15)
+#define GPSR0_D14		BIT(14)
+#define GPSR0_D13		BIT(13)
+#define GPSR0_D12		BIT(12)
+#define GPSR0_D11		BIT(11)
+#define GPSR0_D10		BIT(10)
+#define GPSR0_D9		BIT(9)
+#define GPSR0_D8		BIT(8)
+#define GPSR0_D7		BIT(7)
+#define GPSR0_D6		BIT(6)
+#define GPSR0_D5		BIT(5)
+#define GPSR0_D4		BIT(4)
+#define GPSR0_D3		BIT(3)
+#define GPSR0_D2		BIT(2)
+#define GPSR0_D1		BIT(1)
+#define GPSR0_D0		BIT(0)
+#define GPSR1_EX_WAIT0_A	BIT(27)
+#define GPSR1_WE1		BIT(26)
+#define GPSR1_WE0		BIT(25)
+#define GPSR1_RD_WR		BIT(24)
+#define GPSR1_RD		BIT(23)
+#define GPSR1_BS		BIT(22)
+#define GPSR1_CS1_A26		BIT(21)
+#define GPSR1_CS0		BIT(20)
+#define GPSR1_A19		BIT(19)
+#define GPSR1_A18		BIT(18)
+#define GPSR1_A17		BIT(17)
+#define GPSR1_A16		BIT(16)
+#define GPSR1_A15		BIT(15)
+#define GPSR1_A14		BIT(14)
+#define GPSR1_A13		BIT(13)
+#define GPSR1_A12		BIT(12)
+#define GPSR1_A11		BIT(11)
+#define GPSR1_A10		BIT(10)
+#define GPSR1_A9		BIT(9)
+#define GPSR1_A8		BIT(8)
+#define GPSR1_A7		BIT(7)
+#define GPSR1_A6		BIT(6)
+#define GPSR1_A5		BIT(5)
+#define GPSR1_A4		BIT(4)
+#define GPSR1_A3		BIT(3)
+#define GPSR1_A2		BIT(2)
+#define GPSR1_A1		BIT(1)
+#define GPSR1_A0		BIT(0)
+#define GPSR2_AVB_AVTP_CAPTURE_A	BIT(14)
+#define GPSR2_AVB_AVTP_MATCH_A	BIT(13)
+#define GPSR2_AVB_LINK		BIT(12)
+#define GPSR2_AVB_PHY_INT	BIT(11)
+#define GPSR2_AVB_MAGIC		BIT(10)
+#define GPSR2_AVB_MDC		BIT(9)
+#define GPSR2_PWM2_A		BIT(8)
+#define GPSR2_PWM1_A		BIT(7)
+#define GPSR2_PWM0		BIT(6)
+#define GPSR2_IRQ5		BIT(5)
+#define GPSR2_IRQ4		BIT(4)
+#define GPSR2_IRQ3		BIT(3)
+#define GPSR2_IRQ2		BIT(2)
+#define GPSR2_IRQ1		BIT(1)
+#define GPSR2_IRQ0		BIT(0)
+#define GPSR3_SD1_WP		BIT(15)
+#define GPSR3_SD1_CD		BIT(14)
+#define GPSR3_SD0_WP		BIT(13)
+#define GPSR3_SD0_CD		BIT(12)
+#define GPSR3_SD1_DAT3		BIT(11)
+#define GPSR3_SD1_DAT2		BIT(10)
+#define GPSR3_SD1_DAT1		BIT(9)
+#define GPSR3_SD1_DAT0		BIT(8)
+#define GPSR3_SD1_CMD		BIT(7)
+#define GPSR3_SD1_CLK		BIT(6)
+#define GPSR3_SD0_DAT3		BIT(5)
+#define GPSR3_SD0_DAT2		BIT(4)
+#define GPSR3_SD0_DAT1		BIT(3)
+#define GPSR3_SD0_DAT0		BIT(2)
+#define GPSR3_SD0_CMD		BIT(1)
+#define GPSR3_SD0_CLK		BIT(0)
+#define GPSR4_SD3_DS		BIT(17)
+#define GPSR4_SD3_DAT7		BIT(16)
+#define GPSR4_SD3_DAT6		BIT(15)
+#define GPSR4_SD3_DAT5		BIT(14)
+#define GPSR4_SD3_DAT4		BIT(13)
+#define GPSR4_SD3_DAT3		BIT(12)
+#define GPSR4_SD3_DAT2		BIT(11)
+#define GPSR4_SD3_DAT1		BIT(10)
+#define GPSR4_SD3_DAT0		BIT(9)
+#define GPSR4_SD3_CMD		BIT(8)
+#define GPSR4_SD3_CLK		BIT(7)
+#define GPSR4_SD2_DS		BIT(6)
+#define GPSR4_SD2_DAT3		BIT(5)
+#define GPSR4_SD2_DAT2		BIT(4)
+#define GPSR4_SD2_DAT1		BIT(3)
+#define GPSR4_SD2_DAT0		BIT(2)
+#define GPSR4_SD2_CMD		BIT(1)
+#define GPSR4_SD2_CLK		BIT(0)
+#define GPSR5_MLB_DAT		BIT(25)
+#define GPSR5_MLB_SIG		BIT(24)
+#define GPSR5_MLB_CLK		BIT(23)
+#define GPSR5_MSIOF0_RXD	BIT(22)
+#define GPSR5_MSIOF0_SS2	BIT(21)
+#define GPSR5_MSIOF0_TXD	BIT(20)
+#define GPSR5_MSIOF0_SS1	BIT(19)
+#define GPSR5_MSIOF0_SYNC	BIT(18)
+#define GPSR5_MSIOF0_SCK	BIT(17)
+#define GPSR5_HRTS0		BIT(16)
+#define GPSR5_HCTS0		BIT(15)
+#define GPSR5_HTX0		BIT(14)
+#define GPSR5_HRX0		BIT(13)
+#define GPSR5_HSCK0		BIT(12)
+#define GPSR5_RX2_A		BIT(11)
+#define GPSR5_TX2_A		BIT(10)
+#define GPSR5_SCK2		BIT(9)
+#define GPSR5_RTS1		BIT(8)
+#define GPSR5_CTS1		BIT(7)
+#define GPSR5_TX1_A		BIT(6)
+#define GPSR5_RX1_A		BIT(5)
+#define GPSR5_RTS0		BIT(4)
+#define GPSR5_CTS0		BIT(3)
+#define GPSR5_TX0		BIT(2)
+#define GPSR5_RX0		BIT(1)
+#define GPSR5_SCK0		BIT(0)
+#define GPSR6_USB31_OVC		BIT(31)
+#define GPSR6_USB31_PWEN	BIT(30)
+#define GPSR6_USB30_OVC		BIT(29)
+#define GPSR6_USB30_PWEN	BIT(28)
+#define GPSR6_USB1_OVC		BIT(27)
+#define GPSR6_USB1_PWEN		BIT(26)
+#define GPSR6_USB0_OVC		BIT(25)
+#define GPSR6_USB0_PWEN		BIT(24)
+#define GPSR6_AUDIO_CLKB_B	BIT(23)
+#define GPSR6_AUDIO_CLKA_A	BIT(22)
+#define GPSR6_SSI_SDATA9_A	BIT(21)
+#define GPSR6_SSI_SDATA8	BIT(20)
+#define GPSR6_SSI_SDATA7	BIT(19)
+#define GPSR6_SSI_WS78		BIT(18)
+#define GPSR6_SSI_SCK78		BIT(17)
+#define GPSR6_SSI_SDATA6	BIT(16)
+#define GPSR6_SSI_WS6		BIT(15)
+#define GPSR6_SSI_SCK6		BIT(14)
+#define GPSR6_SSI_SDATA5	BIT(13)
+#define GPSR6_SSI_WS5		BIT(12)
+#define GPSR6_SSI_SCK5		BIT(11)
+#define GPSR6_SSI_SDATA4	BIT(10)
+#define GPSR6_SSI_WS4		BIT(9)
+#define GPSR6_SSI_SCK4		BIT(8)
+#define GPSR6_SSI_SDATA3	BIT(7)
+#define GPSR6_SSI_WS34		BIT(6)
+#define GPSR6_SSI_SCK34		BIT(5)
+#define GPSR6_SSI_SDATA2_A	BIT(4)
+#define GPSR6_SSI_SDATA1_A	BIT(3)
+#define GPSR6_SSI_SDATA0	BIT(2)
+#define GPSR6_SSI_WS0129	BIT(1)
+#define GPSR6_SSI_SCK0129	BIT(0)
+#define GPSR7_AVS2		BIT(1)
+#define GPSR7_AVS1		BIT(0)
+
+#define IPSR_28_FUNC(x)		((uint32_t)(x) << 28U)
+#define IPSR_24_FUNC(x)		((uint32_t)(x) << 24U)
+#define IPSR_20_FUNC(x)		((uint32_t)(x) << 20U)
+#define IPSR_16_FUNC(x)		((uint32_t)(x) << 16U)
+#define IPSR_12_FUNC(x)		((uint32_t)(x) << 12U)
+#define IPSR_8_FUNC(x)		((uint32_t)(x) << 8U)
+#define IPSR_4_FUNC(x)		((uint32_t)(x) << 4U)
+#define IPSR_0_FUNC(x)		((uint32_t)(x) << 0U)
+
+#define POC_SD3_DS_33V		BIT(29)
+#define POC_SD3_DAT7_33V	BIT(28)
+#define POC_SD3_DAT6_33V	BIT(27)
+#define POC_SD3_DAT5_33V	BIT(26)
+#define POC_SD3_DAT4_33V	BIT(25)
+#define POC_SD3_DAT3_33V	BIT(24)
+#define POC_SD3_DAT2_33V	BIT(23)
+#define POC_SD3_DAT1_33V	BIT(22)
+#define POC_SD3_DAT0_33V	BIT(21)
+#define POC_SD3_CMD_33V		BIT(20)
+#define POC_SD3_CLK_33V		BIT(19)
+#define POC_SD2_DS_33V		BIT(18)
+#define POC_SD2_DAT3_33V	BIT(17)
+#define POC_SD2_DAT2_33V	BIT(16)
+#define POC_SD2_DAT1_33V	BIT(15)
+#define POC_SD2_DAT0_33V	BIT(14)
+#define POC_SD2_CMD_33V		BIT(13)
+#define POC_SD2_CLK_33V		BIT(12)
+#define POC_SD1_DAT3_33V	BIT(11)
+#define POC_SD1_DAT2_33V	BIT(10)
+#define POC_SD1_DAT1_33V	BIT(9)
+#define POC_SD1_DAT0_33V	BIT(8)
+#define POC_SD1_CMD_33V		BIT(7)
+#define POC_SD1_CLK_33V		BIT(6)
+#define POC_SD0_DAT3_33V	BIT(5)
+#define POC_SD0_DAT2_33V	BIT(4)
+#define POC_SD0_DAT1_33V	BIT(3)
+#define POC_SD0_DAT0_33V	BIT(2)
+#define POC_SD0_CMD_33V		BIT(1)
+#define POC_SD0_CLK_33V		BIT(0)
+
+#define DRVCTRL0_MASK		(0xCCCCCCCCU)
+#define DRVCTRL1_MASK		(0xCCCCCCC8U)
+#define DRVCTRL2_MASK		(0x88888888U)
+#define DRVCTRL3_MASK		(0x88888888U)
+#define DRVCTRL4_MASK		(0x88888888U)
+#define DRVCTRL5_MASK		(0x88888888U)
+#define DRVCTRL6_MASK		(0x88888888U)
+#define DRVCTRL7_MASK		(0x88888888U)
+#define DRVCTRL8_MASK		(0x88888888U)
+#define DRVCTRL9_MASK		(0x88888888U)
+#define DRVCTRL10_MASK		(0x88888888U)
+#define DRVCTRL11_MASK		(0x888888CCU)
+#define DRVCTRL12_MASK		(0xCCCFFFCFU)
+#define DRVCTRL13_MASK		(0xCC888888U)
+#define DRVCTRL14_MASK		(0x88888888U)
+#define DRVCTRL15_MASK		(0x88888888U)
+#define DRVCTRL16_MASK		(0x88888888U)
+#define DRVCTRL17_MASK		(0x88888888U)
+#define DRVCTRL18_MASK		(0x88888888U)
+#define DRVCTRL19_MASK		(0x88888888U)
+#define DRVCTRL20_MASK		(0x88888888U)
+#define DRVCTRL21_MASK		(0x88888888U)
+#define DRVCTRL22_MASK		(0x88888888U)
+#define DRVCTRL23_MASK		(0x88888888U)
+#define DRVCTRL24_MASK		(0x8888888FU)
+
+#define DRVCTRL0_QSPI0_SPCLK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL0_QSPI0_MOSI_IO0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL0_QSPI0_MISO_IO1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL0_QSPI0_IO2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL0_QSPI0_IO3(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL0_QSPI0_SSL(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL0_QSPI1_SPCLK(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL0_QSPI1_MOSI_IO0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL1_QSPI1_MISO_IO1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL1_QSPI1_IO2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL1_QSPI1_IO3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL1_QSPI1_SS(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL1_RPC_INT(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL1_RPC_WP(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL1_RPC_RESET(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL1_AVB_RX_CTL(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL2_AVB_RXC(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL2_AVB_RD0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL2_AVB_RD1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL2_AVB_RD2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL2_AVB_RD3(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL2_AVB_TX_CTL(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL2_AVB_TXC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL2_AVB_TD0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL3_AVB_TD1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL3_AVB_TD2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL3_AVB_TD3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL3_AVB_TXCREFCLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL3_AVB_MDIO(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL3_AVB_MDC(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL3_AVB_MAGIC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL3_AVB_PHY_INT(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL4_AVB_LINK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL4_AVB_AVTP_MATCH(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL4_AVB_AVTP_CAPTURE(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL4_IRQ0(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL4_IRQ1(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL4_IRQ2(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL4_IRQ3(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL4_IRQ4(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL5_IRQ5(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL5_PWM0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL5_PWM1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL5_PWM2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL5_A0(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL5_A1(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL5_A2(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL5_A3(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL6_A4(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL6_A5(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL6_A6(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL6_A7(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL6_A8(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL6_A9(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL6_A10(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL6_A11(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL7_A12(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL7_A13(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL7_A14(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL7_A15(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL7_A16(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL7_A17(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL7_A18(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL7_A19(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL8_CLKOUT(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL8_CS0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL8_CS1_A2(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL8_BS(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL8_RD(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL8_RD_W(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL8_WE0(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL8_WE1(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL9_EX_WAIT0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL9_PRESETOU(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL9_D0(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL9_D1(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL9_D2(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL9_D3(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL9_D4(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL9_D5(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL10_D6(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL10_D7(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL10_D8(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL10_D9(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL10_D10(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL10_D11(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL10_D12(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL10_D13(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL11_D14(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL11_D15(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL11_AVS1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL11_AVS2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL11_GP7_02(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL11_GP7_03(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL11_DU_DOTCLKIN0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL11_DU_DOTCLKIN1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL12_DU_DOTCLKIN2(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL12_DU_DOTCLKIN3(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL12_DU_FSCLKST(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL12_DU_TMS(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL13_TDO(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL13_ASEBRK(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL13_SD0_CLK(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL13_SD0_CMD(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL13_SD0_DAT0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL13_SD0_DAT1(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL13_SD0_DAT2(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL13_SD0_DAT3(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL14_SD1_CLK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL14_SD1_CMD(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL14_SD1_DAT0(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL14_SD1_DAT1(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL14_SD1_DAT2(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL14_SD1_DAT3(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL14_SD2_CLK(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL14_SD2_CMD(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL15_SD2_DAT0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL15_SD2_DAT1(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL15_SD2_DAT2(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL15_SD2_DAT3(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL15_SD2_DS(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL15_SD3_CLK(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL15_SD3_CMD(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL15_SD3_DAT0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL16_SD3_DAT1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL16_SD3_DAT2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL16_SD3_DAT3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL16_SD3_DAT4(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL16_SD3_DAT5(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL16_SD3_DAT6(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL16_SD3_DAT7(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL16_SD3_DS(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL17_SD0_CD(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL17_SD0_WP(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL17_SD1_CD(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL17_SD1_WP(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL17_SCK0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL17_RX0(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL17_TX0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL17_CTS0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL18_RTS0_TANS(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL18_RX1(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL18_TX1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL18_CTS1(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL18_RTS1_TANS(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL18_SCK2(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL18_TX2(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL18_RX2(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL19_HSCK0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL19_HRX0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL19_HTX0(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL19_HCTS0(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL19_HRTS0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL19_MSIOF0_SCK(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL19_MSIOF0_SYNC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL19_MSIOF0_SS1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL20_MSIOF0_TXD(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL20_MSIOF0_SS2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL20_MSIOF0_RXD(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL20_MLB_CLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL20_MLB_SIG(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL20_MLB_DAT(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL20_MLB_REF(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL20_SSI_SCK0129(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL21_SSI_WS0129(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL21_SSI_SDATA0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL21_SSI_SDATA1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL21_SSI_SDATA2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL21_SSI_SCK34(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL21_SSI_WS34(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL21_SSI_SDATA3(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL21_SSI_SCK4(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL22_SSI_WS4(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL22_SSI_SDATA4(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL22_SSI_SCK5(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL22_SSI_WS5(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL22_SSI_SDATA5(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL22_SSI_SCK6(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL22_SSI_WS6(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL22_SSI_SDATA6(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL23_SSI_SCK78(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL23_SSI_WS78(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL23_SSI_SDATA7(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL23_SSI_SDATA8(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL23_SSI_SDATA9(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL23_AUDIO_CLKA(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL23_AUDIO_CLKB(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL23_USB0_PWEN(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL24_USB0_OVC(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL24_USB1_PWEN(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL24_USB1_OVC(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL24_USB30_PWEN(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL24_USB30_OVC(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL24_USB31_PWEN(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL24_USB31_OVC(x)	((uint32_t)(x) << 4U)
+
+#define MOD_SEL0_MSIOF3_A	((uint32_t)0U << 29U)
+#define MOD_SEL0_MSIOF3_B	((uint32_t)1U << 29U)
+#define MOD_SEL0_MSIOF3_C	((uint32_t)2U << 29U)
+#define MOD_SEL0_MSIOF3_D	((uint32_t)3U << 29U)
+#define MOD_SEL0_MSIOF2_A	((uint32_t)0U << 27U)
+#define MOD_SEL0_MSIOF2_B	((uint32_t)1U << 27U)
+#define MOD_SEL0_MSIOF2_C	((uint32_t)2U << 27U)
+#define MOD_SEL0_MSIOF2_D	((uint32_t)3U << 27U)
+#define MOD_SEL0_MSIOF1_A	((uint32_t)0U << 24U)
+#define MOD_SEL0_MSIOF1_B	((uint32_t)1U << 24U)
+#define MOD_SEL0_MSIOF1_C	((uint32_t)2U << 24U)
+#define MOD_SEL0_MSIOF1_D	((uint32_t)3U << 24U)
+#define MOD_SEL0_MSIOF1_E	((uint32_t)4U << 24U)
+#define MOD_SEL0_MSIOF1_F	((uint32_t)5U << 24U)
+#define MOD_SEL0_MSIOF1_G	((uint32_t)6U << 24U)
+#define MOD_SEL0_LBSC_A		((uint32_t)0U << 23U)
+#define MOD_SEL0_LBSC_B		((uint32_t)1U << 23U)
+#define MOD_SEL0_IEBUS_A	((uint32_t)0U << 22U)
+#define MOD_SEL0_IEBUS_B	((uint32_t)1U << 22U)
+#define MOD_SEL0_I2C6_A		((uint32_t)0U << 20U)
+#define MOD_SEL0_I2C6_B		((uint32_t)1U << 20U)
+#define MOD_SEL0_I2C6_C		((uint32_t)2U << 20U)
+#define MOD_SEL0_I2C2_A		((uint32_t)0U << 19U)
+#define MOD_SEL0_I2C2_B		((uint32_t)1U << 19U)
+#define MOD_SEL0_I2C1_A		((uint32_t)0U << 18U)
+#define MOD_SEL0_I2C1_B		((uint32_t)1U << 18U)
+#define MOD_SEL0_HSCIF4_A	((uint32_t)0U << 17U)
+#define MOD_SEL0_HSCIF4_B	((uint32_t)1U << 17U)
+#define MOD_SEL0_HSCIF3_A	((uint32_t)0U << 15U)
+#define MOD_SEL0_HSCIF3_B	((uint32_t)1U << 15U)
+#define MOD_SEL0_HSCIF3_C	((uint32_t)2U << 15U)
+#define MOD_SEL0_HSCIF3_D	((uint32_t)3U << 15U)
+#define MOD_SEL0_HSCIF2_A	((uint32_t)0U << 14U)
+#define MOD_SEL0_HSCIF2_B	((uint32_t)1U << 14U)
+#define MOD_SEL0_HSCIF1_A	((uint32_t)0U << 13U)
+#define MOD_SEL0_HSCIF1_B	((uint32_t)1U << 13U)
+#define MOD_SEL0_FSO_A		((uint32_t)0U << 12U)
+#define MOD_SEL0_FSO_B		((uint32_t)1U << 12U)
+#define MOD_SEL0_FM_A		((uint32_t)0U << 11U)
+#define MOD_SEL0_FM_B		((uint32_t)1U << 11U)
+#define MOD_SEL0_ETHERAVB_A	((uint32_t)0U << 10U)
+#define MOD_SEL0_ETHERAVB_B	((uint32_t)1U << 10U)
+#define MOD_SEL0_DRIF3_A	((uint32_t)0U << 9U)
+#define MOD_SEL0_DRIF3_B	((uint32_t)1U << 9U)
+#define MOD_SEL0_DRIF2_A	((uint32_t)0U << 8U)
+#define MOD_SEL0_DRIF2_B	((uint32_t)1U << 8U)
+#define MOD_SEL0_DRIF1_A	((uint32_t)0U << 6U)
+#define MOD_SEL0_DRIF1_B	((uint32_t)1U << 6U)
+#define MOD_SEL0_DRIF1_C	((uint32_t)2U << 6U)
+#define MOD_SEL0_DRIF0_A	((uint32_t)0U << 4U)
+#define MOD_SEL0_DRIF0_B	((uint32_t)1U << 4U)
+#define MOD_SEL0_DRIF0_C	((uint32_t)2U << 4U)
+#define MOD_SEL0_CANFD0_A	((uint32_t)0U << 3U)
+#define MOD_SEL0_CANFD0_B	((uint32_t)1U << 3U)
+#define MOD_SEL0_ADG_A		((uint32_t)0U << 1U)
+#define MOD_SEL0_ADG_B		((uint32_t)1U << 1U)
+#define MOD_SEL0_ADG_C		((uint32_t)2U << 1U)
+#define MOD_SEL0_ADG_D		((uint32_t)3U << 1U)
+#define MOD_SEL0_5LINE_A	((uint32_t)0U << 0U)
+#define MOD_SEL0_5LINE_B	((uint32_t)1U << 0U)
+#define MOD_SEL1_TSIF1_A	((uint32_t)0U << 30U)
+#define MOD_SEL1_TSIF1_B	((uint32_t)1U << 30U)
+#define MOD_SEL1_TSIF1_C	((uint32_t)2U << 30U)
+#define MOD_SEL1_TSIF1_D	((uint32_t)3U << 30U)
+#define MOD_SEL1_TSIF0_A	((uint32_t)0U << 27U)
+#define MOD_SEL1_TSIF0_B	((uint32_t)1U << 27U)
+#define MOD_SEL1_TSIF0_C	((uint32_t)2U << 27U)
+#define MOD_SEL1_TSIF0_D	((uint32_t)3U << 27U)
+#define MOD_SEL1_TSIF0_E	((uint32_t)4U << 27U)
+#define MOD_SEL1_TIMER_TMU_A	((uint32_t)0U << 26U)
+#define MOD_SEL1_TIMER_TMU_B	((uint32_t)1U << 26U)
+#define MOD_SEL1_SSP1_1_A	((uint32_t)0U << 24U)
+#define MOD_SEL1_SSP1_1_B	((uint32_t)1U << 24U)
+#define MOD_SEL1_SSP1_1_C	((uint32_t)2U << 24U)
+#define MOD_SEL1_SSP1_1_D	((uint32_t)3U << 24U)
+#define MOD_SEL1_SSP1_0_A	((uint32_t)0U << 21U)
+#define MOD_SEL1_SSP1_0_B	((uint32_t)1U << 21U)
+#define MOD_SEL1_SSP1_0_C	((uint32_t)2U << 21U)
+#define MOD_SEL1_SSP1_0_D	((uint32_t)3U << 21U)
+#define MOD_SEL1_SSP1_0_E	((uint32_t)4U << 21U)
+#define MOD_SEL1_SSI_A		((uint32_t)0U << 20U)
+#define MOD_SEL1_SSI_B		((uint32_t)1U << 20U)
+#define MOD_SEL1_SPEED_PULSE_IF_A	((uint32_t)0U << 19U)
+#define MOD_SEL1_SPEED_PULSE_IF_B	((uint32_t)1U << 19U)
+#define MOD_SEL1_SIMCARD_A	((uint32_t)0U << 17U)
+#define MOD_SEL1_SIMCARD_B	((uint32_t)1U << 17U)
+#define MOD_SEL1_SIMCARD_C	((uint32_t)2U << 17U)
+#define MOD_SEL1_SIMCARD_D	((uint32_t)3U << 17U)
+#define MOD_SEL1_SDHI2_A	((uint32_t)0U << 16U)
+#define MOD_SEL1_SDHI2_B	((uint32_t)1U << 16U)
+#define MOD_SEL1_SCIF4_A	((uint32_t)0U << 14U)
+#define MOD_SEL1_SCIF4_B	((uint32_t)1U << 14U)
+#define MOD_SEL1_SCIF4_C	((uint32_t)2U << 14U)
+#define MOD_SEL1_SCIF3_A	((uint32_t)0U << 13U)
+#define MOD_SEL1_SCIF3_B	((uint32_t)1U << 13U)
+#define MOD_SEL1_SCIF2_A	((uint32_t)0U << 12U)
+#define MOD_SEL1_SCIF2_B	((uint32_t)1U << 12U)
+#define MOD_SEL1_SCIF1_A	((uint32_t)0U << 11U)
+#define MOD_SEL1_SCIF1_B	((uint32_t)1U << 11U)
+#define MOD_SEL1_SCIF_A		((uint32_t)0U << 10U)
+#define MOD_SEL1_SCIF_B		((uint32_t)1U << 10U)
+#define MOD_SEL1_REMOCON_A	((uint32_t)0U << 9U)
+#define MOD_SEL1_REMOCON_B	((uint32_t)1U << 9U)
+#define MOD_SEL1_RCAN0_A	((uint32_t)0U << 6U)
+#define MOD_SEL1_RCAN0_B	((uint32_t)1U << 6U)
+#define MOD_SEL1_PWM6_A		((uint32_t)0U << 5U)
+#define MOD_SEL1_PWM6_B		((uint32_t)1U << 5U)
+#define MOD_SEL1_PWM5_A		((uint32_t)0U << 4U)
+#define MOD_SEL1_PWM5_B		((uint32_t)1U << 4U)
+#define MOD_SEL1_PWM4_A		((uint32_t)0U << 3U)
+#define MOD_SEL1_PWM4_B		((uint32_t)1U << 3U)
+#define MOD_SEL1_PWM3_A		((uint32_t)0U << 2U)
+#define MOD_SEL1_PWM3_B		((uint32_t)1U << 2U)
+#define MOD_SEL1_PWM2_A		((uint32_t)0U << 1U)
+#define MOD_SEL1_PWM2_B		((uint32_t)1U << 1U)
+#define MOD_SEL1_PWM1_A		((uint32_t)0U << 0U)
+#define MOD_SEL1_PWM1_B		((uint32_t)1U << 0U)
+#define MOD_SEL2_I2C_5_A	((uint32_t)0U << 31U)
+#define MOD_SEL2_I2C_5_B	((uint32_t)1U << 31U)
+#define MOD_SEL2_I2C_3_A	((uint32_t)0U << 30U)
+#define MOD_SEL2_I2C_3_B	((uint32_t)1U << 30U)
+#define MOD_SEL2_I2C_0_A	((uint32_t)0U << 29U)
+#define MOD_SEL2_I2C_0_B	((uint32_t)1U << 29U)
+#define MOD_SEL2_VIN4_A		((uint32_t)0U << 0U)
+#define MOD_SEL2_VIN4_B		((uint32_t)1U << 0U)
+
+static void pfc_reg_write(uint32_t addr, uint32_t data)
+{
+	mmio_write_32(PFC_PMMR, ~data);
+	mmio_write_32((uintptr_t)addr, data);
+}
+
+void pfc_init_h3_v1(void)
+{
+	uint32_t reg;
+
+	/* initialize module select */
+	pfc_reg_write(PFC_MOD_SEL0, MOD_SEL0_MSIOF3_A
+		      | MOD_SEL0_MSIOF2_A
+		      | MOD_SEL0_MSIOF1_A
+		      | MOD_SEL0_LBSC_A
+		      | MOD_SEL0_IEBUS_A
+		      | MOD_SEL0_I2C6_A
+		      | MOD_SEL0_I2C2_A
+		      | MOD_SEL0_I2C1_A
+		      | MOD_SEL0_HSCIF4_A
+		      | MOD_SEL0_HSCIF3_A
+		      | MOD_SEL0_HSCIF2_A
+		      | MOD_SEL0_HSCIF1_A
+		      | MOD_SEL0_FM_A
+		      | MOD_SEL0_ETHERAVB_A
+		      | MOD_SEL0_DRIF3_A
+		      | MOD_SEL0_DRIF2_A
+		      | MOD_SEL0_DRIF1_A
+		      | MOD_SEL0_DRIF0_A
+		      | MOD_SEL0_CANFD0_A
+		      | MOD_SEL0_ADG_A
+		      | MOD_SEL0_5LINE_A);
+	pfc_reg_write(PFC_MOD_SEL1, MOD_SEL1_TSIF1_A
+		      | MOD_SEL1_TSIF0_A
+		      | MOD_SEL1_TIMER_TMU_A
+		      | MOD_SEL1_SSP1_1_A
+		      | MOD_SEL1_SSP1_0_A
+		      | MOD_SEL1_SSI_A
+		      | MOD_SEL1_SPEED_PULSE_IF_A
+		      | MOD_SEL1_SIMCARD_A
+		      | MOD_SEL1_SDHI2_A
+		      | MOD_SEL1_SCIF4_A
+		      | MOD_SEL1_SCIF3_A
+		      | MOD_SEL1_SCIF2_A
+		      | MOD_SEL1_SCIF1_A
+		      | MOD_SEL1_SCIF_A
+		      | MOD_SEL1_REMOCON_A
+		      | MOD_SEL1_RCAN0_A
+		      | MOD_SEL1_PWM6_A
+		      | MOD_SEL1_PWM5_A
+		      | MOD_SEL1_PWM4_A
+		      | MOD_SEL1_PWM3_A
+		      | MOD_SEL1_PWM2_A
+		      | MOD_SEL1_PWM1_A);
+	pfc_reg_write(PFC_MOD_SEL2, MOD_SEL2_I2C_5_A
+		      | MOD_SEL2_I2C_3_A
+		      | MOD_SEL2_I2C_0_A
+		      | MOD_SEL2_VIN4_A);
+
+	/* initialize peripheral function select */
+	pfc_reg_write(PFC_IPSR0, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR1, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(3)
+		      | IPSR_8_FUNC(3)
+		      | IPSR_4_FUNC(3)
+		      | IPSR_0_FUNC(3));
+	pfc_reg_write(PFC_IPSR2, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR3, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR4, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR5, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR6, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR7, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR8, IPSR_28_FUNC(1)
+		      | IPSR_24_FUNC(1)
+		      | IPSR_20_FUNC(1)
+		      | IPSR_16_FUNC(1)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR9, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR10, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(4)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(1)
+		      | IPSR_0_FUNC(1));
+	pfc_reg_write(PFC_IPSR11, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(4)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR12, IPSR_28_FUNC(8)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(3)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR13, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(3)
+		      | IPSR_0_FUNC(8));
+	pfc_reg_write(PFC_IPSR14, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR15, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(1)
+		      | IPSR_0_FUNC(1));
+	pfc_reg_write(PFC_IPSR16, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(1)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR17, IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+
+	/* initialize GPIO/perihperal function select */
+	pfc_reg_write(PFC_GPSR0, GPSR0_D15
+		      | GPSR0_D14
+		      | GPSR0_D13
+		      | GPSR0_D12
+		      | GPSR0_D11
+		      | GPSR0_D10
+		      | GPSR0_D9
+		      | GPSR0_D8);
+	pfc_reg_write(PFC_GPSR1, GPSR1_EX_WAIT0_A
+		      | GPSR1_A19
+		      | GPSR1_A18
+		      | GPSR1_A17
+		      | GPSR1_A16
+		      | GPSR1_A15
+		      | GPSR1_A14
+		      | GPSR1_A13
+		      | GPSR1_A12
+		      | GPSR1_A7
+		      | GPSR1_A6
+		      | GPSR1_A5
+		      | GPSR1_A4
+		      | GPSR1_A3
+		      | GPSR1_A2
+		      | GPSR1_A1
+		      | GPSR1_A0);
+	pfc_reg_write(PFC_GPSR2, GPSR2_AVB_AVTP_CAPTURE_A
+		      | GPSR2_AVB_AVTP_MATCH_A
+		      | GPSR2_AVB_LINK
+		      | GPSR2_AVB_PHY_INT
+		      | GPSR2_AVB_MDC
+		      | GPSR2_PWM2_A
+		      | GPSR2_PWM1_A
+		      | GPSR2_IRQ5
+		      | GPSR2_IRQ4
+		      | GPSR2_IRQ3
+		      | GPSR2_IRQ2
+		      | GPSR2_IRQ1
+		      | GPSR2_IRQ0);
+	pfc_reg_write(PFC_GPSR3, GPSR3_SD0_WP
+		      | GPSR3_SD0_CD
+		      | GPSR3_SD1_DAT3
+		      | GPSR3_SD1_DAT2
+		      | GPSR3_SD1_DAT1
+		      | GPSR3_SD1_DAT0
+		      | GPSR3_SD0_DAT3
+		      | GPSR3_SD0_DAT2
+		      | GPSR3_SD0_DAT1
+		      | GPSR3_SD0_DAT0
+		      | GPSR3_SD0_CMD
+		      | GPSR3_SD0_CLK);
+	pfc_reg_write(PFC_GPSR4, GPSR4_SD3_DAT7
+		      | GPSR4_SD3_DAT6
+		      | GPSR4_SD3_DAT3
+		      | GPSR4_SD3_DAT2
+		      | GPSR4_SD3_DAT1
+		      | GPSR4_SD3_DAT0
+		      | GPSR4_SD3_CMD
+		      | GPSR4_SD3_CLK
+		      | GPSR4_SD2_DS
+		      | GPSR4_SD2_DAT3
+		      | GPSR4_SD2_DAT2
+		      | GPSR4_SD2_DAT1
+		      | GPSR4_SD2_DAT0
+		      | GPSR4_SD2_CMD
+		      | GPSR4_SD2_CLK);
+	pfc_reg_write(PFC_GPSR5, GPSR5_MSIOF0_SS2
+		      | GPSR5_MSIOF0_SS1
+		      | GPSR5_MSIOF0_SYNC
+		      | GPSR5_HRTS0
+		      | GPSR5_HCTS0
+		      | GPSR5_HTX0
+		      | GPSR5_HRX0
+		      | GPSR5_HSCK0
+		      | GPSR5_RX2_A
+		      | GPSR5_TX2_A
+		      | GPSR5_SCK2
+		      | GPSR5_RTS1
+		      | GPSR5_CTS1
+		      | GPSR5_TX1_A
+		      | GPSR5_RX1_A
+		      | GPSR5_RTS0
+		      | GPSR5_SCK0);
+	pfc_reg_write(PFC_GPSR6, GPSR6_USB30_OVC
+		      | GPSR6_USB30_PWEN
+		      | GPSR6_USB1_OVC
+		      | GPSR6_USB1_PWEN
+		      | GPSR6_USB0_OVC
+		      | GPSR6_USB0_PWEN
+		      | GPSR6_AUDIO_CLKB_B
+		      | GPSR6_AUDIO_CLKA_A
+		      | GPSR6_SSI_SDATA8
+		      | GPSR6_SSI_SDATA7
+		      | GPSR6_SSI_WS78
+		      | GPSR6_SSI_SCK78
+		      | GPSR6_SSI_WS6
+		      | GPSR6_SSI_SCK6
+		      | GPSR6_SSI_SDATA4
+		      | GPSR6_SSI_WS4
+		      | GPSR6_SSI_SCK4
+		      | GPSR6_SSI_SDATA1_A
+		      | GPSR6_SSI_SDATA0
+		      | GPSR6_SSI_WS0129
+		      | GPSR6_SSI_SCK0129);
+	pfc_reg_write(PFC_GPSR7, GPSR7_AVS2
+		      | GPSR7_AVS1);
+
+	/* initialize POC control register */
+	pfc_reg_write(PFC_POCCTRL0, POC_SD3_DS_33V
+		      | POC_SD3_DAT7_33V
+		      | POC_SD3_DAT6_33V
+		      | POC_SD3_DAT5_33V
+		      | POC_SD3_DAT4_33V
+		      | POC_SD3_DAT3_33V
+		      | POC_SD3_DAT2_33V
+		      | POC_SD3_DAT1_33V
+		      | POC_SD3_DAT0_33V
+		      | POC_SD3_CMD_33V
+		      | POC_SD3_CLK_33V
+		      | POC_SD0_DAT3_33V
+		      | POC_SD0_DAT2_33V
+		      | POC_SD0_DAT1_33V
+		      | POC_SD0_DAT0_33V
+		      | POC_SD0_CMD_33V
+		      | POC_SD0_CLK_33V);
+
+	/* initialize DRV control register */
+	reg = mmio_read_32(PFC_DRVCTRL0);
+	reg = ((reg & DRVCTRL0_MASK) | DRVCTRL0_QSPI0_SPCLK(3)
+	       | DRVCTRL0_QSPI0_MOSI_IO0(3)
+	       | DRVCTRL0_QSPI0_MISO_IO1(3)
+	       | DRVCTRL0_QSPI0_IO2(3)
+	       | DRVCTRL0_QSPI0_IO3(3)
+	       | DRVCTRL0_QSPI0_SSL(3)
+	       | DRVCTRL0_QSPI1_SPCLK(3)
+	       | DRVCTRL0_QSPI1_MOSI_IO0(3));
+	pfc_reg_write(PFC_DRVCTRL0, reg);
+	reg = mmio_read_32(PFC_DRVCTRL1);
+	reg = ((reg & DRVCTRL1_MASK) | DRVCTRL1_QSPI1_MISO_IO1(3)
+	       | DRVCTRL1_QSPI1_IO2(3)
+	       | DRVCTRL1_QSPI1_IO3(3)
+	       | DRVCTRL1_QSPI1_SS(3)
+	       | DRVCTRL1_RPC_INT(3)
+	       | DRVCTRL1_RPC_WP(3)
+	       | DRVCTRL1_RPC_RESET(3)
+	       | DRVCTRL1_AVB_RX_CTL(7));
+	pfc_reg_write(PFC_DRVCTRL1, reg);
+	reg = mmio_read_32(PFC_DRVCTRL2);
+	reg = ((reg & DRVCTRL2_MASK) | DRVCTRL2_AVB_RXC(7)
+	       | DRVCTRL2_AVB_RD0(7)
+	       | DRVCTRL2_AVB_RD1(7)
+	       | DRVCTRL2_AVB_RD2(7)
+	       | DRVCTRL2_AVB_RD3(7)
+	       | DRVCTRL2_AVB_TX_CTL(3)
+	       | DRVCTRL2_AVB_TXC(3)
+	       | DRVCTRL2_AVB_TD0(3));
+	pfc_reg_write(PFC_DRVCTRL2, reg);
+	reg = mmio_read_32(PFC_DRVCTRL3);
+	reg = ((reg & DRVCTRL3_MASK) | DRVCTRL3_AVB_TD1(3)
+	       | DRVCTRL3_AVB_TD2(3)
+	       | DRVCTRL3_AVB_TD3(3)
+	       | DRVCTRL3_AVB_TXCREFCLK(7)
+	       | DRVCTRL3_AVB_MDIO(7)
+	       | DRVCTRL3_AVB_MDC(7)
+	       | DRVCTRL3_AVB_MAGIC(7)
+	       | DRVCTRL3_AVB_PHY_INT(7));
+	pfc_reg_write(PFC_DRVCTRL3, reg);
+	reg = mmio_read_32(PFC_DRVCTRL4);
+	reg = ((reg & DRVCTRL4_MASK) | DRVCTRL4_AVB_LINK(7)
+	       | DRVCTRL4_AVB_AVTP_MATCH(7)
+	       | DRVCTRL4_AVB_AVTP_CAPTURE(7)
+	       | DRVCTRL4_IRQ0(7)
+	       | DRVCTRL4_IRQ1(7)
+	       | DRVCTRL4_IRQ2(7)
+	       | DRVCTRL4_IRQ3(7)
+	       | DRVCTRL4_IRQ4(7));
+	pfc_reg_write(PFC_DRVCTRL4, reg);
+	reg = mmio_read_32(PFC_DRVCTRL5);
+	reg = ((reg & DRVCTRL5_MASK) | DRVCTRL5_IRQ5(7)
+	       | DRVCTRL5_PWM0(7)
+	       | DRVCTRL5_PWM1(7)
+	       | DRVCTRL5_PWM2(7)
+	       | DRVCTRL5_A0(3)
+	       | DRVCTRL5_A1(3)
+	       | DRVCTRL5_A2(3)
+	       | DRVCTRL5_A3(3));
+	pfc_reg_write(PFC_DRVCTRL5, reg);
+	reg = mmio_read_32(PFC_DRVCTRL6);
+	reg = ((reg & DRVCTRL6_MASK) | DRVCTRL6_A4(3)
+	       | DRVCTRL6_A5(3)
+	       | DRVCTRL6_A6(3)
+	       | DRVCTRL6_A7(3)
+	       | DRVCTRL6_A8(7)
+	       | DRVCTRL6_A9(7)
+	       | DRVCTRL6_A10(7)
+	       | DRVCTRL6_A11(7));
+	pfc_reg_write(PFC_DRVCTRL6, reg);
+	reg = mmio_read_32(PFC_DRVCTRL7);
+	reg = ((reg & DRVCTRL7_MASK) | DRVCTRL7_A12(3)
+	       | DRVCTRL7_A13(3)
+	       | DRVCTRL7_A14(3)
+	       | DRVCTRL7_A15(3)
+	       | DRVCTRL7_A16(3)
+	       | DRVCTRL7_A17(3)
+	       | DRVCTRL7_A18(3)
+	       | DRVCTRL7_A19(3));
+	pfc_reg_write(PFC_DRVCTRL7, reg);
+	reg = mmio_read_32(PFC_DRVCTRL8);
+	reg = ((reg & DRVCTRL8_MASK) | DRVCTRL8_CLKOUT(7)
+	       | DRVCTRL8_CS0(7)
+	       | DRVCTRL8_CS1_A2(7)
+	       | DRVCTRL8_BS(7)
+	       | DRVCTRL8_RD(7)
+	       | DRVCTRL8_RD_W(7)
+	       | DRVCTRL8_WE0(7)
+	       | DRVCTRL8_WE1(7));
+	pfc_reg_write(PFC_DRVCTRL8, reg);
+	reg = mmio_read_32(PFC_DRVCTRL9);
+	reg = ((reg & DRVCTRL9_MASK) | DRVCTRL9_EX_WAIT0(7)
+	       | DRVCTRL9_PRESETOU(7)
+	       | DRVCTRL9_D0(7)
+	       | DRVCTRL9_D1(7)
+	       | DRVCTRL9_D2(7)
+	       | DRVCTRL9_D3(7)
+	       | DRVCTRL9_D4(7)
+	       | DRVCTRL9_D5(7));
+	pfc_reg_write(PFC_DRVCTRL9, reg);
+	reg = mmio_read_32(PFC_DRVCTRL10);
+	reg = ((reg & DRVCTRL10_MASK) | DRVCTRL10_D6(7)
+	       | DRVCTRL10_D7(7)
+	       | DRVCTRL10_D8(3)
+	       | DRVCTRL10_D9(3)
+	       | DRVCTRL10_D10(3)
+	       | DRVCTRL10_D11(3)
+	       | DRVCTRL10_D12(3)
+	       | DRVCTRL10_D13(3));
+	pfc_reg_write(PFC_DRVCTRL10, reg);
+	reg = mmio_read_32(PFC_DRVCTRL11);
+	reg = ((reg & DRVCTRL11_MASK) | DRVCTRL11_D14(3)
+	       | DRVCTRL11_D15(3)
+	       | DRVCTRL11_AVS1(7)
+	       | DRVCTRL11_AVS2(7)
+	       | DRVCTRL11_GP7_02(7)
+	       | DRVCTRL11_GP7_03(7)
+	       | DRVCTRL11_DU_DOTCLKIN0(3)
+	       | DRVCTRL11_DU_DOTCLKIN1(3));
+	pfc_reg_write(PFC_DRVCTRL11, reg);
+	reg = mmio_read_32(PFC_DRVCTRL12);
+	reg = ((reg & DRVCTRL12_MASK) | DRVCTRL12_DU_DOTCLKIN2(3)
+	       | DRVCTRL12_DU_DOTCLKIN3(3)
+	       | DRVCTRL12_DU_FSCLKST(3)
+	       | DRVCTRL12_DU_TMS(3));
+	pfc_reg_write(PFC_DRVCTRL12, reg);
+	reg = mmio_read_32(PFC_DRVCTRL13);
+	reg = ((reg & DRVCTRL13_MASK) | DRVCTRL13_TDO(3)
+	       | DRVCTRL13_ASEBRK(3)
+	       | DRVCTRL13_SD0_CLK(2)
+	       | DRVCTRL13_SD0_CMD(2)
+	       | DRVCTRL13_SD0_DAT0(2)
+	       | DRVCTRL13_SD0_DAT1(2)
+	       | DRVCTRL13_SD0_DAT2(2)
+	       | DRVCTRL13_SD0_DAT3(2));
+	pfc_reg_write(PFC_DRVCTRL13, reg);
+	reg = mmio_read_32(PFC_DRVCTRL14);
+	reg = ((reg & DRVCTRL14_MASK) | DRVCTRL14_SD1_CLK(7)
+	       | DRVCTRL14_SD1_CMD(7)
+	       | DRVCTRL14_SD1_DAT0(5)
+	       | DRVCTRL14_SD1_DAT1(5)
+	       | DRVCTRL14_SD1_DAT2(5)
+	       | DRVCTRL14_SD1_DAT3(5)
+	       | DRVCTRL14_SD2_CLK(5)
+	       | DRVCTRL14_SD2_CMD(5));
+	pfc_reg_write(PFC_DRVCTRL14, reg);
+	reg = mmio_read_32(PFC_DRVCTRL15);
+	reg = ((reg & DRVCTRL15_MASK) | DRVCTRL15_SD2_DAT0(5)
+	       | DRVCTRL15_SD2_DAT1(5)
+	       | DRVCTRL15_SD2_DAT2(5)
+	       | DRVCTRL15_SD2_DAT3(5)
+	       | DRVCTRL15_SD2_DS(5)
+	       | DRVCTRL15_SD3_CLK(2)
+	       | DRVCTRL15_SD3_CMD(2)
+	       | DRVCTRL15_SD3_DAT0(2));
+	pfc_reg_write(PFC_DRVCTRL15, reg);
+	reg = mmio_read_32(PFC_DRVCTRL16);
+	reg = ((reg & DRVCTRL16_MASK) | DRVCTRL16_SD3_DAT1(2)
+	       | DRVCTRL16_SD3_DAT2(2)
+	       | DRVCTRL16_SD3_DAT3(2)
+	       | DRVCTRL16_SD3_DAT4(7)
+	       | DRVCTRL16_SD3_DAT5(7)
+	       | DRVCTRL16_SD3_DAT6(7)
+	       | DRVCTRL16_SD3_DAT7(7)
+	       | DRVCTRL16_SD3_DS(7));
+	pfc_reg_write(PFC_DRVCTRL16, reg);
+	reg = mmio_read_32(PFC_DRVCTRL17);
+	reg = ((reg & DRVCTRL17_MASK) | DRVCTRL17_SD0_CD(7)
+	       | DRVCTRL17_SD0_WP(7)
+	       | DRVCTRL17_SD1_CD(7)
+	       | DRVCTRL17_SD1_WP(7)
+	       | DRVCTRL17_SCK0(7)
+	       | DRVCTRL17_RX0(7)
+	       | DRVCTRL17_TX0(7)
+	       | DRVCTRL17_CTS0(7));
+	pfc_reg_write(PFC_DRVCTRL17, reg);
+	reg = mmio_read_32(PFC_DRVCTRL18);
+	reg = ((reg & DRVCTRL18_MASK) | DRVCTRL18_RTS0_TANS(7)
+	       | DRVCTRL18_RX1(7)
+	       | DRVCTRL18_TX1(7)
+	       | DRVCTRL18_CTS1(7)
+	       | DRVCTRL18_RTS1_TANS(7)
+	       | DRVCTRL18_SCK2(7)
+	       | DRVCTRL18_TX2(7)
+	       | DRVCTRL18_RX2(7));
+	pfc_reg_write(PFC_DRVCTRL18, reg);
+	reg = mmio_read_32(PFC_DRVCTRL19);
+	reg = ((reg & DRVCTRL19_MASK) | DRVCTRL19_HSCK0(7)
+	       | DRVCTRL19_HRX0(7)
+	       | DRVCTRL19_HTX0(7)
+	       | DRVCTRL19_HCTS0(7)
+	       | DRVCTRL19_HRTS0(7)
+	       | DRVCTRL19_MSIOF0_SCK(7)
+	       | DRVCTRL19_MSIOF0_SYNC(7)
+	       | DRVCTRL19_MSIOF0_SS1(7));
+	pfc_reg_write(PFC_DRVCTRL19, reg);
+	reg = mmio_read_32(PFC_DRVCTRL20);
+	reg = ((reg & DRVCTRL20_MASK) | DRVCTRL20_MSIOF0_TXD(7)
+	       | DRVCTRL20_MSIOF0_SS2(7)
+	       | DRVCTRL20_MSIOF0_RXD(7)
+	       | DRVCTRL20_MLB_CLK(7)
+	       | DRVCTRL20_MLB_SIG(7)
+	       | DRVCTRL20_MLB_DAT(7)
+	       | DRVCTRL20_MLB_REF(7)
+	       | DRVCTRL20_SSI_SCK0129(7));
+	pfc_reg_write(PFC_DRVCTRL20, reg);
+	reg = mmio_read_32(PFC_DRVCTRL21);
+	reg = ((reg & DRVCTRL21_MASK) | DRVCTRL21_SSI_WS0129(7)
+	       | DRVCTRL21_SSI_SDATA0(7)
+	       | DRVCTRL21_SSI_SDATA1(7)
+	       | DRVCTRL21_SSI_SDATA2(7)
+	       | DRVCTRL21_SSI_SCK34(7)
+	       | DRVCTRL21_SSI_WS34(7)
+	       | DRVCTRL21_SSI_SDATA3(7)
+	       | DRVCTRL21_SSI_SCK4(7));
+	pfc_reg_write(PFC_DRVCTRL21, reg);
+	reg = mmio_read_32(PFC_DRVCTRL22);
+	reg = ((reg & DRVCTRL22_MASK) | DRVCTRL22_SSI_WS4(7)
+	       | DRVCTRL22_SSI_SDATA4(7)
+	       | DRVCTRL22_SSI_SCK5(7)
+	       | DRVCTRL22_SSI_WS5(7)
+	       | DRVCTRL22_SSI_SDATA5(7)
+	       | DRVCTRL22_SSI_SCK6(7)
+	       | DRVCTRL22_SSI_WS6(7)
+	       | DRVCTRL22_SSI_SDATA6(7));
+	pfc_reg_write(PFC_DRVCTRL22, reg);
+	reg = mmio_read_32(PFC_DRVCTRL23);
+	reg = ((reg & DRVCTRL23_MASK) | DRVCTRL23_SSI_SCK78(7)
+	       | DRVCTRL23_SSI_WS78(7)
+	       | DRVCTRL23_SSI_SDATA7(7)
+	       | DRVCTRL23_SSI_SDATA8(7)
+	       | DRVCTRL23_SSI_SDATA9(7)
+	       | DRVCTRL23_AUDIO_CLKA(7)
+	       | DRVCTRL23_AUDIO_CLKB(7)
+	       | DRVCTRL23_USB0_PWEN(7));
+	pfc_reg_write(PFC_DRVCTRL23, reg);
+	reg = mmio_read_32(PFC_DRVCTRL24);
+	reg = ((reg & DRVCTRL24_MASK) | DRVCTRL24_USB0_OVC(7)
+	       | DRVCTRL24_USB1_PWEN(7)
+	       | DRVCTRL24_USB1_OVC(7)
+	       | DRVCTRL24_USB30_PWEN(7)
+	       | DRVCTRL24_USB30_OVC(7)
+	       | DRVCTRL24_USB31_PWEN(7)
+	       | DRVCTRL24_USB31_OVC(7));
+	pfc_reg_write(PFC_DRVCTRL24, reg);
+
+	/* initialize LSI pin pull-up/down control */
+	pfc_reg_write(PFC_PUD0, 0x00005FBFU);
+	pfc_reg_write(PFC_PUD1, 0x00300FFEU);
+	pfc_reg_write(PFC_PUD2, 0x330001E6U);
+	pfc_reg_write(PFC_PUD3, 0x000002E0U);
+	pfc_reg_write(PFC_PUD4, 0xFFFFFF00U);
+	pfc_reg_write(PFC_PUD5, 0x7F5FFF87U);
+	pfc_reg_write(PFC_PUD6, 0x00000055U);
+
+	/* initialize LSI pin pull-enable register */
+	pfc_reg_write(PFC_PUEN0, 0x00000FFFU);
+	pfc_reg_write(PFC_PUEN1, 0x00100234U);
+	pfc_reg_write(PFC_PUEN2, 0x000004C4U);
+	pfc_reg_write(PFC_PUEN3, 0x00000200U);
+	pfc_reg_write(PFC_PUEN4, 0x3E000000U);
+	pfc_reg_write(PFC_PUEN5, 0x1F000805U);
+	pfc_reg_write(PFC_PUEN6, 0x00000006U);
+
+	/* initialize positive/negative logic select */
+	mmio_write_32(GPIO_POSNEG0, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG1, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG2, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG3, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG4, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG5, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG6, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG7, 0x00000000U);
+
+	/* initialize general IO/interrupt switching */
+	mmio_write_32(GPIO_IOINTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL5, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL6, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL7, 0x00000000U);
+
+	/* initialize general output register */
+	mmio_write_32(GPIO_OUTDT1, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT2, 0x00000400U);
+	mmio_write_32(GPIO_OUTDT3, 0x0000C000U);
+	mmio_write_32(GPIO_OUTDT5, 0x00000006U);
+	mmio_write_32(GPIO_OUTDT6, 0x00003880U);
+
+	/* initialize general input/output switching */
+	mmio_write_32(GPIO_INOUTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL1, 0x01000A00U);
+	mmio_write_32(GPIO_INOUTSEL2, 0x00000400U);
+	mmio_write_32(GPIO_INOUTSEL3, 0x0000C000U);
+	mmio_write_32(GPIO_INOUTSEL4, 0x00000000U);
+#if (RCAR_GEN3_ULCB == 1)
+	mmio_write_32(GPIO_INOUTSEL5, 0x0000000EU);
+#else
+	mmio_write_32(GPIO_INOUTSEL5, 0x0000020EU);
+#endif
+	mmio_write_32(GPIO_INOUTSEL6, 0x00013880U);
+	mmio_write_32(GPIO_INOUTSEL7, 0x00000000U);
+}
diff --git a/drivers/renesas/rcar/pfc/H3/pfc_init_h3_v1.h b/drivers/renesas/rcar/pfc/H3/pfc_init_h3_v1.h
new file mode 100644
index 0000000..2478e1c
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/H3/pfc_init_h3_v1.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PFC_INIT_H3_V1_H
+#define PFC_INIT_H3_V1_H
+
+void pfc_init_h3_v1(void);
+
+#endif /* PFC_INIT_H3_V1_H */
diff --git a/drivers/renesas/rcar/pfc/H3/pfc_init_h3_v2.c b/drivers/renesas/rcar/pfc/H3/pfc_init_h3_v2.c
new file mode 100644
index 0000000..a54b14b
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/H3/pfc_init_h3_v2.c
@@ -0,0 +1,1216 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>		/* for uint32_t */
+#include <lib/mmio.h>
+#include "pfc_init_h3_v2.h"
+#include "rcar_def.h"
+#include "../pfc_regs.h"
+
+#define GPSR0_D15		BIT(15)
+#define GPSR0_D14		BIT(14)
+#define GPSR0_D13		BIT(13)
+#define GPSR0_D12		BIT(12)
+#define GPSR0_D11		BIT(11)
+#define GPSR0_D10		BIT(10)
+#define GPSR0_D9		BIT(9)
+#define GPSR0_D8		BIT(8)
+#define GPSR0_D7		BIT(7)
+#define GPSR0_D6		BIT(6)
+#define GPSR0_D5		BIT(5)
+#define GPSR0_D4		BIT(4)
+#define GPSR0_D3		BIT(3)
+#define GPSR0_D2		BIT(2)
+#define GPSR0_D1		BIT(1)
+#define GPSR0_D0		BIT(0)
+#define GPSR1_CLKOUT		BIT(28)
+#define GPSR1_EX_WAIT0_A	BIT(27)
+#define GPSR1_WE1		BIT(26)
+#define GPSR1_WE0		BIT(25)
+#define GPSR1_RD_WR		BIT(24)
+#define GPSR1_RD		BIT(23)
+#define GPSR1_BS		BIT(22)
+#define GPSR1_CS1_A26		BIT(21)
+#define GPSR1_CS0		BIT(20)
+#define GPSR1_A19		BIT(19)
+#define GPSR1_A18		BIT(18)
+#define GPSR1_A17		BIT(17)
+#define GPSR1_A16		BIT(16)
+#define GPSR1_A15		BIT(15)
+#define GPSR1_A14		BIT(14)
+#define GPSR1_A13		BIT(13)
+#define GPSR1_A12		BIT(12)
+#define GPSR1_A11		BIT(11)
+#define GPSR1_A10		BIT(10)
+#define GPSR1_A9		BIT(9)
+#define GPSR1_A8		BIT(8)
+#define GPSR1_A7		BIT(7)
+#define GPSR1_A6		BIT(6)
+#define GPSR1_A5		BIT(5)
+#define GPSR1_A4		BIT(4)
+#define GPSR1_A3		BIT(3)
+#define GPSR1_A2		BIT(2)
+#define GPSR1_A1		BIT(1)
+#define GPSR1_A0		BIT(0)
+#define GPSR2_AVB_AVTP_CAPTURE_A	BIT(14)
+#define GPSR2_AVB_AVTP_MATCH_A	BIT(13)
+#define GPSR2_AVB_LINK		BIT(12)
+#define GPSR2_AVB_PHY_INT	BIT(11)
+#define GPSR2_AVB_MAGIC		BIT(10)
+#define GPSR2_AVB_MDC		BIT(9)
+#define GPSR2_PWM2_A		BIT(8)
+#define GPSR2_PWM1_A		BIT(7)
+#define GPSR2_PWM0		BIT(6)
+#define GPSR2_IRQ5		BIT(5)
+#define GPSR2_IRQ4		BIT(4)
+#define GPSR2_IRQ3		BIT(3)
+#define GPSR2_IRQ2		BIT(2)
+#define GPSR2_IRQ1		BIT(1)
+#define GPSR2_IRQ0		BIT(0)
+#define GPSR3_SD1_WP		BIT(15)
+#define GPSR3_SD1_CD		BIT(14)
+#define GPSR3_SD0_WP		BIT(13)
+#define GPSR3_SD0_CD		BIT(12)
+#define GPSR3_SD1_DAT3		BIT(11)
+#define GPSR3_SD1_DAT2		BIT(10)
+#define GPSR3_SD1_DAT1		BIT(9)
+#define GPSR3_SD1_DAT0		BIT(8)
+#define GPSR3_SD1_CMD		BIT(7)
+#define GPSR3_SD1_CLK		BIT(6)
+#define GPSR3_SD0_DAT3		BIT(5)
+#define GPSR3_SD0_DAT2		BIT(4)
+#define GPSR3_SD0_DAT1		BIT(3)
+#define GPSR3_SD0_DAT0		BIT(2)
+#define GPSR3_SD0_CMD		BIT(1)
+#define GPSR3_SD0_CLK		BIT(0)
+#define GPSR4_SD3_DS		BIT(17)
+#define GPSR4_SD3_DAT7		BIT(16)
+#define GPSR4_SD3_DAT6		BIT(15)
+#define GPSR4_SD3_DAT5		BIT(14)
+#define GPSR4_SD3_DAT4		BIT(13)
+#define GPSR4_SD3_DAT3		BIT(12)
+#define GPSR4_SD3_DAT2		BIT(11)
+#define GPSR4_SD3_DAT1		BIT(10)
+#define GPSR4_SD3_DAT0		BIT(9)
+#define GPSR4_SD3_CMD		BIT(8)
+#define GPSR4_SD3_CLK		BIT(7)
+#define GPSR4_SD2_DS		BIT(6)
+#define GPSR4_SD2_DAT3		BIT(5)
+#define GPSR4_SD2_DAT2		BIT(4)
+#define GPSR4_SD2_DAT1		BIT(3)
+#define GPSR4_SD2_DAT0		BIT(2)
+#define GPSR4_SD2_CMD		BIT(1)
+#define GPSR4_SD2_CLK		BIT(0)
+#define GPSR5_MLB_DAT		BIT(25)
+#define GPSR5_MLB_SIG		BIT(24)
+#define GPSR5_MLB_CLK		BIT(23)
+#define GPSR5_MSIOF0_RXD	BIT(22)
+#define GPSR5_MSIOF0_SS2	BIT(21)
+#define GPSR5_MSIOF0_TXD	BIT(20)
+#define GPSR5_MSIOF0_SS1	BIT(19)
+#define GPSR5_MSIOF0_SYNC	BIT(18)
+#define GPSR5_MSIOF0_SCK	BIT(17)
+#define GPSR5_HRTS0		BIT(16)
+#define GPSR5_HCTS0		BIT(15)
+#define GPSR5_HTX0		BIT(14)
+#define GPSR5_HRX0		BIT(13)
+#define GPSR5_HSCK0		BIT(12)
+#define GPSR5_RX2_A		BIT(11)
+#define GPSR5_TX2_A		BIT(10)
+#define GPSR5_SCK2		BIT(9)
+#define GPSR5_RTS1		BIT(8)
+#define GPSR5_CTS1		BIT(7)
+#define GPSR5_TX1_A		BIT(6)
+#define GPSR5_RX1_A		BIT(5)
+#define GPSR5_RTS0		BIT(4)
+#define GPSR5_CTS0		BIT(3)
+#define GPSR5_TX0		BIT(2)
+#define GPSR5_RX0		BIT(1)
+#define GPSR5_SCK0		BIT(0)
+#define GPSR6_USB31_OVC		BIT(31)
+#define GPSR6_USB31_PWEN	BIT(30)
+#define GPSR6_USB30_OVC		BIT(29)
+#define GPSR6_USB30_PWEN	BIT(28)
+#define GPSR6_USB1_OVC		BIT(27)
+#define GPSR6_USB1_PWEN		BIT(26)
+#define GPSR6_USB0_OVC		BIT(25)
+#define GPSR6_USB0_PWEN		BIT(24)
+#define GPSR6_AUDIO_CLKB_B	BIT(23)
+#define GPSR6_AUDIO_CLKA_A	BIT(22)
+#define GPSR6_SSI_SDATA9_A	BIT(21)
+#define GPSR6_SSI_SDATA8	BIT(20)
+#define GPSR6_SSI_SDATA7	BIT(19)
+#define GPSR6_SSI_WS78		BIT(18)
+#define GPSR6_SSI_SCK78		BIT(17)
+#define GPSR6_SSI_SDATA6	BIT(16)
+#define GPSR6_SSI_WS6		BIT(15)
+#define GPSR6_SSI_SCK6		BIT(14)
+#define GPSR6_SSI_SDATA5	BIT(13)
+#define GPSR6_SSI_WS5		BIT(12)
+#define GPSR6_SSI_SCK5		BIT(11)
+#define GPSR6_SSI_SDATA4	BIT(10)
+#define GPSR6_SSI_WS4		BIT(9)
+#define GPSR6_SSI_SCK4		BIT(8)
+#define GPSR6_SSI_SDATA3	BIT(7)
+#define GPSR6_SSI_WS34		BIT(6)
+#define GPSR6_SSI_SCK34		BIT(5)
+#define GPSR6_SSI_SDATA2_A	BIT(4)
+#define GPSR6_SSI_SDATA1_A	BIT(3)
+#define GPSR6_SSI_SDATA0	BIT(2)
+#define GPSR6_SSI_WS0129	BIT(1)
+#define GPSR6_SSI_SCK0129	BIT(0)
+#define GPSR7_AVS2		BIT(1)
+#define GPSR7_AVS1		BIT(0)
+
+#define IPSR_28_FUNC(x)		((uint32_t)(x) << 28U)
+#define IPSR_24_FUNC(x)		((uint32_t)(x) << 24U)
+#define IPSR_20_FUNC(x)		((uint32_t)(x) << 20U)
+#define IPSR_16_FUNC(x)		((uint32_t)(x) << 16U)
+#define IPSR_12_FUNC(x)		((uint32_t)(x) << 12U)
+#define IPSR_8_FUNC(x)		((uint32_t)(x) << 8U)
+#define IPSR_4_FUNC(x)		((uint32_t)(x) << 4U)
+#define IPSR_0_FUNC(x)		((uint32_t)(x) << 0U)
+
+#define POC_SD3_DS_33V		BIT(29)
+#define POC_SD3_DAT7_33V	BIT(28)
+#define POC_SD3_DAT6_33V	BIT(27)
+#define POC_SD3_DAT5_33V	BIT(26)
+#define POC_SD3_DAT4_33V	BIT(25)
+#define POC_SD3_DAT3_33V	BIT(24)
+#define POC_SD3_DAT2_33V	BIT(23)
+#define POC_SD3_DAT1_33V	BIT(22)
+#define POC_SD3_DAT0_33V	BIT(21)
+#define POC_SD3_CMD_33V		BIT(20)
+#define POC_SD3_CLK_33V		BIT(19)
+#define POC_SD2_DS_33V		BIT(18)
+#define POC_SD2_DAT3_33V	BIT(17)
+#define POC_SD2_DAT2_33V	BIT(16)
+#define POC_SD2_DAT1_33V	BIT(15)
+#define POC_SD2_DAT0_33V	BIT(14)
+#define POC_SD2_CMD_33V		BIT(13)
+#define POC_SD2_CLK_33V		BIT(12)
+#define POC_SD1_DAT3_33V	BIT(11)
+#define POC_SD1_DAT2_33V	BIT(10)
+#define POC_SD1_DAT1_33V	BIT(9)
+#define POC_SD1_DAT0_33V	BIT(8)
+#define POC_SD1_CMD_33V		BIT(7)
+#define POC_SD1_CLK_33V		BIT(6)
+#define POC_SD0_DAT3_33V	BIT(5)
+#define POC_SD0_DAT2_33V	BIT(4)
+#define POC_SD0_DAT1_33V	BIT(3)
+#define POC_SD0_DAT0_33V	BIT(2)
+#define POC_SD0_CMD_33V		BIT(1)
+#define POC_SD0_CLK_33V		BIT(0)
+
+#define DRVCTRL0_MASK		(0xCCCCCCCCU)
+#define DRVCTRL1_MASK		(0xCCCCCCC8U)
+#define DRVCTRL2_MASK		(0x88888888U)
+#define DRVCTRL3_MASK		(0x88888888U)
+#define DRVCTRL4_MASK		(0x88888888U)
+#define DRVCTRL5_MASK		(0x88888888U)
+#define DRVCTRL6_MASK		(0x88888888U)
+#define DRVCTRL7_MASK		(0x88888888U)
+#define DRVCTRL8_MASK		(0x88888888U)
+#define DRVCTRL9_MASK		(0x88888888U)
+#define DRVCTRL10_MASK		(0x88888888U)
+#define DRVCTRL11_MASK		(0x888888CCU)
+#define DRVCTRL12_MASK		(0xCCCFFFCFU)
+#define DRVCTRL13_MASK		(0xCC888888U)
+#define DRVCTRL14_MASK		(0x88888888U)
+#define DRVCTRL15_MASK		(0x88888888U)
+#define DRVCTRL16_MASK		(0x88888888U)
+#define DRVCTRL17_MASK		(0x88888888U)
+#define DRVCTRL18_MASK		(0x88888888U)
+#define DRVCTRL19_MASK		(0x88888888U)
+#define DRVCTRL20_MASK		(0x88888888U)
+#define DRVCTRL21_MASK		(0x88888888U)
+#define DRVCTRL22_MASK		(0x88888888U)
+#define DRVCTRL23_MASK		(0x88888888U)
+#define DRVCTRL24_MASK		(0x8888888FU)
+
+#define DRVCTRL0_QSPI0_SPCLK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL0_QSPI0_MOSI_IO0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL0_QSPI0_MISO_IO1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL0_QSPI0_IO2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL0_QSPI0_IO3(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL0_QSPI0_SSL(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL0_QSPI1_SPCLK(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL0_QSPI1_MOSI_IO0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL1_QSPI1_MISO_IO1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL1_QSPI1_IO2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL1_QSPI1_IO3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL1_QSPI1_SS(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL1_RPC_INT(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL1_RPC_WP(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL1_RPC_RESET(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL1_AVB_RX_CTL(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL2_AVB_RXC(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL2_AVB_RD0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL2_AVB_RD1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL2_AVB_RD2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL2_AVB_RD3(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL2_AVB_TX_CTL(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL2_AVB_TXC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL2_AVB_TD0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL3_AVB_TD1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL3_AVB_TD2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL3_AVB_TD3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL3_AVB_TXCREFCLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL3_AVB_MDIO(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL3_AVB_MDC(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL3_AVB_MAGIC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL3_AVB_PHY_INT(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL4_AVB_LINK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL4_AVB_AVTP_MATCH(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL4_AVB_AVTP_CAPTURE(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL4_IRQ0(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL4_IRQ1(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL4_IRQ2(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL4_IRQ3(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL4_IRQ4(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL5_IRQ5(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL5_PWM0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL5_PWM1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL5_PWM2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL5_A0(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL5_A1(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL5_A2(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL5_A3(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL6_A4(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL6_A5(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL6_A6(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL6_A7(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL6_A8(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL6_A9(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL6_A10(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL6_A11(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL7_A12(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL7_A13(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL7_A14(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL7_A15(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL7_A16(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL7_A17(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL7_A18(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL7_A19(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL8_CLKOUT(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL8_CS0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL8_CS1_A2(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL8_BS(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL8_RD(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL8_RD_W(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL8_WE0(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL8_WE1(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL9_EX_WAIT0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL9_PRESETOU(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL9_D0(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL9_D1(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL9_D2(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL9_D3(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL9_D4(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL9_D5(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL10_D6(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL10_D7(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL10_D8(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL10_D9(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL10_D10(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL10_D11(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL10_D12(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL10_D13(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL11_D14(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL11_D15(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL11_AVS1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL11_AVS2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL11_GP7_02(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL11_GP7_03(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL11_DU_DOTCLKIN0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL11_DU_DOTCLKIN1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL12_DU_DOTCLKIN2(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL12_DU_DOTCLKIN3(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL12_DU_FSCLKST(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL12_DU_TMS(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL13_TDO(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL13_ASEBRK(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL13_SD0_CLK(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL13_SD0_CMD(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL13_SD0_DAT0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL13_SD0_DAT1(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL13_SD0_DAT2(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL13_SD0_DAT3(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL14_SD1_CLK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL14_SD1_CMD(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL14_SD1_DAT0(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL14_SD1_DAT1(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL14_SD1_DAT2(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL14_SD1_DAT3(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL14_SD2_CLK(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL14_SD2_CMD(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL15_SD2_DAT0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL15_SD2_DAT1(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL15_SD2_DAT2(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL15_SD2_DAT3(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL15_SD2_DS(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL15_SD3_CLK(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL15_SD3_CMD(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL15_SD3_DAT0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL16_SD3_DAT1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL16_SD3_DAT2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL16_SD3_DAT3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL16_SD3_DAT4(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL16_SD3_DAT5(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL16_SD3_DAT6(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL16_SD3_DAT7(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL16_SD3_DS(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL17_SD0_CD(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL17_SD0_WP(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL17_SD1_CD(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL17_SD1_WP(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL17_SCK0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL17_RX0(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL17_TX0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL17_CTS0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL18_RTS0_TANS(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL18_RX1(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL18_TX1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL18_CTS1(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL18_RTS1_TANS(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL18_SCK2(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL18_TX2(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL18_RX2(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL19_HSCK0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL19_HRX0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL19_HTX0(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL19_HCTS0(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL19_HRTS0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL19_MSIOF0_SCK(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL19_MSIOF0_SYNC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL19_MSIOF0_SS1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL20_MSIOF0_TXD(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL20_MSIOF0_SS2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL20_MSIOF0_RXD(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL20_MLB_CLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL20_MLB_SIG(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL20_MLB_DAT(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL20_MLB_REF(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL20_SSI_SCK0129(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL21_SSI_WS0129(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL21_SSI_SDATA0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL21_SSI_SDATA1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL21_SSI_SDATA2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL21_SSI_SCK34(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL21_SSI_WS34(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL21_SSI_SDATA3(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL21_SSI_SCK4(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL22_SSI_WS4(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL22_SSI_SDATA4(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL22_SSI_SCK5(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL22_SSI_WS5(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL22_SSI_SDATA5(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL22_SSI_SCK6(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL22_SSI_WS6(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL22_SSI_SDATA6(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL23_SSI_SCK78(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL23_SSI_WS78(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL23_SSI_SDATA7(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL23_SSI_SDATA8(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL23_SSI_SDATA9(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL23_AUDIO_CLKA(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL23_AUDIO_CLKB(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL23_USB0_PWEN(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL24_USB0_OVC(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL24_USB1_PWEN(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL24_USB1_OVC(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL24_USB30_PWEN(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL24_USB30_OVC(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL24_USB31_PWEN(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL24_USB31_OVC(x)	((uint32_t)(x) << 4U)
+
+#define MOD_SEL0_MSIOF3_A	((uint32_t)0U << 29U)
+#define MOD_SEL0_MSIOF3_B	((uint32_t)1U << 29U)
+#define MOD_SEL0_MSIOF3_C	((uint32_t)2U << 29U)
+#define MOD_SEL0_MSIOF3_D	((uint32_t)3U << 29U)
+#define MOD_SEL0_MSIOF3_E	((uint32_t)4U << 29U)
+#define MOD_SEL0_MSIOF2_A	((uint32_t)0U << 27U)
+#define MOD_SEL0_MSIOF2_B	((uint32_t)1U << 27U)
+#define MOD_SEL0_MSIOF2_C	((uint32_t)2U << 27U)
+#define MOD_SEL0_MSIOF2_D	((uint32_t)3U << 27U)
+#define MOD_SEL0_MSIOF1_A	((uint32_t)0U << 24U)
+#define MOD_SEL0_MSIOF1_B	((uint32_t)1U << 24U)
+#define MOD_SEL0_MSIOF1_C	((uint32_t)2U << 24U)
+#define MOD_SEL0_MSIOF1_D	((uint32_t)3U << 24U)
+#define MOD_SEL0_MSIOF1_E	((uint32_t)4U << 24U)
+#define MOD_SEL0_MSIOF1_F	((uint32_t)5U << 24U)
+#define MOD_SEL0_MSIOF1_G	((uint32_t)6U << 24U)
+#define MOD_SEL0_LBSC_A		((uint32_t)0U << 23U)
+#define MOD_SEL0_LBSC_B		((uint32_t)1U << 23U)
+#define MOD_SEL0_IEBUS_A	((uint32_t)0U << 22U)
+#define MOD_SEL0_IEBUS_B	((uint32_t)1U << 22U)
+#define MOD_SEL0_I2C2_A		((uint32_t)0U << 21U)
+#define MOD_SEL0_I2C2_B		((uint32_t)1U << 21U)
+#define MOD_SEL0_I2C1_A		((uint32_t)0U << 20U)
+#define MOD_SEL0_I2C1_B		((uint32_t)1U << 20U)
+#define MOD_SEL0_HSCIF4_A	((uint32_t)0U << 19U)
+#define MOD_SEL0_HSCIF4_B	((uint32_t)1U << 19U)
+#define MOD_SEL0_HSCIF3_A	((uint32_t)0U << 17U)
+#define MOD_SEL0_HSCIF3_B	((uint32_t)1U << 17U)
+#define MOD_SEL0_HSCIF3_C	((uint32_t)2U << 17U)
+#define MOD_SEL0_HSCIF3_D	((uint32_t)3U << 17U)
+#define MOD_SEL0_HSCIF1_A	((uint32_t)0U << 16U)
+#define MOD_SEL0_HSCIF1_B	((uint32_t)1U << 16U)
+#define MOD_SEL0_FSO_A		((uint32_t)0U << 15U)
+#define MOD_SEL0_FSO_B		((uint32_t)1U << 15U)
+#define MOD_SEL0_HSCIF2_A	((uint32_t)0U << 13U)
+#define MOD_SEL0_HSCIF2_B	((uint32_t)1U << 13U)
+#define MOD_SEL0_HSCIF2_C	((uint32_t)2U << 13U)
+#define MOD_SEL0_ETHERAVB_A	((uint32_t)0U << 12U)
+#define MOD_SEL0_ETHERAVB_B	((uint32_t)1U << 12U)
+#define MOD_SEL0_DRIF3_A	((uint32_t)0U << 11U)
+#define MOD_SEL0_DRIF3_B	((uint32_t)1U << 11U)
+#define MOD_SEL0_DRIF2_A	((uint32_t)0U << 10U)
+#define MOD_SEL0_DRIF2_B	((uint32_t)1U << 10U)
+#define MOD_SEL0_DRIF1_A	((uint32_t)0U << 8U)
+#define MOD_SEL0_DRIF1_B	((uint32_t)1U << 8U)
+#define MOD_SEL0_DRIF1_C	((uint32_t)2U << 8U)
+#define MOD_SEL0_DRIF0_A	((uint32_t)0U << 6U)
+#define MOD_SEL0_DRIF0_B	((uint32_t)1U << 6U)
+#define MOD_SEL0_DRIF0_C	((uint32_t)2U << 6U)
+#define MOD_SEL0_CANFD0_A	((uint32_t)0U << 5U)
+#define MOD_SEL0_CANFD0_B	((uint32_t)1U << 5U)
+#define MOD_SEL0_ADG_A_A	((uint32_t)0U << 3U)
+#define MOD_SEL0_ADG_A_B	((uint32_t)1U << 3U)
+#define MOD_SEL0_ADG_A_C	((uint32_t)2U << 3U)
+#define MOD_SEL1_TSIF1_A	((uint32_t)0U << 30U)
+#define MOD_SEL1_TSIF1_B	((uint32_t)1U << 30U)
+#define MOD_SEL1_TSIF1_C	((uint32_t)2U << 30U)
+#define MOD_SEL1_TSIF1_D	((uint32_t)3U << 30U)
+#define MOD_SEL1_TSIF0_A	((uint32_t)0U << 27U)
+#define MOD_SEL1_TSIF0_B	((uint32_t)1U << 27U)
+#define MOD_SEL1_TSIF0_C	((uint32_t)2U << 27U)
+#define MOD_SEL1_TSIF0_D	((uint32_t)3U << 27U)
+#define MOD_SEL1_TSIF0_E	((uint32_t)4U << 27U)
+#define MOD_SEL1_TIMER_TMU_A	((uint32_t)0U << 26U)
+#define MOD_SEL1_TIMER_TMU_B	((uint32_t)1U << 26U)
+#define MOD_SEL1_SSP1_1_A	((uint32_t)0U << 24U)
+#define MOD_SEL1_SSP1_1_B	((uint32_t)1U << 24U)
+#define MOD_SEL1_SSP1_1_C	((uint32_t)2U << 24U)
+#define MOD_SEL1_SSP1_1_D	((uint32_t)3U << 24U)
+#define MOD_SEL1_SSP1_0_A	((uint32_t)0U << 21U)
+#define MOD_SEL1_SSP1_0_B	((uint32_t)1U << 21U)
+#define MOD_SEL1_SSP1_0_C	((uint32_t)2U << 21U)
+#define MOD_SEL1_SSP1_0_D	((uint32_t)3U << 21U)
+#define MOD_SEL1_SSP1_0_E	((uint32_t)4U << 21U)
+#define MOD_SEL1_SSI_A		((uint32_t)0U << 20U)
+#define MOD_SEL1_SSI_B		((uint32_t)1U << 20U)
+#define MOD_SEL1_SPEED_PULSE_IF_A	((uint32_t)0U << 19U)
+#define MOD_SEL1_SPEED_PULSE_IF_B	((uint32_t)1U << 19U)
+#define MOD_SEL1_SIMCARD_A	((uint32_t)0U << 17U)
+#define MOD_SEL1_SIMCARD_B	((uint32_t)1U << 17U)
+#define MOD_SEL1_SIMCARD_C	((uint32_t)2U << 17U)
+#define MOD_SEL1_SIMCARD_D	((uint32_t)3U << 17U)
+#define MOD_SEL1_SDHI2_A	((uint32_t)0U << 16U)
+#define MOD_SEL1_SDHI2_B	((uint32_t)1U << 16U)
+#define MOD_SEL1_SCIF4_A	((uint32_t)0U << 14U)
+#define MOD_SEL1_SCIF4_B	((uint32_t)1U << 14U)
+#define MOD_SEL1_SCIF4_C	((uint32_t)2U << 14U)
+#define MOD_SEL1_SCIF3_A	((uint32_t)0U << 13U)
+#define MOD_SEL1_SCIF3_B	((uint32_t)1U << 13U)
+#define MOD_SEL1_SCIF2_A	((uint32_t)0U << 12U)
+#define MOD_SEL1_SCIF2_B	((uint32_t)1U << 12U)
+#define MOD_SEL1_SCIF1_A	((uint32_t)0U << 11U)
+#define MOD_SEL1_SCIF1_B	((uint32_t)1U << 11U)
+#define MOD_SEL1_SCIF_A		((uint32_t)0U << 10U)
+#define MOD_SEL1_SCIF_B		((uint32_t)1U << 10U)
+#define MOD_SEL1_REMOCON_A	((uint32_t)0U << 9U)
+#define MOD_SEL1_REMOCON_B	((uint32_t)1U << 9U)
+#define MOD_SEL1_RCAN0_A	((uint32_t)0U << 6U)
+#define MOD_SEL1_RCAN0_B	((uint32_t)1U << 6U)
+#define MOD_SEL1_PWM6_A		((uint32_t)0U << 5U)
+#define MOD_SEL1_PWM6_B		((uint32_t)1U << 5U)
+#define MOD_SEL1_PWM5_A		((uint32_t)0U << 4U)
+#define MOD_SEL1_PWM5_B		((uint32_t)1U << 4U)
+#define MOD_SEL1_PWM4_A		((uint32_t)0U << 3U)
+#define MOD_SEL1_PWM4_B		((uint32_t)1U << 3U)
+#define MOD_SEL1_PWM3_A		((uint32_t)0U << 2U)
+#define MOD_SEL1_PWM3_B		((uint32_t)1U << 2U)
+#define MOD_SEL1_PWM2_A		((uint32_t)0U << 1U)
+#define MOD_SEL1_PWM2_B		((uint32_t)1U << 1U)
+#define MOD_SEL1_PWM1_A		((uint32_t)0U << 0U)
+#define MOD_SEL1_PWM1_B		((uint32_t)1U << 0U)
+#define MOD_SEL2_I2C_5_A	((uint32_t)0U << 31U)
+#define MOD_SEL2_I2C_5_B	((uint32_t)1U << 31U)
+#define MOD_SEL2_I2C_3_A	((uint32_t)0U << 30U)
+#define MOD_SEL2_I2C_3_B	((uint32_t)1U << 30U)
+#define MOD_SEL2_I2C_0_A	((uint32_t)0U << 29U)
+#define MOD_SEL2_I2C_0_B	((uint32_t)1U << 29U)
+#define MOD_SEL2_FM_A		((uint32_t)0U << 27U)
+#define MOD_SEL2_FM_B		((uint32_t)1U << 27U)
+#define MOD_SEL2_FM_C		((uint32_t)2U << 27U)
+#define MOD_SEL2_FM_D		((uint32_t)3U << 27U)
+#define MOD_SEL2_SCIF5_A	((uint32_t)0U << 26U)
+#define MOD_SEL2_SCIF5_B	((uint32_t)1U << 26U)
+#define MOD_SEL2_I2C6_A		((uint32_t)0U << 23U)
+#define MOD_SEL2_I2C6_B		((uint32_t)1U << 23U)
+#define MOD_SEL2_I2C6_C		((uint32_t)2U << 23U)
+#define MOD_SEL2_NDF_A		((uint32_t)0U << 22U)
+#define MOD_SEL2_NDF_B		((uint32_t)1U << 22U)
+#define MOD_SEL2_SSI2_A		((uint32_t)0U << 21U)
+#define MOD_SEL2_SSI2_B		((uint32_t)1U << 21U)
+#define MOD_SEL2_SSI9_A		((uint32_t)0U << 20U)
+#define MOD_SEL2_SSI9_B		((uint32_t)1U << 20U)
+#define MOD_SEL2_TIMER_TMU2_A	((uint32_t)0U << 19U)
+#define MOD_SEL2_TIMER_TMU2_B	((uint32_t)1U << 19U)
+#define MOD_SEL2_ADG_B_A	((uint32_t)0U << 18U)
+#define MOD_SEL2_ADG_B_B	((uint32_t)1U << 18U)
+#define MOD_SEL2_ADG_C_A	((uint32_t)0U << 17U)
+#define MOD_SEL2_ADG_C_B	((uint32_t)1U << 17U)
+#define MOD_SEL2_VIN4_A		((uint32_t)0U << 0U)
+#define MOD_SEL2_VIN4_B		((uint32_t)1U << 0U)
+
+static void pfc_reg_write(uint32_t addr, uint32_t data)
+{
+	mmio_write_32(PFC_PMMR, ~data);
+	mmio_write_32((uintptr_t)addr, data);
+}
+
+void pfc_init_h3_v2(void)
+{
+	uint32_t reg;
+
+	/* initialize module select */
+	pfc_reg_write(PFC_MOD_SEL0, MOD_SEL0_MSIOF3_A
+		      | MOD_SEL0_MSIOF2_A
+		      | MOD_SEL0_MSIOF1_A
+		      | MOD_SEL0_LBSC_A
+		      | MOD_SEL0_IEBUS_A
+		      | MOD_SEL0_I2C2_A
+		      | MOD_SEL0_I2C1_A
+		      | MOD_SEL0_HSCIF4_A
+		      | MOD_SEL0_HSCIF3_A
+		      | MOD_SEL0_HSCIF1_A
+		      | MOD_SEL0_FSO_A
+		      | MOD_SEL0_HSCIF2_A
+		      | MOD_SEL0_ETHERAVB_A
+		      | MOD_SEL0_DRIF3_A
+		      | MOD_SEL0_DRIF2_A
+		      | MOD_SEL0_DRIF1_A
+		      | MOD_SEL0_DRIF0_A
+		      | MOD_SEL0_CANFD0_A
+		      | MOD_SEL0_ADG_A_A);
+	pfc_reg_write(PFC_MOD_SEL1, MOD_SEL1_TSIF1_A
+		      | MOD_SEL1_TSIF0_A
+		      | MOD_SEL1_TIMER_TMU_A
+		      | MOD_SEL1_SSP1_1_A
+		      | MOD_SEL1_SSP1_0_A
+		      | MOD_SEL1_SSI_A
+		      | MOD_SEL1_SPEED_PULSE_IF_A
+		      | MOD_SEL1_SIMCARD_A
+		      | MOD_SEL1_SDHI2_A
+		      | MOD_SEL1_SCIF4_A
+		      | MOD_SEL1_SCIF3_A
+		      | MOD_SEL1_SCIF2_A
+		      | MOD_SEL1_SCIF1_A
+		      | MOD_SEL1_SCIF_A
+		      | MOD_SEL1_REMOCON_A
+		      | MOD_SEL1_RCAN0_A
+		      | MOD_SEL1_PWM6_A
+		      | MOD_SEL1_PWM5_A
+		      | MOD_SEL1_PWM4_A
+		      | MOD_SEL1_PWM3_A
+		      | MOD_SEL1_PWM2_A
+		      | MOD_SEL1_PWM1_A);
+	pfc_reg_write(PFC_MOD_SEL2, MOD_SEL2_I2C_5_A
+		      | MOD_SEL2_I2C_3_A
+		      | MOD_SEL2_I2C_0_A
+		      | MOD_SEL2_FM_A
+		      | MOD_SEL2_SCIF5_A
+		      | MOD_SEL2_I2C6_A
+		      | MOD_SEL2_NDF_A
+		      | MOD_SEL2_SSI2_A
+		      | MOD_SEL2_SSI9_A
+		      | MOD_SEL2_TIMER_TMU2_A
+		      | MOD_SEL2_ADG_B_A
+		      | MOD_SEL2_ADG_C_A
+		      | MOD_SEL2_VIN4_A);
+
+	/* initialize peripheral function select */
+	pfc_reg_write(PFC_IPSR0, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR1, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(3)
+		      | IPSR_8_FUNC(3)
+		      | IPSR_4_FUNC(3)
+		      | IPSR_0_FUNC(3));
+	pfc_reg_write(PFC_IPSR2, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR3, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR4, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR5, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR6, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR7, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR8, IPSR_28_FUNC(1)
+		      | IPSR_24_FUNC(1)
+		      | IPSR_20_FUNC(1)
+		      | IPSR_16_FUNC(1)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR9, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR10, IPSR_28_FUNC(1)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR11, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(4)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(1));
+	pfc_reg_write(PFC_IPSR12, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(4)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR13, IPSR_28_FUNC(8)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(3)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR14, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(3)
+		      | IPSR_0_FUNC(8));
+	pfc_reg_write(PFC_IPSR15, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR16, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR17, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(1)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR18, IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+
+	/* initialize GPIO/perihperal function select */
+	pfc_reg_write(PFC_GPSR0, GPSR0_D15
+		      | GPSR0_D14
+		      | GPSR0_D13
+		      | GPSR0_D12
+		      | GPSR0_D11
+		      | GPSR0_D10
+		      | GPSR0_D9
+		      | GPSR0_D8);
+	pfc_reg_write(PFC_GPSR1, GPSR1_CLKOUT
+		      | GPSR1_EX_WAIT0_A
+		      | GPSR1_A19
+		      | GPSR1_A18
+		      | GPSR1_A17
+		      | GPSR1_A16
+		      | GPSR1_A15
+		      | GPSR1_A14
+		      | GPSR1_A13
+		      | GPSR1_A12
+		      | GPSR1_A7
+		      | GPSR1_A6
+		      | GPSR1_A5
+		      | GPSR1_A4
+		      | GPSR1_A3
+		      | GPSR1_A2
+		      | GPSR1_A1
+		      | GPSR1_A0);
+	pfc_reg_write(PFC_GPSR2, GPSR2_AVB_AVTP_CAPTURE_A
+		      | GPSR2_AVB_AVTP_MATCH_A
+		      | GPSR2_AVB_LINK
+		      | GPSR2_AVB_PHY_INT
+		      | GPSR2_AVB_MDC
+		      | GPSR2_PWM2_A
+		      | GPSR2_PWM1_A
+		      | GPSR2_IRQ5
+		      | GPSR2_IRQ4
+		      | GPSR2_IRQ3
+		      | GPSR2_IRQ2
+		      | GPSR2_IRQ1
+		      | GPSR2_IRQ0);
+	pfc_reg_write(PFC_GPSR3, GPSR3_SD0_WP
+		      | GPSR3_SD0_CD
+		      | GPSR3_SD1_DAT3
+		      | GPSR3_SD1_DAT2
+		      | GPSR3_SD1_DAT1
+		      | GPSR3_SD1_DAT0
+		      | GPSR3_SD0_DAT3
+		      | GPSR3_SD0_DAT2
+		      | GPSR3_SD0_DAT1
+		      | GPSR3_SD0_DAT0
+		      | GPSR3_SD0_CMD
+		      | GPSR3_SD0_CLK);
+	pfc_reg_write(PFC_GPSR4, GPSR4_SD3_DAT7
+		      | GPSR4_SD3_DAT6
+		      | GPSR4_SD3_DAT3
+		      | GPSR4_SD3_DAT2
+		      | GPSR4_SD3_DAT1
+		      | GPSR4_SD3_DAT0
+		      | GPSR4_SD3_CMD
+		      | GPSR4_SD3_CLK
+		      | GPSR4_SD2_DS
+		      | GPSR4_SD2_DAT3
+		      | GPSR4_SD2_DAT2
+		      | GPSR4_SD2_DAT1
+		      | GPSR4_SD2_DAT0
+		      | GPSR4_SD2_CMD
+		      | GPSR4_SD2_CLK);
+	pfc_reg_write(PFC_GPSR5, GPSR5_MSIOF0_SS2
+		      | GPSR5_MSIOF0_SS1
+		      | GPSR5_MSIOF0_SYNC
+		      | GPSR5_HRTS0
+		      | GPSR5_HCTS0
+		      | GPSR5_HTX0
+		      | GPSR5_HRX0
+		      | GPSR5_HSCK0
+		      | GPSR5_RX2_A
+		      | GPSR5_TX2_A
+		      | GPSR5_SCK2
+		      | GPSR5_RTS1
+		      | GPSR5_CTS1
+		      | GPSR5_TX1_A
+		      | GPSR5_RX1_A
+		      | GPSR5_RTS0
+		      | GPSR5_SCK0);
+	pfc_reg_write(PFC_GPSR6, GPSR6_USB30_OVC
+		      | GPSR6_USB30_PWEN
+		      | GPSR6_USB1_OVC
+		      | GPSR6_USB1_PWEN
+		      | GPSR6_USB0_OVC
+		      | GPSR6_USB0_PWEN
+		      | GPSR6_AUDIO_CLKB_B
+		      | GPSR6_AUDIO_CLKA_A
+		      | GPSR6_SSI_SDATA8
+		      | GPSR6_SSI_SDATA7
+		      | GPSR6_SSI_WS78
+		      | GPSR6_SSI_SCK78
+		      | GPSR6_SSI_WS6
+		      | GPSR6_SSI_SCK6
+		      | GPSR6_SSI_SDATA4
+		      | GPSR6_SSI_WS4
+		      | GPSR6_SSI_SCK4
+		      | GPSR6_SSI_SDATA1_A
+		      | GPSR6_SSI_SDATA0
+		      | GPSR6_SSI_WS0129
+		      | GPSR6_SSI_SCK0129);
+	pfc_reg_write(PFC_GPSR7, GPSR7_AVS2
+		      | GPSR7_AVS1);
+
+	/* initialize POC control register */
+	pfc_reg_write(PFC_POCCTRL0, POC_SD3_DS_33V
+		      | POC_SD3_DAT7_33V
+		      | POC_SD3_DAT6_33V
+		      | POC_SD3_DAT5_33V
+		      | POC_SD3_DAT4_33V
+		      | POC_SD3_DAT3_33V
+		      | POC_SD3_DAT2_33V
+		      | POC_SD3_DAT1_33V
+		      | POC_SD3_DAT0_33V
+		      | POC_SD3_CMD_33V
+		      | POC_SD3_CLK_33V
+		      | POC_SD0_DAT3_33V
+		      | POC_SD0_DAT2_33V
+		      | POC_SD0_DAT1_33V
+		      | POC_SD0_DAT0_33V
+		      | POC_SD0_CMD_33V
+		      | POC_SD0_CLK_33V);
+
+	/* initialize DRV control register */
+	reg = mmio_read_32(PFC_DRVCTRL0);
+	reg = ((reg & DRVCTRL0_MASK) | DRVCTRL0_QSPI0_SPCLK(3)
+	       | DRVCTRL0_QSPI0_MOSI_IO0(3)
+	       | DRVCTRL0_QSPI0_MISO_IO1(3)
+	       | DRVCTRL0_QSPI0_IO2(3)
+	       | DRVCTRL0_QSPI0_IO3(3)
+	       | DRVCTRL0_QSPI0_SSL(3)
+	       | DRVCTRL0_QSPI1_SPCLK(3)
+	       | DRVCTRL0_QSPI1_MOSI_IO0(3));
+	pfc_reg_write(PFC_DRVCTRL0, reg);
+	reg = mmio_read_32(PFC_DRVCTRL1);
+	reg = ((reg & DRVCTRL1_MASK) | DRVCTRL1_QSPI1_MISO_IO1(3)
+	       | DRVCTRL1_QSPI1_IO2(3)
+	       | DRVCTRL1_QSPI1_IO3(3)
+	       | DRVCTRL1_QSPI1_SS(3)
+	       | DRVCTRL1_RPC_INT(3)
+	       | DRVCTRL1_RPC_WP(3)
+	       | DRVCTRL1_RPC_RESET(3)
+	       | DRVCTRL1_AVB_RX_CTL(7));
+	pfc_reg_write(PFC_DRVCTRL1, reg);
+	reg = mmio_read_32(PFC_DRVCTRL2);
+	reg = ((reg & DRVCTRL2_MASK) | DRVCTRL2_AVB_RXC(7)
+	       | DRVCTRL2_AVB_RD0(7)
+	       | DRVCTRL2_AVB_RD1(7)
+	       | DRVCTRL2_AVB_RD2(7)
+	       | DRVCTRL2_AVB_RD3(7)
+	       | DRVCTRL2_AVB_TX_CTL(3)
+	       | DRVCTRL2_AVB_TXC(3)
+	       | DRVCTRL2_AVB_TD0(3));
+	pfc_reg_write(PFC_DRVCTRL2, reg);
+	reg = mmio_read_32(PFC_DRVCTRL3);
+	reg = ((reg & DRVCTRL3_MASK) | DRVCTRL3_AVB_TD1(3)
+	       | DRVCTRL3_AVB_TD2(3)
+	       | DRVCTRL3_AVB_TD3(3)
+	       | DRVCTRL3_AVB_TXCREFCLK(7)
+	       | DRVCTRL3_AVB_MDIO(7)
+	       | DRVCTRL3_AVB_MDC(7)
+	       | DRVCTRL3_AVB_MAGIC(7)
+	       | DRVCTRL3_AVB_PHY_INT(7));
+	pfc_reg_write(PFC_DRVCTRL3, reg);
+	reg = mmio_read_32(PFC_DRVCTRL4);
+	reg = ((reg & DRVCTRL4_MASK) | DRVCTRL4_AVB_LINK(7)
+	       | DRVCTRL4_AVB_AVTP_MATCH(7)
+	       | DRVCTRL4_AVB_AVTP_CAPTURE(7)
+	       | DRVCTRL4_IRQ0(7)
+	       | DRVCTRL4_IRQ1(7)
+	       | DRVCTRL4_IRQ2(7)
+	       | DRVCTRL4_IRQ3(7)
+	       | DRVCTRL4_IRQ4(7));
+	pfc_reg_write(PFC_DRVCTRL4, reg);
+	reg = mmio_read_32(PFC_DRVCTRL5);
+	reg = ((reg & DRVCTRL5_MASK) | DRVCTRL5_IRQ5(7)
+	       | DRVCTRL5_PWM0(7)
+	       | DRVCTRL5_PWM1(7)
+	       | DRVCTRL5_PWM2(7)
+	       | DRVCTRL5_A0(3)
+	       | DRVCTRL5_A1(3)
+	       | DRVCTRL5_A2(3)
+	       | DRVCTRL5_A3(3));
+	pfc_reg_write(PFC_DRVCTRL5, reg);
+	reg = mmio_read_32(PFC_DRVCTRL6);
+	reg = ((reg & DRVCTRL6_MASK) | DRVCTRL6_A4(3)
+	       | DRVCTRL6_A5(3)
+	       | DRVCTRL6_A6(3)
+	       | DRVCTRL6_A7(3)
+	       | DRVCTRL6_A8(7)
+	       | DRVCTRL6_A9(7)
+	       | DRVCTRL6_A10(7)
+	       | DRVCTRL6_A11(7));
+	pfc_reg_write(PFC_DRVCTRL6, reg);
+	reg = mmio_read_32(PFC_DRVCTRL7);
+	reg = ((reg & DRVCTRL7_MASK) | DRVCTRL7_A12(3)
+	       | DRVCTRL7_A13(3)
+	       | DRVCTRL7_A14(3)
+	       | DRVCTRL7_A15(3)
+	       | DRVCTRL7_A16(3)
+	       | DRVCTRL7_A17(3)
+	       | DRVCTRL7_A18(3)
+	       | DRVCTRL7_A19(3));
+	pfc_reg_write(PFC_DRVCTRL7, reg);
+	reg = mmio_read_32(PFC_DRVCTRL8);
+	reg = ((reg & DRVCTRL8_MASK) | DRVCTRL8_CLKOUT(7)
+	       | DRVCTRL8_CS0(7)
+	       | DRVCTRL8_CS1_A2(7)
+	       | DRVCTRL8_BS(7)
+	       | DRVCTRL8_RD(7)
+	       | DRVCTRL8_RD_W(7)
+	       | DRVCTRL8_WE0(7)
+	       | DRVCTRL8_WE1(7));
+	pfc_reg_write(PFC_DRVCTRL8, reg);
+	reg = mmio_read_32(PFC_DRVCTRL9);
+	reg = ((reg & DRVCTRL9_MASK) | DRVCTRL9_EX_WAIT0(7)
+	       | DRVCTRL9_PRESETOU(7)
+	       | DRVCTRL9_D0(7)
+	       | DRVCTRL9_D1(7)
+	       | DRVCTRL9_D2(7)
+	       | DRVCTRL9_D3(7)
+	       | DRVCTRL9_D4(7)
+	       | DRVCTRL9_D5(7));
+	pfc_reg_write(PFC_DRVCTRL9, reg);
+	reg = mmio_read_32(PFC_DRVCTRL10);
+	reg = ((reg & DRVCTRL10_MASK) | DRVCTRL10_D6(7)
+	       | DRVCTRL10_D7(7)
+	       | DRVCTRL10_D8(3)
+	       | DRVCTRL10_D9(3)
+	       | DRVCTRL10_D10(3)
+	       | DRVCTRL10_D11(3)
+	       | DRVCTRL10_D12(3)
+	       | DRVCTRL10_D13(3));
+	pfc_reg_write(PFC_DRVCTRL10, reg);
+	reg = mmio_read_32(PFC_DRVCTRL11);
+	reg = ((reg & DRVCTRL11_MASK) | DRVCTRL11_D14(3)
+	       | DRVCTRL11_D15(3)
+	       | DRVCTRL11_AVS1(7)
+	       | DRVCTRL11_AVS2(7)
+	       | DRVCTRL11_GP7_02(7)
+	       | DRVCTRL11_GP7_03(7)
+	       | DRVCTRL11_DU_DOTCLKIN0(3)
+	       | DRVCTRL11_DU_DOTCLKIN1(3));
+	pfc_reg_write(PFC_DRVCTRL11, reg);
+	reg = mmio_read_32(PFC_DRVCTRL12);
+	reg = ((reg & DRVCTRL12_MASK) | DRVCTRL12_DU_DOTCLKIN2(3)
+	       | DRVCTRL12_DU_DOTCLKIN3(3)
+	       | DRVCTRL12_DU_FSCLKST(3)
+	       | DRVCTRL12_DU_TMS(3));
+	pfc_reg_write(PFC_DRVCTRL12, reg);
+	reg = mmio_read_32(PFC_DRVCTRL13);
+	reg = ((reg & DRVCTRL13_MASK) | DRVCTRL13_TDO(3)
+	       | DRVCTRL13_ASEBRK(3)
+	       | DRVCTRL13_SD0_CLK(7)
+	       | DRVCTRL13_SD0_CMD(7)
+	       | DRVCTRL13_SD0_DAT0(7)
+	       | DRVCTRL13_SD0_DAT1(7)
+	       | DRVCTRL13_SD0_DAT2(7)
+	       | DRVCTRL13_SD0_DAT3(7));
+	pfc_reg_write(PFC_DRVCTRL13, reg);
+	reg = mmio_read_32(PFC_DRVCTRL14);
+	reg = ((reg & DRVCTRL14_MASK) | DRVCTRL14_SD1_CLK(7)
+	       | DRVCTRL14_SD1_CMD(7)
+	       | DRVCTRL14_SD1_DAT0(5)
+	       | DRVCTRL14_SD1_DAT1(5)
+	       | DRVCTRL14_SD1_DAT2(5)
+	       | DRVCTRL14_SD1_DAT3(5)
+	       | DRVCTRL14_SD2_CLK(5)
+	       | DRVCTRL14_SD2_CMD(5));
+	pfc_reg_write(PFC_DRVCTRL14, reg);
+	reg = mmio_read_32(PFC_DRVCTRL15);
+	reg = ((reg & DRVCTRL15_MASK) | DRVCTRL15_SD2_DAT0(5)
+	       | DRVCTRL15_SD2_DAT1(5)
+	       | DRVCTRL15_SD2_DAT2(5)
+	       | DRVCTRL15_SD2_DAT3(5)
+	       | DRVCTRL15_SD2_DS(5)
+	       | DRVCTRL15_SD3_CLK(7)
+	       | DRVCTRL15_SD3_CMD(7)
+	       | DRVCTRL15_SD3_DAT0(7));
+	pfc_reg_write(PFC_DRVCTRL15, reg);
+	reg = mmio_read_32(PFC_DRVCTRL16);
+	reg = ((reg & DRVCTRL16_MASK) | DRVCTRL16_SD3_DAT1(7)
+	       | DRVCTRL16_SD3_DAT2(7)
+	       | DRVCTRL16_SD3_DAT3(7)
+	       | DRVCTRL16_SD3_DAT4(7)
+	       | DRVCTRL16_SD3_DAT5(7)
+	       | DRVCTRL16_SD3_DAT6(7)
+	       | DRVCTRL16_SD3_DAT7(7)
+	       | DRVCTRL16_SD3_DS(7));
+	pfc_reg_write(PFC_DRVCTRL16, reg);
+	reg = mmio_read_32(PFC_DRVCTRL17);
+	reg = ((reg & DRVCTRL17_MASK) | DRVCTRL17_SD0_CD(7)
+	       | DRVCTRL17_SD0_WP(7)
+	       | DRVCTRL17_SD1_CD(7)
+	       | DRVCTRL17_SD1_WP(7)
+	       | DRVCTRL17_SCK0(7)
+	       | DRVCTRL17_RX0(7)
+	       | DRVCTRL17_TX0(7)
+	       | DRVCTRL17_CTS0(7));
+	pfc_reg_write(PFC_DRVCTRL17, reg);
+	reg = mmio_read_32(PFC_DRVCTRL18);
+	reg = ((reg & DRVCTRL18_MASK) | DRVCTRL18_RTS0_TANS(7)
+	       | DRVCTRL18_RX1(7)
+	       | DRVCTRL18_TX1(7)
+	       | DRVCTRL18_CTS1(7)
+	       | DRVCTRL18_RTS1_TANS(7)
+	       | DRVCTRL18_SCK2(7)
+	       | DRVCTRL18_TX2(7)
+	       | DRVCTRL18_RX2(7));
+	pfc_reg_write(PFC_DRVCTRL18, reg);
+	reg = mmio_read_32(PFC_DRVCTRL19);
+	reg = ((reg & DRVCTRL19_MASK) | DRVCTRL19_HSCK0(7)
+	       | DRVCTRL19_HRX0(7)
+	       | DRVCTRL19_HTX0(7)
+	       | DRVCTRL19_HCTS0(7)
+	       | DRVCTRL19_HRTS0(7)
+	       | DRVCTRL19_MSIOF0_SCK(7)
+	       | DRVCTRL19_MSIOF0_SYNC(7)
+	       | DRVCTRL19_MSIOF0_SS1(7));
+	pfc_reg_write(PFC_DRVCTRL19, reg);
+	reg = mmio_read_32(PFC_DRVCTRL20);
+	reg = ((reg & DRVCTRL20_MASK) | DRVCTRL20_MSIOF0_TXD(7)
+	       | DRVCTRL20_MSIOF0_SS2(7)
+	       | DRVCTRL20_MSIOF0_RXD(7)
+	       | DRVCTRL20_MLB_CLK(7)
+	       | DRVCTRL20_MLB_SIG(7)
+	       | DRVCTRL20_MLB_DAT(7)
+	       | DRVCTRL20_MLB_REF(7)
+	       | DRVCTRL20_SSI_SCK0129(7));
+	pfc_reg_write(PFC_DRVCTRL20, reg);
+	reg = mmio_read_32(PFC_DRVCTRL21);
+	reg = ((reg & DRVCTRL21_MASK) | DRVCTRL21_SSI_WS0129(7)
+	       | DRVCTRL21_SSI_SDATA0(7)
+	       | DRVCTRL21_SSI_SDATA1(7)
+	       | DRVCTRL21_SSI_SDATA2(7)
+	       | DRVCTRL21_SSI_SCK34(7)
+	       | DRVCTRL21_SSI_WS34(7)
+	       | DRVCTRL21_SSI_SDATA3(7)
+	       | DRVCTRL21_SSI_SCK4(7));
+	pfc_reg_write(PFC_DRVCTRL21, reg);
+	reg = mmio_read_32(PFC_DRVCTRL22);
+	reg = ((reg & DRVCTRL22_MASK) | DRVCTRL22_SSI_WS4(7)
+	       | DRVCTRL22_SSI_SDATA4(7)
+	       | DRVCTRL22_SSI_SCK5(7)
+	       | DRVCTRL22_SSI_WS5(7)
+	       | DRVCTRL22_SSI_SDATA5(7)
+	       | DRVCTRL22_SSI_SCK6(7)
+	       | DRVCTRL22_SSI_WS6(7)
+	       | DRVCTRL22_SSI_SDATA6(7));
+	pfc_reg_write(PFC_DRVCTRL22, reg);
+	reg = mmio_read_32(PFC_DRVCTRL23);
+	reg = ((reg & DRVCTRL23_MASK) | DRVCTRL23_SSI_SCK78(7)
+	       | DRVCTRL23_SSI_WS78(7)
+	       | DRVCTRL23_SSI_SDATA7(7)
+	       | DRVCTRL23_SSI_SDATA8(7)
+	       | DRVCTRL23_SSI_SDATA9(7)
+	       | DRVCTRL23_AUDIO_CLKA(7)
+	       | DRVCTRL23_AUDIO_CLKB(7)
+	       | DRVCTRL23_USB0_PWEN(7));
+	pfc_reg_write(PFC_DRVCTRL23, reg);
+	reg = mmio_read_32(PFC_DRVCTRL24);
+	reg = ((reg & DRVCTRL24_MASK) | DRVCTRL24_USB0_OVC(7)
+	       | DRVCTRL24_USB1_PWEN(7)
+	       | DRVCTRL24_USB1_OVC(7)
+	       | DRVCTRL24_USB30_PWEN(7)
+	       | DRVCTRL24_USB30_OVC(7)
+	       | DRVCTRL24_USB31_PWEN(7)
+	       | DRVCTRL24_USB31_OVC(7));
+	pfc_reg_write(PFC_DRVCTRL24, reg);
+
+	/* initialize LSI pin pull-up/down control */
+	pfc_reg_write(PFC_PUD0, 0x00005FBFU);
+	pfc_reg_write(PFC_PUD1, 0x00300FFEU);
+	pfc_reg_write(PFC_PUD2, 0x330001E6U);
+	pfc_reg_write(PFC_PUD3, 0x000002E0U);
+	pfc_reg_write(PFC_PUD4, 0xFFFFFF00U);
+	pfc_reg_write(PFC_PUD5, 0x7F5FFF87U);
+	pfc_reg_write(PFC_PUD6, 0x00000055U);
+
+	/* initialize LSI pin pull-enable register */
+	pfc_reg_write(PFC_PUEN0, 0x00000FFFU);
+	pfc_reg_write(PFC_PUEN1, 0x00100234U);
+	pfc_reg_write(PFC_PUEN2, 0x000004C4U);
+	pfc_reg_write(PFC_PUEN3, 0x00000200U);
+	pfc_reg_write(PFC_PUEN4, 0x3E000000U);
+	pfc_reg_write(PFC_PUEN5, 0x1F000805U);
+	pfc_reg_write(PFC_PUEN6, 0x00000006U);
+
+	/* initialize positive/negative logic select */
+	mmio_write_32(GPIO_POSNEG0, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG1, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG2, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG3, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG4, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG5, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG6, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG7, 0x00000000U);
+
+	/* initialize general IO/interrupt switching */
+	mmio_write_32(GPIO_IOINTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL5, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL6, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL7, 0x00000000U);
+
+	/* initialize general output register */
+	mmio_write_32(GPIO_OUTDT1, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT2, 0x00000400U);
+	mmio_write_32(GPIO_OUTDT3, 0x0000C000U);
+	mmio_write_32(GPIO_OUTDT5, 0x00000006U);
+	mmio_write_32(GPIO_OUTDT6, 0x00003880U);
+
+	/* initialize general input/output switching */
+	mmio_write_32(GPIO_INOUTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL1, 0x01000A00U);
+	mmio_write_32(GPIO_INOUTSEL2, 0x00000400U);
+	mmio_write_32(GPIO_INOUTSEL3, 0x0000C000U);
+	mmio_write_32(GPIO_INOUTSEL4, 0x00000000U);
+#if (RCAR_GEN3_ULCB == 1)
+	mmio_write_32(GPIO_INOUTSEL5, 0x0000000EU);
+#else
+	mmio_write_32(GPIO_INOUTSEL5, 0x0000020EU);
+#endif
+	mmio_write_32(GPIO_INOUTSEL6, 0x00013880U);
+	mmio_write_32(GPIO_INOUTSEL7, 0x00000000U);
+}
diff --git a/drivers/renesas/rcar/pfc/H3/pfc_init_h3_v2.h b/drivers/renesas/rcar/pfc/H3/pfc_init_h3_v2.h
new file mode 100644
index 0000000..b02f93e
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/H3/pfc_init_h3_v2.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PFC_INIT_H3_V2_H
+#define PFC_INIT_H3_V2_H
+
+void pfc_init_h3_v2(void);
+
+#endif /* PFC_INIT_H3_V2_H */
diff --git a/drivers/renesas/rcar/pfc/M3/pfc_init_m3.c b/drivers/renesas/rcar/pfc/M3/pfc_init_m3.c
new file mode 100644
index 0000000..7684c62
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/M3/pfc_init_m3.c
@@ -0,0 +1,1311 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>		/* for uint32_t */
+
+#include <lib/mmio.h>
+
+#include "pfc_init_m3.h"
+#include "rcar_def.h"
+#include "rcar_private.h"
+#include "../pfc_regs.h"
+
+#define GPSR0_D15		BIT(15)
+#define GPSR0_D14		BIT(14)
+#define GPSR0_D13		BIT(13)
+#define GPSR0_D12		BIT(12)
+#define GPSR0_D11		BIT(11)
+#define GPSR0_D10		BIT(10)
+#define GPSR0_D9		BIT(9)
+#define GPSR0_D8		BIT(8)
+#define GPSR0_D7		BIT(7)
+#define GPSR0_D6		BIT(6)
+#define GPSR0_D5		BIT(5)
+#define GPSR0_D4		BIT(4)
+#define GPSR0_D3		BIT(3)
+#define GPSR0_D2		BIT(2)
+#define GPSR0_D1		BIT(1)
+#define GPSR0_D0		BIT(0)
+#define GPSR1_CLKOUT		BIT(28)
+#define GPSR1_EX_WAIT0_A	BIT(27)
+#define GPSR1_WE1		BIT(26)
+#define GPSR1_WE0		BIT(25)
+#define GPSR1_RD_WR		BIT(24)
+#define GPSR1_RD		BIT(23)
+#define GPSR1_BS		BIT(22)
+#define GPSR1_CS1_A26		BIT(21)
+#define GPSR1_CS0		BIT(20)
+#define GPSR1_A19		BIT(19)
+#define GPSR1_A18		BIT(18)
+#define GPSR1_A17		BIT(17)
+#define GPSR1_A16		BIT(16)
+#define GPSR1_A15		BIT(15)
+#define GPSR1_A14		BIT(14)
+#define GPSR1_A13		BIT(13)
+#define GPSR1_A12		BIT(12)
+#define GPSR1_A11		BIT(11)
+#define GPSR1_A10		BIT(10)
+#define GPSR1_A9		BIT(9)
+#define GPSR1_A8		BIT(8)
+#define GPSR1_A7		BIT(7)
+#define GPSR1_A6		BIT(6)
+#define GPSR1_A5		BIT(5)
+#define GPSR1_A4		BIT(4)
+#define GPSR1_A3		BIT(3)
+#define GPSR1_A2		BIT(2)
+#define GPSR1_A1		BIT(1)
+#define GPSR1_A0		BIT(0)
+#define GPSR2_AVB_AVTP_CAPTURE_A	BIT(14)
+#define GPSR2_AVB_AVTP_MATCH_A	BIT(13)
+#define GPSR2_AVB_LINK		BIT(12)
+#define GPSR2_AVB_PHY_INT	BIT(11)
+#define GPSR2_AVB_MAGIC		BIT(10)
+#define GPSR2_AVB_MDC		BIT(9)
+#define GPSR2_PWM2_A		BIT(8)
+#define GPSR2_PWM1_A		BIT(7)
+#define GPSR2_PWM0		BIT(6)
+#define GPSR2_IRQ5		BIT(5)
+#define GPSR2_IRQ4		BIT(4)
+#define GPSR2_IRQ3		BIT(3)
+#define GPSR2_IRQ2		BIT(2)
+#define GPSR2_IRQ1		BIT(1)
+#define GPSR2_IRQ0		BIT(0)
+#define GPSR3_SD1_WP		BIT(15)
+#define GPSR3_SD1_CD		BIT(14)
+#define GPSR3_SD0_WP		BIT(13)
+#define GPSR3_SD0_CD		BIT(12)
+#define GPSR3_SD1_DAT3		BIT(11)
+#define GPSR3_SD1_DAT2		BIT(10)
+#define GPSR3_SD1_DAT1		BIT(9)
+#define GPSR3_SD1_DAT0		BIT(8)
+#define GPSR3_SD1_CMD		BIT(7)
+#define GPSR3_SD1_CLK		BIT(6)
+#define GPSR3_SD0_DAT3		BIT(5)
+#define GPSR3_SD0_DAT2		BIT(4)
+#define GPSR3_SD0_DAT1		BIT(3)
+#define GPSR3_SD0_DAT0		BIT(2)
+#define GPSR3_SD0_CMD		BIT(1)
+#define GPSR3_SD0_CLK		BIT(0)
+#define GPSR4_SD3_DS		BIT(17)
+#define GPSR4_SD3_DAT7		BIT(16)
+#define GPSR4_SD3_DAT6		BIT(15)
+#define GPSR4_SD3_DAT5		BIT(14)
+#define GPSR4_SD3_DAT4		BIT(13)
+#define GPSR4_SD3_DAT3		BIT(12)
+#define GPSR4_SD3_DAT2		BIT(11)
+#define GPSR4_SD3_DAT1		BIT(10)
+#define GPSR4_SD3_DAT0		BIT(9)
+#define GPSR4_SD3_CMD		BIT(8)
+#define GPSR4_SD3_CLK		BIT(7)
+#define GPSR4_SD2_DS		BIT(6)
+#define GPSR4_SD2_DAT3		BIT(5)
+#define GPSR4_SD2_DAT2		BIT(4)
+#define GPSR4_SD2_DAT1		BIT(3)
+#define GPSR4_SD2_DAT0		BIT(2)
+#define GPSR4_SD2_CMD		BIT(1)
+#define GPSR4_SD2_CLK		BIT(0)
+#define GPSR5_MLB_DAT		BIT(25)
+#define GPSR5_MLB_SIG		BIT(24)
+#define GPSR5_MLB_CLK		BIT(23)
+#define GPSR5_MSIOF0_RXD	BIT(22)
+#define GPSR5_MSIOF0_SS2	BIT(21)
+#define GPSR5_MSIOF0_TXD	BIT(20)
+#define GPSR5_MSIOF0_SS1	BIT(19)
+#define GPSR5_MSIOF0_SYNC	BIT(18)
+#define GPSR5_MSIOF0_SCK	BIT(17)
+#define GPSR5_HRTS0		BIT(16)
+#define GPSR5_HCTS0		BIT(15)
+#define GPSR5_HTX0		BIT(14)
+#define GPSR5_HRX0		BIT(13)
+#define GPSR5_HSCK0		BIT(12)
+#define GPSR5_RX2_A		BIT(11)
+#define GPSR5_TX2_A		BIT(10)
+#define GPSR5_SCK2		BIT(9)
+#define GPSR5_RTS1		BIT(8)
+#define GPSR5_CTS1		BIT(7)
+#define GPSR5_TX1_A		BIT(6)
+#define GPSR5_RX1_A		BIT(5)
+#define GPSR5_RTS0		BIT(4)
+#define GPSR5_CTS0		BIT(3)
+#define GPSR5_TX0		BIT(2)
+#define GPSR5_RX0		BIT(1)
+#define GPSR5_SCK0		BIT(0)
+#define GPSR6_USB31_OVC		BIT(31)
+#define GPSR6_USB31_PWEN	BIT(30)
+#define GPSR6_USB30_OVC		BIT(29)
+#define GPSR6_USB30_PWEN	BIT(28)
+#define GPSR6_USB1_OVC		BIT(27)
+#define GPSR6_USB1_PWEN		BIT(26)
+#define GPSR6_USB0_OVC		BIT(25)
+#define GPSR6_USB0_PWEN		BIT(24)
+#define GPSR6_AUDIO_CLKB_B	BIT(23)
+#define GPSR6_AUDIO_CLKA_A	BIT(22)
+#define GPSR6_SSI_SDATA9_A	BIT(21)
+#define GPSR6_SSI_SDATA8	BIT(20)
+#define GPSR6_SSI_SDATA7	BIT(19)
+#define GPSR6_SSI_WS78		BIT(18)
+#define GPSR6_SSI_SCK78		BIT(17)
+#define GPSR6_SSI_SDATA6	BIT(16)
+#define GPSR6_SSI_WS6		BIT(15)
+#define GPSR6_SSI_SCK6		BIT(14)
+#define GPSR6_SSI_SDATA5	BIT(13)
+#define GPSR6_SSI_WS5		BIT(12)
+#define GPSR6_SSI_SCK5		BIT(11)
+#define GPSR6_SSI_SDATA4	BIT(10)
+#define GPSR6_SSI_WS4		BIT(9)
+#define GPSR6_SSI_SCK4		BIT(8)
+#define GPSR6_SSI_SDATA3	BIT(7)
+#define GPSR6_SSI_WS34		BIT(6)
+#define GPSR6_SSI_SCK34		BIT(5)
+#define GPSR6_SSI_SDATA2_A	BIT(4)
+#define GPSR6_SSI_SDATA1_A	BIT(3)
+#define GPSR6_SSI_SDATA0	BIT(2)
+#define GPSR6_SSI_WS0129	BIT(1)
+#define GPSR6_SSI_SCK0129	BIT(0)
+#define GPSR7_AVS2		BIT(1)
+#define GPSR7_AVS1		BIT(0)
+
+#define IPSR_28_FUNC(x)		((uint32_t)(x) << 28U)
+#define IPSR_24_FUNC(x)		((uint32_t)(x) << 24U)
+#define IPSR_20_FUNC(x)		((uint32_t)(x) << 20U)
+#define IPSR_16_FUNC(x)		((uint32_t)(x) << 16U)
+#define IPSR_12_FUNC(x)		((uint32_t)(x) << 12U)
+#define IPSR_8_FUNC(x)		((uint32_t)(x) << 8U)
+#define IPSR_4_FUNC(x)		((uint32_t)(x) << 4U)
+#define IPSR_0_FUNC(x)		((uint32_t)(x) << 0U)
+
+#define POC_SD3_DS_33V		BIT(29)
+#define POC_SD3_DAT7_33V	BIT(28)
+#define POC_SD3_DAT6_33V	BIT(27)
+#define POC_SD3_DAT5_33V	BIT(26)
+#define POC_SD3_DAT4_33V	BIT(25)
+#define POC_SD3_DAT3_33V	BIT(24)
+#define POC_SD3_DAT2_33V	BIT(23)
+#define POC_SD3_DAT1_33V	BIT(22)
+#define POC_SD3_DAT0_33V	BIT(21)
+#define POC_SD3_CMD_33V		BIT(20)
+#define POC_SD3_CLK_33V		BIT(19)
+#define POC_SD2_DS_33V		BIT(18)
+#define POC_SD2_DAT3_33V	BIT(17)
+#define POC_SD2_DAT2_33V	BIT(16)
+#define POC_SD2_DAT1_33V	BIT(15)
+#define POC_SD2_DAT0_33V	BIT(14)
+#define POC_SD2_CMD_33V		BIT(13)
+#define POC_SD2_CLK_33V		BIT(12)
+#define POC_SD1_DAT3_33V	BIT(11)
+#define POC_SD1_DAT2_33V	BIT(10)
+#define POC_SD1_DAT1_33V	BIT(9)
+#define POC_SD1_DAT0_33V	BIT(8)
+#define POC_SD1_CMD_33V		BIT(7)
+#define POC_SD1_CLK_33V		BIT(6)
+#define POC_SD0_DAT3_33V	BIT(5)
+#define POC_SD0_DAT2_33V	BIT(4)
+#define POC_SD0_DAT1_33V	BIT(3)
+#define POC_SD0_DAT0_33V	BIT(2)
+#define POC_SD0_CMD_33V		BIT(1)
+#define POC_SD0_CLK_33V		BIT(0)
+
+#define DRVCTRL0_MASK		(0xCCCCCCCCU)
+#define DRVCTRL1_MASK		(0xCCCCCCC8U)
+#define DRVCTRL2_MASK		(0x88888888U)
+#define DRVCTRL3_MASK		(0x88888888U)
+#define DRVCTRL4_MASK		(0x88888888U)
+#define DRVCTRL5_MASK		(0x88888888U)
+#define DRVCTRL6_MASK		(0x88888888U)
+#define DRVCTRL7_MASK		(0x88888888U)
+#define DRVCTRL8_MASK		(0x88888888U)
+#define DRVCTRL9_MASK		(0x88888888U)
+#define DRVCTRL10_MASK		(0x88888888U)
+#define DRVCTRL11_MASK		(0x888888CCU)
+#define DRVCTRL12_MASK		(0xCCCFFFCFU)
+#define DRVCTRL13_MASK		(0xCC888888U)
+#define DRVCTRL14_MASK		(0x88888888U)
+#define DRVCTRL15_MASK		(0x88888888U)
+#define DRVCTRL16_MASK		(0x88888888U)
+#define DRVCTRL17_MASK		(0x88888888U)
+#define DRVCTRL18_MASK		(0x88888888U)
+#define DRVCTRL19_MASK		(0x88888888U)
+#define DRVCTRL20_MASK		(0x88888888U)
+#define DRVCTRL21_MASK		(0x88888888U)
+#define DRVCTRL22_MASK		(0x88888888U)
+#define DRVCTRL23_MASK		(0x88888888U)
+#define DRVCTRL24_MASK		(0x8888888FU)
+
+#define DRVCTRL0_QSPI0_SPCLK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL0_QSPI0_MOSI_IO0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL0_QSPI0_MISO_IO1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL0_QSPI0_IO2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL0_QSPI0_IO3(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL0_QSPI0_SSL(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL0_QSPI1_SPCLK(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL0_QSPI1_MOSI_IO0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL1_QSPI1_MISO_IO1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL1_QSPI1_IO2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL1_QSPI1_IO3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL1_QSPI1_SS(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL1_RPC_INT(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL1_RPC_WP(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL1_RPC_RESET(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL1_AVB_RX_CTL(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL2_AVB_RXC(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL2_AVB_RD0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL2_AVB_RD1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL2_AVB_RD2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL2_AVB_RD3(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL2_AVB_TX_CTL(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL2_AVB_TXC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL2_AVB_TD0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL3_AVB_TD1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL3_AVB_TD2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL3_AVB_TD3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL3_AVB_TXCREFCLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL3_AVB_MDIO(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL3_AVB_MDC(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL3_AVB_MAGIC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL3_AVB_PHY_INT(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL4_AVB_LINK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL4_AVB_AVTP_MATCH(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL4_AVB_AVTP_CAPTURE(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL4_IRQ0(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL4_IRQ1(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL4_IRQ2(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL4_IRQ3(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL4_IRQ4(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL5_IRQ5(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL5_PWM0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL5_PWM1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL5_PWM2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL5_A0(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL5_A1(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL5_A2(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL5_A3(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL6_A4(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL6_A5(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL6_A6(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL6_A7(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL6_A8(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL6_A9(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL6_A10(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL6_A11(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL7_A12(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL7_A13(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL7_A14(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL7_A15(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL7_A16(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL7_A17(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL7_A18(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL7_A19(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL8_CLKOUT(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL8_CS0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL8_CS1_A2(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL8_BS(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL8_RD(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL8_RD_W(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL8_WE0(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL8_WE1(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL9_EX_WAIT0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL9_PRESETOU(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL9_D0(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL9_D1(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL9_D2(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL9_D3(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL9_D4(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL9_D5(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL10_D6(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL10_D7(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL10_D8(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL10_D9(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL10_D10(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL10_D11(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL10_D12(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL10_D13(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL11_D14(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL11_D15(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL11_AVS1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL11_AVS2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL11_GP7_02(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL11_GP7_03(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL11_DU_DOTCLKIN0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL11_DU_DOTCLKIN1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL12_DU_DOTCLKIN2(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL12_DU_DOTCLKIN3(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL12_DU_FSCLKST(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL12_DU_TMS(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL13_TDO(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL13_ASEBRK(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL13_SD0_CLK(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL13_SD0_CMD(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL13_SD0_DAT0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL13_SD0_DAT1(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL13_SD0_DAT2(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL13_SD0_DAT3(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL14_SD1_CLK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL14_SD1_CMD(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL14_SD1_DAT0(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL14_SD1_DAT1(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL14_SD1_DAT2(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL14_SD1_DAT3(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL14_SD2_CLK(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL14_SD2_CMD(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL15_SD2_DAT0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL15_SD2_DAT1(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL15_SD2_DAT2(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL15_SD2_DAT3(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL15_SD2_DS(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL15_SD3_CLK(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL15_SD3_CMD(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL15_SD3_DAT0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL16_SD3_DAT1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL16_SD3_DAT2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL16_SD3_DAT3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL16_SD3_DAT4(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL16_SD3_DAT5(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL16_SD3_DAT6(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL16_SD3_DAT7(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL16_SD3_DS(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL17_SD0_CD(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL17_SD0_WP(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL17_SD1_CD(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL17_SD1_WP(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL17_SCK0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL17_RX0(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL17_TX0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL17_CTS0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL18_RTS0_TANS(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL18_RX1(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL18_TX1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL18_CTS1(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL18_RTS1_TANS(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL18_SCK2(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL18_TX2(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL18_RX2(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL19_HSCK0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL19_HRX0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL19_HTX0(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL19_HCTS0(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL19_HRTS0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL19_MSIOF0_SCK(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL19_MSIOF0_SYNC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL19_MSIOF0_SS1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL20_MSIOF0_TXD(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL20_MSIOF0_SS2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL20_MSIOF0_RXD(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL20_MLB_CLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL20_MLB_SIG(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL20_MLB_DAT(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL20_MLB_REF(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL20_SSI_SCK0129(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL21_SSI_WS0129(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL21_SSI_SDATA0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL21_SSI_SDATA1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL21_SSI_SDATA2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL21_SSI_SCK34(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL21_SSI_WS34(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL21_SSI_SDATA3(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL21_SSI_SCK4(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL22_SSI_WS4(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL22_SSI_SDATA4(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL22_SSI_SCK5(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL22_SSI_WS5(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL22_SSI_SDATA5(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL22_SSI_SCK6(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL22_SSI_WS6(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL22_SSI_SDATA6(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL23_SSI_SCK78(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL23_SSI_WS78(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL23_SSI_SDATA7(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL23_SSI_SDATA8(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL23_SSI_SDATA9(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL23_AUDIO_CLKA(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL23_AUDIO_CLKB(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL23_USB0_PWEN(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL24_USB0_OVC(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL24_USB1_PWEN(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL24_USB1_OVC(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL24_USB30_PWEN(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL24_USB30_OVC(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL24_USB31_PWEN(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL24_USB31_OVC(x)	((uint32_t)(x) << 4U)
+
+#define MOD_SEL0_MSIOF3_A	((uint32_t)0U << 29U)
+#define MOD_SEL0_MSIOF3_B	((uint32_t)1U << 29U)
+#define MOD_SEL0_MSIOF3_C	((uint32_t)2U << 29U)
+#define MOD_SEL0_MSIOF3_D	((uint32_t)3U << 29U)
+#define MOD_SEL0_MSIOF3_E	((uint32_t)4U << 29U)
+#define MOD_SEL0_MSIOF2_A	((uint32_t)0U << 27U)
+#define MOD_SEL0_MSIOF2_B	((uint32_t)1U << 27U)
+#define MOD_SEL0_MSIOF2_C	((uint32_t)2U << 27U)
+#define MOD_SEL0_MSIOF2_D	((uint32_t)3U << 27U)
+#define MOD_SEL0_MSIOF1_A	((uint32_t)0U << 24U)
+#define MOD_SEL0_MSIOF1_B	((uint32_t)1U << 24U)
+#define MOD_SEL0_MSIOF1_C	((uint32_t)2U << 24U)
+#define MOD_SEL0_MSIOF1_D	((uint32_t)3U << 24U)
+#define MOD_SEL0_MSIOF1_E	((uint32_t)4U << 24U)
+#define MOD_SEL0_MSIOF1_F	((uint32_t)5U << 24U)
+#define MOD_SEL0_MSIOF1_G	((uint32_t)6U << 24U)
+#define MOD_SEL0_LBSC_A		((uint32_t)0U << 23U)
+#define MOD_SEL0_LBSC_B		((uint32_t)1U << 23U)
+#define MOD_SEL0_IEBUS_A	((uint32_t)0U << 22U)
+#define MOD_SEL0_IEBUS_B	((uint32_t)1U << 22U)
+#define MOD_SEL0_I2C2_A		((uint32_t)0U << 21U)
+#define MOD_SEL0_I2C2_B		((uint32_t)1U << 21U)
+#define MOD_SEL0_I2C1_A		((uint32_t)0U << 20U)
+#define MOD_SEL0_I2C1_B		((uint32_t)1U << 20U)
+#define MOD_SEL0_HSCIF4_A	((uint32_t)0U << 19U)
+#define MOD_SEL0_HSCIF4_B	((uint32_t)1U << 19U)
+#define MOD_SEL0_HSCIF3_A	((uint32_t)0U << 17U)
+#define MOD_SEL0_HSCIF3_B	((uint32_t)1U << 17U)
+#define MOD_SEL0_HSCIF3_C	((uint32_t)2U << 17U)
+#define MOD_SEL0_HSCIF3_D	((uint32_t)3U << 17U)
+#define MOD_SEL0_HSCIF1_A	((uint32_t)0U << 16U)
+#define MOD_SEL0_HSCIF1_B	((uint32_t)1U << 16U)
+#define MOD_SEL0_FSO_A		((uint32_t)0U << 15U)
+#define MOD_SEL0_FSO_B		((uint32_t)1U << 15U)
+#define MOD_SEL0_HSCIF2_A	((uint32_t)0U << 13U)
+#define MOD_SEL0_HSCIF2_B	((uint32_t)1U << 13U)
+#define MOD_SEL0_HSCIF2_C	((uint32_t)2U << 13U)
+#define MOD_SEL0_ETHERAVB_A	((uint32_t)0U << 12U)
+#define MOD_SEL0_ETHERAVB_B	((uint32_t)1U << 12U)
+#define MOD_SEL0_DRIF3_A	((uint32_t)0U << 11U)
+#define MOD_SEL0_DRIF3_B	((uint32_t)1U << 11U)
+#define MOD_SEL0_DRIF2_A	((uint32_t)0U << 10U)
+#define MOD_SEL0_DRIF2_B	((uint32_t)1U << 10U)
+#define MOD_SEL0_DRIF1_A	((uint32_t)0U << 8U)
+#define MOD_SEL0_DRIF1_B	((uint32_t)1U << 8U)
+#define MOD_SEL0_DRIF1_C	((uint32_t)2U << 8U)
+#define MOD_SEL0_DRIF0_A	((uint32_t)0U << 6U)
+#define MOD_SEL0_DRIF0_B	((uint32_t)1U << 6U)
+#define MOD_SEL0_DRIF0_C	((uint32_t)2U << 6U)
+#define MOD_SEL0_CANFD0_A	((uint32_t)0U << 5U)
+#define MOD_SEL0_CANFD0_B	((uint32_t)1U << 5U)
+#define MOD_SEL0_ADG_A_A	((uint32_t)0U << 3U)
+#define MOD_SEL0_ADG_A_B	((uint32_t)1U << 3U)
+#define MOD_SEL0_ADG_A_C	((uint32_t)2U << 3U)
+#define MOD_SEL1_TSIF1_A	((uint32_t)0U << 30U)
+#define MOD_SEL1_TSIF1_B	((uint32_t)1U << 30U)
+#define MOD_SEL1_TSIF1_C	((uint32_t)2U << 30U)
+#define MOD_SEL1_TSIF1_D	((uint32_t)3U << 30U)
+#define MOD_SEL1_TSIF0_A	((uint32_t)0U << 27U)
+#define MOD_SEL1_TSIF0_B	((uint32_t)1U << 27U)
+#define MOD_SEL1_TSIF0_C	((uint32_t)2U << 27U)
+#define MOD_SEL1_TSIF0_D	((uint32_t)3U << 27U)
+#define MOD_SEL1_TSIF0_E	((uint32_t)4U << 27U)
+#define MOD_SEL1_TIMER_TMU_A	((uint32_t)0U << 26U)
+#define MOD_SEL1_TIMER_TMU_B	((uint32_t)1U << 26U)
+#define MOD_SEL1_SSP1_1_A	((uint32_t)0U << 24U)
+#define MOD_SEL1_SSP1_1_B	((uint32_t)1U << 24U)
+#define MOD_SEL1_SSP1_1_C	((uint32_t)2U << 24U)
+#define MOD_SEL1_SSP1_1_D	((uint32_t)3U << 24U)
+#define MOD_SEL1_SSP1_0_A	((uint32_t)0U << 21U)
+#define MOD_SEL1_SSP1_0_B	((uint32_t)1U << 21U)
+#define MOD_SEL1_SSP1_0_C	((uint32_t)2U << 21U)
+#define MOD_SEL1_SSP1_0_D	((uint32_t)3U << 21U)
+#define MOD_SEL1_SSP1_0_E	((uint32_t)4U << 21U)
+#define MOD_SEL1_SSI_A		((uint32_t)0U << 20U)
+#define MOD_SEL1_SSI_B		((uint32_t)1U << 20U)
+#define MOD_SEL1_SPEED_PULSE_IF_A	((uint32_t)0U << 19U)
+#define MOD_SEL1_SPEED_PULSE_IF_B	((uint32_t)1U << 19U)
+#define MOD_SEL1_SIMCARD_A	((uint32_t)0U << 17U)
+#define MOD_SEL1_SIMCARD_B	((uint32_t)1U << 17U)
+#define MOD_SEL1_SIMCARD_C	((uint32_t)2U << 17U)
+#define MOD_SEL1_SIMCARD_D	((uint32_t)3U << 17U)
+#define MOD_SEL1_SDHI2_A	((uint32_t)0U << 16U)
+#define MOD_SEL1_SDHI2_B	((uint32_t)1U << 16U)
+#define MOD_SEL1_SCIF4_A	((uint32_t)0U << 14U)
+#define MOD_SEL1_SCIF4_B	((uint32_t)1U << 14U)
+#define MOD_SEL1_SCIF4_C	((uint32_t)2U << 14U)
+#define MOD_SEL1_SCIF3_A	((uint32_t)0U << 13U)
+#define MOD_SEL1_SCIF3_B	((uint32_t)1U << 13U)
+#define MOD_SEL1_SCIF2_A	((uint32_t)0U << 12U)
+#define MOD_SEL1_SCIF2_B	((uint32_t)1U << 12U)
+#define MOD_SEL1_SCIF1_A	((uint32_t)0U << 11U)
+#define MOD_SEL1_SCIF1_B	((uint32_t)1U << 11U)
+#define MOD_SEL1_SCIF_A		((uint32_t)0U << 10U)
+#define MOD_SEL1_SCIF_B		((uint32_t)1U << 10U)
+#define MOD_SEL1_REMOCON_A	((uint32_t)0U << 9U)
+#define MOD_SEL1_REMOCON_B	((uint32_t)1U << 9U)
+#define MOD_SEL1_RCAN0_A	((uint32_t)0U << 6U)
+#define MOD_SEL1_RCAN0_B	((uint32_t)1U << 6U)
+#define MOD_SEL1_PWM6_A		((uint32_t)0U << 5U)
+#define MOD_SEL1_PWM6_B		((uint32_t)1U << 5U)
+#define MOD_SEL1_PWM5_A		((uint32_t)0U << 4U)
+#define MOD_SEL1_PWM5_B		((uint32_t)1U << 4U)
+#define MOD_SEL1_PWM4_A		((uint32_t)0U << 3U)
+#define MOD_SEL1_PWM4_B		((uint32_t)1U << 3U)
+#define MOD_SEL1_PWM3_A		((uint32_t)0U << 2U)
+#define MOD_SEL1_PWM3_B		((uint32_t)1U << 2U)
+#define MOD_SEL1_PWM2_A		((uint32_t)0U << 1U)
+#define MOD_SEL1_PWM2_B		((uint32_t)1U << 1U)
+#define MOD_SEL1_PWM1_A		((uint32_t)0U << 0U)
+#define MOD_SEL1_PWM1_B		((uint32_t)1U << 0U)
+#define MOD_SEL2_I2C_5_A	((uint32_t)0U << 31U)
+#define MOD_SEL2_I2C_5_B	((uint32_t)1U << 31U)
+#define MOD_SEL2_I2C_3_A	((uint32_t)0U << 30U)
+#define MOD_SEL2_I2C_3_B	((uint32_t)1U << 30U)
+#define MOD_SEL2_I2C_0_A	((uint32_t)0U << 29U)
+#define MOD_SEL2_I2C_0_B	((uint32_t)1U << 29U)
+#define MOD_SEL2_FM_A		((uint32_t)0U << 27U)
+#define MOD_SEL2_FM_B		((uint32_t)1U << 27U)
+#define MOD_SEL2_FM_C		((uint32_t)2U << 27U)
+#define MOD_SEL2_FM_D		((uint32_t)3U << 27U)
+#define MOD_SEL2_SCIF5_A	((uint32_t)0U << 26U)
+#define MOD_SEL2_SCIF5_B	((uint32_t)1U << 26U)
+#define MOD_SEL2_I2C6_A		((uint32_t)0U << 23U)
+#define MOD_SEL2_I2C6_B		((uint32_t)1U << 23U)
+#define MOD_SEL2_I2C6_C		((uint32_t)2U << 23U)
+#define MOD_SEL2_NDF_A		((uint32_t)0U << 22U)
+#define MOD_SEL2_NDF_B		((uint32_t)1U << 22U)
+#define MOD_SEL2_SSI2_A		((uint32_t)0U << 21U)
+#define MOD_SEL2_SSI2_B		((uint32_t)1U << 21U)
+#define MOD_SEL2_SSI9_A		((uint32_t)0U << 20U)
+#define MOD_SEL2_SSI9_B		((uint32_t)1U << 20U)
+#define MOD_SEL2_TIMER_TMU2_A	((uint32_t)0U << 19U)
+#define MOD_SEL2_TIMER_TMU2_B	((uint32_t)1U << 19U)
+#define MOD_SEL2_ADG_B_A	((uint32_t)0U << 18U)
+#define MOD_SEL2_ADG_B_B	((uint32_t)1U << 18U)
+#define MOD_SEL2_ADG_C_A	((uint32_t)0U << 17U)
+#define MOD_SEL2_ADG_C_B	((uint32_t)1U << 17U)
+#define MOD_SEL2_VIN4_A		((uint32_t)0U << 0U)
+#define MOD_SEL2_VIN4_B		((uint32_t)1U << 0U)
+
+/* SCIF3 Registers for Dummy write */
+#define SCIF3_BASE		(0xE6C50000U)
+#define SCIF3_SCFCR		(SCIF3_BASE + 0x0018U)
+#define SCIF3_SCFDR		(SCIF3_BASE + 0x001CU)
+#define SCFCR_DATA		(0x0000U)
+
+/* Realtime module stop control */
+#define CPG_BASE		(0xE6150000U)
+#define CPG_SCMSTPCR0		(CPG_BASE + 0x0B20U)
+#define CPG_MSTPSR0		(CPG_BASE + 0x0030U)
+#define SCMSTPCR0_RTDMAC	(0x00200000U)
+
+/* RT-DMAC Registers */
+#define RTDMAC_CH		(0U)	/* choose 0 to 15 */
+
+#define RTDMAC_BASE		(0xFFC10000U)
+#define RTDMAC_RDMOR		(RTDMAC_BASE + 0x0060U)
+#define RTDMAC_RDMCHCLR		(RTDMAC_BASE + 0x0080U)
+#define RTDMAC_RDMSAR(x)	(RTDMAC_BASE + 0x8000U + (0x80U * (x)))
+#define RTDMAC_RDMDAR(x)	(RTDMAC_BASE + 0x8004U + (0x80U * (x)))
+#define RTDMAC_RDMTCR(x)	(RTDMAC_BASE + 0x8008U + (0x80U * (x)))
+#define RTDMAC_RDMCHCR(x)	(RTDMAC_BASE + 0x800CU + (0x80U * (x)))
+#define RTDMAC_RDMCHCRB(x)	(RTDMAC_BASE + 0x801CU + (0x80U * (x)))
+#define RTDMAC_RDMDPBASE(x)	(RTDMAC_BASE + 0x8050U + (0x80U * (x)))
+#define RTDMAC_DESC_BASE	(RTDMAC_BASE + 0xA000U)
+#define RTDMAC_DESC_RDMSAR	(RTDMAC_DESC_BASE + 0x0000U)
+#define RTDMAC_DESC_RDMDAR	(RTDMAC_DESC_BASE + 0x0004U)
+#define RTDMAC_DESC_RDMTCR	(RTDMAC_DESC_BASE + 0x0008U)
+
+#define RDMOR_DME		(0x0001U)	/* DMA Master Enable */
+#define RDMCHCR_DPM_INFINITE	(0x30000000U)	/* Infinite repeat mode */
+#define RDMCHCR_RPT_TCR		(0x02000000U)	/* enable to update TCR */
+#define RDMCHCR_TS_2		(0x00000008U)	/* Word(2byte) units transfer */
+#define RDMCHCR_RS_AUTO		(0x00000400U)	/* Auto request */
+#define RDMCHCR_DE		(0x00000001U)	/* DMA Enable */
+#define RDMCHCRB_DRST		(0x00008000U)	/* Descriptor reset */
+#define RDMCHCRB_SLM_256	(0x00000080U)	/* once in 256 clock cycle */
+#define RDMDPBASE_SEL_EXT	(0x00000001U)	/* External memory use */
+
+static void start_rtdma0_descriptor(void)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(RCAR_PRR);
+	reg &= (PRR_PRODUCT_MASK | PRR_CUT_MASK);
+	if (reg == (PRR_PRODUCT_M3_CUT10)) {
+		/* Enable clock supply to RTDMAC. */
+		mstpcr_write(CPG_SCMSTPCR0, CPG_MSTPSR0, SCMSTPCR0_RTDMAC);
+
+		/* Initialize ch0, Reset Descriptor */
+		mmio_write_32(RTDMAC_RDMCHCLR, BIT(RTDMAC_CH));
+		mmio_write_32(RTDMAC_RDMCHCRB(RTDMAC_CH), RDMCHCRB_DRST);
+
+		/* Enable DMA */
+		mmio_write_16(RTDMAC_RDMOR, RDMOR_DME);
+
+		/* Set first transfer */
+		mmio_write_32(RTDMAC_RDMSAR(RTDMAC_CH), RCAR_PRR);
+		mmio_write_32(RTDMAC_RDMDAR(RTDMAC_CH), SCIF3_SCFDR);
+		mmio_write_32(RTDMAC_RDMTCR(RTDMAC_CH), 0x00000001U);
+
+		/* Set descriptor */
+		mmio_write_32(RTDMAC_DESC_RDMSAR, 0x00000000U);
+		mmio_write_32(RTDMAC_DESC_RDMDAR, 0x00000000U);
+		mmio_write_32(RTDMAC_DESC_RDMTCR, 0x00200000U);
+		mmio_write_32(RTDMAC_RDMCHCRB(RTDMAC_CH), RDMCHCRB_SLM_256);
+		mmio_write_32(RTDMAC_RDMDPBASE(RTDMAC_CH), RTDMAC_DESC_BASE
+			      | RDMDPBASE_SEL_EXT);
+
+		/* Set transfer parameter, Start transfer */
+		mmio_write_32(RTDMAC_RDMCHCR(RTDMAC_CH), RDMCHCR_DPM_INFINITE
+			      | RDMCHCR_RPT_TCR
+			      | RDMCHCR_TS_2
+			      | RDMCHCR_RS_AUTO
+			      | RDMCHCR_DE);
+	}
+}
+
+static void pfc_reg_write(uint32_t addr, uint32_t data)
+{
+	uint32_t prr;
+
+	prr = mmio_read_32(RCAR_PRR);
+	prr &= (PRR_PRODUCT_MASK | PRR_CUT_MASK);
+
+	mmio_write_32(PFC_PMMR, ~data);
+	if (prr == (PRR_PRODUCT_M3_CUT10)) {
+		mmio_write_16(SCIF3_SCFCR, SCFCR_DATA);	/* Dummy write */
+	}
+	mmio_write_32((uintptr_t)addr, data);
+	if (prr == (PRR_PRODUCT_M3_CUT10)) {
+		mmio_write_16(SCIF3_SCFCR, SCFCR_DATA);	/* Dummy write */
+	}
+}
+
+void pfc_init_m3(void)
+{
+	uint32_t reg;
+
+	/* Work around for PFC eratta */
+	start_rtdma0_descriptor();
+
+	/* initialize module select */
+	pfc_reg_write(PFC_MOD_SEL0, MOD_SEL0_MSIOF3_A
+		      | MOD_SEL0_MSIOF2_A
+		      | MOD_SEL0_MSIOF1_A
+		      | MOD_SEL0_LBSC_A
+		      | MOD_SEL0_IEBUS_A
+		      | MOD_SEL0_I2C2_A
+		      | MOD_SEL0_I2C1_A
+		      | MOD_SEL0_HSCIF4_A
+		      | MOD_SEL0_HSCIF3_A
+		      | MOD_SEL0_HSCIF1_A
+		      | MOD_SEL0_FSO_A
+		      | MOD_SEL0_HSCIF2_A
+		      | MOD_SEL0_ETHERAVB_A
+		      | MOD_SEL0_DRIF3_A
+		      | MOD_SEL0_DRIF2_A
+		      | MOD_SEL0_DRIF1_A
+		      | MOD_SEL0_DRIF0_A
+		      | MOD_SEL0_CANFD0_A
+		      | MOD_SEL0_ADG_A_A);
+	pfc_reg_write(PFC_MOD_SEL1, MOD_SEL1_TSIF1_A
+		      | MOD_SEL1_TSIF0_A
+		      | MOD_SEL1_TIMER_TMU_A
+		      | MOD_SEL1_SSP1_1_A
+		      | MOD_SEL1_SSP1_0_A
+		      | MOD_SEL1_SSI_A
+		      | MOD_SEL1_SPEED_PULSE_IF_A
+		      | MOD_SEL1_SIMCARD_A
+		      | MOD_SEL1_SDHI2_A
+		      | MOD_SEL1_SCIF4_A
+		      | MOD_SEL1_SCIF3_A
+		      | MOD_SEL1_SCIF2_A
+		      | MOD_SEL1_SCIF1_A
+		      | MOD_SEL1_SCIF_A
+		      | MOD_SEL1_REMOCON_A
+		      | MOD_SEL1_RCAN0_A
+		      | MOD_SEL1_PWM6_A
+		      | MOD_SEL1_PWM5_A
+		      | MOD_SEL1_PWM4_A
+		      | MOD_SEL1_PWM3_A
+		      | MOD_SEL1_PWM2_A
+		      | MOD_SEL1_PWM1_A);
+	pfc_reg_write(PFC_MOD_SEL2, MOD_SEL2_I2C_5_A
+		      | MOD_SEL2_I2C_3_A
+		      | MOD_SEL2_I2C_0_A
+		      | MOD_SEL2_FM_A
+		      | MOD_SEL2_SCIF5_A
+		      | MOD_SEL2_I2C6_A
+		      | MOD_SEL2_NDF_A
+		      | MOD_SEL2_SSI2_A
+		      | MOD_SEL2_SSI9_A
+		      | MOD_SEL2_TIMER_TMU2_A
+		      | MOD_SEL2_ADG_B_A
+		      | MOD_SEL2_ADG_C_A
+		      | MOD_SEL2_VIN4_A);
+
+	/* initialize peripheral function select */
+	pfc_reg_write(PFC_IPSR0, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR1, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(3)
+		      | IPSR_8_FUNC(3)
+		      | IPSR_4_FUNC(3)
+		      | IPSR_0_FUNC(3));
+	pfc_reg_write(PFC_IPSR2, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR3, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR4, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR5, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR6, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR7, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR8, IPSR_28_FUNC(1)
+		      | IPSR_24_FUNC(1)
+		      | IPSR_20_FUNC(1)
+		      | IPSR_16_FUNC(1)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR9, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR10, IPSR_28_FUNC(1)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR11, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(4)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(1));
+	pfc_reg_write(PFC_IPSR12, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(4)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR13, IPSR_28_FUNC(8)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(3)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR14, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(3)
+		      | IPSR_0_FUNC(8));
+	pfc_reg_write(PFC_IPSR15, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR16, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR17, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(1)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR18, IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+
+	/* initialize GPIO/perihperal function select */
+	pfc_reg_write(PFC_GPSR0, GPSR0_D15
+		      | GPSR0_D14
+		      | GPSR0_D13
+		      | GPSR0_D12
+		      | GPSR0_D11
+		      | GPSR0_D10
+		      | GPSR0_D9
+		      | GPSR0_D8);
+	pfc_reg_write(PFC_GPSR1, GPSR1_CLKOUT
+		      | GPSR1_EX_WAIT0_A
+		      | GPSR1_A19
+		      | GPSR1_A18
+		      | GPSR1_A17
+		      | GPSR1_A16
+		      | GPSR1_A15
+		      | GPSR1_A14
+		      | GPSR1_A13
+		      | GPSR1_A12
+		      | GPSR1_A7
+		      | GPSR1_A6
+		      | GPSR1_A5
+		      | GPSR1_A4
+		      | GPSR1_A3
+		      | GPSR1_A2
+		      | GPSR1_A1
+		      | GPSR1_A0);
+	pfc_reg_write(PFC_GPSR2, GPSR2_AVB_AVTP_CAPTURE_A
+		      | GPSR2_AVB_AVTP_MATCH_A
+		      | GPSR2_AVB_LINK
+		      | GPSR2_AVB_PHY_INT
+		      | GPSR2_AVB_MDC
+		      | GPSR2_PWM2_A
+		      | GPSR2_PWM1_A
+		      | GPSR2_IRQ5
+		      | GPSR2_IRQ4
+		      | GPSR2_IRQ3
+		      | GPSR2_IRQ2
+		      | GPSR2_IRQ1
+		      | GPSR2_IRQ0);
+	pfc_reg_write(PFC_GPSR3, GPSR3_SD0_WP
+		      | GPSR3_SD0_CD
+		      | GPSR3_SD1_DAT3
+		      | GPSR3_SD1_DAT2
+		      | GPSR3_SD1_DAT1
+		      | GPSR3_SD1_DAT0
+		      | GPSR3_SD0_DAT3
+		      | GPSR3_SD0_DAT2
+		      | GPSR3_SD0_DAT1
+		      | GPSR3_SD0_DAT0
+		      | GPSR3_SD0_CMD
+		      | GPSR3_SD0_CLK);
+	pfc_reg_write(PFC_GPSR4, GPSR4_SD3_DAT7
+		      | GPSR4_SD3_DAT6
+		      | GPSR4_SD3_DAT3
+		      | GPSR4_SD3_DAT2
+		      | GPSR4_SD3_DAT1
+		      | GPSR4_SD3_DAT0
+		      | GPSR4_SD3_CMD
+		      | GPSR4_SD3_CLK
+		      | GPSR4_SD2_DS
+		      | GPSR4_SD2_DAT3
+		      | GPSR4_SD2_DAT2
+		      | GPSR4_SD2_DAT1
+		      | GPSR4_SD2_DAT0
+		      | GPSR4_SD2_CMD
+		      | GPSR4_SD2_CLK);
+	pfc_reg_write(PFC_GPSR5, GPSR5_MSIOF0_SS2
+		      | GPSR5_MSIOF0_SS1
+		      | GPSR5_MSIOF0_SYNC
+		      | GPSR5_HRTS0
+		      | GPSR5_HCTS0
+		      | GPSR5_HTX0
+		      | GPSR5_HRX0
+		      | GPSR5_HSCK0
+		      | GPSR5_RX2_A
+		      | GPSR5_TX2_A
+		      | GPSR5_SCK2
+		      | GPSR5_RTS1
+		      | GPSR5_CTS1
+		      | GPSR5_TX1_A
+		      | GPSR5_RX1_A
+		      | GPSR5_RTS0
+		      | GPSR5_SCK0);
+	pfc_reg_write(PFC_GPSR6, GPSR6_USB30_OVC
+		      | GPSR6_USB30_PWEN
+		      | GPSR6_USB1_OVC
+		      | GPSR6_USB1_PWEN
+		      | GPSR6_USB0_OVC
+		      | GPSR6_USB0_PWEN
+		      | GPSR6_AUDIO_CLKB_B
+		      | GPSR6_AUDIO_CLKA_A
+		      | GPSR6_SSI_SDATA8
+		      | GPSR6_SSI_SDATA7
+		      | GPSR6_SSI_WS78
+		      | GPSR6_SSI_SCK78
+		      | GPSR6_SSI_WS6
+		      | GPSR6_SSI_SCK6
+		      | GPSR6_SSI_SDATA4
+		      | GPSR6_SSI_WS4
+		      | GPSR6_SSI_SCK4
+		      | GPSR6_SSI_SDATA1_A
+		      | GPSR6_SSI_SDATA0
+		      | GPSR6_SSI_WS0129
+		      | GPSR6_SSI_SCK0129);
+	pfc_reg_write(PFC_GPSR7, GPSR7_AVS2
+		      | GPSR7_AVS1);
+
+	/* initialize POC control register */
+	pfc_reg_write(PFC_POCCTRL0, POC_SD3_DS_33V
+		      | POC_SD3_DAT7_33V
+		      | POC_SD3_DAT6_33V
+		      | POC_SD3_DAT5_33V
+		      | POC_SD3_DAT4_33V
+		      | POC_SD3_DAT3_33V
+		      | POC_SD3_DAT2_33V
+		      | POC_SD3_DAT1_33V
+		      | POC_SD3_DAT0_33V
+		      | POC_SD3_CMD_33V
+		      | POC_SD3_CLK_33V
+		      | POC_SD0_DAT3_33V
+		      | POC_SD0_DAT2_33V
+		      | POC_SD0_DAT1_33V
+		      | POC_SD0_DAT0_33V
+		      | POC_SD0_CMD_33V
+		      | POC_SD0_CLK_33V);
+
+	/* initialize DRV control register */
+	reg = mmio_read_32(PFC_DRVCTRL0);
+	reg = ((reg & DRVCTRL0_MASK) | DRVCTRL0_QSPI0_SPCLK(3)
+	       | DRVCTRL0_QSPI0_MOSI_IO0(3)
+	       | DRVCTRL0_QSPI0_MISO_IO1(3)
+	       | DRVCTRL0_QSPI0_IO2(3)
+	       | DRVCTRL0_QSPI0_IO3(3)
+	       | DRVCTRL0_QSPI0_SSL(3)
+	       | DRVCTRL0_QSPI1_SPCLK(3)
+	       | DRVCTRL0_QSPI1_MOSI_IO0(3));
+	pfc_reg_write(PFC_DRVCTRL0, reg);
+	reg = mmio_read_32(PFC_DRVCTRL1);
+	reg = ((reg & DRVCTRL1_MASK) | DRVCTRL1_QSPI1_MISO_IO1(3)
+	       | DRVCTRL1_QSPI1_IO2(3)
+	       | DRVCTRL1_QSPI1_IO3(3)
+	       | DRVCTRL1_QSPI1_SS(3)
+	       | DRVCTRL1_RPC_INT(3)
+	       | DRVCTRL1_RPC_WP(3)
+	       | DRVCTRL1_RPC_RESET(3)
+	       | DRVCTRL1_AVB_RX_CTL(7));
+	pfc_reg_write(PFC_DRVCTRL1, reg);
+	reg = mmio_read_32(PFC_DRVCTRL2);
+	reg = ((reg & DRVCTRL2_MASK) | DRVCTRL2_AVB_RXC(7)
+	       | DRVCTRL2_AVB_RD0(7)
+	       | DRVCTRL2_AVB_RD1(7)
+	       | DRVCTRL2_AVB_RD2(7)
+	       | DRVCTRL2_AVB_RD3(7)
+	       | DRVCTRL2_AVB_TX_CTL(3)
+	       | DRVCTRL2_AVB_TXC(3)
+	       | DRVCTRL2_AVB_TD0(3));
+	pfc_reg_write(PFC_DRVCTRL2, reg);
+	reg = mmio_read_32(PFC_DRVCTRL3);
+	reg = ((reg & DRVCTRL3_MASK) | DRVCTRL3_AVB_TD1(3)
+	       | DRVCTRL3_AVB_TD2(3)
+	       | DRVCTRL3_AVB_TD3(3)
+	       | DRVCTRL3_AVB_TXCREFCLK(7)
+	       | DRVCTRL3_AVB_MDIO(7)
+	       | DRVCTRL3_AVB_MDC(7)
+	       | DRVCTRL3_AVB_MAGIC(7)
+	       | DRVCTRL3_AVB_PHY_INT(7));
+	pfc_reg_write(PFC_DRVCTRL3, reg);
+	reg = mmio_read_32(PFC_DRVCTRL4);
+	reg = ((reg & DRVCTRL4_MASK) | DRVCTRL4_AVB_LINK(7)
+	       | DRVCTRL4_AVB_AVTP_MATCH(7)
+	       | DRVCTRL4_AVB_AVTP_CAPTURE(7)
+	       | DRVCTRL4_IRQ0(7)
+	       | DRVCTRL4_IRQ1(7)
+	       | DRVCTRL4_IRQ2(7)
+	       | DRVCTRL4_IRQ3(7)
+	       | DRVCTRL4_IRQ4(7));
+	pfc_reg_write(PFC_DRVCTRL4, reg);
+	reg = mmio_read_32(PFC_DRVCTRL5);
+	reg = ((reg & DRVCTRL5_MASK) | DRVCTRL5_IRQ5(7)
+	       | DRVCTRL5_PWM0(7)
+	       | DRVCTRL5_PWM1(7)
+	       | DRVCTRL5_PWM2(7)
+	       | DRVCTRL5_A0(3)
+	       | DRVCTRL5_A1(3)
+	       | DRVCTRL5_A2(3)
+	       | DRVCTRL5_A3(3));
+	pfc_reg_write(PFC_DRVCTRL5, reg);
+	reg = mmio_read_32(PFC_DRVCTRL6);
+	reg = ((reg & DRVCTRL6_MASK) | DRVCTRL6_A4(3)
+	       | DRVCTRL6_A5(3)
+	       | DRVCTRL6_A6(3)
+	       | DRVCTRL6_A7(3)
+	       | DRVCTRL6_A8(7)
+	       | DRVCTRL6_A9(7)
+	       | DRVCTRL6_A10(7)
+	       | DRVCTRL6_A11(7));
+	pfc_reg_write(PFC_DRVCTRL6, reg);
+	reg = mmio_read_32(PFC_DRVCTRL7);
+	reg = ((reg & DRVCTRL7_MASK) | DRVCTRL7_A12(3)
+	       | DRVCTRL7_A13(3)
+	       | DRVCTRL7_A14(3)
+	       | DRVCTRL7_A15(3)
+	       | DRVCTRL7_A16(3)
+	       | DRVCTRL7_A17(3)
+	       | DRVCTRL7_A18(3)
+	       | DRVCTRL7_A19(3));
+	pfc_reg_write(PFC_DRVCTRL7, reg);
+	reg = mmio_read_32(PFC_DRVCTRL8);
+	reg = ((reg & DRVCTRL8_MASK) | DRVCTRL8_CLKOUT(7)
+	       | DRVCTRL8_CS0(7)
+	       | DRVCTRL8_CS1_A2(7)
+	       | DRVCTRL8_BS(7)
+	       | DRVCTRL8_RD(7)
+	       | DRVCTRL8_RD_W(7)
+	       | DRVCTRL8_WE0(7)
+	       | DRVCTRL8_WE1(7));
+	pfc_reg_write(PFC_DRVCTRL8, reg);
+	reg = mmio_read_32(PFC_DRVCTRL9);
+	reg = ((reg & DRVCTRL9_MASK) | DRVCTRL9_EX_WAIT0(7)
+	       | DRVCTRL9_PRESETOU(7)
+	       | DRVCTRL9_D0(7)
+	       | DRVCTRL9_D1(7)
+	       | DRVCTRL9_D2(7)
+	       | DRVCTRL9_D3(7)
+	       | DRVCTRL9_D4(7)
+	       | DRVCTRL9_D5(7));
+	pfc_reg_write(PFC_DRVCTRL9, reg);
+	reg = mmio_read_32(PFC_DRVCTRL10);
+	reg = ((reg & DRVCTRL10_MASK) | DRVCTRL10_D6(7)
+	       | DRVCTRL10_D7(7)
+	       | DRVCTRL10_D8(3)
+	       | DRVCTRL10_D9(3)
+	       | DRVCTRL10_D10(3)
+	       | DRVCTRL10_D11(3)
+	       | DRVCTRL10_D12(3)
+	       | DRVCTRL10_D13(3));
+	pfc_reg_write(PFC_DRVCTRL10, reg);
+	reg = mmio_read_32(PFC_DRVCTRL11);
+	reg = ((reg & DRVCTRL11_MASK) | DRVCTRL11_D14(3)
+	       | DRVCTRL11_D15(3)
+	       | DRVCTRL11_AVS1(7)
+	       | DRVCTRL11_AVS2(7)
+	       | DRVCTRL11_GP7_02(7)
+	       | DRVCTRL11_GP7_03(7)
+	       | DRVCTRL11_DU_DOTCLKIN0(3)
+	       | DRVCTRL11_DU_DOTCLKIN1(3));
+	pfc_reg_write(PFC_DRVCTRL11, reg);
+	reg = mmio_read_32(PFC_DRVCTRL12);
+	reg = ((reg & DRVCTRL12_MASK) | DRVCTRL12_DU_DOTCLKIN2(3)
+	       | DRVCTRL12_DU_DOTCLKIN3(3)
+	       | DRVCTRL12_DU_FSCLKST(3)
+	       | DRVCTRL12_DU_TMS(3));
+	pfc_reg_write(PFC_DRVCTRL12, reg);
+	reg = mmio_read_32(PFC_DRVCTRL13);
+	reg = ((reg & DRVCTRL13_MASK) | DRVCTRL13_TDO(3)
+	       | DRVCTRL13_ASEBRK(3)
+	       | DRVCTRL13_SD0_CLK(7)
+	       | DRVCTRL13_SD0_CMD(7)
+	       | DRVCTRL13_SD0_DAT0(7)
+	       | DRVCTRL13_SD0_DAT1(7)
+	       | DRVCTRL13_SD0_DAT2(7)
+	       | DRVCTRL13_SD0_DAT3(7));
+	pfc_reg_write(PFC_DRVCTRL13, reg);
+	reg = mmio_read_32(PFC_DRVCTRL14);
+	reg = ((reg & DRVCTRL14_MASK) | DRVCTRL14_SD1_CLK(7)
+	       | DRVCTRL14_SD1_CMD(7)
+	       | DRVCTRL14_SD1_DAT0(5)
+	       | DRVCTRL14_SD1_DAT1(5)
+	       | DRVCTRL14_SD1_DAT2(5)
+	       | DRVCTRL14_SD1_DAT3(5)
+	       | DRVCTRL14_SD2_CLK(5)
+	       | DRVCTRL14_SD2_CMD(5));
+	pfc_reg_write(PFC_DRVCTRL14, reg);
+	reg = mmio_read_32(PFC_DRVCTRL15);
+	reg = ((reg & DRVCTRL15_MASK) | DRVCTRL15_SD2_DAT0(5)
+	       | DRVCTRL15_SD2_DAT1(5)
+	       | DRVCTRL15_SD2_DAT2(5)
+	       | DRVCTRL15_SD2_DAT3(5)
+	       | DRVCTRL15_SD2_DS(5)
+	       | DRVCTRL15_SD3_CLK(7)
+	       | DRVCTRL15_SD3_CMD(7)
+	       | DRVCTRL15_SD3_DAT0(7));
+	pfc_reg_write(PFC_DRVCTRL15, reg);
+	reg = mmio_read_32(PFC_DRVCTRL16);
+	reg = ((reg & DRVCTRL16_MASK) | DRVCTRL16_SD3_DAT1(7)
+	       | DRVCTRL16_SD3_DAT2(7)
+	       | DRVCTRL16_SD3_DAT3(7)
+	       | DRVCTRL16_SD3_DAT4(7)
+	       | DRVCTRL16_SD3_DAT5(7)
+	       | DRVCTRL16_SD3_DAT6(7)
+	       | DRVCTRL16_SD3_DAT7(7)
+	       | DRVCTRL16_SD3_DS(7));
+	pfc_reg_write(PFC_DRVCTRL16, reg);
+	reg = mmio_read_32(PFC_DRVCTRL17);
+	reg = ((reg & DRVCTRL17_MASK) | DRVCTRL17_SD0_CD(7)
+	       | DRVCTRL17_SD0_WP(7)
+	       | DRVCTRL17_SD1_CD(7)
+	       | DRVCTRL17_SD1_WP(7)
+	       | DRVCTRL17_SCK0(7)
+	       | DRVCTRL17_RX0(7)
+	       | DRVCTRL17_TX0(7)
+	       | DRVCTRL17_CTS0(7));
+	pfc_reg_write(PFC_DRVCTRL17, reg);
+	reg = mmio_read_32(PFC_DRVCTRL18);
+	reg = ((reg & DRVCTRL18_MASK) | DRVCTRL18_RTS0_TANS(7)
+	       | DRVCTRL18_RX1(7)
+	       | DRVCTRL18_TX1(7)
+	       | DRVCTRL18_CTS1(7)
+	       | DRVCTRL18_RTS1_TANS(7)
+	       | DRVCTRL18_SCK2(7)
+	       | DRVCTRL18_TX2(7)
+	       | DRVCTRL18_RX2(7));
+	pfc_reg_write(PFC_DRVCTRL18, reg);
+	reg = mmio_read_32(PFC_DRVCTRL19);
+	reg = ((reg & DRVCTRL19_MASK) | DRVCTRL19_HSCK0(7)
+	       | DRVCTRL19_HRX0(7)
+	       | DRVCTRL19_HTX0(7)
+	       | DRVCTRL19_HCTS0(7)
+	       | DRVCTRL19_HRTS0(7)
+	       | DRVCTRL19_MSIOF0_SCK(7)
+	       | DRVCTRL19_MSIOF0_SYNC(7)
+	       | DRVCTRL19_MSIOF0_SS1(7));
+	pfc_reg_write(PFC_DRVCTRL19, reg);
+	reg = mmio_read_32(PFC_DRVCTRL20);
+	reg = ((reg & DRVCTRL20_MASK) | DRVCTRL20_MSIOF0_TXD(7)
+	       | DRVCTRL20_MSIOF0_SS2(7)
+	       | DRVCTRL20_MSIOF0_RXD(7)
+	       | DRVCTRL20_MLB_CLK(7)
+	       | DRVCTRL20_MLB_SIG(7)
+	       | DRVCTRL20_MLB_DAT(7)
+	       | DRVCTRL20_MLB_REF(7)
+	       | DRVCTRL20_SSI_SCK0129(7));
+	pfc_reg_write(PFC_DRVCTRL20, reg);
+	reg = mmio_read_32(PFC_DRVCTRL21);
+	reg = ((reg & DRVCTRL21_MASK) | DRVCTRL21_SSI_WS0129(7)
+	       | DRVCTRL21_SSI_SDATA0(7)
+	       | DRVCTRL21_SSI_SDATA1(7)
+	       | DRVCTRL21_SSI_SDATA2(7)
+	       | DRVCTRL21_SSI_SCK34(7)
+	       | DRVCTRL21_SSI_WS34(7)
+	       | DRVCTRL21_SSI_SDATA3(7)
+	       | DRVCTRL21_SSI_SCK4(7));
+	pfc_reg_write(PFC_DRVCTRL21, reg);
+	reg = mmio_read_32(PFC_DRVCTRL22);
+	reg = ((reg & DRVCTRL22_MASK) | DRVCTRL22_SSI_WS4(7)
+	       | DRVCTRL22_SSI_SDATA4(7)
+	       | DRVCTRL22_SSI_SCK5(7)
+	       | DRVCTRL22_SSI_WS5(7)
+	       | DRVCTRL22_SSI_SDATA5(7)
+	       | DRVCTRL22_SSI_SCK6(7)
+	       | DRVCTRL22_SSI_WS6(7)
+	       | DRVCTRL22_SSI_SDATA6(7));
+	pfc_reg_write(PFC_DRVCTRL22, reg);
+	reg = mmio_read_32(PFC_DRVCTRL23);
+	reg = ((reg & DRVCTRL23_MASK) | DRVCTRL23_SSI_SCK78(7)
+	       | DRVCTRL23_SSI_WS78(7)
+	       | DRVCTRL23_SSI_SDATA7(7)
+	       | DRVCTRL23_SSI_SDATA8(7)
+	       | DRVCTRL23_SSI_SDATA9(7)
+	       | DRVCTRL23_AUDIO_CLKA(7)
+	       | DRVCTRL23_AUDIO_CLKB(7)
+	       | DRVCTRL23_USB0_PWEN(7));
+	pfc_reg_write(PFC_DRVCTRL23, reg);
+	reg = mmio_read_32(PFC_DRVCTRL24);
+	reg = ((reg & DRVCTRL24_MASK) | DRVCTRL24_USB0_OVC(7)
+	       | DRVCTRL24_USB1_PWEN(7)
+	       | DRVCTRL24_USB1_OVC(7)
+	       | DRVCTRL24_USB30_PWEN(7)
+	       | DRVCTRL24_USB30_OVC(7)
+	       | DRVCTRL24_USB31_PWEN(7)
+	       | DRVCTRL24_USB31_OVC(7));
+	pfc_reg_write(PFC_DRVCTRL24, reg);
+
+	/* initialize LSI pin pull-up/down control */
+	pfc_reg_write(PFC_PUD0, 0x00005FBFU);
+	pfc_reg_write(PFC_PUD1, 0x00300FFEU);
+	pfc_reg_write(PFC_PUD2, 0x330001E6U);
+	pfc_reg_write(PFC_PUD3, 0x000002E0U);
+	pfc_reg_write(PFC_PUD4, 0xFFFFFF00U);
+	pfc_reg_write(PFC_PUD5, 0x7F5FFF87U);
+	pfc_reg_write(PFC_PUD6, 0x00000055U);
+
+	/* initialize LSI pin pull-enable register */
+	pfc_reg_write(PFC_PUEN0, 0x00000FFFU);
+	pfc_reg_write(PFC_PUEN1, 0x00100234U);
+	pfc_reg_write(PFC_PUEN2, 0x000004C4U);
+	pfc_reg_write(PFC_PUEN3, 0x00000200U);
+	pfc_reg_write(PFC_PUEN4, 0x3E000000U);
+	pfc_reg_write(PFC_PUEN5, 0x1F000805U);
+	pfc_reg_write(PFC_PUEN6, 0x00000006U);
+
+	/* initialize positive/negative logic select */
+	mmio_write_32(GPIO_POSNEG0, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG1, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG2, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG3, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG4, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG5, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG6, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG7, 0x00000000U);
+
+	/* initialize general IO/interrupt switching */
+	mmio_write_32(GPIO_IOINTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL5, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL6, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL7, 0x00000000U);
+
+	/* initialize general output register */
+	mmio_write_32(GPIO_OUTDT1, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT2, 0x00000400U);
+	mmio_write_32(GPIO_OUTDT3, 0x0000C000U);
+	mmio_write_32(GPIO_OUTDT5, 0x00000006U);
+	mmio_write_32(GPIO_OUTDT6, 0x00003880U);
+
+	/* initialize general input/output switching */
+	mmio_write_32(GPIO_INOUTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL1, 0x01000A00U);
+	mmio_write_32(GPIO_INOUTSEL2, 0x00000400U);
+	mmio_write_32(GPIO_INOUTSEL3, 0x0000C000U);
+	mmio_write_32(GPIO_INOUTSEL4, 0x00000000U);
+#if (RCAR_GEN3_ULCB == 1)
+	mmio_write_32(GPIO_INOUTSEL5, 0x0000000EU);
+#else
+	mmio_write_32(GPIO_INOUTSEL5, 0x0000020EU);
+#endif
+	mmio_write_32(GPIO_INOUTSEL6, 0x00013880U);
+	mmio_write_32(GPIO_INOUTSEL7, 0x00000000U);
+}
diff --git a/drivers/renesas/rcar/pfc/M3/pfc_init_m3.h b/drivers/renesas/rcar/pfc/M3/pfc_init_m3.h
new file mode 100644
index 0000000..70885de
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/M3/pfc_init_m3.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PFC_INIT_M3_H
+#define PFC_INIT_M3_H
+
+void pfc_init_m3(void);
+
+#endif /* PFC_INIT_M3_H */
diff --git a/drivers/renesas/rcar/pfc/M3N/pfc_init_m3n.c b/drivers/renesas/rcar/pfc/M3N/pfc_init_m3n.c
new file mode 100644
index 0000000..5014556
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/M3N/pfc_init_m3n.c
@@ -0,0 +1,1218 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>		/* for uint32_t */
+
+#include <lib/mmio.h>
+
+#include "pfc_init_m3n.h"
+#include "rcar_def.h"
+#include "../pfc_regs.h"
+
+#define GPSR0_D15		BIT(15)
+#define GPSR0_D14		BIT(14)
+#define GPSR0_D13		BIT(13)
+#define GPSR0_D12		BIT(12)
+#define GPSR0_D11		BIT(11)
+#define GPSR0_D10		BIT(10)
+#define GPSR0_D9		BIT(9)
+#define GPSR0_D8		BIT(8)
+#define GPSR0_D7		BIT(7)
+#define GPSR0_D6		BIT(6)
+#define GPSR0_D5		BIT(5)
+#define GPSR0_D4		BIT(4)
+#define GPSR0_D3		BIT(3)
+#define GPSR0_D2		BIT(2)
+#define GPSR0_D1		BIT(1)
+#define GPSR0_D0		BIT(0)
+#define GPSR1_CLKOUT		BIT(28)
+#define GPSR1_EX_WAIT0_A	BIT(27)
+#define GPSR1_WE1		BIT(26)
+#define GPSR1_WE0		BIT(25)
+#define GPSR1_RD_WR		BIT(24)
+#define GPSR1_RD		BIT(23)
+#define GPSR1_BS		BIT(22)
+#define GPSR1_CS1_A26		BIT(21)
+#define GPSR1_CS0		BIT(20)
+#define GPSR1_A19		BIT(19)
+#define GPSR1_A18		BIT(18)
+#define GPSR1_A17		BIT(17)
+#define GPSR1_A16		BIT(16)
+#define GPSR1_A15		BIT(15)
+#define GPSR1_A14		BIT(14)
+#define GPSR1_A13		BIT(13)
+#define GPSR1_A12		BIT(12)
+#define GPSR1_A11		BIT(11)
+#define GPSR1_A10		BIT(10)
+#define GPSR1_A9		BIT(9)
+#define GPSR1_A8		BIT(8)
+#define GPSR1_A7		BIT(7)
+#define GPSR1_A6		BIT(6)
+#define GPSR1_A5		BIT(5)
+#define GPSR1_A4		BIT(4)
+#define GPSR1_A3		BIT(3)
+#define GPSR1_A2		BIT(2)
+#define GPSR1_A1		BIT(1)
+#define GPSR1_A0		BIT(0)
+#define GPSR2_AVB_AVTP_CAPTURE_A	BIT(14)
+#define GPSR2_AVB_AVTP_MATCH_A	BIT(13)
+#define GPSR2_AVB_LINK		BIT(12)
+#define GPSR2_AVB_PHY_INT	BIT(11)
+#define GPSR2_AVB_MAGIC		BIT(10)
+#define GPSR2_AVB_MDC		BIT(9)
+#define GPSR2_PWM2_A		BIT(8)
+#define GPSR2_PWM1_A		BIT(7)
+#define GPSR2_PWM0		BIT(6)
+#define GPSR2_IRQ5		BIT(5)
+#define GPSR2_IRQ4		BIT(4)
+#define GPSR2_IRQ3		BIT(3)
+#define GPSR2_IRQ2		BIT(2)
+#define GPSR2_IRQ1		BIT(1)
+#define GPSR2_IRQ0		BIT(0)
+#define GPSR3_SD1_WP		BIT(15)
+#define GPSR3_SD1_CD		BIT(14)
+#define GPSR3_SD0_WP		BIT(13)
+#define GPSR3_SD0_CD		BIT(12)
+#define GPSR3_SD1_DAT3		BIT(11)
+#define GPSR3_SD1_DAT2		BIT(10)
+#define GPSR3_SD1_DAT1		BIT(9)
+#define GPSR3_SD1_DAT0		BIT(8)
+#define GPSR3_SD1_CMD		BIT(7)
+#define GPSR3_SD1_CLK		BIT(6)
+#define GPSR3_SD0_DAT3		BIT(5)
+#define GPSR3_SD0_DAT2		BIT(4)
+#define GPSR3_SD0_DAT1		BIT(3)
+#define GPSR3_SD0_DAT0		BIT(2)
+#define GPSR3_SD0_CMD		BIT(1)
+#define GPSR3_SD0_CLK		BIT(0)
+#define GPSR4_SD3_DS		BIT(17)
+#define GPSR4_SD3_DAT7		BIT(16)
+#define GPSR4_SD3_DAT6		BIT(15)
+#define GPSR4_SD3_DAT5		BIT(14)
+#define GPSR4_SD3_DAT4		BIT(13)
+#define GPSR4_SD3_DAT3		BIT(12)
+#define GPSR4_SD3_DAT2		BIT(11)
+#define GPSR4_SD3_DAT1		BIT(10)
+#define GPSR4_SD3_DAT0		BIT(9)
+#define GPSR4_SD3_CMD		BIT(8)
+#define GPSR4_SD3_CLK		BIT(7)
+#define GPSR4_SD2_DS		BIT(6)
+#define GPSR4_SD2_DAT3		BIT(5)
+#define GPSR4_SD2_DAT2		BIT(4)
+#define GPSR4_SD2_DAT1		BIT(3)
+#define GPSR4_SD2_DAT0		BIT(2)
+#define GPSR4_SD2_CMD		BIT(1)
+#define GPSR4_SD2_CLK		BIT(0)
+#define GPSR5_MLB_DAT		BIT(25)
+#define GPSR5_MLB_SIG		BIT(24)
+#define GPSR5_MLB_CLK		BIT(23)
+#define GPSR5_MSIOF0_RXD	BIT(22)
+#define GPSR5_MSIOF0_SS2	BIT(21)
+#define GPSR5_MSIOF0_TXD	BIT(20)
+#define GPSR5_MSIOF0_SS1	BIT(19)
+#define GPSR5_MSIOF0_SYNC	BIT(18)
+#define GPSR5_MSIOF0_SCK	BIT(17)
+#define GPSR5_HRTS0		BIT(16)
+#define GPSR5_HCTS0		BIT(15)
+#define GPSR5_HTX0		BIT(14)
+#define GPSR5_HRX0		BIT(13)
+#define GPSR5_HSCK0		BIT(12)
+#define GPSR5_RX2_A		BIT(11)
+#define GPSR5_TX2_A		BIT(10)
+#define GPSR5_SCK2		BIT(9)
+#define GPSR5_RTS1		BIT(8)
+#define GPSR5_CTS1		BIT(7)
+#define GPSR5_TX1_A		BIT(6)
+#define GPSR5_RX1_A		BIT(5)
+#define GPSR5_RTS0		BIT(4)
+#define GPSR5_CTS0		BIT(3)
+#define GPSR5_TX0		BIT(2)
+#define GPSR5_RX0		BIT(1)
+#define GPSR5_SCK0		BIT(0)
+#define GPSR6_USB31_OVC		BIT(31)
+#define GPSR6_USB31_PWEN	BIT(30)
+#define GPSR6_USB30_OVC		BIT(29)
+#define GPSR6_USB30_PWEN	BIT(28)
+#define GPSR6_USB1_OVC		BIT(27)
+#define GPSR6_USB1_PWEN		BIT(26)
+#define GPSR6_USB0_OVC		BIT(25)
+#define GPSR6_USB0_PWEN		BIT(24)
+#define GPSR6_AUDIO_CLKB_B	BIT(23)
+#define GPSR6_AUDIO_CLKA_A	BIT(22)
+#define GPSR6_SSI_SDATA9_A	BIT(21)
+#define GPSR6_SSI_SDATA8	BIT(20)
+#define GPSR6_SSI_SDATA7	BIT(19)
+#define GPSR6_SSI_WS78		BIT(18)
+#define GPSR6_SSI_SCK78		BIT(17)
+#define GPSR6_SSI_SDATA6	BIT(16)
+#define GPSR6_SSI_WS6		BIT(15)
+#define GPSR6_SSI_SCK6		BIT(14)
+#define GPSR6_SSI_SDATA5	BIT(13)
+#define GPSR6_SSI_WS5		BIT(12)
+#define GPSR6_SSI_SCK5		BIT(11)
+#define GPSR6_SSI_SDATA4	BIT(10)
+#define GPSR6_SSI_WS4		BIT(9)
+#define GPSR6_SSI_SCK4		BIT(8)
+#define GPSR6_SSI_SDATA3	BIT(7)
+#define GPSR6_SSI_WS34		BIT(6)
+#define GPSR6_SSI_SCK34		BIT(5)
+#define GPSR6_SSI_SDATA2_A	BIT(4)
+#define GPSR6_SSI_SDATA1_A	BIT(3)
+#define GPSR6_SSI_SDATA0	BIT(2)
+#define GPSR6_SSI_WS0129	BIT(1)
+#define GPSR6_SSI_SCK0129	BIT(0)
+#define GPSR7_AVS2		BIT(1)
+#define GPSR7_AVS1		BIT(0)
+
+#define IPSR_28_FUNC(x)		((uint32_t)(x) << 28U)
+#define IPSR_24_FUNC(x)		((uint32_t)(x) << 24U)
+#define IPSR_20_FUNC(x)		((uint32_t)(x) << 20U)
+#define IPSR_16_FUNC(x)		((uint32_t)(x) << 16U)
+#define IPSR_12_FUNC(x)		((uint32_t)(x) << 12U)
+#define IPSR_8_FUNC(x)		((uint32_t)(x) << 8U)
+#define IPSR_4_FUNC(x)		((uint32_t)(x) << 4U)
+#define IPSR_0_FUNC(x)		((uint32_t)(x) << 0U)
+
+#define POC_SD3_DS_33V		BIT(29)
+#define POC_SD3_DAT7_33V	BIT(28)
+#define POC_SD3_DAT6_33V	BIT(27)
+#define POC_SD3_DAT5_33V	BIT(26)
+#define POC_SD3_DAT4_33V	BIT(25)
+#define POC_SD3_DAT3_33V	BIT(24)
+#define POC_SD3_DAT2_33V	BIT(23)
+#define POC_SD3_DAT1_33V	BIT(22)
+#define POC_SD3_DAT0_33V	BIT(21)
+#define POC_SD3_CMD_33V		BIT(20)
+#define POC_SD3_CLK_33V		BIT(19)
+#define POC_SD2_DS_33V		BIT(18)
+#define POC_SD2_DAT3_33V	BIT(17)
+#define POC_SD2_DAT2_33V	BIT(16)
+#define POC_SD2_DAT1_33V	BIT(15)
+#define POC_SD2_DAT0_33V	BIT(14)
+#define POC_SD2_CMD_33V		BIT(13)
+#define POC_SD2_CLK_33V		BIT(12)
+#define POC_SD1_DAT3_33V	BIT(11)
+#define POC_SD1_DAT2_33V	BIT(10)
+#define POC_SD1_DAT1_33V	BIT(9)
+#define POC_SD1_DAT0_33V	BIT(8)
+#define POC_SD1_CMD_33V		BIT(7)
+#define POC_SD1_CLK_33V		BIT(6)
+#define POC_SD0_DAT3_33V	BIT(5)
+#define POC_SD0_DAT2_33V	BIT(4)
+#define POC_SD0_DAT1_33V	BIT(3)
+#define POC_SD0_DAT0_33V	BIT(2)
+#define POC_SD0_CMD_33V		BIT(1)
+#define POC_SD0_CLK_33V		BIT(0)
+
+#define DRVCTRL0_MASK		(0xCCCCCCCCU)
+#define DRVCTRL1_MASK		(0xCCCCCCC8U)
+#define DRVCTRL2_MASK		(0x88888888U)
+#define DRVCTRL3_MASK		(0x88888888U)
+#define DRVCTRL4_MASK		(0x88888888U)
+#define DRVCTRL5_MASK		(0x88888888U)
+#define DRVCTRL6_MASK		(0x88888888U)
+#define DRVCTRL7_MASK		(0x88888888U)
+#define DRVCTRL8_MASK		(0x88888888U)
+#define DRVCTRL9_MASK		(0x88888888U)
+#define DRVCTRL10_MASK		(0x88888888U)
+#define DRVCTRL11_MASK		(0x888888CCU)
+#define DRVCTRL12_MASK		(0xCCCFFFCFU)
+#define DRVCTRL13_MASK		(0xCC888888U)
+#define DRVCTRL14_MASK		(0x88888888U)
+#define DRVCTRL15_MASK		(0x88888888U)
+#define DRVCTRL16_MASK		(0x88888888U)
+#define DRVCTRL17_MASK		(0x88888888U)
+#define DRVCTRL18_MASK		(0x88888888U)
+#define DRVCTRL19_MASK		(0x88888888U)
+#define DRVCTRL20_MASK		(0x88888888U)
+#define DRVCTRL21_MASK		(0x88888888U)
+#define DRVCTRL22_MASK		(0x88888888U)
+#define DRVCTRL23_MASK		(0x88888888U)
+#define DRVCTRL24_MASK		(0x8888888FU)
+
+#define DRVCTRL0_QSPI0_SPCLK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL0_QSPI0_MOSI_IO0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL0_QSPI0_MISO_IO1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL0_QSPI0_IO2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL0_QSPI0_IO3(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL0_QSPI0_SSL(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL0_QSPI1_SPCLK(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL0_QSPI1_MOSI_IO0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL1_QSPI1_MISO_IO1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL1_QSPI1_IO2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL1_QSPI1_IO3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL1_QSPI1_SS(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL1_RPC_INT(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL1_RPC_WP(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL1_RPC_RESET(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL1_AVB_RX_CTL(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL2_AVB_RXC(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL2_AVB_RD0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL2_AVB_RD1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL2_AVB_RD2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL2_AVB_RD3(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL2_AVB_TX_CTL(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL2_AVB_TXC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL2_AVB_TD0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL3_AVB_TD1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL3_AVB_TD2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL3_AVB_TD3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL3_AVB_TXCREFCLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL3_AVB_MDIO(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL3_AVB_MDC(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL3_AVB_MAGIC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL3_AVB_PHY_INT(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL4_AVB_LINK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL4_AVB_AVTP_MATCH(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL4_AVB_AVTP_CAPTURE(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL4_IRQ0(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL4_IRQ1(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL4_IRQ2(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL4_IRQ3(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL4_IRQ4(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL5_IRQ5(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL5_PWM0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL5_PWM1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL5_PWM2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL5_A0(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL5_A1(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL5_A2(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL5_A3(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL6_A4(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL6_A5(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL6_A6(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL6_A7(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL6_A8(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL6_A9(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL6_A10(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL6_A11(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL7_A12(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL7_A13(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL7_A14(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL7_A15(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL7_A16(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL7_A17(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL7_A18(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL7_A19(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL8_CLKOUT(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL8_CS0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL8_CS1_A2(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL8_BS(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL8_RD(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL8_RD_W(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL8_WE0(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL8_WE1(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL9_EX_WAIT0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL9_PRESETOU(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL9_D0(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL9_D1(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL9_D2(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL9_D3(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL9_D4(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL9_D5(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL10_D6(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL10_D7(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL10_D8(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL10_D9(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL10_D10(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL10_D11(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL10_D12(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL10_D13(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL11_D14(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL11_D15(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL11_AVS1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL11_AVS2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL11_GP7_02(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL11_GP7_03(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL11_DU_DOTCLKIN0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL11_DU_DOTCLKIN1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL12_DU_DOTCLKIN2(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL12_DU_DOTCLKIN3(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL12_DU_FSCLKST(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL12_DU_TMS(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL13_TDO(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL13_ASEBRK(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL13_SD0_CLK(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL13_SD0_CMD(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL13_SD0_DAT0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL13_SD0_DAT1(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL13_SD0_DAT2(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL13_SD0_DAT3(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL14_SD1_CLK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL14_SD1_CMD(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL14_SD1_DAT0(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL14_SD1_DAT1(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL14_SD1_DAT2(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL14_SD1_DAT3(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL14_SD2_CLK(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL14_SD2_CMD(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL15_SD2_DAT0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL15_SD2_DAT1(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL15_SD2_DAT2(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL15_SD2_DAT3(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL15_SD2_DS(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL15_SD3_CLK(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL15_SD3_CMD(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL15_SD3_DAT0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL16_SD3_DAT1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL16_SD3_DAT2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL16_SD3_DAT3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL16_SD3_DAT4(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL16_SD3_DAT5(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL16_SD3_DAT6(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL16_SD3_DAT7(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL16_SD3_DS(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL17_SD0_CD(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL17_SD0_WP(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL17_SD1_CD(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL17_SD1_WP(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL17_SCK0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL17_RX0(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL17_TX0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL17_CTS0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL18_RTS0_TANS(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL18_RX1(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL18_TX1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL18_CTS1(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL18_RTS1_TANS(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL18_SCK2(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL18_TX2(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL18_RX2(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL19_HSCK0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL19_HRX0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL19_HTX0(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL19_HCTS0(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL19_HRTS0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL19_MSIOF0_SCK(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL19_MSIOF0_SYNC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL19_MSIOF0_SS1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL20_MSIOF0_TXD(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL20_MSIOF0_SS2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL20_MSIOF0_RXD(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL20_MLB_CLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL20_MLB_SIG(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL20_MLB_DAT(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL20_MLB_REF(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL20_SSI_SCK0129(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL21_SSI_WS0129(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL21_SSI_SDATA0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL21_SSI_SDATA1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL21_SSI_SDATA2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL21_SSI_SCK34(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL21_SSI_WS34(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL21_SSI_SDATA3(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL21_SSI_SCK4(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL22_SSI_WS4(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL22_SSI_SDATA4(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL22_SSI_SCK5(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL22_SSI_WS5(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL22_SSI_SDATA5(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL22_SSI_SCK6(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL22_SSI_WS6(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL22_SSI_SDATA6(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL23_SSI_SCK78(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL23_SSI_WS78(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL23_SSI_SDATA7(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL23_SSI_SDATA8(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL23_SSI_SDATA9(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL23_AUDIO_CLKA(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL23_AUDIO_CLKB(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL23_USB0_PWEN(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL24_USB0_OVC(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL24_USB1_PWEN(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL24_USB1_OVC(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL24_USB30_PWEN(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL24_USB30_OVC(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL24_USB31_PWEN(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL24_USB31_OVC(x)	((uint32_t)(x) << 4U)
+
+#define MOD_SEL0_MSIOF3_A	((uint32_t)0U << 29U)
+#define MOD_SEL0_MSIOF3_B	((uint32_t)1U << 29U)
+#define MOD_SEL0_MSIOF3_C	((uint32_t)2U << 29U)
+#define MOD_SEL0_MSIOF3_D	((uint32_t)3U << 29U)
+#define MOD_SEL0_MSIOF3_E	((uint32_t)4U << 29U)
+#define MOD_SEL0_MSIOF2_A	((uint32_t)0U << 27U)
+#define MOD_SEL0_MSIOF2_B	((uint32_t)1U << 27U)
+#define MOD_SEL0_MSIOF2_C	((uint32_t)2U << 27U)
+#define MOD_SEL0_MSIOF2_D	((uint32_t)3U << 27U)
+#define MOD_SEL0_MSIOF1_A	((uint32_t)0U << 24U)
+#define MOD_SEL0_MSIOF1_B	((uint32_t)1U << 24U)
+#define MOD_SEL0_MSIOF1_C	((uint32_t)2U << 24U)
+#define MOD_SEL0_MSIOF1_D	((uint32_t)3U << 24U)
+#define MOD_SEL0_MSIOF1_E	((uint32_t)4U << 24U)
+#define MOD_SEL0_MSIOF1_F	((uint32_t)5U << 24U)
+#define MOD_SEL0_MSIOF1_G	((uint32_t)6U << 24U)
+#define MOD_SEL0_LBSC_A		((uint32_t)0U << 23U)
+#define MOD_SEL0_LBSC_B		((uint32_t)1U << 23U)
+#define MOD_SEL0_IEBUS_A	((uint32_t)0U << 22U)
+#define MOD_SEL0_IEBUS_B	((uint32_t)1U << 22U)
+#define MOD_SEL0_I2C2_A		((uint32_t)0U << 21U)
+#define MOD_SEL0_I2C2_B		((uint32_t)1U << 21U)
+#define MOD_SEL0_I2C1_A		((uint32_t)0U << 20U)
+#define MOD_SEL0_I2C1_B		((uint32_t)1U << 20U)
+#define MOD_SEL0_HSCIF4_A	((uint32_t)0U << 19U)
+#define MOD_SEL0_HSCIF4_B	((uint32_t)1U << 19U)
+#define MOD_SEL0_HSCIF3_A	((uint32_t)0U << 17U)
+#define MOD_SEL0_HSCIF3_B	((uint32_t)1U << 17U)
+#define MOD_SEL0_HSCIF3_C	((uint32_t)2U << 17U)
+#define MOD_SEL0_HSCIF3_D	((uint32_t)3U << 17U)
+#define MOD_SEL0_HSCIF1_A	((uint32_t)0U << 16U)
+#define MOD_SEL0_HSCIF1_B	((uint32_t)1U << 16U)
+#define MOD_SEL0_FSO_A		((uint32_t)0U << 15U)
+#define MOD_SEL0_FSO_B		((uint32_t)1U << 15U)
+#define MOD_SEL0_HSCIF2_A	((uint32_t)0U << 13U)
+#define MOD_SEL0_HSCIF2_B	((uint32_t)1U << 13U)
+#define MOD_SEL0_HSCIF2_C	((uint32_t)2U << 13U)
+#define MOD_SEL0_ETHERAVB_A	((uint32_t)0U << 12U)
+#define MOD_SEL0_ETHERAVB_B	((uint32_t)1U << 12U)
+#define MOD_SEL0_DRIF3_A	((uint32_t)0U << 11U)
+#define MOD_SEL0_DRIF3_B	((uint32_t)1U << 11U)
+#define MOD_SEL0_DRIF2_A	((uint32_t)0U << 10U)
+#define MOD_SEL0_DRIF2_B	((uint32_t)1U << 10U)
+#define MOD_SEL0_DRIF1_A	((uint32_t)0U << 8U)
+#define MOD_SEL0_DRIF1_B	((uint32_t)1U << 8U)
+#define MOD_SEL0_DRIF1_C	((uint32_t)2U << 8U)
+#define MOD_SEL0_DRIF0_A	((uint32_t)0U << 6U)
+#define MOD_SEL0_DRIF0_B	((uint32_t)1U << 6U)
+#define MOD_SEL0_DRIF0_C	((uint32_t)2U << 6U)
+#define MOD_SEL0_CANFD0_A	((uint32_t)0U << 5U)
+#define MOD_SEL0_CANFD0_B	((uint32_t)1U << 5U)
+#define MOD_SEL0_ADG_A_A	((uint32_t)0U << 3U)
+#define MOD_SEL0_ADG_A_B	((uint32_t)1U << 3U)
+#define MOD_SEL0_ADG_A_C	((uint32_t)2U << 3U)
+#define MOD_SEL1_TSIF1_A	((uint32_t)0U << 30U)
+#define MOD_SEL1_TSIF1_B	((uint32_t)1U << 30U)
+#define MOD_SEL1_TSIF1_C	((uint32_t)2U << 30U)
+#define MOD_SEL1_TSIF1_D	((uint32_t)3U << 30U)
+#define MOD_SEL1_TSIF0_A	((uint32_t)0U << 27U)
+#define MOD_SEL1_TSIF0_B	((uint32_t)1U << 27U)
+#define MOD_SEL1_TSIF0_C	((uint32_t)2U << 27U)
+#define MOD_SEL1_TSIF0_D	((uint32_t)3U << 27U)
+#define MOD_SEL1_TSIF0_E	((uint32_t)4U << 27U)
+#define MOD_SEL1_TIMER_TMU_A	((uint32_t)0U << 26U)
+#define MOD_SEL1_TIMER_TMU_B	((uint32_t)1U << 26U)
+#define MOD_SEL1_SSP1_1_A	((uint32_t)0U << 24U)
+#define MOD_SEL1_SSP1_1_B	((uint32_t)1U << 24U)
+#define MOD_SEL1_SSP1_1_C	((uint32_t)2U << 24U)
+#define MOD_SEL1_SSP1_1_D	((uint32_t)3U << 24U)
+#define MOD_SEL1_SSP1_0_A	((uint32_t)0U << 21U)
+#define MOD_SEL1_SSP1_0_B	((uint32_t)1U << 21U)
+#define MOD_SEL1_SSP1_0_C	((uint32_t)2U << 21U)
+#define MOD_SEL1_SSP1_0_D	((uint32_t)3U << 21U)
+#define MOD_SEL1_SSP1_0_E	((uint32_t)4U << 21U)
+#define MOD_SEL1_SSI_A		((uint32_t)0U << 20U)
+#define MOD_SEL1_SSI_B		((uint32_t)1U << 20U)
+#define MOD_SEL1_SPEED_PULSE_IF_A	((uint32_t)0U << 19U)
+#define MOD_SEL1_SPEED_PULSE_IF_B	((uint32_t)1U << 19U)
+#define MOD_SEL1_SIMCARD_A	((uint32_t)0U << 17U)
+#define MOD_SEL1_SIMCARD_B	((uint32_t)1U << 17U)
+#define MOD_SEL1_SIMCARD_C	((uint32_t)2U << 17U)
+#define MOD_SEL1_SIMCARD_D	((uint32_t)3U << 17U)
+#define MOD_SEL1_SDHI2_A	((uint32_t)0U << 16U)
+#define MOD_SEL1_SDHI2_B	((uint32_t)1U << 16U)
+#define MOD_SEL1_SCIF4_A	((uint32_t)0U << 14U)
+#define MOD_SEL1_SCIF4_B	((uint32_t)1U << 14U)
+#define MOD_SEL1_SCIF4_C	((uint32_t)2U << 14U)
+#define MOD_SEL1_SCIF3_A	((uint32_t)0U << 13U)
+#define MOD_SEL1_SCIF3_B	((uint32_t)1U << 13U)
+#define MOD_SEL1_SCIF2_A	((uint32_t)0U << 12U)
+#define MOD_SEL1_SCIF2_B	((uint32_t)1U << 12U)
+#define MOD_SEL1_SCIF1_A	((uint32_t)0U << 11U)
+#define MOD_SEL1_SCIF1_B	((uint32_t)1U << 11U)
+#define MOD_SEL1_SCIF_A		((uint32_t)0U << 10U)
+#define MOD_SEL1_SCIF_B		((uint32_t)1U << 10U)
+#define MOD_SEL1_REMOCON_A	((uint32_t)0U << 9U)
+#define MOD_SEL1_REMOCON_B	((uint32_t)1U << 9U)
+#define MOD_SEL1_RCAN0_A	((uint32_t)0U << 6U)
+#define MOD_SEL1_RCAN0_B	((uint32_t)1U << 6U)
+#define MOD_SEL1_PWM6_A		((uint32_t)0U << 5U)
+#define MOD_SEL1_PWM6_B		((uint32_t)1U << 5U)
+#define MOD_SEL1_PWM5_A		((uint32_t)0U << 4U)
+#define MOD_SEL1_PWM5_B		((uint32_t)1U << 4U)
+#define MOD_SEL1_PWM4_A		((uint32_t)0U << 3U)
+#define MOD_SEL1_PWM4_B		((uint32_t)1U << 3U)
+#define MOD_SEL1_PWM3_A		((uint32_t)0U << 2U)
+#define MOD_SEL1_PWM3_B		((uint32_t)1U << 2U)
+#define MOD_SEL1_PWM2_A		((uint32_t)0U << 1U)
+#define MOD_SEL1_PWM2_B		((uint32_t)1U << 1U)
+#define MOD_SEL1_PWM1_A		((uint32_t)0U << 0U)
+#define MOD_SEL1_PWM1_B		((uint32_t)1U << 0U)
+#define MOD_SEL2_I2C_5_A	((uint32_t)0U << 31U)
+#define MOD_SEL2_I2C_5_B	((uint32_t)1U << 31U)
+#define MOD_SEL2_I2C_3_A	((uint32_t)0U << 30U)
+#define MOD_SEL2_I2C_3_B	((uint32_t)1U << 30U)
+#define MOD_SEL2_I2C_0_A	((uint32_t)0U << 29U)
+#define MOD_SEL2_I2C_0_B	((uint32_t)1U << 29U)
+#define MOD_SEL2_FM_A		((uint32_t)0U << 27U)
+#define MOD_SEL2_FM_B		((uint32_t)1U << 27U)
+#define MOD_SEL2_FM_C		((uint32_t)2U << 27U)
+#define MOD_SEL2_FM_D		((uint32_t)3U << 27U)
+#define MOD_SEL2_SCIF5_A	((uint32_t)0U << 26U)
+#define MOD_SEL2_SCIF5_B	((uint32_t)1U << 26U)
+#define MOD_SEL2_I2C6_A		((uint32_t)0U << 23U)
+#define MOD_SEL2_I2C6_B		((uint32_t)1U << 23U)
+#define MOD_SEL2_I2C6_C		((uint32_t)2U << 23U)
+#define MOD_SEL2_NDF_A		((uint32_t)0U << 22U)
+#define MOD_SEL2_NDF_B		((uint32_t)1U << 22U)
+#define MOD_SEL2_SSI2_A		((uint32_t)0U << 21U)
+#define MOD_SEL2_SSI2_B		((uint32_t)1U << 21U)
+#define MOD_SEL2_SSI9_A		((uint32_t)0U << 20U)
+#define MOD_SEL2_SSI9_B		((uint32_t)1U << 20U)
+#define MOD_SEL2_TIMER_TMU2_A	((uint32_t)0U << 19U)
+#define MOD_SEL2_TIMER_TMU2_B	((uint32_t)1U << 19U)
+#define MOD_SEL2_ADG_B_A	((uint32_t)0U << 18U)
+#define MOD_SEL2_ADG_B_B	((uint32_t)1U << 18U)
+#define MOD_SEL2_ADG_C_A	((uint32_t)0U << 17U)
+#define MOD_SEL2_ADG_C_B	((uint32_t)1U << 17U)
+#define MOD_SEL2_VIN4_A		((uint32_t)0U << 0U)
+#define MOD_SEL2_VIN4_B		((uint32_t)1U << 0U)
+
+static void pfc_reg_write(uint32_t addr, uint32_t data)
+{
+	mmio_write_32(PFC_PMMR, ~data);
+	mmio_write_32((uintptr_t)addr, data);
+}
+
+void pfc_init_m3n(void)
+{
+	uint32_t reg;
+
+	/* initialize module select */
+	pfc_reg_write(PFC_MOD_SEL0, MOD_SEL0_MSIOF3_A
+		      | MOD_SEL0_MSIOF2_A
+		      | MOD_SEL0_MSIOF1_A
+		      | MOD_SEL0_LBSC_A
+		      | MOD_SEL0_IEBUS_A
+		      | MOD_SEL0_I2C2_A
+		      | MOD_SEL0_I2C1_A
+		      | MOD_SEL0_HSCIF4_A
+		      | MOD_SEL0_HSCIF3_A
+		      | MOD_SEL0_HSCIF1_A
+		      | MOD_SEL0_FSO_A
+		      | MOD_SEL0_HSCIF2_A
+		      | MOD_SEL0_ETHERAVB_A
+		      | MOD_SEL0_DRIF3_A
+		      | MOD_SEL0_DRIF2_A
+		      | MOD_SEL0_DRIF1_A
+		      | MOD_SEL0_DRIF0_A
+		      | MOD_SEL0_CANFD0_A
+		      | MOD_SEL0_ADG_A_A);
+	pfc_reg_write(PFC_MOD_SEL1, MOD_SEL1_TSIF1_A
+		      | MOD_SEL1_TSIF0_A
+		      | MOD_SEL1_TIMER_TMU_A
+		      | MOD_SEL1_SSP1_1_A
+		      | MOD_SEL1_SSP1_0_A
+		      | MOD_SEL1_SSI_A
+		      | MOD_SEL1_SPEED_PULSE_IF_A
+		      | MOD_SEL1_SIMCARD_A
+		      | MOD_SEL1_SDHI2_A
+		      | MOD_SEL1_SCIF4_A
+		      | MOD_SEL1_SCIF3_A
+		      | MOD_SEL1_SCIF2_A
+		      | MOD_SEL1_SCIF1_A
+		      | MOD_SEL1_SCIF_A
+		      | MOD_SEL1_REMOCON_A
+		      | MOD_SEL1_RCAN0_A
+		      | MOD_SEL1_PWM6_A
+		      | MOD_SEL1_PWM5_A
+		      | MOD_SEL1_PWM4_A
+		      | MOD_SEL1_PWM3_A
+		      | MOD_SEL1_PWM2_A
+		      | MOD_SEL1_PWM1_A);
+	pfc_reg_write(PFC_MOD_SEL2, MOD_SEL2_I2C_5_A
+		      | MOD_SEL2_I2C_3_A
+		      | MOD_SEL2_I2C_0_A
+		      | MOD_SEL2_FM_A
+		      | MOD_SEL2_SCIF5_A
+		      | MOD_SEL2_I2C6_A
+		      | MOD_SEL2_NDF_A
+		      | MOD_SEL2_SSI2_A
+		      | MOD_SEL2_SSI9_A
+		      | MOD_SEL2_TIMER_TMU2_A
+		      | MOD_SEL2_ADG_B_A
+		      | MOD_SEL2_ADG_C_A
+		      | MOD_SEL2_VIN4_A);
+
+	/* initialize peripheral function select */
+	pfc_reg_write(PFC_IPSR0, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR1, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(3)
+		      | IPSR_8_FUNC(3)
+		      | IPSR_4_FUNC(3)
+		      | IPSR_0_FUNC(3));
+	pfc_reg_write(PFC_IPSR2, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR3, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR4, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR5, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR6, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR7, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR8, IPSR_28_FUNC(1)
+		      | IPSR_24_FUNC(1)
+		      | IPSR_20_FUNC(1)
+		      | IPSR_16_FUNC(1)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR9, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR10, IPSR_28_FUNC(1)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR11, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(4)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(1));
+	pfc_reg_write(PFC_IPSR12, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(4)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR13, IPSR_28_FUNC(8)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(3)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR14, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(3)
+		      | IPSR_0_FUNC(8));
+	pfc_reg_write(PFC_IPSR15, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR16, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR17, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(1)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR18, IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+
+	/* initialize GPIO/perihperal function select */
+	pfc_reg_write(PFC_GPSR0, GPSR0_D15
+		      | GPSR0_D14
+		      | GPSR0_D13
+		      | GPSR0_D12
+		      | GPSR0_D11
+		      | GPSR0_D10
+		      | GPSR0_D9
+		      | GPSR0_D8);
+	pfc_reg_write(PFC_GPSR1, GPSR1_CLKOUT
+		      | GPSR1_EX_WAIT0_A
+		      | GPSR1_A19
+		      | GPSR1_A18
+		      | GPSR1_A17
+		      | GPSR1_A16
+		      | GPSR1_A15
+		      | GPSR1_A14
+		      | GPSR1_A13
+		      | GPSR1_A12
+		      | GPSR1_A7
+		      | GPSR1_A6
+		      | GPSR1_A5
+		      | GPSR1_A4
+		      | GPSR1_A3
+		      | GPSR1_A2
+		      | GPSR1_A1
+		      | GPSR1_A0);
+	pfc_reg_write(PFC_GPSR2, GPSR2_AVB_AVTP_CAPTURE_A
+		      | GPSR2_AVB_AVTP_MATCH_A
+		      | GPSR2_AVB_LINK
+		      | GPSR2_AVB_PHY_INT
+		      | GPSR2_AVB_MDC
+		      | GPSR2_PWM2_A
+		      | GPSR2_PWM1_A
+		      | GPSR2_IRQ5
+		      | GPSR2_IRQ4
+		      | GPSR2_IRQ3
+		      | GPSR2_IRQ2
+		      | GPSR2_IRQ1
+		      | GPSR2_IRQ0);
+	pfc_reg_write(PFC_GPSR3, GPSR3_SD0_WP
+		      | GPSR3_SD0_CD
+		      | GPSR3_SD1_DAT3
+		      | GPSR3_SD1_DAT2
+		      | GPSR3_SD1_DAT1
+		      | GPSR3_SD1_DAT0
+		      | GPSR3_SD0_DAT3
+		      | GPSR3_SD0_DAT2
+		      | GPSR3_SD0_DAT1
+		      | GPSR3_SD0_DAT0
+		      | GPSR3_SD0_CMD
+		      | GPSR3_SD0_CLK);
+	pfc_reg_write(PFC_GPSR4, GPSR4_SD3_DAT7
+		      | GPSR4_SD3_DAT6
+		      | GPSR4_SD3_DAT3
+		      | GPSR4_SD3_DAT2
+		      | GPSR4_SD3_DAT1
+		      | GPSR4_SD3_DAT0
+		      | GPSR4_SD3_CMD
+		      | GPSR4_SD3_CLK
+		      | GPSR4_SD2_DS
+		      | GPSR4_SD2_DAT3
+		      | GPSR4_SD2_DAT2
+		      | GPSR4_SD2_DAT1
+		      | GPSR4_SD2_DAT0
+		      | GPSR4_SD2_CMD
+		      | GPSR4_SD2_CLK);
+	pfc_reg_write(PFC_GPSR5, GPSR5_MSIOF0_SS2
+		      | GPSR5_MSIOF0_SS1
+		      | GPSR5_MSIOF0_SYNC
+		      | GPSR5_HRTS0
+		      | GPSR5_HCTS0
+		      | GPSR5_HTX0
+		      | GPSR5_HRX0
+		      | GPSR5_HSCK0
+		      | GPSR5_RX2_A
+		      | GPSR5_TX2_A
+		      | GPSR5_SCK2
+		      | GPSR5_RTS1
+		      | GPSR5_CTS1
+		      | GPSR5_TX1_A
+		      | GPSR5_RX1_A
+		      | GPSR5_RTS0
+		      | GPSR5_SCK0);
+	pfc_reg_write(PFC_GPSR6, GPSR6_USB30_OVC
+		      | GPSR6_USB30_PWEN
+		      | GPSR6_USB1_OVC
+		      | GPSR6_USB1_PWEN
+		      | GPSR6_USB0_OVC
+		      | GPSR6_USB0_PWEN
+		      | GPSR6_AUDIO_CLKB_B
+		      | GPSR6_AUDIO_CLKA_A
+		      | GPSR6_SSI_SDATA8
+		      | GPSR6_SSI_SDATA7
+		      | GPSR6_SSI_WS78
+		      | GPSR6_SSI_SCK78
+		      | GPSR6_SSI_WS6
+		      | GPSR6_SSI_SCK6
+		      | GPSR6_SSI_SDATA4
+		      | GPSR6_SSI_WS4
+		      | GPSR6_SSI_SCK4
+		      | GPSR6_SSI_SDATA1_A
+		      | GPSR6_SSI_SDATA0
+		      | GPSR6_SSI_WS0129
+		      | GPSR6_SSI_SCK0129);
+	pfc_reg_write(PFC_GPSR7, GPSR7_AVS2
+		      | GPSR7_AVS1);
+
+	/* initialize POC control register */
+	pfc_reg_write(PFC_POCCTRL0, POC_SD3_DS_33V
+		      | POC_SD3_DAT7_33V
+		      | POC_SD3_DAT6_33V
+		      | POC_SD3_DAT5_33V
+		      | POC_SD3_DAT4_33V
+		      | POC_SD3_DAT3_33V
+		      | POC_SD3_DAT2_33V
+		      | POC_SD3_DAT1_33V
+		      | POC_SD3_DAT0_33V
+		      | POC_SD3_CMD_33V
+		      | POC_SD3_CLK_33V
+		      | POC_SD0_DAT3_33V
+		      | POC_SD0_DAT2_33V
+		      | POC_SD0_DAT1_33V
+		      | POC_SD0_DAT0_33V
+		      | POC_SD0_CMD_33V
+		      | POC_SD0_CLK_33V);
+
+	/* initialize DRV control register */
+	reg = mmio_read_32(PFC_DRVCTRL0);
+	reg = ((reg & DRVCTRL0_MASK) | DRVCTRL0_QSPI0_SPCLK(3)
+	       | DRVCTRL0_QSPI0_MOSI_IO0(3)
+	       | DRVCTRL0_QSPI0_MISO_IO1(3)
+	       | DRVCTRL0_QSPI0_IO2(3)
+	       | DRVCTRL0_QSPI0_IO3(3)
+	       | DRVCTRL0_QSPI0_SSL(3)
+	       | DRVCTRL0_QSPI1_SPCLK(3)
+	       | DRVCTRL0_QSPI1_MOSI_IO0(3));
+	pfc_reg_write(PFC_DRVCTRL0, reg);
+	reg = mmio_read_32(PFC_DRVCTRL1);
+	reg = ((reg & DRVCTRL1_MASK) | DRVCTRL1_QSPI1_MISO_IO1(3)
+	       | DRVCTRL1_QSPI1_IO2(3)
+	       | DRVCTRL1_QSPI1_IO3(3)
+	       | DRVCTRL1_QSPI1_SS(3)
+	       | DRVCTRL1_RPC_INT(3)
+	       | DRVCTRL1_RPC_WP(3)
+	       | DRVCTRL1_RPC_RESET(3)
+	       | DRVCTRL1_AVB_RX_CTL(7));
+	pfc_reg_write(PFC_DRVCTRL1, reg);
+	reg = mmio_read_32(PFC_DRVCTRL2);
+	reg = ((reg & DRVCTRL2_MASK) | DRVCTRL2_AVB_RXC(7)
+	       | DRVCTRL2_AVB_RD0(7)
+	       | DRVCTRL2_AVB_RD1(7)
+	       | DRVCTRL2_AVB_RD2(7)
+	       | DRVCTRL2_AVB_RD3(7)
+	       | DRVCTRL2_AVB_TX_CTL(3)
+	       | DRVCTRL2_AVB_TXC(3)
+	       | DRVCTRL2_AVB_TD0(3));
+	pfc_reg_write(PFC_DRVCTRL2, reg);
+	reg = mmio_read_32(PFC_DRVCTRL3);
+	reg = ((reg & DRVCTRL3_MASK) | DRVCTRL3_AVB_TD1(3)
+	       | DRVCTRL3_AVB_TD2(3)
+	       | DRVCTRL3_AVB_TD3(3)
+	       | DRVCTRL3_AVB_TXCREFCLK(7)
+	       | DRVCTRL3_AVB_MDIO(7)
+	       | DRVCTRL3_AVB_MDC(7)
+	       | DRVCTRL3_AVB_MAGIC(7)
+	       | DRVCTRL3_AVB_PHY_INT(7));
+	pfc_reg_write(PFC_DRVCTRL3, reg);
+	reg = mmio_read_32(PFC_DRVCTRL4);
+	reg = ((reg & DRVCTRL4_MASK) | DRVCTRL4_AVB_LINK(7)
+	       | DRVCTRL4_AVB_AVTP_MATCH(7)
+	       | DRVCTRL4_AVB_AVTP_CAPTURE(7)
+	       | DRVCTRL4_IRQ0(7)
+	       | DRVCTRL4_IRQ1(7)
+	       | DRVCTRL4_IRQ2(7)
+	       | DRVCTRL4_IRQ3(7)
+	       | DRVCTRL4_IRQ4(7));
+	pfc_reg_write(PFC_DRVCTRL4, reg);
+	reg = mmio_read_32(PFC_DRVCTRL5);
+	reg = ((reg & DRVCTRL5_MASK) | DRVCTRL5_IRQ5(7)
+	       | DRVCTRL5_PWM0(7)
+	       | DRVCTRL5_PWM1(7)
+	       | DRVCTRL5_PWM2(7)
+	       | DRVCTRL5_A0(3)
+	       | DRVCTRL5_A1(3)
+	       | DRVCTRL5_A2(3)
+	       | DRVCTRL5_A3(3));
+	pfc_reg_write(PFC_DRVCTRL5, reg);
+	reg = mmio_read_32(PFC_DRVCTRL6);
+	reg = ((reg & DRVCTRL6_MASK) | DRVCTRL6_A4(3)
+	       | DRVCTRL6_A5(3)
+	       | DRVCTRL6_A6(3)
+	       | DRVCTRL6_A7(3)
+	       | DRVCTRL6_A8(7)
+	       | DRVCTRL6_A9(7)
+	       | DRVCTRL6_A10(7)
+	       | DRVCTRL6_A11(7));
+	pfc_reg_write(PFC_DRVCTRL6, reg);
+	reg = mmio_read_32(PFC_DRVCTRL7);
+	reg = ((reg & DRVCTRL7_MASK) | DRVCTRL7_A12(3)
+	       | DRVCTRL7_A13(3)
+	       | DRVCTRL7_A14(3)
+	       | DRVCTRL7_A15(3)
+	       | DRVCTRL7_A16(3)
+	       | DRVCTRL7_A17(3)
+	       | DRVCTRL7_A18(3)
+	       | DRVCTRL7_A19(3));
+	pfc_reg_write(PFC_DRVCTRL7, reg);
+	reg = mmio_read_32(PFC_DRVCTRL8);
+	reg = ((reg & DRVCTRL8_MASK) | DRVCTRL8_CLKOUT(7)
+	       | DRVCTRL8_CS0(7)
+	       | DRVCTRL8_CS1_A2(7)
+	       | DRVCTRL8_BS(7)
+	       | DRVCTRL8_RD(7)
+	       | DRVCTRL8_RD_W(7)
+	       | DRVCTRL8_WE0(7)
+	       | DRVCTRL8_WE1(7));
+	pfc_reg_write(PFC_DRVCTRL8, reg);
+	reg = mmio_read_32(PFC_DRVCTRL9);
+	reg = ((reg & DRVCTRL9_MASK) | DRVCTRL9_EX_WAIT0(7)
+	       | DRVCTRL9_PRESETOU(7)
+	       | DRVCTRL9_D0(7)
+	       | DRVCTRL9_D1(7)
+	       | DRVCTRL9_D2(7)
+	       | DRVCTRL9_D3(7)
+	       | DRVCTRL9_D4(7)
+	       | DRVCTRL9_D5(7));
+	pfc_reg_write(PFC_DRVCTRL9, reg);
+	reg = mmio_read_32(PFC_DRVCTRL10);
+	reg = ((reg & DRVCTRL10_MASK) | DRVCTRL10_D6(7)
+	       | DRVCTRL10_D7(7)
+	       | DRVCTRL10_D8(3)
+	       | DRVCTRL10_D9(3)
+	       | DRVCTRL10_D10(3)
+	       | DRVCTRL10_D11(3)
+	       | DRVCTRL10_D12(3)
+	       | DRVCTRL10_D13(3));
+	pfc_reg_write(PFC_DRVCTRL10, reg);
+	reg = mmio_read_32(PFC_DRVCTRL11);
+	reg = ((reg & DRVCTRL11_MASK) | DRVCTRL11_D14(3)
+	       | DRVCTRL11_D15(3)
+	       | DRVCTRL11_AVS1(7)
+	       | DRVCTRL11_AVS2(7)
+	       | DRVCTRL11_GP7_02(7)
+	       | DRVCTRL11_GP7_03(7)
+	       | DRVCTRL11_DU_DOTCLKIN0(3)
+	       | DRVCTRL11_DU_DOTCLKIN1(3));
+	pfc_reg_write(PFC_DRVCTRL11, reg);
+	reg = mmio_read_32(PFC_DRVCTRL12);
+	reg = ((reg & DRVCTRL12_MASK) | DRVCTRL12_DU_DOTCLKIN2(3)
+	       | DRVCTRL12_DU_DOTCLKIN3(3)
+	       | DRVCTRL12_DU_FSCLKST(3)
+	       | DRVCTRL12_DU_TMS(3));
+	pfc_reg_write(PFC_DRVCTRL12, reg);
+	reg = mmio_read_32(PFC_DRVCTRL13);
+	reg = ((reg & DRVCTRL13_MASK) | DRVCTRL13_TDO(3)
+	       | DRVCTRL13_ASEBRK(3)
+	       | DRVCTRL13_SD0_CLK(7)
+	       | DRVCTRL13_SD0_CMD(7)
+	       | DRVCTRL13_SD0_DAT0(7)
+	       | DRVCTRL13_SD0_DAT1(7)
+	       | DRVCTRL13_SD0_DAT2(7)
+	       | DRVCTRL13_SD0_DAT3(7));
+	pfc_reg_write(PFC_DRVCTRL13, reg);
+	reg = mmio_read_32(PFC_DRVCTRL14);
+	reg = ((reg & DRVCTRL14_MASK) | DRVCTRL14_SD1_CLK(7)
+	       | DRVCTRL14_SD1_CMD(7)
+	       | DRVCTRL14_SD1_DAT0(5)
+	       | DRVCTRL14_SD1_DAT1(5)
+	       | DRVCTRL14_SD1_DAT2(5)
+	       | DRVCTRL14_SD1_DAT3(5)
+	       | DRVCTRL14_SD2_CLK(5)
+	       | DRVCTRL14_SD2_CMD(5));
+	pfc_reg_write(PFC_DRVCTRL14, reg);
+	reg = mmio_read_32(PFC_DRVCTRL15);
+	reg = ((reg & DRVCTRL15_MASK) | DRVCTRL15_SD2_DAT0(5)
+	       | DRVCTRL15_SD2_DAT1(5)
+	       | DRVCTRL15_SD2_DAT2(5)
+	       | DRVCTRL15_SD2_DAT3(5)
+	       | DRVCTRL15_SD2_DS(5)
+	       | DRVCTRL15_SD3_CLK(7)
+	       | DRVCTRL15_SD3_CMD(7)
+	       | DRVCTRL15_SD3_DAT0(7));
+	pfc_reg_write(PFC_DRVCTRL15, reg);
+	reg = mmio_read_32(PFC_DRVCTRL16);
+	reg = ((reg & DRVCTRL16_MASK) | DRVCTRL16_SD3_DAT1(7)
+	       | DRVCTRL16_SD3_DAT2(7)
+	       | DRVCTRL16_SD3_DAT3(7)
+	       | DRVCTRL16_SD3_DAT4(7)
+	       | DRVCTRL16_SD3_DAT5(7)
+	       | DRVCTRL16_SD3_DAT6(7)
+	       | DRVCTRL16_SD3_DAT7(7)
+	       | DRVCTRL16_SD3_DS(7));
+	pfc_reg_write(PFC_DRVCTRL16, reg);
+	reg = mmio_read_32(PFC_DRVCTRL17);
+	reg = ((reg & DRVCTRL17_MASK) | DRVCTRL17_SD0_CD(7)
+	       | DRVCTRL17_SD0_WP(7)
+	       | DRVCTRL17_SD1_CD(7)
+	       | DRVCTRL17_SD1_WP(7)
+	       | DRVCTRL17_SCK0(7)
+	       | DRVCTRL17_RX0(7)
+	       | DRVCTRL17_TX0(7)
+	       | DRVCTRL17_CTS0(7));
+	pfc_reg_write(PFC_DRVCTRL17, reg);
+	reg = mmio_read_32(PFC_DRVCTRL18);
+	reg = ((reg & DRVCTRL18_MASK) | DRVCTRL18_RTS0_TANS(7)
+	       | DRVCTRL18_RX1(7)
+	       | DRVCTRL18_TX1(7)
+	       | DRVCTRL18_CTS1(7)
+	       | DRVCTRL18_RTS1_TANS(7)
+	       | DRVCTRL18_SCK2(7)
+	       | DRVCTRL18_TX2(7)
+	       | DRVCTRL18_RX2(7));
+	pfc_reg_write(PFC_DRVCTRL18, reg);
+	reg = mmio_read_32(PFC_DRVCTRL19);
+	reg = ((reg & DRVCTRL19_MASK) | DRVCTRL19_HSCK0(7)
+	       | DRVCTRL19_HRX0(7)
+	       | DRVCTRL19_HTX0(7)
+	       | DRVCTRL19_HCTS0(7)
+	       | DRVCTRL19_HRTS0(7)
+	       | DRVCTRL19_MSIOF0_SCK(7)
+	       | DRVCTRL19_MSIOF0_SYNC(7)
+	       | DRVCTRL19_MSIOF0_SS1(7));
+	pfc_reg_write(PFC_DRVCTRL19, reg);
+	reg = mmio_read_32(PFC_DRVCTRL20);
+	reg = ((reg & DRVCTRL20_MASK) | DRVCTRL20_MSIOF0_TXD(7)
+	       | DRVCTRL20_MSIOF0_SS2(7)
+	       | DRVCTRL20_MSIOF0_RXD(7)
+	       | DRVCTRL20_MLB_CLK(7)
+	       | DRVCTRL20_MLB_SIG(7)
+	       | DRVCTRL20_MLB_DAT(7)
+	       | DRVCTRL20_MLB_REF(7)
+	       | DRVCTRL20_SSI_SCK0129(7));
+	pfc_reg_write(PFC_DRVCTRL20, reg);
+	reg = mmio_read_32(PFC_DRVCTRL21);
+	reg = ((reg & DRVCTRL21_MASK) | DRVCTRL21_SSI_WS0129(7)
+	       | DRVCTRL21_SSI_SDATA0(7)
+	       | DRVCTRL21_SSI_SDATA1(7)
+	       | DRVCTRL21_SSI_SDATA2(7)
+	       | DRVCTRL21_SSI_SCK34(7)
+	       | DRVCTRL21_SSI_WS34(7)
+	       | DRVCTRL21_SSI_SDATA3(7)
+	       | DRVCTRL21_SSI_SCK4(7));
+	pfc_reg_write(PFC_DRVCTRL21, reg);
+	reg = mmio_read_32(PFC_DRVCTRL22);
+	reg = ((reg & DRVCTRL22_MASK) | DRVCTRL22_SSI_WS4(7)
+	       | DRVCTRL22_SSI_SDATA4(7)
+	       | DRVCTRL22_SSI_SCK5(7)
+	       | DRVCTRL22_SSI_WS5(7)
+	       | DRVCTRL22_SSI_SDATA5(7)
+	       | DRVCTRL22_SSI_SCK6(7)
+	       | DRVCTRL22_SSI_WS6(7)
+	       | DRVCTRL22_SSI_SDATA6(7));
+	pfc_reg_write(PFC_DRVCTRL22, reg);
+	reg = mmio_read_32(PFC_DRVCTRL23);
+	reg = ((reg & DRVCTRL23_MASK) | DRVCTRL23_SSI_SCK78(7)
+	       | DRVCTRL23_SSI_WS78(7)
+	       | DRVCTRL23_SSI_SDATA7(7)
+	       | DRVCTRL23_SSI_SDATA8(7)
+	       | DRVCTRL23_SSI_SDATA9(7)
+	       | DRVCTRL23_AUDIO_CLKA(7)
+	       | DRVCTRL23_AUDIO_CLKB(7)
+	       | DRVCTRL23_USB0_PWEN(7));
+	pfc_reg_write(PFC_DRVCTRL23, reg);
+	reg = mmio_read_32(PFC_DRVCTRL24);
+	reg = ((reg & DRVCTRL24_MASK) | DRVCTRL24_USB0_OVC(7)
+	       | DRVCTRL24_USB1_PWEN(7)
+	       | DRVCTRL24_USB1_OVC(7)
+	       | DRVCTRL24_USB30_PWEN(7)
+	       | DRVCTRL24_USB30_OVC(7)
+	       | DRVCTRL24_USB31_PWEN(7)
+	       | DRVCTRL24_USB31_OVC(7));
+	pfc_reg_write(PFC_DRVCTRL24, reg);
+
+	/* initialize LSI pin pull-up/down control */
+	pfc_reg_write(PFC_PUD0, 0x00005FBFU);
+	pfc_reg_write(PFC_PUD1, 0x00300FFEU);
+	pfc_reg_write(PFC_PUD2, 0x330001E6U);
+	pfc_reg_write(PFC_PUD3, 0x000002E0U);
+	pfc_reg_write(PFC_PUD4, 0xFFFFFF00U);
+	pfc_reg_write(PFC_PUD5, 0x7F5FFF87U);
+	pfc_reg_write(PFC_PUD6, 0x00000055U);
+
+	/* initialize LSI pin pull-enable register */
+	pfc_reg_write(PFC_PUEN0, 0x00000FFFU);
+	pfc_reg_write(PFC_PUEN1, 0x00100234U);
+	pfc_reg_write(PFC_PUEN2, 0x000004C4U);
+	pfc_reg_write(PFC_PUEN3, 0x00000200U);
+	pfc_reg_write(PFC_PUEN4, 0x3E000000U);
+	pfc_reg_write(PFC_PUEN5, 0x1F000805U);
+	pfc_reg_write(PFC_PUEN6, 0x00000006U);
+
+	/* initialize positive/negative logic select */
+	mmio_write_32(GPIO_POSNEG0, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG1, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG2, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG3, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG4, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG5, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG6, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG7, 0x00000000U);
+
+	/* initialize general IO/interrupt switching */
+	mmio_write_32(GPIO_IOINTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL5, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL6, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL7, 0x00000000U);
+
+	/* initialize general output register */
+	mmio_write_32(GPIO_OUTDT1, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT2, 0x00000400U);
+	mmio_write_32(GPIO_OUTDT3, 0x0000C000U);
+	mmio_write_32(GPIO_OUTDT5, 0x00000006U);
+	mmio_write_32(GPIO_OUTDT6, 0x00003880U);
+
+	/* initialize general input/output switching */
+	mmio_write_32(GPIO_INOUTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL1, 0x01000A00U);
+	mmio_write_32(GPIO_INOUTSEL2, 0x00000400U);
+	mmio_write_32(GPIO_INOUTSEL3, 0x0000C000U);
+	mmio_write_32(GPIO_INOUTSEL4, 0x00000000U);
+#if (RCAR_GEN3_ULCB == 1)
+	mmio_write_32(GPIO_INOUTSEL5, 0x0000000EU);
+#else
+	mmio_write_32(GPIO_INOUTSEL5, 0x0000020EU);
+#endif
+	mmio_write_32(GPIO_INOUTSEL6, 0x00013880U);
+	mmio_write_32(GPIO_INOUTSEL7, 0x00000000U);
+}
diff --git a/drivers/renesas/rcar/pfc/M3N/pfc_init_m3n.h b/drivers/renesas/rcar/pfc/M3N/pfc_init_m3n.h
new file mode 100644
index 0000000..3e6f879
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/M3N/pfc_init_m3n.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PFC_INIT_M3N_H
+#define PFC_INIT_M3N_H
+
+void pfc_init_m3n(void);
+
+#endif /* PFC_INIT_M3N_H */
diff --git a/drivers/renesas/rcar/pfc/V3M/pfc_init_v3m.c b/drivers/renesas/rcar/pfc/V3M/pfc_init_v3m.c
new file mode 100644
index 0000000..6063758
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/V3M/pfc_init_v3m.c
@@ -0,0 +1,906 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>		/* for uint32_t */
+#include <lib/mmio.h>
+#include "pfc_init_v3m.h"
+#include "include/rcar_def.h"
+#include "rcar_private.h"
+#include "../pfc_regs.h"
+
+/* Pin functon bit */
+#define GPSR0_DU_EXODDF_DU_ODDF_DISP_CDE	BIT(21)
+#define GPSR0_DU_EXVSYNC_DU_VSYNC		BIT(20)
+#define GPSR0_DU_EXHSYNC_DU_HSYNC		BIT(19)
+#define GPSR0_DU_DOTCLKOUT			BIT(18)
+#define GPSR0_DU_DB7				BIT(17)
+#define GPSR0_DU_DB6				BIT(16)
+#define GPSR0_DU_DB5				BIT(15)
+#define GPSR0_DU_DB4				BIT(14)
+#define GPSR0_DU_DB3				BIT(13)
+#define GPSR0_DU_DB2				BIT(12)
+#define GPSR0_DU_DG7				BIT(11)
+#define GPSR0_DU_DG6				BIT(10)
+#define GPSR0_DU_DG5				BIT(9)
+#define GPSR0_DU_DG4				BIT(8)
+#define GPSR0_DU_DG3				BIT(7)
+#define GPSR0_DU_DG2				BIT(6)
+#define GPSR0_DU_DR7				BIT(5)
+#define GPSR0_DU_DR6				BIT(4)
+#define GPSR0_DU_DR5				BIT(3)
+#define GPSR0_DU_DR4				BIT(2)
+#define GPSR0_DU_DR3				BIT(1)
+#define GPSR0_DU_DR2				BIT(0)
+
+#define GPSR1_DIGRF_CLKOUT			BIT(27)
+#define GPSR1_DIGRF_CLKIN			BIT(26)
+#define GPSR1_CANFD_CLK				BIT(25)
+#define GPSR1_CANFD1_RX				BIT(24)
+#define GPSR1_CANFD1_TX				BIT(23)
+#define GPSR1_CANFD0_RX				BIT(22)
+#define GPSR1_CANFD0_TX				BIT(21)
+#define GPSR1_AVB0_AVTP_CAPTURE			BIT(20)
+#define GPSR1_AVB0_AVTP_MATCH			BIT(19)
+#define GPSR1_AVB0_LINK				BIT(18)
+#define GPSR1_AVB0_PHY_INT			BIT(17)
+#define GPSR1_AVB0_MAGIC			BIT(16)
+#define GPSR1_AVB0_MDC				BIT(15)
+#define GPSR1_AVB0_MDIO				BIT(14)
+#define GPSR1_AVB0_TXCREFCLK			BIT(13)
+#define GPSR1_AVB0_TD3				BIT(12)
+#define GPSR1_AVB0_TD2				BIT(11)
+#define GPSR1_AVB0_TD1				BIT(10)
+#define GPSR1_AVB0_TD0				BIT(9)
+#define GPSR1_AVB0_TXC				BIT(8)
+#define GPSR1_AVB0_TX_CTL			BIT(7)
+#define GPSR1_AVB0_RD3				BIT(6)
+#define GPSR1_AVB0_RD2				BIT(5)
+#define GPSR1_AVB0_RD1				BIT(4)
+#define GPSR1_AVB0_RD0				BIT(3)
+#define GPSR1_AVB0_RXC				BIT(2)
+#define GPSR1_AVB0_RX_CTL			BIT(1)
+#define GPSR1_IRQ0				BIT(0)
+
+#define GPSR2_VI0_FIELD				BIT(16)
+#define GPSR2_VI0_DATA11			BIT(15)
+#define GPSR2_VI0_DATA10			BIT(14)
+#define GPSR2_VI0_DATA9				BIT(13)
+#define GPSR2_VI0_DATA8				BIT(12)
+#define GPSR2_VI0_DATA7				BIT(11)
+#define GPSR2_VI0_DATA6				BIT(10)
+#define GPSR2_VI0_DATA5				BIT(9)
+#define GPSR2_VI0_DATA4				BIT(8)
+#define GPSR2_VI0_DATA3				BIT(7)
+#define GPSR2_VI0_DATA2				BIT(6)
+#define GPSR2_VI0_DATA1				BIT(5)
+#define GPSR2_VI0_DATA0				BIT(4)
+#define GPSR2_VI0_VSYNC_N			BIT(3)
+#define GPSR2_VI0_HSYNC_N			BIT(2)
+#define GPSR2_VI0_CLKENB			BIT(1)
+#define GPSR2_VI0_CLK				BIT(0)
+
+#define GPSR3_VI1_FIELD				BIT(16)
+#define GPSR3_VI1_DATA11			BIT(15)
+#define GPSR3_VI1_DATA10			BIT(14)
+#define GPSR3_VI1_DATA9				BIT(13)
+#define GPSR3_VI1_DATA8				BIT(12)
+#define GPSR3_VI1_DATA7				BIT(11)
+#define GPSR3_VI1_DATA6				BIT(10)
+#define GPSR3_VI1_DATA5				BIT(9)
+#define GPSR3_VI1_DATA4				BIT(8)
+#define GPSR3_VI1_DATA3				BIT(7)
+#define GPSR3_VI1_DATA2				BIT(6)
+#define GPSR3_VI1_DATA1				BIT(5)
+#define GPSR3_VI1_DATA0				BIT(4)
+#define GPSR3_VI1_VSYNC_N			BIT(3)
+#define GPSR3_VI1_HSYNC_N			BIT(2)
+#define GPSR3_VI1_CLKENB			BIT(1)
+#define GPSR3_VI1_CLK				BIT(0)
+
+#define GPSR4_SDA2				BIT(5)
+#define GPSR4_SCL2				BIT(4)
+#define GPSR4_SDA1				BIT(3)
+#define GPSR4_SCL1				BIT(2)
+#define GPSR4_SDA0				BIT(1)
+#define GPSR4_SCL0				BIT(0)
+
+#define GPSR5_RPC_INT_N				BIT(14)
+#define GPSR5_RPC_WP_N				BIT(13)
+#define GPSR5_RPC_RESET_N			BIT(12)
+#define GPSR5_QSPI1_SSL				BIT(11)
+#define GPSR5_QSPI1_IO3				BIT(10)
+#define GPSR5_QSPI1_IO2				BIT(9)
+#define GPSR5_QSPI1_MISO_IO1			BIT(8)
+#define GPSR5_QSPI1_MOSI_IO0			BIT(7)
+#define GPSR5_QSPI1_SPCLK			BIT(6)
+#define GPSR5_QSPI0_SSL				BIT(5)
+#define GPSR5_QSPI0_IO3				BIT(4)
+#define GPSR5_QSPI0_IO2				BIT(3)
+#define GPSR5_QSPI0_MISO_IO1			BIT(2)
+#define GPSR5_QSPI0_MOSI_IO0			BIT(1)
+#define GPSR5_QSPI0_SPCLK			BIT(0)
+
+#define IPSR_28_FUNC(x)				((uint32_t)(x) << 28U)
+#define IPSR_24_FUNC(x)				((uint32_t)(x) << 24U)
+#define IPSR_20_FUNC(x)				((uint32_t)(x) << 20U)
+#define IPSR_16_FUNC(x)				((uint32_t)(x) << 16U)
+#define IPSR_12_FUNC(x)				((uint32_t)(x) << 12U)
+#define IPSR_8_FUNC(x)				((uint32_t)(x) << 8U)
+#define IPSR_4_FUNC(x)				((uint32_t)(x) << 4U)
+#define IPSR_0_FUNC(x)				((uint32_t)(x) << 0U)
+
+#define IOCTRL30_POC_VI0_DATA5			BIT(31)
+#define IOCTRL30_POC_VI0_DATA4			BIT(30)
+#define IOCTRL30_POC_VI0_DATA3			BIT(29)
+#define IOCTRL30_POC_VI0_DATA2			BIT(28)
+#define IOCTRL30_POC_VI0_DATA1			BIT(27)
+#define IOCTRL30_POC_VI0_DATA0			BIT(26)
+#define IOCTRL30_POC_VI0_VSYNC_N		BIT(25)
+#define IOCTRL30_POC_VI0_HSYNC_N		BIT(24)
+#define IOCTRL30_POC_VI0_CLKENB			BIT(23)
+#define IOCTRL30_POC_VI0_CLK			BIT(22)
+#define IOCTRL30_POC_DU_EXODDF_DU_ODDF_DISP_CDE	BIT(21)
+#define IOCTRL30_POC_DU_EXVSYNC_DU_VSYNC	BIT(20)
+#define IOCTRL30_POC_DU_EXHSYNC_DU_HSYNC	BIT(19)
+#define IOCTRL30_POC_DU_DOTCLKOUT		BIT(18)
+#define IOCTRL30_POC_DU_DB7			BIT(17)
+#define IOCTRL30_POC_DU_DB6			BIT(16)
+#define IOCTRL30_POC_DU_DB5			BIT(15)
+#define IOCTRL30_POC_DU_DB4			BIT(14)
+#define IOCTRL30_POC_DU_DB3			BIT(13)
+#define IOCTRL30_POC_DU_DB2			BIT(12)
+#define IOCTRL30_POC_DU_DG7			BIT(11)
+#define IOCTRL30_POC_DU_DG6			BIT(10)
+#define IOCTRL30_POC_DU_DG5			BIT(9)
+#define IOCTRL30_POC_DU_DG4			BIT(8)
+#define IOCTRL30_POC_DU_DG3			BIT(7)
+#define IOCTRL30_POC_DU_DG2			BIT(6)
+#define IOCTRL30_POC_DU_DR7			BIT(5)
+#define IOCTRL30_POC_DU_DR6			BIT(4)
+#define IOCTRL30_POC_DU_DR5			BIT(3)
+#define IOCTRL30_POC_DU_DR4			BIT(2)
+#define IOCTRL30_POC_DU_DR3			BIT(1)
+#define IOCTRL30_POC_DU_DR2			BIT(0)
+
+#define IOCTRL31_POC_DUMMY_31			BIT(31)
+#define IOCTRL31_POC_DUMMY_30			BIT(30)
+#define IOCTRL31_POC_DUMMY_29			BIT(29)
+#define IOCTRL31_POC_DUMMY_28			BIT(28)
+#define IOCTRL31_POC_DUMMY_27			BIT(27)
+#define IOCTRL31_POC_DUMMY_26			BIT(26)
+#define IOCTRL31_POC_DUMMY_25			BIT(25)
+#define IOCTRL31_POC_DUMMY_24			BIT(24)
+#define IOCTRL31_POC_VI1_FIELD			BIT(23)
+#define IOCTRL31_POC_VI1_DATA11			BIT(22)
+#define IOCTRL31_POC_VI1_DATA10			BIT(21)
+#define IOCTRL31_POC_VI1_DATA9			BIT(20)
+#define IOCTRL31_POC_VI1_DATA8			BIT(19)
+#define IOCTRL31_POC_VI1_DATA7			BIT(18)
+#define IOCTRL31_POC_VI1_DATA6			BIT(17)
+#define IOCTRL31_POC_VI1_DATA5			BIT(16)
+#define IOCTRL31_POC_VI1_DATA4			BIT(15)
+#define IOCTRL31_POC_VI1_DATA3			BIT(14)
+#define IOCTRL31_POC_VI1_DATA2			BIT(13)
+#define IOCTRL31_POC_VI1_DATA1			BIT(12)
+#define IOCTRL31_POC_VI1_DATA0			BIT(11)
+#define IOCTRL31_POC_VI1_VSYNC_N		BIT(10)
+#define IOCTRL31_POC_VI1_HSYNC_N		BIT(9)
+#define IOCTRL31_POC_VI1_CLKENB			BIT(8)
+#define IOCTRL31_POC_VI1_CLK			BIT(7)
+#define IOCTRL31_POC_VI0_FIELD			BIT(6)
+#define IOCTRL31_POC_VI0_DATA11			BIT(5)
+#define IOCTRL31_POC_VI0_DATA10			BIT(4)
+#define IOCTRL31_POC_VI0_DATA9			BIT(3)
+#define IOCTRL31_POC_VI0_DATA8			BIT(2)
+#define IOCTRL31_POC_VI0_DATA7			BIT(1)
+#define IOCTRL31_POC_VI0_DATA6			BIT(0)
+#define IOCTRL32_POC2_VREF			BIT(0)
+#define IOCTRL40_SD0TDSEL1			BIT(1)
+#define IOCTRL40_SD0TDSEL0			BIT(0)
+
+#define PUEN0_PUEN_VI0_CLK			BIT(31)
+#define PUEN0_PUEN_TDI				BIT(30)
+#define PUEN0_PUEN_TMS				BIT(29)
+#define PUEN0_PUEN_TCK				BIT(28)
+#define PUEN0_PUEN_TRST_N			BIT(27)
+#define PUEN0_PUEN_IRQ0				BIT(26)
+#define PUEN0_PUEN_FSCLKST_N			BIT(25)
+#define PUEN0_PUEN_EXTALR			BIT(24)
+#define PUEN0_PUEN_PRESETOUT_N			BIT(23)
+#define PUEN0_PUEN_DU_DOTCLKIN			BIT(22)
+#define PUEN0_PUEN_DU_EXODDF_DU_ODDF_DISP_CDE	BIT(21)
+#define PUEN0_PUEN_DU_EXVSYNC_DU_VSYNC		BIT(20)
+#define PUEN0_PUEN_DU_EXHSYNC_DU_HSYNC		BIT(19)
+#define PUEN0_PUEN_DU_DOTCLKOUT			BIT(18)
+#define PUEN0_PUEN_DU_DB7			BIT(17)
+#define PUEN0_PUEN_DU_DB6			BIT(16)
+#define PUEN0_PUEN_DU_DB5			BIT(15)
+#define PUEN0_PUEN_DU_DB4			BIT(14)
+#define PUEN0_PUEN_DU_DB3			BIT(13)
+#define PUEN0_PUEN_DU_DB2			BIT(12)
+#define PUEN0_PUEN_DU_DG7			BIT(11)
+#define PUEN0_PUEN_DU_DG6			BIT(10)
+#define PUEN0_PUEN_DU_DG5			BIT(9)
+#define PUEN0_PUEN_DU_DG4			BIT(8)
+#define PUEN0_PUEN_DU_DG3			BIT(7)
+#define PUEN0_PUEN_DU_DG2			BIT(6)
+#define PUEN0_PUEN_DU_DR7			BIT(5)
+#define PUEN0_PUEN_DU_DR6			BIT(4)
+#define PUEN0_PUEN_DU_DR5			BIT(3)
+#define PUEN0_PUEN_DU_DR4			BIT(2)
+#define PUEN0_PUEN_DU_DR3			BIT(1)
+#define PUEN0_PUEN_DU_DR2			BIT(0)
+
+#define PUEN1_PUEN_VI1_DATA11			BIT(31)
+#define PUEN1_PUEN_VI1_DATA10			BIT(30)
+#define PUEN1_PUEN_VI1_DATA9			BIT(29)
+#define PUEN1_PUEN_VI1_DATA8			BIT(28)
+#define PUEN1_PUEN_VI1_DATA7			BIT(27)
+#define PUEN1_PUEN_VI1_DATA6			BIT(26)
+#define PUEN1_PUEN_VI1_DATA5			BIT(25)
+#define PUEN1_PUEN_VI1_DATA4			BIT(24)
+#define PUEN1_PUEN_VI1_DATA3			BIT(23)
+#define PUEN1_PUEN_VI1_DATA2			BIT(22)
+#define PUEN1_PUEN_VI1_DATA1			BIT(21)
+#define PUEN1_PUEN_VI1_DATA0			BIT(20)
+#define PUEN1_PUEN_VI1_VSYNC_N			BIT(19)
+#define PUEN1_PUEN_VI1_HSYNC_N			BIT(18)
+#define PUEN1_PUEN_VI1_CLKENB			BIT(17)
+#define PUEN1_PUEN_VI1_CLK			BIT(16)
+#define PUEN1_PUEN_VI0_FIELD			BIT(15)
+#define PUEN1_PUEN_VI0_DATA11			BIT(14)
+#define PUEN1_PUEN_VI0_DATA10			BIT(13)
+#define PUEN1_PUEN_VI0_DATA9			BIT(12)
+#define PUEN1_PUEN_VI0_DATA8			BIT(11)
+#define PUEN1_PUEN_VI0_DATA7			BIT(10)
+#define PUEN1_PUEN_VI0_DATA6			BIT(9)
+#define PUEN1_PUEN_VI0_DATA5			BIT(8)
+#define PUEN1_PUEN_VI0_DATA4			BIT(7)
+#define PUEN1_PUEN_VI0_DATA3			BIT(6)
+#define PUEN1_PUEN_VI0_DATA2			BIT(5)
+#define PUEN1_PUEN_VI0_DATA1			BIT(4)
+#define PUEN1_PUEN_VI0_DATA0			BIT(3)
+#define PUEN1_PUEN_VI0_VSYNC_N			BIT(2)
+#define PUEN1_PUEN_VI0_HSYNC_N			BIT(1)
+#define PUEN1_PUEN_VI0_CLKENB			BIT(0)
+
+#define PUEN2_PUEN_CANFD_CLK			BIT(31)
+#define PUEN2_PUEN_CANFD1_RX			BIT(30)
+#define PUEN2_PUEN_CANFD1_TX			BIT(29)
+#define PUEN2_PUEN_CANFD0_RX			BIT(28)
+#define PUEN2_PUEN_CANFD0_TX			BIT(27)
+#define PUEN2_PUEN_AVB0_AVTP_CAPTURE		BIT(26)
+#define PUEN2_PUEN_AVB0_AVTP_MATCH		BIT(25)
+#define PUEN2_PUEN_AVB0_LINK			BIT(24)
+#define PUEN2_PUEN_AVB0_PHY_INT			BIT(23)
+#define PUEN2_PUEN_AVB0_MAGIC			BIT(22)
+#define PUEN2_PUEN_AVB0_MDC			BIT(21)
+#define PUEN2_PUEN_AVB0_MDIO			BIT(20)
+#define PUEN2_PUEN_AVB0_TXCREFCLK		BIT(19)
+#define PUEN2_PUEN_AVB0_TD3			BIT(18)
+#define PUEN2_PUEN_AVB0_TD2			BIT(17)
+#define PUEN2_PUEN_AVB0_TD1			BIT(16)
+#define PUEN2_PUEN_AVB0_TD0			BIT(15)
+#define PUEN2_PUEN_AVB0_TXC			BIT(14)
+#define PUEN2_PUEN_AVB0_TX_CTL			BIT(13)
+#define PUEN2_PUEN_AVB0_RD3			BIT(12)
+#define PUEN2_PUEN_AVB0_RD2			BIT(11)
+#define PUEN2_PUEN_AVB0_RD1			BIT(10)
+#define PUEN2_PUEN_AVB0_RD0			BIT(9)
+#define PUEN2_PUEN_AVB0_RXC			BIT(8)
+#define PUEN2_PUEN_AVB0_RX_CTL			BIT(7)
+#define PUEN2_PUEN_SDA2				BIT(6)
+#define PUEN2_PUEN_SCL2				BIT(5)
+#define PUEN2_PUEN_SDA1				BIT(4)
+#define PUEN2_PUEN_SCL1				BIT(3)
+#define PUEN2_PUEN_SDA0				BIT(2)
+#define PUEN2_PUEN_SCL0				BIT(1)
+#define PUEN2_PUEN_VI1_FIELD			BIT(0)
+
+#define PUEN3_PUEN_DIGRF_CLKOUT			BIT(16)
+#define PUEN3_PUEN_DIGRF_CLKIN			BIT(15)
+#define PUEN3_PUEN_RPC_INT_N			BIT(14)
+#define PUEN3_PUEN_RPC_WP_N			BIT(13)
+#define PUEN3_PUEN_RPC_RESET_N			BIT(12)
+#define PUEN3_PUEN_QSPI1_SSL			BIT(11)
+#define PUEN3_PUEN_QSPI1_IO3			BIT(10)
+#define PUEN3_PUEN_QSPI1_IO2			BIT(9)
+#define PUEN3_PUEN_QSPI1_MISO_IO1		BIT(8)
+#define PUEN3_PUEN_QSPI1_MOSI_IO0		BIT(7)
+#define PUEN3_PUEN_QSPI1_SPCLK			BIT(6)
+#define PUEN3_PUEN_QSPI0_SSL			BIT(5)
+#define PUEN3_PUEN_QSPI0_IO3			BIT(4)
+#define PUEN3_PUEN_QSPI0_IO2			BIT(3)
+#define PUEN3_PUEN_QSPI0_MISO_IO1		BIT(2)
+#define PUEN3_PUEN_QSPI0_MOSI_IO0		BIT(1)
+#define PUEN3_PUEN_QSPI0_SPCLK			BIT(0)
+
+#define PUD0_PUD_VI0_CLK			BIT(31)
+#define PUD0_PUD_IRQ0				BIT(26)
+#define PUD0_PUD_FSCLKST_N			BIT(25)
+#define PUD0_PUD_PRESETOUT_N			BIT(23)
+#define PUD0_PUD_DU_EXODDF_DU_ODDF_DISP_CDE	BIT(21)
+#define PUD0_PUD_DU_EXVSYNC_DU_VSYNC		BIT(20)
+#define PUD0_PUD_DU_EXHSYNC_DU_HSYNC		BIT(19)
+#define PUD0_PUD_DU_DOTCLKOUT			BIT(18)
+#define PUD0_PUD_DU_DB7				BIT(17)
+#define PUD0_PUD_DU_DB6				BIT(16)
+#define PUD0_PUD_DU_DB5				BIT(15)
+#define PUD0_PUD_DU_DB4				BIT(14)
+#define PUD0_PUD_DU_DB3				BIT(13)
+#define PUD0_PUD_DU_DB2				BIT(12)
+#define PUD0_PUD_DU_DG7				BIT(11)
+#define PUD0_PUD_DU_DG6				BIT(10)
+#define PUD0_PUD_DU_DG5				BIT(9)
+#define PUD0_PUD_DU_DG4				BIT(8)
+#define PUD0_PUD_DU_DG3				BIT(7)
+#define PUD0_PUD_DU_DG2				BIT(6)
+#define PUD0_PUD_DU_DR7				BIT(5)
+#define PUD0_PUD_DU_DR6				BIT(4)
+#define PUD0_PUD_DU_DR5				BIT(3)
+#define PUD0_PUD_DU_DR4				BIT(2)
+#define PUD0_PUD_DU_DR3				BIT(1)
+#define PUD0_PUD_DU_DR2				BIT(0)
+
+#define PUD1_PUD_VI1_DATA11			BIT(31)
+#define PUD1_PUD_VI1_DATA10			BIT(30)
+#define PUD1_PUD_VI1_DATA9			BIT(29)
+#define PUD1_PUD_VI1_DATA8			BIT(28)
+#define PUD1_PUD_VI1_DATA7			BIT(27)
+#define PUD1_PUD_VI1_DATA6			BIT(26)
+#define PUD1_PUD_VI1_DATA5			BIT(25)
+#define PUD1_PUD_VI1_DATA4			BIT(24)
+#define PUD1_PUD_VI1_DATA3			BIT(23)
+#define PUD1_PUD_VI1_DATA2			BIT(22)
+#define PUD1_PUD_VI1_DATA1			BIT(21)
+#define PUD1_PUD_VI1_DATA0			BIT(20)
+#define PUD1_PUD_VI1_VSYNC_N			BIT(19)
+#define PUD1_PUD_VI1_HSYNC_N			BIT(18)
+#define PUD1_PUD_VI1_CLKENB			BIT(17)
+#define PUD1_PUD_VI1_CLK			BIT(16)
+#define PUD1_PUD_VI0_FIELD			BIT(15)
+#define PUD1_PUD_VI0_DATA11			BIT(14)
+#define PUD1_PUD_VI0_DATA10			BIT(13)
+#define PUD1_PUD_VI0_DATA9			BIT(12)
+#define PUD1_PUD_VI0_DATA8			BIT(11)
+#define PUD1_PUD_VI0_DATA7			BIT(10)
+#define PUD1_PUD_VI0_DATA6			BIT(9)
+#define PUD1_PUD_VI0_DATA5			BIT(8)
+#define PUD1_PUD_VI0_DATA4			BIT(7)
+#define PUD1_PUD_VI0_DATA3			BIT(6)
+#define PUD1_PUD_VI0_DATA2			BIT(5)
+#define PUD1_PUD_VI0_DATA1			BIT(4)
+#define PUD1_PUD_VI0_DATA0			BIT(3)
+#define PUD1_PUD_VI0_VSYNC_N			BIT(2)
+#define PUD1_PUD_VI0_HSYNC_N			BIT(1)
+#define PUD1_PUD_VI0_CLKENB			BIT(0)
+
+#define PUD2_PUD_CANFD_CLK			BIT(31)
+#define PUD2_PUD_CANFD1_RX			BIT(30)
+#define PUD2_PUD_CANFD1_TX			BIT(29)
+#define PUD2_PUD_CANFD0_RX			BIT(28)
+#define PUD2_PUD_CANFD0_TX			BIT(27)
+#define PUD2_PUD_AVB0_AVTP_CAPTURE		BIT(26)
+#define PUD2_PUD_AVB0_AVTP_MATCH		BIT(25)
+#define PUD2_PUD_AVB0_LINK			BIT(24)
+#define PUD2_PUD_AVB0_PHY_INT			BIT(23)
+#define PUD2_PUD_AVB0_MAGIC			BIT(22)
+#define PUD2_PUD_AVB0_MDC			BIT(21)
+#define PUD2_PUD_AVB0_MDIO			BIT(20)
+#define PUD2_PUD_AVB0_TXCREFCLK			BIT(19)
+#define PUD2_PUD_AVB0_TD3			BIT(18)
+#define PUD2_PUD_AVB0_TD2			BIT(17)
+#define PUD2_PUD_AVB0_TD1			BIT(16)
+#define PUD2_PUD_AVB0_TD0			BIT(15)
+#define PUD2_PUD_AVB0_TXC			BIT(14)
+#define PUD2_PUD_AVB0_TX_CTL			BIT(13)
+#define PUD2_PUD_AVB0_RD3			BIT(12)
+#define PUD2_PUD_AVB0_RD2			BIT(11)
+#define PUD2_PUD_AVB0_RD1			BIT(10)
+#define PUD2_PUD_AVB0_RD0			BIT(9)
+#define PUD2_PUD_AVB0_RXC			BIT(8)
+#define PUD2_PUD_AVB0_RX_CTL			BIT(7)
+#define PUD2_PUD_SDA2				BIT(6)
+#define PUD2_PUD_SCL2				BIT(5)
+#define PUD2_PUD_SDA1				BIT(4)
+#define PUD2_PUD_SCL1				BIT(3)
+#define PUD2_PUD_SDA0				BIT(2)
+#define PUD2_PUD_SCL0				BIT(1)
+#define PUD2_PUD_VI1_FIELD			BIT(0)
+
+#define PUD3_PUD_DIGRF_CLKOUT			BIT(16)
+#define PUD3_PUD_DIGRF_CLKIN			BIT(15)
+#define PUD3_PUD_RPC_INT_N			BIT(14)
+#define PUD3_PUD_RPC_WP_N			BIT(13)
+#define PUD3_PUD_RPC_RESET_N			BIT(12)
+#define PUD3_PUD_QSPI1_SSL			BIT(11)
+#define PUD3_PUD_QSPI1_IO3			BIT(10)
+#define PUD3_PUD_QSPI1_IO2			BIT(9)
+#define PUD3_PUD_QSPI1_MISO_IO1			BIT(8)
+#define PUD3_PUD_QSPI1_MOSI_IO0			BIT(7)
+#define PUD3_PUD_QSPI1_SPCLK			BIT(6)
+#define PUD3_PUD_QSPI0_SSL			BIT(5)
+#define PUD3_PUD_QSPI0_IO3			BIT(4)
+#define PUD3_PUD_QSPI0_IO2			BIT(3)
+#define PUD3_PUD_QSPI0_MISO_IO1			BIT(2)
+#define PUD3_PUD_QSPI0_MOSI_IO0			BIT(1)
+#define PUD3_PUD_QSPI0_SPCLK			BIT(0)
+
+#define MOD_SEL0_sel_hscif0			BIT(10)
+#define MOD_SEL0_sel_scif1			BIT(9)
+#define MOD_SEL0_sel_canfd0			BIT(8)
+#define MOD_SEL0_sel_pwm4			BIT(7)
+#define MOD_SEL0_sel_pwm3			BIT(6)
+#define MOD_SEL0_sel_pwm2			BIT(5)
+#define MOD_SEL0_sel_pwm1			BIT(4)
+#define MOD_SEL0_sel_pwm0			BIT(3)
+#define MOD_SEL0_sel_rfso			BIT(2)
+#define MOD_SEL0_sel_rsp			BIT(1)
+#define MOD_SEL0_sel_tmu			BIT(0)
+
+/* SCIF3 Registers for Dummy write */
+#define SCIF3_BASE		(0xE6C50000U)
+#define SCIF3_SCFCR		(SCIF3_BASE + 0x0018U)
+#define SCIF3_SCFDR		(SCIF3_BASE + 0x001CU)
+#define SCFCR_DATA		(0x0000U)
+
+/* Realtime module stop control */
+#define CPG_BASE		(0xE6150000U)
+#define CPG_MSTPSR0		(CPG_BASE + 0x0030U)
+#define CPG_RMSTPCR0		(CPG_BASE + 0x0110U)
+#define RMSTPCR0_RTDMAC		(0x00200000U)
+
+/* RT-DMAC Registers */
+#define RTDMAC_CH		(0U)		/* choose 0 to 15 */
+
+#define RTDMAC_BASE		(0xFFC10000U)
+#define RTDMAC_RDMOR		(RTDMAC_BASE + 0x0060U)
+#define RTDMAC_RDMCHCLR		(RTDMAC_BASE + 0x0080U)
+#define RTDMAC_RDMSAR(x)	(RTDMAC_BASE + 0x8000U + (0x80U * (x)))
+#define RTDMAC_RDMDAR(x)	(RTDMAC_BASE + 0x8004U + (0x80U * (x)))
+#define RTDMAC_RDMTCR(x)	(RTDMAC_BASE + 0x8008U + (0x80U * (x)))
+#define RTDMAC_RDMCHCR(x)	(RTDMAC_BASE + 0x800CU + (0x80U * (x)))
+#define RTDMAC_RDMCHCRB(x)	(RTDMAC_BASE + 0x801CU + (0x80U * (x)))
+#define RTDMAC_RDMDPBASE(x)	(RTDMAC_BASE + 0x8050U + (0x80U * (x)))
+#define RTDMAC_DESC_BASE	(RTDMAC_BASE + 0xA000U)
+#define RTDMAC_DESC_RDMSAR	(RTDMAC_DESC_BASE + 0x0000U)
+#define RTDMAC_DESC_RDMDAR	(RTDMAC_DESC_BASE + 0x0004U)
+#define RTDMAC_DESC_RDMTCR	(RTDMAC_DESC_BASE + 0x0008U)
+
+#define RDMOR_DME		(0x0001U)	/* DMA Master Enable */
+#define RDMCHCR_DPM_INFINITE	(0x30000000U)	/* Infinite repeat mode */
+#define RDMCHCR_RPT_TCR		(0x02000000U)	/* enable to update TCR */
+#define RDMCHCR_TS_2		(0x00000008U)	/* Word(2byte) units transfer */
+#define RDMCHCR_RS_AUTO		(0x00000400U)	/* Auto request */
+#define RDMCHCR_DE		(0x00000001U)	/* DMA Enable */
+#define RDMCHCRB_DRST		(0x00008000U)	/* Descriptor reset */
+#define RDMCHCRB_SLM_256	(0x00000080U)	/* once in 256 clock cycle */
+#define RDMDPBASE_SEL_EXT	(0x00000001U)	/* External memory use */
+
+static void pfc_reg_write(uint32_t addr, uint32_t data)
+{
+	mmio_write_32(PFC_PMMR, ~data);
+	mmio_write_32((uintptr_t)addr, data);
+}
+
+static void start_rtdma0_descriptor(void)
+{
+	uint32_t reg;
+
+	/* Module stop clear */
+	while ((mmio_read_32(CPG_MSTPSR0) & RMSTPCR0_RTDMAC) != 0U) {
+		reg = mmio_read_32(CPG_RMSTPCR0);
+		reg &= ~RMSTPCR0_RTDMAC;
+		cpg_write(CPG_RMSTPCR0, reg);
+	}
+
+	/* Initialize ch0, Reset Descriptor */
+	mmio_write_32(RTDMAC_RDMCHCLR, BIT(RTDMAC_CH));
+	mmio_write_32(RTDMAC_RDMCHCRB(RTDMAC_CH), RDMCHCRB_DRST);
+
+	/* Enable DMA */
+	mmio_write_16(RTDMAC_RDMOR, RDMOR_DME);
+
+	/* Set first transfer */
+	mmio_write_32(RTDMAC_RDMSAR(RTDMAC_CH), RCAR_PRR);
+	mmio_write_32(RTDMAC_RDMDAR(RTDMAC_CH), SCIF3_SCFDR);
+	mmio_write_32(RTDMAC_RDMTCR(RTDMAC_CH), 0x00000001U);
+
+	/* Set descriptor */
+	mmio_write_32(RTDMAC_DESC_RDMSAR, 0x00000000U);
+	mmio_write_32(RTDMAC_DESC_RDMDAR, 0x00000000U);
+	mmio_write_32(RTDMAC_DESC_RDMTCR, 0x00200000U);
+	mmio_write_32(RTDMAC_RDMCHCRB(RTDMAC_CH), RDMCHCRB_SLM_256);
+	mmio_write_32(RTDMAC_RDMDPBASE(RTDMAC_CH), RTDMAC_DESC_BASE
+						 | RDMDPBASE_SEL_EXT);
+
+	/* Set transfer parameter, Start transfer */
+	mmio_write_32(RTDMAC_RDMCHCR(RTDMAC_CH), RDMCHCR_DPM_INFINITE
+					       | RDMCHCR_RPT_TCR
+					       | RDMCHCR_TS_2
+					       | RDMCHCR_RS_AUTO
+					       | RDMCHCR_DE);
+}
+
+void pfc_init_v3m(void)
+{
+	/* Work around for PFC eratta */
+	start_rtdma0_descriptor();
+
+	// pin function
+	// md[4:1]!=0000
+	/* initialize GPIO/perihperal function select */
+
+	pfc_reg_write(PFC_GPSR0, 0x00000000);
+
+	pfc_reg_write(PFC_GPSR1, GPSR1_CANFD_CLK);
+
+	pfc_reg_write(PFC_GPSR2, 0x00000000);
+
+	pfc_reg_write(PFC_GPSR3, 0x00000000);
+
+	pfc_reg_write(PFC_GPSR4, GPSR4_SDA2
+				   | GPSR4_SCL2);
+
+	pfc_reg_write(PFC_GPSR5, GPSR5_QSPI1_SSL
+				   | GPSR5_QSPI1_IO3
+				   | GPSR5_QSPI1_IO2
+				   | GPSR5_QSPI1_MISO_IO1
+				   | GPSR5_QSPI1_MOSI_IO0
+				   | GPSR5_QSPI1_SPCLK
+				   | GPSR5_QSPI0_SSL
+				   | GPSR5_QSPI0_IO3
+				   | GPSR5_QSPI0_IO2
+				   | GPSR5_QSPI0_MISO_IO1
+				   | GPSR5_QSPI0_MOSI_IO0
+				   | GPSR5_QSPI0_SPCLK);
+
+	/* initialize peripheral function select */
+	pfc_reg_write(PFC_IPSR0, IPSR_28_FUNC(0)
+				   | IPSR_24_FUNC(0)
+				   | IPSR_20_FUNC(0)
+				   | IPSR_16_FUNC(0)
+				   | IPSR_12_FUNC(0)
+				   | IPSR_8_FUNC(0)
+				   | IPSR_4_FUNC(0)
+				   | IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR1, IPSR_28_FUNC(0)
+				   | IPSR_24_FUNC(0)
+				   | IPSR_20_FUNC(0)
+				   | IPSR_16_FUNC(0)
+				   | IPSR_12_FUNC(0)
+				   | IPSR_8_FUNC(0)
+				   | IPSR_4_FUNC(0)
+				   | IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR2, IPSR_28_FUNC(0)
+				   | IPSR_24_FUNC(0)
+				   | IPSR_20_FUNC(0)
+				   | IPSR_16_FUNC(0)
+				   | IPSR_12_FUNC(0)
+				   | IPSR_8_FUNC(0)
+				   | IPSR_4_FUNC(0)
+				   | IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR3, IPSR_28_FUNC(0)
+				   | IPSR_24_FUNC(0)
+				   | IPSR_20_FUNC(0)
+				   | IPSR_16_FUNC(0)
+				   | IPSR_12_FUNC(0)
+				   | IPSR_8_FUNC(0)
+				   | IPSR_4_FUNC(0)
+				   | IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR4, IPSR_28_FUNC(0)
+				   | IPSR_24_FUNC(0)
+				   | IPSR_20_FUNC(0)
+				   | IPSR_16_FUNC(0)
+				   | IPSR_12_FUNC(0)
+				   | IPSR_8_FUNC(0)
+				   | IPSR_4_FUNC(0)
+				   | IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR5, IPSR_28_FUNC(0)
+				   | IPSR_24_FUNC(0)
+				   | IPSR_20_FUNC(0)
+				   | IPSR_16_FUNC(0)
+				   | IPSR_12_FUNC(0)
+				   | IPSR_8_FUNC(0)
+				   | IPSR_4_FUNC(0)
+				   | IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR6, IPSR_28_FUNC(0)
+				   | IPSR_24_FUNC(0)
+				   | IPSR_20_FUNC(0)
+				   | IPSR_16_FUNC(0)
+				   | IPSR_12_FUNC(0)
+				   | IPSR_8_FUNC(0)
+				   | IPSR_4_FUNC(0)
+				   | IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR7, IPSR_28_FUNC(0)
+				   | IPSR_24_FUNC(4)
+				   | IPSR_20_FUNC(4)
+				   | IPSR_16_FUNC(4)
+				   | IPSR_12_FUNC(4)
+				   | IPSR_8_FUNC(0)
+				   | IPSR_4_FUNC(0)
+				   | IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR8, IPSR_28_FUNC(0)
+				   | IPSR_24_FUNC(0)
+				   | IPSR_20_FUNC(0)
+				   | IPSR_16_FUNC(4)
+				   | IPSR_12_FUNC(0)
+				   | IPSR_8_FUNC(0)
+				   | IPSR_4_FUNC(0)
+				   | IPSR_0_FUNC(0));
+
+	/* initialize POC Control */
+
+	pfc_reg_write(PFC_POCCTRL0, IOCTRL30_POC_VI0_DATA5
+				   | IOCTRL30_POC_VI0_DATA4
+				   | IOCTRL30_POC_VI0_DATA3
+				   | IOCTRL30_POC_VI0_DATA2
+				   | IOCTRL30_POC_VI0_DATA1
+				   | IOCTRL30_POC_VI0_DATA0
+				   | IOCTRL30_POC_VI0_VSYNC_N
+				   | IOCTRL30_POC_VI0_HSYNC_N
+				   | IOCTRL30_POC_VI0_CLKENB
+				   | IOCTRL30_POC_VI0_CLK
+				   | IOCTRL30_POC_DU_EXODDF_DU_ODDF_DISP_CDE
+				   | IOCTRL30_POC_DU_EXVSYNC_DU_VSYNC
+				   | IOCTRL30_POC_DU_EXHSYNC_DU_HSYNC
+				   | IOCTRL30_POC_DU_DOTCLKOUT
+				   | IOCTRL30_POC_DU_DB7
+				   | IOCTRL30_POC_DU_DB6
+				   | IOCTRL30_POC_DU_DB5
+				   | IOCTRL30_POC_DU_DB4
+				   | IOCTRL30_POC_DU_DB3
+				   | IOCTRL30_POC_DU_DB2
+				   | IOCTRL30_POC_DU_DG7
+				   | IOCTRL30_POC_DU_DG6
+				   | IOCTRL30_POC_DU_DG5
+				   | IOCTRL30_POC_DU_DG4
+				   | IOCTRL30_POC_DU_DG3
+				   | IOCTRL30_POC_DU_DG2
+				   | IOCTRL30_POC_DU_DR7
+				   | IOCTRL30_POC_DU_DR6
+				   | IOCTRL30_POC_DU_DR5
+				   | IOCTRL30_POC_DU_DR4
+				   | IOCTRL30_POC_DU_DR3
+				   | IOCTRL30_POC_DU_DR2);
+
+	pfc_reg_write(PFC_IOCTRL31, IOCTRL31_POC_DUMMY_31
+				   | IOCTRL31_POC_DUMMY_30
+				   | IOCTRL31_POC_DUMMY_29
+				   | IOCTRL31_POC_DUMMY_28
+				   | IOCTRL31_POC_DUMMY_27
+				   | IOCTRL31_POC_DUMMY_26
+				   | IOCTRL31_POC_DUMMY_25
+				   | IOCTRL31_POC_DUMMY_24
+				   | IOCTRL31_POC_VI1_FIELD
+				   | IOCTRL31_POC_VI1_DATA11
+				   | IOCTRL31_POC_VI1_DATA10
+				   | IOCTRL31_POC_VI1_DATA9
+				   | IOCTRL31_POC_VI1_DATA8
+				   | IOCTRL31_POC_VI1_DATA7
+				   | IOCTRL31_POC_VI1_DATA6
+				   | IOCTRL31_POC_VI1_DATA5
+				   | IOCTRL31_POC_VI1_DATA4
+				   | IOCTRL31_POC_VI1_DATA3
+				   | IOCTRL31_POC_VI1_DATA2
+				   | IOCTRL31_POC_VI1_DATA1
+				   | IOCTRL31_POC_VI1_DATA0
+				   | IOCTRL31_POC_VI1_VSYNC_N
+				   | IOCTRL31_POC_VI1_HSYNC_N
+				   | IOCTRL31_POC_VI1_CLKENB
+				   | IOCTRL31_POC_VI1_CLK
+				   | IOCTRL31_POC_VI0_FIELD
+				   | IOCTRL31_POC_VI0_DATA11
+				   | IOCTRL31_POC_VI0_DATA10
+				   | IOCTRL31_POC_VI0_DATA9
+				   | IOCTRL31_POC_VI0_DATA8
+				   | IOCTRL31_POC_VI0_DATA7
+				   | IOCTRL31_POC_VI0_DATA6);
+
+	pfc_reg_write(PFC_POCCTRL2, 0x00000000);
+
+	pfc_reg_write(PFC_TDSELCTRL0, 0x00000000);
+
+	/* initialize Pull enable */
+	pfc_reg_write(PFC_PUEN0, PUEN0_PUEN_VI0_CLK
+				   | PUEN0_PUEN_TDI
+				   | PUEN0_PUEN_TMS
+				   | PUEN0_PUEN_TCK
+				   | PUEN0_PUEN_TRST_N
+				   | PUEN0_PUEN_IRQ0
+				   | PUEN0_PUEN_FSCLKST_N
+				   | PUEN0_PUEN_DU_EXHSYNC_DU_HSYNC
+				   | PUEN0_PUEN_DU_DOTCLKOUT
+				   | PUEN0_PUEN_DU_DB7
+				   | PUEN0_PUEN_DU_DB6
+				   | PUEN0_PUEN_DU_DB5
+				   | PUEN0_PUEN_DU_DB4
+				   | PUEN0_PUEN_DU_DB3
+				   | PUEN0_PUEN_DU_DB2
+				   | PUEN0_PUEN_DU_DG7
+				   | PUEN0_PUEN_DU_DG6
+				   | PUEN0_PUEN_DU_DG5
+				   | PUEN0_PUEN_DU_DG4
+				   | PUEN0_PUEN_DU_DG3
+				   | PUEN0_PUEN_DU_DG2
+				   | PUEN0_PUEN_DU_DR7
+				   | PUEN0_PUEN_DU_DR6
+				   | PUEN0_PUEN_DU_DR5
+				   | PUEN0_PUEN_DU_DR4
+				   | PUEN0_PUEN_DU_DR3
+				   | PUEN0_PUEN_DU_DR2);
+
+	pfc_reg_write(PFC_PUEN1, PUEN1_PUEN_VI1_DATA11
+				   | PUEN1_PUEN_VI1_DATA10
+				   | PUEN1_PUEN_VI1_DATA9
+				   | PUEN1_PUEN_VI1_DATA8
+				   | PUEN1_PUEN_VI1_DATA7
+				   | PUEN1_PUEN_VI1_DATA6
+				   | PUEN1_PUEN_VI1_DATA5
+				   | PUEN1_PUEN_VI1_DATA4
+				   | PUEN1_PUEN_VI1_DATA3
+				   | PUEN1_PUEN_VI1_DATA2
+				   | PUEN1_PUEN_VI1_DATA1
+				   | PUEN1_PUEN_VI1_DATA0
+				   | PUEN1_PUEN_VI1_VSYNC_N
+				   | PUEN1_PUEN_VI1_HSYNC_N
+				   | PUEN1_PUEN_VI1_CLKENB
+				   | PUEN1_PUEN_VI1_CLK
+				   | PUEN1_PUEN_VI0_DATA11
+				   | PUEN1_PUEN_VI0_DATA10
+				   | PUEN1_PUEN_VI0_DATA9
+				   | PUEN1_PUEN_VI0_DATA8
+				   | PUEN1_PUEN_VI0_DATA7
+				   | PUEN1_PUEN_VI0_DATA6
+				   | PUEN1_PUEN_VI0_DATA5
+				   | PUEN1_PUEN_VI0_DATA4
+				   | PUEN1_PUEN_VI0_DATA3
+				   | PUEN1_PUEN_VI0_DATA2
+				   | PUEN1_PUEN_VI0_DATA1);
+
+	pfc_reg_write(PFC_PUEN2, PUEN2_PUEN_CANFD_CLK
+				   | PUEN2_PUEN_CANFD1_RX
+				   | PUEN2_PUEN_CANFD1_TX
+				   | PUEN2_PUEN_CANFD0_RX
+				   | PUEN2_PUEN_CANFD0_TX
+				   | PUEN2_PUEN_AVB0_AVTP_CAPTURE
+				   | PUEN2_PUEN_AVB0_AVTP_MATCH
+				   | PUEN2_PUEN_AVB0_LINK
+				   | PUEN2_PUEN_AVB0_PHY_INT
+				   | PUEN2_PUEN_AVB0_MAGIC
+				   | PUEN2_PUEN_AVB0_TXCREFCLK
+				   | PUEN2_PUEN_AVB0_TD3
+				   | PUEN2_PUEN_AVB0_TD2
+				   | PUEN2_PUEN_AVB0_TD1
+				   | PUEN2_PUEN_AVB0_TD0
+				   | PUEN2_PUEN_AVB0_TXC
+				   | PUEN2_PUEN_AVB0_TX_CTL
+				   | PUEN2_PUEN_AVB0_RD3
+				   | PUEN2_PUEN_AVB0_RD2
+				   | PUEN2_PUEN_AVB0_RD1
+				   | PUEN2_PUEN_AVB0_RD0
+				   | PUEN2_PUEN_AVB0_RXC
+				   | PUEN2_PUEN_AVB0_RX_CTL
+				   | PUEN2_PUEN_VI1_FIELD);
+
+	pfc_reg_write(PFC_PUEN3, PUEN3_PUEN_DIGRF_CLKOUT
+				   | PUEN3_PUEN_DIGRF_CLKIN);
+
+	/* initialize PUD Control */
+	pfc_reg_write(PFC_PUD0, PUD0_PUD_VI0_CLK
+				   | PUD0_PUD_IRQ0
+				   | PUD0_PUD_FSCLKST_N
+				   | PUD0_PUD_DU_EXODDF_DU_ODDF_DISP_CDE
+				   | PUD0_PUD_DU_EXVSYNC_DU_VSYNC
+				   | PUD0_PUD_DU_EXHSYNC_DU_HSYNC
+				   | PUD0_PUD_DU_DOTCLKOUT
+				   | PUD0_PUD_DU_DB7
+				   | PUD0_PUD_DU_DB6
+				   | PUD0_PUD_DU_DB5
+				   | PUD0_PUD_DU_DB4
+				   | PUD0_PUD_DU_DB3
+				   | PUD0_PUD_DU_DB2
+				   | PUD0_PUD_DU_DG7
+				   | PUD0_PUD_DU_DG6
+				   | PUD0_PUD_DU_DG5
+				   | PUD0_PUD_DU_DG4
+				   | PUD0_PUD_DU_DG3
+				   | PUD0_PUD_DU_DG2
+				   | PUD0_PUD_DU_DR7
+				   | PUD0_PUD_DU_DR6
+				   | PUD0_PUD_DU_DR5
+				   | PUD0_PUD_DU_DR4
+				   | PUD0_PUD_DU_DR3
+				   | PUD0_PUD_DU_DR2);
+
+	pfc_reg_write(PFC_PUD1, PUD1_PUD_VI1_DATA11
+				   | PUD1_PUD_VI1_DATA10
+				   | PUD1_PUD_VI1_DATA9
+				   | PUD1_PUD_VI1_DATA8
+				   | PUD1_PUD_VI1_DATA7
+				   | PUD1_PUD_VI1_DATA6
+				   | PUD1_PUD_VI1_DATA5
+				   | PUD1_PUD_VI1_DATA4
+				   | PUD1_PUD_VI1_DATA3
+				   | PUD1_PUD_VI1_DATA2
+				   | PUD1_PUD_VI1_DATA1
+				   | PUD1_PUD_VI1_DATA0
+				   | PUD1_PUD_VI1_VSYNC_N
+				   | PUD1_PUD_VI1_HSYNC_N
+				   | PUD1_PUD_VI1_CLKENB
+				   | PUD1_PUD_VI1_CLK
+				   | PUD1_PUD_VI0_DATA11
+				   | PUD1_PUD_VI0_DATA10
+				   | PUD1_PUD_VI0_DATA9
+				   | PUD1_PUD_VI0_DATA8
+				   | PUD1_PUD_VI0_DATA7
+				   | PUD1_PUD_VI0_DATA6
+				   | PUD1_PUD_VI0_DATA5
+				   | PUD1_PUD_VI0_DATA4
+				   | PUD1_PUD_VI0_DATA3
+				   | PUD1_PUD_VI0_DATA2
+				   | PUD1_PUD_VI0_DATA1
+				   | PUD1_PUD_VI0_DATA0
+				   | PUD1_PUD_VI0_VSYNC_N
+				   | PUD1_PUD_VI0_HSYNC_N
+				   | PUD1_PUD_VI0_CLKENB);
+
+	pfc_reg_write(PFC_PUD2, PUD2_PUD_CANFD_CLK
+				   | PUD2_PUD_CANFD1_RX
+				   | PUD2_PUD_CANFD1_TX
+				   | PUD2_PUD_CANFD0_RX
+				   | PUD2_PUD_CANFD0_TX
+				   | PUD2_PUD_AVB0_AVTP_CAPTURE
+				   | PUD2_PUD_VI1_FIELD);
+
+	pfc_reg_write(PFC_PUD3, PUD3_PUD_DIGRF_CLKOUT
+				   | PUD3_PUD_DIGRF_CLKIN);
+
+	/* initialize Module Select */
+	pfc_reg_write(PFC_MOD_SEL0, 0x00000000);
+
+	// gpio
+	/* initialize positive/negative logic select */
+	mmio_write_32(GPIO_POSNEG0, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG1, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG2, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG3, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG4, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG5, 0x00000000U);
+
+	/* initialize general IO/interrupt switching */
+	mmio_write_32(GPIO_IOINTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL5, 0x00000000U);
+
+	/* initialize general output register */
+	mmio_write_32(GPIO_OUTDT0, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT1, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT2, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT3, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT4, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT5, 0x00000000U);
+
+	/* initialize general input/output switching */
+	mmio_write_32(GPIO_INOUTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_INOUTSEL5, 0x00000000U);
+}
diff --git a/drivers/renesas/rcar/pfc/V3M/pfc_init_v3m.h b/drivers/renesas/rcar/pfc/V3M/pfc_init_v3m.h
new file mode 100644
index 0000000..7bab92f
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/V3M/pfc_init_v3m.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PFC_INIT_V3M_H
+#define PFC_INIT_V3M_H
+
+void pfc_init_v3m(void);
+
+#endif	/* PFC_INIT_V3M_H */
diff --git a/drivers/renesas/rcar/pfc/pfc.mk b/drivers/renesas/rcar/pfc/pfc.mk
new file mode 100644
index 0000000..f1dd92c
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/pfc.mk
@@ -0,0 +1,69 @@
+#
+# Copyright (c) 2015-2018, Renesas Electronics Corporation. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${RCAR_LSI},${RCAR_AUTO})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/H3/pfc_init_h3_v1.c
+    BL2_SOURCES += drivers/renesas/rcar/pfc/H3/pfc_init_h3_v2.c
+    BL2_SOURCES += drivers/renesas/rcar/pfc/M3/pfc_init_m3.c
+    BL2_SOURCES += drivers/renesas/rcar/pfc/M3N/pfc_init_m3n.c
+    BL2_SOURCES += drivers/renesas/rcar/pfc/V3M/pfc_init_v3m.c
+
+else ifdef RCAR_LSI_CUT_COMPAT
+  ifeq (${RCAR_LSI},${RCAR_H3})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/H3/pfc_init_h3_v1.c
+    BL2_SOURCES += drivers/renesas/rcar/pfc/H3/pfc_init_h3_v2.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_H3N})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/H3/pfc_init_h3_v1.c
+    BL2_SOURCES += drivers/renesas/rcar/pfc/H3/pfc_init_h3_v2.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_M3})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/M3/pfc_init_m3.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_M3N})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/M3N/pfc_init_m3n.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_V3M})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/V3M/pfc_init_v3m.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_E3})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/E3/pfc_init_e3.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_D3})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/D3/pfc_init_d3.c
+  endif
+else
+  ifeq (${RCAR_LSI},${RCAR_H3})
+    ifeq (${LSI_CUT},10)
+      BL2_SOURCES += drivers/renesas/rcar/pfc/H3/pfc_init_h3_v1.c
+    else ifeq (${LSI_CUT},11)
+      BL2_SOURCES += drivers/renesas/rcar/pfc/H3/pfc_init_h3_v1.c
+    else
+#     LSI_CUT 20 or later
+      BL2_SOURCES += drivers/renesas/rcar/pfc/H3/pfc_init_h3_v2.c
+    endif
+  endif
+  ifeq (${RCAR_LSI},${RCAR_H3N})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/H3/pfc_init_h3_v2.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_M3})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/M3/pfc_init_m3.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_M3N})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/M3N/pfc_init_m3n.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_V3M})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/V3M/pfc_init_v3m.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_E3})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/E3/pfc_init_e3.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_D3})
+    BL2_SOURCES += drivers/renesas/rcar/pfc/D3/pfc_init_d3.c
+  endif
+endif
+
+BL2_SOURCES += drivers/renesas/rcar/pfc/pfc_init.c
diff --git a/drivers/renesas/rcar/pfc/pfc_init.c b/drivers/renesas/rcar/pfc/pfc_init.c
new file mode 100644
index 0000000..8810667
--- /dev/null
+++ b/drivers/renesas/rcar/pfc/pfc_init.c
@@ -0,0 +1,199 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "rcar_def.h"
+#if RCAR_LSI == RCAR_AUTO
+#include "H3/pfc_init_h3_v1.h"
+#include "H3/pfc_init_h3_v2.h"
+#include "M3/pfc_init_m3.h"
+#include "M3N/pfc_init_m3n.h"
+#include "V3M/pfc_init_v3m.h"
+#endif
+#if (RCAR_LSI == RCAR_H3) || (RCAR_LSI == RCAR_H3N)	/* H3 */
+#include "H3/pfc_init_h3_v1.h"
+#include "H3/pfc_init_h3_v2.h"
+#endif
+#if RCAR_LSI == RCAR_M3		/* M3 */
+#include "M3/pfc_init_m3.h"
+#endif
+#if RCAR_LSI == RCAR_M3N	/* M3N */
+#include "M3N/pfc_init_m3n.h"
+#endif
+#if RCAR_LSI == RCAR_V3M	/* V3M */
+#include "V3M/pfc_init_v3m.h"
+#endif
+#if RCAR_LSI == RCAR_E3		/* E3 */
+#include "E3/pfc_init_e3.h"
+#endif
+#if RCAR_LSI == RCAR_D3		/* D3 */
+#include "D3/pfc_init_d3.h"
+#endif
+
+#define PRR_PRODUCT_ERR(reg)				\
+	do {						\
+		ERROR("LSI Product ID(PRR=0x%x) PFC initialize not supported.\n", \
+			reg);				\
+		panic();				\
+	} while (0)
+
+#define PRR_CUT_ERR(reg)				\
+	do {						\
+		ERROR("LSI Cut ID(PRR=0x%x) PFC initialize not supported.\n", \
+			reg);				\
+		panic();\
+	} while (0)
+
+void rcar_pfc_init(void)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(RCAR_PRR);
+#if RCAR_LSI == RCAR_AUTO
+	switch (reg & PRR_PRODUCT_MASK) {
+	case PRR_PRODUCT_H3:
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:	/* H3 Ver.1.0 */
+			pfc_init_h3_v1();
+			break;
+		case PRR_PRODUCT_11:	/* H3 Ver.1.1 */
+			pfc_init_h3_v1();
+			break;
+		default:	/* H3 Ver.2.0 or later */
+			pfc_init_h3_v2();
+			break;
+		}
+		break;
+	case PRR_PRODUCT_M3:
+		pfc_init_m3();
+		break;
+	case PRR_PRODUCT_M3N:
+		pfc_init_m3n();
+		break;
+	case PRR_PRODUCT_V3M:
+		pfc_init_v3m();
+		break;
+	default:
+		PRR_PRODUCT_ERR(reg);
+		break;
+	}
+
+#elif RCAR_LSI_CUT_COMPAT
+	switch (reg & PRR_PRODUCT_MASK) {
+	case PRR_PRODUCT_H3:
+#if (RCAR_LSI != RCAR_H3) && (RCAR_LSI != RCAR_H3N)
+		PRR_PRODUCT_ERR(reg);
+#else
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:	/* H3 Ver.1.0 */
+			pfc_init_h3_v1();
+			break;
+		case PRR_PRODUCT_11:	/* H3 Ver.1.1 */
+			pfc_init_h3_v1();
+			break;
+		default:	/* H3 Ver.2.0 or later */
+			pfc_init_h3_v2();
+			break;
+		}
+#endif
+		break;
+	case PRR_PRODUCT_M3:
+#if RCAR_LSI != RCAR_M3
+		PRR_PRODUCT_ERR(reg);
+#else
+		pfc_init_m3();
+#endif
+		break;
+	case PRR_PRODUCT_M3N:
+#if RCAR_LSI != RCAR_M3N
+		PRR_PRODUCT_ERR(reg);
+#else
+		pfc_init_m3n();
+#endif
+		break;
+	case PRR_PRODUCT_V3M:
+#if RCAR_LSI != RCAR_V3M
+		PRR_PRODUCT_ERR(reg);
+#else
+		pfc_init_v3m();
+#endif
+		break;
+	case PRR_PRODUCT_E3:
+#if RCAR_LSI != RCAR_E3
+		PRR_PRODUCT_ERR(reg);
+#else
+		pfc_init_e3();
+#endif
+		break;
+	case PRR_PRODUCT_D3:
+#if RCAR_LSI != RCAR_D3
+		PRR_PRODUCT_ERR(reg);
+#else
+		pfc_init_d3();
+#endif
+		break;
+	default:
+		PRR_PRODUCT_ERR(reg);
+		break;
+	}
+
+#else
+#if (RCAR_LSI == RCAR_H3) || (RCAR_LSI == RCAR_H3N)	/* H3 */
+#if RCAR_LSI_CUT == RCAR_CUT_10
+	/* H3 Ver.1.0 */
+	if ((PRR_PRODUCT_H3 | PRR_PRODUCT_10)
+	    != (reg & (PRR_PRODUCT_MASK | PRR_CUT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	pfc_init_h3_v1();
+#elif RCAR_LSI_CUT == RCAR_CUT_11
+	/* H3 Ver.1.1 */
+	if ((PRR_PRODUCT_H3 | PRR_PRODUCT_11)
+	    != (reg & (PRR_PRODUCT_MASK | PRR_CUT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	pfc_init_h3_v1();
+#else
+	/* H3 Ver.2.0 or later */
+	if (PRR_PRODUCT_H3 != (reg & PRR_PRODUCT_MASK)) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	pfc_init_h3_v2();
+#endif
+#elif RCAR_LSI == RCAR_M3	/* M3 */
+	if ((PRR_PRODUCT_M3) != (reg & PRR_PRODUCT_MASK)) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	pfc_init_m3();
+#elif RCAR_LSI == RCAR_M3N	/* M3N */
+	if ((PRR_PRODUCT_M3N) != (reg & PRR_PRODUCT_MASK)) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	pfc_init_m3n();
+#elif RCAR_LSI == RCAR_V3M	/* V3M */
+	if ((PRR_PRODUCT_V3M) != (reg & PRR_PRODUCT_MASK)) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	pfc_init_v3m();
+#elif RCAR_LSI == RCAR_E3	/* E3 */
+	if ((PRR_PRODUCT_E3) != (reg & PRR_PRODUCT_MASK)) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	pfc_init_e3();
+#elif RCAR_LSI == RCAR_D3	/* D3 */
+	if ((PRR_PRODUCT_D3) != (reg & PRR_PRODUCT_MASK)) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	pfc_init_d3();
+#else
+#error "Don't have PFC initialize routine(unknown)."
+#endif
+#endif
+}
diff --git a/drivers/renesas/rcar/qos/D3/qos_init_d3.c b/drivers/renesas/rcar/qos/D3/qos_init_d3.c
new file mode 100644
index 0000000..b96e822
--- /dev/null
+++ b/drivers/renesas/rcar/qos/D3/qos_init_d3.c
@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+#include "qos_init_d3.h"
+
+#define	RCAR_QOS_VERSION		"rev.0.05"
+
+#include "qos_init_d3_mstat.h"
+
+struct rcar_gen3_dbsc_qos_settings d3_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218 },
+	{ DBSC_DBCAM0CNF2, 0x000000F4 },
+	{ DBSC_DBSCHCNT0, 0x000F0037 },
+	{ DBSC_DBSCHSZ0, 0x00000001 },
+	{ DBSC_DBSCHRW0, 0x22421111 },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123 },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00 },
+	{ DBSC_DBSCHQOS01, 0x00000B00 },
+	{ DBSC_DBSCHQOS02, 0x00000000 },
+	{ DBSC_DBSCHQOS03, 0x00000000 },
+	{ DBSC_DBSCHQOS40, 0x00000300 },
+	{ DBSC_DBSCHQOS41, 0x000002F0 },
+	{ DBSC_DBSCHQOS42, 0x00000200 },
+	{ DBSC_DBSCHQOS43, 0x00000100 },
+	{ DBSC_DBSCHQOS90, 0x00000300 },
+	{ DBSC_DBSCHQOS91, 0x000002F0 },
+	{ DBSC_DBSCHQOS92, 0x00000200 },
+	{ DBSC_DBSCHQOS93, 0x00000100 },
+	{ DBSC_DBSCHQOS130, 0x00000100 },
+	{ DBSC_DBSCHQOS131, 0x000000F0 },
+	{ DBSC_DBSCHQOS132, 0x000000A0 },
+	{ DBSC_DBSCHQOS133, 0x00000040 },
+	{ DBSC_DBSCHQOS140, 0x000000C0 },
+	{ DBSC_DBSCHQOS141, 0x000000B0 },
+	{ DBSC_DBSCHQOS142, 0x00000080 },
+	{ DBSC_DBSCHQOS143, 0x00000040 },
+	{ DBSC_DBSCHQOS150, 0x00000040 },
+	{ DBSC_DBSCHQOS151, 0x00000030 },
+	{ DBSC_DBSCHQOS152, 0x00000020 },
+	{ DBSC_DBSCHQOS153, 0x00000010 },
+};
+
+void qos_init_d3(void)
+{
+	rcar_qos_dbsc_setting(d3_qos, ARRAY_SIZE(d3_qos), true);
+
+	/* DRAM Split Address mapping */
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH
+	ERROR("DRAM Split 4ch not supported.(D3)");
+	panic();
+#elif RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH
+	ERROR("DRAM Split 2ch not supported.(D3)");
+	panic();
+#elif RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO
+	ERROR("DRAM Split Auto not supported.(D3)");
+	panic();
+#elif RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_LINEAR
+/*	NOTICE("BL2: DRAM Split is OFF\n"); */
+	/* Split setting(DDR 1ch) */
+	io_write_32(AXI_ADSPLCR0, 0x00000000U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else
+	ERROR("DRAM split is an invalid value.(D3)");
+	panic();
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+	/* Resource Alloc setting */
+	io_write_32(QOSCTRL_RAS,   0x00000020U);
+	io_write_32(QOSCTRL_FIXTH, 0x000F0005U);
+	io_write_32(QOSCTRL_RAEN,  0x00000001U);
+	io_write_32(QOSCTRL_REGGD, 0x00000000U);
+	io_write_64(QOSCTRL_DANN,  0x0404020002020201U);
+	io_write_32(QOSCTRL_DANT,  0x00100804U);
+	io_write_32(QOSCTRL_EC,    0x00000000U);
+	io_write_64(QOSCTRL_EMS,   0x0000000000000000U);
+	io_write_32(QOSCTRL_FSS,   0x0000000AU);
+	io_write_32(QOSCTRL_INSFC, 0xC7840001U);
+	io_write_32(QOSCTRL_BERR,  0x00000000U);
+	io_write_32(QOSCTRL_EARLYR,  0x00000000U);
+	io_write_32(QOSCTRL_RACNT0,  0x00010003U);
+	io_write_32(QOSCTRL_STATGEN0, 0x00000000U);
+
+	/* GPU setting */
+	io_write_32(0xFD812030U, 0x00000000U);
+
+	/* QOSBW setting */
+	io_write_32(QOSCTRL_SL_INIT, 0x030500ACU);
+	io_write_32(QOSCTRL_REF_ARS, 0x00780000U);
+
+	/* QOSBW SRAM setting */
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		io_write_64(QOSBW_FIX_QOS_BANK0 + i * 8, mstat_fix[i]);
+		io_write_64(QOSBW_FIX_QOS_BANK1 + i * 8, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		io_write_64(QOSBW_BE_QOS_BANK0 + i * 8, mstat_be[i]);
+		io_write_64(QOSBW_BE_QOS_BANK1 + i * 8, mstat_be[i]);
+	}
+
+	/* 3DG bus Leaf setting */
+	io_write_32(GPU_ACT_GRD, 0x00001234U);
+	io_write_32(GPU_ACT0, 0x00000000U);
+	io_write_32(GPU_ACT1, 0x00000000U);
+	io_write_32(GPU_ACT2, 0x00000000U);
+	io_write_32(GPU_ACT3, 0x00000000U);
+
+	/* RT bus Leaf setting */
+	io_write_32(CPU_ACT0, 0x00000003U);
+	io_write_32(CPU_ACT1, 0x00000003U);
+	io_write_32(RT_ACT0, 0x00000000U);
+	io_write_32(RT_ACT1, 0x00000000U);
+
+	/* Resource Alloc start */
+	io_write_32(QOSCTRL_RAEN,  0x00000001U);
+
+	/* QOSBW start */
+	io_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	/* Resource Alloc setting */
+	io_write_32(QOSCTRL_EC,    0x00000000U);
+	/* Resource Alloc start */
+	io_write_32(QOSCTRL_RAEN,  0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rcar/qos/D3/qos_init_d3.h b/drivers/renesas/rcar/qos/D3/qos_init_d3.h
new file mode 100644
index 0000000..968ee7a
--- /dev/null
+++ b/drivers/renesas/rcar/qos/D3/qos_init_d3.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_H_D3__
+#define QOS_INIT_H_D3__
+
+void qos_init_d3(void);
+
+#endif	/* QOS_INIT_H_D3__ */
diff --git a/drivers/renesas/rcar/qos/D3/qos_init_d3_mstat.h b/drivers/renesas/rcar/qos/D3/qos_init_d3_mstat.h
new file mode 100644
index 0000000..cbf1f65
--- /dev/null
+++ b/drivers/renesas/rcar/qos/D3/qos_init_d3_mstat.h
@@ -0,0 +1,244 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+static const uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004340000FFFFUL,
+	/* 0x0038, */ 0x001004140000FFFFUL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x00140B030000FFFFUL,
+	/* 0x0060, */ 0x001408610000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001410620000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00A0, */ 0x000C041C0000FFFFUL,
+	/* 0x00A8, */ 0x000C04090000FFFFUL,
+	/* 0x00B0, */ 0x000C04110000FFFFUL,
+	/* 0x00B8, */ 0x0000000000000000UL,
+	/* 0x00C0, */ 0x000C041C0000FFFFUL,
+	/* 0x00C8, */ 0x000C04090000FFFFUL,
+	/* 0x00D0, */ 0x000C04110000FFFFUL,
+	/* 0x00D8, */ 0x0000000000000000UL,
+	/* 0x00E0, */ 0x0000000000000000UL,
+	/* 0x00E8, */ 0x0000000000000000UL,
+	/* 0x00F0, */ 0x001018570000FFFFUL,
+	/* 0x00F8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001008570000FFFFUL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x001008520000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00100CA30000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01A0, */ 0x0000000000000000UL,
+	/* 0x01A8, */ 0x0000000000000000UL,
+	/* 0x01B0, */ 0x0000000000000000UL,
+	/* 0x01B8, */ 0x0000000000000000UL,
+	/* 0x01C0, */ 0x0000000000000000UL,
+	/* 0x01C8, */ 0x0000000000000000UL,
+	/* 0x01D0, */ 0x0000000000000000UL,
+	/* 0x01D8, */ 0x0000000000000000UL,
+	/* 0x01E0, */ 0x0000000000000000UL,
+	/* 0x01E8, */ 0x000C04020000FFFFUL,
+	/* 0x01F0, */ 0x0000000000000000UL,
+	/* 0x01F8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04090000FFFFUL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x001410040000FFFFUL,
+	/* 0x0270, */ 0x001404020000FFFFUL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410040000FFFFUL,
+	/* 0x0298, */ 0x001404020000FFFFUL,
+	/* 0x02A0, */ 0x000C04050000FFFFUL,
+	/* 0x02A8, */ 0x000C04050000FFFFUL,
+	/* 0x02B0, */ 0x0000000000000000UL,
+	/* 0x02B8, */ 0x0000000000000000UL,
+	/* 0x02C0, */ 0x0000000000000000UL,
+	/* 0x02C8, */ 0x0000000000000000UL,
+	/* 0x02D0, */ 0x000C04050000FFFFUL,
+	/* 0x02D8, */ 0x000C04050000FFFFUL,
+	/* 0x02E0, */ 0x0000000000000000UL,
+	/* 0x02E8, */ 0x0000000000000000UL,
+	/* 0x02F0, */ 0x0000000000000000UL,
+	/* 0x02F8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x000C04020000FFFFUL,
+	/* 0x0378, */ 0x000C04020000FFFFUL,
+	/* 0x0380, */ 0x000C04090000FFFFUL,
+	/* 0x0388, */ 0x000C04090000FFFFUL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static const uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00A0, */ 0x0000000000000000UL,
+	/* 0x00A8, */ 0x0000000000000000UL,
+	/* 0x00B0, */ 0x0000000000000000UL,
+	/* 0x00B8, */ 0x0000000000000000UL,
+	/* 0x00C0, */ 0x0000000000000000UL,
+	/* 0x00C8, */ 0x0000000000000000UL,
+	/* 0x00D0, */ 0x0000000000000000UL,
+	/* 0x00D8, */ 0x0000000000000000UL,
+	/* 0x00E0, */ 0x0000000000000000UL,
+	/* 0x00E8, */ 0x0000000000000000UL,
+	/* 0x00F0, */ 0x0000000000000000UL,
+	/* 0x00F8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01A0, */ 0x0000000000000000UL,
+	/* 0x01A8, */ 0x0000000000000000UL,
+	/* 0x01B0, */ 0x0000000000000000UL,
+	/* 0x01B8, */ 0x0000000000000000UL,
+	/* 0x01C0, */ 0x00110090060FA001UL,
+	/* 0x01C8, */ 0x00110090060FA001UL,
+	/* 0x01D0, */ 0x0000000000000000UL,
+	/* 0x01D8, */ 0x0000000000000000UL,
+	/* 0x01E0, */ 0x0000000000000000UL,
+	/* 0x01E8, */ 0x0000000000000000UL,
+	/* 0x01F0, */ 0x0011001006004401UL,
+	/* 0x01F8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0011001006004401UL,
+	/* 0x0218, */ 0x0011001006009801UL,
+	/* 0x0220, */ 0x0011001006009801UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011001006009801UL,
+	/* 0x0238, */ 0x0011001006009801UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02A0, */ 0x0000000000000000UL,
+	/* 0x02A8, */ 0x0000000000000000UL,
+	/* 0x02B0, */ 0x0000000000000000UL,
+	/* 0x02B8, */ 0x0011001006003401UL,
+	/* 0x02C0, */ 0x0000000000000000UL,
+	/* 0x02C8, */ 0x0000000000000000UL,
+	/* 0x02D0, */ 0x0000000000000000UL,
+	/* 0x02D8, */ 0x0000000000000000UL,
+	/* 0x02E0, */ 0x0000000000000000UL,
+	/* 0x02E8, */ 0x0011001006003401UL,
+	/* 0x02F0, */ 0x00110090060FA001UL,
+	/* 0x02F8, */ 0x00110090060FA001UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0012001006003401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x00120090060FA001UL,
+	/* 0x0360, */ 0x00120090060FA001UL,
+	/* 0x0368, */ 0x0012001006003401UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0012001006003401UL,
+};
+#endif
+
diff --git a/drivers/renesas/rcar/qos/E3/qos_init_e3_v10.c b/drivers/renesas/rcar/qos/E3/qos_init_e3_v10.c
new file mode 100644
index 0000000..6f4c66c
--- /dev/null
+++ b/drivers/renesas/rcar/qos/E3/qos_init_e3_v10.c
@@ -0,0 +1,142 @@
+/*
+ * Copyright (c) 2018-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+#include "qos_init_e3_v10.h"
+
+#define	RCAR_QOS_VERSION		"rev.0.05"
+
+#define REF_ARS_ARBSTOPCYCLE_E3	(((SL_INIT_SSLOTCLK_E3) - 5U) << 16U)
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_e3_v10_mstat390.h"
+#else
+#include "qos_init_e3_v10_mstat780.h"
+#endif
+
+#endif
+
+struct rcar_gen3_dbsc_qos_settings e3_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218 },
+	{ DBSC_DBCAM0CNF2, 0x000000F4 },
+	{ DBSC_DBSCHCNT0, 0x000F0037 },
+	{ DBSC_DBSCHSZ0, 0x00000001 },
+	{ DBSC_DBSCHRW0, 0x22421111 },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123 },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00 },
+	{ DBSC_DBSCHQOS01, 0x00000B00 },
+	{ DBSC_DBSCHQOS02, 0x00000000 },
+	{ DBSC_DBSCHQOS03, 0x00000000 },
+	{ DBSC_DBSCHQOS40, 0x00000300 },
+	{ DBSC_DBSCHQOS41, 0x000002F0 },
+	{ DBSC_DBSCHQOS42, 0x00000200 },
+	{ DBSC_DBSCHQOS43, 0x00000100 },
+	{ DBSC_DBSCHQOS90, 0x00000100 },
+	{ DBSC_DBSCHQOS91, 0x000000F0 },
+	{ DBSC_DBSCHQOS92, 0x000000A0 },
+	{ DBSC_DBSCHQOS93, 0x00000040 },
+	{ DBSC_DBSCHQOS130, 0x00000100 },
+	{ DBSC_DBSCHQOS131, 0x000000F0 },
+	{ DBSC_DBSCHQOS132, 0x000000A0 },
+	{ DBSC_DBSCHQOS133, 0x00000040 },
+	{ DBSC_DBSCHQOS140, 0x000000C0 },
+	{ DBSC_DBSCHQOS141, 0x000000B0 },
+	{ DBSC_DBSCHQOS142, 0x00000080 },
+	{ DBSC_DBSCHQOS143, 0x00000040 },
+	{ DBSC_DBSCHQOS150, 0x00000040 },
+	{ DBSC_DBSCHQOS151, 0x00000030 },
+	{ DBSC_DBSCHQOS152, 0x00000020 },
+	{ DBSC_DBSCHQOS153, 0x00000010 },
+};
+
+void qos_init_e3_v10(void)
+{
+	rcar_qos_dbsc_setting(e3_qos, ARRAY_SIZE(e3_qos), true);
+
+	/* DRAM Split Address mapping */
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH
+#if RCAR_LSI == RCAR_E3
+#error "Don't set DRAM Split 4ch(E3)"
+#else
+	ERROR("DRAM Split 4ch not supported.(E3)");
+	panic();
+#endif
+#elif (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH)
+#if RCAR_LSI == RCAR_E3
+#error "Don't set DRAM Split 2ch(E3)"
+#else
+	ERROR("DRAM Split 2ch not supported.(E3)");
+	panic();
+#endif
+#else
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+	NOTICE("BL2: DRAM refresh interval 3.9 usec\n");
+#else
+	NOTICE("BL2: DRAM refresh interval 7.8 usec\n");
+#endif
+
+	io_write_32(QOSCTRL_RAS, 0x00000020U);
+	io_write_64(QOSCTRL_DANN, 0x0404020002020201UL);
+	io_write_32(QOSCTRL_DANT, 0x00100804U);
+	io_write_32(QOSCTRL_FSS, 0x0000000AU);
+	io_write_32(QOSCTRL_INSFC, 0x06330001U);
+	io_write_32(QOSCTRL_EARLYR, 0x00000000U);
+	io_write_32(QOSCTRL_RACNT0, 0x00010003U);
+
+	io_write_32(QOSCTRL_SL_INIT,
+		    SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT |
+		    SL_INIT_SSLOTCLK_E3);
+	io_write_32(QOSCTRL_REF_ARS, REF_ARS_ARBSTOPCYCLE_E3);
+
+	/* QOSBW SRAM setting */
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		io_write_64(QOSBW_FIX_QOS_BANK0 + i * 8, mstat_fix[i]);
+		io_write_64(QOSBW_FIX_QOS_BANK1 + i * 8, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		io_write_64(QOSBW_BE_QOS_BANK0 + i * 8, mstat_be[i]);
+		io_write_64(QOSBW_BE_QOS_BANK1 + i * 8, mstat_be[i]);
+	}
+
+	/* RT bus Leaf setting */
+	io_write_32(RT_ACT0, 0x00000000U);
+	io_write_32(RT_ACT1, 0x00000000U);
+
+	/* CCI bus Leaf setting */
+	io_write_32(CPU_ACT0, 0x00000003U);
+	io_write_32(CPU_ACT1, 0x00000003U);
+
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+	io_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif
+}
diff --git a/drivers/renesas/rcar/qos/E3/qos_init_e3_v10.h b/drivers/renesas/rcar/qos/E3/qos_init_e3_v10.h
new file mode 100644
index 0000000..2c1d8c5
--- /dev/null
+++ b/drivers/renesas/rcar/qos/E3/qos_init_e3_v10.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_E3_V10_H
+#define QOS_INIT_E3_V10_H
+
+void qos_init_e3_v10(void);
+
+#endif /* QOS_INIT_E3_V10_H */
diff --git a/drivers/renesas/rcar/qos/E3/qos_init_e3_v10_mstat390.h b/drivers/renesas/rcar/qos/E3/qos_init_e3_v10_mstat390.h
new file mode 100644
index 0000000..d7f9d14
--- /dev/null
+++ b/drivers/renesas/rcar/qos/E3/qos_init_e3_v10_mstat390.h
@@ -0,0 +1,241 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008620000FFFFUL,
+	/* 0x0038, */ 0x001008620000FFFFUL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x001415260000FFFFUL,
+	/* 0x0060, */ 0x001415260000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001414930000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C08380000FFFFUL,
+	/* 0x00a8, */ 0x000C04110000FFFFUL,
+	/* 0x00b0, */ 0x000C04150000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C08380000FFFFUL,
+	/* 0x00c8, */ 0x000C04110000FFFFUL,
+	/* 0x00d0, */ 0x000C04150000FFFFUL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001018580000FFFFUL,
+	/* 0x00f8, */ 0x000C084F0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001008580000FFFFUL,
+	/* 0x0118, */ 0x000C21E40000FFFFUL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x001008530000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00100C960000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x001008530000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0010042A0000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00101D8D0000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x001008530000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04020000FFFFUL,
+	/* 0x01f0, */ 0x000C04090000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04090000FFFFUL,
+	/* 0x0210, */ 0x000C04090000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C2A0000FFFFUL,
+	/* 0x0268, */ 0x001410040000FFFFUL,
+	/* 0x0270, */ 0x001404020000FFFFUL,
+	/* 0x0278, */ 0x000C08110000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410040000FFFFUL,
+	/* 0x0298, */ 0x001404020000FFFFUL,
+	/* 0x02a0, */ 0x000C04090000FFFFUL,
+	/* 0x02a8, */ 0x000C04090000FFFFUL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x000C04020000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04090000FFFFUL,
+	/* 0x02d8, */ 0x000C04090000FFFFUL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x000C04020000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x000C04020000FFFFUL,
+	/* 0x0378, */ 0x000C04020000FFFFUL,
+	/* 0x0380, */ 0x000C04090000FFFFUL,
+	/* 0x0388, */ 0x000C04090000FFFFUL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001005F03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0021060005FFFC01UL,
+	/* 0x01c8, */ 0x0021060005FFFC01UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021010005F79801UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021010005F79801UL,
+	/* 0x0218, */ 0x0011010005F79801UL,
+	/* 0x0220, */ 0x0011010005F79801UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011010005F79801UL,
+	/* 0x0238, */ 0x0011010005F79801UL,
+	/* 0x0240, */ 0x0012010005F79801UL,
+	/* 0x0248, */ 0x0011010005F79801UL,
+	/* 0x0250, */ 0x0012010005F79801UL,
+	/* 0x0258, */ 0x0011010005F79801UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011060005FFFC01UL,
+	/* 0x02f8, */ 0x0011060005FFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0012001005F03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0012060005FFFC01UL,
+	/* 0x0360, */ 0x0012060005FFFC01UL,
+	/* 0x0368, */ 0x0012001005F03401UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0012001005F03401UL,
+};
diff --git a/drivers/renesas/rcar/qos/E3/qos_init_e3_v10_mstat780.h b/drivers/renesas/rcar/qos/E3/qos_init_e3_v10_mstat780.h
new file mode 100644
index 0000000..439cafe
--- /dev/null
+++ b/drivers/renesas/rcar/qos/E3/qos_init_e3_v10_mstat780.h
@@ -0,0 +1,241 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001010C40000FFFFUL,
+	/* 0x0038, */ 0x001010C40000FFFFUL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x00142A4B0000FFFFUL,
+	/* 0x0060, */ 0x00142A4B0000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001429260000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C10700000FFFFUL,
+	/* 0x00a8, */ 0x000C08210000FFFFUL,
+	/* 0x00b0, */ 0x000C082A0000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C10700000FFFFUL,
+	/* 0x00c8, */ 0x000C08210000FFFFUL,
+	/* 0x00d0, */ 0x000C082A0000FFFFUL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x00102CAF0000FFFFUL,
+	/* 0x00f8, */ 0x000C0C9D0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x00100CAF0000FFFFUL,
+	/* 0x0118, */ 0x000C43C80000FFFFUL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100CA50000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0010152C0000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x00100CA50000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008530000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001037190000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x00100CA50000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04040000FFFFUL,
+	/* 0x01f0, */ 0x000C08110000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04110000FFFFUL,
+	/* 0x0210, */ 0x000C08110000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C18530000FFFFUL,
+	/* 0x0268, */ 0x00141C070000FFFFUL,
+	/* 0x0270, */ 0x001404040000FFFFUL,
+	/* 0x0278, */ 0x000C0C210000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x00141C070000FFFFUL,
+	/* 0x0298, */ 0x001404040000FFFFUL,
+	/* 0x02a0, */ 0x000C04110000FFFFUL,
+	/* 0x02a8, */ 0x000C04110000FFFFUL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x000C04040000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04110000FFFFUL,
+	/* 0x02d8, */ 0x000C04110000FFFFUL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x000C04040000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x000C04040000FFFFUL,
+	/* 0x0378, */ 0x000C04040000FFFFUL,
+	/* 0x0380, */ 0x000C04110000FFFFUL,
+	/* 0x0388, */ 0x000C04110000FFFFUL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001002F03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0021060002FFFC01UL,
+	/* 0x01c8, */ 0x0021060002FFFC01UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021010002F3CC01UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021010002F3CC01UL,
+	/* 0x0218, */ 0x0011010002F3CC01UL,
+	/* 0x0220, */ 0x0011010002F3CC01UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011010002F3CC01UL,
+	/* 0x0238, */ 0x0011010002F3CC01UL,
+	/* 0x0240, */ 0x0012010002F3CC01UL,
+	/* 0x0248, */ 0x0011010002F3CC01UL,
+	/* 0x0250, */ 0x0012010002F3CC01UL,
+	/* 0x0258, */ 0x0011010002F3CC01UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011060002FFFC01UL,
+	/* 0x02f8, */ 0x0011060002FFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0012001002F03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0012060002FFFC01UL,
+	/* 0x0360, */ 0x0012060002FFFC01UL,
+	/* 0x0368, */ 0x0012001002F03401UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0012001002F03401UL,
+};
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v10.c b/drivers/renesas/rcar/qos/H3/qos_init_h3_v10.c
new file mode 100644
index 0000000..1fb43a7
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v10.c
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+#include "qos_init_h3_v10.h"
+
+#define	RCAR_QOS_VERSION		"rev.0.36"
+
+#include "qos_init_h3_v10_mstat.h"
+
+void qos_init_h3_v10(void)
+{
+	/* DRAM Split Address mapping */
+#if (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH) || \
+    (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO)
+	NOTICE("BL2: DRAM Split is 4ch\n");
+	io_write_32(AXI_ADSPLCR0, ADSPLCR0_ADRMODE_DEFAULT
+		    | ADSPLCR0_SPLITSEL(0xFFU)
+		    | ADSPLCR0_AREA(0x1BU)
+		    | ADSPLCR0_SWP);
+	io_write_32(AXI_ADSPLCR1, 0x00000000U);
+	io_write_32(AXI_ADSPLCR2, 0xA8A90000U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#elif RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH
+	NOTICE("BL2: DRAM Split is 2ch\n");
+	io_write_32(AXI_ADSPLCR0, 0x00000000U);
+	io_write_32(AXI_ADSPLCR1, ADSPLCR0_ADRMODE_DEFAULT
+		    | ADSPLCR0_SPLITSEL(0xFFU)
+		    | ADSPLCR0_AREA(0x1BU)
+		    | ADSPLCR0_SWP);
+	io_write_32(AXI_ADSPLCR2, 0x00000000U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+	/* AR Cache setting */
+	io_write_32(0xE67D1000U, 0x00000100U);
+	io_write_32(0xE67D1008U, 0x00000100U);
+
+	/* Resource Alloc setting */
+	io_write_32(QOSCTRL_RAS, 0x00000040U);
+	io_write_32(QOSCTRL_FIXTH, 0x000F0005U);
+	io_write_32(QOSCTRL_REGGD, 0x00000004U);
+	io_write_64(QOSCTRL_DANN, 0x0202000004040404UL);
+	io_write_32(QOSCTRL_DANT, 0x003C1110U);
+	io_write_32(QOSCTRL_EC, 0x00080001U);	/* need for H3 v1.* */
+	io_write_64(QOSCTRL_EMS, 0x0000000000000000UL);
+	io_write_32(QOSCTRL_INSFC, 0xC7840001U);
+	io_write_32(QOSCTRL_BERR, 0x00000000U);
+
+	/* QOSBW setting */
+	io_write_32(QOSCTRL_SL_INIT,
+		    SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT | SL_INIT_SSLOTCLK);
+	io_write_32(QOSCTRL_REF_ARS, 0x00330000U);
+
+	/* QOSBW SRAM setting */
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		io_write_64(QOSBW_FIX_QOS_BANK0 + i * 8, mstat_fix[i]);
+		io_write_64(QOSBW_FIX_QOS_BANK1 + i * 8, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		io_write_64(QOSBW_BE_QOS_BANK0 + i * 8, mstat_be[i]);
+		io_write_64(QOSBW_BE_QOS_BANK1 + i * 8, mstat_be[i]);
+	}
+
+	/* 3DG bus Leaf setting */
+	io_write_32(0xFD820808U, 0x00001234U);
+	io_write_32(0xFD820800U, 0x0000003FU);
+	io_write_32(0xFD821800U, 0x0000003FU);
+	io_write_32(0xFD822800U, 0x0000003FU);
+	io_write_32(0xFD823800U, 0x0000003FU);
+	io_write_32(0xFD824800U, 0x0000003FU);
+	io_write_32(0xFD825800U, 0x0000003FU);
+	io_write_32(0xFD826800U, 0x0000003FU);
+	io_write_32(0xFD827800U, 0x0000003FU);
+
+	/* Resource Alloc start */
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+	/* QOSBW start */
+	io_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	/* Resource Alloc setting */
+	io_write_32(QOSCTRL_EC, 0x00080001U);	/* need for H3 v1.* */
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v10.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v10.h
new file mode 100644
index 0000000..f96182a
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v10.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_H3_V10_H
+#define QOS_INIT_H3_V10_H
+
+void qos_init_h3_v10(void);
+
+#endif /* QOS_INIT_H3_V10_H */
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v10_mstat.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v10_mstat.h
new file mode 100644
index 0000000..fe63236
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v10_mstat.h
@@ -0,0 +1,221 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+static const uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x00140C050000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x001404030000FFFFUL,
+	/* 0x0060, */ 0x001408060000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x00140C050000FFFFUL,
+	/* 0x0090, */ 0x001408060000FFFFUL,
+	/* 0x0098, */ 0x001404020000FFFFUL,
+	/* 0x00A0, */ 0x0000000000000000UL,
+	/* 0x00A8, */ 0x0000000000000000UL,
+	/* 0x00B0, */ 0x0000000000000000UL,
+	/* 0x00B8, */ 0x0000000000000000UL,
+	/* 0x00C0, */ 0x0000000000000000UL,
+	/* 0x00C8, */ 0x0000000000000000UL,
+	/* 0x00D0, */ 0x0000000000000000UL,
+	/* 0x00D8, */ 0x0000000000000000UL,
+	/* 0x00E0, */ 0x0000000000000000UL,
+	/* 0x00E8, */ 0x0000000000000000UL,
+	/* 0x00F0, */ 0x0000000000000000UL,
+	/* 0x00F8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x001004020000FFFFUL,
+	/* 0x0140, */ 0x001004020000FFFFUL,
+	/* 0x0148, */ 0x001004020000FFFFUL,
+	/* 0x0150, */ 0x001008050000FFFFUL,
+	/* 0x0158, */ 0x001008050000FFFFUL,
+	/* 0x0160, */ 0x001008050000FFFFUL,
+	/* 0x0168, */ 0x001008050000FFFFUL,
+	/* 0x0170, */ 0x001008050000FFFFUL,
+	/* 0x0178, */ 0x001004030000FFFFUL,
+	/* 0x0180, */ 0x001004030000FFFFUL,
+	/* 0x0188, */ 0x001004030000FFFFUL,
+	/* 0x0190, */ 0x001014140000FFFFUL,
+	/* 0x0198, */ 0x001014140000FFFFUL,
+	/* 0x01A0, */ 0x001008060000FFFFUL,
+	/* 0x01A8, */ 0x001008060000FFFFUL,
+	/* 0x01B0, */ 0x001008060000FFFFUL,
+	/* 0x01B8, */ 0x0000000000000000UL,
+	/* 0x01C0, */ 0x0000000000000000UL,
+	/* 0x01C8, */ 0x0000000000000000UL,
+	/* 0x01D0, */ 0x0000000000000000UL,
+	/* 0x01D8, */ 0x0000000000000000UL,
+	/* 0x01E0, */ 0x0000000000000000UL,
+	/* 0x01E8, */ 0x0000000000000000UL,
+	/* 0x01F0, */ 0x0000000000000000UL,
+	/* 0x01F8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02A0, */ 0x0000000000000000UL,
+	/* 0x02A8, */ 0x0000000000000000UL,
+	/* 0x02B0, */ 0x0000000000000000UL,
+	/* 0x02B8, */ 0x0000000000000000UL,
+	/* 0x02C0, */ 0x0000000000000000UL,
+	/* 0x02C8, */ 0x0000000000000000UL,
+	/* 0x02D0, */ 0x0000000000000000UL,
+	/* 0x02D8, */ 0x0000000000000000UL,
+	/* 0x02E0, */ 0x0000000000000000UL,
+	/* 0x02E8, */ 0x0000000000000000UL,
+	/* 0x02F0, */ 0x0000000000000000UL,
+	/* 0x02F8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+};
+
+static const uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x001000100C8FFC01UL,
+	/* 0x0008, */ 0x001000100C8FFC01UL,
+	/* 0x0010, */ 0x001000100C8FFC01UL,
+	/* 0x0018, */ 0x001000100C8FFC01UL,
+	/* 0x0020, */ 0x001000100C8FFC01UL,
+	/* 0x0028, */ 0x001000100C8FFC01UL,
+	/* 0x0030, */ 0x001000100C8FFC01UL,
+	/* 0x0038, */ 0x001000100C8FFC01UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001000100C8FFC01UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x001000100C8FFC01UL,
+	/* 0x0070, */ 0x001000100C8FFC01UL,
+	/* 0x0078, */ 0x001000100C8FFC01UL,
+	/* 0x0080, */ 0x001000100C8FFC01UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00A0, */ 0x001000100C8FFC01UL,
+	/* 0x00A8, */ 0x001000100C8FFC01UL,
+	/* 0x00B0, */ 0x001000100C8FFC01UL,
+	/* 0x00B8, */ 0x001000100C8FFC01UL,
+	/* 0x00C0, */ 0x001000100C8FFC01UL,
+	/* 0x00C8, */ 0x001000100C8FFC01UL,
+	/* 0x00D0, */ 0x001000100C8FFC01UL,
+	/* 0x00D8, */ 0x002000200C8FFC01UL,
+	/* 0x00E0, */ 0x002000200C8FFC01UL,
+	/* 0x00E8, */ 0x001000100C8FFC01UL,
+	/* 0x00F0, */ 0x001000100C8FFC01UL,
+	/* 0x00F8, */ 0x001000100C8FFC01UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x002000200C8FFC01UL,
+	/* 0x0110, */ 0x001000100C8FFC01UL,
+	/* 0x0118, */ 0x001000100C8FFC01UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x002000200C8FFC01UL,
+	/* 0x0130, */ 0x001000100C8FFC01UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01A0, */ 0x0000000000000000UL,
+	/* 0x01A8, */ 0x0000000000000000UL,
+	/* 0x01B0, */ 0x0000000000000000UL,
+	/* 0x01B8, */ 0x001000100C8FFC01UL,
+	/* 0x01C0, */ 0x001000200C8FFC01UL,
+	/* 0x01C8, */ 0x001000200C8FFC01UL,
+	/* 0x01D0, */ 0x001000200C8FFC01UL,
+	/* 0x01D8, */ 0x001000200C8FFC01UL,
+	/* 0x01E0, */ 0x001000100C8FFC01UL,
+	/* 0x01E8, */ 0x001000100C8FFC01UL,
+	/* 0x01F0, */ 0x001000100C8FFC01UL,
+	/* 0x01F8, */ 0x001000100C8FFC01UL,
+	/* 0x0200, */ 0x001000100C8FFC01UL,
+	/* 0x0208, */ 0x001000100C8FFC01UL,
+	/* 0x0210, */ 0x001000100C8FFC01UL,
+	/* 0x0218, */ 0x001000100C8FFC01UL,
+	/* 0x0220, */ 0x001000100C8FFC01UL,
+	/* 0x0228, */ 0x001000100C8FFC01UL,
+	/* 0x0230, */ 0x001000100C8FFC01UL,
+	/* 0x0238, */ 0x001000100C8FFC01UL,
+	/* 0x0240, */ 0x001000100C8FFC01UL,
+	/* 0x0248, */ 0x001000100C8FFC01UL,
+	/* 0x0250, */ 0x001000100C8FFC01UL,
+	/* 0x0258, */ 0x001000100C8FFC01UL,
+	/* 0x0260, */ 0x001000100C8FFC01UL,
+	/* 0x0268, */ 0x001000100C8FFC01UL,
+	/* 0x0270, */ 0x001000100C8FFC01UL,
+	/* 0x0278, */ 0x001000100C8FFC01UL,
+	/* 0x0280, */ 0x001000100C8FFC01UL,
+	/* 0x0288, */ 0x001000100C8FFC01UL,
+	/* 0x0290, */ 0x001000100C8FFC01UL,
+	/* 0x0298, */ 0x001000100C8FFC01UL,
+	/* 0x02A0, */ 0x001000100C8FFC01UL,
+	/* 0x02A8, */ 0x001000100C8FFC01UL,
+	/* 0x02B0, */ 0x001000100C8FFC01UL,
+	/* 0x02B8, */ 0x001000100C8FFC01UL,
+	/* 0x02C0, */ 0x001000100C8FFC01UL,
+	/* 0x02C8, */ 0x001000100C8FFC01UL,
+	/* 0x02D0, */ 0x001000100C8FFC01UL,
+	/* 0x02D8, */ 0x001000100C8FFC01UL,
+	/* 0x02E0, */ 0x001000100C8FFC01UL,
+	/* 0x02E8, */ 0x001000100C8FFC01UL,
+	/* 0x02F0, */ 0x001000200C8FFC01UL,
+	/* 0x02F8, */ 0x001000300C8FFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x001000200C8FFC01UL,
+	/* 0x0310, */ 0x001000300C8FFC01UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x001000200C8FFC01UL,
+	/* 0x0328, */ 0x001000300C8FFC01UL,
+	/* 0x0330, */ 0x001000200C8FFC01UL,
+	/* 0x0338, */ 0x001000300C8FFC01UL,
+};
+#endif
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v11.c b/drivers/renesas/rcar/qos/H3/qos_init_h3_v11.c
new file mode 100644
index 0000000..329bcb8
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v11.c
@@ -0,0 +1,200 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include <rcar_def.h>
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+#include "qos_init_h3_v11.h"
+
+#define	RCAR_QOS_VERSION		"rev.0.37"
+
+#include "qos_init_h3_v11_mstat.h"
+
+struct rcar_gen3_dbsc_qos_settings h3_v11_qos[] = {
+	/* BUFCAM settings */
+	/* DBSC_DBCAM0CNF0 not set */
+	{ DBSC_DBCAM0CNF1, 0x00044218 },
+	{ DBSC_DBCAM0CNF2, 0x000000F4 },
+	/* DBSC_DBCAM0CNF3 not set */
+	{ DBSC_DBSCHCNT0, 0x080F0037 },
+	{ DBSC_DBSCHCNT1, 0x00001010 },
+	{ DBSC_DBSCHSZ0, 0x00000001 },
+	{ DBSC_DBSCHRW0, 0x22421111 },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123 },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x0000F000 },
+	{ DBSC_DBSCHQOS01, 0x0000E000 },
+	{ DBSC_DBSCHQOS02, 0x00007000 },
+	{ DBSC_DBSCHQOS03, 0x00000000 },
+	{ DBSC_DBSCHQOS40, 0x00000E00 },
+	{ DBSC_DBSCHQOS41, 0x00000DFF },
+	{ DBSC_DBSCHQOS42, 0x00000400 },
+	{ DBSC_DBSCHQOS43, 0x00000200 },
+	{ DBSC_DBSCHQOS90, 0x00000C00 },
+	{ DBSC_DBSCHQOS91, 0x00000BFF },
+	{ DBSC_DBSCHQOS92, 0x00000400 },
+	{ DBSC_DBSCHQOS93, 0x00000200 },
+	{ DBSC_DBSCHQOS130, 0x00000980 },
+	{ DBSC_DBSCHQOS131, 0x0000097F },
+	{ DBSC_DBSCHQOS132, 0x00000300 },
+	{ DBSC_DBSCHQOS133, 0x00000180 },
+	{ DBSC_DBSCHQOS140, 0x00000800 },
+	{ DBSC_DBSCHQOS141, 0x000007FF },
+	{ DBSC_DBSCHQOS142, 0x00000300 },
+	{ DBSC_DBSCHQOS143, 0x00000180 },
+	{ DBSC_DBSCHQOS150, 0x000007D0 },
+	{ DBSC_DBSCHQOS151, 0x000007CF },
+	{ DBSC_DBSCHQOS152, 0x000005D0 },
+	{ DBSC_DBSCHQOS153, 0x000003D0 },
+};
+
+void qos_init_h3_v11(void)
+{
+	rcar_qos_dbsc_setting(h3_v11_qos, ARRAY_SIZE(h3_v11_qos), false);
+
+	/* DRAM Split Address mapping */
+#if (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH) || \
+    (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO)
+	NOTICE("BL2: DRAM Split is 4ch\n");
+	io_write_32(AXI_ADSPLCR0, ADSPLCR0_ADRMODE_DEFAULT
+		    | ADSPLCR0_SPLITSEL(0xFFU)
+		    | ADSPLCR0_AREA(0x1BU)
+		    | ADSPLCR0_SWP);
+	io_write_32(AXI_ADSPLCR1, 0x00000000U);
+	io_write_32(AXI_ADSPLCR2, 0xA8A90000U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#elif RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH
+	NOTICE("BL2: DRAM Split is 2ch\n");
+	io_write_32(AXI_ADSPLCR0, 0x00000000U);
+	io_write_32(AXI_ADSPLCR1, ADSPLCR0_ADRMODE_DEFAULT
+		    | ADSPLCR0_SPLITSEL(0xFFU)
+		    | ADSPLCR0_AREA(0x1BU)
+		    | ADSPLCR0_SWP);
+	io_write_32(AXI_ADSPLCR2, 0x00000000U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+	/* AR Cache setting */
+	io_write_32(0xE67D1000U, 0x00000100U);
+	io_write_32(0xE67D1008U, 0x00000100U);
+
+	/* Resource Alloc setting */
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH
+	io_write_32(QOSCTRL_RAS, 0x00000020U);
+#else
+	io_write_32(QOSCTRL_RAS, 0x00000040U);
+#endif
+	io_write_32(QOSCTRL_FIXTH, 0x000F0005U);
+	io_write_32(QOSCTRL_REGGD, 0x00000000U);
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH
+	io_write_64(QOSCTRL_DANN, 0x0101010102020201UL);
+	io_write_32(QOSCTRL_DANT, 0x00181008U);
+#else
+	io_write_64(QOSCTRL_DANN, 0x0101000004040401UL);
+	io_write_32(QOSCTRL_DANT, 0x003C2010U);
+#endif
+	io_write_32(QOSCTRL_EC, 0x00080001U);	/* need for H3 v1.* */
+	io_write_64(QOSCTRL_EMS, 0x0000000000000000UL);
+	io_write_32(QOSCTRL_INSFC, 0xC7840001U);
+	io_write_32(QOSCTRL_BERR, 0x00000000U);
+	io_write_32(QOSCTRL_RACNT0, 0x00000000U);
+
+	/* QOSBW setting */
+	io_write_32(QOSCTRL_SL_INIT,
+		    SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT | SL_INIT_SSLOTCLK);
+	io_write_32(QOSCTRL_REF_ARS, 0x00330000U);
+
+	/* QOSBW SRAM setting */
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		io_write_64(QOSBW_FIX_QOS_BANK0 + i * 8, mstat_fix[i]);
+		io_write_64(QOSBW_FIX_QOS_BANK1 + i * 8, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		io_write_64(QOSBW_BE_QOS_BANK0 + i * 8, mstat_be[i]);
+		io_write_64(QOSBW_BE_QOS_BANK1 + i * 8, mstat_be[i]);
+	}
+
+	/* 3DG bus Leaf setting */
+	io_write_32(0xFD820808U, 0x00001234U);
+	io_write_32(0xFD820800U, 0x0000003FU);
+	io_write_32(0xFD821800U, 0x0000003FU);
+	io_write_32(0xFD822800U, 0x0000003FU);
+	io_write_32(0xFD823800U, 0x0000003FU);
+	io_write_32(0xFD824800U, 0x0000003FU);
+	io_write_32(0xFD825800U, 0x0000003FU);
+	io_write_32(0xFD826800U, 0x0000003FU);
+	io_write_32(0xFD827800U, 0x0000003FU);
+
+	/* VIO bus Leaf setting */
+	io_write_32(0xFEB89800, 0x00000001U);
+	io_write_32(0xFEB8A800, 0x00000001U);
+	io_write_32(0xFEB8B800, 0x00000001U);
+	io_write_32(0xFEB8C800, 0x00000001U);
+
+	/* HSC bus Leaf setting */
+	io_write_32(0xE6430800, 0x00000001U);
+	io_write_32(0xE6431800, 0x00000001U);
+	io_write_32(0xE6432800, 0x00000001U);
+	io_write_32(0xE6433800, 0x00000001U);
+
+	/* MP bus Leaf setting */
+	io_write_32(0xEC620800, 0x00000001U);
+	io_write_32(0xEC621800, 0x00000001U);
+
+	/* PERIE bus Leaf setting */
+	io_write_32(0xE7760800, 0x00000001U);
+	io_write_32(0xE7768800, 0x00000001U);
+
+	/* PERIW bus Leaf setting */
+	io_write_32(0xE6760800, 0x00000001U);
+	io_write_32(0xE6768800, 0x00000001U);
+
+	/* RT bus Leaf setting */
+	io_write_32(0xFFC50800, 0x00000001U);
+	io_write_32(0xFFC51800, 0x00000001U);
+
+	/* CCI bus Leaf setting */
+	uint32_t modemr = io_read_32(RCAR_MODEMR);
+
+	modemr &= MODEMR_BOOT_CPU_MASK;
+
+	if ((modemr == MODEMR_BOOT_CPU_CA57) ||
+	    (modemr == MODEMR_BOOT_CPU_CA53)) {
+		io_write_32(0xF1300800, 0x00000001U);
+		io_write_32(0xF1340800, 0x00000001U);
+		io_write_32(0xF1380800, 0x00000001U);
+		io_write_32(0xF13C0800, 0x00000001U);
+	}
+
+	/* Resource Alloc start */
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+	/* QOSBW start */
+	io_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	/* Resource Alloc setting */
+	io_write_32(QOSCTRL_EC, 0x00080001U);	/* need for H3 v1.* */
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v11.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v11.h
new file mode 100644
index 0000000..3faeb4f
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v11.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_H3_V11_H
+#define QOS_INIT_H3_V11_H
+
+void qos_init_h3_v11(void);
+
+#endif /* QOS_INIT_H3_V11_H */
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v11_mstat.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v11_mstat.h
new file mode 100644
index 0000000..46c68c8
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v11_mstat.h
@@ -0,0 +1,221 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+static const uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004030000FFFFUL,
+	/* 0x0038, */ 0x001008060000FFFFUL,
+	/* 0x0040, */ 0x001414090000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001410010000FFFFUL,
+	/* 0x0058, */ 0x00140C0C0000FFFFUL,
+	/* 0x0060, */ 0x00140C0C0000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001410010000FFFFUL,
+	/* 0x0078, */ 0x001008060000FFFFUL,
+	/* 0x0080, */ 0x001004020000FFFFUL,
+	/* 0x0088, */ 0x001414090000FFFFUL,
+	/* 0x0090, */ 0x00140C0C0000FFFFUL,
+	/* 0x0098, */ 0x001408080000FFFFUL,
+	/* 0x00A0, */ 0x000C08020000FFFFUL,
+	/* 0x00A8, */ 0x000C04010000FFFFUL,
+	/* 0x00B0, */ 0x000C04010000FFFFUL,
+	/* 0x00B8, */ 0x0000000000000000UL,
+	/* 0x00C0, */ 0x000C08020000FFFFUL,
+	/* 0x00C8, */ 0x000C04010000FFFFUL,
+	/* 0x00D0, */ 0x000C04010000FFFFUL,
+	/* 0x00D8, */ 0x000C04030000FFFFUL,
+	/* 0x00E0, */ 0x000C100F0000FFFFUL,
+	/* 0x00E8, */ 0x0000000000000000UL,
+	/* 0x00F0, */ 0x001010080000FFFFUL,
+	/* 0x00F8, */ 0x001010080000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x000C04030000FFFFUL,
+	/* 0x0110, */ 0x001010080000FFFFUL,
+	/* 0x0118, */ 0x001010080000FFFFUL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x000C100E0000FFFFUL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x001008050000FFFFUL,
+	/* 0x0140, */ 0x001008050000FFFFUL,
+	/* 0x0148, */ 0x001008050000FFFFUL,
+	/* 0x0150, */ 0x001008050000FFFFUL,
+	/* 0x0158, */ 0x001008050000FFFFUL,
+	/* 0x0160, */ 0x001008050000FFFFUL,
+	/* 0x0168, */ 0x001008050000FFFFUL,
+	/* 0x0170, */ 0x001008050000FFFFUL,
+	/* 0x0178, */ 0x001004030000FFFFUL,
+	/* 0x0180, */ 0x001004030000FFFFUL,
+	/* 0x0188, */ 0x001004030000FFFFUL,
+	/* 0x0190, */ 0x001014140000FFFFUL,
+	/* 0x0198, */ 0x001014140000FFFFUL,
+	/* 0x01A0, */ 0x001008050000FFFFUL,
+	/* 0x01A8, */ 0x001008050000FFFFUL,
+	/* 0x01B0, */ 0x001008050000FFFFUL,
+	/* 0x01B8, */ 0x0000000000000000UL,
+	/* 0x01C0, */ 0x0000000000000000UL,
+	/* 0x01C8, */ 0x0000000000000000UL,
+	/* 0x01D0, */ 0x0000000000000000UL,
+	/* 0x01D8, */ 0x0000000000000000UL,
+	/* 0x01E0, */ 0x0000000000000000UL,
+	/* 0x01E8, */ 0x0000000000000000UL,
+	/* 0x01F0, */ 0x0000000000000000UL,
+	/* 0x01F8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x001408010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02A0, */ 0x000C04010000FFFFUL,
+	/* 0x02A8, */ 0x000C04010000FFFFUL,
+	/* 0x02B0, */ 0x001404010000FFFFUL,
+	/* 0x02B8, */ 0x0000000000000000UL,
+	/* 0x02C0, */ 0x0000000000000000UL,
+	/* 0x02C8, */ 0x0000000000000000UL,
+	/* 0x02D0, */ 0x000C04010000FFFFUL,
+	/* 0x02D8, */ 0x000C04010000FFFFUL,
+	/* 0x02E0, */ 0x001404010000FFFFUL,
+	/* 0x02E8, */ 0x0000000000000000UL,
+	/* 0x02F0, */ 0x0000000000000000UL,
+	/* 0x02F8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+};
+
+static const uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x001200100C89C401UL,
+	/* 0x0008, */ 0x001200100C89C401UL,
+	/* 0x0010, */ 0x001200100C89C401UL,
+	/* 0x0018, */ 0x001200100C89C401UL,
+	/* 0x0020, */ 0x001100100C803401UL,
+	/* 0x0028, */ 0x001100100C80FC01UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x001100100C803401UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00A0, */ 0x0000000000000000UL,
+	/* 0x00A8, */ 0x0000000000000000UL,
+	/* 0x00B0, */ 0x0000000000000000UL,
+	/* 0x00B8, */ 0x001100100C803401UL,
+	/* 0x00C0, */ 0x0000000000000000UL,
+	/* 0x00C8, */ 0x0000000000000000UL,
+	/* 0x00D0, */ 0x0000000000000000UL,
+	/* 0x00D8, */ 0x0000000000000000UL,
+	/* 0x00E0, */ 0x0000000000000000UL,
+	/* 0x00E8, */ 0x001100100C803401UL,
+	/* 0x00F0, */ 0x0000000000000000UL,
+	/* 0x00F8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x001100100C803401UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01A0, */ 0x0000000000000000UL,
+	/* 0x01A8, */ 0x0000000000000000UL,
+	/* 0x01B0, */ 0x0000000000000000UL,
+	/* 0x01B8, */ 0x001100100C803401UL,
+	/* 0x01C0, */ 0x001100800C8FFC01UL,
+	/* 0x01C8, */ 0x001100800C8FFC01UL,
+	/* 0x01D0, */ 0x001100800C8FFC01UL,
+	/* 0x01D8, */ 0x001100800C8FFC01UL,
+	/* 0x01E0, */ 0x001100100C80FC01UL,
+	/* 0x01E8, */ 0x001200100C80FC01UL,
+	/* 0x01F0, */ 0x001100100C80FC01UL,
+	/* 0x01F8, */ 0x001100100C803401UL,
+	/* 0x0200, */ 0x001100100C80FC01UL,
+	/* 0x0208, */ 0x001200100C80FC01UL,
+	/* 0x0210, */ 0x001100100C80FC01UL,
+	/* 0x0218, */ 0x001100100C825801UL,
+	/* 0x0220, */ 0x001100100C825801UL,
+	/* 0x0228, */ 0x001100100C803401UL,
+	/* 0x0230, */ 0x001100100C825801UL,
+	/* 0x0238, */ 0x001100100C825801UL,
+	/* 0x0240, */ 0x001200100C8BB801UL,
+	/* 0x0248, */ 0x001100200C8FFC01UL,
+	/* 0x0250, */ 0x001200100C8BB801UL,
+	/* 0x0258, */ 0x001100200C8FFC01UL,
+	/* 0x0260, */ 0x001100100C84E401UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x001100100C81F401UL,
+	/* 0x0280, */ 0x001100100C803401UL,
+	/* 0x0288, */ 0x001100100C803401UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02A0, */ 0x0000000000000000UL,
+	/* 0x02A8, */ 0x0000000000000000UL,
+	/* 0x02B0, */ 0x0000000000000000UL,
+	/* 0x02B8, */ 0x001100100C803401UL,
+	/* 0x02C0, */ 0x001100100C803401UL,
+	/* 0x02C8, */ 0x001100100C803401UL,
+	/* 0x02D0, */ 0x0000000000000000UL,
+	/* 0x02D8, */ 0x0000000000000000UL,
+	/* 0x02E0, */ 0x0000000000000000UL,
+	/* 0x02E8, */ 0x001100100C803401UL,
+	/* 0x02F0, */ 0x001100300C8FFC01UL,
+	/* 0x02F8, */ 0x001100500C8FFC01UL,
+	/* 0x0300, */ 0x001100100C803401UL,
+	/* 0x0308, */ 0x001100300C8FFC01UL,
+	/* 0x0310, */ 0x001100500C8FFC01UL,
+	/* 0x0318, */ 0x001200100C803401UL,
+	/* 0x0320, */ 0x001100300C8FFC01UL,
+	/* 0x0328, */ 0x001100500C8FFC01UL,
+	/* 0x0330, */ 0x001100300C8FFC01UL,
+	/* 0x0338, */ 0x001100500C8FFC01UL,
+};
+#endif
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v20.c b/drivers/renesas/rcar/qos/H3/qos_init_h3_v20.c
new file mode 100644
index 0000000..c20ab08
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v20.c
@@ -0,0 +1,234 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+#include "qos_init_h3_v20.h"
+
+#define RCAR_QOS_VERSION			"rev.0.21"
+
+#define QOSWT_TIME_BANK0			20000000U	/* unit:ns */
+
+#define QOSWT_WTEN_ENABLE			0x1U
+
+#define QOSCTRL_REF_ARS_ARBSTOPCYCLE_H3_20	(SL_INIT_SSLOTCLK_H3_20 - 0x5U)
+
+#define OSWT_WTREF_SLOT0_EN_REQ1_SLOT		3U
+#define OSWT_WTREF_SLOT0_EN_REQ2_SLOT		9U
+#define QOSWT_WTREF_SLOT0_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+#define QOSWT_WTREF_SLOT1_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+
+#define QOSWT_WTSET0_REQ_SSLOT0			5U
+#define WT_BASE_SUB_SLOT_NUM0			12U
+#define QOSWT_WTSET0_PERIOD0_H3_20			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_H3_20) - 1U)
+#define QOSWT_WTSET0_SSLOT0			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET0_SLOTSLOT0			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#define QOSWT_WTSET1_PERIOD1_H3_20			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_H3_20) - 1U)
+#define QOSWT_WTSET1_SSLOT1			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET1_SLOTSLOT1			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_h3_v20_mstat195.h"
+#else
+#include "qos_init_h3_v20_mstat390.h"
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_h3_v20_qoswt195.h"
+#else
+#include "qos_init_h3_v20_qoswt390.h"
+#endif
+
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+#endif
+
+struct rcar_gen3_dbsc_qos_settings h3_v20_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218U },
+	{ DBSC_DBCAM0CNF2, 0x000000F4U },
+	{ DBSC_DBCAM0CNF3, 0x00000000U },
+	{ DBSC_DBSCHCNT0, 0x000F0037U },
+	{ DBSC_DBSCHSZ0, 0x00000001U },
+	{ DBSC_DBSCHRW0, 0x22421111U },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123U },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00U },
+	{ DBSC_DBSCHQOS01, 0x00000B00U },
+	{ DBSC_DBSCHQOS02, 0x00000000U },
+	{ DBSC_DBSCHQOS03, 0x00000000U },
+	{ DBSC_DBSCHQOS40, 0x00000300U },
+	{ DBSC_DBSCHQOS41, 0x000002F0U },
+	{ DBSC_DBSCHQOS42, 0x00000200U },
+	{ DBSC_DBSCHQOS43, 0x00000100U },
+	{ DBSC_DBSCHQOS90, 0x00000100U },
+	{ DBSC_DBSCHQOS91, 0x000000F0U },
+	{ DBSC_DBSCHQOS92, 0x000000A0U },
+	{ DBSC_DBSCHQOS93, 0x00000040U },
+	{ DBSC_DBSCHQOS120, 0x00000040U },
+	{ DBSC_DBSCHQOS121, 0x00000030U },
+	{ DBSC_DBSCHQOS122, 0x00000020U },
+	{ DBSC_DBSCHQOS123, 0x00000010U },
+	{ DBSC_DBSCHQOS130, 0x00000100U },
+	{ DBSC_DBSCHQOS131, 0x000000F0U },
+	{ DBSC_DBSCHQOS132, 0x000000A0U },
+	{ DBSC_DBSCHQOS133, 0x00000040U },
+	{ DBSC_DBSCHQOS140, 0x000000C0U },
+	{ DBSC_DBSCHQOS141, 0x000000B0U },
+	{ DBSC_DBSCHQOS142, 0x00000080U },
+	{ DBSC_DBSCHQOS143, 0x00000040U },
+	{ DBSC_DBSCHQOS150, 0x00000040U },
+	{ DBSC_DBSCHQOS151, 0x00000030U },
+	{ DBSC_DBSCHQOS152, 0x00000020U },
+	{ DBSC_DBSCHQOS153, 0x00000010U },
+};
+
+void qos_init_h3_v20(void)
+{
+	rcar_qos_dbsc_setting(h3_v20_qos, ARRAY_SIZE(h3_v20_qos), true);
+
+	/* DRAM Split Address mapping */
+#if (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH) || \
+    (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO)
+	NOTICE("BL2: DRAM Split is 4ch\n");
+	io_write_32(AXI_ADSPLCR0, ADSPLCR0_ADRMODE_DEFAULT
+		    | ADSPLCR0_SPLITSEL(0xFFU)
+		    | ADSPLCR0_AREA(0x1BU)
+		    | ADSPLCR0_SWP);
+	io_write_32(AXI_ADSPLCR1, 0x00000000U);
+	io_write_32(AXI_ADSPLCR2, 0x00001054U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#elif RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH
+	NOTICE("BL2: DRAM Split is 2ch\n");
+	io_write_32(AXI_ADSPLCR0, 0x00000000U);
+	io_write_32(AXI_ADSPLCR1, ADSPLCR0_ADRMODE_DEFAULT
+		    | ADSPLCR0_SPLITSEL(0xFFU)
+		    | ADSPLCR0_AREA(0x1BU)
+		    | ADSPLCR0_SWP);
+	io_write_32(AXI_ADSPLCR2, 0x00001004U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+	NOTICE("BL2: DRAM refresh interval 1.95 usec\n");
+#else
+	NOTICE("BL2: DRAM refresh interval 3.9 usec\n");
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	NOTICE("BL2: Periodic Write DQ Training\n");
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	io_write_32(QOSCTRL_RAS, 0x00000044U);
+	io_write_64(QOSCTRL_DANN, 0x0404010002020201UL);
+	io_write_32(QOSCTRL_DANT, 0x0020100AU);
+	io_write_32(QOSCTRL_INSFC, 0x06330001U);
+	io_write_32(QOSCTRL_RACNT0, 0x00010003U);
+
+	/* GPU Boost Mode */
+	io_write_32(QOSCTRL_STATGEN0, 0x00000001U);
+
+	io_write_32(QOSCTRL_SL_INIT,
+		    SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT |
+		    SL_INIT_SSLOTCLK_H3_20);
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	io_write_32(QOSCTRL_REF_ARS,
+		    ((QOSCTRL_REF_ARS_ARBSTOPCYCLE_H3_20 << 16)));
+#else
+	io_write_32(QOSCTRL_REF_ARS, 0x00330000U);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		io_write_64(QOSBW_FIX_QOS_BANK0 + i * 8, mstat_fix[i]);
+		io_write_64(QOSBW_FIX_QOS_BANK1 + i * 8, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		io_write_64(QOSBW_BE_QOS_BANK0 + i * 8, mstat_be[i]);
+		io_write_64(QOSBW_BE_QOS_BANK1 + i * 8, mstat_be[i]);
+	}
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	for (i = 0U; i < ARRAY_SIZE(qoswt_fix); i++) {
+		io_write_64(QOSWT_FIX_WTQOS_BANK0 + i * 8,
+			    qoswt_fix[i]);
+		io_write_64(QOSWT_FIX_WTQOS_BANK1 + i * 8,
+			    qoswt_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(qoswt_be); i++) {
+		io_write_64(QOSWT_BE_WTQOS_BANK0 + i * 8, qoswt_be[i]);
+		io_write_64(QOSWT_BE_WTQOS_BANK1 + i * 8, qoswt_be[i]);
+	}
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	/* 3DG bus Leaf setting */
+	io_write_32(GPU_ACT0, 0x00000000U);
+	io_write_32(GPU_ACT1, 0x00000000U);
+	io_write_32(GPU_ACT2, 0x00000000U);
+	io_write_32(GPU_ACT3, 0x00000000U);
+	io_write_32(GPU_ACT4, 0x00000000U);
+	io_write_32(GPU_ACT5, 0x00000000U);
+	io_write_32(GPU_ACT6, 0x00000000U);
+	io_write_32(GPU_ACT7, 0x00000000U);
+
+	/* RT bus Leaf setting */
+	io_write_32(RT_ACT0, 0x00000000U);
+	io_write_32(RT_ACT1, 0x00000000U);
+
+	/* CCI bus Leaf setting */
+	io_write_32(CPU_ACT0, 0x00000003U);
+	io_write_32(CPU_ACT1, 0x00000003U);
+	io_write_32(CPU_ACT2, 0x00000003U);
+	io_write_32(CPU_ACT3, 0x00000003U);
+
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	/*  re-write training setting */
+	io_write_32(QOSWT_WTREF,
+		    ((QOSWT_WTREF_SLOT1_EN << 16) | QOSWT_WTREF_SLOT0_EN));
+	io_write_32(QOSWT_WTSET0,
+		    ((QOSWT_WTSET0_PERIOD0_H3_20 << 16) |
+		     (QOSWT_WTSET0_SSLOT0 << 8) | QOSWT_WTSET0_SLOTSLOT0));
+	io_write_32(QOSWT_WTSET1,
+		    ((QOSWT_WTSET1_PERIOD1_H3_20 << 16) |
+		     (QOSWT_WTSET1_SSLOT1 << 8) | QOSWT_WTSET1_SLOTSLOT1));
+
+	io_write_32(QOSWT_WTEN, QOSWT_WTEN_ENABLE);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	io_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v20.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v20.h
new file mode 100644
index 0000000..9b7619e
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v20.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_H3_V20_H
+#define QOS_INIT_H3_V20_H
+
+void qos_init_h3_v20(void);
+
+#endif /* QOS_INIT_H3_V20_H */
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v20_mstat195.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v20_mstat195.h
new file mode 100644
index 0000000..3995df3
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v20_mstat195.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000FFFFUL,
+	/* 0x0038, */ 0x001008070000FFFFUL,
+	/* 0x0040, */ 0x001424110000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x001410100000FFFFUL,
+	/* 0x0060, */ 0x0014100D0000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x001008070000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001424110000FFFFUL,
+	/* 0x0090, */ 0x0014100D0000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C04020000FFFFUL,
+	/* 0x00a8, */ 0x000C04010000FFFFUL,
+	/* 0x00b0, */ 0x000C04010000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C04020000FFFFUL,
+	/* 0x00c8, */ 0x000C04010000FFFFUL,
+	/* 0x00d0, */ 0x000C04010000FFFFUL,
+	/* 0x00d8, */ 0x001024090000FFFFUL,
+	/* 0x00e0, */ 0x00100C090000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001024090000FFFFUL,
+	/* 0x00f8, */ 0x000C08070000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x00100C090000FFFFUL,
+	/* 0x0118, */ 0x000C10100000FFFFUL,
+	/* 0x0120, */ 0x000C10100000FFFFUL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100C0B0000FFFFUL,
+	/* 0x0140, */ 0x00100C0B0000FFFFUL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0010100D0000FFFFUL,
+	/* 0x0158, */ 0x0010100D0000FFFFUL,
+	/* 0x0160, */ 0x00100C0B0000FFFFUL,
+	/* 0x0168, */ 0x00100C0B0000FFFFUL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008060000FFFFUL,
+	/* 0x0180, */ 0x001008060000FFFFUL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00102C2C0000FFFFUL,
+	/* 0x0198, */ 0x00102C2C0000FFFFUL,
+	/* 0x01a0, */ 0x00100C0B0000FFFFUL,
+	/* 0x01a8, */ 0x00100C0B0000FFFFUL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFFFUL,
+	/* 0x0268, */ 0x001408010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C04010000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x001408010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x001408010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x001200200BDFFC01UL,
+	/* 0x0008, */ 0x001200200BDFFC01UL,
+	/* 0x0010, */ 0x001200200BDFFC01UL,
+	/* 0x0018, */ 0x001200200BDFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x001200100BD0FC01UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100700BDFFC01UL,
+	/* 0x01c8, */ 0x002100700BDFFC01UL,
+	/* 0x01d0, */ 0x002100700BDFFC01UL,
+	/* 0x01d8, */ 0x002100700BDFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x002100200BDFFC01UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x002100200BDFFC01UL,
+	/* 0x0218, */ 0x001100200BDFFC01UL,
+	/* 0x0220, */ 0x001100200BDFFC01UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x001100200BDFFC01UL,
+	/* 0x0238, */ 0x001100200BDFFC01UL,
+	/* 0x0240, */ 0x001200200BDFFC01UL,
+	/* 0x0248, */ 0x001100200BDFFC01UL,
+	/* 0x0250, */ 0x001200200BDFFC01UL,
+	/* 0x0258, */ 0x001100200BDFFC01UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x001100400BDFFC01UL,
+	/* 0x02f8, */ 0x001100600BDFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x001100400BDFFC01UL,
+	/* 0x0310, */ 0x001100600BDFFC01UL,
+	/* 0x0318, */ 0x001200100BD03401UL,
+	/* 0x0320, */ 0x001100400BDFFC01UL,
+	/* 0x0328, */ 0x001100600BDFFC01UL,
+	/* 0x0330, */ 0x001100400BDFFC01UL,
+	/* 0x0338, */ 0x001100600BDFFC01UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x001200100BD0FC01UL,
+};
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v20_mstat390.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v20_mstat390.h
new file mode 100644
index 0000000..770c022
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v20_mstat390.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000FFFFUL,
+	/* 0x0038, */ 0x0010100D0000FFFFUL,
+	/* 0x0040, */ 0x001444210000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x0014201F0000FFFFUL,
+	/* 0x0060, */ 0x00141C190000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x0010100D0000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001444210000FFFFUL,
+	/* 0x0090, */ 0x00141C190000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C08040000FFFFUL,
+	/* 0x00a8, */ 0x000C04020000FFFFUL,
+	/* 0x00b0, */ 0x000C04020000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C08040000FFFFUL,
+	/* 0x00c8, */ 0x000C04020000FFFFUL,
+	/* 0x00d0, */ 0x000C04020000FFFFUL,
+	/* 0x00d8, */ 0x001044110000FFFFUL,
+	/* 0x00e0, */ 0x001014110000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001044110000FFFFUL,
+	/* 0x00f8, */ 0x000C100D0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001014110000FFFFUL,
+	/* 0x0118, */ 0x000C20200000FFFFUL,
+	/* 0x0120, */ 0x000C20200000FFFFUL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x001018150000FFFFUL,
+	/* 0x0140, */ 0x001018150000FFFFUL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00101C190000FFFFUL,
+	/* 0x0158, */ 0x00101C190000FFFFUL,
+	/* 0x0160, */ 0x001018150000FFFFUL,
+	/* 0x0168, */ 0x001018150000FFFFUL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x00100C0B0000FFFFUL,
+	/* 0x0180, */ 0x00100C0B0000FFFFUL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001058570000FFFFUL,
+	/* 0x0198, */ 0x001058570000FFFFUL,
+	/* 0x01a0, */ 0x001018150000FFFFUL,
+	/* 0x01a8, */ 0x001018150000FFFFUL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFFFUL,
+	/* 0x0268, */ 0x001410010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C08020000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x00140C010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x00140C010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0012003005EFFC01UL,
+	/* 0x0008, */ 0x0012003005EFFC01UL,
+	/* 0x0010, */ 0x0012003005EFFC01UL,
+	/* 0x0018, */ 0x0012003005EFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001005E0FC01UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100D005EFFC01UL,
+	/* 0x01c8, */ 0x002100D005EFFC01UL,
+	/* 0x01d0, */ 0x002100D005EFFC01UL,
+	/* 0x01d8, */ 0x002100D005EFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021003005EFFC01UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021003005EFFC01UL,
+	/* 0x0218, */ 0x0011003005EFFC01UL,
+	/* 0x0220, */ 0x0011003005EFFC01UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011003005EFFC01UL,
+	/* 0x0238, */ 0x0011003005EFFC01UL,
+	/* 0x0240, */ 0x0012003005EFFC01UL,
+	/* 0x0248, */ 0x0011003005EFFC01UL,
+	/* 0x0250, */ 0x0012003005EFFC01UL,
+	/* 0x0258, */ 0x0011003005EFFC01UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011007005EFFC01UL,
+	/* 0x02f8, */ 0x001100B005EFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0011007005EFFC01UL,
+	/* 0x0310, */ 0x001100B005EFFC01UL,
+	/* 0x0318, */ 0x0012001005E03401UL,
+	/* 0x0320, */ 0x0011007005EFFC01UL,
+	/* 0x0328, */ 0x001100B005EFFC01UL,
+	/* 0x0330, */ 0x0011007005EFFC01UL,
+	/* 0x0338, */ 0x001100B005EFFC01UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0012001005E0FC01UL,
+};
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v20_qoswt195.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v20_qoswt195.h
new file mode 100644
index 0000000..82e4b01
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v20_qoswt195.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000C010UL,
+	/* 0x0038, */ 0x001008070000C010UL,
+	/* 0x0040, */ 0x001424110000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x001410100000C010UL,
+	/* 0x0060, */ 0x0014100D0000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x001008070000C010UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001424110000FFF0UL,
+	/* 0x0090, */ 0x0014100D0000C010UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFF0UL,
+	/* 0x0268, */ 0x001408010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C04010000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v20_qoswt390.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v20_qoswt390.h
new file mode 100644
index 0000000..f3e7360
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v20_qoswt390.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000C010UL,
+	/* 0x0038, */ 0x0010100D0000C010UL,
+	/* 0x0040, */ 0x001444210000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0014201F0000C010UL,
+	/* 0x0060, */ 0x00141C190000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0010100D0000C010UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001444210000FFF0UL,
+	/* 0x0090, */ 0x00141C190000C010UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFF0UL,
+	/* 0x0268, */ 0x001410010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C08020000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v30.c b/drivers/renesas/rcar/qos/H3/qos_init_h3_v30.c
new file mode 100644
index 0000000..1fe6182
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v30.c
@@ -0,0 +1,236 @@
+/*
+ * Copyright (c) 2018-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+#include "qos_init_h3_v30.h"
+
+#define	RCAR_QOS_VERSION			"rev.0.11"
+
+#define QOSWT_TIME_BANK0			20000000U	/* unit:ns */
+
+#define	QOSWT_WTEN_ENABLE			0x1U
+
+#define QOSCTRL_REF_ARS_ARBSTOPCYCLE_H3_30	(SL_INIT_SSLOTCLK_H3_30 - 0x5U)
+
+#define OSWT_WTREF_SLOT0_EN_REQ1_SLOT		3U
+#define OSWT_WTREF_SLOT0_EN_REQ2_SLOT		9U
+#define QOSWT_WTREF_SLOT0_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+#define QOSWT_WTREF_SLOT1_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+
+#define QOSWT_WTSET0_REQ_SSLOT0			5U
+#define WT_BASE_SUB_SLOT_NUM0			12U
+#define QOSWT_WTSET0_PERIOD0_H3_30			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_H3_30) - 1U)
+#define QOSWT_WTSET0_SSLOT0			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET0_SLOTSLOT0			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#define QOSWT_WTSET1_PERIOD1_H3_30		(QOSWT_WTSET0_PERIOD0_H3_30)
+#define QOSWT_WTSET1_SSLOT1			(QOSWT_WTSET0_SSLOT0)
+#define QOSWT_WTSET1_SLOTSLOT1			(QOSWT_WTSET0_SLOTSLOT0)
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_h3_v30_mstat195.h"
+#else
+#include "qos_init_h3_v30_mstat390.h"
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_h3_v30_qoswt195.h"
+#else
+#include "qos_init_h3_v30_qoswt390.h"
+#endif
+
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+#endif
+
+struct rcar_gen3_dbsc_qos_settings h3_v30_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218U },
+	{ DBSC_DBCAM0CNF2, 0x000000F4U },
+	{ DBSC_DBCAM0CNF3, 0x00000000U },
+	{ DBSC_DBSCHCNT0, 0x000F0037U },
+	{ DBSC_DBSCHSZ0, 0x00000001U },
+	{ DBSC_DBSCHRW0, 0x22421111U },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123U },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00U },
+	{ DBSC_DBSCHQOS01, 0x00000B00U },
+	{ DBSC_DBSCHQOS02, 0x00000000U },
+	{ DBSC_DBSCHQOS03, 0x00000000U },
+	{ DBSC_DBSCHQOS40, 0x00000300U },
+	{ DBSC_DBSCHQOS41, 0x000002F0U },
+	{ DBSC_DBSCHQOS42, 0x00000200U },
+	{ DBSC_DBSCHQOS43, 0x00000100U },
+	{ DBSC_DBSCHQOS90, 0x00000100U },
+	{ DBSC_DBSCHQOS91, 0x000000F0U },
+	{ DBSC_DBSCHQOS92, 0x000000A0U },
+	{ DBSC_DBSCHQOS93, 0x00000040U },
+	{ DBSC_DBSCHQOS120, 0x00000040U },
+	{ DBSC_DBSCHQOS121, 0x00000030U },
+	{ DBSC_DBSCHQOS122, 0x00000020U },
+	{ DBSC_DBSCHQOS123, 0x00000010U },
+	{ DBSC_DBSCHQOS130, 0x00000100U },
+	{ DBSC_DBSCHQOS131, 0x000000F0U },
+	{ DBSC_DBSCHQOS132, 0x000000A0U },
+	{ DBSC_DBSCHQOS133, 0x00000040U },
+	{ DBSC_DBSCHQOS140, 0x000000C0U },
+	{ DBSC_DBSCHQOS141, 0x000000B0U },
+	{ DBSC_DBSCHQOS142, 0x00000080U },
+	{ DBSC_DBSCHQOS143, 0x00000040U },
+	{ DBSC_DBSCHQOS150, 0x00000040U },
+	{ DBSC_DBSCHQOS151, 0x00000030U },
+	{ DBSC_DBSCHQOS152, 0x00000020U },
+	{ DBSC_DBSCHQOS153, 0x00000010U },
+};
+
+void qos_init_h3_v30(void)
+{
+	unsigned int split_area;
+
+	rcar_qos_dbsc_setting(h3_v30_qos, ARRAY_SIZE(h3_v30_qos), true);
+
+#if RCAR_DRAM_LPDDR4_MEMCONF == 0	/* 1GB */
+	split_area = 0x1BU;
+#else /* default 2GB */
+	split_area = 0x1CU;
+#endif
+
+	/* DRAM Split Address mapping */
+#if (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH) || \
+    (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO)
+	NOTICE("BL2: DRAM Split is 4ch(DDR %x)\n", (int)qos_init_ddr_phyvalid);
+
+	io_write_32(AXI_ADSPLCR0, ADSPLCR0_ADRMODE_DEFAULT
+		    | ADSPLCR0_SPLITSEL(0xFFU)
+		    | ADSPLCR0_AREA(split_area)
+		    | ADSPLCR0_SWP);
+	io_write_32(AXI_ADSPLCR1, 0x00000000U);
+	io_write_32(AXI_ADSPLCR2, 0x00001054U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#elif RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH
+	NOTICE("BL2: DRAM Split is 2ch(DDR %x)\n", (int)qos_init_ddr_phyvalid);
+
+	io_write_32(AXI_ADSPLCR0, ADSPLCR0_AREA(split_area));
+	io_write_32(AXI_ADSPLCR1, ADSPLCR0_ADRMODE_DEFAULT
+		    | ADSPLCR0_SPLITSEL(0xFFU)
+		    | ADSPLCR0_AREA(split_area)
+		    | ADSPLCR0_SWP);
+	io_write_32(AXI_ADSPLCR2, 0x00001004U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else
+	io_write_32(AXI_ADSPLCR0, ADSPLCR0_AREA(split_area));
+	NOTICE("BL2: DRAM Split is OFF(DDR %x)\n", (int)qos_init_ddr_phyvalid);
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+	NOTICE("BL2: DRAM refresh interval 1.95 usec\n");
+#else
+	NOTICE("BL2: DRAM refresh interval 3.9 usec\n");
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	NOTICE("BL2: Periodic Write DQ Training\n");
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	io_write_32(QOSCTRL_RAS, 0x00000044U);
+	io_write_64(QOSCTRL_DANN, 0x0404010002020201UL);
+	io_write_32(QOSCTRL_DANT, 0x0020100AU);
+	io_write_32(QOSCTRL_FSS, 0x0000000AU);
+	io_write_32(QOSCTRL_INSFC, 0x06330001U);
+	io_write_32(QOSCTRL_RACNT0, 0x00010003U);
+
+	/* GPU Boost Mode */
+	io_write_32(QOSCTRL_STATGEN0, 0x00000001U);
+
+	io_write_32(QOSCTRL_SL_INIT,
+		    SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT |
+		    SL_INIT_SSLOTCLK_H3_30);
+	io_write_32(QOSCTRL_REF_ARS,
+		    ((QOSCTRL_REF_ARS_ARBSTOPCYCLE_H3_30 << 16)));
+
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		io_write_64(QOSBW_FIX_QOS_BANK0 + i * 8, mstat_fix[i]);
+		io_write_64(QOSBW_FIX_QOS_BANK1 + i * 8, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		io_write_64(QOSBW_BE_QOS_BANK0 + i * 8, mstat_be[i]);
+		io_write_64(QOSBW_BE_QOS_BANK1 + i * 8, mstat_be[i]);
+	}
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	for (i = 0U; i < ARRAY_SIZE(qoswt_fix); i++) {
+		io_write_64(QOSWT_FIX_WTQOS_BANK0 + i * 8,
+			    qoswt_fix[i]);
+		io_write_64(QOSWT_FIX_WTQOS_BANK1 + i * 8,
+			    qoswt_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(qoswt_be); i++) {
+		io_write_64(QOSWT_BE_WTQOS_BANK0 + i * 8, qoswt_be[i]);
+		io_write_64(QOSWT_BE_WTQOS_BANK1 + i * 8, qoswt_be[i]);
+	}
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	/* AXI setting */
+	io_write_32(AXI_MMCR, 0x00010008U);
+	io_write_32(AXI_TR3CR, 0x00010000U);
+	io_write_32(AXI_TR4CR, 0x00010000U);
+
+	/* RT bus Leaf setting */
+	io_write_32(RT_ACT0, 0x00000000U);
+	io_write_32(RT_ACT1, 0x00000000U);
+
+	/* CCI bus Leaf setting */
+	io_write_32(CPU_ACT0, 0x00000003U);
+	io_write_32(CPU_ACT1, 0x00000003U);
+	io_write_32(CPU_ACT2, 0x00000003U);
+	io_write_32(CPU_ACT3, 0x00000003U);
+
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	/*  re-write training setting */
+	io_write_32(QOSWT_WTREF,
+		    ((QOSWT_WTREF_SLOT1_EN << 16) | QOSWT_WTREF_SLOT0_EN));
+	io_write_32(QOSWT_WTSET0,
+		    ((QOSWT_WTSET0_PERIOD0_H3_30 << 16) |
+		     (QOSWT_WTSET0_SSLOT0 << 8) | QOSWT_WTSET0_SLOTSLOT0));
+	io_write_32(QOSWT_WTSET1,
+		    ((QOSWT_WTSET1_PERIOD1_H3_30 << 16) |
+		     (QOSWT_WTSET1_SSLOT1 << 8) | QOSWT_WTSET1_SLOTSLOT1));
+
+	io_write_32(QOSWT_WTEN, QOSWT_WTEN_ENABLE);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	io_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v30.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v30.h
new file mode 100644
index 0000000..d33b43c
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v30.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_H3_V30_H
+#define QOS_INIT_H3_V30_H
+
+void qos_init_h3_v30(void);
+
+#endif /* QOS_INIT_H3_V30_H */
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v30_mstat195.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v30_mstat195.h
new file mode 100644
index 0000000..28a240f
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v30_mstat195.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000FFFFUL,
+	/* 0x0038, */ 0x001008070000FFFFUL,
+	/* 0x0040, */ 0x001410070000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x0014100D0000FFFFUL,
+	/* 0x0060, */ 0x0014100D0000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x001008070000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001410070000FFFFUL,
+	/* 0x0090, */ 0x0014100D0000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C04020000FFFFUL,
+	/* 0x00a8, */ 0x000C04010000FFFFUL,
+	/* 0x00b0, */ 0x000C04010000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C04020000FFFFUL,
+	/* 0x00c8, */ 0x000C04010000FFFFUL,
+	/* 0x00d0, */ 0x000C04010000FFFFUL,
+	/* 0x00d8, */ 0x001024090000FFFFUL,
+	/* 0x00e0, */ 0x00100C090000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001024090000FFFFUL,
+	/* 0x00f8, */ 0x000C100D0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x00100C090000FFFFUL,
+	/* 0x0118, */ 0x000C1C1B0000FFFFUL,
+	/* 0x0120, */ 0x000C1C1B0000FFFFUL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100C0B0000FFFFUL,
+	/* 0x0140, */ 0x00100C0B0000FFFFUL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0010100D0000FFFFUL,
+	/* 0x0158, */ 0x0010100D0000FFFFUL,
+	/* 0x0160, */ 0x00100C0B0000FFFFUL,
+	/* 0x0168, */ 0x00100C0B0000FFFFUL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008060000FFFFUL,
+	/* 0x0180, */ 0x001008060000FFFFUL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00102C2C0000FFFFUL,
+	/* 0x0198, */ 0x00102C2C0000FFFFUL,
+	/* 0x01a0, */ 0x00100C0B0000FFFFUL,
+	/* 0x01a8, */ 0x00100C0B0000FFFFUL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFFFUL,
+	/* 0x0268, */ 0x001408010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C04010000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x001408010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x001408010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x001200600BDFFC01UL,
+	/* 0x0008, */ 0x001200600BDFFC01UL,
+	/* 0x0010, */ 0x001200600BDFFC01UL,
+	/* 0x0018, */ 0x001200600BDFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x001200100BD0FC01UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100600BDFFC01UL,
+	/* 0x01c8, */ 0x002100600BDFFC01UL,
+	/* 0x01d0, */ 0x002100600BDFFC01UL,
+	/* 0x01d8, */ 0x002100600BDFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x002100100BDF2401UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x002100100BDF2401UL,
+	/* 0x0218, */ 0x001100100BDF2401UL,
+	/* 0x0220, */ 0x001100100BDF2401UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x001100100BDF2401UL,
+	/* 0x0238, */ 0x001100100BDF2401UL,
+	/* 0x0240, */ 0x001200100BDF2401UL,
+	/* 0x0248, */ 0x001100100BDF2401UL,
+	/* 0x0250, */ 0x001200100BDF2401UL,
+	/* 0x0258, */ 0x001100100BDF2401UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x001100600BDFFC01UL,
+	/* 0x02f8, */ 0x001100600BDFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x001100600BDFFC01UL,
+	/* 0x0310, */ 0x001100600BDFFC01UL,
+	/* 0x0318, */ 0x001200100BD03401UL,
+	/* 0x0320, */ 0x001100600BDFFC01UL,
+	/* 0x0328, */ 0x001100600BDFFC01UL,
+	/* 0x0330, */ 0x001100600BDFFC01UL,
+	/* 0x0338, */ 0x001100600BDFFC01UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x001200100BD0FC01UL,
+};
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v30_mstat390.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v30_mstat390.h
new file mode 100644
index 0000000..def6585
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v30_mstat390.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000FFFFUL,
+	/* 0x0038, */ 0x0010100D0000FFFFUL,
+	/* 0x0040, */ 0x00141C0E0000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001408010000FFFFUL,
+	/* 0x0058, */ 0x00141C190000FFFFUL,
+	/* 0x0060, */ 0x00141C190000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001408010000FFFFUL,
+	/* 0x0078, */ 0x0010100D0000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x00141C0E0000FFFFUL,
+	/* 0x0090, */ 0x00141C190000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C08040000FFFFUL,
+	/* 0x00a8, */ 0x000C04020000FFFFUL,
+	/* 0x00b0, */ 0x000C04020000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C08040000FFFFUL,
+	/* 0x00c8, */ 0x000C04020000FFFFUL,
+	/* 0x00d0, */ 0x000C04020000FFFFUL,
+	/* 0x00d8, */ 0x001044110000FFFFUL,
+	/* 0x00e0, */ 0x001014110000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001044110000FFFFUL,
+	/* 0x00f8, */ 0x000C1C1A0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001014110000FFFFUL,
+	/* 0x0118, */ 0x000C38360000FFFFUL,
+	/* 0x0120, */ 0x000C38360000FFFFUL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x001018150000FFFFUL,
+	/* 0x0140, */ 0x001018150000FFFFUL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00101C190000FFFFUL,
+	/* 0x0158, */ 0x00101C190000FFFFUL,
+	/* 0x0160, */ 0x001018150000FFFFUL,
+	/* 0x0168, */ 0x001018150000FFFFUL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x00100C0B0000FFFFUL,
+	/* 0x0180, */ 0x00100C0B0000FFFFUL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001058570000FFFFUL,
+	/* 0x0198, */ 0x001058570000FFFFUL,
+	/* 0x01a0, */ 0x001018150000FFFFUL,
+	/* 0x01a8, */ 0x001018150000FFFFUL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFFFUL,
+	/* 0x0268, */ 0x001410010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C08020000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x00140C010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x00140C010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0012006005EFFC01UL,
+	/* 0x0008, */ 0x0012006005EFFC01UL,
+	/* 0x0010, */ 0x0012006005EFFC01UL,
+	/* 0x0018, */ 0x0012006005EFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001005E0FC01UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0021006005EFFC01UL,
+	/* 0x01c8, */ 0x0021006005EFFC01UL,
+	/* 0x01d0, */ 0x0021006005EFFC01UL,
+	/* 0x01d8, */ 0x0021006005EFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021001005E79401UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021001005E79401UL,
+	/* 0x0218, */ 0x0011001005E79401UL,
+	/* 0x0220, */ 0x0011001005E79401UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011001005E79401UL,
+	/* 0x0238, */ 0x0011001005E79401UL,
+	/* 0x0240, */ 0x0012001005E79401UL,
+	/* 0x0248, */ 0x0011001005E79401UL,
+	/* 0x0250, */ 0x0012001005E79401UL,
+	/* 0x0258, */ 0x0011001005E79401UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011006005EFFC01UL,
+	/* 0x02f8, */ 0x0011006005EFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0011006005EFFC01UL,
+	/* 0x0310, */ 0x0011006005EFFC01UL,
+	/* 0x0318, */ 0x0012001005E03401UL,
+	/* 0x0320, */ 0x0011006005EFFC01UL,
+	/* 0x0328, */ 0x0011006005EFFC01UL,
+	/* 0x0330, */ 0x0011006005EFFC01UL,
+	/* 0x0338, */ 0x0011006005EFFC01UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0012001005E0FC01UL,
+};
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v30_qoswt195.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v30_qoswt195.h
new file mode 100644
index 0000000..b0c11cc
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v30_qoswt195.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000C010UL,
+	/* 0x0038, */ 0x001008070000C010UL,
+	/* 0x0040, */ 0x001410070000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0014100D0000C010UL,
+	/* 0x0060, */ 0x0014100D0000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x001008070000C010UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001410070000FFF0UL,
+	/* 0x0090, */ 0x0014100D0000C010UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFF0UL,
+	/* 0x0268, */ 0x001408010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C04010000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3_v30_qoswt390.h b/drivers/renesas/rcar/qos/H3/qos_init_h3_v30_qoswt390.h
new file mode 100644
index 0000000..a1e4c72
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3_v30_qoswt390.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000C010UL,
+	/* 0x0038, */ 0x0010100D0000C010UL,
+	/* 0x0040, */ 0x00141C0E0000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x00141C190000C010UL,
+	/* 0x0060, */ 0x00141C190000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0010100D0000C010UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x00141C0E0000FFF0UL,
+	/* 0x0090, */ 0x00141C190000C010UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFF0UL,
+	/* 0x0268, */ 0x001410010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C08020000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30.c b/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30.c
new file mode 100644
index 0000000..f1ee41b
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30.c
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2018-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+#include "qos_init_h3n_v30.h"
+
+#define	RCAR_QOS_VERSION			"rev.0.07"
+
+#define QOSWT_TIME_BANK0			20000000U	/* unit:ns */
+
+#define	QOSWT_WTEN_ENABLE			0x1U
+
+#define QOSCTRL_REF_ARS_ARBSTOPCYCLE_H3N	(SL_INIT_SSLOTCLK_H3N - 0x5U)
+
+#define OSWT_WTREF_SLOT0_EN_REQ1_SLOT		3U
+#define OSWT_WTREF_SLOT0_EN_REQ2_SLOT		9U
+#define QOSWT_WTREF_SLOT0_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+#define QOSWT_WTREF_SLOT1_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+
+#define QOSWT_WTSET0_REQ_SSLOT0			5U
+#define WT_BASE_SUB_SLOT_NUM0			12U
+#define QOSWT_WTSET0_PERIOD0_H3N			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_H3N) - 1U)
+#define QOSWT_WTSET0_SSLOT0			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET0_SLOTSLOT0			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#define QOSWT_WTSET1_PERIOD1_H3N		(QOSWT_WTSET0_PERIOD0_H3N)
+#define QOSWT_WTSET1_SSLOT1			(QOSWT_WTSET0_SSLOT0)
+#define QOSWT_WTSET1_SLOTSLOT1			(QOSWT_WTSET0_SLOTSLOT0)
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_h3n_v30_mstat195.h"
+#else
+#include "qos_init_h3n_v30_mstat390.h"
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_h3n_v30_qoswt195.h"
+#else
+#include "qos_init_h3n_v30_qoswt390.h"
+#endif
+
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+#endif
+
+struct rcar_gen3_dbsc_qos_settings h3n_v30_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218U },
+	{ DBSC_DBCAM0CNF2, 0x000000F4U },
+	{ DBSC_DBCAM0CNF3, 0x00000000U },
+	{ DBSC_DBSCHCNT0, 0x000F0037U },
+	{ DBSC_DBSCHSZ0, 0x00000001U },
+	{ DBSC_DBSCHRW0, 0x22421111U },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123U },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00U },
+	{ DBSC_DBSCHQOS01, 0x00000B00U },
+	{ DBSC_DBSCHQOS02, 0x00000000U },
+	{ DBSC_DBSCHQOS03, 0x00000000U },
+	{ DBSC_DBSCHQOS40, 0x00000300U },
+	{ DBSC_DBSCHQOS41, 0x000002F0U },
+	{ DBSC_DBSCHQOS42, 0x00000200U },
+	{ DBSC_DBSCHQOS43, 0x00000100U },
+	{ DBSC_DBSCHQOS90, 0x00000100U },
+	{ DBSC_DBSCHQOS91, 0x000000F0U },
+	{ DBSC_DBSCHQOS92, 0x000000A0U },
+	{ DBSC_DBSCHQOS93, 0x00000040U },
+	{ DBSC_DBSCHQOS120, 0x00000040U },
+	{ DBSC_DBSCHQOS121, 0x00000030U },
+	{ DBSC_DBSCHQOS122, 0x00000020U },
+	{ DBSC_DBSCHQOS123, 0x00000010U },
+	{ DBSC_DBSCHQOS130, 0x00000100U },
+	{ DBSC_DBSCHQOS131, 0x000000F0U },
+	{ DBSC_DBSCHQOS132, 0x000000A0U },
+	{ DBSC_DBSCHQOS133, 0x00000040U },
+	{ DBSC_DBSCHQOS140, 0x000000C0U },
+	{ DBSC_DBSCHQOS141, 0x000000B0U },
+	{ DBSC_DBSCHQOS142, 0x00000080U },
+	{ DBSC_DBSCHQOS143, 0x00000040U },
+	{ DBSC_DBSCHQOS150, 0x00000040U },
+	{ DBSC_DBSCHQOS151, 0x00000030U },
+	{ DBSC_DBSCHQOS152, 0x00000020U },
+	{ DBSC_DBSCHQOS153, 0x00000010U },
+};
+
+void qos_init_h3n_v30(void)
+{
+	unsigned int split_area;
+
+	rcar_qos_dbsc_setting(h3n_v30_qos, ARRAY_SIZE(h3n_v30_qos), true);
+
+	/* use 1(2GB) for RCAR_DRAM_LPDDR4_MEMCONF for H3N */
+	split_area = 0x1CU;
+
+	/* DRAM Split Address mapping */
+#if (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH)
+#if RCAR_LSI == RCAR_H3N
+#error "Don't set DRAM Split 4ch(H3N)"
+#else
+	ERROR("DRAM Split 4ch not supported.(H3N)");
+	panic();
+#endif
+#elif (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH) || \
+    (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO)
+	NOTICE("BL2: DRAM Split is 2ch(DDR %x)\n", (int)qos_init_ddr_phyvalid);
+
+	io_write_32(AXI_ADSPLCR0, ADSPLCR0_AREA(split_area));
+	io_write_32(AXI_ADSPLCR1, ADSPLCR0_ADRMODE_DEFAULT
+		    | ADSPLCR0_SPLITSEL(0xFFU)
+		    | ADSPLCR0_AREA(split_area)
+		    | ADSPLCR0_SWP);
+	io_write_32(AXI_ADSPLCR2, 0x00001004U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else
+	io_write_32(AXI_ADSPLCR0, ADSPLCR0_AREA(split_area));
+	NOTICE("BL2: DRAM Split is OFF(DDR %x)\n", (int)qos_init_ddr_phyvalid);
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+	NOTICE("BL2: DRAM refresh interval 1.95 usec\n");
+#else
+	NOTICE("BL2: DRAM refresh interval 3.9 usec\n");
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	NOTICE("BL2: Periodic Write DQ Training\n");
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	io_write_32(QOSCTRL_RAS, 0x00000044U);
+	io_write_64(QOSCTRL_DANN, 0x0404020002020201UL);
+	io_write_32(QOSCTRL_DANT, 0x0020100AU);
+	io_write_32(QOSCTRL_FSS, 0x0000000AU);
+	io_write_32(QOSCTRL_INSFC, 0x06330001U);
+	io_write_32(QOSCTRL_RACNT0, 0x00010003U);
+
+	/* GPU Boost Mode */
+	io_write_32(QOSCTRL_STATGEN0, 0x00000001U);
+
+	io_write_32(QOSCTRL_SL_INIT,
+		    SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT |
+		    SL_INIT_SSLOTCLK_H3N);
+	io_write_32(QOSCTRL_REF_ARS,
+		    ((QOSCTRL_REF_ARS_ARBSTOPCYCLE_H3N << 16)));
+
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		io_write_64(QOSBW_FIX_QOS_BANK0 + i * 8, mstat_fix[i]);
+		io_write_64(QOSBW_FIX_QOS_BANK1 + i * 8, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		io_write_64(QOSBW_BE_QOS_BANK0 + i * 8, mstat_be[i]);
+		io_write_64(QOSBW_BE_QOS_BANK1 + i * 8, mstat_be[i]);
+	}
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	for (i = 0U; i < ARRAY_SIZE(qoswt_fix); i++) {
+		io_write_64(QOSWT_FIX_WTQOS_BANK0 + i * 8,
+			    qoswt_fix[i]);
+		io_write_64(QOSWT_FIX_WTQOS_BANK1 + i * 8,
+			    qoswt_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(qoswt_be); i++) {
+		io_write_64(QOSWT_BE_WTQOS_BANK0 + i * 8, qoswt_be[i]);
+		io_write_64(QOSWT_BE_WTQOS_BANK1 + i * 8, qoswt_be[i]);
+	}
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	/* AXI setting */
+	io_write_32(AXI_MMCR, 0x00010008U);
+	io_write_32(AXI_TR3CR, 0x00010000U);
+	io_write_32(AXI_TR4CR, 0x00010000U);
+
+	/* RT bus Leaf setting */
+	io_write_32(RT_ACT0, 0x00000000U);
+	io_write_32(RT_ACT1, 0x00000000U);
+
+	/* CCI bus Leaf setting */
+	io_write_32(CPU_ACT0, 0x00000003U);
+	io_write_32(CPU_ACT1, 0x00000003U);
+	io_write_32(CPU_ACT2, 0x00000003U);
+	io_write_32(CPU_ACT3, 0x00000003U);
+
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	/*  re-write training setting */
+	io_write_32(QOSWT_WTREF,
+		    ((QOSWT_WTREF_SLOT1_EN << 16) | QOSWT_WTREF_SLOT0_EN));
+	io_write_32(QOSWT_WTSET0,
+		    ((QOSWT_WTSET0_PERIOD0_H3N << 16) |
+		     (QOSWT_WTSET0_SSLOT0 << 8) | QOSWT_WTSET0_SLOTSLOT0));
+	io_write_32(QOSWT_WTSET1,
+		    ((QOSWT_WTSET1_PERIOD1_H3N << 16) |
+		     (QOSWT_WTSET1_SSLOT1 << 8) | QOSWT_WTSET1_SLOTSLOT1));
+
+	io_write_32(QOSWT_WTEN, QOSWT_WTEN_ENABLE);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	io_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30.h b/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30.h
new file mode 100644
index 0000000..46f3440
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_H3N_V30_H
+#define QOS_INIT_H3N_V30_H
+
+void qos_init_h3n_v30(void);
+
+#endif /* QOS_INIT_H3N_V30_H */
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30_mstat195.h b/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30_mstat195.h
new file mode 100644
index 0000000..6dbc88a
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30_mstat195.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000FFFFUL,
+	/* 0x0038, */ 0x001008070000FFFFUL,
+	/* 0x0040, */ 0x001410070000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x0014100D0000FFFFUL,
+	/* 0x0060, */ 0x0014100D0000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001410070000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C04020000FFFFUL,
+	/* 0x00a8, */ 0x000C04010000FFFFUL,
+	/* 0x00b0, */ 0x000C04010000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C04020000FFFFUL,
+	/* 0x00c8, */ 0x000C04010000FFFFUL,
+	/* 0x00d0, */ 0x000C04010000FFFFUL,
+	/* 0x00d8, */ 0x001024090000FFFFUL,
+	/* 0x00e0, */ 0x00100C090000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001024090000FFFFUL,
+	/* 0x00f8, */ 0x000C100D0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x00100C090000FFFFUL,
+	/* 0x0118, */ 0x000C1C1B0000FFFFUL,
+	/* 0x0120, */ 0x000C1C1B0000FFFFUL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100C0B0000FFFFUL,
+	/* 0x0140, */ 0x00100C0B0000FFFFUL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0010100D0000FFFFUL,
+	/* 0x0158, */ 0x0010100D0000FFFFUL,
+	/* 0x0160, */ 0x00100C0B0000FFFFUL,
+	/* 0x0168, */ 0x00100C0B0000FFFFUL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008060000FFFFUL,
+	/* 0x0180, */ 0x001008060000FFFFUL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00102C2C0000FFFFUL,
+	/* 0x0198, */ 0x00102C2C0000FFFFUL,
+	/* 0x01a0, */ 0x00100C0B0000FFFFUL,
+	/* 0x01a8, */ 0x00100C0B0000FFFFUL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFFFUL,
+	/* 0x0268, */ 0x001408010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C04010000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x001408010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x001408010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x001200600BDFFC01UL,
+	/* 0x0008, */ 0x001200600BDFFC01UL,
+	/* 0x0010, */ 0x001200600BDFFC01UL,
+	/* 0x0018, */ 0x001200600BDFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x001200100BD0FC01UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100600BDFFC01UL,
+	/* 0x01c8, */ 0x002100600BDFFC01UL,
+	/* 0x01d0, */ 0x002100600BDFFC01UL,
+	/* 0x01d8, */ 0x002100600BDFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x002100100BDF2401UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x002100100BDF2401UL,
+	/* 0x0218, */ 0x001100100BDF2401UL,
+	/* 0x0220, */ 0x001100100BDF2401UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x001100100BDF2401UL,
+	/* 0x0238, */ 0x001100100BDF2401UL,
+	/* 0x0240, */ 0x001200100BDF2401UL,
+	/* 0x0248, */ 0x001100100BDF2401UL,
+	/* 0x0250, */ 0x001200100BDF2401UL,
+	/* 0x0258, */ 0x001100100BDF2401UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x001100600BDFFC01UL,
+	/* 0x02f8, */ 0x001100600BDFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x001100600BDFFC01UL,
+	/* 0x0310, */ 0x001100600BDFFC01UL,
+	/* 0x0318, */ 0x001200100BD03401UL,
+	/* 0x0320, */ 0x001100600BDFFC01UL,
+	/* 0x0328, */ 0x001100600BDFFC01UL,
+	/* 0x0330, */ 0x001100600BDFFC01UL,
+	/* 0x0338, */ 0x001100600BDFFC01UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x001200100BD0FC01UL,
+};
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30_mstat390.h b/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30_mstat390.h
new file mode 100644
index 0000000..880211c
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30_mstat390.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000FFFFUL,
+	/* 0x0038, */ 0x0010100D0000FFFFUL,
+	/* 0x0040, */ 0x00141C0E0000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001408010000FFFFUL,
+	/* 0x0058, */ 0x00141C190000FFFFUL,
+	/* 0x0060, */ 0x00141C190000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001408010000FFFFUL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x00141C0E0000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C08040000FFFFUL,
+	/* 0x00a8, */ 0x000C04020000FFFFUL,
+	/* 0x00b0, */ 0x000C04020000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C08040000FFFFUL,
+	/* 0x00c8, */ 0x000C04020000FFFFUL,
+	/* 0x00d0, */ 0x000C04020000FFFFUL,
+	/* 0x00d8, */ 0x001044110000FFFFUL,
+	/* 0x00e0, */ 0x001014110000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001044110000FFFFUL,
+	/* 0x00f8, */ 0x000C1C1A0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001014110000FFFFUL,
+	/* 0x0118, */ 0x000C38360000FFFFUL,
+	/* 0x0120, */ 0x000C38360000FFFFUL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x001018150000FFFFUL,
+	/* 0x0140, */ 0x001018150000FFFFUL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00101C190000FFFFUL,
+	/* 0x0158, */ 0x00101C190000FFFFUL,
+	/* 0x0160, */ 0x001018150000FFFFUL,
+	/* 0x0168, */ 0x001018150000FFFFUL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x00100C0B0000FFFFUL,
+	/* 0x0180, */ 0x00100C0B0000FFFFUL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001058570000FFFFUL,
+	/* 0x0198, */ 0x001058570000FFFFUL,
+	/* 0x01a0, */ 0x001018150000FFFFUL,
+	/* 0x01a8, */ 0x001018150000FFFFUL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFFFUL,
+	/* 0x0268, */ 0x001410010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C08020000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x00140C010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x00140C010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0012006005EFFC01UL,
+	/* 0x0008, */ 0x0012006005EFFC01UL,
+	/* 0x0010, */ 0x0012006005EFFC01UL,
+	/* 0x0018, */ 0x0012006005EFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001005E0FC01UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0021006005EFFC01UL,
+	/* 0x01c8, */ 0x0021006005EFFC01UL,
+	/* 0x01d0, */ 0x0021006005EFFC01UL,
+	/* 0x01d8, */ 0x0021006005EFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021001005E79401UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021001005E79401UL,
+	/* 0x0218, */ 0x0011001005E79401UL,
+	/* 0x0220, */ 0x0011001005E79401UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011001005E79401UL,
+	/* 0x0238, */ 0x0011001005E79401UL,
+	/* 0x0240, */ 0x0012001005E79401UL,
+	/* 0x0248, */ 0x0011001005E79401UL,
+	/* 0x0250, */ 0x0012001005E79401UL,
+	/* 0x0258, */ 0x0011001005E79401UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011006005EFFC01UL,
+	/* 0x02f8, */ 0x0011006005EFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0011006005EFFC01UL,
+	/* 0x0310, */ 0x0011006005EFFC01UL,
+	/* 0x0318, */ 0x0012001005E03401UL,
+	/* 0x0320, */ 0x0011006005EFFC01UL,
+	/* 0x0328, */ 0x0011006005EFFC01UL,
+	/* 0x0330, */ 0x0011006005EFFC01UL,
+	/* 0x0338, */ 0x0011006005EFFC01UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0012001005E0FC01UL,
+};
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30_qoswt195.h b/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30_qoswt195.h
new file mode 100644
index 0000000..affd013
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30_qoswt195.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000C010UL,
+	/* 0x0038, */ 0x001008070000C010UL,
+	/* 0x0040, */ 0x001410070000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0014100D0000C010UL,
+	/* 0x0060, */ 0x0014100D0000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001410070000FFF0UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFF0UL,
+	/* 0x0268, */ 0x001408010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C04010000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30_qoswt390.h b/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30_qoswt390.h
new file mode 100644
index 0000000..1c48d28
--- /dev/null
+++ b/drivers/renesas/rcar/qos/H3/qos_init_h3n_v30_qoswt390.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000C010UL,
+	/* 0x0038, */ 0x0010100D0000C010UL,
+	/* 0x0040, */ 0x00141C0E0000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x00141C190000C010UL,
+	/* 0x0060, */ 0x00141C190000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x00141C0E0000FFF0UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFF0UL,
+	/* 0x0268, */ 0x001410010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C08020000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v10.c b/drivers/renesas/rcar/qos/M3/qos_init_m3_v10.c
new file mode 100644
index 0000000..a8264cb
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v10.c
@@ -0,0 +1,149 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+#include "qos_init_m3_v10.h"
+
+#define	RCAR_QOS_VERSION		"rev.0.19"
+
+#include "qos_init_m3_v10_mstat.h"
+
+struct rcar_gen3_dbsc_qos_settings m3_v10_qos[] = {
+	/* BUFCAM settings */
+	/* DBSC_DBCAM0CNF0 not set */
+	{ DBSC_DBCAM0CNF1, 0x00043218 },
+	{ DBSC_DBCAM0CNF2, 0x000000F4 },
+	{ DBSC_DBCAM0CNF3, 0x00000000 },
+	{ DBSC_DBSCHCNT0, 0x080F0037 },
+	/* DBSC_DBSCHCNT1 not set */
+	{ DBSC_DBSCHSZ0, 0x00000001 },
+	{ DBSC_DBSCHRW0, 0x22421111 },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123 },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00 },
+	{ DBSC_DBSCHQOS01, 0x00000B00 },
+	{ DBSC_DBSCHQOS02, 0x00000000 },
+	{ DBSC_DBSCHQOS03, 0x00000000 },
+	{ DBSC_DBSCHQOS40, 0x00000300 },
+	{ DBSC_DBSCHQOS41, 0x000002F0 },
+	{ DBSC_DBSCHQOS42, 0x00000200 },
+	{ DBSC_DBSCHQOS43, 0x00000100 },
+	{ DBSC_DBSCHQOS90, 0x00000300 },
+	{ DBSC_DBSCHQOS91, 0x000002F0 },
+	{ DBSC_DBSCHQOS92, 0x00000200 },
+	{ DBSC_DBSCHQOS93, 0x00000100 },
+	{ DBSC_DBSCHQOS130, 0x00000100 },
+	{ DBSC_DBSCHQOS131, 0x000000F0 },
+	{ DBSC_DBSCHQOS132, 0x000000A0 },
+	{ DBSC_DBSCHQOS133, 0x00000040 },
+	{ DBSC_DBSCHQOS140, 0x000000C0 },
+	{ DBSC_DBSCHQOS141, 0x000000B0 },
+	{ DBSC_DBSCHQOS142, 0x00000080 },
+	{ DBSC_DBSCHQOS143, 0x00000040 },
+	{ DBSC_DBSCHQOS150, 0x00000040 },
+	{ DBSC_DBSCHQOS151, 0x00000030 },
+	{ DBSC_DBSCHQOS152, 0x00000020 },
+	{ DBSC_DBSCHQOS153, 0x00000010 },
+};
+
+void qos_init_m3_v10(void)
+{
+	rcar_qos_dbsc_setting(m3_v10_qos, ARRAY_SIZE(m3_v10_qos), false);
+
+	/* DRAM Split Address mapping */
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH
+#if RCAR_LSI == RCAR_M3
+#error "Don't set DRAM Split 4ch(M3)"
+#else
+	ERROR("DRAM Split 4ch not supported.(M3)");
+	panic();
+#endif
+#elif (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH) || \
+      (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO)
+	NOTICE("BL2: DRAM Split is 2ch\n");
+	io_write_32(AXI_ADSPLCR0, 0x00000000U);
+	io_write_32(AXI_ADSPLCR1, ADSPLCR0_ADRMODE_DEFAULT
+		    | ADSPLCR0_SPLITSEL(0xFFU)
+		    | ADSPLCR0_AREA(0x1CU)
+		    | ADSPLCR0_SWP);
+	io_write_32(AXI_ADSPLCR2, 0x089A0000U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+	/* Resource Alloc setting */
+	io_write_32(QOSCTRL_RAS, 0x00000028U);
+	io_write_32(QOSCTRL_FIXTH, 0x000F0005U);
+	io_write_32(QOSCTRL_REGGD, 0x00000000U);
+	io_write_64(QOSCTRL_DANN, 0x0101010102020201UL);
+	io_write_32(QOSCTRL_DANT, 0x00100804U);
+	io_write_32(QOSCTRL_EC, 0x00000000U);
+	io_write_64(QOSCTRL_EMS, 0x0000000000000000UL);
+	io_write_32(QOSCTRL_FSS, 0x000003e8U);
+	io_write_32(QOSCTRL_INSFC, 0xC7840001U);
+	io_write_32(QOSCTRL_BERR, 0x00000000U);
+	io_write_32(QOSCTRL_RACNT0, 0x00000000U);
+
+	/* QOSBW setting */
+	io_write_32(QOSCTRL_SL_INIT,
+		    SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT | SL_INIT_SSLOTCLK);
+	io_write_32(QOSCTRL_REF_ARS, 0x00330000U);
+
+	/* QOSBW SRAM setting */
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		io_write_64(QOSBW_FIX_QOS_BANK0 + i * 8, mstat_fix[i]);
+		io_write_64(QOSBW_FIX_QOS_BANK1 + i * 8, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		io_write_64(QOSBW_BE_QOS_BANK0 + i * 8, mstat_be[i]);
+		io_write_64(QOSBW_BE_QOS_BANK1 + i * 8, mstat_be[i]);
+	}
+
+	/* 3DG bus Leaf setting */
+	io_write_32(0xFD820808U, 0x00001234U);
+	io_write_32(0xFD820800U, 0x00000006U);
+	io_write_32(0xFD821800U, 0x00000006U);
+	io_write_32(0xFD822800U, 0x00000006U);
+	io_write_32(0xFD823800U, 0x00000006U);
+	io_write_32(0xFD824800U, 0x00000006U);
+	io_write_32(0xFD825800U, 0x00000006U);
+	io_write_32(0xFD826800U, 0x00000006U);
+	io_write_32(0xFD827800U, 0x00000006U);
+
+	/* RT bus Leaf setting */
+	io_write_32(0xFFC50800U, 0x00000000U);
+	io_write_32(0xFFC51800U, 0x00000000U);
+
+	/* Resource Alloc start */
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+	/* QOSBW start */
+	io_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	/* Resource Alloc setting */
+	io_write_32(QOSCTRL_EC, 0x00000000U);
+	/* Resource Alloc start */
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v10.h b/drivers/renesas/rcar/qos/M3/qos_init_m3_v10.h
new file mode 100644
index 0000000..01ef46c
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v10.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_M3_V10_H
+#define QOS_INIT_M3_V10_H
+
+void qos_init_m3_v10(void);
+
+#endif /* QOS_INIT_M3_V10_H */
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v10_mstat.h b/drivers/renesas/rcar/qos/M3/qos_init_m3_v10_mstat.h
new file mode 100644
index 0000000..b78b5f1
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v10_mstat.h
@@ -0,0 +1,227 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+static const uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004030000FFFFUL,
+	/* 0x0038, */ 0x001004030000FFFFUL,
+	/* 0x0040, */ 0x001414090000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001410010000FFFFUL,
+	/* 0x0058, */ 0x00140C090000FFFFUL,
+	/* 0x0060, */ 0x00140C090000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001410010000FFFFUL,
+	/* 0x0078, */ 0x001004020000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001414090000FFFFUL,
+	/* 0x0090, */ 0x001408060000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00A0, */ 0x000C08020000FFFFUL,
+	/* 0x00A8, */ 0x000C04010000FFFFUL,
+	/* 0x00B0, */ 0x000C04010000FFFFUL,
+	/* 0x00B8, */ 0x0000000000000000UL,
+	/* 0x00C0, */ 0x000C08020000FFFFUL,
+	/* 0x00C8, */ 0x000C04010000FFFFUL,
+	/* 0x00D0, */ 0x000C04010000FFFFUL,
+	/* 0x00D8, */ 0x000C04030000FFFFUL,
+	/* 0x00E0, */ 0x000C100F0000FFFFUL,
+	/* 0x00E8, */ 0x0000000000000000UL,
+	/* 0x00F0, */ 0x001010080000FFFFUL,
+	/* 0x00F8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001010080000FFFFUL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100C0A0000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00100C0A0000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x00100C0A0000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008050000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001028280000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01A0, */ 0x00100C0A0000FFFFUL,
+	/* 0x01A8, */ 0x0000000000000000UL,
+	/* 0x01B0, */ 0x0000000000000000UL,
+	/* 0x01B8, */ 0x0000000000000000UL,
+	/* 0x01C0, */ 0x0000000000000000UL,
+	/* 0x01C8, */ 0x0000000000000000UL,
+	/* 0x01D0, */ 0x0000000000000000UL,
+	/* 0x01D8, */ 0x0000000000000000UL,
+	/* 0x01E0, */ 0x0000000000000000UL,
+	/* 0x01E8, */ 0x0000000000000000UL,
+	/* 0x01F0, */ 0x0000000000000000UL,
+	/* 0x01F8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x001408010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02A0, */ 0x000C04010000FFFFUL,
+	/* 0x02A8, */ 0x000C04010000FFFFUL,
+	/* 0x02B0, */ 0x001404010000FFFFUL,
+	/* 0x02B8, */ 0x0000000000000000UL,
+	/* 0x02C0, */ 0x0000000000000000UL,
+	/* 0x02C8, */ 0x0000000000000000UL,
+	/* 0x02D0, */ 0x000C04010000FFFFUL,
+	/* 0x02D8, */ 0x000C04010000FFFFUL,
+	/* 0x02E0, */ 0x001404010000FFFFUL,
+	/* 0x02E8, */ 0x0000000000000000UL,
+	/* 0x02F0, */ 0x0000000000000000UL,
+	/* 0x02F8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static const uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x001200100C89C401UL,
+	/* 0x0008, */ 0x001200100C89C401UL,
+	/* 0x0010, */ 0x001200100C89C401UL,
+	/* 0x0018, */ 0x001200100C89C401UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x001100100C803401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00A0, */ 0x0000000000000000UL,
+	/* 0x00A8, */ 0x0000000000000000UL,
+	/* 0x00B0, */ 0x0000000000000000UL,
+	/* 0x00B8, */ 0x0000000000000000UL,
+	/* 0x00C0, */ 0x0000000000000000UL,
+	/* 0x00C8, */ 0x0000000000000000UL,
+	/* 0x00D0, */ 0x0000000000000000UL,
+	/* 0x00D8, */ 0x0000000000000000UL,
+	/* 0x00E0, */ 0x0000000000000000UL,
+	/* 0x00E8, */ 0x0000000000000000UL,
+	/* 0x00F0, */ 0x0000000000000000UL,
+	/* 0x00F8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01A0, */ 0x0000000000000000UL,
+	/* 0x01A8, */ 0x0000000000000000UL,
+	/* 0x01B0, */ 0x0000000000000000UL,
+	/* 0x01B8, */ 0x0000000000000000UL,
+	/* 0x01C0, */ 0x001100500C8FFC01UL,
+	/* 0x01C8, */ 0x001100500C8FFC01UL,
+	/* 0x01D0, */ 0x001100500C8FFC01UL,
+	/* 0x01D8, */ 0x001100500C8FFC01UL,
+	/* 0x01E0, */ 0x0000000000000000UL,
+	/* 0x01E8, */ 0x001200100C803401UL,
+	/* 0x01F0, */ 0x001100100C80FC01UL,
+	/* 0x01F8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x001200100C80FC01UL,
+	/* 0x0210, */ 0x001100100C80FC01UL,
+	/* 0x0218, */ 0x001100100C825801UL,
+	/* 0x0220, */ 0x001100100C825801UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x001100100C825801UL,
+	/* 0x0238, */ 0x001100100C825801UL,
+	/* 0x0240, */ 0x001200100C8BB801UL,
+	/* 0x0248, */ 0x001100100C8EA401UL,
+	/* 0x0250, */ 0x001200100C8BB801UL,
+	/* 0x0258, */ 0x001100100C8EA401UL,
+	/* 0x0260, */ 0x001100100C84E401UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x001100100C81F401UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02A0, */ 0x0000000000000000UL,
+	/* 0x02A8, */ 0x0000000000000000UL,
+	/* 0x02B0, */ 0x0000000000000000UL,
+	/* 0x02B8, */ 0x001100100C803401UL,
+	/* 0x02C0, */ 0x0000000000000000UL,
+	/* 0x02C8, */ 0x0000000000000000UL,
+	/* 0x02D0, */ 0x0000000000000000UL,
+	/* 0x02D8, */ 0x0000000000000000UL,
+	/* 0x02E0, */ 0x0000000000000000UL,
+	/* 0x02E8, */ 0x001100100C803401UL,
+	/* 0x02F0, */ 0x001100300C8FFC01UL,
+	/* 0x02F8, */ 0x001100500C8FFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x001100300C8FFC01UL,
+	/* 0x0310, */ 0x001100500C8FFC01UL,
+	/* 0x0318, */ 0x001200100C803401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+#endif
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v11.c b/drivers/renesas/rcar/qos/M3/qos_init_m3_v11.c
new file mode 100644
index 0000000..22fd83a
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v11.c
@@ -0,0 +1,223 @@
+/*
+ * Copyright (c) 2017-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+#include "qos_init_m3_v11.h"
+
+#define	RCAR_QOS_VERSION			"rev.0.19"
+
+#define QOSWT_TIME_BANK0			20000000U	/* unit:ns */
+
+#define	QOSWT_WTEN_ENABLE			0x1U
+
+#define QOSCTRL_REF_ARS_ARBSTOPCYCLE_M3_11	(SL_INIT_SSLOTCLK_M3_11 - 0x5U)
+
+#define OSWT_WTREF_SLOT0_EN_REQ1_SLOT		3U
+#define OSWT_WTREF_SLOT0_EN_REQ2_SLOT		9U
+#define QOSWT_WTREF_SLOT0_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+#define QOSWT_WTREF_SLOT1_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+
+#define QOSWT_WTSET0_REQ_SSLOT0			5U
+#define WT_BASE_SUB_SLOT_NUM0			12U
+#define QOSWT_WTSET0_PERIOD0_M3_11			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_M3_11) - 1U)
+#define QOSWT_WTSET0_SSLOT0			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET0_SLOTSLOT0			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#define QOSWT_WTSET1_PERIOD1_M3_11			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_M3_11) - 1U)
+#define QOSWT_WTSET1_SSLOT1			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET1_SLOTSLOT1			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_m3_v11_mstat195.h"
+#else
+#include "qos_init_m3_v11_mstat390.h"
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_m3_v11_qoswt195.h"
+#else
+#include "qos_init_m3_v11_qoswt390.h"
+#endif
+
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+#endif
+
+struct rcar_gen3_dbsc_qos_settings m3_v11_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218 },
+	{ DBSC_DBCAM0CNF2, 0x000000F4 },
+	{ DBSC_DBCAM0CNF3, 0x00000000 },
+	{ DBSC_DBSCHCNT0, 0x000F0037 },
+	{ DBSC_DBSCHSZ0, 0x00000001 },
+	{ DBSC_DBSCHRW0, 0x22421111 },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123 },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00 },
+	{ DBSC_DBSCHQOS01, 0x00000B00 },
+	{ DBSC_DBSCHQOS02, 0x00000000 },
+	{ DBSC_DBSCHQOS03, 0x00000000 },
+	{ DBSC_DBSCHQOS40, 0x00000300 },
+	{ DBSC_DBSCHQOS41, 0x000002F0 },
+	{ DBSC_DBSCHQOS42, 0x00000200 },
+	{ DBSC_DBSCHQOS43, 0x00000100 },
+	{ DBSC_DBSCHQOS90, 0x00000100 },
+	{ DBSC_DBSCHQOS91, 0x000000F0 },
+	{ DBSC_DBSCHQOS92, 0x000000A0 },
+	{ DBSC_DBSCHQOS93, 0x00000040 },
+	{ DBSC_DBSCHQOS120, 0x00000040 },
+	{ DBSC_DBSCHQOS121, 0x00000030 },
+	{ DBSC_DBSCHQOS122, 0x00000020 },
+	{ DBSC_DBSCHQOS123, 0x00000010 },
+	{ DBSC_DBSCHQOS130, 0x00000100 },
+	{ DBSC_DBSCHQOS131, 0x000000F0 },
+	{ DBSC_DBSCHQOS132, 0x000000A0 },
+	{ DBSC_DBSCHQOS133, 0x00000040 },
+	{ DBSC_DBSCHQOS140, 0x000000C0 },
+	{ DBSC_DBSCHQOS141, 0x000000B0 },
+	{ DBSC_DBSCHQOS142, 0x00000080 },
+	{ DBSC_DBSCHQOS143, 0x00000040 },
+	{ DBSC_DBSCHQOS150, 0x00000040 },
+	{ DBSC_DBSCHQOS151, 0x00000030 },
+	{ DBSC_DBSCHQOS152, 0x00000020 },
+	{ DBSC_DBSCHQOS153, 0x00000010 },
+};
+
+void qos_init_m3_v11(void)
+{
+	rcar_qos_dbsc_setting(m3_v11_qos, ARRAY_SIZE(m3_v11_qos), false);
+
+	/* DRAM Split Address mapping */
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH
+#if RCAR_LSI == RCAR_M3
+#error "Don't set DRAM Split 4ch(M3)"
+#else
+	ERROR("DRAM Split 4ch not supported.(M3)");
+	panic();
+#endif
+#elif (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH) || \
+      (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO)
+	NOTICE("BL2: DRAM Split is 2ch\n");
+	io_write_32(AXI_ADSPLCR0, 0x00000000U);
+	io_write_32(AXI_ADSPLCR1, ADSPLCR0_ADRMODE_DEFAULT
+		    | ADSPLCR0_SPLITSEL(0xFFU)
+		    | ADSPLCR0_AREA(0x1CU)
+		    | ADSPLCR0_SWP);
+	io_write_32(AXI_ADSPLCR2, 0x00001004U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+	NOTICE("BL2: DRAM refresh interval 1.95 usec\n");
+#else
+	NOTICE("BL2: DRAM refresh interval 3.9 usec\n");
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	NOTICE("BL2: Periodic Write DQ Training\n");
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	io_write_32(QOSCTRL_RAS, 0x00000044U);
+	io_write_64(QOSCTRL_DANN, 0x0404020002020201UL);
+	io_write_32(QOSCTRL_DANT, 0x0020100AU);
+	io_write_32(QOSCTRL_INSFC, 0x06330001U);
+	io_write_32(QOSCTRL_RACNT0, 0x02010003U);	/* GPU Boost Mode ON */
+
+	io_write_32(QOSCTRL_SL_INIT,
+		    SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT |
+		    SL_INIT_SSLOTCLK_M3_11);
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	io_write_32(QOSCTRL_REF_ARS,
+		    ((QOSCTRL_REF_ARS_ARBSTOPCYCLE_M3_11 << 16)));
+#else
+	io_write_32(QOSCTRL_REF_ARS, 0x00330000U);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		io_write_64(QOSBW_FIX_QOS_BANK0 + i * 8, mstat_fix[i]);
+		io_write_64(QOSBW_FIX_QOS_BANK1 + i * 8, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		io_write_64(QOSBW_BE_QOS_BANK0 + i * 8, mstat_be[i]);
+		io_write_64(QOSBW_BE_QOS_BANK1 + i * 8, mstat_be[i]);
+	}
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	for (i = 0U; i < ARRAY_SIZE(qoswt_fix); i++) {
+		io_write_64(QOSWT_FIX_WTQOS_BANK0 + i * 8, qoswt_fix[i]);
+		io_write_64(QOSWT_FIX_WTQOS_BANK1 + i * 8, qoswt_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(qoswt_be); i++) {
+		io_write_64(QOSWT_BE_WTQOS_BANK0 + i * 8, qoswt_be[i]);
+		io_write_64(QOSWT_BE_WTQOS_BANK1 + i * 8, qoswt_be[i]);
+	}
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	/* 3DG bus Leaf setting */
+	io_write_32(GPU_ACT_GRD, 0x00001234U);
+	io_write_32(GPU_ACT0, 0x00000000U);
+	io_write_32(GPU_ACT1, 0x00000000U);
+	io_write_32(GPU_ACT2, 0x00000000U);
+	io_write_32(GPU_ACT3, 0x00000000U);
+
+	/* RT bus Leaf setting */
+	io_write_32(RT_ACT0, 0x00000000U);
+	io_write_32(RT_ACT1, 0x00000000U);
+
+	/* CCI bus Leaf setting */
+	io_write_32(CPU_ACT0, 0x00000003U);
+	io_write_32(CPU_ACT1, 0x00000003U);
+	io_write_32(CPU_ACT2, 0x00000003U);
+	io_write_32(CPU_ACT3, 0x00000003U);
+
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	/*  re-write training setting */
+	io_write_32(QOSWT_WTREF,
+		    ((QOSWT_WTREF_SLOT1_EN << 16) | QOSWT_WTREF_SLOT0_EN));
+	io_write_32(QOSWT_WTSET0,
+		    ((QOSWT_WTSET0_PERIOD0_M3_11 << 16) |
+		     (QOSWT_WTSET0_SSLOT0 << 8) | QOSWT_WTSET0_SLOTSLOT0));
+	io_write_32(QOSWT_WTSET1,
+		    ((QOSWT_WTSET1_PERIOD1_M3_11 << 16) |
+		     (QOSWT_WTSET1_SSLOT1 << 8) | QOSWT_WTSET1_SLOTSLOT1));
+
+	io_write_32(QOSWT_WTEN, QOSWT_WTEN_ENABLE);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	io_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v11.h b/drivers/renesas/rcar/qos/M3/qos_init_m3_v11.h
new file mode 100644
index 0000000..1552fb6
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v11.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_M3_V11_H
+#define QOS_INIT_M3_V11_H
+
+void qos_init_m3_v11(void);
+
+#endif /* QOS_INIT_M3_V11_H */
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v11_mstat195.h b/drivers/renesas/rcar/qos/M3/qos_init_m3_v11_mstat195.h
new file mode 100644
index 0000000..d7e7777
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v11_mstat195.h
@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000FFFFUL,
+	/* 0x0038, */ 0x001004040000FFFFUL,
+	/* 0x0040, */ 0x001414090000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x00140C0A0000FFFFUL,
+	/* 0x0060, */ 0x00140C0A0000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x001004030000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001414090000FFFFUL,
+	/* 0x0090, */ 0x001408070000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C04020000FFFFUL,
+	/* 0x00a8, */ 0x000C04010000FFFFUL,
+	/* 0x00b0, */ 0x000C04010000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C04020000FFFFUL,
+	/* 0x00c8, */ 0x000C04010000FFFFUL,
+	/* 0x00d0, */ 0x000C04010000FFFFUL,
+	/* 0x00d8, */ 0x000C08050000FFFFUL,
+	/* 0x00e0, */ 0x000C14120000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001024090000FFFFUL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x00100C090000FFFFUL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100C0B0000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0010100D0000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x00100C0B0000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008060000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00102C2C0000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x00100C0B0000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFFFUL,
+	/* 0x0268, */ 0x001408010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C04010000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x001408010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x001408010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x001200200BDFFC01UL,
+	/* 0x0008, */ 0x001200200BDFFC01UL,
+	/* 0x0010, */ 0x001200200BDFFC01UL,
+	/* 0x0018, */ 0x001200200BDFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x001200100BD03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100600BDFFC01UL,
+	/* 0x01c8, */ 0x002100600BDFFC01UL,
+	/* 0x01d0, */ 0x002100600BDFFC01UL,
+	/* 0x01d8, */ 0x002100600BDFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x002100200BDFFC01UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x002100200BDFFC01UL,
+	/* 0x0218, */ 0x001100200BDFFC01UL,
+	/* 0x0220, */ 0x001100200BDFFC01UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x001100200BDFFC01UL,
+	/* 0x0238, */ 0x001100200BDFFC01UL,
+	/* 0x0240, */ 0x001200200BDFFC01UL,
+	/* 0x0248, */ 0x001100200BDFFC01UL,
+	/* 0x0250, */ 0x001200200BDFFC01UL,
+	/* 0x0258, */ 0x001100200BDFFC01UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x001100400BDFFC01UL,
+	/* 0x02f8, */ 0x001100600BDFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x001100400BDFFC01UL,
+	/* 0x0310, */ 0x001100600BDFFC01UL,
+	/* 0x0318, */ 0x001200100BD03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v11_mstat390.h b/drivers/renesas/rcar/qos/M3/qos_init_m3_v11_mstat390.h
new file mode 100644
index 0000000..a9520c3
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v11_mstat390.h
@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000FFFFUL,
+	/* 0x0038, */ 0x001008070000FFFFUL,
+	/* 0x0040, */ 0x001424120000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x001414130000FFFFUL,
+	/* 0x0060, */ 0x001414130000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x001008050000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001424120000FFFFUL,
+	/* 0x0090, */ 0x0014100D0000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C08040000FFFFUL,
+	/* 0x00a8, */ 0x000C04020000FFFFUL,
+	/* 0x00b0, */ 0x000C04020000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C08040000FFFFUL,
+	/* 0x00c8, */ 0x000C04020000FFFFUL,
+	/* 0x00d0, */ 0x000C04020000FFFFUL,
+	/* 0x00d8, */ 0x000C0C0A0000FFFFUL,
+	/* 0x00e0, */ 0x000C24230000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001044110000FFFFUL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001014110000FFFFUL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x001018150000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00101C190000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x001018150000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x00100C0B0000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001058570000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x001018150000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFFFUL,
+	/* 0x0268, */ 0x001410010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C08020000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x00140C010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x00140C010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0012003005EFFC01UL,
+	/* 0x0008, */ 0x0012003005EFFC01UL,
+	/* 0x0010, */ 0x0012003005EFFC01UL,
+	/* 0x0018, */ 0x0012003005EFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001005E03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100B005EFFC01UL,
+	/* 0x01c8, */ 0x002100B005EFFC01UL,
+	/* 0x01d0, */ 0x002100B005EFFC01UL,
+	/* 0x01d8, */ 0x002100B005EFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021003005EFFC01UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021003005EFFC01UL,
+	/* 0x0218, */ 0x0011003005EFFC01UL,
+	/* 0x0220, */ 0x0011003005EFFC01UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011003005EFFC01UL,
+	/* 0x0238, */ 0x0011003005EFFC01UL,
+	/* 0x0240, */ 0x0012003005EFFC01UL,
+	/* 0x0248, */ 0x0011003005EFFC01UL,
+	/* 0x0250, */ 0x0012003005EFFC01UL,
+	/* 0x0258, */ 0x0011003005EFFC01UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011007005EFFC01UL,
+	/* 0x02f8, */ 0x001100B005EFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0011007005EFFC01UL,
+	/* 0x0310, */ 0x001100B005EFFC01UL,
+	/* 0x0318, */ 0x0012001005E03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v11_qoswt195.h b/drivers/renesas/rcar/qos/M3/qos_init_m3_v11_qoswt195.h
new file mode 100644
index 0000000..04c7efd
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v11_qoswt195.h
@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000C010UL,
+	/* 0x0038, */ 0x001004040000C010UL,
+	/* 0x0040, */ 0x001414090000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x00140C0A0000C010UL,
+	/* 0x0060, */ 0x00140C0A0000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x001004030000C010UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001414090000FFF0UL,
+	/* 0x0090, */ 0x001408070000C010UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFF0UL,
+	/* 0x0268, */ 0x001408010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C04010000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v11_qoswt390.h b/drivers/renesas/rcar/qos/M3/qos_init_m3_v11_qoswt390.h
new file mode 100644
index 0000000..73f81f5
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v11_qoswt390.h
@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000C010UL,
+	/* 0x0038, */ 0x001008070000C010UL,
+	/* 0x0040, */ 0x001424120000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x001414130000C010UL,
+	/* 0x0060, */ 0x001414130000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x001008050000C010UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001424120000FFF0UL,
+	/* 0x0090, */ 0x0014100D0000C010UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFF0UL,
+	/* 0x0268, */ 0x001410010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C08020000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v30.c b/drivers/renesas/rcar/qos/M3/qos_init_m3_v30.c
new file mode 100644
index 0000000..43d21d7
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v30.c
@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) 2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+#include "qos_init_m3_v30.h"
+
+#define	RCAR_QOS_VERSION			"rev.0.04"
+
+#define QOSWT_TIME_BANK0			20000000U	/* unit:ns */
+
+#define	QOSWT_WTEN_ENABLE			0x1U
+
+#define QOSCTRL_REF_ARS_ARBSTOPCYCLE_M3_30	(SL_INIT_SSLOTCLK_M3_30 - 0x5U)
+
+#define OSWT_WTREF_SLOT0_EN_REQ1_SLOT		3U
+#define OSWT_WTREF_SLOT0_EN_REQ2_SLOT		9U
+#define QOSWT_WTREF_SLOT0_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+#define QOSWT_WTREF_SLOT1_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+
+#define QOSWT_WTSET0_REQ_SSLOT0			5U
+#define WT_BASE_SUB_SLOT_NUM0			12U
+#define QOSWT_WTSET0_PERIOD0_M3_30			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_M3_30) - 1U)
+#define QOSWT_WTSET0_SSLOT0			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET0_SLOTSLOT0			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#define QOSWT_WTSET1_PERIOD1_M3_30			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_M3_30) - 1U)
+#define QOSWT_WTSET1_SSLOT1			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET1_SLOTSLOT1			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_m3_v30_mstat195.h"
+#else
+#include "qos_init_m3_v30_mstat390.h"
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_m3_v30_qoswt195.h"
+#else
+#include "qos_init_m3_v30_qoswt390.h"
+#endif
+
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+#endif
+
+struct rcar_gen3_dbsc_qos_settings m3_v30_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218 },
+	{ DBSC_DBCAM0CNF2, 0x000000F4 },
+	{ DBSC_DBCAM0CNF3, 0x00000000 },
+	{ DBSC_DBSCHCNT0, 0x000F0037 },
+	{ DBSC_DBSCHSZ0, 0x00000001 },
+	{ DBSC_DBSCHRW0, 0x22421111 },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123 },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00 },
+	{ DBSC_DBSCHQOS01, 0x00000B00 },
+	{ DBSC_DBSCHQOS02, 0x00000000 },
+	{ DBSC_DBSCHQOS03, 0x00000000 },
+	{ DBSC_DBSCHQOS40, 0x00000300 },
+	{ DBSC_DBSCHQOS41, 0x000002F0 },
+	{ DBSC_DBSCHQOS42, 0x00000200 },
+	{ DBSC_DBSCHQOS43, 0x00000100 },
+	{ DBSC_DBSCHQOS90, 0x00000100 },
+	{ DBSC_DBSCHQOS91, 0x000000F0 },
+	{ DBSC_DBSCHQOS92, 0x000000A0 },
+	{ DBSC_DBSCHQOS93, 0x00000040 },
+	{ DBSC_DBSCHQOS120, 0x00000040 },
+	{ DBSC_DBSCHQOS121, 0x00000030 },
+	{ DBSC_DBSCHQOS122, 0x00000020 },
+	{ DBSC_DBSCHQOS123, 0x00000010 },
+	{ DBSC_DBSCHQOS130, 0x00000100 },
+	{ DBSC_DBSCHQOS131, 0x000000F0 },
+	{ DBSC_DBSCHQOS132, 0x000000A0 },
+	{ DBSC_DBSCHQOS133, 0x00000040 },
+	{ DBSC_DBSCHQOS140, 0x000000C0 },
+	{ DBSC_DBSCHQOS141, 0x000000B0 },
+	{ DBSC_DBSCHQOS142, 0x00000080 },
+	{ DBSC_DBSCHQOS143, 0x00000040 },
+	{ DBSC_DBSCHQOS150, 0x00000040 },
+	{ DBSC_DBSCHQOS151, 0x00000030 },
+	{ DBSC_DBSCHQOS152, 0x00000020 },
+	{ DBSC_DBSCHQOS153, 0x00000010 },
+};
+
+void qos_init_m3_v30(void)
+{
+	rcar_qos_dbsc_setting(m3_v30_qos, ARRAY_SIZE(m3_v30_qos), true);
+
+	/* DRAM Split Address mapping */
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH
+ #if RCAR_LSI == RCAR_M3
+  #error "Don't set DRAM Split 4ch(M3)"
+ #else
+	ERROR("DRAM Split 4ch not supported.(M3)");
+	panic();
+ #endif
+#elif (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH) || \
+      (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO)
+	NOTICE("BL2: DRAM Split is 2ch\n");
+	io_write_32(AXI_ADSPLCR0, 0x00000000U);
+	io_write_32(AXI_ADSPLCR1, ADSPLCR0_ADRMODE_DEFAULT
+				  | ADSPLCR0_SPLITSEL(0xFFU)
+				  | ADSPLCR0_AREA(0x1DU)
+				  | ADSPLCR0_SWP);
+	io_write_32(AXI_ADSPLCR2, 0x00001004U);
+	io_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+	NOTICE("BL2: DRAM refresh interval 1.95 usec\n");
+#else
+	NOTICE("BL2: DRAM refresh interval 3.9 usec\n");
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	NOTICE("BL2: Periodic Write DQ Training\n");
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	io_write_32(QOSCTRL_RAS, 0x00000044U);
+	io_write_64(QOSCTRL_DANN, 0x0404020002020201UL);
+	io_write_32(QOSCTRL_DANT, 0x0020100AU);
+	io_write_32(QOSCTRL_FSS, 0x0000000AU);
+	io_write_32(QOSCTRL_INSFC, 0x06330001U);
+	io_write_32(QOSCTRL_EARLYR, 0x00000001U);
+	io_write_32(QOSCTRL_RACNT0, 0x02010003U);	/* GPU Boost Mode ON */
+
+	/* GPU Boost Mode */
+	io_write_32(QOSCTRL_STATGEN0, 0x00000001U);
+
+	io_write_32(QOSCTRL_SL_INIT, SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT | SL_INIT_SSLOTCLK_M3_30);
+	io_write_32(QOSCTRL_REF_ARS, ((QOSCTRL_REF_ARS_ARBSTOPCYCLE_M3_30 << 16)));
+
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		io_write_64(QOSBW_FIX_QOS_BANK0 + i * 8, mstat_fix[i]);
+		io_write_64(QOSBW_FIX_QOS_BANK1 + i * 8, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		io_write_64(QOSBW_BE_QOS_BANK0 + i * 8, mstat_be[i]);
+		io_write_64(QOSBW_BE_QOS_BANK1 + i * 8, mstat_be[i]);
+	}
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	for (i = 0U; i < ARRAY_SIZE(qoswt_fix); i++) {
+		io_write_64(QOSWT_FIX_WTQOS_BANK0 + i * 8, qoswt_fix[i]);
+		io_write_64(QOSWT_FIX_WTQOS_BANK1 + i * 8, qoswt_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(qoswt_be); i++) {
+		io_write_64(QOSWT_BE_WTQOS_BANK0 + i * 8, qoswt_be[i]);
+		io_write_64(QOSWT_BE_WTQOS_BANK1 + i * 8, qoswt_be[i]);
+	}
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	/* RT bus Leaf setting */
+	io_write_32(RT_ACT0, 0x00000000U);
+	io_write_32(RT_ACT1, 0x00000000U);
+
+	/* CCI bus Leaf setting */
+	io_write_32(CPU_ACT0, 0x00000003U);
+	io_write_32(CPU_ACT1, 0x00000003U);
+	io_write_32(CPU_ACT2, 0x00000003U);
+	io_write_32(CPU_ACT3, 0x00000003U);
+
+	io_write_32(QOSCTRL_RAEN,  0x00000001U);
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	/*  re-write training setting */
+	io_write_32(QOSWT_WTREF,  ((QOSWT_WTREF_SLOT1_EN << 16)       | QOSWT_WTREF_SLOT0_EN));
+	io_write_32(QOSWT_WTSET0, ((QOSWT_WTSET0_PERIOD0_M3_30 << 16) | (QOSWT_WTSET0_SSLOT0 << 8) | QOSWT_WTSET0_SLOTSLOT0));
+	io_write_32(QOSWT_WTSET1, ((QOSWT_WTSET1_PERIOD1_M3_30 << 16) | (QOSWT_WTSET1_SSLOT1 << 8) | QOSWT_WTSET1_SLOTSLOT1));
+
+	io_write_32(QOSWT_WTEN,   QOSWT_WTEN_ENABLE);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	io_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	io_write_32(QOSCTRL_RAEN,  0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v30.h b/drivers/renesas/rcar/qos/M3/qos_init_m3_v30.h
new file mode 100644
index 0000000..a89d512
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v30.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_H_M3_V30__
+#define QOS_INIT_H_M3_V30__
+
+void qos_init_m3_v30(void);
+
+#endif	/* QOS_INIT_H_M3_V30__ */
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v30_mstat195.h b/drivers/renesas/rcar/qos/M3/qos_init_m3_v30_mstat195.h
new file mode 100644
index 0000000..2ab14da
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v30_mstat195.h
@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000FFFFUL,
+	/* 0x0038, */ 0x001004040000FFFFUL,
+	/* 0x0040, */ 0x001414090000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x00140C0A0000FFFFUL,
+	/* 0x0060, */ 0x00140C0A0000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x001004030000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001414090000FFFFUL,
+	/* 0x0090, */ 0x001408070000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C04020000FFFFUL,
+	/* 0x00a8, */ 0x000C04010000FFFFUL,
+	/* 0x00b0, */ 0x000C04010000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C04020000FFFFUL,
+	/* 0x00c8, */ 0x000C04010000FFFFUL,
+	/* 0x00d0, */ 0x000C04010000FFFFUL,
+	/* 0x00d8, */ 0x000C08050000FFFFUL,
+	/* 0x00e0, */ 0x000C10100000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001024090000FFFFUL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x00100C090000FFFFUL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x000C10100000FFFFUL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100C0B0000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0010100D0000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x00100C0B0000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008060000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00102C2C0000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x00100C0B0000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFFFUL,
+	/* 0x0268, */ 0x001408010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C04010000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x001408010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x001408010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x001200200BDFFC01UL,
+	/* 0x0008, */ 0x001200200BDFFC01UL,
+	/* 0x0010, */ 0x001200200BDFFC01UL,
+	/* 0x0018, */ 0x001200200BDFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x001200100BD03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100600BDFFC01UL,
+	/* 0x01c8, */ 0x002100600BDFFC01UL,
+	/* 0x01d0, */ 0x002100600BDFFC01UL,
+	/* 0x01d8, */ 0x002100600BDFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x002100200BDFFC01UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x002100200BDFFC01UL,
+	/* 0x0218, */ 0x001100200BDFFC01UL,
+	/* 0x0220, */ 0x001100200BDFFC01UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x001100200BDFFC01UL,
+	/* 0x0238, */ 0x001100200BDFFC01UL,
+	/* 0x0240, */ 0x001200200BDFFC01UL,
+	/* 0x0248, */ 0x001100200BDFFC01UL,
+	/* 0x0250, */ 0x001200200BDFFC01UL,
+	/* 0x0258, */ 0x001100200BDFFC01UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x001100400BDFFC01UL,
+	/* 0x02f8, */ 0x001100600BDFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x001100400BDFFC01UL,
+	/* 0x0310, */ 0x001100600BDFFC01UL,
+	/* 0x0318, */ 0x001200100BD03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v30_mstat390.h b/drivers/renesas/rcar/qos/M3/qos_init_m3_v30_mstat390.h
new file mode 100644
index 0000000..faac3d9
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v30_mstat390.h
@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000FFFFUL,
+	/* 0x0038, */ 0x001008070000FFFFUL,
+	/* 0x0040, */ 0x001424120000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x001414130000FFFFUL,
+	/* 0x0060, */ 0x001414130000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x001008050000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001424120000FFFFUL,
+	/* 0x0090, */ 0x0014100D0000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C08040000FFFFUL,
+	/* 0x00a8, */ 0x000C04020000FFFFUL,
+	/* 0x00b0, */ 0x000C04020000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C08040000FFFFUL,
+	/* 0x00c8, */ 0x000C04020000FFFFUL,
+	/* 0x00d0, */ 0x000C04020000FFFFUL,
+	/* 0x00d8, */ 0x000C0C0A0000FFFFUL,
+	/* 0x00e0, */ 0x000C201F0000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001044110000FFFFUL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001014110000FFFFUL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x000C201F0000FFFFUL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x001018150000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00101C190000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x001018150000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x00100C0B0000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001058570000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x001018150000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFFFUL,
+	/* 0x0268, */ 0x001410010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C08020000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x00140C010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x00140C010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0012003005EFFC01UL,
+	/* 0x0008, */ 0x0012003005EFFC01UL,
+	/* 0x0010, */ 0x0012003005EFFC01UL,
+	/* 0x0018, */ 0x0012003005EFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001005E03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100B005EFFC01UL,
+	/* 0x01c8, */ 0x002100B005EFFC01UL,
+	/* 0x01d0, */ 0x002100B005EFFC01UL,
+	/* 0x01d8, */ 0x002100B005EFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021003005EFFC01UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021003005EFFC01UL,
+	/* 0x0218, */ 0x0011003005EFFC01UL,
+	/* 0x0220, */ 0x0011003005EFFC01UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011003005EFFC01UL,
+	/* 0x0238, */ 0x0011003005EFFC01UL,
+	/* 0x0240, */ 0x0012003005EFFC01UL,
+	/* 0x0248, */ 0x0011003005EFFC01UL,
+	/* 0x0250, */ 0x0012003005EFFC01UL,
+	/* 0x0258, */ 0x0011003005EFFC01UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011007005EFFC01UL,
+	/* 0x02f8, */ 0x001100B005EFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0011007005EFFC01UL,
+	/* 0x0310, */ 0x001100B005EFFC01UL,
+	/* 0x0318, */ 0x0012001005E03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v30_qoswt195.h b/drivers/renesas/rcar/qos/M3/qos_init_m3_v30_qoswt195.h
new file mode 100644
index 0000000..6761f5d
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v30_qoswt195.h
@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000C010UL,
+	/* 0x0038, */ 0x001004040000C010UL,
+	/* 0x0040, */ 0x001414090000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x00140C0A0000C010UL,
+	/* 0x0060, */ 0x00140C0A0000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x001004030000C010UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001414090000FFF0UL,
+	/* 0x0090, */ 0x001408070000C010UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFF0UL,
+	/* 0x0268, */ 0x001408010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C04010000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/M3/qos_init_m3_v30_qoswt390.h b/drivers/renesas/rcar/qos/M3/qos_init_m3_v30_qoswt390.h
new file mode 100644
index 0000000..1deed59
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3/qos_init_m3_v30_qoswt390.h
@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000C010UL,
+	/* 0x0038, */ 0x001008070000C010UL,
+	/* 0x0040, */ 0x001424120000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x001414130000C010UL,
+	/* 0x0060, */ 0x001414130000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x001008050000C010UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001424120000FFF0UL,
+	/* 0x0090, */ 0x0014100D0000C010UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFF0UL,
+	/* 0x0268, */ 0x001410010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C08020000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10.c b/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10.c
new file mode 100644
index 0000000..446340b
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10.c
@@ -0,0 +1,203 @@
+/*
+ * Copyright (c) 2017-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+#include "qos_init_m3n_v10.h"
+
+#define	RCAR_QOS_VERSION			"rev.0.09"
+
+#define REF_ARS_ARBSTOPCYCLE_M3N			\
+	(((SL_INIT_SSLOTCLK_M3N) - 5U) << 16U)
+
+#define QOSWT_TIME_BANK0			20000000U	/* unit:ns */
+
+#define	QOSWT_WTEN_ENABLE			0x1U
+
+#define OSWT_WTREF_SLOT0_EN_REQ1_SLOT		3U
+#define OSWT_WTREF_SLOT0_EN_REQ2_SLOT		9U
+#define QOSWT_WTREF_SLOT0_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+#define QOSWT_WTREF_SLOT1_EN			QOSWT_WTREF_SLOT0_EN
+
+#define QOSWT_WTSET0_REQ_SSLOT0			5U
+#define WT_BASE_SUB_SLOT_NUM0			12U
+#define QOSWT_WTSET0_PERIOD0_M3N			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_M3N) - 1U)
+#define QOSWT_WTSET0_SSLOT0			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET0_SLOTSLOT0			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#define QOSWT_WTSET1_PERIOD1_M3N		QOSWT_WTSET0_PERIOD0_M3N
+#define QOSWT_WTSET1_SSLOT1			QOSWT_WTSET0_SSLOT0
+#define QOSWT_WTSET1_SLOTSLOT1			QOSWT_WTSET0_SLOTSLOT0
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_m3n_v10_mstat195.h"
+#else
+#include "qos_init_m3n_v10_mstat390.h"
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_m3n_v10_qoswt195.h"
+#else
+#include "qos_init_m3n_v10_qoswt390.h"
+#endif
+
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+#endif
+
+struct rcar_gen3_dbsc_qos_settings m3n_v10_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218 },
+	{ DBSC_DBCAM0CNF2, 0x000000F4 },
+	{ DBSC_DBSCHCNT0, 0x000F0037 },
+	{ DBSC_DBSCHSZ0, 0x00000001 },
+	{ DBSC_DBSCHRW0, 0x22421111 },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123 },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00 },
+	{ DBSC_DBSCHQOS01, 0x00000B00 },
+	{ DBSC_DBSCHQOS02, 0x00000000 },
+	{ DBSC_DBSCHQOS03, 0x00000000 },
+	{ DBSC_DBSCHQOS40, 0x00000300 },
+	{ DBSC_DBSCHQOS41, 0x000002F0 },
+	{ DBSC_DBSCHQOS42, 0x00000200 },
+	{ DBSC_DBSCHQOS43, 0x00000100 },
+	{ DBSC_DBSCHQOS90, 0x00000100 },
+	{ DBSC_DBSCHQOS91, 0x000000F0 },
+	{ DBSC_DBSCHQOS92, 0x000000A0 },
+	{ DBSC_DBSCHQOS93, 0x00000040 },
+	{ DBSC_DBSCHQOS130, 0x00000100 },
+	{ DBSC_DBSCHQOS131, 0x000000F0 },
+	{ DBSC_DBSCHQOS132, 0x000000A0 },
+	{ DBSC_DBSCHQOS133, 0x00000040 },
+	{ DBSC_DBSCHQOS140, 0x000000C0 },
+	{ DBSC_DBSCHQOS141, 0x000000B0 },
+	{ DBSC_DBSCHQOS142, 0x00000080 },
+	{ DBSC_DBSCHQOS143, 0x00000040 },
+	{ DBSC_DBSCHQOS150, 0x00000040 },
+	{ DBSC_DBSCHQOS151, 0x00000030 },
+	{ DBSC_DBSCHQOS152, 0x00000020 },
+	{ DBSC_DBSCHQOS153, 0x00000010 },
+};
+
+void qos_init_m3n_v10(void)
+{
+	rcar_qos_dbsc_setting(m3n_v10_qos, ARRAY_SIZE(m3n_v10_qos), true);
+
+	/* DRAM Split Address mapping */
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH
+#if RCAR_LSI == RCAR_M3N
+#error "Don't set DRAM Split 4ch(M3N)"
+#else
+	ERROR("DRAM Split 4ch not supported.(M3N)");
+	panic();
+#endif
+#elif (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH)
+#if RCAR_LSI == RCAR_M3N
+#error "Don't set DRAM Split 2ch(M3N)"
+#else
+	ERROR("DRAM Split 2ch not supported.(M3N)");
+	panic();
+#endif
+#else
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+	NOTICE("BL2: DRAM refresh interval 1.95 usec\n");
+#else
+	NOTICE("BL2: DRAM refresh interval 3.9 usec\n");
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	NOTICE("BL2: Periodic Write DQ Training\n");
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	io_write_32(QOSCTRL_RAS, 0x00000028U);
+	io_write_64(QOSCTRL_DANN, 0x0402000002020201UL);
+	io_write_32(QOSCTRL_DANT, 0x00100804U);
+	io_write_32(QOSCTRL_FSS, 0x0000000AU);
+	io_write_32(QOSCTRL_INSFC, 0x06330001U);
+	io_write_32(QOSCTRL_EARLYR, 0x00000001U);
+	io_write_32(QOSCTRL_RACNT0, 0x00010003U);
+
+	io_write_32(QOSCTRL_SL_INIT,
+		    SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT |
+		    SL_INIT_SSLOTCLK_M3N);
+	io_write_32(QOSCTRL_REF_ARS, REF_ARS_ARBSTOPCYCLE_M3N);
+
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		io_write_64(QOSBW_FIX_QOS_BANK0 + i * 8, mstat_fix[i]);
+		io_write_64(QOSBW_FIX_QOS_BANK1 + i * 8, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		io_write_64(QOSBW_BE_QOS_BANK0 + i * 8, mstat_be[i]);
+		io_write_64(QOSBW_BE_QOS_BANK1 + i * 8, mstat_be[i]);
+	}
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	for (i = 0U; i < ARRAY_SIZE(qoswt_fix); i++) {
+		io_write_64(QOSWT_FIX_WTQOS_BANK0 + i * 8,
+			    qoswt_fix[i]);
+		io_write_64(QOSWT_FIX_WTQOS_BANK1 + i * 8,
+			    qoswt_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(qoswt_be); i++) {
+		io_write_64(QOSWT_BE_WTQOS_BANK0 + i * 8, qoswt_be[i]);
+		io_write_64(QOSWT_BE_WTQOS_BANK1 + i * 8, qoswt_be[i]);
+	}
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	/* RT bus Leaf setting */
+	io_write_32(RT_ACT0, 0x00000000U);
+	io_write_32(RT_ACT1, 0x00000000U);
+
+	/* CCI bus Leaf setting */
+	io_write_32(CPU_ACT0, 0x00000003U);
+	io_write_32(CPU_ACT1, 0x00000003U);
+
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	/*  re-write training setting */
+	io_write_32(QOSWT_WTREF,
+		    ((QOSWT_WTREF_SLOT1_EN << 16) | QOSWT_WTREF_SLOT0_EN));
+	io_write_32(QOSWT_WTSET0,
+		    ((QOSWT_WTSET0_PERIOD0_M3N << 16) |
+		     (QOSWT_WTSET0_SSLOT0 << 8) | QOSWT_WTSET0_SLOTSLOT0));
+	io_write_32(QOSWT_WTSET1,
+		    ((QOSWT_WTSET1_PERIOD1_M3N << 16) |
+		     (QOSWT_WTSET1_SSLOT1 << 8) | QOSWT_WTSET1_SLOTSLOT1));
+
+	io_write_32(QOSWT_WTEN, QOSWT_WTEN_ENABLE);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	io_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	io_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10.h b/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10.h
new file mode 100644
index 0000000..0cd0c85
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2017, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_M3N_V10_H
+#define QOS_INIT_M3N_V10_H
+
+void qos_init_m3n_v10(void);
+
+#endif /* QOS_INIT_M3N_V10_H */
diff --git a/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10_mstat195.h b/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10_mstat195.h
new file mode 100644
index 0000000..9b8b9e9
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10_mstat195.h
@@ -0,0 +1,241 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004320000FFFFUL,
+	/* 0x0038, */ 0x001004320000FFFFUL,
+	/* 0x0040, */ 0x00140C5D0000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404040000FFFFUL,
+	/* 0x0058, */ 0x00140C940000FFFFUL,
+	/* 0x0060, */ 0x00140C940000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404040000FFFFUL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0014041F0000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C041D0000FFFFUL,
+	/* 0x00a8, */ 0x000C04090000FFFFUL,
+	/* 0x00b0, */ 0x000C040B0000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C041D0000FFFFUL,
+	/* 0x00c8, */ 0x000C04090000FFFFUL,
+	/* 0x00d0, */ 0x000C040B0000FFFFUL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001024840000FFFFUL,
+	/* 0x00f8, */ 0x000C084F0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x00100C840000FFFFUL,
+	/* 0x0118, */ 0x000C21E60000FFFFUL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100CA50000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x001010C90000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x00100CA50000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008530000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00101D9D0000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x00100CA50000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04050000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04050000FFFFUL,
+	/* 0x0210, */ 0x000C04050000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08150000FFFFUL,
+	/* 0x0268, */ 0x001408020000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C04090000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408020000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04050000FFFFUL,
+	/* 0x02a8, */ 0x000C04050000FFFFUL,
+	/* 0x02b0, */ 0x001408050000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04050000FFFFUL,
+	/* 0x02d8, */ 0x000C04050000FFFFUL,
+	/* 0x02e0, */ 0x001408050000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x000C04010000FFFFUL,
+	/* 0x0378, */ 0x000C04010000FFFFUL,
+	/* 0x0380, */ 0x000C04050000FFFFUL,
+	/* 0x0388, */ 0x000C04050000FFFFUL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x001200100BD03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002106000BDFFC01UL,
+	/* 0x01c8, */ 0x002106000BDFFC01UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x002101000BDF2401UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x002101000BDF2401UL,
+	/* 0x0218, */ 0x001101000BDF2401UL,
+	/* 0x0220, */ 0x001101000BDF2401UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x001101000BDF2401UL,
+	/* 0x0238, */ 0x001101000BDF2401UL,
+	/* 0x0240, */ 0x001201000BDF2401UL,
+	/* 0x0248, */ 0x001101000BDF2401UL,
+	/* 0x0250, */ 0x001201000BDF2401UL,
+	/* 0x0258, */ 0x001101000BDF2401UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x001106000BDFFC01UL,
+	/* 0x02f8, */ 0x001106000BDFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x001200100BD03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x001206000BDFFC01UL,
+	/* 0x0360, */ 0x001206000BDFFC01UL,
+	/* 0x0368, */ 0x001200100BD03401UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x001200100BD03401UL,
+};
diff --git a/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10_mstat390.h b/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10_mstat390.h
new file mode 100644
index 0000000..19143ed
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10_mstat390.h
@@ -0,0 +1,241 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008630000FFFFUL,
+	/* 0x0038, */ 0x001008630000FFFFUL,
+	/* 0x0040, */ 0x001418BA0000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404070000FFFFUL,
+	/* 0x0058, */ 0x001415270000FFFFUL,
+	/* 0x0060, */ 0x001415270000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404070000FFFFUL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0014083E0000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C08390000FFFFUL,
+	/* 0x00a8, */ 0x000C04110000FFFFUL,
+	/* 0x00b0, */ 0x000C04150000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C08390000FFFFUL,
+	/* 0x00c8, */ 0x000C04110000FFFFUL,
+	/* 0x00d0, */ 0x000C04150000FFFFUL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001045080000FFFFUL,
+	/* 0x00f8, */ 0x000C0C9E0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001015080000FFFFUL,
+	/* 0x0118, */ 0x000C43CB0000FFFFUL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0010194A0000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00101D910000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0010194A0000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x00100CA50000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001037390000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0010194A0000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04020000FFFFUL,
+	/* 0x01f0, */ 0x000C04090000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04090000FFFFUL,
+	/* 0x0210, */ 0x000C04090000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C2A0000FFFFUL,
+	/* 0x0268, */ 0x001410040000FFFFUL,
+	/* 0x0270, */ 0x001404020000FFFFUL,
+	/* 0x0278, */ 0x000C08110000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410040000FFFFUL,
+	/* 0x0298, */ 0x001404020000FFFFUL,
+	/* 0x02a0, */ 0x000C04090000FFFFUL,
+	/* 0x02a8, */ 0x000C04090000FFFFUL,
+	/* 0x02b0, */ 0x00140C090000FFFFUL,
+	/* 0x02b8, */ 0x000C04020000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04090000FFFFUL,
+	/* 0x02d8, */ 0x000C04090000FFFFUL,
+	/* 0x02e0, */ 0x00140C090000FFFFUL,
+	/* 0x02e8, */ 0x000C04020000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x000C04020000FFFFUL,
+	/* 0x0378, */ 0x000C04020000FFFFUL,
+	/* 0x0380, */ 0x000C04090000FFFFUL,
+	/* 0x0388, */ 0x000C04090000FFFFUL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001005E03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0021060005EFFC01UL,
+	/* 0x01c8, */ 0x0021060005EFFC01UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021010005E79401UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021010005E79401UL,
+	/* 0x0218, */ 0x0011010005E79401UL,
+	/* 0x0220, */ 0x0011010005E79401UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011010005E79401UL,
+	/* 0x0238, */ 0x0011010005E79401UL,
+	/* 0x0240, */ 0x0012010005E79401UL,
+	/* 0x0248, */ 0x0011010005E79401UL,
+	/* 0x0250, */ 0x0012010005E79401UL,
+	/* 0x0258, */ 0x0011010005E79401UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011060005EFFC01UL,
+	/* 0x02f8, */ 0x0011060005EFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0012001005E03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0012060005EFFC01UL,
+	/* 0x0360, */ 0x0012060005EFFC01UL,
+	/* 0x0368, */ 0x0012001005E03401UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0012001005E03401UL,
+};
diff --git a/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10_qoswt195.h b/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10_qoswt195.h
new file mode 100644
index 0000000..d2e8040
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10_qoswt195.h
@@ -0,0 +1,241 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004320000C010UL,
+	/* 0x0038, */ 0x001004320000C010UL,
+	/* 0x0040, */ 0x00140C5D0000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x00140C940000C010UL,
+	/* 0x0060, */ 0x00140C940000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0014041F0000FFF0UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08150000FFF0UL,
+	/* 0x0268, */ 0x001408020000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C04090000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408020000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10_qoswt390.h b/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10_qoswt390.h
new file mode 100644
index 0000000..84f657a
--- /dev/null
+++ b/drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10_qoswt390.h
@@ -0,0 +1,241 @@
+/*
+ * Copyright (c) 2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008630000C010UL,
+	/* 0x0038, */ 0x001008630000C010UL,
+	/* 0x0040, */ 0x001418BA0000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x001415270000C010UL,
+	/* 0x0060, */ 0x001415270000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0014083E0000FFF0UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C2A0000FFF0UL,
+	/* 0x0268, */ 0x001410040000FFF0UL,
+	/* 0x0270, */ 0x001404020000FFF0UL,
+	/* 0x0278, */ 0x000C08110000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410040000FFF0UL,
+	/* 0x0298, */ 0x001404020000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
diff --git a/drivers/renesas/rcar/qos/V3M/qos_init_v3m.c b/drivers/renesas/rcar/qos/V3M/qos_init_v3m.c
new file mode 100644
index 0000000..076876c
--- /dev/null
+++ b/drivers/renesas/rcar/qos/V3M/qos_init_v3m.c
@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+#include "qos_init_v3m.h"
+
+#define	RCAR_QOS_VERSION		"rev.0.01"
+
+#include "qos_init_v3m_mstat.h"
+
+struct rcar_gen3_dbsc_qos_settings v3m_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00044218 },
+	{ DBSC_DBCAM0CNF2, 0x000000F4 },
+	{ DBSC_DBSCHCNT0, 0x080F003F },
+	{ DBSC_DBSCHCNT1, 0x00001010 },
+
+	{ DBSC_DBSCHSZ0, 0x00000001 },
+	{ DBSC_DBSCHRW0, 0x22421111 },
+	{ DBSC_DBSCHRW1, 0x00180034 },
+	{ DBSC_SCFCTST0, 0x180B1708 },
+	{ DBSC_SCFCTST1, 0x0808070C },
+	{ DBSC_SCFCTST2, 0x012F1123 },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00,  0x0000F000 },
+	{ DBSC_DBSCHQOS01,  0x0000E000 },
+	{ DBSC_DBSCHQOS02,  0x00007000 },
+	{ DBSC_DBSCHQOS03,  0x00000000 },
+	{ DBSC_DBSCHQOS40,  0x0000F000 },
+	{ DBSC_DBSCHQOS41,  0x0000EFFF },
+	{ DBSC_DBSCHQOS42,  0x0000B000 },
+	{ DBSC_DBSCHQOS43,  0x00000000 },
+	{ DBSC_DBSCHQOS90,  0x0000F000 },
+	{ DBSC_DBSCHQOS91,  0x0000EFFF },
+	{ DBSC_DBSCHQOS92,  0x0000D000 },
+	{ DBSC_DBSCHQOS93,  0x00000000 },
+	{ DBSC_DBSCHQOS130, 0x0000F000 },
+	{ DBSC_DBSCHQOS131, 0x0000EFFF },
+	{ DBSC_DBSCHQOS132, 0x0000E800 },
+	{ DBSC_DBSCHQOS133, 0x00007000 },
+	{ DBSC_DBSCHQOS140, 0x0000F000 },
+	{ DBSC_DBSCHQOS141, 0x0000EFFF },
+	{ DBSC_DBSCHQOS142, 0x0000E800 },
+	{ DBSC_DBSCHQOS143, 0x0000B000 },
+	{ DBSC_DBSCHQOS150, 0x000007D0 },
+	{ DBSC_DBSCHQOS151, 0x000007CF },
+	{ DBSC_DBSCHQOS152, 0x000005D0 },
+	{ DBSC_DBSCHQOS153, 0x000003D0 },
+};
+
+void qos_init_v3m(void)
+{
+return;
+
+	rcar_qos_dbsc_setting(v3m_qos, ARRAY_SIZE(v3m_qos), false);
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+	/* Resource Alloc setting */
+	io_write_32(QOSCTRL_RAS,   0x00000020U);
+	io_write_32(QOSCTRL_FIXTH, 0x000F0005U);
+	io_write_32(QOSCTRL_REGGD, 0x00000004U);
+	io_write_64(QOSCTRL_DANN,  0x0202020104040200U);
+	io_write_32(QOSCTRL_DANT,  0x00201008U);
+	io_write_32(QOSCTRL_EC,    0x00080001U);	/* need for H3 ES1 */
+	io_write_64(QOSCTRL_EMS,   0x0000000000000000U);
+	io_write_32(QOSCTRL_INSFC, 0x63C20001U);
+	io_write_32(QOSCTRL_BERR,  0x00000000U);
+
+	/* QOSBW setting */
+	io_write_32(QOSCTRL_SL_INIT, 0x0305007DU);
+	io_write_32(QOSCTRL_REF_ARS, 0x00330000U);
+
+	/* QOSBW SRAM setting */
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		io_write_64(QOSBW_FIX_QOS_BANK0 + i * 8, mstat_fix[i]);
+		io_write_64(QOSBW_FIX_QOS_BANK1 + i * 8, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		io_write_64(QOSBW_BE_QOS_BANK0 + i * 8, mstat_be[i]);
+		io_write_64(QOSBW_BE_QOS_BANK1 + i * 8, mstat_be[i]);
+	}
+
+	/* AXI-IF arbitration setting */
+	io_write_32(DBSC_AXARB, 0x18010000U);
+
+	/* Resource Alloc start */
+	io_write_32(QOSCTRL_RAEN,  0x00000001U);
+
+	/* QOSBW start */
+	io_write_32(QOSCTRL_STATQC, 0x00000001U);
+
+#else
+	NOTICE("BL2: QoS is None\n");
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rcar/qos/V3M/qos_init_v3m.h b/drivers/renesas/rcar/qos/V3M/qos_init_v3m.h
new file mode 100644
index 0000000..2c4278b
--- /dev/null
+++ b/drivers/renesas/rcar/qos/V3M/qos_init_v3m.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2015-2017, Renesas Electronics Corporation
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_H_V3M__
+#define QOS_INIT_H_V3M__
+
+void qos_init_v3m(void);
+
+#endif	/* QOS_INIT_H_V3M__ */
diff --git a/drivers/renesas/rcar/qos/V3M/qos_init_v3m_mstat.h b/drivers/renesas/rcar/qos/V3M/qos_init_v3m_mstat.h
new file mode 100644
index 0000000..d0b7fc3
--- /dev/null
+++ b/drivers/renesas/rcar/qos/V3M/qos_init_v3m_mstat.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+static const uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x000000000000FFFFUL,
+	/* 0x0008, */ 0x000000000000FFFFUL,
+	/* 0x0010, */ 0x000000000000FFFFUL,
+	/* 0x0018, */ 0x000000000000FFFFUL,
+	/* 0x0020, */ 0x001414090000FFFFUL,
+	/* 0x0028, */ 0x000C00000000FFFFUL,
+	/* 0x0030, */ 0x001008040000FFFFUL,
+	/* 0x0038, */ 0x001004040000FFFFUL,
+	/* 0x0040, */ 0x001004040000FFFFUL,
+	/* 0x0048, */ 0x000000000000FFFFUL,
+	/* 0x0050, */ 0x001004040000FFFFUL,
+	/* 0x0058, */ 0x001004040000FFFFUL,
+	/* 0x0060, */ 0x000000000000FFFFUL,
+	/* 0x0068, */ 0x001404040000FFFFUL,
+	/* 0x0070, */ 0x001008030000FFFFUL,
+	/* 0x0078, */ 0x001004030000FFFFUL,
+	/* 0x0080, */ 0x001004030000FFFFUL,
+	/* 0x0088, */ 0x000000000000FFFFUL,
+	/* 0x0090, */ 0x001004040000FFFFUL,
+	/* 0x0098, */ 0x001004040000FFFFUL,
+	/* 0x00A0, */ 0x000000000000FFFFUL,
+	/* 0x00A8, */ 0x000000000000FFFFUL,
+	/* 0x00B0, */ 0x000000000000FFFFUL,
+	/* 0x00B8, */ 0x000000000000FFFFUL,
+	/* 0x00C0, */ 0x000000000000FFFFUL,
+	/* 0x00C8, */ 0x000000000000FFFFUL,
+	/* 0x00D0, */ 0x000000000000FFFFUL,
+	/* 0x00D8, */ 0x000000000000FFFFUL,
+	/* 0x00E0, */ 0x001404020000FFFFUL,
+	/* 0x00E8, */ 0x000000000000FFFFUL,
+	/* 0x00F0, */ 0x000000000000FFFFUL,
+	/* 0x00F8, */ 0x000000000000FFFFUL,
+	/* 0x0100, */ 0x000000000000FFFFUL,
+	/* 0x0108, */ 0x000C04020000FFFFUL,
+	/* 0x0110, */ 0x000000000000FFFFUL,
+	/* 0x0118, */ 0x001404020000FFFFUL,
+	/* 0x0120, */ 0x000000000000FFFFUL,
+	/* 0x0128, */ 0x000000000000FFFFUL,
+	/* 0x0130, */ 0x000000000000FFFFUL,
+	/* 0x0138, */ 0x000000000000FFFFUL,
+	/* 0x0140, */ 0x000000000000FFFFUL,
+	/* 0x0148, */ 0x000000000000FFFFUL,
+};
+
+static const uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x00100020447FFC01UL,
+	/* 0x0008, */ 0x00100020447FFC01UL,
+	/* 0x0010, */ 0x00100040447FFC01UL,
+	/* 0x0018, */ 0x00100040447FFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00A0, */ 0x00100010447FFC01UL,
+	/* 0x00A8, */ 0x00100010447FFC01UL,
+	/* 0x00B0, */ 0x00100010447FFC01UL,
+	/* 0x00B8, */ 0x00100010447FFC01UL,
+	/* 0x00C0, */ 0x00100010447FFC01UL,
+	/* 0x00C8, */ 0x00100010447FFC01UL,
+	/* 0x00D0, */ 0x0000000000000000UL,
+	/* 0x00D8, */ 0x00100010447FFC01UL,
+	/* 0x00E0, */ 0x0000000000000000UL,
+	/* 0x00E8, */ 0x00100010447FFC01UL,
+	/* 0x00F0, */ 0x00100010447FFC01UL,
+	/* 0x00F8, */ 0x00100010447FFC01UL,
+	/* 0x0100, */ 0x00100010447FFC01UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x00100010447FFC01UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x00100010447FFC01UL,
+	/* 0x0128, */ 0x00100010447FFC01UL,
+	/* 0x0130, */ 0x00100010447FFC01UL,
+	/* 0x0138, */ 0x00100010447FFC01UL,
+	/* 0x0140, */ 0x00100020447FFC01UL,
+	/* 0x0148, */ 0x00100020447FFC01UL,
+};
+#endif
diff --git a/drivers/renesas/rcar/qos/qos.mk b/drivers/renesas/rcar/qos/qos.mk
new file mode 100644
index 0000000..da10da2
--- /dev/null
+++ b/drivers/renesas/rcar/qos/qos.mk
@@ -0,0 +1,106 @@
+#
+# Copyright (c) 2015-2018, Renesas Electronics Corporation. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${RCAR_LSI},${RCAR_AUTO})
+#   E3, H3N not available for LSI_AUTO
+    BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v10.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v11.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v20.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v30.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/M3/qos_init_m3_v10.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/M3/qos_init_m3_v11.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/M3/qos_init_m3_v30.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/V3M/qos_init_v3m.c
+else ifdef RCAR_LSI_CUT_COMPAT
+  ifeq (${RCAR_LSI},${RCAR_H3})
+    BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v10.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v11.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v20.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v30.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_H3N})
+    BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3n_v30.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_M3})
+    BL2_SOURCES += drivers/renesas/rcar/qos/M3/qos_init_m3_v10.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/M3/qos_init_m3_v11.c
+    BL2_SOURCES += drivers/renesas/rcar/qos/M3/qos_init_m3_v30.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_M3N})
+    BL2_SOURCES += drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_V3M})
+    BL2_SOURCES += drivers/renesas/rcar/qos/V3M/qos_init_v3m.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_E3})
+    BL2_SOURCES += drivers/renesas/rcar/qos/E3/qos_init_e3_v10.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_D3})
+    BL2_SOURCES += drivers/renesas/rcar/qos/D3/qos_init_d3.c
+  endif
+else
+  ifeq (${RCAR_LSI},${RCAR_H3})
+    ifeq (${LSI_CUT},10)
+      BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v10.c
+    else ifeq (${LSI_CUT},11)
+      BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v11.c
+    else ifeq (${LSI_CUT},20)
+      BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v20.c
+    else ifeq (${LSI_CUT},30)
+      BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v30.c
+    else
+#     LSI_CUT 30 or later
+      BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3_v30.c
+    endif
+  endif
+  ifeq (${RCAR_LSI},${RCAR_H3N})
+    ifeq (${LSI_CUT},30)
+      BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3n_v30.c
+    else
+#     LSI_CUT 30 or later
+      BL2_SOURCES += drivers/renesas/rcar/qos/H3/qos_init_h3n_v30.c
+    endif
+  endif
+  ifeq (${RCAR_LSI},${RCAR_M3})
+    ifeq (${LSI_CUT},10)
+     BL2_SOURCES += drivers/renesas/rcar/qos/M3/qos_init_m3_v10.c
+    else ifeq (${LSI_CUT},11)
+     BL2_SOURCES += drivers/renesas/rcar/qos/M3/qos_init_m3_v11.c
+    else ifeq (${LSI_CUT},13)
+     BL2_SOURCES += drivers/renesas/rcar/qos/M3/qos_init_m3_v11.c
+    else ifeq (${LSI_CUT},30)
+     BL2_SOURCES += drivers/renesas/rcar/qos/M3/qos_init_m3_v30.c
+    else
+#    LSI_CUT 30 or later
+     BL2_SOURCES += drivers/renesas/rcar/qos/M3/qos_init_m3_v30.c
+    endif
+  endif
+  ifeq (${RCAR_LSI},${RCAR_M3N})
+    ifeq (${LSI_CUT},10)
+     BL2_SOURCES += drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10.c
+    else
+#    LSI_CUT 10 or later
+     BL2_SOURCES += drivers/renesas/rcar/qos/M3N/qos_init_m3n_v10.c
+    endif
+  endif
+  ifeq (${RCAR_LSI},${RCAR_V3M})
+    BL2_SOURCES += drivers/renesas/rcar/qos/V3M/qos_init_v3m.c
+  endif
+  ifeq (${RCAR_LSI},${RCAR_E3})
+    ifeq (${LSI_CUT},10)
+     BL2_SOURCES += drivers/renesas/rcar/qos/E3/qos_init_e3_v10.c
+    else
+#    LSI_CUT 10 or later
+     BL2_SOURCES += drivers/renesas/rcar/qos/E3/qos_init_e3_v10.c
+    endif
+  endif
+  ifeq (${RCAR_LSI},${RCAR_D3})
+    BL2_SOURCES += drivers/renesas/rcar/qos/E3/qos_init_d3.c
+  endif
+endif
+
+BL2_SOURCES += drivers/renesas/rcar/qos/qos_init.c
diff --git a/drivers/renesas/rcar/qos/qos_common.h b/drivers/renesas/rcar/qos/qos_common.h
new file mode 100644
index 0000000..2c130ae
--- /dev/null
+++ b/drivers/renesas/rcar/qos/qos_common.h
@@ -0,0 +1,142 @@
+/*
+ * Copyright (c) 2017-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_COMMON_H
+#define QOS_COMMON_H
+
+#define RCAR_REF_DEFAULT		0U
+
+/* define used for get_refperiod. */
+/* REFPERIOD_CYCLE need smaller than QOSWT_WTSET0_CYCLEs */
+/* refere to plat/renesas/rcar/ddr/ddr_a/ddr_init_e3.h for E3. */
+#if (RCAR_REF_INT == RCAR_REF_DEFAULT)	/* REF default */
+#define REFPERIOD_CYCLE		/* unit:ns */	\
+	((126 * BASE_SUB_SLOT_NUM * 1000U) / 400)
+#else					/* REF option */
+#define REFPERIOD_CYCLE		/* unit:ns */	\
+	((252 * BASE_SUB_SLOT_NUM * 1000U) / 400)
+#endif
+
+#if (RCAR_LSI == RCAR_E3)
+/* define used for E3 */
+#if (RCAR_REF_INT == RCAR_REF_DEFAULT)	/* REF 3.9usec */
+#define SUB_SLOT_CYCLE_E3		0xAFU	/* 175 */
+#else /* REF 7.8usec */
+#define SUB_SLOT_CYCLE_E3		0x15EU	/* 350 */
+#endif /* (RCAR_REF_INT == RCAR_REF_DEFAULT) */
+
+#define OPERATING_FREQ_E3		266U	/* MHz */
+#define SL_INIT_SSLOTCLK_E3		(SUB_SLOT_CYCLE_E3 - 1U)
+#endif
+
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RCAR_M3N)
+/* define used for M3N */
+#if (RCAR_REF_INT == RCAR_REF_DEFAULT)	/* REF 1.95usec */
+#define SUB_SLOT_CYCLE_M3N		0x7EU	/* 126 */
+#else /* REF 3.9usec */
+#define SUB_SLOT_CYCLE_M3N		0xFCU	/* 252 */
+#endif /* (RCAR_REF_INT == RCAR_REF_DEFAULT) */
+
+#define SL_INIT_SSLOTCLK_M3N		(SUB_SLOT_CYCLE_M3N - 1U)
+#define QOSWT_WTSET0_CYCLE_M3N		/* unit:ns */	\
+	((SUB_SLOT_CYCLE_M3N * BASE_SUB_SLOT_NUM * 1000U) / OPERATING_FREQ)
+#endif
+
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RCAR_H3)
+/* define used for H3 */
+#if (RCAR_REF_INT == RCAR_REF_DEFAULT)	/* REF 1.95usec */
+#define SUB_SLOT_CYCLE_H3_20		0x7EU	/* 126 */
+#else /* REF 3.9usec */
+#define SUB_SLOT_CYCLE_H3_20		0xFCU	/* 252 */
+#endif /* (RCAR_REF_INT == RCAR_REF_DEFAULT) */
+
+#define SL_INIT_SSLOTCLK_H3_20		(SUB_SLOT_CYCLE_H3_20 - 1U)
+#define QOSWT_WTSET0_CYCLE_H3_20	/* unit:ns */	\
+	((SUB_SLOT_CYCLE_H3_20 * BASE_SUB_SLOT_NUM * 1000U) / OPERATING_FREQ)
+
+/* define used for H3 Cut 30 */
+#define SUB_SLOT_CYCLE_H3_30		(SUB_SLOT_CYCLE_H3_20)	/* same as H3 Cut 20 */
+#define SL_INIT_SSLOTCLK_H3_30		(SUB_SLOT_CYCLE_H3_30 - 1U)
+#define QOSWT_WTSET0_CYCLE_H3_30	/* unit:ns */	\
+	((SUB_SLOT_CYCLE_H3_30 * BASE_SUB_SLOT_NUM * 1000U) / OPERATING_FREQ)
+
+#endif
+
+#if (RCAR_LSI == RCAR_H3N)
+/* define used for H3N */
+#if (RCAR_REF_INT == RCAR_REF_DEFAULT)	/* REF 1.95usec */
+#define SUB_SLOT_CYCLE_H3N		0x7EU	/* 126 */
+#else /* REF 3.9usec */
+#define SUB_SLOT_CYCLE_H3N		0xFCU	/* 252 */
+#endif /* (RCAR_REF_INT == RCAR_REF_DEFAULT) */
+
+#define SL_INIT_SSLOTCLK_H3N		(SUB_SLOT_CYCLE_H3N - 1U)
+#define QOSWT_WTSET0_CYCLE_H3N		/* unit:ns */	\
+	((SUB_SLOT_CYCLE_H3N * BASE_SUB_SLOT_NUM * 1000U) / OPERATING_FREQ)
+
+#endif
+
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RCAR_M3)
+/* define used for M3 */
+#if (RCAR_REF_INT == RCAR_REF_DEFAULT)	/* REF 1.95usec */
+#define SUB_SLOT_CYCLE_M3_11		0x7EU	/* 126 */
+#define SUB_SLOT_CYCLE_M3_30		0x7EU	/* 126 */
+#else /* REF 3.9usec */
+#define SUB_SLOT_CYCLE_M3_11		0xFCU	/* 252 */
+#define SUB_SLOT_CYCLE_M3_30		0xFCU	/* 252 */
+#endif /* (RCAR_REF_INT == RCAR_REF_DEFAULT) */
+
+#define SL_INIT_SSLOTCLK_M3_11		(SUB_SLOT_CYCLE_M3_11 - 1U)
+#define SL_INIT_SSLOTCLK_M3_30		(SUB_SLOT_CYCLE_M3_30 - 1U)
+#define QOSWT_WTSET0_CYCLE_M3_11	/* unit:ns */	\
+	((SUB_SLOT_CYCLE_M3_11 * BASE_SUB_SLOT_NUM * 1000U) / OPERATING_FREQ)
+#define QOSWT_WTSET0_CYCLE_M3_30	/* unit:ns */	\
+	((SUB_SLOT_CYCLE_M3_30 * BASE_SUB_SLOT_NUM * 1000U) / OPERATING_FREQ)
+#endif
+
+#define OPERATING_FREQ			400U	/* MHz */
+#define BASE_SUB_SLOT_NUM		0x6U
+#define SUB_SLOT_CYCLE			0x7EU	/* 126 */
+
+#define QOSWT_WTSET0_CYCLE		/* unit:ns */	\
+	((SUB_SLOT_CYCLE * BASE_SUB_SLOT_NUM * 1000U) / OPERATING_FREQ)
+
+#define SL_INIT_REFFSSLOT		(0x3U << 24U)
+#define SL_INIT_SLOTSSLOT		((BASE_SUB_SLOT_NUM - 1U) << 16U)
+#define SL_INIT_SSLOTCLK		(SUB_SLOT_CYCLE - 1U)
+
+static inline void io_write_32(uintptr_t addr, uint32_t value)
+{
+	*(volatile uint32_t *)addr = value;
+}
+
+static inline uint32_t io_read_32(uintptr_t addr)
+{
+	return *(volatile uint32_t *)addr;
+}
+
+static inline void io_write_64(uintptr_t addr, uint64_t value)
+{
+	*(volatile uint64_t *)addr = value;
+}
+
+typedef struct {
+	uintptr_t addr;
+	uint64_t value;
+} mstat_slot_t;
+
+struct rcar_gen3_dbsc_qos_settings {
+	uint32_t	reg;
+	uint32_t	val;
+};
+
+extern uint32_t qos_init_ddr_ch;
+extern uint8_t qos_init_ddr_phyvalid;
+
+void rcar_qos_dbsc_setting(struct rcar_gen3_dbsc_qos_settings *qos,
+			   unsigned int qos_size, bool dbsc_wren);
+
+#endif /* QOS_COMMON_H */
diff --git a/drivers/renesas/rcar/qos/qos_init.c b/drivers/renesas/rcar/qos/qos_init.c
new file mode 100644
index 0000000..d0f1730
--- /dev/null
+++ b/drivers/renesas/rcar/qos/qos_init.c
@@ -0,0 +1,394 @@
+/*
+ * Copyright (c) 2015-2019, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "qos_init.h"
+#include "qos_common.h"
+#include "qos_reg.h"
+#include "rcar_def.h"
+#if RCAR_LSI == RCAR_AUTO
+#include "H3/qos_init_h3_v10.h"
+#include "H3/qos_init_h3_v11.h"
+#include "H3/qos_init_h3_v20.h"
+#include "H3/qos_init_h3_v30.h"
+#include "M3/qos_init_m3_v10.h"
+#include "M3/qos_init_m3_v11.h"
+#include "M3/qos_init_m3_v30.h"
+#include "M3N/qos_init_m3n_v10.h"
+#include "V3M/qos_init_v3m.h"
+#endif
+#if RCAR_LSI == RCAR_H3		/* H3 */
+#include "H3/qos_init_h3_v10.h"
+#include "H3/qos_init_h3_v11.h"
+#include "H3/qos_init_h3_v20.h"
+#include "H3/qos_init_h3_v30.h"
+#endif
+#if RCAR_LSI == RCAR_H3N	/* H3 */
+#include "H3/qos_init_h3n_v30.h"
+#endif
+#if RCAR_LSI == RCAR_M3		/* M3 */
+#include "M3/qos_init_m3_v10.h"
+#include "M3/qos_init_m3_v11.h"
+#include "M3/qos_init_m3_v30.h"
+#endif
+#if RCAR_LSI == RCAR_M3N	/* M3N */
+#include "M3N/qos_init_m3n_v10.h"
+#endif
+#if RCAR_LSI == RCAR_V3M	/* V3M */
+#include "V3M/qos_init_v3m.h"
+#endif
+#if RCAR_LSI == RCAR_E3		/* E3 */
+#include "E3/qos_init_e3_v10.h"
+#endif
+#if RCAR_LSI == RCAR_D3		/* D3 */
+#include "D3/qos_init_d3.h"
+#endif
+
+#if (RCAR_LSI != RCAR_E3) && (RCAR_LSI != RCAR_D3) && (RCAR_LSI != RCAR_V3M)
+
+#define DRAM_CH_CNT			0x04
+uint32_t qos_init_ddr_ch;
+uint8_t qos_init_ddr_phyvalid;
+#endif
+
+#define PRR_PRODUCT_ERR(reg)				\
+	do {						\
+		ERROR("LSI Product ID(PRR=0x%x) QoS "	\
+		"initialize not supported.\n", reg);	\
+		panic();				\
+	} while (0)
+
+#define PRR_CUT_ERR(reg)				\
+	do {						\
+		ERROR("LSI Cut ID(PRR=0x%x) QoS "	\
+		"initialize not supported.\n", reg);	\
+		panic();				\
+	} while (0)
+
+void rcar_qos_init(void)
+{
+	uint32_t reg;
+#if (RCAR_LSI != RCAR_E3) && (RCAR_LSI != RCAR_D3) && (RCAR_LSI != RCAR_V3M)
+	uint32_t i;
+
+	qos_init_ddr_ch = 0;
+	qos_init_ddr_phyvalid = get_boardcnf_phyvalid();
+	for (i = 0; i < DRAM_CH_CNT; i++) {
+		if ((qos_init_ddr_phyvalid & (1 << i))) {
+			qos_init_ddr_ch++;
+		}
+	}
+#endif
+
+	reg = mmio_read_32(PRR);
+#if (RCAR_LSI == RCAR_AUTO) || RCAR_LSI_CUT_COMPAT
+	switch (reg & PRR_PRODUCT_MASK) {
+	case PRR_PRODUCT_H3:
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RCAR_H3)
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:
+			qos_init_h3_v10();
+			break;
+		case PRR_PRODUCT_11:
+			qos_init_h3_v11();
+			break;
+		case PRR_PRODUCT_20:
+			qos_init_h3_v20();
+			break;
+		case PRR_PRODUCT_30:
+		default:
+			qos_init_h3_v30();
+			break;
+		}
+#elif (RCAR_LSI == RCAR_H3N)
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_30:
+		default:
+			qos_init_h3n_v30();
+			break;
+		}
+#else
+		PRR_PRODUCT_ERR(reg);
+#endif
+		break;
+	case PRR_PRODUCT_M3:
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RCAR_M3)
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:
+			qos_init_m3_v10();
+			break;
+		case PRR_PRODUCT_21: /* M3 Cut 13 */
+			qos_init_m3_v11();
+			break;
+		case PRR_PRODUCT_30: /* M3 Cut 30 */
+		default:
+			qos_init_m3_v30();
+			break;
+		}
+#else
+		PRR_PRODUCT_ERR(reg);
+#endif
+		break;
+	case PRR_PRODUCT_M3N:
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RCAR_M3N)
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:
+		default:
+			qos_init_m3n_v10();
+			break;
+		}
+#else
+		PRR_PRODUCT_ERR(reg);
+#endif
+		break;
+	case PRR_PRODUCT_V3M:
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RCAR_V3M)
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:
+		case PRR_PRODUCT_20:
+		default:
+			qos_init_v3m();
+			break;
+		}
+#else
+		PRR_PRODUCT_ERR(reg);
+#endif
+		break;
+	case PRR_PRODUCT_E3:
+#if (RCAR_LSI == RCAR_E3)
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:
+		default:
+			qos_init_e3_v10();
+			break;
+		}
+#else
+		PRR_PRODUCT_ERR(reg);
+#endif
+		break;
+	case PRR_PRODUCT_D3:
+#if (RCAR_LSI == RCAR_D3)
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:
+		default:
+			qos_init_d3();
+			break;
+		}
+#else
+		PRR_PRODUCT_ERR(reg);
+#endif
+		break;
+	default:
+		PRR_PRODUCT_ERR(reg);
+		break;
+	}
+#else
+#if RCAR_LSI == RCAR_H3		/* H3 */
+#if RCAR_LSI_CUT == RCAR_CUT_10
+	/* H3 Cut 10 */
+	if ((PRR_PRODUCT_H3 | PRR_PRODUCT_10)
+	    != (reg & (PRR_PRODUCT_MASK | PRR_CUT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_h3_v10();
+#elif RCAR_LSI_CUT == RCAR_CUT_11
+	/* H3 Cut 11 */
+	if ((PRR_PRODUCT_H3 | PRR_PRODUCT_11)
+	    != (reg & (PRR_PRODUCT_MASK | PRR_CUT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_h3_v11();
+#elif RCAR_LSI_CUT == RCAR_CUT_20
+	/* H3 Cut 20 */
+	if ((PRR_PRODUCT_H3 | PRR_PRODUCT_20)
+	    != (reg & (PRR_PRODUCT_MASK | PRR_CUT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_h3_v20();
+#else
+	/* H3 Cut 30 or later */
+	if ((PRR_PRODUCT_H3)
+	    != (reg & (PRR_PRODUCT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_h3_v30();
+#endif
+#elif RCAR_LSI == RCAR_H3N	/* H3 */
+	/* H3N Cut 30 or later */
+	if ((PRR_PRODUCT_H3)
+	    != (reg & (PRR_PRODUCT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_h3n_v30();
+#elif RCAR_LSI == RCAR_M3	/* M3 */
+#if RCAR_LSI_CUT == RCAR_CUT_10
+	/* M3 Cut 10 */
+	if ((PRR_PRODUCT_M3 | PRR_PRODUCT_10)
+	    != (reg & (PRR_PRODUCT_MASK | PRR_CUT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_m3_v10();
+#elif RCAR_LSI_CUT == RCAR_CUT_11
+	/* M3 Cut 11 */
+	if ((PRR_PRODUCT_M3 | PRR_PRODUCT_20)
+	    != (reg & (PRR_PRODUCT_MASK | PRR_CUT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_m3_v11();
+#elif RCAR_LSI_CUT == RCAR_CUT_13
+	/* M3 Cut 13 */
+	if ((PRR_PRODUCT_M3 | PRR_PRODUCT_21)
+	    != (reg & (PRR_PRODUCT_MASK | PRR_CUT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_m3_v11();
+#else
+	/* M3 Cut 30 or later */
+	if ((PRR_PRODUCT_M3)
+	    != (reg & (PRR_PRODUCT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_m3_v30();
+#endif
+#elif RCAR_LSI == RCAR_M3N	/* M3N */
+	/* M3N Cut 10 or later */
+	if ((PRR_PRODUCT_M3N)
+	    != (reg & (PRR_PRODUCT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_m3n_v10();
+#elif RCAR_LSI == RCAR_V3M	/* V3M */
+	/* V3M Cut 10 or later */
+	if ((PRR_PRODUCT_V3M)
+			!= (reg & (PRR_PRODUCT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_v3m();
+#elif RCAR_LSI == RCAR_D3	/* D3 */
+	/* D3 Cut 10 or later */
+	if ((PRR_PRODUCT_D3)
+	    != (reg & (PRR_PRODUCT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_d3();
+#elif RCAR_LSI == RCAR_E3	/* E3 */
+	/* E3 Cut 10 or later */
+	if ((PRR_PRODUCT_E3)
+	    != (reg & (PRR_PRODUCT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_e3_v10();
+#else
+#error "Don't have QoS initialize routine(Unknown chip)."
+#endif
+#endif
+}
+
+#if (RCAR_LSI != RCAR_E3) && (RCAR_LSI != RCAR_D3) && (RCAR_LSI != RCAR_V3M)
+uint32_t get_refperiod(void)
+{
+	uint32_t refperiod = QOSWT_WTSET0_CYCLE;
+
+#if (RCAR_LSI == RCAR_AUTO) || RCAR_LSI_CUT_COMPAT
+	uint32_t reg;
+
+	reg = mmio_read_32(PRR);
+	switch (reg & PRR_PRODUCT_MASK) {
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RCAR_H3)
+	case PRR_PRODUCT_H3:
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:
+		case PRR_PRODUCT_11:
+			break;
+		case PRR_PRODUCT_20:
+		case PRR_PRODUCT_30:
+		default:
+			refperiod = REFPERIOD_CYCLE;
+			break;
+		}
+		break;
+#elif (RCAR_LSI == RCAR_H3N)
+	case PRR_PRODUCT_H3:
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_30:
+		default:
+			refperiod = REFPERIOD_CYCLE;
+			break;
+		}
+		break;
+#endif
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RCAR_M3)
+	case PRR_PRODUCT_M3:
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:
+			break;
+		case PRR_PRODUCT_20: /* M3 Cut 11 */
+		case PRR_PRODUCT_21: /* M3 Cut 13 */
+		case PRR_PRODUCT_30: /* M3 Cut 30 */
+		default:
+			refperiod = REFPERIOD_CYCLE;
+			break;
+		}
+		break;
+#endif
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RCAR_M3N)
+	case PRR_PRODUCT_M3N:
+		refperiod = REFPERIOD_CYCLE;
+		break;
+#endif
+	default:
+		break;
+	}
+#elif RCAR_LSI == RCAR_H3
+#if RCAR_LSI_CUT == RCAR_CUT_10
+	/* H3 Cut 10 */
+#elif RCAR_LSI_CUT == RCAR_CUT_11
+	/* H3 Cut 11 */
+#else
+	/* H3 Cut 20 */
+	/* H3 Cut 30 or later */
+	refperiod = REFPERIOD_CYCLE;
+#endif
+#elif RCAR_LSI == RCAR_H3N
+	/* H3N Cut 30 or later */
+	refperiod = REFPERIOD_CYCLE;
+#elif RCAR_LSI == RCAR_M3
+#if RCAR_LSI_CUT == RCAR_CUT_10
+	/* M3 Cut 10 */
+#else
+	/* M3 Cut 11 */
+	/* M3 Cut 13 */
+	/* M3 Cut 30 or later */
+	refperiod = REFPERIOD_CYCLE;
+#endif
+#elif RCAR_LSI == RCAR_M3N	/* for M3N */
+	refperiod = REFPERIOD_CYCLE;
+#endif
+
+	return refperiod;
+}
+#endif
+
+void rcar_qos_dbsc_setting(struct rcar_gen3_dbsc_qos_settings *qos,
+			   unsigned int qos_size, bool dbsc_wren)
+{
+	int i;
+
+	/* Register write enable */
+	if (dbsc_wren)
+		io_write_32(DBSC_DBSYSCNT0, 0x00001234U);
+
+	for (i = 0; i < qos_size; i++)
+		io_write_32(qos[i].reg, qos[i].val);
+
+	/* Register write protect */
+	if (dbsc_wren)
+		io_write_32(DBSC_DBSYSCNT0, 0x00000000U);
+}
diff --git a/drivers/renesas/rcar/qos/qos_init.h b/drivers/renesas/rcar/qos/qos_init.h
new file mode 100644
index 0000000..1b64992
--- /dev/null
+++ b/drivers/renesas/rcar/qos/qos_init.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2015-2018, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_H
+#define QOS_INIT_H
+
+extern void rcar_qos_init(void);
+extern uint8_t get_boardcnf_phyvalid(void);
+
+#endif /* QOS_INIT_H */
diff --git a/drivers/renesas/rzg/board/board.c b/drivers/renesas/rzg/board/board.c
new file mode 100644
index 0000000..7636372
--- /dev/null
+++ b/drivers/renesas/rzg/board/board.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2020-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+
+#include "board.h"
+#include "rcar_def.h"
+
+#ifndef BOARD_DEFAULT
+#if (RCAR_LSI == RZ_G2H)
+#define BOARD_DEFAULT		(BOARD_HIHOPE_RZ_G2H << BOARD_CODE_SHIFT)
+#elif (RCAR_LSI == RZ_G2N)
+#define BOARD_DEFAULT		(BOARD_HIHOPE_RZ_G2N << BOARD_CODE_SHIFT)
+#elif (RCAR_LSI == RZ_G2E)
+#define BOARD_DEFAULT		(BOARD_EK874_RZ_G2E << BOARD_CODE_SHIFT)
+#else
+#define BOARD_DEFAULT		(BOARD_HIHOPE_RZ_G2M << BOARD_CODE_SHIFT)
+#endif /* RCAR_LSI == RZ_G2H */
+#endif /* BOARD_DEFAULT */
+
+#define BOARD_CODE_MASK		(0xF8U)
+#define BOARD_REV_MASK		(0x07U)
+#define BOARD_CODE_SHIFT	(0x03)
+#define BOARD_ID_UNKNOWN	(0xFFU)
+
+#define GPIO_INDT5	0xE605500C
+#define GP5_19_BIT	(0x01U << 19)
+#define GP5_21_BIT	(0x01U << 21)
+#define GP5_25_BIT	(0x01U << 25)
+
+#define HM_ID	{ 0x10U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU }
+#define HH_ID	HM_ID
+#define HN_ID	{ 0x20U, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU }
+#define EK_ID	HM_ID
+
+const char *g_board_tbl[] = {
+	[BOARD_HIHOPE_RZ_G2M] = "HiHope RZ/G2M",
+	[BOARD_HIHOPE_RZ_G2H] = "HiHope RZ/G2H",
+	[BOARD_HIHOPE_RZ_G2N] = "HiHope RZ/G2N",
+	[BOARD_EK874_RZ_G2E] = "EK874 RZ/G2E",
+	[BOARD_UNKNOWN] = "unknown"
+};
+
+void rzg_get_board_type(uint32_t *type, uint32_t *rev)
+{
+	static uint8_t board_id = BOARD_ID_UNKNOWN;
+	const uint8_t board_tbl[][8] = {
+		[BOARD_HIHOPE_RZ_G2M] = HM_ID,
+		[BOARD_HIHOPE_RZ_G2H] = HH_ID,
+		[BOARD_HIHOPE_RZ_G2N] = HN_ID,
+		[BOARD_EK874_RZ_G2E] = EK_ID,
+	};
+	uint32_t reg;
+#if (RCAR_LSI != RZ_G2E)
+	uint32_t boardInfo;
+#endif /* RCAR_LSI == RZ_G2E */
+
+	if (board_id == BOARD_ID_UNKNOWN) {
+		board_id = BOARD_DEFAULT;
+	}
+
+	*type = ((uint32_t) board_id & BOARD_CODE_MASK) >> BOARD_CODE_SHIFT;
+
+	if (*type >= ARRAY_SIZE(board_tbl)) {
+		/* no revision information, set Rev0.0. */
+		*rev = 0;
+		return;
+	}
+
+	reg = mmio_read_32(RCAR_PRR);
+#if (RCAR_LSI == RZ_G2E)
+	if (reg & RCAR_MINOR_MASK) {
+		*rev = 0x30U;
+	} else {
+		*rev = board_tbl[*type][(uint8_t)(board_id & BOARD_REV_MASK)];
+	}
+#else
+	if ((reg & PRR_CUT_MASK) == RCAR_M3_CUT_VER11) {
+		*rev = board_tbl[*type][(uint8_t)(board_id & BOARD_REV_MASK)];
+	} else {
+		reg = mmio_read_32(GPIO_INDT5);
+		if (reg & GP5_25_BIT) {
+			*rev = board_tbl[*type][(uint8_t)(board_id & BOARD_REV_MASK)];
+		} else {
+			boardInfo = reg & (GP5_19_BIT | GP5_21_BIT);
+			*rev = (((boardInfo & GP5_19_BIT) >> 14) |
+				((boardInfo & GP5_21_BIT) >> 17)) + 0x30U;
+		}
+	}
+#endif /* RCAR_LSI == RZ_G2E */
+}
diff --git a/drivers/renesas/rzg/board/board.h b/drivers/renesas/rzg/board/board.h
new file mode 100644
index 0000000..1a76849
--- /dev/null
+++ b/drivers/renesas/rzg/board/board.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) 2020-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RZ_G2_BOARD_H
+#define RZ_G2_BOARD_H
+
+enum rzg2_board_id {
+	BOARD_HIHOPE_RZ_G2M = 0,
+	BOARD_HIHOPE_RZ_G2H,
+	BOARD_HIHOPE_RZ_G2N,
+	BOARD_EK874_RZ_G2E,
+	BOARD_UNKNOWN
+};
+
+#define BOARD_REV_UNKNOWN	(0xFFU)
+
+extern const char *g_board_tbl[];
+
+/************************************************************************
+ * Revisions are expressed in 8 bits.
+ *  The upper 4 bits are major version.
+ *  The lower 4 bits are minor version.
+ ************************************************************************/
+#define GET_BOARD_MAJOR(a)	((uint32_t)(a) >> 0x4)
+#define GET_BOARD_MINOR(a)	((uint32_t)(a) &  0xF)
+#define GET_BOARD_NAME(a)	(g_board_tbl[(a)])
+
+void rzg_get_board_type(uint32_t *type, uint32_t *rev);
+
+#endif /* RZ_G2_BOARD_H */
diff --git a/drivers/renesas/rzg/pfc/G2E/pfc_init_g2e.c b/drivers/renesas/rzg/pfc/G2E/pfc_init_g2e.c
new file mode 100644
index 0000000..663df50
--- /dev/null
+++ b/drivers/renesas/rzg/pfc/G2E/pfc_init_g2e.c
@@ -0,0 +1,700 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <lib/mmio.h>
+
+#include "pfc_init_g2e.h"
+#include "rcar_def.h"
+
+#include "../pfc_regs.h"
+
+/* PFC */
+#define GPSR0_SDA4		BIT(17)
+#define GPSR0_SCL4		BIT(16)
+#define GPSR0_D15		BIT(15)
+#define GPSR0_D14		BIT(14)
+#define GPSR0_D13		BIT(13)
+#define GPSR0_D12		BIT(12)
+#define GPSR0_D11		BIT(11)
+#define GPSR0_D10		BIT(10)
+#define GPSR0_D9		BIT(9)
+#define GPSR0_D8		BIT(8)
+#define GPSR0_D7		BIT(7)
+#define GPSR0_D6		BIT(6)
+#define GPSR0_D5		BIT(5)
+#define GPSR0_D4		BIT(4)
+#define GPSR0_D3		BIT(3)
+#define GPSR0_D2		BIT(2)
+#define GPSR0_D1		BIT(1)
+#define GPSR0_D0		BIT(0)
+#define GPSR1_WE0		BIT(22)
+#define GPSR1_CS0		BIT(21)
+#define GPSR1_CLKOUT		BIT(20)
+#define GPSR1_A19		BIT(19)
+#define GPSR1_A18		BIT(18)
+#define GPSR1_A17		BIT(17)
+#define GPSR1_A16		BIT(16)
+#define GPSR1_A15		BIT(15)
+#define GPSR1_A14		BIT(14)
+#define GPSR1_A13		BIT(13)
+#define GPSR1_A12		BIT(12)
+#define GPSR1_A11		BIT(11)
+#define GPSR1_A10		BIT(10)
+#define GPSR1_A9		BIT(9)
+#define GPSR1_A8		BIT(8)
+#define GPSR1_A7		BIT(7)
+#define GPSR1_A6		BIT(6)
+#define GPSR1_A5		BIT(5)
+#define GPSR1_A4		BIT(4)
+#define GPSR1_A3		BIT(3)
+#define GPSR1_A2		BIT(2)
+#define GPSR1_A1		BIT(1)
+#define GPSR1_A0		BIT(0)
+#define GPSR2_BIT27_REVERSED	BIT(27)
+#define GPSR2_BIT26_REVERSED	BIT(26)
+#define GPSR2_EX_WAIT0		BIT(25)
+#define GPSR2_RD_WR		BIT(24)
+#define GPSR2_RD		BIT(23)
+#define GPSR2_BS		BIT(22)
+#define GPSR2_AVB_PHY_INT	BIT(21)
+#define GPSR2_AVB_TXCREFCLK	BIT(20)
+#define GPSR2_AVB_RD3		BIT(19)
+#define GPSR2_AVB_RD2		BIT(18)
+#define GPSR2_AVB_RD1		BIT(17)
+#define GPSR2_AVB_RD0		BIT(16)
+#define GPSR2_AVB_RXC		BIT(15)
+#define GPSR2_AVB_RX_CTL	BIT(14)
+#define GPSR2_RPC_RESET		BIT(13)
+#define GPSR2_RPC_RPC_INT	BIT(12)
+#define GPSR2_QSPI1_SSL		BIT(11)
+#define GPSR2_QSPI1_IO3		BIT(10)
+#define GPSR2_QSPI1_IO2		BIT(9)
+#define GPSR2_QSPI1_MISO_IO1	BIT(8)
+#define GPSR2_QSPI1_MOSI_IO0	BIT(7)
+#define GPSR2_QSPI1_SPCLK	BIT(6)
+#define GPSR2_QSPI0_SSL		BIT(5)
+#define GPSR2_QSPI0_IO3		BIT(4)
+#define GPSR2_QSPI0_IO2		BIT(3)
+#define GPSR2_QSPI0_MISO_IO1	BIT(2)
+#define GPSR2_QSPI0_MOSI_IO0	BIT(1)
+#define GPSR2_QSPI0_SPCLK	BIT(0)
+#define GPSR3_SD1_WP		BIT(15)
+#define GPSR3_SD1_CD		BIT(14)
+#define GPSR3_SD0_WP		BIT(13)
+#define GPSR3_SD0_CD		BIT(12)
+#define GPSR3_SD1_DAT3		BIT(11)
+#define GPSR3_SD1_DAT2		BIT(10)
+#define GPSR3_SD1_DAT1		BIT(9)
+#define GPSR3_SD1_DAT0		BIT(8)
+#define GPSR3_SD1_CMD		BIT(7)
+#define GPSR3_SD1_CLK		BIT(6)
+#define GPSR3_SD0_DAT3		BIT(5)
+#define GPSR3_SD0_DAT2		BIT(4)
+#define GPSR3_SD0_DAT1		BIT(3)
+#define GPSR3_SD0_DAT0		BIT(2)
+#define GPSR3_SD0_CMD		BIT(1)
+#define GPSR3_SD0_CLK		BIT(0)
+#define GPSR4_SD3_DS		BIT(10)
+#define GPSR4_SD3_DAT7		BIT(9)
+#define GPSR4_SD3_DAT6		BIT(8)
+#define GPSR4_SD3_DAT5		BIT(7)
+#define GPSR4_SD3_DAT4		BIT(6)
+#define GPSR4_SD3_DAT3		BIT(5)
+#define GPSR4_SD3_DAT2		BIT(4)
+#define GPSR4_SD3_DAT1		BIT(3)
+#define GPSR4_SD3_DAT0		BIT(2)
+#define GPSR4_SD3_CMD		BIT(1)
+#define GPSR4_SD3_CLK		BIT(0)
+#define GPSR5_MLB_DAT		BIT(19)
+#define GPSR5_MLB_SIG		BIT(18)
+#define GPSR5_MLB_CLK		BIT(17)
+#define GPSR5_SSI_SDATA9	BIT(16)
+#define GPSR5_MSIOF0_SS2	BIT(15)
+#define GPSR5_MSIOF0_SS1	BIT(14)
+#define GPSR5_MSIOF0_SYNC	BIT(13)
+#define GPSR5_MSIOF0_TXD	BIT(12)
+#define GPSR5_MSIOF0_RXD	BIT(11)
+#define GPSR5_MSIOF0_SCK	BIT(10)
+#define GPSR5_RX2_A		BIT(9)
+#define GPSR5_TX2_A		BIT(8)
+#define GPSR5_SCK2_A		BIT(7)
+#define GPSR5_TX1		BIT(6)
+#define GPSR5_RX1		BIT(5)
+#define GPSR5_RTS0_A		BIT(4)
+#define GPSR5_CTS0_A		BIT(3)
+#define GPSR5_TX0_A		BIT(2)
+#define GPSR5_RX0_A		BIT(1)
+#define GPSR5_SCK0_A		BIT(0)
+#define GPSR6_USB30_PWEN	BIT(17)
+#define GPSR6_SSI_SDATA6	BIT(16)
+#define GPSR6_SSI_WS6		BIT(15)
+#define GPSR6_SSI_SCK6		BIT(14)
+#define GPSR6_SSI_SDATA5	BIT(13)
+#define GPSR6_SSI_WS5		BIT(12)
+#define GPSR6_SSI_SCK5		BIT(11)
+#define GPSR6_SSI_SDATA4	BIT(10)
+#define GPSR6_USB30_OVC		BIT(9)
+#define GPSR6_AUDIO_CLKA	BIT(8)
+#define GPSR6_SSI_SDATA3	BIT(7)
+#define GPSR6_SSI_WS349		BIT(6)
+#define GPSR6_SSI_SCK349	BIT(5)
+#define GPSR6_SSI_SDATA2	BIT(4)
+#define GPSR6_SSI_SDATA1	BIT(3)
+#define GPSR6_SSI_SDATA0	BIT(2)
+#define GPSR6_SSI_WS01239	BIT(1)
+#define GPSR6_SSI_SCK01239	BIT(0)
+
+#define IPSR_28_FUNC(x)		((uint32_t)(x) << 28U)
+#define IPSR_24_FUNC(x)		((uint32_t)(x) << 24U)
+#define IPSR_20_FUNC(x)		((uint32_t)(x) << 20U)
+#define IPSR_16_FUNC(x)		((uint32_t)(x) << 16U)
+#define IPSR_12_FUNC(x)		((uint32_t)(x) << 12U)
+#define IPSR_8_FUNC(x)		((uint32_t)(x) << 8U)
+#define IPSR_4_FUNC(x)		((uint32_t)(x) << 4U)
+#define IPSR_0_FUNC(x)		((uint32_t)(x) << 0U)
+
+#define POCCTRL0_MASK		(0x0007F000U)
+#define POC_SD3_DS_33V		BIT(29)
+#define POC_SD3_DAT7_33V	BIT(28)
+#define POC_SD3_DAT6_33V	BIT(27)
+#define POC_SD3_DAT5_33V	BIT(26)
+#define POC_SD3_DAT4_33V	BIT(25)
+#define POC_SD3_DAT3_33V	BIT(24)
+#define POC_SD3_DAT2_33V	BIT(23)
+#define POC_SD3_DAT1_33V	BIT(22)
+#define POC_SD3_DAT0_33V	BIT(21)
+#define POC_SD3_CMD_33V		BIT(20)
+#define POC_SD3_CLK_33V		BIT(19)
+#define POC_SD1_DAT3_33V	BIT(11)
+#define POC_SD1_DAT2_33V	BIT(10)
+#define POC_SD1_DAT1_33V	BIT(9)
+#define POC_SD1_DAT0_33V	BIT(8)
+#define POC_SD1_CMD_33V		BIT(7)
+#define POC_SD1_CLK_33V		BIT(6)
+#define POC_SD0_DAT3_33V	BIT(5)
+#define POC_SD0_DAT2_33V	BIT(4)
+#define POC_SD0_DAT1_33V	BIT(3)
+#define POC_SD0_DAT0_33V	BIT(2)
+#define POC_SD0_CMD_33V		BIT(1)
+#define POC_SD0_CLK_33V		BIT(0)
+
+#define POCCTRL2_MASK		(0xFFFFFFFEU)
+#define POC2_VREF_33V		BIT(0)
+
+#define MOD_SEL0_ADGB_A			((uint32_t)0U << 29U)
+#define MOD_SEL0_ADGB_B			((uint32_t)1U << 29U)
+#define MOD_SEL0_ADGB_C			((uint32_t)2U << 29U)
+#define MOD_SEL0_DRIF0_A		((uint32_t)0U << 28U)
+#define MOD_SEL0_DRIF0_B		((uint32_t)1U << 28U)
+#define MOD_SEL0_FM_A			((uint32_t)0U << 26U)
+#define MOD_SEL0_FM_B			((uint32_t)1U << 26U)
+#define MOD_SEL0_FM_C			((uint32_t)2U << 26U)
+#define MOD_SEL0_FSO_A			((uint32_t)0U << 25U)
+#define MOD_SEL0_FSO_B			((uint32_t)1U << 25U)
+#define MOD_SEL0_HSCIF0_A		((uint32_t)0U << 24U)
+#define MOD_SEL0_HSCIF0_B		((uint32_t)1U << 24U)
+#define MOD_SEL0_HSCIF1_A		((uint32_t)0U << 23U)
+#define MOD_SEL0_HSCIF1_B		((uint32_t)1U << 23U)
+#define MOD_SEL0_HSCIF2_A		((uint32_t)0U << 22U)
+#define MOD_SEL0_HSCIF2_B		((uint32_t)1U << 22U)
+#define MOD_SEL0_I2C1_A			((uint32_t)0U << 20U)
+#define MOD_SEL0_I2C1_B			((uint32_t)1U << 20U)
+#define MOD_SEL0_I2C1_C			((uint32_t)2U << 20U)
+#define MOD_SEL0_I2C1_D			((uint32_t)3U << 20U)
+#define MOD_SEL0_I2C2_A			((uint32_t)0U << 17U)
+#define MOD_SEL0_I2C2_B			((uint32_t)1U << 17U)
+#define MOD_SEL0_I2C2_C			((uint32_t)2U << 17U)
+#define MOD_SEL0_I2C2_D			((uint32_t)3U << 17U)
+#define MOD_SEL0_I2C2_E			((uint32_t)4U << 17U)
+#define MOD_SEL0_NDFC_A			((uint32_t)0U << 16U)
+#define MOD_SEL0_NDFC_B			((uint32_t)1U << 16U)
+#define MOD_SEL0_PWM0_A			((uint32_t)0U << 15U)
+#define MOD_SEL0_PWM0_B			((uint32_t)1U << 15U)
+#define MOD_SEL0_PWM1_A			((uint32_t)0U << 14U)
+#define MOD_SEL0_PWM1_B			((uint32_t)1U << 14U)
+#define MOD_SEL0_PWM2_A			((uint32_t)0U << 12U)
+#define MOD_SEL0_PWM2_B			((uint32_t)1U << 12U)
+#define MOD_SEL0_PWM2_C			((uint32_t)2U << 12U)
+#define MOD_SEL0_PWM3_A			((uint32_t)0U << 10U)
+#define MOD_SEL0_PWM3_B			((uint32_t)1U << 10U)
+#define MOD_SEL0_PWM3_C			((uint32_t)2U << 10U)
+#define MOD_SEL0_PWM4_A			((uint32_t)0U << 9U)
+#define MOD_SEL0_PWM4_B			((uint32_t)1U << 9U)
+#define MOD_SEL0_PWM5_A			((uint32_t)0U << 8U)
+#define MOD_SEL0_PWM5_B			((uint32_t)1U << 8U)
+#define MOD_SEL0_PWM6_A			((uint32_t)0U << 7U)
+#define MOD_SEL0_PWM6_B			((uint32_t)1U << 7U)
+#define MOD_SEL0_REMOCON_A		((uint32_t)0U << 5U)
+#define MOD_SEL0_REMOCON_B		((uint32_t)1U << 5U)
+#define MOD_SEL0_REMOCON_C		((uint32_t)2U << 5U)
+#define MOD_SEL0_SCIF_A			((uint32_t)0U << 4U)
+#define MOD_SEL0_SCIF_B			((uint32_t)1U << 4U)
+#define MOD_SEL0_SCIF0_A		((uint32_t)0U << 3U)
+#define MOD_SEL0_SCIF0_B		((uint32_t)1U << 3U)
+#define MOD_SEL0_SCIF2_A		((uint32_t)0U << 2U)
+#define MOD_SEL0_SCIF2_B		((uint32_t)1U << 2U)
+#define MOD_SEL0_SPEED_PULSE_IF_A	((uint32_t)0U << 0U)
+#define MOD_SEL0_SPEED_PULSE_IF_B	((uint32_t)1U << 0U)
+#define MOD_SEL0_SPEED_PULSE_IF_C	((uint32_t)2U << 0U)
+#define MOD_SEL1_SIMCARD_A		((uint32_t)0U << 31U)
+#define MOD_SEL1_SIMCARD_B		((uint32_t)1U << 31U)
+#define MOD_SEL1_SSI2_A			((uint32_t)0U << 30U)
+#define MOD_SEL1_SSI2_B			((uint32_t)1U << 30U)
+#define MOD_SEL1_TIMER_TMU_A		((uint32_t)0U << 29U)
+#define MOD_SEL1_TIMER_TMU_B		((uint32_t)1U << 29U)
+#define MOD_SEL1_USB20_CH0_A		((uint32_t)0U << 28U)
+#define MOD_SEL1_USB20_CH0_B		((uint32_t)1U << 28U)
+#define MOD_SEL1_DRIF2_A		((uint32_t)0U << 26U)
+#define MOD_SEL1_DRIF2_B		((uint32_t)1U << 26U)
+#define MOD_SEL1_DRIF3_A		((uint32_t)0U << 25U)
+#define MOD_SEL1_DRIF3_B		((uint32_t)1U << 25U)
+#define MOD_SEL1_HSCIF3_A		((uint32_t)0U << 22U)
+#define MOD_SEL1_HSCIF3_B		((uint32_t)1U << 22U)
+#define MOD_SEL1_HSCIF3_C		((uint32_t)2U << 22U)
+#define MOD_SEL1_HSCIF3_D		((uint32_t)3U << 22U)
+#define MOD_SEL1_HSCIF3_E		((uint32_t)4U << 22U)
+#define MOD_SEL1_HSCIF4_A		((uint32_t)0U << 19U)
+#define MOD_SEL1_HSCIF4_B		((uint32_t)1U << 19U)
+#define MOD_SEL1_HSCIF4_C		((uint32_t)2U << 19U)
+#define MOD_SEL1_HSCIF4_D		((uint32_t)3U << 19U)
+#define MOD_SEL1_HSCIF4_E		((uint32_t)4U << 19U)
+#define MOD_SEL1_I2C6_A			((uint32_t)0U << 18U)
+#define MOD_SEL1_I2C6_B			((uint32_t)1U << 18U)
+#define MOD_SEL1_I2C7_A			((uint32_t)0U << 17U)
+#define MOD_SEL1_I2C7_B			((uint32_t)1U << 17U)
+#define MOD_SEL1_MSIOF2_A		((uint32_t)0U << 16U)
+#define MOD_SEL1_MSIOF2_B		((uint32_t)1U << 16U)
+#define MOD_SEL1_MSIOF3_A		((uint32_t)0U << 15U)
+#define MOD_SEL1_MSIOF3_B		((uint32_t)1U << 15U)
+#define MOD_SEL1_SCIF3_A		((uint32_t)0U << 13U)
+#define MOD_SEL1_SCIF3_B		((uint32_t)1U << 13U)
+#define MOD_SEL1_SCIF3_C		((uint32_t)2U << 13U)
+#define MOD_SEL1_SCIF4_A		((uint32_t)0U << 11U)
+#define MOD_SEL1_SCIF4_B		((uint32_t)1U << 11U)
+#define MOD_SEL1_SCIF4_C		((uint32_t)2U << 11U)
+#define MOD_SEL1_SCIF5_A		((uint32_t)0U << 9U)
+#define MOD_SEL1_SCIF5_B		((uint32_t)1U << 9U)
+#define MOD_SEL1_SCIF5_C		((uint32_t)2U << 9U)
+#define MOD_SEL1_VIN4_A			((uint32_t)0U << 8U)
+#define MOD_SEL1_VIN4_B			((uint32_t)1U << 8U)
+#define MOD_SEL1_VIN5_A			((uint32_t)0U << 7U)
+#define MOD_SEL1_VIN5_B			((uint32_t)1U << 7U)
+#define MOD_SEL1_ADGC_A			((uint32_t)0U << 5U)
+#define MOD_SEL1_ADGC_B			((uint32_t)1U << 5U)
+#define MOD_SEL1_ADGC_C			((uint32_t)2U << 5U)
+#define MOD_SEL1_SSI9_A			((uint32_t)0U << 4U)
+#define MOD_SEL1_SSI9_B			((uint32_t)1U << 4U)
+
+static void pfc_reg_write(uint32_t addr, uint32_t data)
+{
+	mmio_write_32(PFC_PMMR, ~data);
+	mmio_write_32((uintptr_t)addr, data);
+}
+
+void pfc_init_g2e(void)
+{
+	uint32_t reg;
+
+	/* initialize module select */
+	pfc_reg_write(PFC_MOD_SEL0,
+		      MOD_SEL0_ADGB_A |
+		      MOD_SEL0_DRIF0_A |
+		      MOD_SEL0_FM_A |
+		      MOD_SEL0_FSO_A |
+		      MOD_SEL0_HSCIF0_A |
+		      MOD_SEL0_HSCIF1_A |
+		      MOD_SEL0_HSCIF2_A |
+		      MOD_SEL0_I2C1_A |
+		      MOD_SEL0_I2C2_A |
+		      MOD_SEL0_NDFC_A |
+		      MOD_SEL0_PWM0_A |
+		      MOD_SEL0_PWM1_A |
+		      MOD_SEL0_PWM2_A |
+		      MOD_SEL0_PWM3_A |
+		      MOD_SEL0_PWM4_A |
+		      MOD_SEL0_PWM5_A |
+		      MOD_SEL0_PWM6_A |
+		      MOD_SEL0_REMOCON_A |
+		      MOD_SEL0_SCIF_A |
+		      MOD_SEL0_SCIF0_A |
+		      MOD_SEL0_SCIF2_A |
+		      MOD_SEL0_SPEED_PULSE_IF_A);
+
+	pfc_reg_write(PFC_MOD_SEL1,
+		      MOD_SEL1_SIMCARD_A |
+		      MOD_SEL1_SSI2_A |
+		      MOD_SEL1_TIMER_TMU_A |
+		      MOD_SEL1_USB20_CH0_B |
+		      MOD_SEL1_DRIF2_A |
+		      MOD_SEL1_DRIF3_A |
+		      MOD_SEL1_HSCIF3_C |
+		      MOD_SEL1_HSCIF4_B |
+		      MOD_SEL1_I2C6_A |
+		      MOD_SEL1_I2C7_A |
+		      MOD_SEL1_MSIOF2_A |
+		      MOD_SEL1_MSIOF3_A |
+		      MOD_SEL1_SCIF3_A |
+		      MOD_SEL1_SCIF4_A |
+		      MOD_SEL1_SCIF5_A |
+		      MOD_SEL1_VIN4_A |
+		      MOD_SEL1_VIN5_A |
+		      MOD_SEL1_ADGC_A |
+		      MOD_SEL1_SSI9_A);
+
+	/* initialize peripheral function select */
+	pfc_reg_write(PFC_IPSR0,
+		      IPSR_28_FUNC(2) |	/* HRX4_B */
+		      IPSR_24_FUNC(2) |	/* HTX4_B */
+		      IPSR_20_FUNC(0) |	/* QSPI1_SPCLK */
+		      IPSR_16_FUNC(0) |	/* QSPI0_IO3 */
+		      IPSR_12_FUNC(0) |	/* QSPI0_IO2 */
+		      IPSR_8_FUNC(0) |	/* QSPI0_MISO/IO1 */
+		      IPSR_4_FUNC(0) |	/* QSPI0_MOSI/IO0 */
+		      IPSR_0_FUNC(0));	/* QSPI0_SPCLK */
+
+	pfc_reg_write(PFC_IPSR1,
+		      IPSR_28_FUNC(0) |	/* AVB_RD2 */
+		      IPSR_24_FUNC(0) |	/* AVB_RD1 */
+		      IPSR_20_FUNC(0) |	/* AVB_RD0 */
+		      IPSR_16_FUNC(0) |	/* RPC_RESET# */
+		      IPSR_12_FUNC(0) |	/* RPC_INT# */
+		      IPSR_8_FUNC(0) |	/* QSPI1_SSL */
+		      IPSR_4_FUNC(2) |	/* HRX3_C */
+		      IPSR_0_FUNC(2));	/* HTX3_C */
+
+	pfc_reg_write(PFC_IPSR2,
+		      IPSR_28_FUNC(1) |	/* IRQ0 */
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(2) |	/* AVB_LINK */
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |	/* AVB_MDC */
+		      IPSR_4_FUNC(0) |	/* AVB_MDIO */
+		      IPSR_0_FUNC(0));	/* AVB_TXCREFCLK */
+
+	pfc_reg_write(PFC_IPSR3,
+		      IPSR_28_FUNC(5) |	/* DU_HSYNC */
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(5) |	/* DU_DG4 */
+		      IPSR_8_FUNC(5) |	/* DU_DOTCLKOUT0 */
+		      IPSR_4_FUNC(5) |	/* DU_DISP */
+		      IPSR_0_FUNC(1));	/* IRQ1 */
+
+	pfc_reg_write(PFC_IPSR4,
+		      IPSR_28_FUNC(5) |	/* DU_DB5 */
+		      IPSR_24_FUNC(5) |	/* DU_DB4 */
+		      IPSR_20_FUNC(5) |	/* DU_DB3 */
+		      IPSR_16_FUNC(5) |	/* DU_DB2 */
+		      IPSR_12_FUNC(5) |	/* DU_DG6 */
+		      IPSR_8_FUNC(5) |	/* DU_VSYNC */
+		      IPSR_4_FUNC(5) |	/* DU_DG5 */
+		      IPSR_0_FUNC(5));	/* DU_DG7 */
+
+	pfc_reg_write(PFC_IPSR5,
+		      IPSR_28_FUNC(5) |	/* DU_DR3 */
+		      IPSR_24_FUNC(5) |	/* DU_DB7 */
+		      IPSR_20_FUNC(5) |	/* DU_DR2 */
+		      IPSR_16_FUNC(5) |	/* DU_DR1 */
+		      IPSR_12_FUNC(5) |	/* DU_DR0 */
+		      IPSR_8_FUNC(5) |	/* DU_DB1 */
+		      IPSR_4_FUNC(5) |	/* DU_DB0 */
+		      IPSR_0_FUNC(5));	/* DU_DB6 */
+
+	pfc_reg_write(PFC_IPSR6,
+		      IPSR_28_FUNC(5) |	/* DU_DG1 */
+		      IPSR_24_FUNC(5) |	/* DU_DG0 */
+		      IPSR_20_FUNC(5) |	/* DU_DR7 */
+		      IPSR_16_FUNC(1) |	/* CANFD1_RX */
+		      IPSR_12_FUNC(5) |	/* DU_DR6 */
+		      IPSR_8_FUNC(5) |	/* DU_DR5 */
+		      IPSR_4_FUNC(1) |	/* CANFD1_TX */
+		      IPSR_0_FUNC(5));	/* DU_DR4 */
+
+	pfc_reg_write(PFC_IPSR7,
+		      IPSR_28_FUNC(0) |	/* SD0_CLK */
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(5) |	/* DU_DOTCLKIN0 */
+		      IPSR_16_FUNC(5) |	/* DU_DG3 */
+		      IPSR_12_FUNC(1) |	/* CAN_CLK */
+		      IPSR_8_FUNC(1) |	/* CANFD0_RX */
+		      IPSR_4_FUNC(1) |	/* CANFD0_TX */
+		      IPSR_0_FUNC(5));	/* DU_DG2 */
+
+	pfc_reg_write(PFC_IPSR8,
+		      IPSR_28_FUNC(0) |	/* SD1_DAT0 */
+		      IPSR_24_FUNC(0) |	/* SD1_CMD */
+		      IPSR_20_FUNC(0) |	/* SD1_CLK */
+		      IPSR_16_FUNC(0) |	/* SD0_DAT3 */
+		      IPSR_12_FUNC(0) |	/* SD0_DAT2 */
+		      IPSR_8_FUNC(0) |	/* SD0_DAT1 */
+		      IPSR_4_FUNC(0) |	/* SD0_DAT0 */
+		      IPSR_0_FUNC(0));	/* SD0_CMD */
+
+	pfc_reg_write(PFC_IPSR9,
+		      IPSR_28_FUNC(0) |	/* SD3_DAT2 */
+		      IPSR_24_FUNC(0) |	/* SD3_DAT1 */
+		      IPSR_20_FUNC(0) |	/* SD3_DAT0 */
+		      IPSR_16_FUNC(0) |	/* SD3_CMD */
+		      IPSR_12_FUNC(0) |	/* SD3_CLK */
+		      IPSR_8_FUNC(0) |	/* SD1_DAT3 */
+		      IPSR_4_FUNC(0) |	/* SD1_DAT2 */
+		      IPSR_0_FUNC(0));	/* SD1_DAT1 */
+
+	pfc_reg_write(PFC_IPSR10,
+		      IPSR_24_FUNC(0) |	/* SD0_CD */
+		      IPSR_20_FUNC(0) |	/* SD3_DS */
+		      IPSR_16_FUNC(0) |	/* SD3_DAT7 */
+		      IPSR_12_FUNC(0) |	/* SD3_DAT6 */
+		      IPSR_8_FUNC(0) |	/* SD3_DAT5 */
+		      IPSR_4_FUNC(0) |	/* SD3_DAT4 */
+		      IPSR_0_FUNC(0));	/* SD3_DAT3 */
+
+	pfc_reg_write(PFC_IPSR11,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(8) |	/* USB0_ID */
+		      IPSR_20_FUNC(2) |	/* AUDIO_CLKOUT1_A */
+		      IPSR_16_FUNC(0) |	/* CTS0#_A */
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |	/* SD1_WP */
+		      IPSR_0_FUNC(0));	/* SD1_CD */
+
+	pfc_reg_write(PFC_IPSR12,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |	/* RX2_A */
+		      IPSR_8_FUNC(0) |	/* TX2_A */
+		      IPSR_4_FUNC(0) |	/* SCK2_A */
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR13,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(4) |	/* SDA1_B */
+		      IPSR_12_FUNC(4) |	/* SCL1_B */
+		      IPSR_8_FUNC(0) |	/* SSI_SDATA9 */
+		      IPSR_4_FUNC(1) |	/* HTX2_A */
+		      IPSR_0_FUNC(1));	/* HRX2_A */
+
+	pfc_reg_write(PFC_IPSR14,
+		      IPSR_28_FUNC(0) |	/* SSI_SCK5 */
+		      IPSR_24_FUNC(0) |	/* SSI_SDATA4 */
+		      IPSR_20_FUNC(0) |	/* SSI_SDATA3 */
+		      IPSR_16_FUNC(0) |	/* SSI_WS349 */
+		      IPSR_12_FUNC(0) |	/* SSI_SCK349 */
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |	/* SSI_SDATA1 */
+		      IPSR_0_FUNC(0));/* SSI_SDATA0 */
+
+	pfc_reg_write(PFC_IPSR15,
+		      IPSR_28_FUNC(0) |	/* USB30_OVC */
+		      IPSR_24_FUNC(0) |	/* USB30_PWEN */
+		      IPSR_20_FUNC(0) |	/* AUDIO_CLKA */
+		      IPSR_16_FUNC(1) |	/* HRTS2#_A */
+		      IPSR_12_FUNC(1) |	/* HCTS2#_A */
+		      IPSR_8_FUNC(3) |	/* TPU0TO1 */
+		      IPSR_4_FUNC(3) |	/* TPU0TO0 */
+		      IPSR_0_FUNC(0));	/* SSI_WS5 */
+
+	/* initialize GPIO/peripheral function select */
+	pfc_reg_write(PFC_GPSR0,
+		      GPSR0_SCL4 |
+		      GPSR0_D15 |
+		      GPSR0_D14 |
+		      GPSR0_D13 |
+		      GPSR0_D12 |
+		      GPSR0_D11 |
+		      GPSR0_D10 |
+		      GPSR0_D9 |
+		      GPSR0_D8 |
+		      GPSR0_D7 |
+		      GPSR0_D6 |
+		      GPSR0_D5 |
+		      GPSR0_D4 |
+		      GPSR0_D3 |
+		      GPSR0_D2 |
+		      GPSR0_D1 |
+		      GPSR0_D0);
+
+	pfc_reg_write(PFC_GPSR1,
+		      GPSR1_WE0 |
+		      GPSR1_CS0 |
+		      GPSR1_A19 |
+		      GPSR1_A18 |
+		      GPSR1_A17 |
+		      GPSR1_A16 |
+		      GPSR1_A15 |
+		      GPSR1_A14 |
+		      GPSR1_A13 |
+		      GPSR1_A12 |
+		      GPSR1_A11 |
+		      GPSR1_A10 |
+		      GPSR1_A9 |
+		      GPSR1_A8 |
+		      GPSR1_A4 |
+		      GPSR1_A3 |
+		      GPSR1_A2 |
+		      GPSR1_A1 |
+		      GPSR1_A0);
+
+	pfc_reg_write(PFC_GPSR2,
+		      GPSR2_BIT27_REVERSED |
+		      GPSR2_BIT26_REVERSED |
+		      GPSR2_AVB_PHY_INT |
+		      GPSR2_AVB_TXCREFCLK |
+		      GPSR2_AVB_RD3 |
+		      GPSR2_AVB_RD2 |
+		      GPSR2_AVB_RD1 |
+		      GPSR2_AVB_RD0 |
+		      GPSR2_AVB_RXC |
+		      GPSR2_AVB_RX_CTL |
+		      GPSR2_RPC_RESET |
+		      GPSR2_RPC_RPC_INT |
+		      GPSR2_QSPI1_IO3 |
+		      GPSR2_QSPI1_IO2 |
+		      GPSR2_QSPI1_MISO_IO1 |
+		      GPSR2_QSPI1_MOSI_IO0 |
+		      GPSR2_QSPI0_SSL |
+		      GPSR2_QSPI0_IO3 |
+		      GPSR2_QSPI0_IO2 |
+		      GPSR2_QSPI0_MISO_IO1 |
+		      GPSR2_QSPI0_MOSI_IO0 |
+		      GPSR2_QSPI0_SPCLK);
+
+	pfc_reg_write(PFC_GPSR3,
+		      GPSR3_SD0_CD |
+		      GPSR3_SD1_DAT3 |
+		      GPSR3_SD1_DAT2 |
+		      GPSR3_SD1_DAT1 |
+		      GPSR3_SD1_DAT0 |
+		      GPSR3_SD1_CMD |
+		      GPSR3_SD1_CLK |
+		      GPSR3_SD0_DAT3 |
+		      GPSR3_SD0_DAT2 |
+		      GPSR3_SD0_DAT1 |
+		      GPSR3_SD0_DAT0 |
+		      GPSR3_SD0_CMD |
+		      GPSR3_SD0_CLK);
+
+	pfc_reg_write(PFC_GPSR4,
+		      GPSR4_SD3_DAT3 |
+		      GPSR4_SD3_DAT2 |
+		      GPSR4_SD3_DAT1 |
+		      GPSR4_SD3_DAT0 |
+		      GPSR4_SD3_CMD |
+		      GPSR4_SD3_CLK);
+
+	pfc_reg_write(PFC_GPSR5,
+		      GPSR5_MLB_SIG |
+		      GPSR5_MLB_CLK |
+		      GPSR5_SSI_SDATA9 |
+		      GPSR5_MSIOF0_SS2 |
+		      GPSR5_MSIOF0_SS1 |
+		      GPSR5_MSIOF0_SYNC |
+		      GPSR5_MSIOF0_TXD |
+		      GPSR5_MSIOF0_RXD |
+		      GPSR5_MSIOF0_SCK |
+		      GPSR5_RX2_A |
+		      GPSR5_TX2_A |
+		      GPSR5_RTS0_A |
+		      GPSR5_SCK0_A);
+
+	pfc_reg_write(PFC_GPSR6,
+		      GPSR6_USB30_PWEN |
+		      GPSR6_SSI_SDATA6 |
+		      GPSR6_SSI_WS6 |
+		      GPSR6_SSI_SCK6 |
+		      GPSR6_SSI_SDATA5 |
+		      GPSR6_SSI_SCK5 |
+		      GPSR6_SSI_SDATA4 |
+		      GPSR6_USB30_OVC |
+		      GPSR6_AUDIO_CLKA |
+		      GPSR6_SSI_SDATA3 |
+		      GPSR6_SSI_WS349 |
+		      GPSR6_SSI_SCK349 |
+		      GPSR6_SSI_SDATA0 |
+		      GPSR6_SSI_WS01239 |
+		      GPSR6_SSI_SCK01239);
+
+	/* initialize POC control */
+	reg = mmio_read_32(PFC_POCCTRL0);
+	reg = (reg & POCCTRL0_MASK) |
+	      POC_SD1_DAT3_33V |
+	      POC_SD1_DAT2_33V |
+	      POC_SD1_DAT1_33V |
+	      POC_SD1_DAT0_33V |
+	      POC_SD1_CMD_33V |
+	      POC_SD1_CLK_33V |
+	      POC_SD0_DAT3_33V |
+	      POC_SD0_DAT2_33V |
+	      POC_SD0_DAT1_33V |
+	      POC_SD0_DAT0_33V |
+	      POC_SD0_CMD_33V |
+	      POC_SD0_CLK_33V;
+	pfc_reg_write(PFC_POCCTRL0, reg);
+
+	reg = mmio_read_32(PFC_POCCTRL2);
+	reg = ((reg & POCCTRL2_MASK) & ~POC2_VREF_33V);
+	pfc_reg_write(PFC_POCCTRL2, reg);
+
+	/* initialize LSI pin pull-up/down control */
+	pfc_reg_write(PFC_PUD0, 0x00080000U);
+	pfc_reg_write(PFC_PUD1, 0xCE398464U);
+	pfc_reg_write(PFC_PUD2, 0xA4C380F4U);
+	pfc_reg_write(PFC_PUD3, 0x0000079FU);
+	pfc_reg_write(PFC_PUD4, 0xFFF0FFFFU);
+	pfc_reg_write(PFC_PUD5, 0x40000000U);
+
+	/* initialize LSI pin pull-enable register */
+	pfc_reg_write(PFC_PUEN0, 0x00000000U);
+	pfc_reg_write(PFC_PUEN1, 0x00300000U);
+	pfc_reg_write(PFC_PUEN2, 0x00400074U);
+	pfc_reg_write(PFC_PUEN3, 0x00000000U);
+	pfc_reg_write(PFC_PUEN4, 0x07900600U);
+	pfc_reg_write(PFC_PUEN5, 0x00000000U);
+
+	/* initialize positive/negative logic select */
+	mmio_write_32(GPIO_POSNEG0, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG1, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG2, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG3, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG4, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG5, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG6, 0x00000000U);
+
+	/* initialize general IO/interrupt switching */
+	mmio_write_32(GPIO_IOINTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL5, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL6, 0x00000000U);
+
+	/* initialize general output register */
+	mmio_write_32(GPIO_OUTDT0, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT1, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT2, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT3, 0x00006000U);
+	mmio_write_32(GPIO_OUTDT5, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT6, 0x00000000U);
+
+	/* initialize general input/output switching */
+	mmio_write_32(GPIO_INOUTSEL0, 0x00020000U);
+	mmio_write_32(GPIO_INOUTSEL1, 0x00100000U);
+	mmio_write_32(GPIO_INOUTSEL2, 0x03000000U);
+	mmio_write_32(GPIO_INOUTSEL3, 0x0000E000U);
+	mmio_write_32(GPIO_INOUTSEL4, 0x00000440U);
+	mmio_write_32(GPIO_INOUTSEL5, 0x00080000U);
+	mmio_write_32(GPIO_INOUTSEL6, 0x00000010U);
+}
diff --git a/drivers/renesas/rzg/pfc/G2E/pfc_init_g2e.h b/drivers/renesas/rzg/pfc/G2E/pfc_init_g2e.h
new file mode 100644
index 0000000..677591a
--- /dev/null
+++ b/drivers/renesas/rzg/pfc/G2E/pfc_init_g2e.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PFC_INIT_G2E_H
+#define PFC_INIT_G2E_H
+
+void pfc_init_g2e(void);
+
+#endif /* PFC_INIT_G2E_H */
diff --git a/drivers/renesas/rzg/pfc/G2H/pfc_init_g2h.c b/drivers/renesas/rzg/pfc/G2H/pfc_init_g2h.c
new file mode 100644
index 0000000..90a1c99
--- /dev/null
+++ b/drivers/renesas/rzg/pfc/G2H/pfc_init_g2h.c
@@ -0,0 +1,1310 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <lib/mmio.h>
+
+#include "pfc_init_g2h.h"
+#include "rcar_def.h"
+#include "../pfc_regs.h"
+
+#define GPSR0_D15			BIT(15)
+#define GPSR0_D14			BIT(14)
+#define GPSR0_D13			BIT(13)
+#define GPSR0_D12			BIT(12)
+#define GPSR0_D11			BIT(11)
+#define GPSR0_D10			BIT(10)
+#define GPSR0_D9			BIT(9)
+#define GPSR0_D8			BIT(8)
+#define GPSR0_D7			BIT(7)
+#define GPSR0_D6			BIT(6)
+#define GPSR0_D5			BIT(5)
+#define GPSR0_D4			BIT(4)
+#define GPSR0_D3			BIT(3)
+#define GPSR0_D2			BIT(2)
+#define GPSR0_D1			BIT(1)
+#define GPSR0_D0			BIT(0)
+#define GPSR1_CLKOUT			BIT(28)
+#define GPSR1_EX_WAIT0_A		BIT(27)
+#define GPSR1_WE1			BIT(26)
+#define GPSR1_WE0			BIT(25)
+#define GPSR1_RD_WR			BIT(24)
+#define GPSR1_RD			BIT(23)
+#define GPSR1_BS			BIT(22)
+#define GPSR1_CS1_A26			BIT(21)
+#define GPSR1_CS0			BIT(20)
+#define GPSR1_A19			BIT(19)
+#define GPSR1_A18			BIT(18)
+#define GPSR1_A17			BIT(17)
+#define GPSR1_A16			BIT(16)
+#define GPSR1_A15			BIT(15)
+#define GPSR1_A14			BIT(14)
+#define GPSR1_A13			BIT(13)
+#define GPSR1_A12			BIT(12)
+#define GPSR1_A11			BIT(11)
+#define GPSR1_A10			BIT(10)
+#define GPSR1_A9			BIT(9)
+#define GPSR1_A8			BIT(8)
+#define GPSR1_A7			BIT(7)
+#define GPSR1_A6			BIT(6)
+#define GPSR1_A5			BIT(5)
+#define GPSR1_A4			BIT(4)
+#define GPSR1_A3			BIT(3)
+#define GPSR1_A2			BIT(2)
+#define GPSR1_A1			BIT(1)
+#define GPSR1_A0			BIT(0)
+#define GPSR2_AVB_AVTP_CAPTURE_A	BIT(14)
+#define GPSR2_AVB_AVTP_MATCH_A		BIT(13)
+#define GPSR2_AVB_LINK			BIT(12)
+#define GPSR2_AVB_PHY_INT		BIT(11)
+#define GPSR2_AVB_MAGIC			BIT(10)
+#define GPSR2_AVB_MDC			BIT(9)
+#define GPSR2_PWM2_A			BIT(8)
+#define GPSR2_PWM1_A			BIT(7)
+#define GPSR2_PWM0			BIT(6)
+#define GPSR2_IRQ5			BIT(5)
+#define GPSR2_IRQ4			BIT(4)
+#define GPSR2_IRQ3			BIT(3)
+#define GPSR2_IRQ2			BIT(2)
+#define GPSR2_IRQ1			BIT(1)
+#define GPSR2_IRQ0			BIT(0)
+#define GPSR3_SD1_WP			BIT(15)
+#define GPSR3_SD1_CD			BIT(14)
+#define GPSR3_SD0_WP			BIT(13)
+#define GPSR3_SD0_CD			BIT(12)
+#define GPSR3_SD1_DAT3			BIT(11)
+#define GPSR3_SD1_DAT2			BIT(10)
+#define GPSR3_SD1_DAT1			BIT(9)
+#define GPSR3_SD1_DAT0			BIT(8)
+#define GPSR3_SD1_CMD			BIT(7)
+#define GPSR3_SD1_CLK			BIT(6)
+#define GPSR3_SD0_DAT3			BIT(5)
+#define GPSR3_SD0_DAT2			BIT(4)
+#define GPSR3_SD0_DAT1			BIT(3)
+#define GPSR3_SD0_DAT0			BIT(2)
+#define GPSR3_SD0_CMD			BIT(1)
+#define GPSR3_SD0_CLK			BIT(0)
+#define GPSR4_SD3_DS			BIT(17)
+#define GPSR4_SD3_DAT7			BIT(16)
+#define GPSR4_SD3_DAT6			BIT(15)
+#define GPSR4_SD3_DAT5			BIT(14)
+#define GPSR4_SD3_DAT4			BIT(13)
+#define GPSR4_SD3_DAT3			BIT(12)
+#define GPSR4_SD3_DAT2			BIT(11)
+#define GPSR4_SD3_DAT1			BIT(10)
+#define GPSR4_SD3_DAT0			BIT(9)
+#define GPSR4_SD3_CMD			BIT(8)
+#define GPSR4_SD3_CLK			BIT(7)
+#define GPSR4_SD2_DS			BIT(6)
+#define GPSR4_SD2_DAT3			BIT(5)
+#define GPSR4_SD2_DAT2			BIT(4)
+#define GPSR4_SD2_DAT1			BIT(3)
+#define GPSR4_SD2_DAT0			BIT(2)
+#define GPSR4_SD2_CMD			BIT(1)
+#define GPSR4_SD2_CLK			BIT(0)
+#define GPSR5_MLB_DAT			BIT(25)
+#define GPSR5_MLB_SIG			BIT(24)
+#define GPSR5_MLB_CLK			BIT(23)
+#define GPSR5_MSIOF0_RXD		BIT(22)
+#define GPSR5_MSIOF0_SS2		BIT(21)
+#define GPSR5_MSIOF0_TXD		BIT(20)
+#define GPSR5_MSIOF0_SS1		BIT(19)
+#define GPSR5_MSIOF0_SYNC		BIT(18)
+#define GPSR5_MSIOF0_SCK		BIT(17)
+#define GPSR5_HRTS0			BIT(16)
+#define GPSR5_HCTS0			BIT(15)
+#define GPSR5_HTX0			BIT(14)
+#define GPSR5_HRX0			BIT(13)
+#define GPSR5_HSCK0			BIT(12)
+#define GPSR5_RX2_A			BIT(11)
+#define GPSR5_TX2_A			BIT(10)
+#define GPSR5_SCK2			BIT(9)
+#define GPSR5_RTS1			BIT(8)
+#define GPSR5_CTS1			BIT(7)
+#define GPSR5_TX1_A			BIT(6)
+#define GPSR5_RX1_A			BIT(5)
+#define GPSR5_RTS0			BIT(4)
+#define GPSR5_CTS0			BIT(3)
+#define GPSR5_TX0			BIT(2)
+#define GPSR5_RX0			BIT(1)
+#define GPSR5_SCK0			BIT(0)
+#define GPSR6_USB31_OVC			BIT(31)
+#define GPSR6_USB31_PWEN		BIT(30)
+#define GPSR6_USB30_OVC			BIT(29)
+#define GPSR6_USB30_PWEN		BIT(28)
+#define GPSR6_USB1_OVC			BIT(27)
+#define GPSR6_USB1_PWEN			BIT(26)
+#define GPSR6_USB0_OVC			BIT(25)
+#define GPSR6_USB0_PWEN			BIT(24)
+#define GPSR6_AUDIO_CLKB_B		BIT(23)
+#define GPSR6_AUDIO_CLKA_A		BIT(22)
+#define GPSR6_SSI_SDATA9_A		BIT(21)
+#define GPSR6_SSI_SDATA8		BIT(20)
+#define GPSR6_SSI_SDATA7		BIT(19)
+#define GPSR6_SSI_WS78			BIT(18)
+#define GPSR6_SSI_SCK78			BIT(17)
+#define GPSR6_SSI_SDATA6		BIT(16)
+#define GPSR6_SSI_WS6			BIT(15)
+#define GPSR6_SSI_SCK6			BIT(14)
+#define GPSR6_SSI_SDATA5		BIT(13)
+#define GPSR6_SSI_WS5			BIT(12)
+#define GPSR6_SSI_SCK5			BIT(11)
+#define GPSR6_SSI_SDATA4		BIT(10)
+#define GPSR6_SSI_WS4			BIT(9)
+#define GPSR6_SSI_SCK4			BIT(8)
+#define GPSR6_SSI_SDATA3		BIT(7)
+#define GPSR6_SSI_WS34			BIT(6)
+#define GPSR6_SSI_SCK34			BIT(5)
+#define GPSR6_SSI_SDATA2_A		BIT(4)
+#define GPSR6_SSI_SDATA1_A		BIT(3)
+#define GPSR6_SSI_SDATA0		BIT(2)
+#define GPSR6_SSI_WS0129		BIT(1)
+#define GPSR6_SSI_SCK0129		BIT(0)
+#define GPSR7_AVS2			BIT(1)
+#define GPSR7_AVS1			BIT(0)
+
+#define IPSR_28_FUNC(x)			((uint32_t)(x) << 28U)
+#define IPSR_24_FUNC(x)			((uint32_t)(x) << 24U)
+#define IPSR_20_FUNC(x)			((uint32_t)(x) << 20U)
+#define IPSR_16_FUNC(x)			((uint32_t)(x) << 16U)
+#define IPSR_12_FUNC(x)			((uint32_t)(x) << 12U)
+#define IPSR_8_FUNC(x)			((uint32_t)(x) << 8U)
+#define IPSR_4_FUNC(x)			((uint32_t)(x) << 4U)
+#define IPSR_0_FUNC(x)			((uint32_t)(x) << 0U)
+
+#define POC_SD3_DS_33V			BIT(29)
+#define POC_SD3_DAT7_33V		BIT(28)
+#define POC_SD3_DAT6_33V		BIT(27)
+#define POC_SD3_DAT5_33V		BIT(26)
+#define POC_SD3_DAT4_33V		BIT(25)
+#define POC_SD3_DAT3_33V		BIT(24)
+#define POC_SD3_DAT2_33V		BIT(23)
+#define POC_SD3_DAT1_33V		BIT(22)
+#define POC_SD3_DAT0_33V		BIT(21)
+#define POC_SD3_CMD_33V			BIT(20)
+#define POC_SD3_CLK_33V			BIT(19)
+#define POC_SD2_DS_33V			BIT(18)
+#define POC_SD2_DAT3_33V		BIT(17)
+#define POC_SD2_DAT2_33V		BIT(16)
+#define POC_SD2_DAT1_33V		BIT(15)
+#define POC_SD2_DAT0_33V		BIT(14)
+#define POC_SD2_CMD_33V			BIT(13)
+#define POC_SD2_CLK_33V			BIT(12)
+#define POC_SD1_DAT3_33V		BIT(11)
+#define POC_SD1_DAT2_33V		BIT(10)
+#define POC_SD1_DAT1_33V		BIT(9)
+#define POC_SD1_DAT0_33V		BIT(8)
+#define POC_SD1_CMD_33V			BIT(7)
+#define POC_SD1_CLK_33V			BIT(6)
+#define POC_SD0_DAT3_33V		BIT(5)
+#define POC_SD0_DAT2_33V		BIT(4)
+#define POC_SD0_DAT1_33V		BIT(3)
+#define POC_SD0_DAT0_33V		BIT(2)
+#define POC_SD0_CMD_33V			BIT(1)
+#define POC_SD0_CLK_33V			BIT(0)
+
+#define DRVCTRL0_MASK			(0xCCCCCCCCU)
+#define DRVCTRL1_MASK			(0xCCCCCCC8U)
+#define DRVCTRL2_MASK			(0x88888888U)
+#define DRVCTRL3_MASK			(0x88888888U)
+#define DRVCTRL4_MASK			(0x88888888U)
+#define DRVCTRL5_MASK			(0x88888888U)
+#define DRVCTRL6_MASK			(0x88888888U)
+#define DRVCTRL7_MASK			(0x88888888U)
+#define DRVCTRL8_MASK			(0x88888888U)
+#define DRVCTRL9_MASK			(0x88888888U)
+#define DRVCTRL10_MASK			(0x88888888U)
+#define DRVCTRL11_MASK			(0x888888CCU)
+#define DRVCTRL12_MASK			(0xCCCFFFCFU)
+#define DRVCTRL13_MASK			(0xCC888888U)
+#define DRVCTRL14_MASK			(0x88888888U)
+#define DRVCTRL15_MASK			(0x88888888U)
+#define DRVCTRL16_MASK			(0x88888888U)
+#define DRVCTRL17_MASK			(0x88888888U)
+#define DRVCTRL18_MASK			(0x88888888U)
+#define DRVCTRL19_MASK			(0x88888888U)
+#define DRVCTRL20_MASK			(0x88888888U)
+#define DRVCTRL21_MASK			(0x88888888U)
+#define DRVCTRL22_MASK			(0x88888888U)
+#define DRVCTRL23_MASK			(0x88888888U)
+#define DRVCTRL24_MASK			(0x8888888FU)
+
+#define DRVCTRL0_QSPI0_SPCLK(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL0_QSPI0_MOSI_IO0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL0_QSPI0_MISO_IO1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL0_QSPI0_IO2(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL0_QSPI0_IO3(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL0_QSPI0_SSL(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL0_QSPI1_SPCLK(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL0_QSPI1_MOSI_IO0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL1_QSPI1_MISO_IO1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL1_QSPI1_IO2(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL1_QSPI1_IO3(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL1_QSPI1_SS(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL1_RPC_INT(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL1_RPC_WP(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL1_RPC_RESET(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL1_AVB_RX_CTL(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL2_AVB_RXC(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL2_AVB_RD0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL2_AVB_RD1(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL2_AVB_RD2(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL2_AVB_RD3(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL2_AVB_TX_CTL(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL2_AVB_TXC(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL2_AVB_TD0(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL3_AVB_TD1(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL3_AVB_TD2(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL3_AVB_TD3(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL3_AVB_TXCREFCLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL3_AVB_MDIO(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL3_AVB_MDC(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL3_AVB_MAGIC(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL3_AVB_PHY_INT(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL4_AVB_LINK(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL4_AVB_AVTP_MATCH(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL4_AVB_AVTP_CAPTURE(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL4_IRQ0(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL4_IRQ1(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL4_IRQ2(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL4_IRQ3(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL4_IRQ4(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL5_IRQ5(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL5_PWM0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL5_PWM1(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL5_PWM2(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL5_A0(x)			((uint32_t)(x) << 12U)
+#define DRVCTRL5_A1(x)			((uint32_t)(x) << 8U)
+#define DRVCTRL5_A2(x)			((uint32_t)(x) << 4U)
+#define DRVCTRL5_A3(x)			((uint32_t)(x) << 0U)
+#define DRVCTRL6_A4(x)			((uint32_t)(x) << 28U)
+#define DRVCTRL6_A5(x)			((uint32_t)(x) << 24U)
+#define DRVCTRL6_A6(x)			((uint32_t)(x) << 20U)
+#define DRVCTRL6_A7(x)			((uint32_t)(x) << 16U)
+#define DRVCTRL6_A8(x)			((uint32_t)(x) << 12U)
+#define DRVCTRL6_A9(x)			((uint32_t)(x) << 8U)
+#define DRVCTRL6_A10(x)			((uint32_t)(x) << 4U)
+#define DRVCTRL6_A11(x)			((uint32_t)(x) << 0U)
+#define DRVCTRL7_A12(x)			((uint32_t)(x) << 28U)
+#define DRVCTRL7_A13(x)			((uint32_t)(x) << 24U)
+#define DRVCTRL7_A14(x)			((uint32_t)(x) << 20U)
+#define DRVCTRL7_A15(x)			((uint32_t)(x) << 16U)
+#define DRVCTRL7_A16(x)			((uint32_t)(x) << 12U)
+#define DRVCTRL7_A17(x)			((uint32_t)(x) << 8U)
+#define DRVCTRL7_A18(x)			((uint32_t)(x) << 4U)
+#define DRVCTRL7_A19(x)			((uint32_t)(x) << 0U)
+#define DRVCTRL8_CLKOUT(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL8_CS0(x)			((uint32_t)(x) << 24U)
+#define DRVCTRL8_CS1_A2(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL8_BS(x)			((uint32_t)(x) << 16U)
+#define DRVCTRL8_RD(x)			((uint32_t)(x) << 12U)
+#define DRVCTRL8_RD_W(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL8_WE0(x)			((uint32_t)(x) << 4U)
+#define DRVCTRL8_WE1(x)			((uint32_t)(x) << 0U)
+#define DRVCTRL9_EX_WAIT0(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL9_PRESETOU(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL9_D0(x)			((uint32_t)(x) << 20U)
+#define DRVCTRL9_D1(x)			((uint32_t)(x) << 16U)
+#define DRVCTRL9_D2(x)			((uint32_t)(x) << 12U)
+#define DRVCTRL9_D3(x)			((uint32_t)(x) << 8U)
+#define DRVCTRL9_D4(x)			((uint32_t)(x) << 4U)
+#define DRVCTRL9_D5(x)			((uint32_t)(x) << 0U)
+#define DRVCTRL10_D6(x)			((uint32_t)(x) << 28U)
+#define DRVCTRL10_D7(x)			((uint32_t)(x) << 24U)
+#define DRVCTRL10_D8(x)			((uint32_t)(x) << 20U)
+#define DRVCTRL10_D9(x)			((uint32_t)(x) << 16U)
+#define DRVCTRL10_D10(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL10_D11(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL10_D12(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL10_D13(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL11_D14(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL11_D15(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL11_AVS1(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL11_AVS2(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL11_GP7_02(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL11_GP7_03(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL11_DU_DOTCLKIN0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL11_DU_DOTCLKIN1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL12_DU_DOTCLKIN2(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL12_DU_DOTCLKIN3(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL12_DU_FSCLKST(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL12_DU_TMS(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL13_TDO(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL13_ASEBRK(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL13_SD0_CLK(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL13_SD0_CMD(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL13_SD0_DAT0(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL13_SD0_DAT1(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL13_SD0_DAT2(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL13_SD0_DAT3(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL14_SD1_CLK(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL14_SD1_CMD(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL14_SD1_DAT0(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL14_SD1_DAT1(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL14_SD1_DAT2(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL14_SD1_DAT3(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL14_SD2_CLK(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL14_SD2_CMD(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL15_SD2_DAT0(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL15_SD2_DAT1(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL15_SD2_DAT2(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL15_SD2_DAT3(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL15_SD2_DS(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL15_SD3_CLK(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL15_SD3_CMD(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL15_SD3_DAT0(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL16_SD3_DAT1(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL16_SD3_DAT2(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL16_SD3_DAT3(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL16_SD3_DAT4(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL16_SD3_DAT5(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL16_SD3_DAT6(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL16_SD3_DAT7(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL16_SD3_DS(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL17_SD0_CD(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL17_SD0_WP(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL17_SD1_CD(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL17_SD1_WP(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL17_SCK0(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL17_RX0(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL17_TX0(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL17_CTS0(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL18_RTS0_TANS(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL18_RX1(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL18_TX1(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL18_CTS1(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL18_RTS1_TANS(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL18_SCK2(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL18_TX2(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL18_RX2(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL19_HSCK0(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL19_HRX0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL19_HTX0(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL19_HCTS0(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL19_HRTS0(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL19_MSIOF0_SCK(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL19_MSIOF0_SYNC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL19_MSIOF0_SS1(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL20_MSIOF0_TXD(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL20_MSIOF0_SS2(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL20_MSIOF0_RXD(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL20_MLB_CLK(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL20_MLB_SIG(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL20_MLB_DAT(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL20_MLB_REF(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL20_SSI_SCK0129(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL21_SSI_WS0129(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL21_SSI_SDATA0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL21_SSI_SDATA1(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL21_SSI_SDATA2(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL21_SSI_SCK34(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL21_SSI_WS34(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL21_SSI_SDATA3(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL21_SSI_SCK4(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL22_SSI_WS4(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL22_SSI_SDATA4(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL22_SSI_SCK5(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL22_SSI_WS5(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL22_SSI_SDATA5(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL22_SSI_SCK6(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL22_SSI_WS6(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL22_SSI_SDATA6(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL23_SSI_SCK78(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL23_SSI_WS78(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL23_SSI_SDATA7(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL23_SSI_SDATA8(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL23_SSI_SDATA9(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL23_AUDIO_CLKA(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL23_AUDIO_CLKB(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL23_USB0_PWEN(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL24_USB0_OVC(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL24_USB1_PWEN(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL24_USB1_OVC(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL24_USB30_PWEN(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL24_USB30_OVC(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL24_USB31_PWEN(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL24_USB31_OVC(x)		((uint32_t)(x) << 4U)
+
+#define MOD_SEL0_MSIOF3_A		((uint32_t)0U << 29U)
+#define MOD_SEL0_MSIOF3_B		((uint32_t)1U << 29U)
+#define MOD_SEL0_MSIOF3_C		((uint32_t)2U << 29U)
+#define MOD_SEL0_MSIOF3_D		((uint32_t)3U << 29U)
+#define MOD_SEL0_MSIOF3_E		((uint32_t)4U << 29U)
+#define MOD_SEL0_MSIOF2_A		((uint32_t)0U << 27U)
+#define MOD_SEL0_MSIOF2_B		((uint32_t)1U << 27U)
+#define MOD_SEL0_MSIOF2_C		((uint32_t)2U << 27U)
+#define MOD_SEL0_MSIOF2_D		((uint32_t)3U << 27U)
+#define MOD_SEL0_MSIOF1_A		((uint32_t)0U << 24U)
+#define MOD_SEL0_MSIOF1_B		((uint32_t)1U << 24U)
+#define MOD_SEL0_MSIOF1_C		((uint32_t)2U << 24U)
+#define MOD_SEL0_MSIOF1_D		((uint32_t)3U << 24U)
+#define MOD_SEL0_MSIOF1_E		((uint32_t)4U << 24U)
+#define MOD_SEL0_MSIOF1_F		((uint32_t)5U << 24U)
+#define MOD_SEL0_MSIOF1_G		((uint32_t)6U << 24U)
+#define MOD_SEL0_LBSC_A			((uint32_t)0U << 23U)
+#define MOD_SEL0_LBSC_B			((uint32_t)1U << 23U)
+#define MOD_SEL0_IEBUS_A		((uint32_t)0U << 22U)
+#define MOD_SEL0_IEBUS_B		((uint32_t)1U << 22U)
+#define MOD_SEL0_I2C2_A			((uint32_t)0U << 21U)
+#define MOD_SEL0_I2C2_B			((uint32_t)1U << 21U)
+#define MOD_SEL0_I2C1_A			((uint32_t)0U << 20U)
+#define MOD_SEL0_I2C1_B			((uint32_t)1U << 20U)
+#define MOD_SEL0_HSCIF4_A		((uint32_t)0U << 19U)
+#define MOD_SEL0_HSCIF4_B		((uint32_t)1U << 19U)
+#define MOD_SEL0_HSCIF3_A		((uint32_t)0U << 17U)
+#define MOD_SEL0_HSCIF3_B		((uint32_t)1U << 17U)
+#define MOD_SEL0_HSCIF3_C		((uint32_t)2U << 17U)
+#define MOD_SEL0_HSCIF3_D		((uint32_t)3U << 17U)
+#define MOD_SEL0_HSCIF1_A		((uint32_t)0U << 16U)
+#define MOD_SEL0_HSCIF1_B		((uint32_t)1U << 16U)
+#define MOD_SEL0_FSO_A			((uint32_t)0U << 15U)
+#define MOD_SEL0_FSO_B			((uint32_t)1U << 15U)
+#define MOD_SEL0_HSCIF2_A		((uint32_t)0U << 13U)
+#define MOD_SEL0_HSCIF2_B		((uint32_t)1U << 13U)
+#define MOD_SEL0_HSCIF2_C		((uint32_t)2U << 13U)
+#define MOD_SEL0_ETHERAVB_A		((uint32_t)0U << 12U)
+#define MOD_SEL0_ETHERAVB_B		((uint32_t)1U << 12U)
+#define MOD_SEL0_DRIF3_A		((uint32_t)0U << 11U)
+#define MOD_SEL0_DRIF3_B		((uint32_t)1U << 11U)
+#define MOD_SEL0_DRIF2_A		((uint32_t)0U << 10U)
+#define MOD_SEL0_DRIF2_B		((uint32_t)1U << 10U)
+#define MOD_SEL0_DRIF1_A		((uint32_t)0U << 8U)
+#define MOD_SEL0_DRIF1_B		((uint32_t)1U << 8U)
+#define MOD_SEL0_DRIF1_C		((uint32_t)2U << 8U)
+#define MOD_SEL0_DRIF0_A		((uint32_t)0U << 6U)
+#define MOD_SEL0_DRIF0_B		((uint32_t)1U << 6U)
+#define MOD_SEL0_DRIF0_C		((uint32_t)2U << 6U)
+#define MOD_SEL0_CANFD0_A		((uint32_t)0U << 5U)
+#define MOD_SEL0_CANFD0_B		((uint32_t)1U << 5U)
+#define MOD_SEL0_ADG_A_A		((uint32_t)0U << 3U)
+#define MOD_SEL0_ADG_A_B		((uint32_t)1U << 3U)
+#define MOD_SEL0_ADG_A_C		((uint32_t)2U << 3U)
+#define MOD_SEL1_TSIF1_A		((uint32_t)0U << 30U)
+#define MOD_SEL1_TSIF1_B		((uint32_t)1U << 30U)
+#define MOD_SEL1_TSIF1_C		((uint32_t)2U << 30U)
+#define MOD_SEL1_TSIF1_D		((uint32_t)3U << 30U)
+#define MOD_SEL1_TSIF0_A		((uint32_t)0U << 27U)
+#define MOD_SEL1_TSIF0_B		((uint32_t)1U << 27U)
+#define MOD_SEL1_TSIF0_C		((uint32_t)2U << 27U)
+#define MOD_SEL1_TSIF0_D		((uint32_t)3U << 27U)
+#define MOD_SEL1_TSIF0_E		((uint32_t)4U << 27U)
+#define MOD_SEL1_TIMER_TMU_A		((uint32_t)0U << 26U)
+#define MOD_SEL1_TIMER_TMU_B		((uint32_t)1U << 26U)
+#define MOD_SEL1_SSP1_1_A		((uint32_t)0U << 24U)
+#define MOD_SEL1_SSP1_1_B		((uint32_t)1U << 24U)
+#define MOD_SEL1_SSP1_1_C		((uint32_t)2U << 24U)
+#define MOD_SEL1_SSP1_1_D		((uint32_t)3U << 24U)
+#define MOD_SEL1_SSP1_0_A		((uint32_t)0U << 21U)
+#define MOD_SEL1_SSP1_0_B		((uint32_t)1U << 21U)
+#define MOD_SEL1_SSP1_0_C		((uint32_t)2U << 21U)
+#define MOD_SEL1_SSP1_0_D		((uint32_t)3U << 21U)
+#define MOD_SEL1_SSP1_0_E		((uint32_t)4U << 21U)
+#define MOD_SEL1_SSI_A			((uint32_t)0U << 20U)
+#define MOD_SEL1_SSI_B			((uint32_t)1U << 20U)
+#define MOD_SEL1_SPEED_PULSE_IF_A	((uint32_t)0U << 19U)
+#define MOD_SEL1_SPEED_PULSE_IF_B	((uint32_t)1U << 19U)
+#define MOD_SEL1_SIMCARD_A		((uint32_t)0U << 17U)
+#define MOD_SEL1_SIMCARD_B		((uint32_t)1U << 17U)
+#define MOD_SEL1_SIMCARD_C		((uint32_t)2U << 17U)
+#define MOD_SEL1_SIMCARD_D		((uint32_t)3U << 17U)
+#define MOD_SEL1_SDHI2_A		((uint32_t)0U << 16U)
+#define MOD_SEL1_SDHI2_B		((uint32_t)1U << 16U)
+#define MOD_SEL1_SCIF4_A		((uint32_t)0U << 14U)
+#define MOD_SEL1_SCIF4_B		((uint32_t)1U << 14U)
+#define MOD_SEL1_SCIF4_C		((uint32_t)2U << 14U)
+#define MOD_SEL1_SCIF3_A		((uint32_t)0U << 13U)
+#define MOD_SEL1_SCIF3_B		((uint32_t)1U << 13U)
+#define MOD_SEL1_SCIF2_A		((uint32_t)0U << 12U)
+#define MOD_SEL1_SCIF2_B		((uint32_t)1U << 12U)
+#define MOD_SEL1_SCIF1_A		((uint32_t)0U << 11U)
+#define MOD_SEL1_SCIF1_B		((uint32_t)1U << 11U)
+#define MOD_SEL1_SCIF_A			((uint32_t)0U << 10U)
+#define MOD_SEL1_SCIF_B			((uint32_t)1U << 10U)
+#define MOD_SEL1_REMOCON_A		((uint32_t)0U << 9U)
+#define MOD_SEL1_REMOCON_B		((uint32_t)1U << 9U)
+#define MOD_SEL1_RCAN0_A		((uint32_t)0U << 6U)
+#define MOD_SEL1_RCAN0_B		((uint32_t)1U << 6U)
+#define MOD_SEL1_PWM6_A			((uint32_t)0U << 5U)
+#define MOD_SEL1_PWM6_B			((uint32_t)1U << 5U)
+#define MOD_SEL1_PWM5_A			((uint32_t)0U << 4U)
+#define MOD_SEL1_PWM5_B			((uint32_t)1U << 4U)
+#define MOD_SEL1_PWM4_A			((uint32_t)0U << 3U)
+#define MOD_SEL1_PWM4_B			((uint32_t)1U << 3U)
+#define MOD_SEL1_PWM3_A			((uint32_t)0U << 2U)
+#define MOD_SEL1_PWM3_B			((uint32_t)1U << 2U)
+#define MOD_SEL1_PWM2_A			((uint32_t)0U << 1U)
+#define MOD_SEL1_PWM2_B			((uint32_t)1U << 1U)
+#define MOD_SEL1_PWM1_A			((uint32_t)0U << 0U)
+#define MOD_SEL1_PWM1_B			((uint32_t)1U << 0U)
+#define MOD_SEL2_I2C_5_A		((uint32_t)0U << 31U)
+#define MOD_SEL2_I2C_5_B		((uint32_t)1U << 31U)
+#define MOD_SEL2_I2C_3_A		((uint32_t)0U << 30U)
+#define MOD_SEL2_I2C_3_B		((uint32_t)1U << 30U)
+#define MOD_SEL2_I2C_0_A		((uint32_t)0U << 29U)
+#define MOD_SEL2_I2C_0_B		((uint32_t)1U << 29U)
+#define MOD_SEL2_FM_A			((uint32_t)0U << 27U)
+#define MOD_SEL2_FM_B			((uint32_t)1U << 27U)
+#define MOD_SEL2_FM_C			((uint32_t)2U << 27U)
+#define MOD_SEL2_FM_D			((uint32_t)3U << 27U)
+#define MOD_SEL2_SCIF5_A		((uint32_t)0U << 26U)
+#define MOD_SEL2_SCIF5_B		((uint32_t)1U << 26U)
+#define MOD_SEL2_I2C6_A			((uint32_t)0U << 23U)
+#define MOD_SEL2_I2C6_B			((uint32_t)1U << 23U)
+#define MOD_SEL2_I2C6_C			((uint32_t)2U << 23U)
+#define MOD_SEL2_NDF_A			((uint32_t)0U << 22U)
+#define MOD_SEL2_NDF_B			((uint32_t)1U << 22U)
+#define MOD_SEL2_SSI2_A			((uint32_t)0U << 21U)
+#define MOD_SEL2_SSI2_B			((uint32_t)1U << 21U)
+#define MOD_SEL2_SSI9_A			((uint32_t)0U << 20U)
+#define MOD_SEL2_SSI9_B			((uint32_t)1U << 20U)
+#define MOD_SEL2_TIMER_TMU2_A		((uint32_t)0U << 19U)
+#define MOD_SEL2_TIMER_TMU2_B		((uint32_t)1U << 19U)
+#define MOD_SEL2_ADG_B_A		((uint32_t)0U << 18U)
+#define MOD_SEL2_ADG_B_B		((uint32_t)1U << 18U)
+#define MOD_SEL2_ADG_C_A		((uint32_t)0U << 17U)
+#define MOD_SEL2_ADG_C_B		((uint32_t)1U << 17U)
+#define MOD_SEL2_VIN4_A			((uint32_t)0U << 0U)
+#define MOD_SEL2_VIN4_B			((uint32_t)1U << 0U)
+
+static void pfc_reg_write(uint32_t addr, uint32_t data)
+{
+	mmio_write_32(PFC_PMMR, ~data);
+	mmio_write_32((uintptr_t)addr, data);
+}
+
+void pfc_init_g2h(void)
+{
+	uint32_t reg;
+
+	/* initialize module select */
+	pfc_reg_write(PFC_MOD_SEL0,
+		      MOD_SEL0_MSIOF3_A |
+		      MOD_SEL0_MSIOF2_A |
+		      MOD_SEL0_MSIOF1_A |
+		      MOD_SEL0_LBSC_A |
+		      MOD_SEL0_IEBUS_A |
+		      MOD_SEL0_I2C2_A |
+		      MOD_SEL0_I2C1_A |
+		      MOD_SEL0_HSCIF4_A |
+		      MOD_SEL0_HSCIF3_A |
+		      MOD_SEL0_HSCIF1_A |
+		      MOD_SEL0_FSO_A |
+		      MOD_SEL0_HSCIF2_A |
+		      MOD_SEL0_ETHERAVB_A |
+		      MOD_SEL0_DRIF3_A |
+		      MOD_SEL0_DRIF2_A |
+		      MOD_SEL0_DRIF1_A |
+		      MOD_SEL0_DRIF0_A |
+		      MOD_SEL0_CANFD0_A |
+		      MOD_SEL0_ADG_A_A);
+
+	pfc_reg_write(PFC_MOD_SEL1,
+		      MOD_SEL1_TSIF1_A |
+		      MOD_SEL1_TSIF0_A |
+		      MOD_SEL1_TIMER_TMU_A |
+		      MOD_SEL1_SSP1_1_A |
+		      MOD_SEL1_SSP1_0_A |
+		      MOD_SEL1_SSI_A |
+		      MOD_SEL1_SPEED_PULSE_IF_A |
+		      MOD_SEL1_SIMCARD_A |
+		      MOD_SEL1_SDHI2_A |
+		      MOD_SEL1_SCIF4_A |
+		      MOD_SEL1_SCIF3_A |
+		      MOD_SEL1_SCIF2_A |
+		      MOD_SEL1_SCIF1_A |
+		      MOD_SEL1_SCIF_A |
+		      MOD_SEL1_REMOCON_A |
+		      MOD_SEL1_RCAN0_A |
+		      MOD_SEL1_PWM6_A |
+		      MOD_SEL1_PWM5_A |
+		      MOD_SEL1_PWM4_A |
+		      MOD_SEL1_PWM3_A |
+		      MOD_SEL1_PWM2_A |
+		      MOD_SEL1_PWM1_A);
+
+	pfc_reg_write(PFC_MOD_SEL2,
+		      MOD_SEL2_I2C_5_B |
+		      MOD_SEL2_I2C_3_B |
+		      MOD_SEL2_I2C_0_B |
+		      MOD_SEL2_FM_A |
+		      MOD_SEL2_SCIF5_A |
+		      MOD_SEL2_I2C6_A |
+		      MOD_SEL2_NDF_A |
+		      MOD_SEL2_SSI2_A |
+		      MOD_SEL2_SSI9_A |
+		      MOD_SEL2_TIMER_TMU2_A |
+		      MOD_SEL2_ADG_B_A |
+		      MOD_SEL2_ADG_C_A |
+		      MOD_SEL2_VIN4_A);
+
+	/* initialize peripheral function select */
+	pfc_reg_write(PFC_IPSR0,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR1,
+		      IPSR_28_FUNC(6) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(3) |
+		      IPSR_8_FUNC(3) |
+		      IPSR_4_FUNC(3) |
+		      IPSR_0_FUNC(3));
+
+	pfc_reg_write(PFC_IPSR2,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(6) |
+		      IPSR_20_FUNC(6) |
+		      IPSR_16_FUNC(6) |
+		      IPSR_12_FUNC(6) |
+		      IPSR_8_FUNC(6) |
+		      IPSR_4_FUNC(6) |
+		      IPSR_0_FUNC(6));
+
+	pfc_reg_write(PFC_IPSR3,
+		      IPSR_28_FUNC(6) |
+		      IPSR_24_FUNC(6) |
+		      IPSR_20_FUNC(6) |
+		      IPSR_16_FUNC(6) |
+		      IPSR_12_FUNC(6) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR4,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(6) |
+		      IPSR_4_FUNC(6) |
+		      IPSR_0_FUNC(6));
+
+	pfc_reg_write(PFC_IPSR5,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(6) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR6,
+		      IPSR_28_FUNC(6) |
+		      IPSR_24_FUNC(6) |
+		      IPSR_20_FUNC(6) |
+		      IPSR_16_FUNC(6) |
+		      IPSR_12_FUNC(6) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR7,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(6) |
+		      IPSR_4_FUNC(6) |
+		      IPSR_0_FUNC(6));
+
+	pfc_reg_write(PFC_IPSR8,
+		      IPSR_28_FUNC(1) |
+		      IPSR_24_FUNC(1) |
+		      IPSR_20_FUNC(1) |
+		      IPSR_16_FUNC(1) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR9,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR10,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR11,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(4) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR12,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(4) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR13,
+		      IPSR_28_FUNC(8) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(3) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR14,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(3) |
+		      IPSR_0_FUNC(8));
+
+	pfc_reg_write(PFC_IPSR15,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR16,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR17,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(1) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR18,
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	/* initialize GPIO/peripheral function select */
+	pfc_reg_write(PFC_GPSR0,
+		      GPSR0_D15 |
+		      GPSR0_D14 |
+		      GPSR0_D13 |
+		      GPSR0_D12 |
+		      GPSR0_D11 |
+		      GPSR0_D10 |
+		      GPSR0_D9 |
+		      GPSR0_D8 |
+		      GPSR0_D7 |
+		      GPSR0_D6 |
+		      GPSR0_D5 |
+		      GPSR0_D4 |
+		      GPSR0_D3 |
+		      GPSR0_D2 |
+		      GPSR0_D0);
+
+	pfc_reg_write(PFC_GPSR1,
+		      GPSR1_CLKOUT |
+		      GPSR1_EX_WAIT0_A |
+		      GPSR1_WE1 |
+		      GPSR1_RD |
+		      GPSR1_RD_WR |
+		      GPSR1_CS0 |
+		      GPSR1_A19 |
+		      GPSR1_A18 |
+		      GPSR1_A17 |
+		      GPSR1_A16 |
+		      GPSR1_A15 |
+		      GPSR1_A14 |
+		      GPSR1_A13 |
+		      GPSR1_A12 |
+		      GPSR1_A7 |
+		      GPSR1_A6 |
+		      GPSR1_A5 |
+		      GPSR1_A4 |
+		      GPSR1_A3 |
+		      GPSR1_A2 |
+		      GPSR1_A1 |
+		      GPSR1_A0);
+
+	pfc_reg_write(PFC_GPSR2,
+		      GPSR2_AVB_AVTP_CAPTURE_A |
+		      GPSR2_AVB_AVTP_MATCH_A |
+		      GPSR2_AVB_LINK |
+		      GPSR2_AVB_PHY_INT |
+		      GPSR2_AVB_MDC |
+		      GPSR2_PWM2_A |
+		      GPSR2_PWM1_A |
+		      GPSR2_IRQ4 |
+		      GPSR2_IRQ3 |
+		      GPSR2_IRQ2 |
+		      GPSR2_IRQ1 |
+		      GPSR2_IRQ0);
+
+	pfc_reg_write(PFC_GPSR3,
+		      GPSR3_SD0_CD |
+		      GPSR3_SD1_DAT3 |
+		      GPSR3_SD1_DAT2 |
+		      GPSR3_SD1_DAT1 |
+		      GPSR3_SD1_DAT0 |
+		      GPSR3_SD0_DAT3 |
+		      GPSR3_SD0_DAT2 |
+		      GPSR3_SD0_DAT1 |
+		      GPSR3_SD0_DAT0 |
+		      GPSR3_SD0_CMD |
+		      GPSR3_SD0_CLK);
+
+	pfc_reg_write(PFC_GPSR4,
+		      GPSR4_SD3_DS |
+		      GPSR4_SD3_DAT7 |
+		      GPSR4_SD3_DAT6 |
+		      GPSR4_SD3_DAT5 |
+		      GPSR4_SD3_DAT4 |
+		      GPSR4_SD3_DAT3 |
+		      GPSR4_SD3_DAT2 |
+		      GPSR4_SD3_DAT1 |
+		      GPSR4_SD3_DAT0 |
+		      GPSR4_SD3_CMD |
+		      GPSR4_SD3_CLK |
+		      GPSR4_SD2_DAT3 |
+		      GPSR4_SD2_DAT2 |
+		      GPSR4_SD2_DAT1 |
+		      GPSR4_SD2_DAT0 |
+		      GPSR4_SD2_CMD |
+		      GPSR4_SD2_CLK);
+
+	pfc_reg_write(PFC_GPSR5,
+		      GPSR5_MSIOF0_RXD |
+		      GPSR5_MSIOF0_TXD |
+		      GPSR5_MSIOF0_SYNC |
+		      GPSR5_MSIOF0_SCK |
+		      GPSR5_RX2_A |
+		      GPSR5_TX2_A |
+		      GPSR5_RTS1 |
+		      GPSR5_CTS1 |
+		      GPSR5_TX1_A |
+		      GPSR5_RX1_A |
+		      GPSR5_RTS0 |
+		      GPSR5_SCK0);
+
+	pfc_reg_write(PFC_GPSR6,
+		      GPSR6_AUDIO_CLKB_B |
+		      GPSR6_AUDIO_CLKA_A |
+		      GPSR6_SSI_WS6 |
+		      GPSR6_SSI_SCK6 |
+		      GPSR6_SSI_SDATA4 |
+		      GPSR6_SSI_WS4 |
+		      GPSR6_SSI_SCK4 |
+		      GPSR6_SSI_SDATA1_A |
+		      GPSR6_SSI_SDATA0 |
+		      GPSR6_SSI_WS0129 |
+		      GPSR6_SSI_SCK0129);
+
+	pfc_reg_write(PFC_GPSR7,
+		      GPSR7_AVS2 |
+		      GPSR7_AVS1);
+
+	/* initialize POC control register */
+	pfc_reg_write(PFC_POCCTRL0,
+		      POC_SD0_DAT3_33V |
+		      POC_SD0_DAT2_33V |
+		      POC_SD0_DAT1_33V |
+		      POC_SD0_DAT0_33V |
+		      POC_SD0_CMD_33V |
+		      POC_SD0_CLK_33V);
+
+	/* initialize DRV control register */
+	reg = mmio_read_32(PFC_DRVCTRL0);
+	reg = (reg & DRVCTRL0_MASK) |
+	      DRVCTRL0_QSPI0_SPCLK(3) |
+	      DRVCTRL0_QSPI0_MOSI_IO0(3) |
+	      DRVCTRL0_QSPI0_MISO_IO1(3) |
+	      DRVCTRL0_QSPI0_IO2(3) |
+	      DRVCTRL0_QSPI0_IO3(3) |
+	      DRVCTRL0_QSPI0_SSL(3) |
+	      DRVCTRL0_QSPI1_SPCLK(3) |
+	      DRVCTRL0_QSPI1_MOSI_IO0(3);
+	pfc_reg_write(PFC_DRVCTRL0, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL1);
+	reg = (reg & DRVCTRL1_MASK) |
+	      DRVCTRL1_QSPI1_MISO_IO1(3) |
+	      DRVCTRL1_QSPI1_IO2(3) |
+	      DRVCTRL1_QSPI1_IO3(3) |
+	      DRVCTRL1_QSPI1_SS(3) |
+	      DRVCTRL1_RPC_INT(3) |
+	      DRVCTRL1_RPC_WP(3) |
+	      DRVCTRL1_RPC_RESET(3) |
+	      DRVCTRL1_AVB_RX_CTL(7);
+	pfc_reg_write(PFC_DRVCTRL1, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL2);
+	reg = (reg & DRVCTRL2_MASK) |
+	      DRVCTRL2_AVB_RXC(7) |
+	      DRVCTRL2_AVB_RD0(7) |
+	      DRVCTRL2_AVB_RD1(7) |
+	      DRVCTRL2_AVB_RD2(7) |
+	      DRVCTRL2_AVB_RD3(7) |
+	      DRVCTRL2_AVB_TX_CTL(3) |
+	      DRVCTRL2_AVB_TXC(3) |
+	      DRVCTRL2_AVB_TD0(3);
+	pfc_reg_write(PFC_DRVCTRL2, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL3);
+	reg = (reg & DRVCTRL3_MASK) |
+	      DRVCTRL3_AVB_TD1(3) |
+	      DRVCTRL3_AVB_TD2(3) |
+	      DRVCTRL3_AVB_TD3(3) |
+	      DRVCTRL3_AVB_TXCREFCLK(7) |
+	      DRVCTRL3_AVB_MDIO(7) |
+	      DRVCTRL3_AVB_MDC(7) |
+	      DRVCTRL3_AVB_MAGIC(7) |
+	      DRVCTRL3_AVB_PHY_INT(7);
+	pfc_reg_write(PFC_DRVCTRL3, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL4);
+	reg = (reg & DRVCTRL4_MASK) |
+	      DRVCTRL4_AVB_LINK(7) |
+	      DRVCTRL4_AVB_AVTP_MATCH(7) |
+	      DRVCTRL4_AVB_AVTP_CAPTURE(7) |
+	      DRVCTRL4_IRQ0(7) |
+	      DRVCTRL4_IRQ1(7) |
+	      DRVCTRL4_IRQ2(7) |
+	      DRVCTRL4_IRQ3(7) |
+	      DRVCTRL4_IRQ4(7);
+	pfc_reg_write(PFC_DRVCTRL4, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL5);
+	reg = (reg & DRVCTRL5_MASK) |
+	      DRVCTRL5_IRQ5(7) |
+	      DRVCTRL5_PWM0(7) |
+	      DRVCTRL5_PWM1(7) |
+	      DRVCTRL5_PWM2(7) |
+	      DRVCTRL5_A0(3) |
+	      DRVCTRL5_A1(3) |
+	      DRVCTRL5_A2(3) |
+	      DRVCTRL5_A3(3);
+	pfc_reg_write(PFC_DRVCTRL5, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL6);
+	reg = (reg & DRVCTRL6_MASK) |
+	      DRVCTRL6_A4(3) |
+	      DRVCTRL6_A5(3) |
+	      DRVCTRL6_A6(3) |
+	      DRVCTRL6_A7(3) |
+	      DRVCTRL6_A8(7) |
+	      DRVCTRL6_A9(7) |
+	      DRVCTRL6_A10(7) |
+	      DRVCTRL6_A11(7);
+	pfc_reg_write(PFC_DRVCTRL6, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL7);
+	reg = (reg & DRVCTRL7_MASK) |
+	      DRVCTRL7_A12(3) |
+	      DRVCTRL7_A13(3) |
+	      DRVCTRL7_A14(3) |
+	      DRVCTRL7_A15(3) |
+	      DRVCTRL7_A16(3) |
+	      DRVCTRL7_A17(3) |
+	      DRVCTRL7_A18(3) |
+	      DRVCTRL7_A19(3);
+	pfc_reg_write(PFC_DRVCTRL7, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL8);
+	reg = (reg & DRVCTRL8_MASK) |
+	      DRVCTRL8_CLKOUT(7) |
+	      DRVCTRL8_CS0(7) |
+	      DRVCTRL8_CS1_A2(7) |
+	      DRVCTRL8_BS(7) |
+	      DRVCTRL8_RD(7) |
+	      DRVCTRL8_RD_W(7) |
+	      DRVCTRL8_WE0(7) |
+	      DRVCTRL8_WE1(7);
+	pfc_reg_write(PFC_DRVCTRL8, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL9);
+	reg = (reg & DRVCTRL9_MASK) |
+	      DRVCTRL9_EX_WAIT0(7) |
+	      DRVCTRL9_PRESETOU(7) |
+	      DRVCTRL9_D0(7) |
+	      DRVCTRL9_D1(7) |
+	      DRVCTRL9_D2(7) |
+	      DRVCTRL9_D3(7) |
+	      DRVCTRL9_D4(7) |
+	      DRVCTRL9_D5(7);
+	pfc_reg_write(PFC_DRVCTRL9, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL10);
+	reg = (reg & DRVCTRL10_MASK) |
+	      DRVCTRL10_D6(7) |
+	      DRVCTRL10_D7(7) |
+	      DRVCTRL10_D8(3) |
+	      DRVCTRL10_D9(3) |
+	      DRVCTRL10_D10(3) |
+	      DRVCTRL10_D11(3) |
+	      DRVCTRL10_D12(3) |
+	      DRVCTRL10_D13(3);
+	pfc_reg_write(PFC_DRVCTRL10, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL11);
+	reg = (reg & DRVCTRL11_MASK) |
+	      DRVCTRL11_D14(3) |
+	      DRVCTRL11_D15(3) |
+	      DRVCTRL11_AVS1(7) |
+	      DRVCTRL11_AVS2(7) |
+	      DRVCTRL11_GP7_02(7) |
+	      DRVCTRL11_GP7_03(7) |
+	      DRVCTRL11_DU_DOTCLKIN0(3) |
+	      DRVCTRL11_DU_DOTCLKIN1(3);
+	pfc_reg_write(PFC_DRVCTRL11, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL12);
+	reg = (reg & DRVCTRL12_MASK) |
+	      DRVCTRL12_DU_DOTCLKIN2(3) |
+	      DRVCTRL12_DU_DOTCLKIN3(3) |
+	      DRVCTRL12_DU_FSCLKST(3) |
+	      DRVCTRL12_DU_TMS(3);
+	pfc_reg_write(PFC_DRVCTRL12, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL13);
+	reg = (reg & DRVCTRL13_MASK) |
+	      DRVCTRL13_TDO(3) |
+	      DRVCTRL13_ASEBRK(3) |
+	      DRVCTRL13_SD0_CLK(7) |
+	      DRVCTRL13_SD0_CMD(7) |
+	      DRVCTRL13_SD0_DAT0(7) |
+	      DRVCTRL13_SD0_DAT1(7) |
+	      DRVCTRL13_SD0_DAT2(7) |
+	      DRVCTRL13_SD0_DAT3(7);
+	pfc_reg_write(PFC_DRVCTRL13, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL14);
+	reg = (reg & DRVCTRL14_MASK) |
+	      DRVCTRL14_SD1_CLK(7) |
+	      DRVCTRL14_SD1_CMD(7) |
+	      DRVCTRL14_SD1_DAT0(5) |
+	      DRVCTRL14_SD1_DAT1(5) |
+	      DRVCTRL14_SD1_DAT2(5) |
+	      DRVCTRL14_SD1_DAT3(5) |
+	      DRVCTRL14_SD2_CLK(5) |
+	      DRVCTRL14_SD2_CMD(5);
+	pfc_reg_write(PFC_DRVCTRL14, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL15);
+	reg = (reg & DRVCTRL15_MASK) |
+	      DRVCTRL15_SD2_DAT0(5) |
+	      DRVCTRL15_SD2_DAT1(5) |
+	      DRVCTRL15_SD2_DAT2(5) |
+	      DRVCTRL15_SD2_DAT3(5) |
+	      DRVCTRL15_SD2_DS(5) |
+	      DRVCTRL15_SD3_CLK(7) |
+	      DRVCTRL15_SD3_CMD(7) |
+	      DRVCTRL15_SD3_DAT0(7);
+	pfc_reg_write(PFC_DRVCTRL15, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL16);
+	reg = (reg & DRVCTRL16_MASK) |
+	      DRVCTRL16_SD3_DAT1(7) |
+	      DRVCTRL16_SD3_DAT2(7) |
+	      DRVCTRL16_SD3_DAT3(7) |
+	      DRVCTRL16_SD3_DAT4(7) |
+	      DRVCTRL16_SD3_DAT5(7) |
+	      DRVCTRL16_SD3_DAT6(7) |
+	      DRVCTRL16_SD3_DAT7(7) |
+	      DRVCTRL16_SD3_DS(7);
+	pfc_reg_write(PFC_DRVCTRL16, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL17);
+	reg = (reg & DRVCTRL17_MASK) |
+	      DRVCTRL17_SD0_CD(7) |
+	      DRVCTRL17_SD0_WP(7) |
+	      DRVCTRL17_SD1_CD(7) |
+	      DRVCTRL17_SD1_WP(7) |
+	      DRVCTRL17_SCK0(7) |
+	      DRVCTRL17_RX0(7) |
+	      DRVCTRL17_TX0(7) |
+	      DRVCTRL17_CTS0(7);
+	pfc_reg_write(PFC_DRVCTRL17, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL18);
+	reg = (reg & DRVCTRL18_MASK) |
+	      DRVCTRL18_RTS0_TANS(7) |
+	      DRVCTRL18_RX1(7) |
+	      DRVCTRL18_TX1(7) |
+	      DRVCTRL18_CTS1(7) |
+	      DRVCTRL18_RTS1_TANS(7) |
+	      DRVCTRL18_SCK2(7) |
+	      DRVCTRL18_TX2(7) |
+	      DRVCTRL18_RX2(7);
+	pfc_reg_write(PFC_DRVCTRL18, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL19);
+	reg = (reg & DRVCTRL19_MASK) |
+	      DRVCTRL19_HSCK0(7) |
+	      DRVCTRL19_HRX0(7) |
+	      DRVCTRL19_HTX0(7) |
+	      DRVCTRL19_HCTS0(7) |
+	      DRVCTRL19_HRTS0(7) |
+	      DRVCTRL19_MSIOF0_SCK(7) |
+	      DRVCTRL19_MSIOF0_SYNC(7) |
+	      DRVCTRL19_MSIOF0_SS1(7);
+	pfc_reg_write(PFC_DRVCTRL19, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL20);
+	reg = (reg & DRVCTRL20_MASK) |
+	      DRVCTRL20_MSIOF0_TXD(7) |
+	      DRVCTRL20_MSIOF0_SS2(7) |
+	      DRVCTRL20_MSIOF0_RXD(7) |
+	      DRVCTRL20_MLB_CLK(7) |
+	      DRVCTRL20_MLB_SIG(7) |
+	      DRVCTRL20_MLB_DAT(7) |
+	      DRVCTRL20_MLB_REF(7) |
+	      DRVCTRL20_SSI_SCK0129(7);
+	pfc_reg_write(PFC_DRVCTRL20, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL21);
+	reg = (reg & DRVCTRL21_MASK) |
+	      DRVCTRL21_SSI_WS0129(7) |
+	      DRVCTRL21_SSI_SDATA0(7) |
+	      DRVCTRL21_SSI_SDATA1(7) |
+	      DRVCTRL21_SSI_SDATA2(7) |
+	      DRVCTRL21_SSI_SCK34(7) |
+	      DRVCTRL21_SSI_WS34(7) |
+	      DRVCTRL21_SSI_SDATA3(7) |
+	      DRVCTRL21_SSI_SCK4(7);
+	pfc_reg_write(PFC_DRVCTRL21, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL22);
+	reg = (reg & DRVCTRL22_MASK) |
+	      DRVCTRL22_SSI_WS4(7) |
+	      DRVCTRL22_SSI_SDATA4(7) |
+	      DRVCTRL22_SSI_SCK5(7) |
+	      DRVCTRL22_SSI_WS5(7) |
+	      DRVCTRL22_SSI_SDATA5(7) |
+	      DRVCTRL22_SSI_SCK6(7) |
+	      DRVCTRL22_SSI_WS6(7) |
+	      DRVCTRL22_SSI_SDATA6(7);
+	pfc_reg_write(PFC_DRVCTRL22, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL23);
+	reg = (reg & DRVCTRL23_MASK) |
+	      DRVCTRL23_SSI_SCK78(7) |
+	      DRVCTRL23_SSI_WS78(7) |
+	      DRVCTRL23_SSI_SDATA7(7) |
+	      DRVCTRL23_SSI_SDATA8(7) |
+	      DRVCTRL23_SSI_SDATA9(7) |
+	      DRVCTRL23_AUDIO_CLKA(7) |
+	      DRVCTRL23_AUDIO_CLKB(7) |
+	      DRVCTRL23_USB0_PWEN(7);
+	pfc_reg_write(PFC_DRVCTRL23, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL24);
+	reg = (reg & DRVCTRL24_MASK) |
+	      DRVCTRL24_USB0_OVC(7) |
+	      DRVCTRL24_USB1_PWEN(7) |
+	      DRVCTRL24_USB1_OVC(7) |
+	      DRVCTRL24_USB30_PWEN(7) |
+	      DRVCTRL24_USB30_OVC(7) |
+	      DRVCTRL24_USB31_PWEN(7) |
+	      DRVCTRL24_USB31_OVC(7);
+	pfc_reg_write(PFC_DRVCTRL24, reg);
+
+	/* initialize LSI pin pull-up/down control */
+	pfc_reg_write(PFC_PUD0, 0x00005FBFU);
+	pfc_reg_write(PFC_PUD1, 0x00300EFEU);
+	pfc_reg_write(PFC_PUD2, 0x330001E6U);
+	pfc_reg_write(PFC_PUD3, 0x000002E0U);
+	pfc_reg_write(PFC_PUD4, 0xFFFFFF00U);
+	pfc_reg_write(PFC_PUD5, 0x7F5FFF87U);
+	pfc_reg_write(PFC_PUD6, 0x00000055U);
+
+	/* initialize LSI pin pull-enable register */
+	pfc_reg_write(PFC_PUEN0, 0x00000FFFU);
+	pfc_reg_write(PFC_PUEN1, 0x00100234U);
+	pfc_reg_write(PFC_PUEN2, 0x000004C4U);
+	pfc_reg_write(PFC_PUEN3, 0x00000200U);
+	pfc_reg_write(PFC_PUEN4, 0x3E000000U);
+	pfc_reg_write(PFC_PUEN5, 0x1F000805U);
+	pfc_reg_write(PFC_PUEN6, 0x00000006U);
+
+	/* initialize positive/negative logic select */
+	mmio_write_32(GPIO_POSNEG0, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG1, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG2, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG3, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG4, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG5, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG6, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG7, 0x00000000U);
+
+	/* initialize general IO/interrupt switching */
+	mmio_write_32(GPIO_IOINTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL5, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL6, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL7, 0x00000000U);
+
+	/* initialize general output register */
+	mmio_write_32(GPIO_OUTDT0, 0x00000001U);
+	mmio_write_32(GPIO_OUTDT1, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT2, 0x00000400U);
+	mmio_write_32(GPIO_OUTDT3, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT4, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT5, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT6, 0x00003800U);
+	mmio_write_32(GPIO_OUTDT7, 0x00000003U);
+
+	/* initialize general input/output switching */
+	mmio_write_32(GPIO_INOUTSEL0, 0x00000001U);
+	mmio_write_32(GPIO_INOUTSEL1, 0x00100B00U);
+	mmio_write_32(GPIO_INOUTSEL2, 0x00000418U);
+	mmio_write_32(GPIO_INOUTSEL3, 0x00002000U);
+	mmio_write_32(GPIO_INOUTSEL4, 0x00000040U);
+	mmio_write_32(GPIO_INOUTSEL5, 0x00000208U);
+	mmio_write_32(GPIO_INOUTSEL6, 0x00013F00U);
+	mmio_write_32(GPIO_INOUTSEL7, 0x00000003U);
+}
diff --git a/drivers/renesas/rzg/pfc/G2H/pfc_init_g2h.h b/drivers/renesas/rzg/pfc/G2H/pfc_init_g2h.h
new file mode 100644
index 0000000..5efce45
--- /dev/null
+++ b/drivers/renesas/rzg/pfc/G2H/pfc_init_g2h.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PFC_INIT_G2H_H
+#define PFC_INIT_G2H_H
+
+void pfc_init_g2h(void);
+
+#endif /* PFC_INIT_G2H_H */
diff --git a/drivers/renesas/rzg/pfc/G2M/pfc_init_g2m.c b/drivers/renesas/rzg/pfc/G2M/pfc_init_g2m.c
new file mode 100644
index 0000000..f76b83f
--- /dev/null
+++ b/drivers/renesas/rzg/pfc/G2M/pfc_init_g2m.c
@@ -0,0 +1,1300 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>		/* for uint32_t */
+
+#include <lib/mmio.h>
+
+#include "pfc_init_g2m.h"
+#include "rcar_def.h"
+#include "rcar_private.h"
+#include "pfc_regs.h"
+
+#define GPSR0_D15		BIT(15)
+#define GPSR0_D14		BIT(14)
+#define GPSR0_D13		BIT(13)
+#define GPSR0_D12		BIT(12)
+#define GPSR0_D11		BIT(11)
+#define GPSR0_D10		BIT(10)
+#define GPSR0_D9		BIT(9)
+#define GPSR0_D8		BIT(8)
+#define GPSR0_D7		BIT(7)
+#define GPSR0_D6		BIT(6)
+#define GPSR0_D5		BIT(5)
+#define GPSR0_D4		BIT(4)
+#define GPSR0_D3		BIT(3)
+#define GPSR0_D2		BIT(2)
+#define GPSR0_D1		BIT(1)
+#define GPSR0_D0		BIT(0)
+#define GPSR1_CLKOUT		BIT(28)
+#define GPSR1_EX_WAIT0_A	BIT(27)
+#define GPSR1_WE1		BIT(26)
+#define GPSR1_WE0		BIT(25)
+#define GPSR1_RD_WR		BIT(24)
+#define GPSR1_RD		BIT(23)
+#define GPSR1_BS		BIT(22)
+#define GPSR1_CS1_A26		BIT(21)
+#define GPSR1_CS0		BIT(20)
+#define GPSR1_A19		BIT(19)
+#define GPSR1_A18		BIT(18)
+#define GPSR1_A17		BIT(17)
+#define GPSR1_A16		BIT(16)
+#define GPSR1_A15		BIT(15)
+#define GPSR1_A14		BIT(14)
+#define GPSR1_A13		BIT(13)
+#define GPSR1_A12		BIT(12)
+#define GPSR1_A11		BIT(11)
+#define GPSR1_A10		BIT(10)
+#define GPSR1_A9		BIT(9)
+#define GPSR1_A8		BIT(8)
+#define GPSR1_A7		BIT(7)
+#define GPSR1_A6		BIT(6)
+#define GPSR1_A5		BIT(5)
+#define GPSR1_A4		BIT(4)
+#define GPSR1_A3		BIT(3)
+#define GPSR1_A2		BIT(2)
+#define GPSR1_A1		BIT(1)
+#define GPSR1_A0		BIT(0)
+#define GPSR2_AVB_AVTP_CAPTURE_A	BIT(14)
+#define GPSR2_AVB_AVTP_MATCH_A	BIT(13)
+#define GPSR2_AVB_LINK		BIT(12)
+#define GPSR2_AVB_PHY_INT	BIT(11)
+#define GPSR2_AVB_MAGIC		BIT(10)
+#define GPSR2_AVB_MDC		BIT(9)
+#define GPSR2_PWM2_A		BIT(8)
+#define GPSR2_PWM1_A		BIT(7)
+#define GPSR2_PWM0		BIT(6)
+#define GPSR2_IRQ5		BIT(5)
+#define GPSR2_IRQ4		BIT(4)
+#define GPSR2_IRQ3		BIT(3)
+#define GPSR2_IRQ2		BIT(2)
+#define GPSR2_IRQ1		BIT(1)
+#define GPSR2_IRQ0		BIT(0)
+#define GPSR3_SD1_WP		BIT(15)
+#define GPSR3_SD1_CD		BIT(14)
+#define GPSR3_SD0_WP		BIT(13)
+#define GPSR3_SD0_CD		BIT(12)
+#define GPSR3_SD1_DAT3		BIT(11)
+#define GPSR3_SD1_DAT2		BIT(10)
+#define GPSR3_SD1_DAT1		BIT(9)
+#define GPSR3_SD1_DAT0		BIT(8)
+#define GPSR3_SD1_CMD		BIT(7)
+#define GPSR3_SD1_CLK		BIT(6)
+#define GPSR3_SD0_DAT3		BIT(5)
+#define GPSR3_SD0_DAT2		BIT(4)
+#define GPSR3_SD0_DAT1		BIT(3)
+#define GPSR3_SD0_DAT0		BIT(2)
+#define GPSR3_SD0_CMD		BIT(1)
+#define GPSR3_SD0_CLK		BIT(0)
+#define GPSR4_SD3_DS		BIT(17)
+#define GPSR4_SD3_DAT7		BIT(16)
+#define GPSR4_SD3_DAT6		BIT(15)
+#define GPSR4_SD3_DAT5		BIT(14)
+#define GPSR4_SD3_DAT4		BIT(13)
+#define GPSR4_SD3_DAT3		BIT(12)
+#define GPSR4_SD3_DAT2		BIT(11)
+#define GPSR4_SD3_DAT1		BIT(10)
+#define GPSR4_SD3_DAT0		BIT(9)
+#define GPSR4_SD3_CMD		BIT(8)
+#define GPSR4_SD3_CLK		BIT(7)
+#define GPSR4_SD2_DS		BIT(6)
+#define GPSR4_SD2_DAT3		BIT(5)
+#define GPSR4_SD2_DAT2		BIT(4)
+#define GPSR4_SD2_DAT1		BIT(3)
+#define GPSR4_SD2_DAT0		BIT(2)
+#define GPSR4_SD2_CMD		BIT(1)
+#define GPSR4_SD2_CLK		BIT(0)
+#define GPSR5_MLB_DAT		BIT(25)
+#define GPSR5_MLB_SIG		BIT(24)
+#define GPSR5_MLB_CLK		BIT(23)
+#define GPSR5_MSIOF0_RXD	BIT(22)
+#define GPSR5_MSIOF0_SS2	BIT(21)
+#define GPSR5_MSIOF0_TXD	BIT(20)
+#define GPSR5_MSIOF0_SS1	BIT(19)
+#define GPSR5_MSIOF0_SYNC	BIT(18)
+#define GPSR5_MSIOF0_SCK	BIT(17)
+#define GPSR5_HRTS0		BIT(16)
+#define GPSR5_HCTS0		BIT(15)
+#define GPSR5_HTX0		BIT(14)
+#define GPSR5_HRX0		BIT(13)
+#define GPSR5_HSCK0		BIT(12)
+#define GPSR5_RX2_A		BIT(11)
+#define GPSR5_TX2_A		BIT(10)
+#define GPSR5_SCK2		BIT(9)
+#define GPSR5_RTS1		BIT(8)
+#define GPSR5_CTS1		BIT(7)
+#define GPSR5_TX1_A		BIT(6)
+#define GPSR5_RX1_A		BIT(5)
+#define GPSR5_RTS0		BIT(4)
+#define GPSR5_CTS0		BIT(3)
+#define GPSR5_TX0		BIT(2)
+#define GPSR5_RX0		BIT(1)
+#define GPSR5_SCK0		BIT(0)
+#define GPSR6_USB31_OVC		BIT(31)
+#define GPSR6_USB31_PWEN	BIT(30)
+#define GPSR6_USB30_OVC		BIT(29)
+#define GPSR6_USB30_PWEN	BIT(28)
+#define GPSR6_USB1_OVC		BIT(27)
+#define GPSR6_USB1_PWEN		BIT(26)
+#define GPSR6_USB0_OVC		BIT(25)
+#define GPSR6_USB0_PWEN		BIT(24)
+#define GPSR6_AUDIO_CLKB_B	BIT(23)
+#define GPSR6_AUDIO_CLKA_A	BIT(22)
+#define GPSR6_SSI_SDATA9_A	BIT(21)
+#define GPSR6_SSI_SDATA8	BIT(20)
+#define GPSR6_SSI_SDATA7	BIT(19)
+#define GPSR6_SSI_WS78		BIT(18)
+#define GPSR6_SSI_SCK78		BIT(17)
+#define GPSR6_SSI_SDATA6	BIT(16)
+#define GPSR6_SSI_WS6		BIT(15)
+#define GPSR6_SSI_SCK6		BIT(14)
+#define GPSR6_SSI_SDATA5	BIT(13)
+#define GPSR6_SSI_WS5		BIT(12)
+#define GPSR6_SSI_SCK5		BIT(11)
+#define GPSR6_SSI_SDATA4	BIT(10)
+#define GPSR6_SSI_WS4		BIT(9)
+#define GPSR6_SSI_SCK4		BIT(8)
+#define GPSR6_SSI_SDATA3	BIT(7)
+#define GPSR6_SSI_WS34		BIT(6)
+#define GPSR6_SSI_SCK34		BIT(5)
+#define GPSR6_SSI_SDATA2_A	BIT(4)
+#define GPSR6_SSI_SDATA1_A	BIT(3)
+#define GPSR6_SSI_SDATA0	BIT(2)
+#define GPSR6_SSI_WS0129	BIT(1)
+#define GPSR6_SSI_SCK0129	BIT(0)
+#define GPSR7_AVS2		BIT(1)
+#define GPSR7_AVS1		BIT(0)
+
+#define IPSR_28_FUNC(x)		((uint32_t)(x) << 28U)
+#define IPSR_24_FUNC(x)		((uint32_t)(x) << 24U)
+#define IPSR_20_FUNC(x)		((uint32_t)(x) << 20U)
+#define IPSR_16_FUNC(x)		((uint32_t)(x) << 16U)
+#define IPSR_12_FUNC(x)		((uint32_t)(x) << 12U)
+#define IPSR_8_FUNC(x)		((uint32_t)(x) << 8U)
+#define IPSR_4_FUNC(x)		((uint32_t)(x) << 4U)
+#define IPSR_0_FUNC(x)		((uint32_t)(x) << 0U)
+
+#define POC_SD3_DS_33V		BIT(29)
+#define POC_SD3_DAT7_33V	BIT(28)
+#define POC_SD3_DAT6_33V	BIT(27)
+#define POC_SD3_DAT5_33V	BIT(26)
+#define POC_SD3_DAT4_33V	BIT(25)
+#define POC_SD3_DAT3_33V	BIT(24)
+#define POC_SD3_DAT2_33V	BIT(23)
+#define POC_SD3_DAT1_33V	BIT(22)
+#define POC_SD3_DAT0_33V	BIT(21)
+#define POC_SD3_CMD_33V		BIT(20)
+#define POC_SD3_CLK_33V		BIT(19)
+#define POC_SD2_DS_33V		BIT(18)
+#define POC_SD2_DAT3_33V	BIT(17)
+#define POC_SD2_DAT2_33V	BIT(16)
+#define POC_SD2_DAT1_33V	BIT(15)
+#define POC_SD2_DAT0_33V	BIT(14)
+#define POC_SD2_CMD_33V		BIT(13)
+#define POC_SD2_CLK_33V		BIT(12)
+#define POC_SD1_DAT3_33V	BIT(11)
+#define POC_SD1_DAT2_33V	BIT(10)
+#define POC_SD1_DAT1_33V	BIT(9)
+#define POC_SD1_DAT0_33V	BIT(8)
+#define POC_SD1_CMD_33V		BIT(7)
+#define POC_SD1_CLK_33V		BIT(6)
+#define POC_SD0_DAT3_33V	BIT(5)
+#define POC_SD0_DAT2_33V	BIT(4)
+#define POC_SD0_DAT1_33V	BIT(3)
+#define POC_SD0_DAT0_33V	BIT(2)
+#define POC_SD0_CMD_33V		BIT(1)
+#define POC_SD0_CLK_33V		BIT(0)
+
+#define DRVCTRL0_MASK		(0xCCCCCCCCU)
+#define DRVCTRL1_MASK		(0xCCCCCCC8U)
+#define DRVCTRL2_MASK		(0x88888888U)
+#define DRVCTRL3_MASK		(0x88888888U)
+#define DRVCTRL4_MASK		(0x88888888U)
+#define DRVCTRL5_MASK		(0x88888888U)
+#define DRVCTRL6_MASK		(0x88888888U)
+#define DRVCTRL7_MASK		(0x88888888U)
+#define DRVCTRL8_MASK		(0x88888888U)
+#define DRVCTRL9_MASK		(0x88888888U)
+#define DRVCTRL10_MASK		(0x88888888U)
+#define DRVCTRL11_MASK		(0x888888CCU)
+#define DRVCTRL12_MASK		(0xCCCFFFCFU)
+#define DRVCTRL13_MASK		(0xCC888888U)
+#define DRVCTRL14_MASK		(0x88888888U)
+#define DRVCTRL15_MASK		(0x88888888U)
+#define DRVCTRL16_MASK		(0x88888888U)
+#define DRVCTRL17_MASK		(0x88888888U)
+#define DRVCTRL18_MASK		(0x88888888U)
+#define DRVCTRL19_MASK		(0x88888888U)
+#define DRVCTRL20_MASK		(0x88888888U)
+#define DRVCTRL21_MASK		(0x88888888U)
+#define DRVCTRL22_MASK		(0x88888888U)
+#define DRVCTRL23_MASK		(0x88888888U)
+#define DRVCTRL24_MASK		(0x8888888FU)
+
+#define DRVCTRL0_QSPI0_SPCLK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL0_QSPI0_MOSI_IO0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL0_QSPI0_MISO_IO1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL0_QSPI0_IO2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL0_QSPI0_IO3(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL0_QSPI0_SSL(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL0_QSPI1_SPCLK(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL0_QSPI1_MOSI_IO0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL1_QSPI1_MISO_IO1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL1_QSPI1_IO2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL1_QSPI1_IO3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL1_QSPI1_SS(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL1_RPC_INT(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL1_RPC_WP(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL1_RPC_RESET(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL1_AVB_RX_CTL(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL2_AVB_RXC(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL2_AVB_RD0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL2_AVB_RD1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL2_AVB_RD2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL2_AVB_RD3(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL2_AVB_TX_CTL(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL2_AVB_TXC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL2_AVB_TD0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL3_AVB_TD1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL3_AVB_TD2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL3_AVB_TD3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL3_AVB_TXCREFCLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL3_AVB_MDIO(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL3_AVB_MDC(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL3_AVB_MAGIC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL3_AVB_PHY_INT(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL4_AVB_LINK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL4_AVB_AVTP_MATCH(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL4_AVB_AVTP_CAPTURE(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL4_IRQ0(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL4_IRQ1(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL4_IRQ2(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL4_IRQ3(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL4_IRQ4(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL5_IRQ5(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL5_PWM0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL5_PWM1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL5_PWM2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL5_A0(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL5_A1(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL5_A2(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL5_A3(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL6_A4(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL6_A5(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL6_A6(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL6_A7(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL6_A8(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL6_A9(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL6_A10(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL6_A11(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL7_A12(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL7_A13(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL7_A14(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL7_A15(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL7_A16(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL7_A17(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL7_A18(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL7_A19(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL8_CLKOUT(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL8_CS0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL8_CS1_A2(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL8_BS(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL8_RD(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL8_RD_W(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL8_WE0(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL8_WE1(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL9_EX_WAIT0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL9_PRESETOU(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL9_D0(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL9_D1(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL9_D2(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL9_D3(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL9_D4(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL9_D5(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL10_D6(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL10_D7(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL10_D8(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL10_D9(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL10_D10(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL10_D11(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL10_D12(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL10_D13(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL11_D14(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL11_D15(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL11_AVS1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL11_AVS2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL11_GP7_02(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL11_GP7_03(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL11_DU_DOTCLKIN0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL11_DU_DOTCLKIN1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL12_DU_DOTCLKIN2(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL12_DU_DOTCLKIN3(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL12_DU_FSCLKST(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL12_DU_TMS(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL13_TDO(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL13_ASEBRK(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL13_SD0_CLK(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL13_SD0_CMD(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL13_SD0_DAT0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL13_SD0_DAT1(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL13_SD0_DAT2(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL13_SD0_DAT3(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL14_SD1_CLK(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL14_SD1_CMD(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL14_SD1_DAT0(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL14_SD1_DAT1(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL14_SD1_DAT2(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL14_SD1_DAT3(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL14_SD2_CLK(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL14_SD2_CMD(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL15_SD2_DAT0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL15_SD2_DAT1(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL15_SD2_DAT2(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL15_SD2_DAT3(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL15_SD2_DS(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL15_SD3_CLK(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL15_SD3_CMD(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL15_SD3_DAT0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL16_SD3_DAT1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL16_SD3_DAT2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL16_SD3_DAT3(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL16_SD3_DAT4(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL16_SD3_DAT5(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL16_SD3_DAT6(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL16_SD3_DAT7(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL16_SD3_DS(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL17_SD0_CD(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL17_SD0_WP(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL17_SD1_CD(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL17_SD1_WP(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL17_SCK0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL17_RX0(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL17_TX0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL17_CTS0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL18_RTS0_TANS(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL18_RX1(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL18_TX1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL18_CTS1(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL18_RTS1_TANS(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL18_SCK2(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL18_TX2(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL18_RX2(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL19_HSCK0(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL19_HRX0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL19_HTX0(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL19_HCTS0(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL19_HRTS0(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL19_MSIOF0_SCK(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL19_MSIOF0_SYNC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL19_MSIOF0_SS1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL20_MSIOF0_TXD(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL20_MSIOF0_SS2(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL20_MSIOF0_RXD(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL20_MLB_CLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL20_MLB_SIG(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL20_MLB_DAT(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL20_MLB_REF(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL20_SSI_SCK0129(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL21_SSI_WS0129(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL21_SSI_SDATA0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL21_SSI_SDATA1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL21_SSI_SDATA2(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL21_SSI_SCK34(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL21_SSI_WS34(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL21_SSI_SDATA3(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL21_SSI_SCK4(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL22_SSI_WS4(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL22_SSI_SDATA4(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL22_SSI_SCK5(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL22_SSI_WS5(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL22_SSI_SDATA5(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL22_SSI_SCK6(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL22_SSI_WS6(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL22_SSI_SDATA6(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL23_SSI_SCK78(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL23_SSI_WS78(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL23_SSI_SDATA7(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL23_SSI_SDATA8(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL23_SSI_SDATA9(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL23_AUDIO_CLKA(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL23_AUDIO_CLKB(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL23_USB0_PWEN(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL24_USB0_OVC(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL24_USB1_PWEN(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL24_USB1_OVC(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL24_USB30_PWEN(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL24_USB30_OVC(x)	((uint32_t)(x) << 12U)
+#define DRVCTRL24_USB31_PWEN(x)	((uint32_t)(x) << 8U)
+#define DRVCTRL24_USB31_OVC(x)	((uint32_t)(x) << 4U)
+
+#define MOD_SEL0_MSIOF3_A	((uint32_t)0U << 29U)
+#define MOD_SEL0_MSIOF3_B	((uint32_t)1U << 29U)
+#define MOD_SEL0_MSIOF3_C	((uint32_t)2U << 29U)
+#define MOD_SEL0_MSIOF3_D	((uint32_t)3U << 29U)
+#define MOD_SEL0_MSIOF3_E	((uint32_t)4U << 29U)
+#define MOD_SEL0_MSIOF2_A	((uint32_t)0U << 27U)
+#define MOD_SEL0_MSIOF2_B	((uint32_t)1U << 27U)
+#define MOD_SEL0_MSIOF2_C	((uint32_t)2U << 27U)
+#define MOD_SEL0_MSIOF2_D	((uint32_t)3U << 27U)
+#define MOD_SEL0_MSIOF1_A	((uint32_t)0U << 24U)
+#define MOD_SEL0_MSIOF1_B	((uint32_t)1U << 24U)
+#define MOD_SEL0_MSIOF1_C	((uint32_t)2U << 24U)
+#define MOD_SEL0_MSIOF1_D	((uint32_t)3U << 24U)
+#define MOD_SEL0_MSIOF1_E	((uint32_t)4U << 24U)
+#define MOD_SEL0_MSIOF1_F	((uint32_t)5U << 24U)
+#define MOD_SEL0_MSIOF1_G	((uint32_t)6U << 24U)
+#define MOD_SEL0_LBSC_A		((uint32_t)0U << 23U)
+#define MOD_SEL0_LBSC_B		((uint32_t)1U << 23U)
+#define MOD_SEL0_IEBUS_A	((uint32_t)0U << 22U)
+#define MOD_SEL0_IEBUS_B	((uint32_t)1U << 22U)
+#define MOD_SEL0_I2C2_A		((uint32_t)0U << 21U)
+#define MOD_SEL0_I2C2_B		((uint32_t)1U << 21U)
+#define MOD_SEL0_I2C1_A		((uint32_t)0U << 20U)
+#define MOD_SEL0_I2C1_B		((uint32_t)1U << 20U)
+#define MOD_SEL0_HSCIF4_A	((uint32_t)0U << 19U)
+#define MOD_SEL0_HSCIF4_B	((uint32_t)1U << 19U)
+#define MOD_SEL0_HSCIF3_A	((uint32_t)0U << 17U)
+#define MOD_SEL0_HSCIF3_B	((uint32_t)1U << 17U)
+#define MOD_SEL0_HSCIF3_C	((uint32_t)2U << 17U)
+#define MOD_SEL0_HSCIF3_D	((uint32_t)3U << 17U)
+#define MOD_SEL0_HSCIF1_A	((uint32_t)0U << 16U)
+#define MOD_SEL0_HSCIF1_B	((uint32_t)1U << 16U)
+#define MOD_SEL0_FSO_A		((uint32_t)0U << 15U)
+#define MOD_SEL0_FSO_B		((uint32_t)1U << 15U)
+#define MOD_SEL0_HSCIF2_A	((uint32_t)0U << 13U)
+#define MOD_SEL0_HSCIF2_B	((uint32_t)1U << 13U)
+#define MOD_SEL0_HSCIF2_C	((uint32_t)2U << 13U)
+#define MOD_SEL0_ETHERAVB_A	((uint32_t)0U << 12U)
+#define MOD_SEL0_ETHERAVB_B	((uint32_t)1U << 12U)
+#define MOD_SEL0_DRIF3_A	((uint32_t)0U << 11U)
+#define MOD_SEL0_DRIF3_B	((uint32_t)1U << 11U)
+#define MOD_SEL0_DRIF2_A	((uint32_t)0U << 10U)
+#define MOD_SEL0_DRIF2_B	((uint32_t)1U << 10U)
+#define MOD_SEL0_DRIF1_A	((uint32_t)0U << 8U)
+#define MOD_SEL0_DRIF1_B	((uint32_t)1U << 8U)
+#define MOD_SEL0_DRIF1_C	((uint32_t)2U << 8U)
+#define MOD_SEL0_DRIF0_A	((uint32_t)0U << 6U)
+#define MOD_SEL0_DRIF0_B	((uint32_t)1U << 6U)
+#define MOD_SEL0_DRIF0_C	((uint32_t)2U << 6U)
+#define MOD_SEL0_CANFD0_A	((uint32_t)0U << 5U)
+#define MOD_SEL0_CANFD0_B	((uint32_t)1U << 5U)
+#define MOD_SEL0_ADG_A_A	((uint32_t)0U << 3U)
+#define MOD_SEL0_ADG_A_B	((uint32_t)1U << 3U)
+#define MOD_SEL0_ADG_A_C	((uint32_t)2U << 3U)
+#define MOD_SEL1_TSIF1_A	((uint32_t)0U << 30U)
+#define MOD_SEL1_TSIF1_B	((uint32_t)1U << 30U)
+#define MOD_SEL1_TSIF1_C	((uint32_t)2U << 30U)
+#define MOD_SEL1_TSIF1_D	((uint32_t)3U << 30U)
+#define MOD_SEL1_TSIF0_A	((uint32_t)0U << 27U)
+#define MOD_SEL1_TSIF0_B	((uint32_t)1U << 27U)
+#define MOD_SEL1_TSIF0_C	((uint32_t)2U << 27U)
+#define MOD_SEL1_TSIF0_D	((uint32_t)3U << 27U)
+#define MOD_SEL1_TSIF0_E	((uint32_t)4U << 27U)
+#define MOD_SEL1_TIMER_TMU_A	((uint32_t)0U << 26U)
+#define MOD_SEL1_TIMER_TMU_B	((uint32_t)1U << 26U)
+#define MOD_SEL1_SSP1_1_A	((uint32_t)0U << 24U)
+#define MOD_SEL1_SSP1_1_B	((uint32_t)1U << 24U)
+#define MOD_SEL1_SSP1_1_C	((uint32_t)2U << 24U)
+#define MOD_SEL1_SSP1_1_D	((uint32_t)3U << 24U)
+#define MOD_SEL1_SSP1_0_A	((uint32_t)0U << 21U)
+#define MOD_SEL1_SSP1_0_B	((uint32_t)1U << 21U)
+#define MOD_SEL1_SSP1_0_C	((uint32_t)2U << 21U)
+#define MOD_SEL1_SSP1_0_D	((uint32_t)3U << 21U)
+#define MOD_SEL1_SSP1_0_E	((uint32_t)4U << 21U)
+#define MOD_SEL1_SSI_A		((uint32_t)0U << 20U)
+#define MOD_SEL1_SSI_B		((uint32_t)1U << 20U)
+#define MOD_SEL1_SPEED_PULSE_IF_A	((uint32_t)0U << 19U)
+#define MOD_SEL1_SPEED_PULSE_IF_B	((uint32_t)1U << 19U)
+#define MOD_SEL1_SIMCARD_A	((uint32_t)0U << 17U)
+#define MOD_SEL1_SIMCARD_B	((uint32_t)1U << 17U)
+#define MOD_SEL1_SIMCARD_C	((uint32_t)2U << 17U)
+#define MOD_SEL1_SIMCARD_D	((uint32_t)3U << 17U)
+#define MOD_SEL1_SDHI2_A	((uint32_t)0U << 16U)
+#define MOD_SEL1_SDHI2_B	((uint32_t)1U << 16U)
+#define MOD_SEL1_SCIF4_A	((uint32_t)0U << 14U)
+#define MOD_SEL1_SCIF4_B	((uint32_t)1U << 14U)
+#define MOD_SEL1_SCIF4_C	((uint32_t)2U << 14U)
+#define MOD_SEL1_SCIF3_A	((uint32_t)0U << 13U)
+#define MOD_SEL1_SCIF3_B	((uint32_t)1U << 13U)
+#define MOD_SEL1_SCIF2_A	((uint32_t)0U << 12U)
+#define MOD_SEL1_SCIF2_B	((uint32_t)1U << 12U)
+#define MOD_SEL1_SCIF1_A	((uint32_t)0U << 11U)
+#define MOD_SEL1_SCIF1_B	((uint32_t)1U << 11U)
+#define MOD_SEL1_SCIF_A		((uint32_t)0U << 10U)
+#define MOD_SEL1_SCIF_B		((uint32_t)1U << 10U)
+#define MOD_SEL1_REMOCON_A	((uint32_t)0U << 9U)
+#define MOD_SEL1_REMOCON_B	((uint32_t)1U << 9U)
+#define MOD_SEL1_RCAN0_A	((uint32_t)0U << 6U)
+#define MOD_SEL1_RCAN0_B	((uint32_t)1U << 6U)
+#define MOD_SEL1_PWM6_A		((uint32_t)0U << 5U)
+#define MOD_SEL1_PWM6_B		((uint32_t)1U << 5U)
+#define MOD_SEL1_PWM5_A		((uint32_t)0U << 4U)
+#define MOD_SEL1_PWM5_B		((uint32_t)1U << 4U)
+#define MOD_SEL1_PWM4_A		((uint32_t)0U << 3U)
+#define MOD_SEL1_PWM4_B		((uint32_t)1U << 3U)
+#define MOD_SEL1_PWM3_A		((uint32_t)0U << 2U)
+#define MOD_SEL1_PWM3_B		((uint32_t)1U << 2U)
+#define MOD_SEL1_PWM2_A		((uint32_t)0U << 1U)
+#define MOD_SEL1_PWM2_B		((uint32_t)1U << 1U)
+#define MOD_SEL1_PWM1_A		((uint32_t)0U << 0U)
+#define MOD_SEL1_PWM1_B		((uint32_t)1U << 0U)
+#define MOD_SEL2_I2C_5_A	((uint32_t)0U << 31U)
+#define MOD_SEL2_I2C_5_B	((uint32_t)1U << 31U)
+#define MOD_SEL2_I2C_3_A	((uint32_t)0U << 30U)
+#define MOD_SEL2_I2C_3_B	((uint32_t)1U << 30U)
+#define MOD_SEL2_I2C_0_A	((uint32_t)0U << 29U)
+#define MOD_SEL2_I2C_0_B	((uint32_t)1U << 29U)
+#define MOD_SEL2_FM_A		((uint32_t)0U << 27U)
+#define MOD_SEL2_FM_B		((uint32_t)1U << 27U)
+#define MOD_SEL2_FM_C		((uint32_t)2U << 27U)
+#define MOD_SEL2_FM_D		((uint32_t)3U << 27U)
+#define MOD_SEL2_SCIF5_A	((uint32_t)0U << 26U)
+#define MOD_SEL2_SCIF5_B	((uint32_t)1U << 26U)
+#define MOD_SEL2_I2C6_A		((uint32_t)0U << 23U)
+#define MOD_SEL2_I2C6_B		((uint32_t)1U << 23U)
+#define MOD_SEL2_I2C6_C		((uint32_t)2U << 23U)
+#define MOD_SEL2_NDF_A		((uint32_t)0U << 22U)
+#define MOD_SEL2_NDF_B		((uint32_t)1U << 22U)
+#define MOD_SEL2_SSI2_A		((uint32_t)0U << 21U)
+#define MOD_SEL2_SSI2_B		((uint32_t)1U << 21U)
+#define MOD_SEL2_SSI9_A		((uint32_t)0U << 20U)
+#define MOD_SEL2_SSI9_B		((uint32_t)1U << 20U)
+#define MOD_SEL2_TIMER_TMU2_A	((uint32_t)0U << 19U)
+#define MOD_SEL2_TIMER_TMU2_B	((uint32_t)1U << 19U)
+#define MOD_SEL2_ADG_B_A	((uint32_t)0U << 18U)
+#define MOD_SEL2_ADG_B_B	((uint32_t)1U << 18U)
+#define MOD_SEL2_ADG_C_A	((uint32_t)0U << 17U)
+#define MOD_SEL2_ADG_C_B	((uint32_t)1U << 17U)
+#define MOD_SEL2_VIN4_A		((uint32_t)0U << 0U)
+#define MOD_SEL2_VIN4_B		((uint32_t)1U << 0U)
+
+/* SCIF3 Registers for Dummy write */
+#define SCIF3_BASE		(0xE6C50000U)
+#define SCIF3_SCFCR		(SCIF3_BASE + 0x0018U)
+#define SCIF3_SCFDR		(SCIF3_BASE + 0x001CU)
+#define SCFCR_DATA		(0x0000U)
+
+/* Realtime module stop control */
+#define CPG_BASE		(0xE6150000U)
+#define CPG_SCMSTPCR0		(CPG_BASE + 0x0B20U)
+#define CPG_MSTPSR0		(CPG_BASE + 0x0030U)
+#define SCMSTPCR0_RTDMAC	(0x00200000U)
+
+/* RT-DMAC Registers */
+#define RTDMAC_CH		(0U)	/* choose 0 to 15 */
+
+#define RTDMAC_BASE		(0xFFC10000U)
+#define RTDMAC_RDMOR		(RTDMAC_BASE + 0x0060U)
+#define RTDMAC_RDMCHCLR		(RTDMAC_BASE + 0x0080U)
+#define RTDMAC_RDMSAR(x)	(RTDMAC_BASE + 0x8000U + (0x80U * (x)))
+#define RTDMAC_RDMDAR(x)	(RTDMAC_BASE + 0x8004U + (0x80U * (x)))
+#define RTDMAC_RDMTCR(x)	(RTDMAC_BASE + 0x8008U + (0x80U * (x)))
+#define RTDMAC_RDMCHCR(x)	(RTDMAC_BASE + 0x800CU + (0x80U * (x)))
+#define RTDMAC_RDMCHCRB(x)	(RTDMAC_BASE + 0x801CU + (0x80U * (x)))
+#define RTDMAC_RDMDPBASE(x)	(RTDMAC_BASE + 0x8050U + (0x80U * (x)))
+#define RTDMAC_DESC_BASE	(RTDMAC_BASE + 0xA000U)
+#define RTDMAC_DESC_RDMSAR	(RTDMAC_DESC_BASE + 0x0000U)
+#define RTDMAC_DESC_RDMDAR	(RTDMAC_DESC_BASE + 0x0004U)
+#define RTDMAC_DESC_RDMTCR	(RTDMAC_DESC_BASE + 0x0008U)
+
+#define RDMOR_DME		(0x0001U)	/* DMA Master Enable */
+#define RDMCHCR_DPM_INFINITE	(0x30000000U)	/* Infinite repeat mode */
+#define RDMCHCR_RPT_TCR		(0x02000000U)	/* enable to update TCR */
+#define RDMCHCR_TS_2		(0x00000008U)	/* Word(2byte) units transfer */
+#define RDMCHCR_RS_AUTO		(0x00000400U)	/* Auto request */
+#define RDMCHCR_DE		(0x00000001U)	/* DMA Enable */
+#define RDMCHCRB_DRST		(0x00008000U)	/* Descriptor reset */
+#define RDMCHCRB_SLM_256	(0x00000080U)	/* once in 256 clock cycle */
+#define RDMDPBASE_SEL_EXT	(0x00000001U)	/* External memory use */
+
+static void start_rtdma0_descriptor(void)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(RCAR_PRR);
+	reg &= (PRR_PRODUCT_MASK | PRR_CUT_MASK);
+	if (reg == (PRR_PRODUCT_M3_CUT10)) {
+		/* Enable clock supply to RTDMAC. */
+		mstpcr_write(CPG_SCMSTPCR0, CPG_MSTPSR0, SCMSTPCR0_RTDMAC);
+
+		/* Initialize ch0, Reset Descriptor */
+		mmio_write_32(RTDMAC_RDMCHCLR, BIT(RTDMAC_CH));
+		mmio_write_32(RTDMAC_RDMCHCRB(RTDMAC_CH), RDMCHCRB_DRST);
+
+		/* Enable DMA */
+		mmio_write_16(RTDMAC_RDMOR, RDMOR_DME);
+
+		/* Set first transfer */
+		mmio_write_32(RTDMAC_RDMSAR(RTDMAC_CH), RCAR_PRR);
+		mmio_write_32(RTDMAC_RDMDAR(RTDMAC_CH), SCIF3_SCFDR);
+		mmio_write_32(RTDMAC_RDMTCR(RTDMAC_CH), 0x00000001U);
+
+		/* Set descriptor */
+		mmio_write_32(RTDMAC_DESC_RDMSAR, 0x00000000U);
+		mmio_write_32(RTDMAC_DESC_RDMDAR, 0x00000000U);
+		mmio_write_32(RTDMAC_DESC_RDMTCR, 0x00200000U);
+		mmio_write_32(RTDMAC_RDMCHCRB(RTDMAC_CH), RDMCHCRB_SLM_256);
+		mmio_write_32(RTDMAC_RDMDPBASE(RTDMAC_CH), RTDMAC_DESC_BASE
+			      | RDMDPBASE_SEL_EXT);
+
+		/* Set transfer parameter, Start transfer */
+		mmio_write_32(RTDMAC_RDMCHCR(RTDMAC_CH), RDMCHCR_DPM_INFINITE
+			      | RDMCHCR_RPT_TCR
+			      | RDMCHCR_TS_2
+			      | RDMCHCR_RS_AUTO
+			      | RDMCHCR_DE);
+	}
+}
+
+static void pfc_reg_write(uint32_t addr, uint32_t data)
+{
+	uint32_t prr;
+
+	prr = mmio_read_32(RCAR_PRR);
+	prr &= (PRR_PRODUCT_MASK | PRR_CUT_MASK);
+
+	mmio_write_32(PFC_PMMR, ~data);
+	if (prr == (PRR_PRODUCT_M3_CUT10)) {
+		mmio_write_16(SCIF3_SCFCR, SCFCR_DATA);	/* Dummy write */
+	}
+	mmio_write_32((uintptr_t)addr, data);
+	if (prr == (PRR_PRODUCT_M3_CUT10)) {
+		mmio_write_16(SCIF3_SCFCR, SCFCR_DATA);	/* Dummy write */
+	}
+}
+
+void pfc_init_g2m(void)
+{
+	uint32_t reg;
+
+	/*
+	 * PFC write access problem seen on older SoC's. Added a workaround
+	 * in RT-DMAC for fixing the same.
+	 */
+	start_rtdma0_descriptor();
+
+	/* initialize module select */
+	pfc_reg_write(PFC_MOD_SEL0, MOD_SEL0_MSIOF3_A
+		      | MOD_SEL0_MSIOF2_A
+		      | MOD_SEL0_MSIOF1_A
+		      | MOD_SEL0_LBSC_A
+		      | MOD_SEL0_IEBUS_A
+		      | MOD_SEL0_I2C2_A
+		      | MOD_SEL0_I2C1_A
+		      | MOD_SEL0_HSCIF4_A
+		      | MOD_SEL0_HSCIF3_A
+		      | MOD_SEL0_HSCIF1_A
+		      | MOD_SEL0_FSO_A
+		      | MOD_SEL0_HSCIF2_A
+		      | MOD_SEL0_ETHERAVB_A
+		      | MOD_SEL0_DRIF3_A
+		      | MOD_SEL0_DRIF2_A
+		      | MOD_SEL0_DRIF1_A
+		      | MOD_SEL0_DRIF0_A
+		      | MOD_SEL0_CANFD0_A
+		      | MOD_SEL0_ADG_A_A);
+	pfc_reg_write(PFC_MOD_SEL1, MOD_SEL1_TSIF1_A
+		      | MOD_SEL1_TSIF0_A
+		      | MOD_SEL1_TIMER_TMU_A
+		      | MOD_SEL1_SSP1_1_A
+		      | MOD_SEL1_SSP1_0_A
+		      | MOD_SEL1_SSI_A
+		      | MOD_SEL1_SPEED_PULSE_IF_A
+		      | MOD_SEL1_SIMCARD_A
+		      | MOD_SEL1_SDHI2_A
+		      | MOD_SEL1_SCIF4_A
+		      | MOD_SEL1_SCIF3_A
+		      | MOD_SEL1_SCIF2_A
+		      | MOD_SEL1_SCIF1_A
+		      | MOD_SEL1_SCIF_A
+		      | MOD_SEL1_REMOCON_A
+		      | MOD_SEL1_RCAN0_A
+		      | MOD_SEL1_PWM6_A
+		      | MOD_SEL1_PWM5_A
+		      | MOD_SEL1_PWM4_A
+		      | MOD_SEL1_PWM3_A
+		      | MOD_SEL1_PWM2_A
+		      | MOD_SEL1_PWM1_A);
+	pfc_reg_write(PFC_MOD_SEL2, MOD_SEL2_I2C_5_B
+		      | MOD_SEL2_I2C_3_B
+		      | MOD_SEL2_I2C_0_B
+		      | MOD_SEL2_FM_A
+		      | MOD_SEL2_SCIF5_A
+		      | MOD_SEL2_I2C6_A
+		      | MOD_SEL2_NDF_A
+		      | MOD_SEL2_SSI2_A
+		      | MOD_SEL2_SSI9_A
+		      | MOD_SEL2_TIMER_TMU2_A
+		      | MOD_SEL2_ADG_B_A
+		      | MOD_SEL2_ADG_C_A
+		      | MOD_SEL2_VIN4_A);
+
+	/* initialize peripheral function select */
+	pfc_reg_write(PFC_IPSR0, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR1, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(3)
+		      | IPSR_8_FUNC(3)
+		      | IPSR_4_FUNC(3)
+		      | IPSR_0_FUNC(3));
+	pfc_reg_write(PFC_IPSR2, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR3, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR4, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR5, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR6, IPSR_28_FUNC(6)
+		      | IPSR_24_FUNC(6)
+		      | IPSR_20_FUNC(6)
+		      | IPSR_16_FUNC(6)
+		      | IPSR_12_FUNC(6)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR7, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(6)
+		      | IPSR_4_FUNC(6)
+		      | IPSR_0_FUNC(6));
+	pfc_reg_write(PFC_IPSR8, IPSR_28_FUNC(1)
+		      | IPSR_24_FUNC(1)
+		      | IPSR_20_FUNC(1)
+		      | IPSR_16_FUNC(1)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR9, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR10, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR11, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(4)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR12, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(4)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR13, IPSR_28_FUNC(8)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(3)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR14, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(3)
+		      | IPSR_0_FUNC(8));
+	pfc_reg_write(PFC_IPSR15, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR16, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR17, IPSR_28_FUNC(0)
+		      | IPSR_24_FUNC(0)
+		      | IPSR_20_FUNC(0)
+		      | IPSR_16_FUNC(0)
+		      | IPSR_12_FUNC(0)
+		      | IPSR_8_FUNC(0)
+		      | IPSR_4_FUNC(1)
+		      | IPSR_0_FUNC(0));
+	pfc_reg_write(PFC_IPSR18, IPSR_4_FUNC(0)
+		      | IPSR_0_FUNC(0));
+
+	/* initialize GPIO/perihperal function select */
+	pfc_reg_write(PFC_GPSR0, GPSR0_D15
+		      | GPSR0_D14
+		      | GPSR0_D13
+		      | GPSR0_D12
+		      | GPSR0_D11
+		      | GPSR0_D10
+		      | GPSR0_D9
+		      | GPSR0_D8
+		      | GPSR0_D7
+		      | GPSR0_D6
+		      | GPSR0_D5
+		      | GPSR0_D4
+		      | GPSR0_D3
+		      | GPSR0_D2
+		      | GPSR0_D0);
+	pfc_reg_write(PFC_GPSR1, GPSR1_CLKOUT
+		      | GPSR1_EX_WAIT0_A
+		      | GPSR1_WE1
+		      | GPSR1_RD
+		      | GPSR1_RD_WR
+		      | GPSR1_CS0
+		      | GPSR1_A19
+		      | GPSR1_A18
+		      | GPSR1_A17
+		      | GPSR1_A16
+		      | GPSR1_A15
+		      | GPSR1_A14
+		      | GPSR1_A13
+		      | GPSR1_A12
+		      | GPSR1_A7
+		      | GPSR1_A6
+		      | GPSR1_A5
+		      | GPSR1_A4
+		      | GPSR1_A3
+		      | GPSR1_A2
+		      | GPSR1_A1
+		      | GPSR1_A0);
+	pfc_reg_write(PFC_GPSR2, GPSR2_AVB_AVTP_CAPTURE_A
+		      | GPSR2_AVB_AVTP_MATCH_A
+		      | GPSR2_AVB_LINK
+		      | GPSR2_AVB_PHY_INT
+		      | GPSR2_AVB_MDC
+		      | GPSR2_PWM2_A
+		      | GPSR2_PWM1_A
+		      | GPSR2_IRQ4
+		      | GPSR2_IRQ3
+		      | GPSR2_IRQ2
+		      | GPSR2_IRQ1
+		      | GPSR2_IRQ0);
+	pfc_reg_write(PFC_GPSR3, GPSR3_SD0_CD
+		      | GPSR3_SD1_DAT3
+		      | GPSR3_SD1_DAT2
+		      | GPSR3_SD1_DAT1
+		      | GPSR3_SD1_DAT0
+		      | GPSR3_SD0_DAT3
+		      | GPSR3_SD0_DAT2
+		      | GPSR3_SD0_DAT1
+		      | GPSR3_SD0_DAT0
+		      | GPSR3_SD0_CMD
+		      | GPSR3_SD0_CLK);
+	pfc_reg_write(PFC_GPSR4, GPSR4_SD3_DS
+		      | GPSR4_SD3_DAT7
+		      | GPSR4_SD3_DAT6
+		      | GPSR4_SD3_DAT5
+		      | GPSR4_SD3_DAT4
+		      | GPSR4_SD3_DAT3
+		      | GPSR4_SD3_DAT2
+		      | GPSR4_SD3_DAT1
+		      | GPSR4_SD3_DAT0
+		      | GPSR4_SD3_CMD
+		      | GPSR4_SD3_CLK
+		      | GPSR4_SD2_DAT3
+		      | GPSR4_SD2_DAT2
+		      | GPSR4_SD2_DAT1
+		      | GPSR4_SD2_DAT0
+		      | GPSR4_SD2_CMD
+		      | GPSR4_SD2_CLK);
+	pfc_reg_write(PFC_GPSR5, GPSR5_MSIOF0_RXD
+		      | GPSR5_MSIOF0_TXD
+		      | GPSR5_MSIOF0_SYNC
+		      | GPSR5_MSIOF0_SCK
+		      | GPSR5_RX2_A
+		      | GPSR5_TX2_A
+		      | GPSR5_RTS1
+		      | GPSR5_CTS1
+		      | GPSR5_TX1_A
+		      | GPSR5_RX1_A
+		      | GPSR5_RTS0
+		      | GPSR5_SCK0);
+	pfc_reg_write(PFC_GPSR6, GPSR6_AUDIO_CLKB_B
+		      | GPSR6_AUDIO_CLKA_A
+		      | GPSR6_SSI_WS6
+		      | GPSR6_SSI_SCK6
+		      | GPSR6_SSI_SDATA4
+		      | GPSR6_SSI_WS4
+		      | GPSR6_SSI_SCK4
+		      | GPSR6_SSI_SDATA1_A
+		      | GPSR6_SSI_SDATA0
+		      | GPSR6_SSI_WS0129
+		      | GPSR6_SSI_SCK0129);
+	pfc_reg_write(PFC_GPSR7, GPSR7_AVS2
+		      | GPSR7_AVS1);
+
+	/* initialize POC control register */
+	pfc_reg_write(PFC_POCCTRL0, POC_SD0_DAT3_33V
+		      | POC_SD0_DAT2_33V
+		      | POC_SD0_DAT1_33V
+		      | POC_SD0_DAT0_33V
+		      | POC_SD0_CMD_33V
+		      | POC_SD0_CLK_33V);
+
+	/* initialize DRV control register */
+	reg = mmio_read_32(PFC_DRVCTRL0);
+	reg = ((reg & DRVCTRL0_MASK) | DRVCTRL0_QSPI0_SPCLK(3)
+		      | DRVCTRL0_QSPI0_MOSI_IO0(3)
+		      | DRVCTRL0_QSPI0_MISO_IO1(3)
+		      | DRVCTRL0_QSPI0_IO2(3)
+		      | DRVCTRL0_QSPI0_IO3(3)
+		      | DRVCTRL0_QSPI0_SSL(3)
+		      | DRVCTRL0_QSPI1_SPCLK(3)
+		      | DRVCTRL0_QSPI1_MOSI_IO0(3));
+	pfc_reg_write(PFC_DRVCTRL0, reg);
+	reg = mmio_read_32(PFC_DRVCTRL1);
+	reg = ((reg & DRVCTRL1_MASK) | DRVCTRL1_QSPI1_MISO_IO1(3)
+		      | DRVCTRL1_QSPI1_IO2(3)
+		      | DRVCTRL1_QSPI1_IO3(3)
+		      | DRVCTRL1_QSPI1_SS(3)
+		      | DRVCTRL1_RPC_INT(3)
+		      | DRVCTRL1_RPC_WP(3)
+		      | DRVCTRL1_RPC_RESET(3)
+		      | DRVCTRL1_AVB_RX_CTL(7));
+	pfc_reg_write(PFC_DRVCTRL1, reg);
+	reg = mmio_read_32(PFC_DRVCTRL2);
+	reg = ((reg & DRVCTRL2_MASK) | DRVCTRL2_AVB_RXC(7)
+		      | DRVCTRL2_AVB_RD0(7)
+		      | DRVCTRL2_AVB_RD1(7)
+		      | DRVCTRL2_AVB_RD2(7)
+		      | DRVCTRL2_AVB_RD3(7)
+		      | DRVCTRL2_AVB_TX_CTL(3)
+		      | DRVCTRL2_AVB_TXC(3)
+		      | DRVCTRL2_AVB_TD0(3));
+	pfc_reg_write(PFC_DRVCTRL2, reg);
+	reg = mmio_read_32(PFC_DRVCTRL3);
+	reg = ((reg & DRVCTRL3_MASK) | DRVCTRL3_AVB_TD1(3)
+		      | DRVCTRL3_AVB_TD2(3)
+		      | DRVCTRL3_AVB_TD3(3)
+		      | DRVCTRL3_AVB_TXCREFCLK(7)
+		      | DRVCTRL3_AVB_MDIO(7)
+		      | DRVCTRL3_AVB_MDC(7)
+		      | DRVCTRL3_AVB_MAGIC(7)
+		      | DRVCTRL3_AVB_PHY_INT(7));
+	pfc_reg_write(PFC_DRVCTRL3, reg);
+	reg = mmio_read_32(PFC_DRVCTRL4);
+	reg = ((reg & DRVCTRL4_MASK) | DRVCTRL4_AVB_LINK(7)
+		      | DRVCTRL4_AVB_AVTP_MATCH(7)
+		      | DRVCTRL4_AVB_AVTP_CAPTURE(7)
+		      | DRVCTRL4_IRQ0(7)
+		      | DRVCTRL4_IRQ1(7)
+		      | DRVCTRL4_IRQ2(7)
+		      | DRVCTRL4_IRQ3(7)
+		      | DRVCTRL4_IRQ4(7));
+	pfc_reg_write(PFC_DRVCTRL4, reg);
+	reg = mmio_read_32(PFC_DRVCTRL5);
+	reg = ((reg & DRVCTRL5_MASK) | DRVCTRL5_IRQ5(7)
+		      | DRVCTRL5_PWM0(7)
+		      | DRVCTRL5_PWM1(7)
+		      | DRVCTRL5_PWM2(7)
+		      | DRVCTRL5_A0(3)
+		      | DRVCTRL5_A1(3)
+		      | DRVCTRL5_A2(3)
+		      | DRVCTRL5_A3(3));
+	pfc_reg_write(PFC_DRVCTRL5, reg);
+	reg = mmio_read_32(PFC_DRVCTRL6);
+	reg = ((reg & DRVCTRL6_MASK) | DRVCTRL6_A4(3)
+		      | DRVCTRL6_A5(3)
+		      | DRVCTRL6_A6(3)
+		      | DRVCTRL6_A7(3)
+		      | DRVCTRL6_A8(7)
+		      | DRVCTRL6_A9(7)
+		      | DRVCTRL6_A10(7)
+		      | DRVCTRL6_A11(7));
+	pfc_reg_write(PFC_DRVCTRL6, reg);
+	reg = mmio_read_32(PFC_DRVCTRL7);
+	reg = ((reg & DRVCTRL7_MASK) | DRVCTRL7_A12(3)
+		      | DRVCTRL7_A13(3)
+		      | DRVCTRL7_A14(3)
+		      | DRVCTRL7_A15(3)
+		      | DRVCTRL7_A16(3)
+		      | DRVCTRL7_A17(3)
+		      | DRVCTRL7_A18(3)
+		      | DRVCTRL7_A19(3));
+	pfc_reg_write(PFC_DRVCTRL7, reg);
+	reg = mmio_read_32(PFC_DRVCTRL8);
+	reg = ((reg & DRVCTRL8_MASK) | DRVCTRL8_CLKOUT(7)
+		      | DRVCTRL8_CS0(7)
+		      | DRVCTRL8_CS1_A2(7)
+		      | DRVCTRL8_BS(7)
+		      | DRVCTRL8_RD(7)
+		      | DRVCTRL8_RD_W(7)
+		      | DRVCTRL8_WE0(7)
+		      | DRVCTRL8_WE1(7));
+	pfc_reg_write(PFC_DRVCTRL8, reg);
+	reg = mmio_read_32(PFC_DRVCTRL9);
+	reg = ((reg & DRVCTRL9_MASK) | DRVCTRL9_EX_WAIT0(7)
+		      | DRVCTRL9_PRESETOU(7)
+		      | DRVCTRL9_D0(7)
+		      | DRVCTRL9_D1(7)
+		      | DRVCTRL9_D2(7)
+		      | DRVCTRL9_D3(7)
+		      | DRVCTRL9_D4(7)
+		      | DRVCTRL9_D5(7));
+	pfc_reg_write(PFC_DRVCTRL9, reg);
+	reg = mmio_read_32(PFC_DRVCTRL10);
+	reg = ((reg & DRVCTRL10_MASK) | DRVCTRL10_D6(7)
+		       | DRVCTRL10_D7(7)
+		       | DRVCTRL10_D8(3)
+		       | DRVCTRL10_D9(3)
+		       | DRVCTRL10_D10(3)
+		       | DRVCTRL10_D11(3)
+		       | DRVCTRL10_D12(3)
+		       | DRVCTRL10_D13(3));
+	pfc_reg_write(PFC_DRVCTRL10, reg);
+	reg = mmio_read_32(PFC_DRVCTRL11);
+	reg = ((reg & DRVCTRL11_MASK) | DRVCTRL11_D14(3)
+		       | DRVCTRL11_D15(3)
+		       | DRVCTRL11_AVS1(7)
+		       | DRVCTRL11_AVS2(7)
+		       | DRVCTRL11_GP7_02(7)
+		       | DRVCTRL11_GP7_03(7)
+		       | DRVCTRL11_DU_DOTCLKIN0(3)
+		       | DRVCTRL11_DU_DOTCLKIN1(3));
+	pfc_reg_write(PFC_DRVCTRL11, reg);
+	reg = mmio_read_32(PFC_DRVCTRL12);
+	reg = ((reg & DRVCTRL12_MASK) | DRVCTRL12_DU_DOTCLKIN2(3)
+		       | DRVCTRL12_DU_DOTCLKIN3(3)
+		       | DRVCTRL12_DU_FSCLKST(3)
+		       | DRVCTRL12_DU_TMS(3));
+	pfc_reg_write(PFC_DRVCTRL12, reg);
+	reg = mmio_read_32(PFC_DRVCTRL13);
+	reg = ((reg & DRVCTRL13_MASK) | DRVCTRL13_TDO(3)
+		       | DRVCTRL13_ASEBRK(3)
+		       | DRVCTRL13_SD0_CLK(7)
+		       | DRVCTRL13_SD0_CMD(7)
+		       | DRVCTRL13_SD0_DAT0(7)
+		       | DRVCTRL13_SD0_DAT1(7)
+		       | DRVCTRL13_SD0_DAT2(7)
+		       | DRVCTRL13_SD0_DAT3(7));
+	pfc_reg_write(PFC_DRVCTRL13, reg);
+	reg = mmio_read_32(PFC_DRVCTRL14);
+	reg = ((reg & DRVCTRL14_MASK) | DRVCTRL14_SD1_CLK(7)
+		       | DRVCTRL14_SD1_CMD(7)
+		       | DRVCTRL14_SD1_DAT0(5)
+		       | DRVCTRL14_SD1_DAT1(5)
+		       | DRVCTRL14_SD1_DAT2(5)
+		       | DRVCTRL14_SD1_DAT3(5)
+		       | DRVCTRL14_SD2_CLK(5)
+		       | DRVCTRL14_SD2_CMD(5));
+	pfc_reg_write(PFC_DRVCTRL14, reg);
+	reg = mmio_read_32(PFC_DRVCTRL15);
+	reg = ((reg & DRVCTRL15_MASK) | DRVCTRL15_SD2_DAT0(5)
+		       | DRVCTRL15_SD2_DAT1(5)
+		       | DRVCTRL15_SD2_DAT2(5)
+		       | DRVCTRL15_SD2_DAT3(5)
+		       | DRVCTRL15_SD2_DS(5)
+		       | DRVCTRL15_SD3_CLK(7)
+		       | DRVCTRL15_SD3_CMD(7)
+		       | DRVCTRL15_SD3_DAT0(7));
+	pfc_reg_write(PFC_DRVCTRL15, reg);
+	reg = mmio_read_32(PFC_DRVCTRL16);
+	reg = ((reg & DRVCTRL16_MASK) | DRVCTRL16_SD3_DAT1(7)
+		       | DRVCTRL16_SD3_DAT2(7)
+		       | DRVCTRL16_SD3_DAT3(7)
+		       | DRVCTRL16_SD3_DAT4(7)
+		       | DRVCTRL16_SD3_DAT5(7)
+		       | DRVCTRL16_SD3_DAT6(7)
+		       | DRVCTRL16_SD3_DAT7(7)
+		       | DRVCTRL16_SD3_DS(7));
+	pfc_reg_write(PFC_DRVCTRL16, reg);
+	reg = mmio_read_32(PFC_DRVCTRL17);
+	reg = ((reg & DRVCTRL17_MASK) | DRVCTRL17_SD0_CD(7)
+		       | DRVCTRL17_SD0_WP(7)
+		       | DRVCTRL17_SD1_CD(7)
+		       | DRVCTRL17_SD1_WP(7)
+		       | DRVCTRL17_SCK0(7)
+		       | DRVCTRL17_RX0(7)
+		       | DRVCTRL17_TX0(7)
+		       | DRVCTRL17_CTS0(7));
+	pfc_reg_write(PFC_DRVCTRL17, reg);
+	reg = mmio_read_32(PFC_DRVCTRL18);
+	reg = ((reg & DRVCTRL18_MASK) | DRVCTRL18_RTS0_TANS(7)
+		       | DRVCTRL18_RX1(7)
+		       | DRVCTRL18_TX1(7)
+		       | DRVCTRL18_CTS1(7)
+		       | DRVCTRL18_RTS1_TANS(7)
+		       | DRVCTRL18_SCK2(7)
+		       | DRVCTRL18_TX2(7)
+		       | DRVCTRL18_RX2(7));
+	pfc_reg_write(PFC_DRVCTRL18, reg);
+	reg = mmio_read_32(PFC_DRVCTRL19);
+	reg = ((reg & DRVCTRL19_MASK) | DRVCTRL19_HSCK0(7)
+		       | DRVCTRL19_HRX0(7)
+		       | DRVCTRL19_HTX0(7)
+		       | DRVCTRL19_HCTS0(7)
+		       | DRVCTRL19_HRTS0(7)
+		       | DRVCTRL19_MSIOF0_SCK(7)
+		       | DRVCTRL19_MSIOF0_SYNC(7)
+		       | DRVCTRL19_MSIOF0_SS1(7));
+	pfc_reg_write(PFC_DRVCTRL19, reg);
+	reg = mmio_read_32(PFC_DRVCTRL20);
+	reg = ((reg & DRVCTRL20_MASK) | DRVCTRL20_MSIOF0_TXD(7)
+		       | DRVCTRL20_MSIOF0_SS2(7)
+		       | DRVCTRL20_MSIOF0_RXD(7)
+		       | DRVCTRL20_MLB_CLK(7)
+		       | DRVCTRL20_MLB_SIG(7)
+		       | DRVCTRL20_MLB_DAT(7)
+		       | DRVCTRL20_MLB_REF(7)
+		       | DRVCTRL20_SSI_SCK0129(7));
+	pfc_reg_write(PFC_DRVCTRL20, reg);
+	reg = mmio_read_32(PFC_DRVCTRL21);
+	reg = ((reg & DRVCTRL21_MASK) | DRVCTRL21_SSI_WS0129(7)
+		       | DRVCTRL21_SSI_SDATA0(7)
+		       | DRVCTRL21_SSI_SDATA1(7)
+		       | DRVCTRL21_SSI_SDATA2(7)
+		       | DRVCTRL21_SSI_SCK34(7)
+		       | DRVCTRL21_SSI_WS34(7)
+		       | DRVCTRL21_SSI_SDATA3(7)
+		       | DRVCTRL21_SSI_SCK4(7));
+	pfc_reg_write(PFC_DRVCTRL21, reg);
+	reg = mmio_read_32(PFC_DRVCTRL22);
+	reg = ((reg & DRVCTRL22_MASK) | DRVCTRL22_SSI_WS4(7)
+		       | DRVCTRL22_SSI_SDATA4(7)
+		       | DRVCTRL22_SSI_SCK5(7)
+		       | DRVCTRL22_SSI_WS5(7)
+		       | DRVCTRL22_SSI_SDATA5(7)
+		       | DRVCTRL22_SSI_SCK6(7)
+		       | DRVCTRL22_SSI_WS6(7)
+		       | DRVCTRL22_SSI_SDATA6(7));
+	pfc_reg_write(PFC_DRVCTRL22, reg);
+	reg = mmio_read_32(PFC_DRVCTRL23);
+	reg = ((reg & DRVCTRL23_MASK) | DRVCTRL23_SSI_SCK78(7)
+		       | DRVCTRL23_SSI_WS78(7)
+		       | DRVCTRL23_SSI_SDATA7(7)
+		       | DRVCTRL23_SSI_SDATA8(7)
+		       | DRVCTRL23_SSI_SDATA9(7)
+		       | DRVCTRL23_AUDIO_CLKA(7)
+		       | DRVCTRL23_AUDIO_CLKB(7)
+		       | DRVCTRL23_USB0_PWEN(7));
+	pfc_reg_write(PFC_DRVCTRL23, reg);
+	reg = mmio_read_32(PFC_DRVCTRL24);
+	reg = ((reg & DRVCTRL24_MASK) | DRVCTRL24_USB0_OVC(7)
+		       | DRVCTRL24_USB1_PWEN(7)
+		       | DRVCTRL24_USB1_OVC(7)
+		       | DRVCTRL24_USB30_PWEN(7)
+		       | DRVCTRL24_USB30_OVC(7)
+		       | DRVCTRL24_USB31_PWEN(7)
+		       | DRVCTRL24_USB31_OVC(7));
+	pfc_reg_write(PFC_DRVCTRL24, reg);
+
+	/* initialize LSI pin pull-up/down control */
+	pfc_reg_write(PFC_PUD0, 0x00005FBFU);
+	pfc_reg_write(PFC_PUD1, 0x00300EFEU);
+	pfc_reg_write(PFC_PUD2, 0x330001E6U);
+	pfc_reg_write(PFC_PUD3, 0x000002E0U);
+	pfc_reg_write(PFC_PUD4, 0xFFFFFF00U);
+	pfc_reg_write(PFC_PUD5, 0x7F5FFF87U);
+	pfc_reg_write(PFC_PUD6, 0x00000055U);
+
+	/* initialize LSI pin pull-enable register */
+	pfc_reg_write(PFC_PUEN0, 0x00000FFFU);
+	pfc_reg_write(PFC_PUEN1, 0x00100234U);
+	pfc_reg_write(PFC_PUEN2, 0x000004C4U);
+	pfc_reg_write(PFC_PUEN3, 0x00000200U);
+	pfc_reg_write(PFC_PUEN4, 0x3E000000U);
+	pfc_reg_write(PFC_PUEN5, 0x1F000805U);
+	pfc_reg_write(PFC_PUEN6, 0x00000006U);
+
+	/* initialize positive/negative logic select */
+	mmio_write_32(GPIO_POSNEG0, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG1, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG2, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG3, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG4, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG5, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG6, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG7, 0x00000000U);
+
+	/* initialize general IO/interrupt switching */
+	mmio_write_32(GPIO_IOINTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL5, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL6, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL7, 0x00000000U);
+
+	/* initialize general output register */
+	mmio_write_32(GPIO_OUTDT0, 0x00000001U);
+	mmio_write_32(GPIO_OUTDT1, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT2, 0x00000400U);
+	mmio_write_32(GPIO_OUTDT3, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT4, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT5, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT6, 0x00003800U);
+	mmio_write_32(GPIO_OUTDT7, 0x00000003U);
+
+	/* initialize general input/output switching */
+	mmio_write_32(GPIO_INOUTSEL0, 0x00000001U);
+	mmio_write_32(GPIO_INOUTSEL1, 0x00100B00U);
+	mmio_write_32(GPIO_INOUTSEL2, 0x00000418U);
+	mmio_write_32(GPIO_INOUTSEL3, 0x00002000U);
+	mmio_write_32(GPIO_INOUTSEL4, 0x00000040U);
+	mmio_write_32(GPIO_INOUTSEL5, 0x00000208U);
+	mmio_write_32(GPIO_INOUTSEL6, 0x00013F00U);
+	mmio_write_32(GPIO_INOUTSEL7, 0x00000003U);
+
+}
diff --git a/drivers/renesas/rzg/pfc/G2M/pfc_init_g2m.h b/drivers/renesas/rzg/pfc/G2M/pfc_init_g2m.h
new file mode 100644
index 0000000..3315cd6
--- /dev/null
+++ b/drivers/renesas/rzg/pfc/G2M/pfc_init_g2m.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PFC_INIT_G2M_H
+#define PFC_INIT_G2M_H
+
+void pfc_init_g2m(void);
+
+#endif /* PFC_INIT_G2M_H */
diff --git a/drivers/renesas/rzg/pfc/G2N/pfc_init_g2n.c b/drivers/renesas/rzg/pfc/G2N/pfc_init_g2n.c
new file mode 100644
index 0000000..c951e0a
--- /dev/null
+++ b/drivers/renesas/rzg/pfc/G2N/pfc_init_g2n.c
@@ -0,0 +1,1306 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <lib/mmio.h>
+
+#include "pfc_init_g2n.h"
+#include "rcar_def.h"
+#include "../pfc_regs.h"
+
+#define GPSR0_D15			BIT(15)
+#define GPSR0_D14			BIT(14)
+#define GPSR0_D13			BIT(13)
+#define GPSR0_D12			BIT(12)
+#define GPSR0_D11			BIT(11)
+#define GPSR0_D10			BIT(10)
+#define GPSR0_D9			BIT(9)
+#define GPSR0_D8			BIT(8)
+#define GPSR0_D7			BIT(7)
+#define GPSR0_D6			BIT(6)
+#define GPSR0_D5			BIT(5)
+#define GPSR0_D4			BIT(4)
+#define GPSR0_D3			BIT(3)
+#define GPSR0_D2			BIT(2)
+#define GPSR0_D1			BIT(1)
+#define GPSR0_D0			BIT(0)
+#define GPSR1_CLKOUT			BIT(28)
+#define GPSR1_EX_WAIT0_A		BIT(27)
+#define GPSR1_WE1			BIT(26)
+#define GPSR1_WE0			BIT(25)
+#define GPSR1_RD_WR			BIT(24)
+#define GPSR1_RD			BIT(23)
+#define GPSR1_BS			BIT(22)
+#define GPSR1_CS1_A26			BIT(21)
+#define GPSR1_CS0			BIT(20)
+#define GPSR1_A19			BIT(19)
+#define GPSR1_A18			BIT(18)
+#define GPSR1_A17			BIT(17)
+#define GPSR1_A16			BIT(16)
+#define GPSR1_A15			BIT(15)
+#define GPSR1_A14			BIT(14)
+#define GPSR1_A13			BIT(13)
+#define GPSR1_A12			BIT(12)
+#define GPSR1_A11			BIT(11)
+#define GPSR1_A10			BIT(10)
+#define GPSR1_A9			BIT(9)
+#define GPSR1_A8			BIT(8)
+#define GPSR1_A7			BIT(7)
+#define GPSR1_A6			BIT(6)
+#define GPSR1_A5			BIT(5)
+#define GPSR1_A4			BIT(4)
+#define GPSR1_A3			BIT(3)
+#define GPSR1_A2			BIT(2)
+#define GPSR1_A1			BIT(1)
+#define GPSR1_A0			BIT(0)
+#define GPSR2_AVB_AVTP_CAPTURE_A	BIT(14)
+#define GPSR2_AVB_AVTP_MATCH_A		BIT(13)
+#define GPSR2_AVB_LINK			BIT(12)
+#define GPSR2_AVB_PHY_INT		BIT(11)
+#define GPSR2_AVB_MAGIC			BIT(10)
+#define GPSR2_AVB_MDC			BIT(9)
+#define GPSR2_PWM2_A			BIT(8)
+#define GPSR2_PWM1_A			BIT(7)
+#define GPSR2_PWM0			BIT(6)
+#define GPSR2_IRQ5			BIT(5)
+#define GPSR2_IRQ4			BIT(4)
+#define GPSR2_IRQ3			BIT(3)
+#define GPSR2_IRQ2			BIT(2)
+#define GPSR2_IRQ1			BIT(1)
+#define GPSR2_IRQ0			BIT(0)
+#define GPSR3_SD1_WP			BIT(15)
+#define GPSR3_SD1_CD			BIT(14)
+#define GPSR3_SD0_WP			BIT(13)
+#define GPSR3_SD0_CD			BIT(12)
+#define GPSR3_SD1_DAT3			BIT(11)
+#define GPSR3_SD1_DAT2			BIT(10)
+#define GPSR3_SD1_DAT1			BIT(9)
+#define GPSR3_SD1_DAT0			BIT(8)
+#define GPSR3_SD1_CMD			BIT(7)
+#define GPSR3_SD1_CLK			BIT(6)
+#define GPSR3_SD0_DAT3			BIT(5)
+#define GPSR3_SD0_DAT2			BIT(4)
+#define GPSR3_SD0_DAT1			BIT(3)
+#define GPSR3_SD0_DAT0			BIT(2)
+#define GPSR3_SD0_CMD			BIT(1)
+#define GPSR3_SD0_CLK			BIT(0)
+#define GPSR4_SD3_DS			BIT(17)
+#define GPSR4_SD3_DAT7			BIT(16)
+#define GPSR4_SD3_DAT6			BIT(15)
+#define GPSR4_SD3_DAT5			BIT(14)
+#define GPSR4_SD3_DAT4			BIT(13)
+#define GPSR4_SD3_DAT3			BIT(12)
+#define GPSR4_SD3_DAT2			BIT(11)
+#define GPSR4_SD3_DAT1			BIT(10)
+#define GPSR4_SD3_DAT0			BIT(9)
+#define GPSR4_SD3_CMD			BIT(8)
+#define GPSR4_SD3_CLK			BIT(7)
+#define GPSR4_SD2_DS			BIT(6)
+#define GPSR4_SD2_DAT3			BIT(5)
+#define GPSR4_SD2_DAT2			BIT(4)
+#define GPSR4_SD2_DAT1			BIT(3)
+#define GPSR4_SD2_DAT0			BIT(2)
+#define GPSR4_SD2_CMD			BIT(1)
+#define GPSR4_SD2_CLK			BIT(0)
+#define GPSR5_MLB_DAT			BIT(25)
+#define GPSR5_MLB_SIG			BIT(24)
+#define GPSR5_MLB_CLK			BIT(23)
+#define GPSR5_MSIOF0_RXD		BIT(22)
+#define GPSR5_MSIOF0_SS2		BIT(21)
+#define GPSR5_MSIOF0_TXD		BIT(20)
+#define GPSR5_MSIOF0_SS1		BIT(19)
+#define GPSR5_MSIOF0_SYNC		BIT(18)
+#define GPSR5_MSIOF0_SCK		BIT(17)
+#define GPSR5_HRTS0			BIT(16)
+#define GPSR5_HCTS0			BIT(15)
+#define GPSR5_HTX0			BIT(14)
+#define GPSR5_HRX0			BIT(13)
+#define GPSR5_HSCK0			BIT(12)
+#define GPSR5_RX2_A			BIT(11)
+#define GPSR5_TX2_A			BIT(10)
+#define GPSR5_SCK2			BIT(9)
+#define GPSR5_RTS1			BIT(8)
+#define GPSR5_CTS1			BIT(7)
+#define GPSR5_TX1_A			BIT(6)
+#define GPSR5_RX1_A			BIT(5)
+#define GPSR5_RTS0			BIT(4)
+#define GPSR5_CTS0			BIT(3)
+#define GPSR5_TX0			BIT(2)
+#define GPSR5_RX0			BIT(1)
+#define GPSR5_SCK0			BIT(0)
+#define GPSR6_USB31_OVC			BIT(31)
+#define GPSR6_USB31_PWEN		BIT(30)
+#define GPSR6_USB30_OVC			BIT(29)
+#define GPSR6_USB30_PWEN		BIT(28)
+#define GPSR6_USB1_OVC			BIT(27)
+#define GPSR6_USB1_PWEN			BIT(26)
+#define GPSR6_USB0_OVC			BIT(25)
+#define GPSR6_USB0_PWEN			BIT(24)
+#define GPSR6_AUDIO_CLKB_B		BIT(23)
+#define GPSR6_AUDIO_CLKA_A		BIT(22)
+#define GPSR6_SSI_SDATA9_A		BIT(21)
+#define GPSR6_SSI_SDATA8		BIT(20)
+#define GPSR6_SSI_SDATA7		BIT(19)
+#define GPSR6_SSI_WS78			BIT(18)
+#define GPSR6_SSI_SCK78			BIT(17)
+#define GPSR6_SSI_SDATA6		BIT(16)
+#define GPSR6_SSI_WS6			BIT(15)
+#define GPSR6_SSI_SCK6			BIT(14)
+#define GPSR6_SSI_SDATA5		BIT(13)
+#define GPSR6_SSI_WS5			BIT(12)
+#define GPSR6_SSI_SCK5			BIT(11)
+#define GPSR6_SSI_SDATA4		BIT(10)
+#define GPSR6_SSI_WS4			BIT(9)
+#define GPSR6_SSI_SCK4			BIT(8)
+#define GPSR6_SSI_SDATA3		BIT(7)
+#define GPSR6_SSI_WS34			BIT(6)
+#define GPSR6_SSI_SCK34			BIT(5)
+#define GPSR6_SSI_SDATA2_A		BIT(4)
+#define GPSR6_SSI_SDATA1_A		BIT(3)
+#define GPSR6_SSI_SDATA0		BIT(2)
+#define GPSR6_SSI_WS0129		BIT(1)
+#define GPSR6_SSI_SCK0129		BIT(0)
+#define GPSR7_AVS2			BIT(1)
+#define GPSR7_AVS1			BIT(0)
+
+#define IPSR_28_FUNC(x)		((uint32_t)(x) << 28U)
+#define IPSR_24_FUNC(x)		((uint32_t)(x) << 24U)
+#define IPSR_20_FUNC(x)		((uint32_t)(x) << 20U)
+#define IPSR_16_FUNC(x)		((uint32_t)(x) << 16U)
+#define IPSR_12_FUNC(x)		((uint32_t)(x) << 12U)
+#define IPSR_8_FUNC(x)		((uint32_t)(x) << 8U)
+#define IPSR_4_FUNC(x)		((uint32_t)(x) << 4U)
+#define IPSR_0_FUNC(x)		((uint32_t)(x) << 0U)
+
+#define POC_SD3_DS_33V		BIT(29)
+#define POC_SD3_DAT7_33V	BIT(28)
+#define POC_SD3_DAT6_33V	BIT(27)
+#define POC_SD3_DAT5_33V	BIT(26)
+#define POC_SD3_DAT4_33V	BIT(25)
+#define POC_SD3_DAT3_33V	BIT(24)
+#define POC_SD3_DAT2_33V	BIT(23)
+#define POC_SD3_DAT1_33V	BIT(22)
+#define POC_SD3_DAT0_33V	BIT(21)
+#define POC_SD3_CMD_33V		BIT(20)
+#define POC_SD3_CLK_33V		BIT(19)
+#define POC_SD2_DS_33V		BIT(18)
+#define POC_SD2_DAT3_33V	BIT(17)
+#define POC_SD2_DAT2_33V	BIT(16)
+#define POC_SD2_DAT1_33V	BIT(15)
+#define POC_SD2_DAT0_33V	BIT(14)
+#define POC_SD2_CMD_33V		BIT(13)
+#define POC_SD2_CLK_33V		BIT(12)
+#define POC_SD1_DAT3_33V	BIT(11)
+#define POC_SD1_DAT2_33V	BIT(10)
+#define POC_SD1_DAT1_33V	BIT(9)
+#define POC_SD1_DAT0_33V	BIT(8)
+#define POC_SD1_CMD_33V		BIT(7)
+#define POC_SD1_CLK_33V		BIT(6)
+#define POC_SD0_DAT3_33V	BIT(5)
+#define POC_SD0_DAT2_33V	BIT(4)
+#define POC_SD0_DAT1_33V	BIT(3)
+#define POC_SD0_DAT0_33V	BIT(2)
+#define POC_SD0_CMD_33V		BIT(1)
+#define POC_SD0_CLK_33V		BIT(0)
+
+#define DRVCTRL0_MASK		(0xCCCCCCCCU)
+#define DRVCTRL1_MASK		(0xCCCCCCC8U)
+#define DRVCTRL2_MASK		(0x88888888U)
+#define DRVCTRL3_MASK		(0x88888888U)
+#define DRVCTRL4_MASK		(0x88888888U)
+#define DRVCTRL5_MASK		(0x88888888U)
+#define DRVCTRL6_MASK		(0x88888888U)
+#define DRVCTRL7_MASK		(0x88888888U)
+#define DRVCTRL8_MASK		(0x88888888U)
+#define DRVCTRL9_MASK		(0x88888888U)
+#define DRVCTRL10_MASK		(0x88888888U)
+#define DRVCTRL11_MASK		(0x888888CCU)
+#define DRVCTRL12_MASK		(0xCCCFFFCFU)
+#define DRVCTRL13_MASK		(0xCC888888U)
+#define DRVCTRL14_MASK		(0x88888888U)
+#define DRVCTRL15_MASK		(0x88888888U)
+#define DRVCTRL16_MASK		(0x88888888U)
+#define DRVCTRL17_MASK		(0x88888888U)
+#define DRVCTRL18_MASK		(0x88888888U)
+#define DRVCTRL19_MASK		(0x88888888U)
+#define DRVCTRL20_MASK		(0x88888888U)
+#define DRVCTRL21_MASK		(0x88888888U)
+#define DRVCTRL22_MASK		(0x88888888U)
+#define DRVCTRL23_MASK		(0x88888888U)
+#define DRVCTRL24_MASK		(0x8888888FU)
+
+#define DRVCTRL0_QSPI0_SPCLK(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL0_QSPI0_MOSI_IO0(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL0_QSPI0_MISO_IO1(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL0_QSPI0_IO2(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL0_QSPI0_IO3(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL0_QSPI0_SSL(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL0_QSPI1_SPCLK(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL0_QSPI1_MOSI_IO0(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL1_QSPI1_MISO_IO1(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL1_QSPI1_IO2(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL1_QSPI1_IO3(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL1_QSPI1_SS(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL1_RPC_INT(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL1_RPC_WP(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL1_RPC_RESET(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL1_AVB_RX_CTL(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL2_AVB_RXC(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL2_AVB_RD0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL2_AVB_RD1(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL2_AVB_RD2(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL2_AVB_RD3(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL2_AVB_TX_CTL(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL2_AVB_TXC(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL2_AVB_TD0(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL3_AVB_TD1(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL3_AVB_TD2(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL3_AVB_TD3(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL3_AVB_TXCREFCLK(x)	((uint32_t)(x) << 16U)
+#define DRVCTRL3_AVB_MDIO(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL3_AVB_MDC(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL3_AVB_MAGIC(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL3_AVB_PHY_INT(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL4_AVB_LINK(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL4_AVB_AVTP_MATCH(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL4_AVB_AVTP_CAPTURE(x)	((uint32_t)(x) << 20U)
+#define DRVCTRL4_IRQ0(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL4_IRQ1(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL4_IRQ2(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL4_IRQ3(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL4_IRQ4(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL5_IRQ5(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL5_PWM0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL5_PWM1(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL5_PWM2(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL5_A0(x)			((uint32_t)(x) << 12U)
+#define DRVCTRL5_A1(x)			((uint32_t)(x) << 8U)
+#define DRVCTRL5_A2(x)			((uint32_t)(x) << 4U)
+#define DRVCTRL5_A3(x)			((uint32_t)(x) << 0U)
+#define DRVCTRL6_A4(x)			((uint32_t)(x) << 28U)
+#define DRVCTRL6_A5(x)			((uint32_t)(x) << 24U)
+#define DRVCTRL6_A6(x)			((uint32_t)(x) << 20U)
+#define DRVCTRL6_A7(x)			((uint32_t)(x) << 16U)
+#define DRVCTRL6_A8(x)			((uint32_t)(x) << 12U)
+#define DRVCTRL6_A9(x)			((uint32_t)(x) << 8U)
+#define DRVCTRL6_A10(x)			((uint32_t)(x) << 4U)
+#define DRVCTRL6_A11(x)			((uint32_t)(x) << 0U)
+#define DRVCTRL7_A12(x)			((uint32_t)(x) << 28U)
+#define DRVCTRL7_A13(x)			((uint32_t)(x) << 24U)
+#define DRVCTRL7_A14(x)			((uint32_t)(x) << 20U)
+#define DRVCTRL7_A15(x)			((uint32_t)(x) << 16U)
+#define DRVCTRL7_A16(x)			((uint32_t)(x) << 12U)
+#define DRVCTRL7_A17(x)			((uint32_t)(x) << 8U)
+#define DRVCTRL7_A18(x)			((uint32_t)(x) << 4U)
+#define DRVCTRL7_A19(x)			((uint32_t)(x) << 0U)
+#define DRVCTRL8_CLKOUT(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL8_CS0(x)			((uint32_t)(x) << 24U)
+#define DRVCTRL8_CS1_A2(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL8_BS(x)			((uint32_t)(x) << 16U)
+#define DRVCTRL8_RD(x)			((uint32_t)(x) << 12U)
+#define DRVCTRL8_RD_W(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL8_WE0(x)			((uint32_t)(x) << 4U)
+#define DRVCTRL8_WE1(x)			((uint32_t)(x) << 0U)
+#define DRVCTRL9_EX_WAIT0(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL9_PRESETOU(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL9_D0(x)			((uint32_t)(x) << 20U)
+#define DRVCTRL9_D1(x)			((uint32_t)(x) << 16U)
+#define DRVCTRL9_D2(x)			((uint32_t)(x) << 12U)
+#define DRVCTRL9_D3(x)			((uint32_t)(x) << 8U)
+#define DRVCTRL9_D4(x)			((uint32_t)(x) << 4U)
+#define DRVCTRL9_D5(x)			((uint32_t)(x) << 0U)
+#define DRVCTRL10_D6(x)			((uint32_t)(x) << 28U)
+#define DRVCTRL10_D7(x)			((uint32_t)(x) << 24U)
+#define DRVCTRL10_D8(x)			((uint32_t)(x) << 20U)
+#define DRVCTRL10_D9(x)			((uint32_t)(x) << 16U)
+#define DRVCTRL10_D10(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL10_D11(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL10_D12(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL10_D13(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL11_D14(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL11_D15(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL11_AVS1(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL11_AVS2(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL11_GP7_02(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL11_GP7_03(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL11_DU_DOTCLKIN0(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL11_DU_DOTCLKIN1(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL12_DU_DOTCLKIN2(x)	((uint32_t)(x) << 28U)
+#define DRVCTRL12_DU_DOTCLKIN3(x)	((uint32_t)(x) << 24U)
+#define DRVCTRL12_DU_FSCLKST(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL12_DU_TMS(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL13_TDO(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL13_ASEBRK(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL13_SD0_CLK(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL13_SD0_CMD(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL13_SD0_DAT0(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL13_SD0_DAT1(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL13_SD0_DAT2(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL13_SD0_DAT3(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL14_SD1_CLK(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL14_SD1_CMD(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL14_SD1_DAT0(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL14_SD1_DAT1(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL14_SD1_DAT2(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL14_SD1_DAT3(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL14_SD2_CLK(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL14_SD2_CMD(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL15_SD2_DAT0(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL15_SD2_DAT1(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL15_SD2_DAT2(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL15_SD2_DAT3(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL15_SD2_DS(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL15_SD3_CLK(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL15_SD3_CMD(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL15_SD3_DAT0(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL16_SD3_DAT1(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL16_SD3_DAT2(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL16_SD3_DAT3(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL16_SD3_DAT4(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL16_SD3_DAT5(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL16_SD3_DAT6(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL16_SD3_DAT7(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL16_SD3_DS(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL17_SD0_CD(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL17_SD0_WP(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL17_SD1_CD(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL17_SD1_WP(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL17_SCK0(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL17_RX0(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL17_TX0(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL17_CTS0(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL18_RTS0_TANS(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL18_RX1(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL18_TX1(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL18_CTS1(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL18_RTS1_TANS(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL18_SCK2(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL18_TX2(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL18_RX2(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL19_HSCK0(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL19_HRX0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL19_HTX0(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL19_HCTS0(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL19_HRTS0(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL19_MSIOF0_SCK(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL19_MSIOF0_SYNC(x)	((uint32_t)(x) << 4U)
+#define DRVCTRL19_MSIOF0_SS1(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL20_MSIOF0_TXD(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL20_MSIOF0_SS2(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL20_MSIOF0_RXD(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL20_MLB_CLK(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL20_MLB_SIG(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL20_MLB_DAT(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL20_MLB_REF(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL20_SSI_SCK0129(x)	((uint32_t)(x) << 0U)
+#define DRVCTRL21_SSI_WS0129(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL21_SSI_SDATA0(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL21_SSI_SDATA1(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL21_SSI_SDATA2(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL21_SSI_SCK34(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL21_SSI_WS34(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL21_SSI_SDATA3(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL21_SSI_SCK4(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL22_SSI_WS4(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL22_SSI_SDATA4(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL22_SSI_SCK5(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL22_SSI_WS5(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL22_SSI_SDATA5(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL22_SSI_SCK6(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL22_SSI_WS6(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL22_SSI_SDATA6(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL23_SSI_SCK78(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL23_SSI_WS78(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL23_SSI_SDATA7(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL23_SSI_SDATA8(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL23_SSI_SDATA9(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL23_AUDIO_CLKA(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL23_AUDIO_CLKB(x)		((uint32_t)(x) << 4U)
+#define DRVCTRL23_USB0_PWEN(x)		((uint32_t)(x) << 0U)
+#define DRVCTRL24_USB0_OVC(x)		((uint32_t)(x) << 28U)
+#define DRVCTRL24_USB1_PWEN(x)		((uint32_t)(x) << 24U)
+#define DRVCTRL24_USB1_OVC(x)		((uint32_t)(x) << 20U)
+#define DRVCTRL24_USB30_PWEN(x)		((uint32_t)(x) << 16U)
+#define DRVCTRL24_USB30_OVC(x)		((uint32_t)(x) << 12U)
+#define DRVCTRL24_USB31_PWEN(x)		((uint32_t)(x) << 8U)
+#define DRVCTRL24_USB31_OVC(x)		((uint32_t)(x) << 4U)
+
+#define MOD_SEL0_MSIOF3_A		((uint32_t)0U << 29U)
+#define MOD_SEL0_MSIOF3_B		((uint32_t)1U << 29U)
+#define MOD_SEL0_MSIOF3_C		((uint32_t)2U << 29U)
+#define MOD_SEL0_MSIOF3_D		((uint32_t)3U << 29U)
+#define MOD_SEL0_MSIOF3_E		((uint32_t)4U << 29U)
+#define MOD_SEL0_MSIOF2_A		((uint32_t)0U << 27U)
+#define MOD_SEL0_MSIOF2_B		((uint32_t)1U << 27U)
+#define MOD_SEL0_MSIOF2_C		((uint32_t)2U << 27U)
+#define MOD_SEL0_MSIOF2_D		((uint32_t)3U << 27U)
+#define MOD_SEL0_MSIOF1_A		((uint32_t)0U << 24U)
+#define MOD_SEL0_MSIOF1_B		((uint32_t)1U << 24U)
+#define MOD_SEL0_MSIOF1_C		((uint32_t)2U << 24U)
+#define MOD_SEL0_MSIOF1_D		((uint32_t)3U << 24U)
+#define MOD_SEL0_MSIOF1_E		((uint32_t)4U << 24U)
+#define MOD_SEL0_MSIOF1_F		((uint32_t)5U << 24U)
+#define MOD_SEL0_MSIOF1_G		((uint32_t)6U << 24U)
+#define MOD_SEL0_LBSC_A			((uint32_t)0U << 23U)
+#define MOD_SEL0_LBSC_B			((uint32_t)1U << 23U)
+#define MOD_SEL0_IEBUS_A		((uint32_t)0U << 22U)
+#define MOD_SEL0_IEBUS_B		((uint32_t)1U << 22U)
+#define MOD_SEL0_I2C2_A			((uint32_t)0U << 21U)
+#define MOD_SEL0_I2C2_B			((uint32_t)1U << 21U)
+#define MOD_SEL0_I2C1_A			((uint32_t)0U << 20U)
+#define MOD_SEL0_I2C1_B			((uint32_t)1U << 20U)
+#define MOD_SEL0_HSCIF4_A		((uint32_t)0U << 19U)
+#define MOD_SEL0_HSCIF4_B		((uint32_t)1U << 19U)
+#define MOD_SEL0_HSCIF3_A		((uint32_t)0U << 17U)
+#define MOD_SEL0_HSCIF3_B		((uint32_t)1U << 17U)
+#define MOD_SEL0_HSCIF3_C		((uint32_t)2U << 17U)
+#define MOD_SEL0_HSCIF3_D		((uint32_t)3U << 17U)
+#define MOD_SEL0_HSCIF1_A		((uint32_t)0U << 16U)
+#define MOD_SEL0_HSCIF1_B		((uint32_t)1U << 16U)
+#define MOD_SEL0_FSO_A			((uint32_t)0U << 15U)
+#define MOD_SEL0_FSO_B			((uint32_t)1U << 15U)
+#define MOD_SEL0_HSCIF2_A		((uint32_t)0U << 13U)
+#define MOD_SEL0_HSCIF2_B		((uint32_t)1U << 13U)
+#define MOD_SEL0_HSCIF2_C		((uint32_t)2U << 13U)
+#define MOD_SEL0_ETHERAVB_A		((uint32_t)0U << 12U)
+#define MOD_SEL0_ETHERAVB_B		((uint32_t)1U << 12U)
+#define MOD_SEL0_DRIF3_A		((uint32_t)0U << 11U)
+#define MOD_SEL0_DRIF3_B		((uint32_t)1U << 11U)
+#define MOD_SEL0_DRIF2_A		((uint32_t)0U << 10U)
+#define MOD_SEL0_DRIF2_B		((uint32_t)1U << 10U)
+#define MOD_SEL0_DRIF1_A		((uint32_t)0U << 8U)
+#define MOD_SEL0_DRIF1_B		((uint32_t)1U << 8U)
+#define MOD_SEL0_DRIF1_C		((uint32_t)2U << 8U)
+#define MOD_SEL0_DRIF0_A		((uint32_t)0U << 6U)
+#define MOD_SEL0_DRIF0_B		((uint32_t)1U << 6U)
+#define MOD_SEL0_DRIF0_C		((uint32_t)2U << 6U)
+#define MOD_SEL0_CANFD0_A		((uint32_t)0U << 5U)
+#define MOD_SEL0_CANFD0_B		((uint32_t)1U << 5U)
+#define MOD_SEL0_ADG_A_A		((uint32_t)0U << 3U)
+#define MOD_SEL0_ADG_A_B		((uint32_t)1U << 3U)
+#define MOD_SEL0_ADG_A_C		((uint32_t)2U << 3U)
+#define MOD_SEL1_TSIF1_A		((uint32_t)0U << 30U)
+#define MOD_SEL1_TSIF1_B		((uint32_t)1U << 30U)
+#define MOD_SEL1_TSIF1_C		((uint32_t)2U << 30U)
+#define MOD_SEL1_TSIF1_D		((uint32_t)3U << 30U)
+#define MOD_SEL1_TSIF0_A		((uint32_t)0U << 27U)
+#define MOD_SEL1_TSIF0_B		((uint32_t)1U << 27U)
+#define MOD_SEL1_TSIF0_C		((uint32_t)2U << 27U)
+#define MOD_SEL1_TSIF0_D		((uint32_t)3U << 27U)
+#define MOD_SEL1_TSIF0_E		((uint32_t)4U << 27U)
+#define MOD_SEL1_TIMER_TMU_A		((uint32_t)0U << 26U)
+#define MOD_SEL1_TIMER_TMU_B		((uint32_t)1U << 26U)
+#define MOD_SEL1_SSP1_1_A		((uint32_t)0U << 24U)
+#define MOD_SEL1_SSP1_1_B		((uint32_t)1U << 24U)
+#define MOD_SEL1_SSP1_1_C		((uint32_t)2U << 24U)
+#define MOD_SEL1_SSP1_1_D		((uint32_t)3U << 24U)
+#define MOD_SEL1_SSP1_0_A		((uint32_t)0U << 21U)
+#define MOD_SEL1_SSP1_0_B		((uint32_t)1U << 21U)
+#define MOD_SEL1_SSP1_0_C		((uint32_t)2U << 21U)
+#define MOD_SEL1_SSP1_0_D		((uint32_t)3U << 21U)
+#define MOD_SEL1_SSP1_0_E		((uint32_t)4U << 21U)
+#define MOD_SEL1_SSI_A			((uint32_t)0U << 20U)
+#define MOD_SEL1_SSI_B			((uint32_t)1U << 20U)
+#define MOD_SEL1_SPEED_PULSE_IF_A	((uint32_t)0U << 19U)
+#define MOD_SEL1_SPEED_PULSE_IF_B	((uint32_t)1U << 19U)
+#define MOD_SEL1_SIMCARD_A		((uint32_t)0U << 17U)
+#define MOD_SEL1_SIMCARD_B		((uint32_t)1U << 17U)
+#define MOD_SEL1_SIMCARD_C		((uint32_t)2U << 17U)
+#define MOD_SEL1_SIMCARD_D		((uint32_t)3U << 17U)
+#define MOD_SEL1_SDHI2_A		((uint32_t)0U << 16U)
+#define MOD_SEL1_SDHI2_B		((uint32_t)1U << 16U)
+#define MOD_SEL1_SCIF4_A		((uint32_t)0U << 14U)
+#define MOD_SEL1_SCIF4_B		((uint32_t)1U << 14U)
+#define MOD_SEL1_SCIF4_C		((uint32_t)2U << 14U)
+#define MOD_SEL1_SCIF3_A		((uint32_t)0U << 13U)
+#define MOD_SEL1_SCIF3_B		((uint32_t)1U << 13U)
+#define MOD_SEL1_SCIF2_A		((uint32_t)0U << 12U)
+#define MOD_SEL1_SCIF2_B		((uint32_t)1U << 12U)
+#define MOD_SEL1_SCIF1_A		((uint32_t)0U << 11U)
+#define MOD_SEL1_SCIF1_B		((uint32_t)1U << 11U)
+#define MOD_SEL1_SCIF_A			((uint32_t)0U << 10U)
+#define MOD_SEL1_SCIF_B			((uint32_t)1U << 10U)
+#define MOD_SEL1_REMOCON_A		((uint32_t)0U << 9U)
+#define MOD_SEL1_REMOCON_B		((uint32_t)1U << 9U)
+#define MOD_SEL1_RCAN0_A		((uint32_t)0U << 6U)
+#define MOD_SEL1_RCAN0_B		((uint32_t)1U << 6U)
+#define MOD_SEL1_PWM6_A			((uint32_t)0U << 5U)
+#define MOD_SEL1_PWM6_B			((uint32_t)1U << 5U)
+#define MOD_SEL1_PWM5_A			((uint32_t)0U << 4U)
+#define MOD_SEL1_PWM5_B			((uint32_t)1U << 4U)
+#define MOD_SEL1_PWM4_A			((uint32_t)0U << 3U)
+#define MOD_SEL1_PWM4_B			((uint32_t)1U << 3U)
+#define MOD_SEL1_PWM3_A			((uint32_t)0U << 2U)
+#define MOD_SEL1_PWM3_B			((uint32_t)1U << 2U)
+#define MOD_SEL1_PWM2_A			((uint32_t)0U << 1U)
+#define MOD_SEL1_PWM2_B			((uint32_t)1U << 1U)
+#define MOD_SEL1_PWM1_A			((uint32_t)0U << 0U)
+#define MOD_SEL1_PWM1_B			((uint32_t)1U << 0U)
+#define MOD_SEL2_I2C_5_A		((uint32_t)0U << 31U)
+#define MOD_SEL2_I2C_5_B		((uint32_t)1U << 31U)
+#define MOD_SEL2_I2C_3_A		((uint32_t)0U << 30U)
+#define MOD_SEL2_I2C_3_B		((uint32_t)1U << 30U)
+#define MOD_SEL2_I2C_0_A		((uint32_t)0U << 29U)
+#define MOD_SEL2_I2C_0_B		((uint32_t)1U << 29U)
+#define MOD_SEL2_FM_A			((uint32_t)0U << 27U)
+#define MOD_SEL2_FM_B			((uint32_t)1U << 27U)
+#define MOD_SEL2_FM_C			((uint32_t)2U << 27U)
+#define MOD_SEL2_FM_D			((uint32_t)3U << 27U)
+#define MOD_SEL2_SCIF5_A		((uint32_t)0U << 26U)
+#define MOD_SEL2_SCIF5_B		((uint32_t)1U << 26U)
+#define MOD_SEL2_I2C6_A			((uint32_t)0U << 23U)
+#define MOD_SEL2_I2C6_B			((uint32_t)1U << 23U)
+#define MOD_SEL2_I2C6_C			((uint32_t)2U << 23U)
+#define MOD_SEL2_NDF_A			((uint32_t)0U << 22U)
+#define MOD_SEL2_NDF_B			((uint32_t)1U << 22U)
+#define MOD_SEL2_SSI2_A			((uint32_t)0U << 21U)
+#define MOD_SEL2_SSI2_B			((uint32_t)1U << 21U)
+#define MOD_SEL2_SSI9_A			((uint32_t)0U << 20U)
+#define MOD_SEL2_SSI9_B			((uint32_t)1U << 20U)
+#define MOD_SEL2_TIMER_TMU2_A		((uint32_t)0U << 19U)
+#define MOD_SEL2_TIMER_TMU2_B		((uint32_t)1U << 19U)
+#define MOD_SEL2_ADG_B_A		((uint32_t)0U << 18U)
+#define MOD_SEL2_ADG_B_B		((uint32_t)1U << 18U)
+#define MOD_SEL2_ADG_C_A		((uint32_t)0U << 17U)
+#define MOD_SEL2_ADG_C_B		((uint32_t)1U << 17U)
+#define MOD_SEL2_VIN4_A			((uint32_t)0U << 0U)
+#define MOD_SEL2_VIN4_B			((uint32_t)1U << 0U)
+
+static void pfc_reg_write(uint32_t addr, uint32_t data)
+{
+	mmio_write_32(PFC_PMMR, ~data);
+	mmio_write_32((uintptr_t)addr, data);
+}
+
+void pfc_init_g2n(void)
+{
+	uint32_t reg;
+
+	/* initialize module select */
+	pfc_reg_write(PFC_MOD_SEL0,
+		      MOD_SEL0_MSIOF3_A |
+		      MOD_SEL0_MSIOF2_A |
+		      MOD_SEL0_MSIOF1_A |
+		      MOD_SEL0_LBSC_A |
+		      MOD_SEL0_IEBUS_A |
+		      MOD_SEL0_I2C2_A |
+		      MOD_SEL0_I2C1_A |
+		      MOD_SEL0_HSCIF4_A |
+		      MOD_SEL0_HSCIF3_A |
+		      MOD_SEL0_HSCIF1_A |
+		      MOD_SEL0_FSO_A |
+		      MOD_SEL0_HSCIF2_A |
+		      MOD_SEL0_ETHERAVB_A |
+		      MOD_SEL0_DRIF3_A |
+		      MOD_SEL0_DRIF2_A |
+		      MOD_SEL0_DRIF1_A |
+		      MOD_SEL0_DRIF0_A |
+		      MOD_SEL0_CANFD0_A |
+		      MOD_SEL0_ADG_A_A);
+
+	pfc_reg_write(PFC_MOD_SEL1,
+		      MOD_SEL1_TSIF1_A |
+		      MOD_SEL1_TSIF0_A |
+		      MOD_SEL1_TIMER_TMU_A |
+		      MOD_SEL1_SSP1_1_A |
+		      MOD_SEL1_SSP1_0_A |
+		      MOD_SEL1_SSI_A |
+		      MOD_SEL1_SPEED_PULSE_IF_A |
+		      MOD_SEL1_SIMCARD_A |
+		      MOD_SEL1_SDHI2_A |
+		      MOD_SEL1_SCIF4_A |
+		      MOD_SEL1_SCIF3_A |
+		      MOD_SEL1_SCIF2_A |
+		      MOD_SEL1_SCIF1_A |
+		      MOD_SEL1_SCIF_A |
+		      MOD_SEL1_REMOCON_A |
+		      MOD_SEL1_RCAN0_A |
+		      MOD_SEL1_PWM6_A |
+		      MOD_SEL1_PWM5_A |
+		      MOD_SEL1_PWM4_A |
+		      MOD_SEL1_PWM3_A |
+		      MOD_SEL1_PWM2_A |
+		      MOD_SEL1_PWM1_A);
+
+	pfc_reg_write(PFC_MOD_SEL2,
+		      MOD_SEL2_I2C_5_B |
+		      MOD_SEL2_I2C_3_B |
+		      MOD_SEL2_I2C_0_B |
+		      MOD_SEL2_FM_A |
+		      MOD_SEL2_SCIF5_A |
+		      MOD_SEL2_I2C6_A |
+		      MOD_SEL2_NDF_A |
+		      MOD_SEL2_SSI2_A |
+		      MOD_SEL2_SSI9_A |
+		      MOD_SEL2_TIMER_TMU2_A |
+		      MOD_SEL2_ADG_B_A |
+		      MOD_SEL2_ADG_C_A |
+		      MOD_SEL2_VIN4_A);
+
+	/* initialize peripheral function select */
+	pfc_reg_write(PFC_IPSR0,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR1,
+		      IPSR_28_FUNC(6) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(3) |
+		      IPSR_8_FUNC(3) |
+		      IPSR_4_FUNC(3) |
+		      IPSR_0_FUNC(3));
+
+	pfc_reg_write(PFC_IPSR2,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(6) |
+		      IPSR_20_FUNC(6) |
+		      IPSR_16_FUNC(6) |
+		      IPSR_12_FUNC(6) |
+		      IPSR_8_FUNC(6) |
+		      IPSR_4_FUNC(6) |
+		      IPSR_0_FUNC(6));
+
+	pfc_reg_write(PFC_IPSR3,
+		      IPSR_28_FUNC(6) |
+		      IPSR_24_FUNC(6) |
+		      IPSR_20_FUNC(6) |
+		      IPSR_16_FUNC(6) |
+		      IPSR_12_FUNC(6) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR4,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(6) |
+		      IPSR_4_FUNC(6) |
+		      IPSR_0_FUNC(6));
+
+	pfc_reg_write(PFC_IPSR5,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(6) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR6,
+		      IPSR_28_FUNC(6) |
+		      IPSR_24_FUNC(6) |
+		      IPSR_20_FUNC(6) |
+		      IPSR_16_FUNC(6) |
+		      IPSR_12_FUNC(6) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR7,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(6) |
+		      IPSR_4_FUNC(6) |
+		      IPSR_0_FUNC(6));
+
+	pfc_reg_write(PFC_IPSR8,
+		      IPSR_28_FUNC(1) |
+		      IPSR_24_FUNC(1) |
+		      IPSR_20_FUNC(1) |
+		      IPSR_16_FUNC(1) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR9,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR10,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR11,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(4) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR12,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(4) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR13,
+		      IPSR_28_FUNC(8) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(3) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR14,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(3) |
+		      IPSR_0_FUNC(8));
+
+	pfc_reg_write(PFC_IPSR15,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR16,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(0) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR17,
+		      IPSR_28_FUNC(0) |
+		      IPSR_24_FUNC(0) |
+		      IPSR_20_FUNC(0) |
+		      IPSR_16_FUNC(0) |
+		      IPSR_12_FUNC(0) |
+		      IPSR_8_FUNC(0) |
+		      IPSR_4_FUNC(1) |
+		      IPSR_0_FUNC(0));
+
+	pfc_reg_write(PFC_IPSR18, IPSR_4_FUNC(0) | IPSR_0_FUNC(0));
+
+	/* initialize GPIO/peripheral function select */
+	pfc_reg_write(PFC_GPSR0,
+		      GPSR0_D15 |
+		      GPSR0_D14 |
+		      GPSR0_D13 |
+		      GPSR0_D12 |
+		      GPSR0_D11 |
+		      GPSR0_D10 |
+		      GPSR0_D9 |
+		      GPSR0_D8 |
+		      GPSR0_D7 |
+		      GPSR0_D6 |
+		      GPSR0_D5 |
+		      GPSR0_D4 |
+		      GPSR0_D3 |
+		      GPSR0_D2 |
+		      GPSR0_D0);
+
+	pfc_reg_write(PFC_GPSR1,
+		      GPSR1_CLKOUT |
+		      GPSR1_EX_WAIT0_A |
+		      GPSR1_WE1 |
+		      GPSR1_RD |
+		      GPSR1_RD_WR |
+		      GPSR1_CS0 |
+		      GPSR1_A19 |
+		      GPSR1_A18 |
+		      GPSR1_A17 |
+		      GPSR1_A16 |
+		      GPSR1_A15 |
+		      GPSR1_A14 |
+		      GPSR1_A13 |
+		      GPSR1_A12 |
+		      GPSR1_A7 |
+		      GPSR1_A6 |
+		      GPSR1_A5 |
+		      GPSR1_A4 |
+		      GPSR1_A3 |
+		      GPSR1_A2 |
+		      GPSR1_A1 |
+		      GPSR1_A0);
+
+	pfc_reg_write(PFC_GPSR2,
+		      GPSR2_AVB_AVTP_CAPTURE_A |
+		      GPSR2_AVB_AVTP_MATCH_A |
+		      GPSR2_AVB_LINK |
+		      GPSR2_AVB_PHY_INT |
+		      GPSR2_AVB_MDC |
+		      GPSR2_PWM2_A |
+		      GPSR2_PWM1_A |
+		      GPSR2_IRQ4 |
+		      GPSR2_IRQ3 |
+		      GPSR2_IRQ2 |
+		      GPSR2_IRQ1 |
+		      GPSR2_IRQ0);
+
+	pfc_reg_write(PFC_GPSR3,
+		      GPSR3_SD0_CD |
+		      GPSR3_SD1_DAT3 |
+		      GPSR3_SD1_DAT2 |
+		      GPSR3_SD1_DAT1 |
+		      GPSR3_SD1_DAT0 |
+		      GPSR3_SD0_DAT3 |
+		      GPSR3_SD0_DAT2 |
+		      GPSR3_SD0_DAT1 |
+		      GPSR3_SD0_DAT0 |
+		      GPSR3_SD0_CMD |
+		      GPSR3_SD0_CLK);
+
+	pfc_reg_write(PFC_GPSR4,
+		      GPSR4_SD3_DS |
+		      GPSR4_SD3_DAT7 |
+		      GPSR4_SD3_DAT6 |
+		      GPSR4_SD3_DAT5 |
+		      GPSR4_SD3_DAT4 |
+		      GPSR4_SD3_DAT3 |
+		      GPSR4_SD3_DAT2 |
+		      GPSR4_SD3_DAT1 |
+		      GPSR4_SD3_DAT0 |
+		      GPSR4_SD3_CMD |
+		      GPSR4_SD3_CLK |
+		      GPSR4_SD2_DAT3 |
+		      GPSR4_SD2_DAT2 |
+		      GPSR4_SD2_DAT1 |
+		      GPSR4_SD2_DAT0 |
+		      GPSR4_SD2_CMD |
+		      GPSR4_SD2_CLK);
+
+	pfc_reg_write(PFC_GPSR5,
+		      GPSR5_MSIOF0_RXD |
+		      GPSR5_MSIOF0_TXD |
+		      GPSR5_MSIOF0_SYNC |
+		      GPSR5_MSIOF0_SCK |
+		      GPSR5_RX2_A |
+		      GPSR5_TX2_A |
+		      GPSR5_RTS1 |
+		      GPSR5_CTS1 |
+		      GPSR5_TX1_A |
+		      GPSR5_RX1_A |
+		      GPSR5_RTS0 |
+		      GPSR5_SCK0);
+
+	pfc_reg_write(PFC_GPSR6,
+		      GPSR6_AUDIO_CLKB_B |
+		      GPSR6_AUDIO_CLKA_A |
+		      GPSR6_SSI_WS6 |
+		      GPSR6_SSI_SCK6 |
+		      GPSR6_SSI_SDATA4 |
+		      GPSR6_SSI_WS4 |
+		      GPSR6_SSI_SCK4 |
+		      GPSR6_SSI_SDATA1_A |
+		      GPSR6_SSI_SDATA0 |
+		      GPSR6_SSI_WS0129 |
+		      GPSR6_SSI_SCK0129);
+
+	pfc_reg_write(PFC_GPSR7, GPSR7_AVS2 | GPSR7_AVS1);
+
+	/* initialize POC control register */
+	pfc_reg_write(PFC_POCCTRL0,
+		      POC_SD0_DAT3_33V |
+		      POC_SD0_DAT2_33V |
+		      POC_SD0_DAT1_33V |
+		      POC_SD0_DAT0_33V |
+		      POC_SD0_CMD_33V |
+		      POC_SD0_CLK_33V);
+
+	/* initialize DRV control register */
+	reg = mmio_read_32(PFC_DRVCTRL0);
+	reg = (reg & DRVCTRL0_MASK) |
+	      DRVCTRL0_QSPI0_SPCLK(3) |
+	      DRVCTRL0_QSPI0_MOSI_IO0(3) |
+	      DRVCTRL0_QSPI0_MISO_IO1(3) |
+	      DRVCTRL0_QSPI0_IO2(3) |
+	      DRVCTRL0_QSPI0_IO3(3) |
+	      DRVCTRL0_QSPI0_SSL(3) |
+	      DRVCTRL0_QSPI1_SPCLK(3) |
+	      DRVCTRL0_QSPI1_MOSI_IO0(3);
+	pfc_reg_write(PFC_DRVCTRL0, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL1);
+	reg = (reg & DRVCTRL1_MASK) |
+	      DRVCTRL1_QSPI1_MISO_IO1(3) |
+	      DRVCTRL1_QSPI1_IO2(3) |
+	      DRVCTRL1_QSPI1_IO3(3) |
+	      DRVCTRL1_QSPI1_SS(3) |
+	      DRVCTRL1_RPC_INT(3) |
+	      DRVCTRL1_RPC_WP(3) |
+	      DRVCTRL1_RPC_RESET(3) |
+	      DRVCTRL1_AVB_RX_CTL(7);
+	pfc_reg_write(PFC_DRVCTRL1, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL2);
+	reg = (reg & DRVCTRL2_MASK) |
+	      DRVCTRL2_AVB_RXC(7) |
+	      DRVCTRL2_AVB_RD0(7) |
+	      DRVCTRL2_AVB_RD1(7) |
+	      DRVCTRL2_AVB_RD2(7) |
+	      DRVCTRL2_AVB_RD3(7) |
+	      DRVCTRL2_AVB_TX_CTL(3) |
+	      DRVCTRL2_AVB_TXC(3) |
+	      DRVCTRL2_AVB_TD0(3);
+	pfc_reg_write(PFC_DRVCTRL2, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL3);
+	reg = (reg & DRVCTRL3_MASK) |
+	      DRVCTRL3_AVB_TD1(3) |
+	      DRVCTRL3_AVB_TD2(3) |
+	      DRVCTRL3_AVB_TD3(3) |
+	      DRVCTRL3_AVB_TXCREFCLK(7) |
+	      DRVCTRL3_AVB_MDIO(7) |
+	      DRVCTRL3_AVB_MDC(7) |
+	      DRVCTRL3_AVB_MAGIC(7) |
+	      DRVCTRL3_AVB_PHY_INT(7);
+	pfc_reg_write(PFC_DRVCTRL3, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL4);
+	reg = (reg & DRVCTRL4_MASK) |
+	      DRVCTRL4_AVB_LINK(7) |
+	      DRVCTRL4_AVB_AVTP_MATCH(7) |
+	      DRVCTRL4_AVB_AVTP_CAPTURE(7) |
+	      DRVCTRL4_IRQ0(7) |
+	      DRVCTRL4_IRQ1(7) |
+	      DRVCTRL4_IRQ2(7) |
+	      DRVCTRL4_IRQ3(7) |
+	      DRVCTRL4_IRQ4(7);
+	pfc_reg_write(PFC_DRVCTRL4, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL5);
+	reg = (reg & DRVCTRL5_MASK) |
+	      DRVCTRL5_IRQ5(7) |
+	      DRVCTRL5_PWM0(7) |
+	      DRVCTRL5_PWM1(7) |
+	      DRVCTRL5_PWM2(7) |
+	      DRVCTRL5_A0(3) |
+	      DRVCTRL5_A1(3) |
+	      DRVCTRL5_A2(3) |
+	      DRVCTRL5_A3(3);
+	pfc_reg_write(PFC_DRVCTRL5, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL6);
+	reg = (reg & DRVCTRL6_MASK) |
+	      DRVCTRL6_A4(3) |
+	      DRVCTRL6_A5(3) |
+	      DRVCTRL6_A6(3) |
+	      DRVCTRL6_A7(3) |
+	      DRVCTRL6_A8(7) |
+	      DRVCTRL6_A9(7) |
+	      DRVCTRL6_A10(7) |
+	      DRVCTRL6_A11(7);
+	pfc_reg_write(PFC_DRVCTRL6, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL7);
+	reg = (reg & DRVCTRL7_MASK) |
+	      DRVCTRL7_A12(3) |
+	      DRVCTRL7_A13(3) |
+	      DRVCTRL7_A14(3) |
+	      DRVCTRL7_A15(3) |
+	      DRVCTRL7_A16(3) |
+	      DRVCTRL7_A17(3) |
+	      DRVCTRL7_A18(3) |
+	      DRVCTRL7_A19(3);
+	pfc_reg_write(PFC_DRVCTRL7, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL8);
+	reg = (reg & DRVCTRL8_MASK) |
+	      DRVCTRL8_CLKOUT(7) |
+	      DRVCTRL8_CS0(7) |
+	      DRVCTRL8_CS1_A2(7) |
+	      DRVCTRL8_BS(7) |
+	      DRVCTRL8_RD(7) |
+	      DRVCTRL8_RD_W(7) |
+	      DRVCTRL8_WE0(7) |
+	      DRVCTRL8_WE1(7);
+	pfc_reg_write(PFC_DRVCTRL8, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL9);
+	reg = (reg & DRVCTRL9_MASK) |
+	      DRVCTRL9_EX_WAIT0(7) |
+	      DRVCTRL9_PRESETOU(7) |
+	      DRVCTRL9_D0(7) |
+	      DRVCTRL9_D1(7) |
+	      DRVCTRL9_D2(7) |
+	      DRVCTRL9_D3(7) |
+	      DRVCTRL9_D4(7) |
+	      DRVCTRL9_D5(7);
+	pfc_reg_write(PFC_DRVCTRL9, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL10);
+	reg = (reg & DRVCTRL10_MASK) |
+	      DRVCTRL10_D6(7) |
+	      DRVCTRL10_D7(7) |
+	      DRVCTRL10_D8(3) |
+	      DRVCTRL10_D9(3) |
+	      DRVCTRL10_D10(3) |
+	      DRVCTRL10_D11(3) |
+	      DRVCTRL10_D12(3) |
+	      DRVCTRL10_D13(3);
+	pfc_reg_write(PFC_DRVCTRL10, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL11);
+	reg = (reg & DRVCTRL11_MASK) |
+	      DRVCTRL11_D14(3) |
+	      DRVCTRL11_D15(3) |
+	      DRVCTRL11_AVS1(7) |
+	      DRVCTRL11_AVS2(7) |
+	      DRVCTRL11_GP7_02(7) |
+	      DRVCTRL11_GP7_03(7) |
+	      DRVCTRL11_DU_DOTCLKIN0(3) |
+	      DRVCTRL11_DU_DOTCLKIN1(3);
+	pfc_reg_write(PFC_DRVCTRL11, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL12);
+	reg = (reg & DRVCTRL12_MASK) |
+	      DRVCTRL12_DU_DOTCLKIN2(3) |
+	      DRVCTRL12_DU_DOTCLKIN3(3) |
+	      DRVCTRL12_DU_FSCLKST(3) |
+	      DRVCTRL12_DU_TMS(3);
+	pfc_reg_write(PFC_DRVCTRL12, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL13);
+	reg = (reg & DRVCTRL13_MASK) |
+	      DRVCTRL13_TDO(3) |
+	      DRVCTRL13_ASEBRK(3) |
+	      DRVCTRL13_SD0_CLK(7) |
+	      DRVCTRL13_SD0_CMD(7) |
+	      DRVCTRL13_SD0_DAT0(7) |
+	      DRVCTRL13_SD0_DAT1(7) |
+	      DRVCTRL13_SD0_DAT2(7) |
+	      DRVCTRL13_SD0_DAT3(7);
+	pfc_reg_write(PFC_DRVCTRL13, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL14);
+	reg = (reg & DRVCTRL14_MASK) |
+	      DRVCTRL14_SD1_CLK(7) |
+	      DRVCTRL14_SD1_CMD(7) |
+	      DRVCTRL14_SD1_DAT0(5) |
+	      DRVCTRL14_SD1_DAT1(5) |
+	      DRVCTRL14_SD1_DAT2(5) |
+	      DRVCTRL14_SD1_DAT3(5) |
+	      DRVCTRL14_SD2_CLK(5) |
+	      DRVCTRL14_SD2_CMD(5);
+	pfc_reg_write(PFC_DRVCTRL14, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL15);
+	reg = (reg & DRVCTRL15_MASK) |
+	      DRVCTRL15_SD2_DAT0(5) |
+	      DRVCTRL15_SD2_DAT1(5) |
+	      DRVCTRL15_SD2_DAT2(5) |
+	      DRVCTRL15_SD2_DAT3(5) |
+	      DRVCTRL15_SD2_DS(5) |
+	      DRVCTRL15_SD3_CLK(7) |
+	      DRVCTRL15_SD3_CMD(7) |
+	      DRVCTRL15_SD3_DAT0(7);
+	pfc_reg_write(PFC_DRVCTRL15, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL16);
+	reg = (reg & DRVCTRL16_MASK) |
+	      DRVCTRL16_SD3_DAT1(7) |
+	      DRVCTRL16_SD3_DAT2(7) |
+	      DRVCTRL16_SD3_DAT3(7) |
+	      DRVCTRL16_SD3_DAT4(7) |
+	      DRVCTRL16_SD3_DAT5(7) |
+	      DRVCTRL16_SD3_DAT6(7) |
+	      DRVCTRL16_SD3_DAT7(7) |
+	      DRVCTRL16_SD3_DS(7);
+	pfc_reg_write(PFC_DRVCTRL16, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL17);
+	reg = (reg & DRVCTRL17_MASK) |
+	      DRVCTRL17_SD0_CD(7) |
+	      DRVCTRL17_SD0_WP(7) |
+	      DRVCTRL17_SD1_CD(7) |
+	      DRVCTRL17_SD1_WP(7) |
+	      DRVCTRL17_SCK0(7) |
+	      DRVCTRL17_RX0(7) |
+	      DRVCTRL17_TX0(7) |
+	      DRVCTRL17_CTS0(7);
+	pfc_reg_write(PFC_DRVCTRL17, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL18);
+	reg = (reg & DRVCTRL18_MASK) |
+	      DRVCTRL18_RTS0_TANS(7) |
+	      DRVCTRL18_RX1(7) |
+	      DRVCTRL18_TX1(7) |
+	      DRVCTRL18_CTS1(7) |
+	      DRVCTRL18_RTS1_TANS(7) |
+	      DRVCTRL18_SCK2(7) |
+	      DRVCTRL18_TX2(7) |
+	      DRVCTRL18_RX2(7);
+	pfc_reg_write(PFC_DRVCTRL18, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL19);
+	reg = (reg & DRVCTRL19_MASK) |
+	      DRVCTRL19_HSCK0(7) |
+	      DRVCTRL19_HRX0(7) |
+	      DRVCTRL19_HTX0(7) |
+	      DRVCTRL19_HCTS0(7) |
+	      DRVCTRL19_HRTS0(7) |
+	      DRVCTRL19_MSIOF0_SCK(7) |
+	      DRVCTRL19_MSIOF0_SYNC(7) |
+	      DRVCTRL19_MSIOF0_SS1(7);
+	pfc_reg_write(PFC_DRVCTRL19, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL20);
+	reg = (reg & DRVCTRL20_MASK) |
+	      DRVCTRL20_MSIOF0_TXD(7) |
+	      DRVCTRL20_MSIOF0_SS2(7) |
+	      DRVCTRL20_MSIOF0_RXD(7) |
+	      DRVCTRL20_MLB_CLK(7) |
+	      DRVCTRL20_MLB_SIG(7) |
+	      DRVCTRL20_MLB_DAT(7) |
+	      DRVCTRL20_MLB_REF(7) |
+	      DRVCTRL20_SSI_SCK0129(7);
+	pfc_reg_write(PFC_DRVCTRL20, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL21);
+	reg = (reg & DRVCTRL21_MASK) |
+	      DRVCTRL21_SSI_WS0129(7) |
+	      DRVCTRL21_SSI_SDATA0(7) |
+	      DRVCTRL21_SSI_SDATA1(7) |
+	      DRVCTRL21_SSI_SDATA2(7) |
+	      DRVCTRL21_SSI_SCK34(7) |
+	      DRVCTRL21_SSI_WS34(7) |
+	      DRVCTRL21_SSI_SDATA3(7) |
+	      DRVCTRL21_SSI_SCK4(7);
+	pfc_reg_write(PFC_DRVCTRL21, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL22);
+	reg = (reg & DRVCTRL22_MASK) |
+	      DRVCTRL22_SSI_WS4(7) |
+	      DRVCTRL22_SSI_SDATA4(7) |
+	      DRVCTRL22_SSI_SCK5(7) |
+	      DRVCTRL22_SSI_WS5(7) |
+	      DRVCTRL22_SSI_SDATA5(7) |
+	      DRVCTRL22_SSI_SCK6(7) |
+	      DRVCTRL22_SSI_WS6(7) |
+	      DRVCTRL22_SSI_SDATA6(7);
+	pfc_reg_write(PFC_DRVCTRL22, reg);
+
+	reg = mmio_read_32(PFC_DRVCTRL23);
+	reg = (reg & DRVCTRL23_MASK) |
+	      DRVCTRL23_SSI_SCK78(7) |
+	      DRVCTRL23_SSI_WS78(7) |
+	      DRVCTRL23_SSI_SDATA7(7) |
+	      DRVCTRL23_SSI_SDATA8(7) |
+	      DRVCTRL23_SSI_SDATA9(7) |
+	      DRVCTRL23_AUDIO_CLKA(7) |
+	      DRVCTRL23_AUDIO_CLKB(7) |
+	      DRVCTRL23_USB0_PWEN(7);
+
+	pfc_reg_write(PFC_DRVCTRL23, reg);
+	reg = mmio_read_32(PFC_DRVCTRL24);
+	reg = (reg & DRVCTRL24_MASK) |
+	      DRVCTRL24_USB0_OVC(7) |
+	      DRVCTRL24_USB1_PWEN(7) |
+	      DRVCTRL24_USB1_OVC(7) |
+	      DRVCTRL24_USB30_PWEN(7) |
+	      DRVCTRL24_USB30_OVC(7) |
+	      DRVCTRL24_USB31_PWEN(7) |
+	      DRVCTRL24_USB31_OVC(7);
+	pfc_reg_write(PFC_DRVCTRL24, reg);
+
+	/* initialize LSI pin pull-up/down control */
+	pfc_reg_write(PFC_PUD0, 0x00005FBFU);
+	pfc_reg_write(PFC_PUD1, 0x00300EFEU);
+	pfc_reg_write(PFC_PUD2, 0x330001E6U);
+	pfc_reg_write(PFC_PUD3, 0x000002E0U);
+	pfc_reg_write(PFC_PUD4, 0xFFFFFF00U);
+	pfc_reg_write(PFC_PUD5, 0x7F5FFF87U);
+	pfc_reg_write(PFC_PUD6, 0x00000055U);
+
+	/* initialize LSI pin pull-enable register */
+	pfc_reg_write(PFC_PUEN0, 0x00000FFFU);
+	pfc_reg_write(PFC_PUEN1, 0x00100234U);
+	pfc_reg_write(PFC_PUEN2, 0x000004C4U);
+	pfc_reg_write(PFC_PUEN3, 0x00000200U);
+	pfc_reg_write(PFC_PUEN4, 0x3E000000U);
+	pfc_reg_write(PFC_PUEN5, 0x1F000805U);
+	pfc_reg_write(PFC_PUEN6, 0x00000006U);
+
+	/* initialize positive/negative logic select */
+	mmio_write_32(GPIO_POSNEG0, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG1, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG2, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG3, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG4, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG5, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG6, 0x00000000U);
+	mmio_write_32(GPIO_POSNEG7, 0x00000000U);
+
+	/* initialize general IO/interrupt switching */
+	mmio_write_32(GPIO_IOINTSEL0, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL1, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL2, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL3, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL4, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL5, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL6, 0x00000000U);
+	mmio_write_32(GPIO_IOINTSEL7, 0x00000000U);
+
+	/* initialize general output register */
+	mmio_write_32(GPIO_OUTDT0, 0x00000001U);
+	mmio_write_32(GPIO_OUTDT1, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT2, 0x00000400U);
+	mmio_write_32(GPIO_OUTDT3, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT4, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT5, 0x00000000U);
+	mmio_write_32(GPIO_OUTDT6, 0x00003800U);
+	mmio_write_32(GPIO_OUTDT7, 0x00000003U);
+
+	/* initialize general input/output switching */
+	mmio_write_32(GPIO_INOUTSEL0, 0x00000001U);
+	mmio_write_32(GPIO_INOUTSEL1, 0x00100B00U);
+	mmio_write_32(GPIO_INOUTSEL2, 0x00000418U);
+	mmio_write_32(GPIO_INOUTSEL3, 0x00002000U);
+	mmio_write_32(GPIO_INOUTSEL4, 0x00000040U);
+	mmio_write_32(GPIO_INOUTSEL5, 0x00000208U);
+	mmio_write_32(GPIO_INOUTSEL6, 0x00013F00U);
+	mmio_write_32(GPIO_INOUTSEL7, 0x00000003U);
+}
diff --git a/drivers/renesas/rzg/pfc/G2N/pfc_init_g2n.h b/drivers/renesas/rzg/pfc/G2N/pfc_init_g2n.h
new file mode 100644
index 0000000..f0616b6
--- /dev/null
+++ b/drivers/renesas/rzg/pfc/G2N/pfc_init_g2n.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PFC_INIT_G2N_H
+#define PFC_INIT_G2N_H
+
+void pfc_init_g2n(void);
+
+#endif /* PFC_INIT_G2N_H */
diff --git a/drivers/renesas/rzg/pfc/pfc.mk b/drivers/renesas/rzg/pfc/pfc.mk
new file mode 100644
index 0000000..15d0e8d
--- /dev/null
+++ b/drivers/renesas/rzg/pfc/pfc.mk
@@ -0,0 +1,41 @@
+#
+# Copyright (c) 2020-2021, Renesas Electronics Corporation. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${RCAR_LSI},${RCAR_AUTO})
+    BL2_SOURCES += drivers/renesas/rzg/pfc/G2M/pfc_init_g2m.c
+    BL2_SOURCES += drivers/renesas/rzg/pfc/G2H/pfc_init_g2h.c
+    BL2_SOURCES += drivers/renesas/rzg/pfc/G2N/pfc_init_g2n.c
+    BL2_SOURCES += drivers/renesas/rzg/pfc/G2E/pfc_init_g2e.c
+
+else ifdef RCAR_LSI_CUT_COMPAT
+  ifeq (${RCAR_LSI},${RZ_G2M})
+    BL2_SOURCES += drivers/renesas/rzg/pfc/G2M/pfc_init_g2m.c
+  endif
+  ifeq (${RCAR_LSI},${RZ_G2H})
+    BL2_SOURCES += drivers/renesas/rzg/pfc/G2H/pfc_init_g2h.c
+  endif
+  ifeq (${RCAR_LSI},${RZ_G2N})
+    BL2_SOURCES += drivers/renesas/rzg/pfc/G2N/pfc_init_g2n.c
+  endif
+  ifeq (${RCAR_LSI},${RZ_G2E})
+    BL2_SOURCES += drivers/renesas/rzg/pfc/G2E/pfc_init_g2e.c
+  endif
+else
+  ifeq (${RCAR_LSI},${RZ_G2M})
+    BL2_SOURCES += drivers/renesas/rzg/pfc/G2M/pfc_init_g2m.c
+  endif
+  ifeq (${RCAR_LSI},${RZ_G2H})
+    BL2_SOURCES += drivers/renesas/rzg/pfc/G2H/pfc_init_g2h.c
+  endif
+  ifeq (${RCAR_LSI},${RZ_G2N})
+    BL2_SOURCES += drivers/renesas/rzg/pfc/G2N/pfc_init_g2n.c
+  endif
+  ifeq (${RCAR_LSI},${RZ_G2E})
+    BL2_SOURCES += drivers/renesas/rzg/pfc/G2E/pfc_init_g2e.c
+  endif
+endif
+
+BL2_SOURCES += drivers/renesas/rzg/pfc/pfc_init.c
diff --git a/drivers/renesas/rzg/pfc/pfc_init.c b/drivers/renesas/rzg/pfc/pfc_init.c
new file mode 100644
index 0000000..762450c
--- /dev/null
+++ b/drivers/renesas/rzg/pfc/pfc_init.c
@@ -0,0 +1,129 @@
+/*
+ * Copyright (c) 2020-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#if RCAR_LSI == RCAR_AUTO
+#include "G2E/pfc_init_g2e.h"
+#include "G2H/pfc_init_g2h.h"
+#include "G2M/pfc_init_g2m.h"
+#include "G2N/pfc_init_g2n.h"
+#endif /* RCAR_LSI == RCAR_AUTO */
+#if (RCAR_LSI == RZ_G2E)
+#include "G2E/pfc_init_g2e.h"
+#endif /* RCAR_LSI == RZ_G2N */
+#if (RCAR_LSI == RZ_G2H)
+#include "G2H/pfc_init_g2h.h"
+#endif /* RCAR_LSI == RZ_G2H */
+#if (RCAR_LSI == RZ_G2M)
+#include "G2M/pfc_init_g2m.h"
+#endif /* RCAR_LSI == RZ_G2M */
+#if (RCAR_LSI == RZ_G2N)
+#include "G2N/pfc_init_g2n.h"
+#endif /* RCAR_LSI == RZ_G2N */
+#include "rcar_def.h"
+
+#define PRR_PRODUCT_ERR(reg)				\
+	do {						\
+		ERROR("LSI Product ID(PRR=0x%x) PFC init not supported.\n", \
+			reg);				\
+		panic();				\
+	} while (0)
+
+#define PRR_CUT_ERR(reg)				\
+	do {						\
+		ERROR("LSI Cut ID(PRR=0x%x) PFC init not supported.\n", \
+			reg);				\
+		panic();\
+	} while (0)
+
+void rzg_pfc_init(void)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(RCAR_PRR);
+#if RCAR_LSI == RCAR_AUTO
+	switch (reg & PRR_PRODUCT_MASK) {
+	case PRR_PRODUCT_M3:
+		pfc_init_g2m();
+		break;
+	case PRR_PRODUCT_H3:
+		pfc_init_g2h();
+		break;
+	case PRR_PRODUCT_M3N:
+		pfc_init_g2n();
+		break;
+	case PRR_PRODUCT_E3:
+		pfc_init_g2e();
+		break;
+	default:
+		PRR_PRODUCT_ERR(reg);
+		break;
+	}
+
+#elif RCAR_LSI_CUT_COMPAT /* RCAR_LSI == RCAR_AUTO */
+	switch (reg & PRR_PRODUCT_MASK) {
+	case PRR_PRODUCT_M3:
+#if RCAR_LSI != RZ_G2M
+		PRR_PRODUCT_ERR(reg);
+#else /* RCAR_LSI != RZ_G2M */
+		pfc_init_g2m();
+#endif /* RCAR_LSI != RZ_G2M */
+		break;
+	case PRR_PRODUCT_H3:
+#if (RCAR_LSI != RZ_G2H)
+		PRR_PRODUCT_ERR(reg);
+#else /* RCAR_LSI != RZ_G2H */
+		pfc_init_g2h();
+#endif /* RCAR_LSI != RZ_G2H */
+		break;
+	case PRR_PRODUCT_M3N:
+#if RCAR_LSI != RZ_G2N
+		PRR_PRODUCT_ERR(reg);
+#else
+		pfc_init_g2n();
+#endif /* RCAR_LSI != RZ_G2N */
+		break;
+	case PRR_PRODUCT_E3:
+#if RCAR_LSI != RZ_G2E
+		PRR_PRODUCT_ERR(reg);
+#else
+		pfc_init_g2e();
+#endif
+		break;
+	default:
+		PRR_PRODUCT_ERR(reg);
+		break;
+	}
+
+#else /* RCAR_LSI == RCAR_AUTO */
+#if (RCAR_LSI == RZ_G2M)
+	if ((reg & PRR_PRODUCT_MASK) != PRR_PRODUCT_M3) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	pfc_init_m3();
+#elif (RCAR_LSI == RZ_G2H)
+	if ((reg & PRR_PRODUCT_MASK) != PRR_PRODUCT_H3) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	pfc_init_g2h();
+#elif (RCAR_LSI == RZ_G2N)	/* G2N */
+	if ((reg & PRR_PRODUCT_MASK) != PRR_PRODUCT_M3N) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	pfc_init_g2n();
+#elif (RCAR_LSI == RZ_G2E)
+	if ((reg & PRR_PRODUCT_MASK) != PRR_PRODUCT_E3) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	pfc_init_g2e();
+#else /* RCAR_LSI == RZ_G2M */
+#error "Don't have PFC initialize routine(unknown)."
+#endif /* RCAR_LSI == RZ_G2M */
+#endif /* RCAR_LSI == RCAR_AUTO */
+}
diff --git a/drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10.c b/drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10.c
new file mode 100644
index 0000000..14ccc21
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10.c
@@ -0,0 +1,140 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "qos_init_g2e_v10.h"
+#include "../qos_common.h"
+#include "../qos_reg.h"
+
+#define RCAR_QOS_VERSION	"rev.0.05"
+
+#define REF_ARS_ARBSTOPCYCLE_G2E	(((SL_INIT_SSLOTCLK_G2E) - 5U) << 16U)
+
+#if RCAR_QOS_TYPE == RCAR_QOS_TYPE_DEFAULT
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_g2e_v10_mstat390.h"
+#else
+#include "qos_init_g2e_v10_mstat780.h"
+#endif /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+#endif /* RCAR_QOS_TYPE == RCAR_QOS_TYPE_DEFAULT */
+
+static const struct rcar_gen3_dbsc_qos_settings g2e_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218U },
+	{ DBSC_DBCAM0CNF2, 0x000000F4U },
+	{ DBSC_DBSCHCNT0, 0x000F0037U },
+	{ DBSC_DBSCHSZ0, 0x00000001U },
+	{ DBSC_DBSCHRW0, 0x22421111U },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123U },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00U },
+	{ DBSC_DBSCHQOS01, 0x00000B00U },
+	{ DBSC_DBSCHQOS02, 0x00000000U },
+	{ DBSC_DBSCHQOS03, 0x00000000U },
+	{ DBSC_DBSCHQOS40, 0x00000300U },
+	{ DBSC_DBSCHQOS41, 0x000002F0U },
+	{ DBSC_DBSCHQOS42, 0x00000200U },
+	{ DBSC_DBSCHQOS43, 0x00000100U },
+	{ DBSC_DBSCHQOS90, 0x00000100U },
+	{ DBSC_DBSCHQOS91, 0x000000F0U },
+	{ DBSC_DBSCHQOS92, 0x000000A0U },
+	{ DBSC_DBSCHQOS93, 0x00000040U },
+	{ DBSC_DBSCHQOS130, 0x00000100U },
+	{ DBSC_DBSCHQOS131, 0x000000F0U },
+	{ DBSC_DBSCHQOS132, 0x000000A0U },
+	{ DBSC_DBSCHQOS133, 0x00000040U },
+	{ DBSC_DBSCHQOS140, 0x000000C0U },
+	{ DBSC_DBSCHQOS141, 0x000000B0U },
+	{ DBSC_DBSCHQOS142, 0x00000080U },
+	{ DBSC_DBSCHQOS143, 0x00000040U },
+	{ DBSC_DBSCHQOS150, 0x00000040U },
+	{ DBSC_DBSCHQOS151, 0x00000030U },
+	{ DBSC_DBSCHQOS152, 0x00000020U },
+	{ DBSC_DBSCHQOS153, 0x00000010U },
+};
+
+void qos_init_g2e_v10(void)
+{
+	rzg_qos_dbsc_setting(g2e_qos, ARRAY_SIZE(g2e_qos), true);
+
+	/* DRAM Split Address mapping */
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH
+#if RCAR_LSI == RCAR_RZ_G2E
+#error "Don't set DRAM Split 4ch(G2E)"
+#else
+	ERROR("DRAM Split 4ch not supported.(G2E)");
+	panic();
+#endif
+#elif (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH)
+#if RCAR_LSI == RCAR_RZ_G2E
+#error "Don't set DRAM Split 2ch(G2E)"
+#else
+	ERROR("DRAM Split 2ch not supported.(G2E)");
+	panic();
+#endif
+#else
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+	NOTICE("BL2: DRAM refresh interval 3.9 usec\n");
+#else
+	NOTICE("BL2: DRAM refresh interval 7.8 usec\n");
+#endif
+
+	mmio_write_32(QOSCTRL_RAS, 0x00000020U);
+	mmio_write_64(QOSCTRL_DANN, 0x0404020002020201UL);
+	mmio_write_32(QOSCTRL_DANT, 0x00100804U);
+	mmio_write_32(QOSCTRL_FSS, 0x0000000AU);
+	mmio_write_32(QOSCTRL_INSFC, 0x06330001U);
+	mmio_write_32(QOSCTRL_EARLYR, 0x00000000U);
+	mmio_write_32(QOSCTRL_RACNT0, 0x00010003U);
+
+	mmio_write_32(QOSCTRL_SL_INIT, SL_INIT_REFFSSLOT |
+		      SL_INIT_SLOTSSLOT | SL_INIT_SSLOTCLK_G2E);
+	mmio_write_32(QOSCTRL_REF_ARS, REF_ARS_ARBSTOPCYCLE_G2E);
+
+	/* QOSBW SRAM setting */
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		mmio_write_64(QOSBW_FIX_QOS_BANK0 + i * 8U, mstat_fix[i]);
+		mmio_write_64(QOSBW_FIX_QOS_BANK1 + i * 8U, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		mmio_write_64(QOSBW_BE_QOS_BANK0 + i * 8U, mstat_be[i]);
+		mmio_write_64(QOSBW_BE_QOS_BANK1 + i * 8U, mstat_be[i]);
+	}
+
+	/* RT bus Leaf setting */
+	mmio_write_32(RT_ACT0, 0x00000000U);
+	mmio_write_32(RT_ACT1, 0x00000000U);
+
+	/* CCI bus Leaf setting */
+	mmio_write_32(CPU_ACT0, 0x00000003U);
+	mmio_write_32(CPU_ACT1, 0x00000003U);
+
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+	mmio_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif
+}
diff --git a/drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10.h b/drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10.h
new file mode 100644
index 0000000..d27de1b
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2E_V10_H
+#define QOS_INIT_G2E_V10_H
+
+void qos_init_g2e_v10(void);
+
+#endif /* QOS_INIT_G2E_V10_H */
diff --git a/drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10_mstat390.h b/drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10_mstat390.h
new file mode 100644
index 0000000..63b08c4
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10_mstat390.h
@@ -0,0 +1,245 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2E_V10_MSTAT390_H
+#define QOS_INIT_G2E_V10_MSTAT390_H
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008620000FFFFUL,
+	/* 0x0038, */ 0x001008620000FFFFUL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x001415260000FFFFUL,
+	/* 0x0060, */ 0x001415260000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001414930000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C08380000FFFFUL,
+	/* 0x00a8, */ 0x000C04110000FFFFUL,
+	/* 0x00b0, */ 0x000C04150000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C08380000FFFFUL,
+	/* 0x00c8, */ 0x000C04110000FFFFUL,
+	/* 0x00d0, */ 0x000C04150000FFFFUL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x000C084F0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x000C21E40000FFFFUL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x001008530000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00100C960000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x001008530000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0010042A0000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00101D8D0000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x001008530000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x001410040000FFFFUL,
+	/* 0x0270, */ 0x001404020000FFFFUL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410040000FFFFUL,
+	/* 0x0298, */ 0x001404020000FFFFUL,
+	/* 0x02a0, */ 0x000C04090000FFFFUL,
+	/* 0x02a8, */ 0x000C04090000FFFFUL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04090000FFFFUL,
+	/* 0x02d8, */ 0x000C04090000FFFFUL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x000C04020000FFFFUL,
+	/* 0x0378, */ 0x000C04020000FFFFUL,
+	/* 0x0380, */ 0x000C04090000FFFFUL,
+	/* 0x0388, */ 0x000C04090000FFFFUL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001005F03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0021060005FFFC01UL,
+	/* 0x01c8, */ 0x0021060005FFFC01UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0011010005F79801UL,
+	/* 0x0220, */ 0x0011010005F79801UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011010005F79801UL,
+	/* 0x0238, */ 0x0011010005F79801UL,
+	/* 0x0240, */ 0x0012010005F79801UL,
+	/* 0x0248, */ 0x0011010005F79801UL,
+	/* 0x0250, */ 0x0012010005F79801UL,
+	/* 0x0258, */ 0x0011010005F79801UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011060005FFFC01UL,
+	/* 0x02f8, */ 0x0011060005FFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0012001005F03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0012060005FFFC01UL,
+	/* 0x0360, */ 0x0012060005FFFC01UL,
+	/* 0x0368, */ 0x0012001005F03401UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0012001005F03401UL,
+};
+#endif /* QOS_INIT_G2E_V10_MSTAT390_H */
diff --git a/drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10_mstat780.h b/drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10_mstat780.h
new file mode 100644
index 0000000..3b888ea
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10_mstat780.h
@@ -0,0 +1,246 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2E_V10_MSTAT780_H
+#define QOS_INIT_G2E_V10_MSTAT780_H
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001010C40000FFFFUL,
+	/* 0x0038, */ 0x001010C40000FFFFUL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x00142A4B0000FFFFUL,
+	/* 0x0060, */ 0x00142A4B0000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001429260000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C10700000FFFFUL,
+	/* 0x00a8, */ 0x000C08210000FFFFUL,
+	/* 0x00b0, */ 0x000C082A0000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C10700000FFFFUL,
+	/* 0x00c8, */ 0x000C08210000FFFFUL,
+	/* 0x00d0, */ 0x000C082A0000FFFFUL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x00102CAF0000FFFFUL,
+	/* 0x00f8, */ 0x000C0C9D0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x00100CAF0000FFFFUL,
+	/* 0x0118, */ 0x000C43C80000FFFFUL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100CA50000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0010152C0000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x00100CA50000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008530000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001037190000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x00100CA50000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04040000FFFFUL,
+	/* 0x01f0, */ 0x000C08110000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04110000FFFFUL,
+	/* 0x0210, */ 0x000C08110000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C18530000FFFFUL,
+	/* 0x0268, */ 0x00141C070000FFFFUL,
+	/* 0x0270, */ 0x001404040000FFFFUL,
+	/* 0x0278, */ 0x000C0C210000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x00141C070000FFFFUL,
+	/* 0x0298, */ 0x001404040000FFFFUL,
+	/* 0x02a0, */ 0x000C04110000FFFFUL,
+	/* 0x02a8, */ 0x000C04110000FFFFUL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x000C04040000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04110000FFFFUL,
+	/* 0x02d8, */ 0x000C04110000FFFFUL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x000C04040000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x000C04040000FFFFUL,
+	/* 0x0378, */ 0x000C04040000FFFFUL,
+	/* 0x0380, */ 0x000C04110000FFFFUL,
+	/* 0x0388, */ 0x000C04110000FFFFUL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001002F03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0021060002FFFC01UL,
+	/* 0x01c8, */ 0x0021060002FFFC01UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021010002F3CC01UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021010002F3CC01UL,
+	/* 0x0218, */ 0x0011010002F3CC01UL,
+	/* 0x0220, */ 0x0011010002F3CC01UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011010002F3CC01UL,
+	/* 0x0238, */ 0x0011010002F3CC01UL,
+	/* 0x0240, */ 0x0012010002F3CC01UL,
+	/* 0x0248, */ 0x0011010002F3CC01UL,
+	/* 0x0250, */ 0x0012010002F3CC01UL,
+	/* 0x0258, */ 0x0011010002F3CC01UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011060002FFFC01UL,
+	/* 0x02f8, */ 0x0011060002FFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0012001002F03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0012060002FFFC01UL,
+	/* 0x0360, */ 0x0012060002FFFC01UL,
+	/* 0x0368, */ 0x0012001002F03401UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0012001002F03401UL,
+};
+
+#endif /* QOS_INIT_G2E_V10_MSTAT780_H */
diff --git a/drivers/renesas/rzg/qos/G2H/qos_init_g2h_mstat195.h b/drivers/renesas/rzg/qos/G2H/qos_init_g2h_mstat195.h
new file mode 100644
index 0000000..7bb34aa
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2H/qos_init_g2h_mstat195.h
@@ -0,0 +1,236 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2H_MSTAT195_H
+#define QOS_INIT_G2H_MSTAT195_H
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000FFFFUL,
+	/* 0x0038, */ 0x001008070000FFFFUL,
+	/* 0x0040, */ 0x001410070000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x0014100D0000FFFFUL,
+	/* 0x0060, */ 0x0014100D0000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001410070000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C04020000FFFFUL,
+	/* 0x00a8, */ 0x000C04010000FFFFUL,
+	/* 0x00b0, */ 0x000C04010000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C04020000FFFFUL,
+	/* 0x00c8, */ 0x000C04010000FFFFUL,
+	/* 0x00d0, */ 0x000C04010000FFFFUL,
+	/* 0x00d8, */ 0x001024090000FFFFUL,
+	/* 0x00e0, */ 0x00100C090000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001024090000FFFFUL,
+	/* 0x00f8, */ 0x000C100D0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x00100C090000FFFFUL,
+	/* 0x0118, */ 0x000C1C1B0000FFFFUL,
+	/* 0x0120, */ 0x000C1C1B0000FFFFUL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100C0B0000FFFFUL,
+	/* 0x0140, */ 0x00100C0B0000FFFFUL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0010100D0000FFFFUL,
+	/* 0x0158, */ 0x0010100D0000FFFFUL,
+	/* 0x0160, */ 0x00100C0B0000FFFFUL,
+	/* 0x0168, */ 0x00100C0B0000FFFFUL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008060000FFFFUL,
+	/* 0x0180, */ 0x001008060000FFFFUL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00102C2C0000FFFFUL,
+	/* 0x0198, */ 0x00102C2C0000FFFFUL,
+	/* 0x01a0, */ 0x00100C0B0000FFFFUL,
+	/* 0x01a8, */ 0x00100C0B0000FFFFUL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFFFUL,
+	/* 0x0268, */ 0x001408010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C04010000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x001408010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x001408010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x001200600BDFFC01UL,
+	/* 0x0008, */ 0x001200600BDFFC01UL,
+	/* 0x0010, */ 0x001200600BDFFC01UL,
+	/* 0x0018, */ 0x001200600BDFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x001200100BD0FC01UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100600BDFFC01UL,
+	/* 0x01c8, */ 0x002100600BDFFC01UL,
+	/* 0x01d0, */ 0x002100600BDFFC01UL,
+	/* 0x01d8, */ 0x002100600BDFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x002100100BDF2401UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x002100100BDF2401UL,
+	/* 0x0218, */ 0x001100100BDF2401UL,
+	/* 0x0220, */ 0x001100100BDF2401UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x001100100BDF2401UL,
+	/* 0x0238, */ 0x001100100BDF2401UL,
+	/* 0x0240, */ 0x001200100BDF2401UL,
+	/* 0x0248, */ 0x001100100BDF2401UL,
+	/* 0x0250, */ 0x001200100BDF2401UL,
+	/* 0x0258, */ 0x001100100BDF2401UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x001100600BDFFC01UL,
+	/* 0x02f8, */ 0x001100600BDFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x001100600BDFFC01UL,
+	/* 0x0310, */ 0x001100600BDFFC01UL,
+	/* 0x0318, */ 0x001200100BD03401UL,
+	/* 0x0320, */ 0x001100600BDFFC01UL,
+	/* 0x0328, */ 0x001100600BDFFC01UL,
+	/* 0x0330, */ 0x001100600BDFFC01UL,
+	/* 0x0338, */ 0x001100600BDFFC01UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x001200100BD0FC01UL,
+};
+
+#endif /* QOS_INIT_G2H_MSTAT195_H */
diff --git a/drivers/renesas/rzg/qos/G2H/qos_init_g2h_mstat390.h b/drivers/renesas/rzg/qos/G2H/qos_init_g2h_mstat390.h
new file mode 100644
index 0000000..9696a40
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2H/qos_init_g2h_mstat390.h
@@ -0,0 +1,236 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2H_MSTAT390_H
+#define QOS_INIT_G2H_MSTAT390_H
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000FFFFUL,
+	/* 0x0038, */ 0x0010100D0000FFFFUL,
+	/* 0x0040, */ 0x00141C0E0000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001408010000FFFFUL,
+	/* 0x0058, */ 0x00141C190000FFFFUL,
+	/* 0x0060, */ 0x00141C190000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001408010000FFFFUL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x00141C0E0000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C08040000FFFFUL,
+	/* 0x00a8, */ 0x000C04020000FFFFUL,
+	/* 0x00b0, */ 0x000C04020000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C08040000FFFFUL,
+	/* 0x00c8, */ 0x000C04020000FFFFUL,
+	/* 0x00d0, */ 0x000C04020000FFFFUL,
+	/* 0x00d8, */ 0x001044110000FFFFUL,
+	/* 0x00e0, */ 0x001014110000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001044110000FFFFUL,
+	/* 0x00f8, */ 0x000C1C1A0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001014110000FFFFUL,
+	/* 0x0118, */ 0x000C38360000FFFFUL,
+	/* 0x0120, */ 0x000C38360000FFFFUL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x001018150000FFFFUL,
+	/* 0x0140, */ 0x001018150000FFFFUL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00101C190000FFFFUL,
+	/* 0x0158, */ 0x00101C190000FFFFUL,
+	/* 0x0160, */ 0x001018150000FFFFUL,
+	/* 0x0168, */ 0x001018150000FFFFUL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x00100C0B0000FFFFUL,
+	/* 0x0180, */ 0x00100C0B0000FFFFUL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001058570000FFFFUL,
+	/* 0x0198, */ 0x001058570000FFFFUL,
+	/* 0x01a0, */ 0x001018150000FFFFUL,
+	/* 0x01a8, */ 0x001018150000FFFFUL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFFFUL,
+	/* 0x0268, */ 0x001410010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C08020000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x00140C010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x00140C010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0012006005EFFC01UL,
+	/* 0x0008, */ 0x0012006005EFFC01UL,
+	/* 0x0010, */ 0x0012006005EFFC01UL,
+	/* 0x0018, */ 0x0012006005EFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001005E0FC01UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0021006005EFFC01UL,
+	/* 0x01c8, */ 0x0021006005EFFC01UL,
+	/* 0x01d0, */ 0x0021006005EFFC01UL,
+	/* 0x01d8, */ 0x0021006005EFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021001005E79401UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021001005E79401UL,
+	/* 0x0218, */ 0x0011001005E79401UL,
+	/* 0x0220, */ 0x0011001005E79401UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011001005E79401UL,
+	/* 0x0238, */ 0x0011001005E79401UL,
+	/* 0x0240, */ 0x0012001005E79401UL,
+	/* 0x0248, */ 0x0011001005E79401UL,
+	/* 0x0250, */ 0x0012001005E79401UL,
+	/* 0x0258, */ 0x0011001005E79401UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011006005EFFC01UL,
+	/* 0x02f8, */ 0x0011006005EFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0011006005EFFC01UL,
+	/* 0x0310, */ 0x0011006005EFFC01UL,
+	/* 0x0318, */ 0x0012001005E03401UL,
+	/* 0x0320, */ 0x0011006005EFFC01UL,
+	/* 0x0328, */ 0x0011006005EFFC01UL,
+	/* 0x0330, */ 0x0011006005EFFC01UL,
+	/* 0x0338, */ 0x0011006005EFFC01UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0012001005E0FC01UL,
+};
+
+#endif /* QOS_INIT_G2H_MSTAT390_H */
diff --git a/drivers/renesas/rzg/qos/G2H/qos_init_g2h_qoswt195.h b/drivers/renesas/rzg/qos/G2H/qos_init_g2h_qoswt195.h
new file mode 100644
index 0000000..044f246
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2H/qos_init_g2h_qoswt195.h
@@ -0,0 +1,236 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2H_QOSWT195_H
+#define QOS_INIT_G2H_QOSWT195_H
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000C010UL,
+	/* 0x0038, */ 0x001008070000C010UL,
+	/* 0x0040, */ 0x001410070000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0014100D0000C010UL,
+	/* 0x0060, */ 0x0014100D0000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001410070000FFF0UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFF0UL,
+	/* 0x0268, */ 0x001408010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C04010000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+#endif /* QOS_INIT_G2H_QOSWT195_H */
diff --git a/drivers/renesas/rzg/qos/G2H/qos_init_g2h_qoswt390.h b/drivers/renesas/rzg/qos/G2H/qos_init_g2h_qoswt390.h
new file mode 100644
index 0000000..2ae07ab
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2H/qos_init_g2h_qoswt390.h
@@ -0,0 +1,236 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2H_QOSWT390_H
+#define QOS_INIT_G2H_QOSWT390_H
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000C010UL,
+	/* 0x0038, */ 0x0010100D0000C010UL,
+	/* 0x0040, */ 0x00141C0E0000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x00141C190000C010UL,
+	/* 0x0060, */ 0x00141C190000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x00141C0E0000FFF0UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFF0UL,
+	/* 0x0268, */ 0x001410010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C08020000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+};
+
+#endif /* QOS_INIT_G2H_QOSWT390_H */
diff --git a/drivers/renesas/rzg/qos/G2H/qos_init_g2h_v30.c b/drivers/renesas/rzg/qos/G2H/qos_init_g2h_v30.c
new file mode 100644
index 0000000..7f466c8
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2H/qos_init_g2h_v30.c
@@ -0,0 +1,217 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "qos_init_g2h_v30.h"
+#include "../qos_common.h"
+#include "../qos_reg.h"
+
+#define RCAR_QOS_VERSION			"rev.0.07"
+
+#define QOSWT_TIME_BANK0			20000000U	/* unit:ns */
+#define QOSWT_WTEN_ENABLE			0x1U
+
+#define QOSCTRL_REF_ARS_ARBSTOPCYCLE_G2H	(SL_INIT_SSLOTCLK_G2H - 0x5U)
+
+#define OSWT_WTREF_SLOT0_EN_REQ1_SLOT		3U
+#define OSWT_WTREF_SLOT0_EN_REQ2_SLOT		9U
+#define QOSWT_WTREF_SLOT0_EN			((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) | \
+						(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+#define QOSWT_WTREF_SLOT1_EN			((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) | \
+						(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+
+#define QOSWT_WTSET0_REQ_SSLOT0			5U
+#define WT_BASE_SUB_SLOT_NUM0			12U
+#define QOSWT_WTSET0_PERIOD0_G2H		((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_G2H) - 1U)
+#define QOSWT_WTSET0_SSLOT0			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET0_SLOTSLOT0			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#define QOSWT_WTSET1_PERIOD1_G2H		(QOSWT_WTSET0_PERIOD0_G2H)
+#define QOSWT_WTSET1_SSLOT1			(QOSWT_WTSET0_SSLOT0)
+#define QOSWT_WTSET1_SLOTSLOT1			(QOSWT_WTSET0_SLOTSLOT0)
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_g2h_mstat195.h"
+#else
+#include "qos_init_g2h_mstat390.h"
+#endif /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_g2h_qoswt195.h"
+#else
+#include "qos_init_g2h_qoswt390.h"
+#endif /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+#endif /* RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT */
+
+static const struct rcar_gen3_dbsc_qos_settings g2h_v30_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218U },
+	{ DBSC_DBCAM0CNF2, 0x000000F4U },
+	{ DBSC_DBCAM0CNF3, 0x00000000U },
+	{ DBSC_DBSCHCNT0, 0x000F0037U },
+	{ DBSC_DBSCHSZ0, 0x00000001U },
+	{ DBSC_DBSCHRW0, 0x22421111U },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123U },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00U },
+	{ DBSC_DBSCHQOS01, 0x00000B00U },
+	{ DBSC_DBSCHQOS02, 0x00000000U },
+	{ DBSC_DBSCHQOS03, 0x00000000U },
+	{ DBSC_DBSCHQOS40, 0x00000300U },
+	{ DBSC_DBSCHQOS41, 0x000002F0U },
+	{ DBSC_DBSCHQOS42, 0x00000200U },
+	{ DBSC_DBSCHQOS43, 0x00000100U },
+	{ DBSC_DBSCHQOS90, 0x00000100U },
+	{ DBSC_DBSCHQOS91, 0x000000F0U },
+	{ DBSC_DBSCHQOS92, 0x000000A0U },
+	{ DBSC_DBSCHQOS93, 0x00000040U },
+	{ DBSC_DBSCHQOS120, 0x00000040U },
+	{ DBSC_DBSCHQOS121, 0x00000030U },
+	{ DBSC_DBSCHQOS122, 0x00000020U },
+	{ DBSC_DBSCHQOS123, 0x00000010U },
+	{ DBSC_DBSCHQOS130, 0x00000100U },
+	{ DBSC_DBSCHQOS131, 0x000000F0U },
+	{ DBSC_DBSCHQOS132, 0x000000A0U },
+	{ DBSC_DBSCHQOS133, 0x00000040U },
+	{ DBSC_DBSCHQOS140, 0x000000C0U },
+	{ DBSC_DBSCHQOS141, 0x000000B0U },
+	{ DBSC_DBSCHQOS142, 0x00000080U },
+	{ DBSC_DBSCHQOS143, 0x00000040U },
+	{ DBSC_DBSCHQOS150, 0x00000040U },
+	{ DBSC_DBSCHQOS151, 0x00000030U },
+	{ DBSC_DBSCHQOS152, 0x00000020U },
+	{ DBSC_DBSCHQOS153, 0x00000010U },
+};
+
+void qos_init_g2h_v30(void)
+{
+	unsigned int split_area;
+
+	rzg_qos_dbsc_setting(g2h_v30_qos, ARRAY_SIZE(g2h_v30_qos), true);
+
+	/* use 1(2GB) for RCAR_DRAM_LPDDR4_MEMCONF for G2H */
+	split_area = 0x1CU;
+
+	/* DRAM split address mapping */
+#if (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH)
+#if RCAR_LSI == RZ_G2H
+#error "Don't set DRAM Split 4ch(G2H)"
+#else /* RCAR_LSI == RZ_G2H */
+	ERROR("DRAM split 4ch not supported.(G2H)");
+	panic();
+#endif /* RCAR_LSI == RZ_G2H */
+#elif (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH) || \
+	(RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO)
+	NOTICE("BL2: DRAM Split is 2ch(DDR %x)\n", (int)qos_init_ddr_phyvalid);
+
+	mmio_write_32(AXI_ADSPLCR0, ADSPLCR0_AREA(split_area));
+	mmio_write_32(AXI_ADSPLCR1, ADSPLCR0_ADRMODE_DEFAULT |
+		    ADSPLCR0_SPLITSEL(0xFFU) | ADSPLCR0_AREA(split_area) |
+		    ADSPLCR0_SWP);
+	mmio_write_32(AXI_ADSPLCR2, 0x00001004U);
+	mmio_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else /* RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH */
+	mmio_write_32(AXI_ADSPLCR0, ADSPLCR0_AREA(split_area));
+	NOTICE("BL2: DRAM Split is OFF(DDR %x)\n", (int)qos_init_ddr_phyvalid);
+#endif /* RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH */
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+	NOTICE("BL2: DRAM refresh interval 1.95 usec\n");
+#else
+	NOTICE("BL2: DRAM refresh interval 3.9 usec\n");
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	NOTICE("BL2: Periodic Write DQ Training\n");
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	mmio_write_32(QOSCTRL_RAS, 0x00000044U);
+	mmio_write_64(QOSCTRL_DANN, 0x0404020002020201UL);
+	mmio_write_32(QOSCTRL_DANT, 0x0020100AU);
+	mmio_write_32(QOSCTRL_FSS, 0x0000000AU);
+	mmio_write_32(QOSCTRL_INSFC, 0x06330001U);
+	mmio_write_32(QOSCTRL_RACNT0, 0x00010003U);
+
+	/* GPU Boost Mode */
+	mmio_write_32(QOSCTRL_STATGEN0, 0x00000001U);
+
+	mmio_write_32(QOSCTRL_SL_INIT, SL_INIT_REFFSSLOT |
+		      SL_INIT_SLOTSSLOT | SL_INIT_SSLOTCLK_G2H);
+	mmio_write_32(QOSCTRL_REF_ARS, ((QOSCTRL_REF_ARS_ARBSTOPCYCLE_G2H << 16)));
+
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		mmio_write_64(QOSBW_FIX_QOS_BANK0 + i * 8U, mstat_fix[i]);
+		mmio_write_64(QOSBW_FIX_QOS_BANK1 + i * 8U, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		mmio_write_64(QOSBW_BE_QOS_BANK0 + i * 8U, mstat_be[i]);
+		mmio_write_64(QOSBW_BE_QOS_BANK1 + i * 8U, mstat_be[i]);
+	}
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	for (i = 0U; i < ARRAY_SIZE(qoswt_fix); i++) {
+		mmio_write_64(QOSWT_FIX_WTQOS_BANK0 + i * 8U, qoswt_fix[i]);
+		mmio_write_64(QOSWT_FIX_WTQOS_BANK1 + i * 8U, qoswt_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(qoswt_be); i++) {
+		mmio_write_64(QOSWT_BE_WTQOS_BANK0 + i * 8U, qoswt_be[i]);
+		mmio_write_64(QOSWT_BE_WTQOS_BANK1 + i * 8U, qoswt_be[i]);
+	}
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	/* AXI setting */
+	mmio_write_32(AXI_MMCR, 0x00010008U);
+	mmio_write_32(AXI_TR3CR, 0x00010000U);
+	mmio_write_32(AXI_TR4CR, 0x00010000U);
+
+	/* RT bus Leaf setting */
+	mmio_write_32(RT_ACT0, 0x00000000U);
+	mmio_write_32(RT_ACT1, 0x00000000U);
+
+	/* CCI bus Leaf setting */
+	mmio_write_32(CPU_ACT0, 0x00000003U);
+	mmio_write_32(CPU_ACT1, 0x00000003U);
+	mmio_write_32(CPU_ACT2, 0x00000003U);
+	mmio_write_32(CPU_ACT3, 0x00000003U);
+
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	/*  re-write training setting */
+	mmio_write_32(QOSWT_WTREF,
+		      ((QOSWT_WTREF_SLOT1_EN << 16) | QOSWT_WTREF_SLOT0_EN));
+	mmio_write_32(QOSWT_WTSET0,
+		      ((QOSWT_WTSET0_PERIOD0_G2H << 16) |
+		      (QOSWT_WTSET0_SSLOT0 << 8) | QOSWT_WTSET0_SLOTSLOT0));
+	mmio_write_32(QOSWT_WTSET1,
+		      ((QOSWT_WTSET1_PERIOD1_G2H << 16) |
+		      (QOSWT_WTSET1_SSLOT1 << 8) | QOSWT_WTSET1_SLOTSLOT1));
+
+	mmio_write_32(QOSWT_WTEN, QOSWT_WTEN_ENABLE);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	mmio_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rzg/qos/G2H/qos_init_g2h_v30.h b/drivers/renesas/rzg/qos/G2H/qos_init_g2h_v30.h
new file mode 100644
index 0000000..acd9627
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2H/qos_init_g2h_v30.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2H_V30_H
+#define QOS_INIT_G2H_V30_H
+
+void qos_init_g2h_v30(void);
+
+#endif /* QOS_INIT_G2H_V30_H */
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v10.c b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v10.c
new file mode 100644
index 0000000..ceaad25
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v10.c
@@ -0,0 +1,148 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "../qos_common.h"
+#include "qos_init_g2m_v10.h"
+#include "qos_init_g2m_v10_mstat.h"
+#include "qos_reg.h"
+
+#define RCAR_QOS_VERSION	"rev.0.19"
+
+static const struct rcar_gen3_dbsc_qos_settings g2m_v10_qos[] = {
+	/* BUFCAM settings */
+	/* DBSC_DBCAM0CNF0 not set */
+	{ DBSC_DBCAM0CNF1, 0x00043218U },
+	{ DBSC_DBCAM0CNF2, 0x000000F4U },
+	{ DBSC_DBCAM0CNF3, 0x00000000U },
+	{ DBSC_DBSCHCNT0, 0x080F0037U },
+	/* DBSC_DBSCHCNT1 not set */
+	{ DBSC_DBSCHSZ0, 0x00000001U },
+	{ DBSC_DBSCHRW0, 0x22421111U },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123U },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00U },
+	{ DBSC_DBSCHQOS01, 0x00000B00U },
+	{ DBSC_DBSCHQOS02, 0x00000000U },
+	{ DBSC_DBSCHQOS03, 0x00000000U },
+	{ DBSC_DBSCHQOS40, 0x00000300U },
+	{ DBSC_DBSCHQOS41, 0x000002F0U },
+	{ DBSC_DBSCHQOS42, 0x00000200U },
+	{ DBSC_DBSCHQOS43, 0x00000100U },
+	{ DBSC_DBSCHQOS90, 0x00000300U },
+	{ DBSC_DBSCHQOS91, 0x000002F0U },
+	{ DBSC_DBSCHQOS92, 0x00000200U },
+	{ DBSC_DBSCHQOS93, 0x00000100U },
+	{ DBSC_DBSCHQOS130, 0x00000100U },
+	{ DBSC_DBSCHQOS131, 0x000000F0U },
+	{ DBSC_DBSCHQOS132, 0x000000A0U },
+	{ DBSC_DBSCHQOS133, 0x00000040U },
+	{ DBSC_DBSCHQOS140, 0x000000C0U },
+	{ DBSC_DBSCHQOS141, 0x000000B0U },
+	{ DBSC_DBSCHQOS142, 0x00000080U },
+	{ DBSC_DBSCHQOS143, 0x00000040U },
+	{ DBSC_DBSCHQOS150, 0x00000040U },
+	{ DBSC_DBSCHQOS151, 0x00000030U },
+	{ DBSC_DBSCHQOS152, 0x00000020U },
+	{ DBSC_DBSCHQOS153, 0x00000010U },
+};
+
+void qos_init_g2m_v10(void)
+{
+	rzg_qos_dbsc_setting(g2m_v10_qos, ARRAY_SIZE(g2m_v10_qos), false);
+
+	/* DRAM split address mapping */
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH
+#if RCAR_LSI == RZ_G2M
+#error "Don't set DRAM Split 4ch(G2M)"
+#else /* RCAR_LSI == RZ_G2M */
+	ERROR("DRAM Split 4ch not supported.(G2M)");
+	panic();
+#endif /* RCAR_LSI == RZ_G2M */
+#elif (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH) || \
+	(RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO)
+	NOTICE("BL2: DRAM Split is 2ch\n");
+	mmio_write_32(AXI_ADSPLCR0, 0x00000000U);
+	mmio_write_32(AXI_ADSPLCR1, ADSPLCR0_ADRMODE_DEFAULT |
+		      ADSPLCR0_SPLITSEL(0xFFU) | ADSPLCR0_AREA(0x1CU) |
+		      ADSPLCR0_SWP);
+	mmio_write_32(AXI_ADSPLCR2, 0x089A0000U);
+	mmio_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else /* RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH */
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif /* RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH */
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+	/* Resource Alloc setting */
+	mmio_write_32(QOSCTRL_RAS, 0x00000028U);
+	mmio_write_32(QOSCTRL_FIXTH, 0x000F0005U);
+	mmio_write_32(QOSCTRL_REGGD, 0x00000000U);
+	mmio_write_64(QOSCTRL_DANN, 0x0101010102020201UL);
+	mmio_write_32(QOSCTRL_DANT, 0x00100804U);
+	mmio_write_32(QOSCTRL_EC, 0x00000000U);
+	mmio_write_64(QOSCTRL_EMS, 0x0000000000000000UL);
+	mmio_write_32(QOSCTRL_FSS, 0x000003e8U);
+	mmio_write_32(QOSCTRL_INSFC, 0xC7840001U);
+	mmio_write_32(QOSCTRL_BERR, 0x00000000U);
+	mmio_write_32(QOSCTRL_RACNT0, 0x00000000U);
+
+	/* QOSBW setting */
+	mmio_write_32(QOSCTRL_SL_INIT, SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT |
+		      SL_INIT_SSLOTCLK);
+	mmio_write_32(QOSCTRL_REF_ARS, 0x00330000U);
+
+	/* QOSBW SRAM setting */
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		mmio_write_64(QOSBW_FIX_QOS_BANK0 + i * 8U, mstat_fix[i]);
+		mmio_write_64(QOSBW_FIX_QOS_BANK1 + i * 8U, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		mmio_write_64(QOSBW_BE_QOS_BANK0 + i * 8U, mstat_be[i]);
+		mmio_write_64(QOSBW_BE_QOS_BANK1 + i * 8U, mstat_be[i]);
+	}
+
+	/* 3DG bus Leaf setting */
+	mmio_write_32(0xFD820808U, 0x00001234U);
+	mmio_write_32(0xFD820800U, 0x00000006U);
+	mmio_write_32(0xFD821800U, 0x00000006U);
+	mmio_write_32(0xFD822800U, 0x00000006U);
+	mmio_write_32(0xFD823800U, 0x00000006U);
+	mmio_write_32(0xFD824800U, 0x00000006U);
+	mmio_write_32(0xFD825800U, 0x00000006U);
+	mmio_write_32(0xFD826800U, 0x00000006U);
+	mmio_write_32(0xFD827800U, 0x00000006U);
+
+	/* RT bus Leaf setting */
+	mmio_write_32(0xFFC50800U, 0x00000000U);
+	mmio_write_32(0xFFC51800U, 0x00000000U);
+
+	/* Resource Alloc start */
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+	/* QOSBW start */
+	mmio_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+	NOTICE("BL2: QoS is None\n");
+
+	/* Resource Alloc setting */
+	mmio_write_32(QOSCTRL_EC, 0x00000000U);
+	/* Resource Alloc start */
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v10.h b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v10.h
new file mode 100644
index 0000000..627974a
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v10.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2M_V10_H
+#define QOS_INIT_G2M_V10_H
+
+void qos_init_g2m_v10(void);
+
+#endif /* QOS_INIT_G2M_V10_H */
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v10_mstat.h b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v10_mstat.h
new file mode 100644
index 0000000..c37915c
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v10_mstat.h
@@ -0,0 +1,232 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2M_V10_MSTAT_H
+#define QOS_INIT_G2M_V10_MSTAT_H
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+static const uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004030000FFFFUL,
+	/* 0x0038, */ 0x001004030000FFFFUL,
+	/* 0x0040, */ 0x001414090000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001410010000FFFFUL,
+	/* 0x0058, */ 0x00140C090000FFFFUL,
+	/* 0x0060, */ 0x00140C090000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001410010000FFFFUL,
+	/* 0x0078, */ 0x001004020000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001414090000FFFFUL,
+	/* 0x0090, */ 0x001408060000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00A0, */ 0x000C08020000FFFFUL,
+	/* 0x00A8, */ 0x000C04010000FFFFUL,
+	/* 0x00B0, */ 0x000C04010000FFFFUL,
+	/* 0x00B8, */ 0x0000000000000000UL,
+	/* 0x00C0, */ 0x000C08020000FFFFUL,
+	/* 0x00C8, */ 0x000C04010000FFFFUL,
+	/* 0x00D0, */ 0x000C04010000FFFFUL,
+	/* 0x00D8, */ 0x000C04030000FFFFUL,
+	/* 0x00E0, */ 0x000C100F0000FFFFUL,
+	/* 0x00E8, */ 0x0000000000000000UL,
+	/* 0x00F0, */ 0x001010080000FFFFUL,
+	/* 0x00F8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001010080000FFFFUL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100C0A0000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00100C0A0000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x00100C0A0000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008050000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001028280000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01A0, */ 0x00100C0A0000FFFFUL,
+	/* 0x01A8, */ 0x0000000000000000UL,
+	/* 0x01B0, */ 0x0000000000000000UL,
+	/* 0x01B8, */ 0x0000000000000000UL,
+	/* 0x01C0, */ 0x0000000000000000UL,
+	/* 0x01C8, */ 0x0000000000000000UL,
+	/* 0x01D0, */ 0x0000000000000000UL,
+	/* 0x01D8, */ 0x0000000000000000UL,
+	/* 0x01E0, */ 0x0000000000000000UL,
+	/* 0x01E8, */ 0x0000000000000000UL,
+	/* 0x01F0, */ 0x0000000000000000UL,
+	/* 0x01F8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x001408010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02A0, */ 0x000C04010000FFFFUL,
+	/* 0x02A8, */ 0x000C04010000FFFFUL,
+	/* 0x02B0, */ 0x001404010000FFFFUL,
+	/* 0x02B8, */ 0x0000000000000000UL,
+	/* 0x02C0, */ 0x0000000000000000UL,
+	/* 0x02C8, */ 0x0000000000000000UL,
+	/* 0x02D0, */ 0x000C04010000FFFFUL,
+	/* 0x02D8, */ 0x000C04010000FFFFUL,
+	/* 0x02E0, */ 0x001404010000FFFFUL,
+	/* 0x02E8, */ 0x0000000000000000UL,
+	/* 0x02F0, */ 0x0000000000000000UL,
+	/* 0x02F8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static const uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x001200100C89C401UL,
+	/* 0x0008, */ 0x001200100C89C401UL,
+	/* 0x0010, */ 0x001200100C89C401UL,
+	/* 0x0018, */ 0x001200100C89C401UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x001100100C803401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00A0, */ 0x0000000000000000UL,
+	/* 0x00A8, */ 0x0000000000000000UL,
+	/* 0x00B0, */ 0x0000000000000000UL,
+	/* 0x00B8, */ 0x0000000000000000UL,
+	/* 0x00C0, */ 0x0000000000000000UL,
+	/* 0x00C8, */ 0x0000000000000000UL,
+	/* 0x00D0, */ 0x0000000000000000UL,
+	/* 0x00D8, */ 0x0000000000000000UL,
+	/* 0x00E0, */ 0x0000000000000000UL,
+	/* 0x00E8, */ 0x0000000000000000UL,
+	/* 0x00F0, */ 0x0000000000000000UL,
+	/* 0x00F8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01A0, */ 0x0000000000000000UL,
+	/* 0x01A8, */ 0x0000000000000000UL,
+	/* 0x01B0, */ 0x0000000000000000UL,
+	/* 0x01B8, */ 0x0000000000000000UL,
+	/* 0x01C0, */ 0x001100500C8FFC01UL,
+	/* 0x01C8, */ 0x001100500C8FFC01UL,
+	/* 0x01D0, */ 0x001100500C8FFC01UL,
+	/* 0x01D8, */ 0x001100500C8FFC01UL,
+	/* 0x01E0, */ 0x0000000000000000UL,
+	/* 0x01E8, */ 0x001200100C803401UL,
+	/* 0x01F0, */ 0x001100100C80FC01UL,
+	/* 0x01F8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x001200100C80FC01UL,
+	/* 0x0210, */ 0x001100100C80FC01UL,
+	/* 0x0218, */ 0x001100100C825801UL,
+	/* 0x0220, */ 0x001100100C825801UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x001100100C825801UL,
+	/* 0x0238, */ 0x001100100C825801UL,
+	/* 0x0240, */ 0x001200100C8BB801UL,
+	/* 0x0248, */ 0x001100100C8EA401UL,
+	/* 0x0250, */ 0x001200100C8BB801UL,
+	/* 0x0258, */ 0x001100100C8EA401UL,
+	/* 0x0260, */ 0x001100100C84E401UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x001100100C81F401UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02A0, */ 0x0000000000000000UL,
+	/* 0x02A8, */ 0x0000000000000000UL,
+	/* 0x02B0, */ 0x0000000000000000UL,
+	/* 0x02B8, */ 0x001100100C803401UL,
+	/* 0x02C0, */ 0x0000000000000000UL,
+	/* 0x02C8, */ 0x0000000000000000UL,
+	/* 0x02D0, */ 0x0000000000000000UL,
+	/* 0x02D8, */ 0x0000000000000000UL,
+	/* 0x02E0, */ 0x0000000000000000UL,
+	/* 0x02E8, */ 0x001100100C803401UL,
+	/* 0x02F0, */ 0x001100300C8FFC01UL,
+	/* 0x02F8, */ 0x001100500C8FFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x001100300C8FFC01UL,
+	/* 0x0310, */ 0x001100500C8FFC01UL,
+	/* 0x0318, */ 0x001200100C803401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+#endif /* RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT */
+
+#endif /* QOS_INIT_G2M_V10_MSTAT_H */
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11.c b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11.c
new file mode 100644
index 0000000..db61858
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11.c
@@ -0,0 +1,218 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "../qos_common.h"
+#include "qos_init_g2m_v11.h"
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_g2m_v11_mstat195.h"
+#else /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+#include "qos_init_g2m_v11_mstat390.h"
+#endif /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_g2m_v11_qoswt195.h"
+#else /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+#include "qos_init_g2m_v11_qoswt390.h"
+#endif /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+#endif /* RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT */
+#include "qos_reg.h"
+
+#define RCAR_QOS_VERSION			"rev.0.19"
+
+#define QOSWT_TIME_BANK0			20000000U	/* unit:ns */
+
+#define QOSWT_WTEN_ENABLE			0x1U
+
+#define QOSCTRL_REF_ARS_ARBSTOPCYCLE_G2M_11	(SL_INIT_SSLOTCLK_G2M_11 - 0x5U)
+
+#define OSWT_WTREF_SLOT0_EN_REQ1_SLOT		3U
+#define OSWT_WTREF_SLOT0_EN_REQ2_SLOT		9U
+#define QOSWT_WTREF_SLOT0_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+#define QOSWT_WTREF_SLOT1_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+
+#define QOSWT_WTSET0_REQ_SSLOT0			5U
+#define WT_BASE_SUB_SLOT_NUM0			12U
+#define QOSWT_WTSET0_PERIOD0_G2M_11			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_G2M_11) - 1U)
+#define QOSWT_WTSET0_SSLOT0			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET0_SLOTSLOT0			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#define QOSWT_WTSET1_PERIOD1_G2M_11			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_G2M_11) - 1U)
+#define QOSWT_WTSET1_SSLOT1			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET1_SLOTSLOT1			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+static const struct rcar_gen3_dbsc_qos_settings g2m_v11_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218U },
+	{ DBSC_DBCAM0CNF2, 0x000000F4U },
+	{ DBSC_DBCAM0CNF3, 0x00000000U },
+	{ DBSC_DBSCHCNT0, 0x000F0037U },
+	{ DBSC_DBSCHSZ0, 0x00000001U },
+	{ DBSC_DBSCHRW0, 0x22421111U },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123U },
+
+	/* QoS settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00U },
+	{ DBSC_DBSCHQOS01, 0x00000B00U },
+	{ DBSC_DBSCHQOS02, 0x00000000U },
+	{ DBSC_DBSCHQOS03, 0x00000000U },
+	{ DBSC_DBSCHQOS40, 0x00000300U },
+	{ DBSC_DBSCHQOS41, 0x000002F0U },
+	{ DBSC_DBSCHQOS42, 0x00000200U },
+	{ DBSC_DBSCHQOS43, 0x00000100U },
+	{ DBSC_DBSCHQOS90, 0x00000100U },
+	{ DBSC_DBSCHQOS91, 0x000000F0U },
+	{ DBSC_DBSCHQOS92, 0x000000A0U },
+	{ DBSC_DBSCHQOS93, 0x00000040U },
+	{ DBSC_DBSCHQOS120, 0x00000040U },
+	{ DBSC_DBSCHQOS121, 0x00000030U },
+	{ DBSC_DBSCHQOS122, 0x00000020U },
+	{ DBSC_DBSCHQOS123, 0x00000010U },
+	{ DBSC_DBSCHQOS130, 0x00000100U },
+	{ DBSC_DBSCHQOS131, 0x000000F0U },
+	{ DBSC_DBSCHQOS132, 0x000000A0U },
+	{ DBSC_DBSCHQOS133, 0x00000040U },
+	{ DBSC_DBSCHQOS140, 0x000000C0U },
+	{ DBSC_DBSCHQOS141, 0x000000B0U },
+	{ DBSC_DBSCHQOS142, 0x00000080U },
+	{ DBSC_DBSCHQOS143, 0x00000040U },
+	{ DBSC_DBSCHQOS150, 0x00000040U },
+	{ DBSC_DBSCHQOS151, 0x00000030U },
+	{ DBSC_DBSCHQOS152, 0x00000020U },
+	{ DBSC_DBSCHQOS153, 0x00000010U },
+};
+
+void qos_init_g2m_v11(void)
+{
+	uint32_t i;
+
+	rzg_qos_dbsc_setting(g2m_v11_qos, ARRAY_SIZE(g2m_v11_qos), false);
+
+	/* DRAM Split Address mapping */
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH
+#if RCAR_LSI == RZ_G2M
+#error "Don't set DRAM Split 4ch(G2M)"
+#else /* RCAR_LSI == RZ_G2M */
+	ERROR("DRAM Split 4ch not supported.(G2M)");
+	panic();
+#endif /* RCAR_LSI == RZ_G2M */
+#elif (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH) || \
+	(RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO)
+	NOTICE("BL2: DRAM Split is 2ch\n");
+	mmio_write_32(AXI_ADSPLCR0, 0x00000000U);
+	mmio_write_32(AXI_ADSPLCR1, ADSPLCR0_ADRMODE_DEFAULT |
+		      ADSPLCR0_SPLITSEL(0xFFU) | ADSPLCR0_AREA(0x1CU) |
+		      ADSPLCR0_SWP);
+	mmio_write_32(AXI_ADSPLCR2, 0x00001004U);
+	mmio_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else /* RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH */
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif /* RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH */
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif /* RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT */
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+	NOTICE("BL2: DRAM refresh interval 1.95 usec\n");
+#else /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+	NOTICE("BL2: DRAM refresh interval 3.9 usec\n");
+#endif /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	NOTICE("BL2: Periodic Write DQ Training\n");
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	mmio_write_32(QOSCTRL_RAS, 0x00000044U);
+	mmio_write_64(QOSCTRL_DANN, 0x0404020002020201UL);
+	mmio_write_32(QOSCTRL_DANT, 0x0020100AU);
+	mmio_write_32(QOSCTRL_INSFC, 0x06330001U);
+	mmio_write_32(QOSCTRL_RACNT0, 0x02010003U); /* GPU Boost Mode ON */
+
+	mmio_write_32(QOSCTRL_SL_INIT,
+		    SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT |
+		    SL_INIT_SSLOTCLK_G2M_11);
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	mmio_write_32(QOSCTRL_REF_ARS,
+		      QOSCTRL_REF_ARS_ARBSTOPCYCLE_G2M_11 << 16);
+#else /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+	mmio_write_32(QOSCTRL_REF_ARS, 0x00330000U);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		mmio_write_64(QOSBW_FIX_QOS_BANK0 + i * 8U, mstat_fix[i]);
+		mmio_write_64(QOSBW_FIX_QOS_BANK1 + i * 8U, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		mmio_write_64(QOSBW_BE_QOS_BANK0 + i * 8U, mstat_be[i]);
+		mmio_write_64(QOSBW_BE_QOS_BANK1 + i * 8U, mstat_be[i]);
+	}
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	for (i = 0U; i < ARRAY_SIZE(qoswt_fix); i++) {
+		mmio_write_64(QOSWT_FIX_WTQOS_BANK0 + i * 8U, qoswt_fix[i]);
+		mmio_write_64(QOSWT_FIX_WTQOS_BANK1 + i * 8U, qoswt_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(qoswt_be); i++) {
+		mmio_write_64(QOSWT_BE_WTQOS_BANK0 + i * 8U, qoswt_be[i]);
+		mmio_write_64(QOSWT_BE_WTQOS_BANK1 + i * 8U, qoswt_be[i]);
+	}
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	/* 3DG bus Leaf setting */
+	mmio_write_32(GPU_ACT_GRD, 0x00001234U);
+	mmio_write_32(GPU_ACT0, 0x00000000U);
+	mmio_write_32(GPU_ACT1, 0x00000000U);
+	mmio_write_32(GPU_ACT2, 0x00000000U);
+	mmio_write_32(GPU_ACT3, 0x00000000U);
+
+	/* RT bus Leaf setting */
+	mmio_write_32(RT_ACT0, 0x00000000U);
+	mmio_write_32(RT_ACT1, 0x00000000U);
+
+	/* CCI bus Leaf setting */
+	mmio_write_32(CPU_ACT0, 0x00000003U);
+	mmio_write_32(CPU_ACT1, 0x00000003U);
+	mmio_write_32(CPU_ACT2, 0x00000003U);
+	mmio_write_32(CPU_ACT3, 0x00000003U);
+
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	/*  re-write training setting */
+	mmio_write_32(QOSWT_WTREF,
+		      (QOSWT_WTREF_SLOT1_EN << 16) | QOSWT_WTREF_SLOT0_EN);
+	mmio_write_32(QOSWT_WTSET0,
+		      (QOSWT_WTSET0_PERIOD0_G2M_11 << 16) |
+		      (QOSWT_WTSET0_SSLOT0 << 8) | QOSWT_WTSET0_SLOTSLOT0);
+	mmio_write_32(QOSWT_WTSET1,
+		      (QOSWT_WTSET1_PERIOD1_G2M_11 << 16) |
+		      (QOSWT_WTSET1_SSLOT1 << 8) | QOSWT_WTSET1_SLOTSLOT1);
+
+	mmio_write_32(QOSWT_WTEN, QOSWT_WTEN_ENABLE);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	mmio_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+	NOTICE("BL2: QoS is None\n");
+
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11.h b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11.h
new file mode 100644
index 0000000..d042493
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2M_V11_H
+#define QOS_INIT_G2M_V11_H
+
+void qos_init_g2m_v11(void);
+
+#endif /* QOS_INIT_G2M_V11_H */
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11_mstat195.h b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11_mstat195.h
new file mode 100644
index 0000000..950abd6
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11_mstat195.h
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2M_V11_MSTAT195_H
+#define QOS_INIT_G2M_V11_MSTAT195_H
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000FFFFUL,
+	/* 0x0038, */ 0x001004040000FFFFUL,
+	/* 0x0040, */ 0x001414090000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x00140C0A0000FFFFUL,
+	/* 0x0060, */ 0x00140C0A0000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x001004030000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001414090000FFFFUL,
+	/* 0x0090, */ 0x001408070000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C04020000FFFFUL,
+	/* 0x00a8, */ 0x000C04010000FFFFUL,
+	/* 0x00b0, */ 0x000C04010000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C04020000FFFFUL,
+	/* 0x00c8, */ 0x000C04010000FFFFUL,
+	/* 0x00d0, */ 0x000C04010000FFFFUL,
+	/* 0x00d8, */ 0x000C08050000FFFFUL,
+	/* 0x00e0, */ 0x000C14120000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100C0B0000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0010100D0000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x00100C0B0000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008060000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00102C2C0000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x00100C0B0000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x001408010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x001200100BD03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100600BDFFC01UL,
+	/* 0x01c8, */ 0x002100600BDFFC01UL,
+	/* 0x01d0, */ 0x002100600BDFFC01UL,
+	/* 0x01d8, */ 0x002100600BDFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x001100200BDFFC01UL,
+	/* 0x0220, */ 0x001100200BDFFC01UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x001100200BDFFC01UL,
+	/* 0x0238, */ 0x001100200BDFFC01UL,
+	/* 0x0240, */ 0x001200200BDFFC01UL,
+	/* 0x0248, */ 0x001100200BDFFC01UL,
+	/* 0x0250, */ 0x001200200BDFFC01UL,
+	/* 0x0258, */ 0x001100200BDFFC01UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x001100400BDFFC01UL,
+	/* 0x02f8, */ 0x001100600BDFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x001100400BDFFC01UL,
+	/* 0x0310, */ 0x001100600BDFFC01UL,
+	/* 0x0318, */ 0x001200100BD03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+#endif /* QOS_INIT_G2M_V11_MSTAT195_H */
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11_mstat390.h b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11_mstat390.h
new file mode 100644
index 0000000..5c6fd24
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11_mstat390.h
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2M_V11_MSTAT390_H
+#define QOS_INIT_G2M_V11_MSTAT390_H
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000FFFFUL,
+	/* 0x0038, */ 0x001008070000FFFFUL,
+	/* 0x0040, */ 0x001424120000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x001414130000FFFFUL,
+	/* 0x0060, */ 0x001414130000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x001008050000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001424120000FFFFUL,
+	/* 0x0090, */ 0x0014100D0000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C08040000FFFFUL,
+	/* 0x00a8, */ 0x000C04020000FFFFUL,
+	/* 0x00b0, */ 0x000C04020000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C08040000FFFFUL,
+	/* 0x00c8, */ 0x000C04020000FFFFUL,
+	/* 0x00d0, */ 0x000C04020000FFFFUL,
+	/* 0x00d8, */ 0x000C0C0A0000FFFFUL,
+	/* 0x00e0, */ 0x000C24230000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001044110000FFFFUL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001014110000FFFFUL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x001018150000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00101C190000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x001018150000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x00100C0B0000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001058570000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x001018150000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFFFUL,
+	/* 0x0268, */ 0x001410010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C08020000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x00140C010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x00140C010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0012003005EFFC01UL,
+	/* 0x0008, */ 0x0012003005EFFC01UL,
+	/* 0x0010, */ 0x0012003005EFFC01UL,
+	/* 0x0018, */ 0x0012003005EFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001005E03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100B005EFFC01UL,
+	/* 0x01c8, */ 0x002100B005EFFC01UL,
+	/* 0x01d0, */ 0x002100B005EFFC01UL,
+	/* 0x01d8, */ 0x002100B005EFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021003005EFFC01UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021003005EFFC01UL,
+	/* 0x0218, */ 0x0011003005EFFC01UL,
+	/* 0x0220, */ 0x0011003005EFFC01UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011003005EFFC01UL,
+	/* 0x0238, */ 0x0011003005EFFC01UL,
+	/* 0x0240, */ 0x0012003005EFFC01UL,
+	/* 0x0248, */ 0x0011003005EFFC01UL,
+	/* 0x0250, */ 0x0012003005EFFC01UL,
+	/* 0x0258, */ 0x0011003005EFFC01UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011007005EFFC01UL,
+	/* 0x02f8, */ 0x001100B005EFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0011007005EFFC01UL,
+	/* 0x0310, */ 0x001100B005EFFC01UL,
+	/* 0x0318, */ 0x0012001005E03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+#endif /* QOS_INIT_G2M_V11_MSTAT390_H */
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11_qoswt195.h b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11_qoswt195.h
new file mode 100644
index 0000000..f526a82
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11_qoswt195.h
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2M_V11_QOSWT195_H
+#define QOS_INIT_G2M_V11_QOSWT195_H
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000C010UL,
+	/* 0x0038, */ 0x001004040000C010UL,
+	/* 0x0040, */ 0x001414090000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x00140C0A0000C010UL,
+	/* 0x0060, */ 0x00140C0A0000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x001004030000C010UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001414090000FFF0UL,
+	/* 0x0090, */ 0x001408070000C010UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFF0UL,
+	/* 0x0268, */ 0x001408010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C04010000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+#endif /* QOS_INIT_G2M_V11_QOSWT195_H */
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11_qoswt390.h b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11_qoswt390.h
new file mode 100644
index 0000000..bfb80e3
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11_qoswt390.h
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2M_V11_QOSWT390_H
+#define QOS_INIT_G2M_V11_QOSWT390_H
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000C010UL,
+	/* 0x0038, */ 0x001008070000C010UL,
+	/* 0x0040, */ 0x001424120000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x001414130000C010UL,
+	/* 0x0060, */ 0x001414130000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x001008050000C010UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001424120000FFF0UL,
+	/* 0x0090, */ 0x0014100D0000C010UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFF0UL,
+	/* 0x0268, */ 0x001410010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C08020000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+#endif /* QOS_INIT_G2M_V11_QOSWT390_H */
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30.c b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30.c
new file mode 100644
index 0000000..321cd2b
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30.c
@@ -0,0 +1,210 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "../qos_common.h"
+#include "qos_init_g2m_v30.h"
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_g2m_v30_mstat195.h"
+#else /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+#include "qos_init_g2m_v30_mstat390.h"
+#endif /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_g2m_v30_qoswt195.h"
+#else /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+#include "qos_init_g2m_v30_qoswt390.h"
+#endif /* RCAR_REF_INT == RCAR_REF_DEFAULT */
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+#endif /* RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT */
+#include "qos_reg.h"
+
+#define RCAR_QOS_VERSION			"rev.0.04"
+
+#define QOSWT_TIME_BANK0			20000000U	/* unit:ns */
+
+#define QOSWT_WTEN_ENABLE			0x1U
+
+#define QOSCTRL_REF_ARS_ARBSTOPCYCLE_G2M_30	(SL_INIT_SSLOTCLK_G2M_30 - 0x5U)
+
+#define OSWT_WTREF_SLOT0_EN_REQ1_SLOT		3U
+#define OSWT_WTREF_SLOT0_EN_REQ2_SLOT		9U
+#define QOSWT_WTREF_SLOT0_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+#define QOSWT_WTREF_SLOT1_EN				\
+	((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) |	\
+	(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+
+#define QOSWT_WTSET0_REQ_SSLOT0			5U
+#define WT_BASE_SUB_SLOT_NUM0			12U
+#define QOSWT_WTSET0_PERIOD0_G2M_30			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_G2M_30) - 1U)
+#define QOSWT_WTSET0_SSLOT0			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET0_SLOTSLOT0			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#define QOSWT_WTSET1_PERIOD1_G2M_30			\
+	((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_G2M_30) - 1U)
+#define QOSWT_WTSET1_SSLOT1			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET1_SLOTSLOT1			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+static const struct rcar_gen3_dbsc_qos_settings g2m_v30_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218U },
+	{ DBSC_DBCAM0CNF2, 0x000000F4U },
+	{ DBSC_DBCAM0CNF3, 0x00000000U },
+	{ DBSC_DBSCHCNT0, 0x000F0037U },
+	{ DBSC_DBSCHSZ0, 0x00000001U },
+	{ DBSC_DBSCHRW0, 0x22421111U },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123U },
+
+	/* QoS settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00U },
+	{ DBSC_DBSCHQOS01, 0x00000B00U },
+	{ DBSC_DBSCHQOS02, 0x00000000U },
+	{ DBSC_DBSCHQOS03, 0x00000000U },
+	{ DBSC_DBSCHQOS40, 0x00000300U },
+	{ DBSC_DBSCHQOS41, 0x000002F0U },
+	{ DBSC_DBSCHQOS42, 0x00000200U },
+	{ DBSC_DBSCHQOS43, 0x00000100U },
+	{ DBSC_DBSCHQOS90, 0x00000100U },
+	{ DBSC_DBSCHQOS91, 0x000000F0U },
+	{ DBSC_DBSCHQOS92, 0x000000A0U },
+	{ DBSC_DBSCHQOS93, 0x00000040U },
+	{ DBSC_DBSCHQOS120, 0x00000040U },
+	{ DBSC_DBSCHQOS121, 0x00000030U },
+	{ DBSC_DBSCHQOS122, 0x00000020U },
+	{ DBSC_DBSCHQOS123, 0x00000010U },
+	{ DBSC_DBSCHQOS130, 0x00000100U },
+	{ DBSC_DBSCHQOS131, 0x000000F0U },
+	{ DBSC_DBSCHQOS132, 0x000000A0U },
+	{ DBSC_DBSCHQOS133, 0x00000040U },
+	{ DBSC_DBSCHQOS140, 0x000000C0U },
+	{ DBSC_DBSCHQOS141, 0x000000B0U },
+	{ DBSC_DBSCHQOS142, 0x00000080U },
+	{ DBSC_DBSCHQOS143, 0x00000040U },
+	{ DBSC_DBSCHQOS150, 0x00000040U },
+	{ DBSC_DBSCHQOS151, 0x00000030U },
+	{ DBSC_DBSCHQOS152, 0x00000020U },
+	{ DBSC_DBSCHQOS153, 0x00000010U },
+};
+
+void qos_init_g2m_v30(void)
+{
+	uint32_t i;
+
+	rzg_qos_dbsc_setting(g2m_v30_qos, ARRAY_SIZE(g2m_v30_qos), true);
+
+	/* DRAM Split Address mapping */
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH
+#if RCAR_LSI == RZ_G2M
+  #error "Don't set DRAM Split 4ch(G2M)"
+#else /* RCAR_LSI == RZ_G2M */
+	ERROR("DRAM Split 4ch not supported.(G2M)");
+	panic();
+#endif /* RCAR_LSI == RZ_G2M */
+#elif (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH) || \
+	(RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_AUTO)
+	NOTICE("BL2: DRAM Split is 2ch\n");
+	mmio_write_32(AXI_ADSPLCR0, 0x00000000U);
+	mmio_write_32(AXI_ADSPLCR1, ADSPLCR0_ADRMODE_DEFAULT |
+		      ADSPLCR0_SPLITSEL(0xFFU) | ADSPLCR0_AREA(0x1DU) |
+		      ADSPLCR0_SWP);
+	mmio_write_32(AXI_ADSPLCR2, 0x00001004U);
+	mmio_write_32(AXI_ADSPLCR3, 0x00000000U);
+#else /* RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH */
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif /* RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH */
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+	NOTICE("BL2: DRAM refresh interval 1.95 usec\n");
+#else /*  RCAR_REF_INT == RCAR_REF_DEFAULT */
+	NOTICE("BL2: DRAM refresh interval 3.9 usec\n");
+#endif /*  RCAR_REF_INT == RCAR_REF_DEFAULT */
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	NOTICE("BL2: Periodic Write DQ Training\n");
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	mmio_write_32(QOSCTRL_RAS, 0x00000044U);
+	mmio_write_64(QOSCTRL_DANN, 0x0404020002020201UL);
+	mmio_write_32(QOSCTRL_DANT, 0x0020100AU);
+	mmio_write_32(QOSCTRL_FSS, 0x0000000AU);
+	mmio_write_32(QOSCTRL_INSFC, 0x06330001U);
+	mmio_write_32(QOSCTRL_EARLYR, 0x00000001U);
+	mmio_write_32(QOSCTRL_RACNT0, 0x02010003U); /* GPU Boost Mode ON */
+
+	/* GPU Boost Mode */
+	mmio_write_32(QOSCTRL_STATGEN0, 0x00000001U);
+
+	mmio_write_32(QOSCTRL_SL_INIT,
+		      SL_INIT_REFFSSLOT | SL_INIT_SLOTSSLOT |
+		      SL_INIT_SSLOTCLK_G2M_30);
+	mmio_write_32(QOSCTRL_REF_ARS,
+		      QOSCTRL_REF_ARS_ARBSTOPCYCLE_G2M_30 << 16);
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		mmio_write_64(QOSBW_FIX_QOS_BANK0 + i * 8U, mstat_fix[i]);
+		mmio_write_64(QOSBW_FIX_QOS_BANK1 + i * 8U, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		mmio_write_64(QOSBW_BE_QOS_BANK0 + i * 8U, mstat_be[i]);
+		mmio_write_64(QOSBW_BE_QOS_BANK1 + i * 8U, mstat_be[i]);
+	}
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	for (i = 0U; i < ARRAY_SIZE(qoswt_fix); i++) {
+		mmio_write_64(QOSWT_FIX_WTQOS_BANK0 + i * 8U, qoswt_fix[i]);
+		mmio_write_64(QOSWT_FIX_WTQOS_BANK1 + i * 8U, qoswt_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(qoswt_be); i++) {
+		mmio_write_64(QOSWT_BE_WTQOS_BANK0 + i * 8U, qoswt_be[i]);
+		mmio_write_64(QOSWT_BE_WTQOS_BANK1 + i * 8U, qoswt_be[i]);
+	}
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	/* RT bus Leaf setting */
+	mmio_write_32(RT_ACT0, 0x00000000U);
+	mmio_write_32(RT_ACT1, 0x00000000U);
+
+	/* CCI bus Leaf setting */
+	mmio_write_32(CPU_ACT0, 0x00000003U);
+	mmio_write_32(CPU_ACT1, 0x00000003U);
+	mmio_write_32(CPU_ACT2, 0x00000003U);
+	mmio_write_32(CPU_ACT3, 0x00000003U);
+
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	/*  re-write training setting */
+	mmio_write_32(QOSWT_WTREF,
+		      (QOSWT_WTREF_SLOT1_EN << 16) | QOSWT_WTREF_SLOT0_EN);
+	mmio_write_32(QOSWT_WTSET0, (QOSWT_WTSET0_PERIOD0_G2M_30 << 16) |
+		      (QOSWT_WTSET0_SSLOT0 << 8) | QOSWT_WTSET0_SLOTSLOT0);
+	mmio_write_32(QOSWT_WTSET1, (QOSWT_WTSET1_PERIOD1_G2M_30 << 16) |
+		      (QOSWT_WTSET1_SSLOT1 << 8) | QOSWT_WTSET1_SLOTSLOT1);
+
+	mmio_write_32(QOSWT_WTEN,   QOSWT_WTEN_ENABLE);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	mmio_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+	NOTICE("BL2: QoS is None\n");
+
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30.h b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30.h
new file mode 100644
index 0000000..f89eabf
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2M_V30_H
+#define QOS_INIT_G2M_V30_H
+
+void qos_init_g2m_v30(void);
+
+#endif	/* QOS_INIT_G2M_V30_H */
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30_mstat195.h b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30_mstat195.h
new file mode 100644
index 0000000..fd15788
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30_mstat195.h
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2M_V30_MSTAT195_H
+#define QOS_INIT_G2M_V30_MSTAT195_H
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000FFFFUL,
+	/* 0x0038, */ 0x001004040000FFFFUL,
+	/* 0x0040, */ 0x001414090000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x00140C0A0000FFFFUL,
+	/* 0x0060, */ 0x00140C0A0000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x001004030000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001414090000FFFFUL,
+	/* 0x0090, */ 0x001408070000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C04020000FFFFUL,
+	/* 0x00a8, */ 0x000C04010000FFFFUL,
+	/* 0x00b0, */ 0x000C04010000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C04020000FFFFUL,
+	/* 0x00c8, */ 0x000C04010000FFFFUL,
+	/* 0x00d0, */ 0x000C04010000FFFFUL,
+	/* 0x00d8, */ 0x000C08050000FFFFUL,
+	/* 0x00e0, */ 0x000C10100000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001024090000FFFFUL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x00100C090000FFFFUL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x000C10100000FFFFUL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100C0B0000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0010100D0000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x00100C0B0000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008060000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00102C2C0000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x00100C0B0000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFFFUL,
+	/* 0x0268, */ 0x001408010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C04010000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x001408010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x001408010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x001200200BDFFC01UL,
+	/* 0x0008, */ 0x001200200BDFFC01UL,
+	/* 0x0010, */ 0x001200200BDFFC01UL,
+	/* 0x0018, */ 0x001200200BDFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x001200100BD03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100600BDFFC01UL,
+	/* 0x01c8, */ 0x002100600BDFFC01UL,
+	/* 0x01d0, */ 0x002100600BDFFC01UL,
+	/* 0x01d8, */ 0x002100600BDFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x002100200BDFFC01UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x002100200BDFFC01UL,
+	/* 0x0218, */ 0x001100200BDFFC01UL,
+	/* 0x0220, */ 0x001100200BDFFC01UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x001100200BDFFC01UL,
+	/* 0x0238, */ 0x001100200BDFFC01UL,
+	/* 0x0240, */ 0x001200200BDFFC01UL,
+	/* 0x0248, */ 0x001100200BDFFC01UL,
+	/* 0x0250, */ 0x001200200BDFFC01UL,
+	/* 0x0258, */ 0x001100200BDFFC01UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x001100400BDFFC01UL,
+	/* 0x02f8, */ 0x001100600BDFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x001100400BDFFC01UL,
+	/* 0x0310, */ 0x001100600BDFFC01UL,
+	/* 0x0318, */ 0x001200100BD03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+#endif	/* QOS_INIT_G2M_V30_MSTAT195_H */
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30_mstat390.h b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30_mstat390.h
new file mode 100644
index 0000000..aa2036d
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30_mstat390.h
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2M_V30_MSTAT390_H
+#define QOS_INIT_G2M_V30_MSTAT390_H
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000FFFFUL,
+	/* 0x0038, */ 0x001008070000FFFFUL,
+	/* 0x0040, */ 0x001424120000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404010000FFFFUL,
+	/* 0x0058, */ 0x001414130000FFFFUL,
+	/* 0x0060, */ 0x001414130000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404010000FFFFUL,
+	/* 0x0078, */ 0x001008050000FFFFUL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001424120000FFFFUL,
+	/* 0x0090, */ 0x0014100D0000FFFFUL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C08040000FFFFUL,
+	/* 0x00a8, */ 0x000C04020000FFFFUL,
+	/* 0x00b0, */ 0x000C04020000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C08040000FFFFUL,
+	/* 0x00c8, */ 0x000C04020000FFFFUL,
+	/* 0x00d0, */ 0x000C04020000FFFFUL,
+	/* 0x00d8, */ 0x000C0C0A0000FFFFUL,
+	/* 0x00e0, */ 0x000C201F0000FFFFUL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001044110000FFFFUL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001014110000FFFFUL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x000C201F0000FFFFUL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x001018150000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00101C190000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x001018150000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x00100C0B0000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001058570000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x001018150000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x000C04010000FFFFUL,
+	/* 0x01d8, */ 0x000C04010000FFFFUL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04010000FFFFUL,
+	/* 0x01f0, */ 0x000C04010000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04010000FFFFUL,
+	/* 0x0210, */ 0x000C04010000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFFFUL,
+	/* 0x0268, */ 0x001410010000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x000C08020000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04010000FFFFUL,
+	/* 0x02a8, */ 0x000C04010000FFFFUL,
+	/* 0x02b0, */ 0x00140C010000FFFFUL,
+	/* 0x02b8, */ 0x000C04010000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04010000FFFFUL,
+	/* 0x02d8, */ 0x000C04010000FFFFUL,
+	/* 0x02e0, */ 0x00140C010000FFFFUL,
+	/* 0x02e8, */ 0x000C04010000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0012003005EFFC01UL,
+	/* 0x0008, */ 0x0012003005EFFC01UL,
+	/* 0x0010, */ 0x0012003005EFFC01UL,
+	/* 0x0018, */ 0x0012003005EFFC01UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001005E03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002100B005EFFC01UL,
+	/* 0x01c8, */ 0x002100B005EFFC01UL,
+	/* 0x01d0, */ 0x002100B005EFFC01UL,
+	/* 0x01d8, */ 0x002100B005EFFC01UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021003005EFFC01UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021003005EFFC01UL,
+	/* 0x0218, */ 0x0011003005EFFC01UL,
+	/* 0x0220, */ 0x0011003005EFFC01UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011003005EFFC01UL,
+	/* 0x0238, */ 0x0011003005EFFC01UL,
+	/* 0x0240, */ 0x0012003005EFFC01UL,
+	/* 0x0248, */ 0x0011003005EFFC01UL,
+	/* 0x0250, */ 0x0012003005EFFC01UL,
+	/* 0x0258, */ 0x0011003005EFFC01UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011007005EFFC01UL,
+	/* 0x02f8, */ 0x001100B005EFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0011007005EFFC01UL,
+	/* 0x0310, */ 0x001100B005EFFC01UL,
+	/* 0x0318, */ 0x0012001005E03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+#endif /* QOS_INIT_G2M_V30_MSTAT390_H */
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30_qoswt195.h b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30_qoswt195.h
new file mode 100644
index 0000000..27c9c51
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30_qoswt195.h
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2M_V30_QOSWT195_H
+#define QOS_INIT_G2M_V30_QOSWT195_H
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004040000C010UL,
+	/* 0x0038, */ 0x001004040000C010UL,
+	/* 0x0040, */ 0x001414090000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x00140C0A0000C010UL,
+	/* 0x0060, */ 0x00140C0A0000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x001004030000C010UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001414090000FFF0UL,
+	/* 0x0090, */ 0x001408070000C010UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08020000FFF0UL,
+	/* 0x0268, */ 0x001408010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C04010000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+#endif /* QOS_INIT_G2M_V30_QOSWT195_H */
diff --git a/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30_qoswt390.h b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30_qoswt390.h
new file mode 100644
index 0000000..5d18212
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30_qoswt390.h
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2020, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2M_V30_QOSWT390_H
+#define QOS_INIT_G2M_V30_QOSWT390_H
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008070000C010UL,
+	/* 0x0038, */ 0x001008070000C010UL,
+	/* 0x0040, */ 0x001424120000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x001414130000C010UL,
+	/* 0x0060, */ 0x001414130000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x001008050000C010UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x001424120000FFF0UL,
+	/* 0x0090, */ 0x0014100D0000C010UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C030000FFF0UL,
+	/* 0x0268, */ 0x001410010000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C08020000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410010000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+};
+
+#endif /* QOS_INIT_G2M_V30_QOSWT390_H */
diff --git a/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10.c b/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10.c
new file mode 100644
index 0000000..00b0948
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10.c
@@ -0,0 +1,196 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "qos_init_g2n_v10.h"
+
+#include "../qos_common.h"
+#include "../qos_reg.h"
+
+#define RCAR_QOS_VERSION			"rev.0.09"
+
+#define REF_ARS_ARBSTOPCYCLE_G2N		(((SL_INIT_SSLOTCLK_G2N) - 5U) << 16U)
+
+#define QOSWT_TIME_BANK0			20000000U	/* unit:ns */
+
+#define	QOSWT_WTEN_ENABLE			0x1U
+
+#define OSWT_WTREF_SLOT0_EN_REQ1_SLOT		3U
+#define OSWT_WTREF_SLOT0_EN_REQ2_SLOT		9U
+#define QOSWT_WTREF_SLOT0_EN			((0x1U << OSWT_WTREF_SLOT0_EN_REQ1_SLOT) | \
+						(0x1U << OSWT_WTREF_SLOT0_EN_REQ2_SLOT))
+#define QOSWT_WTREF_SLOT1_EN			QOSWT_WTREF_SLOT0_EN
+
+#define QOSWT_WTSET0_REQ_SSLOT0			5U
+#define WT_BASE_SUB_SLOT_NUM0			12U
+#define QOSWT_WTSET0_PERIOD0_G2N		((QOSWT_TIME_BANK0 / QOSWT_WTSET0_CYCLE_G2N) - 1U)
+#define QOSWT_WTSET0_SSLOT0			(QOSWT_WTSET0_REQ_SSLOT0 - 1U)
+#define QOSWT_WTSET0_SLOTSLOT0			(WT_BASE_SUB_SLOT_NUM0 - 1U)
+
+#define QOSWT_WTSET1_PERIOD1_G2N		QOSWT_WTSET0_PERIOD0_G2N
+#define QOSWT_WTSET1_SSLOT1			QOSWT_WTSET0_SSLOT0
+#define QOSWT_WTSET1_SLOTSLOT1			QOSWT_WTSET0_SLOTSLOT0
+
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_g2n_v10_mstat195.h"
+#else
+#include "qos_init_g2n_v10_mstat390.h"
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+#include "qos_init_g2n_v10_qoswt195.h"
+#else
+#include "qos_init_g2n_v10_qoswt390.h"
+#endif
+
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+#endif
+
+static const struct rcar_gen3_dbsc_qos_settings g2n_v10_qos[] = {
+	/* BUFCAM settings */
+	{ DBSC_DBCAM0CNF1, 0x00043218U },
+	{ DBSC_DBCAM0CNF2, 0x000000F4U },
+	{ DBSC_DBSCHCNT0, 0x000F0037U },
+	{ DBSC_DBSCHSZ0, 0x00000001U },
+	{ DBSC_DBSCHRW0, 0x22421111U },
+
+	/* DDR3 */
+	{ DBSC_SCFCTST2, 0x012F1123U },
+
+	/* QoS Settings */
+	{ DBSC_DBSCHQOS00, 0x00000F00U },
+	{ DBSC_DBSCHQOS01, 0x00000B00U },
+	{ DBSC_DBSCHQOS02, 0x00000000U },
+	{ DBSC_DBSCHQOS03, 0x00000000U },
+	{ DBSC_DBSCHQOS40, 0x00000300U },
+	{ DBSC_DBSCHQOS41, 0x000002F0U },
+	{ DBSC_DBSCHQOS42, 0x00000200U },
+	{ DBSC_DBSCHQOS43, 0x00000100U },
+	{ DBSC_DBSCHQOS90, 0x00000100U },
+	{ DBSC_DBSCHQOS91, 0x000000F0U },
+	{ DBSC_DBSCHQOS92, 0x000000A0U },
+	{ DBSC_DBSCHQOS93, 0x00000040U },
+	{ DBSC_DBSCHQOS130, 0x00000100U },
+	{ DBSC_DBSCHQOS131, 0x000000F0U },
+	{ DBSC_DBSCHQOS132, 0x000000A0U },
+	{ DBSC_DBSCHQOS133, 0x00000040U },
+	{ DBSC_DBSCHQOS140, 0x000000C0U },
+	{ DBSC_DBSCHQOS141, 0x000000B0U },
+	{ DBSC_DBSCHQOS142, 0x00000080U },
+	{ DBSC_DBSCHQOS143, 0x00000040U },
+	{ DBSC_DBSCHQOS150, 0x00000040U },
+	{ DBSC_DBSCHQOS151, 0x00000030U },
+	{ DBSC_DBSCHQOS152, 0x00000020U },
+	{ DBSC_DBSCHQOS153, 0x00000010U },
+};
+
+void qos_init_g2n_v10(void)
+{
+	rzg_qos_dbsc_setting(g2n_v10_qos, ARRAY_SIZE(g2n_v10_qos), true);
+
+	/* DRAM Split Address mapping */
+#if RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_4CH
+#if RCAR_LSI == RZ_G2N
+#error "Don't set DRAM Split 4ch(G2N)"
+#else
+	ERROR("DRAM Split 4ch not supported.(G2N)");
+	panic();
+#endif
+#elif (RCAR_DRAM_SPLIT == RCAR_DRAM_SPLIT_2CH)
+#if RCAR_LSI == RZ_G2N
+#error "Don't set DRAM Split 2ch(G2N)"
+#else
+	ERROR("DRAM Split 2ch not supported.(G2N)");
+	panic();
+#endif
+#else
+	NOTICE("BL2: DRAM Split is OFF\n");
+#endif
+
+#if !(RCAR_QOS_TYPE == RCAR_QOS_NONE)
+#if RCAR_QOS_TYPE  == RCAR_QOS_TYPE_DEFAULT
+	NOTICE("BL2: QoS is default setting(%s)\n", RCAR_QOS_VERSION);
+#endif
+
+#if RCAR_REF_INT == RCAR_REF_DEFAULT
+	NOTICE("BL2: DRAM refresh interval 1.95 usec\n");
+#else
+	NOTICE("BL2: DRAM refresh interval 3.9 usec\n");
+#endif
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	NOTICE("BL2: Periodic Write DQ Training\n");
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	mmio_write_32(QOSCTRL_RAS, 0x00000028U);
+	mmio_write_64(QOSCTRL_DANN, 0x0402000002020201UL);
+	mmio_write_32(QOSCTRL_DANT, 0x00100804U);
+	mmio_write_32(QOSCTRL_FSS, 0x0000000AU);
+	mmio_write_32(QOSCTRL_INSFC, 0x06330001U);
+	mmio_write_32(QOSCTRL_EARLYR, 0x00000001U);
+	mmio_write_32(QOSCTRL_RACNT0, 0x00010003U);
+
+	mmio_write_32(QOSCTRL_SL_INIT, SL_INIT_REFFSSLOT |
+		      SL_INIT_SLOTSSLOT | SL_INIT_SSLOTCLK_G2N);
+	mmio_write_32(QOSCTRL_REF_ARS, REF_ARS_ARBSTOPCYCLE_G2N);
+
+	uint32_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(mstat_fix); i++) {
+		mmio_write_64(QOSBW_FIX_QOS_BANK0 + i * 8U, mstat_fix[i]);
+		mmio_write_64(QOSBW_FIX_QOS_BANK1 + i * 8U, mstat_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(mstat_be); i++) {
+		mmio_write_64(QOSBW_BE_QOS_BANK0 + i * 8U, mstat_be[i]);
+		mmio_write_64(QOSBW_BE_QOS_BANK1 + i * 8U, mstat_be[i]);
+	}
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	for (i = 0U; i < ARRAY_SIZE(qoswt_fix); i++) {
+		mmio_write_64(QOSWT_FIX_WTQOS_BANK0 + i * 8U, qoswt_fix[i]);
+		mmio_write_64(QOSWT_FIX_WTQOS_BANK1 + i * 8U, qoswt_fix[i]);
+	}
+	for (i = 0U; i < ARRAY_SIZE(qoswt_be); i++) {
+		mmio_write_64(QOSWT_BE_WTQOS_BANK0 + i * 8U, qoswt_be[i]);
+		mmio_write_64(QOSWT_BE_WTQOS_BANK1 + i * 8U, qoswt_be[i]);
+	}
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	/* RT bus Leaf setting */
+	mmio_write_32(RT_ACT0, 0x00000000U);
+	mmio_write_32(RT_ACT1, 0x00000000U);
+
+	/* CCI bus Leaf setting */
+	mmio_write_32(CPU_ACT0, 0x00000003U);
+	mmio_write_32(CPU_ACT1, 0x00000003U);
+
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+
+#if RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE
+	/*  re-write training setting */
+	mmio_write_32(QOSWT_WTREF, ((QOSWT_WTREF_SLOT1_EN << 16) | QOSWT_WTREF_SLOT0_EN));
+	mmio_write_32(QOSWT_WTSET0, ((QOSWT_WTSET0_PERIOD0_G2N << 16) |
+		      (QOSWT_WTSET0_SSLOT0 << 8) | QOSWT_WTSET0_SLOTSLOT0));
+	mmio_write_32(QOSWT_WTSET1, ((QOSWT_WTSET1_PERIOD1_G2N << 16) |
+		      (QOSWT_WTSET1_SSLOT1 << 8) | QOSWT_WTSET1_SLOTSLOT1));
+
+	mmio_write_32(QOSWT_WTEN, QOSWT_WTEN_ENABLE);
+#endif /* RCAR_REWT_TRAINING != RCAR_REWT_TRAINING_DISABLE */
+
+	mmio_write_32(QOSCTRL_STATQC, 0x00000001U);
+#else
+	NOTICE("BL2: QoS is None\n");
+
+	mmio_write_32(QOSCTRL_RAEN, 0x00000001U);
+#endif /* !(RCAR_QOS_TYPE == RCAR_QOS_NONE) */
+}
diff --git a/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10.h b/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10.h
new file mode 100644
index 0000000..c7f02d9
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2N_V10_H
+#define QOS_INIT_G2N_V10_H
+
+void qos_init_g2n_v10(void);
+
+#endif /* QOS_INIT_G2N_V10_H */
diff --git a/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10_mstat195.h b/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10_mstat195.h
new file mode 100644
index 0000000..6e304b0
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10_mstat195.h
@@ -0,0 +1,245 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2N_MSTAT195_H
+#define QOS_INIT_G2N_MSTAT195_H
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004320000FFFFUL,
+	/* 0x0038, */ 0x001004320000FFFFUL,
+	/* 0x0040, */ 0x00140C5D0000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404040000FFFFUL,
+	/* 0x0058, */ 0x00140C940000FFFFUL,
+	/* 0x0060, */ 0x00140C940000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404040000FFFFUL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0014041F0000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C041D0000FFFFUL,
+	/* 0x00a8, */ 0x000C04090000FFFFUL,
+	/* 0x00b0, */ 0x000C040B0000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C041D0000FFFFUL,
+	/* 0x00c8, */ 0x000C04090000FFFFUL,
+	/* 0x00d0, */ 0x000C040B0000FFFFUL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x000C084F0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x000C21E60000FFFFUL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x00100CA50000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x001010C90000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x00100CA50000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x001008530000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x00101D9D0000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x00100CA50000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x001408020000FFFFUL,
+	/* 0x0270, */ 0x001404010000FFFFUL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408020000FFFFUL,
+	/* 0x0298, */ 0x001404010000FFFFUL,
+	/* 0x02a0, */ 0x000C04050000FFFFUL,
+	/* 0x02a8, */ 0x000C04050000FFFFUL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04050000FFFFUL,
+	/* 0x02d8, */ 0x000C04050000FFFFUL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x000C04050000FFFFUL,
+	/* 0x0388, */ 0x000C04050000FFFFUL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x001200100BD03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x002106000BDFFC01UL,
+	/* 0x01c8, */ 0x002106000BDFFC01UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x001101000BDF2401UL,
+	/* 0x0220, */ 0x001101000BDF2401UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x001101000BDF2401UL,
+	/* 0x0238, */ 0x001101000BDF2401UL,
+	/* 0x0240, */ 0x001201000BDF2401UL,
+	/* 0x0248, */ 0x001101000BDF2401UL,
+	/* 0x0250, */ 0x001201000BDF2401UL,
+	/* 0x0258, */ 0x001101000BDF2401UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x001106000BDFFC01UL,
+	/* 0x02f8, */ 0x001106000BDFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x001200100BD03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x001206000BDFFC01UL,
+	/* 0x0360, */ 0x001206000BDFFC01UL,
+	/* 0x0368, */ 0x001200100BD03401UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x001200100BD03401UL,
+};
+#endif /* QOS_INIT_G2N_MSTAT195_H */
diff --git a/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10_mstat390.h b/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10_mstat390.h
new file mode 100644
index 0000000..4632413
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10_mstat390.h
@@ -0,0 +1,245 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2N_MSTAT390_H
+#define QOS_INIT_G2N_MSTAT390_H
+
+static uint64_t mstat_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008630000FFFFUL,
+	/* 0x0038, */ 0x001008630000FFFFUL,
+	/* 0x0040, */ 0x001418BA0000FFFFUL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x001404070000FFFFUL,
+	/* 0x0058, */ 0x001415270000FFFFUL,
+	/* 0x0060, */ 0x001415270000FFFFUL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x001404070000FFFFUL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0014083E0000FFFFUL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x000C08390000FFFFUL,
+	/* 0x00a8, */ 0x000C04110000FFFFUL,
+	/* 0x00b0, */ 0x000C04150000FFFFUL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x000C08390000FFFFUL,
+	/* 0x00c8, */ 0x000C04110000FFFFUL,
+	/* 0x00d0, */ 0x000C04150000FFFFUL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x001045080000FFFFUL,
+	/* 0x00f8, */ 0x000C0C9E0000FFFFUL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x001015080000FFFFUL,
+	/* 0x0118, */ 0x000C43CB0000FFFFUL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0010194A0000FFFFUL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x00101D910000FFFFUL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0010194A0000FFFFUL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x00100CA50000FFFFUL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x001037390000FFFFUL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0010194A0000FFFFUL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x000C04010000FFFFUL,
+	/* 0x01c8, */ 0x000C04010000FFFFUL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x000C04020000FFFFUL,
+	/* 0x01f0, */ 0x000C04090000FFFFUL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x000C04090000FFFFUL,
+	/* 0x0210, */ 0x000C04090000FFFFUL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C2A0000FFFFUL,
+	/* 0x0268, */ 0x001410040000FFFFUL,
+	/* 0x0270, */ 0x001404020000FFFFUL,
+	/* 0x0278, */ 0x000C08110000FFFFUL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410040000FFFFUL,
+	/* 0x0298, */ 0x001404020000FFFFUL,
+	/* 0x02a0, */ 0x000C04090000FFFFUL,
+	/* 0x02a8, */ 0x000C04090000FFFFUL,
+	/* 0x02b0, */ 0x00140C090000FFFFUL,
+	/* 0x02b8, */ 0x000C04020000FFFFUL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x000C04090000FFFFUL,
+	/* 0x02d8, */ 0x000C04090000FFFFUL,
+	/* 0x02e0, */ 0x00140C090000FFFFUL,
+	/* 0x02e8, */ 0x000C04020000FFFFUL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x000C04020000FFFFUL,
+	/* 0x0378, */ 0x000C04020000FFFFUL,
+	/* 0x0380, */ 0x000C04090000FFFFUL,
+	/* 0x0388, */ 0x000C04090000FFFFUL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static uint64_t mstat_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0012001005E03401UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0021060005EFFC01UL,
+	/* 0x01c8, */ 0x0021060005EFFC01UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0021010005E79401UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0021010005E79401UL,
+	/* 0x0218, */ 0x0011010005E79401UL,
+	/* 0x0220, */ 0x0011010005E79401UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0011010005E79401UL,
+	/* 0x0238, */ 0x0011010005E79401UL,
+	/* 0x0240, */ 0x0012010005E79401UL,
+	/* 0x0248, */ 0x0011010005E79401UL,
+	/* 0x0250, */ 0x0012010005E79401UL,
+	/* 0x0258, */ 0x0011010005E79401UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0011060005EFFC01UL,
+	/* 0x02f8, */ 0x0011060005EFFC01UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0012001005E03401UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0012060005EFFC01UL,
+	/* 0x0360, */ 0x0012060005EFFC01UL,
+	/* 0x0368, */ 0x0012001005E03401UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0012001005E03401UL,
+};
+#endif /* QOS_INIT_G2N_MSTAT390_H */
diff --git a/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10_qoswt195.h b/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10_qoswt195.h
new file mode 100644
index 0000000..eea1fce
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10_qoswt195.h
@@ -0,0 +1,245 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2N_QOSWT195_H
+#define QOS_INIT_G2N_QOSWT195_H
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001004320000C010UL,
+	/* 0x0038, */ 0x001004320000C010UL,
+	/* 0x0040, */ 0x00140C5D0000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x00140C940000C010UL,
+	/* 0x0060, */ 0x00140C940000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0014041F0000FFF0UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C08150000FFF0UL,
+	/* 0x0268, */ 0x001408020000FFF0UL,
+	/* 0x0270, */ 0x001404010000FFF0UL,
+	/* 0x0278, */ 0x000C04090000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001408020000FFF0UL,
+	/* 0x0298, */ 0x001404010000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+#endif /* QOS_INIT_G2N_QOSWT195_H */
diff --git a/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10_qoswt390.h b/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10_qoswt390.h
new file mode 100644
index 0000000..7043303
--- /dev/null
+++ b/drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10_qoswt390.h
@@ -0,0 +1,245 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_INIT_G2N_QOSWT390_H
+#define QOS_INIT_G2N_QOSWT390_H
+
+static uint64_t qoswt_fix[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x001008630000C010UL,
+	/* 0x0038, */ 0x001008630000C010UL,
+	/* 0x0040, */ 0x001418BA0000FFF0UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x001415270000C010UL,
+	/* 0x0060, */ 0x001415270000C010UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0014083E0000FFF0UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x000C0C2A0000FFF0UL,
+	/* 0x0268, */ 0x001410040000FFF0UL,
+	/* 0x0270, */ 0x001404020000FFF0UL,
+	/* 0x0278, */ 0x000C08110000FFF0UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x001410040000FFF0UL,
+	/* 0x0298, */ 0x001404020000FFF0UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+
+static uint64_t qoswt_be[] = {
+	/* 0x0000, */ 0x0000000000000000UL,
+	/* 0x0008, */ 0x0000000000000000UL,
+	/* 0x0010, */ 0x0000000000000000UL,
+	/* 0x0018, */ 0x0000000000000000UL,
+	/* 0x0020, */ 0x0000000000000000UL,
+	/* 0x0028, */ 0x0000000000000000UL,
+	/* 0x0030, */ 0x0000000000000000UL,
+	/* 0x0038, */ 0x0000000000000000UL,
+	/* 0x0040, */ 0x0000000000000000UL,
+	/* 0x0048, */ 0x0000000000000000UL,
+	/* 0x0050, */ 0x0000000000000000UL,
+	/* 0x0058, */ 0x0000000000000000UL,
+	/* 0x0060, */ 0x0000000000000000UL,
+	/* 0x0068, */ 0x0000000000000000UL,
+	/* 0x0070, */ 0x0000000000000000UL,
+	/* 0x0078, */ 0x0000000000000000UL,
+	/* 0x0080, */ 0x0000000000000000UL,
+	/* 0x0088, */ 0x0000000000000000UL,
+	/* 0x0090, */ 0x0000000000000000UL,
+	/* 0x0098, */ 0x0000000000000000UL,
+	/* 0x00a0, */ 0x0000000000000000UL,
+	/* 0x00a8, */ 0x0000000000000000UL,
+	/* 0x00b0, */ 0x0000000000000000UL,
+	/* 0x00b8, */ 0x0000000000000000UL,
+	/* 0x00c0, */ 0x0000000000000000UL,
+	/* 0x00c8, */ 0x0000000000000000UL,
+	/* 0x00d0, */ 0x0000000000000000UL,
+	/* 0x00d8, */ 0x0000000000000000UL,
+	/* 0x00e0, */ 0x0000000000000000UL,
+	/* 0x00e8, */ 0x0000000000000000UL,
+	/* 0x00f0, */ 0x0000000000000000UL,
+	/* 0x00f8, */ 0x0000000000000000UL,
+	/* 0x0100, */ 0x0000000000000000UL,
+	/* 0x0108, */ 0x0000000000000000UL,
+	/* 0x0110, */ 0x0000000000000000UL,
+	/* 0x0118, */ 0x0000000000000000UL,
+	/* 0x0120, */ 0x0000000000000000UL,
+	/* 0x0128, */ 0x0000000000000000UL,
+	/* 0x0130, */ 0x0000000000000000UL,
+	/* 0x0138, */ 0x0000000000000000UL,
+	/* 0x0140, */ 0x0000000000000000UL,
+	/* 0x0148, */ 0x0000000000000000UL,
+	/* 0x0150, */ 0x0000000000000000UL,
+	/* 0x0158, */ 0x0000000000000000UL,
+	/* 0x0160, */ 0x0000000000000000UL,
+	/* 0x0168, */ 0x0000000000000000UL,
+	/* 0x0170, */ 0x0000000000000000UL,
+	/* 0x0178, */ 0x0000000000000000UL,
+	/* 0x0180, */ 0x0000000000000000UL,
+	/* 0x0188, */ 0x0000000000000000UL,
+	/* 0x0190, */ 0x0000000000000000UL,
+	/* 0x0198, */ 0x0000000000000000UL,
+	/* 0x01a0, */ 0x0000000000000000UL,
+	/* 0x01a8, */ 0x0000000000000000UL,
+	/* 0x01b0, */ 0x0000000000000000UL,
+	/* 0x01b8, */ 0x0000000000000000UL,
+	/* 0x01c0, */ 0x0000000000000000UL,
+	/* 0x01c8, */ 0x0000000000000000UL,
+	/* 0x01d0, */ 0x0000000000000000UL,
+	/* 0x01d8, */ 0x0000000000000000UL,
+	/* 0x01e0, */ 0x0000000000000000UL,
+	/* 0x01e8, */ 0x0000000000000000UL,
+	/* 0x01f0, */ 0x0000000000000000UL,
+	/* 0x01f8, */ 0x0000000000000000UL,
+	/* 0x0200, */ 0x0000000000000000UL,
+	/* 0x0208, */ 0x0000000000000000UL,
+	/* 0x0210, */ 0x0000000000000000UL,
+	/* 0x0218, */ 0x0000000000000000UL,
+	/* 0x0220, */ 0x0000000000000000UL,
+	/* 0x0228, */ 0x0000000000000000UL,
+	/* 0x0230, */ 0x0000000000000000UL,
+	/* 0x0238, */ 0x0000000000000000UL,
+	/* 0x0240, */ 0x0000000000000000UL,
+	/* 0x0248, */ 0x0000000000000000UL,
+	/* 0x0250, */ 0x0000000000000000UL,
+	/* 0x0258, */ 0x0000000000000000UL,
+	/* 0x0260, */ 0x0000000000000000UL,
+	/* 0x0268, */ 0x0000000000000000UL,
+	/* 0x0270, */ 0x0000000000000000UL,
+	/* 0x0278, */ 0x0000000000000000UL,
+	/* 0x0280, */ 0x0000000000000000UL,
+	/* 0x0288, */ 0x0000000000000000UL,
+	/* 0x0290, */ 0x0000000000000000UL,
+	/* 0x0298, */ 0x0000000000000000UL,
+	/* 0x02a0, */ 0x0000000000000000UL,
+	/* 0x02a8, */ 0x0000000000000000UL,
+	/* 0x02b0, */ 0x0000000000000000UL,
+	/* 0x02b8, */ 0x0000000000000000UL,
+	/* 0x02c0, */ 0x0000000000000000UL,
+	/* 0x02c8, */ 0x0000000000000000UL,
+	/* 0x02d0, */ 0x0000000000000000UL,
+	/* 0x02d8, */ 0x0000000000000000UL,
+	/* 0x02e0, */ 0x0000000000000000UL,
+	/* 0x02e8, */ 0x0000000000000000UL,
+	/* 0x02f0, */ 0x0000000000000000UL,
+	/* 0x02f8, */ 0x0000000000000000UL,
+	/* 0x0300, */ 0x0000000000000000UL,
+	/* 0x0308, */ 0x0000000000000000UL,
+	/* 0x0310, */ 0x0000000000000000UL,
+	/* 0x0318, */ 0x0000000000000000UL,
+	/* 0x0320, */ 0x0000000000000000UL,
+	/* 0x0328, */ 0x0000000000000000UL,
+	/* 0x0330, */ 0x0000000000000000UL,
+	/* 0x0338, */ 0x0000000000000000UL,
+	/* 0x0340, */ 0x0000000000000000UL,
+	/* 0x0348, */ 0x0000000000000000UL,
+	/* 0x0350, */ 0x0000000000000000UL,
+	/* 0x0358, */ 0x0000000000000000UL,
+	/* 0x0360, */ 0x0000000000000000UL,
+	/* 0x0368, */ 0x0000000000000000UL,
+	/* 0x0370, */ 0x0000000000000000UL,
+	/* 0x0378, */ 0x0000000000000000UL,
+	/* 0x0380, */ 0x0000000000000000UL,
+	/* 0x0388, */ 0x0000000000000000UL,
+	/* 0x0390, */ 0x0000000000000000UL,
+};
+#endif /* QOS_INIT_G2N_QOSWT390_H */
diff --git a/drivers/renesas/rzg/qos/qos.mk b/drivers/renesas/rzg/qos/qos.mk
new file mode 100644
index 0000000..f05d126
--- /dev/null
+++ b/drivers/renesas/rzg/qos/qos.mk
@@ -0,0 +1,60 @@
+#
+# Copyright (c) 2020-2021, Renesas Electronics Corporation. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${RCAR_LSI},${RCAR_AUTO})
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10.c
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2H/qos_init_g2h_v30.c
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2M/qos_init_g2m_v10.c
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11.c
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30.c
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10.c
+else ifeq (${RCAR_LSI_CUT_COMPAT},1)
+  ifeq (${RCAR_LSI},${RZ_G2M})
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2M/qos_init_g2m_v10.c
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11.c
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30.c
+  endif
+  ifeq (${RCAR_LSI},${RZ_G2H})
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2H/qos_init_g2h_v30.c
+  endif
+  ifeq (${RCAR_LSI},${RZ_G2N})
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10.c
+  endif
+  ifeq (${RCAR_LSI},${RZ_G2E})
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10.c
+  endif
+else
+  ifeq (${RCAR_LSI},${RZ_G2M})
+    ifeq (${LSI_CUT},10)
+     BL2_SOURCES += drivers/renesas/rzg/qos/G2M/qos_init_g2m_v10.c
+    else ifeq (${LSI_CUT},11)
+     BL2_SOURCES += drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11.c
+    else ifeq (${LSI_CUT},13)
+     BL2_SOURCES += drivers/renesas/rzg/qos/G2M/qos_init_g2m_v11.c
+    else ifeq (${LSI_CUT},30)
+     BL2_SOURCES += drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30.c
+    else
+#    LSI_CUT 30 or later
+     BL2_SOURCES += drivers/renesas/rzg/qos/G2M/qos_init_g2m_v30.c
+    endif
+  endif
+  ifeq (${RCAR_LSI},${RZ_G2H})
+     BL2_SOURCES += drivers/renesas/rzg/qos/G2H/qos_init_g2h_v30.c
+  endif
+  ifeq (${RCAR_LSI},${RZ_G2N})
+    ifeq (${LSI_CUT},10)
+     BL2_SOURCES += drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10.c
+    else
+#    LSI_CUT 10 or later
+     BL2_SOURCES += drivers/renesas/rzg/qos/G2N/qos_init_g2n_v10.c
+    endif
+  endif
+  ifeq (${RCAR_LSI},${RZ_G2E})
+    BL2_SOURCES += drivers/renesas/rzg/qos/G2E/qos_init_g2e_v10.c
+  endif
+endif
+
+BL2_SOURCES += drivers/renesas/rzg/qos/qos_init.c
diff --git a/drivers/renesas/rzg/qos/qos_common.h b/drivers/renesas/rzg/qos/qos_common.h
new file mode 100644
index 0000000..535bf4c
--- /dev/null
+++ b/drivers/renesas/rzg/qos/qos_common.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef QOS_COMMON_H
+#define QOS_COMMON_H
+
+#define RCAR_REF_DEFAULT		0U
+
+/* define used for get_refperiod. */
+/* REFPERIOD_CYCLE need smaller than QOSWT_WTSET0_CYCLEs */
+#if (RCAR_REF_INT == RCAR_REF_DEFAULT)	/* REF default */
+#define REFPERIOD_CYCLE		/* unit:ns */	\
+	((126U * BASE_SUB_SLOT_NUM * 1000U) / 400U)
+#else					/* REF option */
+#define REFPERIOD_CYCLE		/* unit:ns */	\
+	((252U * BASE_SUB_SLOT_NUM * 1000U) / 400U)
+#endif
+
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2M)
+/* define used for G2M */
+#if (RCAR_REF_INT == RCAR_REF_DEFAULT)	/* REF 1.95usec */
+#define SUB_SLOT_CYCLE_G2M_11		0x7EU	/* 126 */
+#define SUB_SLOT_CYCLE_G2M_30		0x7EU	/* 126 */
+#else /* REF 3.9usec */
+#define SUB_SLOT_CYCLE_G2M_11		0xFCU	/* 252 */
+#define SUB_SLOT_CYCLE_G2M_30		0xFCU	/* 252 */
+#endif /* (RCAR_REF_INT == RCAR_REF_DEFAULT) */
+
+#define SL_INIT_SSLOTCLK_G2M_11		(SUB_SLOT_CYCLE_G2M_11 - 1U)
+#define SL_INIT_SSLOTCLK_G2M_30		(SUB_SLOT_CYCLE_G2M_30 - 1U)
+#define QOSWT_WTSET0_CYCLE_G2M_11	/* unit:ns */	\
+	((SUB_SLOT_CYCLE_G2M_11 * BASE_SUB_SLOT_NUM * 1000U) / OPERATING_FREQ)
+#define QOSWT_WTSET0_CYCLE_G2M_30	/* unit:ns */	\
+	((SUB_SLOT_CYCLE_G2M_30 * BASE_SUB_SLOT_NUM * 1000U) / OPERATING_FREQ)
+#endif
+
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2N)
+/* define used for G2N */
+#if (RCAR_REF_INT == RCAR_REF_DEFAULT)	/* REF 1.95usec */
+#define SUB_SLOT_CYCLE_G2N		0x7EU	/* 126 */
+#else /* REF 3.9usec */
+#define SUB_SLOT_CYCLE_G2N		0xFCU	/* 252 */
+#endif /* (RCAR_REF_INT == RCAR_REF_DEFAULT) */
+
+#define SL_INIT_SSLOTCLK_G2N		(SUB_SLOT_CYCLE_G2N - 1U)
+#define QOSWT_WTSET0_CYCLE_G2N		/* unit:ns */	\
+	((SUB_SLOT_CYCLE_G2N * BASE_SUB_SLOT_NUM * 1000U) / OPERATING_FREQ)
+#endif /* (RCAR_LSI == RZ_G2N) */
+
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2H)
+/* define used for G2H */
+#if (RCAR_REF_INT == RCAR_REF_DEFAULT)	/* REF 1.95usec */
+#define SUB_SLOT_CYCLE_G2H		0x7EU	/* 126 */
+#else /* REF 3.9usec */
+#define SUB_SLOT_CYCLE_G2H		0xFCU	/* 252 */
+#endif /* (RCAR_REF_INT == RCAR_REF_DEFAULT) */
+
+#define SL_INIT_SSLOTCLK_G2H		(SUB_SLOT_CYCLE_G2H - 1U)
+#define QOSWT_WTSET0_CYCLE_G2H		/* unit:ns */	\
+	((SUB_SLOT_CYCLE_G2H * BASE_SUB_SLOT_NUM * 1000U) / OPERATING_FREQ)
+#endif
+
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2E)
+/* define used for G2E */
+#if (RCAR_REF_INT == RCAR_REF_DEFAULT)	/* REF 3.9usec */
+#define SUB_SLOT_CYCLE_G2E		0xAFU	/* 175 */
+#else /* REF 7.8usec */
+#define SUB_SLOT_CYCLE_G2E		0x15EU	/* 350 */
+#endif /* (RCAR_REF_INT == RCAR_REF_DEFAULT) */
+
+#define OPERATING_FREQ_G2E		266U	/* MHz */
+#define SL_INIT_SSLOTCLK_G2E		(SUB_SLOT_CYCLE_G2E - 1U)
+#endif
+
+#define OPERATING_FREQ			400U	/* MHz */
+#define BASE_SUB_SLOT_NUM		0x6U
+#define SUB_SLOT_CYCLE			0x7EU	/* 126 */
+
+#define QOSWT_WTSET0_CYCLE		/* unit:ns */	\
+	((SUB_SLOT_CYCLE * BASE_SUB_SLOT_NUM * 1000U) / OPERATING_FREQ)
+
+#define SL_INIT_REFFSSLOT		(0x3U << 24U)
+#define SL_INIT_SLOTSSLOT		((BASE_SUB_SLOT_NUM - 1U) << 16U)
+#define SL_INIT_SSLOTCLK		(SUB_SLOT_CYCLE - 1U)
+
+typedef struct {
+	uintptr_t addr;
+	uint64_t value;
+} mstat_slot_t;
+
+struct rcar_gen3_dbsc_qos_settings {
+	uint32_t	reg;
+	uint32_t	val;
+};
+
+extern uint32_t qos_init_ddr_ch;
+extern uint8_t qos_init_ddr_phyvalid;
+
+void rzg_qos_dbsc_setting(const struct rcar_gen3_dbsc_qos_settings *qos,
+			  unsigned int qos_size, bool dbsc_wren);
+
+#endif /* QOS_COMMON_H */
diff --git a/drivers/renesas/rzg/qos/qos_init.c b/drivers/renesas/rzg/qos/qos_init.c
new file mode 100644
index 0000000..e527a61
--- /dev/null
+++ b/drivers/renesas/rzg/qos/qos_init.c
@@ -0,0 +1,267 @@
+/*
+ * Copyright (c) 2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#if RCAR_LSI == RCAR_AUTO
+#include "G2E/qos_init_g2e_v10.h"
+#include "G2H/qos_init_g2h_v30.h"
+#include "G2M/qos_init_g2m_v10.h"
+#include "G2M/qos_init_g2m_v11.h"
+#include "G2M/qos_init_g2m_v30.h"
+#include "G2N/qos_init_g2n_v10.h"
+#endif /* RCAR_LSI == RCAR_AUTO */
+#if (RCAR_LSI == RZ_G2M)
+#include "G2M/qos_init_g2m_v10.h"
+#include "G2M/qos_init_g2m_v11.h"
+#include "G2M/qos_init_g2m_v30.h"
+#endif /* RCAR_LSI == RZ_G2M */
+#if RCAR_LSI == RZ_G2H
+#include "G2H/qos_init_g2h_v30.h"
+#endif /* RCAR_LSI == RZ_G2H */
+#if RCAR_LSI == RZ_G2N
+#include "G2N/qos_init_g2n_v10.h"
+#endif /* RCAR_LSI == RZ_G2N */
+#if RCAR_LSI == RZ_G2E
+#include "G2E/qos_init_g2e_v10.h"
+#endif /* RCAR_LSI == RZ_G2E */
+#include "qos_common.h"
+#include "qos_init.h"
+#include "qos_reg.h"
+#include "rcar_def.h"
+
+#if (RCAR_LSI != RZ_G2E)
+#define DRAM_CH_CNT	0x04U
+uint32_t qos_init_ddr_ch;
+uint8_t qos_init_ddr_phyvalid;
+#endif /* RCAR_LSI != RZ_G2E */
+
+#define PRR_PRODUCT_ERR(reg)				\
+	{						\
+		ERROR("LSI Product ID(PRR=0x%x) QoS "	\
+		"initialize not supported.\n", reg);	\
+		panic();				\
+	}
+
+#define PRR_CUT_ERR(reg)				\
+	{						\
+		ERROR("LSI Cut ID(PRR=0x%x) QoS "	\
+		"initialize not supported.\n", reg);	\
+		panic();				\
+	}
+
+void rzg_qos_init(void)
+{
+	uint32_t reg;
+#if (RCAR_LSI != RZ_G2E)
+	uint32_t i;
+
+	qos_init_ddr_ch = 0U;
+	qos_init_ddr_phyvalid = get_boardcnf_phyvalid();
+	for (i = 0U; i < DRAM_CH_CNT; i++) {
+		if ((qos_init_ddr_phyvalid & (1U << i))) {
+			qos_init_ddr_ch++;
+		}
+	}
+#endif /* RCAR_LSI != RZ_G2E */
+
+	reg = mmio_read_32(PRR);
+#if (RCAR_LSI == RCAR_AUTO) || RCAR_LSI_CUT_COMPAT
+	switch (reg & PRR_PRODUCT_MASK) {
+	case PRR_PRODUCT_M3:
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2M)
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:
+			qos_init_g2m_v10();
+			break;
+		case PRR_PRODUCT_21: /* G2M Cut 13 */
+			qos_init_g2m_v11();
+			break;
+		case PRR_PRODUCT_30: /* G2M Cut 30 */
+		default:
+			qos_init_g2m_v30();
+			break;
+		}
+#else /* (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2M) */
+		PRR_PRODUCT_ERR(reg);
+#endif /* (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2M) */
+		break;
+	case PRR_PRODUCT_H3:
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2H)
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_30:
+		default:
+			qos_init_g2h_v30();
+			break;
+		}
+#else
+		PRR_PRODUCT_ERR(reg);
+#endif /* (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2H) */
+		break;
+	case PRR_PRODUCT_M3N:
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2N)
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:
+		default:
+			qos_init_g2n_v10();
+			break;
+		}
+#else
+		PRR_PRODUCT_ERR(reg);
+#endif /* (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2N) */
+		break;
+	case PRR_PRODUCT_E3:
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2E)
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:
+		default:
+			qos_init_g2e_v10();
+			break;
+		}
+#else
+		PRR_PRODUCT_ERR(reg);
+#endif /* (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2E) */
+		break;
+	default:
+		PRR_PRODUCT_ERR(reg);
+		break;
+	}
+#else /* RCAR_LSI == RCAR_AUTO || RCAR_LSI_CUT_COMPAT */
+#if (RCAR_LSI == RZ_G2M)
+#if RCAR_LSI_CUT == RCAR_CUT_10
+	/* G2M Cut 10 */
+	if ((PRR_PRODUCT_M3 | PRR_PRODUCT_10)
+	    != (reg & (PRR_PRODUCT_MASK | PRR_CUT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_g2m_v10();
+#elif RCAR_LSI_CUT == RCAR_CUT_11
+	/* G2M Cut 11 */
+	if ((PRR_PRODUCT_M3 | PRR_PRODUCT_20)
+	    != (reg & (PRR_PRODUCT_MASK | PRR_CUT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_g2m_v11();
+#elif RCAR_LSI_CUT == RCAR_CUT_13
+	/* G2M Cut 13 */
+	if ((PRR_PRODUCT_M3 | PRR_PRODUCT_21)
+	    != (reg & (PRR_PRODUCT_MASK | PRR_CUT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_g2m_v11();
+#else
+	/* G2M Cut 30 or later */
+	if ((PRR_PRODUCT_M3)
+	    != (reg & (PRR_PRODUCT_MASK))) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_g2m_v30();
+#endif /* RCAR_LSI_CUT == RCAR_CUT_10 */
+#elif (RCAR_LSI == RZ_G2H)
+	/* G2H Cut 30 or later */
+	if ((reg & PRR_PRODUCT_MASK) != PRR_PRODUCT_H3) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_g2h_v30();
+#elif (RCAR_LSI == RZ_G2N)
+	/* G2N Cut 10 or later */
+	if ((reg & (PRR_PRODUCT_MASK)) != PRR_PRODUCT_M3N) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_g2n_v10();
+#elif RCAR_LSI == RZ_G2E
+	/* G2E Cut 10 or later */
+	if ((reg & (PRR_PRODUCT_MASK)) != PRR_PRODUCT_E3) {
+		PRR_PRODUCT_ERR(reg);
+	}
+	qos_init_g2e_v10();
+#else /* (RCAR_LSI == RZ_G2M) */
+#error "Don't have QoS initialize routine(Unknown chip)."
+#endif /* (RCAR_LSI == RZ_G2M) */
+#endif /* RCAR_LSI == RCAR_AUTO || RCAR_LSI_CUT_COMPAT */
+}
+
+#if (RCAR_LSI != RZ_G2E)
+uint32_t get_refperiod(void)
+{
+	uint32_t refperiod = QOSWT_WTSET0_CYCLE;
+
+#if (RCAR_LSI == RCAR_AUTO) || RCAR_LSI_CUT_COMPAT
+	uint32_t reg;
+
+	reg = mmio_read_32(PRR);
+	switch (reg & PRR_PRODUCT_MASK) {
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2M)
+	case PRR_PRODUCT_M3:
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_10:
+			break;
+		case PRR_PRODUCT_20: /* G2M Cut 11 */
+		case PRR_PRODUCT_21: /* G2M Cut 13 */
+		case PRR_PRODUCT_30: /* G2M Cut 30 */
+		default:
+			refperiod = REFPERIOD_CYCLE;
+			break;
+		}
+		break;
+#endif /* (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2M) */
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2H)
+	case PRR_PRODUCT_H3:
+		switch (reg & PRR_CUT_MASK) {
+		case PRR_PRODUCT_30:
+		default:
+			refperiod = REFPERIOD_CYCLE;
+			break;
+		}
+		break;
+#endif /* (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2H) */
+#if (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2N)
+	case PRR_PRODUCT_M3N:
+		refperiod = REFPERIOD_CYCLE;
+		break;
+#endif /* (RCAR_LSI == RCAR_AUTO) || (RCAR_LSI == RZ_G2N) */
+	default:
+		break;
+	}
+#elif RCAR_LSI == RZ_G2M
+#if RCAR_LSI_CUT == RCAR_CUT_10
+	/* G2M Cut 10 */
+#else /* RCAR_LSI_CUT == RCAR_CUT_10 */
+	/* G2M Cut 11|13|30 or later */
+	refperiod = REFPERIOD_CYCLE;
+#endif /* RCAR_LSI_CUT == RCAR_CUT_10 */
+#elif RCAR_LSI == RZ_G2N
+	refperiod = REFPERIOD_CYCLE;
+#elif RCAR_LSI == RZ_G2H
+	/* G2H Cut 30 or later */
+	refperiod = REFPERIOD_CYCLE;
+#endif /* RCAR_LSI == RCAR_AUTO || RCAR_LSI_CUT_COMPAT */
+	return refperiod;
+}
+#endif /* RCAR_LSI != RZ_G2E */
+
+void rzg_qos_dbsc_setting(const struct rcar_gen3_dbsc_qos_settings *qos,
+			  unsigned int qos_size, bool dbsc_wren)
+{
+	unsigned int i;
+
+	/* Register write enable */
+	if (dbsc_wren) {
+		mmio_write_32(DBSC_DBSYSCNT0, 0x00001234U);
+	}
+
+	for (i = 0; i < qos_size; i++) {
+		mmio_write_32(qos[i].reg, qos[i].val);
+	}
+
+	/* Register write protect */
+	if (dbsc_wren) {
+		mmio_write_32(DBSC_DBSYSCNT0, 0x00000000U);
+	}
+}
diff --git a/drivers/renesas/rzg/qos/qos_init.h b/drivers/renesas/rzg/qos/qos_init.h
new file mode 100644
index 0000000..3d62744
--- /dev/null
+++ b/drivers/renesas/rzg/qos/qos_init.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2020-2021, Renesas Electronics Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RZG_QOS_INIT_H
+#define RZG_QOS_INIT_H
+
+void rzg_qos_init(void);
+uint8_t get_boardcnf_phyvalid(void);
+
+#endif /* RZG_QOS_INIT_H */
diff --git a/drivers/rpi3/gpio/rpi3_gpio.c b/drivers/rpi3/gpio/rpi3_gpio.c
new file mode 100644
index 0000000..55a8832
--- /dev/null
+++ b/drivers/rpi3/gpio/rpi3_gpio.c
@@ -0,0 +1,164 @@
+/*
+ * Copyright (c) 2019, Linaro Limited
+ * Copyright (c) 2019, Ying-Chun Liu (PaulLiu) <paul.liu@linaro.org>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+#include <assert.h>
+#include <lib/mmio.h>
+#include <drivers/delay_timer.h>
+#include <drivers/rpi3/gpio/rpi3_gpio.h>
+#include <platform_def.h>
+
+static uintptr_t reg_base;
+
+static int rpi3_gpio_get_direction(int gpio);
+static void rpi3_gpio_set_direction(int gpio, int direction);
+static int rpi3_gpio_get_value(int gpio);
+static void rpi3_gpio_set_value(int gpio, int value);
+static void rpi3_gpio_set_pull(int gpio, int pull);
+
+static const gpio_ops_t rpi3_gpio_ops = {
+	.get_direction  = rpi3_gpio_get_direction,
+	.set_direction  = rpi3_gpio_set_direction,
+	.get_value      = rpi3_gpio_get_value,
+	.set_value      = rpi3_gpio_set_value,
+	.set_pull       = rpi3_gpio_set_pull,
+};
+
+/**
+ * Get selection of GPIO pinmux settings.
+ *
+ * @param gpio The pin number of GPIO. From 0 to 53.
+ * @return The selection of pinmux. RPI3_GPIO_FUNC_INPUT: input,
+ *                                  RPI3_GPIO_FUNC_OUTPUT: output,
+ *                                  RPI3_GPIO_FUNC_ALT0: alt-0,
+ *                                  RPI3_GPIO_FUNC_ALT1: alt-1,
+ *                                  RPI3_GPIO_FUNC_ALT2: alt-2,
+ *                                  RPI3_GPIO_FUNC_ALT3: alt-3,
+ *                                  RPI3_GPIO_FUNC_ALT4: alt-4,
+ *                                  RPI3_GPIO_FUNC_ALT5: alt-5
+ */
+int rpi3_gpio_get_select(int gpio)
+{
+	int ret;
+	int regN = gpio / 10;
+	int shift = 3 * (gpio % 10);
+	uintptr_t reg_sel = reg_base + RPI3_GPIO_GPFSEL(regN);
+	uint32_t sel = mmio_read_32(reg_sel);
+
+	ret = (sel >> shift) & 0x07;
+
+	return ret;
+}
+
+/**
+ * Set selection of GPIO pinmux settings.
+ *
+ * @param gpio The pin number of GPIO. From 0 to 53.
+ * @param fsel The selection of pinmux. RPI3_GPIO_FUNC_INPUT: input,
+ *                                      RPI3_GPIO_FUNC_OUTPUT: output,
+ *                                      RPI3_GPIO_FUNC_ALT0: alt-0,
+ *                                      RPI3_GPIO_FUNC_ALT1: alt-1,
+ *                                      RPI3_GPIO_FUNC_ALT2: alt-2,
+ *                                      RPI3_GPIO_FUNC_ALT3: alt-3,
+ *                                      RPI3_GPIO_FUNC_ALT4: alt-4,
+ *                                      RPI3_GPIO_FUNC_ALT5: alt-5
+ */
+void rpi3_gpio_set_select(int gpio, int fsel)
+{
+	int regN = gpio / 10;
+	int shift = 3 * (gpio % 10);
+	uintptr_t reg_sel = reg_base + RPI3_GPIO_GPFSEL(regN);
+	uint32_t sel = mmio_read_32(reg_sel);
+	uint32_t mask = U(0x07) << shift;
+
+	sel = (sel & (~mask)) | ((fsel << shift) & mask);
+	mmio_write_32(reg_sel, sel);
+}
+
+static int rpi3_gpio_get_direction(int gpio)
+{
+	int result = rpi3_gpio_get_select(gpio);
+
+	if (result == RPI3_GPIO_FUNC_INPUT)
+		return GPIO_DIR_IN;
+	else if (result == RPI3_GPIO_FUNC_OUTPUT)
+		return GPIO_DIR_OUT;
+
+	return GPIO_DIR_IN;
+}
+
+static void rpi3_gpio_set_direction(int gpio, int direction)
+{
+	switch (direction) {
+	case GPIO_DIR_IN:
+		rpi3_gpio_set_select(gpio, RPI3_GPIO_FUNC_INPUT);
+		break;
+	case GPIO_DIR_OUT:
+		rpi3_gpio_set_select(gpio, RPI3_GPIO_FUNC_OUTPUT);
+		break;
+	}
+}
+
+static int rpi3_gpio_get_value(int gpio)
+{
+	int regN = gpio / 32;
+	int shift = gpio % 32;
+	uintptr_t reg_lev = reg_base + RPI3_GPIO_GPLEV(regN);
+	uint32_t value = mmio_read_32(reg_lev);
+
+	if ((value >> shift) & 0x01)
+		return GPIO_LEVEL_HIGH;
+	return GPIO_LEVEL_LOW;
+}
+
+static void rpi3_gpio_set_value(int gpio, int value)
+{
+	int regN = gpio / 32;
+	int shift = gpio % 32;
+	uintptr_t reg_set = reg_base + RPI3_GPIO_GPSET(regN);
+	uintptr_t reg_clr = reg_base + RPI3_GPIO_GPSET(regN);
+
+	switch (value) {
+	case GPIO_LEVEL_LOW:
+		mmio_write_32(reg_clr, U(1) << shift);
+		break;
+	case GPIO_LEVEL_HIGH:
+		mmio_write_32(reg_set, U(1) << shift);
+		break;
+	}
+}
+
+static void rpi3_gpio_set_pull(int gpio, int pull)
+{
+	int regN = gpio / 32;
+	int shift = gpio % 32;
+	uintptr_t reg_pud = reg_base + RPI3_GPIO_GPPUD;
+	uintptr_t reg_clk = reg_base + RPI3_GPIO_GPPUDCLK(regN);
+
+	switch (pull) {
+	case GPIO_PULL_NONE:
+		mmio_write_32(reg_pud, 0x0);
+		break;
+	case GPIO_PULL_UP:
+		mmio_write_32(reg_pud, 0x2);
+		break;
+	case GPIO_PULL_DOWN:
+		mmio_write_32(reg_pud, 0x1);
+		break;
+	}
+	mdelay(150);
+	mmio_write_32(reg_clk, U(1) << shift);
+	mdelay(150);
+	mmio_write_32(reg_clk, 0x0);
+	mmio_write_32(reg_pud, 0x0);
+}
+
+void rpi3_gpio_init(void)
+{
+	reg_base = RPI3_GPIO_BASE;
+	gpio_init(&rpi3_gpio_ops);
+}
diff --git a/drivers/rpi3/mailbox/rpi3_mbox.c b/drivers/rpi3/mailbox/rpi3_mbox.c
new file mode 100644
index 0000000..aef1f39
--- /dev/null
+++ b/drivers/rpi3/mailbox/rpi3_mbox.c
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include <rpi_hw.h>
+
+#include <drivers/rpi3/mailbox/rpi3_mbox.h>
+
+#define RPI3_MAILBOX_MAX_RETRIES	U(1000000)
+
+/*******************************************************************************
+ * Routine to send requests to the VideoCore using the mailboxes.
+ ******************************************************************************/
+void rpi3_vc_mailbox_request_send(rpi3_mbox_request_t *req, int req_size)
+{
+	uint32_t st, data;
+	uintptr_t resp_addr, addr;
+	unsigned int retries;
+
+	/* This is the location of the request buffer */
+	addr = (uintptr_t)req;
+
+	/* Make sure that the changes are seen by the VideoCore */
+	flush_dcache_range(addr, req_size);
+
+	/* Wait until the outbound mailbox is empty */
+	retries = 0U;
+
+	do {
+		st = mmio_read_32(RPI3_MBOX_BASE + RPI3_MBOX1_STATUS_OFFSET);
+
+		retries++;
+		if (retries == RPI3_MAILBOX_MAX_RETRIES) {
+			ERROR("rpi3: mbox: Send request timeout\n");
+			return;
+		}
+
+	} while ((st & RPI3_MBOX_STATUS_EMPTY_MASK) == 0U);
+
+	/* Send base address of this message to start request */
+	mmio_write_32(RPI3_MBOX_BASE + RPI3_MBOX1_WRITE_OFFSET,
+		      RPI3_CHANNEL_ARM_TO_VC | (uint32_t) addr);
+
+	/* Wait until the inbound mailbox isn't empty */
+	retries = 0U;
+
+	do {
+		st = mmio_read_32(RPI3_MBOX_BASE + RPI3_MBOX0_STATUS_OFFSET);
+
+		retries++;
+		if (retries == RPI3_MAILBOX_MAX_RETRIES) {
+			ERROR("rpi3: mbox: Receive response timeout\n");
+			return;
+		}
+
+	} while ((st & RPI3_MBOX_STATUS_EMPTY_MASK) != 0U);
+
+	/* Get location and channel */
+	data = mmio_read_32(RPI3_MBOX_BASE + RPI3_MBOX0_READ_OFFSET);
+
+	if ((data & RPI3_CHANNEL_MASK) != RPI3_CHANNEL_ARM_TO_VC) {
+		ERROR("rpi3: mbox: Wrong channel: 0x%08x\n", data);
+		panic();
+	}
+
+	resp_addr = (uintptr_t)(data & ~RPI3_CHANNEL_MASK);
+	if (addr != resp_addr) {
+		ERROR("rpi3: mbox: Unexpected address: 0x%08x\n", data);
+		panic();
+	}
+
+	/* Make sure that the data seen by the CPU is up to date */
+	inv_dcache_range(addr, req_size);
+}
diff --git a/drivers/rpi3/rng/rpi3_rng.c b/drivers/rpi3/rng/rpi3_rng.c
new file mode 100644
index 0000000..b6bf005
--- /dev/null
+++ b/drivers/rpi3/rng/rpi3_rng.c
@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <lib/mmio.h>
+
+#include <rpi_hw.h>
+
+/* Initial amount of values to discard */
+#define RNG_WARMUP_COUNT	U(0x40000)
+
+static void rpi3_rng_initialize(void)
+{
+	uint32_t int_mask, ctrl;
+
+	/* Return if it is already enabled */
+	ctrl = mmio_read_32(RPI3_RNG_BASE + RPI3_RNG_CTRL_OFFSET);
+	if ((ctrl & RPI3_RNG_CTRL_ENABLE) != 0U) {
+		return;
+	}
+
+	/* Mask interrupts */
+	int_mask = mmio_read_32(RPI3_RNG_BASE + RPI3_RNG_INT_MASK_OFFSET);
+	int_mask |= RPI3_RNG_INT_MASK_DISABLE;
+	mmio_write_32(RPI3_RNG_BASE + RPI3_RNG_INT_MASK_OFFSET, int_mask);
+
+	/* Discard several values when initializing to give it time to warmup */
+	mmio_write_32(RPI3_RNG_BASE + RPI3_RNG_STATUS_OFFSET, RNG_WARMUP_COUNT);
+
+	mmio_write_32(RPI3_RNG_BASE + RPI3_RNG_CTRL_OFFSET,
+		      RPI3_RNG_CTRL_ENABLE);
+}
+
+static uint32_t rpi3_rng_get_word(void)
+{
+	size_t nwords;
+
+	do {
+		/* Get number of available words to read */
+		nwords = (mmio_read_32(RPI3_RNG_BASE + RPI3_RNG_STATUS_OFFSET)
+				       >> RPI3_RNG_STATUS_NUM_WORDS_SHIFT)
+				       & RPI3_RNG_STATUS_NUM_WORDS_MASK;
+	} while (nwords == 0U);
+
+	return mmio_read_32(RPI3_RNG_BASE + RPI3_RNG_DATA_OFFSET);
+}
+
+void rpi3_rng_read(void *buf, size_t len)
+{
+	uint32_t data;
+	size_t left = len;
+	uint32_t *dst = buf;
+
+	assert(buf != NULL);
+	assert(len != 0U);
+	assert(check_uptr_overflow((uintptr_t) buf, (uintptr_t) len) == 0);
+
+	rpi3_rng_initialize();
+
+	while (left >= sizeof(uint32_t)) {
+		data = rpi3_rng_get_word();
+		*dst++ = data;
+		left -= sizeof(uint32_t);
+	}
+
+	if (left > 0U) {
+		data = rpi3_rng_get_word();
+		memcpy(dst, &data, left);
+	}
+}
diff --git a/drivers/rpi3/sdhost/rpi3_sdhost.c b/drivers/rpi3/sdhost/rpi3_sdhost.c
new file mode 100644
index 0000000..c4b6fca
--- /dev/null
+++ b/drivers/rpi3/sdhost/rpi3_sdhost.c
@@ -0,0 +1,679 @@
+/*
+ * Copyright (c) 2019, Linaro Limited
+ * Copyright (c) 2019, Ying-Chun Liu (PaulLiu) <paul.liu@linaro.org>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <assert.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <drivers/delay_timer.h>
+#include <drivers/rpi3/sdhost/rpi3_sdhost.h>
+#include <drivers/mmc.h>
+#include <drivers/rpi3/gpio/rpi3_gpio.h>
+#include <errno.h>
+#include <string.h>
+
+static void rpi3_sdhost_initialize(void);
+static int rpi3_sdhost_send_cmd(struct mmc_cmd *cmd);
+static int rpi3_sdhost_set_ios(unsigned int clk, unsigned int width);
+static int rpi3_sdhost_prepare(int lba, uintptr_t buf, size_t size);
+static int rpi3_sdhost_read(int lba, uintptr_t buf, size_t size);
+static int rpi3_sdhost_write(int lba, uintptr_t buf, size_t size);
+
+static const struct mmc_ops rpi3_sdhost_ops = {
+	.init		= rpi3_sdhost_initialize,
+	.send_cmd	= rpi3_sdhost_send_cmd,
+	.set_ios	= rpi3_sdhost_set_ios,
+	.prepare	= rpi3_sdhost_prepare,
+	.read		= rpi3_sdhost_read,
+	.write		= rpi3_sdhost_write,
+};
+
+static struct rpi3_sdhost_params rpi3_sdhost_params;
+
+/**
+ * Wait for command being processed.
+ *
+ * This function waits the command being processed. It compares
+ * the ENABLE flag of the HC_COMMAND register. When ENABLE flag disappeared
+ * it means the command is processed by the SDHOST.
+ * The timeout is currently 1000*100 us = 100 ms.
+ *
+ * @return 0: command finished. 1: command timed out.
+ */
+static int rpi3_sdhost_waitcommand(void)
+{
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+
+	volatile int timeout = 1000;
+
+	while ((mmio_read_32(reg_base + HC_COMMAND) & HC_CMD_ENABLE)
+	       && (--timeout > 0)) {
+		udelay(100);
+	}
+
+	return ((timeout > 0) ? 0 : (-(ETIMEDOUT)));
+}
+
+/**
+ * Send the command and argument to the SDHOST
+ *
+ * This function will wait for the previous command finished. And then
+ * clear any error status of previous command. And then
+ * send out the command and args. The command will be turned on the ENABLE
+ * flag before sending out.
+ */
+static void send_command_raw(unsigned int cmd, unsigned int arg)
+{
+	unsigned int status;
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+
+	/* wait for previous command finish */
+	rpi3_sdhost_waitcommand();
+
+	/* clean error status */
+	status = mmio_read_32(reg_base + HC_HOSTSTATUS);
+	if (status & HC_HSTST_MASK_ERROR_ALL)
+		mmio_write_32(reg_base + HC_HOSTSTATUS, status);
+
+	/* recording the command */
+	rpi3_sdhost_params.current_cmd = cmd & HC_CMD_COMMAND_MASK;
+
+	/* send the argument and command */
+	mmio_write_32(reg_base + HC_ARGUMENT, arg);
+	mmio_write_32(reg_base + HC_COMMAND, cmd | HC_CMD_ENABLE);
+}
+
+/**
+ * Send the command and argument to the SDHOST, decorated with control
+ * flags.
+ *
+ * This function will use send_command_raw to send the commands to SDHOST.
+ * But before sending it will decorate the command with control flags specific
+ * to SDHOST.
+ */
+static void send_command_decorated(unsigned int cmd, unsigned int arg)
+{
+	unsigned int cmd_flags = 0;
+
+	switch (cmd & HC_CMD_COMMAND_MASK) {
+	case MMC_CMD(0):
+		cmd_flags |= HC_CMD_RESPONSE_NONE;
+		break;
+	case MMC_ACMD(51):
+		cmd_flags |= HC_CMD_READ;
+		break;
+	case MMC_CMD(8):
+	case MMC_CMD(11):
+	case MMC_CMD(17):
+	case MMC_CMD(18):
+		cmd_flags |= HC_CMD_READ;
+		break;
+	case MMC_CMD(20):
+	case MMC_CMD(24):
+	case MMC_CMD(25):
+		cmd_flags |= HC_CMD_WRITE;
+		break;
+	case MMC_CMD(12):
+		cmd_flags |= HC_CMD_BUSY;
+		break;
+	default:
+		break;
+	}
+	send_command_raw(cmd | cmd_flags, arg);
+}
+
+/**
+ * drains the FIFO on DATA port
+ *
+ * This function drains any data left in the DATA port.
+ */
+static void rpi3_drain_fifo(void)
+{
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+	volatile int timeout = 100000;
+
+	rpi3_sdhost_waitcommand();
+
+	while (mmio_read_32(reg_base + HC_HOSTSTATUS) & HC_HSTST_HAVEDATA) {
+		mmio_read_32(reg_base + HC_DATAPORT);
+		udelay(100);
+	}
+
+	while (1) {
+		uint32_t edm, fsm;
+
+		edm = mmio_read_32(reg_base + HC_DEBUG);
+		fsm = edm & HC_DBG_FSM_MASK;
+
+		if ((fsm == HC_DBG_FSM_IDENTMODE) ||
+		    (fsm == HC_DBG_FSM_DATAMODE))
+			break;
+
+		if ((fsm == HC_DBG_FSM_READWAIT) ||
+		    (fsm == HC_DBG_FSM_WRITESTART1) ||
+		    (fsm == HC_DBG_FSM_READDATA)) {
+			mmio_write_32(reg_base + HC_DEBUG,
+				      edm | HC_DBG_FORCE_DATA_MODE);
+			break;
+		}
+
+		if (--timeout <= 0) {
+			ERROR("rpi3_sdhost: %s cannot recover stat\n",
+			      __func__);
+			return;
+		}
+	}
+}
+
+/**
+ * Dump SDHOST registers
+ */
+static void rpi3_sdhost_print_regs(void)
+{
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+
+	INFO("rpi3_sdhost: HC_COMMAND:        0x%08x\n",
+	     mmio_read_32(reg_base + HC_COMMAND));
+	INFO("rpi3_sdhost: HC_ARGUMENT:       0x%08x\n",
+	     mmio_read_32(reg_base + HC_ARGUMENT));
+	INFO("rpi3_sdhost: HC_TIMEOUTCOUNTER: 0x%08x\n",
+	     mmio_read_32(reg_base + HC_TIMEOUTCOUNTER));
+	INFO("rpi3_sdhost: HC_CLOCKDIVISOR:   0x%08x\n",
+	     mmio_read_32(reg_base + HC_CLOCKDIVISOR));
+	INFO("rpi3_sdhost: HC_RESPONSE_0:     0x%08x\n",
+	     mmio_read_32(reg_base + HC_RESPONSE_0));
+	INFO("rpi3_sdhost: HC_RESPONSE_1:     0x%08x\n",
+	     mmio_read_32(reg_base + HC_RESPONSE_1));
+	INFO("rpi3_sdhost: HC_RESPONSE_2:     0x%08x\n",
+	     mmio_read_32(reg_base + HC_RESPONSE_2));
+	INFO("rpi3_sdhost: HC_RESPONSE_3:     0x%08x\n",
+	     mmio_read_32(reg_base + HC_RESPONSE_3));
+	INFO("rpi3_sdhost: HC_HOSTSTATUS:     0x%08x\n",
+	     mmio_read_32(reg_base + HC_HOSTSTATUS));
+	INFO("rpi3_sdhost: HC_POWER:          0x%08x\n",
+	     mmio_read_32(reg_base + HC_POWER));
+	INFO("rpi3_sdhost: HC_DEBUG:          0x%08x\n",
+	     mmio_read_32(reg_base + HC_DEBUG));
+	INFO("rpi3_sdhost: HC_HOSTCONFIG:     0x%08x\n",
+	     mmio_read_32(reg_base + HC_HOSTCONFIG));
+	INFO("rpi3_sdhost: HC_BLOCKSIZE:      0x%08x\n",
+	     mmio_read_32(reg_base + HC_BLOCKSIZE));
+	INFO("rpi3_sdhost: HC_BLOCKCOUNT:     0x%08x\n",
+	     mmio_read_32(reg_base + HC_BLOCKCOUNT));
+}
+
+/**
+ * Reset SDHOST
+ */
+static void rpi3_sdhost_reset(void)
+{
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+	unsigned int dbg;
+	uint32_t tmp1;
+
+	mmio_write_32(reg_base + HC_POWER, 0);
+	mmio_write_32(reg_base + HC_COMMAND, 0);
+	mmio_write_32(reg_base + HC_ARGUMENT, 0);
+
+	mmio_write_32(reg_base + HC_TIMEOUTCOUNTER, HC_TIMEOUT_DEFAULT);
+	mmio_write_32(reg_base + HC_CLOCKDIVISOR, 0);
+	mmio_write_32(reg_base + HC_HOSTSTATUS, HC_HSTST_RESET);
+	mmio_write_32(reg_base + HC_HOSTCONFIG, 0);
+	mmio_write_32(reg_base + HC_BLOCKSIZE, 0);
+	mmio_write_32(reg_base + HC_BLOCKCOUNT, 0);
+
+	dbg = mmio_read_32(reg_base + HC_DEBUG);
+	dbg &= ~((HC_DBG_FIFO_THRESH_MASK << HC_DBG_FIFO_THRESH_READ_SHIFT) |
+		 (HC_DBG_FIFO_THRESH_MASK << HC_DBG_FIFO_THRESH_WRITE_SHIFT));
+	dbg |= (HC_FIFO_THRESH_READ << HC_DBG_FIFO_THRESH_READ_SHIFT) |
+		(HC_FIFO_THRESH_WRITE << HC_DBG_FIFO_THRESH_WRITE_SHIFT);
+	mmio_write_32(reg_base + HC_DEBUG, dbg);
+	mdelay(250);
+	mmio_write_32(reg_base + HC_POWER, 1);
+	mdelay(250);
+	rpi3_sdhost_params.clk_rate = 0;
+
+	mmio_write_32(reg_base + HC_CLOCKDIVISOR, HC_CLOCKDIVISOR_MAXVAL);
+	tmp1 = mmio_read_32(reg_base + HC_HOSTCONFIG);
+	mmio_write_32(reg_base + HC_HOSTCONFIG, tmp1 | HC_HSTCF_INT_BUSY);
+}
+
+static void rpi3_sdhost_initialize(void)
+{
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+
+	assert((rpi3_sdhost_params.reg_base & MMC_BLOCK_MASK) == 0);
+
+	rpi3_sdhost_reset();
+
+	mmio_write_32(reg_base + HC_CLOCKDIVISOR, HC_CLOCKDIVISOR_PREFERVAL);
+	udelay(300);
+}
+
+static int rpi3_sdhost_send_cmd(struct mmc_cmd *cmd)
+{
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+	int err = 0;
+	uint32_t cmd_idx;
+	uint32_t cmd_arg;
+	uint32_t cmd_flags = 0;
+	uint32_t intmask;
+
+	/* Wait for the command done */
+	err = rpi3_sdhost_waitcommand();
+	if (err != 0) {
+		WARN("previous command not done yet\n");
+		return err;
+	}
+
+	cmd_idx = cmd->cmd_idx & HC_CMD_COMMAND_MASK;
+
+	cmd_arg = cmd->cmd_arg;
+	if (cmd_idx == MMC_ACMD(51)) {
+		/* if previous cmd send to SDHOST is not MMC_CMD(55).
+		 * It means this MMC_ACMD(51) is a resend.
+		 * And we must also resend MMC_CMD(55) in this case
+		 */
+		if (rpi3_sdhost_params.current_cmd != MMC_CMD(55)) {
+			send_command_decorated(
+				MMC_CMD(55),
+				rpi3_sdhost_params.sdcard_rca <<
+				RCA_SHIFT_OFFSET);
+			rpi3_sdhost_params.mmc_app_cmd = 1;
+			rpi3_sdhost_waitcommand();
+
+			/* Also we need to call prepare to clean the buffer */
+			rpi3_sdhost_prepare(0, (uintptr_t)NULL, 8);
+		}
+	}
+
+	/* We ignore MMC_CMD(12) sending from the TF-A's MMC driver
+	 * because we send MMC_CMD(12) by ourselves.
+	 */
+	if (cmd_idx == MMC_CMD(12))
+		return 0;
+
+	if ((cmd->resp_type & MMC_RSP_136) &&
+	    (cmd->resp_type & MMC_RSP_BUSY)) {
+		ERROR("rpi3_sdhost: unsupported response type!\n");
+		return -(EOPNOTSUPP);
+	}
+
+	if (cmd->resp_type & MMC_RSP_48 && cmd->resp_type != MMC_RESPONSE_R2) {
+		/* 48-bit command
+		 * We don't need to set any flags here because it is default.
+		 */
+	} else if (cmd->resp_type & MMC_RSP_136) {
+		/* 136-bit command */
+		cmd_flags |= HC_CMD_RESPONSE_LONG;
+	} else {
+		/* no respond command */
+		cmd_flags |= HC_CMD_RESPONSE_NONE;
+	}
+
+	rpi3_sdhost_params.cmdbusy = 0;
+	if (cmd->resp_type & MMC_RSP_BUSY) {
+		cmd_flags |= HC_CMD_BUSY;
+		rpi3_sdhost_params.cmdbusy = 1;
+	}
+
+	if (rpi3_sdhost_params.mmc_app_cmd) {
+		switch (cmd_idx) {
+		case MMC_ACMD(41):
+			if (cmd_arg == OCR_HCS)
+				cmd_arg |= OCR_3_3_3_4;
+			break;
+		default:
+			break;
+		}
+		rpi3_sdhost_params.mmc_app_cmd = 0;
+	}
+
+	if (cmd_idx == MMC_CMD(55))
+		rpi3_sdhost_params.mmc_app_cmd = 1;
+
+	send_command_decorated(cmd_idx | cmd_flags, cmd_arg);
+
+	intmask = mmio_read_32(reg_base + HC_HOSTSTATUS);
+	if (rpi3_sdhost_params.cmdbusy && (intmask & HC_HSTST_INT_BUSY)) {
+		mmio_write_32(reg_base + HC_HOSTSTATUS, HC_HSTST_INT_BUSY);
+		rpi3_sdhost_params.cmdbusy = 0;
+	}
+
+	if (!(cmd_flags & HC_CMD_RESPONSE_NONE)) {
+		err = rpi3_sdhost_waitcommand();
+		if (err != 0)
+			ERROR("rpi3_sdhost: cmd cannot be finished\n");
+	}
+
+	cmd->resp_data[0] = mmio_read_32(reg_base + HC_RESPONSE_0);
+	cmd->resp_data[1] = mmio_read_32(reg_base + HC_RESPONSE_1);
+	cmd->resp_data[2] = mmio_read_32(reg_base + HC_RESPONSE_2);
+	cmd->resp_data[3] = mmio_read_32(reg_base + HC_RESPONSE_3);
+
+	if (mmio_read_32(reg_base + HC_COMMAND) & HC_CMD_FAILED) {
+		uint32_t sdhsts = mmio_read_32(reg_base + HC_HOSTSTATUS);
+
+		mmio_write_32(reg_base + HC_HOSTSTATUS,
+			      HC_HSTST_MASK_ERROR_ALL);
+
+		/*
+		 * If the command SEND_OP_COND returns with CRC7 error,
+		 * it can be considered as having completed successfully.
+		 */
+		if (!(sdhsts & HC_HSTST_ERROR_CRC7)
+		    || (cmd_idx != MMC_CMD(1))) {
+			if (sdhsts & HC_HSTST_TIMEOUT_CMD) {
+				ERROR("rpi3_sdhost: timeout status 0x%x\n",
+				      sdhsts);
+				err = -(ETIMEDOUT);
+			} else {
+				ERROR("rpi3_sdhost: unknown err, cmd = 0x%x\n",
+				      mmio_read_32(reg_base + HC_COMMAND));
+				ERROR("rpi3_sdhost status: 0x%x\n", sdhsts);
+				err = -(EILSEQ);
+			}
+		}
+	}
+
+	if ((!err) && (cmd_idx == MMC_CMD(3))) {
+		/* we keep the RCA in case to send MMC_CMD(55) ourselves */
+		rpi3_sdhost_params.sdcard_rca = (cmd->resp_data[0]
+						 & 0xFFFF0000U) >> 16;
+	}
+
+	return err;
+}
+
+static int rpi3_sdhost_set_clock(unsigned int clk)
+{
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+	uint32_t max_clk = 250000000;
+	uint32_t div;
+
+	if (clk < 100000) {
+		mmio_write_32(reg_base + HC_CLOCKDIVISOR,
+			      HC_CLOCKDIVISOR_MAXVAL);
+		return 0;
+	}
+
+	div = max_clk / clk;
+	if (div < 2)
+		div = 2;
+
+	if ((max_clk / div) > clk)
+		div++;
+
+	div -= 2;
+	if (div > HC_CLOCKDIVISOR_MAXVAL)
+		div = HC_CLOCKDIVISOR_MAXVAL;
+
+	rpi3_sdhost_params.clk_rate = max_clk / (div + 2);
+	rpi3_sdhost_params.ns_per_fifo_word = (1000000000 /
+					       rpi3_sdhost_params.clk_rate)
+		* 8;
+
+	mmio_write_32(reg_base + HC_CLOCKDIVISOR, div);
+	return 0;
+}
+
+static int rpi3_sdhost_set_ios(unsigned int clk, unsigned int width)
+{
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+	uint32_t tmp1;
+
+	rpi3_sdhost_set_clock(clk);
+	VERBOSE("rpi3_sdhost: Changing clock to %dHz for data mode\n", clk);
+
+	if (width != MMC_BUS_WIDTH_4 && width != MMC_BUS_WIDTH_1) {
+		ERROR("rpi3_sdhost: width %d not supported\n", width);
+		return -(EOPNOTSUPP);
+	}
+	rpi3_sdhost_params.bus_width = width;
+
+	tmp1 = mmio_read_32(reg_base + HC_HOSTCONFIG);
+	tmp1 &= ~(HC_HSTCF_EXTBUS_4BIT);
+	if (rpi3_sdhost_params.bus_width == MMC_BUS_WIDTH_4)
+		tmp1 |= HC_HSTCF_EXTBUS_4BIT;
+
+	mmio_write_32(reg_base + HC_HOSTCONFIG, tmp1);
+	tmp1 = mmio_read_32(reg_base + HC_HOSTCONFIG);
+	mmio_write_32(reg_base + HC_HOSTCONFIG, tmp1 |
+		      HC_HSTCF_SLOW_CARD | HC_HSTCF_INTBUS_WIDE);
+
+	return 0;
+}
+
+static int rpi3_sdhost_prepare(int lba, uintptr_t buf, size_t size)
+{
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+	size_t blocks;
+	size_t blocksize;
+
+	if (size < 512) {
+		blocksize = size;
+		blocks = 1;
+	} else {
+		blocksize = 512;
+		blocks = size / blocksize;
+		if (size % blocksize != 0)
+			blocks++;
+	}
+
+	rpi3_drain_fifo();
+
+	mmio_write_32(reg_base + HC_BLOCKSIZE, blocksize);
+	mmio_write_32(reg_base + HC_BLOCKCOUNT, blocks);
+	udelay(100);
+	return 0;
+}
+
+static int rpi3_sdhost_read(int lba, uintptr_t buf, size_t size)
+{
+	int err = 0;
+	uint32_t *buf1 = ((uint32_t *) buf);
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+	int timeout = 100000;
+	int remaining_words = 0;
+
+	for (int i = 0; i < size / 4; i++) {
+		volatile int t = timeout;
+		uint32_t hsts_err;
+
+		while ((mmio_read_32(reg_base + HC_HOSTSTATUS)
+			& HC_HSTST_HAVEDATA) == 0) {
+			if (t == 0) {
+				ERROR("rpi3_sdhost: fifo timeout after %dus\n",
+				      timeout);
+				err = -(ETIMEDOUT);
+				break;
+			}
+			t--;
+			udelay(10);
+		}
+		if (t == 0)
+			break;
+
+		uint32_t data = mmio_read_32(reg_base + HC_DATAPORT);
+
+		hsts_err = mmio_read_32(reg_base + HC_HOSTSTATUS)
+			& HC_HSTST_MASK_ERROR_ALL;
+		if (hsts_err) {
+			ERROR("rpi3_sdhost: transfer FIFO word %d: 0x%x\n",
+			      i,
+			      mmio_read_32(reg_base + HC_HOSTSTATUS));
+			rpi3_sdhost_print_regs();
+
+			err = -(EILSEQ);
+
+			/* clean the error status */
+			mmio_write_32(reg_base + HC_HOSTSTATUS, hsts_err);
+		}
+
+		if (buf1)
+			buf1[i] = data;
+
+		/* speeding up if the remaining words are still a lot */
+		remaining_words = (mmio_read_32(reg_base + HC_DEBUG) >> 4)
+			& HC_DBG_FIFO_THRESH_MASK;
+		if (remaining_words >= 7)
+			continue;
+
+		/* delay. slowing down the read process */
+		udelay(100);
+	}
+
+	/* We decide to stop by ourselves.
+	 * It is because MMC_CMD(18) -> MMC_CMD(13) -> MMC_CMD(12)
+	 * doesn't work for RPi3 SDHost.
+	 */
+	if (rpi3_sdhost_params.current_cmd == MMC_CMD(18))
+		send_command_decorated(MMC_CMD(12), 0);
+
+	return err;
+}
+
+static int rpi3_sdhost_write(int lba, uintptr_t buf, size_t size)
+{
+	uint32_t *buf1 = ((uint32_t *) buf);
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+	int err = 0;
+	int remaining_words = 0;
+
+	for (int i = 0; i < size / 4; i++) {
+		uint32_t hsts_err;
+		uint32_t data = buf1[i];
+		uint32_t dbg;
+		uint32_t fsm_state;
+
+		mmio_write_32(reg_base + HC_DATAPORT, data);
+
+		dbg = mmio_read_32(reg_base + HC_DEBUG);
+		fsm_state = dbg & HC_DBG_FSM_MASK;
+		if (fsm_state != HC_DBG_FSM_WRITEDATA
+		    && fsm_state != HC_DBG_FSM_WRITESTART1
+		    && fsm_state != HC_DBG_FSM_WRITESTART2
+		    && fsm_state != HC_DBG_FSM_WRITECRC
+		    && fsm_state != HC_DBG_FSM_WRITEWAIT1
+		    && fsm_state != HC_DBG_FSM_WRITEWAIT2) {
+			hsts_err = mmio_read_32(reg_base + HC_HOSTSTATUS)
+				& HC_HSTST_MASK_ERROR_ALL;
+			if (hsts_err)
+				err = -(EILSEQ);
+		}
+
+		/* speeding up if the remaining words are not many */
+		remaining_words = (mmio_read_32(reg_base + HC_DEBUG) >> 4)
+			& HC_DBG_FIFO_THRESH_MASK;
+		if (remaining_words <= 4)
+			continue;
+
+		udelay(100);
+	}
+
+	/* We decide to stop by ourselves.
+	 * It is because MMC_CMD(25) -> MMC_CMD(13) -> MMC_CMD(12)
+	 * doesn't work for RPi3 SDHost.
+	 */
+	if (rpi3_sdhost_params.current_cmd == MMC_CMD(25))
+		send_command_decorated(MMC_CMD(12), 0);
+
+	return err;
+}
+
+void rpi3_sdhost_init(struct rpi3_sdhost_params *params,
+		    struct mmc_device_info *mmc_dev_info)
+{
+	assert((params != 0) &&
+	       ((params->reg_base & MMC_BLOCK_MASK) == 0));
+
+	memcpy(&rpi3_sdhost_params, params, sizeof(struct rpi3_sdhost_params));
+
+	/* backup GPIO 48 to 53 configurations */
+	for (int i = 48; i <= 53; i++) {
+		rpi3_sdhost_params.gpio48_pinselect[i - 48]
+			= rpi3_gpio_get_select(i);
+		VERBOSE("rpi3_sdhost: Original GPIO state %d: %d\n",
+			i,
+			rpi3_sdhost_params.gpio48_pinselect[i - 48]);
+	}
+
+	/* setting pull resistors for 48 to 53.
+	 * It is debatable to set SD_CLK to UP or NONE. We massively
+	 * tested different brands of SD Cards and found NONE works
+	 * most stable.
+	 *
+	 * GPIO 48 (SD_CLK) to GPIO_PULL_NONE
+	 * GPIO 49 (SD_CMD) to GPIO_PULL_UP
+	 * GPIO 50 (SD_D0)  to GPIO_PULL_UP
+	 * GPIO 51 (SD_D1)  to GPIO_PULL_UP
+	 * GPIO 52 (SD_D2)  to GPIO_PULL_UP
+	 * GPIO 53 (SD_D3)  to GPIO_PULL_UP
+	 */
+	gpio_set_pull(48, GPIO_PULL_NONE);
+	for (int i = 49; i <= 53; i++)
+		gpio_set_pull(i, GPIO_PULL_UP);
+
+	/* Set pin 48-53 to alt-0. It means route SDHOST to card slot */
+	for (int i = 48; i <= 53; i++)
+		rpi3_gpio_set_select(i, RPI3_GPIO_FUNC_ALT0);
+
+	mmc_init(&rpi3_sdhost_ops, params->clk_rate, params->bus_width,
+		 params->flags, mmc_dev_info);
+}
+
+void rpi3_sdhost_stop(void)
+{
+	uintptr_t reg_base = rpi3_sdhost_params.reg_base;
+
+	VERBOSE("rpi3_sdhost: Shutting down: drain FIFO out\n");
+	rpi3_drain_fifo();
+
+	VERBOSE("rpi3_sdhost: Shutting down: slowing down the clock\n");
+	mmio_write_32(reg_base+HC_CLOCKDIVISOR, HC_CLOCKDIVISOR_SLOWVAL);
+	udelay(500);
+
+	VERBOSE("rpi3_sdhost: Shutting down: put SDHost into idle state\n");
+	send_command_decorated(MMC_CMD(0), 0);
+	udelay(500);
+
+	mmio_write_32(reg_base + HC_COMMAND, 0);
+	mmio_write_32(reg_base + HC_ARGUMENT, 0);
+	mmio_write_32(reg_base + HC_TIMEOUTCOUNTER, HC_TIMEOUT_IDLE);
+	mmio_write_32(reg_base + HC_CLOCKDIVISOR, HC_CLOCKDIVISOR_STOPVAL);
+
+	udelay(100);
+
+	mmio_write_32(reg_base + HC_POWER, 0);
+	mmio_write_32(reg_base + HC_HOSTCONFIG, 0);
+	mmio_write_32(reg_base + HC_BLOCKSIZE, 0x400);
+	mmio_write_32(reg_base + HC_BLOCKCOUNT, 0);
+	mmio_write_32(reg_base + HC_HOSTSTATUS, 0x7f8);
+
+	mmio_write_32(reg_base + HC_COMMAND, 0);
+	mmio_write_32(reg_base + HC_ARGUMENT, 0);
+
+	udelay(100);
+
+	/* Restore the pinmux to original state */
+	for (int i = 48; i <= 53; i++) {
+		rpi3_gpio_set_select(i,
+				     rpi3_sdhost_params.gpio48_pinselect[i-48]);
+	}
+
+	/* Reset the pull resistors before entering BL33.
+	 * GPIO 48 (SD_CLK) to GPIO_PULL_UP
+	 * GPIO 49 (SD_CMD) to GPIO_PULL_UP
+	 * GPIO 50 (SD_D0)  to GPIO_PULL_UP
+	 * GPIO 51 (SD_D1)  to GPIO_PULL_UP
+	 * GPIO 52 (SD_D2)  to GPIO_PULL_UP
+	 * GPIO 53 (SD_D3)  to GPIO_PULL_UP
+	 */
+	for (int i = 48; i <= 53; i++)
+		gpio_set_pull(i, GPIO_PULL_UP);
+}
diff --git a/drivers/scmi-msg/base.c b/drivers/scmi-msg/base.c
new file mode 100644
index 0000000..2db4d7e
--- /dev/null
+++ b/drivers/scmi-msg/base.c
@@ -0,0 +1,193 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2022, Linaro Limited
+ */
+#include <assert.h>
+#include <string.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+#include <lib/utils.h>
+#include <lib/utils_def.h>
+
+#include "common.h"
+
+static bool message_id_is_supported(unsigned int message_id);
+
+static void report_version(struct scmi_msg *msg)
+{
+	struct scmi_protocol_version_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.version = SCMI_PROTOCOL_VERSION_BASE,
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_attributes(struct scmi_msg *msg)
+{
+	size_t protocol_count = plat_scmi_protocol_count();
+	struct scmi_protocol_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		/* Null agent count since agent discovery is not supported */
+		.attributes = SCMI_BASE_PROTOCOL_ATTRIBUTES(protocol_count, 0U),
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_message_attributes(struct scmi_msg *msg)
+{
+	struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_protocol_message_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		/* For this protocol, attributes shall be zero */
+		.attributes = 0U,
+	};
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	if (!message_id_is_supported(in_args->message_id)) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void discover_vendor(struct scmi_msg *msg)
+{
+	const char *name = plat_scmi_vendor_name();
+	struct scmi_base_discover_vendor_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	COPY_NAME_IDENTIFIER(return_values.vendor_identifier, name);
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void discover_sub_vendor(struct scmi_msg *msg)
+{
+	const char *name = plat_scmi_sub_vendor_name();
+	struct scmi_base_discover_sub_vendor_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	COPY_NAME_IDENTIFIER(return_values.sub_vendor_identifier, name);
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void discover_implementation_version(struct scmi_msg *msg)
+{
+	struct scmi_protocol_version_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.version = SCMI_IMPL_VERSION,
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static unsigned int count_protocols_in_list(const uint8_t *protocol_list)
+{
+	unsigned int count = 0U;
+
+	if (protocol_list != NULL) {
+		while (protocol_list[count] != 0U) {
+			count++;
+		}
+	}
+
+	return count;
+}
+
+static void discover_list_protocols(struct scmi_msg *msg)
+{
+	const struct scmi_base_discover_list_protocols_a2p *a2p = NULL;
+	struct scmi_base_discover_list_protocols_p2a p2a = {
+		.status = SCMI_SUCCESS,
+	};
+	const uint8_t *list = NULL;
+	unsigned int count = 0U;
+
+	if (msg->in_size != sizeof(*a2p)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	a2p = (void *)msg->in;
+
+	list = plat_scmi_protocol_list(msg->agent_id);
+	count = count_protocols_in_list(list);
+
+	if (count > a2p->skip) {
+		count = MIN(count - a2p->skip, msg->out_size - sizeof(p2a));
+	} else {
+		count = 0U;
+	}
+
+	p2a.num_protocols = count;
+
+	memcpy(msg->out, &p2a, sizeof(p2a));
+	memcpy(msg->out + sizeof(p2a), list + a2p->skip, count);
+	msg->out_size_out = sizeof(p2a) + round_up(count, sizeof(uint32_t));
+}
+
+static const scmi_msg_handler_t scmi_base_handler_table[] = {
+	[SCMI_PROTOCOL_VERSION] = report_version,
+	[SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
+	[SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
+	[SCMI_BASE_DISCOVER_VENDOR] = discover_vendor,
+	[SCMI_BASE_DISCOVER_SUB_VENDOR] = discover_sub_vendor,
+	[SCMI_BASE_DISCOVER_IMPLEMENTATION_VERSION] =
+					discover_implementation_version,
+	[SCMI_BASE_DISCOVER_LIST_PROTOCOLS] = discover_list_protocols,
+};
+
+static bool message_id_is_supported(unsigned int message_id)
+{
+	return (message_id < ARRAY_SIZE(scmi_base_handler_table)) &&
+	       (scmi_base_handler_table[message_id] != NULL);
+}
+
+scmi_msg_handler_t scmi_msg_get_base_handler(struct scmi_msg *msg)
+{
+	unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id);
+
+	if (message_id >= ARRAY_SIZE(scmi_base_handler_table)) {
+		VERBOSE("Base handle not found %u\n", msg->message_id);
+		return NULL;
+	}
+
+	return scmi_base_handler_table[message_id];
+}
diff --git a/drivers/scmi-msg/base.h b/drivers/scmi-msg/base.h
new file mode 100644
index 0000000..c4a9c64
--- /dev/null
+++ b/drivers/scmi-msg/base.h
@@ -0,0 +1,75 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+
+#ifndef SCMI_MSG_BASE_H
+#define SCMI_MSG_BASE_H
+
+#include <stdint.h>
+
+#define SCMI_PROTOCOL_VERSION_BASE	0x20000U
+
+#define SCMI_DEFAULT_STRING_LENGTH	16U
+
+enum scmi_base_message_id {
+	SCMI_BASE_DISCOVER_VENDOR			= 0x003,
+	SCMI_BASE_DISCOVER_SUB_VENDOR			= 0x004,
+	SCMI_BASE_DISCOVER_IMPLEMENTATION_VERSION	= 0x005,
+	SCMI_BASE_DISCOVER_LIST_PROTOCOLS		= 0x006,
+	SCMI_BASE_DISCOVER_AGENT			= 0x007,
+	SCMI_BASE_NOTIFY_ERRORS				= 0x008,
+};
+
+/*
+ * PROTOCOL_ATTRIBUTES
+ */
+
+#define SCMI_BASE_PROTOCOL_ATTRS_NUM_PROTOCOLS_POS	0
+#define SCMI_BASE_PROTOCOL_ATTRS_NUM_AGENTS_POS		8
+
+#define SCMI_BASE_PROTOCOL_ATTRS_NUM_PROTOCOLS_MASK	0xFFU
+#define SCMI_BASE_PROTOCOL_ATTRS_NUM_AGENTS_MASK	0xFF00U
+
+#define SCMI_BASE_PROTOCOL_ATTRIBUTES(NUM_PROTOCOLS, NUM_AGENTS) \
+	((((NUM_PROTOCOLS) << SCMI_BASE_PROTOCOL_ATTRS_NUM_PROTOCOLS_POS) & \
+	  SCMI_BASE_PROTOCOL_ATTRS_NUM_PROTOCOLS_MASK) | \
+	(((NUM_AGENTS) << SCMI_BASE_PROTOCOL_ATTRS_NUM_AGENTS_POS) & \
+	 SCMI_BASE_PROTOCOL_ATTRS_NUM_AGENTS_MASK))
+
+/*
+ * BASE_DISCOVER_VENDOR
+ */
+struct scmi_base_discover_vendor_p2a {
+	int32_t status;
+	char vendor_identifier[SCMI_DEFAULT_STRING_LENGTH];
+};
+
+/*
+ * BASE_DISCOVER_SUB_VENDOR
+ */
+struct scmi_base_discover_sub_vendor_p2a {
+	int32_t status;
+	char sub_vendor_identifier[SCMI_DEFAULT_STRING_LENGTH];
+};
+
+/*
+ * BASE_DISCOVER_IMPLEMENTATION_VERSION
+ * No special structure right now, see protocol_version.
+ */
+
+/*
+ * BASE_DISCOVER_LIST_PROTOCOLS
+ */
+struct scmi_base_discover_list_protocols_a2p {
+	uint32_t skip;
+};
+
+struct scmi_base_discover_list_protocols_p2a {
+	int32_t status;
+	uint32_t num_protocols;
+	uint32_t protocols[];
+};
+
+#endif /* SCMI_MSG_BASE_H */
diff --git a/drivers/scmi-msg/clock.c b/drivers/scmi-msg/clock.c
new file mode 100644
index 0000000..85bf7d2
--- /dev/null
+++ b/drivers/scmi-msg/clock.c
@@ -0,0 +1,381 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+#include <cdefs.h>
+#include <string.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+#include <lib/utils_def.h>
+
+#include "common.h"
+
+#pragma weak plat_scmi_clock_count
+#pragma weak plat_scmi_clock_get_name
+#pragma weak plat_scmi_clock_rates_array
+#pragma weak plat_scmi_clock_rates_by_step
+#pragma weak plat_scmi_clock_get_rate
+#pragma weak plat_scmi_clock_set_rate
+#pragma weak plat_scmi_clock_get_state
+#pragma weak plat_scmi_clock_set_state
+
+static bool message_id_is_supported(unsigned int message_id);
+
+size_t plat_scmi_clock_count(unsigned int agent_id __unused)
+{
+	return 0U;
+}
+
+const char *plat_scmi_clock_get_name(unsigned int agent_id __unused,
+				     unsigned int scmi_id __unused)
+{
+	return NULL;
+}
+
+int32_t plat_scmi_clock_rates_array(unsigned int agent_id __unused,
+				    unsigned int scmi_id __unused,
+				    unsigned long *rates __unused,
+				    size_t *nb_elts __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+int32_t plat_scmi_clock_rates_by_step(unsigned int agent_id __unused,
+				      unsigned int scmi_id __unused,
+				      unsigned long *steps __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+unsigned long plat_scmi_clock_get_rate(unsigned int agent_id __unused,
+				       unsigned int scmi_id __unused)
+{
+	return 0U;
+}
+
+int32_t plat_scmi_clock_set_rate(unsigned int agent_id __unused,
+				 unsigned int scmi_id __unused,
+				 unsigned long rate __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+int32_t plat_scmi_clock_get_state(unsigned int agent_id __unused,
+				  unsigned int scmi_id __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+int32_t plat_scmi_clock_set_state(unsigned int agent_id __unused,
+				  unsigned int scmi_id __unused,
+				  bool enable_not_disable __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+static void report_version(struct scmi_msg *msg)
+{
+	struct scmi_protocol_version_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.version = SCMI_PROTOCOL_VERSION_CLOCK,
+	};
+
+	if (msg->in_size != 0) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_attributes(struct scmi_msg *msg)
+{
+	size_t agent_count = plat_scmi_clock_count(msg->agent_id);
+	struct scmi_protocol_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.attributes = SCMI_CLOCK_PROTOCOL_ATTRIBUTES(1U, agent_count),
+	};
+
+	if (msg->in_size != 0) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_message_attributes(struct scmi_msg *msg)
+{
+	struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_protocol_message_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		/* For this protocol, attributes shall be zero */
+		.attributes = 0U,
+	};
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	if (!message_id_is_supported(in_args->message_id)) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_clock_attributes(struct scmi_msg *msg)
+{
+	const struct scmi_clock_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_clock_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+	const char *name = NULL;
+	unsigned int clock_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
+
+	if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+
+	name = plat_scmi_clock_get_name(msg->agent_id, clock_id);
+	if (name == NULL) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	COPY_NAME_IDENTIFIER(return_values.clock_name, name);
+
+	return_values.attributes = plat_scmi_clock_get_state(msg->agent_id,
+							     clock_id);
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_clock_rate_get(struct scmi_msg *msg)
+{
+	const struct scmi_clock_rate_get_a2p *in_args = (void *)msg->in;
+	unsigned long rate = 0U;
+	struct scmi_clock_rate_get_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+	unsigned int clock_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
+
+	if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	rate = plat_scmi_clock_get_rate(msg->agent_id, clock_id);
+
+	return_values.rate[0] = (uint32_t)rate;
+	return_values.rate[1] = (uint32_t)((uint64_t)rate >> 32);
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_clock_rate_set(struct scmi_msg *msg)
+{
+	const struct scmi_clock_rate_set_a2p *in_args = (void *)msg->in;
+	unsigned long rate = 0U;
+	int32_t status = 0;
+	unsigned int clock_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
+
+	if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	rate = (unsigned long)(((uint64_t)in_args->rate[1] << 32) |
+			       in_args->rate[0]);
+
+	status = plat_scmi_clock_set_rate(msg->agent_id, clock_id, rate);
+
+	scmi_status_response(msg, status);
+}
+
+static void scmi_clock_config_set(struct scmi_msg *msg)
+{
+	const struct scmi_clock_config_set_a2p *in_args = (void *)msg->in;
+	int32_t status = SCMI_GENERIC_ERROR;
+	bool enable = false;
+	unsigned int clock_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
+
+	if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	enable = in_args->attributes & SCMI_CLOCK_CONFIG_SET_ENABLE_MASK;
+
+	status = plat_scmi_clock_set_state(msg->agent_id, clock_id, enable);
+
+	scmi_status_response(msg, status);
+}
+
+#define RATES_ARRAY_SIZE_MAX	(SCMI_PLAYLOAD_MAX - \
+				 sizeof(struct scmi_clock_describe_rates_p2a))
+
+#define SCMI_RATES_BY_ARRAY(_nb_rates, _rem_rates) \
+	SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS((_nb_rates), \
+						SCMI_CLOCK_RATE_FORMAT_LIST, \
+						(_rem_rates))
+#define SCMI_RATES_BY_STEP \
+	SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS(3U, \
+						SCMI_CLOCK_RATE_FORMAT_RANGE, \
+						0U)
+
+#define RATE_DESC_SIZE		sizeof(struct scmi_clock_rate)
+
+static void write_rate_desc_array_in_buffer(char *dest, unsigned long *rates,
+					    size_t nb_elt)
+{
+	uint32_t *out = (uint32_t *)(uintptr_t)dest;
+	size_t n;
+
+	ASSERT_SYM_PTR_ALIGN(out);
+
+	for (n = 0U; n < nb_elt; n++) {
+		out[2 * n] = (uint32_t)rates[n];
+		out[2 * n + 1] = (uint32_t)((uint64_t)rates[n] >> 32);
+	}
+}
+
+static void scmi_clock_describe_rates(struct scmi_msg *msg)
+{
+	const struct scmi_clock_describe_rates_a2p *in_args = (void *)msg->in;
+	struct scmi_clock_describe_rates_p2a p2a = {
+		.status = SCMI_SUCCESS,
+	};
+	size_t nb_rates;
+	int32_t status;
+	unsigned int clock_id;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
+
+	if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	/* Platform may support array rate description */
+	status = plat_scmi_clock_rates_array(msg->agent_id, clock_id, NULL,
+					     &nb_rates);
+	if (status == SCMI_SUCCESS) {
+		/* Currently 12 cells mex, so it's affordable for the stack */
+		unsigned long plat_rates[RATES_ARRAY_SIZE_MAX / RATE_DESC_SIZE];
+		size_t max_nb = RATES_ARRAY_SIZE_MAX / RATE_DESC_SIZE;
+		size_t ret_nb = MIN(nb_rates - in_args->rate_index, max_nb);
+		size_t rem_nb = nb_rates - in_args->rate_index - ret_nb;
+
+		status =  plat_scmi_clock_rates_array(msg->agent_id, clock_id,
+						      plat_rates, &ret_nb);
+		if (status == SCMI_SUCCESS) {
+			write_rate_desc_array_in_buffer(msg->out + sizeof(p2a),
+							plat_rates, ret_nb);
+
+			p2a.num_rates_flags = SCMI_RATES_BY_ARRAY(ret_nb,
+								  rem_nb);
+			p2a.status = SCMI_SUCCESS;
+
+			memcpy(msg->out, &p2a, sizeof(p2a));
+			msg->out_size_out = sizeof(p2a) +
+					    ret_nb * RATE_DESC_SIZE;
+		}
+	} else if (status == SCMI_NOT_SUPPORTED) {
+		unsigned long triplet[3] = { 0U, 0U, 0U };
+
+		/* Platform may support min§max/step triplet description */
+		status =  plat_scmi_clock_rates_by_step(msg->agent_id, clock_id,
+							triplet);
+		if (status == SCMI_SUCCESS) {
+			write_rate_desc_array_in_buffer(msg->out + sizeof(p2a),
+							triplet, 3U);
+
+			p2a.num_rates_flags = SCMI_RATES_BY_STEP;
+			p2a.status = SCMI_SUCCESS;
+
+			memcpy(msg->out, &p2a, sizeof(p2a));
+			msg->out_size_out = sizeof(p2a) + (3U * RATE_DESC_SIZE);
+		}
+	} else {
+		/* Fallthrough generic exit sequence below with error status */
+	}
+
+	if (status != SCMI_SUCCESS) {
+		scmi_status_response(msg, status);
+	} else {
+		/*
+		 * Message payload is already writen to msg->out, and
+		 * msg->out_size_out updated.
+		 */
+	}
+}
+
+static const scmi_msg_handler_t scmi_clock_handler_table[] = {
+	[SCMI_PROTOCOL_VERSION] = report_version,
+	[SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
+	[SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
+	[SCMI_CLOCK_ATTRIBUTES] = scmi_clock_attributes,
+	[SCMI_CLOCK_DESCRIBE_RATES] = scmi_clock_describe_rates,
+	[SCMI_CLOCK_RATE_SET] = scmi_clock_rate_set,
+	[SCMI_CLOCK_RATE_GET] = scmi_clock_rate_get,
+	[SCMI_CLOCK_CONFIG_SET] = scmi_clock_config_set,
+};
+
+static bool message_id_is_supported(unsigned int message_id)
+{
+	return (message_id < ARRAY_SIZE(scmi_clock_handler_table)) &&
+	       (scmi_clock_handler_table[message_id] != NULL);
+}
+
+scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg)
+{
+	const size_t array_size = ARRAY_SIZE(scmi_clock_handler_table);
+	unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id);
+
+	if (message_id >= array_size) {
+		VERBOSE("Clock handle not found %u", msg->message_id);
+		return NULL;
+	}
+
+	return scmi_clock_handler_table[message_id];
+}
diff --git a/drivers/scmi-msg/clock.h b/drivers/scmi-msg/clock.h
new file mode 100644
index 0000000..a637934
--- /dev/null
+++ b/drivers/scmi-msg/clock.h
@@ -0,0 +1,150 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019, Linaro Limited
+ */
+
+#ifndef SCMI_MSG_CLOCK_H
+#define SCMI_MSG_CLOCK_H
+
+#include <stdint.h>
+
+#include <lib/utils_def.h>
+
+#define SCMI_PROTOCOL_VERSION_CLOCK	0x20000U
+
+/*
+ * Identifiers of the SCMI Clock Management Protocol commands
+ */
+enum scmi_clock_command_id {
+	SCMI_CLOCK_ATTRIBUTES = 0x003,
+	SCMI_CLOCK_DESCRIBE_RATES = 0x004,
+	SCMI_CLOCK_RATE_SET = 0x005,
+	SCMI_CLOCK_RATE_GET = 0x006,
+	SCMI_CLOCK_CONFIG_SET = 0x007,
+};
+
+/* Protocol attributes */
+#define SCMI_CLOCK_CLOCK_COUNT_MASK			GENMASK(15, 0)
+#define SCMI_CLOCK_MAX_PENDING_TRANSITIONS_MASK		GENMASK(23, 16)
+
+#define SCMI_CLOCK_PROTOCOL_ATTRIBUTES(_max_pending, _clk_count) \
+	((((_max_pending) << 16) & SCMI_CLOCK_MAX_PENDING_TRANSITIONS_MASK) | \
+	 (((_clk_count) & SCMI_CLOCK_CLOCK_COUNT_MASK)))
+
+struct scmi_clock_attributes_a2p {
+	uint32_t clock_id;
+};
+
+#define SCMI_CLOCK_NAME_LENGTH_MAX	16U
+
+struct scmi_clock_attributes_p2a {
+	int32_t status;
+	uint32_t attributes;
+	char clock_name[SCMI_CLOCK_NAME_LENGTH_MAX];
+};
+
+/*
+ * Clock Rate Get
+ */
+
+struct scmi_clock_rate_get_a2p {
+	uint32_t clock_id;
+};
+
+struct scmi_clock_rate_get_p2a {
+	int32_t status;
+	uint32_t rate[2];
+};
+
+/*
+ * Clock Rate Set
+ */
+
+/* If set, set the new clock rate asynchronously */
+#define SCMI_CLOCK_RATE_SET_ASYNC_POS			0
+/* If set, do not send a delayed asynchronous response */
+#define SCMI_CLOCK_RATE_SET_NO_DELAYED_RESPONSE_POS	1
+/* Round up, if set, otherwise round down */
+#define SCMI_CLOCK_RATE_SET_ROUND_UP_POS		2
+/* If set, the platform chooses the appropriate rounding mode */
+#define SCMI_CLOCK_RATE_SET_ROUND_AUTO_POS		3
+
+#define SCMI_CLOCK_RATE_SET_ASYNC_MASK \
+		BIT(SCMI_CLOCK_RATE_SET_ASYNC_POS)
+#define SCMI_CLOCK_RATE_SET_NO_DELAYED_RESPONSE_MASK \
+		BIT(SCMI_CLOCK_RATE_SET_NO_DELAYED_RESPONSE_POS)
+#define SCMI_CLOCK_RATE_SET_ROUND_UP_MASK \
+		BIT(SCMI_CLOCK_RATE_SET_ROUND_UP_POS)
+#define SCMI_CLOCK_RATE_SET_ROUND_AUTO_MASK \
+		BIT(SCMI_CLOCK_RATE_SET_ROUND_AUTO_POS)
+
+struct scmi_clock_rate_set_a2p {
+	uint32_t flags;
+	uint32_t clock_id;
+	uint32_t rate[2];
+};
+
+struct scmi_clock_rate_set_p2a {
+	int32_t status;
+};
+
+/*
+ * Clock Config Set
+ */
+
+#define SCMI_CLOCK_CONFIG_SET_ENABLE_POS	0
+
+#define SCMI_CLOCK_CONFIG_SET_ENABLE_MASK \
+	BIT(SCMI_CLOCK_CONFIG_SET_ENABLE_POS)
+
+struct scmi_clock_config_set_a2p {
+	uint32_t clock_id;
+	uint32_t attributes;
+};
+
+struct scmi_clock_config_set_p2a {
+	int32_t status;
+};
+
+/*
+ * Clock Describe Rates
+ */
+
+#define SCMI_CLOCK_RATE_FORMAT_RANGE			1U
+#define SCMI_CLOCK_RATE_FORMAT_LIST			0U
+
+#define SCMI_CLOCK_DESCRIBE_RATES_REMAINING_MASK	GENMASK_32(31, 16)
+#define SCMI_CLOCK_DESCRIBE_RATES_REMAINING_POS		16
+
+#define SCMI_CLOCK_DESCRIBE_RATES_FORMAT_MASK		BIT(12)
+#define SCMI_CLOCK_DESCRIBE_RATES_FORMAT_POS		12
+
+#define SCMI_CLOCK_DESCRIBE_RATES_COUNT_MASK		GENMASK_32(11, 0)
+
+#define SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS(_count, _fmt, _rem_rates) \
+	( \
+		((_count) & SCMI_CLOCK_DESCRIBE_RATES_COUNT_MASK) | \
+		(((_rem_rates) << SCMI_CLOCK_DESCRIBE_RATES_REMAINING_POS) & \
+		 SCMI_CLOCK_DESCRIBE_RATES_REMAINING_MASK) | \
+		(((_fmt) << SCMI_CLOCK_DESCRIBE_RATES_FORMAT_POS) & \
+		 SCMI_CLOCK_DESCRIBE_RATES_FORMAT_MASK) \
+	)
+
+struct scmi_clock_rate {
+	uint32_t low;
+	uint32_t high;
+};
+
+struct scmi_clock_describe_rates_a2p {
+	uint32_t clock_id;
+	uint32_t rate_index;
+};
+
+struct scmi_clock_describe_rates_p2a {
+	int32_t status;
+	uint32_t num_rates_flags;
+	struct scmi_clock_rate rates[];
+};
+
+#endif /* SCMI_MSG_CLOCK_H */
diff --git a/drivers/scmi-msg/common.h b/drivers/scmi-msg/common.h
new file mode 100644
index 0000000..62f3087
--- /dev/null
+++ b/drivers/scmi-msg/common.h
@@ -0,0 +1,144 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+#ifndef SCMI_MSG_COMMON_H
+#define SCMI_MSG_COMMON_H
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "base.h"
+#include "clock.h"
+#include "power_domain.h"
+#include "reset_domain.h"
+
+#define SCMI_VERSION			0x20000U
+#define SCMI_IMPL_VERSION		0U
+
+#define SCMI_PLAYLOAD_MAX		92U
+
+/*
+ * Copy name identifier in target buffer following the SCMI specification
+ * that state name identifier shall be a null terminated string.
+ */
+#define COPY_NAME_IDENTIFIER(_dst_array, _name)				\
+	do {								\
+		assert(strlen(_name) < sizeof(_dst_array));		\
+		strlcpy((_dst_array), (_name), sizeof(_dst_array));	\
+	} while (0)
+
+/* Common command identifiers shared by all procotols */
+enum scmi_common_message_id {
+	SCMI_PROTOCOL_VERSION = 0x000,
+	SCMI_PROTOCOL_ATTRIBUTES = 0x001,
+	SCMI_PROTOCOL_MESSAGE_ATTRIBUTES = 0x002
+};
+
+/* Common platform-to-agent (p2a) PROTOCOL_VERSION structure */
+struct scmi_protocol_version_p2a {
+	int32_t status;
+	uint32_t version;
+};
+
+/* Generic platform-to-agent (p2a) PROTOCOL_ATTRIBUTES structure */
+struct scmi_protocol_attributes_p2a {
+	int32_t status;
+	uint32_t attributes;
+};
+
+/* Generic agent-to-platform (a2p) PROTOCOL_MESSAGE_ATTRIBUTES structure */
+struct scmi_protocol_message_attributes_a2p {
+	uint32_t message_id;
+};
+
+/* Generic platform-to-agent (p2a) PROTOCOL_MESSAGE_ATTRIBUTES structure */
+struct scmi_protocol_message_attributes_p2a {
+	int32_t status;
+	uint32_t attributes;
+};
+
+/*
+ * struct scmi_msg - SCMI message context
+ *
+ * @agent_id: SCMI agent ID, safely set from secure world
+ * @protocol_id: SCMI protocol ID for the related message, set by caller agent
+ * @message_id: SCMI message ID for the related message, set by caller agent
+ * @in: Address of the incoming message payload copied in secure memory
+ * @in_size: Byte length of the incoming message payload, set by caller agent
+ * @out: Address of of the output message payload message in non-secure memory
+ * @out_size: Byte length of the provisionned output buffer
+ * @out_size_out: Byte length of the output message payload
+ */
+struct scmi_msg {
+	unsigned int agent_id;
+	unsigned int protocol_id;
+	unsigned int message_id;
+	char *in;
+	size_t in_size;
+	char *out;
+	size_t out_size;
+	size_t out_size_out;
+};
+
+/*
+ * Type scmi_msg_handler_t is used by procotol drivers to safely find
+ * the handler function for the incoming message ID.
+ */
+typedef void (*scmi_msg_handler_t)(struct scmi_msg *msg);
+
+/*
+ * scmi_msg_get_base_handler - Return a handler for a base message
+ * @msg - message to process
+ * Return a function handler for the message or NULL
+ */
+scmi_msg_handler_t scmi_msg_get_base_handler(struct scmi_msg *msg);
+
+/*
+ * scmi_msg_get_clock_handler - Return a handler for a clock message
+ * @msg - message to process
+ * Return a function handler for the message or NULL
+ */
+scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg);
+
+/*
+ * scmi_msg_get_rstd_handler - Return a handler for a reset domain message
+ * @msg - message to process
+ * Return a function handler for the message or NULL
+ */
+scmi_msg_handler_t scmi_msg_get_rstd_handler(struct scmi_msg *msg);
+
+/*
+ * scmi_msg_get_pd_handler - Return a handler for a power domain message
+ * @msg - message to process
+ * Return a function handler for the message or NULL
+ */
+scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg);
+
+/*
+ * Process Read, process and write response for input SCMI message
+ *
+ * @msg: SCMI message context
+ */
+void scmi_process_message(struct scmi_msg *msg);
+
+/*
+ * Write SCMI response payload to output message shared memory
+ *
+ * @msg: SCMI message context
+ * @payload: Output message payload
+ * @size: Byte size of output message payload
+ */
+void scmi_write_response(struct scmi_msg *msg, void *payload, size_t size);
+
+/*
+ * Write status only SCMI response payload to output message shared memory
+ *
+ * @msg: SCMI message context
+ * @status: SCMI status value returned to caller
+ */
+void scmi_status_response(struct scmi_msg *msg, int32_t status);
+#endif /* SCMI_MSG_COMMON_H */
diff --git a/drivers/scmi-msg/entry.c b/drivers/scmi-msg/entry.c
new file mode 100644
index 0000000..399115c
--- /dev/null
+++ b/drivers/scmi-msg/entry.c
@@ -0,0 +1,91 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+
+#include <assert.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+
+#include "common.h"
+
+#pragma weak scmi_msg_get_clock_handler
+#pragma weak scmi_msg_get_rstd_handler
+#pragma weak scmi_msg_get_pd_handler
+#pragma weak scmi_msg_get_voltage_handler
+
+scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+scmi_msg_handler_t scmi_msg_get_rstd_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+scmi_msg_handler_t scmi_msg_get_voltage_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+void scmi_status_response(struct scmi_msg *msg, int32_t status)
+{
+	assert(msg->out && msg->out_size >= sizeof(int32_t));
+
+	memcpy(msg->out, &status, sizeof(int32_t));
+	msg->out_size_out = sizeof(int32_t);
+}
+
+void scmi_write_response(struct scmi_msg *msg, void *payload, size_t size)
+{
+	/*
+	 * Output payload shall be at least the size of the status
+	 * Output buffer shall be at least be the size of the status
+	 * Output paylaod shall fit in output buffer
+	 */
+	assert(payload && size >= sizeof(int32_t) && size <= msg->out_size &&
+	       msg->out && msg->out_size >= sizeof(int32_t));
+
+	memcpy(msg->out, payload, size);
+	msg->out_size_out = size;
+}
+
+void scmi_process_message(struct scmi_msg *msg)
+{
+	scmi_msg_handler_t handler = NULL;
+
+	switch (msg->protocol_id) {
+	case SCMI_PROTOCOL_ID_BASE:
+		handler = scmi_msg_get_base_handler(msg);
+		break;
+	case SCMI_PROTOCOL_ID_CLOCK:
+		handler = scmi_msg_get_clock_handler(msg);
+		break;
+	case SCMI_PROTOCOL_ID_RESET_DOMAIN:
+		handler = scmi_msg_get_rstd_handler(msg);
+		break;
+	case SCMI_PROTOCOL_ID_POWER_DOMAIN:
+		handler = scmi_msg_get_pd_handler(msg);
+		break;
+	default:
+		break;
+	}
+
+	if (handler) {
+		handler(msg);
+		return;
+	}
+
+	ERROR("Agent %u Protocol 0x%x Message 0x%x: not supported\n",
+	      msg->agent_id, msg->protocol_id, msg->message_id);
+
+	scmi_status_response(msg, SCMI_NOT_SUPPORTED);
+}
diff --git a/drivers/scmi-msg/power_domain.c b/drivers/scmi-msg/power_domain.c
new file mode 100644
index 0000000..87c41dd
--- /dev/null
+++ b/drivers/scmi-msg/power_domain.c
@@ -0,0 +1,239 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+#include <cdefs.h>
+#include <string.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+#include <lib/utils_def.h>
+
+#include "common.h"
+
+#pragma weak plat_scmi_pd_count
+#pragma weak plat_scmi_pd_get_name
+#pragma weak plat_scmi_pd_get_state
+#pragma weak plat_scmi_pd_set_state
+#pragma weak plat_scmi_pd_statistics
+#pragma weak plat_scmi_pd_get_attributes
+
+static bool message_id_is_supported(unsigned int message_id);
+
+size_t plat_scmi_pd_count(unsigned int agent_id __unused)
+{
+	return 0U;
+}
+
+const char *plat_scmi_pd_get_name(unsigned int agent_id __unused,
+				  unsigned int pd_id __unused)
+{
+	return NULL;
+}
+
+unsigned int plat_scmi_pd_statistics(unsigned int agent_id __unused,
+				     unsigned long *pd_id __unused)
+{
+	return 0U;
+}
+
+unsigned int plat_scmi_pd_get_attributes(unsigned int agent_id __unused,
+					 unsigned int pd_id __unused)
+{
+	return 0U;
+}
+
+unsigned int plat_scmi_pd_get_state(unsigned int agent_id __unused,
+				    unsigned int pd_id __unused)
+{
+	return 0U;
+}
+
+int32_t plat_scmi_pd_set_state(unsigned int agent_id __unused,
+			       unsigned int flags __unused,
+			       unsigned int pd_id __unused,
+			       unsigned int state __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+static void report_version(struct scmi_msg *msg)
+{
+	struct scmi_protocol_version_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.version = SCMI_PROTOCOL_VERSION_PD,
+	};
+
+	if (msg->in_size != 0) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_attributes(struct scmi_msg *msg)
+{
+	unsigned long addr = 0UL;
+	unsigned int len;
+
+	struct scmi_protocol_attributes_p2a_pd return_values = {
+		.status = SCMI_SUCCESS,
+	};
+
+	if (msg->in_size != 0) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	return_values.attributes = plat_scmi_pd_count(msg->agent_id);
+	len = plat_scmi_pd_statistics(msg->agent_id, &addr);
+	if (len != 0U) {
+		return_values.statistics_addr_low = (unsigned int)addr;
+		return_values.statistics_addr_high = (uint32_t)(addr >> 32);
+		return_values.statistics_len = len;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_message_attributes(struct scmi_msg *msg)
+{
+	struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_protocol_message_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		/* For this protocol, attributes shall be zero */
+		.attributes = 0U,
+	};
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	if (!message_id_is_supported(in_args->message_id)) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_pd_attributes(struct scmi_msg *msg)
+{
+	const struct scmi_pd_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_pd_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+	const char *name = NULL;
+	unsigned int pd_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	pd_id = SPECULATION_SAFE_VALUE(in_args->pd_id);
+
+	if (pd_id >= plat_scmi_pd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	name = plat_scmi_pd_get_name(msg->agent_id, pd_id);
+	if (name == NULL) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	COPY_NAME_IDENTIFIER(return_values.pd_name, name);
+
+	return_values.attributes = plat_scmi_pd_get_attributes(msg->agent_id, pd_id);
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_pd_state_get(struct scmi_msg *msg)
+{
+	const struct scmi_pd_state_get_a2p *in_args = (void *)msg->in;
+	unsigned int state = 0U;
+	struct scmi_pd_state_get_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+	unsigned int pd_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	pd_id = SPECULATION_SAFE_VALUE(in_args->pd_id);
+
+	if (pd_id >= plat_scmi_pd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	state = plat_scmi_pd_get_state(msg->agent_id, pd_id);
+
+	return_values.power_state = state;
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_pd_state_set(struct scmi_msg *msg)
+{
+	const struct scmi_pd_state_set_a2p *in_args = (void *)msg->in;
+	unsigned int flags = 0U;
+	int32_t status = 0;
+	unsigned int pd_id = 0U;
+	unsigned int state = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	pd_id = SPECULATION_SAFE_VALUE(in_args->pd_id);
+
+	if (pd_id >= plat_scmi_pd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	flags = SPECULATION_SAFE_VALUE(in_args->flags);
+	state = SPECULATION_SAFE_VALUE(in_args->power_state);
+
+	status = plat_scmi_pd_set_state(msg->agent_id, flags, pd_id, state);
+
+	scmi_status_response(msg, status);
+}
+
+static const scmi_msg_handler_t scmi_pd_handler_table[] = {
+	[SCMI_PROTOCOL_VERSION] = report_version,
+	[SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
+	[SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
+	[SCMI_PD_ATTRIBUTES] = scmi_pd_attributes,
+	[SCMI_PD_STATE_SET] = scmi_pd_state_set,
+	[SCMI_PD_STATE_GET] = scmi_pd_state_get,
+};
+
+static bool message_id_is_supported(unsigned int message_id)
+{
+	return (message_id < ARRAY_SIZE(scmi_pd_handler_table)) &&
+	       (scmi_pd_handler_table[message_id] != NULL);
+}
+
+scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg)
+{
+	const size_t array_size = ARRAY_SIZE(scmi_pd_handler_table);
+	unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id);
+
+	if (message_id >= array_size) {
+		VERBOSE("pd handle not found %u", msg->message_id);
+		return NULL;
+	}
+
+	return scmi_pd_handler_table[message_id];
+}
diff --git a/drivers/scmi-msg/power_domain.h b/drivers/scmi-msg/power_domain.h
new file mode 100644
index 0000000..48551fd
--- /dev/null
+++ b/drivers/scmi-msg/power_domain.h
@@ -0,0 +1,72 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright 2021 NXP
+ */
+
+#ifndef SCMI_MSG_PD_H
+#define SCMI_MSG_PD_H
+
+#include <stdint.h>
+
+#include <lib/utils_def.h>
+
+#define SCMI_PROTOCOL_VERSION_PD	0x21000U
+
+/*
+ * Identifiers of the SCMI POWER DOMAIN Protocol commands
+ */
+enum scmi_pd_command_id {
+	SCMI_PD_ATTRIBUTES = 0x003,
+	SCMI_PD_STATE_SET = 0x004,
+	SCMI_PD_STATE_GET = 0x005,
+};
+
+/* Protocol attributes */
+struct scmi_pd_attributes_a2p {
+	uint32_t pd_id;
+};
+
+struct scmi_protocol_attributes_p2a_pd {
+	int32_t status;
+	uint32_t attributes;
+	uint32_t statistics_addr_low;
+	uint32_t statistics_addr_high;
+	uint32_t statistics_len;
+};
+
+#define SCMI_PD_NAME_LENGTH_MAX	16U
+
+struct scmi_pd_attributes_p2a {
+	int32_t status;
+	uint32_t attributes;
+	char pd_name[SCMI_PD_NAME_LENGTH_MAX];
+};
+
+/*
+ * Power Domain State Get
+ */
+
+struct scmi_pd_state_get_a2p {
+	uint32_t pd_id;
+};
+
+struct scmi_pd_state_get_p2a {
+	int32_t status;
+	uint32_t power_state;
+};
+
+/*
+ * Power domain State Set
+ */
+
+struct scmi_pd_state_set_a2p {
+	uint32_t flags;
+	uint32_t pd_id;
+	uint32_t power_state;
+};
+
+struct scmi_pd_state_set_p2a {
+	int32_t status;
+};
+
+#endif /* SCMI_MSG_PD_H */
diff --git a/drivers/scmi-msg/reset_domain.c b/drivers/scmi-msg/reset_domain.c
new file mode 100644
index 0000000..76ac47e
--- /dev/null
+++ b/drivers/scmi-msg/reset_domain.c
@@ -0,0 +1,197 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+#include <cdefs.h>
+#include <string.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+#include <lib/utils.h>
+#include <lib/utils_def.h>
+
+#include "common.h"
+
+static bool message_id_is_supported(unsigned int message_id);
+
+#pragma weak plat_scmi_rstd_count
+#pragma weak plat_scmi_rstd_get_name
+#pragma weak plat_scmi_rstd_autonomous
+#pragma weak plat_scmi_rstd_set_state
+
+size_t plat_scmi_rstd_count(unsigned int agent_id __unused)
+{
+	return 0U;
+}
+
+const char *plat_scmi_rstd_get_name(unsigned int agent_id __unused,
+				  unsigned int scmi_id __unused)
+{
+	return NULL;
+}
+
+int32_t plat_scmi_rstd_autonomous(unsigned int agent_id __unused,
+				unsigned int scmi_id __unused,
+				unsigned int state __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+int32_t plat_scmi_rstd_set_state(unsigned int agent_id __unused,
+			       unsigned int scmi_id __unused,
+			       bool assert_not_deassert __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+static void report_version(struct scmi_msg *msg)
+{
+	struct scmi_protocol_version_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.version = SCMI_PROTOCOL_VERSION_RESET_DOMAIN,
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_attributes(struct scmi_msg *msg)
+{
+	struct scmi_protocol_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.attributes = plat_scmi_rstd_count(msg->agent_id),
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_message_attributes(struct scmi_msg *msg)
+{
+	struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_protocol_message_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		/* For this protocol, attributes shall be zero */
+		.attributes = 0U,
+	};
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	if (!message_id_is_supported(in_args->message_id)) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void reset_domain_attributes(struct scmi_msg *msg)
+{
+	struct scmi_reset_domain_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_reset_domain_attributes_p2a return_values;
+	const char *name = NULL;
+	unsigned int domain_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	domain_id = SPECULATION_SAFE_VALUE(in_args->domain_id);
+
+	if (domain_id >= plat_scmi_rstd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	name = plat_scmi_rstd_get_name(msg->agent_id, domain_id);
+	if (name == NULL) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	zeromem(&return_values, sizeof(return_values));
+	COPY_NAME_IDENTIFIER(return_values.name, name);
+	return_values.status = SCMI_SUCCESS;
+	return_values.flags = 0U; /* Async and Notif are not supported */
+	return_values.latency = SCMI_RESET_DOMAIN_ATTR_UNK_LAT;
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void reset_request(struct scmi_msg *msg)
+{
+	struct scmi_reset_domain_request_a2p *in_args = (void *)msg->in;
+	struct scmi_reset_domain_request_p2a out_args = {
+		.status = SCMI_SUCCESS,
+	};
+	unsigned int domain_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	domain_id = SPECULATION_SAFE_VALUE(in_args->domain_id);
+
+	if (domain_id >= plat_scmi_rstd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	if ((in_args->flags & SCMI_RESET_DOMAIN_AUTO) != 0U) {
+		out_args.status = plat_scmi_rstd_autonomous(msg->agent_id,
+							  domain_id,
+							  in_args->reset_state);
+	} else if ((in_args->flags & SCMI_RESET_DOMAIN_EXPLICIT) != 0U) {
+		out_args.status = plat_scmi_rstd_set_state(msg->agent_id,
+							 domain_id, true);
+	} else {
+		out_args.status = plat_scmi_rstd_set_state(msg->agent_id,
+							 domain_id, false);
+	}
+
+	if (out_args.status != SCMI_SUCCESS) {
+		scmi_status_response(msg, out_args.status);
+	} else {
+		scmi_write_response(msg, &out_args, sizeof(out_args));
+	}
+}
+
+static const scmi_msg_handler_t scmi_rstd_handler_table[] = {
+	[SCMI_PROTOCOL_VERSION] = report_version,
+	[SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
+	[SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
+	[SCMI_RESET_DOMAIN_ATTRIBUTES] = reset_domain_attributes,
+	[SCMI_RESET_DOMAIN_REQUEST] = reset_request,
+};
+
+static bool message_id_is_supported(unsigned int message_id)
+{
+	return (message_id < ARRAY_SIZE(scmi_rstd_handler_table)) &&
+	       (scmi_rstd_handler_table[message_id] != NULL);
+}
+
+scmi_msg_handler_t scmi_msg_get_rstd_handler(struct scmi_msg *msg)
+{
+	unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id);
+
+	if (message_id >= ARRAY_SIZE(scmi_rstd_handler_table)) {
+		VERBOSE("Reset domain handle not found %u\n", msg->message_id);
+		return NULL;
+	}
+
+	return scmi_rstd_handler_table[message_id];
+}
diff --git a/drivers/scmi-msg/reset_domain.h b/drivers/scmi-msg/reset_domain.h
new file mode 100644
index 0000000..47bee5e
--- /dev/null
+++ b/drivers/scmi-msg/reset_domain.h
@@ -0,0 +1,122 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019, Linaro Limited
+ */
+#ifndef SCMI_MSG_RESET_DOMAIN_H
+#define SCMI_MSG_RESET_DOMAIN_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <lib/utils_def.h>
+
+#define SCMI_PROTOCOL_VERSION_RESET_DOMAIN	0x10000U
+
+#define SCMI_RESET_STATE_ARCH			BIT(31)
+#define SCMI_RESET_STATE_IMPL			0U
+
+/*
+ * Identifiers of the SCMI Reset Domain Management Protocol commands
+ */
+enum scmi_reset_domain_command_id {
+	SCMI_RESET_DOMAIN_ATTRIBUTES = 0x03,
+	SCMI_RESET_DOMAIN_REQUEST = 0x04,
+	SCMI_RESET_DOMAIN_NOTIFY = 0x05,
+};
+
+/*
+ * Identifiers of the SCMI Reset Domain Management Protocol responses
+ */
+enum scmi_reset_domain_response_id {
+	SCMI_RESET_ISSUED = 0x00,
+	SCMI_RESET_COMPLETE = 0x04,
+};
+
+/*
+ * PROTOCOL_ATTRIBUTES
+ */
+
+#define SCMI_RESET_DOMAIN_COUNT_MASK		GENMASK_32(15, 0)
+
+struct scmi_reset_domain_protocol_attributes_p2a {
+	int32_t status;
+	uint32_t attributes;
+};
+
+/* Value for scmi_reset_domain_attributes_p2a:flags */
+#define SCMI_RESET_DOMAIN_ATTR_ASYNC		BIT(31)
+#define SCMI_RESET_DOMAIN_ATTR_NOTIF		BIT(30)
+
+/* Value for scmi_reset_domain_attributes_p2a:latency */
+#define SCMI_RESET_DOMAIN_ATTR_UNK_LAT		0x7fffffffU
+#define SCMI_RESET_DOMAIN_ATTR_MAX_LAT		0x7ffffffeU
+
+/* Macro for scmi_reset_domain_attributes_p2a:name */
+#define SCMI_RESET_DOMAIN_ATTR_NAME_SZ		16U
+
+struct scmi_reset_domain_attributes_a2p {
+	uint32_t domain_id;
+};
+
+struct scmi_reset_domain_attributes_p2a {
+	int32_t status;
+	uint32_t flags;
+	uint32_t latency;
+	char name[SCMI_RESET_DOMAIN_ATTR_NAME_SZ];
+};
+
+/*
+ * RESET
+ */
+
+/* Values for scmi_reset_domain_request_a2p:flags */
+#define SCMI_RESET_DOMAIN_ASYNC			BIT(2)
+#define SCMI_RESET_DOMAIN_EXPLICIT		BIT(1)
+#define SCMI_RESET_DOMAIN_AUTO			BIT(0)
+
+struct scmi_reset_domain_request_a2p {
+	uint32_t domain_id;
+	uint32_t flags;
+	uint32_t reset_state;
+};
+
+struct scmi_reset_domain_request_p2a {
+	int32_t status;
+};
+
+/*
+ * RESET_NOTIFY
+ */
+
+/* Values for scmi_reset_notify_p2a:flags */
+#define SCMI_RESET_DOMAIN_DO_NOTIFY		BIT(0)
+
+struct scmi_reset_domain_notify_a2p {
+	uint32_t domain_id;
+	uint32_t notify_enable;
+};
+
+struct scmi_reset_domain_notify_p2a {
+	int32_t status;
+};
+
+/*
+ * RESET_COMPLETE
+ */
+
+struct scmi_reset_domain_complete_p2a {
+	int32_t status;
+	uint32_t domain_id;
+};
+
+/*
+ * RESET_ISSUED
+ */
+
+struct scmi_reset_domain_issued_p2a {
+	uint32_t domain_id;
+	uint32_t reset_state;
+};
+
+#endif /* SCMI_MSG_RESET_DOMAIN_H */
diff --git a/drivers/scmi-msg/smt.c b/drivers/scmi-msg/smt.c
new file mode 100644
index 0000000..9b079c7
--- /dev/null
+++ b/drivers/scmi-msg/smt.c
@@ -0,0 +1,206 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+#include <assert.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+#include <lib/cassert.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+
+#include "common.h"
+
+/* Legacy SMT/SCMI messages are 128 bytes at most including SMT header */
+#define SCMI_PLAYLOAD_MAX		92U
+#define SCMI_PLAYLOAD_U32_MAX		(SCMI_PLAYLOAD_MAX / sizeof(uint32_t))
+
+/**
+ * struct smt_header - SMT formatted header for SMT base shared memory transfer
+ *
+ * @status: Bit flags, see SMT_STATUS_*
+ * @flags: Bit flags, see SMT_FLAG_*
+ * @length: Byte size of message payload (variable) + ::message_header (32bit)
+ * payload: SCMI message payload data
+ */
+struct smt_header {
+	uint32_t reserved0;
+	uint32_t status;
+	uint64_t reserved1;
+	uint32_t flags;
+	uint32_t length; /* message_header + payload */
+	uint32_t message_header;
+	uint32_t payload[];
+};
+
+CASSERT(SCMI_PLAYLOAD_MAX + sizeof(struct smt_header) <= SMT_BUF_SLOT_SIZE,
+	assert_scmi_message_max_length_fits_in_smt_buffer_slot);
+
+/* Flag set in smt_header::status when SMT does not contain pending message */
+#define SMT_STATUS_FREE			BIT_32(0)
+/* Flag set in smt_header::status when SMT reports an error */
+#define SMT_STATUS_ERROR		BIT_32(1)
+
+/* Flag set in smt_header::flags when SMT uses interrupts */
+#define SMT_FLAG_INTR_ENABLED		BIT_32(1)
+
+/* Bit fields packed in smt_header::message_header */
+#define SMT_MSG_ID_MASK			GENMASK_32(7, 0)
+#define SMT_HDR_MSG_ID(_hdr)		((_hdr) & SMT_MSG_ID_MASK)
+
+#define SMT_MSG_TYPE_MASK		GENMASK_32(9, 8)
+#define SMT_HDR_TYPE_ID(_hdr)		(((_hdr) & SMT_MSG_TYPE_MASK) >> 8)
+
+#define SMT_MSG_PROT_ID_MASK		GENMASK_32(17, 10)
+#define SMT_HDR_PROT_ID(_hdr)		(((_hdr) & SMT_MSG_PROT_ID_MASK) >> 10)
+
+/*
+ * Provision input message payload buffers for fastcall SMC context entries
+ * and for interrupt context execution entries.
+ */
+static uint32_t fast_smc_payload[PLATFORM_CORE_COUNT][SCMI_PLAYLOAD_U32_MAX];
+static uint32_t interrupt_payload[PLATFORM_CORE_COUNT][SCMI_PLAYLOAD_U32_MAX];
+
+/* SMP protection on channel access */
+static struct spinlock smt_channels_lock;
+
+/* If channel is not busy, set busy and return true, otherwise return false */
+static bool channel_set_busy(struct scmi_msg_channel *chan)
+{
+	bool channel_is_busy;
+
+	spin_lock(&smt_channels_lock);
+
+	channel_is_busy = chan->busy;
+
+	if (!channel_is_busy) {
+		chan->busy = true;
+	}
+
+	spin_unlock(&smt_channels_lock);
+
+	return !channel_is_busy;
+}
+
+static void channel_release_busy(struct scmi_msg_channel *chan)
+{
+	chan->busy = false;
+}
+
+static struct smt_header *channel_to_smt_hdr(struct scmi_msg_channel *chan)
+{
+	return (struct smt_header *)chan->shm_addr;
+}
+
+/*
+ * Creates a SCMI message instance in secure memory and pushes it in the SCMI
+ * message drivers. Message structure contains SCMI protocol meta-data and
+ * references to input payload in secure memory and output message buffer
+ * in shared memory.
+ */
+static void scmi_proccess_smt(unsigned int agent_id, uint32_t *payload_buf)
+{
+	struct scmi_msg_channel *chan;
+	struct smt_header *smt_hdr;
+	size_t in_payload_size;
+	uint32_t smt_status;
+	struct scmi_msg msg;
+	bool error = true;
+
+	chan = plat_scmi_get_channel(agent_id);
+	if (chan == NULL) {
+		return;
+	}
+
+	smt_hdr = channel_to_smt_hdr(chan);
+	assert(smt_hdr);
+
+	smt_status = __atomic_load_n(&smt_hdr->status, __ATOMIC_RELAXED);
+
+	if (!channel_set_busy(chan)) {
+		VERBOSE("SCMI channel %u busy", agent_id);
+		goto out;
+	}
+
+	in_payload_size = __atomic_load_n(&smt_hdr->length, __ATOMIC_RELAXED) -
+			  sizeof(smt_hdr->message_header);
+
+	if (in_payload_size > SCMI_PLAYLOAD_MAX) {
+		VERBOSE("SCMI payload too big %zu", in_payload_size);
+		goto out;
+	}
+
+	if ((smt_status & (SMT_STATUS_ERROR | SMT_STATUS_FREE)) != 0U) {
+		VERBOSE("SCMI channel bad status 0x%x",
+			smt_hdr->status & (SMT_STATUS_ERROR | SMT_STATUS_FREE));
+		goto out;
+	}
+
+	/* Fill message */
+	zeromem(&msg, sizeof(msg));
+	msg.in = (char *)payload_buf;
+	msg.in_size = in_payload_size;
+	msg.out = (char *)smt_hdr->payload;
+	msg.out_size = chan->shm_size - sizeof(*smt_hdr);
+
+	assert((msg.out != NULL) && (msg.out_size >= sizeof(int32_t)));
+
+	/* Here the payload is copied in secure memory */
+	memcpy(msg.in, smt_hdr->payload, in_payload_size);
+
+	msg.protocol_id = SMT_HDR_PROT_ID(smt_hdr->message_header);
+	msg.message_id = SMT_HDR_MSG_ID(smt_hdr->message_header);
+	msg.agent_id = agent_id;
+
+	scmi_process_message(&msg);
+
+	/* Update message length with the length of the response message */
+	smt_hdr->length = msg.out_size_out + sizeof(smt_hdr->message_header);
+
+	channel_release_busy(chan);
+	error = false;
+
+out:
+	if (error) {
+		VERBOSE("SCMI error");
+		smt_hdr->status |= SMT_STATUS_ERROR | SMT_STATUS_FREE;
+	} else {
+		smt_hdr->status |= SMT_STATUS_FREE;
+	}
+}
+
+void scmi_smt_fastcall_smc_entry(unsigned int agent_id)
+{
+	scmi_proccess_smt(agent_id,
+			  fast_smc_payload[plat_my_core_pos()]);
+}
+
+void scmi_smt_interrupt_entry(unsigned int agent_id)
+{
+	scmi_proccess_smt(agent_id,
+			  interrupt_payload[plat_my_core_pos()]);
+}
+
+/* Init a SMT header for a shared memory buffer: state it a free/no-error */
+void scmi_smt_init_agent_channel(struct scmi_msg_channel *chan)
+{
+	if (chan != NULL) {
+		struct smt_header *smt_header = channel_to_smt_hdr(chan);
+
+		if (smt_header != NULL) {
+			memset(smt_header, 0, sizeof(*smt_header));
+			smt_header->status = SMT_STATUS_FREE;
+
+			return;
+		}
+	}
+
+	panic();
+}
diff --git a/drivers/st/bsec/bsec2.c b/drivers/st/bsec/bsec2.c
new file mode 100644
index 0000000..68d3a5b
--- /dev/null
+++ b/drivers/st/bsec/bsec2.c
@@ -0,0 +1,961 @@
+/*
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <limits.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/st/bsec.h>
+#include <drivers/st/bsec2_reg.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#define BSEC_IP_VERSION_1_1	U(0x11)
+#define BSEC_IP_VERSION_2_0	U(0x20)
+#define BSEC_IP_ID_2		U(0x100032)
+
+#define OTP_ACCESS_SIZE (round_up(OTP_MAX_SIZE, __WORD_BIT) / __WORD_BIT)
+
+static uint32_t otp_nsec_access[OTP_ACCESS_SIZE] __unused;
+
+static uint32_t bsec_power_safmem(bool power);
+
+/* BSEC access protection */
+static spinlock_t bsec_spinlock;
+static uintptr_t bsec_base;
+
+static void bsec_lock(void)
+{
+	if (stm32mp_lock_available()) {
+		spin_lock(&bsec_spinlock);
+	}
+}
+
+static void bsec_unlock(void)
+{
+	if (stm32mp_lock_available()) {
+		spin_unlock(&bsec_spinlock);
+	}
+}
+
+static bool is_otp_invalid_mode(void)
+{
+	bool ret = ((bsec_get_status() & BSEC_MODE_INVALID) == BSEC_MODE_INVALID);
+
+	if (ret) {
+		ERROR("OTP mode is OTP-INVALID\n");
+	}
+
+	return ret;
+}
+
+#if defined(IMAGE_BL32)
+static int bsec_get_dt_node(struct dt_node_info *info)
+{
+	int node;
+
+	node = dt_get_node(info, -1, DT_BSEC_COMPAT);
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return node;
+}
+
+static void enable_non_secure_access(uint32_t otp)
+{
+	otp_nsec_access[otp / __WORD_BIT] |= BIT(otp % __WORD_BIT);
+
+	if (bsec_shadow_register(otp) != BSEC_OK) {
+		panic();
+	}
+}
+
+static bool non_secure_can_access(uint32_t otp)
+{
+	return (otp_nsec_access[otp / __WORD_BIT] &
+		BIT(otp % __WORD_BIT)) != 0U;
+}
+
+static void bsec_dt_otp_nsec_access(void *fdt, int bsec_node)
+{
+	int bsec_subnode;
+
+	fdt_for_each_subnode(bsec_subnode, fdt, bsec_node) {
+		const fdt32_t *cuint;
+		uint32_t otp;
+		uint32_t i;
+		uint32_t size;
+		uint32_t offset;
+		uint32_t length;
+
+		cuint = fdt_getprop(fdt, bsec_subnode, "reg", NULL);
+		if (cuint == NULL) {
+			panic();
+		}
+
+		offset = fdt32_to_cpu(*cuint);
+		cuint++;
+		length = fdt32_to_cpu(*cuint);
+
+		otp = offset / sizeof(uint32_t);
+
+		if (otp < STM32MP1_UPPER_OTP_START) {
+			unsigned int otp_end = round_up(offset + length,
+						       sizeof(uint32_t)) /
+					       sizeof(uint32_t);
+
+			if (otp_end > STM32MP1_UPPER_OTP_START) {
+				/*
+				 * OTP crosses Lower/Upper boundary, consider
+				 * only the upper part.
+				 */
+				otp = STM32MP1_UPPER_OTP_START;
+				length -= (STM32MP1_UPPER_OTP_START *
+					   sizeof(uint32_t)) - offset;
+				offset = STM32MP1_UPPER_OTP_START *
+					 sizeof(uint32_t);
+
+				WARN("OTP crosses Lower/Upper boundary\n");
+			} else {
+				continue;
+			}
+		}
+
+		if ((fdt_getprop(fdt, bsec_subnode,
+				 "st,non-secure-otp", NULL)) == NULL) {
+			continue;
+		}
+
+		if (((offset % sizeof(uint32_t)) != 0U) ||
+		    ((length % sizeof(uint32_t)) != 0U)) {
+			ERROR("Unaligned non-secure OTP\n");
+			panic();
+		}
+
+		size = length / sizeof(uint32_t);
+
+		for (i = otp; i < (otp + size); i++) {
+			enable_non_secure_access(i);
+		}
+	}
+}
+
+static void bsec_late_init(void)
+{
+	void *fdt;
+	int node;
+	struct dt_node_info bsec_info;
+
+	if (fdt_get_address(&fdt) == 0) {
+		panic();
+	}
+
+	node = bsec_get_dt_node(&bsec_info);
+	if (node < 0) {
+		panic();
+	}
+
+	assert(bsec_base == bsec_info.base);
+
+	bsec_dt_otp_nsec_access(fdt, node);
+}
+#endif
+
+static uint32_t otp_bank_offset(uint32_t otp)
+{
+	assert(otp <= STM32MP1_OTP_MAX_ID);
+
+	return ((otp & ~BSEC_OTP_MASK) >> BSEC_OTP_BANK_SHIFT) *
+	       sizeof(uint32_t);
+}
+
+/*
+ * bsec_check_error: check BSEC error status.
+ * otp: OTP number.
+ * check_disturbed: check only error (false),
+ *	or error and disturbed status (true).
+ * return value: BSEC_OK if no error.
+ */
+static uint32_t bsec_check_error(uint32_t otp, bool check_disturbed)
+{
+	uint32_t bit = BIT(otp & BSEC_OTP_MASK);
+	uint32_t bank = otp_bank_offset(otp);
+
+	if ((mmio_read_32(bsec_base + BSEC_ERROR_OFF + bank) & bit) != 0U) {
+		return BSEC_ERROR;
+	}
+
+	if (!check_disturbed) {
+		return BSEC_OK;
+	}
+
+	if ((mmio_read_32(bsec_base + BSEC_DISTURBED_OFF + bank) & bit) != 0U) {
+		return BSEC_DISTURBED;
+	}
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_probe: initialize BSEC driver.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_probe(void)
+{
+	bsec_base = BSEC_BASE;
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	if ((((bsec_get_version() & BSEC_IPVR_MSK) != BSEC_IP_VERSION_1_1) &&
+	     ((bsec_get_version() & BSEC_IPVR_MSK) != BSEC_IP_VERSION_2_0)) ||
+	    (bsec_get_id() != BSEC_IP_ID_2)) {
+		panic();
+	}
+
+#if defined(IMAGE_BL32)
+	bsec_late_init();
+#endif
+	return BSEC_OK;
+}
+
+/*
+ * bsec_get_base: return BSEC base address.
+ */
+uint32_t bsec_get_base(void)
+{
+	return bsec_base;
+}
+
+/*
+ * bsec_set_config: enable and configure BSEC.
+ * cfg: pointer to param structure used to set register.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_set_config(struct bsec_config *cfg)
+{
+	uint32_t value;
+	uint32_t result;
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	value = ((((uint32_t)cfg->freq << BSEC_CONF_FRQ_SHIFT) &
+						BSEC_CONF_FRQ_MASK) |
+		 (((uint32_t)cfg->pulse_width << BSEC_CONF_PRG_WIDTH_SHIFT) &
+						BSEC_CONF_PRG_WIDTH_MASK) |
+		 (((uint32_t)cfg->tread << BSEC_CONF_TREAD_SHIFT) &
+						BSEC_CONF_TREAD_MASK));
+
+	bsec_lock();
+
+	mmio_write_32(bsec_base + BSEC_OTP_CONF_OFF, value);
+
+	bsec_unlock();
+
+	result = bsec_power_safmem((bool)cfg->power &
+				   BSEC_CONF_POWER_UP_MASK);
+	if (result != BSEC_OK) {
+		return result;
+	}
+
+	value = ((((uint32_t)cfg->upper_otp_lock << UPPER_OTP_LOCK_SHIFT) &
+						UPPER_OTP_LOCK_MASK) |
+		 (((uint32_t)cfg->den_lock << DENREG_LOCK_SHIFT) &
+						DENREG_LOCK_MASK) |
+		 (((uint32_t)cfg->prog_lock << GPLOCK_LOCK_SHIFT) &
+						GPLOCK_LOCK_MASK));
+
+	bsec_lock();
+
+	mmio_write_32(bsec_base + BSEC_OTP_LOCK_OFF, value);
+
+	bsec_unlock();
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_get_config: return config parameters set in BSEC registers.
+ * cfg: config param return.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_get_config(struct bsec_config *cfg)
+{
+	uint32_t value;
+
+	if (cfg == NULL) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	value = mmio_read_32(bsec_base + BSEC_OTP_CONF_OFF);
+	cfg->power = (uint8_t)((value & BSEC_CONF_POWER_UP_MASK) >>
+						BSEC_CONF_POWER_UP_SHIFT);
+	cfg->freq = (uint8_t)((value & BSEC_CONF_FRQ_MASK) >>
+						BSEC_CONF_FRQ_SHIFT);
+	cfg->pulse_width = (uint8_t)((value & BSEC_CONF_PRG_WIDTH_MASK) >>
+						BSEC_CONF_PRG_WIDTH_SHIFT);
+	cfg->tread = (uint8_t)((value & BSEC_CONF_TREAD_MASK) >>
+						BSEC_CONF_TREAD_SHIFT);
+
+	value = mmio_read_32(bsec_base + BSEC_OTP_LOCK_OFF);
+	cfg->upper_otp_lock = (uint8_t)((value & UPPER_OTP_LOCK_MASK) >>
+						UPPER_OTP_LOCK_SHIFT);
+	cfg->den_lock = (uint8_t)((value & DENREG_LOCK_MASK) >>
+						DENREG_LOCK_SHIFT);
+	cfg->prog_lock = (uint8_t)((value & GPLOCK_LOCK_MASK) >>
+						GPLOCK_LOCK_SHIFT);
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_shadow_register: copy SAFMEM OTP to BSEC data.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_shadow_register(uint32_t otp)
+{
+	uint32_t result;
+	bool value;
+	bool power_up = false;
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	result = bsec_read_sr_lock(otp, &value);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u Sticky-read bit read Error %u\n", otp, result);
+		return result;
+	}
+
+	if (value) {
+		VERBOSE("BSEC: OTP %u is locked and will not be refreshed\n",
+			otp);
+	}
+
+	if ((bsec_get_status() & BSEC_MODE_PWR_MASK) == 0U) {
+		result = bsec_power_safmem(true);
+
+		if (result != BSEC_OK) {
+			return result;
+		}
+
+		power_up = true;
+	}
+
+	bsec_lock();
+
+	mmio_write_32(bsec_base + BSEC_OTP_CTRL_OFF, otp | BSEC_READ);
+
+	while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
+		;
+	}
+
+	result = bsec_check_error(otp, true);
+
+	bsec_unlock();
+
+	if (power_up) {
+		if (bsec_power_safmem(false) != BSEC_OK) {
+			panic();
+		}
+	}
+
+	return result;
+}
+
+/*
+ * bsec_read_otp: read an OTP data value.
+ * val: read value.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_read_otp(uint32_t *val, uint32_t otp)
+{
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	*val = mmio_read_32(bsec_base + BSEC_OTP_DATA_OFF +
+			    (otp * sizeof(uint32_t)));
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_write_otp: write value in BSEC data register.
+ * val: value to write.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_write_otp(uint32_t val, uint32_t otp)
+{
+	uint32_t result;
+	bool value;
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	result = bsec_read_sw_lock(otp, &value);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u Sticky-write bit read Error %u\n", otp, result);
+		return result;
+	}
+
+	if (value) {
+		VERBOSE("BSEC: OTP %u is locked and write will be ignored\n",
+			otp);
+	}
+
+	/* Ensure integrity of each register access sequence */
+	bsec_lock();
+
+	mmio_write_32(bsec_base + BSEC_OTP_DATA_OFF +
+		      (otp * sizeof(uint32_t)), val);
+
+	bsec_unlock();
+
+	return result;
+}
+
+/*
+ * bsec_program_otp: program a bit in SAFMEM after the prog.
+ *	The OTP data is not refreshed.
+ * val: value to program.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_program_otp(uint32_t val, uint32_t otp)
+{
+	uint32_t result;
+	bool power_up = false;
+	bool sp_lock;
+	bool perm_lock;
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	result = bsec_read_sp_lock(otp, &sp_lock);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u Sticky-prog bit read Error %u\n", otp, result);
+		return result;
+	}
+
+	result = bsec_read_permanent_lock(otp, &perm_lock);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u permanent bit read Error %u\n", otp, result);
+		return result;
+	}
+
+	if (sp_lock || perm_lock) {
+		WARN("BSEC: OTP locked, prog will be ignored\n");
+		return BSEC_PROG_FAIL;
+	}
+
+	if ((mmio_read_32(bsec_base + BSEC_OTP_LOCK_OFF) &
+	     BIT(BSEC_LOCK_PROGRAM)) != 0U) {
+		WARN("BSEC: GPLOCK activated, prog will be ignored\n");
+	}
+
+	if ((bsec_get_status() & BSEC_MODE_PWR_MASK) == 0U) {
+		result = bsec_power_safmem(true);
+
+		if (result != BSEC_OK) {
+			return result;
+		}
+
+		power_up = true;
+	}
+
+	bsec_lock();
+
+	mmio_write_32(bsec_base + BSEC_OTP_WRDATA_OFF, val);
+
+	mmio_write_32(bsec_base + BSEC_OTP_CTRL_OFF, otp | BSEC_WRITE);
+
+	while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
+		;
+	}
+
+	if ((bsec_get_status() & BSEC_MODE_PROGFAIL_MASK) != 0U) {
+		result = BSEC_PROG_FAIL;
+	} else {
+		result = bsec_check_error(otp, true);
+	}
+
+	bsec_unlock();
+
+	if (power_up) {
+		if (bsec_power_safmem(false) != BSEC_OK) {
+			panic();
+		}
+	}
+
+	return result;
+}
+
+/*
+ * bsec_permanent_lock_otp: permanent lock of OTP in SAFMEM.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_permanent_lock_otp(uint32_t otp)
+{
+	uint32_t result;
+	bool power_up = false;
+	uint32_t data;
+	uint32_t addr;
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	if ((bsec_get_status() & BSEC_MODE_PWR_MASK) == 0U) {
+		result = bsec_power_safmem(true);
+
+		if (result != BSEC_OK) {
+			return result;
+		}
+
+		power_up = true;
+	}
+
+	if (otp < STM32MP1_UPPER_OTP_START) {
+		addr = otp >> ADDR_LOWER_OTP_PERLOCK_SHIFT;
+		data = DATA_LOWER_OTP_PERLOCK_BIT <<
+		       ((otp & DATA_LOWER_OTP_PERLOCK_MASK) << 1U);
+	} else {
+		addr = (otp >> ADDR_UPPER_OTP_PERLOCK_SHIFT) + 2U;
+		data = DATA_UPPER_OTP_PERLOCK_BIT <<
+		       (otp & DATA_UPPER_OTP_PERLOCK_MASK);
+	}
+
+	bsec_lock();
+
+	mmio_write_32(bsec_base + BSEC_OTP_WRDATA_OFF, data);
+
+	mmio_write_32(bsec_base + BSEC_OTP_CTRL_OFF,
+		      addr | BSEC_WRITE | BSEC_LOCK);
+
+	while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
+		;
+	}
+
+	if ((bsec_get_status() & BSEC_MODE_PROGFAIL_MASK) != 0U) {
+		result = BSEC_PROG_FAIL;
+	} else {
+		result = bsec_check_error(otp, false);
+	}
+
+	bsec_unlock();
+
+	if (power_up) {
+		if (bsec_power_safmem(false) != BSEC_OK) {
+			panic();
+		}
+	}
+
+	return result;
+}
+
+/*
+ * bsec_write_debug_conf: write value in debug feature.
+ *	to enable/disable debug service.
+ * val: value to write.
+ * return value: none.
+ */
+void bsec_write_debug_conf(uint32_t val)
+{
+	if (is_otp_invalid_mode()) {
+		return;
+	}
+
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_DEN_OFF, val & BSEC_DEN_ALL_MSK);
+	bsec_unlock();
+}
+
+/*
+ * bsec_read_debug_conf: return debug configuration register value.
+ */
+uint32_t bsec_read_debug_conf(void)
+{
+	return mmio_read_32(bsec_base + BSEC_DEN_OFF);
+}
+
+/*
+ * bsec_write_scratch: write value in scratch register.
+ * val: value to write.
+ * return value: none.
+ */
+void bsec_write_scratch(uint32_t val)
+{
+#if defined(IMAGE_BL32)
+	if (is_otp_invalid_mode()) {
+		return;
+	}
+
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_SCRATCH_OFF, val);
+	bsec_unlock();
+#else
+	mmio_write_32(BSEC_BASE + BSEC_SCRATCH_OFF, val);
+#endif
+}
+
+/*
+ * bsec_read_scratch: return scratch register value.
+ */
+uint32_t bsec_read_scratch(void)
+{
+	return mmio_read_32(bsec_base + BSEC_SCRATCH_OFF);
+}
+
+/*
+ * bsec_get_status: return status register value.
+ */
+uint32_t bsec_get_status(void)
+{
+	return mmio_read_32(bsec_base + BSEC_OTP_STATUS_OFF);
+}
+
+/*
+ * bsec_get_hw_conf: return hardware configuration register value.
+ */
+uint32_t bsec_get_hw_conf(void)
+{
+	return mmio_read_32(bsec_base + BSEC_IPHW_CFG_OFF);
+}
+
+/*
+ * bsec_get_version: return BSEC version register value.
+ */
+uint32_t bsec_get_version(void)
+{
+	return mmio_read_32(bsec_base + BSEC_IPVR_OFF);
+}
+
+/*
+ * bsec_get_id: return BSEC ID register value.
+ */
+uint32_t bsec_get_id(void)
+{
+	return mmio_read_32(bsec_base + BSEC_IP_ID_OFF);
+}
+
+/*
+ * bsec_get_magic_id: return BSEC magic number register value.
+ */
+uint32_t bsec_get_magic_id(void)
+{
+	return mmio_read_32(bsec_base + BSEC_IP_MAGIC_ID_OFF);
+}
+
+/*
+ * bsec_set_sr_lock: set shadow-read lock.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_set_sr_lock(uint32_t otp)
+{
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_SRLOCK_OFF + bank, otp_mask);
+	bsec_unlock();
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_read_sr_lock: read shadow-read lock.
+ * otp: OTP number.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_read_sr_lock(uint32_t otp, bool *value)
+{
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+	uint32_t bank_value;
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bank_value = mmio_read_32(bsec_base + BSEC_SRLOCK_OFF + bank);
+
+	*value = ((bank_value & otp_mask) != 0U);
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_set_sw_lock: set shadow-write lock.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_set_sw_lock(uint32_t otp)
+{
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_SWLOCK_OFF + bank, otp_mask);
+	bsec_unlock();
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_read_sw_lock: read shadow-write lock.
+ * otp: OTP number.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_read_sw_lock(uint32_t otp, bool *value)
+{
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+	uint32_t bank_value;
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bank_value = mmio_read_32(bsec_base + BSEC_SWLOCK_OFF + bank);
+
+	*value = ((bank_value & otp_mask) != 0U);
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_set_sp_lock: set shadow-program lock.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_set_sp_lock(uint32_t otp)
+{
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_SPLOCK_OFF + bank, otp_mask);
+	bsec_unlock();
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_read_sp_lock: read shadow-program lock.
+ * otp: OTP number.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_read_sp_lock(uint32_t otp, bool *value)
+{
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+	uint32_t bank_value;
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bank_value = mmio_read_32(bsec_base + BSEC_SPLOCK_OFF + bank);
+
+	*value = ((bank_value & otp_mask) != 0U);
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_read_permanent_lock: Read permanent lock status.
+ * otp: OTP number.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_read_permanent_lock(uint32_t otp, bool *value)
+{
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+	uint32_t bank_value;
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bank_value = mmio_read_32(bsec_base + BSEC_WRLOCK_OFF + bank);
+
+	*value = ((bank_value & otp_mask) != 0U);
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_otp_lock: Lock Upper OTP or Global Programming or Debug Enable.
+ * service: Service to lock, see header file.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_otp_lock(uint32_t service)
+{
+	uintptr_t reg = bsec_base + BSEC_OTP_LOCK_OFF;
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	switch (service) {
+	case BSEC_LOCK_UPPER_OTP:
+		mmio_write_32(reg, BIT(BSEC_LOCK_UPPER_OTP));
+		break;
+	case BSEC_LOCK_DEBUG:
+		mmio_write_32(reg, BIT(BSEC_LOCK_DEBUG));
+		break;
+	case BSEC_LOCK_PROGRAM:
+		mmio_write_32(reg, BIT(BSEC_LOCK_PROGRAM));
+		break;
+	default:
+		return BSEC_INVALID_PARAM;
+	}
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_power_safmem: Activate or deactivate SAFMEM power.
+ * power: true to power up, false to power down.
+ * return value: BSEC_OK if no error.
+ */
+static uint32_t bsec_power_safmem(bool power)
+{
+	uint32_t register_val;
+	uint32_t timeout = BSEC_TIMEOUT_VALUE;
+
+	bsec_lock();
+
+	register_val = mmio_read_32(bsec_base + BSEC_OTP_CONF_OFF);
+
+	if (power) {
+		register_val |= BSEC_CONF_POWER_UP_MASK;
+	} else {
+		register_val &= ~BSEC_CONF_POWER_UP_MASK;
+	}
+
+	mmio_write_32(bsec_base + BSEC_OTP_CONF_OFF, register_val);
+
+	if (power) {
+		while (((bsec_get_status() & BSEC_MODE_PWR_MASK) == 0U) &&
+		       (timeout != 0U)) {
+			timeout--;
+		}
+	} else {
+		while (((bsec_get_status() & BSEC_MODE_PWR_MASK) != 0U) &&
+		       (timeout != 0U)) {
+			timeout--;
+		}
+	}
+
+	bsec_unlock();
+
+	if (timeout == 0U) {
+		return BSEC_TIMEOUT;
+	}
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_shadow_read_otp: Load OTP from SAFMEM and provide its value.
+ * otp_value: read value.
+ * word: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_shadow_read_otp(uint32_t *otp_value, uint32_t word)
+{
+	uint32_t result;
+
+	result = bsec_shadow_register(word);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u Shadowing Error %u\n", word, result);
+		return result;
+	}
+
+	result = bsec_read_otp(otp_value, word);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u Read Error %u\n", word, result);
+	}
+
+	return result;
+}
+
+/*
+ * bsec_check_nsec_access_rights: check non-secure access rights to target OTP.
+ * otp: OTP number.
+ * return value: BSEC_OK if authorized access.
+ */
+uint32_t bsec_check_nsec_access_rights(uint32_t otp)
+{
+#if defined(IMAGE_BL32)
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	if (otp >= STM32MP1_UPPER_OTP_START) {
+		if (!non_secure_can_access(otp)) {
+			return BSEC_ERROR;
+		}
+	}
+#endif
+
+	return BSEC_OK;
+}
+
diff --git a/drivers/st/clk/clk-stm32-core.c b/drivers/st/clk/clk-stm32-core.c
new file mode 100644
index 0000000..bb03125
--- /dev/null
+++ b/drivers/st/clk/clk-stm32-core.c
@@ -0,0 +1,1096 @@
+/*
+ * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include "clk-stm32-core.h"
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+
+static struct spinlock reg_lock;
+static struct spinlock refcount_lock;
+
+static struct stm32_clk_priv *stm32_clock_data;
+
+const struct stm32_clk_ops clk_mux_ops;
+
+struct stm32_clk_priv *clk_stm32_get_priv(void)
+{
+	return stm32_clock_data;
+}
+
+static void stm32mp1_clk_lock(struct spinlock *lock)
+{
+	if (stm32mp_lock_available()) {
+		/* Assume interrupts are masked */
+		spin_lock(lock);
+	}
+}
+
+static void stm32mp1_clk_unlock(struct spinlock *lock)
+{
+	if (stm32mp_lock_available()) {
+		spin_unlock(lock);
+	}
+}
+
+void stm32mp1_clk_rcc_regs_lock(void)
+{
+	stm32mp1_clk_lock(&reg_lock);
+}
+
+void stm32mp1_clk_rcc_regs_unlock(void)
+{
+	stm32mp1_clk_unlock(&reg_lock);
+}
+
+#define TIMEOUT_US_1S	U(1000000)
+#define OSCRDY_TIMEOUT	TIMEOUT_US_1S
+
+struct clk_oscillator_data *clk_oscillator_get_data(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct stm32_osc_cfg *osc_cfg = clk->clock_cfg;
+	int osc_id = osc_cfg->osc_id;
+
+	return &priv->osci_data[osc_id];
+}
+
+void clk_oscillator_set_bypass(struct stm32_clk_priv *priv, int id, bool digbyp, bool bypass)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	struct stm32_clk_bypass *bypass_data = osc_data->bypass;
+	uintptr_t address;
+
+	if (bypass_data == NULL) {
+		return;
+	}
+
+	address = priv->base + bypass_data->offset;
+
+	if (digbyp) {
+		mmio_setbits_32(address, BIT(bypass_data->bit_digbyp));
+	}
+
+	if (bypass || digbyp) {
+		mmio_setbits_32(address, BIT(bypass_data->bit_byp));
+	}
+}
+
+void clk_oscillator_set_css(struct stm32_clk_priv *priv, int id, bool css)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	struct stm32_clk_css *css_data = osc_data->css;
+	uintptr_t address;
+
+	if (css_data == NULL) {
+		return;
+	}
+
+	address = priv->base + css_data->offset;
+
+	if (css) {
+		mmio_setbits_32(address, BIT(css_data->bit_css));
+	}
+}
+
+void clk_oscillator_set_drive(struct stm32_clk_priv *priv, int id, uint8_t lsedrv)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	struct stm32_clk_drive *drive_data = osc_data->drive;
+	uintptr_t address;
+	uint32_t mask;
+	uint32_t value;
+
+	if (drive_data == NULL) {
+		return;
+	}
+
+	address = priv->base + drive_data->offset;
+
+	mask = (BIT(drive_data->drv_width) - 1U) <<  drive_data->drv_shift;
+
+	/*
+	 * Warning: not recommended to switch directly from "high drive"
+	 * to "medium low drive", and vice-versa.
+	 */
+	value = (mmio_read_32(address) & mask) >> drive_data->drv_shift;
+
+	while (value != lsedrv) {
+		if (value > lsedrv) {
+			value--;
+		} else {
+			value++;
+		}
+
+		mmio_clrsetbits_32(address, mask, value << drive_data->drv_shift);
+	}
+}
+
+int clk_oscillator_wait_ready(struct stm32_clk_priv *priv, int id, bool ready_on)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	return _clk_stm32_gate_wait_ready(priv, osc_data->gate_rdy_id, ready_on);
+}
+
+int clk_oscillator_wait_ready_on(struct stm32_clk_priv *priv, int id)
+{
+	return clk_oscillator_wait_ready(priv, id, true);
+}
+
+int clk_oscillator_wait_ready_off(struct stm32_clk_priv *priv, int id)
+{
+	return clk_oscillator_wait_ready(priv, id, false);
+}
+
+static int clk_gate_enable(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_gate_cfg *cfg = clk->clock_cfg;
+
+	mmio_setbits_32(priv->base + cfg->offset, BIT(cfg->bit_idx));
+
+	return 0;
+}
+
+static void clk_gate_disable(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_gate_cfg *cfg = clk->clock_cfg;
+
+	mmio_clrbits_32(priv->base + cfg->offset, BIT(cfg->bit_idx));
+}
+
+static bool clk_gate_is_enabled(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_gate_cfg *cfg = clk->clock_cfg;
+
+	return ((mmio_read_32(priv->base + cfg->offset) & BIT(cfg->bit_idx)) != 0U);
+}
+
+const struct stm32_clk_ops clk_gate_ops = {
+	.enable		= clk_gate_enable,
+	.disable	= clk_gate_disable,
+	.is_enabled	= clk_gate_is_enabled,
+};
+
+void _clk_stm32_gate_disable(struct stm32_clk_priv *priv, uint16_t gate_id)
+{
+	const struct gate_cfg *gate = &priv->gates[gate_id];
+	uintptr_t addr = priv->base + gate->offset;
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(addr + RCC_MP_ENCLRR_OFFSET, BIT(gate->bit_idx));
+	} else {
+		mmio_clrbits_32(addr, BIT(gate->bit_idx));
+	}
+}
+
+int _clk_stm32_gate_enable(struct stm32_clk_priv *priv, uint16_t gate_id)
+{
+	const struct gate_cfg *gate = &priv->gates[gate_id];
+	uintptr_t addr = priv->base + gate->offset;
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(addr, BIT(gate->bit_idx));
+
+	} else {
+		mmio_setbits_32(addr, BIT(gate->bit_idx));
+	}
+
+	return 0;
+}
+
+const struct clk_stm32 *_clk_get(struct stm32_clk_priv *priv, int id)
+{
+	if ((unsigned int)id < priv->num) {
+		return &priv->clks[id];
+	}
+
+	return NULL;
+}
+
+#define clk_div_mask(_width) GENMASK(((_width) - 1U), 0U)
+
+static unsigned int _get_table_div(const struct clk_div_table *table,
+				   unsigned int val)
+{
+	const struct clk_div_table *clkt;
+
+	for (clkt = table; clkt->div; clkt++) {
+		if (clkt->val == val) {
+			return clkt->div;
+		}
+	}
+
+	return 0;
+}
+
+static unsigned int _get_div(const struct clk_div_table *table,
+			     unsigned int val, unsigned long flags,
+			     uint8_t width)
+{
+	if ((flags & CLK_DIVIDER_ONE_BASED) != 0UL) {
+		return val;
+	}
+
+	if ((flags & CLK_DIVIDER_POWER_OF_TWO) != 0UL) {
+		return BIT(val);
+	}
+
+	if ((flags & CLK_DIVIDER_MAX_AT_ZERO) != 0UL) {
+		return (val != 0U) ? val : BIT(width);
+	}
+
+	if (table != NULL) {
+		return _get_table_div(table, val);
+	}
+
+	return val + 1U;
+}
+
+#define TIMEOUT_US_200MS	U(200000)
+#define CLKSRC_TIMEOUT		TIMEOUT_US_200MS
+
+int clk_mux_set_parent(struct stm32_clk_priv *priv, uint16_t pid, uint8_t sel)
+{
+	const struct parent_cfg *parents = &priv->parents[pid & MUX_PARENT_MASK];
+	const struct mux_cfg *mux = parents->mux;
+	uintptr_t address = priv->base + mux->offset;
+	uint32_t mask;
+	uint64_t timeout;
+
+	mask = MASK_WIDTH_SHIFT(mux->width, mux->shift);
+
+	mmio_clrsetbits_32(address, mask, (sel << mux->shift) & mask);
+
+	if (mux->bitrdy == MUX_NO_BIT_RDY) {
+		return 0;
+	}
+
+	timeout = timeout_init_us(CLKSRC_TIMEOUT);
+
+	mask = BIT(mux->bitrdy);
+
+	while ((mmio_read_32(address) & mask) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+int _clk_stm32_set_parent(struct stm32_clk_priv *priv, int clk, int clkp)
+{
+	const struct parent_cfg *parents;
+	uint16_t pid;
+	uint8_t sel;
+	int old_parent;
+
+	pid = priv->clks[clk].parent;
+
+	if ((pid == CLK_IS_ROOT) || (pid < MUX_MAX_PARENTS)) {
+		return -EINVAL;
+	}
+
+	old_parent = _clk_stm32_get_parent(priv, clk);
+	if (old_parent < 0) {
+		return old_parent;
+	}
+	if (old_parent == clkp) {
+		return 0;
+	}
+
+	parents = &priv->parents[pid & MUX_PARENT_MASK];
+
+	for (sel = 0; sel <  parents->num_parents; sel++) {
+		if (parents->id_parents[sel] == (uint16_t)clkp) {
+			bool clk_was_enabled = _clk_stm32_is_enabled(priv, clk);
+			int err = 0;
+
+			/* Enable the parents (for glitch free mux) */
+			_clk_stm32_enable(priv, clkp);
+			_clk_stm32_enable(priv, old_parent);
+
+			err = clk_mux_set_parent(priv, pid, sel);
+
+			_clk_stm32_disable(priv, old_parent);
+
+			if (clk_was_enabled) {
+				_clk_stm32_disable(priv, old_parent);
+			} else {
+				_clk_stm32_disable(priv, clkp);
+			}
+
+			return err;
+		}
+	}
+
+	return -EINVAL;
+}
+
+int clk_mux_get_parent(struct stm32_clk_priv *priv, uint32_t mux_id)
+{
+	const struct parent_cfg *parent;
+	const struct mux_cfg *mux;
+	uint32_t mask;
+
+	if (mux_id >= priv->nb_parents) {
+		panic();
+	}
+
+	parent = &priv->parents[mux_id];
+	mux = parent->mux;
+
+	mask = MASK_WIDTH_SHIFT(mux->width, mux->shift);
+
+	return (mmio_read_32(priv->base + mux->offset) & mask) >> mux->shift;
+}
+
+int _clk_stm32_set_parent_by_index(struct stm32_clk_priv *priv, int clk, int sel)
+{
+	uint16_t pid;
+
+	pid = priv->clks[clk].parent;
+
+	if ((pid == CLK_IS_ROOT) || (pid < MUX_MAX_PARENTS)) {
+		return -EINVAL;
+	}
+
+	return clk_mux_set_parent(priv, pid, sel);
+}
+
+int _clk_stm32_get_parent(struct stm32_clk_priv *priv, int clk_id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, clk_id);
+	const struct parent_cfg *parent;
+	uint16_t mux_id;
+	int sel;
+
+	mux_id = priv->clks[clk_id].parent;
+	if (mux_id == CLK_IS_ROOT) {
+		return CLK_IS_ROOT;
+	}
+
+	if (mux_id < MUX_MAX_PARENTS) {
+		return mux_id & MUX_PARENT_MASK;
+	}
+
+	mux_id &= MUX_PARENT_MASK;
+	parent = &priv->parents[mux_id];
+
+	if (clk->ops->get_parent != NULL) {
+		sel = clk->ops->get_parent(priv, clk_id);
+	} else {
+		sel = clk_mux_get_parent(priv, mux_id);
+	}
+
+	if ((sel >= 0) && (sel < parent->num_parents)) {
+		return parent->id_parents[sel];
+	}
+
+	return -EINVAL;
+}
+
+int _clk_stm32_get_parent_index(struct stm32_clk_priv *priv, int clk_id)
+{
+	uint16_t mux_id;
+
+	mux_id = priv->clks[clk_id].parent;
+	if (mux_id == CLK_IS_ROOT) {
+		return CLK_IS_ROOT;
+	}
+
+	if (mux_id < MUX_MAX_PARENTS) {
+		return mux_id & MUX_PARENT_MASK;
+	}
+
+	mux_id &= MUX_PARENT_MASK;
+
+	return clk_mux_get_parent(priv, mux_id);
+}
+
+int _clk_stm32_get_parent_by_index(struct stm32_clk_priv *priv, int clk_id, int idx)
+{
+	const struct parent_cfg *parent;
+	uint16_t mux_id;
+
+	mux_id = priv->clks[clk_id].parent;
+	if (mux_id == CLK_IS_ROOT) {
+		return CLK_IS_ROOT;
+	}
+
+	if (mux_id < MUX_MAX_PARENTS) {
+		return mux_id & MUX_PARENT_MASK;
+	}
+
+	mux_id &= MUX_PARENT_MASK;
+	parent = &priv->parents[mux_id];
+
+	if (idx < parent->num_parents) {
+		return parent->id_parents[idx];
+	}
+
+	return -EINVAL;
+}
+
+int clk_get_index(struct stm32_clk_priv *priv, unsigned long binding_id)
+{
+	unsigned int i;
+
+	for (i = 0U; i < priv->num; i++) {
+		if (binding_id == priv->clks[i].binding) {
+			return (int)i;
+		}
+	}
+
+	return -EINVAL;
+}
+
+unsigned long _clk_stm32_get_rate(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	int parent;
+	unsigned long rate = 0UL;
+
+	if ((unsigned int)id >= priv->num) {
+		return rate;
+	}
+
+	parent = _clk_stm32_get_parent(priv, id);
+	if (parent < 0) {
+		return 0UL;
+	}
+
+	if (clk->ops->recalc_rate != NULL) {
+		unsigned long prate = 0UL;
+
+		if (parent != CLK_IS_ROOT) {
+			prate = _clk_stm32_get_rate(priv, parent);
+		}
+
+		rate = clk->ops->recalc_rate(priv, id, prate);
+
+		return rate;
+	}
+
+	switch (parent) {
+	case CLK_IS_ROOT:
+		panic();
+
+	default:
+		rate = _clk_stm32_get_rate(priv, parent);
+		break;
+	}
+	return rate;
+
+}
+
+unsigned long _clk_stm32_get_parent_rate(struct stm32_clk_priv *priv, int id)
+{
+	int parent_id = _clk_stm32_get_parent(priv, id);
+
+	if (parent_id < 0) {
+		return 0UL;
+	}
+
+	return _clk_stm32_get_rate(priv, parent_id);
+}
+
+static uint8_t _stm32_clk_get_flags(struct stm32_clk_priv *priv, int id)
+{
+	return priv->clks[id].flags;
+}
+
+bool _stm32_clk_is_flags(struct stm32_clk_priv *priv, int id, uint8_t flag)
+{
+	if (_stm32_clk_get_flags(priv, id) & flag) {
+		return true;
+	}
+
+	return false;
+}
+
+int clk_stm32_enable_call_ops(struct stm32_clk_priv *priv, uint16_t id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+
+	if (clk->ops->enable != NULL) {
+		clk->ops->enable(priv, id);
+	}
+
+	return 0;
+}
+
+static int _clk_stm32_enable_core(struct stm32_clk_priv *priv, int id)
+{
+	int parent;
+	int ret = 0;
+
+	if (priv->gate_refcounts[id] == 0U) {
+		parent = _clk_stm32_get_parent(priv, id);
+		if (parent < 0) {
+			return parent;
+		}
+		if (parent != CLK_IS_ROOT) {
+			ret = _clk_stm32_enable_core(priv, parent);
+			if (ret) {
+				return ret;
+			}
+		}
+		clk_stm32_enable_call_ops(priv, id);
+	}
+
+	priv->gate_refcounts[id]++;
+
+	if (priv->gate_refcounts[id] == UINT_MAX) {
+		ERROR("%s: %d max enable count !", __func__, id);
+		panic();
+	}
+
+	return 0;
+}
+
+int _clk_stm32_enable(struct stm32_clk_priv *priv, int id)
+{
+	int ret;
+
+	stm32mp1_clk_lock(&refcount_lock);
+	ret = _clk_stm32_enable_core(priv, id);
+	stm32mp1_clk_unlock(&refcount_lock);
+
+	return ret;
+}
+
+void clk_stm32_disable_call_ops(struct stm32_clk_priv *priv, uint16_t id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+
+	if (clk->ops->disable != NULL) {
+		clk->ops->disable(priv, id);
+	}
+}
+
+static void _clk_stm32_disable_core(struct stm32_clk_priv *priv, int id)
+{
+	int parent;
+
+	if ((priv->gate_refcounts[id] == 1U) && _stm32_clk_is_flags(priv, id, CLK_IS_CRITICAL)) {
+		return;
+	}
+
+	if (priv->gate_refcounts[id] == 0U) {
+		/* case of clock ignore unused */
+		if (_clk_stm32_is_enabled(priv, id)) {
+			clk_stm32_disable_call_ops(priv, id);
+			return;
+		}
+		VERBOSE("%s: %d already disabled !\n\n", __func__, id);
+		return;
+	}
+
+	if (--priv->gate_refcounts[id] > 0U) {
+		return;
+	}
+
+	clk_stm32_disable_call_ops(priv, id);
+
+	parent = _clk_stm32_get_parent(priv, id);
+	if ((parent >= 0) && (parent != CLK_IS_ROOT)) {
+		_clk_stm32_disable_core(priv, parent);
+	}
+}
+
+void _clk_stm32_disable(struct stm32_clk_priv *priv, int id)
+{
+	stm32mp1_clk_lock(&refcount_lock);
+
+	_clk_stm32_disable_core(priv, id);
+
+	stm32mp1_clk_unlock(&refcount_lock);
+}
+
+bool _clk_stm32_is_enabled(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+
+	if (clk->ops->is_enabled != NULL) {
+		return clk->ops->is_enabled(priv, id);
+	}
+
+	return priv->gate_refcounts[id];
+}
+
+static int clk_stm32_enable(unsigned long binding_id)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	int id;
+
+	id = clk_get_index(priv, binding_id);
+	if (id == -EINVAL) {
+		return id;
+	}
+
+	return _clk_stm32_enable(priv, id);
+}
+
+static void clk_stm32_disable(unsigned long binding_id)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	int id;
+
+	id = clk_get_index(priv, binding_id);
+	if (id != -EINVAL) {
+		_clk_stm32_disable(priv, id);
+	}
+}
+
+static bool clk_stm32_is_enabled(unsigned long binding_id)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	int id;
+
+	id = clk_get_index(priv, binding_id);
+	if (id == -EINVAL) {
+		return false;
+	}
+
+	return _clk_stm32_is_enabled(priv, id);
+}
+
+static unsigned long clk_stm32_get_rate(unsigned long binding_id)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	int id;
+
+	id = clk_get_index(priv, binding_id);
+	if (id == -EINVAL) {
+		return 0UL;
+	}
+
+	return _clk_stm32_get_rate(priv, id);
+}
+
+static int clk_stm32_get_parent(unsigned long binding_id)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	int id;
+
+	id = clk_get_index(priv, binding_id);
+	if (id == -EINVAL) {
+		return id;
+	}
+
+	return _clk_stm32_get_parent(priv, id);
+}
+
+static const struct clk_ops stm32mp_clk_ops = {
+	.enable		= clk_stm32_enable,
+	.disable	= clk_stm32_disable,
+	.is_enabled	= clk_stm32_is_enabled,
+	.get_rate	= clk_stm32_get_rate,
+	.get_parent	= clk_stm32_get_parent,
+};
+
+void clk_stm32_enable_critical_clocks(void)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	unsigned int i;
+
+	for (i = 0U; i < priv->num; i++) {
+		if (_stm32_clk_is_flags(priv, i, CLK_IS_CRITICAL)) {
+			_clk_stm32_enable(priv, i);
+		}
+	}
+}
+
+static void stm32_clk_register(void)
+{
+	clk_register(&stm32mp_clk_ops);
+}
+
+uint32_t clk_stm32_div_get_value(struct stm32_clk_priv *priv, int div_id)
+{
+	const struct div_cfg *divider = &priv->div[div_id];
+	uint32_t val = 0;
+
+	val = mmio_read_32(priv->base + divider->offset) >> divider->shift;
+	val &= clk_div_mask(divider->width);
+
+	return val;
+}
+
+unsigned long _clk_stm32_divider_recalc(struct stm32_clk_priv *priv,
+					int div_id,
+					unsigned long prate)
+{
+	const struct div_cfg *divider = &priv->div[div_id];
+	uint32_t val = clk_stm32_div_get_value(priv, div_id);
+	unsigned int div = 0U;
+
+	div = _get_div(divider->table, val, divider->flags, divider->width);
+	if (div == 0U) {
+		return prate;
+	}
+
+	return div_round_up((uint64_t)prate, div);
+}
+
+unsigned long clk_stm32_divider_recalc(struct stm32_clk_priv *priv, int id,
+				       unsigned long prate)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_stm32_div_cfg *div_cfg = clk->clock_cfg;
+
+	return _clk_stm32_divider_recalc(priv, div_cfg->id, prate);
+}
+
+const struct stm32_clk_ops clk_stm32_divider_ops = {
+	.recalc_rate	= clk_stm32_divider_recalc,
+};
+
+int clk_stm32_set_div(struct stm32_clk_priv *priv, uint32_t div_id, uint32_t value)
+{
+	const struct div_cfg *divider;
+	uintptr_t address;
+	uint64_t timeout;
+	uint32_t mask;
+
+	if (div_id >= priv->nb_div) {
+		panic();
+	}
+
+	divider = &priv->div[div_id];
+	address = priv->base + divider->offset;
+
+	mask = MASK_WIDTH_SHIFT(divider->width, divider->shift);
+	mmio_clrsetbits_32(address, mask, (value << divider->shift) & mask);
+
+	if (divider->bitrdy == DIV_NO_BIT_RDY) {
+		return 0;
+	}
+
+	timeout = timeout_init_us(CLKSRC_TIMEOUT);
+	mask = BIT(divider->bitrdy);
+
+	while ((mmio_read_32(address) & mask) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+int _clk_stm32_gate_wait_ready(struct stm32_clk_priv *priv, uint16_t gate_id,
+			       bool ready_on)
+{
+	const struct gate_cfg *gate = &priv->gates[gate_id];
+	uintptr_t address = priv->base + gate->offset;
+	uint32_t mask_rdy = BIT(gate->bit_idx);
+	uint64_t timeout;
+	uint32_t mask_test;
+
+	if (ready_on) {
+		mask_test = BIT(gate->bit_idx);
+	} else {
+		mask_test = 0U;
+	}
+
+	timeout = timeout_init_us(OSCRDY_TIMEOUT);
+
+	while ((mmio_read_32(address) & mask_rdy) != mask_test) {
+		if (timeout_elapsed(timeout)) {
+			break;
+		}
+	}
+
+	if ((mmio_read_32(address) & mask_rdy) != mask_test) {
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+int clk_stm32_gate_enable(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_stm32_gate_cfg *cfg = clk->clock_cfg;
+	const struct gate_cfg *gate = &priv->gates[cfg->id];
+	uintptr_t addr = priv->base + gate->offset;
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(addr, BIT(gate->bit_idx));
+
+	} else {
+		mmio_setbits_32(addr, BIT(gate->bit_idx));
+	}
+
+	return 0;
+}
+
+void clk_stm32_gate_disable(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_stm32_gate_cfg *cfg = clk->clock_cfg;
+	const struct gate_cfg *gate = &priv->gates[cfg->id];
+	uintptr_t addr = priv->base + gate->offset;
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(addr + RCC_MP_ENCLRR_OFFSET, BIT(gate->bit_idx));
+	} else {
+		mmio_clrbits_32(addr, BIT(gate->bit_idx));
+	}
+}
+
+bool _clk_stm32_gate_is_enabled(struct stm32_clk_priv *priv, int gate_id)
+{
+	const struct gate_cfg *gate;
+	uint32_t addr;
+
+	gate = &priv->gates[gate_id];
+	addr = priv->base + gate->offset;
+
+	return ((mmio_read_32(addr) & BIT(gate->bit_idx)) != 0U);
+}
+
+bool clk_stm32_gate_is_enabled(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_stm32_gate_cfg *cfg = clk->clock_cfg;
+
+	return _clk_stm32_gate_is_enabled(priv, cfg->id);
+}
+
+const struct stm32_clk_ops clk_stm32_gate_ops = {
+	.enable		= clk_stm32_gate_enable,
+	.disable	= clk_stm32_gate_disable,
+	.is_enabled	= clk_stm32_gate_is_enabled,
+};
+
+const struct stm32_clk_ops clk_fixed_factor_ops = {
+	.recalc_rate	= fixed_factor_recalc_rate,
+};
+
+unsigned long fixed_factor_recalc_rate(struct stm32_clk_priv *priv,
+				       int id, unsigned long prate)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	const struct fixed_factor_cfg *cfg = clk->clock_cfg;
+	unsigned long long rate;
+
+	rate = (unsigned long long)prate * cfg->mult;
+
+	if (cfg->div == 0U) {
+		ERROR("division by zero\n");
+		panic();
+	}
+
+	return (unsigned long)(rate / cfg->div);
+};
+
+#define APB_DIV_MASK	GENMASK(2, 0)
+#define TIM_PRE_MASK	BIT(0)
+
+static unsigned long timer_recalc_rate(struct stm32_clk_priv *priv,
+				       int id, unsigned long prate)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	const struct clk_timer_cfg *cfg = clk->clock_cfg;
+	uint32_t prescaler, timpre;
+	uintptr_t rcc_base = priv->base;
+
+	prescaler = mmio_read_32(rcc_base + cfg->apbdiv) &
+		APB_DIV_MASK;
+
+	timpre = mmio_read_32(rcc_base + cfg->timpre) &
+		TIM_PRE_MASK;
+
+	if (prescaler == 0U) {
+		return prate;
+	}
+
+	return prate * (timpre + 1U) * 2U;
+};
+
+const struct stm32_clk_ops clk_timer_ops = {
+	.recalc_rate	= timer_recalc_rate,
+};
+
+static unsigned long clk_fixed_rate_recalc(struct stm32_clk_priv *priv, int id,
+					   unsigned long prate)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_stm32_fixed_rate_cfg *cfg = clk->clock_cfg;
+
+	return cfg->rate;
+}
+
+const struct stm32_clk_ops clk_stm32_fixed_rate_ops = {
+	.recalc_rate	= clk_fixed_rate_recalc,
+};
+
+static unsigned long clk_stm32_osc_recalc_rate(struct stm32_clk_priv *priv,
+					       int id, unsigned long prate)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	return osc_data->frequency;
+};
+
+bool clk_stm32_osc_gate_is_enabled(struct stm32_clk_priv *priv, int id)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	return _clk_stm32_gate_is_enabled(priv, osc_data->gate_id);
+
+}
+
+int clk_stm32_osc_gate_enable(struct stm32_clk_priv *priv, int id)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	_clk_stm32_gate_enable(priv, osc_data->gate_id);
+
+	if (_clk_stm32_gate_wait_ready(priv, osc_data->gate_rdy_id, true) != 0U) {
+		ERROR("%s: %s (%d)\n", __func__, osc_data->name, __LINE__);
+		panic();
+	}
+
+	return 0;
+}
+
+void clk_stm32_osc_gate_disable(struct stm32_clk_priv *priv, int id)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	_clk_stm32_gate_disable(priv, osc_data->gate_id);
+
+	if (_clk_stm32_gate_wait_ready(priv, osc_data->gate_rdy_id, false) != 0U) {
+		ERROR("%s: %s (%d)\n", __func__, osc_data->name, __LINE__);
+		panic();
+	}
+}
+
+static unsigned long clk_stm32_get_dt_oscillator_frequency(const char *name)
+{
+	void *fdt = NULL;
+	int node = 0;
+	int subnode = 0;
+
+	if (fdt_get_address(&fdt) == 0) {
+		panic();
+	}
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return 0UL;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar = NULL;
+		const fdt32_t *cuint = NULL;
+		int ret = 0;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			continue;
+		}
+
+		if (strncmp(cchar, name, (size_t)ret) ||
+		    fdt_get_status(subnode) == DT_DISABLED) {
+			continue;
+		}
+
+		cuint = fdt_getprop(fdt, subnode, "clock-frequency", &ret);
+		if (cuint == NULL) {
+			return 0UL;
+		}
+
+		return fdt32_to_cpu(*cuint);
+	}
+
+	return 0UL;
+}
+
+void clk_stm32_osc_init(struct stm32_clk_priv *priv, int id)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+	const char *name = osc_data->name;
+
+	osc_data->frequency = clk_stm32_get_dt_oscillator_frequency(name);
+}
+
+const struct stm32_clk_ops clk_stm32_osc_ops = {
+	.recalc_rate	= clk_stm32_osc_recalc_rate,
+	.is_enabled	= clk_stm32_osc_gate_is_enabled,
+	.enable		= clk_stm32_osc_gate_enable,
+	.disable	= clk_stm32_osc_gate_disable,
+	.init		= clk_stm32_osc_init,
+};
+
+const struct stm32_clk_ops clk_stm32_osc_nogate_ops = {
+	.recalc_rate	= clk_stm32_osc_recalc_rate,
+	.init		= clk_stm32_osc_init,
+};
+
+int stm32_clk_parse_fdt_by_name(void *fdt, int node, const char *name, uint32_t *tab, uint32_t *nb)
+{
+	const fdt32_t *cell;
+	int len = 0;
+	uint32_t i;
+
+	cell = fdt_getprop(fdt, node, name, &len);
+	if (cell == NULL) {
+		*nb = 0U;
+		return 0;
+	}
+
+	for (i = 0; i < ((uint32_t)len / sizeof(uint32_t)); i++) {
+		uint32_t val = fdt32_to_cpu(cell[i]);
+
+		tab[i] = val;
+	}
+
+	*nb = (uint32_t)len / sizeof(uint32_t);
+
+	return 0;
+}
+
+int clk_stm32_init(struct stm32_clk_priv *priv, uintptr_t base)
+{
+	unsigned int i;
+
+	stm32_clock_data = priv;
+
+	priv->base = base;
+
+	for (i = 0U; i < priv->num; i++) {
+		const struct clk_stm32 *clk = _clk_get(priv, i);
+
+		assert(clk->ops != NULL);
+
+		if (clk->ops->init != NULL) {
+			clk->ops->init(priv, i);
+		}
+	}
+
+	stm32_clk_register();
+
+	return 0;
+}
diff --git a/drivers/st/clk/clk-stm32-core.h b/drivers/st/clk/clk-stm32-core.h
new file mode 100644
index 0000000..8bfb513
--- /dev/null
+++ b/drivers/st/clk/clk-stm32-core.h
@@ -0,0 +1,393 @@
+/*
+ * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#ifndef CLK_STM32_CORE_H
+#define CLK_STM32_CORE_H
+
+struct mux_cfg {
+	uint16_t offset;
+	uint8_t shift;
+	uint8_t width;
+	uint8_t bitrdy;
+};
+
+struct gate_cfg {
+	uint16_t offset;
+	uint8_t bit_idx;
+	uint8_t set_clr;
+};
+
+struct clk_div_table {
+	unsigned int val;
+	unsigned int div;
+};
+
+struct div_cfg {
+	uint16_t offset;
+	uint8_t shift;
+	uint8_t width;
+	uint8_t flags;
+	uint8_t bitrdy;
+	const struct clk_div_table *table;
+};
+
+struct parent_cfg {
+	uint8_t num_parents;
+	const uint16_t *id_parents;
+	struct mux_cfg *mux;
+};
+
+struct stm32_clk_priv;
+
+struct stm32_clk_ops {
+	unsigned long (*recalc_rate)(struct stm32_clk_priv *priv, int id, unsigned long rate);
+	int (*get_parent)(struct stm32_clk_priv *priv, int id);
+	int (*set_rate)(struct stm32_clk_priv *priv, int id, unsigned long rate,
+			unsigned long prate);
+	int (*enable)(struct stm32_clk_priv *priv, int id);
+	void (*disable)(struct stm32_clk_priv *priv, int id);
+	bool (*is_enabled)(struct stm32_clk_priv *priv, int id);
+	void (*init)(struct stm32_clk_priv *priv, int id);
+};
+
+struct clk_stm32 {
+	uint16_t binding;
+	uint16_t parent;
+	uint8_t flags;
+	void *clock_cfg;
+	const struct stm32_clk_ops *ops;
+};
+
+struct stm32_clk_priv {
+	uintptr_t base;
+	const uint32_t num;
+	const struct clk_stm32 *clks;
+	const struct parent_cfg *parents;
+	const uint32_t nb_parents;
+	const struct gate_cfg *gates;
+	const uint32_t nb_gates;
+	const struct div_cfg *div;
+	const uint32_t nb_div;
+	struct clk_oscillator_data *osci_data;
+	const uint32_t nb_osci_data;
+	uint32_t *gate_refcounts;
+	void *pdata;
+};
+
+struct stm32_clk_bypass {
+	uint16_t offset;
+	uint8_t bit_byp;
+	uint8_t bit_digbyp;
+};
+
+struct stm32_clk_css {
+	uint16_t offset;
+	uint8_t bit_css;
+};
+
+struct stm32_clk_drive {
+	uint16_t offset;
+	uint8_t drv_shift;
+	uint8_t drv_width;
+	uint8_t drv_default;
+};
+
+struct clk_oscillator_data {
+	const char *name;
+	uint16_t id_clk;
+	unsigned long frequency;
+	uint16_t gate_id;
+	uint16_t gate_rdy_id;
+	struct stm32_clk_bypass *bypass;
+	struct stm32_clk_css *css;
+	struct stm32_clk_drive *drive;
+};
+
+struct clk_fixed_rate {
+	const char *name;
+	unsigned long fixed_rate;
+};
+
+struct clk_gate_cfg {
+	uint32_t offset;
+	uint8_t bit_idx;
+};
+
+/* CLOCK FLAGS */
+#define CLK_IS_CRITICAL			BIT(0)
+#define CLK_IGNORE_UNUSED		BIT(1)
+#define CLK_SET_RATE_PARENT		BIT(2)
+
+#define CLK_DIVIDER_ONE_BASED		BIT(0)
+#define CLK_DIVIDER_POWER_OF_TWO	BIT(1)
+#define CLK_DIVIDER_ALLOW_ZERO		BIT(2)
+#define CLK_DIVIDER_HIWORD_MASK		BIT(3)
+#define CLK_DIVIDER_ROUND_CLOSEST	BIT(4)
+#define CLK_DIVIDER_READ_ONLY		BIT(5)
+#define CLK_DIVIDER_MAX_AT_ZERO		BIT(6)
+#define CLK_DIVIDER_BIG_ENDIAN		BIT(7)
+
+#define MUX_MAX_PARENTS			U(0x8000)
+#define MUX_PARENT_MASK			GENMASK(14, 0)
+#define MUX_FLAG			U(0x8000)
+#define MUX(mux)			((mux) | MUX_FLAG)
+
+#define NO_GATE				0
+#define _NO_ID				UINT16_MAX
+#define CLK_IS_ROOT			UINT16_MAX
+#define MUX_NO_BIT_RDY			UINT8_MAX
+#define DIV_NO_BIT_RDY			UINT8_MAX
+
+#define MASK_WIDTH_SHIFT(_width, _shift) \
+	GENMASK(((_width) + (_shift) - 1U), (_shift))
+
+int clk_stm32_init(struct stm32_clk_priv *priv, uintptr_t base);
+void clk_stm32_enable_critical_clocks(void);
+
+struct stm32_clk_priv *clk_stm32_get_priv(void);
+
+int clk_get_index(struct stm32_clk_priv *priv, unsigned long binding_id);
+const struct clk_stm32 *_clk_get(struct stm32_clk_priv *priv, int id);
+
+void clk_oscillator_set_bypass(struct stm32_clk_priv *priv, int id, bool digbyp, bool bypass);
+void clk_oscillator_set_drive(struct stm32_clk_priv *priv, int id, uint8_t lsedrv);
+void clk_oscillator_set_css(struct stm32_clk_priv *priv, int id, bool css);
+
+int _clk_stm32_gate_wait_ready(struct stm32_clk_priv *priv, uint16_t gate_id, bool ready_on);
+
+int clk_oscillator_wait_ready(struct stm32_clk_priv *priv, int id, bool ready_on);
+int clk_oscillator_wait_ready_on(struct stm32_clk_priv *priv, int id);
+int clk_oscillator_wait_ready_off(struct stm32_clk_priv *priv, int id);
+
+int clk_stm32_get_counter(unsigned long binding_id);
+
+void _clk_stm32_gate_disable(struct stm32_clk_priv *priv, uint16_t gate_id);
+int _clk_stm32_gate_enable(struct stm32_clk_priv *priv, uint16_t gate_id);
+
+int _clk_stm32_set_parent(struct stm32_clk_priv *priv, int id, int src_id);
+int _clk_stm32_set_parent_by_index(struct stm32_clk_priv *priv, int clk, int sel);
+
+int _clk_stm32_get_parent(struct stm32_clk_priv *priv, int id);
+int _clk_stm32_get_parent_by_index(struct stm32_clk_priv *priv, int clk_id, int idx);
+int _clk_stm32_get_parent_index(struct stm32_clk_priv *priv, int clk_id);
+
+unsigned long _clk_stm32_get_rate(struct stm32_clk_priv *priv, int id);
+unsigned long _clk_stm32_get_parent_rate(struct stm32_clk_priv *priv, int id);
+
+bool _stm32_clk_is_flags(struct stm32_clk_priv *priv, int id, uint8_t flag);
+
+int _clk_stm32_enable(struct stm32_clk_priv *priv, int id);
+void _clk_stm32_disable(struct stm32_clk_priv *priv, int id);
+
+int clk_stm32_enable_call_ops(struct stm32_clk_priv *priv, uint16_t id);
+void clk_stm32_disable_call_ops(struct stm32_clk_priv *priv, uint16_t id);
+
+bool _clk_stm32_is_enabled(struct stm32_clk_priv *priv, int id);
+
+int _clk_stm32_divider_set_rate(struct stm32_clk_priv *priv, int div_id,
+				unsigned long rate, unsigned long parent_rate);
+
+int clk_stm32_divider_set_rate(struct stm32_clk_priv *priv, int id, unsigned long rate,
+			       unsigned long prate);
+
+unsigned long _clk_stm32_divider_recalc(struct stm32_clk_priv *priv,
+					int div_id,
+					unsigned long prate);
+
+unsigned long clk_stm32_divider_recalc(struct stm32_clk_priv *priv, int idx,
+				       unsigned long prate);
+
+int clk_stm32_gate_enable(struct stm32_clk_priv *priv, int idx);
+void clk_stm32_gate_disable(struct stm32_clk_priv *priv, int idx);
+
+bool _clk_stm32_gate_is_enabled(struct stm32_clk_priv *priv, int gate_id);
+bool clk_stm32_gate_is_enabled(struct stm32_clk_priv *priv, int idx);
+
+uint32_t clk_stm32_div_get_value(struct stm32_clk_priv *priv, int div_id);
+int clk_stm32_set_div(struct stm32_clk_priv *priv, uint32_t div_id, uint32_t value);
+int clk_mux_set_parent(struct stm32_clk_priv *priv, uint16_t pid, uint8_t sel);
+int clk_mux_get_parent(struct stm32_clk_priv *priv, uint32_t mux_id);
+
+int stm32_clk_parse_fdt_by_name(void *fdt, int node, const char *name, uint32_t *tab, uint32_t *nb);
+
+#ifdef CFG_STM32_CLK_DEBUG
+void clk_stm32_display_clock_info(void);
+#endif
+
+struct clk_stm32_div_cfg {
+	int id;
+};
+
+#define STM32_DIV(idx, _binding, _parent, _flags, _div_id) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_binding),\
+		.parent		=  (_parent),\
+		.flags		= (_flags),\
+		.clock_cfg	= &(struct clk_stm32_div_cfg){\
+			.id	= (_div_id),\
+		},\
+		.ops		= &clk_stm32_divider_ops,\
+	}
+
+struct clk_stm32_gate_cfg {
+	int id;
+};
+
+#define STM32_GATE(idx, _binding, _parent, _flags, _gate_id) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_binding),\
+		.parent		=  (_parent),\
+		.flags		= (_flags),\
+		.clock_cfg	= &(struct clk_stm32_gate_cfg){\
+			.id	= (_gate_id),\
+		},\
+		.ops		= &clk_stm32_gate_ops,\
+	}
+
+struct fixed_factor_cfg {
+	unsigned int mult;
+	unsigned int div;
+};
+
+unsigned long fixed_factor_recalc_rate(struct stm32_clk_priv *priv,
+				       int _idx, unsigned long prate);
+
+#define FIXED_FACTOR(idx, _idx, _parent, _mult, _div) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_idx),\
+		.parent		= (_parent),\
+		.clock_cfg	= &(struct fixed_factor_cfg){\
+			.mult	= (_mult),\
+			.div	= (_div),\
+		},\
+		.ops		= &clk_fixed_factor_ops,\
+	}
+
+#define GATE(idx, _binding, _parent, _flags, _offset, _bit_idx) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_binding),\
+		.parent		=  (_parent),\
+		.flags		= (_flags),\
+		.clock_cfg	= &(struct clk_gate_cfg){\
+			.offset		= (_offset),\
+			.bit_idx	= (_bit_idx),\
+		},\
+		.ops		= &clk_gate_ops,\
+	}
+
+#define STM32_MUX(idx, _binding, _mux_id, _flags) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_binding),\
+		.parent		= (MUX(_mux_id)),\
+		.flags		= (_flags),\
+		.clock_cfg	= NULL,\
+		.ops		= (&clk_mux_ops),\
+	}
+
+struct clk_timer_cfg {
+	uint32_t apbdiv;
+	uint32_t timpre;
+};
+
+#define CK_TIMER(idx, _idx, _parent, _flags, _apbdiv, _timpre) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_idx),\
+		.parent		= (_parent),\
+		.flags		= (CLK_SET_RATE_PARENT | (_flags)),\
+		.clock_cfg	= &(struct clk_timer_cfg){\
+			.apbdiv = (_apbdiv),\
+			.timpre = (_timpre),\
+		},\
+		.ops		= &clk_timer_ops,\
+	}
+
+struct clk_stm32_fixed_rate_cfg {
+	unsigned long rate;
+};
+
+#define CLK_FIXED_RATE(idx, _binding, _rate) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_binding),\
+		.parent		= (CLK_IS_ROOT),\
+		.clock_cfg	= &(struct clk_stm32_fixed_rate_cfg){\
+			.rate	= (_rate),\
+		},\
+		.ops		= &clk_stm32_fixed_rate_ops,\
+	}
+
+#define BYPASS(_offset, _bit_byp, _bit_digbyp) &(struct stm32_clk_bypass){\
+	.offset		= (_offset),\
+	.bit_byp	= (_bit_byp),\
+	.bit_digbyp	= (_bit_digbyp),\
+}
+
+#define CSS(_offset, _bit_css)	&(struct stm32_clk_css){\
+	.offset		= (_offset),\
+	.bit_css	= (_bit_css),\
+}
+
+#define DRIVE(_offset, _shift, _width, _default) &(struct stm32_clk_drive){\
+	.offset		= (_offset),\
+	.drv_shift	= (_shift),\
+	.drv_width	= (_width),\
+	.drv_default	= (_default),\
+}
+
+#define OSCILLATOR(idx_osc, _id, _name, _gate_id, _gate_rdy_id, _bypass, _css, _drive) \
+	[(idx_osc)] = (struct clk_oscillator_data){\
+		.name		= (_name),\
+		.id_clk		= (_id),\
+		.gate_id	= (_gate_id),\
+		.gate_rdy_id	= (_gate_rdy_id),\
+		.bypass		= (_bypass),\
+		.css		= (_css),\
+		.drive		= (_drive),\
+	}
+
+struct clk_oscillator_data *clk_oscillator_get_data(struct stm32_clk_priv *priv, int id);
+
+void clk_stm32_osc_init(struct stm32_clk_priv *priv, int id);
+bool clk_stm32_osc_gate_is_enabled(struct stm32_clk_priv *priv, int id);
+int clk_stm32_osc_gate_enable(struct stm32_clk_priv *priv, int id);
+void clk_stm32_osc_gate_disable(struct stm32_clk_priv *priv, int id);
+
+struct stm32_osc_cfg {
+	int osc_id;
+};
+
+#define CLK_OSC(idx, _idx, _parent, _osc_id) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_idx),\
+		.parent		= (_parent),\
+		.flags		= CLK_IS_CRITICAL,\
+		.clock_cfg	= &(struct stm32_osc_cfg){\
+			.osc_id = (_osc_id),\
+		},\
+		.ops		= &clk_stm32_osc_ops,\
+	}
+
+#define CLK_OSC_FIXED(idx, _idx, _parent, _osc_id) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_idx),\
+		.parent		= (_parent),\
+		.flags		= CLK_IS_CRITICAL,\
+		.clock_cfg	= &(struct stm32_osc_cfg){\
+			.osc_id	= (_osc_id),\
+		},\
+		.ops		= &clk_stm32_osc_nogate_ops,\
+	}
+
+extern const struct stm32_clk_ops clk_mux_ops;
+extern const struct stm32_clk_ops clk_stm32_divider_ops;
+extern const struct stm32_clk_ops clk_stm32_gate_ops;
+extern const struct stm32_clk_ops clk_fixed_factor_ops;
+extern const struct stm32_clk_ops clk_gate_ops;
+extern const struct stm32_clk_ops clk_timer_ops;
+extern const struct stm32_clk_ops clk_stm32_fixed_rate_ops;
+extern const struct stm32_clk_ops clk_stm32_osc_ops;
+extern const struct stm32_clk_ops clk_stm32_osc_nogate_ops;
+
+#endif /* CLK_STM32_CORE_H */
diff --git a/drivers/st/clk/clk-stm32mp13.c b/drivers/st/clk/clk-stm32mp13.c
new file mode 100644
index 0000000..c960928
--- /dev/null
+++ b/drivers/st/clk/clk-stm32mp13.c
@@ -0,0 +1,2332 @@
+/*
+ * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <limits.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include "clk-stm32-core.h"
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp13_rcc.h>
+#include <drivers/st/stm32mp1_clk.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <dt-bindings/clock/stm32mp13-clksrc.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+struct stm32_osci_dt_cfg {
+	unsigned long freq;
+	bool bypass;
+	bool digbyp;
+	bool css;
+	uint32_t drive;
+};
+
+enum pll_mn {
+	PLL_CFG_M,
+	PLL_CFG_N,
+	PLL_DIV_MN_NB
+};
+
+enum pll_pqr {
+	PLL_CFG_P,
+	PLL_CFG_Q,
+	PLL_CFG_R,
+	PLL_DIV_PQR_NB
+};
+
+enum pll_csg {
+	PLL_CSG_MOD_PER,
+	PLL_CSG_INC_STEP,
+	PLL_CSG_SSCG_MODE,
+	PLL_CSG_NB
+};
+
+struct stm32_pll_vco {
+	uint32_t status;
+	uint32_t src;
+	uint32_t div_mn[PLL_DIV_MN_NB];
+	uint32_t frac;
+	bool csg_enabled;
+	uint32_t csg[PLL_CSG_NB];
+};
+
+struct stm32_pll_output {
+	uint32_t output[PLL_DIV_PQR_NB];
+};
+
+struct stm32_pll_dt_cfg {
+	struct stm32_pll_vco vco;
+	struct stm32_pll_output output;
+};
+
+struct stm32_clk_platdata {
+	uint32_t nosci;
+	struct stm32_osci_dt_cfg *osci;
+	uint32_t npll;
+	struct stm32_pll_dt_cfg *pll;
+	uint32_t nclksrc;
+	uint32_t *clksrc;
+	uint32_t nclkdiv;
+	uint32_t *clkdiv;
+};
+
+enum stm32_clock {
+	/* ROOT CLOCKS */
+	_CK_OFF,
+	_CK_HSI,
+	_CK_HSE,
+	_CK_CSI,
+	_CK_LSI,
+	_CK_LSE,
+	_I2SCKIN,
+	_CSI_DIV122,
+	_HSE_DIV,
+	_HSE_DIV2,
+	_CK_PLL1,
+	_CK_PLL2,
+	_CK_PLL3,
+	_CK_PLL4,
+	_PLL1P,
+	_PLL1P_DIV,
+	_PLL2P,
+	_PLL2Q,
+	_PLL2R,
+	_PLL3P,
+	_PLL3Q,
+	_PLL3R,
+	_PLL4P,
+	_PLL4Q,
+	_PLL4R,
+	_PCLK1,
+	_PCLK2,
+	_PCLK3,
+	_PCLK4,
+	_PCLK5,
+	_PCLK6,
+	_CKMPU,
+	_CKAXI,
+	_CKMLAHB,
+	_CKPER,
+	_CKTIMG1,
+	_CKTIMG2,
+	_CKTIMG3,
+	_USB_PHY_48,
+	_MCO1_K,
+	_MCO2_K,
+	_TRACECK,
+	/* BUS and KERNEL CLOCKS */
+	_DDRC1,
+	_DDRC1LP,
+	_DDRPHYC,
+	_DDRPHYCLP,
+	_DDRCAPB,
+	_DDRCAPBLP,
+	_AXIDCG,
+	_DDRPHYCAPB,
+	_DDRPHYCAPBLP,
+	_SYSCFG,
+	_DDRPERFM,
+	_IWDG2APB,
+	_USBPHY_K,
+	_USBO_K,
+	_RTCAPB,
+	_TZC,
+	_ETZPC,
+	_IWDG1APB,
+	_BSEC,
+	_STGENC,
+	_USART1_K,
+	_USART2_K,
+	_I2C3_K,
+	_I2C4_K,
+	_I2C5_K,
+	_TIM12,
+	_TIM15,
+	_RTCCK,
+	_GPIOA,
+	_GPIOB,
+	_GPIOC,
+	_GPIOD,
+	_GPIOE,
+	_GPIOF,
+	_GPIOG,
+	_GPIOH,
+	_GPIOI,
+	_PKA,
+	_SAES_K,
+	_CRYP1,
+	_HASH1,
+	_RNG1_K,
+	_BKPSRAM,
+	_SDMMC1_K,
+	_SDMMC2_K,
+	_DBGCK,
+	_USART3_K,
+	_UART4_K,
+	_UART5_K,
+	_UART7_K,
+	_UART8_K,
+	_USART6_K,
+	_MCE,
+	_FMC_K,
+	_QSPI_K,
+#if defined(IMAGE_BL32)
+	_LTDC,
+	_DMA1,
+	_DMA2,
+	_MDMA,
+	_ETH1MAC,
+	_USBH,
+	_TIM2,
+	_TIM3,
+	_TIM4,
+	_TIM5,
+	_TIM6,
+	_TIM7,
+	_LPTIM1_K,
+	_SPI2_K,
+	_SPI3_K,
+	_SPDIF_K,
+	_TIM1,
+	_TIM8,
+	_SPI1_K,
+	_SAI1_K,
+	_SAI2_K,
+	_DFSDM,
+	_FDCAN_K,
+	_TIM13,
+	_TIM14,
+	_TIM16,
+	_TIM17,
+	_SPI4_K,
+	_SPI5_K,
+	_I2C1_K,
+	_I2C2_K,
+	_ADFSDM,
+	_LPTIM2_K,
+	_LPTIM3_K,
+	_LPTIM4_K,
+	_LPTIM5_K,
+	_VREF,
+	_DTS,
+	_PMBCTRL,
+	_HDP,
+	_STGENRO,
+	_DCMIPP_K,
+	_DMAMUX1,
+	_DMAMUX2,
+	_DMA3,
+	_ADC1_K,
+	_ADC2_K,
+	_TSC,
+	_AXIMC,
+	_ETH1CK,
+	_ETH1TX,
+	_ETH1RX,
+	_CRC1,
+	_ETH2CK,
+	_ETH2TX,
+	_ETH2RX,
+	_ETH2MAC,
+#endif
+	CK_LAST
+};
+
+/* PARENT CONFIG */
+static const uint16_t RTC_src[] = {
+	 _CK_OFF, _CK_LSE, _CK_LSI, _CK_HSE
+};
+
+static const uint16_t MCO1_src[] = {
+	 _CK_HSI, _CK_HSE, _CK_CSI, _CK_LSI, _CK_LSE
+};
+
+static const uint16_t MCO2_src[] = {
+	 _CKMPU, _CKAXI, _CKMLAHB, _PLL4P, _CK_HSE, _CK_HSI
+};
+
+static const uint16_t PLL12_src[] = {
+	 _CK_HSI, _CK_HSE
+};
+
+static const uint16_t PLL3_src[] = {
+	 _CK_HSI, _CK_HSE, _CK_CSI
+};
+
+static const uint16_t PLL4_src[] = {
+	 _CK_HSI, _CK_HSE, _CK_CSI, _I2SCKIN
+};
+
+static const uint16_t MPU_src[] = {
+	 _CK_HSI, _CK_HSE, _PLL1P, _PLL1P_DIV
+};
+
+static const uint16_t AXI_src[] = {
+	 _CK_HSI, _CK_HSE, _PLL2P
+};
+
+static const uint16_t MLAHBS_src[] = {
+	 _CK_HSI, _CK_HSE, _CK_CSI, _PLL3P
+};
+
+static const uint16_t CKPER_src[] = {
+	 _CK_HSI, _CK_CSI, _CK_HSE, _CK_OFF
+};
+
+static const uint16_t I2C12_src[] = {
+	 _PCLK1, _PLL4R, _CK_HSI, _CK_CSI
+};
+
+static const uint16_t I2C3_src[] = {
+	 _PCLK6, _PLL4R, _CK_HSI, _CK_CSI
+};
+
+static const uint16_t I2C4_src[] = {
+	 _PCLK6, _PLL4R, _CK_HSI, _CK_CSI
+};
+
+static const uint16_t I2C5_src[] = {
+	 _PCLK6, _PLL4R, _CK_HSI, _CK_CSI
+};
+
+static const uint16_t SPI1_src[] = {
+	 _PLL4P, _PLL3Q, _I2SCKIN, _CKPER, _PLL3R
+};
+
+static const uint16_t SPI23_src[] = {
+	 _PLL4P, _PLL3Q, _I2SCKIN, _CKPER, _PLL3R
+};
+
+static const uint16_t SPI4_src[] = {
+	 _PCLK6, _PLL4Q, _CK_HSI, _CK_CSI, _CK_HSE, _I2SCKIN
+};
+
+static const uint16_t SPI5_src[] = {
+	 _PCLK6, _PLL4Q, _CK_HSI, _CK_CSI, _CK_HSE
+};
+
+static const uint16_t UART1_src[] = {
+	 _PCLK6, _PLL3Q, _CK_HSI, _CK_CSI, _PLL4Q, _CK_HSE
+};
+
+static const uint16_t UART2_src[] = {
+	 _PCLK6, _PLL3Q, _CK_HSI, _CK_CSI, _PLL4Q, _CK_HSE
+};
+
+static const uint16_t UART35_src[] = {
+	 _PCLK1, _PLL4Q, _CK_HSI, _CK_CSI, _CK_HSE
+};
+
+static const uint16_t UART4_src[] = {
+	 _PCLK1, _PLL4Q, _CK_HSI, _CK_CSI, _CK_HSE
+};
+
+static const uint16_t UART6_src[] = {
+	 _PCLK2, _PLL4Q, _CK_HSI, _CK_CSI, _CK_HSE
+};
+
+static const uint16_t UART78_src[] = {
+	 _PCLK1, _PLL4Q, _CK_HSI, _CK_CSI, _CK_HSE
+};
+
+static const uint16_t LPTIM1_src[] = {
+	 _PCLK1, _PLL4P, _PLL3Q, _CK_LSE, _CK_LSI, _CKPER
+};
+
+static const uint16_t LPTIM2_src[] = {
+	 _PCLK3, _PLL4Q, _CKPER, _CK_LSE, _CK_LSI
+};
+
+static const uint16_t LPTIM3_src[] = {
+	 _PCLK3, _PLL4Q, _CKPER, _CK_LSE, _CK_LSI
+};
+
+static const uint16_t LPTIM45_src[] = {
+	 _PCLK3, _PLL4P, _PLL3Q, _CK_LSE, _CK_LSI, _CKPER
+};
+
+static const uint16_t SAI1_src[] = {
+	 _PLL4Q, _PLL3Q, _I2SCKIN, _CKPER, _PLL3R
+};
+
+static const uint16_t SAI2_src[] = {
+	 _PLL4Q, _PLL3Q, _I2SCKIN, _CKPER, _NO_ID, _PLL3R
+};
+
+static const uint16_t FDCAN_src[] = {
+	 _CK_HSE, _PLL3Q, _PLL4Q, _PLL4R
+};
+
+static const uint16_t SPDIF_src[] = {
+	 _PLL4P, _PLL3Q, _CK_HSI
+};
+
+static const uint16_t ADC1_src[] = {
+	 _PLL4R, _CKPER, _PLL3Q
+};
+
+static const uint16_t ADC2_src[] = {
+	 _PLL4R, _CKPER, _PLL3Q
+};
+
+static const uint16_t SDMMC1_src[] = {
+	 _CKAXI, _PLL3R, _PLL4P, _CK_HSI
+};
+
+static const uint16_t SDMMC2_src[] = {
+	 _CKAXI, _PLL3R, _PLL4P, _CK_HSI
+};
+
+static const uint16_t ETH1_src[] = {
+	 _PLL4P, _PLL3Q
+};
+
+static const uint16_t ETH2_src[] = {
+	 _PLL4P, _PLL3Q
+};
+
+static const uint16_t USBPHY_src[] = {
+	 _CK_HSE, _PLL4R, _HSE_DIV2
+};
+
+static const uint16_t USBO_src[] = {
+	 _PLL4R, _USB_PHY_48
+};
+
+static const uint16_t QSPI_src[] = {
+	 _CKAXI, _PLL3R, _PLL4P, _CKPER
+};
+
+static const uint16_t FMC_src[] = {
+	 _CKAXI, _PLL3R, _PLL4P, _CKPER
+};
+
+/* Position 2 of RNG1 mux is reserved */
+static const uint16_t RNG1_src[] = {
+	 _CK_CSI, _PLL4R, _CK_OFF, _CK_LSI
+};
+
+static const uint16_t STGEN_src[] = {
+	 _CK_HSI, _CK_HSE
+};
+
+static const uint16_t DCMIPP_src[] = {
+	 _CKAXI, _PLL2Q, _PLL4P, _CKPER
+};
+
+static const uint16_t SAES_src[] = {
+	 _CKAXI, _CKPER, _PLL4R, _CK_LSI
+};
+
+#define MUX_CFG(id, src, _offset, _shift, _witdh)[id] = {\
+	.id_parents	= src,\
+	.num_parents	= ARRAY_SIZE(src),\
+	.mux		= &(struct mux_cfg) {\
+		.offset	= (_offset),\
+		.shift	= (_shift),\
+		.width	= (_witdh),\
+		.bitrdy = MUX_NO_BIT_RDY,\
+	},\
+}
+
+#define MUX_RDY_CFG(id, src, _offset, _shift, _witdh)[id] = {\
+	.id_parents	= src,\
+	.num_parents	= ARRAY_SIZE(src),\
+	.mux		= &(struct mux_cfg) {\
+		.offset	= (_offset),\
+		.shift	= (_shift),\
+		.width	= (_witdh),\
+		.bitrdy = 31,\
+	},\
+}
+
+static const struct parent_cfg parent_mp13[] = {
+	MUX_CFG(MUX_ADC1,	ADC1_src,	RCC_ADC12CKSELR, 0, 2),
+	MUX_CFG(MUX_ADC2,	ADC2_src,	RCC_ADC12CKSELR, 2, 2),
+	MUX_RDY_CFG(MUX_AXI,	AXI_src,	RCC_ASSCKSELR, 0, 3),
+	MUX_CFG(MUX_CKPER,	CKPER_src,	RCC_CPERCKSELR, 0, 2),
+	MUX_CFG(MUX_DCMIPP,	DCMIPP_src,	RCC_DCMIPPCKSELR, 0, 2),
+	MUX_CFG(MUX_ETH1,	ETH1_src,	RCC_ETH12CKSELR, 0, 2),
+	MUX_CFG(MUX_ETH2,	ETH2_src,	RCC_ETH12CKSELR, 8, 2),
+	MUX_CFG(MUX_FDCAN,	FDCAN_src,	RCC_FDCANCKSELR, 0, 2),
+	MUX_CFG(MUX_FMC,	FMC_src,	RCC_FMCCKSELR, 0, 2),
+	MUX_CFG(MUX_I2C12,	I2C12_src,	RCC_I2C12CKSELR, 0, 3),
+	MUX_CFG(MUX_I2C3,	I2C3_src,	RCC_I2C345CKSELR, 0, 3),
+	MUX_CFG(MUX_I2C4,	I2C4_src,	RCC_I2C345CKSELR, 3, 3),
+	MUX_CFG(MUX_I2C5,	I2C5_src,	RCC_I2C345CKSELR, 6, 3),
+	MUX_CFG(MUX_LPTIM1,	LPTIM1_src,	RCC_LPTIM1CKSELR, 0, 3),
+	MUX_CFG(MUX_LPTIM2,	LPTIM2_src,	RCC_LPTIM23CKSELR, 0, 3),
+	MUX_CFG(MUX_LPTIM3,	LPTIM3_src,	RCC_LPTIM23CKSELR, 3, 3),
+	MUX_CFG(MUX_LPTIM45,	LPTIM45_src,	RCC_LPTIM45CKSELR, 0, 3),
+	MUX_CFG(MUX_MCO1,	MCO1_src,	RCC_MCO1CFGR, 0, 3),
+	MUX_CFG(MUX_MCO2,	MCO2_src,	RCC_MCO2CFGR, 0, 3),
+	MUX_RDY_CFG(MUX_MLAHB,	MLAHBS_src,	RCC_MSSCKSELR, 0, 2),
+	MUX_RDY_CFG(MUX_MPU,	MPU_src,	RCC_MPCKSELR, 0, 2),
+	MUX_RDY_CFG(MUX_PLL12,	PLL12_src,	RCC_RCK12SELR, 0, 2),
+	MUX_RDY_CFG(MUX_PLL3,	PLL3_src,	RCC_RCK3SELR, 0, 2),
+	MUX_RDY_CFG(MUX_PLL4,	PLL4_src,	RCC_RCK4SELR, 0, 2),
+	MUX_CFG(MUX_QSPI,	QSPI_src,	RCC_QSPICKSELR, 0, 2),
+	MUX_CFG(MUX_RNG1,	RNG1_src,	RCC_RNG1CKSELR, 0, 2),
+	MUX_CFG(MUX_RTC,	RTC_src,	RCC_BDCR, 16, 2),
+	MUX_CFG(MUX_SAES,	SAES_src,	RCC_SAESCKSELR, 0, 2),
+	MUX_CFG(MUX_SAI1,	SAI1_src,	RCC_SAI1CKSELR, 0, 3),
+	MUX_CFG(MUX_SAI2,	SAI2_src,	RCC_SAI2CKSELR, 0, 3),
+	MUX_CFG(MUX_SDMMC1,	SDMMC1_src,	RCC_SDMMC12CKSELR, 0, 3),
+	MUX_CFG(MUX_SDMMC2,	SDMMC2_src,	RCC_SDMMC12CKSELR, 3, 3),
+	MUX_CFG(MUX_SPDIF,	SPDIF_src,	RCC_SPDIFCKSELR, 0, 2),
+	MUX_CFG(MUX_SPI1,	SPI1_src,	RCC_SPI2S1CKSELR, 0, 3),
+	MUX_CFG(MUX_SPI23,	SPI23_src,	RCC_SPI2S23CKSELR, 0, 3),
+	MUX_CFG(MUX_SPI4,	SPI4_src,	RCC_SPI45CKSELR, 0, 3),
+	MUX_CFG(MUX_SPI5,	SPI5_src,	RCC_SPI45CKSELR, 3, 3),
+	MUX_CFG(MUX_STGEN,	STGEN_src,	RCC_STGENCKSELR, 0, 2),
+	MUX_CFG(MUX_UART1,	UART1_src,	RCC_UART12CKSELR, 0, 3),
+	MUX_CFG(MUX_UART2,	UART2_src,	RCC_UART12CKSELR, 3, 3),
+	MUX_CFG(MUX_UART35,	UART35_src,	RCC_UART35CKSELR, 0, 3),
+	MUX_CFG(MUX_UART4,	UART4_src,	RCC_UART4CKSELR, 0, 3),
+	MUX_CFG(MUX_UART6,	UART6_src,	RCC_UART6CKSELR, 0, 3),
+	MUX_CFG(MUX_UART78,	UART78_src,	RCC_UART78CKSELR, 0, 3),
+	MUX_CFG(MUX_USBO,	USBO_src,	RCC_USBCKSELR, 4, 1),
+	MUX_CFG(MUX_USBPHY,	USBPHY_src,	RCC_USBCKSELR, 0, 2),
+};
+
+/*
+ * GATE CONFIG
+ */
+
+enum enum_gate_cfg {
+	GATE_ZERO, /* reserved for no gate */
+	GATE_LSE,
+	GATE_RTCCK,
+	GATE_LSI,
+	GATE_HSI,
+	GATE_CSI,
+	GATE_HSE,
+	GATE_LSI_RDY,
+	GATE_CSI_RDY,
+	GATE_LSE_RDY,
+	GATE_HSE_RDY,
+	GATE_HSI_RDY,
+	GATE_MCO1,
+	GATE_MCO2,
+	GATE_DBGCK,
+	GATE_TRACECK,
+	GATE_PLL1,
+	GATE_PLL1_DIVP,
+	GATE_PLL1_DIVQ,
+	GATE_PLL1_DIVR,
+	GATE_PLL2,
+	GATE_PLL2_DIVP,
+	GATE_PLL2_DIVQ,
+	GATE_PLL2_DIVR,
+	GATE_PLL3,
+	GATE_PLL3_DIVP,
+	GATE_PLL3_DIVQ,
+	GATE_PLL3_DIVR,
+	GATE_PLL4,
+	GATE_PLL4_DIVP,
+	GATE_PLL4_DIVQ,
+	GATE_PLL4_DIVR,
+	GATE_DDRC1,
+	GATE_DDRC1LP,
+	GATE_DDRPHYC,
+	GATE_DDRPHYCLP,
+	GATE_DDRCAPB,
+	GATE_DDRCAPBLP,
+	GATE_AXIDCG,
+	GATE_DDRPHYCAPB,
+	GATE_DDRPHYCAPBLP,
+	GATE_TIM2,
+	GATE_TIM3,
+	GATE_TIM4,
+	GATE_TIM5,
+	GATE_TIM6,
+	GATE_TIM7,
+	GATE_LPTIM1,
+	GATE_SPI2,
+	GATE_SPI3,
+	GATE_USART3,
+	GATE_UART4,
+	GATE_UART5,
+	GATE_UART7,
+	GATE_UART8,
+	GATE_I2C1,
+	GATE_I2C2,
+	GATE_SPDIF,
+	GATE_TIM1,
+	GATE_TIM8,
+	GATE_SPI1,
+	GATE_USART6,
+	GATE_SAI1,
+	GATE_SAI2,
+	GATE_DFSDM,
+	GATE_ADFSDM,
+	GATE_FDCAN,
+	GATE_LPTIM2,
+	GATE_LPTIM3,
+	GATE_LPTIM4,
+	GATE_LPTIM5,
+	GATE_VREF,
+	GATE_DTS,
+	GATE_PMBCTRL,
+	GATE_HDP,
+	GATE_SYSCFG,
+	GATE_DCMIPP,
+	GATE_DDRPERFM,
+	GATE_IWDG2APB,
+	GATE_USBPHY,
+	GATE_STGENRO,
+	GATE_LTDC,
+	GATE_RTCAPB,
+	GATE_TZC,
+	GATE_ETZPC,
+	GATE_IWDG1APB,
+	GATE_BSEC,
+	GATE_STGENC,
+	GATE_USART1,
+	GATE_USART2,
+	GATE_SPI4,
+	GATE_SPI5,
+	GATE_I2C3,
+	GATE_I2C4,
+	GATE_I2C5,
+	GATE_TIM12,
+	GATE_TIM13,
+	GATE_TIM14,
+	GATE_TIM15,
+	GATE_TIM16,
+	GATE_TIM17,
+	GATE_DMA1,
+	GATE_DMA2,
+	GATE_DMAMUX1,
+	GATE_DMA3,
+	GATE_DMAMUX2,
+	GATE_ADC1,
+	GATE_ADC2,
+	GATE_USBO,
+	GATE_TSC,
+	GATE_GPIOA,
+	GATE_GPIOB,
+	GATE_GPIOC,
+	GATE_GPIOD,
+	GATE_GPIOE,
+	GATE_GPIOF,
+	GATE_GPIOG,
+	GATE_GPIOH,
+	GATE_GPIOI,
+	GATE_PKA,
+	GATE_SAES,
+	GATE_CRYP1,
+	GATE_HASH1,
+	GATE_RNG1,
+	GATE_BKPSRAM,
+	GATE_AXIMC,
+	GATE_MCE,
+	GATE_ETH1CK,
+	GATE_ETH1TX,
+	GATE_ETH1RX,
+	GATE_ETH1MAC,
+	GATE_FMC,
+	GATE_QSPI,
+	GATE_SDMMC1,
+	GATE_SDMMC2,
+	GATE_CRC1,
+	GATE_USBH,
+	GATE_ETH2CK,
+	GATE_ETH2TX,
+	GATE_ETH2RX,
+	GATE_ETH2MAC,
+	GATE_MDMA,
+
+	LAST_GATE
+};
+
+#define GATE_CFG(id, _offset, _bit_idx, _offset_clr)[id] = {\
+	.offset		= (_offset),\
+	.bit_idx	= (_bit_idx),\
+	.set_clr	= (_offset_clr),\
+}
+
+static const struct gate_cfg gates_mp13[LAST_GATE] = {
+	GATE_CFG(GATE_LSE,		RCC_BDCR,	0,	0),
+	GATE_CFG(GATE_RTCCK,		RCC_BDCR,	20,	0),
+	GATE_CFG(GATE_LSI,		RCC_RDLSICR,	0,	0),
+	GATE_CFG(GATE_HSI,		RCC_OCENSETR,	0,	1),
+	GATE_CFG(GATE_CSI,		RCC_OCENSETR,	4,	1),
+	GATE_CFG(GATE_HSE,		RCC_OCENSETR,	8,	1),
+	GATE_CFG(GATE_LSI_RDY,		RCC_RDLSICR,	1,	0),
+	GATE_CFG(GATE_CSI_RDY,		RCC_OCRDYR,	4,	0),
+	GATE_CFG(GATE_LSE_RDY,		RCC_BDCR,	2,	0),
+	GATE_CFG(GATE_HSE_RDY,		RCC_OCRDYR,	8,	0),
+	GATE_CFG(GATE_HSI_RDY,		RCC_OCRDYR,	0,	0),
+	GATE_CFG(GATE_MCO1,		RCC_MCO1CFGR,	12,	0),
+	GATE_CFG(GATE_MCO2,		RCC_MCO2CFGR,	12,	0),
+	GATE_CFG(GATE_DBGCK,		RCC_DBGCFGR,	8,	0),
+	GATE_CFG(GATE_TRACECK,		RCC_DBGCFGR,	9,	0),
+	GATE_CFG(GATE_PLL1,		RCC_PLL1CR,	0,	0),
+	GATE_CFG(GATE_PLL1_DIVP,	RCC_PLL1CR,	4,	0),
+	GATE_CFG(GATE_PLL1_DIVQ,	RCC_PLL1CR,	5,	0),
+	GATE_CFG(GATE_PLL1_DIVR,	RCC_PLL1CR,	6,	0),
+	GATE_CFG(GATE_PLL2,		RCC_PLL2CR,	0,	0),
+	GATE_CFG(GATE_PLL2_DIVP,	RCC_PLL2CR,	4,	0),
+	GATE_CFG(GATE_PLL2_DIVQ,	RCC_PLL2CR,	5,	0),
+	GATE_CFG(GATE_PLL2_DIVR,	RCC_PLL2CR,	6,	0),
+	GATE_CFG(GATE_PLL3,		RCC_PLL3CR,	0,	0),
+	GATE_CFG(GATE_PLL3_DIVP,	RCC_PLL3CR,	4,	0),
+	GATE_CFG(GATE_PLL3_DIVQ,	RCC_PLL3CR,	5,	0),
+	GATE_CFG(GATE_PLL3_DIVR,	RCC_PLL3CR,	6,	0),
+	GATE_CFG(GATE_PLL4,		RCC_PLL4CR,	0,	0),
+	GATE_CFG(GATE_PLL4_DIVP,	RCC_PLL4CR,	4,	0),
+	GATE_CFG(GATE_PLL4_DIVQ,	RCC_PLL4CR,	5,	0),
+	GATE_CFG(GATE_PLL4_DIVR,	RCC_PLL4CR,	6,	0),
+	GATE_CFG(GATE_DDRC1,		RCC_DDRITFCR,	0,	0),
+	GATE_CFG(GATE_DDRC1LP,		RCC_DDRITFCR,	1,	0),
+	GATE_CFG(GATE_DDRPHYC,		RCC_DDRITFCR,	4,	0),
+	GATE_CFG(GATE_DDRPHYCLP,	RCC_DDRITFCR,	5,	0),
+	GATE_CFG(GATE_DDRCAPB,		RCC_DDRITFCR,	6,	0),
+	GATE_CFG(GATE_DDRCAPBLP,	RCC_DDRITFCR,	7,	0),
+	GATE_CFG(GATE_AXIDCG,		RCC_DDRITFCR,	8,	0),
+	GATE_CFG(GATE_DDRPHYCAPB,	RCC_DDRITFCR,	9,	0),
+	GATE_CFG(GATE_DDRPHYCAPBLP,	RCC_DDRITFCR,	10,	0),
+	GATE_CFG(GATE_TIM2,		RCC_MP_APB1ENSETR,	0,	1),
+	GATE_CFG(GATE_TIM3,		RCC_MP_APB1ENSETR,	1,	1),
+	GATE_CFG(GATE_TIM4,		RCC_MP_APB1ENSETR,	2,	1),
+	GATE_CFG(GATE_TIM5,		RCC_MP_APB1ENSETR,	3,	1),
+	GATE_CFG(GATE_TIM6,		RCC_MP_APB1ENSETR,	4,	1),
+	GATE_CFG(GATE_TIM7,		RCC_MP_APB1ENSETR,	5,	1),
+	GATE_CFG(GATE_LPTIM1,		RCC_MP_APB1ENSETR,	9,	1),
+	GATE_CFG(GATE_SPI2,		RCC_MP_APB1ENSETR,	11,	1),
+	GATE_CFG(GATE_SPI3,		RCC_MP_APB1ENSETR,	12,	1),
+	GATE_CFG(GATE_USART3,		RCC_MP_APB1ENSETR,	15,	1),
+	GATE_CFG(GATE_UART4,		RCC_MP_APB1ENSETR,	16,	1),
+	GATE_CFG(GATE_UART5,		RCC_MP_APB1ENSETR,	17,	1),
+	GATE_CFG(GATE_UART7,		RCC_MP_APB1ENSETR,	18,	1),
+	GATE_CFG(GATE_UART8,		RCC_MP_APB1ENSETR,	19,	1),
+	GATE_CFG(GATE_I2C1,		RCC_MP_APB1ENSETR,	21,	1),
+	GATE_CFG(GATE_I2C2,		RCC_MP_APB1ENSETR,	22,	1),
+	GATE_CFG(GATE_SPDIF,		RCC_MP_APB1ENSETR,	26,	1),
+	GATE_CFG(GATE_TIM1,		RCC_MP_APB2ENSETR,	0,	1),
+	GATE_CFG(GATE_TIM8,		RCC_MP_APB2ENSETR,	1,	1),
+	GATE_CFG(GATE_SPI1,		RCC_MP_APB2ENSETR,	8,	1),
+	GATE_CFG(GATE_USART6,		RCC_MP_APB2ENSETR,	13,	1),
+	GATE_CFG(GATE_SAI1,		RCC_MP_APB2ENSETR,	16,	1),
+	GATE_CFG(GATE_SAI2,		RCC_MP_APB2ENSETR,	17,	1),
+	GATE_CFG(GATE_DFSDM,		RCC_MP_APB2ENSETR,	20,	1),
+	GATE_CFG(GATE_ADFSDM,		RCC_MP_APB2ENSETR,	21,	1),
+	GATE_CFG(GATE_FDCAN,		RCC_MP_APB2ENSETR,	24,	1),
+	GATE_CFG(GATE_LPTIM2,		RCC_MP_APB3ENSETR,	0,	1),
+	GATE_CFG(GATE_LPTIM3,		RCC_MP_APB3ENSETR,	1,	1),
+	GATE_CFG(GATE_LPTIM4,		RCC_MP_APB3ENSETR,	2,	1),
+	GATE_CFG(GATE_LPTIM5,		RCC_MP_APB3ENSETR,	3,	1),
+	GATE_CFG(GATE_VREF,		RCC_MP_APB3ENSETR,	13,	1),
+	GATE_CFG(GATE_DTS,		RCC_MP_APB3ENSETR,	16,	1),
+	GATE_CFG(GATE_PMBCTRL,		RCC_MP_APB3ENSETR,	17,	1),
+	GATE_CFG(GATE_HDP,		RCC_MP_APB3ENSETR,	20,	1),
+	GATE_CFG(GATE_SYSCFG,		RCC_MP_S_APB3ENSETR,	0,	1),
+	GATE_CFG(GATE_DCMIPP,		RCC_MP_APB4ENSETR,	1,	1),
+	GATE_CFG(GATE_DDRPERFM,		RCC_MP_APB4ENSETR,	8,	1),
+	GATE_CFG(GATE_IWDG2APB,		RCC_MP_APB4ENSETR,	15,	1),
+	GATE_CFG(GATE_USBPHY,		RCC_MP_APB4ENSETR,	16,	1),
+	GATE_CFG(GATE_STGENRO,		RCC_MP_APB4ENSETR,	20,	1),
+	GATE_CFG(GATE_LTDC,		RCC_MP_S_APB4ENSETR,	0,	1),
+	GATE_CFG(GATE_RTCAPB,		RCC_MP_APB5ENSETR,	8,	1),
+	GATE_CFG(GATE_TZC,		RCC_MP_APB5ENSETR,	11,	1),
+	GATE_CFG(GATE_ETZPC,		RCC_MP_APB5ENSETR,	13,	1),
+	GATE_CFG(GATE_IWDG1APB,		RCC_MP_APB5ENSETR,	15,	1),
+	GATE_CFG(GATE_BSEC,		RCC_MP_APB5ENSETR,	16,	1),
+	GATE_CFG(GATE_STGENC,		RCC_MP_APB5ENSETR,	20,	1),
+	GATE_CFG(GATE_USART1,		RCC_MP_APB6ENSETR,	0,	1),
+	GATE_CFG(GATE_USART2,		RCC_MP_APB6ENSETR,	1,	1),
+	GATE_CFG(GATE_SPI4,		RCC_MP_APB6ENSETR,	2,	1),
+	GATE_CFG(GATE_SPI5,		RCC_MP_APB6ENSETR,	3,	1),
+	GATE_CFG(GATE_I2C3,		RCC_MP_APB6ENSETR,	4,	1),
+	GATE_CFG(GATE_I2C4,		RCC_MP_APB6ENSETR,	5,	1),
+	GATE_CFG(GATE_I2C5,		RCC_MP_APB6ENSETR,	6,	1),
+	GATE_CFG(GATE_TIM12,		RCC_MP_APB6ENSETR,	7,	1),
+	GATE_CFG(GATE_TIM13,		RCC_MP_APB6ENSETR,	8,	1),
+	GATE_CFG(GATE_TIM14,		RCC_MP_APB6ENSETR,	9,	1),
+	GATE_CFG(GATE_TIM15,		RCC_MP_APB6ENSETR,	10,	1),
+	GATE_CFG(GATE_TIM16,		RCC_MP_APB6ENSETR,	11,	1),
+	GATE_CFG(GATE_TIM17,		RCC_MP_APB6ENSETR,	12,	1),
+	GATE_CFG(GATE_DMA1,		RCC_MP_AHB2ENSETR,	0,	1),
+	GATE_CFG(GATE_DMA2,		RCC_MP_AHB2ENSETR,	1,	1),
+	GATE_CFG(GATE_DMAMUX1,		RCC_MP_AHB2ENSETR,	2,	1),
+	GATE_CFG(GATE_DMA3,		RCC_MP_AHB2ENSETR,	3,	1),
+	GATE_CFG(GATE_DMAMUX2,		RCC_MP_AHB2ENSETR,	4,	1),
+	GATE_CFG(GATE_ADC1,		RCC_MP_AHB2ENSETR,	5,	1),
+	GATE_CFG(GATE_ADC2,		RCC_MP_AHB2ENSETR,	6,	1),
+	GATE_CFG(GATE_USBO,		RCC_MP_AHB2ENSETR,	8,	1),
+	GATE_CFG(GATE_TSC,		RCC_MP_AHB4ENSETR,	15,	1),
+
+	GATE_CFG(GATE_GPIOA,		RCC_MP_S_AHB4ENSETR,	0,	1),
+	GATE_CFG(GATE_GPIOB,		RCC_MP_S_AHB4ENSETR,	1,	1),
+	GATE_CFG(GATE_GPIOC,		RCC_MP_S_AHB4ENSETR,	2,	1),
+	GATE_CFG(GATE_GPIOD,		RCC_MP_S_AHB4ENSETR,	3,	1),
+	GATE_CFG(GATE_GPIOE,		RCC_MP_S_AHB4ENSETR,	4,	1),
+	GATE_CFG(GATE_GPIOF,		RCC_MP_S_AHB4ENSETR,	5,	1),
+	GATE_CFG(GATE_GPIOG,		RCC_MP_S_AHB4ENSETR,	6,	1),
+	GATE_CFG(GATE_GPIOH,		RCC_MP_S_AHB4ENSETR,	7,	1),
+	GATE_CFG(GATE_GPIOI,		RCC_MP_S_AHB4ENSETR,	8,	1),
+
+	GATE_CFG(GATE_PKA,		RCC_MP_AHB5ENSETR,	2,	1),
+	GATE_CFG(GATE_SAES,		RCC_MP_AHB5ENSETR,	3,	1),
+	GATE_CFG(GATE_CRYP1,		RCC_MP_AHB5ENSETR,	4,	1),
+	GATE_CFG(GATE_HASH1,		RCC_MP_AHB5ENSETR,	5,	1),
+	GATE_CFG(GATE_RNG1,		RCC_MP_AHB5ENSETR,	6,	1),
+	GATE_CFG(GATE_BKPSRAM,		RCC_MP_AHB5ENSETR,	8,	1),
+	GATE_CFG(GATE_AXIMC,		RCC_MP_AHB5ENSETR,	16,	1),
+	GATE_CFG(GATE_MCE,		RCC_MP_AHB6ENSETR,	1,	1),
+	GATE_CFG(GATE_ETH1CK,		RCC_MP_AHB6ENSETR,	7,	1),
+	GATE_CFG(GATE_ETH1TX,		RCC_MP_AHB6ENSETR,	8,	1),
+	GATE_CFG(GATE_ETH1RX,		RCC_MP_AHB6ENSETR,	9,	1),
+	GATE_CFG(GATE_ETH1MAC,		RCC_MP_AHB6ENSETR,	10,	1),
+	GATE_CFG(GATE_FMC,		RCC_MP_AHB6ENSETR,	12,	1),
+	GATE_CFG(GATE_QSPI,		RCC_MP_AHB6ENSETR,	14,	1),
+	GATE_CFG(GATE_SDMMC1,		RCC_MP_AHB6ENSETR,	16,	1),
+	GATE_CFG(GATE_SDMMC2,		RCC_MP_AHB6ENSETR,	17,	1),
+	GATE_CFG(GATE_CRC1,		RCC_MP_AHB6ENSETR,	20,	1),
+	GATE_CFG(GATE_USBH,		RCC_MP_AHB6ENSETR,	24,	1),
+	GATE_CFG(GATE_ETH2CK,		RCC_MP_AHB6ENSETR,	27,	1),
+	GATE_CFG(GATE_ETH2TX,		RCC_MP_AHB6ENSETR,	28,	1),
+	GATE_CFG(GATE_ETH2RX,		RCC_MP_AHB6ENSETR,	29,	1),
+	GATE_CFG(GATE_ETH2MAC,		RCC_MP_AHB6ENSETR,	30,	1),
+	GATE_CFG(GATE_MDMA,		RCC_MP_S_AHB6ENSETR,	0,	1),
+};
+
+/*
+ * DIV CONFIG
+ */
+
+static const struct clk_div_table axi_div_table[] = {
+	{ 0, 1 }, { 1, 2 }, { 2, 3 }, { 3, 4 },
+	{ 4, 4 }, { 5, 4 }, { 6, 4 }, { 7, 4 },
+	{ 0 },
+};
+
+static const struct clk_div_table mlahb_div_table[] = {
+	{ 0, 1 }, { 1, 2 }, { 2, 4 }, { 3, 8 },
+	{ 4, 16 }, { 5, 32 }, { 6, 64 }, { 7, 128 },
+	{ 8, 256 }, { 9, 512 }, { 10, 512}, { 11, 512 },
+	{ 12, 512 }, { 13, 512 }, { 14, 512}, { 15, 512 },
+	{ 0 },
+};
+
+static const struct clk_div_table apb_div_table[] = {
+	{ 0, 1 }, { 1, 2 }, { 2, 4 }, { 3, 8 },
+	{ 4, 16 }, { 5, 16 }, { 6, 16 }, { 7, 16 },
+	{ 0 },
+};
+
+#define DIV_CFG(id, _offset, _shift, _width, _flags, _table, _bitrdy)[id] = {\
+		.offset	= _offset,\
+		.shift	= _shift,\
+		.width	= _width,\
+		.flags	= _flags,\
+		.table	= _table,\
+		.bitrdy	= _bitrdy,\
+}
+
+static const struct div_cfg dividers_mp13[] = {
+	DIV_CFG(DIV_PLL1DIVP, RCC_PLL1CFGR2, 0, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL2DIVP, RCC_PLL2CFGR2, 0, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL2DIVQ, RCC_PLL2CFGR2, 8, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL2DIVR, RCC_PLL2CFGR2, 16, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL3DIVP, RCC_PLL3CFGR2, 0, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL3DIVQ, RCC_PLL3CFGR2, 8, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL3DIVR, RCC_PLL3CFGR2, 16, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL4DIVP, RCC_PLL4CFGR2, 0, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL4DIVQ, RCC_PLL4CFGR2, 8, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL4DIVR, RCC_PLL4CFGR2, 16, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_MPU, RCC_MPCKDIVR, 0, 4, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_AXI, RCC_AXIDIVR, 0, 3, 0, axi_div_table, 31),
+	DIV_CFG(DIV_MLAHB, RCC_MLAHBDIVR, 0, 4, 0, mlahb_div_table, 31),
+	DIV_CFG(DIV_APB1, RCC_APB1DIVR, 0, 3, 0, apb_div_table, 31),
+	DIV_CFG(DIV_APB2, RCC_APB2DIVR, 0, 3, 0, apb_div_table, 31),
+	DIV_CFG(DIV_APB3, RCC_APB3DIVR, 0, 3, 0, apb_div_table, 31),
+	DIV_CFG(DIV_APB4, RCC_APB4DIVR, 0, 3, 0, apb_div_table, 31),
+	DIV_CFG(DIV_APB5, RCC_APB5DIVR, 0, 3, 0, apb_div_table, 31),
+	DIV_CFG(DIV_APB6, RCC_APB6DIVR, 0, 3, 0, apb_div_table, 31),
+	DIV_CFG(DIV_RTC, RCC_RTCDIVR, 0, 6, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_MCO1, RCC_MCO1CFGR, 4, 4, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_MCO2, RCC_MCO2CFGR, 4, 4, 0, NULL, DIV_NO_BIT_RDY),
+
+	DIV_CFG(DIV_HSI, RCC_HSICFGR, 0, 2, CLK_DIVIDER_POWER_OF_TWO, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_TRACE, RCC_DBGCFGR, 0, 3, CLK_DIVIDER_POWER_OF_TWO, NULL, DIV_NO_BIT_RDY),
+
+	DIV_CFG(DIV_ETH1PTP, RCC_ETH12CKSELR, 4, 4, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_ETH2PTP, RCC_ETH12CKSELR, 12, 4, 0, NULL, DIV_NO_BIT_RDY),
+};
+
+#define MAX_HSI_HZ		64000000
+#define USB_PHY_48_MHZ		48000000
+
+#define TIMEOUT_US_200MS	U(200000)
+#define TIMEOUT_US_1S		U(1000000)
+
+#define PLLRDY_TIMEOUT		TIMEOUT_US_200MS
+#define CLKSRC_TIMEOUT		TIMEOUT_US_200MS
+#define CLKDIV_TIMEOUT		TIMEOUT_US_200MS
+#define HSIDIV_TIMEOUT		TIMEOUT_US_200MS
+#define OSCRDY_TIMEOUT		TIMEOUT_US_1S
+
+enum stm32_osc {
+	OSC_HSI,
+	OSC_HSE,
+	OSC_CSI,
+	OSC_LSI,
+	OSC_LSE,
+	OSC_I2SCKIN,
+	NB_OSCILLATOR
+};
+
+enum stm32mp1_pll_id {
+	_PLL1,
+	_PLL2,
+	_PLL3,
+	_PLL4,
+	_PLL_NB
+};
+
+enum stm32mp1_plltype {
+	PLL_800,
+	PLL_1600,
+	PLL_2000,
+	PLL_TYPE_NB
+};
+
+#define RCC_OFFSET_PLLXCR		0
+#define RCC_OFFSET_PLLXCFGR1		4
+#define RCC_OFFSET_PLLXCFGR2		8
+#define RCC_OFFSET_PLLXFRACR		12
+#define RCC_OFFSET_PLLXCSGR		16
+
+struct stm32_clk_pll {
+	enum stm32mp1_plltype plltype;
+	uint16_t clk_id;
+	uint16_t reg_pllxcr;
+};
+
+struct stm32mp1_pll {
+	uint8_t refclk_min;
+	uint8_t refclk_max;
+};
+
+/* Define characteristic of PLL according type */
+static const struct stm32mp1_pll stm32mp1_pll[PLL_TYPE_NB] = {
+	[PLL_800] = {
+		.refclk_min = 4,
+		.refclk_max = 16,
+	},
+	[PLL_1600] = {
+		.refclk_min = 8,
+		.refclk_max = 16,
+	},
+	[PLL_2000] = {
+		.refclk_min = 8,
+		.refclk_max = 16,
+	},
+};
+
+#if STM32MP_USB_PROGRAMMER
+static bool pll4_bootrom;
+#endif
+
+/* RCC clock device driver private */
+static unsigned int refcounts_mp13[CK_LAST];
+
+static const struct stm32_clk_pll *clk_st32_pll_data(unsigned int idx);
+
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+static void clk_oscillator_check_bypass(struct stm32_clk_priv *priv, int idx,
+					bool digbyp, bool bypass)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, idx);
+	struct stm32_clk_bypass *bypass_data = osc_data->bypass;
+	uintptr_t address;
+
+	if (bypass_data == NULL) {
+		return;
+	}
+
+	address = priv->base + bypass_data->offset;
+	if ((mmio_read_32(address) & RCC_OCENR_HSEBYP) &&
+	    (!(digbyp || bypass))) {
+		panic();
+	}
+}
+#endif
+
+static void stm32_enable_oscillator_hse(struct stm32_clk_priv *priv)
+{
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	struct stm32_osci_dt_cfg *osci = &pdata->osci[OSC_HSE];
+	bool digbyp =  osci->digbyp;
+	bool bypass = osci->bypass;
+	bool css = osci->css;
+
+	if (_clk_stm32_get_rate(priv, _CK_HSE) == 0U) {
+		return;
+	}
+
+	clk_oscillator_set_bypass(priv, _CK_HSE, digbyp, bypass);
+
+	_clk_stm32_enable(priv, _CK_HSE);
+
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+	clk_oscillator_check_bypass(priv, _CK_HSE, digbyp, bypass);
+#endif
+
+	clk_oscillator_set_css(priv, _CK_HSE, css);
+}
+
+static void stm32_enable_oscillator_lse(struct stm32_clk_priv *priv)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, _CK_LSE);
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	struct stm32_osci_dt_cfg *osci = &pdata->osci[OSC_LSE];
+	bool digbyp =  osci->digbyp;
+	bool bypass = osci->bypass;
+	uint8_t drive = osci->drive;
+
+	if (_clk_stm32_get_rate(priv, _CK_LSE) == 0U) {
+		return;
+	}
+
+	clk_oscillator_set_bypass(priv, _CK_LSE, digbyp, bypass);
+
+	clk_oscillator_set_drive(priv, _CK_LSE, drive);
+
+	_clk_stm32_gate_enable(priv, osc_data->gate_id);
+}
+
+static int stm32mp1_set_hsidiv(uint8_t hsidiv)
+{
+	uint64_t timeout;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uintptr_t address = rcc_base + RCC_OCRDYR;
+
+	mmio_clrsetbits_32(rcc_base + RCC_HSICFGR,
+			   RCC_HSICFGR_HSIDIV_MASK,
+			   RCC_HSICFGR_HSIDIV_MASK & (uint32_t)hsidiv);
+
+	timeout = timeout_init_us(HSIDIV_TIMEOUT);
+	while ((mmio_read_32(address) & RCC_OCRDYR_HSIDIVRDY) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("HSIDIV failed @ 0x%lx: 0x%x\n",
+			      address, mmio_read_32(address));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32mp1_hsidiv(unsigned long hsifreq)
+{
+	uint8_t hsidiv;
+	uint32_t hsidivfreq = MAX_HSI_HZ;
+
+	for (hsidiv = 0; hsidiv < 4U; hsidiv++) {
+		if (hsidivfreq == hsifreq) {
+			break;
+		}
+
+		hsidivfreq /= 2U;
+	}
+
+	if (hsidiv == 4U) {
+		ERROR("Invalid clk-hsi frequency\n");
+		return -EINVAL;
+	}
+
+	if (hsidiv != 0U) {
+		return stm32mp1_set_hsidiv(hsidiv);
+	}
+
+	return 0;
+}
+
+static int stm32_clk_oscillators_lse_set_css(struct stm32_clk_priv *priv)
+{
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	struct stm32_osci_dt_cfg *osci = &pdata->osci[OSC_LSE];
+
+	clk_oscillator_set_css(priv, _CK_LSE, osci->css);
+
+	return 0;
+}
+
+static int stm32mp1_come_back_to_hsi(void)
+{
+	int ret;
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+
+	/* Come back to HSI */
+	ret = _clk_stm32_set_parent(priv, _CKMPU, _CK_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = _clk_stm32_set_parent(priv, _CKAXI, _CK_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = _clk_stm32_set_parent(priv, _CKMLAHB, _CK_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return 0;
+}
+
+static int stm32_clk_configure_clk_get_binding_id(struct stm32_clk_priv *priv, uint32_t data)
+{
+	unsigned long binding_id = ((unsigned long)data & CLK_ID_MASK) >> CLK_ID_SHIFT;
+
+	return clk_get_index(priv, binding_id);
+}
+
+static int stm32_clk_configure_clk(struct stm32_clk_priv *priv, uint32_t data)
+{
+	int sel = (data & CLK_SEL_MASK) >> CLK_SEL_SHIFT;
+	int enable = (data & CLK_ON_MASK) >> CLK_ON_SHIFT;
+	int clk_id;
+	int ret;
+
+	clk_id = stm32_clk_configure_clk_get_binding_id(priv, data);
+	if (clk_id < 0) {
+		return clk_id;
+	}
+
+	ret = _clk_stm32_set_parent_by_index(priv, clk_id, sel);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (enable) {
+		clk_stm32_enable_call_ops(priv, clk_id);
+	} else {
+		clk_stm32_disable_call_ops(priv, clk_id);
+	}
+
+	return 0;
+}
+
+static int stm32_clk_configure_mux(struct stm32_clk_priv *priv, uint32_t data)
+{
+	int mux = (data & MUX_ID_MASK) >> MUX_ID_SHIFT;
+	int sel = (data & MUX_SEL_MASK) >> MUX_SEL_SHIFT;
+
+	return clk_mux_set_parent(priv, mux, sel);
+}
+
+static int stm32_clk_dividers_configure(struct stm32_clk_priv *priv)
+{
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	uint32_t i;
+
+	for (i = 0; i < pdata->nclkdiv; i++) {
+		int div_id, div_n;
+		int val;
+		int ret;
+
+		val = pdata->clkdiv[i] & CMD_DATA_MASK;
+		div_id = (val & DIV_ID_MASK) >> DIV_ID_SHIFT;
+		div_n = (val & DIV_DIVN_MASK) >> DIV_DIVN_SHIFT;
+
+		ret = clk_stm32_set_div(priv, div_id, div_n);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32_clk_source_configure(struct stm32_clk_priv *priv)
+{
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	bool ckper_disabled = false;
+	int clk_id;
+	int ret;
+	uint32_t i;
+
+	for (i = 0; i < pdata->nclksrc; i++) {
+		uint32_t val = pdata->clksrc[i];
+		uint32_t cmd, cmd_data;
+
+		if (val == (uint32_t)CLK_CKPER_DISABLED) {
+			ckper_disabled = true;
+			continue;
+		}
+
+		if (val == (uint32_t)CLK_RTC_DISABLED) {
+			continue;
+		}
+
+		cmd = (val & CMD_MASK) >> CMD_SHIFT;
+		cmd_data = val & ~CMD_MASK;
+
+		switch (cmd) {
+		case CMD_MUX:
+			ret = stm32_clk_configure_mux(priv, cmd_data);
+			break;
+
+		case CMD_CLK:
+			clk_id = stm32_clk_configure_clk_get_binding_id(priv, cmd_data);
+
+			if (clk_id == _RTCCK) {
+				if ((_clk_stm32_is_enabled(priv, _RTCCK) == true)) {
+					continue;
+				}
+			}
+
+			ret = stm32_clk_configure_clk(priv, cmd_data);
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	/*
+	 * CKPER is source for some peripheral clocks
+	 * (FMC-NAND / QPSI-NOR) and switching source is allowed
+	 * only if previous clock is still ON
+	 * => deactivate CKPER only after switching clock
+	 */
+	if (ckper_disabled) {
+		ret = stm32_clk_configure_mux(priv, CLK_CKPER_DISABLED & CMD_MASK);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32_clk_stgen_configure(struct stm32_clk_priv *priv, int id)
+{
+	unsigned long stgen_freq;
+
+	stgen_freq = _clk_stm32_get_rate(priv, id);
+
+	stm32mp_stgen_config(stgen_freq);
+
+	return 0;
+}
+
+#define CLK_PLL_CFG(_idx, _clk_id, _type, _reg)\
+	[(_idx)] = {\
+		.clk_id = (_clk_id),\
+		.plltype = (_type),\
+		.reg_pllxcr = (_reg),\
+	}
+
+static int clk_stm32_pll_compute_cfgr1(struct stm32_clk_priv *priv,
+				       const struct stm32_clk_pll *pll,
+				       struct stm32_pll_vco *vco,
+				       uint32_t *value)
+{
+	uint32_t divm = vco->div_mn[PLL_CFG_M];
+	uint32_t divn = vco->div_mn[PLL_CFG_N];
+	unsigned long prate = 0UL;
+	unsigned long refclk = 0UL;
+
+	prate = _clk_stm32_get_parent_rate(priv, pll->clk_id);
+	refclk = prate / (divm + 1U);
+
+	if ((refclk < (stm32mp1_pll[pll->plltype].refclk_min * 1000000U)) ||
+	    (refclk > (stm32mp1_pll[pll->plltype].refclk_max * 1000000U))) {
+		return -EINVAL;
+	}
+
+	*value = 0;
+
+	if ((pll->plltype == PLL_800) && (refclk >= 8000000U)) {
+		*value = 1U << RCC_PLLNCFGR1_IFRGE_SHIFT;
+	}
+
+	*value |= (divn << RCC_PLLNCFGR1_DIVN_SHIFT) & RCC_PLLNCFGR1_DIVN_MASK;
+	*value |= (divm << RCC_PLLNCFGR1_DIVM_SHIFT) & RCC_PLLNCFGR1_DIVM_MASK;
+
+	return 0;
+}
+
+static uint32_t  clk_stm32_pll_compute_cfgr2(struct stm32_pll_output *out)
+{
+	uint32_t value = 0;
+
+	value |= (out->output[PLL_CFG_P] << RCC_PLLNCFGR2_DIVP_SHIFT) & RCC_PLLNCFGR2_DIVP_MASK;
+	value |= (out->output[PLL_CFG_Q] << RCC_PLLNCFGR2_DIVQ_SHIFT) & RCC_PLLNCFGR2_DIVQ_MASK;
+	value |= (out->output[PLL_CFG_R] << RCC_PLLNCFGR2_DIVR_SHIFT) & RCC_PLLNCFGR2_DIVR_MASK;
+
+	return value;
+}
+
+static void clk_stm32_pll_config_vco(struct stm32_clk_priv *priv,
+				     const struct stm32_clk_pll *pll,
+				     struct stm32_pll_vco *vco)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	uint32_t value = 0;
+
+	if (clk_stm32_pll_compute_cfgr1(priv, pll, vco, &value) != 0) {
+		ERROR("Invalid Vref clock !\n");
+		panic();
+	}
+
+	/* Write N / M / IFREGE fields */
+	mmio_write_32(pll_base + RCC_OFFSET_PLLXCFGR1, value);
+
+	/* Fractional configuration */
+	mmio_write_32(pll_base + RCC_OFFSET_PLLXFRACR, 0);
+
+	/* Frac must be enabled only once its configuration is loaded */
+	mmio_write_32(pll_base + RCC_OFFSET_PLLXFRACR, vco->frac << RCC_PLLNFRACR_FRACV_SHIFT);
+	mmio_setbits_32(pll_base + RCC_OFFSET_PLLXFRACR, RCC_PLLNFRACR_FRACLE);
+}
+
+static void clk_stm32_pll_config_csg(struct stm32_clk_priv *priv,
+				     const struct stm32_clk_pll *pll,
+				     struct stm32_pll_vco *vco)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	uint32_t mod_per = 0;
+	uint32_t inc_step = 0;
+	uint32_t sscg_mode = 0;
+	uint32_t value = 0;
+
+	if (!vco->csg_enabled) {
+		return;
+	}
+
+	mod_per = vco->csg[PLL_CSG_MOD_PER];
+	inc_step = vco->csg[PLL_CSG_INC_STEP];
+	sscg_mode = vco->csg[PLL_CSG_SSCG_MODE];
+
+	value |= (mod_per << RCC_PLLNCSGR_MOD_PER_SHIFT) & RCC_PLLNCSGR_MOD_PER_MASK;
+	value |= (inc_step << RCC_PLLNCSGR_INC_STEP_SHIFT) & RCC_PLLNCSGR_INC_STEP_MASK;
+	value |= (sscg_mode << RCC_PLLNCSGR_SSCG_MODE_SHIFT) & RCC_PLLNCSGR_SSCG_MODE_MASK;
+
+	mmio_write_32(pll_base + RCC_OFFSET_PLLXCSGR, value);
+	mmio_setbits_32(pll_base + RCC_OFFSET_PLLXCR, RCC_PLLNCR_SSCG_CTRL);
+}
+
+static void clk_stm32_pll_config_out(struct stm32_clk_priv *priv, const struct stm32_clk_pll *pll,
+				     struct stm32_pll_output *out)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	uint32_t value = 0;
+
+	value = clk_stm32_pll_compute_cfgr2(out);
+
+	mmio_write_32(pll_base + RCC_OFFSET_PLLXCFGR2, value);
+}
+
+static inline struct stm32_pll_dt_cfg *clk_stm32_pll_get_pdata(int pll_idx)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	struct stm32_clk_platdata *pdata = priv->pdata;
+
+	return &pdata->pll[pll_idx];
+}
+
+static bool _clk_stm32_pll_is_enabled(struct stm32_clk_priv *priv, const struct stm32_clk_pll *pll)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+
+	return ((mmio_read_32(pll_base) & RCC_PLLNCR_PLLON) != 0U);
+}
+
+static void _clk_stm32_pll_set_on(struct stm32_clk_priv *priv, const struct stm32_clk_pll *pll)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+
+	/* Preserve RCC_PLLNCR_SSCG_CTRL value */
+	mmio_clrsetbits_32(pll_base, RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN,
+			   RCC_PLLNCR_PLLON);
+}
+
+static void _clk_stm32_pll_set_off(struct stm32_clk_priv *priv, const struct stm32_clk_pll *pll)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+
+	/* Stop all output */
+	mmio_clrbits_32(pll_base, RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN);
+
+	/* Stop PLL */
+	mmio_clrbits_32(pll_base, RCC_PLLNCR_PLLON);
+}
+
+static int _clk_stm32_pll_wait_ready_on(struct stm32_clk_priv *priv,
+					const struct stm32_clk_pll *pll)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	uint64_t timeout = timeout_init_us(PLLRDY_TIMEOUT);
+
+	/* Wait PLL lock */
+	while ((mmio_read_32(pll_base) & RCC_PLLNCR_PLLRDY) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("%d clock start failed @ 0x%x: 0x%x\n",
+			      pll->clk_id, pll->reg_pllxcr, mmio_read_32(pll_base));
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int _clk_stm32_pll_wait_ready_off(struct stm32_clk_priv *priv,
+					 const struct stm32_clk_pll *pll)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	uint64_t timeout = timeout_init_us(PLLRDY_TIMEOUT);
+
+	/* Wait PLL lock */
+	while ((mmio_read_32(pll_base) & RCC_PLLNCR_PLLRDY) != 0U) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("%d clock stop failed @ 0x%x: 0x%x\n",
+			      pll->clk_id, pll->reg_pllxcr, mmio_read_32(pll_base));
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int _clk_stm32_pll_enable(struct stm32_clk_priv *priv, const struct stm32_clk_pll *pll)
+{
+	if (_clk_stm32_pll_is_enabled(priv, pll)) {
+		return 0;
+	}
+
+	/* Preserve RCC_PLLNCR_SSCG_CTRL value */
+	_clk_stm32_pll_set_on(priv, pll);
+
+	/* Wait PLL lock */
+	return _clk_stm32_pll_wait_ready_on(priv, pll);
+}
+
+static void _clk_stm32_pll_disable(struct stm32_clk_priv *priv, const struct stm32_clk_pll *pll)
+{
+	if (!_clk_stm32_pll_is_enabled(priv, pll)) {
+		return;
+	}
+
+	/* Stop all outputs and the PLL */
+	_clk_stm32_pll_set_off(priv, pll);
+
+	/* Wait PLL stopped */
+	_clk_stm32_pll_wait_ready_off(priv, pll);
+}
+
+static int _clk_stm32_pll_init(struct stm32_clk_priv *priv, int pll_idx,
+			       struct stm32_pll_dt_cfg *pll_conf)
+{
+	const struct stm32_clk_pll *pll = clk_st32_pll_data(pll_idx);
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	int ret = 0;
+
+	/* Configure PLLs source */
+	ret = stm32_clk_configure_mux(priv, pll_conf->vco.src);
+	if (ret) {
+		return ret;
+	}
+
+#if STM32MP_USB_PROGRAMMER
+	if ((pll_idx == _PLL4) && pll4_bootrom) {
+		clk_stm32_pll_config_out(priv, pll, &pll_conf->output);
+
+		mmio_setbits_32(pll_base,
+				RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN);
+
+		return 0;
+	}
+#endif
+	/* Stop the PLL before */
+	_clk_stm32_pll_disable(priv, pll);
+
+	clk_stm32_pll_config_vco(priv, pll, &pll_conf->vco);
+	clk_stm32_pll_config_out(priv, pll, &pll_conf->output);
+	clk_stm32_pll_config_csg(priv, pll, &pll_conf->vco);
+
+	ret = _clk_stm32_pll_enable(priv, pll);
+	if (ret != 0) {
+		return ret;
+	}
+
+	mmio_setbits_32(pll_base, RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN);
+
+	return 0;
+}
+
+static int clk_stm32_pll_init(struct stm32_clk_priv *priv, int pll_idx)
+{
+	struct stm32_pll_dt_cfg *pll_conf = clk_stm32_pll_get_pdata(pll_idx);
+
+	if (pll_conf->vco.status) {
+		return _clk_stm32_pll_init(priv, pll_idx, pll_conf);
+	}
+
+	return 0;
+}
+
+static int stm32_clk_pll_configure(struct stm32_clk_priv *priv)
+{
+	int err = 0;
+
+	err = clk_stm32_pll_init(priv, _PLL1);
+	if (err) {
+		return err;
+	}
+
+	err = clk_stm32_pll_init(priv, _PLL2);
+	if (err) {
+		return err;
+	}
+
+	err = clk_stm32_pll_init(priv, _PLL3);
+	if (err) {
+		return err;
+	}
+
+	err = clk_stm32_pll_init(priv, _PLL4);
+	if (err) {
+		return err;
+	}
+
+	return 0;
+}
+
+static int stm32_clk_oscillators_wait_lse_ready(struct stm32_clk_priv *priv)
+{
+	int ret = 0;
+
+	if (_clk_stm32_get_rate(priv, _CK_LSE) != 0U) {
+		ret = clk_oscillator_wait_ready_on(priv, _CK_LSE);
+	}
+
+	return ret;
+}
+
+static void stm32_clk_oscillators_enable(struct stm32_clk_priv *priv)
+{
+	stm32_enable_oscillator_hse(priv);
+	stm32_enable_oscillator_lse(priv);
+	_clk_stm32_enable(priv, _CK_LSI);
+	_clk_stm32_enable(priv, _CK_CSI);
+}
+
+static int stm32_clk_hsidiv_configure(struct stm32_clk_priv *priv)
+{
+	return stm32mp1_hsidiv(_clk_stm32_get_rate(priv, _CK_HSI));
+}
+
+#if STM32MP_USB_PROGRAMMER
+static bool stm32mp1_clk_is_pll4_used_by_bootrom(struct stm32_clk_priv *priv, int usbphy_p)
+{
+	/* Don't initialize PLL4, when used by BOOTROM */
+	if ((stm32mp_get_boot_itf_selected() ==
+	     BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB) &&
+	    (usbphy_p == _PLL4R)) {
+		return true;
+	}
+
+	return false;
+}
+
+static int stm32mp1_clk_check_usb_conflict(struct stm32_clk_priv *priv, int usbphy_p, int usbo_p)
+{
+	int _usbo_p;
+	int _usbphy_p;
+
+	if (!pll4_bootrom) {
+		return 0;
+	}
+
+	_usbo_p = _clk_stm32_get_parent(priv, _USBO_K);
+	_usbphy_p = _clk_stm32_get_parent(priv, _USBPHY_K);
+
+	if ((_usbo_p != usbo_p) || (_usbphy_p != usbphy_p)) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	return 0;
+}
+#endif
+
+static struct clk_oscillator_data stm32mp13_osc_data[NB_OSCILLATOR] = {
+	OSCILLATOR(OSC_HSI, _CK_HSI, "clk-hsi", GATE_HSI, GATE_HSI_RDY,
+		   NULL, NULL, NULL),
+
+	OSCILLATOR(OSC_LSI, _CK_LSI, "clk-lsi", GATE_LSI, GATE_LSI_RDY,
+		   NULL, NULL, NULL),
+
+	OSCILLATOR(OSC_CSI, _CK_CSI, "clk-csi", GATE_CSI, GATE_CSI_RDY,
+		   NULL, NULL, NULL),
+
+	OSCILLATOR(OSC_LSE, _CK_LSE, "clk-lse", GATE_LSE, GATE_LSE_RDY,
+		   BYPASS(RCC_BDCR, 1, 3),
+		   CSS(RCC_BDCR, 8),
+		   DRIVE(RCC_BDCR, 4, 2, 2)),
+
+	OSCILLATOR(OSC_HSE, _CK_HSE, "clk-hse", GATE_HSE, GATE_HSE_RDY,
+		   BYPASS(RCC_OCENSETR, 10, 7),
+		   CSS(RCC_OCENSETR, 11),
+		   NULL),
+
+	OSCILLATOR(OSC_I2SCKIN, _I2SCKIN, "i2s_ckin", NO_GATE, NO_GATE,
+		   NULL, NULL, NULL),
+};
+
+static const char *clk_stm32_get_oscillator_name(enum stm32_osc id)
+{
+	if (id < NB_OSCILLATOR) {
+		return stm32mp13_osc_data[id].name;
+	}
+
+	return NULL;
+}
+
+#define CLK_PLL_CFG(_idx, _clk_id, _type, _reg)\
+	[(_idx)] = {\
+		.clk_id = (_clk_id),\
+		.plltype = (_type),\
+		.reg_pllxcr = (_reg),\
+	}
+
+static const struct stm32_clk_pll stm32_mp13_clk_pll[_PLL_NB] = {
+	CLK_PLL_CFG(_PLL1, _CK_PLL1, PLL_2000, RCC_PLL1CR),
+	CLK_PLL_CFG(_PLL2, _CK_PLL2, PLL_1600, RCC_PLL2CR),
+	CLK_PLL_CFG(_PLL3, _CK_PLL3, PLL_800, RCC_PLL3CR),
+	CLK_PLL_CFG(_PLL4, _CK_PLL4, PLL_800, RCC_PLL4CR),
+};
+
+static const struct stm32_clk_pll *clk_st32_pll_data(unsigned int idx)
+{
+	return &stm32_mp13_clk_pll[idx];
+}
+
+struct stm32_pll_cfg {
+	int pll_id;
+};
+
+static unsigned long clk_stm32_pll_recalc_rate(struct stm32_clk_priv *priv,  int id,
+					       unsigned long prate)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct stm32_pll_cfg *pll_cfg = clk->clock_cfg;
+	const struct stm32_clk_pll *pll = clk_st32_pll_data(pll_cfg->pll_id);
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	uint32_t cfgr1, fracr, divm, divn;
+	unsigned long fvco;
+
+	cfgr1 = mmio_read_32(pll_base + RCC_OFFSET_PLLXCFGR1);
+	fracr = mmio_read_32(pll_base + RCC_OFFSET_PLLXFRACR);
+
+	divm = (cfgr1 & (RCC_PLLNCFGR1_DIVM_MASK)) >> RCC_PLLNCFGR1_DIVM_SHIFT;
+	divn = cfgr1 & RCC_PLLNCFGR1_DIVN_MASK;
+
+	/*
+	 * With FRACV :
+	 *   Fvco = Fck_ref * ((DIVN + 1) + FRACV / 2^13) / (DIVM + 1)
+	 * Without FRACV
+	 *   Fvco = Fck_ref * ((DIVN + 1) / (DIVM + 1)
+	 */
+	if ((fracr & RCC_PLLNFRACR_FRACLE) != 0U) {
+		uint32_t fracv = (fracr & RCC_PLLNFRACR_FRACV_MASK) >>
+				 RCC_PLLNFRACR_FRACV_SHIFT;
+		unsigned long long numerator, denominator;
+
+		numerator = (((unsigned long long)divn + 1U) << 13) + fracv;
+		numerator = prate * numerator;
+		denominator = ((unsigned long long)divm + 1U) << 13;
+		fvco = (unsigned long)(numerator / denominator);
+	} else {
+		fvco = (unsigned long)(prate * (divn + 1U) / (divm + 1U));
+	}
+
+	return fvco;
+};
+
+static bool clk_stm32_pll_is_enabled(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct stm32_pll_cfg *pll_cfg = clk->clock_cfg;
+	const struct stm32_clk_pll *pll = clk_st32_pll_data(pll_cfg->pll_id);
+
+	return _clk_stm32_pll_is_enabled(priv, pll);
+}
+
+static int clk_stm32_pll_enable(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct stm32_pll_cfg *pll_cfg = clk->clock_cfg;
+	const struct stm32_clk_pll *pll = clk_st32_pll_data(pll_cfg->pll_id);
+
+	return _clk_stm32_pll_enable(priv, pll);
+}
+
+static void clk_stm32_pll_disable(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct stm32_pll_cfg *pll_cfg = clk->clock_cfg;
+	const struct stm32_clk_pll *pll = clk_st32_pll_data(pll_cfg->pll_id);
+
+	_clk_stm32_pll_disable(priv, pll);
+}
+
+static const struct stm32_clk_ops clk_stm32_pll_ops = {
+	.recalc_rate	= clk_stm32_pll_recalc_rate,
+	.enable		= clk_stm32_pll_enable,
+	.disable	= clk_stm32_pll_disable,
+	.is_enabled	= clk_stm32_pll_is_enabled,
+};
+
+#define CLK_PLL(idx, _idx, _parent, _gate, _pll_id, _flags)[idx] = {\
+	.binding = _idx,\
+	.parent = _parent,\
+	.flags = (_flags),\
+	.clock_cfg	= &(struct stm32_pll_cfg) {\
+		.pll_id = _pll_id,\
+	},\
+	.ops = &clk_stm32_pll_ops,\
+}
+
+struct clk_stm32_composite_cfg {
+	int gate_id;
+	int div_id;
+};
+
+static unsigned long clk_stm32_composite_recalc_rate(struct stm32_clk_priv *priv,
+						     int idx, unsigned long prate)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, idx);
+	struct clk_stm32_composite_cfg *composite_cfg = clk->clock_cfg;
+
+	return _clk_stm32_divider_recalc(priv, composite_cfg->div_id, prate);
+};
+
+static bool clk_stm32_composite_gate_is_enabled(struct stm32_clk_priv *priv, int idx)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, idx);
+	struct clk_stm32_composite_cfg *composite_cfg = clk->clock_cfg;
+
+	return _clk_stm32_gate_is_enabled(priv, composite_cfg->gate_id);
+}
+
+static int clk_stm32_composite_gate_enable(struct stm32_clk_priv *priv, int idx)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, idx);
+	struct clk_stm32_composite_cfg *composite_cfg = clk->clock_cfg;
+
+	return _clk_stm32_gate_enable(priv, composite_cfg->gate_id);
+}
+
+static void clk_stm32_composite_gate_disable(struct stm32_clk_priv *priv, int idx)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, idx);
+	struct clk_stm32_composite_cfg *composite_cfg = clk->clock_cfg;
+
+	_clk_stm32_gate_disable(priv, composite_cfg->gate_id);
+}
+
+static const struct stm32_clk_ops clk_stm32_composite_ops = {
+	.recalc_rate = clk_stm32_composite_recalc_rate,
+	.is_enabled = clk_stm32_composite_gate_is_enabled,
+	.enable = clk_stm32_composite_gate_enable,
+	.disable = clk_stm32_composite_gate_disable,
+};
+
+#define STM32_COMPOSITE(idx, _binding, _parent, _flags, _gate_id,\
+			_div_id)[idx] = {\
+	.binding = (_binding),\
+	.parent =  (_parent),\
+	.flags = (_flags),\
+	.clock_cfg	= &(struct clk_stm32_composite_cfg) {\
+		.gate_id	= (_gate_id),\
+		.div_id	= (_div_id),\
+	},\
+	.ops = &clk_stm32_composite_ops,\
+}
+
+static const struct clk_stm32 stm32mp13_clk[CK_LAST] = {
+	/* ROOT CLOCKS */
+	CLK_FIXED_RATE(_CK_OFF, _NO_ID, 0),
+	CLK_OSC(_CK_HSE, CK_HSE, CLK_IS_ROOT, OSC_HSE),
+	CLK_OSC(_CK_HSI, CK_HSI, CLK_IS_ROOT, OSC_HSI),
+	CLK_OSC(_CK_CSI, CK_CSI, CLK_IS_ROOT, OSC_CSI),
+	CLK_OSC(_CK_LSI, CK_LSI, CLK_IS_ROOT, OSC_LSI),
+	CLK_OSC(_CK_LSE, CK_LSE, CLK_IS_ROOT, OSC_LSE),
+
+	CLK_OSC_FIXED(_I2SCKIN, _NO_ID, CLK_IS_ROOT, OSC_I2SCKIN),
+
+	CLK_FIXED_RATE(_USB_PHY_48, _NO_ID, USB_PHY_48_MHZ),
+
+	STM32_DIV(_HSE_DIV, _NO_ID, _CK_HSE, 0, DIV_RTC),
+
+	FIXED_FACTOR(_HSE_DIV2, CK_HSE_DIV2, _CK_HSE, 1, 2),
+	FIXED_FACTOR(_CSI_DIV122, _NO_ID, _CK_CSI, 1, 122),
+
+	CLK_PLL(_CK_PLL1, PLL1, MUX(MUX_PLL12), GATE_PLL1, _PLL1, 0),
+	CLK_PLL(_CK_PLL2, PLL2, MUX(MUX_PLL12), GATE_PLL2, _PLL2, 0),
+	CLK_PLL(_CK_PLL3, PLL3, MUX(MUX_PLL3), GATE_PLL3, _PLL3, 0),
+	CLK_PLL(_CK_PLL4, PLL4, MUX(MUX_PLL4), GATE_PLL4, _PLL4, 0),
+
+	STM32_COMPOSITE(_PLL1P, PLL1_P, _CK_PLL1, CLK_IS_CRITICAL, GATE_PLL1_DIVP, DIV_PLL1DIVP),
+	STM32_DIV(_PLL1P_DIV, _NO_ID, _CK_PLL1, 0, DIV_MPU),
+
+	STM32_COMPOSITE(_PLL2P, PLL2_P, _CK_PLL2, CLK_IS_CRITICAL, GATE_PLL2_DIVP, DIV_PLL2DIVP),
+	STM32_COMPOSITE(_PLL2Q, PLL2_Q, _CK_PLL2, 0, GATE_PLL2_DIVQ, DIV_PLL2DIVQ),
+	STM32_COMPOSITE(_PLL2R, PLL2_R, _CK_PLL2, CLK_IS_CRITICAL, GATE_PLL2_DIVR, DIV_PLL2DIVR),
+
+	STM32_COMPOSITE(_PLL3P, PLL3_P, _CK_PLL3, 0, GATE_PLL3_DIVP, DIV_PLL3DIVP),
+	STM32_COMPOSITE(_PLL3Q, PLL3_Q, _CK_PLL3, 0, GATE_PLL3_DIVQ, DIV_PLL3DIVQ),
+	STM32_COMPOSITE(_PLL3R, PLL3_R, _CK_PLL3, 0, GATE_PLL3_DIVR, DIV_PLL3DIVR),
+
+	STM32_COMPOSITE(_PLL4P, PLL4_P, _CK_PLL4, 0, GATE_PLL4_DIVP, DIV_PLL4DIVP),
+	STM32_COMPOSITE(_PLL4Q, PLL4_Q, _CK_PLL4, 0, GATE_PLL4_DIVQ, DIV_PLL4DIVQ),
+	STM32_COMPOSITE(_PLL4R, PLL4_R, _CK_PLL4, 0, GATE_PLL4_DIVR, DIV_PLL4DIVR),
+
+	STM32_MUX(_CKMPU, CK_MPU, MUX_MPU, 0),
+	STM32_DIV(_CKAXI, CK_AXI, MUX(MUX_AXI), 0, DIV_AXI),
+	STM32_DIV(_CKMLAHB, CK_MLAHB, MUX(MUX_MLAHB), CLK_IS_CRITICAL, DIV_MLAHB),
+	STM32_MUX(_CKPER, CK_PER, MUX(MUX_CKPER), 0),
+
+	STM32_DIV(_PCLK1, PCLK1, _CKMLAHB, 0, DIV_APB1),
+	STM32_DIV(_PCLK2, PCLK2, _CKMLAHB, 0, DIV_APB2),
+	STM32_DIV(_PCLK3, PCLK3, _CKMLAHB, 0, DIV_APB3),
+	STM32_DIV(_PCLK4, PCLK4, _CKAXI, 0, DIV_APB4),
+	STM32_DIV(_PCLK5, PCLK5, _CKAXI, 0, DIV_APB5),
+	STM32_DIV(_PCLK6, PCLK6, _CKMLAHB, 0, DIV_APB6),
+
+	CK_TIMER(_CKTIMG1, CK_TIMG1, _PCLK1, 0, RCC_APB1DIVR, RCC_TIMG1PRER),
+	CK_TIMER(_CKTIMG2, CK_TIMG2, _PCLK2, 0, RCC_APB2DIVR, RCC_TIMG2PRER),
+	CK_TIMER(_CKTIMG3, CK_TIMG3, _PCLK6, 0, RCC_APB6DIVR, RCC_TIMG3PRER),
+
+	/* END ROOT CLOCKS */
+
+	STM32_GATE(_DDRC1, DDRC1, _CKAXI, CLK_IS_CRITICAL, GATE_DDRC1),
+	STM32_GATE(_DDRC1LP, DDRC1LP, _CKAXI, CLK_IS_CRITICAL, GATE_DDRC1LP),
+	STM32_GATE(_DDRPHYC, DDRPHYC, _PLL2R, CLK_IS_CRITICAL, GATE_DDRPHYC),
+	STM32_GATE(_DDRPHYCLP, DDRPHYCLP, _PLL2R, CLK_IS_CRITICAL, GATE_DDRPHYCLP),
+	STM32_GATE(_DDRCAPB, DDRCAPB, _PCLK4, CLK_IS_CRITICAL, GATE_DDRCAPB),
+	STM32_GATE(_DDRCAPBLP, DDRCAPBLP, _PCLK4, CLK_IS_CRITICAL, GATE_DDRCAPBLP),
+	STM32_GATE(_AXIDCG, AXIDCG, _CKAXI, CLK_IS_CRITICAL, GATE_AXIDCG),
+	STM32_GATE(_DDRPHYCAPB, DDRPHYCAPB, _PCLK4, CLK_IS_CRITICAL, GATE_DDRPHYCAPB),
+	STM32_GATE(_DDRPHYCAPBLP, DDRPHYCAPBLP, _PCLK4, CLK_IS_CRITICAL,  GATE_DDRPHYCAPBLP),
+
+	STM32_GATE(_SYSCFG, SYSCFG, _PCLK3, 0, GATE_SYSCFG),
+	STM32_GATE(_DDRPERFM, DDRPERFM, _PCLK4, 0, GATE_DDRPERFM),
+	STM32_GATE(_IWDG2APB, IWDG2, _PCLK4, 0, GATE_IWDG2APB),
+	STM32_GATE(_USBPHY_K, USBPHY_K, MUX(MUX_USBPHY), 0, GATE_USBPHY),
+	STM32_GATE(_USBO_K, USBO_K, MUX(MUX_USBO), 0, GATE_USBO),
+
+	STM32_GATE(_RTCAPB, RTCAPB, _PCLK5, CLK_IS_CRITICAL, GATE_RTCAPB),
+	STM32_GATE(_TZC, TZC, _PCLK5, CLK_IS_CRITICAL, GATE_TZC),
+	STM32_GATE(_ETZPC, TZPC, _PCLK5, CLK_IS_CRITICAL, GATE_ETZPC),
+	STM32_GATE(_IWDG1APB, IWDG1, _PCLK5, 0, GATE_IWDG1APB),
+	STM32_GATE(_BSEC, BSEC, _PCLK5, CLK_IS_CRITICAL, GATE_BSEC),
+	STM32_GATE(_STGENC, STGEN_K, MUX(MUX_STGEN), CLK_IS_CRITICAL, GATE_STGENC),
+
+	STM32_GATE(_USART1_K, USART1_K, MUX(MUX_UART1), 0, GATE_USART1),
+	STM32_GATE(_USART2_K, USART2_K, MUX(MUX_UART2), 0, GATE_USART2),
+	STM32_GATE(_I2C3_K, I2C3_K, MUX(MUX_I2C3), 0, GATE_I2C3),
+	STM32_GATE(_I2C4_K, I2C4_K, MUX(MUX_I2C4), 0, GATE_I2C4),
+	STM32_GATE(_I2C5_K, I2C5_K, MUX(MUX_I2C5), 0, GATE_I2C5),
+	STM32_GATE(_TIM12, TIM12_K, _CKTIMG3, 0, GATE_TIM12),
+	STM32_GATE(_TIM15, TIM15_K, _CKTIMG3, 0, GATE_TIM15),
+
+	STM32_GATE(_RTCCK, RTC, MUX(MUX_RTC), 0, GATE_RTCCK),
+
+	STM32_GATE(_GPIOA, GPIOA, _CKMLAHB, 0, GATE_GPIOA),
+	STM32_GATE(_GPIOB, GPIOB, _CKMLAHB, 0, GATE_GPIOB),
+	STM32_GATE(_GPIOC, GPIOC, _CKMLAHB, 0, GATE_GPIOC),
+	STM32_GATE(_GPIOD, GPIOD, _CKMLAHB, 0, GATE_GPIOD),
+	STM32_GATE(_GPIOE, GPIOE, _CKMLAHB, 0, GATE_GPIOE),
+	STM32_GATE(_GPIOF, GPIOF, _CKMLAHB, 0, GATE_GPIOF),
+	STM32_GATE(_GPIOG, GPIOG, _CKMLAHB, 0, GATE_GPIOG),
+	STM32_GATE(_GPIOH, GPIOH, _CKMLAHB, 0, GATE_GPIOH),
+	STM32_GATE(_GPIOI, GPIOI, _CKMLAHB, 0, GATE_GPIOI),
+
+	STM32_GATE(_PKA, PKA, _CKAXI, 0, GATE_PKA),
+	STM32_GATE(_SAES_K, SAES_K, MUX(MUX_SAES), 0, GATE_SAES),
+	STM32_GATE(_CRYP1, CRYP1, _PCLK5, 0, GATE_CRYP1),
+	STM32_GATE(_HASH1, HASH1, _PCLK5, 0, GATE_HASH1),
+
+	STM32_GATE(_RNG1_K, RNG1_K, MUX(MUX_RNG1), 0, GATE_RNG1),
+	STM32_GATE(_BKPSRAM, BKPSRAM, _PCLK5, CLK_IS_CRITICAL, GATE_BKPSRAM),
+
+	STM32_GATE(_SDMMC1_K, SDMMC1_K, MUX(MUX_SDMMC1), 0, GATE_SDMMC1),
+	STM32_GATE(_SDMMC2_K, SDMMC2_K, MUX(MUX_SDMMC2), 0, GATE_SDMMC2),
+	STM32_GATE(_DBGCK, CK_DBG, _CKAXI, 0, GATE_DBGCK),
+
+/* TODO: CHECK CLOCK FOR BL2/BL32 AND IF ONLY FOR TEST OR NOT */
+	STM32_GATE(_USART3_K, USART3_K, MUX(MUX_UART35), 0, GATE_USART3),
+	STM32_GATE(_UART4_K, UART4_K, MUX(MUX_UART4), 0, GATE_UART4),
+	STM32_GATE(_UART5_K, UART5_K, MUX(MUX_UART35), 0, GATE_UART5),
+	STM32_GATE(_UART7_K, UART7_K, MUX(MUX_UART78), 0, GATE_UART7),
+	STM32_GATE(_UART8_K, UART8_K, MUX(MUX_UART78), 0, GATE_UART8),
+	STM32_GATE(_USART6_K, USART6_K, MUX(MUX_UART6), 0, GATE_USART6),
+	STM32_GATE(_MCE, MCE, _CKAXI, CLK_IS_CRITICAL, GATE_MCE),
+	STM32_GATE(_FMC_K, FMC_K, MUX(MUX_FMC), 0, GATE_FMC),
+	STM32_GATE(_QSPI_K, QSPI_K, MUX(MUX_QSPI), 0, GATE_QSPI),
+
+	STM32_COMPOSITE(_MCO1_K, CK_MCO1, MUX(MUX_MCO1), 0, GATE_MCO1, DIV_MCO1),
+	STM32_COMPOSITE(_MCO2_K, CK_MCO2, MUX(MUX_MCO2), 0, GATE_MCO2, DIV_MCO2),
+	STM32_COMPOSITE(_TRACECK, CK_TRACE, _CKAXI, 0, GATE_TRACECK, DIV_TRACE),
+
+#if defined(IMAGE_BL32)
+	STM32_GATE(_TIM2, TIM2_K, _CKTIMG1, 0, GATE_TIM2),
+	STM32_GATE(_TIM3, TIM3_K, _CKTIMG1, 0, GATE_TIM3),
+	STM32_GATE(_TIM4, TIM4_K, _CKTIMG1, 0, GATE_TIM4),
+	STM32_GATE(_TIM5, TIM5_K, _CKTIMG1, 0, GATE_TIM5),
+	STM32_GATE(_TIM6, TIM6_K, _CKTIMG1, 0, GATE_TIM6),
+	STM32_GATE(_TIM7, TIM7_K, _CKTIMG1, 0, GATE_TIM7),
+	STM32_GATE(_TIM13, TIM13_K, _CKTIMG3, 0, GATE_TIM13),
+	STM32_GATE(_TIM14, TIM14_K, _CKTIMG3, 0, GATE_TIM14),
+	STM32_GATE(_LPTIM1_K, LPTIM1_K, MUX(MUX_LPTIM1), 0, GATE_LPTIM1),
+	STM32_GATE(_SPI2_K, SPI2_K, MUX(MUX_SPI23), 0, GATE_SPI2),
+	STM32_GATE(_SPI3_K, SPI3_K, MUX(MUX_SPI23), 0, GATE_SPI3),
+	STM32_GATE(_SPDIF_K, SPDIF_K, MUX(MUX_SPDIF), 0, GATE_SPDIF),
+	STM32_GATE(_TIM1, TIM1_K, _CKTIMG2, 0, GATE_TIM1),
+	STM32_GATE(_TIM8, TIM8_K, _CKTIMG2, 0, GATE_TIM8),
+	STM32_GATE(_TIM16, TIM16_K, _CKTIMG3, 0, GATE_TIM16),
+	STM32_GATE(_TIM17, TIM17_K, _CKTIMG3, 0, GATE_TIM17),
+	STM32_GATE(_SPI1_K, SPI1_K, MUX(MUX_SPI1), 0, GATE_SPI1),
+	STM32_GATE(_SPI4_K, SPI4_K, MUX(MUX_SPI4), 0, GATE_SPI4),
+	STM32_GATE(_SPI5_K, SPI5_K, MUX(MUX_SPI5), 0, GATE_SPI5),
+	STM32_GATE(_SAI1_K, SAI1_K, MUX(MUX_SAI1), 0, GATE_SAI1),
+	STM32_GATE(_SAI2_K, SAI2_K, MUX(MUX_SAI2), 0, GATE_SAI2),
+	STM32_GATE(_DFSDM, DFSDM_K, MUX(MUX_SAI1), 0, GATE_DFSDM),
+	STM32_GATE(_FDCAN_K, FDCAN_K, MUX(MUX_FDCAN), 0, GATE_FDCAN),
+	STM32_GATE(_USBH, USBH, _CKAXI, 0, GATE_USBH),
+	STM32_GATE(_I2C1_K, I2C1_K, MUX(MUX_I2C12), 0, GATE_I2C1),
+	STM32_GATE(_I2C2_K, I2C2_K, MUX(MUX_I2C12), 0, GATE_I2C2),
+	STM32_GATE(_ADFSDM, ADFSDM_K, MUX(MUX_SAI1), 0, GATE_ADFSDM),
+	STM32_GATE(_LPTIM2_K, LPTIM2_K, MUX(MUX_LPTIM2), 0, GATE_LPTIM2),
+	STM32_GATE(_LPTIM3_K, LPTIM3_K, MUX(MUX_LPTIM3), 0, GATE_LPTIM3),
+	STM32_GATE(_LPTIM4_K, LPTIM4_K, MUX(MUX_LPTIM45), 0, GATE_LPTIM4),
+	STM32_GATE(_LPTIM5_K, LPTIM5_K, MUX(MUX_LPTIM45), 0, GATE_LPTIM5),
+	STM32_GATE(_VREF, VREF, _PCLK3, 0, GATE_VREF),
+	STM32_GATE(_DTS, TMPSENS, _PCLK3, 0, GATE_DTS),
+	STM32_GATE(_PMBCTRL, PMBCTRL, _PCLK3, 0, GATE_HDP),
+	STM32_GATE(_HDP, HDP, _PCLK3, 0, GATE_PMBCTRL),
+	STM32_GATE(_STGENRO, STGENRO, _PCLK4, 0, GATE_DCMIPP),
+	STM32_GATE(_DCMIPP_K, DCMIPP_K, MUX(MUX_DCMIPP), 0, GATE_DCMIPP),
+	STM32_GATE(_DMAMUX1, DMAMUX1, _CKAXI, 0, GATE_DMAMUX1),
+	STM32_GATE(_DMAMUX2, DMAMUX2, _CKAXI, 0, GATE_DMAMUX2),
+	STM32_GATE(_DMA3, DMA3, _CKAXI, 0, GATE_DMAMUX2),
+	STM32_GATE(_ADC1_K, ADC1_K, MUX(MUX_ADC1), 0, GATE_ADC1),
+	STM32_GATE(_ADC2_K, ADC2_K, MUX(MUX_ADC2), 0, GATE_ADC2),
+	STM32_GATE(_TSC, TSC, _CKAXI, 0, GATE_TSC),
+	STM32_GATE(_AXIMC, AXIMC, _CKAXI, 0, GATE_AXIMC),
+	STM32_GATE(_CRC1, CRC1, _CKAXI, 0, GATE_ETH1TX),
+	STM32_GATE(_ETH1CK, ETH1CK_K, MUX(MUX_ETH1), 0, GATE_ETH1CK),
+	STM32_GATE(_ETH1TX, ETH1TX, _CKAXI, 0, GATE_ETH1TX),
+	STM32_GATE(_ETH1RX, ETH1RX, _CKAXI, 0, GATE_ETH1RX),
+	STM32_GATE(_ETH2CK, ETH2CK_K, MUX(MUX_ETH2), 0, GATE_ETH2CK),
+	STM32_GATE(_ETH2TX, ETH2TX, _CKAXI, 0, GATE_ETH2TX),
+	STM32_GATE(_ETH2RX, ETH2RX, _CKAXI, 0, GATE_ETH2RX),
+	STM32_GATE(_ETH2MAC, ETH2MAC, _CKAXI, 0, GATE_ETH2MAC),
+#endif
+};
+
+static struct stm32_pll_dt_cfg mp13_pll[_PLL_NB];
+
+static struct stm32_osci_dt_cfg mp13_osci[NB_OSCILLATOR];
+
+static uint32_t mp13_clksrc[MUX_MAX];
+
+static uint32_t mp13_clkdiv[DIV_MAX];
+
+static struct stm32_clk_platdata stm32mp13_clock_pdata = {
+	.osci		= mp13_osci,
+	.nosci		= NB_OSCILLATOR,
+	.pll		= mp13_pll,
+	.npll		= _PLL_NB,
+	.clksrc		= mp13_clksrc,
+	.nclksrc	= MUX_MAX,
+	.clkdiv		= mp13_clkdiv,
+	.nclkdiv	= DIV_MAX,
+};
+
+static struct stm32_clk_priv stm32mp13_clock_data = {
+	.base		= RCC_BASE,
+	.num		= ARRAY_SIZE(stm32mp13_clk),
+	.clks		= stm32mp13_clk,
+	.parents	= parent_mp13,
+	.nb_parents	= ARRAY_SIZE(parent_mp13),
+	.gates		= gates_mp13,
+	.nb_gates	= ARRAY_SIZE(gates_mp13),
+	.div		= dividers_mp13,
+	.nb_div		= ARRAY_SIZE(dividers_mp13),
+	.osci_data	= stm32mp13_osc_data,
+	.nb_osci_data	= ARRAY_SIZE(stm32mp13_osc_data),
+	.gate_refcounts	= refcounts_mp13,
+	.pdata		= &stm32mp13_clock_pdata,
+};
+
+static int stm32mp1_init_clock_tree(void)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	int ret;
+
+#if STM32MP_USB_PROGRAMMER
+	int usbphy_p = _clk_stm32_get_parent(priv, _USBPHY_K);
+	int usbo_p = _clk_stm32_get_parent(priv, _USBO_K);
+
+	/* Don't initialize PLL4, when used by BOOTROM */
+	pll4_bootrom = stm32mp1_clk_is_pll4_used_by_bootrom(priv, usbphy_p);
+#endif
+
+	/*
+	 * Switch ON oscillators found in device-tree.
+	 * Note: HSI already ON after BootROM stage.
+	 */
+	stm32_clk_oscillators_enable(priv);
+
+	/* Come back to HSI */
+	ret = stm32mp1_come_back_to_hsi();
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = stm32_clk_hsidiv_configure(priv);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = stm32_clk_stgen_configure(priv, _STGENC);
+	if (ret != 0) {
+		panic();
+	}
+
+	ret = stm32_clk_dividers_configure(priv);
+	if (ret != 0) {
+		panic();
+	}
+
+	ret = stm32_clk_pll_configure(priv);
+	if (ret != 0) {
+		panic();
+	}
+
+	/* Wait LSE ready before to use it */
+	ret = stm32_clk_oscillators_wait_lse_ready(priv);
+	if (ret != 0) {
+		panic();
+	}
+
+	/* Configure with expected clock source */
+	ret = stm32_clk_source_configure(priv);
+	if (ret != 0) {
+		panic();
+	}
+
+	/* Configure LSE css after RTC source configuration */
+	ret = stm32_clk_oscillators_lse_set_css(priv);
+	if (ret != 0) {
+		panic();
+	}
+
+#if STM32MP_USB_PROGRAMMER
+	ret = stm32mp1_clk_check_usb_conflict(priv, usbphy_p, usbo_p);
+	if (ret != 0) {
+		return ret;
+	}
+#endif
+	/* reconfigure STGEN with DT config */
+	ret = stm32_clk_stgen_configure(priv, _STGENC);
+	if (ret != 0) {
+		panic();
+	}
+
+	/* Software Self-Refresh mode (SSR) during DDR initilialization */
+	mmio_clrsetbits_32(priv->base + RCC_DDRITFCR,
+			   RCC_DDRITFCR_DDRCKMOD_MASK,
+			   RCC_DDRITFCR_DDRCKMOD_SSR <<
+			   RCC_DDRITFCR_DDRCKMOD_SHIFT);
+
+	return 0;
+}
+
+#define LSEDRV_MEDIUM_HIGH 2
+
+static int clk_stm32_parse_oscillator_fdt(void *fdt, int node, const char *name,
+					  struct stm32_osci_dt_cfg *osci)
+{
+	int subnode = 0;
+
+	/* default value oscillator not found, freq=0 */
+	osci->freq = 0;
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar = NULL;
+		const fdt32_t *cuint = NULL;
+		int ret = 0;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return ret;
+		}
+
+		if (strncmp(cchar, name, (size_t)ret) ||
+		    fdt_get_status(subnode) == DT_DISABLED) {
+			continue;
+		}
+
+		cuint = fdt_getprop(fdt, subnode, "clock-frequency", &ret);
+		if (cuint == NULL) {
+			return ret;
+		}
+
+		osci->freq = fdt32_to_cpu(*cuint);
+
+		if (fdt_getprop(fdt, subnode, "st,bypass", NULL) != NULL) {
+			osci->bypass = true;
+		}
+
+		if (fdt_getprop(fdt, subnode, "st,digbypass", NULL) != NULL) {
+			osci->digbyp = true;
+		}
+
+		if (fdt_getprop(fdt, subnode, "st,css", NULL) != NULL) {
+			osci->css = true;
+		}
+
+		osci->drive = fdt_read_uint32_default(fdt, subnode, "st,drive", LSEDRV_MEDIUM_HIGH);
+
+		return 0;
+	}
+
+	return 0;
+}
+
+static int stm32_clk_parse_fdt_all_oscillator(void *fdt, struct stm32_clk_platdata *pdata)
+{
+	int fdt_err = 0;
+	uint32_t i = 0;
+	int node = 0;
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	for (i = 0; i < pdata->nosci; i++) {
+		const char *name = NULL;
+
+		name = clk_stm32_get_oscillator_name((enum stm32_osc)i);
+		if (name == NULL) {
+			continue;
+		}
+
+		fdt_err = clk_stm32_parse_oscillator_fdt(fdt, node, name, &pdata->osci[i]);
+		if (fdt_err < 0) {
+			panic();
+		}
+	}
+
+	return 0;
+}
+
+#define RCC_PLL_NAME_SIZE 12
+
+static int clk_stm32_load_vco_config(void *fdt, int subnode, struct stm32_pll_vco *vco)
+{
+	int err = 0;
+
+	err = fdt_read_uint32_array(fdt, subnode, "divmn", (int)PLL_DIV_MN_NB, vco->div_mn);
+	if (err != 0) {
+		return err;
+	}
+
+	err = fdt_read_uint32_array(fdt, subnode, "csg", (int)PLL_CSG_NB, vco->csg);
+
+	vco->csg_enabled = (err == 0);
+
+	if (err == -FDT_ERR_NOTFOUND) {
+		err = 0;
+	}
+
+	if (err != 0) {
+		return err;
+	}
+
+	vco->status = RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN | RCC_PLLNCR_PLLON;
+
+	vco->frac = fdt_read_uint32_default(fdt, subnode, "frac", 0);
+
+	vco->src = fdt_read_uint32_default(fdt, subnode, "src", UINT32_MAX);
+
+	return 0;
+}
+
+static int clk_stm32_load_output_config(void *fdt, int subnode, struct stm32_pll_output *output)
+{
+	int err = 0;
+
+	err = fdt_read_uint32_array(fdt, subnode, "st,pll_div_pqr", (int)PLL_DIV_PQR_NB,
+				    output->output);
+	if (err != 0) {
+		return err;
+	}
+
+	return 0;
+}
+
+static int clk_stm32_parse_pll_fdt(void *fdt, int subnode, struct stm32_pll_dt_cfg *pll)
+{
+	const fdt32_t *cuint = NULL;
+	int subnode_pll = 0;
+	int subnode_vco = 0;
+	int err = 0;
+
+	cuint = fdt_getprop(fdt, subnode, "st,pll", NULL);
+	if (!cuint) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	subnode_pll = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+	if (subnode_pll < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	cuint = fdt_getprop(fdt, subnode_pll, "st,pll_vco", NULL);
+	if (!cuint) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	subnode_vco = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+	if (subnode_vco < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	err = clk_stm32_load_vco_config(fdt, subnode_vco, &pll->vco);
+	if (err != 0) {
+		return err;
+	}
+
+	err = clk_stm32_load_output_config(fdt, subnode_pll, &pll->output);
+	if (err != 0) {
+		return err;
+	}
+
+	return 0;
+}
+
+static int stm32_clk_parse_fdt_all_pll(void *fdt, int node, struct stm32_clk_platdata *pdata)
+{
+	size_t i = 0U;
+
+	for (i = _PLL1; i < pdata->npll; i++) {
+		struct stm32_pll_dt_cfg *pll = pdata->pll + i;
+		char name[RCC_PLL_NAME_SIZE];
+		int subnode = 0;
+		int err = 0;
+
+		snprintf(name, sizeof(name), "st,pll@%u", i);
+
+		subnode = fdt_subnode_offset(fdt, node, name);
+		if (!fdt_check_node(subnode)) {
+			continue;
+		}
+
+		err = clk_stm32_parse_pll_fdt(fdt, subnode, pll);
+		if (err != 0) {
+			panic();
+		}
+	}
+
+	return 0;
+}
+
+static int stm32_clk_parse_fdt(struct stm32_clk_platdata *pdata)
+{
+	void *fdt = NULL;
+	int node;
+	uint32_t err;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+	if (node < 0) {
+		panic();
+	}
+
+	err = stm32_clk_parse_fdt_all_oscillator(fdt, pdata);
+	if (err != 0) {
+		return err;
+	}
+
+	err = stm32_clk_parse_fdt_all_pll(fdt, node, pdata);
+	if (err != 0) {
+		return err;
+	}
+
+	err = stm32_clk_parse_fdt_by_name(fdt, node, "st,clkdiv", pdata->clkdiv, &pdata->nclkdiv);
+	if (err != 0) {
+		return err;
+	}
+
+	err = stm32_clk_parse_fdt_by_name(fdt, node, "st,clksrc", pdata->clksrc, &pdata->nclksrc);
+	if (err != 0) {
+		return err;
+	}
+
+	return 0;
+}
+
+int stm32mp1_clk_init(void)
+{
+	return 0;
+}
+
+int stm32mp1_clk_probe(void)
+{
+	uintptr_t base = RCC_BASE;
+	int ret;
+
+	ret = stm32_clk_parse_fdt(&stm32mp13_clock_pdata);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = clk_stm32_init(&stm32mp13_clock_data, base);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = stm32mp1_init_clock_tree();
+	if (ret != 0) {
+		return ret;
+	}
+
+	clk_stm32_enable_critical_clocks();
+
+	return 0;
+}
diff --git a/drivers/st/clk/stm32mp1_clk.c b/drivers/st/clk/stm32mp1_clk.c
new file mode 100644
index 0000000..aa5db6f
--- /dev/null
+++ b/drivers/st/clk/stm32mp1_clk.c
@@ -0,0 +1,2373 @@
+/*
+ * Copyright (C) 2018-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <drivers/st/stm32mp1_clk.h>
+#include <drivers/st/stm32mp1_rcc.h>
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+#define MAX_HSI_HZ		64000000
+#define USB_PHY_48_MHZ		48000000
+
+#define TIMEOUT_US_200MS	U(200000)
+#define TIMEOUT_US_1S		U(1000000)
+
+#define PLLRDY_TIMEOUT		TIMEOUT_US_200MS
+#define CLKSRC_TIMEOUT		TIMEOUT_US_200MS
+#define CLKDIV_TIMEOUT		TIMEOUT_US_200MS
+#define HSIDIV_TIMEOUT		TIMEOUT_US_200MS
+#define OSCRDY_TIMEOUT		TIMEOUT_US_1S
+
+const char *stm32mp_osc_node_label[NB_OSC] = {
+	[_LSI] = "clk-lsi",
+	[_LSE] = "clk-lse",
+	[_HSI] = "clk-hsi",
+	[_HSE] = "clk-hse",
+	[_CSI] = "clk-csi",
+	[_I2S_CKIN] = "i2s_ckin",
+};
+
+enum stm32mp1_parent_id {
+/* Oscillators are defined in enum stm32mp_osc_id */
+
+/* Other parent source */
+	_HSI_KER = NB_OSC,
+	_HSE_KER,
+	_HSE_KER_DIV2,
+	_HSE_RTC,
+	_CSI_KER,
+	_PLL1_P,
+	_PLL1_Q,
+	_PLL1_R,
+	_PLL2_P,
+	_PLL2_Q,
+	_PLL2_R,
+	_PLL3_P,
+	_PLL3_Q,
+	_PLL3_R,
+	_PLL4_P,
+	_PLL4_Q,
+	_PLL4_R,
+	_ACLK,
+	_PCLK1,
+	_PCLK2,
+	_PCLK3,
+	_PCLK4,
+	_PCLK5,
+	_HCLK6,
+	_HCLK2,
+	_CK_PER,
+	_CK_MPU,
+	_CK_MCU,
+	_USB_PHY_48,
+	_PARENT_NB,
+	_UNKNOWN_ID = 0xff,
+};
+
+/* Lists only the parent clock we are interested in */
+enum stm32mp1_parent_sel {
+	_I2C12_SEL,
+	_I2C35_SEL,
+	_STGEN_SEL,
+	_I2C46_SEL,
+	_SPI6_SEL,
+	_UART1_SEL,
+	_RNG1_SEL,
+	_UART6_SEL,
+	_UART24_SEL,
+	_UART35_SEL,
+	_UART78_SEL,
+	_SDMMC12_SEL,
+	_SDMMC3_SEL,
+	_QSPI_SEL,
+	_FMC_SEL,
+	_AXIS_SEL,
+	_MCUS_SEL,
+	_USBPHY_SEL,
+	_USBO_SEL,
+	_MPU_SEL,
+	_CKPER_SEL,
+	_RTC_SEL,
+	_PARENT_SEL_NB,
+	_UNKNOWN_SEL = 0xff,
+};
+
+/* State the parent clock ID straight related to a clock */
+static const uint8_t parent_id_clock_id[_PARENT_NB] = {
+	[_HSE] = CK_HSE,
+	[_HSI] = CK_HSI,
+	[_CSI] = CK_CSI,
+	[_LSE] = CK_LSE,
+	[_LSI] = CK_LSI,
+	[_I2S_CKIN] = _UNKNOWN_ID,
+	[_USB_PHY_48] = _UNKNOWN_ID,
+	[_HSI_KER] = CK_HSI,
+	[_HSE_KER] = CK_HSE,
+	[_HSE_KER_DIV2] = CK_HSE_DIV2,
+	[_HSE_RTC] = _UNKNOWN_ID,
+	[_CSI_KER] = CK_CSI,
+	[_PLL1_P] = PLL1_P,
+	[_PLL1_Q] = PLL1_Q,
+	[_PLL1_R] = PLL1_R,
+	[_PLL2_P] = PLL2_P,
+	[_PLL2_Q] = PLL2_Q,
+	[_PLL2_R] = PLL2_R,
+	[_PLL3_P] = PLL3_P,
+	[_PLL3_Q] = PLL3_Q,
+	[_PLL3_R] = PLL3_R,
+	[_PLL4_P] = PLL4_P,
+	[_PLL4_Q] = PLL4_Q,
+	[_PLL4_R] = PLL4_R,
+	[_ACLK] = CK_AXI,
+	[_PCLK1] = CK_AXI,
+	[_PCLK2] = CK_AXI,
+	[_PCLK3] = CK_AXI,
+	[_PCLK4] = CK_AXI,
+	[_PCLK5] = CK_AXI,
+	[_CK_PER] = CK_PER,
+	[_CK_MPU] = CK_MPU,
+	[_CK_MCU] = CK_MCU,
+};
+
+static unsigned int clock_id2parent_id(unsigned long id)
+{
+	unsigned int n;
+
+	for (n = 0U; n < ARRAY_SIZE(parent_id_clock_id); n++) {
+		if (parent_id_clock_id[n] == id) {
+			return n;
+		}
+	}
+
+	return _UNKNOWN_ID;
+}
+
+enum stm32mp1_pll_id {
+	_PLL1,
+	_PLL2,
+	_PLL3,
+	_PLL4,
+	_PLL_NB
+};
+
+enum stm32mp1_div_id {
+	_DIV_P,
+	_DIV_Q,
+	_DIV_R,
+	_DIV_NB,
+};
+
+enum stm32mp1_clksrc_id {
+	CLKSRC_MPU,
+	CLKSRC_AXI,
+	CLKSRC_MCU,
+	CLKSRC_PLL12,
+	CLKSRC_PLL3,
+	CLKSRC_PLL4,
+	CLKSRC_RTC,
+	CLKSRC_MCO1,
+	CLKSRC_MCO2,
+	CLKSRC_NB
+};
+
+enum stm32mp1_clkdiv_id {
+	CLKDIV_MPU,
+	CLKDIV_AXI,
+	CLKDIV_MCU,
+	CLKDIV_APB1,
+	CLKDIV_APB2,
+	CLKDIV_APB3,
+	CLKDIV_APB4,
+	CLKDIV_APB5,
+	CLKDIV_RTC,
+	CLKDIV_MCO1,
+	CLKDIV_MCO2,
+	CLKDIV_NB
+};
+
+enum stm32mp1_pllcfg {
+	PLLCFG_M,
+	PLLCFG_N,
+	PLLCFG_P,
+	PLLCFG_Q,
+	PLLCFG_R,
+	PLLCFG_O,
+	PLLCFG_NB
+};
+
+enum stm32mp1_pllcsg {
+	PLLCSG_MOD_PER,
+	PLLCSG_INC_STEP,
+	PLLCSG_SSCG_MODE,
+	PLLCSG_NB
+};
+
+enum stm32mp1_plltype {
+	PLL_800,
+	PLL_1600,
+	PLL_TYPE_NB
+};
+
+struct stm32mp1_pll {
+	uint8_t refclk_min;
+	uint8_t refclk_max;
+};
+
+struct stm32mp1_clk_gate {
+	uint16_t offset;
+	uint8_t bit;
+	uint8_t index;
+	uint8_t set_clr;
+	uint8_t secure;
+	uint8_t sel; /* Relates to enum stm32mp1_parent_sel */
+	uint8_t fixed; /* Relates to enum stm32mp1_parent_id */
+};
+
+struct stm32mp1_clk_sel {
+	uint16_t offset;
+	uint8_t src;
+	uint8_t msk;
+	uint8_t nb_parent;
+	const uint8_t *parent;
+};
+
+#define REFCLK_SIZE 4
+struct stm32mp1_clk_pll {
+	enum stm32mp1_plltype plltype;
+	uint16_t rckxselr;
+	uint16_t pllxcfgr1;
+	uint16_t pllxcfgr2;
+	uint16_t pllxfracr;
+	uint16_t pllxcr;
+	uint16_t pllxcsgr;
+	enum stm32mp_osc_id refclk[REFCLK_SIZE];
+};
+
+/* Clocks with selectable source and non set/clr register access */
+#define _CLK_SELEC(sec, off, b, idx, s)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 0,				\
+		.secure = (sec),			\
+		.sel = (s),				\
+		.fixed = _UNKNOWN_ID,			\
+	}
+
+/* Clocks with fixed source and non set/clr register access */
+#define _CLK_FIXED(sec, off, b, idx, f)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 0,				\
+		.secure = (sec),			\
+		.sel = _UNKNOWN_SEL,			\
+		.fixed = (f),				\
+	}
+
+/* Clocks with selectable source and set/clr register access */
+#define _CLK_SC_SELEC(sec, off, b, idx, s)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 1,				\
+		.secure = (sec),			\
+		.sel = (s),				\
+		.fixed = _UNKNOWN_ID,			\
+	}
+
+/* Clocks with fixed source and set/clr register access */
+#define _CLK_SC_FIXED(sec, off, b, idx, f)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 1,				\
+		.secure = (sec),			\
+		.sel = _UNKNOWN_SEL,			\
+		.fixed = (f),				\
+	}
+
+#define _CLK_PARENT_SEL(_label, _rcc_selr, _parents)		\
+	[_ ## _label ## _SEL] = {				\
+		.offset = _rcc_selr,				\
+		.src = _rcc_selr ## _ ## _label ## SRC_SHIFT,	\
+		.msk = (_rcc_selr ## _ ## _label ## SRC_MASK) >> \
+		       (_rcc_selr ## _ ## _label ## SRC_SHIFT), \
+		.parent = (_parents),				\
+		.nb_parent = ARRAY_SIZE(_parents)		\
+	}
+
+#define _CLK_PLL(idx, type, off1, off2, off3,		\
+		 off4, off5, off6,			\
+		 p1, p2, p3, p4)			\
+	[(idx)] = {					\
+		.plltype = (type),			\
+		.rckxselr = (off1),			\
+		.pllxcfgr1 = (off2),			\
+		.pllxcfgr2 = (off3),			\
+		.pllxfracr = (off4),			\
+		.pllxcr = (off5),			\
+		.pllxcsgr = (off6),			\
+		.refclk[0] = (p1),			\
+		.refclk[1] = (p2),			\
+		.refclk[2] = (p3),			\
+		.refclk[3] = (p4),			\
+	}
+
+#define NB_GATES	ARRAY_SIZE(stm32mp1_clk_gate)
+
+#define SEC		1
+#define N_S		0
+
+static const struct stm32mp1_clk_gate stm32mp1_clk_gate[] = {
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 0, DDRC1, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 1, DDRC1LP, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 2, DDRC2, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 3, DDRC2LP, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 4, DDRPHYC, _PLL2_R),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 5, DDRPHYCLP, _PLL2_R),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 6, DDRCAPB, _PCLK4),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 7, DDRCAPBLP, _PCLK4),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 8, AXIDCG, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 9, DDRPHYCAPB, _PCLK4),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 10, DDRPHYCAPBLP, _PCLK4),
+
+#if defined(IMAGE_BL32)
+	_CLK_SC_FIXED(N_S, RCC_MP_APB1ENSETR, 6, TIM12_K, _PCLK1),
+#endif
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 14, USART2_K, _UART24_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 15, USART3_K, _UART35_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 16, UART4_K, _UART24_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 17, UART5_K, _UART35_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 18, UART7_K, _UART78_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 19, UART8_K, _UART78_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 21, I2C1_K, _I2C12_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 22, I2C2_K, _I2C12_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 23, I2C3_K, _I2C35_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 24, I2C5_K, _I2C35_SEL),
+
+#if defined(IMAGE_BL32)
+	_CLK_SC_FIXED(N_S, RCC_MP_APB2ENSETR, 2, TIM15_K, _PCLK2),
+#endif
+	_CLK_SC_SELEC(N_S, RCC_MP_APB2ENSETR, 13, USART6_K, _UART6_SEL),
+
+	_CLK_SC_FIXED(N_S, RCC_MP_APB3ENSETR, 11, SYSCFG, _UNKNOWN_ID),
+
+	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 8, DDRPERFM, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 15, IWDG2, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 16, USBPHY_K, _USBPHY_SEL),
+
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 0, SPI6_K, _SPI6_SEL),
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 2, I2C4_K, _I2C46_SEL),
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 3, I2C6_K, _I2C46_SEL),
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 4, USART1_K, _UART1_SEL),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 8, RTCAPB, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 11, TZC1, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 12, TZC2, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 13, TZPC, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 15, IWDG1, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 16, BSEC, _PCLK5),
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 20, STGEN_K, _STGEN_SEL),
+
+#if defined(IMAGE_BL32)
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB2ENSETR, 8, USBO_K, _USBO_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB2ENSETR, 16, SDMMC3_K, _SDMMC3_SEL),
+#endif
+
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 0, GPIOA, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 1, GPIOB, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 2, GPIOC, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 3, GPIOD, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 4, GPIOE, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 5, GPIOF, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 6, GPIOG, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 7, GPIOH, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 8, GPIOI, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 9, GPIOJ, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 10, GPIOK, _UNKNOWN_SEL),
+
+	_CLK_SC_FIXED(SEC, RCC_MP_AHB5ENSETR, 0, GPIOZ, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_AHB5ENSETR, 4, CRYP1, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_AHB5ENSETR, 5, HASH1, _PCLK5),
+	_CLK_SC_SELEC(SEC, RCC_MP_AHB5ENSETR, 6, RNG1_K, _RNG1_SEL),
+	_CLK_SC_FIXED(SEC, RCC_MP_AHB5ENSETR, 8, BKPSRAM, _PCLK5),
+
+#if defined(IMAGE_BL2)
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 12, FMC_K, _FMC_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 14, QSPI_K, _QSPI_SEL),
+#endif
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 16, SDMMC1_K, _SDMMC12_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 17, SDMMC2_K, _SDMMC12_SEL),
+#if defined(IMAGE_BL32)
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 24, USBH, _UNKNOWN_SEL),
+#endif
+
+	_CLK_SELEC(SEC, RCC_BDCR, 20, RTC, _RTC_SEL),
+	_CLK_SELEC(N_S, RCC_DBGCFGR, 8, CK_DBG, _UNKNOWN_SEL),
+};
+
+static const uint8_t i2c12_parents[] = {
+	_PCLK1, _PLL4_R, _HSI_KER, _CSI_KER
+};
+
+static const uint8_t i2c35_parents[] = {
+	_PCLK1, _PLL4_R, _HSI_KER, _CSI_KER
+};
+
+static const uint8_t stgen_parents[] = {
+	_HSI_KER, _HSE_KER
+};
+
+static const uint8_t i2c46_parents[] = {
+	_PCLK5, _PLL3_Q, _HSI_KER, _CSI_KER
+};
+
+static const uint8_t spi6_parents[] = {
+	_PCLK5, _PLL4_Q, _HSI_KER, _CSI_KER, _HSE_KER, _PLL3_Q
+};
+
+static const uint8_t usart1_parents[] = {
+	_PCLK5, _PLL3_Q, _HSI_KER, _CSI_KER, _PLL4_Q, _HSE_KER
+};
+
+static const uint8_t rng1_parents[] = {
+	_CSI, _PLL4_R, _LSE, _LSI
+};
+
+static const uint8_t uart6_parents[] = {
+	_PCLK2, _PLL4_Q, _HSI_KER, _CSI_KER, _HSE_KER
+};
+
+static const uint8_t uart234578_parents[] = {
+	_PCLK1, _PLL4_Q, _HSI_KER, _CSI_KER, _HSE_KER
+};
+
+static const uint8_t sdmmc12_parents[] = {
+	_HCLK6, _PLL3_R, _PLL4_P, _HSI_KER
+};
+
+static const uint8_t sdmmc3_parents[] = {
+	_HCLK2, _PLL3_R, _PLL4_P, _HSI_KER
+};
+
+static const uint8_t qspi_parents[] = {
+	_ACLK, _PLL3_R, _PLL4_P, _CK_PER
+};
+
+static const uint8_t fmc_parents[] = {
+	_ACLK, _PLL3_R, _PLL4_P, _CK_PER
+};
+
+static const uint8_t axiss_parents[] = {
+	_HSI, _HSE, _PLL2_P
+};
+
+static const uint8_t mcuss_parents[] = {
+	_HSI, _HSE, _CSI, _PLL3_P
+};
+
+static const uint8_t usbphy_parents[] = {
+	_HSE_KER, _PLL4_R, _HSE_KER_DIV2
+};
+
+static const uint8_t usbo_parents[] = {
+	_PLL4_R, _USB_PHY_48
+};
+
+static const uint8_t mpu_parents[] = {
+	_HSI, _HSE, _PLL1_P, _PLL1_P /* specific div */
+};
+
+static const uint8_t per_parents[] = {
+	_HSI, _HSE, _CSI,
+};
+
+static const uint8_t rtc_parents[] = {
+	_UNKNOWN_ID, _LSE, _LSI, _HSE_RTC
+};
+
+static const struct stm32mp1_clk_sel stm32mp1_clk_sel[_PARENT_SEL_NB] = {
+	_CLK_PARENT_SEL(I2C12, RCC_I2C12CKSELR, i2c12_parents),
+	_CLK_PARENT_SEL(I2C35, RCC_I2C35CKSELR, i2c35_parents),
+	_CLK_PARENT_SEL(STGEN, RCC_STGENCKSELR, stgen_parents),
+	_CLK_PARENT_SEL(I2C46, RCC_I2C46CKSELR, i2c46_parents),
+	_CLK_PARENT_SEL(SPI6, RCC_SPI6CKSELR, spi6_parents),
+	_CLK_PARENT_SEL(UART1, RCC_UART1CKSELR, usart1_parents),
+	_CLK_PARENT_SEL(RNG1, RCC_RNG1CKSELR, rng1_parents),
+	_CLK_PARENT_SEL(MPU, RCC_MPCKSELR, mpu_parents),
+	_CLK_PARENT_SEL(CKPER, RCC_CPERCKSELR, per_parents),
+	_CLK_PARENT_SEL(RTC, RCC_BDCR, rtc_parents),
+	_CLK_PARENT_SEL(UART6, RCC_UART6CKSELR, uart6_parents),
+	_CLK_PARENT_SEL(UART24, RCC_UART24CKSELR, uart234578_parents),
+	_CLK_PARENT_SEL(UART35, RCC_UART35CKSELR, uart234578_parents),
+	_CLK_PARENT_SEL(UART78, RCC_UART78CKSELR, uart234578_parents),
+	_CLK_PARENT_SEL(SDMMC12, RCC_SDMMC12CKSELR, sdmmc12_parents),
+	_CLK_PARENT_SEL(SDMMC3, RCC_SDMMC3CKSELR, sdmmc3_parents),
+	_CLK_PARENT_SEL(QSPI, RCC_QSPICKSELR, qspi_parents),
+	_CLK_PARENT_SEL(FMC, RCC_FMCCKSELR, fmc_parents),
+	_CLK_PARENT_SEL(AXIS, RCC_ASSCKSELR, axiss_parents),
+	_CLK_PARENT_SEL(MCUS, RCC_MSSCKSELR, mcuss_parents),
+	_CLK_PARENT_SEL(USBPHY, RCC_USBCKSELR, usbphy_parents),
+	_CLK_PARENT_SEL(USBO, RCC_USBCKSELR, usbo_parents),
+};
+
+/* Define characteristic of PLL according type */
+#define DIVN_MIN	24
+static const struct stm32mp1_pll stm32mp1_pll[PLL_TYPE_NB] = {
+	[PLL_800] = {
+		.refclk_min = 4,
+		.refclk_max = 16,
+	},
+	[PLL_1600] = {
+		.refclk_min = 8,
+		.refclk_max = 16,
+	},
+};
+
+/* PLLNCFGR2 register divider by output */
+static const uint8_t pllncfgr2[_DIV_NB] = {
+	[_DIV_P] = RCC_PLLNCFGR2_DIVP_SHIFT,
+	[_DIV_Q] = RCC_PLLNCFGR2_DIVQ_SHIFT,
+	[_DIV_R] = RCC_PLLNCFGR2_DIVR_SHIFT,
+};
+
+static const struct stm32mp1_clk_pll stm32mp1_clk_pll[_PLL_NB] = {
+	_CLK_PLL(_PLL1, PLL_1600,
+		 RCC_RCK12SELR, RCC_PLL1CFGR1, RCC_PLL1CFGR2,
+		 RCC_PLL1FRACR, RCC_PLL1CR, RCC_PLL1CSGR,
+		 _HSI, _HSE, _UNKNOWN_OSC_ID, _UNKNOWN_OSC_ID),
+	_CLK_PLL(_PLL2, PLL_1600,
+		 RCC_RCK12SELR, RCC_PLL2CFGR1, RCC_PLL2CFGR2,
+		 RCC_PLL2FRACR, RCC_PLL2CR, RCC_PLL2CSGR,
+		 _HSI, _HSE, _UNKNOWN_OSC_ID, _UNKNOWN_OSC_ID),
+	_CLK_PLL(_PLL3, PLL_800,
+		 RCC_RCK3SELR, RCC_PLL3CFGR1, RCC_PLL3CFGR2,
+		 RCC_PLL3FRACR, RCC_PLL3CR, RCC_PLL3CSGR,
+		 _HSI, _HSE, _CSI, _UNKNOWN_OSC_ID),
+	_CLK_PLL(_PLL4, PLL_800,
+		 RCC_RCK4SELR, RCC_PLL4CFGR1, RCC_PLL4CFGR2,
+		 RCC_PLL4FRACR, RCC_PLL4CR, RCC_PLL4CSGR,
+		 _HSI, _HSE, _CSI, _I2S_CKIN),
+};
+
+/* Prescaler table lookups for clock computation */
+/* div = /1 /2 /4 /8 / 16 /64 /128 /512 */
+static const uint8_t stm32mp1_mcu_div[16] = {
+	0, 1, 2, 3, 4, 6, 7, 8, 9, 9, 9, 9, 9, 9, 9, 9
+};
+
+/* div = /1 /2 /4 /8 /16 : same divider for PMU and APBX */
+#define stm32mp1_mpu_div stm32mp1_mpu_apbx_div
+#define stm32mp1_apbx_div stm32mp1_mpu_apbx_div
+static const uint8_t stm32mp1_mpu_apbx_div[8] = {
+	0, 1, 2, 3, 4, 4, 4, 4
+};
+
+/* div = /1 /2 /3 /4 */
+static const uint8_t stm32mp1_axi_div[8] = {
+	1, 2, 3, 4, 4, 4, 4, 4
+};
+
+static const char * const stm32mp1_clk_parent_name[_PARENT_NB] __unused = {
+	[_HSI] = "HSI",
+	[_HSE] = "HSE",
+	[_CSI] = "CSI",
+	[_LSI] = "LSI",
+	[_LSE] = "LSE",
+	[_I2S_CKIN] = "I2S_CKIN",
+	[_HSI_KER] = "HSI_KER",
+	[_HSE_KER] = "HSE_KER",
+	[_HSE_KER_DIV2] = "HSE_KER_DIV2",
+	[_HSE_RTC] = "HSE_RTC",
+	[_CSI_KER] = "CSI_KER",
+	[_PLL1_P] = "PLL1_P",
+	[_PLL1_Q] = "PLL1_Q",
+	[_PLL1_R] = "PLL1_R",
+	[_PLL2_P] = "PLL2_P",
+	[_PLL2_Q] = "PLL2_Q",
+	[_PLL2_R] = "PLL2_R",
+	[_PLL3_P] = "PLL3_P",
+	[_PLL3_Q] = "PLL3_Q",
+	[_PLL3_R] = "PLL3_R",
+	[_PLL4_P] = "PLL4_P",
+	[_PLL4_Q] = "PLL4_Q",
+	[_PLL4_R] = "PLL4_R",
+	[_ACLK] = "ACLK",
+	[_PCLK1] = "PCLK1",
+	[_PCLK2] = "PCLK2",
+	[_PCLK3] = "PCLK3",
+	[_PCLK4] = "PCLK4",
+	[_PCLK5] = "PCLK5",
+	[_HCLK6] = "KCLK6",
+	[_HCLK2] = "HCLK2",
+	[_CK_PER] = "CK_PER",
+	[_CK_MPU] = "CK_MPU",
+	[_CK_MCU] = "CK_MCU",
+	[_USB_PHY_48] = "USB_PHY_48",
+};
+
+/* RCC clock device driver private */
+static unsigned long stm32mp1_osc[NB_OSC];
+static struct spinlock reg_lock;
+static unsigned int gate_refcounts[NB_GATES];
+static struct spinlock refcount_lock;
+
+static const struct stm32mp1_clk_gate *gate_ref(unsigned int idx)
+{
+	return &stm32mp1_clk_gate[idx];
+}
+
+#if defined(IMAGE_BL32)
+static bool gate_is_non_secure(const struct stm32mp1_clk_gate *gate)
+{
+	return gate->secure == N_S;
+}
+#endif
+
+static const struct stm32mp1_clk_sel *clk_sel_ref(unsigned int idx)
+{
+	return &stm32mp1_clk_sel[idx];
+}
+
+static const struct stm32mp1_clk_pll *pll_ref(unsigned int idx)
+{
+	return &stm32mp1_clk_pll[idx];
+}
+
+static void stm32mp1_clk_lock(struct spinlock *lock)
+{
+	if (stm32mp_lock_available()) {
+		/* Assume interrupts are masked */
+		spin_lock(lock);
+	}
+}
+
+static void stm32mp1_clk_unlock(struct spinlock *lock)
+{
+	if (stm32mp_lock_available()) {
+		spin_unlock(lock);
+	}
+}
+
+bool stm32mp1_rcc_is_secure(void)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uint32_t mask = RCC_TZCR_TZEN;
+
+	return (mmio_read_32(rcc_base + RCC_TZCR) & mask) == mask;
+}
+
+bool stm32mp1_rcc_is_mckprot(void)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uint32_t mask = RCC_TZCR_TZEN | RCC_TZCR_MCKPROT;
+
+	return (mmio_read_32(rcc_base + RCC_TZCR) & mask) == mask;
+}
+
+void stm32mp1_clk_rcc_regs_lock(void)
+{
+	stm32mp1_clk_lock(&reg_lock);
+}
+
+void stm32mp1_clk_rcc_regs_unlock(void)
+{
+	stm32mp1_clk_unlock(&reg_lock);
+}
+
+static unsigned long stm32mp1_clk_get_fixed(enum stm32mp_osc_id idx)
+{
+	if (idx >= NB_OSC) {
+		return 0;
+	}
+
+	return stm32mp1_osc[idx];
+}
+
+static int stm32mp1_clk_get_gated_id(unsigned long id)
+{
+	unsigned int i;
+
+	for (i = 0U; i < NB_GATES; i++) {
+		if (gate_ref(i)->index == id) {
+			return i;
+		}
+	}
+
+	ERROR("%s: clk id %lu not found\n", __func__, id);
+
+	return -EINVAL;
+}
+
+static enum stm32mp1_parent_sel stm32mp1_clk_get_sel(int i)
+{
+	return (enum stm32mp1_parent_sel)(gate_ref(i)->sel);
+}
+
+static enum stm32mp1_parent_id stm32mp1_clk_get_fixed_parent(int i)
+{
+	return (enum stm32mp1_parent_id)(gate_ref(i)->fixed);
+}
+
+static int stm32mp1_clk_get_parent(unsigned long id)
+{
+	const struct stm32mp1_clk_sel *sel;
+	uint32_t p_sel;
+	int i;
+	enum stm32mp1_parent_id p;
+	enum stm32mp1_parent_sel s;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	/* Few non gateable clock have a static parent ID, find them */
+	i = (int)clock_id2parent_id(id);
+	if (i != _UNKNOWN_ID) {
+		return i;
+	}
+
+	i = stm32mp1_clk_get_gated_id(id);
+	if (i < 0) {
+		panic();
+	}
+
+	p = stm32mp1_clk_get_fixed_parent(i);
+	if (p < _PARENT_NB) {
+		return (int)p;
+	}
+
+	s = stm32mp1_clk_get_sel(i);
+	if (s == _UNKNOWN_SEL) {
+		return -EINVAL;
+	}
+	if (s >= _PARENT_SEL_NB) {
+		panic();
+	}
+
+	sel = clk_sel_ref(s);
+	p_sel = (mmio_read_32(rcc_base + sel->offset) &
+		 (sel->msk << sel->src)) >> sel->src;
+	if (p_sel < sel->nb_parent) {
+		return (int)sel->parent[p_sel];
+	}
+
+	return -EINVAL;
+}
+
+static unsigned long stm32mp1_pll_get_fref(const struct stm32mp1_clk_pll *pll)
+{
+	uint32_t selr = mmio_read_32(stm32mp_rcc_base() + pll->rckxselr);
+	uint32_t src = selr & RCC_SELR_REFCLK_SRC_MASK;
+
+	return stm32mp1_clk_get_fixed(pll->refclk[src]);
+}
+
+/*
+ * pll_get_fvco() : return the VCO or (VCO / 2) frequency for the requested PLL
+ * - PLL1 & PLL2 => return VCO / 2 with Fpll_y_ck = FVCO / 2 * (DIVy + 1)
+ * - PLL3 & PLL4 => return VCO     with Fpll_y_ck = FVCO / (DIVy + 1)
+ * => in all cases Fpll_y_ck = pll_get_fvco() / (DIVy + 1)
+ */
+static unsigned long stm32mp1_pll_get_fvco(const struct stm32mp1_clk_pll *pll)
+{
+	unsigned long refclk, fvco;
+	uint32_t cfgr1, fracr, divm, divn;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	cfgr1 = mmio_read_32(rcc_base + pll->pllxcfgr1);
+	fracr = mmio_read_32(rcc_base + pll->pllxfracr);
+
+	divm = (cfgr1 & (RCC_PLLNCFGR1_DIVM_MASK)) >> RCC_PLLNCFGR1_DIVM_SHIFT;
+	divn = cfgr1 & RCC_PLLNCFGR1_DIVN_MASK;
+
+	refclk = stm32mp1_pll_get_fref(pll);
+
+	/*
+	 * With FRACV :
+	 *   Fvco = Fck_ref * ((DIVN + 1) + FRACV / 2^13) / (DIVM + 1)
+	 * Without FRACV
+	 *   Fvco = Fck_ref * ((DIVN + 1) / (DIVM + 1)
+	 */
+	if ((fracr & RCC_PLLNFRACR_FRACLE) != 0U) {
+		uint32_t fracv = (fracr & RCC_PLLNFRACR_FRACV_MASK) >>
+				 RCC_PLLNFRACR_FRACV_SHIFT;
+		unsigned long long numerator, denominator;
+
+		numerator = (((unsigned long long)divn + 1U) << 13) + fracv;
+		numerator = refclk * numerator;
+		denominator = ((unsigned long long)divm + 1U) << 13;
+		fvco = (unsigned long)(numerator / denominator);
+	} else {
+		fvco = (unsigned long)(refclk * (divn + 1U) / (divm + 1U));
+	}
+
+	return fvco;
+}
+
+static unsigned long stm32mp1_read_pll_freq(enum stm32mp1_pll_id pll_id,
+					    enum stm32mp1_div_id div_id)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	unsigned long dfout;
+	uint32_t cfgr2, divy;
+
+	if (div_id >= _DIV_NB) {
+		return 0;
+	}
+
+	cfgr2 = mmio_read_32(stm32mp_rcc_base() + pll->pllxcfgr2);
+	divy = (cfgr2 >> pllncfgr2[div_id]) & RCC_PLLNCFGR2_DIVX_MASK;
+
+	dfout = stm32mp1_pll_get_fvco(pll) / (divy + 1U);
+
+	return dfout;
+}
+
+static unsigned long get_clock_rate(int p)
+{
+	uint32_t reg, clkdiv;
+	unsigned long clock = 0;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	switch (p) {
+	case _CK_MPU:
+	/* MPU sub system */
+		reg = mmio_read_32(rcc_base + RCC_MPCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_MPCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(_HSI);
+			break;
+		case RCC_MPCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(_HSE);
+			break;
+		case RCC_MPCKSELR_PLL:
+			clock = stm32mp1_read_pll_freq(_PLL1, _DIV_P);
+			break;
+		case RCC_MPCKSELR_PLL_MPUDIV:
+			clock = stm32mp1_read_pll_freq(_PLL1, _DIV_P);
+
+			reg = mmio_read_32(rcc_base + RCC_MPCKDIVR);
+			clkdiv = reg & RCC_MPUDIV_MASK;
+			clock >>= stm32mp1_mpu_div[clkdiv];
+			break;
+		default:
+			break;
+		}
+		break;
+	/* AXI sub system */
+	case _ACLK:
+	case _HCLK2:
+	case _HCLK6:
+	case _PCLK4:
+	case _PCLK5:
+		reg = mmio_read_32(rcc_base + RCC_ASSCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_ASSCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(_HSI);
+			break;
+		case RCC_ASSCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(_HSE);
+			break;
+		case RCC_ASSCKSELR_PLL:
+			clock = stm32mp1_read_pll_freq(_PLL2, _DIV_P);
+			break;
+		default:
+			break;
+		}
+
+		/* System clock divider */
+		reg = mmio_read_32(rcc_base + RCC_AXIDIVR);
+		clock /= stm32mp1_axi_div[reg & RCC_AXIDIV_MASK];
+
+		switch (p) {
+		case _PCLK4:
+			reg = mmio_read_32(rcc_base + RCC_APB4DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _PCLK5:
+			reg = mmio_read_32(rcc_base + RCC_APB5DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		default:
+			break;
+		}
+		break;
+	/* MCU sub system */
+	case _CK_MCU:
+	case _PCLK1:
+	case _PCLK2:
+	case _PCLK3:
+		reg = mmio_read_32(rcc_base + RCC_MSSCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_MSSCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(_HSI);
+			break;
+		case RCC_MSSCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(_HSE);
+			break;
+		case RCC_MSSCKSELR_CSI:
+			clock = stm32mp1_clk_get_fixed(_CSI);
+			break;
+		case RCC_MSSCKSELR_PLL:
+			clock = stm32mp1_read_pll_freq(_PLL3, _DIV_P);
+			break;
+		default:
+			break;
+		}
+
+		/* MCU clock divider */
+		reg = mmio_read_32(rcc_base + RCC_MCUDIVR);
+		clock >>= stm32mp1_mcu_div[reg & RCC_MCUDIV_MASK];
+
+		switch (p) {
+		case _PCLK1:
+			reg = mmio_read_32(rcc_base + RCC_APB1DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _PCLK2:
+			reg = mmio_read_32(rcc_base + RCC_APB2DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _PCLK3:
+			reg = mmio_read_32(rcc_base + RCC_APB3DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _CK_MCU:
+		default:
+			break;
+		}
+		break;
+	case _CK_PER:
+		reg = mmio_read_32(rcc_base + RCC_CPERCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_CPERCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(_HSI);
+			break;
+		case RCC_CPERCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(_HSE);
+			break;
+		case RCC_CPERCKSELR_CSI:
+			clock = stm32mp1_clk_get_fixed(_CSI);
+			break;
+		default:
+			break;
+		}
+		break;
+	case _HSI:
+	case _HSI_KER:
+		clock = stm32mp1_clk_get_fixed(_HSI);
+		break;
+	case _CSI:
+	case _CSI_KER:
+		clock = stm32mp1_clk_get_fixed(_CSI);
+		break;
+	case _HSE:
+	case _HSE_KER:
+		clock = stm32mp1_clk_get_fixed(_HSE);
+		break;
+	case _HSE_KER_DIV2:
+		clock = stm32mp1_clk_get_fixed(_HSE) >> 1;
+		break;
+	case _HSE_RTC:
+		clock = stm32mp1_clk_get_fixed(_HSE);
+		clock /= (mmio_read_32(rcc_base + RCC_RTCDIVR) & RCC_DIVR_DIV_MASK) + 1U;
+		break;
+	case _LSI:
+		clock = stm32mp1_clk_get_fixed(_LSI);
+		break;
+	case _LSE:
+		clock = stm32mp1_clk_get_fixed(_LSE);
+		break;
+	/* PLL */
+	case _PLL1_P:
+		clock = stm32mp1_read_pll_freq(_PLL1, _DIV_P);
+		break;
+	case _PLL1_Q:
+		clock = stm32mp1_read_pll_freq(_PLL1, _DIV_Q);
+		break;
+	case _PLL1_R:
+		clock = stm32mp1_read_pll_freq(_PLL1, _DIV_R);
+		break;
+	case _PLL2_P:
+		clock = stm32mp1_read_pll_freq(_PLL2, _DIV_P);
+		break;
+	case _PLL2_Q:
+		clock = stm32mp1_read_pll_freq(_PLL2, _DIV_Q);
+		break;
+	case _PLL2_R:
+		clock = stm32mp1_read_pll_freq(_PLL2, _DIV_R);
+		break;
+	case _PLL3_P:
+		clock = stm32mp1_read_pll_freq(_PLL3, _DIV_P);
+		break;
+	case _PLL3_Q:
+		clock = stm32mp1_read_pll_freq(_PLL3, _DIV_Q);
+		break;
+	case _PLL3_R:
+		clock = stm32mp1_read_pll_freq(_PLL3, _DIV_R);
+		break;
+	case _PLL4_P:
+		clock = stm32mp1_read_pll_freq(_PLL4, _DIV_P);
+		break;
+	case _PLL4_Q:
+		clock = stm32mp1_read_pll_freq(_PLL4, _DIV_Q);
+		break;
+	case _PLL4_R:
+		clock = stm32mp1_read_pll_freq(_PLL4, _DIV_R);
+		break;
+	/* Other */
+	case _USB_PHY_48:
+		clock = USB_PHY_48_MHZ;
+		break;
+	default:
+		break;
+	}
+
+	return clock;
+}
+
+static void __clk_enable(struct stm32mp1_clk_gate const *gate)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	VERBOSE("Enable clock %u\n", gate->index);
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(rcc_base + gate->offset, BIT(gate->bit));
+	} else {
+		mmio_setbits_32(rcc_base + gate->offset, BIT(gate->bit));
+	}
+}
+
+static void __clk_disable(struct stm32mp1_clk_gate const *gate)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	VERBOSE("Disable clock %u\n", gate->index);
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(rcc_base + gate->offset + RCC_MP_ENCLRR_OFFSET,
+			      BIT(gate->bit));
+	} else {
+		mmio_clrbits_32(rcc_base + gate->offset, BIT(gate->bit));
+	}
+}
+
+static bool __clk_is_enabled(struct stm32mp1_clk_gate const *gate)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	return mmio_read_32(rcc_base + gate->offset) & BIT(gate->bit);
+}
+
+/* Oscillators and PLLs are not gated at runtime */
+static bool clock_is_always_on(unsigned long id)
+{
+	switch (id) {
+	case CK_HSE:
+	case CK_CSI:
+	case CK_LSI:
+	case CK_LSE:
+	case CK_HSI:
+	case CK_HSE_DIV2:
+	case PLL1_Q:
+	case PLL1_R:
+	case PLL2_P:
+	case PLL2_Q:
+	case PLL2_R:
+	case PLL3_P:
+	case PLL3_Q:
+	case PLL3_R:
+	case CK_AXI:
+	case CK_MPU:
+	case CK_MCU:
+	case RTC:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static void __stm32mp1_clk_enable(unsigned long id, bool with_refcnt)
+{
+	const struct stm32mp1_clk_gate *gate;
+	int i;
+
+	if (clock_is_always_on(id)) {
+		return;
+	}
+
+	i = stm32mp1_clk_get_gated_id(id);
+	if (i < 0) {
+		ERROR("Clock %lu can't be enabled\n", id);
+		panic();
+	}
+
+	gate = gate_ref(i);
+
+	if (!with_refcnt) {
+		__clk_enable(gate);
+		return;
+	}
+
+#if defined(IMAGE_BL32)
+	if (gate_is_non_secure(gate)) {
+		/* Enable non-secure clock w/o any refcounting */
+		__clk_enable(gate);
+		return;
+	}
+#endif
+
+	stm32mp1_clk_lock(&refcount_lock);
+
+	if (gate_refcounts[i] == 0U) {
+		__clk_enable(gate);
+	}
+
+	gate_refcounts[i]++;
+	if (gate_refcounts[i] == UINT_MAX) {
+		ERROR("Clock %lu refcount reached max value\n", id);
+		panic();
+	}
+
+	stm32mp1_clk_unlock(&refcount_lock);
+}
+
+static void __stm32mp1_clk_disable(unsigned long id, bool with_refcnt)
+{
+	const struct stm32mp1_clk_gate *gate;
+	int i;
+
+	if (clock_is_always_on(id)) {
+		return;
+	}
+
+	i = stm32mp1_clk_get_gated_id(id);
+	if (i < 0) {
+		ERROR("Clock %lu can't be disabled\n", id);
+		panic();
+	}
+
+	gate = gate_ref(i);
+
+	if (!with_refcnt) {
+		__clk_disable(gate);
+		return;
+	}
+
+#if defined(IMAGE_BL32)
+	if (gate_is_non_secure(gate)) {
+		/* Don't disable non-secure clocks */
+		return;
+	}
+#endif
+
+	stm32mp1_clk_lock(&refcount_lock);
+
+	if (gate_refcounts[i] == 0U) {
+		ERROR("Clock %lu refcount reached 0\n", id);
+		panic();
+	}
+	gate_refcounts[i]--;
+
+	if (gate_refcounts[i] == 0U) {
+		__clk_disable(gate);
+	}
+
+	stm32mp1_clk_unlock(&refcount_lock);
+}
+
+static int stm32mp_clk_enable(unsigned long id)
+{
+	__stm32mp1_clk_enable(id, true);
+
+	return 0;
+}
+
+static void stm32mp_clk_disable(unsigned long id)
+{
+	__stm32mp1_clk_disable(id, true);
+}
+
+static bool stm32mp_clk_is_enabled(unsigned long id)
+{
+	int i;
+
+	if (clock_is_always_on(id)) {
+		return true;
+	}
+
+	i = stm32mp1_clk_get_gated_id(id);
+	if (i < 0) {
+		panic();
+	}
+
+	return __clk_is_enabled(gate_ref(i));
+}
+
+static unsigned long stm32mp_clk_get_rate(unsigned long id)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	int p = stm32mp1_clk_get_parent(id);
+	uint32_t prescaler, timpre;
+	unsigned long parent_rate;
+
+	if (p < 0) {
+		return 0;
+	}
+
+	parent_rate = get_clock_rate(p);
+
+	switch (id) {
+	case TIM2_K:
+	case TIM3_K:
+	case TIM4_K:
+	case TIM5_K:
+	case TIM6_K:
+	case TIM7_K:
+	case TIM12_K:
+	case TIM13_K:
+	case TIM14_K:
+		prescaler = mmio_read_32(rcc_base + RCC_APB1DIVR) &
+			    RCC_APBXDIV_MASK;
+		timpre = mmio_read_32(rcc_base + RCC_TIMG1PRER) &
+			 RCC_TIMGXPRER_TIMGXPRE;
+		break;
+
+	case TIM1_K:
+	case TIM8_K:
+	case TIM15_K:
+	case TIM16_K:
+	case TIM17_K:
+		prescaler = mmio_read_32(rcc_base + RCC_APB2DIVR) &
+			    RCC_APBXDIV_MASK;
+		timpre = mmio_read_32(rcc_base + RCC_TIMG2PRER) &
+			 RCC_TIMGXPRER_TIMGXPRE;
+		break;
+
+	default:
+		return parent_rate;
+	}
+
+	if (prescaler == 0U) {
+		return parent_rate;
+	}
+
+	return parent_rate * (timpre + 1U) * 2U;
+}
+
+static void stm32mp1_ls_osc_set(bool enable, uint32_t offset, uint32_t mask_on)
+{
+	uintptr_t address = stm32mp_rcc_base() + offset;
+
+	if (enable) {
+		mmio_setbits_32(address, mask_on);
+	} else {
+		mmio_clrbits_32(address, mask_on);
+	}
+}
+
+static void stm32mp1_hs_ocs_set(bool enable, uint32_t mask_on)
+{
+	uint32_t offset = enable ? RCC_OCENSETR : RCC_OCENCLRR;
+	uintptr_t address = stm32mp_rcc_base() + offset;
+
+	mmio_write_32(address, mask_on);
+}
+
+static int stm32mp1_osc_wait(bool enable, uint32_t offset, uint32_t mask_rdy)
+{
+	uint64_t timeout;
+	uint32_t mask_test;
+	uintptr_t address = stm32mp_rcc_base() + offset;
+
+	if (enable) {
+		mask_test = mask_rdy;
+	} else {
+		mask_test = 0;
+	}
+
+	timeout = timeout_init_us(OSCRDY_TIMEOUT);
+	while ((mmio_read_32(address) & mask_rdy) != mask_test) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("OSC %x @ %lx timeout for enable=%d : 0x%x\n",
+			      mask_rdy, address, enable, mmio_read_32(address));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static void stm32mp1_lse_enable(bool bypass, bool digbyp, uint32_t lsedrv)
+{
+	uint32_t value;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	if (digbyp) {
+		mmio_setbits_32(rcc_base + RCC_BDCR, RCC_BDCR_DIGBYP);
+	}
+
+	if (bypass || digbyp) {
+		mmio_setbits_32(rcc_base + RCC_BDCR, RCC_BDCR_LSEBYP);
+	}
+
+	/*
+	 * Warning: not recommended to switch directly from "high drive"
+	 * to "medium low drive", and vice-versa.
+	 */
+	value = (mmio_read_32(rcc_base + RCC_BDCR) & RCC_BDCR_LSEDRV_MASK) >>
+		RCC_BDCR_LSEDRV_SHIFT;
+
+	while (value != lsedrv) {
+		if (value > lsedrv) {
+			value--;
+		} else {
+			value++;
+		}
+
+		mmio_clrsetbits_32(rcc_base + RCC_BDCR,
+				   RCC_BDCR_LSEDRV_MASK,
+				   value << RCC_BDCR_LSEDRV_SHIFT);
+	}
+
+	stm32mp1_ls_osc_set(true, RCC_BDCR, RCC_BDCR_LSEON);
+}
+
+static void stm32mp1_lse_wait(void)
+{
+	if (stm32mp1_osc_wait(true, RCC_BDCR, RCC_BDCR_LSERDY) != 0) {
+		VERBOSE("%s: failed\n", __func__);
+	}
+}
+
+static void stm32mp1_lsi_set(bool enable)
+{
+	stm32mp1_ls_osc_set(enable, RCC_RDLSICR, RCC_RDLSICR_LSION);
+
+	if (stm32mp1_osc_wait(enable, RCC_RDLSICR, RCC_RDLSICR_LSIRDY) != 0) {
+		VERBOSE("%s: failed\n", __func__);
+	}
+}
+
+static void stm32mp1_hse_enable(bool bypass, bool digbyp, bool css)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	if (digbyp) {
+		mmio_write_32(rcc_base + RCC_OCENSETR, RCC_OCENR_DIGBYP);
+	}
+
+	if (bypass || digbyp) {
+		mmio_write_32(rcc_base + RCC_OCENSETR, RCC_OCENR_HSEBYP);
+	}
+
+	stm32mp1_hs_ocs_set(true, RCC_OCENR_HSEON);
+	if (stm32mp1_osc_wait(true, RCC_OCRDYR, RCC_OCRDYR_HSERDY) != 0) {
+		VERBOSE("%s: failed\n", __func__);
+	}
+
+	if (css) {
+		mmio_write_32(rcc_base + RCC_OCENSETR, RCC_OCENR_HSECSSON);
+	}
+
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+	if ((mmio_read_32(rcc_base + RCC_OCENSETR) & RCC_OCENR_HSEBYP) &&
+	    (!(digbyp || bypass))) {
+		panic();
+	}
+#endif
+}
+
+static void stm32mp1_csi_set(bool enable)
+{
+	stm32mp1_hs_ocs_set(enable, RCC_OCENR_CSION);
+	if (stm32mp1_osc_wait(enable, RCC_OCRDYR, RCC_OCRDYR_CSIRDY) != 0) {
+		VERBOSE("%s: failed\n", __func__);
+	}
+}
+
+static void stm32mp1_hsi_set(bool enable)
+{
+	stm32mp1_hs_ocs_set(enable, RCC_OCENR_HSION);
+	if (stm32mp1_osc_wait(enable, RCC_OCRDYR, RCC_OCRDYR_HSIRDY) != 0) {
+		VERBOSE("%s: failed\n", __func__);
+	}
+}
+
+static int stm32mp1_set_hsidiv(uint8_t hsidiv)
+{
+	uint64_t timeout;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uintptr_t address = rcc_base + RCC_OCRDYR;
+
+	mmio_clrsetbits_32(rcc_base + RCC_HSICFGR,
+			   RCC_HSICFGR_HSIDIV_MASK,
+			   RCC_HSICFGR_HSIDIV_MASK & (uint32_t)hsidiv);
+
+	timeout = timeout_init_us(HSIDIV_TIMEOUT);
+	while ((mmio_read_32(address) & RCC_OCRDYR_HSIDIVRDY) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("HSIDIV failed @ 0x%lx: 0x%x\n",
+			      address, mmio_read_32(address));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32mp1_hsidiv(unsigned long hsifreq)
+{
+	uint8_t hsidiv;
+	uint32_t hsidivfreq = MAX_HSI_HZ;
+
+	for (hsidiv = 0; hsidiv < 4U; hsidiv++) {
+		if (hsidivfreq == hsifreq) {
+			break;
+		}
+
+		hsidivfreq /= 2U;
+	}
+
+	if (hsidiv == 4U) {
+		ERROR("Invalid clk-hsi frequency\n");
+		return -1;
+	}
+
+	if (hsidiv != 0U) {
+		return stm32mp1_set_hsidiv(hsidiv);
+	}
+
+	return 0;
+}
+
+static bool stm32mp1_check_pll_conf(enum stm32mp1_pll_id pll_id,
+				    unsigned int clksrc,
+				    uint32_t *pllcfg, int plloff)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uintptr_t pllxcr = rcc_base + pll->pllxcr;
+	enum stm32mp1_plltype type = pll->plltype;
+	uintptr_t clksrc_address = rcc_base + (clksrc >> 4);
+	unsigned long refclk;
+	uint32_t ifrge = 0U;
+	uint32_t src, value, fracv = 0;
+	void *fdt;
+
+	/* Check PLL output */
+	if (mmio_read_32(pllxcr) != RCC_PLLNCR_PLLON) {
+		return false;
+	}
+
+	/* Check current clksrc */
+	src = mmio_read_32(clksrc_address) & RCC_SELR_SRC_MASK;
+	if (src != (clksrc & RCC_SELR_SRC_MASK)) {
+		return false;
+	}
+
+	/* Check Div */
+	src = mmio_read_32(rcc_base + pll->rckxselr) & RCC_SELR_REFCLK_SRC_MASK;
+
+	refclk = stm32mp1_clk_get_fixed(pll->refclk[src]) /
+		 (pllcfg[PLLCFG_M] + 1U);
+
+	if ((refclk < (stm32mp1_pll[type].refclk_min * 1000000U)) ||
+	    (refclk > (stm32mp1_pll[type].refclk_max * 1000000U))) {
+		return false;
+	}
+
+	if ((type == PLL_800) && (refclk >= 8000000U)) {
+		ifrge = 1U;
+	}
+
+	value = (pllcfg[PLLCFG_N] << RCC_PLLNCFGR1_DIVN_SHIFT) &
+		RCC_PLLNCFGR1_DIVN_MASK;
+	value |= (pllcfg[PLLCFG_M] << RCC_PLLNCFGR1_DIVM_SHIFT) &
+		 RCC_PLLNCFGR1_DIVM_MASK;
+	value |= (ifrge << RCC_PLLNCFGR1_IFRGE_SHIFT) &
+		 RCC_PLLNCFGR1_IFRGE_MASK;
+	if (mmio_read_32(rcc_base + pll->pllxcfgr1) != value) {
+		return false;
+	}
+
+	/* Fractional configuration */
+	if (fdt_get_address(&fdt) == 1) {
+		fracv = fdt_read_uint32_default(fdt, plloff, "frac", 0);
+	}
+
+	value = fracv << RCC_PLLNFRACR_FRACV_SHIFT;
+	value |= RCC_PLLNFRACR_FRACLE;
+	if (mmio_read_32(rcc_base + pll->pllxfracr) != value) {
+		return false;
+	}
+
+	/* Output config */
+	value = (pllcfg[PLLCFG_P] << RCC_PLLNCFGR2_DIVP_SHIFT) &
+		RCC_PLLNCFGR2_DIVP_MASK;
+	value |= (pllcfg[PLLCFG_Q] << RCC_PLLNCFGR2_DIVQ_SHIFT) &
+		 RCC_PLLNCFGR2_DIVQ_MASK;
+	value |= (pllcfg[PLLCFG_R] << RCC_PLLNCFGR2_DIVR_SHIFT) &
+		 RCC_PLLNCFGR2_DIVR_MASK;
+	if (mmio_read_32(rcc_base + pll->pllxcfgr2) != value) {
+		return false;
+	}
+
+	return true;
+}
+
+static void stm32mp1_pll_start(enum stm32mp1_pll_id pll_id)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t pllxcr = stm32mp_rcc_base() + pll->pllxcr;
+
+	/* Preserve RCC_PLLNCR_SSCG_CTRL value */
+	mmio_clrsetbits_32(pllxcr,
+			   RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN |
+			   RCC_PLLNCR_DIVREN,
+			   RCC_PLLNCR_PLLON);
+}
+
+static int stm32mp1_pll_output(enum stm32mp1_pll_id pll_id, uint32_t output)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t pllxcr = stm32mp_rcc_base() + pll->pllxcr;
+	uint64_t timeout = timeout_init_us(PLLRDY_TIMEOUT);
+
+	/* Wait PLL lock */
+	while ((mmio_read_32(pllxcr) & RCC_PLLNCR_PLLRDY) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("PLL%u start failed @ 0x%lx: 0x%x\n",
+			      pll_id, pllxcr, mmio_read_32(pllxcr));
+			return -ETIMEDOUT;
+		}
+	}
+
+	/* Start the requested output */
+	mmio_setbits_32(pllxcr, output << RCC_PLLNCR_DIVEN_SHIFT);
+
+	return 0;
+}
+
+static int stm32mp1_pll_stop(enum stm32mp1_pll_id pll_id)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t pllxcr = stm32mp_rcc_base() + pll->pllxcr;
+	uint64_t timeout;
+
+	/* Stop all output */
+	mmio_clrbits_32(pllxcr, RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN |
+			RCC_PLLNCR_DIVREN);
+
+	/* Stop PLL */
+	mmio_clrbits_32(pllxcr, RCC_PLLNCR_PLLON);
+
+	timeout = timeout_init_us(PLLRDY_TIMEOUT);
+	/* Wait PLL stopped */
+	while ((mmio_read_32(pllxcr) & RCC_PLLNCR_PLLRDY) != 0U) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("PLL%u stop failed @ 0x%lx: 0x%x\n",
+			      pll_id, pllxcr, mmio_read_32(pllxcr));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static void stm32mp1_pll_config_output(enum stm32mp1_pll_id pll_id,
+				       uint32_t *pllcfg)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uint32_t value;
+
+	value = (pllcfg[PLLCFG_P] << RCC_PLLNCFGR2_DIVP_SHIFT) &
+		RCC_PLLNCFGR2_DIVP_MASK;
+	value |= (pllcfg[PLLCFG_Q] << RCC_PLLNCFGR2_DIVQ_SHIFT) &
+		 RCC_PLLNCFGR2_DIVQ_MASK;
+	value |= (pllcfg[PLLCFG_R] << RCC_PLLNCFGR2_DIVR_SHIFT) &
+		 RCC_PLLNCFGR2_DIVR_MASK;
+	mmio_write_32(rcc_base + pll->pllxcfgr2, value);
+}
+
+static int stm32mp1_pll_config(enum stm32mp1_pll_id pll_id,
+			       uint32_t *pllcfg, uint32_t fracv)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	enum stm32mp1_plltype type = pll->plltype;
+	unsigned long refclk;
+	uint32_t ifrge = 0;
+	uint32_t src, value;
+
+	src = mmio_read_32(rcc_base + pll->rckxselr) &
+		RCC_SELR_REFCLK_SRC_MASK;
+
+	refclk = stm32mp1_clk_get_fixed(pll->refclk[src]) /
+		 (pllcfg[PLLCFG_M] + 1U);
+
+	if ((refclk < (stm32mp1_pll[type].refclk_min * 1000000U)) ||
+	    (refclk > (stm32mp1_pll[type].refclk_max * 1000000U))) {
+		return -EINVAL;
+	}
+
+	if ((type == PLL_800) && (refclk >= 8000000U)) {
+		ifrge = 1U;
+	}
+
+	value = (pllcfg[PLLCFG_N] << RCC_PLLNCFGR1_DIVN_SHIFT) &
+		RCC_PLLNCFGR1_DIVN_MASK;
+	value |= (pllcfg[PLLCFG_M] << RCC_PLLNCFGR1_DIVM_SHIFT) &
+		 RCC_PLLNCFGR1_DIVM_MASK;
+	value |= (ifrge << RCC_PLLNCFGR1_IFRGE_SHIFT) &
+		 RCC_PLLNCFGR1_IFRGE_MASK;
+	mmio_write_32(rcc_base + pll->pllxcfgr1, value);
+
+	/* Fractional configuration */
+	value = 0;
+	mmio_write_32(rcc_base + pll->pllxfracr, value);
+
+	value = fracv << RCC_PLLNFRACR_FRACV_SHIFT;
+	mmio_write_32(rcc_base + pll->pllxfracr, value);
+
+	value |= RCC_PLLNFRACR_FRACLE;
+	mmio_write_32(rcc_base + pll->pllxfracr, value);
+
+	stm32mp1_pll_config_output(pll_id, pllcfg);
+
+	return 0;
+}
+
+static void stm32mp1_pll_csg(enum stm32mp1_pll_id pll_id, uint32_t *csg)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uint32_t pllxcsg = 0;
+
+	pllxcsg |= (csg[PLLCSG_MOD_PER] << RCC_PLLNCSGR_MOD_PER_SHIFT) &
+		    RCC_PLLNCSGR_MOD_PER_MASK;
+
+	pllxcsg |= (csg[PLLCSG_INC_STEP] << RCC_PLLNCSGR_INC_STEP_SHIFT) &
+		    RCC_PLLNCSGR_INC_STEP_MASK;
+
+	pllxcsg |= (csg[PLLCSG_SSCG_MODE] << RCC_PLLNCSGR_SSCG_MODE_SHIFT) &
+		    RCC_PLLNCSGR_SSCG_MODE_MASK;
+
+	mmio_write_32(stm32mp_rcc_base() + pll->pllxcsgr, pllxcsg);
+
+	mmio_setbits_32(stm32mp_rcc_base() + pll->pllxcr,
+			RCC_PLLNCR_SSCG_CTRL);
+}
+
+static int stm32mp1_set_clksrc(unsigned int clksrc)
+{
+	uintptr_t clksrc_address = stm32mp_rcc_base() + (clksrc >> 4);
+	uint64_t timeout;
+
+	mmio_clrsetbits_32(clksrc_address, RCC_SELR_SRC_MASK,
+			   clksrc & RCC_SELR_SRC_MASK);
+
+	timeout = timeout_init_us(CLKSRC_TIMEOUT);
+	while ((mmio_read_32(clksrc_address) & RCC_SELR_SRCRDY) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("CLKSRC %x start failed @ 0x%lx: 0x%x\n", clksrc,
+			      clksrc_address, mmio_read_32(clksrc_address));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32mp1_set_clkdiv(unsigned int clkdiv, uintptr_t address)
+{
+	uint64_t timeout;
+
+	mmio_clrsetbits_32(address, RCC_DIVR_DIV_MASK,
+			   clkdiv & RCC_DIVR_DIV_MASK);
+
+	timeout = timeout_init_us(CLKDIV_TIMEOUT);
+	while ((mmio_read_32(address) & RCC_DIVR_DIVRDY) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("CLKDIV %x start failed @ 0x%lx: 0x%x\n",
+			      clkdiv, address, mmio_read_32(address));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static void stm32mp1_mco_csg(uint32_t clksrc, uint32_t clkdiv)
+{
+	uintptr_t clksrc_address = stm32mp_rcc_base() + (clksrc >> 4);
+
+	/*
+	 * Binding clksrc :
+	 *      bit15-4 offset
+	 *      bit3:   disable
+	 *      bit2-0: MCOSEL[2:0]
+	 */
+	if ((clksrc & 0x8U) != 0U) {
+		mmio_clrbits_32(clksrc_address, RCC_MCOCFG_MCOON);
+	} else {
+		mmio_clrsetbits_32(clksrc_address,
+				   RCC_MCOCFG_MCOSRC_MASK,
+				   clksrc & RCC_MCOCFG_MCOSRC_MASK);
+		mmio_clrsetbits_32(clksrc_address,
+				   RCC_MCOCFG_MCODIV_MASK,
+				   clkdiv << RCC_MCOCFG_MCODIV_SHIFT);
+		mmio_setbits_32(clksrc_address, RCC_MCOCFG_MCOON);
+	}
+}
+
+static void stm32mp1_set_rtcsrc(unsigned int clksrc, bool lse_css)
+{
+	uintptr_t address = stm32mp_rcc_base() + RCC_BDCR;
+
+	if (((mmio_read_32(address) & RCC_BDCR_RTCCKEN) == 0U) ||
+	    (clksrc != (uint32_t)CLK_RTC_DISABLED)) {
+		mmio_clrsetbits_32(address,
+				   RCC_BDCR_RTCSRC_MASK,
+				   (clksrc & RCC_SELR_SRC_MASK) << RCC_BDCR_RTCSRC_SHIFT);
+
+		mmio_setbits_32(address, RCC_BDCR_RTCCKEN);
+	}
+
+	if (lse_css) {
+		mmio_setbits_32(address, RCC_BDCR_LSECSSON);
+	}
+}
+
+static void stm32mp1_pkcs_config(uint32_t pkcs)
+{
+	uintptr_t address = stm32mp_rcc_base() + ((pkcs >> 4) & 0xFFFU);
+	uint32_t value = pkcs & 0xFU;
+	uint32_t mask = 0xFU;
+
+	if ((pkcs & BIT(31)) != 0U) {
+		mask <<= 4;
+		value <<= 4;
+	}
+
+	mmio_clrsetbits_32(address, mask, value);
+}
+
+static int clk_get_pll_settings_from_dt(int plloff, unsigned int *pllcfg,
+					uint32_t *fracv, uint32_t *csg,
+					bool *csg_set)
+{
+	void *fdt;
+	int ret;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	ret = fdt_read_uint32_array(fdt, plloff, "cfg", (uint32_t)PLLCFG_NB,
+				    pllcfg);
+	if (ret < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	*fracv = fdt_read_uint32_default(fdt, plloff, "frac", 0);
+
+	ret = fdt_read_uint32_array(fdt, plloff, "csg", (uint32_t)PLLCSG_NB,
+				    csg);
+
+	*csg_set = (ret == 0);
+
+	if (ret == -FDT_ERR_NOTFOUND) {
+		ret = 0;
+	}
+
+	return ret;
+}
+
+int stm32mp1_clk_init(void)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uint32_t pllfracv[_PLL_NB];
+	uint32_t pllcsg[_PLL_NB][PLLCSG_NB];
+	unsigned int clksrc[CLKSRC_NB];
+	unsigned int clkdiv[CLKDIV_NB];
+	unsigned int pllcfg[_PLL_NB][PLLCFG_NB];
+	int plloff[_PLL_NB];
+	int ret, len;
+	enum stm32mp1_pll_id i;
+	bool pllcsg_set[_PLL_NB];
+	bool pllcfg_valid[_PLL_NB];
+	bool lse_css = false;
+	bool pll3_preserve = false;
+	bool pll4_preserve = false;
+	bool pll4_bootrom = false;
+	const fdt32_t *pkcs_cell;
+	void *fdt;
+	int stgen_p = stm32mp1_clk_get_parent(STGEN_K);
+	int usbphy_p = stm32mp1_clk_get_parent(USBPHY_K);
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	ret = fdt_rcc_read_uint32_array("st,clksrc", (uint32_t)CLKSRC_NB,
+					clksrc);
+	if (ret < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	ret = fdt_rcc_read_uint32_array("st,clkdiv", (uint32_t)CLKDIV_NB,
+					clkdiv);
+	if (ret < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		char name[12];
+
+		snprintf(name, sizeof(name), "st,pll@%u", i);
+		plloff[i] = fdt_rcc_subnode_offset(name);
+
+		pllcfg_valid[i] = fdt_check_node(plloff[i]);
+		if (!pllcfg_valid[i]) {
+			continue;
+		}
+
+		ret = clk_get_pll_settings_from_dt(plloff[i], pllcfg[i],
+						   &pllfracv[i], pllcsg[i],
+						   &pllcsg_set[i]);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	stm32mp1_mco_csg(clksrc[CLKSRC_MCO1], clkdiv[CLKDIV_MCO1]);
+	stm32mp1_mco_csg(clksrc[CLKSRC_MCO2], clkdiv[CLKDIV_MCO2]);
+
+	/*
+	 * Switch ON oscillator found in device-tree.
+	 * Note: HSI already ON after BootROM stage.
+	 */
+	if (stm32mp1_osc[_LSI] != 0U) {
+		stm32mp1_lsi_set(true);
+	}
+	if (stm32mp1_osc[_LSE] != 0U) {
+		const char *name = stm32mp_osc_node_label[_LSE];
+		bool bypass, digbyp;
+		uint32_t lsedrv;
+
+		bypass = fdt_clk_read_bool(name, "st,bypass");
+		digbyp = fdt_clk_read_bool(name, "st,digbypass");
+		lse_css = fdt_clk_read_bool(name, "st,css");
+		lsedrv = fdt_clk_read_uint32_default(name, "st,drive",
+						     LSEDRV_MEDIUM_HIGH);
+		stm32mp1_lse_enable(bypass, digbyp, lsedrv);
+	}
+	if (stm32mp1_osc[_HSE] != 0U) {
+		const char *name = stm32mp_osc_node_label[_HSE];
+		bool bypass, digbyp, css;
+
+		bypass = fdt_clk_read_bool(name, "st,bypass");
+		digbyp = fdt_clk_read_bool(name, "st,digbypass");
+		css = fdt_clk_read_bool(name, "st,css");
+		stm32mp1_hse_enable(bypass, digbyp, css);
+	}
+	/*
+	 * CSI is mandatory for automatic I/O compensation (SYSCFG_CMPCR)
+	 * => switch on CSI even if node is not present in device tree
+	 */
+	stm32mp1_csi_set(true);
+
+	/* Come back to HSI */
+	ret = stm32mp1_set_clksrc(CLK_MPU_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clksrc(CLK_AXI_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clksrc(CLK_MCU_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((mmio_read_32(rcc_base + RCC_MP_RSTSCLRR) &
+	     RCC_MP_RSTSCLRR_MPUP0RSTF) != 0) {
+		if (pllcfg_valid[_PLL3]) {
+			pll3_preserve =
+				stm32mp1_check_pll_conf(_PLL3,
+							clksrc[CLKSRC_PLL3],
+							pllcfg[_PLL3],
+							plloff[_PLL3]);
+		}
+
+		if (pllcfg_valid[_PLL4]) {
+			pll4_preserve =
+				stm32mp1_check_pll_conf(_PLL4,
+							clksrc[CLKSRC_PLL4],
+							pllcfg[_PLL4],
+							plloff[_PLL4]);
+		}
+	}
+	/* Don't initialize PLL4, when used by BOOTROM */
+	if ((stm32mp_get_boot_itf_selected() ==
+	     BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB) &&
+	    ((stgen_p == (int)_PLL4_R) || (usbphy_p == (int)_PLL4_R))) {
+		pll4_bootrom = true;
+		pll4_preserve = true;
+	}
+
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		if (((i == _PLL3) && pll3_preserve) ||
+		    ((i == _PLL4) && pll4_preserve)) {
+			continue;
+		}
+
+		ret = stm32mp1_pll_stop(i);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	/* Configure HSIDIV */
+	if (stm32mp1_osc[_HSI] != 0U) {
+		ret = stm32mp1_hsidiv(stm32mp1_osc[_HSI]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		stm32mp_stgen_config(stm32mp_clk_get_rate(STGEN_K));
+	}
+
+	/* Select DIV */
+	/* No ready bit when MPUSRC != CLK_MPU_PLL1P_DIV, MPUDIV is disabled */
+	mmio_write_32(rcc_base + RCC_MPCKDIVR,
+		      clkdiv[CLKDIV_MPU] & RCC_DIVR_DIV_MASK);
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_AXI], rcc_base + RCC_AXIDIVR);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB4], rcc_base + RCC_APB4DIVR);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB5], rcc_base + RCC_APB5DIVR);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_MCU], rcc_base + RCC_MCUDIVR);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB1], rcc_base + RCC_APB1DIVR);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB2], rcc_base + RCC_APB2DIVR);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB3], rcc_base + RCC_APB3DIVR);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* No ready bit for RTC */
+	mmio_write_32(rcc_base + RCC_RTCDIVR,
+		      clkdiv[CLKDIV_RTC] & RCC_DIVR_DIV_MASK);
+
+	/* Configure PLLs source */
+	ret = stm32mp1_set_clksrc(clksrc[CLKSRC_PLL12]);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (!pll3_preserve) {
+		ret = stm32mp1_set_clksrc(clksrc[CLKSRC_PLL3]);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	if (!pll4_preserve) {
+		ret = stm32mp1_set_clksrc(clksrc[CLKSRC_PLL4]);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	/* Configure and start PLLs */
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		if (((i == _PLL3) && pll3_preserve) ||
+		    ((i == _PLL4) && pll4_preserve && !pll4_bootrom)) {
+			continue;
+		}
+
+		if (!pllcfg_valid[i]) {
+			continue;
+		}
+
+		if ((i == _PLL4) && pll4_bootrom) {
+			/* Set output divider if not done by the Bootrom */
+			stm32mp1_pll_config_output(i, pllcfg[i]);
+			continue;
+		}
+
+		ret = stm32mp1_pll_config(i, pllcfg[i], pllfracv[i]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if (pllcsg_set[i]) {
+			stm32mp1_pll_csg(i, pllcsg[i]);
+		}
+
+		stm32mp1_pll_start(i);
+	}
+	/* Wait and start PLLs ouptut when ready */
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		if (!pllcfg_valid[i]) {
+			continue;
+		}
+
+		ret = stm32mp1_pll_output(i, pllcfg[i][PLLCFG_O]);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+	/* Wait LSE ready before to use it */
+	if (stm32mp1_osc[_LSE] != 0U) {
+		stm32mp1_lse_wait();
+	}
+
+	/* Configure with expected clock source */
+	ret = stm32mp1_set_clksrc(clksrc[CLKSRC_MPU]);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clksrc(clksrc[CLKSRC_AXI]);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clksrc(clksrc[CLKSRC_MCU]);
+	if (ret != 0) {
+		return ret;
+	}
+	stm32mp1_set_rtcsrc(clksrc[CLKSRC_RTC], lse_css);
+
+	/* Configure PKCK */
+	pkcs_cell = fdt_rcc_read_prop("st,pkcs", &len);
+	if (pkcs_cell != NULL) {
+		bool ckper_disabled = false;
+		uint32_t j;
+		uint32_t usbreg_bootrom = 0U;
+
+		if (pll4_bootrom) {
+			usbreg_bootrom = mmio_read_32(rcc_base + RCC_USBCKSELR);
+		}
+
+		for (j = 0; j < ((uint32_t)len / sizeof(uint32_t)); j++) {
+			uint32_t pkcs = fdt32_to_cpu(pkcs_cell[j]);
+
+			if (pkcs == (uint32_t)CLK_CKPER_DISABLED) {
+				ckper_disabled = true;
+				continue;
+			}
+			stm32mp1_pkcs_config(pkcs);
+		}
+
+		/*
+		 * CKPER is source for some peripheral clocks
+		 * (FMC-NAND / QPSI-NOR) and switching source is allowed
+		 * only if previous clock is still ON
+		 * => deactivated CKPER only after switching clock
+		 */
+		if (ckper_disabled) {
+			stm32mp1_pkcs_config(CLK_CKPER_DISABLED);
+		}
+
+		if (pll4_bootrom) {
+			uint32_t usbreg_value, usbreg_mask;
+			const struct stm32mp1_clk_sel *sel;
+
+			sel = clk_sel_ref(_USBPHY_SEL);
+			usbreg_mask = (uint32_t)sel->msk << sel->src;
+			sel = clk_sel_ref(_USBO_SEL);
+			usbreg_mask |= (uint32_t)sel->msk << sel->src;
+
+			usbreg_value = mmio_read_32(rcc_base + RCC_USBCKSELR) &
+				       usbreg_mask;
+			usbreg_bootrom &= usbreg_mask;
+			if (usbreg_bootrom != usbreg_value) {
+				VERBOSE("forbidden new USB clk path\n");
+				VERBOSE("vs bootrom on USB boot\n");
+				return -FDT_ERR_BADVALUE;
+			}
+		}
+	}
+
+	/* Switch OFF HSI if not found in device-tree */
+	if (stm32mp1_osc[_HSI] == 0U) {
+		stm32mp1_hsi_set(false);
+	}
+
+	stm32mp_stgen_config(stm32mp_clk_get_rate(STGEN_K));
+
+	/* Software Self-Refresh mode (SSR) during DDR initilialization */
+	mmio_clrsetbits_32(rcc_base + RCC_DDRITFCR,
+			   RCC_DDRITFCR_DDRCKMOD_MASK,
+			   RCC_DDRITFCR_DDRCKMOD_SSR <<
+			   RCC_DDRITFCR_DDRCKMOD_SHIFT);
+
+	return 0;
+}
+
+static void stm32mp1_osc_clk_init(const char *name,
+				  enum stm32mp_osc_id index)
+{
+	uint32_t frequency;
+
+	if (fdt_osc_read_freq(name, &frequency) == 0) {
+		stm32mp1_osc[index] = frequency;
+	}
+}
+
+static void stm32mp1_osc_init(void)
+{
+	enum stm32mp_osc_id i;
+
+	for (i = (enum stm32mp_osc_id)0 ; i < NB_OSC; i++) {
+		stm32mp1_osc_clk_init(stm32mp_osc_node_label[i], i);
+	}
+}
+
+#ifdef STM32MP_SHARED_RESOURCES
+/*
+ * Get the parent ID of the target parent clock, for tagging as secure
+ * shared clock dependencies.
+ */
+static int get_parent_id_parent(unsigned int parent_id)
+{
+	enum stm32mp1_parent_sel s = _UNKNOWN_SEL;
+	enum stm32mp1_pll_id pll_id;
+	uint32_t p_sel;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	switch (parent_id) {
+	case _ACLK:
+	case _PCLK4:
+	case _PCLK5:
+		s = _AXIS_SEL;
+		break;
+	case _PLL1_P:
+	case _PLL1_Q:
+	case _PLL1_R:
+		pll_id = _PLL1;
+		break;
+	case _PLL2_P:
+	case _PLL2_Q:
+	case _PLL2_R:
+		pll_id = _PLL2;
+		break;
+	case _PLL3_P:
+	case _PLL3_Q:
+	case _PLL3_R:
+		pll_id = _PLL3;
+		break;
+	case _PLL4_P:
+	case _PLL4_Q:
+	case _PLL4_R:
+		pll_id = _PLL4;
+		break;
+	case _PCLK1:
+	case _PCLK2:
+	case _HCLK2:
+	case _HCLK6:
+	case _CK_PER:
+	case _CK_MPU:
+	case _CK_MCU:
+	case _USB_PHY_48:
+		/* We do not expect to access these */
+		panic();
+		break;
+	default:
+		/* Other parents have no parent */
+		return -1;
+	}
+
+	if (s != _UNKNOWN_SEL) {
+		const struct stm32mp1_clk_sel *sel = clk_sel_ref(s);
+
+		p_sel = (mmio_read_32(rcc_base + sel->offset) >> sel->src) &
+			sel->msk;
+
+		if (p_sel < sel->nb_parent) {
+			return (int)sel->parent[p_sel];
+		}
+	} else {
+		const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+
+		p_sel = mmio_read_32(rcc_base + pll->rckxselr) &
+			RCC_SELR_REFCLK_SRC_MASK;
+
+		if (pll->refclk[p_sel] != _UNKNOWN_OSC_ID) {
+			return (int)pll->refclk[p_sel];
+		}
+	}
+
+	VERBOSE("No parent selected for %s\n",
+		stm32mp1_clk_parent_name[parent_id]);
+
+	return -1;
+}
+
+static void secure_parent_clocks(unsigned long parent_id)
+{
+	int grandparent_id;
+
+	switch (parent_id) {
+	case _PLL3_P:
+	case _PLL3_Q:
+	case _PLL3_R:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL3);
+		break;
+
+	/* These clocks are always secure when RCC is secure */
+	case _ACLK:
+	case _HCLK2:
+	case _HCLK6:
+	case _PCLK4:
+	case _PCLK5:
+	case _PLL1_P:
+	case _PLL1_Q:
+	case _PLL1_R:
+	case _PLL2_P:
+	case _PLL2_Q:
+	case _PLL2_R:
+	case _HSI:
+	case _HSI_KER:
+	case _LSI:
+	case _CSI:
+	case _CSI_KER:
+	case _HSE:
+	case _HSE_KER:
+	case _HSE_KER_DIV2:
+	case _HSE_RTC:
+	case _LSE:
+		break;
+
+	default:
+		VERBOSE("Cannot secure parent clock %s\n",
+			stm32mp1_clk_parent_name[parent_id]);
+		panic();
+	}
+
+	grandparent_id = get_parent_id_parent(parent_id);
+	if (grandparent_id >= 0) {
+		secure_parent_clocks(grandparent_id);
+	}
+}
+
+void stm32mp1_register_clock_parents_secure(unsigned long clock_id)
+{
+	int parent_id;
+
+	if (!stm32mp1_rcc_is_secure()) {
+		return;
+	}
+
+	switch (clock_id) {
+	case PLL1:
+	case PLL2:
+		/* PLL1/PLL2 are always secure: nothing to do */
+		break;
+	case PLL3:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL3);
+		break;
+	case PLL4:
+		ERROR("PLL4 cannot be secured\n");
+		panic();
+		break;
+	default:
+		/* Others are expected gateable clock */
+		parent_id = stm32mp1_clk_get_parent(clock_id);
+		if (parent_id < 0) {
+			INFO("No parent found for clock %lu\n", clock_id);
+		} else {
+			secure_parent_clocks(parent_id);
+		}
+		break;
+	}
+}
+#endif /* STM32MP_SHARED_RESOURCES */
+
+static void sync_earlyboot_clocks_state(void)
+{
+	unsigned int idx;
+	const unsigned long secure_enable[] = {
+		AXIDCG,
+		BSEC,
+		DDRC1, DDRC1LP,
+		DDRC2, DDRC2LP,
+		DDRCAPB, DDRPHYCAPB, DDRPHYCAPBLP,
+		DDRPHYC, DDRPHYCLP,
+		RTCAPB,
+		TZC1, TZC2,
+		TZPC,
+		STGEN_K,
+	};
+
+	for (idx = 0U; idx < ARRAY_SIZE(secure_enable); idx++) {
+		stm32mp_clk_enable(secure_enable[idx]);
+	}
+}
+
+static const struct clk_ops stm32mp_clk_ops = {
+	.enable		= stm32mp_clk_enable,
+	.disable	= stm32mp_clk_disable,
+	.is_enabled	= stm32mp_clk_is_enabled,
+	.get_rate	= stm32mp_clk_get_rate,
+	.get_parent	= stm32mp1_clk_get_parent,
+};
+
+int stm32mp1_clk_probe(void)
+{
+#if defined(IMAGE_BL32)
+	if (!fdt_get_rcc_secure_state()) {
+		mmio_write_32(stm32mp_rcc_base() + RCC_TZCR, 0U);
+	}
+#endif
+
+	stm32mp1_osc_init();
+
+	sync_earlyboot_clocks_state();
+
+	clk_register(&stm32mp_clk_ops);
+
+	return 0;
+}
diff --git a/drivers/st/clk/stm32mp_clkfunc.c b/drivers/st/clk/stm32mp_clkfunc.c
new file mode 100644
index 0000000..01d1420
--- /dev/null
+++ b/drivers/st/clk/stm32mp_clkfunc.c
@@ -0,0 +1,377 @@
+/*
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+
+#include <arch_helpers.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/generic_delay_timer.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+/*
+ * Get the frequency of an oscillator from its name in device tree.
+ * @param name: oscillator name
+ * @param freq: stores the frequency of the oscillator
+ * @return: 0 on success, and a negative FDT/ERRNO error code on failure.
+ */
+int fdt_osc_read_freq(const char *name, uint32_t *freq)
+{
+	int node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar;
+		int ret;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return ret;
+		}
+
+		if ((strncmp(cchar, name, (size_t)ret) == 0) &&
+		    (fdt_get_status(subnode) != DT_DISABLED)) {
+			const fdt32_t *cuint;
+
+			cuint = fdt_getprop(fdt, subnode, "clock-frequency",
+					    &ret);
+			if (cuint == NULL) {
+				return ret;
+			}
+
+			*freq = fdt32_to_cpu(*cuint);
+
+			return 0;
+		}
+	}
+
+	/* Oscillator not found, freq=0 */
+	*freq = 0;
+	return 0;
+}
+
+/*
+ * Check the presence of an oscillator property from its id.
+ * @param node_label: clock node name
+ * @param prop_name: property name
+ * @return: true/false regarding search result.
+ */
+bool fdt_clk_read_bool(const char *node_label, const char *prop_name)
+{
+	int node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return false;
+	}
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return false;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar;
+		int ret;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return false;
+		}
+
+		if (strncmp(cchar, node_label, (size_t)ret) != 0) {
+			continue;
+		}
+
+		if (fdt_getprop(fdt, subnode, prop_name, NULL) != NULL) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+/*
+ * Get the value of a oscillator property from its name.
+ * @param node_label: oscillator name
+ * @param prop_name: property name
+ * @param dflt_value: default value
+ * @return oscillator value on success, default value if property not found.
+ */
+uint32_t fdt_clk_read_uint32_default(const char *node_label,
+				     const char *prop_name, uint32_t dflt_value)
+{
+	int node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return dflt_value;
+	}
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return dflt_value;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar;
+		int ret;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return dflt_value;
+		}
+
+		if (strncmp(cchar, node_label, (size_t)ret) != 0) {
+			continue;
+		}
+
+		return fdt_read_uint32_default(fdt, subnode, prop_name,
+					       dflt_value);
+	}
+
+	return dflt_value;
+}
+
+/*
+ * Get the RCC node offset from the device tree
+ * @param fdt: Device tree reference
+ * @return: Node offset or a negative value on error
+ */
+static int fdt_get_rcc_node(void *fdt)
+{
+	static int node;
+
+	if (node <= 0) {
+		node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+	}
+
+	return node;
+}
+
+/*
+ * Read a series of parameters in rcc-clk section in device tree
+ * @param prop_name: Name of the RCC property to be read
+ * @param array: the array to store the property parameters
+ * @param count: number of parameters to be read
+ * @return: 0 on succes or a negative value on error
+ */
+int fdt_rcc_read_uint32_array(const char *prop_name, uint32_t count,
+			      uint32_t *array)
+{
+	int node;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node = fdt_get_rcc_node(fdt);
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return fdt_read_uint32_array(fdt, node, prop_name, count, array);
+}
+
+/*
+ * Get the subnode offset in rcc-clk section from its name in device tree
+ * @param name: name of the RCC property
+ * @return: offset on success, and a negative FDT/ERRNO error code on failure.
+ */
+int fdt_rcc_subnode_offset(const char *name)
+{
+	int node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node = fdt_get_rcc_node(fdt);
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	subnode = fdt_subnode_offset(fdt, node, name);
+	if (subnode <= 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return subnode;
+}
+
+/*
+ * Get the pointer to a rcc-clk property from its name.
+ * @param name: name of the RCC property
+ * @param lenp: stores the length of the property.
+ * @return: pointer to the property on success, and NULL value on failure.
+ */
+const fdt32_t *fdt_rcc_read_prop(const char *prop_name, int *lenp)
+{
+	const fdt32_t *cuint;
+	int node, len;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return NULL;
+	}
+
+	node = fdt_get_rcc_node(fdt);
+	if (node < 0) {
+		return NULL;
+	}
+
+	cuint = fdt_getprop(fdt, node, prop_name, &len);
+	if (cuint == NULL) {
+		return NULL;
+	}
+
+	*lenp = len;
+	return cuint;
+}
+
+/*
+ * Get the secure state for rcc node in device tree.
+ * @return: true if rcc is configured for secure world access, false if not.
+ */
+bool fdt_get_rcc_secure_state(void)
+{
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return false;
+	}
+
+	if (fdt_node_offset_by_compatible(fdt, -1, DT_RCC_SEC_CLK_COMPAT) < 0) {
+		return false;
+	}
+
+	return true;
+}
+
+/*
+ * Get the clock ID of the given node in device tree.
+ * @param node: node offset
+ * @return: Clock ID on success, and a negative FDT/ERRNO error code on failure.
+ */
+int fdt_get_clock_id(int node)
+{
+	const fdt32_t *cuint;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	cuint = fdt_getprop(fdt, node, "clocks", NULL);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	cuint++;
+	return (int)fdt32_to_cpu(*cuint);
+}
+
+/*
+ * Get the frequency of the specified UART instance.
+ * @param instance: UART interface registers base address.
+ * @return: clock frequency on success, 0 value on failure.
+ */
+unsigned long fdt_get_uart_clock_freq(uintptr_t instance)
+{
+	void *fdt;
+	int node;
+	int clk_id;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return 0UL;
+	}
+
+	/* Check for UART nodes */
+	node = dt_match_instance_by_compatible(DT_UART_COMPAT, instance);
+	if (node < 0) {
+		return 0UL;
+	}
+
+	clk_id = fdt_get_clock_id(node);
+	if (clk_id < 0) {
+		return 0UL;
+	}
+
+	return clk_get_rate((unsigned long)clk_id);
+}
+
+/*******************************************************************************
+ * This function configures and restores the STGEN counter depending on the
+ * connected clock.
+ ******************************************************************************/
+void stm32mp_stgen_config(unsigned long rate)
+{
+	uint32_t cntfid0;
+	unsigned long long counter;
+
+	cntfid0 = mmio_read_32(STGEN_BASE + CNTFID_OFF);
+
+	if (cntfid0 == rate) {
+		return;
+	}
+
+	mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
+	counter = stm32mp_stgen_get_counter() * rate / cntfid0;
+
+	mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)counter);
+	mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(counter >> 32));
+	mmio_write_32(STGEN_BASE + CNTFID_OFF, rate);
+	mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
+
+	write_cntfrq_el0(rate);
+
+	/* Need to update timer with new frequency */
+	generic_delay_timer_init();
+}
+
+/*******************************************************************************
+ * This function returns the STGEN counter value.
+ ******************************************************************************/
+unsigned long long stm32mp_stgen_get_counter(void)
+{
+	return (((unsigned long long)mmio_read_32(STGEN_BASE + CNTCVU_OFF) << 32) |
+		mmio_read_32(STGEN_BASE + CNTCVL_OFF));
+}
+
+/*******************************************************************************
+ * This function restores the STGEN counter value.
+ * It takes a first input value as a counter backup value to be restored and a
+ * offset in ms to be added.
+ ******************************************************************************/
+void stm32mp_stgen_restore_counter(unsigned long long value,
+				   unsigned long long offset_in_ms)
+{
+	unsigned long long cnt;
+
+	cnt = value + ((offset_in_ms *
+			mmio_read_32(STGEN_BASE + CNTFID_OFF)) / 1000U);
+
+	mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
+	mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)cnt);
+	mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(cnt >> 32));
+	mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
+}
diff --git a/drivers/st/crypto/stm32_hash.c b/drivers/st/crypto/stm32_hash.c
new file mode 100644
index 0000000..e92f980
--- /dev/null
+++ b/drivers/st/crypto/stm32_hash.c
@@ -0,0 +1,364 @@
+/*
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_hash.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+#if STM32_HASH_VER == 2
+#define DT_HASH_COMPAT			"st,stm32f756-hash"
+#endif
+#if STM32_HASH_VER == 4
+#define DT_HASH_COMPAT			"st,stm32mp13-hash"
+#endif
+
+#define HASH_CR				0x00U
+#define HASH_DIN			0x04U
+#define HASH_STR			0x08U
+#define HASH_SR				0x24U
+#define HASH_HREG(x)			(0x310U + ((x) * 0x04U))
+
+/* Control Register */
+#define HASH_CR_INIT			BIT(2)
+#define HASH_CR_DATATYPE_SHIFT		U(4)
+#if STM32_HASH_VER == 2
+#define HASH_CR_ALGO_SHA1		0x0U
+#define HASH_CR_ALGO_MD5		BIT(7)
+#define HASH_CR_ALGO_SHA224		BIT(18)
+#define HASH_CR_ALGO_SHA256		(BIT(18) | BIT(7))
+#endif
+#if STM32_HASH_VER == 4
+#define HASH_CR_ALGO_SHIFT		U(17)
+#define HASH_CR_ALGO_SHA1		(0x0U << HASH_CR_ALGO_SHIFT)
+#define HASH_CR_ALGO_SHA224		(0x2U << HASH_CR_ALGO_SHIFT)
+#define HASH_CR_ALGO_SHA256		(0x3U << HASH_CR_ALGO_SHIFT)
+#define HASH_CR_ALGO_SHA384		(0xCU << HASH_CR_ALGO_SHIFT)
+#define HASH_CR_ALGO_SHA512_224		(0xDU << HASH_CR_ALGO_SHIFT)
+#define HASH_CR_ALGO_SHA512_256		(0xEU << HASH_CR_ALGO_SHIFT)
+#define HASH_CR_ALGO_SHA512		(0xFU << HASH_CR_ALGO_SHIFT)
+#endif
+
+/* Status Flags */
+#define HASH_SR_DCIS			BIT(1)
+#define HASH_SR_BUSY			BIT(3)
+
+/* STR Register */
+#define HASH_STR_NBLW_MASK		GENMASK(4, 0)
+#define HASH_STR_DCAL			BIT(8)
+
+#define MD5_DIGEST_SIZE			16U
+#define SHA1_DIGEST_SIZE		20U
+#define SHA224_DIGEST_SIZE		28U
+#define SHA256_DIGEST_SIZE		32U
+#define SHA384_DIGEST_SIZE		48U
+#define SHA512_224_DIGEST_SIZE		28U
+#define SHA512_256_DIGEST_SIZE		32U
+#define SHA512_DIGEST_SIZE		64U
+
+#define RESET_TIMEOUT_US_1MS		1000U
+#define HASH_TIMEOUT_US			10000U
+
+enum stm32_hash_data_format {
+	HASH_DATA_32_BITS,
+	HASH_DATA_16_BITS,
+	HASH_DATA_8_BITS,
+	HASH_DATA_1_BIT
+};
+
+struct stm32_hash_instance {
+	uintptr_t base;
+	unsigned int clock;
+	size_t digest_size;
+};
+
+struct stm32_hash_remain {
+	uint32_t buffer;
+	size_t length;
+};
+
+/* Expect a single HASH peripheral */
+static struct stm32_hash_instance stm32_hash;
+static struct stm32_hash_remain stm32_remain;
+
+static uintptr_t hash_base(void)
+{
+	return stm32_hash.base;
+}
+
+static int hash_wait_busy(void)
+{
+	uint64_t timeout = timeout_init_us(HASH_TIMEOUT_US);
+
+	while ((mmio_read_32(hash_base() + HASH_SR) & HASH_SR_BUSY) != 0U) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("%s: busy timeout\n", __func__);
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static int hash_wait_computation(void)
+{
+	uint64_t timeout = timeout_init_us(HASH_TIMEOUT_US);
+
+	while ((mmio_read_32(hash_base() + HASH_SR) & HASH_SR_DCIS) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("%s: busy timeout\n", __func__);
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static int hash_write_data(uint32_t data)
+{
+	int ret;
+
+	ret = hash_wait_busy();
+	if (ret != 0) {
+		return ret;
+	}
+
+	mmio_write_32(hash_base() + HASH_DIN, data);
+
+	return 0;
+}
+
+static void hash_hw_init(enum stm32_hash_algo_mode mode)
+{
+	uint32_t reg;
+
+	reg = HASH_CR_INIT | (HASH_DATA_8_BITS << HASH_CR_DATATYPE_SHIFT);
+
+	switch (mode) {
+#if STM32_HASH_VER == 2
+	case HASH_MD5SUM:
+		reg |= HASH_CR_ALGO_MD5;
+		stm32_hash.digest_size = MD5_DIGEST_SIZE;
+		break;
+#endif
+	case HASH_SHA1:
+		reg |= HASH_CR_ALGO_SHA1;
+		stm32_hash.digest_size = SHA1_DIGEST_SIZE;
+		break;
+	case HASH_SHA224:
+		reg |= HASH_CR_ALGO_SHA224;
+		stm32_hash.digest_size = SHA224_DIGEST_SIZE;
+		break;
+#if STM32_HASH_VER == 4
+	case HASH_SHA384:
+		reg |= HASH_CR_ALGO_SHA384;
+		stm32_hash.digest_size = SHA384_DIGEST_SIZE;
+		break;
+	case HASH_SHA512:
+		reg |= HASH_CR_ALGO_SHA512;
+		stm32_hash.digest_size = SHA512_DIGEST_SIZE;
+		break;
+#endif
+	/* Default selected algo is SHA256 */
+	case HASH_SHA256:
+	default:
+		reg |= HASH_CR_ALGO_SHA256;
+		stm32_hash.digest_size = SHA256_DIGEST_SIZE;
+		break;
+	}
+
+	mmio_write_32(hash_base() + HASH_CR, reg);
+}
+
+static int hash_get_digest(uint8_t *digest)
+{
+	int ret;
+	uint32_t i;
+	uint32_t dsg;
+
+	ret = hash_wait_computation();
+	if (ret != 0) {
+		return ret;
+	}
+
+	for (i = 0U; i < (stm32_hash.digest_size / sizeof(uint32_t)); i++) {
+		dsg = __builtin_bswap32(mmio_read_32(hash_base() +
+						     HASH_HREG(i)));
+		memcpy(digest + (i * sizeof(uint32_t)), &dsg, sizeof(uint32_t));
+	}
+
+	/*
+	 * Clean hardware context as HASH could be used later
+	 * by non-secure software
+	 */
+	hash_hw_init(HASH_SHA256);
+
+	return 0;
+}
+
+int stm32_hash_update(const uint8_t *buffer, size_t length)
+{
+	size_t remain_length = length;
+	int ret = 0;
+
+	if ((length == 0U) || (buffer == NULL)) {
+		return 0;
+	}
+
+	clk_enable(stm32_hash.clock);
+
+	if (stm32_remain.length != 0U) {
+		uint32_t copysize;
+
+		copysize = MIN((sizeof(uint32_t) - stm32_remain.length),
+			       length);
+		memcpy(((uint8_t *)&stm32_remain.buffer) + stm32_remain.length,
+		       buffer, copysize);
+		remain_length -= copysize;
+		buffer += copysize;
+		if (stm32_remain.length == sizeof(uint32_t)) {
+			ret = hash_write_data(stm32_remain.buffer);
+			if (ret != 0) {
+				goto exit;
+			}
+
+			zeromem(&stm32_remain, sizeof(stm32_remain));
+		}
+	}
+
+	while (remain_length / sizeof(uint32_t) != 0U) {
+		uint32_t tmp_buf;
+
+		memcpy(&tmp_buf, buffer, sizeof(uint32_t));
+		ret = hash_write_data(tmp_buf);
+		if (ret != 0) {
+			goto exit;
+		}
+
+		buffer += sizeof(uint32_t);
+		remain_length -= sizeof(uint32_t);
+	}
+
+	if (remain_length != 0U) {
+		assert(stm32_remain.length == 0U);
+
+		memcpy((uint8_t *)&stm32_remain.buffer, buffer, remain_length);
+		stm32_remain.length = remain_length;
+	}
+
+exit:
+	clk_disable(stm32_hash.clock);
+
+	return ret;
+}
+
+int stm32_hash_final(uint8_t *digest)
+{
+	int ret;
+
+	clk_enable(stm32_hash.clock);
+
+	if (stm32_remain.length != 0U) {
+		ret = hash_write_data(stm32_remain.buffer);
+		if (ret != 0) {
+			clk_disable(stm32_hash.clock);
+			return ret;
+		}
+
+		mmio_clrsetbits_32(hash_base() + HASH_STR, HASH_STR_NBLW_MASK,
+				   8U * stm32_remain.length);
+		zeromem(&stm32_remain, sizeof(stm32_remain));
+	} else {
+		mmio_clrbits_32(hash_base() + HASH_STR, HASH_STR_NBLW_MASK);
+	}
+
+	mmio_setbits_32(hash_base() + HASH_STR, HASH_STR_DCAL);
+
+	ret = hash_get_digest(digest);
+
+	clk_disable(stm32_hash.clock);
+
+	return ret;
+}
+
+int stm32_hash_final_update(const uint8_t *buffer, uint32_t length,
+			    uint8_t *digest)
+{
+	int ret;
+
+	ret = stm32_hash_update(buffer, length);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return stm32_hash_final(digest);
+}
+
+void stm32_hash_init(enum stm32_hash_algo_mode mode)
+{
+	clk_enable(stm32_hash.clock);
+
+	hash_hw_init(mode);
+
+	clk_disable(stm32_hash.clock);
+
+	zeromem(&stm32_remain, sizeof(stm32_remain));
+}
+
+int stm32_hash_register(void)
+{
+	struct dt_node_info hash_info;
+	int node;
+
+	for (node = dt_get_node(&hash_info, -1, DT_HASH_COMPAT);
+	     node != -FDT_ERR_NOTFOUND;
+	     node = dt_get_node(&hash_info, node, DT_HASH_COMPAT)) {
+		if (hash_info.status != DT_DISABLED) {
+			break;
+		}
+	}
+
+	if (node == -FDT_ERR_NOTFOUND) {
+		return -ENODEV;
+	}
+
+	if (hash_info.clock < 0) {
+		return -EINVAL;
+	}
+
+	stm32_hash.base = hash_info.base;
+	stm32_hash.clock = hash_info.clock;
+
+	clk_enable(stm32_hash.clock);
+
+	if (hash_info.reset >= 0) {
+		uint32_t id = (uint32_t)hash_info.reset;
+
+		if (stm32mp_reset_assert(id, RESET_TIMEOUT_US_1MS) != 0) {
+			panic();
+		}
+		udelay(20);
+		if (stm32mp_reset_deassert(id, RESET_TIMEOUT_US_1MS) != 0) {
+			panic();
+		}
+	}
+
+	clk_disable(stm32_hash.clock);
+
+	return 0;
+}
diff --git a/drivers/st/crypto/stm32_pka.c b/drivers/st/crypto/stm32_pka.c
new file mode 100644
index 0000000..e03cf0f
--- /dev/null
+++ b/drivers/st/crypto/stm32_pka.c
@@ -0,0 +1,707 @@
+/*
+ * Copyright (c) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_pka.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+/*
+ * For our comprehension in this file
+ *  _len are in BITs
+ *  _size are in BYTEs
+ *  _nbw are in number of PKA_word (PKA_word = u64)
+ */
+
+#define UINT8_LEN			8U
+#define UINT64_LEN			(UINT8_LEN * sizeof(uint64_t))
+#define WORD_SIZE			(sizeof(uint64_t))
+#define OP_NBW_FROM_LEN(len)		(DIV_ROUND_UP_2EVAL((len), UINT64_LEN) + 1)
+#define OP_NBW_FROM_SIZE(s)		OP_NBW_FROM_LEN((s) * UINT8_LEN)
+#define OP_SIZE_FROM_SIZE(s)		(OP_NBW_FROM_SIZE(s) * WORD_SIZE)
+
+#define DT_PKA_COMPAT			"st,stm32-pka64"
+
+#define MAX_ECC_SIZE_LEN		640U
+#define MAX_EO_NBW			OP_NBW_FROM_LEN(MAX_ECC_SIZE_LEN)
+
+/* PKA registers */
+/* PKA control register */
+#define _PKA_CR				0x0U
+/* PKA status register */
+#define _PKA_SR				0x4U
+/* PKA clear flag register */
+#define _PKA_CLRFR			0x8U
+/* PKA version register */
+#define _PKA_VERR			0x1FF4U
+/* PKA identification register */
+#define _PKA_IPIDR			0x1FF8U
+
+/* PKA control register fields */
+#define _PKA_CR_MODE_MASK		GENMASK(13, 8)
+#define _PKA_CR_MODE_SHIFT		8U
+#define _PKA_CR_MODE_ADD		0x9U
+#define _PKA_CR_MODE_ECDSA_VERIF	0x26U
+#define _PKA_CR_START			BIT(1)
+#define _PKA_CR_EN			BIT(0)
+
+/* PKA status register fields */
+#define _PKA_SR_BUSY			BIT(16)
+#define _PKA_SR_LMF			BIT(1)
+#define _PKA_SR_INITOK			BIT(0)
+
+/* PKA it flag fields (used in CR, SR and CLRFR) */
+#define _PKA_IT_MASK			(GENMASK(21, 19) | BIT(17))
+#define _PKA_IT_SHIFT			17U
+#define _PKA_IT_OPERR			BIT(21)
+#define _PKA_IT_ADDRERR			BIT(20)
+#define _PKA_IT_RAMERR			BIT(19)
+#define _PKA_IT_PROCEND			BIT(17)
+
+/* PKA version register fields */
+#define _PKA_VERR_MAJREV_MASK		GENMASK(7, 4)
+#define _PKA_VERR_MAJREV_SHIFT		4U
+#define _PKA_VERR_MINREV_MASK		GENMASK(3, 0)
+#define _PKA_VERR_MINREV_SHIFT		0U
+
+/* RAM magic offset */
+#define _PKA_RAM_START			0x400U
+#define _PKA_RAM_SIZE			5336U
+
+/* ECDSA verification */
+#define _PKA_RAM_N_LEN			0x408U /* 64 */
+#define _PKA_RAM_P_LEN			0x4C8U /* 64 */
+#define _PKA_RAM_A_SIGN			0x468U /* 64 */
+#define _PKA_RAM_A			0x470U /* EOS */
+#define _PKA_RAM_P			0x4D0U /* EOS */
+#define _PKA_RAM_XG			0x678U /* EOS */
+#define _PKA_RAM_YG			0x6D0U /* EOS */
+#define _PKA_RAM_XQ			0x12F8U /* EOS */
+#define _PKA_RAM_YQ			0x1350U /* EOS */
+#define _PKA_RAM_SIGN_R			0x10E0U /* EOS */
+#define _PKA_RAM_SIGN_S			0xC68U /* EOS */
+#define _PKA_RAM_HASH_Z			0x13A8U /* EOS */
+#define _PKA_RAM_PRIME_N		0x1088U /* EOS */
+#define _PKA_RAM_ECDSA_VERIFY		0x5D0U /* 64 */
+#define _PKA_RAM_ECDSA_VERIFY_VALID	0xD60DULL
+#define _PKA_RAM_ECDSA_VERIFY_INVALID	0xA3B7ULL
+
+#define PKA_TIMEOUT_US			1000000U
+#define TIMEOUT_US_1MS			1000U
+#define PKA_RESET_DELAY			20U
+
+struct curve_parameters {
+	uint32_t a_sign;  /* 0 positive, 1 negative */
+	uint8_t *a;    /* Curve coefficient |a| */
+	size_t a_size;
+	uint8_t *p;    /* Curve modulus value */
+	uint32_t p_len;
+	uint8_t *xg;   /* Curve base point G coordinate x */
+	size_t xg_size;
+	uint8_t *yg;   /* Curve base point G coordinate y */
+	size_t yg_size;
+	uint8_t *n;    /* Curve prime order n */
+	uint32_t n_len;
+};
+
+static const struct curve_parameters curve_def[] = {
+#if PKA_USE_NIST_P256
+	[PKA_NIST_P256] = {
+		.p_len = 256U,
+		.n_len = 256U,
+		.p  = (uint8_t[]){0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x01,
+				  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+				  0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
+				  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF},
+		.n  = (uint8_t[]){0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00,
+				  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+				  0xBC, 0xE6, 0xFA, 0xAD, 0xA7, 0x17, 0x9E, 0x84,
+				  0xF3, 0xB9, 0xCA, 0xC2, 0xFC, 0x63, 0x25, 0x51},
+		.a_sign = 1U,
+		.a = (uint8_t[]){0x03},
+		.a_size = 1U,
+		.xg = (uint8_t[]){0x6B, 0x17, 0xD1, 0xF2, 0xE1, 0x2C, 0x42, 0x47,
+				  0xF8, 0xBC, 0xE6, 0xE5, 0x63, 0xA4, 0x40, 0xF2,
+				  0x77, 0x03, 0x7D, 0x81, 0x2D, 0xEB, 0x33, 0xA0,
+				  0xF4, 0xA1, 0x39, 0x45, 0xD8, 0x98, 0xC2, 0x96},
+		.xg_size = 32U,
+		.yg = (uint8_t[]){0x4F, 0xE3, 0x42, 0xE2, 0xFE, 0x1A, 0x7F, 0x9B,
+				  0x8E, 0xE7, 0xEB, 0x4A, 0x7C, 0x0F, 0x9E, 0x16,
+				  0x2B, 0xCE, 0x33, 0x57, 0x6B, 0x31, 0x5E, 0xCE,
+				  0xCB, 0xB6, 0x40, 0x68, 0x37, 0xBF, 0x51, 0xF5},
+		.yg_size = 32U,
+	},
+#endif
+#if PKA_USE_BRAINPOOL_P256R1
+	[PKA_BRAINPOOL_P256R1] = {
+		.p_len = 256,
+		.n_len = 256,
+		.p  = (uint8_t[]){0xA9, 0xFB, 0x57, 0xDB, 0xA1, 0xEE, 0xA9, 0xBC,
+				  0x3E, 0x66, 0x0A, 0x90, 0x9D, 0x83, 0x8D, 0x72,
+				  0x6E, 0x3B, 0xF6, 0x23, 0xD5, 0x26, 0x20, 0x28,
+				  0x20, 0x13, 0x48, 0x1D, 0x1F, 0x6E, 0x53, 0x77},
+		.n  = (uint8_t[]){0xA9, 0xFB, 0x57, 0xDB, 0xA1, 0xEE, 0xA9, 0xBC,
+				  0x3E, 0x66, 0x0A, 0x90, 0x9D, 0x83, 0x8D, 0x71,
+				  0x8C, 0x39, 0x7A, 0xA3, 0xB5, 0x61, 0xA6, 0xF7,
+				  0x90, 0x1E, 0x0E, 0x82, 0x97, 0x48, 0x56, 0xA7},
+		.a  = (uint8_t[]){0x7D, 0x5A, 0x09, 0x75, 0xFC, 0x2C, 0x30, 0x57,
+				  0xEE, 0xF6, 0x75, 0x30, 0x41, 0x7A, 0xFF, 0xE7,
+				  0xFB, 0x80, 0x55, 0xC1, 0x26, 0xDC, 0x5C, 0x6C,
+				  0xE9, 0x4A, 0x4B, 0x44, 0xF3, 0x30, 0xB5, 0xD9},
+		.a_size = 32U,
+		.xg = (uint8_t[]){0x8B, 0xD2, 0xAE, 0xB9, 0xCB, 0x7E, 0x57, 0xCB,
+				  0x2C, 0x4B, 0x48, 0x2F, 0xFC, 0x81, 0xB7, 0xAF,
+				  0xB9, 0xDE, 0x27, 0xE1, 0xE3, 0xBD, 0x23, 0xC2,
+				  0x3A, 0x44, 0x53, 0xBD, 0x9A, 0xCE, 0x32, 0x62},
+		.xg_size = 32U,
+		.yg = (uint8_t[]){0x54, 0x7E, 0xF8, 0x35, 0xC3, 0xDA, 0xC4, 0xFD,
+				  0x97, 0xF8, 0x46, 0x1A, 0x14, 0x61, 0x1D, 0xC9,
+				  0xC2, 0x77, 0x45, 0x13, 0x2D, 0xED, 0x8E, 0x54,
+				  0x5C, 0x1D, 0x54, 0xC7, 0x2F, 0x04, 0x69, 0x97},
+		.yg_size = 32U,
+	},
+#endif
+#if PKA_USE_BRAINPOOL_P256T1
+	[PKA_BRAINPOOL_P256T1] = {
+		.p_len = 256,
+		.n_len = 256,
+		.p  = (uint8_t[]){0xA9, 0xFB, 0x57, 0xDB, 0xA1, 0xEE, 0xA9, 0xBC,
+				  0x3E, 0x66, 0x0A, 0x90, 0x9D, 0x83, 0x8D, 0x72,
+				  0x6E, 0x3B, 0xF6, 0x23, 0xD5, 0x26, 0x20, 0x28,
+				  0x20, 0x13, 0x48, 0x1D, 0x1F, 0x6E, 0x53, 0x77},
+		.n  = (uint8_t[]){0xA9, 0xFB, 0x57, 0xDB, 0xA1, 0xEE, 0xA9, 0xBC,
+				  0x3E, 0x66, 0x0A, 0x90, 0x9D, 0x83, 0x8D, 0x71,
+				  0x8C, 0x39, 0x7A, 0xA3, 0xB5, 0x61, 0xA6, 0xF7,
+				  0x90, 0x1E, 0x0E, 0x82, 0x97, 0x48, 0x56, 0xA7},
+		.a  = (uint8_t[]){0xA9, 0xFB, 0x57, 0xDB, 0xA1, 0xEE, 0xA9, 0xBC,
+				  0x3E, 0x66, 0x0A, 0x90, 0x9D, 0x83, 0x8D, 0x72,
+				  0x6E, 0x3B, 0xF6, 0x23, 0xD5, 0x26, 0x20, 0x28,
+				  0x20, 0x13, 0x48, 0x1D, 0x1F, 0x6E, 0x53, 0x74},
+		.a_size = 32U,
+		.xg = (uint8_t[]){0xA3, 0xE8, 0xEB, 0x3C, 0xC1, 0xCF, 0xE7, 0xB7,
+				  0x73, 0x22, 0x13, 0xB2, 0x3A, 0x65, 0x61, 0x49,
+				  0xAF, 0xA1, 0x42, 0xC4, 0x7A, 0xAF, 0xBC, 0x2B,
+				  0x79, 0xA1, 0x91, 0x56, 0x2E, 0x13, 0x05, 0xF4},
+		.xg_size = 32U,
+		.yg = (uint8_t[]){0x2D, 0x99, 0x6C, 0x82, 0x34, 0x39, 0xC5, 0x6D,
+				  0x7F, 0x7B, 0x22, 0xE1, 0x46, 0x44, 0x41, 0x7E,
+				  0x69, 0xBC, 0xB6, 0xDE, 0x39, 0xD0, 0x27, 0x00,
+				  0x1D, 0xAB, 0xE8, 0xF3, 0x5B, 0x25, 0xC9, 0xBE},
+		.yg_size = 32U,
+	},
+#endif
+#if PKA_USE_NIST_P521
+	[PKA_NIST_P521] = {
+		.p_len = 521,
+		.n_len = 521,
+		.p  = (uint8_t[]){                                    0x01, 0xff,
+				  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+				  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+				  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+				  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+				  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+				  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+				  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+				  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff},
+		.n  = (uint8_t[]){                                    0x01, 0xff,
+				  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+				  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+				  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+				  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfa,
+				  0x51, 0x86, 0x87, 0x83, 0xbf, 0x2f, 0x96, 0x6b,
+				  0x7f, 0xcc, 0x01, 0x48, 0xf7, 0x09, 0xa5, 0xd0,
+				  0x3b, 0xb5, 0xc9, 0xb8, 0x89, 0x9c, 0x47, 0xae,
+				  0xbb, 0x6f, 0xb7, 0x1e, 0x91, 0x38, 0x64, 0x09},
+		.a_sign = 1,
+		.a  = (uint8_t[]){0x03},
+		.a_size = 1U,
+		.xg = (uint8_t[]){                                          0xc6,
+				  0x85, 0x8e, 0x06, 0xb7, 0x04, 0x04, 0xe9, 0xcd,
+				  0x9e, 0x3e, 0xcb, 0x66, 0x23, 0x95, 0xb4, 0x42,
+				  0x9c, 0x64, 0x81, 0x39, 0x05, 0x3f, 0xb5, 0x21,
+				  0xf8, 0x28, 0xaf, 0x60, 0x6b, 0x4d, 0x3d, 0xba,
+				  0xa1, 0x4b, 0x5e, 0x77, 0xef, 0xe7, 0x59, 0x28,
+				  0xfe, 0x1d, 0xc1, 0x27, 0xa2, 0xff, 0xa8, 0xde,
+				  0x33, 0x48, 0xb3, 0xc1, 0x85, 0x6a, 0x42, 0x9b,
+				  0xf9, 0x7e, 0x7e, 0x31, 0xc2, 0xe5, 0xbd, 0x66},
+		.xg_size = 65U,
+		.yg = (uint8_t[]){                                    0x01, 0x18,
+				  0x39, 0x29, 0x6a, 0x78, 0x9a, 0x3b, 0xc0, 0x04,
+				  0x5c, 0x8a, 0x5f, 0xb4, 0x2c, 0x7d, 0x1b, 0xd9,
+				  0x98, 0xf5, 0x44, 0x49, 0x57, 0x9b, 0x44, 0x68,
+				  0x17, 0xaf, 0xbd, 0x17, 0x27, 0x3e, 0x66, 0x2c,
+				  0x97, 0xee, 0x72, 0x99, 0x5e, 0xf4, 0x26, 0x40,
+				  0xc5, 0x50, 0xb9, 0x01, 0x3f, 0xad, 0x07, 0x61,
+				  0x35, 0x3c, 0x70, 0x86, 0xa2, 0x72, 0xc2, 0x40,
+				  0x88, 0xbe, 0x94, 0x76, 0x9f, 0xd1, 0x66, 0x50},
+		.yg_size = 66U,
+	},
+#endif
+};
+
+static struct stm32_pka_platdata pka_pdata;
+
+#pragma weak stm32_pka_get_platdata
+
+int stm32_pka_get_platdata(struct stm32_pka_platdata *pdata)
+{
+	return -ENODEV;
+}
+
+static int stm32_pka_parse_fdt(void)
+{
+	int node;
+	struct dt_node_info info;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	node = dt_get_node(&info, -1, DT_PKA_COMPAT);
+	if (node < 0) {
+		ERROR("No PKA entry in DT\n");
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if (info.status == DT_DISABLED) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if ((info.base == 0) || (info.clock < 0) || (info.reset < 0)) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	pka_pdata.base = (uintptr_t)info.base;
+	pka_pdata.clock_id = (unsigned long)info.clock;
+	pka_pdata.reset_id = (unsigned int)info.reset;
+
+	return 0;
+}
+
+static int pka_wait_bit(uintptr_t base, uint32_t bit)
+{
+	uint64_t timeout = timeout_init_us(PKA_TIMEOUT_US);
+
+	while ((mmio_read_32(base + _PKA_SR) & bit) != bit) {
+		if (timeout_elapsed(timeout)) {
+			WARN("timeout waiting %x\n", bit);
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+
+}
+
+static void pka_disable(uintptr_t base)
+{
+	mmio_clrbits_32(base + _PKA_CR, _PKA_CR_EN);
+}
+
+static int pka_enable(uintptr_t base, uint32_t mode)
+{
+	/* Set mode and disable interrupts */
+	mmio_clrsetbits_32(base + _PKA_CR, _PKA_IT_MASK | _PKA_CR_MODE_MASK,
+			   _PKA_CR_MODE_MASK & (mode << _PKA_CR_MODE_SHIFT));
+
+	mmio_setbits_32(base + _PKA_CR, _PKA_CR_EN);
+
+	return pka_wait_bit(base, _PKA_SR_INITOK);
+}
+
+/*
+ * Data are already loaded in PKA internal RAM
+ * MODE is set
+ * We start process, and wait for its end.
+ */
+static int stm32_pka_process(uintptr_t base)
+{
+	mmio_setbits_32(base + _PKA_CR, _PKA_CR_START);
+
+	return pka_wait_bit(base, _PKA_IT_PROCEND);
+}
+
+/**
+ * @brief Write ECC operand to PKA RAM.
+ * @note  PKA expect to write u64 word, each u64 are: the least significant bit is
+ *        bit 0; the most significant bit is bit 63.
+ *        We write eo_nbw (ECC operand Size) u64, value that depends of the chosen
+ *        prime modulus length in bits.
+ *        First less signicant u64 is written to low address
+ *        Most significant u64 to higher address.
+ *        And at last address we write a u64(0x0)
+ * @note  This function doesn't only manage endianness (as bswap64 do), but also
+ *        complete most significant incomplete u64 with 0 (if data is not a u64
+ *        multiple), and fill u64 last address with 0.
+ * @param addr: PKA_RAM address to write the buffer 'data'
+ * @param data: is a BYTE list with most significant bytes first
+ * @param data_size: nb of byte in data
+ * @param eo_nbw: is ECC Operand size in 64bits word (including the extra 0)
+ *                (note it depends of the prime modulus length, not the data size)
+ * @retval 0 if OK.
+ *         -EINVAL if data_size and eo_nbw are inconsistent, ie data doesn't
+ *         fit in defined eo_nbw, or eo_nbw bigger than hardware limit.
+ */
+static int write_eo_data(uintptr_t addr, uint8_t *data, unsigned int data_size,
+			 unsigned int eo_nbw)
+{
+	uint32_t word_index;
+	int data_index;
+
+	if ((eo_nbw < OP_NBW_FROM_SIZE(data_size)) || (eo_nbw > MAX_EO_NBW)) {
+		return -EINVAL;
+	}
+
+	/* Fill value */
+	data_index = (int)data_size - 1;
+	for (word_index = 0U; word_index < eo_nbw; word_index++) {
+		uint64_t tmp = 0ULL;
+		unsigned int i = 0U;  /* index in the tmp U64 word */
+
+		/* Stop if end of tmp or end of data */
+		while ((i < sizeof(tmp)) && (data_index >= 0)) {
+			tmp |= (uint64_t)(data[data_index]) << (UINT8_LEN * i);
+			i++; /* Move byte index in current (u64)tmp */
+			data_index--; /* Move to just next most significat byte */
+		}
+
+		mmio_write_64(addr + word_index * sizeof(tmp), tmp);
+	}
+
+	return 0;
+}
+
+static unsigned int get_ecc_op_nbword(enum stm32_pka_ecdsa_curve_id cid)
+{
+	if (cid >= ARRAY_SIZE(curve_def)) {
+		ERROR("CID %u is out of boundaries\n", cid);
+		panic();
+	}
+
+	return OP_NBW_FROM_LEN(curve_def[cid].n_len);
+}
+
+static int stm32_pka_ecdsa_verif_configure_curve(uintptr_t base, enum stm32_pka_ecdsa_curve_id cid)
+{
+	int ret;
+	unsigned int eo_nbw = get_ecc_op_nbword(cid);
+
+	mmio_write_64(base + _PKA_RAM_N_LEN, curve_def[cid].n_len);
+	mmio_write_64(base + _PKA_RAM_P_LEN, curve_def[cid].p_len);
+	mmio_write_64(base + _PKA_RAM_A_SIGN, curve_def[cid].a_sign);
+
+	ret = write_eo_data(base + _PKA_RAM_A, curve_def[cid].a, curve_def[cid].a_size, eo_nbw);
+	if (ret < 0) {
+		return ret;
+	}
+
+	ret = write_eo_data(base + _PKA_RAM_PRIME_N,
+			    curve_def[cid].n, div_round_up(curve_def[cid].n_len, UINT8_LEN),
+			    eo_nbw);
+	if (ret < 0) {
+		return ret;
+	}
+
+	ret = write_eo_data(base + _PKA_RAM_P, curve_def[cid].p,
+			    div_round_up(curve_def[cid].p_len, UINT8_LEN), eo_nbw);
+	if (ret < 0) {
+		return ret;
+	}
+
+	ret = write_eo_data(base + _PKA_RAM_XG, curve_def[cid].xg, curve_def[cid].xg_size, eo_nbw);
+	if (ret < 0) {
+		return ret;
+	}
+
+	ret = write_eo_data(base + _PKA_RAM_YG, curve_def[cid].yg, curve_def[cid].yg_size, eo_nbw);
+	if (ret < 0) {
+		return ret;
+	}
+
+	return 0;
+}
+
+static int stm32_pka_ecdsa_verif_check_return(uintptr_t base)
+{
+	uint64_t value;
+	uint32_t sr;
+
+	sr = mmio_read_32(base + _PKA_SR);
+	if ((sr & (_PKA_IT_OPERR | _PKA_IT_ADDRERR | _PKA_IT_RAMERR)) != 0) {
+		WARN("Detected error(s): %s%s%s\n",
+		     (sr & _PKA_IT_OPERR) ? "Operation " : "",
+		     (sr & _PKA_IT_ADDRERR) ? "Address " : "",
+		     (sr & _PKA_IT_RAMERR) ? "RAM" : "");
+		return -EINVAL;
+	}
+
+	value = mmio_read_64(base + _PKA_RAM_ECDSA_VERIFY);
+	if (value == _PKA_RAM_ECDSA_VERIFY_VALID) {
+		return 0;
+	}
+
+	if (value == _PKA_RAM_ECDSA_VERIFY_INVALID) {
+		return -EAUTH;
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * @brief Check if BigInt stored in data is 0
+ *
+ * @param data: a BYTE array with most significant bytes first
+ * @param size: data size
+ *
+ * @retval: true: if data represents a 0 value (ie all bytes == 0)
+ *          false: if data represents a non-zero value.
+ */
+static bool is_zero(uint8_t *data, unsigned int size)
+{
+	unsigned int i;
+
+	for (i = 0U; i < size; i++) {
+		if (data[i] != 0U) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+/**
+ * @brief Compare two BigInt:
+ * @param xdata_a: a BYTE array with most significant bytes first
+ * @param size_a: nb of Byte of 'a'
+ * @param data_b: a BYTE array with most significant bytes first
+ * @param size_b: nb of Byte of 'b'
+ *
+ * @retval: true if data_a < data_b
+ *          false if data_a >= data_b
+ */
+static bool is_smaller(uint8_t *data_a, unsigned int size_a,
+		       uint8_t *data_b, unsigned int size_b)
+{
+	unsigned int i;
+
+	i = MAX(size_a, size_b) + 1U;
+	do {
+		uint8_t a, b;
+
+		i--;
+		if (size_a < i) {
+			a = 0U;
+		} else {
+			a = data_a[size_a - i];
+		}
+
+		if (size_b < i) {
+			b = 0U;
+		} else {
+			b = data_b[size_b - i];
+		}
+
+		if (a < b) {
+			return true;
+		}
+
+		if (a > b) {
+			return false;
+		}
+	} while (i != 0U);
+
+	return false;
+}
+
+static int stm32_pka_ecdsa_check_param(void *sig_r_ptr, unsigned int sig_r_size,
+				       void *sig_s_ptr, unsigned int sig_s_size,
+				       void *pk_x_ptr, unsigned int pk_x_size,
+				       void *pk_y_ptr, unsigned int pk_y_size,
+				       enum stm32_pka_ecdsa_curve_id cid)
+{
+	/* Public Key check */
+	/* Check Xq < p */
+	if (!is_smaller(pk_x_ptr, pk_x_size,
+			curve_def[cid].p, div_round_up(curve_def[cid].p_len, UINT8_LEN))) {
+		WARN("%s Xq < p inval\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Check Yq < p */
+	if (!is_smaller(pk_y_ptr, pk_y_size,
+			curve_def[cid].p, div_round_up(curve_def[cid].p_len, UINT8_LEN))) {
+		WARN("%s Yq < p inval\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Signature check */
+	/* Check 0 < r < n */
+	if (!is_smaller(sig_r_ptr, sig_r_size,
+			curve_def[cid].n, div_round_up(curve_def[cid].n_len, UINT8_LEN)) &&
+	    !is_zero(sig_r_ptr, sig_r_size)) {
+		WARN("%s 0< r < n inval\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Check 0 < s < n */
+	if (!is_smaller(sig_s_ptr, sig_s_size,
+			curve_def[cid].n, div_round_up(curve_def[cid].n_len, UINT8_LEN)) &&
+	    !is_zero(sig_s_ptr, sig_s_size)) {
+		WARN("%s 0< s < n inval\n", __func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  Initialize the PKA driver.
+ * @param  None.
+ * @retval 0 if OK, negative value else.
+ */
+int stm32_pka_init(void)
+{
+	int err;
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+	uint32_t ver;
+	uint32_t id;
+#endif
+
+	err = stm32_pka_parse_fdt();
+	if (err != 0) {
+		err = stm32_pka_get_platdata(&pka_pdata);
+		if (err != 0) {
+			return err;
+		}
+	}
+
+	clk_enable(pka_pdata.clock_id);
+
+	if (stm32mp_reset_assert((unsigned long)pka_pdata.reset_id, TIMEOUT_US_1MS) != 0) {
+		panic();
+	}
+
+	udelay(PKA_RESET_DELAY);
+	if (stm32mp_reset_deassert((unsigned long)pka_pdata.reset_id, TIMEOUT_US_1MS) != 0) {
+		panic();
+	}
+
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+	id = mmio_read_32(pka_pdata.base + _PKA_IPIDR);
+	ver = mmio_read_32(pka_pdata.base + _PKA_VERR);
+
+	VERBOSE("STM32 PKA[%x] V%u.%u\n", id,
+		(ver & _PKA_VERR_MAJREV_MASK) >> _PKA_VERR_MAJREV_SHIFT,
+		(ver & _PKA_VERR_MINREV_MASK) >> _PKA_VERR_MINREV_SHIFT);
+#endif
+	return 0;
+}
+
+int stm32_pka_ecdsa_verif(void *hash, unsigned int hash_size,
+			  void *sig_r_ptr, unsigned int sig_r_size,
+			  void *sig_s_ptr, unsigned int sig_s_size,
+			  void *pk_x_ptr, unsigned int pk_x_size,
+			  void *pk_y_ptr, unsigned int pk_y_size,
+			  enum stm32_pka_ecdsa_curve_id cid)
+{
+	int ret;
+	uintptr_t base = pka_pdata.base;
+	unsigned int eo_nbw = get_ecc_op_nbword(cid);
+
+	if ((hash == NULL) || (sig_r_ptr == NULL) || (sig_s_ptr == NULL) ||
+	    (pk_x_ptr == NULL) || (pk_y_ptr == NULL)) {
+		INFO("%s invalid input param\n", __func__);
+		return -EINVAL;
+	}
+
+	ret = stm32_pka_ecdsa_check_param(sig_r_ptr, sig_r_size,
+					  sig_s_ptr, sig_s_size,
+					  pk_x_ptr, pk_x_size,
+					  pk_y_ptr, pk_y_size,
+					  cid);
+	if (ret < 0) {
+		INFO("%s check param error %d\n", __func__, ret);
+		goto out;
+	}
+
+	if ((mmio_read_32(base + _PKA_SR) & _PKA_SR_BUSY) == _PKA_SR_BUSY) {
+		INFO("%s busy\n", __func__);
+		ret = -EBUSY;
+		goto out;
+	}
+
+	/* Fill PKA RAM */
+	/* With curve id values */
+	ret = stm32_pka_ecdsa_verif_configure_curve(base, cid);
+	if (ret < 0) {
+		goto out;
+	}
+
+	/* With pubkey */
+	ret = write_eo_data(base + _PKA_RAM_XQ, pk_x_ptr, pk_x_size, eo_nbw);
+	if (ret < 0) {
+		goto out;
+	}
+
+	ret = write_eo_data(base + _PKA_RAM_YQ, pk_y_ptr, pk_y_size, eo_nbw);
+	if (ret < 0) {
+		goto out;
+	}
+
+	/* With hash */
+	ret = write_eo_data(base + _PKA_RAM_HASH_Z, hash, hash_size, eo_nbw);
+	if (ret < 0) {
+		goto out;
+	}
+
+	/* With signature */
+	ret = write_eo_data(base + _PKA_RAM_SIGN_R, sig_r_ptr, sig_r_size, eo_nbw);
+	if (ret < 0) {
+		goto out;
+	}
+
+	ret = write_eo_data(base + _PKA_RAM_SIGN_S, sig_s_ptr, sig_s_size, eo_nbw);
+	if (ret < 0) {
+		goto out;
+	}
+
+	/* Set mode to ecdsa signature verification */
+	ret = pka_enable(base, _PKA_CR_MODE_ECDSA_VERIF);
+	if (ret < 0) {
+		WARN("%s set mode pka error %d\n", __func__, ret);
+		goto out;
+	}
+
+	/* Start processing and wait end */
+	ret = stm32_pka_process(base);
+	if (ret < 0) {
+		WARN("%s process error %d\n", __func__, ret);
+		goto out;
+	}
+
+	/* Check return status */
+	ret = stm32_pka_ecdsa_verif_check_return(base);
+
+	/* Unset end proc */
+	mmio_setbits_32(base + _PKA_CLRFR, _PKA_IT_PROCEND);
+
+out:
+	/* Disable PKA (will stop all pending proccess and reset RAM) */
+	pka_disable(base);
+
+	return ret;
+}
diff --git a/drivers/st/crypto/stm32_rng.c b/drivers/st/crypto/stm32_rng.c
new file mode 100644
index 0000000..a9dc43f
--- /dev/null
+++ b/drivers/st/crypto/stm32_rng.c
@@ -0,0 +1,269 @@
+/*
+ * Copyright (c) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include <arch_helpers.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_rng.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#if STM32_RNG_VER == 2
+#define DT_RNG_COMPAT		"st,stm32-rng"
+#endif
+#if STM32_RNG_VER == 4
+#define DT_RNG_COMPAT		"st,stm32mp13-rng"
+#endif
+#define RNG_CR			0x00U
+#define RNG_SR			0x04U
+#define RNG_DR			0x08U
+
+#define RNG_CR_RNGEN		BIT(2)
+#define RNG_CR_IE		BIT(3)
+#define RNG_CR_CED		BIT(5)
+#define RNG_CR_CLKDIV		GENMASK(19, 16)
+#define RNG_CR_CLKDIV_SHIFT	16U
+#define RNG_CR_CONDRST		BIT(30)
+
+#define RNG_SR_DRDY		BIT(0)
+#define RNG_SR_CECS		BIT(1)
+#define RNG_SR_SECS		BIT(2)
+#define RNG_SR_CEIS		BIT(5)
+#define RNG_SR_SEIS		BIT(6)
+
+#define RNG_TIMEOUT_US		100000U
+#define RNG_TIMEOUT_STEP_US	10U
+
+#define TIMEOUT_US_1MS		1000U
+
+#define RNG_NIST_CONFIG_A	0x00F40F00U
+#define RNG_NIST_CONFIG_B	0x01801000U
+#define RNG_NIST_CONFIG_C	0x00F00D00U
+#define RNG_NIST_CONFIG_MASK	GENMASK(25, 8)
+
+#define RNG_MAX_NOISE_CLK_FREQ	48000000U
+
+struct stm32_rng_instance {
+	uintptr_t base;
+	unsigned long clock;
+};
+
+static struct stm32_rng_instance stm32_rng;
+
+static void seed_error_recovery(void)
+{
+	uint8_t i __maybe_unused;
+
+	/* Recommended by the SoC reference manual */
+	mmio_clrbits_32(stm32_rng.base + RNG_SR, RNG_SR_SEIS);
+	dmbsy();
+
+#if STM32_RNG_VER == 2
+	/* No Auto-reset on version 2, need to clean FIFO */
+	for (i = 12U; i != 0U; i--) {
+		(void)mmio_read_32(stm32_rng.base + RNG_DR);
+	}
+
+	dmbsy();
+#endif
+
+	if ((mmio_read_32(stm32_rng.base + RNG_SR) & RNG_SR_SEIS) != 0U) {
+		ERROR("RNG noise\n");
+		panic();
+	}
+}
+
+static uint32_t stm32_rng_clock_freq_restrain(void)
+{
+	unsigned long clock_rate;
+	uint32_t clock_div = 0U;
+
+	clock_rate = clk_get_rate(stm32_rng.clock);
+
+	/*
+	 * Get the exponent to apply on the CLKDIV field in RNG_CR register
+	 * No need to handle the case when clock-div > 0xF as it is physically
+	 * impossible
+	 */
+	while ((clock_rate >> clock_div) > RNG_MAX_NOISE_CLK_FREQ) {
+		clock_div++;
+	}
+
+	VERBOSE("RNG clk rate : %lu\n", clk_get_rate(stm32_rng.clock) >> clock_div);
+
+	return clock_div;
+}
+
+static int stm32_rng_enable(void)
+{
+	uint32_t sr;
+	uint64_t timeout;
+	uint32_t clock_div __maybe_unused;
+
+#if STM32_RNG_VER == 2
+	mmio_write_32(stm32_rng.base + RNG_CR, RNG_CR_RNGEN | RNG_CR_CED);
+#endif
+#if STM32_RNG_VER == 4
+	/* Reset internal block and disable CED bit */
+	clock_div = stm32_rng_clock_freq_restrain();
+
+	/* Update configuration fields */
+	mmio_clrsetbits_32(stm32_rng.base + RNG_CR, RNG_NIST_CONFIG_MASK,
+			   RNG_NIST_CONFIG_A | RNG_CR_CONDRST | RNG_CR_CED);
+
+	mmio_clrsetbits_32(stm32_rng.base + RNG_CR, RNG_CR_CLKDIV,
+			   (clock_div << RNG_CR_CLKDIV_SHIFT));
+
+	mmio_clrsetbits_32(stm32_rng.base + RNG_CR, RNG_CR_CONDRST, RNG_CR_RNGEN);
+#endif
+	timeout = timeout_init_us(RNG_TIMEOUT_US);
+	sr = mmio_read_32(stm32_rng.base + RNG_SR);
+	while ((sr & RNG_SR_DRDY) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			WARN("Timeout waiting\n");
+			return -ETIMEDOUT;
+		}
+
+		if ((sr & (RNG_SR_SECS | RNG_SR_SEIS)) != 0U) {
+			seed_error_recovery();
+			timeout = timeout_init_us(RNG_TIMEOUT_US);
+		}
+
+		udelay(RNG_TIMEOUT_STEP_US);
+		sr = mmio_read_32(stm32_rng.base + RNG_SR);
+	}
+
+	VERBOSE("Init RNG done\n");
+
+	return 0;
+}
+
+/*
+ * stm32_rng_read - Read a number of random bytes from RNG
+ * out: pointer to the output buffer
+ * size: number of bytes to be read
+ * Return 0 on success, non-0 on failure
+ */
+int stm32_rng_read(uint8_t *out, uint32_t size)
+{
+	uint8_t *buf = out;
+	size_t len = size;
+	int nb_tries;
+	uint32_t data32;
+	int rc = 0;
+	unsigned int count;
+
+	if (stm32_rng.base == 0U) {
+		return -EPERM;
+	}
+
+	while (len != 0U) {
+		nb_tries = RNG_TIMEOUT_US / RNG_TIMEOUT_STEP_US;
+		do {
+			uint32_t status = mmio_read_32(stm32_rng.base + RNG_SR);
+
+			if ((status & (RNG_SR_SECS | RNG_SR_SEIS)) != 0U) {
+				seed_error_recovery();
+			}
+
+			udelay(RNG_TIMEOUT_STEP_US);
+			nb_tries--;
+			if (nb_tries == 0) {
+				rc = -ETIMEDOUT;
+				goto bail;
+			}
+		} while ((mmio_read_32(stm32_rng.base + RNG_SR) &
+			  RNG_SR_DRDY) == 0U);
+
+		count = 4U;
+		while (len != 0U) {
+			data32 = mmio_read_32(stm32_rng.base + RNG_DR);
+			count--;
+
+			memcpy(buf, &data32, MIN(len, sizeof(uint32_t)));
+			buf += MIN(len, sizeof(uint32_t));
+			len -= MIN(len, sizeof(uint32_t));
+
+			if (count == 0U) {
+				break;
+			}
+		}
+	}
+
+bail:
+	if (rc != 0) {
+		memset(out, 0, buf - out);
+	}
+
+	return rc;
+}
+
+/*
+ * stm32_rng_init: Initialize rng from DT
+ * return 0 on success, negative value on failure
+ */
+int stm32_rng_init(void)
+{
+	void *fdt;
+	struct dt_node_info dt_rng;
+	int node;
+
+	if (stm32_rng.base != 0U) {
+		/* Driver is already initialized */
+		return 0;
+	}
+
+	if (fdt_get_address(&fdt) == 0) {
+		panic();
+	}
+
+	node = dt_get_node(&dt_rng, -1, DT_RNG_COMPAT);
+	if (node < 0) {
+		return 0;
+	}
+
+	if (dt_rng.status == DT_DISABLED) {
+		return 0;
+	}
+
+	assert(dt_rng.base != 0U);
+
+	stm32_rng.base = dt_rng.base;
+
+	if (dt_rng.clock < 0) {
+		panic();
+	}
+
+	stm32_rng.clock = (unsigned long)dt_rng.clock;
+	clk_enable(stm32_rng.clock);
+
+	if (dt_rng.reset >= 0) {
+		int ret;
+
+		ret = stm32mp_reset_assert((unsigned long)dt_rng.reset,
+					   TIMEOUT_US_1MS);
+		if (ret != 0) {
+			panic();
+		}
+
+		udelay(20);
+
+		ret = stm32mp_reset_deassert((unsigned long)dt_rng.reset,
+					     TIMEOUT_US_1MS);
+		if (ret != 0) {
+			panic();
+		}
+	}
+
+	return stm32_rng_enable();
+}
diff --git a/drivers/st/crypto/stm32_saes.c b/drivers/st/crypto/stm32_saes.c
new file mode 100644
index 0000000..02baf21
--- /dev/null
+++ b/drivers/st/crypto/stm32_saes.c
@@ -0,0 +1,913 @@
+/*
+ * Copyright (c) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <assert.h>
+#include <endian.h>
+#include <errno.h>
+#include <stdint.h>
+
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_saes.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#define UINT8_BIT			8U
+#define AES_BLOCK_SIZE_BIT		128U
+#define AES_BLOCK_SIZE			(AES_BLOCK_SIZE_BIT / UINT8_BIT)
+
+#define AES_KEYSIZE_128			16U
+#define AES_KEYSIZE_256			32U
+#define AES_IVSIZE			16U
+
+/* SAES control register */
+#define _SAES_CR			0x0U
+/* SAES status register */
+#define _SAES_SR			0x04U
+/* SAES data input register */
+#define _SAES_DINR			0x08U
+/* SAES data output register */
+#define _SAES_DOUTR			0x0CU
+/* SAES key registers [0-3] */
+#define _SAES_KEYR0			0x10U
+#define _SAES_KEYR1			0x14U
+#define _SAES_KEYR2			0x18U
+#define _SAES_KEYR3			0x1CU
+/* SAES initialization vector registers [0-3] */
+#define _SAES_IVR0			0x20U
+#define _SAES_IVR1			0x24U
+#define _SAES_IVR2			0x28U
+#define _SAES_IVR3			0x2CU
+/* SAES key registers [4-7] */
+#define _SAES_KEYR4			0x30U
+#define _SAES_KEYR5			0x34U
+#define _SAES_KEYR6			0x38U
+#define _SAES_KEYR7			0x3CU
+/* SAES suspend registers [0-7] */
+#define _SAES_SUSPR0			0x40U
+#define _SAES_SUSPR1			0x44U
+#define _SAES_SUSPR2			0x48U
+#define _SAES_SUSPR3			0x4CU
+#define _SAES_SUSPR4			0x50U
+#define _SAES_SUSPR5			0x54U
+#define _SAES_SUSPR6			0x58U
+#define _SAES_SUSPR7			0x5CU
+/* SAES Interrupt Enable Register */
+#define _SAES_IER			0x300U
+/* SAES Interrupt Status Register */
+#define _SAES_ISR			0x304U
+/* SAES Interrupt Clear Register */
+#define _SAES_ICR			0x308U
+
+/* SAES control register fields */
+#define _SAES_CR_RESET_VALUE		0x0U
+#define _SAES_CR_IPRST			BIT(31)
+#define _SAES_CR_KEYSEL_MASK		GENMASK(30, 28)
+#define _SAES_CR_KEYSEL_SHIFT		28U
+#define _SAES_CR_KEYSEL_SOFT		0x0U
+#define _SAES_CR_KEYSEL_DHUK		0x1U
+#define _SAES_CR_KEYSEL_BHK		0x2U
+#define _SAES_CR_KEYSEL_BHU_XOR_BH_K	0x4U
+#define _SAES_CR_KEYSEL_TEST		0x7U
+#define _SAES_CR_KSHAREID_MASK		GENMASK(27, 26)
+#define _SAES_CR_KSHAREID_SHIFT		26U
+#define _SAES_CR_KSHAREID_CRYP		0x0U
+#define _SAES_CR_KEYMOD_MASK		GENMASK(25, 24)
+#define _SAES_CR_KEYMOD_SHIFT		24U
+#define _SAES_CR_KEYMOD_NORMAL		0x0U
+#define _SAES_CR_KEYMOD_WRAPPED		0x1U
+#define _SAES_CR_KEYMOD_SHARED		0x2U
+#define _SAES_CR_NPBLB_MASK		GENMASK(23, 20)
+#define _SAES_CR_NPBLB_SHIFT		20U
+#define _SAES_CR_KEYPROT		BIT(19)
+#define _SAES_CR_KEYSIZE		BIT(18)
+#define _SAES_CR_GCMPH_MASK		GENMASK(14, 13)
+#define _SAES_CR_GCMPH_SHIFT		13U
+#define _SAES_CR_GCMPH_INIT		0U
+#define _SAES_CR_GCMPH_HEADER		1U
+#define _SAES_CR_GCMPH_PAYLOAD		2U
+#define _SAES_CR_GCMPH_FINAL		3U
+#define _SAES_CR_DMAOUTEN		BIT(12)
+#define _SAES_CR_DMAINEN		BIT(11)
+#define _SAES_CR_CHMOD_MASK		(BIT(16) | GENMASK(6, 5))
+#define _SAES_CR_CHMOD_SHIFT		5U
+#define _SAES_CR_CHMOD_ECB		0x0U
+#define _SAES_CR_CHMOD_CBC		0x1U
+#define _SAES_CR_CHMOD_CTR		0x2U
+#define _SAES_CR_CHMOD_GCM		0x3U
+#define _SAES_CR_CHMOD_GMAC		0x3U
+#define _SAES_CR_CHMOD_CCM		0x800U
+#define _SAES_CR_MODE_MASK		GENMASK(4, 3)
+#define _SAES_CR_MODE_SHIFT		3U
+#define _SAES_CR_MODE_ENC		0U
+#define _SAES_CR_MODE_KEYPREP		1U
+#define _SAES_CR_MODE_DEC		2U
+#define _SAES_CR_DATATYPE_MASK		GENMASK(2, 1)
+#define _SAES_CR_DATATYPE_SHIFT		1U
+#define _SAES_CR_DATATYPE_NONE		0U
+#define _SAES_CR_DATATYPE_HALF_WORD	1U
+#define _SAES_CR_DATATYPE_BYTE		2U
+#define _SAES_CR_DATATYPE_BIT		3U
+#define _SAES_CR_EN			BIT(0)
+
+/* SAES status register fields */
+#define _SAES_SR_KEYVALID		BIT(7)
+#define _SAES_SR_BUSY			BIT(3)
+#define _SAES_SR_WRERR			BIT(2)
+#define _SAES_SR_RDERR			BIT(1)
+#define _SAES_SR_CCF			BIT(0)
+
+/* SAES interrupt registers fields */
+#define _SAES_I_RNG_ERR			BIT(3)
+#define _SAES_I_KEY_ERR			BIT(2)
+#define _SAES_I_RW_ERR			BIT(1)
+#define _SAES_I_CC			BIT(0)
+
+#define SAES_TIMEOUT_US			100000U
+#define TIMEOUT_US_1MS			1000U
+#define SAES_RESET_DELAY		20U
+
+#define IS_CHAINING_MODE(mod, cr) \
+	(((cr) & _SAES_CR_CHMOD_MASK) == (_SAES_CR_CHMOD_##mod << _SAES_CR_CHMOD_SHIFT))
+
+#define SET_CHAINING_MODE(mod, cr) \
+	mmio_clrsetbits_32((cr), _SAES_CR_CHMOD_MASK, _SAES_CR_CHMOD_##mod << _SAES_CR_CHMOD_SHIFT)
+
+#define pragma weak stm32_saes_get_platdata
+
+static struct stm32_saes_platdata saes_pdata;
+
+int stm32_saes_get_platdata(struct stm32_saes_platdata *pdata)
+{
+	return -ENODEV;
+}
+
+static int stm32_saes_parse_fdt(struct stm32_saes_platdata *pdata)
+{
+	int node;
+	struct dt_node_info info;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	node = dt_get_node(&info, -1, DT_SAES_COMPAT);
+	if (node < 0) {
+		ERROR("No SAES entry in DT\n");
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if (info.status == DT_DISABLED) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if ((info.base == 0U) || (info.clock < 0) || (info.reset < 0)) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	pdata->base = (uintptr_t)info.base;
+	pdata->clock_id = (unsigned long)info.clock;
+	pdata->reset_id = (unsigned int)info.reset;
+
+	return 0;
+}
+
+static bool does_chaining_mode_need_iv(uint32_t cr)
+{
+	return !(IS_CHAINING_MODE(ECB, cr));
+}
+
+static bool is_encrypt(uint32_t cr)
+{
+	return (cr & _SAES_CR_MODE_MASK) == (_SAES_CR_MODE_ENC << _SAES_CR_MODE_SHIFT);
+}
+
+static bool is_decrypt(uint32_t cr)
+{
+	return (cr & _SAES_CR_MODE_MASK) == (_SAES_CR_MODE_DEC << _SAES_CR_MODE_SHIFT);
+}
+
+static int wait_computation_completed(uintptr_t base)
+{
+	uint64_t timeout = timeout_init_us(SAES_TIMEOUT_US);
+
+	while ((mmio_read_32(base + _SAES_SR) & _SAES_SR_CCF) != _SAES_SR_CCF) {
+		if (timeout_elapsed(timeout)) {
+			WARN("%s: timeout\n", __func__);
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static void clear_computation_completed(uintptr_t base)
+{
+	mmio_setbits_32(base + _SAES_ICR, _SAES_I_CC);
+}
+
+static int saes_start(struct stm32_saes_context *ctx)
+{
+	uint64_t timeout;
+
+	/* Reset IP */
+	mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_IPRST);
+	udelay(SAES_RESET_DELAY);
+	mmio_clrbits_32(ctx->base + _SAES_CR, _SAES_CR_IPRST);
+
+	timeout = timeout_init_us(SAES_TIMEOUT_US);
+	while ((mmio_read_32(ctx->base + _SAES_SR) & _SAES_SR_BUSY) == _SAES_SR_BUSY) {
+		if (timeout_elapsed(timeout)) {
+			WARN("%s: timeout\n", __func__);
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static void saes_end(struct stm32_saes_context *ctx, int prev_error)
+{
+	if (prev_error != 0) {
+		/* Reset IP */
+		mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_IPRST);
+		udelay(SAES_RESET_DELAY);
+		mmio_clrbits_32(ctx->base + _SAES_CR, _SAES_CR_IPRST);
+	}
+
+	/* Disable the SAES peripheral */
+	mmio_clrbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+}
+
+static void saes_write_iv(struct stm32_saes_context *ctx)
+{
+	/* If chaining mode need to restore IV */
+	if (does_chaining_mode_need_iv(ctx->cr)) {
+		uint8_t i;
+
+		/* Restore the _SAES_IVRx */
+		for (i = 0U; i < AES_IVSIZE / sizeof(uint32_t); i++) {
+			mmio_write_32(ctx->base + _SAES_IVR0 + i * sizeof(uint32_t), ctx->iv[i]);
+		}
+	}
+
+}
+
+static void saes_write_key(struct stm32_saes_context *ctx)
+{
+	/* Restore the _SAES_KEYRx if SOFTWARE key */
+	if ((ctx->cr & _SAES_CR_KEYSEL_MASK) == (_SAES_CR_KEYSEL_SOFT << _SAES_CR_KEYSEL_SHIFT)) {
+		uint8_t i;
+
+		for (i = 0U; i < AES_KEYSIZE_128 / sizeof(uint32_t); i++) {
+			mmio_write_32(ctx->base + _SAES_KEYR0 + i * sizeof(uint32_t), ctx->key[i]);
+		}
+
+		if ((ctx->cr & _SAES_CR_KEYSIZE) == _SAES_CR_KEYSIZE) {
+			for (i = 0U; i < (AES_KEYSIZE_256 / 2U) / sizeof(uint32_t); i++) {
+				mmio_write_32(ctx->base + _SAES_KEYR4 + i * sizeof(uint32_t),
+					      ctx->key[i + 4U]);
+			}
+		}
+	}
+}
+
+static int saes_prepare_key(struct stm32_saes_context *ctx)
+{
+	/* Disable the SAES peripheral */
+	mmio_clrbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+
+	/* Set key size */
+	if ((ctx->cr & _SAES_CR_KEYSIZE) != 0U) {
+		mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_KEYSIZE);
+	} else {
+		mmio_clrbits_32(ctx->base + _SAES_CR, _SAES_CR_KEYSIZE);
+	}
+
+	saes_write_key(ctx);
+
+	/* For ECB/CBC decryption, key preparation mode must be selected to populate the key */
+	if ((IS_CHAINING_MODE(ECB, ctx->cr) || IS_CHAINING_MODE(CBC, ctx->cr)) &&
+	    is_decrypt(ctx->cr)) {
+		int ret;
+
+		/* Select Mode 2 */
+		mmio_clrsetbits_32(ctx->base + _SAES_CR, _SAES_CR_MODE_MASK,
+				   _SAES_CR_MODE_KEYPREP << _SAES_CR_MODE_SHIFT);
+
+		/* Enable SAES */
+		mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+
+		/* Wait Computation completed */
+		ret = wait_computation_completed(ctx->base);
+		if (ret != 0) {
+			return ret;
+		}
+
+		clear_computation_completed(ctx->base);
+
+		/* Set Mode 3 */
+		mmio_clrsetbits_32(ctx->base + _SAES_CR, _SAES_CR_MODE_MASK,
+				   _SAES_CR_MODE_DEC << _SAES_CR_MODE_SHIFT);
+	}
+
+	return 0;
+}
+
+static int save_context(struct stm32_saes_context *ctx)
+{
+	if ((mmio_read_32(ctx->base + _SAES_SR) & _SAES_SR_CCF) != 0U) {
+		/* Device should not be in a processing phase */
+		return -EINVAL;
+	}
+
+	/* Save CR */
+	ctx->cr = mmio_read_32(ctx->base + _SAES_CR);
+
+	/* If chaining mode need to save current IV */
+	if (does_chaining_mode_need_iv(ctx->cr)) {
+		uint8_t i;
+
+		/* Save IV */
+		for (i = 0U; i < AES_IVSIZE / sizeof(uint32_t); i++) {
+			ctx->iv[i] = mmio_read_32(ctx->base + _SAES_IVR0 + i * sizeof(uint32_t));
+		}
+	}
+
+	/* Disable the SAES peripheral */
+	mmio_clrbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+
+	return 0;
+}
+
+/* To resume the processing of a message */
+static int restore_context(struct stm32_saes_context *ctx)
+{
+	int ret;
+
+	/* IP should be disabled */
+	if ((mmio_read_32(ctx->base + _SAES_CR) & _SAES_CR_EN) != 0U) {
+		VERBOSE("%s: Device is still enabled\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Reset internal state */
+	mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_IPRST);
+
+	/* Restore the _SAES_CR */
+	mmio_write_32(ctx->base + _SAES_CR, ctx->cr);
+
+	/* Preparation decrypt key */
+	ret = saes_prepare_key(ctx);
+	if (ret != 0) {
+		return ret;
+	}
+
+	saes_write_iv(ctx);
+
+	/* Enable the SAES peripheral */
+	mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+
+	return 0;
+}
+
+/**
+ * @brief Initialize SAES driver.
+ * @param None.
+ * @retval 0 if OK; negative value else.
+ */
+int stm32_saes_driver_init(void)
+{
+	int err;
+
+	err = stm32_saes_parse_fdt(&saes_pdata);
+	if (err != 0) {
+		err = stm32_saes_get_platdata(&saes_pdata);
+		if (err != 0) {
+			return err;
+		}
+	}
+
+	clk_enable(saes_pdata.clock_id);
+	if (stm32mp_reset_assert(saes_pdata.reset_id, TIMEOUT_US_1MS) != 0) {
+		panic();
+	}
+
+	udelay(SAES_RESET_DELAY);
+	if (stm32mp_reset_deassert(saes_pdata.reset_id, TIMEOUT_US_1MS) != 0) {
+		panic();
+	}
+
+	return 0;
+}
+
+/**
+ * @brief Start a AES computation.
+ * @param ctx: SAES process context
+ * @param is_dec: true if decryption, false if encryption
+ * @param ch_mode: define the chaining mode
+ * @param key_select: define where the key comes from.
+ * @param key: pointer to key (if key_select is KEY_SOFT, else unused)
+ * @param key_size: key size
+ * @param iv: pointer to initialization vectore (unsed if ch_mode is ECB)
+ * @param iv_size: iv size
+ * @note this function doesn't access to hardware but store in ctx the values
+ *
+ * @retval 0 if OK; negative value else.
+ */
+int stm32_saes_init(struct stm32_saes_context *ctx, bool is_dec,
+		    enum stm32_saes_chaining_mode ch_mode, enum stm32_saes_key_selection key_select,
+		    const void *key, size_t key_size, const void *iv, size_t iv_size)
+{
+	unsigned int i;
+	const uint32_t *iv_u32;
+	const uint32_t *key_u32;
+
+	ctx->assoc_len = 0U;
+	ctx->load_len = 0U;
+
+	ctx->base = saes_pdata.base;
+	ctx->cr = _SAES_CR_RESET_VALUE;
+
+	/* We want buffer to be u32 aligned */
+	assert((uintptr_t)key % __alignof__(uint32_t) == 0);
+	assert((uintptr_t)iv % __alignof__(uint32_t) == 0);
+
+	iv_u32 = iv;
+	key_u32 = key;
+
+	if (is_dec) {
+		/* Save Mode 3 = decrypt */
+		mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_MODE_MASK,
+				   _SAES_CR_MODE_DEC << _SAES_CR_MODE_SHIFT);
+	} else {
+		/* Save Mode 1 = crypt */
+		mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_MODE_MASK,
+				   _SAES_CR_MODE_ENC << _SAES_CR_MODE_SHIFT);
+	}
+
+	/* Save chaining mode */
+	switch (ch_mode) {
+	case STM32_SAES_MODE_ECB:
+		SET_CHAINING_MODE(ECB, (uintptr_t)&(ctx->cr));
+		break;
+	case STM32_SAES_MODE_CBC:
+		SET_CHAINING_MODE(CBC, (uintptr_t)&(ctx->cr));
+		break;
+	case STM32_SAES_MODE_CTR:
+		SET_CHAINING_MODE(CTR, (uintptr_t)&(ctx->cr));
+		break;
+	case STM32_SAES_MODE_GCM:
+		SET_CHAINING_MODE(GCM, (uintptr_t)&(ctx->cr));
+		break;
+	case STM32_SAES_MODE_CCM:
+		SET_CHAINING_MODE(CCM, (uintptr_t)&(ctx->cr));
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* We will use HW Byte swap (_SAES_CR_DATATYPE_BYTE) for data.
+	 * so we won't need to
+	 * htobe32(data) before write to DINR
+	 * nor
+	 * be32toh after reading from DOUTR
+	 *
+	 * But note that wrap key only accept _SAES_CR_DATATYPE_NONE
+	 */
+	mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_DATATYPE_MASK,
+			   _SAES_CR_DATATYPE_BYTE << _SAES_CR_DATATYPE_SHIFT);
+
+	/* Configure keysize */
+	switch (key_size) {
+	case AES_KEYSIZE_128:
+		mmio_clrbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSIZE);
+		break;
+	case AES_KEYSIZE_256:
+		mmio_setbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSIZE);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Configure key */
+	switch (key_select) {
+	case STM32_SAES_KEY_SOFT:
+		mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSEL_MASK,
+				   _SAES_CR_KEYSEL_SOFT << _SAES_CR_KEYSEL_SHIFT);
+		/* Save key */
+		switch (key_size) {
+		case AES_KEYSIZE_128:
+			/* First 16 bytes == 4 u32 */
+			for (i = 0U; i < AES_KEYSIZE_128 / sizeof(uint32_t); i++) {
+				mmio_write_32((uintptr_t)(ctx->key + i), htobe32(key_u32[3 - i]));
+				/* /!\ we save the key in HW byte order
+				 * and word order : key[i] is for _SAES_KEYRi
+				 */
+			}
+			break;
+		case AES_KEYSIZE_256:
+			for (i = 0U; i < AES_KEYSIZE_256 / sizeof(uint32_t); i++) {
+				mmio_write_32((uintptr_t)(ctx->key + i), htobe32(key_u32[7 - i]));
+				/* /!\ we save the key in HW byte order
+				 * and word order : key[i] is for _SAES_KEYRi
+				 */
+			}
+			break;
+		default:
+			return -EINVAL;
+		}
+
+		break;
+	case STM32_SAES_KEY_DHU:
+		mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSEL_MASK,
+				   _SAES_CR_KEYSEL_DHUK << _SAES_CR_KEYSEL_SHIFT);
+		break;
+	case STM32_SAES_KEY_BH:
+		mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSEL_MASK,
+				   _SAES_CR_KEYSEL_BHK << _SAES_CR_KEYSEL_SHIFT);
+		break;
+	case STM32_SAES_KEY_BHU_XOR_BH:
+		mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSEL_MASK,
+				   _SAES_CR_KEYSEL_BHU_XOR_BH_K << _SAES_CR_KEYSEL_SHIFT);
+		break;
+	case STM32_SAES_KEY_WRAPPED:
+		mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSEL_MASK,
+				   _SAES_CR_KEYSEL_SOFT << _SAES_CR_KEYSEL_SHIFT);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	/* Save IV */
+	if (ch_mode != STM32_SAES_MODE_ECB) {
+		if ((iv == NULL) || (iv_size != AES_IVSIZE)) {
+			return -EINVAL;
+		}
+
+		for (i = 0U; i < AES_IVSIZE / sizeof(uint32_t); i++) {
+			mmio_write_32((uintptr_t)(ctx->iv + i), htobe32(iv_u32[3 - i]));
+			/* /!\ We save the iv in HW byte order */
+		}
+	}
+
+	return saes_start(ctx);
+}
+
+/**
+ * @brief Update (or start) a AES authentificate process of associated data (CCM or GCM).
+ * @param ctx: SAES process context
+ * @param last_block: true if last assoc data block
+ * @param data: pointer to associated data
+ * @param data_size: data size
+ *
+ * @retval 0 if OK; negative value else.
+ */
+int stm32_saes_update_assodata(struct stm32_saes_context *ctx, bool last_block,
+			       uint8_t *data, size_t data_size)
+{
+	int ret;
+	uint32_t *data_u32;
+	unsigned int i = 0U;
+
+	/* We want buffers to be u32 aligned */
+	assert((uintptr_t)data % __alignof__(uint32_t) == 0);
+	data_u32 = (uint32_t *)data;
+
+	/* Init phase */
+	ret = restore_context(ctx);
+	if (ret != 0) {
+		goto out;
+	}
+
+	ret = wait_computation_completed(ctx->base);
+	if (ret != 0) {
+		return ret;
+	}
+
+	clear_computation_completed(ctx->base);
+
+	if ((data == NULL) || (data_size == 0U)) {
+		/* No associated data */
+		/* ret already = 0 */
+		goto out;
+	}
+
+	/* There is an header/associated data phase */
+	mmio_clrsetbits_32(ctx->base + _SAES_CR, _SAES_CR_GCMPH_MASK,
+			   _SAES_CR_GCMPH_HEADER << _SAES_CR_GCMPH_SHIFT);
+
+	/* Enable the SAES peripheral */
+	mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+
+	while (i < round_down(data_size, AES_BLOCK_SIZE)) {
+		unsigned int w; /* Word index */
+
+		w = i / sizeof(uint32_t);
+		/* No need to htobe() as we configure the HW to swap bytes */
+		mmio_write_32(ctx->base + _SAES_DINR, data_u32[w + 0U]);
+		mmio_write_32(ctx->base + _SAES_DINR, data_u32[w + 1U]);
+		mmio_write_32(ctx->base + _SAES_DINR, data_u32[w + 2U]);
+		mmio_write_32(ctx->base + _SAES_DINR, data_u32[w + 3U]);
+
+		ret = wait_computation_completed(ctx->base);
+		if (ret != 0) {
+			goto out;
+		}
+
+		clear_computation_completed(ctx->base);
+
+		/* Process next block */
+		i += AES_BLOCK_SIZE;
+		ctx->assoc_len += AES_BLOCK_SIZE_BIT;
+	}
+
+	/* Manage last block if not a block size multiple */
+	if ((last_block) && (i < data_size)) {
+		/* We don't manage unaligned last block yet */
+		ret = -ENODEV;
+		goto out;
+	}
+
+out:
+	if (ret != 0) {
+		saes_end(ctx, ret);
+	}
+
+	return ret;
+}
+
+/**
+ * @brief Update (or start) a AES authenticate and de/encrypt with payload data (CCM or GCM).
+ * @param ctx: SAES process context
+ * @param last_block: true if last payload data block
+ * @param data_in: pointer to payload
+ * @param data_out: pointer where to save de/encrypted payload
+ * @param data_size: payload size
+ *
+ * @retval 0 if OK; negative value else.
+ */
+int stm32_saes_update_load(struct stm32_saes_context *ctx, bool last_block,
+			   uint8_t *data_in, uint8_t *data_out, size_t data_size)
+{
+	int ret = 0;
+	uint32_t *data_in_u32;
+	uint32_t *data_out_u32;
+	unsigned int i = 0U;
+	uint32_t prev_cr;
+
+	/* We want buffers to be u32 aligned */
+	assert((uintptr_t)data_in % __alignof__(uint32_t) == 0);
+	assert((uintptr_t)data_out % __alignof__(uint32_t) == 0);
+	data_in_u32 = (uint32_t *)data_in;
+	data_out_u32 = (uint32_t *)data_out;
+
+	prev_cr = mmio_read_32(ctx->base + _SAES_CR);
+
+	if ((data_in == NULL) || (data_size == 0U)) {
+		/* there is no data */
+		goto out;
+	}
+
+	/* There is a load phase */
+	mmio_clrsetbits_32(ctx->base + _SAES_CR, _SAES_CR_GCMPH_MASK,
+			   _SAES_CR_GCMPH_PAYLOAD << _SAES_CR_GCMPH_SHIFT);
+
+	if ((prev_cr & _SAES_CR_GCMPH_MASK) ==
+	    (_SAES_CR_GCMPH_INIT << _SAES_CR_GCMPH_SHIFT)) {
+		/* Still in initialization phase, no header
+		 * We need to enable the SAES peripheral
+		 */
+		mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+	}
+
+	while (i < round_down(data_size, AES_BLOCK_SIZE)) {
+		unsigned int w; /* Word index */
+
+		w = i / sizeof(uint32_t);
+		/* No need to htobe() as we configure the HW to swap bytes */
+		mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 0U]);
+		mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 1U]);
+		mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 2U]);
+		mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 3U]);
+
+		ret = wait_computation_completed(ctx->base);
+		if (ret != 0) {
+			goto out;
+		}
+
+		/* No need to htobe() as we configure the HW to swap bytes */
+		data_out_u32[w + 0U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+		data_out_u32[w + 1U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+		data_out_u32[w + 2U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+		data_out_u32[w + 3U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+
+		clear_computation_completed(ctx->base);
+
+		/* Process next block */
+		i += AES_BLOCK_SIZE;
+		ctx->load_len += AES_BLOCK_SIZE_BIT;
+	}
+	/* Manage last block if not a block size multiple */
+	if ((last_block) && (i < data_size)) {
+		uint32_t block_in[AES_BLOCK_SIZE / sizeof(uint32_t)] = {0};
+		uint32_t block_out[AES_BLOCK_SIZE / sizeof(uint32_t)] = {0};
+
+		memcpy(block_in, data_in + i, data_size - i);
+
+		/* No need to htobe() as we configure the HW to swap bytes */
+		mmio_write_32(ctx->base + _SAES_DINR, block_in[0U]);
+		mmio_write_32(ctx->base + _SAES_DINR, block_in[1U]);
+		mmio_write_32(ctx->base + _SAES_DINR, block_in[2U]);
+		mmio_write_32(ctx->base + _SAES_DINR, block_in[3U]);
+
+		ret = wait_computation_completed(ctx->base);
+		if (ret != 0) {
+			VERBOSE("%s %d\n", __func__, __LINE__);
+			goto out;
+		}
+
+		/* No need to htobe() as we configure the HW to swap bytes */
+		block_out[0U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+		block_out[1U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+		block_out[2U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+		block_out[3U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+
+		clear_computation_completed(ctx->base);
+
+		memcpy(data_out + i, block_out, data_size - i);
+
+		ctx->load_len += (data_size - i) * UINT8_BIT;
+	}
+
+out:
+	if (ret != 0) {
+		saes_end(ctx, ret);
+	}
+
+	return ret;
+}
+
+/**
+ * @brief Get authentication tag for AES authenticated algorithms (CCM or GCM).
+ * @param ctx: SAES process context
+ * @param tag: pointer where to save the tag
+ * @param data_size: tag size
+ *
+ * @retval 0 if OK; negative value else.
+ */
+int stm32_saes_final(struct stm32_saes_context *ctx, uint8_t *tag,
+		     size_t tag_size)
+{
+	int ret;
+	uint32_t tag_u32[4];
+	uint32_t prev_cr;
+
+	prev_cr = mmio_read_32(ctx->base + _SAES_CR);
+
+	mmio_clrsetbits_32(ctx->base + _SAES_CR, _SAES_CR_GCMPH_MASK,
+			   _SAES_CR_GCMPH_FINAL << _SAES_CR_GCMPH_SHIFT);
+
+	if ((prev_cr & _SAES_CR_GCMPH_MASK) == (_SAES_CR_GCMPH_INIT << _SAES_CR_GCMPH_SHIFT)) {
+		/* Still in initialization phase, no header
+		 * We need to enable the SAES peripheral
+		 */
+		mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+	}
+
+	/* No need to htobe() as we configure the HW to swap bytes */
+	mmio_write_32(ctx->base + _SAES_DINR, 0);
+	mmio_write_32(ctx->base + _SAES_DINR, ctx->assoc_len);
+	mmio_write_32(ctx->base + _SAES_DINR, 0);
+	mmio_write_32(ctx->base + _SAES_DINR, ctx->load_len);
+
+	ret = wait_computation_completed(ctx->base);
+	if (ret != 0) {
+		goto out;
+	}
+
+	/* No need to htobe() as we configure the HW to swap bytes */
+	tag_u32[0] = mmio_read_32(ctx->base + _SAES_DOUTR);
+	tag_u32[1] = mmio_read_32(ctx->base + _SAES_DOUTR);
+	tag_u32[2] = mmio_read_32(ctx->base + _SAES_DOUTR);
+	tag_u32[3] = mmio_read_32(ctx->base + _SAES_DOUTR);
+
+	clear_computation_completed(ctx->base);
+
+	memcpy(tag, tag_u32, MIN(sizeof(tag_u32), tag_size));
+
+out:
+	saes_end(ctx, ret);
+
+	return ret;
+}
+
+/**
+ * @brief Update (or start) a AES de/encrypt process (ECB, CBC or CTR).
+ * @param ctx: SAES process context
+ * @param last_block: true if last payload data block
+ * @param data_in: pointer to payload
+ * @param data_out: pointer where to save de/encrypted payload
+ * @param data_size: payload size
+ *
+ * @retval 0 if OK; negative value else.
+ */
+int stm32_saes_update(struct stm32_saes_context *ctx, bool last_block,
+		      uint8_t *data_in, uint8_t *data_out, size_t data_size)
+{
+	int ret;
+	uint32_t *data_in_u32;
+	uint32_t *data_out_u32;
+	unsigned int i = 0U;
+
+	/* We want buffers to be u32 aligned */
+	assert((uintptr_t)data_in % __alignof__(uint32_t) == 0);
+	assert((uintptr_t)data_out % __alignof__(uint32_t) == 0);
+	data_in_u32 = (uint32_t *)data_in;
+	data_out_u32 = (uint32_t *)data_out;
+
+	if ((!last_block) &&
+	    (round_down(data_size, AES_BLOCK_SIZE) != data_size)) {
+		ERROR("%s: non last block must be multiple of 128 bits\n",
+		      __func__);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* In CBC encryption we need to manage specifically last 2 128bits
+	 * blocks if total size in not a block size aligned
+	 * work TODO. Currently return ENODEV.
+	 * Morevoer as we need to know last 2 block, if unaligned and
+	 * call with less than two block, return -EINVAL.
+	 */
+	if (last_block && IS_CHAINING_MODE(CBC, ctx->cr) && is_encrypt(ctx->cr) &&
+	    (round_down(data_size, AES_BLOCK_SIZE) != data_size)) {
+		if (data_size < AES_BLOCK_SIZE * 2U) {
+			ERROR("if CBC, last part size should be at least 2 * AES_BLOCK_SIZE\n");
+			ret = -EINVAL;
+			goto out;
+		}
+		/* Moreover the CBC specific padding for encrypt is not yet implemented */
+		ret = -ENODEV;
+		goto out;
+	}
+
+	ret = restore_context(ctx);
+	if (ret != 0) {
+		goto out;
+	}
+
+	while (i < round_down(data_size, AES_BLOCK_SIZE)) {
+		unsigned int w; /* Word index */
+
+		w = i / sizeof(uint32_t);
+		/* No need to htobe() as we configure the HW to swap bytes */
+		mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 0U]);
+		mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 1U]);
+		mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 2U]);
+		mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 3U]);
+
+		ret = wait_computation_completed(ctx->base);
+		if (ret != 0) {
+			goto out;
+		}
+
+		/* No need to htobe() as we configure the HW to swap bytes */
+		data_out_u32[w + 0U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+		data_out_u32[w + 1U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+		data_out_u32[w + 2U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+		data_out_u32[w + 3U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+
+		clear_computation_completed(ctx->base);
+
+		/* Process next block */
+		i += AES_BLOCK_SIZE;
+	}
+	/* Manage last block if not a block size multiple */
+
+	if ((last_block) && (i < data_size)) {
+		/* In and out buffer have same size so should be AES_BLOCK_SIZE multiple */
+		ret = -ENODEV;
+		goto out;
+	}
+
+	if (!last_block) {
+		ret = save_context(ctx);
+	}
+
+out:
+	/* If last block or error, end of SAES process */
+	if (last_block || (ret != 0)) {
+		saes_end(ctx, ret);
+	}
+
+	return ret;
+}
diff --git a/drivers/st/ddr/stm32mp1_ddr.c b/drivers/st/ddr/stm32mp1_ddr.c
new file mode 100644
index 0000000..4719e1e
--- /dev/null
+++ b/drivers/st/ddr/stm32mp1_ddr.c
@@ -0,0 +1,764 @@
+/*
+ * Copyright (C) 2018-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <stddef.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp1_ddr.h>
+#include <drivers/st/stm32mp1_ddr_regs.h>
+#include <drivers/st/stm32mp1_pwr.h>
+#include <drivers/st/stm32mp1_ram.h>
+#include <drivers/st/stm32mp_ddr.h>
+#include <lib/mmio.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+#define DDRCTL_REG(x, y)					\
+	{							\
+		.name = #x,					\
+		.offset = offsetof(struct stm32mp_ddrctl, x),	\
+		.par_offset = offsetof(struct y, x)		\
+	}
+
+#define DDRPHY_REG(x, y)					\
+	{							\
+		.name = #x,					\
+		.offset = offsetof(struct stm32mp_ddrphy, x),	\
+		.par_offset = offsetof(struct y, x)		\
+	}
+
+/*
+ * PARAMETERS: value get from device tree :
+ *             size / order need to be aligned with binding
+ *             modification NOT ALLOWED !!!
+ */
+#define DDRCTL_REG_REG_SIZE	25	/* st,ctl-reg */
+#define DDRCTL_REG_TIMING_SIZE	12	/* st,ctl-timing */
+#define DDRCTL_REG_MAP_SIZE	9	/* st,ctl-map */
+#if STM32MP_DDR_DUAL_AXI_PORT
+#define DDRCTL_REG_PERF_SIZE	17	/* st,ctl-perf */
+#else
+#define DDRCTL_REG_PERF_SIZE	11	/* st,ctl-perf */
+#endif
+
+#if STM32MP_DDR_32BIT_INTERFACE
+#define DDRPHY_REG_REG_SIZE	11	/* st,phy-reg */
+#else
+#define DDRPHY_REG_REG_SIZE	9	/* st,phy-reg */
+#endif
+#define	DDRPHY_REG_TIMING_SIZE	10	/* st,phy-timing */
+
+#define DDRCTL_REG_REG(x)	DDRCTL_REG(x, stm32mp1_ddrctrl_reg)
+static const struct stm32mp_ddr_reg_desc ddr_reg[DDRCTL_REG_REG_SIZE] = {
+	DDRCTL_REG_REG(mstr),
+	DDRCTL_REG_REG(mrctrl0),
+	DDRCTL_REG_REG(mrctrl1),
+	DDRCTL_REG_REG(derateen),
+	DDRCTL_REG_REG(derateint),
+	DDRCTL_REG_REG(pwrctl),
+	DDRCTL_REG_REG(pwrtmg),
+	DDRCTL_REG_REG(hwlpctl),
+	DDRCTL_REG_REG(rfshctl0),
+	DDRCTL_REG_REG(rfshctl3),
+	DDRCTL_REG_REG(crcparctl0),
+	DDRCTL_REG_REG(zqctl0),
+	DDRCTL_REG_REG(dfitmg0),
+	DDRCTL_REG_REG(dfitmg1),
+	DDRCTL_REG_REG(dfilpcfg0),
+	DDRCTL_REG_REG(dfiupd0),
+	DDRCTL_REG_REG(dfiupd1),
+	DDRCTL_REG_REG(dfiupd2),
+	DDRCTL_REG_REG(dfiphymstr),
+	DDRCTL_REG_REG(odtmap),
+	DDRCTL_REG_REG(dbg0),
+	DDRCTL_REG_REG(dbg1),
+	DDRCTL_REG_REG(dbgcmd),
+	DDRCTL_REG_REG(poisoncfg),
+	DDRCTL_REG_REG(pccfg),
+};
+
+#define DDRCTL_REG_TIMING(x)	DDRCTL_REG(x, stm32mp1_ddrctrl_timing)
+static const struct stm32mp_ddr_reg_desc ddr_timing[DDRCTL_REG_TIMING_SIZE] = {
+	DDRCTL_REG_TIMING(rfshtmg),
+	DDRCTL_REG_TIMING(dramtmg0),
+	DDRCTL_REG_TIMING(dramtmg1),
+	DDRCTL_REG_TIMING(dramtmg2),
+	DDRCTL_REG_TIMING(dramtmg3),
+	DDRCTL_REG_TIMING(dramtmg4),
+	DDRCTL_REG_TIMING(dramtmg5),
+	DDRCTL_REG_TIMING(dramtmg6),
+	DDRCTL_REG_TIMING(dramtmg7),
+	DDRCTL_REG_TIMING(dramtmg8),
+	DDRCTL_REG_TIMING(dramtmg14),
+	DDRCTL_REG_TIMING(odtcfg),
+};
+
+#define DDRCTL_REG_MAP(x)	DDRCTL_REG(x, stm32mp1_ddrctrl_map)
+static const struct stm32mp_ddr_reg_desc ddr_map[DDRCTL_REG_MAP_SIZE] = {
+	DDRCTL_REG_MAP(addrmap1),
+	DDRCTL_REG_MAP(addrmap2),
+	DDRCTL_REG_MAP(addrmap3),
+	DDRCTL_REG_MAP(addrmap4),
+	DDRCTL_REG_MAP(addrmap5),
+	DDRCTL_REG_MAP(addrmap6),
+	DDRCTL_REG_MAP(addrmap9),
+	DDRCTL_REG_MAP(addrmap10),
+	DDRCTL_REG_MAP(addrmap11),
+};
+
+#define DDRCTL_REG_PERF(x)	DDRCTL_REG(x, stm32mp1_ddrctrl_perf)
+static const struct stm32mp_ddr_reg_desc ddr_perf[DDRCTL_REG_PERF_SIZE] = {
+	DDRCTL_REG_PERF(sched),
+	DDRCTL_REG_PERF(sched1),
+	DDRCTL_REG_PERF(perfhpr1),
+	DDRCTL_REG_PERF(perflpr1),
+	DDRCTL_REG_PERF(perfwr1),
+	DDRCTL_REG_PERF(pcfgr_0),
+	DDRCTL_REG_PERF(pcfgw_0),
+	DDRCTL_REG_PERF(pcfgqos0_0),
+	DDRCTL_REG_PERF(pcfgqos1_0),
+	DDRCTL_REG_PERF(pcfgwqos0_0),
+	DDRCTL_REG_PERF(pcfgwqos1_0),
+#if STM32MP_DDR_DUAL_AXI_PORT
+	DDRCTL_REG_PERF(pcfgr_1),
+	DDRCTL_REG_PERF(pcfgw_1),
+	DDRCTL_REG_PERF(pcfgqos0_1),
+	DDRCTL_REG_PERF(pcfgqos1_1),
+	DDRCTL_REG_PERF(pcfgwqos0_1),
+	DDRCTL_REG_PERF(pcfgwqos1_1),
+#endif
+};
+
+#define DDRPHY_REG_REG(x)	DDRPHY_REG(x, stm32mp1_ddrphy_reg)
+static const struct stm32mp_ddr_reg_desc ddrphy_reg[DDRPHY_REG_REG_SIZE] = {
+	DDRPHY_REG_REG(pgcr),
+	DDRPHY_REG_REG(aciocr),
+	DDRPHY_REG_REG(dxccr),
+	DDRPHY_REG_REG(dsgcr),
+	DDRPHY_REG_REG(dcr),
+	DDRPHY_REG_REG(odtcr),
+	DDRPHY_REG_REG(zq0cr1),
+	DDRPHY_REG_REG(dx0gcr),
+	DDRPHY_REG_REG(dx1gcr),
+#if STM32MP_DDR_32BIT_INTERFACE
+	DDRPHY_REG_REG(dx2gcr),
+	DDRPHY_REG_REG(dx3gcr),
+#endif
+};
+
+#define DDRPHY_REG_TIMING(x)	DDRPHY_REG(x, stm32mp1_ddrphy_timing)
+static const struct stm32mp_ddr_reg_desc ddrphy_timing[DDRPHY_REG_TIMING_SIZE] = {
+	DDRPHY_REG_TIMING(ptr0),
+	DDRPHY_REG_TIMING(ptr1),
+	DDRPHY_REG_TIMING(ptr2),
+	DDRPHY_REG_TIMING(dtpr0),
+	DDRPHY_REG_TIMING(dtpr1),
+	DDRPHY_REG_TIMING(dtpr2),
+	DDRPHY_REG_TIMING(mr0),
+	DDRPHY_REG_TIMING(mr1),
+	DDRPHY_REG_TIMING(mr2),
+	DDRPHY_REG_TIMING(mr3),
+};
+
+/*
+ * REGISTERS ARRAY: used to parse device tree and interactive mode
+ */
+static const struct stm32mp_ddr_reg_info ddr_registers[REG_TYPE_NB] = {
+	[REG_REG] = {
+		.name = "static",
+		.desc = ddr_reg,
+		.size = DDRCTL_REG_REG_SIZE,
+		.base = DDR_BASE
+	},
+	[REG_TIMING] = {
+		.name = "timing",
+		.desc = ddr_timing,
+		.size = DDRCTL_REG_TIMING_SIZE,
+		.base = DDR_BASE
+	},
+	[REG_PERF] = {
+		.name = "perf",
+		.desc = ddr_perf,
+		.size = DDRCTL_REG_PERF_SIZE,
+		.base = DDR_BASE
+	},
+	[REG_MAP] = {
+		.name = "map",
+		.desc = ddr_map,
+		.size = DDRCTL_REG_MAP_SIZE,
+		.base = DDR_BASE
+	},
+	[REGPHY_REG] = {
+		.name = "static",
+		.desc = ddrphy_reg,
+		.size = DDRPHY_REG_REG_SIZE,
+		.base = DDRPHY_BASE
+	},
+	[REGPHY_TIMING] = {
+		.name = "timing",
+		.desc = ddrphy_timing,
+		.size = DDRPHY_REG_TIMING_SIZE,
+		.base = DDRPHY_BASE
+	},
+};
+
+static void stm32mp1_ddrphy_idone_wait(struct stm32mp_ddrphy *phy)
+{
+	uint32_t pgsr;
+	int error = 0;
+	uint64_t timeout = timeout_init_us(TIMEOUT_US_1S);
+
+	do {
+		pgsr = mmio_read_32((uintptr_t)&phy->pgsr);
+
+		VERBOSE("  > [0x%lx] pgsr = 0x%x &\n",
+			(uintptr_t)&phy->pgsr, pgsr);
+
+		if (timeout_elapsed(timeout)) {
+			panic();
+		}
+
+		if ((pgsr & DDRPHYC_PGSR_DTERR) != 0U) {
+			VERBOSE("DQS Gate Trainig Error\n");
+			error++;
+		}
+
+		if ((pgsr & DDRPHYC_PGSR_DTIERR) != 0U) {
+			VERBOSE("DQS Gate Trainig Intermittent Error\n");
+			error++;
+		}
+
+		if ((pgsr & DDRPHYC_PGSR_DFTERR) != 0U) {
+			VERBOSE("DQS Drift Error\n");
+			error++;
+		}
+
+		if ((pgsr & DDRPHYC_PGSR_RVERR) != 0U) {
+			VERBOSE("Read Valid Training Error\n");
+			error++;
+		}
+
+		if ((pgsr & DDRPHYC_PGSR_RVEIRR) != 0U) {
+			VERBOSE("Read Valid Training Intermittent Error\n");
+			error++;
+		}
+	} while (((pgsr & DDRPHYC_PGSR_IDONE) == 0U) && (error == 0));
+	VERBOSE("\n[0x%lx] pgsr = 0x%x\n",
+		(uintptr_t)&phy->pgsr, pgsr);
+}
+
+static void stm32mp1_ddrphy_init(struct stm32mp_ddrphy *phy, uint32_t pir)
+{
+	uint32_t pir_init = pir | DDRPHYC_PIR_INIT;
+
+	mmio_write_32((uintptr_t)&phy->pir, pir_init);
+	VERBOSE("[0x%lx] pir = 0x%x -> 0x%x\n",
+		(uintptr_t)&phy->pir, pir_init,
+		mmio_read_32((uintptr_t)&phy->pir));
+
+	/* Need to wait 10 configuration clock before start polling */
+	udelay(10);
+
+	/* Wait DRAM initialization and Gate Training Evaluation complete */
+	stm32mp1_ddrphy_idone_wait(phy);
+}
+
+/* Wait quasi dynamic register update */
+static void stm32mp1_wait_operating_mode(struct stm32mp_ddr_priv *priv, uint32_t mode)
+{
+	uint64_t timeout;
+	uint32_t stat;
+	int break_loop = 0;
+
+	timeout = timeout_init_us(TIMEOUT_US_1S);
+	for ( ; ; ) {
+		uint32_t operating_mode;
+		uint32_t selref_type;
+
+		stat = mmio_read_32((uintptr_t)&priv->ctl->stat);
+		operating_mode = stat & DDRCTRL_STAT_OPERATING_MODE_MASK;
+		selref_type = stat & DDRCTRL_STAT_SELFREF_TYPE_MASK;
+		VERBOSE("[0x%lx] stat = 0x%x\n",
+			(uintptr_t)&priv->ctl->stat, stat);
+		if (timeout_elapsed(timeout)) {
+			panic();
+		}
+
+		if (mode == DDRCTRL_STAT_OPERATING_MODE_SR) {
+			/*
+			 * Self-refresh due to software
+			 * => checking also STAT.selfref_type.
+			 */
+			if ((operating_mode ==
+			     DDRCTRL_STAT_OPERATING_MODE_SR) &&
+			    (selref_type == DDRCTRL_STAT_SELFREF_TYPE_SR)) {
+				break_loop = 1;
+			}
+		} else if (operating_mode == mode) {
+			break_loop = 1;
+		} else if ((mode == DDRCTRL_STAT_OPERATING_MODE_NORMAL) &&
+			   (operating_mode == DDRCTRL_STAT_OPERATING_MODE_SR) &&
+			   (selref_type == DDRCTRL_STAT_SELFREF_TYPE_ASR)) {
+			/* Normal mode: handle also automatic self refresh */
+			break_loop = 1;
+		}
+
+		if (break_loop == 1) {
+			break;
+		}
+	}
+
+	VERBOSE("[0x%lx] stat = 0x%x\n",
+		(uintptr_t)&priv->ctl->stat, stat);
+}
+
+/* Mode Register Writes (MRW or MRS) */
+static void stm32mp1_mode_register_write(struct stm32mp_ddr_priv *priv, uint8_t addr,
+					 uint32_t data)
+{
+	uint32_t mrctrl0;
+
+	VERBOSE("MRS: %d = %x\n", addr, data);
+
+	/*
+	 * 1. Poll MRSTAT.mr_wr_busy until it is '0'.
+	 *    This checks that there is no outstanding MR transaction.
+	 *    No write should be performed to MRCTRL0 and MRCTRL1
+	 *    if MRSTAT.mr_wr_busy = 1.
+	 */
+	while ((mmio_read_32((uintptr_t)&priv->ctl->mrstat) &
+		DDRCTRL_MRSTAT_MR_WR_BUSY) != 0U) {
+		;
+	}
+
+	/*
+	 * 2. Write the MRCTRL0.mr_type, MRCTRL0.mr_addr, MRCTRL0.mr_rank
+	 *    and (for MRWs) MRCTRL1.mr_data to define the MR transaction.
+	 */
+	mrctrl0 = DDRCTRL_MRCTRL0_MR_TYPE_WRITE |
+		  DDRCTRL_MRCTRL0_MR_RANK_ALL |
+		  (((uint32_t)addr << DDRCTRL_MRCTRL0_MR_ADDR_SHIFT) &
+		   DDRCTRL_MRCTRL0_MR_ADDR_MASK);
+	mmio_write_32((uintptr_t)&priv->ctl->mrctrl0, mrctrl0);
+	VERBOSE("[0x%lx] mrctrl0 = 0x%x (0x%x)\n",
+		(uintptr_t)&priv->ctl->mrctrl0,
+		mmio_read_32((uintptr_t)&priv->ctl->mrctrl0), mrctrl0);
+	mmio_write_32((uintptr_t)&priv->ctl->mrctrl1, data);
+	VERBOSE("[0x%lx] mrctrl1 = 0x%x\n",
+		(uintptr_t)&priv->ctl->mrctrl1,
+		mmio_read_32((uintptr_t)&priv->ctl->mrctrl1));
+
+	/*
+	 * 3. In a separate APB transaction, write the MRCTRL0.mr_wr to 1. This
+	 *    bit is self-clearing, and triggers the MR transaction.
+	 *    The uMCTL2 then asserts the MRSTAT.mr_wr_busy while it performs
+	 *    the MR transaction to SDRAM, and no further access can be
+	 *    initiated until it is deasserted.
+	 */
+	mrctrl0 |= DDRCTRL_MRCTRL0_MR_WR;
+	mmio_write_32((uintptr_t)&priv->ctl->mrctrl0, mrctrl0);
+
+	while ((mmio_read_32((uintptr_t)&priv->ctl->mrstat) &
+	       DDRCTRL_MRSTAT_MR_WR_BUSY) != 0U) {
+		;
+	}
+
+	VERBOSE("[0x%lx] mrctrl0 = 0x%x\n",
+		(uintptr_t)&priv->ctl->mrctrl0, mrctrl0);
+}
+
+/* Switch DDR3 from DLL-on to DLL-off */
+static void stm32mp1_ddr3_dll_off(struct stm32mp_ddr_priv *priv)
+{
+	uint32_t mr1 = mmio_read_32((uintptr_t)&priv->phy->mr1);
+	uint32_t mr2 = mmio_read_32((uintptr_t)&priv->phy->mr2);
+	uint32_t dbgcam;
+
+	VERBOSE("mr1: 0x%x\n", mr1);
+	VERBOSE("mr2: 0x%x\n", mr2);
+
+	/*
+	 * 1. Set the DBG1.dis_hif = 1.
+	 *    This prevents further reads/writes being received on the HIF.
+	 */
+	mmio_setbits_32((uintptr_t)&priv->ctl->dbg1, DDRCTRL_DBG1_DIS_HIF);
+	VERBOSE("[0x%lx] dbg1 = 0x%x\n",
+		(uintptr_t)&priv->ctl->dbg1,
+		mmio_read_32((uintptr_t)&priv->ctl->dbg1));
+
+	/*
+	 * 2. Ensure all commands have been flushed from the uMCTL2 by polling
+	 *    DBGCAM.wr_data_pipeline_empty = 1,
+	 *    DBGCAM.rd_data_pipeline_empty = 1,
+	 *    DBGCAM.dbg_wr_q_depth = 0 ,
+	 *    DBGCAM.dbg_lpr_q_depth = 0, and
+	 *    DBGCAM.dbg_hpr_q_depth = 0.
+	 */
+	do {
+		dbgcam = mmio_read_32((uintptr_t)&priv->ctl->dbgcam);
+		VERBOSE("[0x%lx] dbgcam = 0x%x\n",
+			(uintptr_t)&priv->ctl->dbgcam, dbgcam);
+	} while ((((dbgcam & DDRCTRL_DBGCAM_DATA_PIPELINE_EMPTY) ==
+		   DDRCTRL_DBGCAM_DATA_PIPELINE_EMPTY)) &&
+		 ((dbgcam & DDRCTRL_DBGCAM_DBG_Q_DEPTH) == 0U));
+
+	/*
+	 * 3. Perform an MRS command (using MRCTRL0 and MRCTRL1 registers)
+	 *    to disable RTT_NOM:
+	 *    a. DDR3: Write to MR1[9], MR1[6] and MR1[2]
+	 *    b. DDR4: Write to MR1[10:8]
+	 */
+	mr1 &= ~(BIT(9) | BIT(6) | BIT(2));
+	stm32mp1_mode_register_write(priv, 1, mr1);
+
+	/*
+	 * 4. For DDR4 only: Perform an MRS command
+	 *    (using MRCTRL0 and MRCTRL1 registers) to write to MR5[8:6]
+	 *    to disable RTT_PARK
+	 */
+
+	/*
+	 * 5. Perform an MRS command (using MRCTRL0 and MRCTRL1 registers)
+	 *    to write to MR2[10:9], to disable RTT_WR
+	 *    (and therefore disable dynamic ODT).
+	 *    This applies for both DDR3 and DDR4.
+	 */
+	mr2 &= ~GENMASK(10, 9);
+	stm32mp1_mode_register_write(priv, 2, mr2);
+
+	/*
+	 * 6. Perform an MRS command (using MRCTRL0 and MRCTRL1 registers)
+	 *    to disable the DLL. The timing of this MRS is automatically
+	 *    handled by the uMCTL2.
+	 *    a. DDR3: Write to MR1[0]
+	 *    b. DDR4: Write to MR1[0]
+	 */
+	mr1 |= BIT(0);
+	stm32mp1_mode_register_write(priv, 1, mr1);
+
+	/*
+	 * 7. Put the SDRAM into self-refresh mode by setting
+	 *    PWRCTL.selfref_sw = 1, and polling STAT.operating_mode to ensure
+	 *    the DDRC has entered self-refresh.
+	 */
+	mmio_setbits_32((uintptr_t)&priv->ctl->pwrctl,
+			DDRCTRL_PWRCTL_SELFREF_SW);
+	VERBOSE("[0x%lx] pwrctl = 0x%x\n",
+		(uintptr_t)&priv->ctl->pwrctl,
+		mmio_read_32((uintptr_t)&priv->ctl->pwrctl));
+
+	/*
+	 * 8. Wait until STAT.operating_mode[1:0]==11 indicating that the
+	 *    DWC_ddr_umctl2 core is in self-refresh mode.
+	 *    Ensure transition to self-refresh was due to software
+	 *    by checking that STAT.selfref_type[1:0]=2.
+	 */
+	stm32mp1_wait_operating_mode(priv, DDRCTRL_STAT_OPERATING_MODE_SR);
+
+	/*
+	 * 9. Set the MSTR.dll_off_mode = 1.
+	 *    warning: MSTR.dll_off_mode is a quasi-dynamic type 2 field
+	 */
+	stm32mp_ddr_start_sw_done(priv->ctl);
+
+	mmio_setbits_32((uintptr_t)&priv->ctl->mstr, DDRCTRL_MSTR_DLL_OFF_MODE);
+	VERBOSE("[0x%lx] mstr = 0x%x\n",
+		(uintptr_t)&priv->ctl->mstr,
+		mmio_read_32((uintptr_t)&priv->ctl->mstr));
+
+	stm32mp_ddr_wait_sw_done_ack(priv->ctl);
+
+	/* 10. Change the clock frequency to the desired value. */
+
+	/*
+	 * 11. Update any registers which may be required to change for the new
+	 *     frequency. This includes static and dynamic registers.
+	 *     This includes both uMCTL2 registers and PHY registers.
+	 */
+
+	/* Change Bypass Mode Frequency Range */
+	if (clk_get_rate(DDRPHYC) < 100000000U) {
+		mmio_clrbits_32((uintptr_t)&priv->phy->dllgcr,
+				DDRPHYC_DLLGCR_BPS200);
+	} else {
+		mmio_setbits_32((uintptr_t)&priv->phy->dllgcr,
+				DDRPHYC_DLLGCR_BPS200);
+	}
+
+	mmio_setbits_32((uintptr_t)&priv->phy->acdllcr, DDRPHYC_ACDLLCR_DLLDIS);
+
+	mmio_setbits_32((uintptr_t)&priv->phy->dx0dllcr,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+	mmio_setbits_32((uintptr_t)&priv->phy->dx1dllcr,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+#if STM32MP_DDR_32BIT_INTERFACE
+	mmio_setbits_32((uintptr_t)&priv->phy->dx2dllcr,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+	mmio_setbits_32((uintptr_t)&priv->phy->dx3dllcr,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+#endif
+
+	/* 12. Exit the self-refresh state by setting PWRCTL.selfref_sw = 0. */
+	mmio_clrbits_32((uintptr_t)&priv->ctl->pwrctl,
+			DDRCTRL_PWRCTL_SELFREF_SW);
+	stm32mp1_wait_operating_mode(priv, DDRCTRL_STAT_OPERATING_MODE_NORMAL);
+
+	/*
+	 * 13. If ZQCTL0.dis_srx_zqcl = 0, the uMCTL2 performs a ZQCL command
+	 *     at this point.
+	 */
+
+	/*
+	 * 14. Perform MRS commands as required to re-program timing registers
+	 *     in the SDRAM for the new frequency
+	 *     (in particular, CL, CWL and WR may need to be changed).
+	 */
+
+	/* 15. Write DBG1.dis_hif = 0 to re-enable reads and writes. */
+	mmio_clrbits_32((uintptr_t)&priv->ctl->dbg1, DDRCTRL_DBG1_DIS_HIF);
+	VERBOSE("[0x%lx] dbg1 = 0x%x\n",
+		(uintptr_t)&priv->ctl->dbg1,
+		mmio_read_32((uintptr_t)&priv->ctl->dbg1));
+}
+
+static void stm32mp1_refresh_disable(struct stm32mp_ddrctl *ctl)
+{
+	stm32mp_ddr_start_sw_done(ctl);
+	/* Quasi-dynamic register update*/
+	mmio_setbits_32((uintptr_t)&ctl->rfshctl3,
+			DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH);
+	mmio_clrbits_32((uintptr_t)&ctl->pwrctl, DDRCTRL_PWRCTL_POWERDOWN_EN);
+	mmio_clrbits_32((uintptr_t)&ctl->dfimisc,
+			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+	stm32mp_ddr_wait_sw_done_ack(ctl);
+}
+
+static void stm32mp1_refresh_restore(struct stm32mp_ddrctl *ctl,
+				     uint32_t rfshctl3, uint32_t pwrctl)
+{
+	stm32mp_ddr_start_sw_done(ctl);
+	if ((rfshctl3 & DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH) == 0U) {
+		mmio_clrbits_32((uintptr_t)&ctl->rfshctl3,
+				DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH);
+	}
+	if ((pwrctl & DDRCTRL_PWRCTL_POWERDOWN_EN) != 0U) {
+		mmio_setbits_32((uintptr_t)&ctl->pwrctl,
+				DDRCTRL_PWRCTL_POWERDOWN_EN);
+	}
+	mmio_setbits_32((uintptr_t)&ctl->dfimisc,
+			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+	stm32mp_ddr_wait_sw_done_ack(ctl);
+}
+
+void stm32mp1_ddr_init(struct stm32mp_ddr_priv *priv,
+		       struct stm32mp_ddr_config *config)
+{
+	uint32_t pir;
+	int ret = -EINVAL;
+
+	if ((config->c_reg.mstr & DDRCTRL_MSTR_DDR3) != 0U) {
+		ret = stm32mp_board_ddr_power_init(STM32MP_DDR3);
+	} else if ((config->c_reg.mstr & DDRCTRL_MSTR_LPDDR2) != 0U) {
+		ret = stm32mp_board_ddr_power_init(STM32MP_LPDDR2);
+	} else if ((config->c_reg.mstr & DDRCTRL_MSTR_LPDDR3) != 0U) {
+		ret = stm32mp_board_ddr_power_init(STM32MP_LPDDR3);
+	} else {
+		ERROR("DDR type not supported\n");
+	}
+
+	if (ret != 0) {
+		panic();
+	}
+
+	VERBOSE("name = %s\n", config->info.name);
+	VERBOSE("speed = %u kHz\n", config->info.speed);
+	VERBOSE("size  = 0x%x\n", config->info.size);
+
+	/* DDR INIT SEQUENCE */
+
+	/*
+	 * 1. Program the DWC_ddr_umctl2 registers
+	 *     nota: check DFIMISC.dfi_init_complete = 0
+	 */
+
+	/* 1.1 RESETS: presetn, core_ddrc_rstn, aresetn */
+	mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCAPBRST);
+	mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCAXIRST);
+	mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCORERST);
+	mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DPHYAPBRST);
+	mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DPHYRST);
+	mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DPHYCTLRST);
+
+	/* 1.2. start CLOCK */
+	if (stm32mp1_ddr_clk_enable(priv, config->info.speed) != 0) {
+		panic();
+	}
+
+	/* 1.3. deassert reset */
+	/* De-assert PHY rstn and ctl_rstn via DPHYRST and DPHYCTLRST. */
+	mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DPHYRST);
+	mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DPHYCTLRST);
+	/*
+	 * De-assert presetn once the clocks are active
+	 * and stable via DDRCAPBRST bit.
+	 */
+	mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCAPBRST);
+
+	/* 1.4. wait 128 cycles to permit initialization of end logic */
+	udelay(2);
+	/* For PCLK = 133MHz => 1 us is enough, 2 to allow lower frequency */
+
+	/* 1.5. initialize registers ddr_umctl2 */
+	/* Stop uMCTL2 before PHY is ready */
+	mmio_clrbits_32((uintptr_t)&priv->ctl->dfimisc,
+			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+	VERBOSE("[0x%lx] dfimisc = 0x%x\n",
+		(uintptr_t)&priv->ctl->dfimisc,
+		mmio_read_32((uintptr_t)&priv->ctl->dfimisc));
+
+	stm32mp_ddr_set_reg(priv, REG_REG, &config->c_reg, ddr_registers);
+
+	/* DDR3 = don't set DLLOFF for init mode */
+	if ((config->c_reg.mstr &
+	     (DDRCTRL_MSTR_DDR3 | DDRCTRL_MSTR_DLL_OFF_MODE))
+	    == (DDRCTRL_MSTR_DDR3 | DDRCTRL_MSTR_DLL_OFF_MODE)) {
+		VERBOSE("deactivate DLL OFF in mstr\n");
+		mmio_clrbits_32((uintptr_t)&priv->ctl->mstr,
+				DDRCTRL_MSTR_DLL_OFF_MODE);
+		VERBOSE("[0x%lx] mstr = 0x%x\n",
+			(uintptr_t)&priv->ctl->mstr,
+			mmio_read_32((uintptr_t)&priv->ctl->mstr));
+	}
+
+	stm32mp_ddr_set_reg(priv, REG_TIMING, &config->c_timing, ddr_registers);
+	stm32mp_ddr_set_reg(priv, REG_MAP, &config->c_map, ddr_registers);
+
+	/* Skip CTRL init, SDRAM init is done by PHY PUBL */
+	mmio_clrsetbits_32((uintptr_t)&priv->ctl->init0,
+			   DDRCTRL_INIT0_SKIP_DRAM_INIT_MASK,
+			   DDRCTRL_INIT0_SKIP_DRAM_INIT_NORMAL);
+	VERBOSE("[0x%lx] init0 = 0x%x\n",
+		(uintptr_t)&priv->ctl->init0,
+		mmio_read_32((uintptr_t)&priv->ctl->init0));
+
+	stm32mp_ddr_set_reg(priv, REG_PERF, &config->c_perf, ddr_registers);
+
+	/*  2. deassert reset signal core_ddrc_rstn, aresetn and presetn */
+	mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCORERST);
+	mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCAXIRST);
+	mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DPHYAPBRST);
+
+	/*
+	 * 3. start PHY init by accessing relevant PUBL registers
+	 *    (DXGCR, DCR, PTR*, MR*, DTPR*)
+	 */
+	stm32mp_ddr_set_reg(priv, REGPHY_REG, &config->p_reg, ddr_registers);
+	stm32mp_ddr_set_reg(priv, REGPHY_TIMING, &config->p_timing, ddr_registers);
+
+	/* DDR3 = don't set DLLOFF for init mode */
+	if ((config->c_reg.mstr &
+	     (DDRCTRL_MSTR_DDR3 | DDRCTRL_MSTR_DLL_OFF_MODE))
+	    == (DDRCTRL_MSTR_DDR3 | DDRCTRL_MSTR_DLL_OFF_MODE)) {
+		VERBOSE("deactivate DLL OFF in mr1\n");
+		mmio_clrbits_32((uintptr_t)&priv->phy->mr1, BIT(0));
+		VERBOSE("[0x%lx] mr1 = 0x%x\n",
+			(uintptr_t)&priv->phy->mr1,
+			mmio_read_32((uintptr_t)&priv->phy->mr1));
+	}
+
+	/*
+	 *  4. Monitor PHY init status by polling PUBL register PGSR.IDONE
+	 *     Perform DDR PHY DRAM initialization and Gate Training Evaluation
+	 */
+	stm32mp1_ddrphy_idone_wait(priv->phy);
+
+	/*
+	 *  5. Indicate to PUBL that controller performs SDRAM initialization
+	 *     by setting PIR.INIT and PIR CTLDINIT and pool PGSR.IDONE
+	 *     DRAM init is done by PHY, init0.skip_dram.init = 1
+	 */
+
+	pir = DDRPHYC_PIR_DLLSRST | DDRPHYC_PIR_DLLLOCK | DDRPHYC_PIR_ZCAL |
+	      DDRPHYC_PIR_ITMSRST | DDRPHYC_PIR_DRAMINIT | DDRPHYC_PIR_ICPC;
+
+	if ((config->c_reg.mstr & DDRCTRL_MSTR_DDR3) != 0U) {
+		pir |= DDRPHYC_PIR_DRAMRST; /* Only for DDR3 */
+	}
+
+	stm32mp1_ddrphy_init(priv->phy, pir);
+
+	/*
+	 *  6. SET DFIMISC.dfi_init_complete_en to 1
+	 *  Enable quasi-dynamic register programming.
+	 */
+	stm32mp_ddr_start_sw_done(priv->ctl);
+
+	mmio_setbits_32((uintptr_t)&priv->ctl->dfimisc,
+			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+	VERBOSE("[0x%lx] dfimisc = 0x%x\n",
+		(uintptr_t)&priv->ctl->dfimisc,
+		mmio_read_32((uintptr_t)&priv->ctl->dfimisc));
+
+	stm32mp_ddr_wait_sw_done_ack(priv->ctl);
+
+	/*
+	 *  7. Wait for DWC_ddr_umctl2 to move to normal operation mode
+	 *     by monitoring STAT.operating_mode signal
+	 */
+
+	/* Wait uMCTL2 ready */
+	stm32mp1_wait_operating_mode(priv, DDRCTRL_STAT_OPERATING_MODE_NORMAL);
+
+	/* Switch to DLL OFF mode */
+	if ((config->c_reg.mstr & DDRCTRL_MSTR_DLL_OFF_MODE) != 0U) {
+		stm32mp1_ddr3_dll_off(priv);
+	}
+
+	VERBOSE("DDR DQS training : ");
+
+	/*
+	 *  8. Disable Auto refresh and power down by setting
+	 *    - RFSHCTL3.dis_au_refresh = 1
+	 *    - PWRCTL.powerdown_en = 0
+	 *    - DFIMISC.dfiinit_complete_en = 0
+	 */
+	stm32mp1_refresh_disable(priv->ctl);
+
+	/*
+	 *  9. Program PUBL PGCR to enable refresh during training
+	 *     and rank to train
+	 *     not done => keep the programed value in PGCR
+	 */
+
+	/*
+	 * 10. configure PUBL PIR register to specify which training step
+	 * to run
+	 * RVTRN is executed only on LPDDR2/LPDDR3
+	 */
+	pir = DDRPHYC_PIR_QSTRN;
+	if ((config->c_reg.mstr & DDRCTRL_MSTR_DDR3) == 0U) {
+		pir |= DDRPHYC_PIR_RVTRN;
+	}
+
+	stm32mp1_ddrphy_init(priv->phy, pir);
+
+	/* 11. monitor PUB PGSR.IDONE to poll cpmpletion of training sequence */
+	stm32mp1_ddrphy_idone_wait(priv->phy);
+
+	/*
+	 * 12. set back registers in step 8 to the orginal values if desidered
+	 */
+	stm32mp1_refresh_restore(priv->ctl, config->c_reg.rfshctl3,
+				 config->c_reg.pwrctl);
+
+	stm32mp_ddr_enable_axi_port(priv->ctl);
+}
diff --git a/drivers/st/ddr/stm32mp1_ddr_helpers.c b/drivers/st/ddr/stm32mp1_ddr_helpers.c
new file mode 100644
index 0000000..e0621b5
--- /dev/null
+++ b/drivers/st/ddr/stm32mp1_ddr_helpers.c
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <drivers/st/stm32mp1_ddr_helpers.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+void ddr_enable_clock(void)
+{
+	stm32mp1_clk_rcc_regs_lock();
+
+	mmio_setbits_32(stm32mp_rcc_base() + RCC_DDRITFCR,
+			RCC_DDRITFCR_DDRC1EN |
+#if STM32MP_DDR_DUAL_AXI_PORT
+			RCC_DDRITFCR_DDRC2EN |
+#endif
+			RCC_DDRITFCR_DDRPHYCEN |
+			RCC_DDRITFCR_DDRPHYCAPBEN |
+			RCC_DDRITFCR_DDRCAPBEN);
+
+	stm32mp1_clk_rcc_regs_unlock();
+}
diff --git a/drivers/st/ddr/stm32mp1_ram.c b/drivers/st/ddr/stm32mp1_ram.c
new file mode 100644
index 0000000..b510c8f
--- /dev/null
+++ b/drivers/st/ddr/stm32mp1_ram.c
@@ -0,0 +1,151 @@
+/*
+ * Copyright (C) 2018-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <errno.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/st/stm32mp1_ddr.h>
+#include <drivers/st/stm32mp1_ddr_helpers.h>
+#include <drivers/st/stm32mp1_ram.h>
+#include <drivers/st/stm32mp_ddr.h>
+#include <drivers/st/stm32mp_ddr_test.h>
+#include <drivers/st/stm32mp_ram.h>
+#include <lib/mmio.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+static struct stm32mp_ddr_priv ddr_priv_data;
+
+int stm32mp1_ddr_clk_enable(struct stm32mp_ddr_priv *priv, uint32_t mem_speed)
+{
+	unsigned long ddrphy_clk, ddr_clk, mem_speed_hz;
+
+	ddr_enable_clock();
+
+	ddrphy_clk = clk_get_rate(DDRPHYC);
+
+	VERBOSE("DDR: mem_speed (%u kHz), RCC %lu kHz\n",
+		mem_speed, ddrphy_clk / 1000U);
+
+	mem_speed_hz = mem_speed * 1000U;
+
+	/* Max 10% frequency delta */
+	if (ddrphy_clk > mem_speed_hz) {
+		ddr_clk = ddrphy_clk - mem_speed_hz;
+	} else {
+		ddr_clk = mem_speed_hz - ddrphy_clk;
+	}
+	if (ddr_clk > (mem_speed_hz / 10)) {
+		ERROR("DDR expected freq %u kHz, current is %lu kHz\n",
+		      mem_speed, ddrphy_clk / 1000U);
+		return -1;
+	}
+	return 0;
+}
+
+static int stm32mp1_ddr_setup(void)
+{
+	struct stm32mp_ddr_priv *priv = &ddr_priv_data;
+	int ret;
+	struct stm32mp_ddr_config config;
+	int node;
+	uint32_t uret;
+	void *fdt;
+
+	const struct stm32mp_ddr_param param[] = {
+		CTL_PARAM(reg),
+		CTL_PARAM(timing),
+		CTL_PARAM(map),
+		CTL_PARAM(perf),
+		PHY_PARAM(reg),
+		PHY_PARAM(timing),
+	};
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_DDR_COMPAT);
+	if (node < 0) {
+		ERROR("%s: Cannot read DDR node in DT\n", __func__);
+		return -EINVAL;
+	}
+
+	ret = stm32mp_ddr_dt_get_info(fdt, node, &config.info);
+	if (ret < 0) {
+		return ret;
+	}
+
+	ret = stm32mp_ddr_dt_get_param(fdt, node, param, ARRAY_SIZE(param), (uintptr_t)&config);
+	if (ret < 0) {
+		return ret;
+	}
+
+	/* Disable axidcg clock gating during init */
+	mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+
+	stm32mp1_ddr_init(priv, &config);
+
+	/* Enable axidcg clock gating */
+	mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+
+	priv->info.size = config.info.size;
+
+	VERBOSE("%s : ram size(%x, %x)\n", __func__,
+		(uint32_t)priv->info.base, (uint32_t)priv->info.size);
+
+	if (stm32mp_map_ddr_non_cacheable() != 0) {
+		panic();
+	}
+
+	uret = stm32mp_ddr_test_data_bus();
+	if (uret != 0U) {
+		ERROR("DDR data bus test: can't access memory @ 0x%x\n",
+		      uret);
+		panic();
+	}
+
+	uret = stm32mp_ddr_test_addr_bus(config.info.size);
+	if (uret != 0U) {
+		ERROR("DDR addr bus test: can't access memory @ 0x%x\n",
+		      uret);
+		panic();
+	}
+
+	uret = stm32mp_ddr_check_size();
+	if (uret < config.info.size) {
+		ERROR("DDR size: 0x%x does not match DT config: 0x%x\n",
+		      uret, config.info.size);
+		panic();
+	}
+
+	if (stm32mp_unmap_ddr() != 0) {
+		panic();
+	}
+
+	return 0;
+}
+
+int stm32mp1_ddr_probe(void)
+{
+	struct stm32mp_ddr_priv *priv = &ddr_priv_data;
+
+	VERBOSE("STM32MP DDR probe\n");
+
+	priv->ctl = (struct stm32mp_ddrctl *)stm32mp_ddrctrl_base();
+	priv->phy = (struct stm32mp_ddrphy *)stm32mp_ddrphyc_base();
+	priv->pwr = stm32mp_pwr_base();
+	priv->rcc = stm32mp_rcc_base();
+
+	priv->info.base = STM32MP_DDR_BASE;
+	priv->info.size = 0;
+
+	return stm32mp1_ddr_setup();
+}
diff --git a/drivers/st/ddr/stm32mp_ddr.c b/drivers/st/ddr/stm32mp_ddr.c
new file mode 100644
index 0000000..6776e3b
--- /dev/null
+++ b/drivers/st/ddr/stm32mp_ddr.c
@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp_ddr.h>
+#include <drivers/st/stm32mp_ddrctrl_regs.h>
+#include <drivers/st/stm32mp_pmic.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+#define INVALID_OFFSET	0xFFU
+
+static uintptr_t get_base_addr(const struct stm32mp_ddr_priv *priv, enum stm32mp_ddr_base_type base)
+{
+	if (base == DDRPHY_BASE) {
+		return (uintptr_t)priv->phy;
+	} else {
+		return (uintptr_t)priv->ctl;
+	}
+}
+
+void stm32mp_ddr_set_reg(const struct stm32mp_ddr_priv *priv, enum stm32mp_ddr_reg_type type,
+			 const void *param, const struct stm32mp_ddr_reg_info *ddr_registers)
+{
+	unsigned int i;
+	unsigned int value;
+	enum stm32mp_ddr_base_type base = ddr_registers[type].base;
+	uintptr_t base_addr = get_base_addr(priv, base);
+	const struct stm32mp_ddr_reg_desc *desc = ddr_registers[type].desc;
+
+	VERBOSE("init %s\n", ddr_registers[type].name);
+	for (i = 0; i < ddr_registers[type].size; i++) {
+		uintptr_t ptr = base_addr + desc[i].offset;
+
+		if (desc[i].par_offset == INVALID_OFFSET) {
+			ERROR("invalid parameter offset for %s", desc[i].name);
+			panic();
+		} else {
+			value = *((uint32_t *)((uintptr_t)param +
+					       desc[i].par_offset));
+			mmio_write_32(ptr, value);
+		}
+	}
+}
+
+/* Start quasi dynamic register update */
+void stm32mp_ddr_start_sw_done(struct stm32mp_ddrctl *ctl)
+{
+	mmio_clrbits_32((uintptr_t)&ctl->swctl, DDRCTRL_SWCTL_SW_DONE);
+	VERBOSE("[0x%lx] swctl = 0x%x\n",
+		(uintptr_t)&ctl->swctl,  mmio_read_32((uintptr_t)&ctl->swctl));
+}
+
+/* Wait quasi dynamic register update */
+void stm32mp_ddr_wait_sw_done_ack(struct stm32mp_ddrctl *ctl)
+{
+	uint64_t timeout;
+	uint32_t swstat;
+
+	mmio_setbits_32((uintptr_t)&ctl->swctl, DDRCTRL_SWCTL_SW_DONE);
+	VERBOSE("[0x%lx] swctl = 0x%x\n",
+		(uintptr_t)&ctl->swctl, mmio_read_32((uintptr_t)&ctl->swctl));
+
+	timeout = timeout_init_us(TIMEOUT_US_1S);
+	do {
+		swstat = mmio_read_32((uintptr_t)&ctl->swstat);
+		VERBOSE("[0x%lx] swstat = 0x%x ",
+			(uintptr_t)&ctl->swstat, swstat);
+		if (timeout_elapsed(timeout)) {
+			panic();
+		}
+	} while ((swstat & DDRCTRL_SWSTAT_SW_DONE_ACK) == 0U);
+
+	VERBOSE("[0x%lx] swstat = 0x%x\n",
+		(uintptr_t)&ctl->swstat, swstat);
+}
+
+void stm32mp_ddr_enable_axi_port(struct stm32mp_ddrctl *ctl)
+{
+	/* Enable uMCTL2 AXI port 0 */
+	mmio_setbits_32((uintptr_t)&ctl->pctrl_0, DDRCTRL_PCTRL_N_PORT_EN);
+	VERBOSE("[0x%lx] pctrl_0 = 0x%x\n", (uintptr_t)&ctl->pctrl_0,
+		mmio_read_32((uintptr_t)&ctl->pctrl_0));
+
+#if STM32MP_DDR_DUAL_AXI_PORT
+	/* Enable uMCTL2 AXI port 1 */
+	mmio_setbits_32((uintptr_t)&ctl->pctrl_1, DDRCTRL_PCTRL_N_PORT_EN);
+	VERBOSE("[0x%lx] pctrl_1 = 0x%x\n", (uintptr_t)&ctl->pctrl_1,
+		mmio_read_32((uintptr_t)&ctl->pctrl_1));
+#endif
+
+}
+
+int stm32mp_board_ddr_power_init(enum ddr_type ddr_type)
+{
+	if (dt_pmic_status() > 0) {
+		return pmic_ddr_power_init(ddr_type);
+	}
+
+	return 0;
+}
diff --git a/drivers/st/ddr/stm32mp_ddr_test.c b/drivers/st/ddr/stm32mp_ddr_test.c
new file mode 100644
index 0000000..6733cc6
--- /dev/null
+++ b/drivers/st/ddr/stm32mp_ddr_test.c
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <drivers/st/stm32mp_ddr_test.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+#define DDR_PATTERN	0xAAAAAAAAU
+#define DDR_ANTIPATTERN	0x55555555U
+
+/*******************************************************************************
+ * This function tests a simple read/write access to the DDR.
+ * Note that the previous content is restored after test.
+ * Returns 0 if success, and address value else.
+ ******************************************************************************/
+uint32_t stm32mp_ddr_test_rw_access(void)
+{
+	uint32_t saved_value = mmio_read_32(STM32MP_DDR_BASE);
+
+	mmio_write_32(STM32MP_DDR_BASE, DDR_PATTERN);
+
+	if (mmio_read_32(STM32MP_DDR_BASE) != DDR_PATTERN) {
+		return (uint32_t)STM32MP_DDR_BASE;
+	}
+
+	mmio_write_32(STM32MP_DDR_BASE, saved_value);
+
+	return 0U;
+}
+
+/*******************************************************************************
+ * This function tests the DDR data bus wiring.
+ * This is inspired from the Data Bus Test algorithm written by Michael Barr
+ * in "Programming Embedded Systems in C and C++" book.
+ * resources.oreilly.com/examples/9781565923546/blob/master/Chapter6/
+ * File: memtest.c - This source code belongs to Public Domain.
+ * Returns 0 if success, and address value else.
+ ******************************************************************************/
+uint32_t stm32mp_ddr_test_data_bus(void)
+{
+	uint32_t pattern;
+
+	for (pattern = 1U; pattern != 0U; pattern <<= 1U) {
+		mmio_write_32(STM32MP_DDR_BASE, pattern);
+
+		if (mmio_read_32(STM32MP_DDR_BASE) != pattern) {
+			return (uint32_t)STM32MP_DDR_BASE;
+		}
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function tests the DDR address bus wiring.
+ * This is inspired from the Data Bus Test algorithm written by Michael Barr
+ * in "Programming Embedded Systems in C and C++" book.
+ * resources.oreilly.com/examples/9781565923546/blob/master/Chapter6/
+ * File: memtest.c - This source code belongs to Public Domain.
+ * size: size in bytes of the DDR memory device.
+ * Returns 0 if success, and address value else.
+ ******************************************************************************/
+uint32_t stm32mp_ddr_test_addr_bus(uint64_t size)
+{
+	uint64_t addressmask = size - 1U;
+	uint64_t offset;
+	uint64_t testoffset = 0U;
+
+	/* Write the default pattern at each of the power-of-two offsets. */
+	for (offset = sizeof(uint32_t); (offset & addressmask) != 0U;
+	     offset <<= 1U) {
+		mmio_write_32(STM32MP_DDR_BASE + (uint32_t)offset,
+			      DDR_PATTERN);
+	}
+
+	/* Check for address bits stuck high. */
+	mmio_write_32(STM32MP_DDR_BASE + (uint32_t)testoffset,
+		      DDR_ANTIPATTERN);
+
+	for (offset = sizeof(uint32_t); (offset & addressmask) != 0U;
+	     offset <<= 1U) {
+		if (mmio_read_32(STM32MP_DDR_BASE + (uint32_t)offset) !=
+		    DDR_PATTERN) {
+			return (uint32_t)(STM32MP_DDR_BASE + offset);
+		}
+	}
+
+	mmio_write_32(STM32MP_DDR_BASE + (uint32_t)testoffset, DDR_PATTERN);
+
+	/* Check for address bits stuck low or shorted. */
+	for (testoffset = sizeof(uint32_t); (testoffset & addressmask) != 0U;
+	     testoffset <<= 1U) {
+		mmio_write_32(STM32MP_DDR_BASE + (uint32_t)testoffset,
+			      DDR_ANTIPATTERN);
+
+		if (mmio_read_32(STM32MP_DDR_BASE) != DDR_PATTERN) {
+			return STM32MP_DDR_BASE;
+		}
+
+		for (offset = sizeof(uint32_t); (offset & addressmask) != 0U;
+		     offset <<= 1) {
+			if ((mmio_read_32(STM32MP_DDR_BASE +
+					  (uint32_t)offset) != DDR_PATTERN) &&
+			    (offset != testoffset)) {
+				return (uint32_t)(STM32MP_DDR_BASE + offset);
+			}
+		}
+
+		mmio_write_32(STM32MP_DDR_BASE + (uint32_t)testoffset,
+			      DDR_PATTERN);
+	}
+
+	return 0U;
+}
+
+/*******************************************************************************
+ * This function checks the DDR size. It has to be run with Data Cache off.
+ * This test is run before data have been put in DDR, and is only done for
+ * cold boot. The DDR data can then be overwritten, and it is not useful to
+ * restore its content.
+ * Returns DDR computed size.
+ ******************************************************************************/
+uint32_t stm32mp_ddr_check_size(void)
+{
+	uint32_t offset = sizeof(uint32_t);
+
+	mmio_write_32(STM32MP_DDR_BASE, DDR_PATTERN);
+
+	while (offset < STM32MP_DDR_MAX_SIZE) {
+		mmio_write_32(STM32MP_DDR_BASE + offset, DDR_ANTIPATTERN);
+		dsb();
+
+		if (mmio_read_32(STM32MP_DDR_BASE) != DDR_PATTERN) {
+			break;
+		}
+
+		offset <<= 1U;
+	}
+
+	INFO("Memory size = 0x%x (%u MB)\n", offset, offset / (1024U * 1024U));
+
+	return offset;
+}
diff --git a/drivers/st/ddr/stm32mp_ram.c b/drivers/st/ddr/stm32mp_ram.c
new file mode 100644
index 0000000..0804568
--- /dev/null
+++ b/drivers/st/ddr/stm32mp_ram.c
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/st/stm32mp_ram.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+int stm32mp_ddr_dt_get_info(void *fdt, int node, struct stm32mp_ddr_info *info)
+{
+	int ret;
+
+	ret = fdt_read_uint32(fdt, node, "st,mem-speed", &info->speed);
+	if (ret < 0) {
+		VERBOSE("%s: no st,mem-speed\n", __func__);
+		return -EINVAL;
+	}
+	ret = fdt_read_uint32(fdt, node, "st,mem-size", &info->size);
+	if (ret < 0) {
+		VERBOSE("%s: no st,mem-size\n", __func__);
+		return -EINVAL;
+	}
+	info->name = fdt_getprop(fdt, node, "st,mem-name", NULL);
+	if (info->name == NULL) {
+		VERBOSE("%s: no st,mem-name\n", __func__);
+		return -EINVAL;
+	}
+
+	INFO("RAM: %s\n", info->name);
+
+	return 0;
+}
+
+int stm32mp_ddr_dt_get_param(void *fdt, int node, const struct stm32mp_ddr_param *param,
+			     uint32_t param_size, uintptr_t config)
+{
+	int ret;
+	uint32_t idx;
+
+	for (idx = 0U; idx < param_size; idx++) {
+		ret = fdt_read_uint32_array(fdt, node, param[idx].name, param[idx].size,
+					    (void *)(config + param[idx].offset));
+
+		VERBOSE("%s: %s[0x%x] = %d\n", __func__, param[idx].name, param[idx].size, ret);
+		if (ret != 0) {
+			ERROR("%s: Cannot read %s, error=%d\n", __func__, param[idx].name, ret);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
diff --git a/drivers/st/etzpc/etzpc.c b/drivers/st/etzpc/etzpc.c
new file mode 100644
index 0000000..4c3c26d
--- /dev/null
+++ b/drivers/st/etzpc/etzpc.c
@@ -0,0 +1,246 @@
+/*
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/st/etzpc.h>
+#include <dt-bindings/soc/st,stm32-etzpc.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+/* Device Tree related definitions */
+#define ETZPC_COMPAT			"st,stm32-etzpc"
+#define ETZPC_LOCK_MASK			0x1U
+#define ETZPC_MODE_SHIFT		8
+#define ETZPC_MODE_MASK			GENMASK(1, 0)
+#define ETZPC_ID_SHIFT			16
+#define ETZPC_ID_MASK			GENMASK(7, 0)
+
+/* ID Registers */
+#define ETZPC_TZMA0_SIZE		0x000U
+#define ETZPC_DECPROT0			0x010U
+#define ETZPC_DECPROT_LOCK0		0x030U
+#define ETZPC_HWCFGR			0x3F0U
+#define ETZPC_VERR			0x3F4U
+
+/* ID Registers fields */
+#define ETZPC_TZMA0_SIZE_LOCK		BIT(31)
+#define ETZPC_DECPROT0_MASK		GENMASK(1, 0)
+#define ETZPC_HWCFGR_NUM_TZMA_SHIFT	0
+#define ETZPC_HWCFGR_NUM_PER_SEC_SHIFT	8
+#define ETZPC_HWCFGR_NUM_AHB_SEC_SHIFT	16
+#define ETZPC_HWCFGR_CHUNCKS1N4_SHIFT	24
+
+#define DECPROT_SHIFT			1
+#define IDS_PER_DECPROT_REGS		16U
+#define IDS_PER_DECPROT_LOCK_REGS	32U
+
+/*
+ * etzpc_instance.
+ * base : register base address set during init given by user
+ * chunk_size : supported TZMA size steps
+ * num_tzma: number of TZMA zone read from register at init
+ * num_ahb_sec : number of securable AHB master zone read from register
+ * num_per_sec : number of securable AHB & APB Peripherals read from register
+ * revision : IP revision read from register at init
+ */
+struct etzpc_instance {
+	uintptr_t base;
+	uint8_t chunck_size;
+	uint8_t num_tzma;
+	uint8_t num_per_sec;
+	uint8_t num_ahb_sec;
+	uint8_t revision;
+};
+
+/* Only 1 instance of the ETZPC is expected per platform */
+static struct etzpc_instance etzpc_dev;
+
+/*
+ * Implementation uses uint8_t to store each securable DECPROT configuration.
+ * When resuming from deep suspend, the DECPROT configurations are restored.
+ */
+#define PERIPH_LOCK_BIT		BIT(7)
+#define PERIPH_ATTR_MASK	GENMASK(2, 0)
+
+#if ENABLE_ASSERTIONS
+static bool valid_decprot_id(unsigned int id)
+{
+	return id < (unsigned int)etzpc_dev.num_per_sec;
+}
+
+static bool valid_tzma_id(unsigned int id)
+{
+	return id < (unsigned int)etzpc_dev.num_tzma;
+}
+#endif
+
+/*
+ * etzpc_configure_decprot : Load a DECPROT configuration
+ * decprot_id : ID of the IP
+ * decprot_attr : Restriction access attribute
+ */
+void etzpc_configure_decprot(uint32_t decprot_id,
+			     enum etzpc_decprot_attributes decprot_attr)
+{
+	uintptr_t offset = 4U * (decprot_id / IDS_PER_DECPROT_REGS);
+	uint32_t shift = (decprot_id % IDS_PER_DECPROT_REGS) << DECPROT_SHIFT;
+	uint32_t masked_decprot = (uint32_t)decprot_attr & ETZPC_DECPROT0_MASK;
+
+	assert(valid_decprot_id(decprot_id));
+
+	mmio_clrsetbits_32(etzpc_dev.base + ETZPC_DECPROT0 + offset,
+			   (uint32_t)ETZPC_DECPROT0_MASK << shift,
+			   masked_decprot << shift);
+}
+
+/*
+ * etzpc_get_decprot : Get the DECPROT attribute
+ * decprot_id : ID of the IP
+ * return : Attribute of this DECPROT
+ */
+enum etzpc_decprot_attributes etzpc_get_decprot(uint32_t decprot_id)
+{
+	uintptr_t offset = 4U * (decprot_id / IDS_PER_DECPROT_REGS);
+	uint32_t shift = (decprot_id % IDS_PER_DECPROT_REGS) << DECPROT_SHIFT;
+	uintptr_t base_decprot = etzpc_dev.base + offset;
+	uint32_t value;
+
+	assert(valid_decprot_id(decprot_id));
+
+	value = (mmio_read_32(base_decprot + ETZPC_DECPROT0) >> shift) &
+		ETZPC_DECPROT0_MASK;
+
+	return (enum etzpc_decprot_attributes)value;
+}
+
+/*
+ * etzpc_lock_decprot : Lock access to the DECPROT attribute
+ * decprot_id : ID of the IP
+ */
+void etzpc_lock_decprot(uint32_t decprot_id)
+{
+	uintptr_t offset = 4U * (decprot_id / IDS_PER_DECPROT_LOCK_REGS);
+	uint32_t shift = BIT(decprot_id % IDS_PER_DECPROT_LOCK_REGS);
+	uintptr_t base_decprot = etzpc_dev.base + offset;
+
+	assert(valid_decprot_id(decprot_id));
+
+	mmio_write_32(base_decprot + ETZPC_DECPROT_LOCK0, shift);
+}
+
+/*
+ * etzpc_configure_tzma : Configure the target TZMA read only size
+ * tzma_id : ID of the memory
+ * tzma_value : read-only size
+ */
+void etzpc_configure_tzma(uint32_t tzma_id, uint16_t tzma_value)
+{
+	assert(valid_tzma_id(tzma_id));
+
+	mmio_write_32(etzpc_dev.base + ETZPC_TZMA0_SIZE +
+		      (sizeof(uint32_t) * tzma_id), tzma_value);
+}
+
+/*
+ * etzpc_get_tzma : Get the target TZMA read only size
+ * tzma_id : TZMA ID
+ * return : Size of read only size
+ */
+uint16_t etzpc_get_tzma(uint32_t tzma_id)
+{
+	assert(valid_tzma_id(tzma_id));
+
+	return (uint16_t)mmio_read_32(etzpc_dev.base + ETZPC_TZMA0_SIZE +
+				      (sizeof(uint32_t) * tzma_id));
+}
+
+/*
+ * etzpc_lock_tzma : Lock the target TZMA
+ * tzma_id : TZMA ID
+ */
+void etzpc_lock_tzma(uint32_t tzma_id)
+{
+	assert(valid_tzma_id(tzma_id));
+
+	mmio_setbits_32(etzpc_dev.base + ETZPC_TZMA0_SIZE +
+			(sizeof(uint32_t) * tzma_id), ETZPC_TZMA0_SIZE_LOCK);
+}
+
+/*
+ * etzpc_get_lock_tzma : Return the lock status of the target TZMA
+ * tzma_id : TZMA ID
+ * return : True if TZMA is locked, false otherwise
+ */
+bool etzpc_get_lock_tzma(uint32_t tzma_id)
+{
+	uint32_t tzma_size;
+
+	assert(valid_tzma_id(tzma_id));
+
+	tzma_size = mmio_read_32(etzpc_dev.base + ETZPC_TZMA0_SIZE +
+				 (sizeof(uint32_t) * tzma_id));
+
+	return (tzma_size & ETZPC_TZMA0_SIZE_LOCK) != 0;
+}
+
+/*
+ * etzpc_get_num_per_sec : Return the DECPROT ID limit value
+ */
+uint8_t etzpc_get_num_per_sec(void)
+{
+	return etzpc_dev.num_per_sec;
+}
+
+/*
+ * etzpc_get_revision : Return the ETZPC IP revision
+ */
+uint8_t etzpc_get_revision(void)
+{
+	return etzpc_dev.revision;
+}
+
+/*
+ * etzpc_get_base_address : Return the ETZPC IP base address
+ */
+uintptr_t etzpc_get_base_address(void)
+{
+	return etzpc_dev.base;
+}
+
+/*
+ * etzpc_init : Initialize the ETZPC driver
+ * Return 0 on success and a negative errno on failure
+ */
+int etzpc_init(void)
+{
+	uint32_t hwcfg;
+
+	etzpc_dev.base = STM32MP1_ETZPC_BASE;
+
+	hwcfg = mmio_read_32(etzpc_dev.base + ETZPC_HWCFGR);
+
+	etzpc_dev.num_tzma = (uint8_t)(hwcfg >> ETZPC_HWCFGR_NUM_TZMA_SHIFT);
+	etzpc_dev.num_per_sec = (uint8_t)(hwcfg >>
+					  ETZPC_HWCFGR_NUM_PER_SEC_SHIFT);
+	etzpc_dev.num_ahb_sec = (uint8_t)(hwcfg >>
+					  ETZPC_HWCFGR_NUM_AHB_SEC_SHIFT);
+	etzpc_dev.chunck_size = (uint8_t)(hwcfg >>
+					  ETZPC_HWCFGR_CHUNCKS1N4_SHIFT);
+
+	etzpc_dev.revision = mmio_read_8(etzpc_dev.base + ETZPC_VERR);
+
+	VERBOSE("ETZPC version 0x%x", etzpc_dev.revision);
+
+	return 0;
+}
diff --git a/drivers/st/fmc/stm32_fmc2_nand.c b/drivers/st/fmc/stm32_fmc2_nand.c
new file mode 100644
index 0000000..9bdc854
--- /dev/null
+++ b/drivers/st/fmc/stm32_fmc2_nand.c
@@ -0,0 +1,934 @@
+/*
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <limits.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/raw_nand.h>
+#include <drivers/st/stm32_fmc2_nand.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+/* Timeout for device interface reset */
+#define TIMEOUT_US_1_MS			1000U
+
+/* FMC2 Compatibility */
+#define DT_FMC2_EBI_COMPAT		"st,stm32mp1-fmc2-ebi"
+#define DT_FMC2_NFC_COMPAT		"st,stm32mp1-fmc2-nfc"
+#define MAX_CS				2U
+#define MAX_BANK			5U
+
+/* FMC2 Controller Registers */
+#define FMC2_BCR1			0x00U
+#define FMC2_PCR			0x80U
+#define FMC2_SR				0x84U
+#define FMC2_PMEM			0x88U
+#define FMC2_PATT			0x8CU
+#define FMC2_HECCR			0x94U
+#define FMC2_BCHISR			0x254U
+#define FMC2_BCHICR			0x258U
+#define FMC2_BCHDSR0			0x27CU
+#define FMC2_BCHDSR1			0x280U
+#define FMC2_BCHDSR2			0x284U
+#define FMC2_BCHDSR3			0x288U
+#define FMC2_BCHDSR4			0x28CU
+
+/* FMC2_BCR1 register */
+#define FMC2_BCR1_FMC2EN		BIT(31)
+/* FMC2_PCR register */
+#define FMC2_PCR_PWAITEN		BIT(1)
+#define FMC2_PCR_PBKEN			BIT(2)
+#define FMC2_PCR_PWID_MASK		GENMASK_32(5, 4)
+#define FMC2_PCR_PWID(x)		(((x) << 4) & FMC2_PCR_PWID_MASK)
+#define FMC2_PCR_PWID_8			0x0U
+#define FMC2_PCR_PWID_16		0x1U
+#define FMC2_PCR_ECCEN			BIT(6)
+#define FMC2_PCR_ECCALG			BIT(8)
+#define FMC2_PCR_TCLR_MASK		GENMASK_32(12, 9)
+#define FMC2_PCR_TCLR(x)		(((x) << 9) & FMC2_PCR_TCLR_MASK)
+#define FMC2_PCR_TCLR_DEFAULT		0xFU
+#define FMC2_PCR_TAR_MASK		GENMASK_32(16, 13)
+#define FMC2_PCR_TAR(x)			(((x) << 13) & FMC2_PCR_TAR_MASK)
+#define FMC2_PCR_TAR_DEFAULT		0xFU
+#define FMC2_PCR_ECCSS_MASK		GENMASK_32(19, 17)
+#define FMC2_PCR_ECCSS(x)		(((x) << 17) & FMC2_PCR_ECCSS_MASK)
+#define FMC2_PCR_ECCSS_512		0x1U
+#define FMC2_PCR_ECCSS_2048		0x3U
+#define FMC2_PCR_BCHECC			BIT(24)
+#define FMC2_PCR_WEN			BIT(25)
+/* FMC2_SR register */
+#define FMC2_SR_NWRF			BIT(6)
+/* FMC2_PMEM register*/
+#define FMC2_PMEM_MEMSET(x)		(((x) & GENMASK_32(7, 0)) << 0)
+#define FMC2_PMEM_MEMWAIT(x)		(((x) & GENMASK_32(7, 0)) << 8)
+#define FMC2_PMEM_MEMHOLD(x)		(((x) & GENMASK_32(7, 0)) << 16)
+#define FMC2_PMEM_MEMHIZ(x)		(((x) & GENMASK_32(7, 0)) << 24)
+#define FMC2_PMEM_DEFAULT		0x0A0A0A0AU
+/* FMC2_PATT register */
+#define FMC2_PATT_ATTSET(x)		(((x) & GENMASK_32(7, 0)) << 0)
+#define FMC2_PATT_ATTWAIT(x)		(((x) & GENMASK_32(7, 0)) << 8)
+#define FMC2_PATT_ATTHOLD(x)		(((x) & GENMASK_32(7, 0)) << 16)
+#define FMC2_PATT_ATTHIZ(x)		(((x) & GENMASK_32(7, 0)) << 24)
+#define FMC2_PATT_DEFAULT		0x0A0A0A0AU
+/* FMC2_BCHISR register */
+#define FMC2_BCHISR_DERF		BIT(1)
+/* FMC2_BCHICR register */
+#define FMC2_BCHICR_CLEAR_IRQ		GENMASK_32(4, 0)
+/* FMC2_BCHDSR0 register */
+#define FMC2_BCHDSR0_DUE		BIT(0)
+#define FMC2_BCHDSR0_DEF		BIT(1)
+#define FMC2_BCHDSR0_DEN_MASK		GENMASK_32(7, 4)
+#define FMC2_BCHDSR0_DEN_SHIFT		4U
+/* FMC2_BCHDSR1 register */
+#define FMC2_BCHDSR1_EBP1_MASK		GENMASK_32(12, 0)
+#define FMC2_BCHDSR1_EBP2_MASK		GENMASK_32(28, 16)
+#define FMC2_BCHDSR1_EBP2_SHIFT		16U
+/* FMC2_BCHDSR2 register */
+#define FMC2_BCHDSR2_EBP3_MASK		GENMASK_32(12, 0)
+#define FMC2_BCHDSR2_EBP4_MASK		GENMASK_32(28, 16)
+#define FMC2_BCHDSR2_EBP4_SHIFT		16U
+/* FMC2_BCHDSR3 register */
+#define FMC2_BCHDSR3_EBP5_MASK		GENMASK_32(12, 0)
+#define FMC2_BCHDSR3_EBP6_MASK		GENMASK_32(28, 16)
+#define FMC2_BCHDSR3_EBP6_SHIFT		16U
+/* FMC2_BCHDSR4 register */
+#define FMC2_BCHDSR4_EBP7_MASK		GENMASK_32(12, 0)
+#define FMC2_BCHDSR4_EBP8_MASK		GENMASK_32(28, 16)
+#define FMC2_BCHDSR4_EBP8_SHIFT		16U
+
+/* Timings */
+#define FMC2_THIZ			0x01U
+#define FMC2_TIO			8000U
+#define FMC2_TSYNC			3000U
+#define FMC2_PCR_TIMING_MASK		GENMASK_32(3, 0)
+#define FMC2_PMEM_PATT_TIMING_MASK	GENMASK_32(7, 0)
+
+#define FMC2_BBM_LEN			2U
+#define FMC2_MAX_ECC_BYTES		14U
+#define TIMEOUT_US_10_MS		10000U
+#define FMC2_PSEC_PER_MSEC		(1000UL * 1000UL * 1000UL)
+
+enum stm32_fmc2_ecc {
+	FMC2_ECC_HAM = 1U,
+	FMC2_ECC_BCH4 = 4U,
+	FMC2_ECC_BCH8 = 8U
+};
+
+struct stm32_fmc2_cs_reg {
+	uintptr_t data_base;
+	uintptr_t cmd_base;
+	uintptr_t addr_base;
+};
+
+struct stm32_fmc2_nand_timings {
+	uint8_t tclr;
+	uint8_t tar;
+	uint8_t thiz;
+	uint8_t twait;
+	uint8_t thold_mem;
+	uint8_t tset_mem;
+	uint8_t thold_att;
+	uint8_t tset_att;
+};
+
+struct stm32_fmc2_nfc {
+	uintptr_t reg_base;
+	struct stm32_fmc2_cs_reg cs[MAX_CS];
+	unsigned long clock_id;
+	unsigned int reset_id;
+	uint8_t cs_sel;
+};
+
+static struct stm32_fmc2_nfc stm32_fmc2;
+
+static uintptr_t fmc2_base(void)
+{
+	return stm32_fmc2.reg_base;
+}
+
+static void stm32_fmc2_nand_setup_timing(void)
+{
+	struct stm32_fmc2_nand_timings tims;
+	unsigned long hclk = clk_get_rate(stm32_fmc2.clock_id);
+	unsigned long hclkp = FMC2_PSEC_PER_MSEC / (hclk / 1000U);
+	unsigned long timing, tar, tclr, thiz, twait;
+	unsigned long tset_mem, tset_att, thold_mem, thold_att;
+	uint32_t pcr, pmem, patt;
+
+	tar = MAX(hclkp, NAND_TAR_MIN);
+	timing = div_round_up(tar, hclkp) - 1U;
+	tims.tar = MIN(timing, (unsigned long)FMC2_PCR_TIMING_MASK);
+
+	tclr = MAX(hclkp, NAND_TCLR_MIN);
+	timing = div_round_up(tclr, hclkp) - 1U;
+	tims.tclr = MIN(timing, (unsigned long)FMC2_PCR_TIMING_MASK);
+
+	tims.thiz = FMC2_THIZ;
+	thiz = (tims.thiz + 1U) * hclkp;
+
+	/*
+	 * tWAIT > tRP
+	 * tWAIT > tWP
+	 * tWAIT > tREA + tIO
+	 */
+	twait = MAX(hclkp, NAND_TRP_MIN);
+	twait = MAX(twait, NAND_TWP_MIN);
+	twait = MAX(twait, NAND_TREA_MAX + FMC2_TIO);
+	timing = div_round_up(twait, hclkp);
+	tims.twait = CLAMP(timing, 1UL,
+			   (unsigned long)FMC2_PMEM_PATT_TIMING_MASK);
+
+	/*
+	 * tSETUP_MEM > tCS - tWAIT
+	 * tSETUP_MEM > tALS - tWAIT
+	 * tSETUP_MEM > tDS - (tWAIT - tHIZ)
+	 */
+	tset_mem = hclkp;
+	if ((twait < NAND_TCS_MIN) && (tset_mem < (NAND_TCS_MIN - twait))) {
+		tset_mem = NAND_TCS_MIN - twait;
+	}
+	if ((twait > thiz) && ((twait - thiz) < NAND_TDS_MIN) &&
+	    (tset_mem < (NAND_TDS_MIN - (twait - thiz)))) {
+		tset_mem = NAND_TDS_MIN - (twait - thiz);
+	}
+	timing = div_round_up(tset_mem, hclkp);
+	tims.tset_mem = CLAMP(timing, 1UL,
+			      (unsigned long)FMC2_PMEM_PATT_TIMING_MASK);
+
+	/*
+	 * tHOLD_MEM > tCH
+	 * tHOLD_MEM > tREH - tSETUP_MEM
+	 * tHOLD_MEM > max(tRC, tWC) - (tSETUP_MEM + tWAIT)
+	 */
+	thold_mem = MAX(hclkp, NAND_TCH_MIN);
+	if ((tset_mem < NAND_TREH_MIN) &&
+	    (thold_mem < (NAND_TREH_MIN - tset_mem))) {
+		thold_mem = NAND_TREH_MIN - tset_mem;
+	}
+	if (((tset_mem + twait) < NAND_TRC_MIN) &&
+	    (thold_mem < (NAND_TRC_MIN - (tset_mem + twait)))) {
+		thold_mem = NAND_TRC_MIN  - (tset_mem + twait);
+	}
+	if (((tset_mem + twait) < NAND_TWC_MIN) &&
+	    (thold_mem < (NAND_TWC_MIN - (tset_mem + twait)))) {
+		thold_mem = NAND_TWC_MIN - (tset_mem + twait);
+	}
+	timing = div_round_up(thold_mem, hclkp);
+	tims.thold_mem = CLAMP(timing, 1UL,
+			       (unsigned long)FMC2_PMEM_PATT_TIMING_MASK);
+
+	/*
+	 * tSETUP_ATT > tCS - tWAIT
+	 * tSETUP_ATT > tCLS - tWAIT
+	 * tSETUP_ATT > tALS - tWAIT
+	 * tSETUP_ATT > tRHW - tHOLD_MEM
+	 * tSETUP_ATT > tDS - (tWAIT - tHIZ)
+	 */
+	tset_att = hclkp;
+	if ((twait < NAND_TCS_MIN) && (tset_att < (NAND_TCS_MIN - twait))) {
+		tset_att = NAND_TCS_MIN - twait;
+	}
+	if ((thold_mem < NAND_TRHW_MIN) &&
+	    (tset_att < (NAND_TRHW_MIN - thold_mem))) {
+		tset_att = NAND_TRHW_MIN - thold_mem;
+	}
+	if ((twait > thiz) && ((twait - thiz) < NAND_TDS_MIN) &&
+	    (tset_att < (NAND_TDS_MIN - (twait - thiz)))) {
+		tset_att = NAND_TDS_MIN - (twait - thiz);
+	}
+	timing = div_round_up(tset_att, hclkp);
+	tims.tset_att = CLAMP(timing, 1UL,
+			      (unsigned long)FMC2_PMEM_PATT_TIMING_MASK);
+
+	/*
+	 * tHOLD_ATT > tALH
+	 * tHOLD_ATT > tCH
+	 * tHOLD_ATT > tCLH
+	 * tHOLD_ATT > tCOH
+	 * tHOLD_ATT > tDH
+	 * tHOLD_ATT > tWB + tIO + tSYNC - tSETUP_MEM
+	 * tHOLD_ATT > tADL - tSETUP_MEM
+	 * tHOLD_ATT > tWH - tSETUP_MEM
+	 * tHOLD_ATT > tWHR - tSETUP_MEM
+	 * tHOLD_ATT > tRC - (tSETUP_ATT + tWAIT)
+	 * tHOLD_ATT > tWC - (tSETUP_ATT + tWAIT)
+	 */
+	thold_att = MAX(hclkp, NAND_TALH_MIN);
+	thold_att = MAX(thold_att, NAND_TCH_MIN);
+	thold_att = MAX(thold_att, NAND_TCLH_MIN);
+	thold_att = MAX(thold_att, NAND_TCOH_MIN);
+	thold_att = MAX(thold_att, NAND_TDH_MIN);
+	if (((NAND_TWB_MAX + FMC2_TIO + FMC2_TSYNC) > tset_mem) &&
+	    (thold_att < (NAND_TWB_MAX + FMC2_TIO + FMC2_TSYNC - tset_mem))) {
+		thold_att = NAND_TWB_MAX + FMC2_TIO + FMC2_TSYNC - tset_mem;
+	}
+	if ((tset_mem < NAND_TADL_MIN) &&
+	    (thold_att < (NAND_TADL_MIN - tset_mem))) {
+		thold_att = NAND_TADL_MIN - tset_mem;
+	}
+	if ((tset_mem < NAND_TWH_MIN) &&
+	    (thold_att < (NAND_TWH_MIN - tset_mem))) {
+		thold_att = NAND_TWH_MIN - tset_mem;
+	}
+	if ((tset_mem < NAND_TWHR_MIN) &&
+	    (thold_att < (NAND_TWHR_MIN - tset_mem))) {
+		thold_att = NAND_TWHR_MIN - tset_mem;
+	}
+	if (((tset_att + twait) < NAND_TRC_MIN) &&
+	    (thold_att < (NAND_TRC_MIN - (tset_att + twait)))) {
+		thold_att = NAND_TRC_MIN - (tset_att + twait);
+	}
+	if (((tset_att + twait) < NAND_TWC_MIN) &&
+	    (thold_att < (NAND_TWC_MIN - (tset_att + twait)))) {
+		thold_att = NAND_TWC_MIN - (tset_att + twait);
+	}
+	timing = div_round_up(thold_att, hclkp);
+	tims.thold_att = CLAMP(timing, 1UL,
+			       (unsigned long)FMC2_PMEM_PATT_TIMING_MASK);
+
+	VERBOSE("NAND timings: %u - %u - %u - %u - %u - %u - %u - %u\n",
+		tims.tclr, tims.tar, tims.thiz, tims.twait,
+		tims.thold_mem, tims.tset_mem,
+		tims.thold_att, tims.tset_att);
+
+	/* Set tclr/tar timings */
+	pcr = mmio_read_32(fmc2_base() + FMC2_PCR);
+	pcr &= ~FMC2_PCR_TCLR_MASK;
+	pcr |= FMC2_PCR_TCLR(tims.tclr);
+	pcr &= ~FMC2_PCR_TAR_MASK;
+	pcr |= FMC2_PCR_TAR(tims.tar);
+
+	/* Set tset/twait/thold/thiz timings in common bank */
+	pmem = FMC2_PMEM_MEMSET(tims.tset_mem);
+	pmem |= FMC2_PMEM_MEMWAIT(tims.twait);
+	pmem |=	FMC2_PMEM_MEMHOLD(tims.thold_mem);
+	pmem |= FMC2_PMEM_MEMHIZ(tims.thiz);
+
+	/* Set tset/twait/thold/thiz timings in attribute bank */
+	patt = FMC2_PATT_ATTSET(tims.tset_att);
+	patt |= FMC2_PATT_ATTWAIT(tims.twait);
+	patt |= FMC2_PATT_ATTHOLD(tims.thold_att);
+	patt |= FMC2_PATT_ATTHIZ(tims.thiz);
+
+	mmio_write_32(fmc2_base() + FMC2_PCR, pcr);
+	mmio_write_32(fmc2_base() + FMC2_PMEM, pmem);
+	mmio_write_32(fmc2_base() + FMC2_PATT, patt);
+}
+
+static void stm32_fmc2_set_buswidth_16(bool set)
+{
+	mmio_clrsetbits_32(fmc2_base() + FMC2_PCR, FMC2_PCR_PWID_MASK,
+			   (set ? FMC2_PCR_PWID(FMC2_PCR_PWID_16) : 0U));
+}
+
+static void stm32_fmc2_set_ecc(bool enable)
+{
+	mmio_clrsetbits_32(fmc2_base() + FMC2_PCR, FMC2_PCR_ECCEN,
+			   (enable ? FMC2_PCR_ECCEN : 0U));
+}
+
+static int stm32_fmc2_ham_correct(uint8_t *buffer, uint8_t *eccbuffer,
+				  uint8_t *ecc)
+{
+	uint8_t xor_ecc_ones;
+	uint16_t xor_ecc_1b, xor_ecc_2b, xor_ecc_3b;
+	union {
+		uint32_t val;
+		uint8_t  bytes[4];
+	} xor_ecc;
+
+	/* Page size--------ECC_Code Size
+	 * 256---------------22 bits LSB  (ECC_CODE & 0x003FFFFF)
+	 * 512---------------24 bits      (ECC_CODE & 0x00FFFFFF)
+	 * 1024--------------26 bits      (ECC_CODE & 0x03FFFFFF)
+	 * 2048--------------28 bits      (ECC_CODE & 0x0FFFFFFF)
+	 * 4096--------------30 bits      (ECC_CODE & 0x3FFFFFFF)
+	 * 8192--------------32 bits      (ECC_CODE & 0xFFFFFFFF)
+	 */
+
+	/* For Page size 512, ECC_Code size 24 bits */
+	xor_ecc_1b = ecc[0] ^ eccbuffer[0];
+	xor_ecc_2b = ecc[1] ^ eccbuffer[1];
+	xor_ecc_3b = ecc[2] ^ eccbuffer[2];
+
+	xor_ecc.val = 0U;
+	xor_ecc.bytes[2] = xor_ecc_3b;
+	xor_ecc.bytes[1] = xor_ecc_2b;
+	xor_ecc.bytes[0] = xor_ecc_1b;
+
+	if (xor_ecc.val == 0U) {
+		return 0; /* No Error */
+	}
+
+	xor_ecc_ones = __builtin_popcount(xor_ecc.val);
+	if (xor_ecc_ones < 23U) {
+		if (xor_ecc_ones == 12U) {
+			uint16_t bit_address, byte_address;
+
+			/* Correctable ERROR */
+			bit_address = ((xor_ecc_1b >> 1) & BIT(0)) |
+				      ((xor_ecc_1b >> 2) & BIT(1)) |
+				      ((xor_ecc_1b >> 3) & BIT(2));
+
+			byte_address = ((xor_ecc_1b >> 7) & BIT(0)) |
+				       ((xor_ecc_2b) & BIT(1)) |
+				       ((xor_ecc_2b >> 1) & BIT(2)) |
+				       ((xor_ecc_2b >> 2) & BIT(3)) |
+				       ((xor_ecc_2b >> 3) & BIT(4)) |
+				       ((xor_ecc_3b << 4) & BIT(5)) |
+				       ((xor_ecc_3b << 3) & BIT(6)) |
+				       ((xor_ecc_3b << 2) & BIT(7)) |
+				       ((xor_ecc_3b << 1) & BIT(8));
+
+			/* Correct bit error in the data */
+			buffer[byte_address] =
+				buffer[byte_address] ^ BIT(bit_address);
+			VERBOSE("Hamming: 1 ECC error corrected\n");
+
+			return 0;
+		}
+
+		/* Non Correctable ERROR */
+		ERROR("%s: Uncorrectable ECC Errors\n", __func__);
+		return -1;
+	}
+
+	/* ECC ERROR */
+	ERROR("%s: Hamming correction error\n", __func__);
+	return -1;
+}
+
+
+static int stm32_fmc2_ham_calculate(uint8_t *buffer, uint8_t *ecc)
+{
+	uint32_t heccr;
+	uint64_t timeout = timeout_init_us(TIMEOUT_US_10_MS);
+
+	while ((mmio_read_32(fmc2_base() + FMC2_SR) & FMC2_SR_NWRF) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			return -ETIMEDOUT;
+		}
+	}
+
+	heccr = mmio_read_32(fmc2_base() + FMC2_HECCR);
+
+	ecc[0] = heccr;
+	ecc[1] = heccr >> 8;
+	ecc[2] = heccr >> 16;
+
+	/* Disable ECC */
+	stm32_fmc2_set_ecc(false);
+
+	return 0;
+}
+
+static int stm32_fmc2_bch_correct(uint8_t *buffer, unsigned int eccsize)
+{
+	uint32_t bchdsr0, bchdsr1, bchdsr2, bchdsr3, bchdsr4;
+	uint16_t pos[8];
+	int i, den;
+	uint64_t timeout = timeout_init_us(TIMEOUT_US_10_MS);
+
+	while ((mmio_read_32(fmc2_base() + FMC2_BCHISR) &
+		FMC2_BCHISR_DERF) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			return -ETIMEDOUT;
+		}
+	}
+
+	bchdsr0 = mmio_read_32(fmc2_base() + FMC2_BCHDSR0);
+	bchdsr1 = mmio_read_32(fmc2_base() + FMC2_BCHDSR1);
+	bchdsr2 = mmio_read_32(fmc2_base() + FMC2_BCHDSR2);
+	bchdsr3 = mmio_read_32(fmc2_base() + FMC2_BCHDSR3);
+	bchdsr4 = mmio_read_32(fmc2_base() + FMC2_BCHDSR4);
+
+	/* Disable ECC */
+	stm32_fmc2_set_ecc(false);
+
+	/* No error found */
+	if ((bchdsr0 & FMC2_BCHDSR0_DEF) == 0U) {
+		return 0;
+	}
+
+	/* Too many errors detected */
+	if ((bchdsr0 & FMC2_BCHDSR0_DUE) != 0U) {
+		return -EBADMSG;
+	}
+
+	pos[0] = bchdsr1 & FMC2_BCHDSR1_EBP1_MASK;
+	pos[1] = (bchdsr1 & FMC2_BCHDSR1_EBP2_MASK) >> FMC2_BCHDSR1_EBP2_SHIFT;
+	pos[2] = bchdsr2 & FMC2_BCHDSR2_EBP3_MASK;
+	pos[3] = (bchdsr2 & FMC2_BCHDSR2_EBP4_MASK) >> FMC2_BCHDSR2_EBP4_SHIFT;
+	pos[4] = bchdsr3 & FMC2_BCHDSR3_EBP5_MASK;
+	pos[5] = (bchdsr3 & FMC2_BCHDSR3_EBP6_MASK) >> FMC2_BCHDSR3_EBP6_SHIFT;
+	pos[6] = bchdsr4 & FMC2_BCHDSR4_EBP7_MASK;
+	pos[7] = (bchdsr4 & FMC2_BCHDSR4_EBP8_MASK) >> FMC2_BCHDSR4_EBP8_SHIFT;
+
+	den = (bchdsr0 & FMC2_BCHDSR0_DEN_MASK) >> FMC2_BCHDSR0_DEN_SHIFT;
+	for (i = 0; i < den; i++) {
+		if (pos[i] < (eccsize * 8U)) {
+			uint8_t bitmask = BIT(pos[i] % 8U);
+			uint32_t offset = pos[i] / 8U;
+
+			*(buffer + offset) ^= bitmask;
+		}
+	}
+
+	return 0;
+}
+
+static void stm32_fmc2_hwctl(struct nand_device *nand)
+{
+	stm32_fmc2_set_ecc(false);
+
+	if (nand->ecc.max_bit_corr != FMC2_ECC_HAM) {
+		mmio_clrbits_32(fmc2_base() + FMC2_PCR, FMC2_PCR_WEN);
+		mmio_write_32(fmc2_base() + FMC2_BCHICR, FMC2_BCHICR_CLEAR_IRQ);
+	}
+
+	stm32_fmc2_set_ecc(true);
+}
+
+static int stm32_fmc2_read_page(struct nand_device *nand,
+				unsigned int page, uintptr_t buffer)
+{
+	unsigned int eccsize = nand->ecc.size;
+	unsigned int eccbytes = nand->ecc.bytes;
+	unsigned int eccsteps = nand->page_size / eccsize;
+	uint8_t ecc_corr[FMC2_MAX_ECC_BYTES];
+	uint8_t ecc_cal[FMC2_MAX_ECC_BYTES] = {0U};
+	uint8_t *p;
+	unsigned int i;
+	unsigned int s;
+	int ret;
+
+	VERBOSE(">%s page %u buffer %lx\n", __func__, page, buffer);
+
+	ret = nand_read_page_cmd(page, 0U, 0U, 0U);
+	if (ret != 0) {
+		return ret;
+	}
+
+	for (s = 0U, i = nand->page_size + FMC2_BBM_LEN, p = (uint8_t *)buffer;
+	     s < eccsteps;
+	     s++, i += eccbytes, p += eccsize) {
+		stm32_fmc2_hwctl(nand);
+
+		/* Read the NAND page sector (512 bytes) */
+		ret = nand_change_read_column_cmd(s * eccsize, (uintptr_t)p,
+						  eccsize);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if (nand->ecc.max_bit_corr == FMC2_ECC_HAM) {
+			ret = stm32_fmc2_ham_calculate(p, ecc_cal);
+			if (ret != 0) {
+				return ret;
+			}
+		}
+
+		/* Read the corresponding ECC bytes */
+		ret = nand_change_read_column_cmd(i, (uintptr_t)ecc_corr,
+						  eccbytes);
+		if (ret != 0) {
+			return ret;
+		}
+
+		/* Correct the data */
+		if (nand->ecc.max_bit_corr == FMC2_ECC_HAM) {
+			ret = stm32_fmc2_ham_correct(p, ecc_corr, ecc_cal);
+		} else {
+			ret = stm32_fmc2_bch_correct(p, eccsize);
+		}
+
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static void stm32_fmc2_read_data(struct nand_device *nand,
+				 uint8_t *buff, unsigned int length,
+				 bool use_bus8)
+{
+	uintptr_t data_base = stm32_fmc2.cs[stm32_fmc2.cs_sel].data_base;
+
+	if (use_bus8 && (nand->buswidth == NAND_BUS_WIDTH_16)) {
+		stm32_fmc2_set_buswidth_16(false);
+	}
+
+	if ((((uintptr_t)buff & BIT(0)) != 0U) && (length != 0U)) {
+		*buff = mmio_read_8(data_base);
+		buff += sizeof(uint8_t);
+		length -= sizeof(uint8_t);
+	}
+
+	if ((((uintptr_t)buff & GENMASK_32(1, 0)) != 0U) &&
+	    (length >= sizeof(uint16_t))) {
+		*(uint16_t *)buff = mmio_read_16(data_base);
+		buff += sizeof(uint16_t);
+		length -= sizeof(uint16_t);
+	}
+
+	/* 32bit aligned */
+	while (length >= sizeof(uint32_t)) {
+		*(uint32_t *)buff = mmio_read_32(data_base);
+		buff += sizeof(uint32_t);
+		length -= sizeof(uint32_t);
+	}
+
+	/* Read remaining bytes */
+	if (length >= sizeof(uint16_t)) {
+		*(uint16_t *)buff = mmio_read_16(data_base);
+		buff += sizeof(uint16_t);
+		length -= sizeof(uint16_t);
+	}
+
+	if (length != 0U) {
+		*buff = mmio_read_8(data_base);
+	}
+
+	if (use_bus8 && (nand->buswidth == NAND_BUS_WIDTH_16)) {
+		/* Reconfigure bus width to 16-bit */
+		stm32_fmc2_set_buswidth_16(true);
+	}
+}
+
+static void stm32_fmc2_write_data(struct nand_device *nand,
+				  uint8_t *buff, unsigned int length,
+				  bool use_bus8)
+{
+	uintptr_t data_base = stm32_fmc2.cs[stm32_fmc2.cs_sel].data_base;
+
+	if (use_bus8 && (nand->buswidth == NAND_BUS_WIDTH_16)) {
+		/* Reconfigure bus width to 8-bit */
+		stm32_fmc2_set_buswidth_16(false);
+	}
+
+	if ((((uintptr_t)buff & BIT(0)) != 0U) && (length != 0U)) {
+		mmio_write_8(data_base, *buff);
+		buff += sizeof(uint8_t);
+		length -= sizeof(uint8_t);
+	}
+
+	if ((((uintptr_t)buff & GENMASK_32(1, 0)) != 0U) &&
+	    (length >= sizeof(uint16_t))) {
+		mmio_write_16(data_base, *(uint16_t *)buff);
+		buff += sizeof(uint16_t);
+		length -= sizeof(uint16_t);
+	}
+
+	/* 32bits aligned */
+	while (length >= sizeof(uint32_t)) {
+		mmio_write_32(data_base, *(uint32_t *)buff);
+		buff += sizeof(uint32_t);
+		length -= sizeof(uint32_t);
+	}
+
+	/* Read remaining bytes */
+	if (length >= sizeof(uint16_t)) {
+		mmio_write_16(data_base, *(uint16_t *)buff);
+		buff += sizeof(uint16_t);
+		length -= sizeof(uint16_t);
+	}
+
+	if (length != 0U) {
+		mmio_write_8(data_base, *buff);
+	}
+
+	if (use_bus8 && (nand->buswidth == NAND_BUS_WIDTH_16)) {
+		/* Reconfigure bus width to 16-bit */
+		stm32_fmc2_set_buswidth_16(true);
+	}
+}
+
+static void stm32_fmc2_ctrl_init(void)
+{
+	uint32_t pcr = mmio_read_32(fmc2_base() + FMC2_PCR);
+	uint32_t bcr1 = mmio_read_32(fmc2_base() + FMC2_BCR1);
+
+	/* Enable wait feature and NAND flash memory bank */
+	pcr |= FMC2_PCR_PWAITEN;
+	pcr |= FMC2_PCR_PBKEN;
+
+	/* Set buswidth to 8 bits mode for identification */
+	pcr &= ~FMC2_PCR_PWID_MASK;
+
+	/* ECC logic is disabled */
+	pcr &= ~FMC2_PCR_ECCEN;
+
+	/* Default mode */
+	pcr &= ~FMC2_PCR_ECCALG;
+	pcr &= ~FMC2_PCR_BCHECC;
+	pcr &= ~FMC2_PCR_WEN;
+
+	/* Set default ECC sector size */
+	pcr &= ~FMC2_PCR_ECCSS_MASK;
+	pcr |= FMC2_PCR_ECCSS(FMC2_PCR_ECCSS_2048);
+
+	/* Set default TCLR/TAR timings */
+	pcr &= ~FMC2_PCR_TCLR_MASK;
+	pcr |= FMC2_PCR_TCLR(FMC2_PCR_TCLR_DEFAULT);
+	pcr &= ~FMC2_PCR_TAR_MASK;
+	pcr |= FMC2_PCR_TAR(FMC2_PCR_TAR_DEFAULT);
+
+	/* Enable FMC2 controller */
+	bcr1 |= FMC2_BCR1_FMC2EN;
+
+	mmio_write_32(fmc2_base() + FMC2_BCR1, bcr1);
+	mmio_write_32(fmc2_base() + FMC2_PCR, pcr);
+	mmio_write_32(fmc2_base() + FMC2_PMEM, FMC2_PMEM_DEFAULT);
+	mmio_write_32(fmc2_base() + FMC2_PATT, FMC2_PATT_DEFAULT);
+}
+
+static int stm32_fmc2_exec(struct nand_req *req)
+{
+	int ret = 0;
+
+	switch (req->type & NAND_REQ_MASK) {
+	case NAND_REQ_CMD:
+		VERBOSE("Write CMD %x\n", (uint8_t)req->type);
+		mmio_write_8(stm32_fmc2.cs[stm32_fmc2.cs_sel].cmd_base,
+			     (uint8_t)req->type);
+		break;
+	case NAND_REQ_ADDR:
+		VERBOSE("Write ADDR %x\n", *(req->addr));
+		mmio_write_8(stm32_fmc2.cs[stm32_fmc2.cs_sel].addr_base,
+			     *(req->addr));
+		break;
+	case NAND_REQ_DATAIN:
+		VERBOSE("Read data\n");
+		stm32_fmc2_read_data(req->nand, req->addr, req->length,
+				     ((req->type & NAND_REQ_BUS_WIDTH_8) !=
+				      0U));
+		break;
+	case NAND_REQ_DATAOUT:
+		VERBOSE("Write data\n");
+		stm32_fmc2_write_data(req->nand, req->addr, req->length,
+				      ((req->type & NAND_REQ_BUS_WIDTH_8) !=
+				      0U));
+		break;
+	case NAND_REQ_WAIT:
+		VERBOSE("WAIT Ready\n");
+		ret = nand_wait_ready(req->delay_ms);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	};
+
+	return ret;
+}
+
+static void stm32_fmc2_setup(struct nand_device *nand)
+{
+	uint32_t pcr = mmio_read_32(fmc2_base() + FMC2_PCR);
+
+	/* Set buswidth */
+	pcr &= ~FMC2_PCR_PWID_MASK;
+	if (nand->buswidth == NAND_BUS_WIDTH_16) {
+		pcr |= FMC2_PCR_PWID(FMC2_PCR_PWID_16);
+	}
+
+	if (nand->ecc.mode == NAND_ECC_HW) {
+		nand->mtd_read_page = stm32_fmc2_read_page;
+
+		pcr &= ~FMC2_PCR_ECCALG;
+		pcr &= ~FMC2_PCR_BCHECC;
+
+		pcr &= ~FMC2_PCR_ECCSS_MASK;
+		pcr |= FMC2_PCR_ECCSS(FMC2_PCR_ECCSS_512);
+
+		switch (nand->ecc.max_bit_corr) {
+		case FMC2_ECC_HAM:
+			nand->ecc.bytes = 3;
+			break;
+		case FMC2_ECC_BCH8:
+			pcr |= FMC2_PCR_ECCALG;
+			pcr |= FMC2_PCR_BCHECC;
+			nand->ecc.bytes = 13;
+			break;
+		default:
+			/* Use FMC2 ECC BCH4 */
+			pcr |= FMC2_PCR_ECCALG;
+			nand->ecc.bytes = 7;
+			break;
+		}
+
+		if ((nand->buswidth & NAND_BUS_WIDTH_16) != 0) {
+			nand->ecc.bytes++;
+		}
+	}
+
+	mmio_write_32(stm32_fmc2.reg_base + FMC2_PCR, pcr);
+}
+
+static const struct nand_ctrl_ops ctrl_ops = {
+	.setup = stm32_fmc2_setup,
+	.exec = stm32_fmc2_exec
+};
+
+int stm32_fmc2_init(void)
+{
+	int fmc_ebi_node;
+	int fmc_nfc_node;
+	int fmc_flash_node = 0;
+	int nchips = 0;
+	unsigned int i;
+	void *fdt = NULL;
+	const fdt32_t *cuint;
+	struct dt_node_info info;
+	uintptr_t bank_address[MAX_BANK] = { 0, 0, 0, 0, 0 };
+	uint8_t bank_assigned = 0;
+	uint8_t bank;
+	int ret;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	fmc_ebi_node = dt_get_node(&info, -1, DT_FMC2_EBI_COMPAT);
+	if (fmc_ebi_node < 0) {
+		return fmc_ebi_node;
+	}
+
+	if (info.status == DT_DISABLED) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	stm32_fmc2.reg_base = info.base;
+
+	if ((info.clock < 0) || (info.reset < 0)) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	stm32_fmc2.clock_id = (unsigned long)info.clock;
+	stm32_fmc2.reset_id = (unsigned int)info.reset;
+
+	cuint = fdt_getprop(fdt, fmc_ebi_node, "ranges", NULL);
+	if (cuint == NULL) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	for (i = 0U; i < MAX_BANK; i++) {
+		bank = fdt32_to_cpu(*cuint);
+		if ((bank >= MAX_BANK) || ((bank_assigned & BIT(bank)) != 0U)) {
+			return -FDT_ERR_BADVALUE;
+		}
+		bank_assigned |= BIT(bank);
+		bank_address[bank] = fdt32_to_cpu(*(cuint + 2));
+		cuint += 4;
+	}
+
+	/* Pinctrl initialization */
+	if (dt_set_pinctrl_config(fmc_ebi_node) != 0) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	/* Parse NFC controller node */
+	fmc_nfc_node = fdt_node_offset_by_compatible(fdt, fmc_ebi_node,
+						     DT_FMC2_NFC_COMPAT);
+	if (fmc_nfc_node < 0) {
+		return fmc_nfc_node;
+	}
+
+	if (fdt_get_status(fmc_nfc_node) == DT_DISABLED) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	cuint = fdt_getprop(fdt, fmc_nfc_node, "reg", NULL);
+	if (cuint == NULL) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	for (i = 0U; i < MAX_CS; i++) {
+		bank = fdt32_to_cpu(*cuint);
+		if (bank >= MAX_BANK) {
+			return -FDT_ERR_BADVALUE;
+		}
+		stm32_fmc2.cs[i].data_base = fdt32_to_cpu(*(cuint + 1)) +
+					     bank_address[bank];
+
+		bank = fdt32_to_cpu(*(cuint + 3));
+		if (bank >= MAX_BANK) {
+			return -FDT_ERR_BADVALUE;
+		}
+		stm32_fmc2.cs[i].cmd_base = fdt32_to_cpu(*(cuint + 4)) +
+					    bank_address[bank];
+
+		bank = fdt32_to_cpu(*(cuint + 6));
+		if (bank >= MAX_BANK) {
+			return -FDT_ERR_BADVALUE;
+		}
+		stm32_fmc2.cs[i].addr_base = fdt32_to_cpu(*(cuint + 7)) +
+					     bank_address[bank];
+
+		cuint += 9;
+	}
+
+	/* Parse flash nodes */
+	fdt_for_each_subnode(fmc_flash_node, fdt, fmc_nfc_node) {
+		nchips++;
+	}
+
+	if (nchips != 1) {
+		WARN("Only one SLC NAND device supported\n");
+		return -FDT_ERR_BADVALUE;
+	}
+
+	fdt_for_each_subnode(fmc_flash_node, fdt, fmc_nfc_node) {
+		/* Get chip select */
+		cuint = fdt_getprop(fdt, fmc_flash_node, "reg", NULL);
+		if (cuint == NULL) {
+			WARN("Chip select not well defined\n");
+			return -FDT_ERR_BADVALUE;
+		}
+
+		stm32_fmc2.cs_sel = fdt32_to_cpu(*cuint);
+		if (stm32_fmc2.cs_sel >= MAX_CS) {
+			return -FDT_ERR_BADVALUE;
+		}
+
+		VERBOSE("NAND CS %i\n", stm32_fmc2.cs_sel);
+	}
+
+	/* Enable Clock */
+	clk_enable(stm32_fmc2.clock_id);
+
+	/* Reset IP */
+	ret = stm32mp_reset_assert(stm32_fmc2.reset_id, TIMEOUT_US_1_MS);
+	if (ret != 0) {
+		panic();
+	}
+	ret = stm32mp_reset_deassert(stm32_fmc2.reset_id, TIMEOUT_US_1_MS);
+	if (ret != 0) {
+		panic();
+	}
+
+	/* Setup default IP registers */
+	stm32_fmc2_ctrl_init();
+
+	/* Setup default timings */
+	stm32_fmc2_nand_setup_timing();
+
+	/* Init NAND RAW framework */
+	nand_raw_ctrl_init(&ctrl_ops);
+
+	return 0;
+}
diff --git a/drivers/st/gpio/stm32_gpio.c b/drivers/st/gpio/stm32_gpio.c
new file mode 100644
index 0000000..708989f
--- /dev/null
+++ b/drivers/st/gpio/stm32_gpio.c
@@ -0,0 +1,323 @@
+/*
+ * Copyright (c) 2016-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#define DT_GPIO_BANK_SHIFT	12
+#define DT_GPIO_BANK_MASK	GENMASK(16, 12)
+#define DT_GPIO_PIN_SHIFT	8
+#define DT_GPIO_PIN_MASK	GENMASK(11, 8)
+#define DT_GPIO_MODE_MASK	GENMASK(7, 0)
+
+static void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t type,
+		     uint32_t speed, uint32_t pull, uint32_t od,
+		     uint32_t alternate, uint8_t status);
+
+/*******************************************************************************
+ * This function gets GPIO bank node in DT.
+ * Returns node offset if status is okay in DT, else return 0
+ ******************************************************************************/
+static int ckeck_gpio_bank(void *fdt, uint32_t bank, int pinctrl_node)
+{
+	int pinctrl_subnode;
+	uint32_t bank_offset = stm32_get_gpio_bank_offset(bank);
+
+	fdt_for_each_subnode(pinctrl_subnode, fdt, pinctrl_node) {
+		const fdt32_t *cuint;
+
+		if (fdt_getprop(fdt, pinctrl_subnode,
+				"gpio-controller", NULL) == NULL) {
+			continue;
+		}
+
+		cuint = fdt_getprop(fdt, pinctrl_subnode, "reg", NULL);
+		if (cuint == NULL) {
+			continue;
+		}
+
+		if ((fdt32_to_cpu(*cuint) == bank_offset) &&
+		    (fdt_get_status(pinctrl_subnode) != DT_DISABLED)) {
+			return pinctrl_subnode;
+		}
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function gets the pin settings from DT information.
+ * When analyze and parsing is done, set the GPIO registers.
+ * Returns 0 on success and a negative FDT error code on failure.
+ ******************************************************************************/
+static int dt_set_gpio_config(void *fdt, int node, uint8_t status)
+{
+	const fdt32_t *cuint, *slewrate;
+	int len;
+	int pinctrl_node;
+	uint32_t i;
+	uint32_t speed = GPIO_SPEED_LOW;
+	uint32_t pull = GPIO_NO_PULL;
+
+	cuint = fdt_getprop(fdt, node, "pinmux", &len);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	pinctrl_node = fdt_parent_offset(fdt, fdt_parent_offset(fdt, node));
+	if (pinctrl_node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	slewrate = fdt_getprop(fdt, node, "slew-rate", NULL);
+	if (slewrate != NULL) {
+		speed = fdt32_to_cpu(*slewrate);
+	}
+
+	if (fdt_getprop(fdt, node, "bias-pull-up", NULL) != NULL) {
+		pull = GPIO_PULL_UP;
+	} else if (fdt_getprop(fdt, node, "bias-pull-down", NULL) != NULL) {
+		pull = GPIO_PULL_DOWN;
+	} else {
+		VERBOSE("No bias configured in node %d\n", node);
+	}
+
+	for (i = 0U; i < ((uint32_t)len / sizeof(uint32_t)); i++) {
+		uint32_t pincfg;
+		uint32_t bank;
+		uint32_t pin;
+		uint32_t mode;
+		uint32_t alternate = GPIO_ALTERNATE_(0);
+		uint32_t type;
+		uint32_t od = GPIO_OD_OUTPUT_LOW;
+		int bank_node;
+		int clk;
+
+		pincfg = fdt32_to_cpu(*cuint);
+		cuint++;
+
+		bank = (pincfg & DT_GPIO_BANK_MASK) >> DT_GPIO_BANK_SHIFT;
+
+		pin = (pincfg & DT_GPIO_PIN_MASK) >> DT_GPIO_PIN_SHIFT;
+
+		mode = pincfg & DT_GPIO_MODE_MASK;
+
+		switch (mode) {
+		case 0:
+			mode = GPIO_MODE_INPUT;
+			break;
+		case 1 ... 16:
+			alternate = mode - 1U;
+			mode = GPIO_MODE_ALTERNATE;
+			break;
+		case 17:
+			mode = GPIO_MODE_ANALOG;
+			break;
+		default:
+			mode = GPIO_MODE_OUTPUT;
+			break;
+		}
+
+		if (fdt_getprop(fdt, node, "drive-open-drain", NULL) != NULL) {
+			type = GPIO_TYPE_OPEN_DRAIN;
+		} else {
+			type = GPIO_TYPE_PUSH_PULL;
+		}
+
+		if (fdt_getprop(fdt, node, "output-high", NULL) != NULL) {
+			if (mode == GPIO_MODE_INPUT) {
+				mode = GPIO_MODE_OUTPUT;
+				od = GPIO_OD_OUTPUT_HIGH;
+			}
+		}
+
+		if (fdt_getprop(fdt, node, "output-low", NULL) != NULL) {
+			if (mode == GPIO_MODE_INPUT) {
+				mode = GPIO_MODE_OUTPUT;
+				od = GPIO_OD_OUTPUT_LOW;
+			}
+		}
+
+		bank_node = ckeck_gpio_bank(fdt, bank, pinctrl_node);
+		if (bank_node == 0) {
+			ERROR("PINCTRL inconsistent in DT\n");
+			panic();
+		}
+
+		clk = fdt_get_clock_id(bank_node);
+		if (clk < 0) {
+			return -FDT_ERR_NOTFOUND;
+		}
+
+		/* Platform knows the clock: assert it is okay */
+		assert((unsigned long)clk == stm32_get_gpio_bank_clock(bank));
+
+		set_gpio(bank, pin, mode, type, speed, pull, od, alternate, status);
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function gets the pin settings from DT information.
+ * When analyze and parsing is done, set the GPIO registers.
+ * Returns 0 on success and a negative FDT/ERRNO error code on failure.
+ ******************************************************************************/
+int dt_set_pinctrl_config(int node)
+{
+	const fdt32_t *cuint;
+	int lenp;
+	uint32_t i;
+	uint8_t status;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	status = fdt_get_status(node);
+	if (status == DT_DISABLED) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	cuint = fdt_getprop(fdt, node, "pinctrl-0", &lenp);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	for (i = 0; i < ((uint32_t)lenp / 4U); i++) {
+		int p_node, p_subnode;
+
+		p_node = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+		if (p_node < 0) {
+			return -FDT_ERR_NOTFOUND;
+		}
+
+		fdt_for_each_subnode(p_subnode, fdt, p_node) {
+			int ret = dt_set_gpio_config(fdt, p_subnode, status);
+
+			if (ret < 0) {
+				return ret;
+			}
+		}
+
+		cuint++;
+	}
+
+	return 0;
+}
+
+static void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t type,
+		     uint32_t speed, uint32_t pull, uint32_t od,
+		     uint32_t alternate, uint8_t status)
+{
+	uintptr_t base = stm32_get_gpio_bank_base(bank);
+	unsigned long clock = stm32_get_gpio_bank_clock(bank);
+
+	assert(pin <= GPIO_PIN_MAX);
+
+	clk_enable(clock);
+
+	mmio_clrsetbits_32(base + GPIO_MODE_OFFSET,
+			   (uint32_t)GPIO_MODE_MASK << (pin << 1),
+			   mode << (pin << 1));
+
+	mmio_clrsetbits_32(base + GPIO_TYPE_OFFSET,
+			   (uint32_t)GPIO_TYPE_MASK << pin,
+			   type << pin);
+
+	mmio_clrsetbits_32(base + GPIO_SPEED_OFFSET,
+			   (uint32_t)GPIO_SPEED_MASK << (pin << 1),
+			   speed << (pin << 1));
+
+	mmio_clrsetbits_32(base + GPIO_PUPD_OFFSET,
+			   (uint32_t)GPIO_PULL_MASK << (pin << 1),
+			   pull << (pin << 1));
+
+	if (pin < GPIO_ALT_LOWER_LIMIT) {
+		mmio_clrsetbits_32(base + GPIO_AFRL_OFFSET,
+				   (uint32_t)GPIO_ALTERNATE_MASK << (pin << 2),
+				   alternate << (pin << 2));
+	} else {
+		size_t shift = (pin - GPIO_ALT_LOWER_LIMIT) << 2;
+
+		mmio_clrsetbits_32(base + GPIO_AFRH_OFFSET,
+				   (uint32_t)GPIO_ALTERNATE_MASK << shift,
+				   alternate << shift);
+	}
+
+	mmio_clrsetbits_32(base + GPIO_OD_OFFSET,
+			   (uint32_t)GPIO_OD_MASK << pin,
+			   od << pin);
+
+	VERBOSE("GPIO %u mode set to 0x%x\n", bank,
+		mmio_read_32(base + GPIO_MODE_OFFSET));
+	VERBOSE("GPIO %u type set to 0x%x\n", bank,
+		mmio_read_32(base + GPIO_TYPE_OFFSET));
+	VERBOSE("GPIO %u speed set to 0x%x\n", bank,
+		mmio_read_32(base + GPIO_SPEED_OFFSET));
+	VERBOSE("GPIO %u mode pull to 0x%x\n", bank,
+		mmio_read_32(base + GPIO_PUPD_OFFSET));
+	VERBOSE("GPIO %u mode alternate low to 0x%x\n", bank,
+		mmio_read_32(base + GPIO_AFRL_OFFSET));
+	VERBOSE("GPIO %u mode alternate high to 0x%x\n", bank,
+		mmio_read_32(base + GPIO_AFRH_OFFSET));
+	VERBOSE("GPIO %u output data set to 0x%x\n", bank,
+		mmio_read_32(base + GPIO_OD_OFFSET));
+
+	clk_disable(clock);
+
+	if (status == DT_SECURE) {
+		stm32mp_register_secure_gpio(bank, pin);
+#if !IMAGE_BL2
+		set_gpio_secure_cfg(bank, pin, true);
+#endif
+
+	} else {
+		stm32mp_register_non_secure_gpio(bank, pin);
+#if !IMAGE_BL2
+		set_gpio_secure_cfg(bank, pin, false);
+#endif
+	}
+}
+
+void set_gpio_secure_cfg(uint32_t bank, uint32_t pin, bool secure)
+{
+	uintptr_t base = stm32_get_gpio_bank_base(bank);
+	unsigned long clock = stm32_get_gpio_bank_clock(bank);
+
+	assert(pin <= GPIO_PIN_MAX);
+
+	clk_enable(clock);
+
+	if (secure) {
+		mmio_setbits_32(base + GPIO_SECR_OFFSET, BIT(pin));
+	} else {
+		mmio_clrbits_32(base + GPIO_SECR_OFFSET, BIT(pin));
+	}
+
+	clk_disable(clock);
+}
+
+void set_gpio_reset_cfg(uint32_t bank, uint32_t pin)
+{
+	set_gpio(bank, pin, GPIO_MODE_ANALOG, GPIO_TYPE_PUSH_PULL,
+		 GPIO_SPEED_LOW, GPIO_NO_PULL, GPIO_OD_OUTPUT_LOW,
+		 GPIO_ALTERNATE_(0), DT_DISABLED);
+	set_gpio_secure_cfg(bank, pin, stm32_gpio_is_secure_at_reset(bank));
+}
diff --git a/drivers/st/i2c/stm32_i2c.c b/drivers/st/i2c/stm32_i2c.c
new file mode 100644
index 0000000..bf6c3ee
--- /dev/null
+++ b/drivers/st/i2c/stm32_i2c.c
@@ -0,0 +1,982 @@
+/*
+ * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stdlib.h>
+
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32_i2c.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+
+/* STM32 I2C registers offsets */
+#define I2C_CR1			0x00U
+#define I2C_CR2			0x04U
+#define I2C_OAR1		0x08U
+#define I2C_OAR2		0x0CU
+#define I2C_TIMINGR		0x10U
+#define I2C_TIMEOUTR		0x14U
+#define I2C_ISR			0x18U
+#define I2C_ICR			0x1CU
+#define I2C_PECR		0x20U
+#define I2C_RXDR		0x24U
+#define I2C_TXDR		0x28U
+
+#define TIMINGR_CLEAR_MASK	0xF0FFFFFFU
+
+#define MAX_NBYTE_SIZE		255U
+
+#define I2C_NSEC_PER_SEC	1000000000L
+
+/* I2C Timing hard-coded value, for I2C clock source is HSI at 64MHz */
+#define I2C_TIMING			0x10D07DB5
+
+static void notif_i2c_timeout(struct i2c_handle_s *hi2c)
+{
+	hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+	hi2c->i2c_state = I2C_STATE_READY;
+}
+
+/*
+ * @brief  Configure I2C Analog noise filter.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C peripheral.
+ * @param  analog_filter: New state of the Analog filter
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_config_analog_filter(struct i2c_handle_s *hi2c,
+				    uint32_t analog_filter)
+{
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return -EBUSY;
+	}
+
+	hi2c->lock = 1;
+
+	hi2c->i2c_state = I2C_STATE_BUSY;
+
+	/* Disable the selected I2C peripheral */
+	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
+
+	/* Reset I2Cx ANOFF bit */
+	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_ANFOFF);
+
+	/* Set analog filter bit*/
+	mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR1, analog_filter);
+
+	/* Enable the selected I2C peripheral */
+	mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
+
+	hi2c->i2c_state = I2C_STATE_READY;
+
+	hi2c->lock = 0;
+
+	return 0;
+}
+
+/*
+ * @brief  Get I2C setup information from the device tree and set pinctrl
+ *         configuration.
+ * @param  fdt: Pointer to the device tree
+ * @param  node: I2C node offset
+ * @param  init: Ref to the initialization configuration structure
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_get_setup_from_fdt(void *fdt, int node,
+				 struct stm32_i2c_init_s *init)
+{
+	const fdt32_t *cuint;
+
+	cuint = fdt_getprop(fdt, node, "i2c-scl-rising-time-ns", NULL);
+	if (cuint == NULL) {
+		init->rise_time = STM32_I2C_RISE_TIME_DEFAULT;
+	} else {
+		init->rise_time = fdt32_to_cpu(*cuint);
+	}
+
+	cuint = fdt_getprop(fdt, node, "i2c-scl-falling-time-ns", NULL);
+	if (cuint == NULL) {
+		init->fall_time = STM32_I2C_FALL_TIME_DEFAULT;
+	} else {
+		init->fall_time = fdt32_to_cpu(*cuint);
+	}
+
+	cuint = fdt_getprop(fdt, node, "clock-frequency", NULL);
+	if (cuint == NULL) {
+		init->speed_mode = STM32_I2C_SPEED_DEFAULT;
+	} else {
+		switch (fdt32_to_cpu(*cuint)) {
+		case STANDARD_RATE:
+			init->speed_mode = I2C_SPEED_STANDARD;
+			break;
+		case FAST_RATE:
+			init->speed_mode = I2C_SPEED_FAST;
+			break;
+		case FAST_PLUS_RATE:
+			init->speed_mode = I2C_SPEED_FAST_PLUS;
+			break;
+		default:
+			init->speed_mode = STM32_I2C_SPEED_DEFAULT;
+			break;
+		}
+	}
+
+	return dt_set_pinctrl_config(node);
+}
+
+/*
+ * @brief  Initialize the I2C device.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  init_data: Initialization configuration structure
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_init(struct i2c_handle_s *hi2c,
+		   struct stm32_i2c_init_s *init_data)
+{
+	int rc = 0;
+	uint32_t timing = I2C_TIMING;
+
+	if (hi2c == NULL) {
+		return -ENOENT;
+	}
+
+	if (hi2c->i2c_state == I2C_STATE_RESET) {
+		hi2c->lock = 0;
+	}
+
+	hi2c->i2c_state = I2C_STATE_BUSY;
+
+	clk_enable(hi2c->clock);
+
+	/* Disable the selected I2C peripheral */
+	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
+
+	/* Configure I2Cx: Frequency range */
+	mmio_write_32(hi2c->i2c_base_addr + I2C_TIMINGR,
+		      timing & TIMINGR_CLEAR_MASK);
+
+	/* Disable Own Address1 before set the Own Address1 configuration */
+	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_OAR1, I2C_OAR1_OA1EN);
+
+	/* Configure I2Cx: Own Address1 and ack own address1 mode */
+	if (init_data->addressing_mode == I2C_ADDRESSINGMODE_7BIT) {
+		mmio_write_32(hi2c->i2c_base_addr + I2C_OAR1,
+			      I2C_OAR1_OA1EN | init_data->own_address1);
+	} else { /* I2C_ADDRESSINGMODE_10BIT */
+		mmio_write_32(hi2c->i2c_base_addr + I2C_OAR1,
+			      I2C_OAR1_OA1EN | I2C_OAR1_OA1MODE |
+			      init_data->own_address1);
+	}
+
+	mmio_write_32(hi2c->i2c_base_addr + I2C_CR2, 0);
+
+	/* Configure I2Cx: Addressing Master mode */
+	if (init_data->addressing_mode == I2C_ADDRESSINGMODE_10BIT) {
+		mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_CR2_ADD10);
+	}
+
+	/*
+	 * Enable the AUTOEND by default, and enable NACK
+	 * (should be disabled only during Slave process).
+	 */
+	mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR2,
+			I2C_CR2_AUTOEND | I2C_CR2_NACK);
+
+	/* Disable Own Address2 before set the Own Address2 configuration */
+	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_OAR2, I2C_DUALADDRESS_ENABLE);
+
+	/* Configure I2Cx: Dual mode and Own Address2 */
+	mmio_write_32(hi2c->i2c_base_addr + I2C_OAR2,
+		      init_data->dual_address_mode |
+		      init_data->own_address2 |
+		      (init_data->own_address2_masks << 8));
+
+	/* Configure I2Cx: Generalcall and NoStretch mode */
+	mmio_write_32(hi2c->i2c_base_addr + I2C_CR1,
+		      init_data->general_call_mode |
+		      init_data->no_stretch_mode);
+
+	/* Enable the selected I2C peripheral */
+	mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
+
+	hi2c->i2c_err = I2C_ERROR_NONE;
+	hi2c->i2c_state = I2C_STATE_READY;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+
+	rc = i2c_config_analog_filter(hi2c, init_data->analog_filter ?
+						I2C_ANALOGFILTER_ENABLE :
+						I2C_ANALOGFILTER_DISABLE);
+	if (rc != 0) {
+		ERROR("Cannot initialize I2C analog filter (%d)\n", rc);
+		clk_disable(hi2c->clock);
+		return rc;
+	}
+
+	clk_disable(hi2c->clock);
+
+	return rc;
+}
+
+/*
+ * @brief  I2C Tx data register flush process.
+ * @param  hi2c: I2C handle
+ * @retval None
+ */
+static void i2c_flush_txdr(struct i2c_handle_s *hi2c)
+{
+	/*
+	 * If a pending TXIS flag is set,
+	 * write a dummy data in TXDR to clear it.
+	 */
+	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_TXIS) !=
+	    0U) {
+		mmio_write_32(hi2c->i2c_base_addr + I2C_TXDR, 0);
+	}
+
+	/* Flush TX register if not empty */
+	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_TXE) ==
+	    0U) {
+		mmio_setbits_32(hi2c->i2c_base_addr + I2C_ISR,
+				I2C_FLAG_TXE);
+	}
+}
+
+/*
+ * @brief  This function handles I2C Communication timeout.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  flag: Specifies the I2C flag to check
+ * @param  awaited_value: The awaited bit value for the flag (0 or 1)
+ * @param  timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_flag(struct i2c_handle_s *hi2c, uint32_t flag,
+			 uint8_t awaited_value, uint64_t timeout_ref)
+{
+	for ( ; ; ) {
+		uint32_t isr = mmio_read_32(hi2c->i2c_base_addr + I2C_ISR);
+
+		if (!!(isr & flag) != !!awaited_value) {
+			return 0;
+		}
+
+		if (timeout_elapsed(timeout_ref)) {
+			notif_i2c_timeout(hi2c);
+			hi2c->lock = 0;
+
+			return -EIO;
+		}
+	}
+}
+
+/*
+ * @brief  This function handles Acknowledge failed detection during
+ *	   an I2C Communication.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_ack_failed(struct i2c_handle_s *hi2c, uint64_t timeout_ref)
+{
+	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_AF) == 0U) {
+		return 0;
+	}
+
+	/*
+	 * Wait until STOP Flag is reset.
+	 * AutoEnd should be initiate after AF.
+	 */
+	while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+		I2C_FLAG_STOPF) == 0U) {
+		if (timeout_elapsed(timeout_ref)) {
+			notif_i2c_timeout(hi2c);
+			hi2c->lock = 0;
+
+			return -EIO;
+		}
+	}
+
+	mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_AF);
+
+	mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
+
+	i2c_flush_txdr(hi2c);
+
+	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_RESET_CR2);
+
+	hi2c->i2c_err |= I2C_ERROR_AF;
+	hi2c->i2c_state = I2C_STATE_READY;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+
+	hi2c->lock = 0;
+
+	return -EIO;
+}
+
+/*
+ * @brief  This function handles I2C Communication timeout for specific usage
+ *	   of TXIS flag.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_txis(struct i2c_handle_s *hi2c, uint64_t timeout_ref)
+{
+	while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+		I2C_FLAG_TXIS) == 0U) {
+		if (i2c_ack_failed(hi2c, timeout_ref) != 0) {
+			return -EIO;
+		}
+
+		if (timeout_elapsed(timeout_ref)) {
+			notif_i2c_timeout(hi2c);
+			hi2c->lock = 0;
+
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  This function handles I2C Communication timeout for specific
+ *	   usage of STOP flag.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_stop(struct i2c_handle_s *hi2c, uint64_t timeout_ref)
+{
+	while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+		 I2C_FLAG_STOPF) == 0U) {
+		if (i2c_ack_failed(hi2c, timeout_ref) != 0) {
+			return -EIO;
+		}
+
+		if (timeout_elapsed(timeout_ref)) {
+			notif_i2c_timeout(hi2c);
+			hi2c->lock = 0;
+
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  Handles I2Cx communication when starting transfer or during transfer
+ *	   (TC or TCR flag are set).
+ * @param  hi2c: I2C handle
+ * @param  dev_addr: Specifies the slave address to be programmed
+ * @param  size: Specifies the number of bytes to be programmed.
+ *   This parameter must be a value between 0 and 255.
+ * @param  i2c_mode: New state of the I2C START condition generation.
+ *   This parameter can be one of the following values:
+ *     @arg @ref I2C_RELOAD_MODE: Enable Reload mode.
+ *     @arg @ref I2C_AUTOEND_MODE: Enable Automatic end mode.
+ *     @arg @ref I2C_SOFTEND_MODE: Enable Software end mode.
+ * @param  request: New state of the I2C START condition generation.
+ *   This parameter can be one of the following values:
+ *     @arg @ref I2C_NO_STARTSTOP: Don't Generate stop and start condition.
+ *     @arg @ref I2C_GENERATE_STOP: Generate stop condition
+ *                                  (size should be set to 0).
+ *     @arg @ref I2C_GENERATE_START_READ: Generate Restart for read request.
+ *     @arg @ref I2C_GENERATE_START_WRITE: Generate Restart for write request.
+ * @retval None
+ */
+static void i2c_transfer_config(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+				uint16_t size, uint32_t i2c_mode,
+				uint32_t request)
+{
+	uint32_t clr_value, set_value;
+
+	clr_value = (I2C_CR2_SADD | I2C_CR2_NBYTES | I2C_CR2_RELOAD |
+		     I2C_CR2_AUTOEND | I2C_CR2_START | I2C_CR2_STOP) |
+		(I2C_CR2_RD_WRN & (request >> (31U - I2C_CR2_RD_WRN_OFFSET)));
+
+	set_value = ((uint32_t)dev_addr & I2C_CR2_SADD) |
+		(((uint32_t)size << I2C_CR2_NBYTES_OFFSET) & I2C_CR2_NBYTES) |
+		i2c_mode | request;
+
+	mmio_clrsetbits_32(hi2c->i2c_base_addr + I2C_CR2, clr_value, set_value);
+}
+
+/*
+ * @brief  Master sends target device address followed by internal memory
+ *	   address for write request.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address
+ * @param  mem_add_size: Size of internal memory address
+ * @param  timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_request_memory_write(struct i2c_handle_s *hi2c,
+				    uint16_t dev_addr, uint16_t mem_addr,
+				    uint16_t mem_add_size, uint64_t timeout_ref)
+{
+	i2c_transfer_config(hi2c, dev_addr, mem_add_size, I2C_RELOAD_MODE,
+			    I2C_GENERATE_START_WRITE);
+
+	if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
+		return -EIO;
+	}
+
+	if (mem_add_size == I2C_MEMADD_SIZE_8BIT) {
+		/* Send Memory Address */
+		mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
+			     (uint8_t)(mem_addr & 0x00FFU));
+	} else {
+		/* Send MSB of Memory Address */
+		mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
+			     (uint8_t)((mem_addr & 0xFF00U) >> 8));
+
+		if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
+			return -EIO;
+		}
+
+		/* Send LSB of Memory Address */
+		mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
+			     (uint8_t)(mem_addr & 0x00FFU));
+	}
+
+	if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0, timeout_ref) != 0) {
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  Master sends target device address followed by internal memory
+ *	   address for read request.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address
+ * @param  mem_add_size: Size of internal memory address
+ * @param  timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_request_memory_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+				   uint16_t mem_addr, uint16_t mem_add_size,
+				   uint64_t timeout_ref)
+{
+	i2c_transfer_config(hi2c, dev_addr, mem_add_size, I2C_SOFTEND_MODE,
+			    I2C_GENERATE_START_WRITE);
+
+	if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
+		return -EIO;
+	}
+
+	if (mem_add_size == I2C_MEMADD_SIZE_8BIT) {
+		/* Send Memory Address */
+		mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
+			     (uint8_t)(mem_addr & 0x00FFU));
+	} else {
+		/* Send MSB of Memory Address */
+		mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
+			     (uint8_t)((mem_addr & 0xFF00U) >> 8));
+
+		if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
+			return -EIO;
+		}
+
+		/* Send LSB of Memory Address */
+		mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
+			     (uint8_t)(mem_addr & 0x00FFU));
+	}
+
+	if (i2c_wait_flag(hi2c, I2C_FLAG_TC, 0, timeout_ref) != 0) {
+		return -EIO;
+	}
+
+	return 0;
+}
+/*
+ * @brief  Generic function to write an amount of data in blocking mode
+ *         (for Memory Mode and Master Mode)
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address (if Memory Mode)
+ * @param  mem_add_size: Size of internal memory address (if Memory Mode)
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @param  mode: Communication mode
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+		     uint16_t mem_addr, uint16_t mem_add_size,
+		     uint8_t *p_data, uint16_t size, uint32_t timeout_ms,
+		     enum i2c_mode_e mode)
+{
+	uint64_t timeout_ref;
+	int rc = -EIO;
+	uint8_t *p_buff = p_data;
+	uint32_t xfer_size;
+	uint32_t xfer_count = size;
+
+	if ((mode != I2C_MODE_MASTER) && (mode != I2C_MODE_MEM)) {
+		return -1;
+	}
+
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return -EBUSY;
+	}
+
+	if ((p_data == NULL) || (size == 0U)) {
+		return -EINVAL;
+	}
+
+	clk_enable(hi2c->clock);
+
+	hi2c->lock = 1;
+
+	timeout_ref = timeout_init_us(I2C_TIMEOUT_BUSY_MS * 1000);
+	if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, timeout_ref) != 0) {
+		goto bail;
+	}
+
+	hi2c->i2c_state = I2C_STATE_BUSY_TX;
+	hi2c->i2c_mode = mode;
+	hi2c->i2c_err = I2C_ERROR_NONE;
+
+	timeout_ref = timeout_init_us(timeout_ms * 1000);
+
+	if (mode == I2C_MODE_MEM) {
+		/* In Memory Mode, Send Slave Address and Memory Address */
+		if (i2c_request_memory_write(hi2c, dev_addr, mem_addr,
+					     mem_add_size, timeout_ref) != 0) {
+			goto bail;
+		}
+
+		if (xfer_count > MAX_NBYTE_SIZE) {
+			xfer_size = MAX_NBYTE_SIZE;
+			i2c_transfer_config(hi2c, dev_addr, xfer_size,
+					    I2C_RELOAD_MODE, I2C_NO_STARTSTOP);
+		} else {
+			xfer_size = xfer_count;
+			i2c_transfer_config(hi2c, dev_addr, xfer_size,
+					    I2C_AUTOEND_MODE, I2C_NO_STARTSTOP);
+		}
+	} else {
+		/* In Master Mode, Send Slave Address */
+		if (xfer_count > MAX_NBYTE_SIZE) {
+			xfer_size = MAX_NBYTE_SIZE;
+			i2c_transfer_config(hi2c, dev_addr, xfer_size,
+					    I2C_RELOAD_MODE,
+					    I2C_GENERATE_START_WRITE);
+		} else {
+			xfer_size = xfer_count;
+			i2c_transfer_config(hi2c, dev_addr, xfer_size,
+					    I2C_AUTOEND_MODE,
+					    I2C_GENERATE_START_WRITE);
+		}
+	}
+
+	do {
+		if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
+			goto bail;
+		}
+
+		mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR, *p_buff);
+		p_buff++;
+		xfer_count--;
+		xfer_size--;
+
+		if ((xfer_count != 0U) && (xfer_size == 0U)) {
+			/* Wait until TCR flag is set */
+			if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0,
+					  timeout_ref) != 0) {
+				goto bail;
+			}
+
+			if (xfer_count > MAX_NBYTE_SIZE) {
+				xfer_size = MAX_NBYTE_SIZE;
+				i2c_transfer_config(hi2c, dev_addr,
+						    xfer_size,
+						    I2C_RELOAD_MODE,
+						    I2C_NO_STARTSTOP);
+			} else {
+				xfer_size = xfer_count;
+				i2c_transfer_config(hi2c, dev_addr,
+						    xfer_size,
+						    I2C_AUTOEND_MODE,
+						    I2C_NO_STARTSTOP);
+			}
+		}
+
+	} while (xfer_count > 0U);
+
+	/*
+	 * No need to Check TC flag, with AUTOEND mode the stop
+	 * is automatically generated.
+	 * Wait until STOPF flag is reset.
+	 */
+	if (i2c_wait_stop(hi2c, timeout_ref) != 0) {
+		goto bail;
+	}
+
+	mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
+
+	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_RESET_CR2);
+
+	hi2c->i2c_state = I2C_STATE_READY;
+	hi2c->i2c_mode  = I2C_MODE_NONE;
+
+	rc = 0;
+
+bail:
+	hi2c->lock = 0;
+	clk_disable(hi2c->clock);
+
+	return rc;
+}
+
+/*
+ * @brief  Write an amount of data in blocking mode to a specific memory
+ *         address.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address
+ * @param  mem_add_size: Size of internal memory address
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_mem_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			uint16_t mem_addr, uint16_t mem_add_size,
+			uint8_t *p_data, uint16_t size, uint32_t timeout_ms)
+{
+	return i2c_write(hi2c, dev_addr, mem_addr, mem_add_size,
+			 p_data, size, timeout_ms, I2C_MODE_MEM);
+}
+
+/*
+ * @brief  Transmits in master mode an amount of data in blocking mode.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_master_transmit(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			      uint8_t *p_data, uint16_t size,
+			      uint32_t timeout_ms)
+{
+	return i2c_write(hi2c, dev_addr, 0, 0,
+			 p_data, size, timeout_ms, I2C_MODE_MASTER);
+}
+
+/*
+ * @brief  Generic function to read an amount of data in blocking mode
+ *         (for Memory Mode and Master Mode)
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address (if Memory Mode)
+ * @param  mem_add_size: Size of internal memory address (if Memory Mode)
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @param  mode: Communication mode
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+		    uint16_t mem_addr, uint16_t mem_add_size,
+		    uint8_t *p_data, uint16_t size, uint32_t timeout_ms,
+		    enum i2c_mode_e mode)
+{
+	uint64_t timeout_ref;
+	int rc = -EIO;
+	uint8_t *p_buff = p_data;
+	uint32_t xfer_count = size;
+	uint32_t xfer_size;
+
+	if ((mode != I2C_MODE_MASTER) && (mode != I2C_MODE_MEM)) {
+		return -1;
+	}
+
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return -EBUSY;
+	}
+
+	if ((p_data == NULL) || (size == 0U)) {
+		return  -EINVAL;
+	}
+
+	clk_enable(hi2c->clock);
+
+	hi2c->lock = 1;
+
+	timeout_ref = timeout_init_us(I2C_TIMEOUT_BUSY_MS * 1000);
+	if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, timeout_ref) != 0) {
+		goto bail;
+	}
+
+	hi2c->i2c_state = I2C_STATE_BUSY_RX;
+	hi2c->i2c_mode = mode;
+	hi2c->i2c_err = I2C_ERROR_NONE;
+
+	if (mode == I2C_MODE_MEM) {
+		/* Send Memory Address */
+		if (i2c_request_memory_read(hi2c, dev_addr, mem_addr,
+					    mem_add_size, timeout_ref) != 0) {
+			goto bail;
+		}
+	}
+
+	/*
+	 * Send Slave Address.
+	 * Set NBYTES to write and reload if xfer_count > MAX_NBYTE_SIZE
+	 * and generate RESTART.
+	 */
+	if (xfer_count > MAX_NBYTE_SIZE) {
+		xfer_size = MAX_NBYTE_SIZE;
+		i2c_transfer_config(hi2c, dev_addr, xfer_size,
+				    I2C_RELOAD_MODE, I2C_GENERATE_START_READ);
+	} else {
+		xfer_size = xfer_count;
+		i2c_transfer_config(hi2c, dev_addr, xfer_size,
+				    I2C_AUTOEND_MODE, I2C_GENERATE_START_READ);
+	}
+
+	do {
+		if (i2c_wait_flag(hi2c, I2C_FLAG_RXNE, 0, timeout_ref) != 0) {
+			goto bail;
+		}
+
+		*p_buff = mmio_read_8(hi2c->i2c_base_addr + I2C_RXDR);
+		p_buff++;
+		xfer_size--;
+		xfer_count--;
+
+		if ((xfer_count != 0U) && (xfer_size == 0U)) {
+			if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0,
+					  timeout_ref) != 0) {
+				goto bail;
+			}
+
+			if (xfer_count > MAX_NBYTE_SIZE) {
+				xfer_size = MAX_NBYTE_SIZE;
+				i2c_transfer_config(hi2c, dev_addr,
+						    xfer_size,
+						    I2C_RELOAD_MODE,
+						    I2C_NO_STARTSTOP);
+			} else {
+				xfer_size = xfer_count;
+				i2c_transfer_config(hi2c, dev_addr,
+						    xfer_size,
+						    I2C_AUTOEND_MODE,
+						    I2C_NO_STARTSTOP);
+			}
+		}
+	} while (xfer_count > 0U);
+
+	/*
+	 * No need to Check TC flag, with AUTOEND mode the stop
+	 * is automatically generated.
+	 * Wait until STOPF flag is reset.
+	 */
+	if (i2c_wait_stop(hi2c, timeout_ref) != 0) {
+		goto bail;
+	}
+
+	mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
+
+	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_RESET_CR2);
+
+	hi2c->i2c_state = I2C_STATE_READY;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+
+	rc = 0;
+
+bail:
+	hi2c->lock = 0;
+	clk_disable(hi2c->clock);
+
+	return rc;
+}
+
+/*
+ * @brief  Read an amount of data in blocking mode from a specific memory
+ *	   address.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address
+ * @param  mem_add_size: Size of internal memory address
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_mem_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+		       uint16_t mem_addr, uint16_t mem_add_size,
+		       uint8_t *p_data, uint16_t size, uint32_t timeout_ms)
+{
+	return i2c_read(hi2c, dev_addr, mem_addr, mem_add_size,
+			p_data, size, timeout_ms, I2C_MODE_MEM);
+}
+
+/*
+ * @brief  Receives in master mode an amount of data in blocking mode.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_master_receive(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			     uint8_t *p_data, uint16_t size,
+			     uint32_t timeout_ms)
+{
+	return i2c_read(hi2c, dev_addr, 0, 0,
+			p_data, size, timeout_ms, I2C_MODE_MASTER);
+}
+
+/*
+ * @brief  Checks if target device is ready for communication.
+ * @note   This function is used with Memory devices
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  trials: Number of trials
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval True if device is ready, false else
+ */
+bool stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c,
+			       uint16_t dev_addr, uint32_t trials,
+			       uint32_t timeout_ms)
+{
+	uint32_t i2c_trials = 0U;
+	bool rc = false;
+
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return rc;
+	}
+
+	clk_enable(hi2c->clock);
+
+	hi2c->lock = 1;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+
+	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_BUSY) !=
+	    0U) {
+		goto bail;
+	}
+
+	hi2c->i2c_state = I2C_STATE_BUSY;
+	hi2c->i2c_err = I2C_ERROR_NONE;
+
+	do {
+		uint64_t timeout_ref;
+
+		/* Generate Start */
+		if ((mmio_read_32(hi2c->i2c_base_addr + I2C_OAR1) &
+		     I2C_OAR1_OA1MODE) == 0) {
+			mmio_write_32(hi2c->i2c_base_addr + I2C_CR2,
+				      (((uint32_t)dev_addr & I2C_CR2_SADD) |
+				       I2C_CR2_START | I2C_CR2_AUTOEND) &
+				      ~I2C_CR2_RD_WRN);
+		} else {
+			mmio_write_32(hi2c->i2c_base_addr + I2C_CR2,
+				      (((uint32_t)dev_addr & I2C_CR2_SADD) |
+				       I2C_CR2_START | I2C_CR2_ADD10) &
+				      ~I2C_CR2_RD_WRN);
+		}
+
+		/*
+		 * No need to Check TC flag, with AUTOEND mode the stop
+		 * is automatically generated.
+		 * Wait until STOPF flag is set or a NACK flag is set.
+		 */
+		timeout_ref = timeout_init_us(timeout_ms * 1000);
+		do {
+			if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+			     (I2C_FLAG_STOPF | I2C_FLAG_AF)) != 0U) {
+				break;
+			}
+
+			if (timeout_elapsed(timeout_ref)) {
+				notif_i2c_timeout(hi2c);
+				goto bail;
+			}
+		} while (true);
+
+		if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+		     I2C_FLAG_AF) == 0U) {
+			if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0,
+					  timeout_ref) != 0) {
+				goto bail;
+			}
+
+			mmio_write_32(hi2c->i2c_base_addr + I2C_ICR,
+				      I2C_FLAG_STOPF);
+
+			hi2c->i2c_state = I2C_STATE_READY;
+
+			rc = true;
+			goto bail;
+		}
+
+		if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0, timeout_ref) != 0) {
+			goto bail;
+		}
+
+		mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_AF);
+
+		mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
+
+		if (i2c_trials == trials) {
+			mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR2,
+					I2C_CR2_STOP);
+
+			if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0,
+					  timeout_ref) != 0) {
+				goto bail;
+			}
+
+			mmio_write_32(hi2c->i2c_base_addr + I2C_ICR,
+				      I2C_FLAG_STOPF);
+		}
+
+		i2c_trials++;
+	} while (i2c_trials < trials);
+
+	notif_i2c_timeout(hi2c);
+
+bail:
+	hi2c->lock = 0;
+	clk_disable(hi2c->clock);
+
+	return rc;
+}
+
diff --git a/drivers/st/io/io_mmc.c b/drivers/st/io/io_mmc.c
new file mode 100644
index 0000000..2bf88e6
--- /dev/null
+++ b/drivers/st/io/io_mmc.c
@@ -0,0 +1,143 @@
+/*
+ * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_storage.h>
+#include <drivers/mmc.h>
+#include <drivers/st/io_mmc.h>
+#include <drivers/st/stm32_sdmmc2.h>
+
+/* SDMMC device functions */
+static int mmc_dev_open(const uintptr_t init_params, io_dev_info_t **dev_info);
+static int mmc_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			  io_entity_t *entity);
+static int mmc_dev_init(io_dev_info_t *dev_info, const uintptr_t init_params);
+static int mmc_block_seek(io_entity_t *entity, int mode,
+			  signed long long offset);
+static int mmc_block_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+			  size_t *length_read);
+static int mmc_block_close(io_entity_t *entity);
+static int mmc_dev_close(io_dev_info_t *dev_info);
+static io_type_t device_type_mmc(void);
+
+static signed long long seek_offset;
+static size_t (*_read_blocks)(int lba, uintptr_t buf, size_t size);
+
+static const io_dev_connector_t mmc_dev_connector = {
+	.dev_open = mmc_dev_open
+};
+
+static const io_dev_funcs_t mmc_dev_funcs = {
+	.type = device_type_mmc,
+	.open = mmc_block_open,
+	.seek = mmc_block_seek,
+	.size = NULL,
+	.read = mmc_block_read,
+	.write = NULL,
+	.close = mmc_block_close,
+	.dev_init = mmc_dev_init,
+	.dev_close = mmc_dev_close,
+};
+
+static const io_dev_info_t mmc_dev_info = {
+	.funcs = &mmc_dev_funcs,
+	.info = 0,
+};
+
+/* Identify the device type as mmc device */
+static io_type_t device_type_mmc(void)
+{
+	return IO_TYPE_MMC;
+}
+
+/* Open a connection to the mmc device */
+static int mmc_dev_open(const uintptr_t init_params, io_dev_info_t **dev_info)
+{
+	struct io_mmc_dev_spec *device_spec =
+		(struct io_mmc_dev_spec *)init_params;
+
+	assert(dev_info != NULL);
+	*dev_info = (io_dev_info_t *)&mmc_dev_info;
+
+	_read_blocks = !device_spec->use_boot_part ?
+		mmc_read_blocks : mmc_boot_part_read_blocks;
+
+	return 0;
+}
+
+static int mmc_dev_init(io_dev_info_t *dev_info, const uintptr_t init_params)
+{
+	return 0;
+}
+
+/* Close a connection to the mmc device */
+static int mmc_dev_close(io_dev_info_t *dev_info)
+{
+	return 0;
+}
+
+/* Open a file on the mmc device */
+static int mmc_block_open(io_dev_info_t *dev_info, const  uintptr_t spec,
+			  io_entity_t *entity)
+{
+	seek_offset = 0;
+	return 0;
+}
+
+/* Seek to a particular file offset on the mmc device */
+static int mmc_block_seek(io_entity_t *entity, int mode,
+			  signed long long offset)
+{
+	seek_offset = offset;
+	return 0;
+}
+
+/* Read data from a file on the mmc device */
+static int mmc_block_read(io_entity_t *entity, uintptr_t buffer,
+			  size_t length, size_t *length_read)
+{
+	uint8_t retries;
+
+	for (retries = 0U; retries < 3U; retries++) {
+		*length_read = _read_blocks(seek_offset / MMC_BLOCK_SIZE,
+					    buffer, length);
+
+		if (*length_read == length) {
+			return 0;
+		}
+		WARN("%s: length_read = %lu (!= %lu), retry %u\n", __func__,
+		     (unsigned long)*length_read, (unsigned long)length,
+		     retries + 1U);
+	}
+
+	return -EIO;
+}
+
+/* Close a file on the mmc device */
+static int mmc_block_close(io_entity_t *entity)
+{
+	return 0;
+}
+
+/* Register the mmc driver with the IO abstraction */
+int register_io_dev_mmc(const io_dev_connector_t **dev_con)
+{
+	int result;
+
+	assert(dev_con != NULL);
+
+	result = io_register_device(&mmc_dev_info);
+	if (result == 0) {
+		*dev_con = &mmc_dev_connector;
+	}
+
+	return result;
+}
diff --git a/drivers/st/iwdg/stm32_iwdg.c b/drivers/st/iwdg/stm32_iwdg.c
new file mode 100644
index 0000000..74451d7
--- /dev/null
+++ b/drivers/st/iwdg/stm32_iwdg.c
@@ -0,0 +1,157 @@
+/*
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/gicv2.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_iwdg.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+
+/* IWDG registers offsets */
+#define IWDG_KR_OFFSET		0x00U
+
+/* Registers values */
+#define IWDG_KR_RELOAD_KEY	0xAAAA
+
+struct stm32_iwdg_instance {
+	uintptr_t base;
+	unsigned long clock;
+	uint8_t flags;
+	int num_irq;
+};
+
+static struct stm32_iwdg_instance stm32_iwdg[IWDG_MAX_INSTANCE];
+
+static int stm32_iwdg_get_dt_node(struct dt_node_info *info, int offset)
+{
+	int node;
+
+	node = dt_get_node(info, offset, DT_IWDG_COMPAT);
+	if (node < 0) {
+		if (offset == -1) {
+			VERBOSE("%s: No IDWG found\n", __func__);
+		}
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return node;
+}
+
+void stm32_iwdg_refresh(void)
+{
+	uint8_t i;
+
+	for (i = 0U; i < IWDG_MAX_INSTANCE; i++) {
+		struct stm32_iwdg_instance *iwdg = &stm32_iwdg[i];
+
+		/* 0x00000000 is not a valid address for IWDG peripherals */
+		if (iwdg->base != 0U) {
+			clk_enable(iwdg->clock);
+
+			mmio_write_32(iwdg->base + IWDG_KR_OFFSET,
+				      IWDG_KR_RELOAD_KEY);
+
+			clk_disable(iwdg->clock);
+		}
+	}
+}
+
+int stm32_iwdg_init(void)
+{
+	int node = -1;
+	struct dt_node_info dt_info;
+	void *fdt;
+	uint32_t __unused count = 0;
+
+	if (fdt_get_address(&fdt) == 0) {
+		panic();
+	}
+
+	for (node = stm32_iwdg_get_dt_node(&dt_info, node);
+	     node != -FDT_ERR_NOTFOUND;
+	     node = stm32_iwdg_get_dt_node(&dt_info, node)) {
+		struct stm32_iwdg_instance *iwdg;
+		uint32_t hw_init;
+		uint32_t idx;
+
+		count++;
+
+		idx = stm32_iwdg_get_instance(dt_info.base);
+		iwdg = &stm32_iwdg[idx];
+		iwdg->base = dt_info.base;
+		iwdg->clock = (unsigned long)dt_info.clock;
+
+		/* DT can specify low power cases */
+		if (fdt_getprop(fdt, node, "stm32,enable-on-stop", NULL) ==
+		    NULL) {
+			iwdg->flags |= IWDG_DISABLE_ON_STOP;
+		}
+
+		if (fdt_getprop(fdt, node, "stm32,enable-on-standby", NULL) ==
+		    NULL) {
+			iwdg->flags |= IWDG_DISABLE_ON_STANDBY;
+		}
+
+		/* Explicit list of supported bit flags */
+		hw_init = stm32_iwdg_get_otp_config(idx);
+
+		if ((hw_init & IWDG_HW_ENABLED) != 0) {
+			if (dt_info.status == DT_DISABLED) {
+				ERROR("OTP enabled but iwdg%u DT-disabled\n",
+				      idx + 1U);
+				panic();
+			}
+			iwdg->flags |= IWDG_HW_ENABLED;
+		}
+
+		if (dt_info.status == DT_DISABLED) {
+			zeromem((void *)iwdg,
+				sizeof(struct stm32_iwdg_instance));
+			continue;
+		}
+
+		if ((hw_init & IWDG_DISABLE_ON_STOP) != 0) {
+			iwdg->flags |= IWDG_DISABLE_ON_STOP;
+		}
+
+		if ((hw_init & IWDG_DISABLE_ON_STANDBY) != 0) {
+			iwdg->flags |= IWDG_DISABLE_ON_STANDBY;
+		}
+
+		VERBOSE("IWDG%u found, %ssecure\n", idx + 1U,
+			((dt_info.status & DT_NON_SECURE) != 0) ?
+			"non-" : "");
+
+		if ((dt_info.status & DT_NON_SECURE) != 0) {
+			stm32mp_register_non_secure_periph_iomem(iwdg->base);
+		} else {
+			stm32mp_register_secure_periph_iomem(iwdg->base);
+		}
+
+#if defined(IMAGE_BL2)
+		if (stm32_iwdg_shadow_update(idx, iwdg->flags) != BSEC_OK) {
+			return -1;
+		}
+#endif
+	}
+
+	VERBOSE("%u IWDG instance%s found\n", count, (count > 1U) ? "s" : "");
+
+	return 0;
+}
diff --git a/drivers/st/mmc/stm32_sdmmc2.c b/drivers/st/mmc/stm32_sdmmc2.c
new file mode 100644
index 0000000..6bdd782
--- /dev/null
+++ b/drivers/st/mmc/stm32_sdmmc2.c
@@ -0,0 +1,798 @@
+/*
+ * Copyright (c) 2018-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/mmc.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32_sdmmc2.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+/* Registers offsets */
+#define SDMMC_POWER			0x00U
+#define SDMMC_CLKCR			0x04U
+#define SDMMC_ARGR			0x08U
+#define SDMMC_CMDR			0x0CU
+#define SDMMC_RESPCMDR			0x10U
+#define SDMMC_RESP1R			0x14U
+#define SDMMC_RESP2R			0x18U
+#define SDMMC_RESP3R			0x1CU
+#define SDMMC_RESP4R			0x20U
+#define SDMMC_DTIMER			0x24U
+#define SDMMC_DLENR			0x28U
+#define SDMMC_DCTRLR			0x2CU
+#define SDMMC_DCNTR			0x30U
+#define SDMMC_STAR			0x34U
+#define SDMMC_ICR			0x38U
+#define SDMMC_MASKR			0x3CU
+#define SDMMC_ACKTIMER			0x40U
+#define SDMMC_IDMACTRLR			0x50U
+#define SDMMC_IDMABSIZER		0x54U
+#define SDMMC_IDMABASE0R		0x58U
+#define SDMMC_IDMABASE1R		0x5CU
+#define SDMMC_FIFOR			0x80U
+
+/* SDMMC power control register */
+#define SDMMC_POWER_PWRCTRL		GENMASK(1, 0)
+#define SDMMC_POWER_PWRCTRL_PWR_CYCLE	BIT(1)
+#define SDMMC_POWER_DIRPOL		BIT(4)
+
+/* SDMMC clock control register */
+#define SDMMC_CLKCR_WIDBUS_4		BIT(14)
+#define SDMMC_CLKCR_WIDBUS_8		BIT(15)
+#define SDMMC_CLKCR_NEGEDGE		BIT(16)
+#define SDMMC_CLKCR_HWFC_EN		BIT(17)
+#define SDMMC_CLKCR_SELCLKRX_0		BIT(20)
+
+/* SDMMC command register */
+#define SDMMC_CMDR_CMDTRANS		BIT(6)
+#define SDMMC_CMDR_CMDSTOP		BIT(7)
+#define SDMMC_CMDR_WAITRESP		GENMASK(9, 8)
+#define SDMMC_CMDR_WAITRESP_SHORT	BIT(8)
+#define SDMMC_CMDR_WAITRESP_SHORT_NOCRC	BIT(9)
+#define SDMMC_CMDR_CPSMEN		BIT(12)
+
+/* SDMMC data control register */
+#define SDMMC_DCTRLR_DTEN		BIT(0)
+#define SDMMC_DCTRLR_DTDIR		BIT(1)
+#define SDMMC_DCTRLR_DTMODE		GENMASK(3, 2)
+#define SDMMC_DCTRLR_DBLOCKSIZE		GENMASK(7, 4)
+#define SDMMC_DCTRLR_DBLOCKSIZE_SHIFT	4
+#define SDMMC_DCTRLR_FIFORST		BIT(13)
+
+#define SDMMC_DCTRLR_CLEAR_MASK		(SDMMC_DCTRLR_DTEN | \
+					 SDMMC_DCTRLR_DTDIR | \
+					 SDMMC_DCTRLR_DTMODE | \
+					 SDMMC_DCTRLR_DBLOCKSIZE)
+
+/* SDMMC status register */
+#define SDMMC_STAR_CCRCFAIL		BIT(0)
+#define SDMMC_STAR_DCRCFAIL		BIT(1)
+#define SDMMC_STAR_CTIMEOUT		BIT(2)
+#define SDMMC_STAR_DTIMEOUT		BIT(3)
+#define SDMMC_STAR_TXUNDERR		BIT(4)
+#define SDMMC_STAR_RXOVERR		BIT(5)
+#define SDMMC_STAR_CMDREND		BIT(6)
+#define SDMMC_STAR_CMDSENT		BIT(7)
+#define SDMMC_STAR_DATAEND		BIT(8)
+#define SDMMC_STAR_DBCKEND		BIT(10)
+#define SDMMC_STAR_DPSMACT		BIT(12)
+#define SDMMC_STAR_RXFIFOHF		BIT(15)
+#define SDMMC_STAR_RXFIFOE		BIT(19)
+#define SDMMC_STAR_IDMATE		BIT(27)
+#define SDMMC_STAR_IDMABTC		BIT(28)
+
+/* SDMMC DMA control register */
+#define SDMMC_IDMACTRLR_IDMAEN		BIT(0)
+
+#define SDMMC_STATIC_FLAGS		(SDMMC_STAR_CCRCFAIL | \
+					 SDMMC_STAR_DCRCFAIL | \
+					 SDMMC_STAR_CTIMEOUT | \
+					 SDMMC_STAR_DTIMEOUT | \
+					 SDMMC_STAR_TXUNDERR | \
+					 SDMMC_STAR_RXOVERR  | \
+					 SDMMC_STAR_CMDREND  | \
+					 SDMMC_STAR_CMDSENT  | \
+					 SDMMC_STAR_DATAEND  | \
+					 SDMMC_STAR_DBCKEND  | \
+					 SDMMC_STAR_IDMATE   | \
+					 SDMMC_STAR_IDMABTC)
+
+#define TIMEOUT_US_1_MS			1000U
+#define TIMEOUT_US_10_MS		10000U
+#define TIMEOUT_US_1_S			1000000U
+
+/* Power cycle delays in ms */
+#define VCC_POWER_OFF_DELAY		2
+#define VCC_POWER_ON_DELAY		2
+#define POWER_CYCLE_DELAY		2
+#define POWER_OFF_DELAY			2
+#define POWER_ON_DELAY			1
+
+#ifndef DT_SDMMC2_COMPAT
+#define DT_SDMMC2_COMPAT		"st,stm32-sdmmc2"
+#endif
+
+#define SDMMC_FIFO_SIZE			64U
+
+static void stm32_sdmmc2_init(void);
+static int stm32_sdmmc2_send_cmd_req(struct mmc_cmd *cmd);
+static int stm32_sdmmc2_send_cmd(struct mmc_cmd *cmd);
+static int stm32_sdmmc2_set_ios(unsigned int clk, unsigned int width);
+static int stm32_sdmmc2_prepare(int lba, uintptr_t buf, size_t size);
+static int stm32_sdmmc2_read(int lba, uintptr_t buf, size_t size);
+static int stm32_sdmmc2_write(int lba, uintptr_t buf, size_t size);
+
+static const struct mmc_ops stm32_sdmmc2_ops = {
+	.init		= stm32_sdmmc2_init,
+	.send_cmd	= stm32_sdmmc2_send_cmd,
+	.set_ios	= stm32_sdmmc2_set_ios,
+	.prepare	= stm32_sdmmc2_prepare,
+	.read		= stm32_sdmmc2_read,
+	.write		= stm32_sdmmc2_write,
+};
+
+static struct stm32_sdmmc2_params sdmmc2_params;
+
+static bool next_cmd_is_acmd;
+
+#pragma weak plat_sdmmc2_use_dma
+bool plat_sdmmc2_use_dma(unsigned int instance, unsigned int memory)
+{
+	return false;
+}
+
+static void stm32_sdmmc2_init(void)
+{
+	uint32_t clock_div;
+	uint32_t freq = STM32MP_MMC_INIT_FREQ;
+	uintptr_t base = sdmmc2_params.reg_base;
+	int ret;
+
+	if (sdmmc2_params.max_freq != 0U) {
+		freq = MIN(sdmmc2_params.max_freq, freq);
+	}
+
+	if (sdmmc2_params.vmmc_regu != NULL) {
+		ret = regulator_disable(sdmmc2_params.vmmc_regu);
+		if (ret < 0) {
+			panic();
+		}
+	}
+
+	mdelay(VCC_POWER_OFF_DELAY);
+
+	mmio_write_32(base + SDMMC_POWER,
+		      SDMMC_POWER_PWRCTRL_PWR_CYCLE | sdmmc2_params.dirpol);
+	mdelay(POWER_CYCLE_DELAY);
+
+	if (sdmmc2_params.vmmc_regu != NULL) {
+		ret = regulator_enable(sdmmc2_params.vmmc_regu);
+		if (ret < 0) {
+			panic();
+		}
+	}
+
+	mdelay(VCC_POWER_ON_DELAY);
+
+	mmio_write_32(base + SDMMC_POWER, sdmmc2_params.dirpol);
+	mdelay(POWER_OFF_DELAY);
+
+	clock_div = div_round_up(sdmmc2_params.clk_rate, freq * 2U);
+
+	mmio_write_32(base + SDMMC_CLKCR, SDMMC_CLKCR_HWFC_EN | clock_div |
+		      sdmmc2_params.negedge |
+		      sdmmc2_params.pin_ckin);
+
+	mmio_write_32(base + SDMMC_POWER,
+		      SDMMC_POWER_PWRCTRL | sdmmc2_params.dirpol);
+
+	mdelay(POWER_ON_DELAY);
+}
+
+static int stm32_sdmmc2_stop_transfer(void)
+{
+	struct mmc_cmd cmd_stop;
+
+	zeromem(&cmd_stop, sizeof(struct mmc_cmd));
+
+	cmd_stop.cmd_idx = MMC_CMD(12);
+	cmd_stop.resp_type = MMC_RESPONSE_R1B;
+
+	return stm32_sdmmc2_send_cmd(&cmd_stop);
+}
+
+static int stm32_sdmmc2_send_cmd_req(struct mmc_cmd *cmd)
+{
+	uint64_t timeout;
+	uint32_t flags_cmd, status;
+	uint32_t flags_data = 0;
+	int err = 0;
+	uintptr_t base = sdmmc2_params.reg_base;
+	unsigned int cmd_reg, arg_reg;
+
+	if (cmd == NULL) {
+		return -EINVAL;
+	}
+
+	flags_cmd = SDMMC_STAR_CTIMEOUT;
+	arg_reg = cmd->cmd_arg;
+
+	if ((mmio_read_32(base + SDMMC_CMDR) & SDMMC_CMDR_CPSMEN) != 0U) {
+		mmio_write_32(base + SDMMC_CMDR, 0);
+	}
+
+	cmd_reg = cmd->cmd_idx | SDMMC_CMDR_CPSMEN;
+
+	if (cmd->resp_type == 0U) {
+		flags_cmd |= SDMMC_STAR_CMDSENT;
+	}
+
+	if ((cmd->resp_type & MMC_RSP_48) != 0U) {
+		if ((cmd->resp_type & MMC_RSP_136) != 0U) {
+			flags_cmd |= SDMMC_STAR_CMDREND;
+			cmd_reg |= SDMMC_CMDR_WAITRESP;
+		} else if ((cmd->resp_type & MMC_RSP_CRC) != 0U) {
+			flags_cmd |= SDMMC_STAR_CMDREND | SDMMC_STAR_CCRCFAIL;
+			cmd_reg |= SDMMC_CMDR_WAITRESP_SHORT;
+		} else {
+			flags_cmd |= SDMMC_STAR_CMDREND;
+			cmd_reg |= SDMMC_CMDR_WAITRESP_SHORT_NOCRC;
+		}
+	}
+
+	switch (cmd->cmd_idx) {
+	case MMC_CMD(1):
+		arg_reg |= OCR_POWERUP;
+		break;
+	case MMC_CMD(6):
+		if ((sdmmc2_params.device_info->mmc_dev_type == MMC_IS_SD_HC) &&
+		    (!next_cmd_is_acmd)) {
+			cmd_reg |= SDMMC_CMDR_CMDTRANS;
+			if (sdmmc2_params.use_dma) {
+				flags_data |= SDMMC_STAR_DCRCFAIL |
+					SDMMC_STAR_DTIMEOUT |
+					SDMMC_STAR_DATAEND |
+					SDMMC_STAR_RXOVERR |
+					SDMMC_STAR_IDMATE |
+					SDMMC_STAR_DBCKEND;
+			}
+		}
+		break;
+	case MMC_CMD(8):
+		if (sdmmc2_params.device_info->mmc_dev_type == MMC_IS_EMMC) {
+			cmd_reg |= SDMMC_CMDR_CMDTRANS;
+		}
+		break;
+	case MMC_CMD(12):
+		cmd_reg |= SDMMC_CMDR_CMDSTOP;
+		break;
+	case MMC_CMD(17):
+	case MMC_CMD(18):
+		cmd_reg |= SDMMC_CMDR_CMDTRANS;
+		if (sdmmc2_params.use_dma) {
+			flags_data |= SDMMC_STAR_DCRCFAIL |
+				      SDMMC_STAR_DTIMEOUT |
+				      SDMMC_STAR_DATAEND |
+				      SDMMC_STAR_RXOVERR |
+				      SDMMC_STAR_IDMATE;
+		}
+		break;
+	case MMC_ACMD(41):
+		arg_reg |= OCR_3_2_3_3 | OCR_3_3_3_4;
+		break;
+	case MMC_ACMD(51):
+		cmd_reg |= SDMMC_CMDR_CMDTRANS;
+		if (sdmmc2_params.use_dma) {
+			flags_data |= SDMMC_STAR_DCRCFAIL |
+				      SDMMC_STAR_DTIMEOUT |
+				      SDMMC_STAR_DATAEND |
+				      SDMMC_STAR_RXOVERR |
+				      SDMMC_STAR_IDMATE |
+				      SDMMC_STAR_DBCKEND;
+		}
+		break;
+	default:
+		break;
+	}
+
+	next_cmd_is_acmd = (cmd->cmd_idx == MMC_CMD(55));
+
+	mmio_write_32(base + SDMMC_ICR, SDMMC_STATIC_FLAGS);
+
+	/*
+	 * Clear the SDMMC_DCTRLR if the command does not await data.
+	 * Skip CMD55 as the next command could be data related, and
+	 * the register could have been set in prepare function.
+	 */
+	if (((cmd_reg & SDMMC_CMDR_CMDTRANS) == 0U) && !next_cmd_is_acmd) {
+		mmio_write_32(base + SDMMC_DCTRLR, 0U);
+	}
+
+	if ((cmd->resp_type & MMC_RSP_BUSY) != 0U) {
+		mmio_write_32(base + SDMMC_DTIMER, UINT32_MAX);
+	}
+
+	mmio_write_32(base + SDMMC_ARGR, arg_reg);
+
+	mmio_write_32(base + SDMMC_CMDR, cmd_reg);
+
+	status = mmio_read_32(base + SDMMC_STAR);
+
+	timeout = timeout_init_us(TIMEOUT_US_10_MS);
+
+	while ((status & flags_cmd) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			err = -ETIMEDOUT;
+			ERROR("%s: timeout 10ms (cmd = %u,status = %x)\n",
+			      __func__, cmd->cmd_idx, status);
+			goto err_exit;
+		}
+
+		status = mmio_read_32(base + SDMMC_STAR);
+	}
+
+	if ((status & (SDMMC_STAR_CTIMEOUT | SDMMC_STAR_CCRCFAIL)) != 0U) {
+		if ((status & SDMMC_STAR_CTIMEOUT) != 0U) {
+			err = -ETIMEDOUT;
+			/*
+			 * Those timeouts can occur, and framework will handle
+			 * the retries. CMD8 is expected to return this timeout
+			 * for eMMC
+			 */
+			if (!((cmd->cmd_idx == MMC_CMD(1)) ||
+			      (cmd->cmd_idx == MMC_CMD(13)) ||
+			      ((cmd->cmd_idx == MMC_CMD(8)) &&
+			       (cmd->resp_type == MMC_RESPONSE_R7)))) {
+				ERROR("%s: CTIMEOUT (cmd = %u,status = %x)\n",
+				      __func__, cmd->cmd_idx, status);
+			}
+		} else {
+			err = -EIO;
+			ERROR("%s: CRCFAIL (cmd = %u,status = %x)\n",
+			      __func__, cmd->cmd_idx, status);
+		}
+
+		goto err_exit;
+	}
+
+	if ((cmd_reg & SDMMC_CMDR_WAITRESP) != 0U) {
+		if ((cmd->cmd_idx == MMC_CMD(9)) &&
+		    ((cmd_reg & SDMMC_CMDR_WAITRESP) == SDMMC_CMDR_WAITRESP)) {
+			/* Need to invert response to match CSD structure */
+			cmd->resp_data[0] = mmio_read_32(base + SDMMC_RESP4R);
+			cmd->resp_data[1] = mmio_read_32(base + SDMMC_RESP3R);
+			cmd->resp_data[2] = mmio_read_32(base + SDMMC_RESP2R);
+			cmd->resp_data[3] = mmio_read_32(base + SDMMC_RESP1R);
+		} else {
+			cmd->resp_data[0] = mmio_read_32(base + SDMMC_RESP1R);
+			if ((cmd_reg & SDMMC_CMDR_WAITRESP) ==
+			    SDMMC_CMDR_WAITRESP) {
+				cmd->resp_data[1] = mmio_read_32(base +
+								 SDMMC_RESP2R);
+				cmd->resp_data[2] = mmio_read_32(base +
+								 SDMMC_RESP3R);
+				cmd->resp_data[3] = mmio_read_32(base +
+								 SDMMC_RESP4R);
+			}
+		}
+	}
+
+	if (flags_data == 0U) {
+		mmio_write_32(base + SDMMC_ICR, SDMMC_STATIC_FLAGS);
+
+		return 0;
+	}
+
+	status = mmio_read_32(base + SDMMC_STAR);
+
+	timeout = timeout_init_us(TIMEOUT_US_10_MS);
+
+	while ((status & flags_data) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("%s: timeout 10ms (cmd = %u,status = %x)\n",
+			      __func__, cmd->cmd_idx, status);
+			err = -ETIMEDOUT;
+			goto err_exit;
+		}
+
+		status = mmio_read_32(base + SDMMC_STAR);
+	};
+
+	if ((status & (SDMMC_STAR_DTIMEOUT | SDMMC_STAR_DCRCFAIL |
+		       SDMMC_STAR_TXUNDERR | SDMMC_STAR_RXOVERR |
+		       SDMMC_STAR_IDMATE)) != 0U) {
+		ERROR("%s: Error flag (cmd = %u,status = %x)\n", __func__,
+		      cmd->cmd_idx, status);
+		err = -EIO;
+	}
+
+err_exit:
+	mmio_write_32(base + SDMMC_ICR, SDMMC_STATIC_FLAGS);
+	mmio_clrbits_32(base + SDMMC_CMDR, SDMMC_CMDR_CMDTRANS);
+
+	if ((err != 0) && ((status & SDMMC_STAR_DPSMACT) != 0U)) {
+		int ret_stop = stm32_sdmmc2_stop_transfer();
+
+		if (ret_stop != 0) {
+			return ret_stop;
+		}
+	}
+
+	return err;
+}
+
+static int stm32_sdmmc2_send_cmd(struct mmc_cmd *cmd)
+{
+	uint8_t retry;
+	int err;
+
+	assert(cmd != NULL);
+
+	for (retry = 0U; retry < 3U; retry++) {
+		err = stm32_sdmmc2_send_cmd_req(cmd);
+		if (err == 0) {
+			return 0;
+		}
+
+		if ((cmd->cmd_idx == MMC_CMD(1)) ||
+		    (cmd->cmd_idx == MMC_CMD(13))) {
+			return 0; /* Retry managed by framework */
+		}
+
+		/* Command 8 is expected to fail for eMMC */
+		if (cmd->cmd_idx != MMC_CMD(8)) {
+			WARN(" CMD%u, Retry: %u, Error: %d\n",
+			     cmd->cmd_idx, retry + 1U, err);
+		}
+
+		udelay(10U);
+	}
+
+	return err;
+}
+
+static int stm32_sdmmc2_set_ios(unsigned int clk, unsigned int width)
+{
+	uintptr_t base = sdmmc2_params.reg_base;
+	uint32_t bus_cfg = 0;
+	uint32_t clock_div, max_freq, freq;
+	uint32_t clk_rate = sdmmc2_params.clk_rate;
+	uint32_t max_bus_freq = sdmmc2_params.device_info->max_bus_freq;
+
+	switch (width) {
+	case MMC_BUS_WIDTH_1:
+		break;
+	case MMC_BUS_WIDTH_4:
+		bus_cfg |= SDMMC_CLKCR_WIDBUS_4;
+		break;
+	case MMC_BUS_WIDTH_8:
+		bus_cfg |= SDMMC_CLKCR_WIDBUS_8;
+		break;
+	default:
+		panic();
+		break;
+	}
+
+	if (sdmmc2_params.device_info->mmc_dev_type == MMC_IS_EMMC) {
+		if (max_bus_freq >= 52000000U) {
+			max_freq = STM32MP_EMMC_HIGH_SPEED_MAX_FREQ;
+		} else {
+			max_freq = STM32MP_EMMC_NORMAL_SPEED_MAX_FREQ;
+		}
+	} else {
+		if (max_bus_freq >= 50000000U) {
+			max_freq = STM32MP_SD_HIGH_SPEED_MAX_FREQ;
+		} else {
+			max_freq = STM32MP_SD_NORMAL_SPEED_MAX_FREQ;
+		}
+	}
+
+	if (sdmmc2_params.max_freq != 0U) {
+		freq = MIN(sdmmc2_params.max_freq, max_freq);
+	} else {
+		freq = max_freq;
+	}
+
+	clock_div = div_round_up(clk_rate, freq * 2U);
+
+	mmio_write_32(base + SDMMC_CLKCR,
+		      SDMMC_CLKCR_HWFC_EN | clock_div | bus_cfg |
+		      sdmmc2_params.negedge |
+		      sdmmc2_params.pin_ckin);
+
+	return 0;
+}
+
+static int stm32_sdmmc2_prepare(int lba, uintptr_t buf, size_t size)
+{
+	struct mmc_cmd cmd;
+	int ret;
+	uintptr_t base = sdmmc2_params.reg_base;
+	uint32_t data_ctrl = SDMMC_DCTRLR_DTDIR;
+	uint32_t arg_size;
+
+	assert(size != 0U);
+
+	if (size > MMC_BLOCK_SIZE) {
+		arg_size = MMC_BLOCK_SIZE;
+	} else {
+		arg_size = size;
+	}
+
+	sdmmc2_params.use_dma = plat_sdmmc2_use_dma(base, buf);
+
+	if (sdmmc2_params.use_dma) {
+		inv_dcache_range(buf, size);
+	}
+
+	/* Prepare CMD 16*/
+	mmio_write_32(base + SDMMC_DTIMER, 0);
+
+	mmio_write_32(base + SDMMC_DLENR, 0);
+
+	mmio_write_32(base + SDMMC_DCTRLR, 0);
+
+	zeromem(&cmd, sizeof(struct mmc_cmd));
+
+	cmd.cmd_idx = MMC_CMD(16);
+	cmd.cmd_arg = arg_size;
+	cmd.resp_type = MMC_RESPONSE_R1;
+
+	ret = stm32_sdmmc2_send_cmd(&cmd);
+	if (ret != 0) {
+		ERROR("CMD16 failed\n");
+		return ret;
+	}
+
+	/* Prepare data command */
+	mmio_write_32(base + SDMMC_DTIMER, UINT32_MAX);
+
+	mmio_write_32(base + SDMMC_DLENR, size);
+
+	if (sdmmc2_params.use_dma) {
+		mmio_write_32(base + SDMMC_IDMACTRLR,
+			      SDMMC_IDMACTRLR_IDMAEN);
+		mmio_write_32(base + SDMMC_IDMABASE0R, buf);
+
+		flush_dcache_range(buf, size);
+	}
+
+	data_ctrl |= __builtin_ctz(arg_size) << SDMMC_DCTRLR_DBLOCKSIZE_SHIFT;
+
+	mmio_clrsetbits_32(base + SDMMC_DCTRLR,
+			   SDMMC_DCTRLR_CLEAR_MASK,
+			   data_ctrl);
+
+	return 0;
+}
+
+static int stm32_sdmmc2_read(int lba, uintptr_t buf, size_t size)
+{
+	uint32_t error_flags = SDMMC_STAR_RXOVERR | SDMMC_STAR_DCRCFAIL |
+			       SDMMC_STAR_DTIMEOUT;
+	uint32_t flags = error_flags | SDMMC_STAR_DATAEND;
+	uint32_t status;
+	uint32_t *buffer;
+	uintptr_t base = sdmmc2_params.reg_base;
+	uintptr_t fifo_reg = base + SDMMC_FIFOR;
+	uint64_t timeout;
+	int ret;
+
+	/* Assert buf is 4 bytes aligned */
+	assert((buf & GENMASK(1, 0)) == 0U);
+
+	buffer = (uint32_t *)buf;
+
+	if (sdmmc2_params.use_dma) {
+		inv_dcache_range(buf, size);
+
+		return 0;
+	}
+
+	if (size <= MMC_BLOCK_SIZE) {
+		flags |= SDMMC_STAR_DBCKEND;
+	}
+
+	timeout = timeout_init_us(TIMEOUT_US_1_S);
+
+	do {
+		status = mmio_read_32(base + SDMMC_STAR);
+
+		if ((status & error_flags) != 0U) {
+			ERROR("%s: Read error (status = %x)\n", __func__,
+			      status);
+			mmio_write_32(base + SDMMC_DCTRLR,
+				      SDMMC_DCTRLR_FIFORST);
+
+			mmio_write_32(base + SDMMC_ICR,
+				      SDMMC_STATIC_FLAGS);
+
+			ret = stm32_sdmmc2_stop_transfer();
+			if (ret != 0) {
+				return ret;
+			}
+
+			return -EIO;
+		}
+
+		if (timeout_elapsed(timeout)) {
+			ERROR("%s: timeout 1s (status = %x)\n",
+			      __func__, status);
+			mmio_write_32(base + SDMMC_ICR,
+				      SDMMC_STATIC_FLAGS);
+
+			ret = stm32_sdmmc2_stop_transfer();
+			if (ret != 0) {
+				return ret;
+			}
+
+			return -ETIMEDOUT;
+		}
+
+		if (size < (SDMMC_FIFO_SIZE / 2U)) {
+			if ((mmio_read_32(base + SDMMC_DCNTR) > 0U) &&
+			    ((status & SDMMC_STAR_RXFIFOE) == 0U)) {
+				*buffer = mmio_read_32(fifo_reg);
+				buffer++;
+			}
+		} else if ((status & SDMMC_STAR_RXFIFOHF) != 0U) {
+			uint32_t count;
+
+			/* Read data from SDMMC Rx FIFO */
+			for (count = 0; count < (SDMMC_FIFO_SIZE / 2U);
+			     count += sizeof(uint32_t)) {
+				*buffer = mmio_read_32(fifo_reg);
+				buffer++;
+			}
+		}
+	} while ((status & flags) == 0U);
+
+	mmio_write_32(base + SDMMC_ICR, SDMMC_STATIC_FLAGS);
+
+	if ((status & SDMMC_STAR_DPSMACT) != 0U) {
+		WARN("%s: DPSMACT=1, send stop\n", __func__);
+		return stm32_sdmmc2_stop_transfer();
+	}
+
+	return 0;
+}
+
+static int stm32_sdmmc2_write(int lba, uintptr_t buf, size_t size)
+{
+	return 0;
+}
+
+static int stm32_sdmmc2_dt_get_config(void)
+{
+	int sdmmc_node;
+	void *fdt = NULL;
+	const fdt32_t *cuint;
+	struct dt_node_info dt_info;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if (fdt == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	sdmmc_node = dt_match_instance_by_compatible(DT_SDMMC2_COMPAT,
+						     sdmmc2_params.reg_base);
+	if (sdmmc_node == -FDT_ERR_NOTFOUND) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	dt_fill_device_info(&dt_info, sdmmc_node);
+	if (dt_info.status == DT_DISABLED) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if (dt_set_pinctrl_config(sdmmc_node) != 0) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	sdmmc2_params.clock_id = dt_info.clock;
+	sdmmc2_params.reset_id = dt_info.reset;
+
+	if ((fdt_getprop(fdt, sdmmc_node, "st,use-ckin", NULL)) != NULL) {
+		sdmmc2_params.pin_ckin = SDMMC_CLKCR_SELCLKRX_0;
+	}
+
+	if ((fdt_getprop(fdt, sdmmc_node, "st,sig-dir", NULL)) != NULL) {
+		sdmmc2_params.dirpol = SDMMC_POWER_DIRPOL;
+	}
+
+	if ((fdt_getprop(fdt, sdmmc_node, "st,neg-edge", NULL)) != NULL) {
+		sdmmc2_params.negedge = SDMMC_CLKCR_NEGEDGE;
+	}
+
+	cuint = fdt_getprop(fdt, sdmmc_node, "bus-width", NULL);
+	if (cuint != NULL) {
+		switch (fdt32_to_cpu(*cuint)) {
+		case 4:
+			sdmmc2_params.bus_width = MMC_BUS_WIDTH_4;
+			break;
+
+		case 8:
+			sdmmc2_params.bus_width = MMC_BUS_WIDTH_8;
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	cuint = fdt_getprop(fdt, sdmmc_node, "max-frequency", NULL);
+	if (cuint != NULL) {
+		sdmmc2_params.max_freq = fdt32_to_cpu(*cuint);
+	}
+
+	sdmmc2_params.vmmc_regu = regulator_get_by_supply_name(fdt, sdmmc_node, "vmmc");
+
+	return 0;
+}
+
+unsigned long long stm32_sdmmc2_mmc_get_device_size(void)
+{
+	return sdmmc2_params.device_info->device_size;
+}
+
+int stm32_sdmmc2_mmc_init(struct stm32_sdmmc2_params *params)
+{
+	assert((params != NULL) &&
+	       ((params->reg_base & MMC_BLOCK_MASK) == 0U) &&
+	       ((params->bus_width == MMC_BUS_WIDTH_1) ||
+		(params->bus_width == MMC_BUS_WIDTH_4) ||
+		(params->bus_width == MMC_BUS_WIDTH_8)));
+
+	memcpy(&sdmmc2_params, params, sizeof(struct stm32_sdmmc2_params));
+
+	sdmmc2_params.vmmc_regu = NULL;
+
+	if (stm32_sdmmc2_dt_get_config() != 0) {
+		ERROR("%s: DT error\n", __func__);
+		return -ENOMEM;
+	}
+
+	clk_enable(sdmmc2_params.clock_id);
+
+	if ((int)sdmmc2_params.reset_id >= 0) {
+		int rc;
+
+		rc = stm32mp_reset_assert(sdmmc2_params.reset_id, TIMEOUT_US_1_MS);
+		if (rc != 0) {
+			panic();
+		}
+		udelay(2);
+		rc = stm32mp_reset_deassert(sdmmc2_params.reset_id, TIMEOUT_US_1_MS);
+		if (rc != 0) {
+			panic();
+		}
+		mdelay(1);
+	}
+
+	sdmmc2_params.clk_rate = clk_get_rate(sdmmc2_params.clock_id);
+	sdmmc2_params.device_info->ocr_voltage = OCR_3_2_3_3 | OCR_3_3_3_4;
+
+	return mmc_init(&stm32_sdmmc2_ops, sdmmc2_params.clk_rate,
+			sdmmc2_params.bus_width, sdmmc2_params.flags,
+			sdmmc2_params.device_info);
+}
diff --git a/drivers/st/pmic/stm32mp_pmic.c b/drivers/st/pmic/stm32mp_pmic.c
new file mode 100644
index 0000000..5b43760
--- /dev/null
+++ b/drivers/st/pmic/stm32mp_pmic.c
@@ -0,0 +1,526 @@
+/*
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/regulator.h>
+#include <drivers/st/stm32_i2c.h>
+#include <drivers/st/stm32mp_pmic.h>
+#include <drivers/st/stpmic1.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#define PMIC_NODE_NOT_FOUND	1
+
+static struct i2c_handle_s i2c_handle;
+static uint32_t pmic_i2c_addr;
+
+static int register_pmic(void);
+
+static int dt_get_pmic_node(void *fdt)
+{
+	static int node = -FDT_ERR_BADOFFSET;
+
+	if (node == -FDT_ERR_BADOFFSET) {
+		node = fdt_node_offset_by_compatible(fdt, -1, "st,stpmic1");
+	}
+
+	return node;
+}
+
+int dt_pmic_status(void)
+{
+	static int status = -FDT_ERR_BADVALUE;
+	int node;
+	void *fdt;
+
+	if (status != -FDT_ERR_BADVALUE) {
+		return status;
+	}
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node = dt_get_pmic_node(fdt);
+	if (node <= 0) {
+		status = -FDT_ERR_NOTFOUND;
+
+		return status;
+	}
+
+	status = (int)fdt_get_status(node);
+
+	return status;
+}
+
+static bool dt_pmic_is_secure(void)
+{
+	int status = dt_pmic_status();
+
+	return (status >= 0) &&
+	       (status == DT_SECURE) &&
+	       (i2c_handle.dt_status == DT_SECURE);
+}
+
+/*
+ * Get PMIC and its I2C bus configuration from the device tree.
+ * Return 0 on success, negative on error, 1 if no PMIC node is defined.
+ */
+static int dt_pmic_i2c_config(struct dt_node_info *i2c_info,
+			      struct stm32_i2c_init_s *init)
+{
+	static int i2c_node = -FDT_ERR_NOTFOUND;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if (i2c_node == -FDT_ERR_NOTFOUND) {
+		int pmic_node;
+		const fdt32_t *cuint;
+
+		pmic_node = dt_get_pmic_node(fdt);
+		if (pmic_node < 0) {
+			return PMIC_NODE_NOT_FOUND;
+		}
+
+		cuint = fdt_getprop(fdt, pmic_node, "reg", NULL);
+		if (cuint == NULL) {
+			return -FDT_ERR_NOTFOUND;
+		}
+
+		pmic_i2c_addr = fdt32_to_cpu(*cuint) << 1;
+		if (pmic_i2c_addr > UINT16_MAX) {
+			return -FDT_ERR_BADVALUE;
+		}
+
+		i2c_node = fdt_parent_offset(fdt, pmic_node);
+		if (i2c_node < 0) {
+			return -FDT_ERR_NOTFOUND;
+		}
+	}
+
+	dt_fill_device_info(i2c_info, i2c_node);
+	if (i2c_info->base == 0U) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return stm32_i2c_get_setup_from_fdt(fdt, i2c_node, init);
+}
+
+bool initialize_pmic_i2c(void)
+{
+	int ret;
+	struct dt_node_info i2c_info;
+	struct i2c_handle_s *i2c = &i2c_handle;
+	struct stm32_i2c_init_s i2c_init;
+
+	ret = dt_pmic_i2c_config(&i2c_info, &i2c_init);
+	if (ret < 0) {
+		ERROR("I2C configuration failed %d\n", ret);
+		panic();
+	}
+
+	if (ret != 0) {
+		return false;
+	}
+
+	/* Initialize PMIC I2C */
+	i2c->i2c_base_addr		= i2c_info.base;
+	i2c->dt_status			= i2c_info.status;
+	i2c->clock			= i2c_info.clock;
+	i2c->i2c_state			= I2C_STATE_RESET;
+	i2c_init.own_address1		= pmic_i2c_addr;
+	i2c_init.addressing_mode	= I2C_ADDRESSINGMODE_7BIT;
+	i2c_init.dual_address_mode	= I2C_DUALADDRESS_DISABLE;
+	i2c_init.own_address2		= 0;
+	i2c_init.own_address2_masks	= I2C_OAR2_OA2NOMASK;
+	i2c_init.general_call_mode	= I2C_GENERALCALL_DISABLE;
+	i2c_init.no_stretch_mode	= I2C_NOSTRETCH_DISABLE;
+	i2c_init.analog_filter		= 1;
+	i2c_init.digital_filter_coef	= 0;
+
+	ret = stm32_i2c_init(i2c, &i2c_init);
+	if (ret != 0) {
+		ERROR("Cannot initialize I2C %x (%d)\n",
+		      i2c->i2c_base_addr, ret);
+		panic();
+	}
+
+	if (!stm32_i2c_is_device_ready(i2c, pmic_i2c_addr, 1,
+				       I2C_TIMEOUT_BUSY_MS)) {
+		ERROR("I2C device not ready\n");
+		panic();
+	}
+
+	stpmic1_bind_i2c(i2c, (uint16_t)pmic_i2c_addr);
+
+	return true;
+}
+
+static void register_pmic_shared_peripherals(void)
+{
+	uintptr_t i2c_base = i2c_handle.i2c_base_addr;
+
+	if (dt_pmic_is_secure()) {
+		stm32mp_register_secure_periph_iomem(i2c_base);
+	} else {
+		if (i2c_base != 0U) {
+			stm32mp_register_non_secure_periph_iomem(i2c_base);
+		}
+	}
+}
+
+void initialize_pmic(void)
+{
+	if (!initialize_pmic_i2c()) {
+		VERBOSE("No PMIC\n");
+		return;
+	}
+
+	register_pmic_shared_peripherals();
+
+	if (register_pmic() < 0) {
+		panic();
+	}
+
+	if (stpmic1_powerctrl_on() < 0) {
+		panic();
+	}
+
+}
+
+#if DEBUG
+void print_pmic_info_and_debug(void)
+{
+	unsigned long pmic_version;
+
+	if (stpmic1_get_version(&pmic_version) != 0) {
+		ERROR("Failed to access PMIC\n");
+		panic();
+	}
+
+	INFO("PMIC version = 0x%02lx\n", pmic_version);
+}
+#endif
+
+int pmic_ddr_power_init(enum ddr_type ddr_type)
+{
+	int status;
+	uint16_t buck3_min_mv;
+	struct rdev *buck2, *buck3, *vref;
+	struct rdev *ldo3 __unused;
+
+	buck2 = regulator_get_by_name("buck2");
+	if (buck2 == NULL) {
+		return -ENOENT;
+	}
+
+#if STM32MP15
+	ldo3 = regulator_get_by_name("ldo3");
+	if (ldo3 == NULL) {
+		return -ENOENT;
+	}
+#endif
+
+	vref = regulator_get_by_name("vref_ddr");
+	if (vref == NULL) {
+		return -ENOENT;
+	}
+
+	switch (ddr_type) {
+	case STM32MP_DDR3:
+#if STM32MP15
+		status = regulator_set_flag(ldo3, REGUL_SINK_SOURCE);
+		if (status != 0) {
+			return status;
+		}
+#endif
+
+		status = regulator_set_min_voltage(buck2);
+		if (status != 0) {
+			return status;
+		}
+
+		status = regulator_enable(buck2);
+		if (status != 0) {
+			return status;
+		}
+
+		status = regulator_enable(vref);
+		if (status != 0) {
+			return status;
+		}
+
+#if STM32MP15
+		status = regulator_enable(ldo3);
+		if (status != 0) {
+			return status;
+		}
+#endif
+		break;
+
+	case STM32MP_LPDDR2:
+	case STM32MP_LPDDR3:
+		/*
+		 * Set LDO3 to 1.8V
+		 * Set LDO3 to bypass mode if BUCK3 = 1.8V
+		 * Set LDO3 to normal mode if BUCK3 != 1.8V
+		 */
+		buck3 = regulator_get_by_name("buck3");
+		if (buck3 == NULL) {
+			return -ENOENT;
+		}
+
+		regulator_get_range(buck3, &buck3_min_mv, NULL);
+
+#if STM32MP15
+		if (buck3_min_mv != 1800) {
+			status = regulator_set_min_voltage(ldo3);
+			if (status != 0) {
+				return status;
+			}
+		} else {
+			status = regulator_set_flag(ldo3, REGUL_ENABLE_BYPASS);
+			if (status != 0) {
+				return status;
+			}
+		}
+#endif
+
+		status = regulator_set_min_voltage(buck2);
+		if (status != 0) {
+			return status;
+		}
+
+#if STM32MP15
+		status = regulator_enable(ldo3);
+		if (status != 0) {
+			return status;
+		}
+#endif
+
+		status = regulator_enable(buck2);
+		if (status != 0) {
+			return status;
+		}
+
+		status = regulator_enable(vref);
+		if (status != 0) {
+			return status;
+		}
+		break;
+
+	default:
+		break;
+	};
+
+	return 0;
+}
+
+int pmic_voltages_init(void)
+{
+#if STM32MP13
+	struct rdev *buck1, *buck4;
+	int status;
+
+	buck1 = regulator_get_by_name("buck1");
+	if (buck1 == NULL) {
+		return -ENOENT;
+	}
+
+	buck4 = regulator_get_by_name("buck4");
+	if (buck4 == NULL) {
+		return -ENOENT;
+	}
+
+	status = regulator_set_min_voltage(buck1);
+	if (status != 0) {
+		return status;
+	}
+
+	status = regulator_set_min_voltage(buck4);
+	if (status != 0) {
+		return status;
+	}
+#endif
+
+	return 0;
+}
+
+enum {
+	STPMIC1_BUCK1 = 0,
+	STPMIC1_BUCK2,
+	STPMIC1_BUCK3,
+	STPMIC1_BUCK4,
+	STPMIC1_LDO1,
+	STPMIC1_LDO2,
+	STPMIC1_LDO3,
+	STPMIC1_LDO4,
+	STPMIC1_LDO5,
+	STPMIC1_LDO6,
+	STPMIC1_VREF_DDR,
+	STPMIC1_BOOST,
+	STPMIC1_VBUS_OTG,
+	STPMIC1_SW_OUT,
+};
+
+static int pmic_set_state(const struct regul_description *desc, bool enable)
+{
+	VERBOSE("%s: set state to %d\n", desc->node_name, enable);
+
+	if (enable == STATE_ENABLE) {
+		return stpmic1_regulator_enable(desc->node_name);
+	} else {
+		return stpmic1_regulator_disable(desc->node_name);
+	}
+}
+
+static int pmic_get_state(const struct regul_description *desc)
+{
+	VERBOSE("%s: get state\n", desc->node_name);
+
+	return stpmic1_is_regulator_enabled(desc->node_name);
+}
+
+static int pmic_get_voltage(const struct regul_description *desc)
+{
+	VERBOSE("%s: get volt\n", desc->node_name);
+
+	return stpmic1_regulator_voltage_get(desc->node_name);
+}
+
+static int pmic_set_voltage(const struct regul_description *desc, uint16_t mv)
+{
+	VERBOSE("%s: get volt\n", desc->node_name);
+
+	return stpmic1_regulator_voltage_set(desc->node_name, mv);
+}
+
+static int pmic_list_voltages(const struct regul_description *desc,
+			      const uint16_t **levels, size_t *count)
+{
+	VERBOSE("%s: list volt\n", desc->node_name);
+
+	return stpmic1_regulator_levels_mv(desc->node_name, levels, count);
+}
+
+static int pmic_set_flag(const struct regul_description *desc, uint16_t flag)
+{
+	VERBOSE("%s: set_flag 0x%x\n", desc->node_name, flag);
+
+	switch (flag) {
+	case REGUL_OCP:
+		return stpmic1_regulator_icc_set(desc->node_name);
+
+	case REGUL_ACTIVE_DISCHARGE:
+		return stpmic1_active_discharge_mode_set(desc->node_name);
+
+	case REGUL_PULL_DOWN:
+		return stpmic1_regulator_pull_down_set(desc->node_name);
+
+	case REGUL_MASK_RESET:
+		return stpmic1_regulator_mask_reset_set(desc->node_name);
+
+	case REGUL_SINK_SOURCE:
+		return stpmic1_regulator_sink_mode_set(desc->node_name);
+
+	case REGUL_ENABLE_BYPASS:
+		return stpmic1_regulator_bypass_mode_set(desc->node_name);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct regul_ops pmic_ops = {
+	.set_state = pmic_set_state,
+	.get_state = pmic_get_state,
+	.set_voltage = pmic_set_voltage,
+	.get_voltage = pmic_get_voltage,
+	.list_voltages = pmic_list_voltages,
+	.set_flag = pmic_set_flag,
+};
+
+#define DEFINE_REGU(name) { \
+	.node_name = name, \
+	.ops = &pmic_ops, \
+	.driver_data = NULL, \
+	.enable_ramp_delay = 1000, \
+}
+
+static const struct regul_description pmic_regs[] = {
+	[STPMIC1_BUCK1] = DEFINE_REGU("buck1"),
+	[STPMIC1_BUCK2] = DEFINE_REGU("buck2"),
+	[STPMIC1_BUCK3] = DEFINE_REGU("buck3"),
+	[STPMIC1_BUCK4] = DEFINE_REGU("buck4"),
+	[STPMIC1_LDO1] = DEFINE_REGU("ldo1"),
+	[STPMIC1_LDO2] = DEFINE_REGU("ldo2"),
+	[STPMIC1_LDO3] = DEFINE_REGU("ldo3"),
+	[STPMIC1_LDO4] = DEFINE_REGU("ldo4"),
+	[STPMIC1_LDO5] = DEFINE_REGU("ldo5"),
+	[STPMIC1_LDO6] = DEFINE_REGU("ldo6"),
+	[STPMIC1_VREF_DDR] = DEFINE_REGU("vref_ddr"),
+	[STPMIC1_BOOST] = DEFINE_REGU("boost"),
+	[STPMIC1_VBUS_OTG] = DEFINE_REGU("pwr_sw1"),
+	[STPMIC1_SW_OUT] = DEFINE_REGU("pwr_sw2"),
+};
+
+#define NB_REG ARRAY_SIZE(pmic_regs)
+
+static int register_pmic(void)
+{
+	void *fdt;
+	int pmic_node, regulators_node, subnode;
+
+	VERBOSE("Register pmic\n");
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	pmic_node = dt_get_pmic_node(fdt);
+	if (pmic_node < 0) {
+		return pmic_node;
+	}
+
+	regulators_node = fdt_subnode_offset(fdt, pmic_node, "regulators");
+	if (regulators_node < 0) {
+		return -ENOENT;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, regulators_node) {
+		const char *reg_name = fdt_get_name(fdt, subnode, NULL);
+		const struct regul_description *desc;
+		unsigned int i;
+		int ret;
+
+		for (i = 0; i < NB_REG; i++) {
+			desc = &pmic_regs[i];
+			if (strcmp(desc->node_name, reg_name) == 0) {
+				break;
+			}
+		}
+		assert(i < NB_REG);
+
+		ret = regulator_register(desc, subnode);
+		if (ret != 0) {
+			WARN("%s:%d failed to register %s\n", __func__,
+			     __LINE__, reg_name);
+			return ret;
+		}
+	}
+
+	return 0;
+}
diff --git a/drivers/st/pmic/stpmic1.c b/drivers/st/pmic/stpmic1.c
new file mode 100644
index 0000000..37eb50b
--- /dev/null
+++ b/drivers/st/pmic/stpmic1.c
@@ -0,0 +1,937 @@
+/*
+ * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/st/stpmic1.h>
+
+#define I2C_TIMEOUT_MS		25
+
+struct regul_struct {
+	const char *dt_node_name;
+	const uint16_t *voltage_table;
+	uint8_t voltage_table_size;
+	uint8_t control_reg;
+	uint8_t enable_mask;
+	uint8_t low_power_reg;
+	uint8_t pull_down_reg;
+	uint8_t pull_down;
+	uint8_t mask_reset_reg;
+	uint8_t mask_reset;
+	uint8_t icc_reg;
+	uint8_t icc_mask;
+};
+
+static struct i2c_handle_s *pmic_i2c_handle;
+static uint16_t pmic_i2c_addr;
+/*
+ * Special mode corresponds to LDO3 in sink source mode or in bypass mode.
+ * LDO3 doesn't switch back from special to normal mode.
+ */
+static bool ldo3_special_mode;
+
+/* Voltage tables in mV */
+static const uint16_t buck1_voltage_table[] = {
+	725,
+	725,
+	725,
+	725,
+	725,
+	725,
+	750,
+	775,
+	800,
+	825,
+	850,
+	875,
+	900,
+	925,
+	950,
+	975,
+	1000,
+	1025,
+	1050,
+	1075,
+	1100,
+	1125,
+	1150,
+	1175,
+	1200,
+	1225,
+	1250,
+	1275,
+	1300,
+	1325,
+	1350,
+	1375,
+	1400,
+	1425,
+	1450,
+	1475,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+};
+
+static const uint16_t buck2_voltage_table[] = {
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1050,
+	1050,
+	1100,
+	1100,
+	1150,
+	1150,
+	1200,
+	1200,
+	1250,
+	1250,
+	1300,
+	1300,
+	1350,
+	1350,
+	1400,
+	1400,
+	1450,
+	1450,
+	1500,
+};
+
+static const uint16_t buck3_voltage_table[] = {
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1100,
+	1100,
+	1100,
+	1100,
+	1200,
+	1200,
+	1200,
+	1200,
+	1300,
+	1300,
+	1300,
+	1300,
+	1400,
+	1400,
+	1400,
+	1400,
+	1500,
+	1600,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+	3400,
+};
+
+static const uint16_t buck4_voltage_table[] = {
+	600,
+	625,
+	650,
+	675,
+	700,
+	725,
+	750,
+	775,
+	800,
+	825,
+	850,
+	875,
+	900,
+	925,
+	950,
+	975,
+	1000,
+	1025,
+	1050,
+	1075,
+	1100,
+	1125,
+	1150,
+	1175,
+	1200,
+	1225,
+	1250,
+	1275,
+	1300,
+	1300,
+	1350,
+	1350,
+	1400,
+	1400,
+	1450,
+	1450,
+	1500,
+	1600,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+	3400,
+	3500,
+	3600,
+	3700,
+	3800,
+	3900,
+};
+
+static const uint16_t ldo1_voltage_table[] = {
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+};
+
+static const uint16_t ldo2_voltage_table[] = {
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+};
+
+static const uint16_t ldo3_voltage_table[] = {
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+	3300,
+	3300,
+	3300,
+	3300,
+	3300,
+	3300,
+};
+
+/* Special mode table is used for sink source OR bypass mode */
+static const uint16_t ldo3_special_mode_table[] = {
+	0,
+};
+
+static const uint16_t ldo5_voltage_table[] = {
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+	3400,
+	3500,
+	3600,
+	3700,
+	3800,
+	3900,
+};
+
+static const uint16_t ldo6_voltage_table[] = {
+	900,
+	1000,
+	1100,
+	1200,
+	1300,
+	1400,
+	1500,
+	1600,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+};
+
+static const uint16_t ldo4_voltage_table[] = {
+	3300,
+};
+
+static const uint16_t vref_ddr_voltage_table[] = {
+	3300,
+};
+
+static const uint16_t fixed_5v_voltage_table[] = {
+	5000,
+};
+
+/* Table of Regulators in PMIC SoC */
+static const struct regul_struct regulators_table[] = {
+	{
+		.dt_node_name	= "buck1",
+		.voltage_table	= buck1_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(buck1_voltage_table),
+		.control_reg	= BUCK1_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
+		.low_power_reg	= BUCK1_PWRCTRL_REG,
+		.pull_down_reg	= BUCK_PULL_DOWN_REG,
+		.pull_down	= BUCK1_PULL_DOWN_SHIFT,
+		.mask_reset_reg	= MASK_RESET_BUCK_REG,
+		.mask_reset	= BUCK1_MASK_RESET,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= BUCK1_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "buck2",
+		.voltage_table	= buck2_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(buck2_voltage_table),
+		.control_reg	= BUCK2_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
+		.low_power_reg	= BUCK2_PWRCTRL_REG,
+		.pull_down_reg	= BUCK_PULL_DOWN_REG,
+		.pull_down	= BUCK2_PULL_DOWN_SHIFT,
+		.mask_reset_reg	= MASK_RESET_BUCK_REG,
+		.mask_reset	= BUCK2_MASK_RESET,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= BUCK2_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "buck3",
+		.voltage_table	= buck3_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(buck3_voltage_table),
+		.control_reg	= BUCK3_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
+		.low_power_reg	= BUCK3_PWRCTRL_REG,
+		.pull_down_reg	= BUCK_PULL_DOWN_REG,
+		.pull_down	= BUCK3_PULL_DOWN_SHIFT,
+		.mask_reset_reg	= MASK_RESET_BUCK_REG,
+		.mask_reset	= BUCK3_MASK_RESET,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= BUCK3_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "buck4",
+		.voltage_table	= buck4_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(buck4_voltage_table),
+		.control_reg	= BUCK4_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
+		.low_power_reg	= BUCK4_PWRCTRL_REG,
+		.pull_down_reg	= BUCK_PULL_DOWN_REG,
+		.pull_down	= BUCK4_PULL_DOWN_SHIFT,
+		.mask_reset_reg	= MASK_RESET_BUCK_REG,
+		.mask_reset	= BUCK4_MASK_RESET,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= BUCK4_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "ldo1",
+		.voltage_table	= ldo1_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo1_voltage_table),
+		.control_reg	= LDO1_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
+		.low_power_reg	= LDO1_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= LDO1_MASK_RESET,
+		.icc_reg	= LDO_ICC_TURNOFF_REG,
+		.icc_mask	= LDO1_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "ldo2",
+		.voltage_table	= ldo2_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo2_voltage_table),
+		.control_reg	= LDO2_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
+		.low_power_reg	= LDO2_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= LDO2_MASK_RESET,
+		.icc_reg	= LDO_ICC_TURNOFF_REG,
+		.icc_mask	= LDO2_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "ldo3",
+		.voltage_table	= ldo3_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo3_voltage_table),
+		.control_reg	= LDO3_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
+		.low_power_reg	= LDO3_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= LDO3_MASK_RESET,
+		.icc_reg	= LDO_ICC_TURNOFF_REG,
+		.icc_mask	= LDO3_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "ldo4",
+		.voltage_table	= ldo4_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo4_voltage_table),
+		.control_reg	= LDO4_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
+		.low_power_reg	= LDO4_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= LDO4_MASK_RESET,
+		.icc_reg	= LDO_ICC_TURNOFF_REG,
+		.icc_mask	= LDO4_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "ldo5",
+		.voltage_table	= ldo5_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo5_voltage_table),
+		.control_reg	= LDO5_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
+		.low_power_reg	= LDO5_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= LDO5_MASK_RESET,
+		.icc_reg	= LDO_ICC_TURNOFF_REG,
+		.icc_mask	= LDO5_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "ldo6",
+		.voltage_table	= ldo6_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo6_voltage_table),
+		.control_reg	= LDO6_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
+		.low_power_reg	= LDO6_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= LDO6_MASK_RESET,
+		.icc_reg	= LDO_ICC_TURNOFF_REG,
+		.icc_mask	= LDO6_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "vref_ddr",
+		.voltage_table	= vref_ddr_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(vref_ddr_voltage_table),
+		.control_reg	= VREF_DDR_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
+		.low_power_reg	= VREF_DDR_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= VREF_DDR_MASK_RESET,
+	},
+	{
+		.dt_node_name	= "boost",
+		.voltage_table	= fixed_5v_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(fixed_5v_voltage_table),
+		.control_reg	= USB_CONTROL_REG,
+		.enable_mask	= BOOST_ENABLED,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= BOOST_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "pwr_sw1",
+		.voltage_table	= fixed_5v_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(fixed_5v_voltage_table),
+		.control_reg	= USB_CONTROL_REG,
+		.enable_mask	= USBSW_OTG_SWITCH_ENABLED,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= PWR_SW1_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "pwr_sw2",
+		.voltage_table	= fixed_5v_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(fixed_5v_voltage_table),
+		.control_reg	= USB_CONTROL_REG,
+		.enable_mask	= SWIN_SWOUT_ENABLED,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= PWR_SW2_ICC_SHIFT,
+	},
+};
+
+#define MAX_REGUL	ARRAY_SIZE(regulators_table)
+
+static const struct regul_struct *get_regulator_data(const char *name)
+{
+	uint8_t i;
+
+	for (i = 0 ; i < MAX_REGUL ; i++) {
+		if (strncmp(name, regulators_table[i].dt_node_name,
+			    strlen(regulators_table[i].dt_node_name)) == 0) {
+			return &regulators_table[i];
+		}
+	}
+
+	/* Regulator not found */
+	panic();
+	return NULL;
+}
+
+static uint8_t voltage_to_index(const char *name, uint16_t millivolts)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+	uint8_t i;
+
+	for (i = 0 ; i < regul->voltage_table_size ; i++) {
+		if (regul->voltage_table[i] == millivolts) {
+			return i;
+		}
+	}
+
+	/* Voltage not found */
+	panic();
+
+	return 0;
+}
+
+int stpmic1_powerctrl_on(void)
+{
+	return stpmic1_register_update(MAIN_CONTROL_REG, PWRCTRL_PIN_VALID,
+				       PWRCTRL_PIN_VALID);
+}
+
+int stpmic1_switch_off(void)
+{
+	return stpmic1_register_update(MAIN_CONTROL_REG, 1,
+				       SOFTWARE_SWITCH_OFF_ENABLED);
+}
+
+int stpmic1_regulator_enable(const char *name)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	return stpmic1_register_update(regul->control_reg, regul->enable_mask,
+				       regul->enable_mask);
+}
+
+int stpmic1_regulator_disable(const char *name)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	return stpmic1_register_update(regul->control_reg, 0,
+				       regul->enable_mask);
+}
+
+bool stpmic1_is_regulator_enabled(const char *name)
+{
+	uint8_t val;
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	if (stpmic1_register_read(regul->control_reg, &val) != 0) {
+		panic();
+	}
+
+	return (val & regul->enable_mask) == regul->enable_mask;
+}
+
+int stpmic1_regulator_voltage_set(const char *name, uint16_t millivolts)
+{
+	uint8_t voltage_index = voltage_to_index(name, millivolts);
+	const struct regul_struct *regul = get_regulator_data(name);
+	uint8_t mask;
+
+	if ((strncmp(name, "ldo3", 5) == 0) && ldo3_special_mode) {
+		/*
+		 * when the LDO3 is in special mode, we do not change voltage,
+		 * because by setting voltage, the LDO would leaves sink-source
+		 * mode. There is obviously no reason to leave sink-source mode
+		 * at runtime.
+		 */
+		return 0;
+	}
+
+	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
+	if (strncmp(name, "buck", 4) == 0) {
+		mask = BUCK_VOLTAGE_MASK;
+	} else if ((strncmp(name, "ldo", 3) == 0) &&
+		   (strncmp(name, "ldo4", 5) != 0)) {
+		mask = LDO_VOLTAGE_MASK;
+	} else {
+		return 0;
+	}
+
+	return stpmic1_register_update(regul->control_reg,
+				       voltage_index << LDO_BUCK_VOLTAGE_SHIFT,
+				       mask);
+}
+
+int stpmic1_regulator_pull_down_set(const char *name)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	if (regul->pull_down_reg != 0) {
+		return stpmic1_register_update(regul->pull_down_reg,
+					       BIT(regul->pull_down),
+					       LDO_BUCK_PULL_DOWN_MASK <<
+					       regul->pull_down);
+	}
+
+	return 0;
+}
+
+int stpmic1_regulator_mask_reset_set(const char *name)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	if (regul->mask_reset_reg == 0U) {
+		return -EPERM;
+	}
+
+	return stpmic1_register_update(regul->mask_reset_reg,
+				       BIT(regul->mask_reset),
+				       LDO_BUCK_RESET_MASK <<
+				       regul->mask_reset);
+}
+
+int stpmic1_regulator_icc_set(const char *name)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	if (regul->mask_reset_reg == 0U) {
+		return -EPERM;
+	}
+
+	return stpmic1_register_update(regul->icc_reg,
+				       BIT(regul->icc_mask),
+				       BIT(regul->icc_mask));
+}
+
+int stpmic1_regulator_sink_mode_set(const char *name)
+{
+	if (strncmp(name, "ldo3", 5) != 0) {
+		return -EPERM;
+	}
+
+	ldo3_special_mode = true;
+
+	/* disable bypass mode, enable sink mode */
+	return stpmic1_register_update(LDO3_CONTROL_REG,
+				       LDO3_DDR_SEL << LDO_BUCK_VOLTAGE_SHIFT,
+				       LDO3_BYPASS | LDO_VOLTAGE_MASK);
+}
+
+int stpmic1_regulator_bypass_mode_set(const char *name)
+{
+	if (strncmp(name, "ldo3", 5) != 0) {
+		return -EPERM;
+	}
+
+	ldo3_special_mode = true;
+
+	/* enable bypass mode, disable sink mode */
+	return stpmic1_register_update(LDO3_CONTROL_REG,
+				       LDO3_BYPASS,
+				       LDO3_BYPASS | LDO_VOLTAGE_MASK);
+}
+
+int stpmic1_active_discharge_mode_set(const char *name)
+{
+	if (strncmp(name, "pwr_sw1", 8) == 0) {
+		return stpmic1_register_update(USB_CONTROL_REG,
+					       VBUS_OTG_DISCHARGE,
+					       VBUS_OTG_DISCHARGE);
+	}
+
+	if (strncmp(name, "pwr_sw2", 8) == 0) {
+		return stpmic1_register_update(USB_CONTROL_REG,
+					       SW_OUT_DISCHARGE,
+					       SW_OUT_DISCHARGE);
+	}
+
+	return -EPERM;
+}
+
+int stpmic1_regulator_levels_mv(const char *name, const uint16_t **levels,
+				size_t *levels_count)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	if ((strncmp(name, "ldo3", 5) == 0) && ldo3_special_mode) {
+		*levels_count = ARRAY_SIZE(ldo3_special_mode_table);
+		*levels = ldo3_special_mode_table;
+	} else {
+		*levels_count = regul->voltage_table_size;
+		*levels = regul->voltage_table;
+	}
+
+	return 0;
+}
+
+int stpmic1_regulator_voltage_get(const char *name)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+	uint8_t value;
+	uint8_t mask;
+	int status;
+
+	if ((strncmp(name, "ldo3", 5) == 0) && ldo3_special_mode) {
+		return 0;
+	}
+
+	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
+	if (strncmp(name, "buck", 4) == 0) {
+		mask = BUCK_VOLTAGE_MASK;
+	} else if ((strncmp(name, "ldo", 3) == 0) &&
+		   (strncmp(name, "ldo4", 5) != 0)) {
+		mask = LDO_VOLTAGE_MASK;
+	} else {
+		return 0;
+	}
+
+	status = stpmic1_register_read(regul->control_reg, &value);
+	if (status < 0) {
+		return status;
+	}
+
+	value = (value & mask) >> LDO_BUCK_VOLTAGE_SHIFT;
+
+	if (value > regul->voltage_table_size) {
+		return -ERANGE;
+	}
+
+	return (int)regul->voltage_table[value];
+}
+
+int stpmic1_register_read(uint8_t register_id,  uint8_t *value)
+{
+	return stm32_i2c_mem_read(pmic_i2c_handle, pmic_i2c_addr,
+				  (uint16_t)register_id,
+				  I2C_MEMADD_SIZE_8BIT, value,
+				  1, I2C_TIMEOUT_MS);
+}
+
+int stpmic1_register_write(uint8_t register_id, uint8_t value)
+{
+	int status;
+
+	status = stm32_i2c_mem_write(pmic_i2c_handle, pmic_i2c_addr,
+				     (uint16_t)register_id,
+				     I2C_MEMADD_SIZE_8BIT, &value,
+				     1, I2C_TIMEOUT_MS);
+
+#if ENABLE_ASSERTIONS
+	if (status != 0) {
+		return status;
+	}
+
+	if ((register_id != WATCHDOG_CONTROL_REG) && (register_id <= 0x40U)) {
+		uint8_t readval;
+
+		status = stpmic1_register_read(register_id, &readval);
+		if (status != 0) {
+			return status;
+		}
+
+		if (readval != value) {
+			return -EIO;
+		}
+	}
+#endif
+
+	return status;
+}
+
+int stpmic1_register_update(uint8_t register_id, uint8_t value, uint8_t mask)
+{
+	int status;
+	uint8_t val;
+
+	status = stpmic1_register_read(register_id, &val);
+	if (status != 0) {
+		return status;
+	}
+
+	val = (val & ~mask) | (value & mask);
+
+	return stpmic1_register_write(register_id, val);
+}
+
+void stpmic1_bind_i2c(struct i2c_handle_s *i2c_handle, uint16_t i2c_addr)
+{
+	pmic_i2c_handle = i2c_handle;
+	pmic_i2c_addr = i2c_addr;
+}
+
+void stpmic1_dump_regulators(void)
+{
+	uint32_t i;
+
+	for (i = 0U; i < MAX_REGUL; i++) {
+		const char *name __unused = regulators_table[i].dt_node_name;
+
+		VERBOSE("PMIC regul %s: %sable, %dmV",
+			name,
+			stpmic1_is_regulator_enabled(name) ? "en" : "dis",
+			stpmic1_regulator_voltage_get(name));
+	}
+}
+
+int stpmic1_get_version(unsigned long *version)
+{
+	uint8_t read_val;
+	int status;
+
+	status = stpmic1_register_read(VERSION_STATUS_REG, &read_val);
+	if (status < 0) {
+		return status;
+	}
+
+	*version = (unsigned long)read_val;
+
+	return 0;
+}
diff --git a/drivers/st/regulator/regulator_core.c b/drivers/st/regulator/regulator_core.c
new file mode 100644
index 0000000..5cc8329
--- /dev/null
+++ b/drivers/st/regulator/regulator_core.c
@@ -0,0 +1,560 @@
+/*
+ * Copyright (c) 2021-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <limits.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/regulator.h>
+#include <libfdt.h>
+
+#define MAX_PROPERTY_LEN 64
+
+static struct rdev rdev_array[PLAT_NB_RDEVS];
+
+#define for_each_rdev(rdev) \
+	for (rdev = rdev_array; rdev < (rdev_array + PLAT_NB_RDEVS); rdev++)
+
+#define for_each_registered_rdev(rdev) \
+	for (rdev = rdev_array; \
+	     (rdev < (rdev_array + PLAT_NB_RDEVS)) && (rdev->desc != NULL); rdev++)
+
+static void lock_driver(const struct rdev *rdev)
+{
+	if (rdev->desc->ops->lock != NULL) {
+		rdev->desc->ops->lock(rdev->desc);
+	}
+}
+
+static void unlock_driver(const struct rdev *rdev)
+{
+	if (rdev->desc->ops->unlock != NULL) {
+		rdev->desc->ops->unlock(rdev->desc);
+	}
+}
+
+static struct rdev *regulator_get_by_phandle(int32_t phandle)
+{
+	struct rdev *rdev;
+
+	for_each_registered_rdev(rdev) {
+		if (rdev->phandle == phandle) {
+			return rdev;
+		}
+	}
+
+	WARN("%s: phandle %d not found\n", __func__, phandle);
+	return NULL;
+}
+
+/*
+ * Get a regulator from its node name
+ *
+ * @fdt - pointer to device tree memory
+ * @node_name - name of the node "ldo1"
+ * Return pointer to rdev if succeed, NULL else.
+ */
+struct rdev *regulator_get_by_name(const char *node_name)
+{
+	struct rdev *rdev;
+
+	assert(node_name != NULL);
+	VERBOSE("get %s\n", node_name);
+
+	for_each_registered_rdev(rdev) {
+		if (strcmp(rdev->desc->node_name, node_name) == 0) {
+			return rdev;
+		}
+	}
+
+	WARN("%s: %s not found\n", __func__, node_name);
+	return NULL;
+}
+
+static int32_t get_supply_phandle(const void *fdt, int node, const char *name)
+{
+	const fdt32_t *cuint;
+	int len __unused;
+	int supply_phandle = -FDT_ERR_NOTFOUND;
+	char prop_name[MAX_PROPERTY_LEN];
+
+	len = snprintf(prop_name, MAX_PROPERTY_LEN - 1, "%s-supply", name);
+	assert((len >= 0) && (len < MAX_PROPERTY_LEN - 1));
+
+	cuint = fdt_getprop(fdt, node, prop_name, NULL);
+	if (cuint != NULL) {
+		supply_phandle = fdt32_to_cpu(*cuint);
+		VERBOSE("%s: supplied by %d\n", name, supply_phandle);
+	}
+
+	return supply_phandle;
+}
+
+/*
+ * Get a regulator from a supply name
+ *
+ * @fdt - pointer to device tree memory
+ * @node - offset of the node that contains the supply description
+ * @name - name of the supply "vdd" for "vdd-supply'
+ * Return pointer to rdev if succeed, NULL else.
+ */
+struct rdev *regulator_get_by_supply_name(const void *fdt, int node, const char *name)
+{
+	const int p = get_supply_phandle(fdt, node, name);
+
+	if (p < 0) {
+		return NULL;
+	}
+
+	return regulator_get_by_phandle(p);
+}
+
+static int __regulator_set_state(struct rdev *rdev, bool state)
+{
+	if (rdev->desc->ops->set_state == NULL) {
+		return -ENODEV;
+	}
+
+	return rdev->desc->ops->set_state(rdev->desc, state);
+}
+
+/*
+ * Enable regulator
+ *
+ * @rdev - pointer to rdev struct
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_enable(struct rdev *rdev)
+{
+	int ret;
+
+	assert(rdev != NULL);
+
+	ret = __regulator_set_state(rdev, STATE_ENABLE);
+
+	udelay(rdev->enable_ramp_delay);
+
+	return ret;
+}
+
+/*
+ * Disable regulator
+ *
+ * @rdev - pointer to rdev struct
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_disable(struct rdev *rdev)
+{
+	int ret;
+
+	assert(rdev != NULL);
+
+	if (rdev->flags & REGUL_ALWAYS_ON) {
+		return 0;
+	}
+
+	ret = __regulator_set_state(rdev, STATE_DISABLE);
+
+	udelay(rdev->enable_ramp_delay);
+
+	return ret;
+}
+
+/*
+ * Regulator enabled query
+ *
+ * @rdev - pointer to rdev struct
+ * Return 0 if disabled, 1 if enabled, <0 else.
+ */
+int regulator_is_enabled(const struct rdev *rdev)
+{
+	int ret;
+
+	assert(rdev != NULL);
+
+	VERBOSE("%s: is en\n", rdev->desc->node_name);
+
+	if (rdev->desc->ops->get_state == NULL) {
+		return -ENODEV;
+	}
+
+	lock_driver(rdev);
+
+	ret = rdev->desc->ops->get_state(rdev->desc);
+	if (ret < 0) {
+		ERROR("regul %s get state failed: err:%d\n",
+		      rdev->desc->node_name, ret);
+	}
+
+	unlock_driver(rdev);
+
+	return ret;
+}
+
+/*
+ * Set regulator voltage
+ *
+ * @rdev - pointer to rdev struct
+ * @mvolt - Target voltage level in millivolt
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_set_voltage(struct rdev *rdev, uint16_t mvolt)
+{
+	int ret;
+
+	assert(rdev != NULL);
+
+	VERBOSE("%s: set mvolt\n", rdev->desc->node_name);
+
+	if (rdev->desc->ops->set_voltage == NULL) {
+		return -ENODEV;
+	}
+
+	if ((mvolt < rdev->min_mv) || (mvolt > rdev->max_mv)) {
+		return -EPERM;
+	}
+
+	lock_driver(rdev);
+
+	ret = rdev->desc->ops->set_voltage(rdev->desc, mvolt);
+	if (ret < 0) {
+		ERROR("regul %s set volt failed: err:%d\n",
+		      rdev->desc->node_name, ret);
+	}
+
+	unlock_driver(rdev);
+
+	return ret;
+}
+
+/*
+ * Set regulator min voltage
+ *
+ * @rdev - pointer to rdev struct
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_set_min_voltage(struct rdev *rdev)
+{
+	return regulator_set_voltage(rdev, rdev->min_mv);
+}
+
+/*
+ * Get regulator voltage
+ *
+ * @rdev - pointer to rdev struct
+ * Return milli volts if succeed, <0 else.
+ */
+int regulator_get_voltage(const struct rdev *rdev)
+{
+	int ret;
+
+	assert(rdev != NULL);
+
+	VERBOSE("%s: get volt\n", rdev->desc->node_name);
+
+	if (rdev->desc->ops->get_voltage == NULL) {
+		return rdev->min_mv;
+	}
+
+	lock_driver(rdev);
+
+	ret = rdev->desc->ops->get_voltage(rdev->desc);
+	if (ret < 0) {
+		ERROR("regul %s get voltage failed: err:%d\n",
+		      rdev->desc->node_name, ret);
+	}
+
+	unlock_driver(rdev);
+
+	return ret;
+}
+
+/*
+ * List regulator voltages
+ *
+ * @rdev - pointer to rdev struct
+ * @levels - out: array of supported millitvolt levels from min to max value
+ * @count - out: number of possible millivolt values
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_list_voltages(const struct rdev *rdev, const uint16_t **levels, size_t *count)
+{
+	int ret;
+	size_t n;
+
+	assert(rdev != NULL);
+	assert(levels != NULL);
+	assert(count != NULL);
+
+	VERBOSE("%s: list volt\n", rdev->desc->node_name);
+
+	if (rdev->desc->ops->list_voltages == NULL) {
+		return -ENODEV;
+	}
+
+	lock_driver(rdev);
+
+	ret = rdev->desc->ops->list_voltages(rdev->desc, levels, count);
+
+	unlock_driver(rdev);
+
+	if (ret < 0) {
+		ERROR("regul %s list_voltages failed: err: %d\n",
+		      rdev->desc->node_name, ret);
+		return ret;
+	}
+
+	/*
+	 * Reduce the possible values depending on min and max from device-tree
+	 */
+	n = *count;
+	while ((n > 1U) && ((*levels)[n - 1U] > rdev->max_mv)) {
+		n--;
+	}
+
+	/* Verify that max val is a valid value */
+	if (rdev->max_mv != (*levels)[n - 1]) {
+		ERROR("regul %s: max value %u is invalid\n",
+		      rdev->desc->node_name, rdev->max_mv);
+		return -EINVAL;
+	}
+
+	while ((n > 1U) && ((*levels[0U]) < rdev->min_mv)) {
+		(*levels)++;
+		n--;
+	}
+
+	/* Verify that min is not too high */
+	if (n == 0U) {
+		ERROR("regul %s set min voltage is too high\n",
+		      rdev->desc->node_name);
+		return -EINVAL;
+	}
+
+	/* Verify that min val is a valid vlue */
+	if (rdev->min_mv != (*levels)[0U]) {
+		ERROR("regul %s: min value %u is invalid\n",
+		      rdev->desc->node_name, rdev->min_mv);
+		return -EINVAL;
+	}
+
+	*count = n;
+
+	VERBOSE("rdev->min_mv=%u rdev->max_mv=%u\n", rdev->min_mv, rdev->max_mv);
+
+	return 0;
+}
+
+/*
+ * Get regulator voltages range
+ *
+ * @rdev - pointer to rdev struct
+ * @min_mv - out: min possible millivolt value
+ * @max_mv - out: max possible millivolt value
+ * Return 0 if succeed, non 0 else.
+ */
+void regulator_get_range(const struct rdev *rdev, uint16_t *min_mv, uint16_t *max_mv)
+{
+	assert(rdev != NULL);
+
+	if (min_mv != NULL) {
+		*min_mv = rdev->min_mv;
+	}
+	if (max_mv != NULL) {
+		*max_mv = rdev->max_mv;
+	}
+}
+
+/*
+ * Set regulator flag
+ *
+ * @rdev - pointer to rdev struct
+ * @flag - flag value to set (eg: REGUL_OCP)
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_set_flag(struct rdev *rdev, uint16_t flag)
+{
+	int ret;
+
+	/* check that only one bit is set on flag */
+	if (__builtin_popcount(flag) != 1) {
+		return -EINVAL;
+	}
+
+	/* REGUL_ALWAYS_ON and REGUL_BOOT_ON are internal properties of the core */
+	if ((flag == REGUL_ALWAYS_ON) || (flag == REGUL_BOOT_ON)) {
+		rdev->flags |= flag;
+		return 0;
+	}
+
+	if (rdev->desc->ops->set_flag == NULL) {
+		ERROR("%s can not set any flag\n", rdev->desc->node_name);
+		return -ENODEV;
+	}
+
+	lock_driver(rdev);
+
+	ret = rdev->desc->ops->set_flag(rdev->desc, flag);
+
+	unlock_driver(rdev);
+
+	if (ret != 0) {
+		ERROR("%s: could not set flag %d ret=%d\n",
+		      rdev->desc->node_name, flag, ret);
+		return ret;
+	}
+
+	rdev->flags |= flag;
+
+	return 0;
+}
+
+static int parse_properties(const void *fdt, struct rdev *rdev, int node)
+{
+	int ret;
+
+	if (fdt_getprop(fdt, node, "regulator-always-on", NULL) != NULL) {
+		VERBOSE("%s: set regulator-always-on\n", rdev->desc->node_name);
+		ret = regulator_set_flag(rdev, REGUL_ALWAYS_ON);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Parse the device-tree for a regulator
+ *
+ * Read min/max voltage from dt and check its validity
+ * Read the properties, and call the driver to set flags
+ * Read power supply phandle
+ * Read and store low power mode states
+ *
+ * @rdev - pointer to rdev struct
+ * @node - device-tree node offset of the regulator
+ * Return 0 if disabled, 1 if enabled, <0 else.
+ */
+static int parse_dt(struct rdev *rdev, int node)
+{
+	void *fdt;
+	const fdt32_t *cuint;
+	const uint16_t *levels;
+	size_t size;
+	int ret;
+
+	VERBOSE("%s: parse dt\n", rdev->desc->node_name);
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	rdev->phandle = fdt_get_phandle(fdt, node);
+
+	cuint = fdt_getprop(fdt, node, "regulator-min-microvolt", NULL);
+	if (cuint != NULL) {
+		uint16_t min_mv;
+
+		min_mv = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
+		VERBOSE("%s: min_mv=%d\n", rdev->desc->node_name, (int)min_mv);
+		if (min_mv <= rdev->max_mv) {
+			rdev->min_mv = min_mv;
+		} else {
+			ERROR("%s: min_mv=%d is too high\n",
+			      rdev->desc->node_name, (int)min_mv);
+			return -EINVAL;
+		}
+	}
+
+	cuint = fdt_getprop(fdt, node, "regulator-max-microvolt", NULL);
+	if (cuint != NULL) {
+		uint16_t max_mv;
+
+		max_mv = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
+		VERBOSE("%s: max_mv=%d\n", rdev->desc->node_name, (int)max_mv);
+		if (max_mv >= rdev->min_mv) {
+			rdev->max_mv = max_mv;
+		} else {
+			ERROR("%s: max_mv=%d is too low\n",
+			      rdev->desc->node_name, (int)max_mv);
+			return -EINVAL;
+		}
+	}
+
+	/* validate that min and max values can be used */
+	ret = regulator_list_voltages(rdev, &levels, &size);
+	if ((ret != 0) && (ret != -ENODEV)) {
+		return ret;
+	}
+
+	ret = parse_properties(fdt, rdev, node);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Register a regulator driver in regulator framework.
+ * Initialize voltage range from driver description
+ *
+ * @desc - pointer to the regulator description
+ * @node - device-tree node offset of the regulator
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_register(const struct regul_description *desc, int node)
+{
+	struct rdev *rdev;
+
+	assert(desc != NULL);
+
+	VERBOSE("register %s\n", desc->node_name);
+
+	for_each_rdev(rdev) {
+		if (rdev->desc == NULL) {
+			break;
+		}
+	}
+
+	if (rdev == rdev_array + PLAT_NB_RDEVS) {
+		WARN("Not enough place for regulators, PLAT_NB_RDEVS should be increased.\n");
+		return -ENOMEM;
+	}
+
+	rdev->desc = desc;
+	rdev->enable_ramp_delay = rdev->desc->enable_ramp_delay;
+
+	if (rdev->desc->ops->list_voltages != NULL) {
+		int ret;
+		const uint16_t *levels;
+		size_t count;
+
+		lock_driver(rdev);
+
+		ret = rdev->desc->ops->list_voltages(rdev->desc, &levels, &count);
+
+		unlock_driver(rdev);
+
+		if (ret < 0) {
+			ERROR("regul %s set state failed: err:%d\n",
+			      rdev->desc->node_name, ret);
+			return ret;
+		}
+
+		rdev->min_mv = levels[0];
+		rdev->max_mv = levels[count - 1U];
+	} else {
+		rdev->max_mv = UINT16_MAX;
+	}
+
+	return parse_dt(rdev, node);
+}
diff --git a/drivers/st/regulator/regulator_fixed.c b/drivers/st/regulator/regulator_fixed.c
new file mode 100644
index 0000000..f1c224e
--- /dev/null
+++ b/drivers/st/regulator/regulator_fixed.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/st/regulator.h>
+#include <drivers/st/regulator_fixed.h>
+#include <libfdt.h>
+
+#ifndef PLAT_NB_FIXED_REGS
+#error "Missing PLAT_NB_FIXED_REGS"
+#endif
+
+#define FIXED_NAME_LEN 32
+
+struct fixed_data {
+	char name[FIXED_NAME_LEN];
+	uint16_t volt;
+	struct regul_description desc;
+};
+
+static struct fixed_data data[PLAT_NB_FIXED_REGS];
+
+static int fixed_set_state(const struct regul_description *desc, bool state)
+{
+	return 0;
+}
+
+static int fixed_get_state(const struct regul_description *desc)
+{
+	return 1;
+}
+
+static struct regul_ops fixed_ops = {
+	.set_state = fixed_set_state,
+	.get_state = fixed_get_state,
+};
+
+int fixed_regulator_register(void)
+{
+	uint32_t count = 0;
+	void *fdt;
+	int node;
+
+	VERBOSE("fixed reg init!\n");
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	fdt_for_each_compatible_node(fdt, node, "regulator-fixed") {
+		int len __unused;
+		int ret;
+		struct fixed_data *d = &data[count];
+		const char *reg_name;
+
+		reg_name = fdt_get_name(fdt, node, NULL);
+
+		VERBOSE("register fixed reg %s!\n", reg_name);
+
+		len = snprintf(d->name, FIXED_NAME_LEN - 1, "%s", reg_name);
+		assert((len > 0) && (len < (FIXED_NAME_LEN - 1)));
+
+		d->desc.node_name = d->name;
+		d->desc.driver_data = d;
+		d->desc.ops = &fixed_ops;
+
+		ret = regulator_register(&d->desc, node);
+		if (ret != 0) {
+			WARN("%s:%d failed to register %s\n", __func__,
+			     __LINE__, reg_name);
+			return ret;
+		}
+
+		count++;
+		assert(count <= PLAT_NB_FIXED_REGS);
+
+	}
+
+	return 0;
+}
diff --git a/drivers/st/reset/stm32mp1_reset.c b/drivers/st/reset/stm32mp1_reset.c
new file mode 100644
index 0000000..98c8dcf
--- /dev/null
+++ b/drivers/st/reset/stm32mp1_reset.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <limits.h>
+
+#include <platform_def.h>
+
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+
+static uint32_t id2reg_offset(unsigned int reset_id)
+{
+	return ((reset_id & GENMASK(31, 5)) >> 5) * sizeof(uint32_t);
+}
+
+static uint8_t id2reg_bit_pos(unsigned int reset_id)
+{
+	return (uint8_t)(reset_id & GENMASK(4, 0));
+}
+
+int stm32mp_reset_assert(uint32_t id, unsigned int to_us)
+{
+	uint32_t offset = id2reg_offset(id);
+	uint32_t bitmsk = BIT(id2reg_bit_pos(id));
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	mmio_write_32(rcc_base + offset, bitmsk);
+
+	if (to_us != 0U) {
+		uint64_t timeout_ref = timeout_init_us(to_us);
+
+		while ((mmio_read_32(rcc_base + offset) & bitmsk) == 0U) {
+			if (timeout_elapsed(timeout_ref)) {
+				return -ETIMEDOUT;
+			}
+		}
+	}
+
+	return 0;
+}
+
+int stm32mp_reset_deassert(uint32_t id, unsigned int to_us)
+{
+	uint32_t offset = id2reg_offset(id) + RCC_RSTCLRR_OFFSET;
+	uint32_t bitmsk = BIT(id2reg_bit_pos(id));
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	mmio_write_32(rcc_base + offset, bitmsk);
+
+	if (to_us != 0U) {
+		uint64_t timeout_ref = timeout_init_us(to_us);
+
+		while ((mmio_read_32(rcc_base + offset) & bitmsk) != 0U) {
+			if (timeout_elapsed(timeout_ref)) {
+				return -ETIMEDOUT;
+			}
+		}
+	}
+
+	return 0;
+}
diff --git a/drivers/st/spi/stm32_qspi.c b/drivers/st/spi/stm32_qspi.c
new file mode 100644
index 0000000..73aa9ac
--- /dev/null
+++ b/drivers/st/spi/stm32_qspi.c
@@ -0,0 +1,508 @@
+/*
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <inttypes.h>
+
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/spi_mem.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32_qspi.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+/* Timeout for device interface reset */
+#define TIMEOUT_US_1_MS			1000U
+
+/* QUADSPI registers */
+#define QSPI_CR			0x00U
+#define QSPI_DCR		0x04U
+#define QSPI_SR			0x08U
+#define QSPI_FCR		0x0CU
+#define QSPI_DLR		0x10U
+#define QSPI_CCR		0x14U
+#define QSPI_AR			0x18U
+#define QSPI_ABR		0x1CU
+#define QSPI_DR			0x20U
+#define QSPI_PSMKR		0x24U
+#define QSPI_PSMAR		0x28U
+#define QSPI_PIR		0x2CU
+#define QSPI_LPTR		0x30U
+
+/* QUADSPI control register */
+#define QSPI_CR_EN		BIT(0)
+#define QSPI_CR_ABORT		BIT(1)
+#define QSPI_CR_DMAEN		BIT(2)
+#define QSPI_CR_TCEN		BIT(3)
+#define QSPI_CR_SSHIFT		BIT(4)
+#define QSPI_CR_DFM		BIT(6)
+#define QSPI_CR_FSEL		BIT(7)
+#define QSPI_CR_FTHRES_SHIFT	8U
+#define QSPI_CR_TEIE		BIT(16)
+#define QSPI_CR_TCIE		BIT(17)
+#define QSPI_CR_FTIE		BIT(18)
+#define QSPI_CR_SMIE		BIT(19)
+#define QSPI_CR_TOIE		BIT(20)
+#define QSPI_CR_APMS		BIT(22)
+#define QSPI_CR_PMM		BIT(23)
+#define QSPI_CR_PRESCALER_MASK	GENMASK_32(31, 24)
+#define QSPI_CR_PRESCALER_SHIFT	24U
+
+/* QUADSPI device configuration register */
+#define QSPI_DCR_CKMODE		BIT(0)
+#define QSPI_DCR_CSHT_MASK	GENMASK_32(10, 8)
+#define QSPI_DCR_CSHT_SHIFT	8U
+#define QSPI_DCR_FSIZE_MASK	GENMASK_32(20, 16)
+#define QSPI_DCR_FSIZE_SHIFT	16U
+
+/* QUADSPI status register */
+#define QSPI_SR_TEF		BIT(0)
+#define QSPI_SR_TCF		BIT(1)
+#define QSPI_SR_FTF		BIT(2)
+#define QSPI_SR_SMF		BIT(3)
+#define QSPI_SR_TOF		BIT(4)
+#define QSPI_SR_BUSY		BIT(5)
+
+/* QUADSPI flag clear register */
+#define QSPI_FCR_CTEF		BIT(0)
+#define QSPI_FCR_CTCF		BIT(1)
+#define QSPI_FCR_CSMF		BIT(3)
+#define QSPI_FCR_CTOF		BIT(4)
+
+/* QUADSPI communication configuration register */
+#define QSPI_CCR_DDRM		BIT(31)
+#define QSPI_CCR_DHHC		BIT(30)
+#define QSPI_CCR_SIOO		BIT(28)
+#define QSPI_CCR_FMODE_SHIFT	26U
+#define QSPI_CCR_DMODE_SHIFT	24U
+#define QSPI_CCR_DCYC_SHIFT	18U
+#define QSPI_CCR_ABSIZE_SHIFT	16U
+#define QSPI_CCR_ABMODE_SHIFT	14U
+#define QSPI_CCR_ADSIZE_SHIFT	12U
+#define QSPI_CCR_ADMODE_SHIFT	10U
+#define QSPI_CCR_IMODE_SHIFT	8U
+#define QSPI_CCR_IND_WRITE	0U
+#define QSPI_CCR_IND_READ	1U
+#define QSPI_CCR_MEM_MAP	3U
+
+#define QSPI_MAX_CHIP		2U
+
+#define QSPI_FIFO_TIMEOUT_US	30U
+#define QSPI_CMD_TIMEOUT_US	1000U
+#define QSPI_BUSY_TIMEOUT_US	100U
+#define QSPI_ABT_TIMEOUT_US	100U
+
+#define DT_QSPI_COMPAT		"st,stm32f469-qspi"
+
+#define FREQ_100MHZ		100000000U
+
+struct stm32_qspi_ctrl {
+	uintptr_t reg_base;
+	uintptr_t mm_base;
+	size_t mm_size;
+	unsigned long clock_id;
+	unsigned int reset_id;
+};
+
+static struct stm32_qspi_ctrl stm32_qspi;
+
+static uintptr_t qspi_base(void)
+{
+	return stm32_qspi.reg_base;
+}
+
+static int stm32_qspi_wait_for_not_busy(void)
+{
+	uint64_t timeout = timeout_init_us(QSPI_BUSY_TIMEOUT_US);
+
+	while ((mmio_read_32(qspi_base() + QSPI_SR) & QSPI_SR_BUSY) != 0U) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("%s: busy timeout\n", __func__);
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32_qspi_wait_cmd(const struct spi_mem_op *op)
+{
+	int ret = 0;
+	uint64_t timeout;
+
+	timeout = timeout_init_us(QSPI_CMD_TIMEOUT_US);
+	while ((mmio_read_32(qspi_base() + QSPI_SR) & QSPI_SR_TCF) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			ret = -ETIMEDOUT;
+			break;
+		}
+	}
+
+	if (ret == 0) {
+		if ((mmio_read_32(qspi_base() + QSPI_SR) & QSPI_SR_TEF) != 0U) {
+			ERROR("%s: transfer error\n", __func__);
+			ret = -EIO;
+		}
+	} else {
+		ERROR("%s: cmd timeout\n", __func__);
+	}
+
+	/* Clear flags */
+	mmio_write_32(qspi_base() + QSPI_FCR, QSPI_FCR_CTCF | QSPI_FCR_CTEF);
+
+	if (ret == 0) {
+		ret = stm32_qspi_wait_for_not_busy();
+	}
+
+	return ret;
+}
+
+static void stm32_qspi_read_fifo(uint8_t *val, uintptr_t addr)
+{
+	*val = mmio_read_8(addr);
+}
+
+static void stm32_qspi_write_fifo(uint8_t *val, uintptr_t addr)
+{
+	mmio_write_8(addr, *val);
+}
+
+static int stm32_qspi_poll(const struct spi_mem_op *op)
+{
+	void (*fifo)(uint8_t *val, uintptr_t addr);
+	uint32_t len;
+	uint8_t *buf;
+
+	if (op->data.dir == SPI_MEM_DATA_IN) {
+		fifo = stm32_qspi_read_fifo;
+	} else {
+		fifo = stm32_qspi_write_fifo;
+	}
+
+	buf = (uint8_t *)op->data.buf;
+
+	for (len = op->data.nbytes; len != 0U; len--) {
+		uint64_t timeout = timeout_init_us(QSPI_FIFO_TIMEOUT_US);
+
+		while ((mmio_read_32(qspi_base() + QSPI_SR) &
+			QSPI_SR_FTF) == 0U) {
+			if (timeout_elapsed(timeout)) {
+				ERROR("%s: fifo timeout\n", __func__);
+				return -ETIMEDOUT;
+			}
+		}
+
+		fifo(buf++, qspi_base() + QSPI_DR);
+	}
+
+	return 0;
+}
+
+static int stm32_qspi_mm(const struct spi_mem_op *op)
+{
+	memcpy(op->data.buf,
+	       (void *)(stm32_qspi.mm_base + (size_t)op->addr.val),
+	       op->data.nbytes);
+
+	return 0;
+}
+
+static int stm32_qspi_tx(const struct spi_mem_op *op, uint8_t mode)
+{
+	if (op->data.nbytes == 0U) {
+		return 0;
+	}
+
+	if (mode == QSPI_CCR_MEM_MAP) {
+		return stm32_qspi_mm(op);
+	}
+
+	return stm32_qspi_poll(op);
+}
+
+static unsigned int stm32_qspi_get_mode(uint8_t buswidth)
+{
+	if (buswidth == 4U) {
+		return 3U;
+	}
+
+	return buswidth;
+}
+
+static int stm32_qspi_exec_op(const struct spi_mem_op *op)
+{
+	uint64_t timeout;
+	uint32_t ccr;
+	size_t addr_max;
+	uint8_t mode = QSPI_CCR_IND_WRITE;
+	int ret;
+
+	VERBOSE("%s: cmd:%x mode:%d.%d.%d.%d addr:%" PRIx64 " len:%x\n",
+		__func__, op->cmd.opcode, op->cmd.buswidth, op->addr.buswidth,
+		op->dummy.buswidth, op->data.buswidth,
+		op->addr.val, op->data.nbytes);
+
+	addr_max = op->addr.val + op->data.nbytes + 1U;
+
+	if ((op->data.dir == SPI_MEM_DATA_IN) && (op->data.nbytes != 0U)) {
+		if ((addr_max < stm32_qspi.mm_size) &&
+		    (op->addr.buswidth != 0U)) {
+			mode = QSPI_CCR_MEM_MAP;
+		} else {
+			mode = QSPI_CCR_IND_READ;
+		}
+	}
+
+	if (op->data.nbytes != 0U) {
+		mmio_write_32(qspi_base() + QSPI_DLR, op->data.nbytes - 1U);
+	}
+
+	ccr = mode << QSPI_CCR_FMODE_SHIFT;
+	ccr |= op->cmd.opcode;
+	ccr |= stm32_qspi_get_mode(op->cmd.buswidth) << QSPI_CCR_IMODE_SHIFT;
+
+	if (op->addr.nbytes != 0U) {
+		ccr |= (op->addr.nbytes - 1U) << QSPI_CCR_ADSIZE_SHIFT;
+		ccr |= stm32_qspi_get_mode(op->addr.buswidth) <<
+			QSPI_CCR_ADMODE_SHIFT;
+	}
+
+	if ((op->dummy.buswidth != 0U) && (op->dummy.nbytes != 0U)) {
+		ccr |= (op->dummy.nbytes * 8U / op->dummy.buswidth) <<
+			QSPI_CCR_DCYC_SHIFT;
+	}
+
+	if (op->data.nbytes != 0U) {
+		ccr |= stm32_qspi_get_mode(op->data.buswidth) <<
+			QSPI_CCR_DMODE_SHIFT;
+	}
+
+	mmio_write_32(qspi_base() + QSPI_CCR, ccr);
+
+	if ((op->addr.nbytes != 0U) && (mode != QSPI_CCR_MEM_MAP)) {
+		mmio_write_32(qspi_base() + QSPI_AR, op->addr.val);
+	}
+
+	ret = stm32_qspi_tx(op, mode);
+
+	/*
+	 * Abort in:
+	 * - Error case.
+	 * - Memory mapped read: prefetching must be stopped if we read the last
+	 *   byte of device (device size - fifo size). If device size is not
+	 *   known then prefetching is always stopped.
+	 */
+	if ((ret != 0) || (mode == QSPI_CCR_MEM_MAP)) {
+		goto abort;
+	}
+
+	/* Wait end of TX in indirect mode */
+	ret = stm32_qspi_wait_cmd(op);
+	if (ret != 0) {
+		goto abort;
+	}
+
+	return 0;
+
+abort:
+	mmio_setbits_32(qspi_base() + QSPI_CR, QSPI_CR_ABORT);
+
+	/* Wait clear of abort bit by hardware */
+	timeout = timeout_init_us(QSPI_ABT_TIMEOUT_US);
+	while ((mmio_read_32(qspi_base() + QSPI_CR) & QSPI_CR_ABORT) != 0U) {
+		if (timeout_elapsed(timeout)) {
+			ret = -ETIMEDOUT;
+			break;
+		}
+	}
+
+	mmio_write_32(qspi_base() + QSPI_FCR, QSPI_FCR_CTCF);
+
+	if (ret != 0) {
+		ERROR("%s: exec op error\n", __func__);
+	}
+
+	return ret;
+}
+
+static int stm32_qspi_claim_bus(unsigned int cs)
+{
+	uint32_t cr;
+
+	if (cs >= QSPI_MAX_CHIP) {
+		return -ENODEV;
+	}
+
+	/* Set chip select and enable the controller */
+	cr = QSPI_CR_EN;
+	if (cs == 1U) {
+		cr |= QSPI_CR_FSEL;
+	}
+
+	mmio_clrsetbits_32(qspi_base() + QSPI_CR, QSPI_CR_FSEL, cr);
+
+	return 0;
+}
+
+static void stm32_qspi_release_bus(void)
+{
+	mmio_clrbits_32(qspi_base() + QSPI_CR, QSPI_CR_EN);
+}
+
+static int stm32_qspi_set_speed(unsigned int hz)
+{
+	unsigned long qspi_clk = clk_get_rate(stm32_qspi.clock_id);
+	uint32_t prescaler = UINT8_MAX;
+	uint32_t csht;
+	int ret;
+
+	if (qspi_clk == 0U) {
+		return -EINVAL;
+	}
+
+	if (hz > 0U) {
+		prescaler = div_round_up(qspi_clk, hz) - 1U;
+		if (prescaler > UINT8_MAX) {
+			prescaler = UINT8_MAX;
+		}
+	}
+
+	csht = div_round_up((5U * qspi_clk) / (prescaler + 1U), FREQ_100MHZ);
+	csht = ((csht - 1U) << QSPI_DCR_CSHT_SHIFT) & QSPI_DCR_CSHT_MASK;
+
+	ret = stm32_qspi_wait_for_not_busy();
+	if (ret != 0) {
+		return ret;
+	}
+
+	mmio_clrsetbits_32(qspi_base() + QSPI_CR, QSPI_CR_PRESCALER_MASK,
+			   prescaler << QSPI_CR_PRESCALER_SHIFT);
+
+	mmio_clrsetbits_32(qspi_base() + QSPI_DCR, QSPI_DCR_CSHT_MASK, csht);
+
+	VERBOSE("%s: speed=%lu\n", __func__, qspi_clk / (prescaler + 1U));
+
+	return 0;
+}
+
+static int stm32_qspi_set_mode(unsigned int mode)
+{
+	int ret;
+
+	ret = stm32_qspi_wait_for_not_busy();
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((mode & SPI_CS_HIGH) != 0U) {
+		return -ENODEV;
+	}
+
+	if (((mode & SPI_CPHA) != 0U) && ((mode & SPI_CPOL) != 0U)) {
+		mmio_setbits_32(qspi_base() + QSPI_DCR, QSPI_DCR_CKMODE);
+	} else if (((mode & SPI_CPHA) == 0U) && ((mode & SPI_CPOL) == 0U)) {
+		mmio_clrbits_32(qspi_base() + QSPI_DCR, QSPI_DCR_CKMODE);
+	} else {
+		return -ENODEV;
+	}
+
+	VERBOSE("%s: mode=0x%x\n", __func__, mode);
+
+	if ((mode & SPI_RX_QUAD) != 0U) {
+		VERBOSE("rx: quad\n");
+	} else if ((mode & SPI_RX_DUAL) != 0U) {
+		VERBOSE("rx: dual\n");
+	} else {
+		VERBOSE("rx: single\n");
+	}
+
+	if ((mode & SPI_TX_QUAD) != 0U) {
+		VERBOSE("tx: quad\n");
+	} else if ((mode & SPI_TX_DUAL) != 0U) {
+		VERBOSE("tx: dual\n");
+	} else {
+		VERBOSE("tx: single\n");
+	}
+
+	return 0;
+}
+
+static const struct spi_bus_ops stm32_qspi_bus_ops = {
+	.claim_bus = stm32_qspi_claim_bus,
+	.release_bus = stm32_qspi_release_bus,
+	.set_speed = stm32_qspi_set_speed,
+	.set_mode = stm32_qspi_set_mode,
+	.exec_op = stm32_qspi_exec_op,
+};
+
+int stm32_qspi_init(void)
+{
+	size_t size;
+	int qspi_node;
+	struct dt_node_info info;
+	void *fdt = NULL;
+	int ret;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	qspi_node = dt_get_node(&info, -1, DT_QSPI_COMPAT);
+	if (qspi_node < 0) {
+		ERROR("No QSPI ctrl found\n");
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if (info.status == DT_DISABLED) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	ret = fdt_get_reg_props_by_name(fdt, qspi_node, "qspi",
+					&stm32_qspi.reg_base, &size);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = fdt_get_reg_props_by_name(fdt, qspi_node, "qspi_mm",
+					&stm32_qspi.mm_base,
+					&stm32_qspi.mm_size);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (dt_set_pinctrl_config(qspi_node) != 0) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	if ((info.clock < 0) || (info.reset < 0)) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	stm32_qspi.clock_id = (unsigned long)info.clock;
+	stm32_qspi.reset_id = (unsigned int)info.reset;
+
+	clk_enable(stm32_qspi.clock_id);
+
+	ret = stm32mp_reset_assert(stm32_qspi.reset_id, TIMEOUT_US_1_MS);
+	if (ret != 0) {
+		panic();
+	}
+	ret = stm32mp_reset_deassert(stm32_qspi.reset_id, TIMEOUT_US_1_MS);
+	if (ret != 0) {
+		panic();
+	}
+
+	mmio_write_32(qspi_base() + QSPI_CR, QSPI_CR_SSHIFT);
+	mmio_write_32(qspi_base() + QSPI_DCR, QSPI_DCR_FSIZE_MASK);
+
+	return spi_mem_init_slave(fdt, qspi_node, &stm32_qspi_bus_ops);
+};
diff --git a/drivers/st/uart/aarch32/stm32_console.S b/drivers/st/uart/aarch32/stm32_console.S
new file mode 100644
index 0000000..e3e0e67
--- /dev/null
+++ b/drivers/st/uart/aarch32/stm32_console.S
@@ -0,0 +1,261 @@
+/*
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <asm_macros.S>
+#include <assert_macros.S>
+#include <console_macros.S>
+#include <drivers/st/stm32_console.h>
+#include <drivers/st/stm32_uart_regs.h>
+
+#define USART_TIMEOUT		0x1000
+
+	/*
+	 * "core" functions are low-level implementations that don't require
+	 * writeable memory and are thus safe to call in BL1 crash context.
+	 */
+	.globl	console_stm32_core_init
+	.globl	console_stm32_core_putc
+	.globl	console_stm32_core_getc
+	.globl	console_stm32_core_flush
+
+	.globl	console_stm32_putc
+	.globl	console_stm32_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_stm32_core_init
+	/* Check the input base address */
+	cmp	r0, #0
+	beq	core_init_fail
+#if !defined(IMAGE_BL2)
+#if STM32MP_RECONFIGURE_CONSOLE
+	/* UART clock rate is set to 0 in BL32, skip init in that case */
+	cmp	r1, #0
+	beq	1f
+#else /* STM32MP_RECONFIGURE_CONSOLE */
+	/* Skip UART initialization if it is already enabled */
+	ldr	r3, [r0, #USART_CR1]
+	ands	r3, r3, #USART_CR1_UE
+	bne	1f
+#endif /* STM32MP_RECONFIGURE_CONSOLE */
+#endif /* IMAGE_BL2 */
+	/* Check baud rate and uart clock for sanity */
+	cmp	r1, #0
+	beq	core_init_fail
+	cmp	r2, #0
+	beq	core_init_fail
+	/* Disable UART */
+	ldr	r3, [r0, #USART_CR1]
+	bic	r3, r3, #USART_CR1_UE
+	str	r3, [r0, #USART_CR1]
+	/* Configure UART */
+	orr	r3, r3, #(USART_CR1_TE | USART_CR1_FIFOEN)
+	str	r3, [r0, #USART_CR1]
+	ldr	r3, [r0, #USART_CR2]
+	bic	r3, r3, #USART_CR2_STOP
+	str	r3, [r0, #USART_CR2]
+	/* Divisor =  (Uart clock + (baudrate / 2)) / baudrate */
+	lsr	r3, r2, #1
+	add	r3, r1, r3
+	udiv	r3, r3, r2
+	cmp	r3, #16
+	bhi	2f
+	/* Oversampling 8 */
+	/* Divisor =  (2 * Uart clock + (baudrate / 2)) / baudrate */
+	lsr	r3, r2, #1
+	add	r3, r3, r1, lsl #1
+	udiv	r3, r3, r2
+	and	r1, r3, #USART_BRR_DIV_FRACTION
+	lsr	r1, r1, #1
+	bic	r3, r3, #USART_BRR_DIV_FRACTION
+	orr	r3, r3, r1
+	ldr	r1, [r0, #USART_CR1]
+	orr	r1, r1, #USART_CR1_OVER8
+	str	r1, [r0, #USART_CR1]
+2:
+	str	r3, [r0, #USART_BRR]
+	/* Enable UART */
+	ldr	r3, [r0, #USART_CR1]
+	orr	r3, r3, #USART_CR1_UE
+	str	r3, [r0, #USART_CR1]
+	/* Check TEACK bit */
+	mov	r2, #USART_TIMEOUT
+teack_loop:
+	subs	r2, r2, #1
+	beq	core_init_fail
+	ldr	r3, [r0, #USART_ISR]
+	tst	r3, #USART_ISR_TEACK
+	beq	teack_loop
+1:
+	mov	r0, #1
+	bx	lr
+core_init_fail:
+	mov	r0, #0
+	bx	lr
+endfunc console_stm32_core_init
+
+	.globl console_stm32_register
+
+	/* -------------------------------------------------------
+	 * int console_stm32_register(uintptr_t baseaddr,
+	 *     uint32_t clock, uint32_t baud,
+	 *     console_t *console);
+	 * Function to initialize and register a new STM32
+	 * 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_stm32_register
+	push	{r4, lr}
+	mov	r4, r3
+	cmp	r4, #0
+	beq	register_fail
+	str	r0, [r4, #CONSOLE_T_BASE]
+
+	bl console_stm32_core_init
+	cmp	r0, #0
+	beq	register_fail
+
+	mov	r0, r4
+	pop	{r4, lr}
+	finish_console_register stm32 putc=1, getc=0, flush=1
+
+register_fail:
+	pop	{r4, pc}
+endfunc console_stm32_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_stm32_core_putc
+	/* Check the input parameter */
+	cmp	r1, #0
+	beq	putc_error
+
+	/* Check Transmit Data Register Empty */
+txe_loop:
+	ldr	r2, [r1, #USART_ISR]
+	tst	r2, #USART_ISR_TXE
+	beq	txe_loop
+	str	r0, [r1, #USART_TDR]
+	/* Check transmit complete flag */
+tc_loop:
+	ldr	r2, [r1, #USART_ISR]
+	tst	r2, #USART_ISR_TC
+	beq	tc_loop
+	bx	lr
+putc_error:
+	mov	r0, #-1
+	bx	lr
+endfunc console_stm32_core_putc
+
+	/* ------------------------------------------------------------
+	 * int console_stm32_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_stm32_putc
+#if ENABLE_ASSERTIONS
+	cmp	r1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	r1, [r1, #CONSOLE_T_BASE]
+	b	console_stm32_core_putc
+endfunc console_stm32_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
+	 * Out : return -1.
+	 * Clobber list : r0, r1
+	 * -----------------------------------------------------------
+	 */
+func console_stm32_core_getc
+	/* Not supported */
+	mov	r0, #-1
+	bx	lr
+endfunc console_stm32_core_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_stm32_core_flush
+#if ENABLE_ASSERTIONS
+	cmp	r0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	/* Check Transmit Data Register Empty */
+txe_loop_3:
+	ldr	r1, [r0, #USART_ISR]
+	tst	r1, #USART_ISR_TXE
+	beq	txe_loop_3
+	bx	lr
+endfunc console_stm32_core_flush
+
+	/* ------------------------------------------------------
+	 * void console_stm32_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_stm32_flush
+#if ENABLE_ASSERTIONS
+	cmp	r0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	r0, [r0, #CONSOLE_T_BASE]
+	b	console_stm32_core_flush
+endfunc console_stm32_flush
diff --git a/drivers/st/uart/stm32_uart.c b/drivers/st/uart/stm32_uart.c
new file mode 100644
index 0000000..63970c7
--- /dev/null
+++ b/drivers/st/uart/stm32_uart.c
@@ -0,0 +1,439 @@
+/*
+ * Copyright (c) 2021-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <common/bl_common.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32_uart.h>
+#include <drivers/st/stm32_uart_regs.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+/* UART time-out value */
+#define STM32_UART_TIMEOUT_US	20000U
+
+/* Mask to clear ALL the configuration registers */
+
+#define STM32_UART_CR1_FIELDS \
+		(USART_CR1_M | USART_CR1_PCE | USART_CR1_PS | USART_CR1_TE | \
+		 USART_CR1_RE | USART_CR1_OVER8 | USART_CR1_FIFOEN)
+
+#define STM32_UART_CR2_FIELDS \
+		(USART_CR2_SLVEN | USART_CR2_DIS_NSS | USART_CR2_ADDM7 | \
+		 USART_CR2_LBDL | USART_CR2_LBDIE | USART_CR2_LBCL | \
+		 USART_CR2_CPHA | USART_CR2_CPOL | USART_CR2_CLKEN | \
+		 USART_CR2_STOP | USART_CR2_LINEN | USART_CR2_SWAP | \
+		 USART_CR2_RXINV | USART_CR2_TXINV | USART_CR2_DATAINV | \
+		 USART_CR2_MSBFIRST | USART_CR2_ABREN | USART_CR2_ABRMODE | \
+		 USART_CR2_RTOEN | USART_CR2_ADD)
+
+#define STM32_UART_CR3_FIELDS \
+		(USART_CR3_EIE | USART_CR3_IREN | USART_CR3_IRLP | \
+		 USART_CR3_HDSEL | USART_CR3_NACK | USART_CR3_SCEN | \
+		 USART_CR3_DMAR | USART_CR3_DMAT | USART_CR3_RTSE | \
+		 USART_CR3_CTSE | USART_CR3_CTSIE | USART_CR3_ONEBIT | \
+		 USART_CR3_OVRDIS | USART_CR3_DDRE | USART_CR3_DEM | \
+		 USART_CR3_DEP | USART_CR3_SCARCNT | USART_CR3_WUS | \
+		 USART_CR3_WUFIE | USART_CR3_TXFTIE | USART_CR3_TCBGTIE | \
+		 USART_CR3_RXFTCFG | USART_CR3_RXFTIE | USART_CR3_TXFTCFG)
+
+#define STM32_UART_ISR_ERRORS	 \
+		(USART_ISR_ORE | USART_ISR_NE |  USART_ISR_FE | USART_ISR_PE)
+
+static const uint16_t presc_table[STM32_UART_PRESCALER_NB] = {
+	1U, 2U, 4U, 6U, 8U, 10U, 12U, 16U, 32U, 64U, 128U, 256U
+};
+
+/* @brief  BRR division operation to set BRR register in 8-bit oversampling
+ * mode.
+ * @param  clockfreq: UART clock.
+ * @param  baud_rate: Baud rate set by the user.
+ * @param  prescaler: UART prescaler value.
+ * @retval Division result.
+ */
+static uint32_t uart_div_sampling8(unsigned long clockfreq,
+				   uint32_t baud_rate,
+				   uint32_t prescaler)
+{
+	uint32_t scaled_freq = clockfreq / presc_table[prescaler];
+
+	return ((scaled_freq * 2) + (baud_rate / 2)) / baud_rate;
+
+}
+
+/* @brief  BRR division operation to set BRR register in 16-bit oversampling
+ * mode.
+ * @param  clockfreq: UART clock.
+ * @param  baud_rate: Baud rate set by the user.
+ * @param  prescaler: UART prescaler value.
+ * @retval Division result.
+ */
+static uint32_t uart_div_sampling16(unsigned long clockfreq,
+				    uint32_t baud_rate,
+				    uint32_t prescaler)
+{
+	uint32_t scaled_freq = clockfreq / presc_table[prescaler];
+
+	return (scaled_freq + (baud_rate / 2)) / baud_rate;
+
+}
+
+/*
+ * @brief  Return the UART clock frequency.
+ * @param  huart: UART handle.
+ * @retval Frequency value in Hz.
+ */
+static unsigned long uart_get_clock_freq(struct stm32_uart_handle_s *huart)
+{
+	return fdt_get_uart_clock_freq((uintptr_t)huart->base);
+}
+
+/*
+ * @brief  Configure the UART peripheral.
+ * @param  huart: UART handle.
+ * @retval UART status.
+ */
+static int uart_set_config(struct stm32_uart_handle_s *huart,
+			   const struct stm32_uart_init_s *init)
+{
+	uint32_t tmpreg;
+	unsigned long clockfreq;
+	unsigned long int_div;
+	uint32_t brrtemp;
+	uint32_t over_sampling;
+
+	/*---------------------- USART BRR configuration --------------------*/
+	clockfreq = uart_get_clock_freq(huart);
+	if (clockfreq == 0UL) {
+		return -ENODEV;
+	}
+
+	int_div = clockfreq / init->baud_rate;
+	if (int_div < 16U) {
+		uint32_t usartdiv = uart_div_sampling8(clockfreq,
+						       init->baud_rate,
+						       init->prescaler);
+
+		brrtemp = (usartdiv & USART_BRR_DIV_MANTISSA) |
+			  ((usartdiv & USART_BRR_DIV_FRACTION) >> 1);
+		over_sampling = USART_CR1_OVER8;
+	} else {
+		brrtemp = uart_div_sampling16(clockfreq,
+					      init->baud_rate,
+					      init->prescaler) &
+			  (USART_BRR_DIV_FRACTION | USART_BRR_DIV_MANTISSA);
+		over_sampling = 0x0U;
+	}
+	mmio_write_32(huart->base + USART_BRR, brrtemp);
+
+	/*
+	 * ---------------------- USART CR1 Configuration --------------------
+	 * Clear M, PCE, PS, TE, RE and OVER8 bits and configure
+	 * the UART word length, parity, mode and oversampling:
+	 * - set the M bits according to init->word_length value,
+	 * - set PCE and PS bits according to init->parity value,
+	 * - set TE and RE bits according to init->mode value,
+	 * - set OVER8 bit according baudrate and clock.
+	 */
+	tmpreg = init->word_length |
+		 init->parity |
+		 init->mode |
+		 over_sampling |
+		 init->fifo_mode;
+	mmio_clrsetbits_32(huart->base + USART_CR1, STM32_UART_CR1_FIELDS, tmpreg);
+
+	/*
+	 * --------------------- USART CR2 Configuration ---------------------
+	 * Configure the UART Stop Bits: Set STOP[13:12] bits according
+	 * to init->stop_bits value.
+	 */
+	mmio_clrsetbits_32(huart->base + USART_CR2, STM32_UART_CR2_FIELDS,
+			   init->stop_bits);
+
+	/*
+	 * --------------------- USART CR3 Configuration ---------------------
+	 * Configure:
+	 * - UART HardWare Flow Control: set CTSE and RTSE bits according
+	 *   to init->hw_flow_control value,
+	 * - one-bit sampling method versus three samples' majority rule
+	 *   according to init->one_bit_sampling (not applicable to
+	 *   LPUART),
+	 * - set TXFTCFG bit according to init->tx_fifo_threshold value,
+	 * - set RXFTCFG bit according to init->rx_fifo_threshold value.
+	 */
+	tmpreg = init->hw_flow_control | init->one_bit_sampling;
+
+	if (init->fifo_mode == USART_CR1_FIFOEN) {
+		tmpreg |= init->tx_fifo_threshold |
+			  init->rx_fifo_threshold;
+	}
+
+	mmio_clrsetbits_32(huart->base + USART_CR3, STM32_UART_CR3_FIELDS, tmpreg);
+
+	/*
+	 * --------------------- USART PRESC Configuration -------------------
+	 * Configure UART Clock Prescaler : set PRESCALER according to
+	 * init->prescaler value.
+	 */
+	assert(init->prescaler < STM32_UART_PRESCALER_NB);
+	mmio_clrsetbits_32(huart->base + USART_PRESC, USART_PRESC_PRESCALER,
+			   init->prescaler);
+
+	return 0;
+}
+
+/*
+ * @brief  Handle UART communication timeout.
+ * @param  huart: UART handle.
+ * @param  flag: Specifies the UART flag to check.
+ * @retval UART status.
+ */
+static int stm32_uart_wait_flag(struct stm32_uart_handle_s *huart, uint32_t flag)
+{
+	uint64_t timeout_ref = timeout_init_us(STM32_UART_TIMEOUT_US);
+
+	while ((mmio_read_32(huart->base + USART_ISR) & flag) == 0U) {
+		if (timeout_elapsed(timeout_ref)) {
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  Check the UART idle State.
+ * @param  huart: UART handle.
+ * @retval UART status.
+ */
+static int stm32_uart_check_idle(struct stm32_uart_handle_s *huart)
+{
+	int ret;
+
+	/* Check if the transmitter is enabled */
+	if ((mmio_read_32(huart->base + USART_CR1) & USART_CR1_TE) == USART_CR1_TE) {
+		ret = stm32_uart_wait_flag(huart, USART_ISR_TEACK);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	/* Check if the receiver is enabled */
+	if ((mmio_read_32(huart->base + USART_CR1) & USART_CR1_RE) == USART_CR1_RE) {
+		ret = stm32_uart_wait_flag(huart, USART_ISR_REACK);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  Compute RDR register mask depending on word length.
+ * @param  huart: UART handle.
+ * @retval Mask value.
+ */
+static unsigned int stm32_uart_rdr_mask(const struct stm32_uart_init_s *init)
+{
+	unsigned int mask = 0U;
+
+	switch (init->word_length) {
+	case STM32_UART_WORDLENGTH_9B:
+		mask = GENMASK(8, 0);
+		break;
+	case STM32_UART_WORDLENGTH_8B:
+		mask = GENMASK(7, 0);
+		break;
+	case STM32_UART_WORDLENGTH_7B:
+		mask = GENMASK(6, 0);
+		break;
+	default:
+		break; /* not reached */
+	}
+
+	if (init->parity != STM32_UART_PARITY_NONE) {
+		mask >>= 1;
+	}
+
+	return mask;
+}
+
+/*
+ * @brief  Check interrupt and status errors.
+ * @retval True if error detected, false otherwise.
+ */
+static bool stm32_uart_error_detected(struct stm32_uart_handle_s *huart)
+{
+	return (mmio_read_32(huart->base + USART_ISR) & STM32_UART_ISR_ERRORS) != 0U;
+}
+
+/*
+ * @brief  Clear status errors.
+ */
+static void stm32_uart_error_clear(struct stm32_uart_handle_s *huart)
+{
+	mmio_write_32(huart->base + USART_ICR, STM32_UART_ISR_ERRORS);
+}
+
+/*
+ * @brief  Stop the UART.
+ * @param  base: UART base address.
+ */
+void stm32_uart_stop(uintptr_t base)
+{
+	mmio_clrbits_32(base + USART_CR1, USART_CR1_UE);
+}
+
+/*
+ * @brief  Initialize UART.
+ * @param  huart: UART handle.
+ * @param  base_addr: base address of UART.
+ * @param  init: UART initialization parameter.
+ * @retval UART status.
+ */
+int stm32_uart_init(struct stm32_uart_handle_s *huart,
+		    uintptr_t base_addr,
+		    const struct stm32_uart_init_s *init)
+{
+	int ret;
+	int uart_node;
+	int clk;
+	void *fdt = NULL;
+
+	if (huart == NULL || init == NULL || base_addr == 0U) {
+		return -EINVAL;
+	}
+
+	huart->base = base_addr;
+
+	/* Search UART instance in DT */
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if (fdt == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	uart_node = dt_match_instance_by_compatible(DT_UART_COMPAT, base_addr);
+	if (uart_node == -FDT_ERR_NOTFOUND) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	/* Pinctrl initialization */
+	if (dt_set_pinctrl_config(uart_node) != 0) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	/* Clock initialization */
+	clk = fdt_get_clock_id(uart_node);
+	if (clk < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+	clk_enable(clk);
+
+	/* Disable the peripheral */
+	stm32_uart_stop(huart->base);
+
+	/* Computation of UART mask to apply to RDR register */
+	huart->rdr_mask = stm32_uart_rdr_mask(init);
+
+	/* Init the peripheral */
+	ret = uart_set_config(huart, init);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Enable the peripheral */
+	mmio_setbits_32(huart->base + USART_CR1, USART_CR1_UE);
+
+	/* TEACK and/or REACK to check */
+	return stm32_uart_check_idle(huart);
+}
+
+/*
+ * @brief  Transmit one data in no blocking mode.
+ * @param  huart: UART handle.
+ * @param  c: data to sent.
+ * @retval UART status.
+ */
+int stm32_uart_putc(struct stm32_uart_handle_s *huart, int c)
+{
+	int ret;
+
+	if (huart == NULL) {
+		return -EINVAL;
+	}
+
+	ret = stm32_uart_wait_flag(huart, USART_ISR_TXE);
+	if (ret != 0) {
+		return ret;
+	}
+
+	mmio_write_32(huart->base + USART_TDR, c);
+	if (stm32_uart_error_detected(huart)) {
+		stm32_uart_error_clear(huart);
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  Flush TX Transmit fifo
+ * @param  huart: UART handle.
+ * @retval UART status.
+ */
+int stm32_uart_flush(struct stm32_uart_handle_s *huart)
+{
+	int ret;
+
+	if (huart == NULL) {
+		return -EINVAL;
+	}
+
+	ret = stm32_uart_wait_flag(huart, USART_ISR_TXE);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return stm32_uart_wait_flag(huart, USART_ISR_TC);
+}
+
+/*
+ * @brief  Receive a data in no blocking mode.
+ * @retval value if >0 or UART status.
+ */
+int stm32_uart_getc(struct stm32_uart_handle_s *huart)
+{
+	uint32_t data;
+
+	if (huart == NULL) {
+		return -EINVAL;
+	}
+
+	/* Check if data is available */
+	if ((mmio_read_32(huart->base + USART_ISR) & USART_ISR_RXNE) == 0U) {
+		return -EAGAIN;
+	}
+
+	data = mmio_read_32(huart->base + USART_RDR) & huart->rdr_mask;
+
+	if (stm32_uart_error_detected(huart)) {
+		stm32_uart_error_clear(huart);
+		return -EFAULT;
+	}
+
+	return (int)data;
+}
diff --git a/drivers/st/usb/stm32mp1_usb.c b/drivers/st/usb/stm32mp1_usb.c
new file mode 100644
index 0000000..9a49690
--- /dev/null
+++ b/drivers/st/usb/stm32mp1_usb.c
@@ -0,0 +1,1091 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp1_usb.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+#define USB_OTG_MODE_DEVICE			0U
+#define USB_OTG_MODE_HOST			1U
+#define USB_OTG_MODE_DRD			2U
+
+#define EP_TYPE_CTRL				0U
+#define EP_TYPE_ISOC				1U
+#define EP_TYPE_BULK				2U
+#define EP_TYPE_INTR				3U
+
+#define USBD_FIFO_FLUSH_TIMEOUT_US		1000U
+#define EP0_FIFO_SIZE				64U
+
+/* OTG registers offsets */
+#define OTG_GOTGINT				0x004U
+#define OTG_GAHBCFG				0x008U
+#define OTG_GUSBCFG				0x00CU
+#define OTG_GRSTCTL				0x010U
+#define OTG_GINTSTS				0x014U
+#define OTG_GINTMSK				0x018U
+#define OTG_GRXSTSP				0x020U
+#define OTG_GLPMCFG				0x054U
+#define OTG_DCFG				0x800U
+#define OTG_DCTL				0x804U
+#define OTG_DSTS				0x808U
+#define OTG_DIEPMSK				0x810U
+#define OTG_DOEPMSK				0x814U
+#define OTG_DAINT				0x818U
+#define OTG_DAINTMSK				0x81CU
+#define OTG_DIEPEMPMSK				0x834U
+
+/* Definitions for OTG_DIEPx registers */
+#define OTG_DIEP_BASE				0x900U
+#define OTG_DIEP_SIZE				0x20U
+#define OTG_DIEPCTL				0x00U
+#define OTG_DIEPINT				0x08U
+#define OTG_DIEPTSIZ				0x10U
+#define OTG_DIEPDMA				0x14U
+#define OTG_DTXFSTS				0x18U
+#define OTG_DIEP_MAX_NB				9U
+
+/* Definitions for OTG_DOEPx registers */
+#define OTG_DOEP_BASE				0xB00U
+#define OTG_DOEP_SIZE				0x20U
+#define OTG_DOEPCTL				0x00U
+#define OTG_DOEPINT				0x08U
+#define OTG_DOEPTSIZ				0x10U
+#define OTG_DOEPDMA				0x14U
+#define OTG_D0EP_MAX_NB				9U
+
+/* Definitions for OTG_DAINT registers */
+#define OTG_DAINT_OUT_MASK			GENMASK(31, 16)
+#define OTG_DAINT_OUT_SHIFT			16U
+#define OTG_DAINT_IN_MASK			GENMASK(15, 0)
+#define OTG_DAINT_IN_SHIFT			0U
+
+#define OTG_DAINT_EP0_IN			BIT(16)
+#define OTG_DAINT_EP0_OUT			BIT(0)
+
+/* Definitions for FIFOs */
+#define OTG_FIFO_BASE				0x1000U
+#define OTG_FIFO_SIZE				0x1000U
+
+/* Bit definitions for OTG_GOTGINT register */
+#define OTG_GOTGINT_SEDET			BIT(2)
+
+/* Bit definitions for OTG_GAHBCFG register */
+#define OTG_GAHBCFG_GINT			BIT(0)
+
+/* Bit definitions for OTG_GUSBCFG register */
+#define OTG_GUSBCFG_TRDT			GENMASK(13, 10)
+#define OTG_GUSBCFG_TRDT_SHIFT			10U
+
+#define USBD_HS_TRDT_VALUE			9U
+
+/* Bit definitions for OTG_GRSTCTL register */
+#define OTG_GRSTCTL_RXFFLSH			BIT(4)
+#define OTG_GRSTCTL_TXFFLSH			BIT(5)
+#define OTG_GRSTCTL_TXFNUM_SHIFT		6U
+
+/* Bit definitions for OTG_GINTSTS register */
+#define OTG_GINTSTS_CMOD			BIT(0)
+#define OTG_GINTSTS_MMIS			BIT(1)
+#define OTG_GINTSTS_OTGINT			BIT(2)
+#define OTG_GINTSTS_SOF				BIT(3)
+#define OTG_GINTSTS_RXFLVL			BIT(4)
+#define OTG_GINTSTS_USBSUSP			BIT(11)
+#define OTG_GINTSTS_USBRST			BIT(12)
+#define OTG_GINTSTS_ENUMDNE			BIT(13)
+#define OTG_GINTSTS_IEPINT			BIT(18)
+#define OTG_GINTSTS_OEPINT			BIT(19)
+#define OTG_GINTSTS_IISOIXFR			BIT(20)
+#define OTG_GINTSTS_IPXFR_INCOMPISOOUT		BIT(21)
+#define OTG_GINTSTS_LPMINT			BIT(27)
+#define OTG_GINTSTS_SRQINT			BIT(30)
+#define OTG_GINTSTS_WKUPINT			BIT(31)
+
+/* Bit definitions for OTG_GRXSTSP register */
+#define OTG_GRXSTSP_EPNUM			GENMASK(3, 0)
+#define OTG_GRXSTSP_BCNT			GENMASK(14, 4)
+#define OTG_GRXSTSP_BCNT_SHIFT			4U
+#define OTG_GRXSTSP_PKTSTS			GENMASK(20, 17)
+#define OTG_GRXSTSP_PKTSTS_SHIFT		17U
+
+#define STS_GOUT_NAK				1U
+#define STS_DATA_UPDT				2U
+#define STS_XFER_COMP				3U
+#define STS_SETUP_COMP				4U
+#define STS_SETUP_UPDT				6U
+
+/* Bit definitions for OTG_GLPMCFG register */
+#define OTG_GLPMCFG_BESL			GENMASK(5, 2)
+
+/* Bit definitions for OTG_DCFG register */
+#define OTG_DCFG_DAD				GENMASK(10, 4)
+#define OTG_DCFG_DAD_SHIFT			4U
+
+/* Bit definitions for OTG_DCTL register */
+#define OTG_DCTL_RWUSIG				BIT(0)
+#define OTG_DCTL_SDIS				BIT(1)
+#define OTG_DCTL_CGINAK				BIT(8)
+
+/* Bit definitions for OTG_DSTS register */
+#define OTG_DSTS_SUSPSTS			BIT(0)
+#define OTG_DSTS_ENUMSPD_MASK			GENMASK(2, 1)
+#define OTG_DSTS_FNSOF0				BIT(8)
+
+#define OTG_DSTS_ENUMSPD(val)			((val) << 1)
+#define OTG_DSTS_ENUMSPD_HS_PHY_30MHZ_OR_60MHZ	OTG_DSTS_ENUMSPD(0U)
+#define OTG_DSTS_ENUMSPD_FS_PHY_30MHZ_OR_60MHZ	OTG_DSTS_ENUMSPD(1U)
+#define OTG_DSTS_ENUMSPD_LS_PHY_6MHZ		OTG_DSTS_ENUMSPD(2U)
+#define OTG_DSTS_ENUMSPD_FS_PHY_48MHZ		OTG_DSTS_ENUMSPD(3U)
+
+/* Bit definitions for OTG_DIEPMSK register */
+#define OTG_DIEPMSK_XFRCM			BIT(0)
+#define OTG_DIEPMSK_EPDM			BIT(1)
+#define OTG_DIEPMSK_TOM				BIT(3)
+
+/* Bit definitions for OTG_DOEPMSK register */
+#define OTG_DOEPMSK_XFRCM			BIT(0)
+#define OTG_DOEPMSK_EPDM			BIT(1)
+#define OTG_DOEPMSK_STUPM			BIT(3)
+
+/* Bit definitions for OTG_DIEPCTLx registers */
+#define OTG_DIEPCTL_MPSIZ			GENMASK(10, 0)
+#define OTG_DIEPCTL_STALL			BIT(21)
+#define OTG_DIEPCTL_CNAK			BIT(26)
+#define OTG_DIEPCTL_SD0PID_SEVNFRM		BIT(28)
+#define OTG_DIEPCTL_SODDFRM			BIT(29)
+#define OTG_DIEPCTL_EPDIS			BIT(30)
+#define OTG_DIEPCTL_EPENA			BIT(31)
+
+/* Bit definitions for OTG_DIEPINTx registers */
+#define OTG_DIEPINT_XFRC			BIT(0)
+#define OTG_DIEPINT_EPDISD			BIT(1)
+#define OTG_DIEPINT_TOC				BIT(3)
+#define OTG_DIEPINT_ITTXFE			BIT(4)
+#define OTG_DIEPINT_INEPNE			BIT(6)
+#define OTG_DIEPINT_TXFE			BIT(7)
+#define OTG_DIEPINT_TXFE_SHIFT			7U
+
+#define OTG_DIEPINT_MASK			(BIT(13) | BIT(11) | GENMASK(9, 0))
+
+/* Bit definitions for OTG_DIEPTSIZx registers */
+#define OTG_DIEPTSIZ_XFRSIZ			GENMASK(18, 0)
+#define OTG_DIEPTSIZ_PKTCNT			GENMASK(28, 19)
+#define OTG_DIEPTSIZ_PKTCNT_SHIFT		19U
+#define OTG_DIEPTSIZ_MCNT_MASK			GENMASK(30, 29)
+#define OTG_DIEPTSIZ_MCNT_DATA0			BIT(29)
+
+#define OTG_DIEPTSIZ_PKTCNT_1			BIT(19)
+
+/* Bit definitions for OTG_DTXFSTSx registers */
+#define OTG_DTXFSTS_INEPTFSAV			GENMASK(15, 0)
+
+/* Bit definitions for OTG_DOEPCTLx registers */
+#define OTG_DOEPCTL_STALL			BIT(21)
+#define OTG_DOEPCTL_CNAK			BIT(26)
+#define OTG_DOEPCTL_SD0PID_SEVNFRM		BIT(28) /* other than endpoint 0 */
+#define OTG_DOEPCTL_SD1PID_SODDFRM		BIT(29) /* other than endpoint 0 */
+#define OTG_DOEPCTL_EPDIS			BIT(30)
+#define OTG_DOEPCTL_EPENA			BIT(31)
+
+/* Bit definitions for OTG_DOEPTSIZx registers */
+#define OTG_DOEPTSIZ_XFRSIZ			GENMASK(18, 0)
+#define OTG_DOEPTSIZ_PKTCNT			GENMASK(28, 19)
+#define OTG_DOEPTSIZ_RXDPID_STUPCNT		GENMASK(30, 29)
+
+/* Bit definitions for OTG_DOEPINTx registers */
+#define OTG_DOEPINT_XFRC			BIT(0)
+#define OTG_DOEPINT_STUP			BIT(3)
+#define OTG_DOEPINT_OTEPDIS			BIT(4)
+
+#define OTG_DOEPINT_MASK			(GENMASK(15, 12) | GENMASK(9, 8) | GENMASK(6, 0))
+
+#define EP_NB					15U
+#define EP_ALL					0x10U
+
+/*
+ * Flush TX FIFO.
+ * handle: PCD handle.
+ * num: FIFO number.
+ *	   This parameter can be a value from 1 to 15 or EP_ALL.
+ *	   EP_ALL= 0x10 means Flush all TX FIFOs
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_flush_tx_fifo(void *handle, uint32_t num)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint64_t timeout = timeout_init_us(USBD_FIFO_FLUSH_TIMEOUT_US);
+
+	mmio_write_32(usb_base_addr + OTG_GRSTCTL,
+		      OTG_GRSTCTL_TXFFLSH | (uint32_t)(num << OTG_GRSTCTL_TXFNUM_SHIFT));
+
+	while ((mmio_read_32(usb_base_addr + OTG_GRSTCTL) &
+		OTG_GRSTCTL_TXFFLSH) == OTG_GRSTCTL_TXFFLSH) {
+		if (timeout_elapsed(timeout)) {
+			return USBD_TIMEOUT;
+		}
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Flush RX FIFO.
+ * handle: PCD handle.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_flush_rx_fifo(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint64_t timeout = timeout_init_us(USBD_FIFO_FLUSH_TIMEOUT_US);
+
+	mmio_write_32(usb_base_addr + OTG_GRSTCTL, OTG_GRSTCTL_RXFFLSH);
+
+	while ((mmio_read_32(usb_base_addr + OTG_GRSTCTL) &
+		 OTG_GRSTCTL_RXFFLSH) == OTG_GRSTCTL_RXFFLSH) {
+		if (timeout_elapsed(timeout)) {
+			return USBD_TIMEOUT;
+		}
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Return the global USB interrupt status.
+ * handle: PCD handle.
+ * return: Interrupt register value.
+ */
+static uint32_t usb_dwc2_read_int(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return mmio_read_32(usb_base_addr + OTG_GINTSTS) &
+	       mmio_read_32(usb_base_addr + OTG_GINTMSK);
+}
+
+/*
+ * Return the USB device OUT endpoints interrupt.
+ * handle: PCD handle.
+ * return: Device OUT endpoint interrupts.
+ */
+static uint32_t usb_dwc2_all_out_ep_int(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return ((mmio_read_32(usb_base_addr + OTG_DAINT) &
+		 mmio_read_32(usb_base_addr + OTG_DAINTMSK)) &
+		OTG_DAINT_OUT_MASK) >> OTG_DAINT_OUT_SHIFT;
+}
+
+/*
+ * Return the USB device IN endpoints interrupt.
+ * handle: PCD handle.
+ * return: Device IN endpoint interrupts.
+ */
+static uint32_t usb_dwc2_all_in_ep_int(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return ((mmio_read_32(usb_base_addr + OTG_DAINT) &
+		 mmio_read_32(usb_base_addr + OTG_DAINTMSK)) &
+		OTG_DAINT_IN_MASK) >> OTG_DAINT_IN_SHIFT;
+}
+
+/*
+ * Return Device OUT EP interrupt register.
+ * handle: PCD handle.
+ * epnum: Endpoint number.
+ *         This parameter can be a value from 0 to 15.
+ * return: Device OUT EP Interrupt register.
+ */
+static uint32_t usb_dwc2_out_ep_int(void *handle, uint8_t epnum)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return mmio_read_32(usb_base_addr + OTG_DOEP_BASE +
+			    (epnum * OTG_DOEP_SIZE) + OTG_DOEPINT) &
+	       mmio_read_32(usb_base_addr + OTG_DOEPMSK);
+}
+
+/*
+ * Return Device IN EP interrupt register.
+ * handle: PCD handle.
+ * epnum: Endpoint number.
+ *         This parameter can be a value from 0 to 15.
+ * return: Device IN EP Interrupt register.
+ */
+static uint32_t usb_dwc2_in_ep_int(void *handle, uint8_t epnum)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t msk;
+	uint32_t emp;
+
+	msk = mmio_read_32(usb_base_addr + OTG_DIEPMSK);
+	emp = mmio_read_32(usb_base_addr + OTG_DIEPEMPMSK);
+	msk |= ((emp >> epnum) << OTG_DIEPINT_TXFE_SHIFT) & OTG_DIEPINT_TXFE;
+
+	return mmio_read_32(usb_base_addr + OTG_DIEP_BASE +
+			    (epnum * OTG_DIEP_SIZE) + OTG_DIEPINT) & msk;
+}
+
+/*
+ * Return USB core mode.
+ * handle: PCD handle.
+ * return: Core mode.
+ *         This parameter can be 0 (host) or 1 (device).
+ */
+static uint32_t usb_dwc2_get_mode(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return mmio_read_32(usb_base_addr + OTG_GINTSTS) & OTG_GINTSTS_CMOD;
+}
+
+/*
+ * Activate EP0 for detup transactions.
+ * handle: PCD handle.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_activate_setup(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uintptr_t reg_offset = usb_base_addr + OTG_DIEP_BASE;
+
+	/* Set the MPS of the IN EP based on the enumeration speed */
+	mmio_clrbits_32(reg_offset + OTG_DIEPCTL, OTG_DIEPCTL_MPSIZ);
+
+	if ((mmio_read_32(usb_base_addr + OTG_DSTS) & OTG_DSTS_ENUMSPD_MASK) ==
+	    OTG_DSTS_ENUMSPD_LS_PHY_6MHZ) {
+		mmio_setbits_32(reg_offset + OTG_DIEPCTL, 3U);
+	}
+
+	mmio_setbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_CGINAK);
+
+	return USBD_OK;
+}
+
+/*
+ * Prepare the EP0 to start the first control setup.
+ * handle: Selected device.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_ep0_out_start(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uintptr_t reg_offset = usb_base_addr + OTG_DIEP_BASE + OTG_DIEPTSIZ;
+	uint32_t reg_value = 0U;
+
+	/* PKTCNT = 1 and XFRSIZ = 24 bytes for endpoint 0 */
+	reg_value |= OTG_DIEPTSIZ_PKTCNT_1;
+	reg_value |= (EP0_FIFO_SIZE & OTG_DIEPTSIZ_XFRSIZ);
+	reg_value |= OTG_DOEPTSIZ_RXDPID_STUPCNT;
+
+	mmio_write_32(reg_offset, reg_value);
+
+	return USBD_OK;
+}
+
+/*
+ * Write a packet into the TX FIFO associated with the EP/channel.
+ * handle: Selected device.
+ * src: Pointer to source buffer.
+ * ch_ep_num: Endpoint or host channel number.
+ * len: Number of bytes to write.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_write_packet(void *handle, uint8_t *src,
+					  uint8_t ch_ep_num, uint16_t len)
+{
+	uint32_t reg_offset;
+	uint32_t count32b = (len + 3U) / 4U;
+	uint32_t i;
+
+	reg_offset = (uintptr_t)handle + OTG_FIFO_BASE +
+		     (ch_ep_num * OTG_FIFO_SIZE);
+
+	for (i = 0U; i < count32b; i++) {
+		uint32_t src_copy = 0U;
+		uint32_t j;
+
+		/* Data written to FIFO need to be 4 bytes aligned */
+		for (j = 0U; j < 4U; j++) {
+			src_copy += (*(src + j)) << (8U * j);
+		}
+
+		mmio_write_32(reg_offset, src_copy);
+		src += 4U;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Read a packet from the RX FIFO associated with the EP/channel.
+ * handle: Selected device.
+ * dst: Destination pointer.
+ * len: Number of bytes to read.
+ * return: Pointer to destination buffer.
+ */
+static void *usb_dwc2_read_packet(void *handle, uint8_t *dest, uint16_t len)
+{
+	uint32_t reg_offset;
+	uint32_t count32b = (len + 3U) / 4U;
+	uint32_t i;
+
+	VERBOSE("read packet length %i to 0x%lx\n", len, (uintptr_t)dest);
+
+	reg_offset = (uintptr_t)handle + OTG_FIFO_BASE;
+
+	for (i = 0U; i < count32b; i++) {
+		*(uint32_t *)dest = mmio_read_32(reg_offset);
+		dest += 4U;
+		dsb();
+	}
+
+	return (void *)dest;
+}
+
+/*
+ * Setup and start a transfer over an EP.
+ * handle: Selected device
+ * ep: Pointer to endpoint structure.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_ep_start_xfer(void *handle, struct usbd_ep *ep)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t reg_offset;
+	uint32_t reg_value;
+	uint32_t clear_value;
+
+	if (ep->is_in) {
+		reg_offset = usb_base_addr + OTG_DIEP_BASE + (ep->num * OTG_DIEP_SIZE);
+		clear_value = OTG_DIEPTSIZ_PKTCNT | OTG_DIEPTSIZ_XFRSIZ;
+		if (ep->xfer_len == 0U) {
+			reg_value = OTG_DIEPTSIZ_PKTCNT_1;
+		} else {
+			/*
+			 * Program the transfer size and packet count
+			 * as follows:
+			 * xfersize = N * maxpacket + short_packet
+			 * pktcnt = N + (short_packet exist ? 1 : 0)
+			 */
+			reg_value = (OTG_DIEPTSIZ_PKTCNT &
+				     (((ep->xfer_len + ep->maxpacket - 1U) /
+				       ep->maxpacket) << OTG_DIEPTSIZ_PKTCNT_SHIFT))
+				    | ep->xfer_len;
+
+			if (ep->type == EP_TYPE_ISOC) {
+				clear_value |= OTG_DIEPTSIZ_MCNT_MASK;
+				reg_value |= OTG_DIEPTSIZ_MCNT_DATA0;
+			}
+		}
+
+		mmio_clrsetbits_32(reg_offset + OTG_DIEPTSIZ, clear_value, reg_value);
+
+		if ((ep->type != EP_TYPE_ISOC) && (ep->xfer_len > 0U)) {
+			/* Enable the TX FIFO empty interrupt for this EP */
+			mmio_setbits_32(usb_base_addr + OTG_DIEPEMPMSK, BIT(ep->num));
+		}
+
+		/* EP enable, IN data in FIFO */
+		reg_value = OTG_DIEPCTL_CNAK | OTG_DIEPCTL_EPENA;
+
+		if (ep->type == EP_TYPE_ISOC) {
+			if ((mmio_read_32(usb_base_addr + OTG_DSTS) & OTG_DSTS_FNSOF0) == 0U) {
+				reg_value |= OTG_DIEPCTL_SODDFRM;
+			} else {
+				reg_value |= OTG_DIEPCTL_SD0PID_SEVNFRM;
+			}
+		}
+
+		mmio_setbits_32(reg_offset + OTG_DIEPCTL, reg_value);
+
+		if (ep->type == EP_TYPE_ISOC) {
+			usb_dwc2_write_packet(handle, ep->xfer_buff, ep->num, ep->xfer_len);
+		}
+	} else {
+		reg_offset = usb_base_addr + OTG_DOEP_BASE + (ep->num * OTG_DOEP_SIZE);
+		/*
+		 * Program the transfer size and packet count as follows:
+		 * pktcnt = N
+		 * xfersize = N * maxpacket
+		 */
+		if (ep->xfer_len == 0U) {
+			reg_value = ep->maxpacket | OTG_DIEPTSIZ_PKTCNT_1;
+		} else {
+			uint16_t pktcnt = (ep->xfer_len + ep->maxpacket - 1U) / ep->maxpacket;
+
+			reg_value = (pktcnt << OTG_DIEPTSIZ_PKTCNT_SHIFT) |
+				    (ep->maxpacket * pktcnt);
+		}
+
+		mmio_clrsetbits_32(reg_offset + OTG_DOEPTSIZ,
+				   OTG_DOEPTSIZ_XFRSIZ & OTG_DOEPTSIZ_PKTCNT,
+				   reg_value);
+
+		/* EP enable */
+		reg_value = OTG_DOEPCTL_CNAK | OTG_DOEPCTL_EPENA;
+
+		if (ep->type == EP_TYPE_ISOC) {
+			if ((mmio_read_32(usb_base_addr + OTG_DSTS) & OTG_DSTS_FNSOF0) == 0U) {
+				reg_value |= OTG_DOEPCTL_SD1PID_SODDFRM;
+			} else {
+				reg_value |= OTG_DOEPCTL_SD0PID_SEVNFRM;
+			}
+		}
+
+		mmio_setbits_32(reg_offset + OTG_DOEPCTL, reg_value);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Setup and start a transfer over the EP0.
+ * handle: Selected device.
+ * ep: Pointer to endpoint structure.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_ep0_start_xfer(void *handle, struct usbd_ep *ep)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t reg_offset;
+	uint32_t reg_value;
+
+	if (ep->is_in) {
+		reg_offset = usb_base_addr + OTG_DIEP_BASE +
+			     (ep->num * OTG_DIEP_SIZE);
+
+		if (ep->xfer_len == 0U) {
+			reg_value = OTG_DIEPTSIZ_PKTCNT_1;
+		} else {
+			/*
+			 * Program the transfer size and packet count
+			 * as follows:
+			 * xfersize = N * maxpacket + short_packet
+			 * pktcnt = N + (short_packet exist ? 1 : 0)
+			 */
+
+			if (ep->xfer_len > ep->maxpacket) {
+				ep->xfer_len = ep->maxpacket;
+			}
+
+			reg_value = OTG_DIEPTSIZ_PKTCNT_1 | ep->xfer_len;
+		}
+
+		mmio_clrsetbits_32(reg_offset + OTG_DIEPTSIZ,
+				   OTG_DIEPTSIZ_XFRSIZ | OTG_DIEPTSIZ_PKTCNT,
+				   reg_value);
+
+		/* Enable the TX FIFO empty interrupt for this EP */
+		if (ep->xfer_len > 0U) {
+			mmio_setbits_32(usb_base_addr +	OTG_DIEPEMPMSK,
+					BIT(ep->num));
+		}
+
+		/* EP enable, IN data in FIFO */
+		mmio_setbits_32(reg_offset + OTG_DIEPCTL,
+				OTG_DIEPCTL_CNAK | OTG_DIEPCTL_EPENA);
+	} else {
+		reg_offset = usb_base_addr + OTG_DOEP_BASE +
+			     (ep->num * OTG_DOEP_SIZE);
+
+		/*
+		 * Program the transfer size and packet count as follows:
+		 * pktcnt = N
+		 * xfersize = N * maxpacket
+		 */
+		if (ep->xfer_len > 0U) {
+			ep->xfer_len = ep->maxpacket;
+		}
+
+		reg_value = OTG_DIEPTSIZ_PKTCNT_1 | ep->maxpacket;
+
+		mmio_clrsetbits_32(reg_offset + OTG_DIEPTSIZ,
+				   OTG_DIEPTSIZ_XFRSIZ | OTG_DIEPTSIZ_PKTCNT,
+				   reg_value);
+
+		/* EP enable */
+		mmio_setbits_32(reg_offset + OTG_DOEPCTL,
+				OTG_DOEPCTL_CNAK | OTG_DOEPCTL_EPENA);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Set a stall condition over an EP.
+ * handle: Selected device.
+ * ep: Pointer to endpoint structure.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_ep_set_stall(void *handle, struct usbd_ep *ep)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t reg_offset;
+	uint32_t reg_value;
+
+	if (ep->is_in) {
+		reg_offset = usb_base_addr + OTG_DIEP_BASE +
+			     (ep->num * OTG_DIEP_SIZE);
+		reg_value = mmio_read_32(reg_offset + OTG_DIEPCTL);
+
+		if ((reg_value & OTG_DIEPCTL_EPENA) == 0U) {
+			reg_value &= ~OTG_DIEPCTL_EPDIS;
+		}
+
+		reg_value |= OTG_DIEPCTL_STALL;
+
+		mmio_write_32(reg_offset + OTG_DIEPCTL, reg_value);
+	} else {
+		reg_offset = usb_base_addr + OTG_DOEP_BASE +
+			     (ep->num * OTG_DOEP_SIZE);
+		reg_value = mmio_read_32(reg_offset + OTG_DOEPCTL);
+
+		if ((reg_value & OTG_DOEPCTL_EPENA) == 0U) {
+			reg_value &= ~OTG_DOEPCTL_EPDIS;
+		}
+
+		reg_value |= OTG_DOEPCTL_STALL;
+
+		mmio_write_32(reg_offset + OTG_DOEPCTL, reg_value);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Stop the USB device mode.
+ * handle: Selected device.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_stop_device(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t i;
+
+	/* Disable Int */
+	mmio_clrbits_32(usb_base_addr + OTG_GAHBCFG, OTG_GAHBCFG_GINT);
+
+	/* Clear pending interrupts */
+	for (i = 0U; i < EP_NB; i++) {
+		mmio_write_32(usb_base_addr + OTG_DIEP_BASE + (i * OTG_DIEP_SIZE) + OTG_DIEPINT,
+			      OTG_DIEPINT_MASK);
+		mmio_write_32(usb_base_addr + OTG_DOEP_BASE + (i * OTG_DOEP_SIZE) + OTG_DOEPINT,
+			      OTG_DOEPINT_MASK);
+	}
+
+	mmio_write_32(usb_base_addr + OTG_DAINT, OTG_DAINT_IN_MASK | OTG_DAINT_OUT_MASK);
+
+	/* Clear interrupt masks */
+	mmio_write_32(usb_base_addr + OTG_DIEPMSK, 0U);
+	mmio_write_32(usb_base_addr + OTG_DOEPMSK, 0U);
+	mmio_write_32(usb_base_addr + OTG_DAINTMSK, 0U);
+
+	/* Flush the FIFO */
+	usb_dwc2_flush_rx_fifo(handle);
+	usb_dwc2_flush_tx_fifo(handle, EP_ALL);
+
+	/* Disconnect the USB device by disabling the pull-up/pull-down */
+	mmio_setbits_32((uintptr_t)handle + OTG_DCTL, OTG_DCTL_SDIS);
+
+	return USBD_OK;
+}
+
+/*
+ * Stop the USB device mode.
+ * handle: Selected device.
+ * address: New device address to be assigned.
+ *         This parameter can be a value from 0 to 255.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_set_address(void *handle, uint8_t address)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	mmio_clrsetbits_32(usb_base_addr + OTG_DCFG,
+			   OTG_DCFG_DAD,
+			   address << OTG_DCFG_DAD_SHIFT);
+
+	return USBD_OK;
+}
+
+/*
+ * Check FIFO for the next packet to be loaded.
+ * handle: Selected device.
+ * epnum : Endpoint number.
+ * xfer_len: Block length.
+ * xfer_count: Number of blocks.
+ * maxpacket: Max packet length.
+ * xfer_buff: Buffer pointer.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_write_empty_tx_fifo(void *handle,
+						    uint32_t epnum,
+						    uint32_t xfer_len,
+						    uint32_t *xfer_count,
+						    uint32_t maxpacket,
+						    uint8_t **xfer_buff)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t reg_offset;
+	int32_t len;
+	uint32_t len32b;
+	enum usb_status ret;
+
+	len = xfer_len - *xfer_count;
+
+	if ((len > 0) && ((uint32_t)len > maxpacket)) {
+		len = maxpacket;
+	}
+
+	len32b = (len + 3U) / 4U;
+
+	reg_offset = usb_base_addr + OTG_DIEP_BASE + (epnum * OTG_DIEP_SIZE);
+
+	while (((mmio_read_32(reg_offset + OTG_DTXFSTS) &
+		OTG_DTXFSTS_INEPTFSAV) > len32b) &&
+	       (*xfer_count < xfer_len) && (xfer_len != 0U)) {
+		/* Write the FIFO */
+		len = xfer_len - *xfer_count;
+
+		if ((len > 0) && ((uint32_t)len > maxpacket)) {
+			len = maxpacket;
+		}
+
+		len32b = (len + 3U) / 4U;
+
+		ret = usb_dwc2_write_packet(handle, *xfer_buff, epnum, len);
+		if (ret != USBD_OK) {
+			return ret;
+		}
+
+		*xfer_buff  += len;
+		*xfer_count += len;
+	}
+
+	if (len <= 0) {
+		mmio_clrbits_32(usb_base_addr + OTG_DIEPEMPMSK, BIT(epnum));
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Handle PCD interrupt request.
+ * handle: PCD handle.
+ * param: Pointer to information updated by the IT handling.
+ * return: Action to do after IT handling.
+ */
+static enum usb_action usb_dwc2_it_handler(void *handle, uint32_t *param)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t ep_intr;
+	uint32_t epint;
+	uint32_t epnum;
+	uint32_t temp;
+	enum usb_status ret;
+
+	if (usb_dwc2_get_mode(handle) != USB_OTG_MODE_DEVICE) {
+		return USB_NOTHING;
+	}
+
+	/* Avoid spurious interrupt */
+	if (usb_dwc2_read_int(handle) == 0U) {
+		return USB_NOTHING;
+	}
+
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_MMIS) != 0U) {
+		/* Incorrect mode, acknowledge the interrupt */
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_MMIS);
+	}
+
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_OEPINT) != 0U) {
+		uint32_t reg_offset;
+
+		/* Read in the device interrupt bits */
+		ep_intr = usb_dwc2_all_out_ep_int(handle);
+		epnum = 0U;
+		while ((ep_intr & BIT(0)) != BIT(0)) {
+			epnum++;
+			ep_intr >>= 1;
+		}
+
+		reg_offset = usb_base_addr + OTG_DOEP_BASE + (epnum * OTG_DOEP_SIZE) + OTG_DOEPINT;
+
+		epint = usb_dwc2_out_ep_int(handle, epnum);
+
+		if ((epint & OTG_DOEPINT_XFRC) == OTG_DOEPINT_XFRC) {
+			mmio_write_32(reg_offset, OTG_DOEPINT_XFRC);
+			*param = epnum;
+
+			return USB_DATA_OUT;
+		}
+
+		if ((epint & OTG_DOEPINT_STUP) == OTG_DOEPINT_STUP) {
+			/* Inform  that a setup packet is available */
+			mmio_write_32(reg_offset, OTG_DOEPINT_STUP);
+
+			return USB_SETUP;
+		}
+
+		if ((epint & OTG_DOEPINT_OTEPDIS) == OTG_DOEPINT_OTEPDIS) {
+			mmio_write_32(reg_offset, OTG_DOEPINT_OTEPDIS);
+		}
+	}
+
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_IEPINT) != 0U) {
+		uint32_t reg_offset;
+
+		/* Read in the device interrupt bits */
+		ep_intr = usb_dwc2_all_in_ep_int(handle);
+		epnum = 0U;
+		while ((ep_intr & BIT(0)) != BIT(0)) {
+			epnum++;
+			ep_intr >>= 1;
+		}
+
+		reg_offset = usb_base_addr + OTG_DIEP_BASE + (epnum * OTG_DIEP_SIZE) + OTG_DIEPINT;
+
+		epint = usb_dwc2_in_ep_int(handle, epnum);
+
+		if ((epint & OTG_DIEPINT_XFRC) == OTG_DIEPINT_XFRC) {
+			mmio_clrbits_32(usb_base_addr + OTG_DIEPEMPMSK, BIT(epnum));
+			mmio_write_32(reg_offset, OTG_DIEPINT_XFRC);
+			*param = epnum;
+
+			return USB_DATA_IN;
+		}
+
+		if ((epint & OTG_DIEPINT_TOC) == OTG_DIEPINT_TOC) {
+			mmio_write_32(reg_offset, OTG_DIEPINT_TOC);
+		}
+
+		if ((epint & OTG_DIEPINT_ITTXFE) == OTG_DIEPINT_ITTXFE) {
+			mmio_write_32(reg_offset, OTG_DIEPINT_ITTXFE);
+		}
+
+		if ((epint & OTG_DIEPINT_INEPNE) == OTG_DIEPINT_INEPNE) {
+			mmio_write_32(reg_offset, OTG_DIEPINT_INEPNE);
+		}
+
+		if ((epint & OTG_DIEPINT_EPDISD) == OTG_DIEPINT_EPDISD) {
+			mmio_write_32(reg_offset, OTG_DIEPINT_EPDISD);
+		}
+
+		if ((epint & OTG_DIEPINT_TXFE) == OTG_DIEPINT_TXFE) {
+			*param = epnum;
+
+			return USB_WRITE_EMPTY;
+		}
+	}
+
+	/* Handle resume interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_WKUPINT) != 0U) {
+		INFO("handle USB : Resume\n");
+
+		/* Clear the remote wake-up signaling */
+		mmio_clrbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_RWUSIG);
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_WKUPINT);
+
+		return USB_RESUME;
+	}
+
+	/* Handle suspend interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_USBSUSP) != 0U) {
+		INFO("handle USB : Suspend int\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_USBSUSP);
+
+		if ((mmio_read_32(usb_base_addr + OTG_DSTS) &
+		     OTG_DSTS_SUSPSTS) == OTG_DSTS_SUSPSTS) {
+			return USB_SUSPEND;
+		}
+	}
+
+	/* Handle LPM interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_LPMINT) != 0U) {
+		INFO("handle USB : LPM int enter in suspend\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_LPMINT);
+		*param = (mmio_read_32(usb_base_addr + OTG_GLPMCFG) &
+			  OTG_GLPMCFG_BESL) >> 2;
+
+		return USB_LPM;
+	}
+
+	/* Handle reset interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_USBRST) != 0U) {
+		INFO("handle USB : Reset\n");
+
+		mmio_clrbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_RWUSIG);
+
+		usb_dwc2_flush_tx_fifo(handle, 0U);
+
+		mmio_write_32(usb_base_addr + OTG_DAINT, OTG_DAINT_IN_MASK | OTG_DAINT_OUT_MASK);
+		mmio_setbits_32(usb_base_addr + OTG_DAINTMSK, OTG_DAINT_EP0_IN | OTG_DAINT_EP0_OUT);
+
+		mmio_setbits_32(usb_base_addr + OTG_DOEPMSK, OTG_DOEPMSK_STUPM |
+							     OTG_DOEPMSK_XFRCM |
+							     OTG_DOEPMSK_EPDM);
+		mmio_setbits_32(usb_base_addr + OTG_DIEPMSK, OTG_DIEPMSK_TOM |
+							     OTG_DIEPMSK_XFRCM |
+							     OTG_DIEPMSK_EPDM);
+
+		/* Set default address to 0 */
+		mmio_clrbits_32(usb_base_addr + OTG_DCFG, OTG_DCFG_DAD);
+
+		/* Setup EP0 to receive SETUP packets */
+		ret = usb_dwc2_ep0_out_start(handle);
+		if (ret != USBD_OK) {
+			return ret;
+		}
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_USBRST);
+
+		return USB_RESET;
+	}
+
+	/* Handle enumeration done interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_ENUMDNE) != 0U) {
+		ret = usb_dwc2_activate_setup(handle);
+		if (ret != USBD_OK) {
+			return ret;
+		}
+
+		mmio_clrbits_32(usb_base_addr + OTG_GUSBCFG, OTG_GUSBCFG_TRDT);
+
+		mmio_setbits_32(usb_base_addr + OTG_GUSBCFG,
+				(USBD_HS_TRDT_VALUE << OTG_GUSBCFG_TRDT_SHIFT) & OTG_GUSBCFG_TRDT);
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_ENUMDNE);
+
+		return USB_ENUM_DONE;
+	}
+
+	/* Handle RXQLevel interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_RXFLVL) != 0U) {
+		mmio_clrbits_32(usb_base_addr + OTG_GINTMSK,
+				OTG_GINTSTS_RXFLVL);
+
+		temp = mmio_read_32(usb_base_addr + OTG_GRXSTSP);
+
+		*param = temp & OTG_GRXSTSP_EPNUM;
+		*param |= (temp & OTG_GRXSTSP_BCNT) << (USBD_OUT_COUNT_SHIFT -
+							OTG_GRXSTSP_BCNT_SHIFT);
+
+		if (((temp & OTG_GRXSTSP_PKTSTS) >> OTG_GRXSTSP_PKTSTS_SHIFT) == STS_DATA_UPDT) {
+			if ((temp & OTG_GRXSTSP_BCNT) != 0U) {
+				mmio_setbits_32(usb_base_addr + OTG_GINTMSK, OTG_GINTSTS_RXFLVL);
+
+				return USB_READ_DATA_PACKET;
+			}
+		} else if (((temp & OTG_GRXSTSP_PKTSTS) >> OTG_GRXSTSP_PKTSTS_SHIFT) ==
+			    STS_SETUP_UPDT) {
+			mmio_setbits_32(usb_base_addr + OTG_GINTMSK, OTG_GINTSTS_RXFLVL);
+
+			return USB_READ_SETUP_PACKET;
+		}
+
+		mmio_setbits_32(usb_base_addr + OTG_GINTMSK, OTG_GINTSTS_RXFLVL);
+	}
+
+	/* Handle SOF interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_SOF) != 0U) {
+		INFO("handle USB : SOF\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_SOF);
+
+		return USB_SOF;
+	}
+
+	/* Handle incomplete ISO IN interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_IISOIXFR) != 0U) {
+		INFO("handle USB : ISO IN\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS,
+			      OTG_GINTSTS_IISOIXFR);
+	}
+
+	/* Handle incomplete ISO OUT interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_IPXFR_INCOMPISOOUT) !=
+	    0U) {
+		INFO("handle USB : ISO OUT\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS,
+			      OTG_GINTSTS_IPXFR_INCOMPISOOUT);
+	}
+
+	/* Handle connection event interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_SRQINT) != 0U) {
+		INFO("handle USB : Connect\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_SRQINT);
+	}
+
+	/* Handle disconnection event interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_OTGINT) != 0U) {
+		INFO("handle USB : Disconnect\n");
+
+		temp = mmio_read_32(usb_base_addr + OTG_GOTGINT);
+
+		if ((temp & OTG_GOTGINT_SEDET) == OTG_GOTGINT_SEDET) {
+			return USB_DISCONNECT;
+		}
+	}
+
+	return USB_NOTHING;
+}
+
+/*
+ * Start the usb device mode
+ * usb_core_handle: USB core driver handle.
+ * return  USB status.
+ */
+static enum usb_status usb_dwc2_start_device(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	mmio_clrbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_SDIS);
+	mmio_setbits_32(usb_base_addr + OTG_GAHBCFG, OTG_GAHBCFG_GINT);
+
+	return USBD_OK;
+}
+
+static const struct usb_driver usb_dwc2driver = {
+	.ep0_out_start = usb_dwc2_ep0_out_start,
+	.ep_start_xfer = usb_dwc2_ep_start_xfer,
+	.ep0_start_xfer = usb_dwc2_ep0_start_xfer,
+	.write_packet = usb_dwc2_write_packet,
+	.read_packet = usb_dwc2_read_packet,
+	.ep_set_stall = usb_dwc2_ep_set_stall,
+	.start_device = usb_dwc2_start_device,
+	.stop_device = usb_dwc2_stop_device,
+	.set_address = usb_dwc2_set_address,
+	.write_empty_tx_fifo = usb_dwc2_write_empty_tx_fifo,
+	.it_handler = usb_dwc2_it_handler
+};
+
+/*
+ * Initialize USB DWC2 driver.
+ * usb_core_handle: USB core driver handle.
+ * pcd_handle: PCD handle.
+ * base_register: USB global register base address.
+ */
+void stm32mp1_usb_init_driver(struct usb_handle *usb_core_handle,
+			      struct pcd_handle *pcd_handle,
+			      void *base_register)
+{
+	register_usb_driver(usb_core_handle, pcd_handle, &usb_dwc2driver,
+			    base_register);
+}
diff --git a/drivers/synopsys/emmc/dw_mmc.c b/drivers/synopsys/emmc/dw_mmc.c
new file mode 100644
index 0000000..04f4673
--- /dev/null
+++ b/drivers/synopsys/emmc/dw_mmc.c
@@ -0,0 +1,432 @@
+/*
+ * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/mmc.h>
+#include <drivers/synopsys/dw_mmc.h>
+#include <lib/utils_def.h>
+#include <lib/mmio.h>
+
+#define DWMMC_CTRL			(0x00)
+#define CTRL_IDMAC_EN			(1 << 25)
+#define CTRL_DMA_EN			(1 << 5)
+#define CTRL_INT_EN			(1 << 4)
+#define CTRL_DMA_RESET			(1 << 2)
+#define CTRL_FIFO_RESET			(1 << 1)
+#define CTRL_RESET			(1 << 0)
+#define CTRL_RESET_ALL			(CTRL_DMA_RESET | CTRL_FIFO_RESET | \
+					 CTRL_RESET)
+
+#define DWMMC_PWREN			(0x04)
+#define DWMMC_CLKDIV			(0x08)
+#define DWMMC_CLKSRC			(0x0c)
+#define DWMMC_CLKENA			(0x10)
+#define DWMMC_TMOUT			(0x14)
+#define DWMMC_CTYPE			(0x18)
+#define CTYPE_8BIT			(1 << 16)
+#define CTYPE_4BIT			(1)
+#define CTYPE_1BIT			(0)
+
+#define DWMMC_BLKSIZ			(0x1c)
+#define DWMMC_BYTCNT			(0x20)
+#define DWMMC_INTMASK			(0x24)
+#define INT_EBE				(1 << 15)
+#define INT_SBE				(1 << 13)
+#define INT_HLE				(1 << 12)
+#define INT_FRUN			(1 << 11)
+#define INT_DRT				(1 << 9)
+#define INT_RTO				(1 << 8)
+#define INT_DCRC			(1 << 7)
+#define INT_RCRC			(1 << 6)
+#define INT_RXDR			(1 << 5)
+#define INT_TXDR			(1 << 4)
+#define INT_DTO				(1 << 3)
+#define INT_CMD_DONE			(1 << 2)
+#define INT_RE				(1 << 1)
+
+#define DWMMC_CMDARG			(0x28)
+#define DWMMC_CMD			(0x2c)
+#define CMD_START			(U(1) << 31)
+#define CMD_USE_HOLD_REG		(1 << 29)	/* 0 if SDR50/100 */
+#define CMD_UPDATE_CLK_ONLY		(1 << 21)
+#define CMD_SEND_INIT			(1 << 15)
+#define CMD_STOP_ABORT_CMD		(1 << 14)
+#define CMD_WAIT_PRVDATA_COMPLETE	(1 << 13)
+#define CMD_WRITE			(1 << 10)
+#define CMD_DATA_TRANS_EXPECT		(1 << 9)
+#define CMD_CHECK_RESP_CRC		(1 << 8)
+#define CMD_RESP_LEN			(1 << 7)
+#define CMD_RESP_EXPECT			(1 << 6)
+#define CMD(x)				(x & 0x3f)
+
+#define DWMMC_RESP0			(0x30)
+#define DWMMC_RESP1			(0x34)
+#define DWMMC_RESP2			(0x38)
+#define DWMMC_RESP3			(0x3c)
+#define DWMMC_RINTSTS			(0x44)
+#define DWMMC_STATUS			(0x48)
+#define STATUS_DATA_BUSY		(1 << 9)
+
+#define DWMMC_FIFOTH			(0x4c)
+#define FIFOTH_TWMARK(x)		(x & 0xfff)
+#define FIFOTH_RWMARK(x)		((x & 0x1ff) << 16)
+#define FIFOTH_DMA_BURST_SIZE(x)	((x & 0x7) << 28)
+
+#define DWMMC_DEBNCE			(0x64)
+#define DWMMC_BMOD			(0x80)
+#define BMOD_ENABLE			(1 << 7)
+#define BMOD_FB				(1 << 1)
+#define BMOD_SWRESET			(1 << 0)
+
+#define DWMMC_DBADDR			(0x88)
+#define DWMMC_IDSTS			(0x8c)
+#define DWMMC_IDINTEN			(0x90)
+#define DWMMC_CARDTHRCTL		(0x100)
+#define CARDTHRCTL_RD_THR(x)		((x & 0xfff) << 16)
+#define CARDTHRCTL_RD_THR_EN		(1 << 0)
+
+#define IDMAC_DES0_DIC			(1 << 1)
+#define IDMAC_DES0_LD			(1 << 2)
+#define IDMAC_DES0_FS			(1 << 3)
+#define IDMAC_DES0_CH			(1 << 4)
+#define IDMAC_DES0_ER			(1 << 5)
+#define IDMAC_DES0_CES			(1 << 30)
+#define IDMAC_DES0_OWN			(U(1) << 31)
+#define IDMAC_DES1_BS1(x)		((x) & 0x1fff)
+#define IDMAC_DES2_BS2(x)		(((x) & 0x1fff) << 13)
+
+#define DWMMC_DMA_MAX_BUFFER_SIZE	(512 * 8)
+
+#define DWMMC_8BIT_MODE			(1 << 6)
+
+#define DWMMC_ADDRESS_MASK		U(0x0f)
+
+#define TIMEOUT				100000
+
+struct dw_idmac_desc {
+	unsigned int	des0;
+	unsigned int	des1;
+	unsigned int	des2;
+	unsigned int	des3;
+};
+
+static void dw_init(void);
+static int dw_send_cmd(struct mmc_cmd *cmd);
+static int dw_set_ios(unsigned int clk, unsigned int width);
+static int dw_prepare(int lba, uintptr_t buf, size_t size);
+static int dw_read(int lba, uintptr_t buf, size_t size);
+static int dw_write(int lba, uintptr_t buf, size_t size);
+
+static const struct mmc_ops dw_mmc_ops = {
+	.init		= dw_init,
+	.send_cmd	= dw_send_cmd,
+	.set_ios	= dw_set_ios,
+	.prepare	= dw_prepare,
+	.read		= dw_read,
+	.write		= dw_write,
+};
+
+static dw_mmc_params_t dw_params;
+
+static void dw_update_clk(void)
+{
+	unsigned int data;
+
+	mmio_write_32(dw_params.reg_base + DWMMC_CMD,
+		      CMD_WAIT_PRVDATA_COMPLETE | CMD_UPDATE_CLK_ONLY |
+		      CMD_START);
+	while (1) {
+		data = mmio_read_32(dw_params.reg_base + DWMMC_CMD);
+		if ((data & CMD_START) == 0)
+			break;
+		data = mmio_read_32(dw_params.reg_base + DWMMC_RINTSTS);
+		assert((data & INT_HLE) == 0);
+	}
+}
+
+static void dw_set_clk(int clk)
+{
+	unsigned int data;
+	int div;
+
+	assert(clk > 0);
+
+	for (div = 1; div < 256; div++) {
+		if ((dw_params.clk_rate / (2 * div)) <= clk) {
+			break;
+		}
+	}
+	assert(div < 256);
+
+	/* wait until controller is idle */
+	do {
+		data = mmio_read_32(dw_params.reg_base + DWMMC_STATUS);
+	} while (data & STATUS_DATA_BUSY);
+
+	/* disable clock before change clock rate */
+	mmio_write_32(dw_params.reg_base + DWMMC_CLKENA, 0);
+	dw_update_clk();
+
+	mmio_write_32(dw_params.reg_base + DWMMC_CLKDIV, div);
+	dw_update_clk();
+
+	/* enable clock */
+	mmio_write_32(dw_params.reg_base + DWMMC_CLKENA, 1);
+	mmio_write_32(dw_params.reg_base + DWMMC_CLKSRC, 0);
+	dw_update_clk();
+}
+
+static void dw_init(void)
+{
+	unsigned int data;
+	uintptr_t base;
+
+	assert((dw_params.reg_base & MMC_BLOCK_MASK) == 0);
+
+	base = dw_params.reg_base;
+	mmio_write_32(base + DWMMC_PWREN, 1);
+	mmio_write_32(base + DWMMC_CTRL, CTRL_RESET_ALL);
+	do {
+		data = mmio_read_32(base + DWMMC_CTRL);
+	} while (data);
+
+	/* enable DMA in CTRL */
+	data = CTRL_INT_EN | CTRL_DMA_EN | CTRL_IDMAC_EN;
+	mmio_write_32(base + DWMMC_CTRL, data);
+	mmio_write_32(base + DWMMC_RINTSTS, ~0);
+	mmio_write_32(base + DWMMC_INTMASK, 0);
+	mmio_write_32(base + DWMMC_TMOUT, ~0);
+	mmio_write_32(base + DWMMC_IDINTEN, ~0);
+	mmio_write_32(base + DWMMC_BLKSIZ, MMC_BLOCK_SIZE);
+	mmio_write_32(base + DWMMC_BYTCNT, 256 * 1024);
+	mmio_write_32(base + DWMMC_DEBNCE, 0x00ffffff);
+	mmio_write_32(base + DWMMC_BMOD, BMOD_SWRESET);
+	do {
+		data = mmio_read_32(base + DWMMC_BMOD);
+	} while (data & BMOD_SWRESET);
+	/* enable DMA in BMOD */
+	data |= BMOD_ENABLE | BMOD_FB;
+	mmio_write_32(base + DWMMC_BMOD, data);
+
+	udelay(100);
+	dw_set_clk(MMC_BOOT_CLK_RATE);
+	udelay(100);
+}
+
+static int dw_send_cmd(struct mmc_cmd *cmd)
+{
+	unsigned int op, data, err_mask;
+	uintptr_t base;
+	int timeout;
+
+	assert(cmd);
+
+	base = dw_params.reg_base;
+
+	switch (cmd->cmd_idx) {
+	case 0:
+		op = CMD_SEND_INIT;
+		break;
+	case 12:
+		op = CMD_STOP_ABORT_CMD;
+		break;
+	case 13:
+		op = CMD_WAIT_PRVDATA_COMPLETE;
+		break;
+	case 8:
+		if (dw_params.mmc_dev_type == MMC_IS_EMMC)
+			op = CMD_DATA_TRANS_EXPECT | CMD_WAIT_PRVDATA_COMPLETE;
+		else
+			op = CMD_WAIT_PRVDATA_COMPLETE;
+		break;
+	case 17:
+	case 18:
+		op = CMD_DATA_TRANS_EXPECT | CMD_WAIT_PRVDATA_COMPLETE;
+		break;
+	case 24:
+	case 25:
+		op = CMD_WRITE | CMD_DATA_TRANS_EXPECT |
+		     CMD_WAIT_PRVDATA_COMPLETE;
+		break;
+	case 51:
+		op = CMD_DATA_TRANS_EXPECT;
+		break;
+	default:
+		op = 0;
+		break;
+	}
+	op |= CMD_USE_HOLD_REG | CMD_START;
+	switch (cmd->resp_type) {
+	case 0:
+		break;
+	case MMC_RESPONSE_R2:
+		op |= CMD_RESP_EXPECT | CMD_CHECK_RESP_CRC |
+		      CMD_RESP_LEN;
+		break;
+	case MMC_RESPONSE_R3:
+		op |= CMD_RESP_EXPECT;
+		break;
+	default:
+		op |= CMD_RESP_EXPECT | CMD_CHECK_RESP_CRC;
+		break;
+	}
+	timeout = TIMEOUT;
+	do {
+		data = mmio_read_32(base + DWMMC_STATUS);
+		if (--timeout <= 0)
+			panic();
+	} while (data & STATUS_DATA_BUSY);
+
+	mmio_write_32(base + DWMMC_RINTSTS, ~0);
+	mmio_write_32(base + DWMMC_CMDARG, cmd->cmd_arg);
+	mmio_write_32(base + DWMMC_CMD, op | cmd->cmd_idx);
+
+	err_mask = INT_EBE | INT_HLE | INT_RTO | INT_RCRC | INT_RE |
+		   INT_DCRC | INT_DRT | INT_SBE;
+	timeout = TIMEOUT;
+	do {
+		udelay(500);
+		data = mmio_read_32(base + DWMMC_RINTSTS);
+
+		if (data & err_mask)
+			return -EIO;
+		if (data & INT_DTO)
+			break;
+		if (--timeout == 0) {
+			ERROR("%s, RINTSTS:0x%x\n", __func__, data);
+			panic();
+		}
+	} while (!(data & INT_CMD_DONE));
+
+	if (op & CMD_RESP_EXPECT) {
+		cmd->resp_data[0] = mmio_read_32(base + DWMMC_RESP0);
+		if (op & CMD_RESP_LEN) {
+			cmd->resp_data[1] = mmio_read_32(base + DWMMC_RESP1);
+			cmd->resp_data[2] = mmio_read_32(base + DWMMC_RESP2);
+			cmd->resp_data[3] = mmio_read_32(base + DWMMC_RESP3);
+		}
+	}
+	return 0;
+}
+
+static int dw_set_ios(unsigned int clk, unsigned int width)
+{
+	switch (width) {
+	case MMC_BUS_WIDTH_1:
+		mmio_write_32(dw_params.reg_base + DWMMC_CTYPE, CTYPE_1BIT);
+		break;
+	case MMC_BUS_WIDTH_4:
+		mmio_write_32(dw_params.reg_base + DWMMC_CTYPE, CTYPE_4BIT);
+		break;
+	case MMC_BUS_WIDTH_8:
+		mmio_write_32(dw_params.reg_base + DWMMC_CTYPE, CTYPE_8BIT);
+		break;
+	default:
+		assert(0);
+		break;
+	}
+	dw_set_clk(clk);
+	return 0;
+}
+
+static int dw_prepare(int lba, uintptr_t buf, size_t size)
+{
+	struct dw_idmac_desc *desc;
+	int desc_cnt, i, last;
+	uintptr_t base;
+
+	assert(((buf & DWMMC_ADDRESS_MASK) == 0) &&
+	       (dw_params.desc_size > 0) &&
+	       ((dw_params.reg_base & MMC_BLOCK_MASK) == 0) &&
+	       ((dw_params.desc_base & MMC_BLOCK_MASK) == 0) &&
+	       ((dw_params.desc_size & MMC_BLOCK_MASK) == 0));
+
+	flush_dcache_range(buf, size);
+
+	desc_cnt = (size + DWMMC_DMA_MAX_BUFFER_SIZE - 1) /
+		   DWMMC_DMA_MAX_BUFFER_SIZE;
+	assert(desc_cnt * sizeof(struct dw_idmac_desc) < dw_params.desc_size);
+
+	base = dw_params.reg_base;
+	desc = (struct dw_idmac_desc *)dw_params.desc_base;
+	mmio_write_32(base + DWMMC_BYTCNT, size);
+
+	if (size < MMC_BLOCK_SIZE)
+		mmio_write_32(base + DWMMC_BLKSIZ, size);
+	else
+		mmio_write_32(base + DWMMC_BLKSIZ, MMC_BLOCK_SIZE);
+
+	mmio_write_32(base + DWMMC_RINTSTS, ~0);
+	for (i = 0; i < desc_cnt; i++) {
+		desc[i].des0 = IDMAC_DES0_OWN | IDMAC_DES0_CH | IDMAC_DES0_DIC;
+		desc[i].des1 = IDMAC_DES1_BS1(DWMMC_DMA_MAX_BUFFER_SIZE);
+		desc[i].des2 = buf + DWMMC_DMA_MAX_BUFFER_SIZE * i;
+		desc[i].des3 = dw_params.desc_base +
+			       (sizeof(struct dw_idmac_desc)) * (i + 1);
+	}
+	/* first descriptor */
+	desc->des0 |= IDMAC_DES0_FS;
+	/* last descriptor */
+	last = desc_cnt - 1;
+	(desc + last)->des0 |= IDMAC_DES0_LD;
+	(desc + last)->des0 &= ~(IDMAC_DES0_DIC | IDMAC_DES0_CH);
+	(desc + last)->des1 = IDMAC_DES1_BS1(size - (last *
+				  DWMMC_DMA_MAX_BUFFER_SIZE));
+	/* set next descriptor address as 0 */
+	(desc + last)->des3 = 0;
+
+	mmio_write_32(base + DWMMC_DBADDR, dw_params.desc_base);
+	flush_dcache_range(dw_params.desc_base,
+			   desc_cnt * DWMMC_DMA_MAX_BUFFER_SIZE);
+
+
+	return 0;
+}
+
+static int dw_read(int lba, uintptr_t buf, size_t size)
+{
+	uint32_t data = 0;
+	int timeout = TIMEOUT;
+
+	do {
+		data = mmio_read_32(dw_params.reg_base + DWMMC_RINTSTS);
+		udelay(50);
+	} while (!(data & INT_DTO) && timeout-- > 0);
+
+	inv_dcache_range(buf, size);
+
+	return 0;
+}
+
+static int dw_write(int lba, uintptr_t buf, size_t size)
+{
+	return 0;
+}
+
+void dw_mmc_init(dw_mmc_params_t *params, struct mmc_device_info *info)
+{
+	assert((params != 0) &&
+	       ((params->reg_base & MMC_BLOCK_MASK) == 0) &&
+	       ((params->desc_base & MMC_BLOCK_MASK) == 0) &&
+	       ((params->desc_size & MMC_BLOCK_MASK) == 0) &&
+	       (params->desc_size > 0) &&
+	       (params->clk_rate > 0) &&
+	       ((params->bus_width == MMC_BUS_WIDTH_1) ||
+		(params->bus_width == MMC_BUS_WIDTH_4) ||
+		(params->bus_width == MMC_BUS_WIDTH_8)));
+
+	memcpy(&dw_params, params, sizeof(dw_mmc_params_t));
+	dw_params.mmc_dev_type = info->mmc_dev_type;
+	mmc_init(&dw_mmc_ops, params->clk_rate, params->bus_width,
+		 params->flags, info);
+}
diff --git a/drivers/synopsys/ufs/dw_ufs.c b/drivers/synopsys/ufs/dw_ufs.c
new file mode 100644
index 0000000..6bed981
--- /dev/null
+++ b/drivers/synopsys/ufs/dw_ufs.c
@@ -0,0 +1,202 @@
+/*
+ * 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 <common/debug.h>
+#include <drivers/dw_ufs.h>
+#include <drivers/ufs.h>
+#include <lib/mmio.h>
+
+static int dwufs_phy_init(ufs_params_t *params)
+{
+	uintptr_t base;
+	unsigned int fsm0, fsm1;
+	unsigned int data;
+	int result;
+
+	assert((params != NULL) && (params->reg_base != 0));
+
+	base = params->reg_base;
+
+	/* Unipro VS_MPHY disable */
+	ufshc_dme_set(VS_MPHY_DISABLE_OFFSET, 0, VS_MPHY_DISABLE_MPHYDIS);
+	ufshc_dme_set(PA_HS_SERIES_OFFSET, 0, 2);
+	/* MPHY CBRATESEL */
+	ufshc_dme_set(0x8114, 0, 1);
+	/* MPHY CBOVRCTRL2 */
+	ufshc_dme_set(0x8121, 0, 0x2d);
+	/* MPHY CBOVRCTRL3 */
+	ufshc_dme_set(0x8122, 0, 0x1);
+	ufshc_dme_set(VS_MPHY_CFG_UPDT_OFFSET, 0, 1);
+
+	/* MPHY RXOVRCTRL4 rx0 */
+	ufshc_dme_set(0x800d, 4, 0x58);
+	/* MPHY RXOVRCTRL4 rx1 */
+	ufshc_dme_set(0x800d, 5, 0x58);
+	/* MPHY RXOVRCTRL5 rx0 */
+	ufshc_dme_set(0x800e, 4, 0xb);
+	/* MPHY RXOVRCTRL5 rx1 */
+	ufshc_dme_set(0x800e, 5, 0xb);
+	/* MPHY RXSQCONTROL rx0 */
+	ufshc_dme_set(0x8009, 4, 0x1);
+	/* MPHY RXSQCONTROL rx1 */
+	ufshc_dme_set(0x8009, 5, 0x1);
+	ufshc_dme_set(VS_MPHY_CFG_UPDT_OFFSET, 0, 1);
+
+	ufshc_dme_set(0x8113, 0, 0x1);
+	ufshc_dme_set(VS_MPHY_CFG_UPDT_OFFSET, 0, 1);
+
+	ufshc_dme_set(RX_HS_G3_SYNC_LENGTH_CAP_OFFSET, 4, 0x4a);
+	ufshc_dme_set(RX_HS_G3_SYNC_LENGTH_CAP_OFFSET, 5, 0x4a);
+	ufshc_dme_set(RX_HS_G2_SYNC_LENGTH_CAP_OFFSET, 4, 0x4a);
+	ufshc_dme_set(RX_HS_G2_SYNC_LENGTH_CAP_OFFSET, 5, 0x4a);
+	ufshc_dme_set(RX_MIN_ACTIVATETIME_CAP_OFFSET, 4, 0x7);
+	ufshc_dme_set(RX_MIN_ACTIVATETIME_CAP_OFFSET, 5, 0x7);
+	ufshc_dme_set(TX_HIBERN8TIME_CAP_OFFSET, 0, 0x5);
+	ufshc_dme_set(TX_HIBERN8TIME_CAP_OFFSET, 1, 0x5);
+	ufshc_dme_set(VS_MPHY_CFG_UPDT_OFFSET, 0, 1);
+
+	result = ufshc_dme_get(VS_MPHY_DISABLE_OFFSET, 0, &data);
+	assert((result == 0) && (data == VS_MPHY_DISABLE_MPHYDIS));
+	/* enable Unipro VS MPHY */
+	ufshc_dme_set(VS_MPHY_DISABLE_OFFSET, 0, 0);
+
+	while (1) {
+		result = ufshc_dme_get(TX_FSM_STATE_OFFSET, 0, &fsm0);
+		assert(result == 0);
+		result = ufshc_dme_get(TX_FSM_STATE_OFFSET, 1, &fsm1);
+		assert(result == 0);
+		if ((fsm0 == TX_FSM_STATE_HIBERN8) &&
+		    (fsm1 == TX_FSM_STATE_HIBERN8))
+			break;
+	}
+
+	mmio_write_32(base + HCLKDIV, 0xE4);
+	mmio_clrbits_32(base + AHIT, 0x3FF);
+
+	ufshc_dme_set(PA_LOCAL_TX_LCC_ENABLE_OFFSET, 0, 0);
+	ufshc_dme_set(VS_MK2_EXTN_SUPPORT_OFFSET, 0, 0);
+
+	result = ufshc_dme_get(VS_MK2_EXTN_SUPPORT_OFFSET, 0, &data);
+	assert((result == 0) && (data == 0));
+
+	ufshc_dme_set(DL_AFC0_CREDIT_THRESHOLD_OFFSET, 0, 0);
+	ufshc_dme_set(DL_TC0_OUT_ACK_THRESHOLD_OFFSET, 0, 0);
+	ufshc_dme_set(DL_TC0_TX_FC_THRESHOLD_OFFSET, 0, 9);
+	(void)result;
+	return 0;
+}
+
+static int dwufs_phy_set_pwr_mode(ufs_params_t *params)
+{
+	int result;
+	unsigned int data, tx_lanes, rx_lanes;
+	uintptr_t base;
+	unsigned int flags;
+
+	assert((params != NULL) && (params->reg_base != 0));
+
+	base = params->reg_base;
+	flags = params->flags;
+	if ((flags & UFS_FLAGS_VENDOR_SKHYNIX) != 0U) {
+		NOTICE("ufs: H**** device must set VS_DebugSaveConfigTime 0x10\n");
+		/* VS_DebugSaveConfigTime */
+		result = ufshc_dme_set(0xd0a0, 0x0, 0x10);
+		assert(result == 0);
+		/* sync length */
+		result = ufshc_dme_set(0x1556, 0x0, 0x48);
+		assert(result == 0);
+	}
+
+	result = ufshc_dme_get(PA_TACTIVATE_OFFSET, 0, &data);
+	assert(result == 0);
+	if (data < 7) {
+		result = ufshc_dme_set(PA_TACTIVATE_OFFSET, 0, 7);
+		assert(result == 0);
+	}
+	result = ufshc_dme_get(PA_CONNECTED_TX_DATA_LANES_OFFSET, 0, &tx_lanes);
+	assert(result == 0);
+	result = ufshc_dme_get(PA_CONNECTED_RX_DATA_LANES_OFFSET, 0, &rx_lanes);
+	assert(result == 0);
+
+	result = ufshc_dme_set(PA_TX_SKIP_OFFSET, 0, 0);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_TX_GEAR_OFFSET, 0, 3);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_RX_GEAR_OFFSET, 0, 3);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_HS_SERIES_OFFSET, 0, 2);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_TX_TERMINATION_OFFSET, 0, 1);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_RX_TERMINATION_OFFSET, 0, 1);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_SCRAMBLING_OFFSET, 0, 0);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_ACTIVE_TX_DATA_LANES_OFFSET, 0, tx_lanes);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_ACTIVE_RX_DATA_LANES_OFFSET, 0, rx_lanes);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_PWR_MODE_USER_DATA0_OFFSET, 0, 8191);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_PWR_MODE_USER_DATA1_OFFSET, 0, 65535);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_PWR_MODE_USER_DATA2_OFFSET, 0, 32767);
+	assert(result == 0);
+	result = ufshc_dme_set(DME_FC0_PROTECTION_TIMEOUT_OFFSET, 0, 8191);
+	assert(result == 0);
+	result = ufshc_dme_set(DME_TC0_REPLAY_TIMEOUT_OFFSET, 0, 65535);
+	assert(result == 0);
+	result = ufshc_dme_set(DME_AFC0_REQ_TIMEOUT_OFFSET, 0, 32767);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_PWR_MODE_USER_DATA3_OFFSET, 0, 8191);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_PWR_MODE_USER_DATA4_OFFSET, 0, 65535);
+	assert(result == 0);
+	result = ufshc_dme_set(PA_PWR_MODE_USER_DATA5_OFFSET, 0, 32767);
+	assert(result == 0);
+	result = ufshc_dme_set(DME_FC1_PROTECTION_TIMEOUT_OFFSET, 0, 8191);
+	assert(result == 0);
+	result = ufshc_dme_set(DME_TC1_REPLAY_TIMEOUT_OFFSET, 0, 65535);
+	assert(result == 0);
+	result = ufshc_dme_set(DME_AFC1_REQ_TIMEOUT_OFFSET, 0, 32767);
+	assert(result == 0);
+
+	result = ufshc_dme_set(PA_PWR_MODE_OFFSET, 0, 0x11);
+	assert(result == 0);
+	do {
+		data = mmio_read_32(base + IS);
+	} while ((data & UFS_INT_UPMS) == 0);
+	mmio_write_32(base + IS, UFS_INT_UPMS);
+	data = mmio_read_32(base + HCS);
+	if ((data & HCS_UPMCRS_MASK) == HCS_PWR_LOCAL)
+		INFO("ufs: change power mode success\n");
+	else
+		WARN("ufs: HCS.UPMCRS error, HCS:0x%x\n", data);
+	(void)result;
+	return 0;
+}
+
+static const ufs_ops_t dw_ufs_ops = {
+	.phy_init		= dwufs_phy_init,
+	.phy_set_pwr_mode	= dwufs_phy_set_pwr_mode,
+};
+
+int dw_ufs_init(dw_ufs_params_t *params)
+{
+	ufs_params_t ufs_params;
+
+	memset(&ufs_params, 0, sizeof(ufs_params));
+	ufs_params.reg_base = params->reg_base;
+	ufs_params.desc_base = params->desc_base;
+	ufs_params.desc_size = params->desc_size;
+	ufs_params.flags = params->flags;
+	ufs_init(&dw_ufs_ops, &ufs_params);
+	return 0;
+}
diff --git a/drivers/ti/uart/aarch32/16550_console.S b/drivers/ti/uart/aarch32/16550_console.S
new file mode 100644
index 0000000..0429f87
--- /dev/null
+++ b/drivers/ti/uart/aarch32/16550_console.S
@@ -0,0 +1,274 @@
+/*
+ * Copyright (c) 2015-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/ti/uart/uart_16550.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_16550_core_init
+	.globl console_16550_core_putc
+	.globl console_16550_core_getc
+	.globl console_16550_core_flush
+
+	.globl console_16550_putc
+	.globl console_16550_getc
+	.globl console_16550_flush
+
+	/* -----------------------------------------------
+	 * int console_16550_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, 0 on error
+	 * Clobber list : r1, r2, r3
+	 * -----------------------------------------------
+	 */
+func console_16550_core_init
+	/* Check the input base address */
+	cmp	r0, #0
+	beq	init_fail
+	/* Check baud rate and uart clock for sanity */
+	cmp	r1, #0
+	beq	init_fail
+	cmp	r2, #0
+	beq	init_fail
+
+	/* Program the baudrate */
+	/* Divisor =  Uart clock / (16 * baudrate) */
+	lsl	r2, r2, #4
+	udiv	r2, r1, r2
+	and	r1, r2, #0xff		/* w1 = DLL */
+	lsr	r2, r2, #8
+	and	r2, r2, #0xff		/* w2 = DLLM */
+	ldr	r3, [r0, #UARTLCR]
+	orr	r3, r3, #UARTLCR_DLAB
+	str	r3, [r0, #UARTLCR]	/* enable DLL, DLLM programming */
+	str	r1, [r0, #UARTDLL]	/* program DLL */
+	str	r2, [r0, #UARTDLLM]	/* program DLLM */
+	mov	r2, #~UARTLCR_DLAB
+	and	r3, r3, r2
+	str	r3, [r0, #UARTLCR]	/* disable DLL, DLLM programming */
+
+	/* 8n1 */
+	mov	r3, #3
+	str	r3, [r0, #UARTLCR]
+	/* no interrupt */
+	mov	r3, #0
+	str	r3, [r0, #UARTIER]
+#ifdef TI_16550_MDR_QUIRK
+	/* UART must be enabled on some platforms via the MDR register */
+	str	r3, [r0, #UARTMDR1]
+#endif /* TI_16550_MDR_QUIRK */
+	/* enable fifo, DMA */
+	mov	r3, #(UARTFCR_FIFOEN | UARTFCR_DMAEN)
+	str	r3, [r0, #UARTFCR]
+	/* DTR + RTS */
+	mov	r3, #3
+	str	r3, [r0, #UARTMCR]
+	mov	r0, #1
+	bx	lr
+init_fail:
+	mov	r0, #0
+	bx	lr
+endfunc console_16550_core_init
+
+	.globl console_16550_register
+
+	/* -------------------------------------------------------
+	 * int console_16550_register(uintptr_t baseaddr,
+	 *     uint32_t clock, uint32_t baud,
+	 *     console_t *console);
+	 * Function to initialize and register a new 16550
+	 * console. Storage passed in for the console struct
+	 * *must* be persistent (i.e. not from the stack).
+	 * If r1 (UART clock) is 0, initialisation will be
+         * skipped, relying on previous code to have done
+         * this already. r2 is ignored then as well.
+	 * In: r0 - UART register base address
+	 *     r1 - UART clock in Hz
+	 *     r2 - Baud rate (ignored if r1 is 0)
+	 *     r3 - pointer to empty console_t struct
+	 * Out: return 1 on success, 0 on error
+	 * Clobber list : r0, r1, r2
+	 * -------------------------------------------------------
+	 */
+func console_16550_register
+	push	{r4, lr}
+	mov	r4, r3
+	cmp	r4, #0
+	beq	register_fail
+	str	r0, [r4, #CONSOLE_T_BASE]
+
+	/* A clock rate of zero means to skip the initialisation. */
+	cmp	r1, #0
+	beq	register_16550
+
+	bl	console_16550_core_init
+	cmp	r0, #0
+	beq	register_fail
+
+register_16550:
+	mov	r0, r4
+	pop	{r4, lr}
+	finish_console_register 16550 putc=1, getc=1, flush=1
+
+register_fail:
+	pop	{r4, pc}
+endfunc console_16550_register
+
+	/* --------------------------------------------------------
+	 * int console_16550_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_16550_core_putc
+#if ENABLE_ASSERTIONS
+	cmp	r1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Prepend '\r' to '\n' */
+	cmp	r0, #0xA
+	bne	2f
+	/* Check if the transmit FIFO is full */
+1:	ldr	r2, [r1, #UARTLSR]
+	and	r2, r2, #(UARTLSR_TEMT | UARTLSR_THRE)
+	cmp	r2, #(UARTLSR_TEMT | UARTLSR_THRE)
+	bne	1b
+	mov	r2, #0xD		/* '\r' */
+	str	r2, [r1, #UARTTX]
+
+	/* Check if the transmit FIFO is full */
+2:	ldr	r2, [r1, #UARTLSR]
+	and	r2, r2, #(UARTLSR_TEMT | UARTLSR_THRE)
+	cmp	r2, #(UARTLSR_TEMT | UARTLSR_THRE)
+	bne	2b
+	str	r0, [r1, #UARTTX]
+	bx	lr
+endfunc console_16550_core_putc
+
+	/* --------------------------------------------------------
+	 * int console_16550_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_16550_putc
+#if ENABLE_ASSERTIONS
+	cmp	r1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	r1, [r1, #CONSOLE_T_BASE]
+	b	console_16550_core_putc
+endfunc console_16550_putc
+
+	/* ---------------------------------------------
+	 * int console_16550_core_getc(uintptr_t base_addr)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 on if no character is available.
+	 * In :  r0 - console base address
+	 * Clobber list : r0, r1
+	 * ---------------------------------------------
+	 */
+func console_16550_core_getc
+#if ENABLE_ASSERTIONS
+	cmp	r0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Check if the receive FIFO is empty */
+1:	ldr	r1, [r0, #UARTLSR]
+	tst	r1, #UARTLSR_RDR_BIT
+	beq	no_char
+	ldr	r1, [r0, #UARTRX]
+	mov	r0, r1
+	bx	lr
+no_char:
+	mov	r0, #ERROR_NO_PENDING_CHAR
+	bx	lr
+endfunc console_16550_core_getc
+
+	/* ---------------------------------------------
+	 * int console_16550_getc(console_t *console)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 on if no character is available.
+	 * In :  r0 - pointer to console_t stucture
+	 * Out : r0 - character if available, else -1
+	 * Clobber list : r0, r1
+	 * ---------------------------------------------
+	 */
+func console_16550_getc
+#if ENABLE_ASSERTIONS
+	cmp	r0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	r0, [r0, #CONSOLE_T_BASE]
+	b	console_16550_core_getc
+endfunc console_16550_getc
+
+	/* ---------------------------------------------
+	 * void console_16550_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_16550_core_flush
+#if ENABLE_ASSERTIONS
+	cmp	r0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Loop until the transmit FIFO is empty */
+1:	ldr	r1, [r0, #UARTLSR]
+	and	r1, r1, #(UARTLSR_TEMT | UARTLSR_THRE)
+	cmp	r1, #(UARTLSR_TEMT | UARTLSR_THRE)
+	bne	1b
+
+	bx	lr
+endfunc console_16550_core_flush
+
+	/* ---------------------------------------------
+	 * void console_16550_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_16550_flush
+#if ENABLE_ASSERTIONS
+	cmp	r0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	r0, [r0, #CONSOLE_T_BASE]
+	b	console_16550_core_flush
+endfunc console_16550_flush
diff --git a/drivers/ti/uart/aarch64/16550_console.S b/drivers/ti/uart/aarch64/16550_console.S
new file mode 100644
index 0000000..cb21512
--- /dev/null
+++ b/drivers/ti/uart/aarch64/16550_console.S
@@ -0,0 +1,267 @@
+/*
+ * Copyright (c) 2015-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/ti/uart/uart_16550.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_16550_core_init
+	.globl console_16550_core_putc
+	.globl console_16550_core_getc
+	.globl console_16550_core_flush
+
+	.globl console_16550_putc
+	.globl console_16550_getc
+	.globl console_16550_flush
+
+	/* -----------------------------------------------
+	 * int console_16550_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, 0 on error
+	 * Clobber list : x1, x2, x3
+	 * -----------------------------------------------
+	 */
+func console_16550_core_init
+	/* Check the input base address */
+	cbz	x0, init_fail
+	/* Check baud rate and uart clock for sanity */
+	cbz	w1, init_fail
+	cbz	w2, init_fail
+
+	/* Program the baudrate */
+	/* Divisor =  Uart clock / (16 * baudrate) */
+	lsl	w2, w2, #4
+	udiv	w2, w1, w2
+	and	w1, w2, #0xff		/* w1 = DLL */
+	lsr	w2, w2, #8
+	and	w2, w2, #0xff		/* w2 = DLLM */
+	ldr	w3, [x0, #UARTLCR]
+	orr	w3, w3, #UARTLCR_DLAB
+	str	w3, [x0, #UARTLCR]	/* enable DLL, DLLM programming */
+	str	w1, [x0, #UARTDLL]	/* program DLL */
+	str	w2, [x0, #UARTDLLM]	/* program DLLM */
+	mov	w2, #~UARTLCR_DLAB
+	and	w3, w3, w2
+	str	w3, [x0, #UARTLCR]	/* disable DLL, DLLM programming */
+
+	/* 8n1 */
+	mov	w3, #3
+	str	w3, [x0, #UARTLCR]
+	/* no interrupt */
+	mov	w3, #0
+	str	w3, [x0, #UARTIER]
+#ifdef TI_16550_MDR_QUIRK
+	/* UART must be enabled on some platforms via the MDR register */
+	str	w3, [x0, #UARTMDR1]
+#endif /* TI_16550_MDR_QUIRK */
+	/* enable fifo, DMA */
+	mov	w3, #(UARTFCR_FIFOEN | UARTFCR_DMAEN)
+	str	w3, [x0, #UARTFCR]
+	/* DTR + RTS */
+	mov	w3, #3
+	str	w3, [x0, #UARTMCR]
+	mov	w0, #1
+	ret
+init_fail:
+	mov	w0, #0
+	ret
+endfunc console_16550_core_init
+
+	.globl console_16550_register
+
+	/* -----------------------------------------------
+	 * int console_16550_register(uintptr_t baseaddr,
+	 *     uint32_t clock, uint32_t baud,
+	 *     console_t *console);
+	 * Function to initialize and register a new 16550
+	 * console. Storage passed in for the console struct
+	 * *must* be persistent (i.e. not from the stack).
+	 * If w1 (UART clock) is 0, initialisation will be
+	 * skipped, relying on previous code to have done
+	 * this already. w2 is ignored then as well.
+	 * In: x0 - UART register base address
+	 *     w1 - UART clock in Hz
+	 *     w2 - Baud rate (ignored if w1 is 0)
+	 *     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_16550_register
+	mov	x7, x30
+	mov	x6, x3
+	cbz	x6, register_fail
+	str	x0, [x6, #CONSOLE_T_BASE]
+
+	/* A clock rate of zero means to skip the initialisation. */
+	cbz	w1, register_16550
+
+	bl	console_16550_core_init
+	cbz	x0, register_fail
+
+register_16550:
+	mov	x0, x6
+	mov	x30, x7
+	finish_console_register 16550 putc=1, getc=1, flush=1
+
+register_fail:
+	ret	x7
+endfunc console_16550_register
+
+	/* --------------------------------------------------------
+	 * int console_16550_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_16550_core_putc
+#if ENABLE_ASSERTIONS
+	cmp	x1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Prepend '\r' to '\n' */
+	cmp	w0, #0xA
+	b.ne	2f
+	/* Check if the transmit FIFO is full */
+1:	ldr	w2, [x1, #UARTLSR]
+	and	w2, w2, #(UARTLSR_TEMT | UARTLSR_THRE)
+	cmp	w2, #(UARTLSR_TEMT | UARTLSR_THRE)
+	b.ne	1b
+	mov	w2, #0xD		/* '\r' */
+	str	w2, [x1, #UARTTX]
+
+	/* Check if the transmit FIFO is full */
+2:	ldr	w2, [x1, #UARTLSR]
+	and	w2, w2, #(UARTLSR_TEMT | UARTLSR_THRE)
+	cmp	w2, #(UARTLSR_TEMT | UARTLSR_THRE)
+	b.ne	2b
+	str	w0, [x1, #UARTTX]
+	ret
+endfunc console_16550_core_putc
+
+	/* --------------------------------------------------------
+	 * int console_16550_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_16550_putc
+#if ENABLE_ASSERTIONS
+	cmp	x1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x1, [x1, #CONSOLE_T_BASE]
+	b	console_16550_core_putc
+endfunc console_16550_putc
+
+	/* ---------------------------------------------
+	 * int console_16550_core_getc(uintptr_t base_addr)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 on if no character is available.
+	 * In :  x0 - console base address
+	 * Out : w0 - character if available, else -1
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func console_16550_core_getc
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Check if the receive FIFO is empty */
+1:	ldr	w1, [x0, #UARTLSR]
+	tbz	w1, #UARTLSR_RDR_BIT, no_char
+	ldr	w0, [x0, #UARTRX]
+	ret
+no_char:
+	mov	w0, #ERROR_NO_PENDING_CHAR
+	ret
+endfunc console_16550_core_getc
+
+	/* ---------------------------------------------
+	 * int console_16550_getc(console_t *console)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 on if no character is available.
+	 * In :  x0 - pointer to console_t stucture
+	 * Out : w0 - character if available, else -1
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func console_16550_getc
+#if ENABLE_ASSERTIONS
+	cmp	x1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x0, [x0, #CONSOLE_T_BASE]
+	b	console_16550_core_getc
+endfunc console_16550_getc
+
+	/* ---------------------------------------------
+	 * void console_16550_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_16550_core_flush
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Loop until the transmit FIFO is empty */
+1:	ldr	w1, [x0, #UARTLSR]
+	and	w1, w1, #(UARTLSR_TEMT | UARTLSR_THRE)
+	cmp	w1, #(UARTLSR_TEMT | UARTLSR_THRE)
+	b.ne	1b
+
+	ret
+endfunc console_16550_core_flush
+
+	/* ---------------------------------------------
+	 * void console_16550_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_16550_flush
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x0, [x0, #CONSOLE_T_BASE]
+	b	console_16550_core_flush
+endfunc console_16550_flush
diff --git a/drivers/ufs/ufs.c b/drivers/ufs/ufs.c
new file mode 100644
index 0000000..d8c0a14
--- /dev/null
+++ b/drivers/ufs/ufs.c
@@ -0,0 +1,911 @@
+/*
+ * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <endian.h>
+#include <errno.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/ufs.h>
+#include <lib/mmio.h>
+
+#define CDB_ADDR_MASK			127
+#define ALIGN_CDB(x)			(((x) + CDB_ADDR_MASK) & ~CDB_ADDR_MASK)
+#define ALIGN_8(x)			(((x) + 7) & ~7)
+
+#define UFS_DESC_SIZE			0x400
+#define MAX_UFS_DESC_SIZE		0x8000		/* 32 descriptors */
+
+#define MAX_PRDT_SIZE			0x40000		/* 256KB */
+
+static ufs_params_t ufs_params;
+static int nutrs;	/* Number of UTP Transfer Request Slots */
+
+int ufshc_send_uic_cmd(uintptr_t base, uic_cmd_t *cmd)
+{
+	unsigned int data;
+
+	if (base == 0 || cmd == NULL)
+		return -EINVAL;
+
+	data = mmio_read_32(base + HCS);
+	if ((data & HCS_UCRDY) == 0)
+		return -EBUSY;
+	mmio_write_32(base + IS, ~0);
+	mmio_write_32(base + UCMDARG1, cmd->arg1);
+	mmio_write_32(base + UCMDARG2, cmd->arg2);
+	mmio_write_32(base + UCMDARG3, cmd->arg3);
+	mmio_write_32(base + UICCMD, cmd->op);
+
+	do {
+		data = mmio_read_32(base + IS);
+	} while ((data & UFS_INT_UCCS) == 0);
+	mmio_write_32(base + IS, UFS_INT_UCCS);
+	return mmio_read_32(base + UCMDARG2) & CONFIG_RESULT_CODE_MASK;
+}
+
+int ufshc_dme_get(unsigned int attr, unsigned int idx, unsigned int *val)
+{
+	uintptr_t base;
+	unsigned int data;
+	int result, retries;
+	uic_cmd_t cmd;
+
+	assert(ufs_params.reg_base != 0);
+
+	if (val == NULL)
+		return -EINVAL;
+
+	base = ufs_params.reg_base;
+	for (retries = 0; retries < 100; retries++) {
+		data = mmio_read_32(base + HCS);
+		if ((data & HCS_UCRDY) != 0)
+			break;
+		mdelay(1);
+	}
+	if (retries >= 100)
+		return -EBUSY;
+
+	cmd.arg1 = (attr << 16) | GEN_SELECTOR_IDX(idx);
+	cmd.arg2 = 0;
+	cmd.arg3 = 0;
+	cmd.op = DME_GET;
+	for (retries = 0; retries < UFS_UIC_COMMAND_RETRIES; ++retries) {
+		result = ufshc_send_uic_cmd(base, &cmd);
+		if (result == 0)
+			break;
+		data = mmio_read_32(base + IS);
+		if (data & UFS_INT_UE)
+			return -EINVAL;
+	}
+	if (retries >= UFS_UIC_COMMAND_RETRIES)
+		return -EIO;
+
+	*val = mmio_read_32(base + UCMDARG3);
+	return 0;
+}
+
+int ufshc_dme_set(unsigned int attr, unsigned int idx, unsigned int val)
+{
+	uintptr_t base;
+	unsigned int data;
+	int result, retries;
+	uic_cmd_t cmd;
+
+	assert((ufs_params.reg_base != 0));
+
+	base = ufs_params.reg_base;
+	cmd.arg1 = (attr << 16) | GEN_SELECTOR_IDX(idx);
+	cmd.arg2 = 0;
+	cmd.arg3 = val;
+	cmd.op = DME_SET;
+
+	for (retries = 0; retries < UFS_UIC_COMMAND_RETRIES; ++retries) {
+		result = ufshc_send_uic_cmd(base, &cmd);
+		if (result == 0)
+			break;
+		data = mmio_read_32(base + IS);
+		if (data & UFS_INT_UE)
+			return -EINVAL;
+	}
+	if (retries >= UFS_UIC_COMMAND_RETRIES)
+		return -EIO;
+
+	return 0;
+}
+
+static int ufshc_hce_enable(uintptr_t base)
+{
+	unsigned int data;
+	int retries;
+
+	/* Enable Host Controller */
+	mmio_write_32(base + HCE, HCE_ENABLE);
+
+	/* Wait until basic initialization sequence completed */
+	for (retries = 0; retries < HCE_ENABLE_INNER_RETRIES; ++retries) {
+		data = mmio_read_32(base + HCE);
+		if (data & HCE_ENABLE) {
+			break;
+		}
+		udelay(HCE_ENABLE_TIMEOUT_US);
+	}
+	if (retries >= HCE_ENABLE_INNER_RETRIES) {
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int ufshc_hce_disable(uintptr_t base)
+{
+	unsigned int data;
+	int timeout;
+
+	/* Disable Host Controller */
+	mmio_write_32(base + HCE, HCE_DISABLE);
+	timeout = HCE_DISABLE_TIMEOUT_US;
+	do {
+		data = mmio_read_32(base + HCE);
+		if ((data & HCE_ENABLE) == HCE_DISABLE) {
+			break;
+		}
+		udelay(1);
+	} while (--timeout > 0);
+
+	if (timeout <= 0) {
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+
+static int ufshc_reset(uintptr_t base)
+{
+	unsigned int data;
+	int retries, result;
+
+	/* disable controller if enabled */
+	if (mmio_read_32(base + HCE) & HCE_ENABLE) {
+		result = ufshc_hce_disable(base);
+		if (result != 0) {
+			return -EIO;
+		}
+	}
+
+	for (retries = 0; retries < HCE_ENABLE_OUTER_RETRIES; ++retries) {
+		result = ufshc_hce_enable(base);
+		if (result == 0) {
+			break;
+		}
+	}
+	if (retries >= HCE_ENABLE_OUTER_RETRIES) {
+		return -EIO;
+	}
+
+	/* Enable Interrupts */
+	data = UFS_INT_UCCS | UFS_INT_ULSS | UFS_INT_UE | UFS_INT_UTPES |
+	       UFS_INT_DFES | UFS_INT_HCFES | UFS_INT_SBFES;
+	mmio_write_32(base + IE, data);
+
+	return 0;
+}
+
+static int ufshc_dme_link_startup(uintptr_t base)
+{
+	uic_cmd_t cmd;
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.op = DME_LINKSTARTUP;
+	return ufshc_send_uic_cmd(base, &cmd);
+}
+
+static int ufshc_link_startup(uintptr_t base)
+{
+	int data, result;
+	int retries;
+
+	for (retries = DME_LINKSTARTUP_RETRIES; retries > 0; retries--) {
+		result = ufshc_dme_link_startup(base);
+		if (result != 0) {
+			/* Reset controller before trying again */
+			result = ufshc_reset(base);
+			if (result != 0) {
+				return result;
+			}
+			continue;
+		}
+		assert((mmio_read_32(base + HCS) & HCS_DP) == 0);
+		data = mmio_read_32(base + IS);
+		if (data & UFS_INT_ULSS)
+			mmio_write_32(base + IS, UFS_INT_ULSS);
+		return 0;
+	}
+	return -EIO;
+}
+
+/* Check Door Bell register to get an empty slot */
+static int get_empty_slot(int *slot)
+{
+	unsigned int data;
+	int i;
+
+	data = mmio_read_32(ufs_params.reg_base + UTRLDBR);
+	for (i = 0; i < nutrs; i++) {
+		if ((data & 1) == 0)
+			break;
+		data = data >> 1;
+	}
+	if (i >= nutrs)
+		return -EBUSY;
+	*slot = i;
+	return 0;
+}
+
+static void get_utrd(utp_utrd_t *utrd)
+{
+	uintptr_t base;
+	int slot = 0, result;
+	utrd_header_t *hd;
+
+	assert(utrd != NULL);
+	result = get_empty_slot(&slot);
+	assert(result == 0);
+
+	/* clear utrd */
+	memset((void *)utrd, 0, sizeof(utp_utrd_t));
+	base = ufs_params.desc_base + (slot * sizeof(utrd_header_t));
+	/* clear the descriptor */
+	memset((void *)base, 0, UFS_DESC_SIZE);
+
+	utrd->header = base;
+	utrd->task_tag = slot + 1;
+	/* CDB address should be aligned with 128 bytes */
+	utrd->upiu = ALIGN_CDB(utrd->header + sizeof(utrd_header_t));
+	utrd->resp_upiu = ALIGN_8(utrd->upiu + sizeof(cmd_upiu_t));
+	utrd->size_upiu = utrd->resp_upiu - utrd->upiu;
+	utrd->size_resp_upiu = ALIGN_8(sizeof(resp_upiu_t));
+	utrd->prdt = utrd->resp_upiu + utrd->size_resp_upiu;
+
+	hd = (utrd_header_t *)utrd->header;
+	hd->ucdba = utrd->upiu & UINT32_MAX;
+	hd->ucdbau = (utrd->upiu >> 32) & UINT32_MAX;
+	/* Both RUL and RUO is based on DWORD */
+	hd->rul = utrd->size_resp_upiu >> 2;
+	hd->ruo = utrd->size_upiu >> 2;
+	(void)result;
+}
+
+/*
+ * Prepare UTRD, Command UPIU, Response UPIU.
+ */
+static int ufs_prepare_cmd(utp_utrd_t *utrd, uint8_t op, uint8_t lun,
+			   int lba, uintptr_t buf, size_t length)
+{
+	utrd_header_t *hd;
+	cmd_upiu_t *upiu;
+	prdt_t *prdt;
+	unsigned int ulba;
+	unsigned int lba_cnt;
+	int prdt_size;
+
+	hd = (utrd_header_t *)utrd->header;
+	upiu = (cmd_upiu_t *)utrd->upiu;
+
+	hd->i = 1;
+	hd->ct = CT_UFS_STORAGE;
+	hd->ocs = OCS_MASK;
+
+	upiu->trans_type = CMD_UPIU;
+	upiu->task_tag = utrd->task_tag;
+	upiu->cdb[0] = op;
+	ulba = (unsigned int)lba;
+	lba_cnt = (unsigned int)(length >> UFS_BLOCK_SHIFT);
+	switch (op) {
+	case CDBCMD_TEST_UNIT_READY:
+		break;
+	case CDBCMD_READ_CAPACITY_10:
+		hd->dd = DD_OUT;
+		upiu->flags = UPIU_FLAGS_R | UPIU_FLAGS_ATTR_S;
+		upiu->lun = lun;
+		break;
+	case CDBCMD_READ_10:
+		hd->dd = DD_OUT;
+		upiu->flags = UPIU_FLAGS_R | UPIU_FLAGS_ATTR_S;
+		upiu->lun = lun;
+		upiu->cdb[1] = RW_WITHOUT_CACHE;
+		/* set logical block address */
+		upiu->cdb[2] = (ulba >> 24) & 0xff;
+		upiu->cdb[3] = (ulba >> 16) & 0xff;
+		upiu->cdb[4] = (ulba >> 8) & 0xff;
+		upiu->cdb[5] = ulba & 0xff;
+		/* set transfer length */
+		upiu->cdb[7] = (lba_cnt >> 8) & 0xff;
+		upiu->cdb[8] = lba_cnt & 0xff;
+		break;
+	case CDBCMD_WRITE_10:
+		hd->dd = DD_IN;
+		upiu->flags = UPIU_FLAGS_W | UPIU_FLAGS_ATTR_S;
+		upiu->lun = lun;
+		upiu->cdb[1] = RW_WITHOUT_CACHE;
+		/* set logical block address */
+		upiu->cdb[2] = (ulba >> 24) & 0xff;
+		upiu->cdb[3] = (ulba >> 16) & 0xff;
+		upiu->cdb[4] = (ulba >> 8) & 0xff;
+		upiu->cdb[5] = ulba & 0xff;
+		/* set transfer length */
+		upiu->cdb[7] = (lba_cnt >> 8) & 0xff;
+		upiu->cdb[8] = lba_cnt & 0xff;
+		break;
+	default:
+		assert(0);
+		break;
+	}
+	if (hd->dd == DD_IN)
+		flush_dcache_range(buf, length);
+	else if (hd->dd == DD_OUT)
+		inv_dcache_range(buf, length);
+	if (length) {
+		upiu->exp_data_trans_len = htobe32(length);
+		assert(lba_cnt <= UINT16_MAX);
+		prdt = (prdt_t *)utrd->prdt;
+
+		prdt_size = 0;
+		while (length > 0) {
+			prdt->dba = (unsigned int)(buf & UINT32_MAX);
+			prdt->dbau = (unsigned int)((buf >> 32) & UINT32_MAX);
+			/* prdt->dbc counts from 0 */
+			if (length > MAX_PRDT_SIZE) {
+				prdt->dbc = MAX_PRDT_SIZE - 1;
+				length = length - MAX_PRDT_SIZE;
+			} else {
+				prdt->dbc = length - 1;
+				length = 0;
+			}
+			buf += MAX_PRDT_SIZE;
+			prdt++;
+			prdt_size += sizeof(prdt_t);
+		}
+		utrd->size_prdt = ALIGN_8(prdt_size);
+		hd->prdtl = utrd->size_prdt >> 2;
+		hd->prdto = (utrd->size_upiu + utrd->size_resp_upiu) >> 2;
+	}
+
+	flush_dcache_range((uintptr_t)utrd->header, UFS_DESC_SIZE);
+	return 0;
+}
+
+static int ufs_prepare_query(utp_utrd_t *utrd, uint8_t op, uint8_t idn,
+			     uint8_t index, uint8_t sel,
+			     uintptr_t buf, size_t length)
+{
+	utrd_header_t *hd;
+	query_upiu_t *query_upiu;
+
+
+	hd = (utrd_header_t *)utrd->header;
+	query_upiu = (query_upiu_t *)utrd->upiu;
+
+	hd->i = 1;
+	hd->ct = CT_UFS_STORAGE;
+	hd->ocs = OCS_MASK;
+
+	query_upiu->trans_type = QUERY_REQUEST_UPIU;
+	query_upiu->task_tag = utrd->task_tag;
+	query_upiu->ts.desc.opcode = op;
+	query_upiu->ts.desc.idn = idn;
+	query_upiu->ts.desc.index = index;
+	query_upiu->ts.desc.selector = sel;
+	switch (op) {
+	case QUERY_READ_DESC:
+		query_upiu->query_func = QUERY_FUNC_STD_READ;
+		query_upiu->ts.desc.length = htobe16(length);
+		break;
+	case QUERY_WRITE_DESC:
+		query_upiu->query_func = QUERY_FUNC_STD_WRITE;
+		query_upiu->ts.desc.length = htobe16(length);
+		memcpy((void *)(utrd->upiu + sizeof(query_upiu_t)),
+		       (void *)buf, length);
+		break;
+	case QUERY_READ_ATTR:
+	case QUERY_READ_FLAG:
+		query_upiu->query_func = QUERY_FUNC_STD_READ;
+		break;
+	case QUERY_CLEAR_FLAG:
+	case QUERY_SET_FLAG:
+		query_upiu->query_func = QUERY_FUNC_STD_WRITE;
+		break;
+	case QUERY_WRITE_ATTR:
+		query_upiu->query_func = QUERY_FUNC_STD_WRITE;
+		query_upiu->ts.attr.value = htobe32(*((uint32_t *)buf));
+		break;
+	default:
+		assert(0);
+		break;
+	}
+	flush_dcache_range((uintptr_t)utrd->header, UFS_DESC_SIZE);
+	return 0;
+}
+
+static void ufs_prepare_nop_out(utp_utrd_t *utrd)
+{
+	utrd_header_t *hd;
+	nop_out_upiu_t *nop_out;
+
+	hd = (utrd_header_t *)utrd->header;
+	nop_out = (nop_out_upiu_t *)utrd->upiu;
+
+	hd->i = 1;
+	hd->ct = CT_UFS_STORAGE;
+	hd->ocs = OCS_MASK;
+
+	nop_out->trans_type = 0;
+	nop_out->task_tag = utrd->task_tag;
+	flush_dcache_range((uintptr_t)utrd->header, UFS_DESC_SIZE);
+}
+
+static void ufs_send_request(int task_tag)
+{
+	unsigned int data;
+	int slot;
+
+	slot = task_tag - 1;
+	/* clear all interrupts */
+	mmio_write_32(ufs_params.reg_base + IS, ~0);
+
+	mmio_write_32(ufs_params.reg_base + UTRLRSR, 1);
+	assert(mmio_read_32(ufs_params.reg_base + UTRLRSR) == 1);
+
+	data = UTRIACR_IAEN | UTRIACR_CTR | UTRIACR_IACTH(0x1F) |
+	       UTRIACR_IATOVAL(0xFF);
+	mmio_write_32(ufs_params.reg_base + UTRIACR, data);
+	/* send request */
+	mmio_setbits_32(ufs_params.reg_base + UTRLDBR, 1 << slot);
+}
+
+static int ufs_check_resp(utp_utrd_t *utrd, int trans_type)
+{
+	utrd_header_t *hd;
+	resp_upiu_t *resp;
+	sense_data_t *sense;
+	unsigned int data;
+	int slot;
+
+	hd = (utrd_header_t *)utrd->header;
+	resp = (resp_upiu_t *)utrd->resp_upiu;
+	do {
+		data = mmio_read_32(ufs_params.reg_base + IS);
+		if ((data & ~(UFS_INT_UCCS | UFS_INT_UTRCS)) != 0)
+			return -EIO;
+	} while ((data & UFS_INT_UTRCS) == 0);
+	slot = utrd->task_tag - 1;
+
+	data = mmio_read_32(ufs_params.reg_base + UTRLDBR);
+	assert((data & (1 << slot)) == 0);
+	/*
+	 * Invalidate the header after DMA read operation has
+	 * completed to avoid cpu referring to the prefetched
+	 * data brought in before DMA completion.
+	 */
+	inv_dcache_range((uintptr_t)hd, UFS_DESC_SIZE);
+	assert(hd->ocs == OCS_SUCCESS);
+	assert((resp->trans_type & TRANS_TYPE_CODE_MASK) == trans_type);
+
+	sense = &resp->sd.sense;
+	if (sense->resp_code == SENSE_DATA_VALID &&
+	    sense->sense_key == SENSE_KEY_UNIT_ATTENTION && sense->asc == 0x29 &&
+	    sense->ascq == 0) {
+		WARN("Unit Attention Condition\n");
+		return -EAGAIN;
+	}
+
+	(void)resp;
+	(void)slot;
+	return 0;
+}
+
+static void ufs_send_cmd(utp_utrd_t *utrd, uint8_t cmd_op, uint8_t lun, int lba, uintptr_t buf,
+			 size_t length)
+{
+	int result, i;
+
+	for (i = 0; i < UFS_CMD_RETRIES; ++i) {
+		get_utrd(utrd);
+		result = ufs_prepare_cmd(utrd, cmd_op, lun, lba, buf, length);
+		assert(result == 0);
+		ufs_send_request(utrd->task_tag);
+		result = ufs_check_resp(utrd, RESPONSE_UPIU);
+		if (result == 0 || result == -EIO) {
+			break;
+		}
+	}
+	assert(result == 0);
+	(void)result;
+}
+
+#ifdef UFS_RESP_DEBUG
+static void dump_upiu(utp_utrd_t *utrd)
+{
+	utrd_header_t *hd;
+	int i;
+
+	hd = (utrd_header_t *)utrd->header;
+	INFO("utrd:0x%x, ruo:0x%x, rul:0x%x, ocs:0x%x, UTRLDBR:0x%x\n",
+		(unsigned int)(uintptr_t)utrd, hd->ruo, hd->rul, hd->ocs,
+		mmio_read_32(ufs_params.reg_base + UTRLDBR));
+	for (i = 0; i < sizeof(utrd_header_t); i += 4) {
+		INFO("[%lx]:0x%x\n",
+			(uintptr_t)utrd->header + i,
+			*(unsigned int *)((uintptr_t)utrd->header + i));
+	}
+
+	for (i = 0; i < sizeof(cmd_upiu_t); i += 4) {
+		INFO("cmd[%lx]:0x%x\n",
+			utrd->upiu + i,
+			*(unsigned int *)(utrd->upiu + i));
+	}
+	for (i = 0; i < sizeof(resp_upiu_t); i += 4) {
+		INFO("resp[%lx]:0x%x\n",
+			utrd->resp_upiu + i,
+			*(unsigned int *)(utrd->resp_upiu + i));
+	}
+	for (i = 0; i < sizeof(prdt_t); i += 4) {
+		INFO("prdt[%lx]:0x%x\n",
+			utrd->prdt + i,
+			*(unsigned int *)(utrd->prdt + i));
+	}
+}
+#endif
+
+static void ufs_verify_init(void)
+{
+	utp_utrd_t utrd;
+	int result;
+
+	get_utrd(&utrd);
+	ufs_prepare_nop_out(&utrd);
+	ufs_send_request(utrd.task_tag);
+	result = ufs_check_resp(&utrd, NOP_IN_UPIU);
+	assert(result == 0);
+	(void)result;
+}
+
+static void ufs_verify_ready(void)
+{
+	utp_utrd_t utrd;
+	ufs_send_cmd(&utrd, CDBCMD_TEST_UNIT_READY, 0, 0, 0, 0);
+}
+
+static void ufs_query(uint8_t op, uint8_t idn, uint8_t index, uint8_t sel,
+		      uintptr_t buf, size_t size)
+{
+	utp_utrd_t utrd;
+	query_resp_upiu_t *resp;
+	int result;
+
+	switch (op) {
+	case QUERY_READ_FLAG:
+	case QUERY_READ_ATTR:
+	case QUERY_READ_DESC:
+	case QUERY_WRITE_DESC:
+	case QUERY_WRITE_ATTR:
+		assert(((buf & 3) == 0) && (size != 0));
+		break;
+	default:
+		/* Do nothing in default case */
+		break;
+	}
+	get_utrd(&utrd);
+	ufs_prepare_query(&utrd, op, idn, index, sel, buf, size);
+	ufs_send_request(utrd.task_tag);
+	result = ufs_check_resp(&utrd, QUERY_RESPONSE_UPIU);
+	assert(result == 0);
+	resp = (query_resp_upiu_t *)utrd.resp_upiu;
+#ifdef UFS_RESP_DEBUG
+	dump_upiu(&utrd);
+#endif
+	assert(resp->query_resp == QUERY_RESP_SUCCESS);
+
+	switch (op) {
+	case QUERY_READ_FLAG:
+		*(uint32_t *)buf = (uint32_t)resp->ts.flag.value;
+		break;
+	case QUERY_READ_DESC:
+		memcpy((void *)buf,
+		       (void *)(utrd.resp_upiu + sizeof(query_resp_upiu_t)),
+		       size);
+		break;
+	case QUERY_READ_ATTR:
+		*(uint32_t *)buf = htobe32(resp->ts.attr.value);
+		break;
+	default:
+		/* Do nothing in default case */
+		break;
+	}
+	(void)result;
+}
+
+unsigned int ufs_read_attr(int idn)
+{
+	unsigned int value;
+
+	ufs_query(QUERY_READ_ATTR, idn, 0, 0,
+		  (uintptr_t)&value, sizeof(value));
+	return value;
+}
+
+void ufs_write_attr(int idn, unsigned int value)
+{
+	ufs_query(QUERY_WRITE_ATTR, idn, 0, 0,
+		  (uintptr_t)&value, sizeof(value));
+}
+
+unsigned int ufs_read_flag(int idn)
+{
+	unsigned int value;
+
+	ufs_query(QUERY_READ_FLAG, idn, 0, 0,
+		  (uintptr_t)&value, sizeof(value));
+	return value;
+}
+
+void ufs_set_flag(int idn)
+{
+	ufs_query(QUERY_SET_FLAG, idn, 0, 0, 0, 0);
+}
+
+void ufs_clear_flag(int idn)
+{
+	ufs_query(QUERY_CLEAR_FLAG, idn, 0, 0, 0, 0);
+}
+
+void ufs_read_desc(int idn, int index, uintptr_t buf, size_t size)
+{
+	ufs_query(QUERY_READ_DESC, idn, index, 0, buf, size);
+}
+
+void ufs_write_desc(int idn, int index, uintptr_t buf, size_t size)
+{
+	ufs_query(QUERY_WRITE_DESC, idn, index, 0, buf, size);
+}
+
+static int ufs_read_capacity(int lun, unsigned int *num, unsigned int *size)
+{
+	utp_utrd_t utrd;
+	resp_upiu_t *resp;
+	sense_data_t *sense;
+	unsigned char data[CACHE_WRITEBACK_GRANULE << 1];
+	uintptr_t buf;
+	int retries = UFS_READ_CAPACITY_RETRIES;
+
+	assert((ufs_params.reg_base != 0) &&
+	       (ufs_params.desc_base != 0) &&
+	       (ufs_params.desc_size >= UFS_DESC_SIZE) &&
+	       (num != NULL) && (size != NULL));
+
+	/* align buf address */
+	buf = (uintptr_t)data;
+	buf = (buf + CACHE_WRITEBACK_GRANULE - 1) &
+	      ~(CACHE_WRITEBACK_GRANULE - 1);
+	do {
+		ufs_send_cmd(&utrd, CDBCMD_READ_CAPACITY_10, lun, 0,
+			    buf, READ_CAPACITY_LENGTH);
+#ifdef UFS_RESP_DEBUG
+		dump_upiu(&utrd);
+#endif
+		resp = (resp_upiu_t *)utrd.resp_upiu;
+		sense = &resp->sd.sense;
+		if (!((sense->resp_code == SENSE_DATA_VALID) &&
+		    (sense->sense_key == SENSE_KEY_UNIT_ATTENTION) &&
+		    (sense->asc == 0x29) && (sense->ascq == 0))) {
+			inv_dcache_range(buf, CACHE_WRITEBACK_GRANULE);
+			/* last logical block address */
+			*num = be32toh(*(unsigned int *)buf);
+			if (*num)
+				*num += 1;
+			/* logical block length in bytes */
+			*size = be32toh(*(unsigned int *)(buf + 4));
+
+			return 0;
+		}
+
+	} while (retries-- > 0);
+
+	return -ETIMEDOUT;
+}
+
+size_t ufs_read_blocks(int lun, int lba, uintptr_t buf, size_t size)
+{
+	utp_utrd_t utrd;
+	resp_upiu_t *resp;
+
+	assert((ufs_params.reg_base != 0) &&
+	       (ufs_params.desc_base != 0) &&
+	       (ufs_params.desc_size >= UFS_DESC_SIZE));
+
+	ufs_send_cmd(&utrd, CDBCMD_READ_10, lun, lba, buf, size);
+#ifdef UFS_RESP_DEBUG
+	dump_upiu(&utrd);
+#endif
+	/*
+	 * Invalidate prefetched cache contents before cpu
+	 * accesses the buf.
+	 */
+	inv_dcache_range(buf, size);
+	resp = (resp_upiu_t *)utrd.resp_upiu;
+	return size - resp->res_trans_cnt;
+}
+
+size_t ufs_write_blocks(int lun, int lba, const uintptr_t buf, size_t size)
+{
+	utp_utrd_t utrd;
+	resp_upiu_t *resp;
+
+	assert((ufs_params.reg_base != 0) &&
+	       (ufs_params.desc_base != 0) &&
+	       (ufs_params.desc_size >= UFS_DESC_SIZE));
+
+	ufs_send_cmd(&utrd, CDBCMD_WRITE_10, lun, lba, buf, size);
+#ifdef UFS_RESP_DEBUG
+	dump_upiu(&utrd);
+#endif
+	resp = (resp_upiu_t *)utrd.resp_upiu;
+	return size - resp->res_trans_cnt;
+}
+
+static int ufs_set_fdevice_init(void)
+{
+	unsigned int result;
+	int timeout;
+
+	ufs_set_flag(FLAG_DEVICE_INIT);
+
+	timeout = FDEVICEINIT_TIMEOUT_MS;
+	do {
+		result = ufs_read_flag(FLAG_DEVICE_INIT);
+		if (!result) {
+			break;
+		}
+		mdelay(5);
+		timeout -= 5;
+	} while (timeout > 0);
+
+	if (result != 0U) {
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static void ufs_enum(void)
+{
+	unsigned int blk_num, blk_size;
+	int i, result;
+
+	mmio_write_32(ufs_params.reg_base + UTRLBA,
+		      ufs_params.desc_base & UINT32_MAX);
+	mmio_write_32(ufs_params.reg_base + UTRLBAU,
+		      (ufs_params.desc_base >> 32) & UINT32_MAX);
+
+	ufs_verify_init();
+	ufs_verify_ready();
+
+	result = ufs_set_fdevice_init();
+	assert(result == 0);
+
+	blk_num = 0;
+	blk_size = 0;
+
+	/* dump available LUNs */
+	for (i = 0; i < UFS_MAX_LUNS; i++) {
+		result = ufs_read_capacity(i, &blk_num, &blk_size);
+		if (result != 0) {
+			WARN("UFS LUN%d dump failed\n", i);
+		}
+		if (blk_num && blk_size) {
+			INFO("UFS LUN%d contains %d blocks with %d-byte size\n",
+			     i, blk_num, blk_size);
+		}
+	}
+
+	(void)result;
+}
+
+static void ufs_get_device_info(struct ufs_dev_desc *card_data)
+{
+	uint8_t desc_buf[DESC_DEVICE_MAX_SIZE];
+
+	ufs_query(QUERY_READ_DESC, DESC_TYPE_DEVICE, 0, 0,
+				(uintptr_t)desc_buf, DESC_DEVICE_MAX_SIZE);
+
+	/*
+	 * getting vendor (manufacturerID) and Bank Index in big endian
+	 * format
+	 */
+	card_data->wmanufacturerid = (uint16_t)((desc_buf[DEVICE_DESC_PARAM_MANF_ID] << 8) |
+				     (desc_buf[DEVICE_DESC_PARAM_MANF_ID + 1]));
+}
+
+int ufs_init(const ufs_ops_t *ops, ufs_params_t *params)
+{
+	int result;
+	unsigned int data;
+	uic_cmd_t cmd;
+	struct ufs_dev_desc card = {0};
+
+	assert((params != NULL) &&
+	       (params->reg_base != 0) &&
+	       (params->desc_base != 0) &&
+	       (params->desc_size >= UFS_DESC_SIZE));
+
+	memcpy(&ufs_params, params, sizeof(ufs_params_t));
+
+	/* 0 means 1 slot */
+	nutrs = (mmio_read_32(ufs_params.reg_base + CAP) & CAP_NUTRS_MASK) + 1;
+	if (nutrs > (ufs_params.desc_size / UFS_DESC_SIZE)) {
+		nutrs = ufs_params.desc_size / UFS_DESC_SIZE;
+	}
+
+
+	if (ufs_params.flags & UFS_FLAGS_SKIPINIT) {
+		mmio_write_32(ufs_params.reg_base + UTRLBA,
+			      ufs_params.desc_base & UINT32_MAX);
+		mmio_write_32(ufs_params.reg_base + UTRLBAU,
+			      (ufs_params.desc_base >> 32) & UINT32_MAX);
+
+		result = ufshc_dme_get(0x1571, 0, &data);
+		assert(result == 0);
+		result = ufshc_dme_get(0x41, 0, &data);
+		assert(result == 0);
+		if (data == 1) {
+			/* prepare to exit hibernate mode */
+			memset(&cmd, 0, sizeof(uic_cmd_t));
+			cmd.op = DME_HIBERNATE_EXIT;
+			result = ufshc_send_uic_cmd(ufs_params.reg_base,
+						    &cmd);
+			assert(result == 0);
+			data = mmio_read_32(ufs_params.reg_base + UCMDARG2);
+			assert(data == 0);
+			do {
+				data = mmio_read_32(ufs_params.reg_base + IS);
+			} while ((data & UFS_INT_UHXS) == 0);
+			mmio_write_32(ufs_params.reg_base + IS, UFS_INT_UHXS);
+			data = mmio_read_32(ufs_params.reg_base + HCS);
+			assert((data & HCS_UPMCRS_MASK) == HCS_PWR_LOCAL);
+		}
+		result = ufshc_dme_get(0x1568, 0, &data);
+		assert(result == 0);
+		assert((data > 0) && (data <= 3));
+	} else {
+		assert((ops != NULL) && (ops->phy_init != NULL) &&
+		       (ops->phy_set_pwr_mode != NULL));
+
+		result = ufshc_reset(ufs_params.reg_base);
+		assert(result == 0);
+		ops->phy_init(&ufs_params);
+		result = ufshc_link_startup(ufs_params.reg_base);
+		assert(result == 0);
+
+		ufs_enum();
+
+		ufs_get_device_info(&card);
+		if (card.wmanufacturerid == UFS_VENDOR_SKHYNIX) {
+			ufs_params.flags |= UFS_FLAGS_VENDOR_SKHYNIX;
+		}
+
+		ops->phy_set_pwr_mode(&ufs_params);
+	}
+
+	(void)result;
+	return 0;
+}
diff --git a/drivers/usb/usb_device.c b/drivers/usb/usb_device.c
new file mode 100644
index 0000000..701f301
--- /dev/null
+++ b/drivers/usb/usb_device.c
@@ -0,0 +1,845 @@
+/*
+ * Copyright (c) 2021-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/usb_device.h>
+
+/* Define for EP address */
+#define EP_DIR_MASK		BIT(7)
+#define EP_DIR_IN		BIT(7)
+#define EP_NUM_MASK		GENMASK(3, 0)
+
+#define EP0_IN			(0U | EP_DIR_IN)
+#define EP0_OUT			0U
+
+/* USB address between 1 through 127 = 0x7F mask */
+#define ADDRESS_MASK		GENMASK(6, 0)
+
+/*
+ * Set a STALL condition over an endpoint
+ * pdev: USB handle
+ * ep_addr: endpoint address
+ * return : status
+ */
+static enum usb_status usb_core_set_stall(struct usb_handle *pdev, uint8_t ep_addr)
+{
+	struct usbd_ep *ep;
+	struct pcd_handle *hpcd = (struct pcd_handle *)pdev->data;
+	uint8_t num;
+
+	num = ep_addr & EP_NUM_MASK;
+	if (num >= USBD_EP_NB) {
+		return USBD_FAIL;
+	}
+	if ((EP_DIR_MASK & ep_addr) == EP_DIR_IN) {
+		ep = &hpcd->in_ep[num];
+		ep->is_in = true;
+	} else {
+		ep = &hpcd->out_ep[num];
+		ep->is_in = false;
+	}
+	ep->num = num;
+
+	pdev->driver->ep_set_stall(hpcd->instance, ep);
+	if (num == 0U) {
+		pdev->driver->ep0_out_start(hpcd->instance);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_get_desc
+ *         Handle Get Descriptor requests
+ * pdev : device instance
+ * req : usb request
+ */
+static void usb_core_get_desc(struct usb_handle *pdev, struct usb_setup_req *req)
+{
+	uint16_t len;
+	uint8_t *pbuf;
+	uint8_t desc_type = HIBYTE(req->value);
+	uint8_t desc_idx = LOBYTE(req->value);
+
+	switch (desc_type) {
+	case USB_DESC_TYPE_DEVICE:
+		pbuf = pdev->desc->get_device_desc(&len);
+		break;
+
+	case USB_DESC_TYPE_CONFIGURATION:
+		pbuf = pdev->desc->get_config_desc(&len);
+		break;
+
+	case USB_DESC_TYPE_STRING:
+		switch (desc_idx) {
+		case USBD_IDX_LANGID_STR:
+			pbuf = pdev->desc->get_lang_id_desc(&len);
+			break;
+
+		case USBD_IDX_MFC_STR:
+			pbuf = pdev->desc->get_manufacturer_desc(&len);
+			break;
+
+		case USBD_IDX_PRODUCT_STR:
+			pbuf = pdev->desc->get_product_desc(&len);
+			break;
+
+		case USBD_IDX_SERIAL_STR:
+			pbuf = pdev->desc->get_serial_desc(&len);
+			break;
+
+		case USBD_IDX_CONFIG_STR:
+			pbuf = pdev->desc->get_configuration_desc(&len);
+			break;
+
+		case USBD_IDX_INTERFACE_STR:
+			pbuf = pdev->desc->get_interface_desc(&len);
+			break;
+
+		/* For all USER string */
+		case USBD_IDX_USER0_STR:
+		default:
+			pbuf = pdev->desc->get_usr_desc(desc_idx - USBD_IDX_USER0_STR, &len);
+			break;
+		}
+		break;
+
+	case USB_DESC_TYPE_DEVICE_QUALIFIER:
+		pbuf = pdev->desc->get_device_qualifier_desc(&len);
+		break;
+
+	case USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION:
+		if (pdev->desc->get_other_speed_config_desc == NULL) {
+			usb_core_ctl_error(pdev);
+			return;
+		}
+		pbuf = pdev->desc->get_other_speed_config_desc(&len);
+		break;
+
+	default:
+		ERROR("Unknown request %i\n", desc_type);
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	if ((len != 0U) && (req->length != 0U)) {
+		len = MIN(len, req->length);
+
+		/* Start the transfer */
+		usb_core_transmit_ep0(pdev, pbuf, len);
+	}
+}
+
+/*
+ * usb_core_set_config
+ *         Handle Set device configuration request
+ * pdev : device instance
+ * req : usb request
+ */
+static void usb_core_set_config(struct usb_handle *pdev, struct usb_setup_req *req)
+{
+	static uint8_t cfgidx;
+
+	cfgidx = LOBYTE(req->value);
+
+	if (cfgidx > USBD_MAX_NUM_CONFIGURATION) {
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	switch (pdev->dev_state) {
+	case USBD_STATE_ADDRESSED:
+		if (cfgidx != 0U) {
+			pdev->dev_config = cfgidx;
+			pdev->dev_state = USBD_STATE_CONFIGURED;
+			if (!pdev->class) {
+				usb_core_ctl_error(pdev);
+				return;
+			}
+			/* Set configuration and Start the Class */
+			if (pdev->class->init(pdev, cfgidx) != 0U) {
+				usb_core_ctl_error(pdev);
+				return;
+			}
+		}
+		break;
+
+	case USBD_STATE_CONFIGURED:
+		if (cfgidx == 0U) {
+			pdev->dev_state = USBD_STATE_ADDRESSED;
+			pdev->dev_config = cfgidx;
+			pdev->class->de_init(pdev, cfgidx);
+		} else if (cfgidx != pdev->dev_config) {
+			if (pdev->class == NULL) {
+				usb_core_ctl_error(pdev);
+				return;
+			}
+			/* Clear old configuration */
+			pdev->class->de_init(pdev, pdev->dev_config);
+			/* Set new configuration */
+			pdev->dev_config = cfgidx;
+			/* Set configuration and start the USB class */
+			if (pdev->class->init(pdev, cfgidx) != 0U) {
+				usb_core_ctl_error(pdev);
+				return;
+			}
+		}
+		break;
+
+	default:
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	/* Send status */
+	usb_core_transmit_ep0(pdev, NULL, 0U);
+}
+
+/*
+ * usb_core_get_status
+ *         Handle Get Status request
+ * pdev : device instance
+ * req : usb request
+ */
+static void usb_core_get_status(struct usb_handle *pdev,
+				struct usb_setup_req *req)
+{
+	if ((pdev->dev_state != USBD_STATE_ADDRESSED) &&
+	    (pdev->dev_state != USBD_STATE_CONFIGURED)) {
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	pdev->dev_config_status = USB_CONFIG_SELF_POWERED;
+
+	if (pdev->dev_remote_wakeup != 0U) {
+		pdev->dev_config_status |= USB_CONFIG_REMOTE_WAKEUP;
+	}
+
+	/* Start the transfer */
+	usb_core_transmit_ep0(pdev, (uint8_t *)&pdev->dev_config_status, 2U);
+}
+
+/*
+ * usb_core_set_address
+ *         Set device address
+ * pdev : device instance
+ * req : usb request
+ */
+static void usb_core_set_address(struct usb_handle *pdev,
+				 struct usb_setup_req *req)
+{
+	uint8_t dev_addr;
+
+	if ((req->index != 0U) || (req->length != 0U)) {
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	dev_addr = req->value & ADDRESS_MASK;
+	if (pdev->dev_state != USBD_STATE_DEFAULT) {
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	pdev->dev_address = dev_addr;
+	pdev->driver->set_address(((struct pcd_handle *)(pdev->data))->instance, dev_addr);
+
+	/* Send status */
+	usb_core_transmit_ep0(pdev, NULL, 0U);
+
+	if (dev_addr != 0U) {
+		pdev->dev_state  = USBD_STATE_ADDRESSED;
+	} else {
+		pdev->dev_state  = USBD_STATE_DEFAULT;
+	}
+}
+
+/*
+ * usb_core_dev_req
+ *         Handle standard usb device requests
+ * pdev : device instance
+ * req : usb request
+ * return : status
+ */
+static enum usb_status usb_core_dev_req(struct usb_handle *pdev,
+					struct usb_setup_req *req)
+{
+	VERBOSE("receive request %i\n", req->b_request);
+	switch (req->b_request) {
+	case USB_REQ_GET_DESCRIPTOR:
+		usb_core_get_desc(pdev, req);
+		break;
+
+	case USB_REQ_SET_CONFIGURATION:
+		usb_core_set_config(pdev, req);
+		break;
+
+	case USB_REQ_GET_STATUS:
+		usb_core_get_status(pdev, req);
+		break;
+
+	case USB_REQ_SET_ADDRESS:
+		usb_core_set_address(pdev, req);
+		break;
+
+	case USB_REQ_GET_CONFIGURATION:
+	case USB_REQ_SET_FEATURE:
+	case USB_REQ_CLEAR_FEATURE:
+	default:
+		ERROR("NOT SUPPORTED %i\n", req->b_request);
+		usb_core_ctl_error(pdev);
+		break;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_itf_req
+ *         Handle standard usb interface requests
+ * pdev : device instance
+ * req : usb request
+ * return : status
+ */
+static enum usb_status usb_core_itf_req(struct usb_handle *pdev,
+					struct usb_setup_req *req)
+{
+	if (pdev->dev_state != USBD_STATE_CONFIGURED) {
+		usb_core_ctl_error(pdev);
+		return USBD_OK;
+	}
+
+	if (LOBYTE(req->index) <= USBD_MAX_NUM_INTERFACES) {
+		pdev->class->setup(pdev, req);
+
+		if (req->length == 0U) {
+			usb_core_transmit_ep0(pdev, NULL, 0U);
+		}
+	} else {
+		usb_core_ctl_error(pdev);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_setup_stage
+ *         Handle the setup stage
+ * pdev: device instance
+ * psetup : setup buffer
+ * return : status
+ */
+static enum usb_status usb_core_setup_stage(struct usb_handle *pdev,
+					    uint8_t *psetup)
+{
+	struct usb_setup_req *req = &pdev->request;
+
+	/* Copy setup buffer into req structure */
+	req->bm_request = psetup[0];
+	req->b_request = psetup[1];
+	req->value = psetup[2] + (psetup[3] << 8);
+	req->index = psetup[4] + (psetup[5] << 8);
+	req->length = psetup[6] + (psetup[7] << 8);
+
+	pdev->ep0_state = USBD_EP0_SETUP;
+	pdev->ep0_data_len = pdev->request.length;
+
+	switch (pdev->request.bm_request & USB_REQ_RECIPIENT_MASK) {
+	case USB_REQ_RECIPIENT_DEVICE:
+		usb_core_dev_req(pdev, &pdev->request);
+		break;
+
+	case USB_REQ_RECIPIENT_INTERFACE:
+		usb_core_itf_req(pdev, &pdev->request);
+		break;
+
+	case USB_REQ_RECIPIENT_ENDPOINT:
+	default:
+		ERROR("receive unsupported request %u",
+		      pdev->request.bm_request & USB_REQ_RECIPIENT_MASK);
+		usb_core_set_stall(pdev, pdev->request.bm_request & USB_REQ_DIRECTION);
+		return USBD_FAIL;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_data_out
+ *         Handle data OUT stage
+ * pdev: device instance
+ * epnum: endpoint index
+ * pdata: buffer to sent
+ * return : status
+ */
+static enum usb_status usb_core_data_out(struct usb_handle *pdev, uint8_t epnum,
+					 uint8_t *pdata)
+{
+	struct usb_endpoint *pep;
+
+	if (epnum == 0U) {
+		pep = &pdev->ep_out[0];
+		if (pdev->ep0_state == USBD_EP0_DATA_OUT) {
+			if (pep->rem_length > pep->maxpacket) {
+				pep->rem_length -= pep->maxpacket;
+
+				usb_core_receive(pdev, 0U, pdata,
+						 MIN(pep->rem_length,
+						     pep->maxpacket));
+			} else {
+				if (pdev->class->ep0_rx_ready &&
+				    (pdev->dev_state == USBD_STATE_CONFIGURED)) {
+					pdev->class->ep0_rx_ready(pdev);
+				}
+
+				usb_core_transmit_ep0(pdev, NULL, 0U);
+			}
+		}
+	} else if (pdev->class->data_out != NULL &&
+		   (pdev->dev_state == USBD_STATE_CONFIGURED)) {
+		pdev->class->data_out(pdev, epnum);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_data_in
+ *         Handle data in stage
+ * pdev: device instance
+ * epnum: endpoint index
+ * pdata: buffer to fill
+ * return : status
+ */
+static enum usb_status usb_core_data_in(struct usb_handle *pdev, uint8_t epnum,
+					uint8_t *pdata)
+{
+	if (epnum == 0U) {
+		struct usb_endpoint *pep = &pdev->ep_in[0];
+
+		if (pdev->ep0_state == USBD_EP0_DATA_IN) {
+			if (pep->rem_length > pep->maxpacket) {
+				pep->rem_length -= pep->maxpacket;
+
+				usb_core_transmit(pdev, 0U, pdata,
+						  pep->rem_length);
+
+				/* Prepare EP for premature end of transfer */
+				usb_core_receive(pdev, 0U, NULL, 0U);
+			} else {
+				/* Last packet is MPS multiple, send ZLP packet */
+				if ((pep->total_length % pep->maxpacket == 0U) &&
+				    (pep->total_length >= pep->maxpacket) &&
+				    (pep->total_length < pdev->ep0_data_len)) {
+					usb_core_transmit(pdev, 0U, NULL, 0U);
+
+					pdev->ep0_data_len = 0U;
+
+					/* Prepare endpoint for premature end of transfer */
+					usb_core_receive(pdev, 0U, NULL, 0U);
+				} else {
+					if (pdev->class->ep0_tx_sent != NULL &&
+					    (pdev->dev_state ==
+					     USBD_STATE_CONFIGURED)) {
+						pdev->class->ep0_tx_sent(pdev);
+					}
+					/* Start the transfer */
+					usb_core_receive_ep0(pdev, NULL, 0U);
+				}
+			}
+		}
+	} else if ((pdev->class->data_in != NULL) &&
+		  (pdev->dev_state == USBD_STATE_CONFIGURED)) {
+		pdev->class->data_in(pdev, epnum);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_suspend
+ *         Handle suspend event
+ * pdev : device instance
+ * return : status
+ */
+static enum usb_status usb_core_suspend(struct usb_handle  *pdev)
+{
+	INFO("USB Suspend mode\n");
+	pdev->dev_old_state =  pdev->dev_state;
+	pdev->dev_state  = USBD_STATE_SUSPENDED;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_resume
+ *         Handle resume event
+ * pdev : device instance
+ * return : status
+ */
+static enum usb_status usb_core_resume(struct usb_handle *pdev)
+{
+	INFO("USB Resume\n");
+	pdev->dev_state = pdev->dev_old_state;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_sof
+ *         Handle SOF event
+ * pdev : device instance
+ * return : status
+ */
+static enum usb_status usb_core_sof(struct usb_handle *pdev)
+{
+	if (pdev->dev_state == USBD_STATE_CONFIGURED) {
+		if (pdev->class->sof != NULL) {
+			pdev->class->sof(pdev);
+		}
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_disconnect
+ *         Handle device disconnection event
+ * pdev : device instance
+ * return : status
+ */
+static enum usb_status usb_core_disconnect(struct usb_handle *pdev)
+{
+	/* Free class resources */
+	pdev->dev_state = USBD_STATE_DEFAULT;
+	pdev->class->de_init(pdev, pdev->dev_config);
+
+	return USBD_OK;
+}
+
+enum usb_status usb_core_handle_it(struct usb_handle *pdev)
+{
+	uint32_t param = 0U;
+	uint32_t len = 0U;
+	struct usbd_ep *ep;
+
+	switch (pdev->driver->it_handler(pdev->data->instance, &param)) {
+	case USB_DATA_OUT:
+		usb_core_data_out(pdev, param,
+				  pdev->data->out_ep[param].xfer_buff);
+		break;
+
+	case USB_DATA_IN:
+		usb_core_data_in(pdev, param,
+				 pdev->data->in_ep[param].xfer_buff);
+		break;
+
+	case USB_SETUP:
+		usb_core_setup_stage(pdev, (uint8_t *)pdev->data->setup);
+		break;
+
+	case USB_ENUM_DONE:
+		break;
+
+	case USB_READ_DATA_PACKET:
+		ep = &pdev->data->out_ep[param &  USBD_OUT_EPNUM_MASK];
+		len = (param &  USBD_OUT_COUNT_MASK) >> USBD_OUT_COUNT_SHIFT;
+		pdev->driver->read_packet(pdev->data->instance,
+					  ep->xfer_buff, len);
+		ep->xfer_buff += len;
+		ep->xfer_count += len;
+		break;
+
+	case USB_READ_SETUP_PACKET:
+		ep = &pdev->data->out_ep[param &  USBD_OUT_EPNUM_MASK];
+		len = (param &  USBD_OUT_COUNT_MASK) >> 0x10;
+		pdev->driver->read_packet(pdev->data->instance,
+					  (uint8_t *)pdev->data->setup, 8);
+		ep->xfer_count += len;
+		break;
+
+	case USB_RESET:
+		pdev->dev_state = USBD_STATE_DEFAULT;
+		break;
+
+	case USB_RESUME:
+		if (pdev->data->lpm_state == LPM_L1) {
+			pdev->data->lpm_state = LPM_L0;
+		} else {
+			usb_core_resume(pdev);
+		}
+		break;
+
+	case USB_SUSPEND:
+		usb_core_suspend(pdev);
+		break;
+
+	case USB_LPM:
+		if (pdev->data->lpm_state == LPM_L0) {
+			pdev->data->lpm_state = LPM_L1;
+		} else {
+			usb_core_suspend(pdev);
+		}
+		break;
+
+	case USB_SOF:
+		usb_core_sof(pdev);
+		break;
+
+	case USB_DISCONNECT:
+		usb_core_disconnect(pdev);
+		break;
+
+	case USB_WRITE_EMPTY:
+		pdev->driver->write_empty_tx_fifo(pdev->data->instance, param,
+				     pdev->data->in_ep[param].xfer_len,
+				     (uint32_t *)&pdev->data->in_ep[param].xfer_count,
+				     pdev->data->in_ep[param].maxpacket,
+				     &pdev->data->in_ep[param].xfer_buff);
+		break;
+
+	case USB_NOTHING:
+	default:
+		break;
+	}
+
+	return USBD_OK;
+}
+
+static void usb_core_start_xfer(struct usb_handle *pdev,
+				void *handle,
+				struct usbd_ep *ep)
+{
+	if (ep->num == 0U) {
+		pdev->driver->ep0_start_xfer(handle, ep);
+	} else {
+		pdev->driver->ep_start_xfer(handle, ep);
+	}
+}
+
+/*
+ * usb_core_receive
+ *          Receive an amount of data
+ * pdev: USB handle
+ * ep_addr: endpoint address
+ * buf: pointer to the reception buffer
+ * len: amount of data to be received
+ * return : status
+ */
+enum usb_status usb_core_receive(struct usb_handle *pdev, uint8_t ep_addr,
+				 uint8_t *buf, uint32_t len)
+{
+	struct usbd_ep *ep;
+	struct pcd_handle *hpcd = (struct pcd_handle *)pdev->data;
+	uint8_t num;
+
+	num = ep_addr & EP_NUM_MASK;
+	if (num >= USBD_EP_NB) {
+		return USBD_FAIL;
+	}
+	ep = &hpcd->out_ep[num];
+
+	/* Setup and start the Xfer */
+	ep->xfer_buff = buf;
+	ep->xfer_len = len;
+	ep->xfer_count = 0U;
+	ep->is_in = false;
+	ep->num = num;
+
+	usb_core_start_xfer(pdev, hpcd->instance, ep);
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_transmit
+ *          Send an amount of data
+ * pdev: USB handle
+ * ep_addr: endpoint address
+ * buf: pointer to the transmission buffer
+ * len: amount of data to be sent
+ * return : status
+ */
+enum usb_status usb_core_transmit(struct usb_handle *pdev, uint8_t ep_addr,
+				  uint8_t *buf, uint32_t len)
+{
+	struct usbd_ep *ep;
+	struct pcd_handle *hpcd = (struct pcd_handle *)pdev->data;
+	uint8_t num;
+
+	num = ep_addr & EP_NUM_MASK;
+	if (num >= USBD_EP_NB) {
+		return USBD_FAIL;
+	}
+	ep = &hpcd->in_ep[num];
+
+	/* Setup and start the Xfer */
+	ep->xfer_buff = buf;
+	ep->xfer_len = len;
+	ep->xfer_count = 0U;
+	ep->is_in = true;
+	ep->num = num;
+
+	usb_core_start_xfer(pdev, hpcd->instance, ep);
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_receive_ep0
+ *          Receive an amount of data on ep0
+ * pdev: USB handle
+ * buf: pointer to the reception buffer
+ * len: amount of data to be received
+ * return : status
+ */
+enum usb_status usb_core_receive_ep0(struct usb_handle *pdev, uint8_t *buf,
+				     uint32_t len)
+{
+	/* Prepare the reception of the buffer over EP0 */
+	if (len != 0U) {
+		pdev->ep0_state = USBD_EP0_DATA_OUT;
+	} else {
+		pdev->ep0_state = USBD_EP0_STATUS_OUT;
+	}
+
+	pdev->ep_out[0].total_length = len;
+	pdev->ep_out[0].rem_length = len;
+
+	/* Start the transfer */
+	return usb_core_receive(pdev, 0U, buf, len);
+}
+
+/*
+ * usb_core_transmit_ep0
+ *          Send an amount of data on ep0
+ * pdev: USB handle
+ * buf: pointer to the transmission buffer
+ * len: amount of data to be sent
+ * return : status
+ */
+enum usb_status usb_core_transmit_ep0(struct usb_handle *pdev, uint8_t *buf,
+				      uint32_t len)
+{
+	/* Set EP0 State */
+	if (len != 0U) {
+		pdev->ep0_state = USBD_EP0_DATA_IN;
+	} else {
+		pdev->ep0_state = USBD_EP0_STATUS_IN;
+	}
+
+	pdev->ep_in[0].total_length = len;
+	pdev->ep_in[0].rem_length = len;
+
+	/* Start the transfer */
+	return usb_core_transmit(pdev, 0U, buf, len);
+}
+
+/*
+ * usb_core_ctl_error
+ *         Handle USB low level error
+ * pdev: device instance
+ * req: usb request
+ * return : None
+ */
+
+void usb_core_ctl_error(struct usb_handle *pdev)
+{
+	ERROR("%s : Send an ERROR\n", __func__);
+	usb_core_set_stall(pdev, EP0_IN);
+	usb_core_set_stall(pdev, EP0_OUT);
+}
+
+/*
+ * usb_core_start
+ *         Start the USB device core.
+ * pdev: Device Handle
+ * return : USBD Status
+ */
+enum usb_status usb_core_start(struct usb_handle *pdev)
+{
+	/* Start the low level driver */
+	pdev->driver->start_device(pdev->data->instance);
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_stop
+ *         Stop the USB device core.
+ * pdev: Device Handle
+ * return : USBD Status
+ */
+enum usb_status usb_core_stop(struct usb_handle *pdev)
+{
+	/* Free class resources */
+	pdev->class->de_init(pdev, pdev->dev_config);
+
+	/* Stop the low level driver */
+	pdev->driver->stop_device(pdev->data->instance);
+
+	return USBD_OK;
+}
+
+/*
+ * register_usb_driver
+ *         Stop the USB device core.
+ * pdev: Device Handle
+ * pcd_handle: PCD handle
+ * driver: USB driver
+ * driver_handle: USB driver handle
+ * return : USBD Status
+ */
+enum usb_status register_usb_driver(struct usb_handle *pdev,
+				    struct pcd_handle *pcd_handle,
+				    const struct usb_driver *driver,
+				    void *driver_handle)
+{
+	uint8_t i;
+
+	assert(pdev != NULL);
+	assert(pcd_handle != NULL);
+	assert(driver != NULL);
+	assert(driver_handle != NULL);
+
+	/* Free class resources */
+	pdev->driver = driver;
+	pdev->data = pcd_handle;
+	pdev->data->instance = driver_handle;
+	pdev->dev_state = USBD_STATE_DEFAULT;
+	pdev->ep0_state = USBD_EP0_IDLE;
+
+	/* Copy endpoint information */
+	for (i = 0U; i < USBD_EP_NB; i++) {
+		pdev->ep_in[i].maxpacket = pdev->data->in_ep[i].maxpacket;
+		pdev->ep_out[i].maxpacket = pdev->data->out_ep[i].maxpacket;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * register_platform
+ *         Register the USB device core.
+ * pdev: Device Handle
+ * plat_call_back: callback
+ * return : USBD Status
+ */
+enum usb_status register_platform(struct usb_handle *pdev,
+			       const struct usb_desc *plat_call_back)
+{
+	assert(pdev != NULL);
+	assert(plat_call_back != NULL);
+
+	/* Save platform info in class resources */
+	pdev->desc = plat_call_back;
+
+	return USBD_OK;
+}