diff --git a/bl1/aarch64/bl1_context_mgmt.c b/bl1/aarch64/bl1_context_mgmt.c
index b9304dc..9bfb309 100644
--- a/bl1/aarch64/bl1_context_mgmt.c
+++ b/bl1/aarch64/bl1_context_mgmt.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -49,9 +49,9 @@
 	 * Ensure that the build flag to save AArch32 system registers in CPU
 	 * context is not set for AArch64-only platforms.
 	 */
-	if (EL_IMPLEMENTED(1) == EL_IMPL_A64ONLY) {
+	if (el_implemented(1) == EL_IMPL_A64ONLY) {
 		ERROR("EL1 supports AArch64-only. Please set build flag "
-				"CTX_INCLUDE_AARCH32_REGS = 0");
+				"CTX_INCLUDE_AARCH32_REGS = 0\n");
 		panic();
 	}
 #endif
@@ -76,7 +76,7 @@
 				   DISABLE_ALL_EXCEPTIONS);
 	} else {
 		/* Use EL2 if supported; else use EL1. */
-		if (EL_IMPLEMENTED(2)) {
+		if (el_implemented(2) != EL_IMPL_NONE) {
 			next_bl_ep->spsr = SPSR_64(MODE_EL2, MODE_SP_ELX,
 				DISABLE_ALL_EXCEPTIONS);
 		} else {
diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
index 62bea01..77b59ed 100644
--- a/bl31/bl31_main.c
+++ b/bl31/bl31_main.c
@@ -159,9 +159,9 @@
 	 * Ensure that the build flag to save AArch32 system registers in CPU
 	 * context is not set for AArch64-only platforms.
 	 */
-	if (EL_IMPLEMENTED(1) == EL_IMPL_A64ONLY) {
+	if (el_implemented(1) == EL_IMPL_A64ONLY) {
 		ERROR("EL1 supports AArch64-only. Please set build flag "
-				"CTX_INCLUDE_AARCH32_REGS = 0");
+				"CTX_INCLUDE_AARCH32_REGS = 0\n");
 		panic();
 	}
 #endif
diff --git a/docs/marvell/porting.txt b/docs/marvell/porting.txt
index 78000e9..f9a39a0 100644
--- a/docs/marvell/porting.txt
+++ b/docs/marvell/porting.txt
@@ -1,3 +1,5 @@
+.. _porting:
+
 TF-A Porting Guide
 =================
 
@@ -64,3 +66,53 @@
 	- Please refer to '<path_to_mv_ddr_sources>/doc/porting_guide.txt' for detailed porting description.
 	- The build target directory is "build/<platform>/release/ble".
 
+  - Comphy Porting (phy-porting-layer.h or phy-default-porting-layer.h)
+	- Background:
+		Some of the comphy's parameters value depend on the HW connection between the SoC and the PHY. Every
+		board type has specific HW characteristics like wire length. Due to those differences some comphy
+		parameters vary between board types. Therefore each board type can have its own list of values for
+		all relevant comphy parameters. The PHY porting layer specifies which parameters need to be suited and
+		the board designer should provide relevant values.
+
+		.. seealso::
+			For XFI/SFI comphy type there is procedure "rx_training" which eases process of suiting some of
+			the parameters. Please see :ref:`uboot_cmd` section: rx_training.
+
+		The PHY porting layer simplifies updating static values per board type, which are now grouped in one place.
+
+		.. note::
+			The parameters for the same type of comphy may vary even for the same board type, it is because
+			the lanes from comphy-x to some PHY may have different HW characteristic than lanes from
+			comphy-y to the same (multiplexed) or other PHY.
+
+	- Porting:
+		The porting layer for PHY was introduced in TF-A. There is one file
+		``drivers/marvell/comphy/phy-default-porting-layer.h`` which contains the defaults. Those default
+		parameters are used only if there is no appropriate phy-porting-layer.h file under:
+		``plat/marvell/<soc family>/<platform>/board/phy-porting-layer.h``. If the phy-porting-layer.h exists,
+		the phy-default-porting-layer.h is not going to be included.
+
+		.. warning::
+			Not all comphy types are already reworked to support the PHY porting layer, currently the porting
+			layer is supported for XFI/SFI and SATA comphy types.
+
+		The easiest way to prepare the PHY porting layer for custom board is to copy existing example to a new
+		platform:
+
+		- cp ``plat/marvell/a8k/a80x0/board/phy-porting-layer.h`` "plat/marvell/<soc family>/<platform>/board/phy-porting-layer.h"
+		- adjust relevant parameters or
+		- if different comphy index is used for specific feature, move it to proper table entry and then adjust.
+
+		.. note::
+			The final table size with comphy parameters can be different, depending on the CP module count for
+			given SoC type.
+
+	- Example:
+		Example porting layer for armada-8040-db is under: ``plat/marvell/a8k/a80x0/board/phy-porting-layer.h``
+
+		.. note::
+			If there is no PHY porting layer for new platform (missing phy-porting-layer.h), the default
+			values are used (drivers/marvell/comphy/phy-default-porting-layer.h) and the user is warned:
+
+		.. warning::
+			"Using default comphy parameters - it may be required to suit them for your board".
diff --git a/drivers/allwinner/sunxi_rsb.c b/drivers/allwinner/sunxi_rsb.c
new file mode 100644
index 0000000..7075c67
--- /dev/null
+++ b/drivers/allwinner/sunxi_rsb.c
@@ -0,0 +1,136 @@
+/*
+ * Copyright (c) 2017-2018 ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <debug.h>
+#include <delay_timer.h>
+#include <errno.h>
+#include <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/marvell/comphy/comphy-cp110.h b/drivers/marvell/comphy/comphy-cp110.h
index 6afa2c2..1d7aec8 100644
--- a/drivers/marvell/comphy/comphy-cp110.h
+++ b/drivers/marvell/comphy/comphy-cp110.h
@@ -199,6 +199,11 @@
 #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		\
@@ -326,6 +331,16 @@
 #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			\
@@ -357,10 +372,27 @@
 			(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		\
@@ -434,6 +466,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		\
@@ -484,6 +525,19 @@
 #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		\
@@ -510,6 +564,26 @@
 #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		\
@@ -548,6 +622,23 @@
 #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		\
diff --git a/drivers/marvell/comphy/phy-comphy-3700.c b/drivers/marvell/comphy/phy-comphy-3700.c
new file mode 100644
index 0000000..53a59b0
--- /dev/null
+++ b/drivers/marvell/comphy/phy-comphy-3700.c
@@ -0,0 +1,975 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <debug.h>
+#include <delay_timer.h>
+#include <errno.h>
+#include <mmio.h>
+#include <mvebu.h>
+#include <mvebu_def.h>
+#include <spinlock.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)
+
+/*
+ * Below address in used only for reading, therefore no problem with concurrent
+ * Linux access.
+ */
+#define MVEBU_TEST_PIN_LATCH_N (MVEBU_NB_GPIO_REG_BASE + 0x8)
+ #define MVEBU_XTAL_MODE_MASK		BIT(9)
+ #define MVEBU_XTAL_MODE_OFFS		9
+ #define MVEBU_XTAL_CLOCK_25MHZ		0x0
+
+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 */
+};
+
+/* returns reference clock in MHz (25 or 40) */
+static uint32_t get_ref_clk(void)
+{
+	uint32_t val;
+
+	val = (mmio_read_32(MVEBU_TEST_PIN_LATCH_N) & MVEBU_XTAL_MODE_MASK) >>
+		MVEBU_XTAL_MODE_OFFS;
+
+	if (val == MVEBU_XTAL_CLOCK_25MHZ)
+		return 25;
+	else
+		return 40;
+}
+
+/* PHY selector configures with corresponding modes */
+static void 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_HS_SGMII_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;
+error:
+	ERROR("COMPHY[%d] mode[%d] is invalid\n", comphy_index, mode);
+}
+
+/* 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, int mode)
+{
+	/*
+	 * 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 (mode == COMPHY_UNUSED)
+		return;
+
+	if (mode == COMPHY_SATA_MODE)
+		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 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, int mode)
+{
+	reg_set16((reg_offset * PHY_SHFT(USB3) + addr), data, mask);
+}
+
+static void comphy_sgmii_phy_init(uint32_t comphy_index, uint32_t mode,
+				  uintptr_t sd_ip_addr)
+{
+	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 ((mode != COMPHY_SGMII_MODE) &&
+		    (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 = 0;
+	uint32_t offset, data = 0, ref_clk;
+	uintptr_t comphy_indir_regs = COMPHY_INDIRECT_REG;
+	int mode = COMPHY_GET_MODE(comphy_mode);
+	int invert = COMPHY_GET_POLARITY_INVERT(comphy_mode);
+
+	debug_enter();
+
+	/* Configure phy selector for SATA */
+	mvebu_a3700_comphy_set_phy_selector(comphy_index, comphy_mode);
+
+	/* Clear phy isolation mode to make it work in normal mode */
+	offset =  COMPHY_ISOLATION_CTRL_REG + SATAPHY_LANE2_REG_BASE_OFFSET;
+	comphy_set_indirect(comphy_indir_regs, offset, 0, PHY_ISOLATE_MODE,
+			    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_REG + SATAPHY_LANE2_REG_BASE_OFFSET;
+	comphy_set_indirect(comphy_indir_regs, offset, data, TXD_INVERT_BIT |
+			    RXD_INVERT_BIT, mode);
+
+	/* 1. Select 40-bit data width width */
+	offset = COMPHY_LOOPBACK_REG0 + SATAPHY_LANE2_REG_BASE_OFFSET;
+	comphy_set_indirect(comphy_indir_regs, offset, DATA_WIDTH_40BIT,
+			    SEL_DATA_WIDTH_MASK, mode);
+
+	/* 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_CLOCK_SPEED_40M;
+	else
+		ref_clk = REF_CLOCK_SPEED_25M;
+
+	comphy_set_indirect(comphy_indir_regs, offset, ref_clk | PHY_MODE_SATA,
+			    REF_FREF_SEL_MASK | PHY_MODE_MASK, mode);
+
+	/* 3. Use maximum PLL rate (no power save) */
+	offset = COMPHY_KVCO_CAL_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
+	comphy_set_indirect(comphy_indir_regs, offset, USE_MAX_PLL_RATE_BIT,
+			    USE_MAX_PLL_RATE_BIT, mode);
+
+	/* 4. Reset reserved bit */
+	comphy_set_indirect(comphy_indir_regs, COMPHY_RESERVED_REG, 0,
+			    PHYCTRL_FRM_PIN_BIT, mode);
+
+	/* 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_LOOPBACK_REG0 + 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);
+
+	debug_exit();
+
+	return ret;
+}
+
+static int mvebu_a3700_comphy_sgmii_power_on(uint8_t comphy_index,
+					     uint32_t comphy_mode)
+{
+	int ret = 0;
+	uint32_t mask, data, 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 */
+	mvebu_a3700_comphy_set_phy_selector(comphy_index, comphy_mode);
+
+	/* 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_IVEREF_BIT | PIN_TX_IDLE_BIT | PIN_RESET_COMPHY_BIT;
+	mask = 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_HS_SGMII_MODE) {
+		/* HS SGMII (2.5G), SerDes speed 3.125G */
+		data |= SD_SPEED_2_5_G << GEN_RX_SEL_OFFSET;
+		data |= SD_SPEED_2_5_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_REG0_ADDR, sd_ip_addr), data, mask);
+
+	/*
+	 * 9. Set correct reference clock frequency in COMPHY register
+	 * REF_FREF_SEL.
+	 */
+	if (get_ref_clk() == 40)
+		data = REF_CLOCK_SPEED_50M;
+	else
+		data = REF_CLOCK_SPEED_25M;
+
+	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_LOOPBACK_REG0, 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_HS_SGMII_MODE ? "2G5" : "1G");
+	if (get_ref_clk() == 40)
+		comphy_sgmii_phy_init(comphy_index, mode, sd_ip_addr);
+
+	/*
+	 * 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;
+	reg_set16(SGMIIPHY_ADDR(COMPHY_SYNC_PATTERN_REG, sd_ip_addr), data, 0);
+
+	/*
+	 * 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);
+
+	/*
+	 * 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, 0x0);
+
+	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);
+
+
+	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);
+
+	debug_exit();
+
+	return ret;
+}
+
+static int mvebu_a3700_comphy_usb3_power_on(uint8_t comphy_index,
+					    uint32_t comphy_mode)
+{
+	int ret = 0;
+	uintptr_t reg_base = 0;
+	uint32_t mask, data, addr, cfg, ref_clk;
+	void (*usb3_reg_set)(uintptr_t addr, uint32_t reg_offset, uint16_t data,
+			     uint16_t mask, int mode);
+	int mode = COMPHY_GET_MODE(comphy_mode);
+	int invert = COMPHY_GET_POLARITY_INVERT(comphy_mode);
+
+	debug_enter();
+
+	/* Set phy seclector */
+	mvebu_a3700_comphy_set_phy_selector(comphy_index, comphy_mode);
+
+	/* Set usb3 reg access func, Lane2 is indirect access */
+	if (comphy_index == COMPHY_LANE2) {
+		usb3_reg_set = &comphy_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_REG_LANE_CFG0_ADDR, PRD_TXDEEMPH0_MASK,
+		     mask, mode);
+
+	/*
+	 * 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_REG_LANE_CFG0_ADDR 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_REG_LANE_CFG1_ADDR, data, mask, mode);
+
+	/*
+	 * 3. Set Spread Spectrum Clock Enabled
+	 */
+	usb3_reg_set(reg_base, COMPHY_REG_LANE_CFG4_ADDR,
+		     SPREAD_SPECTRUM_CLK_EN, SPREAD_SPECTRUM_CLK_EN, mode);
+
+	/*
+	 * 4. Set Override Margining Controls From the MAC:
+	 *    Use margining signals from lane configuration
+	 */
+	usb3_reg_set(reg_base, COMPHY_REG_TEST_MODE_CTRL_ADDR,
+		     MODE_MARGIN_OVERRIDE, REG_16_BIT_MASK, mode);
+
+	/*
+	 * 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_REG_GLOB_CLK_SRC_LO_ADDR, 0x0,
+		     (MODE_CLK_SRC | BUNDLE_PERIOD_SEL | BUNDLE_PERIOD_SCALE |
+		      BUNDLE_SAMPLE_CTRL | PLL_READY_DLY), mode);
+
+	/*
+	 * 6. Set G2 Spread Spectrum Clock Amplitude at 4K
+	 */
+	usb3_reg_set(reg_base, COMPHY_REG_GEN2_SET_2,
+		     G2_TX_SSC_AMP_VALUE_20, G2_TX_SSC_AMP_MASK, mode);
+
+	/*
+	 * 7. Unset G3 Spread Spectrum Clock Amplitude
+	 *    set G3 TX and RX Register Master Current Select
+	 */
+	mask = G3_TX_SSC_AMP_MASK | G3_VREG_RXTX_MAS_ISET_MASK |
+		RSVD_PH03FH_6_0_MASK;
+	usb3_reg_set(reg_base, COMPHY_REG_GEN2_SET_3,
+		     G3_VREG_RXTX_MAS_ISET_60U, mask, mode);
+
+	/*
+	 * 8. Check crystal jumper setting and program the Power and PLL Control
+	 * accordingly Change RX wait
+	 */
+	if (get_ref_clk() == 40) {
+		ref_clk = REF_CLOCK_SPEED_40M;
+		cfg = CFG_PM_RXDLOZ_WAIT_12_UNIT;
+
+	} else {
+		/* 25 MHz */
+		ref_clk = USB3_REF_CLOCK_SPEED_25M;
+		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, mode);
+
+	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_REG_PWR_MGM_TIM1_ADDR, data, mask, mode);
+
+	/*
+	 * 9. Enable idle sync
+	 */
+	data = UNIT_CTRL_DEFAULT_VALUE | IDLE_SYNC_EN;
+	usb3_reg_set(reg_base, COMPHY_REG_UNIT_CTRL_ADDR, data, REG_16_BIT_MASK,
+		     mode);
+
+	/*
+	 * 10. Enable the output of 500M clock
+	 */
+	data = MISC_REG0_DEFAULT_VALUE | CLK500M_EN;
+	usb3_reg_set(reg_base, COMPHY_MISC_REG0_ADDR, data, REG_16_BIT_MASK,
+		     mode);
+
+	/*
+	 * 11. Set 20-bit data width
+	 */
+	usb3_reg_set(reg_base, COMPHY_LOOPBACK_REG0, DATA_WIDTH_20BIT,
+		     REG_16_BIT_MASK, mode);
+
+	/*
+	 * 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, mode);
+
+	/*
+	 * 13. Check the Polarity invert bit
+	 */
+	if (invert & COMPHY_POLARITY_TXD_INVERT)
+		usb3_reg_set(reg_base, COMPHY_SYNC_PATTERN_REG, TXD_INVERT_BIT,
+			     TXD_INVERT_BIT, mode);
+	if (invert & COMPHY_POLARITY_RXD_INVERT)
+		usb3_reg_set(reg_base, COMPHY_SYNC_PATTERN_REG, RXD_INVERT_BIT,
+			     RXD_INVERT_BIT, mode);
+
+	/*
+	 * 14. Set max speed generation to USB3.0 5Gbps
+	 */
+	usb3_reg_set(reg_base, COMPHY_SYNC_MASK_GEN_REG, PHY_GEN_USB3_5G,
+		     PHY_GEN_MAX_MASK, mode);
+
+	/*
+	 * 15. Set capacitor value for FFE gain peaking to 0xF
+	 */
+	usb3_reg_set(reg_base, COMPHY_REG_GEN3_SETTINGS_3,
+		     COMPHY_GEN_FFE_CAP_SEL_VALUE, COMPHY_GEN_FFE_CAP_SEL_MASK,
+		     mode);
+
+	/*
+	 * 16. Release SW reset
+	 */
+	data = MODE_CORE_CLK_FREQ_SEL | MODE_PIPE_WIDTH_32 | MODE_REFDIV_BY_4;
+	usb3_reg_set(reg_base, COMPHY_REG_GLOB_PHY_CTRL0_ADDR, data,
+		     REG_16_BIT_MASK, mode);
+
+	/* Wait for > 55 us to allow PCLK be enabled */
+	udelay(PLL_SET_DELAY_US);
+
+	if (comphy_index == COMPHY_LANE2) {
+		data = COMPHY_LOOPBACK_REG0 + USB3PHY_LANE2_REG_BASE_OFFSET;
+		mmio_write_32(reg_base + COMPHY_LANE2_INDIR_ADDR_OFFSET,
+			      data);
+
+		addr = COMPHY_LOOPBACK_REG0 + USB3PHY_LANE2_REG_BASE_OFFSET;
+		ret = polling_with_timeout(addr, TXDCLK_PCLK_EN, TXDCLK_PCLK_EN,
+					   COMPHY_PLL_TIMEOUT, REG_32BIT);
+	} else {
+		ret = polling_with_timeout(LANE_STATUS1_ADDR(USB3) + reg_base,
+					   TXDCLK_PCLK_EN, TXDCLK_PCLK_EN,
+					   COMPHY_PLL_TIMEOUT, REG_16BIT);
+	}
+	if (ret)
+		ERROR("Failed to lock USB3 PLL\n");
+
+	debug_exit();
+
+	return ret;
+}
+
+static int mvebu_a3700_comphy_pcie_power_on(uint8_t comphy_index,
+					    uint32_t comphy_mode)
+{
+	int ret;
+	uint32_t ref_clk;
+	int invert = COMPHY_GET_POLARITY_INVERT(comphy_mode);
+
+	debug_enter();
+
+	/* 1. Enable max PLL. */
+	reg_set16(LANE_CFG1_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  USE_MAX_PLL_RATE_EN, 0x0);
+
+	/* 2. Select 20 bit SERDES interface. */
+	reg_set16(GLOB_CLK_SRC_LO_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  CFG_SEL_20B, 0);
+
+	/* 3. Force to use reg setting for PCIe mode */
+	reg_set16(MISC_REG1_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  SEL_BITS_PCIE_FORCE, 0);
+
+	/* 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(UNIT_CTRL_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  UNIT_CTRL_DEFAULT_VALUE | IDLE_SYNC_EN, REG_16_BIT_MASK);
+
+	/* 6. Enable the output of 100M/125M/500M clock */
+	reg_set16(MISC_REG0_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  MISC_REG0_DEFAULT_VALUE | CLK500M_EN | 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_CLOCK_SPEED_40M;
+	else
+		ref_clk = PCIE_REF_CLOCK_SPEED_25M;
+
+	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 */
+	if (invert & COMPHY_POLARITY_TXD_INVERT)
+		reg_set16(SYNC_PATTERN_REG_ADDR(PCIE) + COMPHY_SD_ADDR,
+			  TXD_INVERT_BIT, 0x0);
+
+	if (invert & COMPHY_POLARITY_RXD_INVERT)
+		reg_set16(SYNC_PATTERN_REG_ADDR(PCIE) + COMPHY_SD_ADDR,
+			  RXD_INVERT_BIT, 0x0);
+
+	/* 11. Release SW reset */
+	reg_set16(GLOB_PHY_CTRL0_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  MODE_CORE_CLK_FREQ_SEL | MODE_PIPE_WIDTH_32,
+		  SOFT_RESET | MODE_REFDIV);
+
+	/* Wait for > 55 us to allow PCLK be enabled */
+	udelay(PLL_SET_DELAY_US);
+
+	ret = polling_with_timeout(LANE_STATUS1_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");
+
+	debug_exit();
+
+	return ret;
+}
+
+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_HS_SGMII_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(uint32_t comphy_mode)
+{
+	uintptr_t comphy_indir_regs = COMPHY_INDIRECT_REG;
+	int mode = COMPHY_GET_MODE(comphy_mode);
+	uint32_t offset;
+
+	debug_enter();
+
+	/* Set phy isolation mode */
+	offset = COMPHY_ISOLATION_CTRL_REG + SATAPHY_LANE2_REG_BASE_OFFSET;
+	comphy_set_indirect(comphy_indir_regs, offset, PHY_ISOLATE_MODE,
+			    PHY_ISOLATE_MODE, mode);
+
+	/* Power off PLL, Tx, Rx */
+	offset = COMPHY_POWER_PLL_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
+	comphy_set_indirect(comphy_indir_regs, offset, 0,
+			    PU_PLL_BIT | PU_RX_BIT | PU_TX_BIT, mode);
+
+	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();
+
+	switch (mode) {
+	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(comphy_mode);
+		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_LOOPBACK_REG0 + 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..714a41c
--- /dev/null
+++ b/drivers/marvell/comphy/phy-comphy-3700.h
@@ -0,0 +1,258 @@
+/*
+ * Copyright (C) 2018 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: Lane0 is GBE0; 1: Lane1 is PCIE */
+#define COMPHY_SELECTOR_PCIE_GBE0_SEL_BIT	BIT(0)
+/* bit4: 0: Lane1 is GBE1; 1: Lane1 is USB3 */
+#define COMPHY_SELECTOR_USB3_GBE1_SEL_BIT	BIT(4)
+/* bit8: 0: Lane1 is USB, 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_CLOCK_SPEED_25M		(0x1 << REF_FREF_SEL_OFFSET)
+#define REF_CLOCK_SPEED_30M		(0x2 << REF_FREF_SEL_OFFSET)
+#define PCIE_REF_CLOCK_SPEED_25M	REF_CLOCK_SPEED_30M
+#define USB3_REF_CLOCK_SPEED_25M	REF_CLOCK_SPEED_30M
+#define REF_CLOCK_SPEED_40M		(0x3 << REF_FREF_SEL_OFFSET)
+#define REF_CLOCK_SPEED_50M		(0x4 << 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_RESERVED_REG		0x0E
+#define PHYCTRL_FRM_PIN_BIT		BIT(13)
+
+#define COMPHY_LOOPBACK_REG0		0x23
+#define DIG_LB_EN_ADDR(unit)		(COMPHY_LOOPBACK_REG0 * 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_REG		0x24
+#define SYNC_PATTERN_REG_ADDR(unit)	(COMPHY_SYNC_PATTERN_REG * \
+					 PHY_SHFT(unit))
+#define TXD_INVERT_BIT			BIT(10)
+#define RXD_INVERT_BIT			BIT(11)
+
+#define COMPHY_SYNC_MASK_GEN_REG	0x25
+#define PHY_GEN_MAX_OFFSET		10
+#define PHY_GEN_MAX_MASK		(3 << PHY_GEN_MAX_OFFSET)
+#define PHY_GEN_USB3_5G			(1 << PHY_GEN_MAX_OFFSET)
+
+#define COMPHY_ISOLATION_CTRL_REG	0x26
+#define ISOLATION_CTRL_REG_ADDR(unit)	(COMPHY_ISOLATION_CTRL_REG * \
+					 PHY_SHFT(unit))
+#define PHY_ISOLATE_MODE		BIT(15)
+
+#define COMPHY_MISC_REG0_ADDR		0x4F
+#define MISC_REG0_ADDR(unit)		(COMPHY_MISC_REG0_ADDR * PHY_SHFT(unit))
+#define CLK100M_125M_EN			BIT(4)
+#define CLK500M_EN			BIT(7)
+#define PHY_REF_CLK_SEL			BIT(10)
+#define MISC_REG0_DEFAULT_VALUE		0xA00D
+
+#define COMPHY_REG_GEN2_SET_2	0x3e
+#define GEN2_SETTING_2_ADDR(unit)	(COMPHY_REG_GEN2_SET_2 * PHY_SHFT(unit))
+#define G2_TX_SSC_AMP_VALUE_20		BIT(14)
+#define G2_TX_SSC_AMP_OFF		9
+#define G2_TX_SSC_AMP_LEN		7
+#define G2_TX_SSC_AMP_MASK		(((1 << G2_TX_SSC_AMP_LEN) - 1) << \
+					 G2_TX_SSC_AMP_OFF)
+
+#define COMPHY_REG_GEN2_SET_3	0x3f
+#define GEN2_SETTING_3_ADDR(unit)	(COMPHY_REG_GEN2_SET_3 * PHY_SHFT(unit))
+#define G3_TX_SSC_AMP_OFF		9
+#define G3_TX_SSC_AMP_LEN		7
+#define G3_TX_SSC_AMP_MASK		(((1 << G2_TX_SSC_AMP_LEN) - 1) << \
+					 G2_TX_SSC_AMP_OFF)
+#define G3_VREG_RXTX_MAS_ISET_OFF	7
+#define G3_VREG_RXTX_MAS_ISET_60U	(0 << G3_VREG_RXTX_MAS_ISET_OFF)
+#define G3_VREG_RXTX_MAS_ISET_80U	(1 << G3_VREG_RXTX_MAS_ISET_OFF)
+#define G3_VREG_RXTX_MAS_ISET_100U	(2 << G3_VREG_RXTX_MAS_ISET_OFF)
+#define G3_VREG_RXTX_MAS_ISET_120U	(3 << G3_VREG_RXTX_MAS_ISET_OFF)
+#define G3_VREG_RXTX_MAS_ISET_MASK	(BIT(7) | BIT(8))
+#define RSVD_PH03FH_6_0_OFF		0
+#define RSVD_PH03FH_6_0_LEN		7
+#define RSVD_PH03FH_6_0_MASK		(((1 << RSVD_PH03FH_6_0_LEN) - 1) << \
+					 RSVD_PH03FH_6_0_OFF)
+
+#define COMPHY_REG_UNIT_CTRL_ADDR	0x48
+#define UNIT_CTRL_ADDR(unit)		(COMPHY_REG_UNIT_CTRL_ADDR * \
+					 PHY_SHFT(unit))
+#define IDLE_SYNC_EN			BIT(12)
+#define UNIT_CTRL_DEFAULT_VALUE		0x60
+
+#define COMPHY_MISC_REG1_ADDR		0x73
+#define MISC_REG1_ADDR(unit)		(COMPHY_MISC_REG1_ADDR * PHY_SHFT(unit))
+#define SEL_BITS_PCIE_FORCE		BIT(15)
+
+#define COMPHY_REG_GEN3_SETTINGS_3	0x112
+#define COMPHY_GEN_FFE_CAP_SEL_MASK	0xF
+#define COMPHY_GEN_FFE_CAP_SEL_VALUE	0xF
+
+#define COMPHY_REG_LANE_CFG0_ADDR	0x180
+#define LANE_CFG0_ADDR(unit)		(COMPHY_REG_LANE_CFG0_ADDR * \
+					 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_REG_LANE_CFG1_ADDR	0x181
+#define LANE_CFG1_ADDR(unit)		(COMPHY_REG_LANE_CFG1_ADDR * \
+					 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_REG_LANE_STATUS1_ADDR	0x183
+#define LANE_STATUS1_ADDR(unit)		(COMPHY_REG_LANE_STATUS1_ADDR * \
+					 PHY_SHFT(unit))
+#define TXDCLK_PCLK_EN				BIT(0)
+
+#define COMPHY_REG_LANE_CFG4_ADDR	0x188
+#define LANE_CFG4_ADDR(unit)		(COMPHY_REG_LANE_CFG4_ADDR * \
+					 PHY_SHFT(unit))
+#define SPREAD_SPECTRUM_CLK_EN		BIT(7)
+
+#define COMPHY_REG_GLOB_PHY_CTRL0_ADDR	0x1C1
+#define GLOB_PHY_CTRL0_ADDR(unit)	(COMPHY_REG_GLOB_PHY_CTRL0_ADDR * \
+					 PHY_SHFT(unit))
+#define SOFT_RESET			BIT(0)
+#define MODE_REFDIV			0x30
+#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_REG_TEST_MODE_CTRL_ADDR	0x1C2
+#define TEST_MODE_CTRL_ADDR(unit)	(COMPHY_REG_TEST_MODE_CTRL_ADDR * \
+					 PHY_SHFT(unit))
+#define MODE_MARGIN_OVERRIDE		BIT(2)
+
+#define COMPHY_REG_GLOB_CLK_SRC_LO_ADDR	0x1C3
+#define GLOB_CLK_SRC_LO_ADDR(unit)	(COMPHY_REG_GLOB_CLK_SRC_LO_ADDR * \
+					 PHY_SHFT(unit))
+#define MODE_CLK_SRC			BIT(0)
+#define BUNDLE_PERIOD_SEL		BIT(1)
+#define BUNDLE_PERIOD_SCALE		(BIT(2) | BIT(3))
+#define BUNDLE_SAMPLE_CTRL		BIT(4)
+#define PLL_READY_DLY			(BIT(5) | BIT(6) | BIT(7))
+#define CFG_SEL_20B			BIT(15)
+
+#define COMPHY_REG_PWR_MGM_TIM1_ADDR	0x1D0
+#define PWR_MGM_TIM1_ADDR(unit)		(COMPHY_REG_PWR_MGM_TIM1_ADDR * \
+					 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)
+
+/* SGMII */
+#define COMPHY_PHY_CFG1_OFFSET(lane)	((1 - (lane)) * 0x28)
+#define PIN_PU_IVEREF_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_2_5_G			0x8
+
+/* COMPHY status reg:
+ * lane0: PCIe/GbE0 PHY Status 1
+ * lane1: USB3/GbE1 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..ba1a83c
--- /dev/null
+++ b/drivers/marvell/comphy/phy-comphy-common.h
@@ -0,0 +1,156 @@
+/*
+ * Copyright (C) 2018 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 19-18 represents pcie width (in case of pcie comphy config.)
+ *      - bit 31~20 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_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 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_HS_SGMII_MODE	0x3	/* SGMII 2.5G */
+#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);
+
+	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);
+
+	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
index 19bd182..25893a9 100644
--- a/drivers/marvell/comphy/phy-comphy-cp110.c
+++ b/drivers/marvell/comphy/phy-comphy-cp110.c
@@ -15,78 +15,15 @@
 #include <spinlock.h>
 #include "mvebu.h"
 #include "comphy-cp110.h"
+#include "phy-comphy-cp110.h"
+#include "phy-comphy-common.h"
 
-/* #define DEBUG_COMPHY */
-#ifdef DEBUG_COMPHY
-#define debug(format...) printf(format)
+#if __has_include("phy-porting-layer.h")
+#include "phy-porting-layer.h"
 #else
-#define debug(format, arg...)
+#include "phy-default-porting-layer.h"
 #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 19-18 represents pcie width (in case of pcie comphy config.)
- *      - bit 31~20 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_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)
-
-#define COMPHY_SATA_MODE	0x1
-#define COMPHY_SGMII_MODE	0x2	/* SGMII 1G */
-#define COMPHY_HS_SGMII_MODE	0x3	/* SGMII 2.5G */
-#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
-
 /* COMPHY speed macro */
 #define COMPHY_SPEED_1_25G		0 /* SGMII 1G */
 #define COMPHY_SPEED_2_5G		1
@@ -129,21 +66,6 @@
  */
 spinlock_t cp110_mac_reset_lock;
 
-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,
-};
-
 /* These values come from the PCI Express Spec */
 enum pcie_link_width {
 	PCIE_LNK_WIDTH_RESRV	= 0x00,
@@ -157,36 +79,24 @@
 	PCIE_LNK_WIDTH_UNKNOWN  = 0xFF,
 };
 
-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)
+_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)
 {
-	uint32_t data;
+#if (AP_NUM == 1)
+	*ap_nr = 0;
+#else
+	*ap_nr = (((comphy_base & ~0xffffff) - MVEBU_AP_IO_BASE(0)) /
+			 AP_IO_OFFSET);
+#endif
 
-	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);
+	*cp_nr = (((comphy_base & ~0xffffff) - MVEBU_AP_IO_BASE(*ap_nr)) /
+		 MVEBU_CP_OFFSET);
 
-	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 = %#010lx, data = %#010x (mask = %#010x) - ",
-	      addr, data, mask);
-	debug("old value = %#010x ==> ", mmio_read_32(addr));
-	mmio_clrsetbits_32(addr, mask, data);
-
-	debug("new val %#010x\n", mmio_read_32(addr));
+	debug("cp_base 0x%llx, 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 */
@@ -413,10 +323,17 @@
 {
 	uintptr_t hpipe_addr, sd_ip_addr, comphy_addr;
 	uint32_t mask, data;
+	uint8_t ap_nr, cp_nr;
 	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];
+
+
 	/* configure phy selector for SATA */
 	mvebu_cp110_comphy_set_phy_selector(comphy_base,
 					    comphy_index, comphy_mode);
@@ -480,13 +397,17 @@
 	debug("stage: Analog parameters from ETP(HW)\n");
 	/* G1 settings */
 	mask = HPIPE_G1_SET_1_G1_RX_SELMUPI_MASK;
-	data = 0x0 << HPIPE_G1_SET_1_G1_RX_SELMUPI_OFFSET;
+	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 |= 0x1 << HPIPE_G1_SET_1_G1_RX_SELMUPF_OFFSET;
+	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 |= 0x0 << HPIPE_G1_SET_1_G1_RX_SELMUFI_OFFSET;
+	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 |= 0x3 << HPIPE_G1_SET_1_G1_RX_SELMUFF_OFFSET;
+	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);
@@ -505,26 +426,34 @@
 
 	/* G2 settings */
 	mask = HPIPE_G2_SET_1_G2_RX_SELMUPI_MASK;
-	data = 0x0 << HPIPE_G2_SET_1_G2_RX_SELMUPI_OFFSET;
+	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 |= 0x1 << HPIPE_G2_SET_1_G2_RX_SELMUPF_OFFSET;
+	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 |= 0x0 << HPIPE_G2_SET_1_G2_RX_SELMUFI_OFFSET;
+	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 |= 0x3 << HPIPE_G2_SET_1_G2_RX_SELMUFF_OFFSET;
+	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 = 0x2 << HPIPE_G3_SET_1_G3_RX_SELMUPI_OFFSET;
+	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 |= 0x2 << HPIPE_G3_SET_1_G3_RX_SELMUPF_OFFSET;
+	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 |= 0x3 << HPIPE_G3_SET_1_G3_RX_SELMUFI_OFFSET;
+	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 |= 0x3 << HPIPE_G3_SET_1_G3_RX_SELMUFF_OFFSET;
+	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;
@@ -577,9 +506,11 @@
 
 	/* G3 Setting 3 */
 	mask = HPIPE_G3_FFE_CAP_SEL_MASK;
-	data = 0xf << HPIPE_G3_FFE_CAP_SEL_OFFSET;
+	data = sata_static_values->g3_ffe_cap_sel <<
+			HPIPE_G3_FFE_CAP_SEL_OFFSET;
 	mask |= HPIPE_G3_FFE_RES_SEL_MASK;
-	data |= 0x4 << HPIPE_G3_FFE_RES_SEL_OFFSET;
+	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;
@@ -590,12 +521,12 @@
 
 	/* G3 Setting 4 */
 	mask = HPIPE_G3_DFE_RES_MASK;
-	data = 0x1 << HPIPE_G3_DFE_RES_OFFSET;
+	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 = 0x61 << HPIPE_OS_PH_OFFSET_OFFSET;
+	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;
@@ -610,41 +541,77 @@
 
 	/* Set G1 TX amplitude and TX post emphasis value */
 	mask = HPIPE_G1_SET_0_G1_TX_AMP_MASK;
-	data = 0x8 << HPIPE_G1_SET_0_G1_TX_AMP_OFFSET;
+	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 |= 0x1 << HPIPE_G1_SET_0_G1_TX_AMP_ADJ_OFFSET;
+	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 |= 0x1 << HPIPE_G1_SET_0_G1_TX_EMPH1_OFFSET;
+	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 |= 0x1 << HPIPE_G1_SET_0_G1_TX_EMPH1_EN_OFFSET;
+	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 = 0xa << HPIPE_G2_SET_0_G2_TX_AMP_OFFSET;
+	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 |= 0x1 << HPIPE_G2_SET_0_G2_TX_AMP_ADJ_OFFSET;
+	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 |= 0x2 << HPIPE_G2_SET_0_G2_TX_EMPH1_OFFSET;
+	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 |= 0x1 << HPIPE_G2_SET_0_G2_TX_EMPH1_EN_OFFSET;
+	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 = 0x1e << HPIPE_G3_SET_0_G3_TX_AMP_OFFSET;
+	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 |= 0x1 << HPIPE_G3_SET_0_G3_TX_AMP_ADJ_OFFSET;
+	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 |= 0xe << HPIPE_G3_SET_0_G3_TX_EMPH1_OFFSET;
+	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 |= 0x1 << HPIPE_G3_SET_0_G3_TX_EMPH1_EN_OFFSET;
+	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;
@@ -779,7 +746,7 @@
 	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 datas */
+	/* 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,
@@ -835,9 +802,37 @@
 	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)) {
@@ -966,16 +961,27 @@
 		data = 0x6 << HPIPE_G1_SET_0_G1_TX_EMPH1_OFFSET;
 	} else {
 		mask = HPIPE_G1_SET_0_G1_TX_AMP_MASK;
-		data = 0x1c << HPIPE_G1_SET_0_G1_TX_AMP_OFFSET;
+		data = xfi_static_values->g1_amp <<
+				HPIPE_G1_SET_0_G1_TX_AMP_OFFSET;
 		mask |= HPIPE_G1_SET_0_G1_TX_EMPH1_MASK;
-		data |= 0xe << HPIPE_G1_SET_0_G1_TX_EMPH1_OFFSET;
+		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 = 0x0 << HPIPE_G1_SET_2_G1_TX_EMPH0_OFFSET;
+	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 |= 0x1 << HPIPE_G1_SET_2_G1_TX_EMPH0_EN_OFFSET;
+	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;
@@ -1004,13 +1010,17 @@
 		data |= 0x1 << HPIPE_G1_SET_1_G1_RX_SELMUPF_OFFSET;
 	} else {
 		mask |= HPIPE_G1_SET_1_G1_RX_SELMUPI_MASK;
-		data |= 0x2 << HPIPE_G1_SET_1_G1_RX_SELMUPI_OFFSET;
+		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 |= 0x2 << HPIPE_G1_SET_1_G1_RX_SELMUPF_OFFSET;
+		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 |= 0x0 << HPIPE_G1_SET_1_G1_RX_SELMUFI_OFFSET;
+		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 |= 0x1 << HPIPE_G1_SET_1_G1_RX_SELMUFF_OFFSET;
+		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;
 	}
@@ -1038,8 +1048,43 @@
 		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);
 	}
-	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_3_REG, data, mask);
 
 	/* Connfigure RX training timer */
 	mask = HPIPE_RX_TRAIN_TIMER_MASK;
@@ -1949,192 +1994,255 @@
 	return ret;
 }
 
-/* This function performs RX training for one Feed Forward Equalization (FFE)
- * value.
- * The RX traiing result is stored in 'Saved DFE values Register' (SAV_F0D).
- *
- * Return '0' on success, error code in  a case of failure.
- */
-static int mvebu_cp110_comphy_test_single_ffe(uint64_t comphy_base,
-					      uint8_t comphy_index,
-					      uint32_t ffe, uint32_t *result)
+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, sd_ip_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);
-
 	sd_ip_addr = SD_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
 			     comphy_index);
 
+	debug_enter();
+
+	debug("stage: RF Reset\n");
+
+	/* 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 50ms - until band gap and ref clock ready */
+	mdelay(50);
+
+	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("PRBS31 loppback\n\n");
+
 	/* Configure PRBS counters */
 	mask = HPIPE_PHY_TEST_PATTERN_SEL_MASK;
 	data = 0xe << HPIPE_PHY_TEST_PATTERN_SEL_OFFSET;
 	reg_set(hpipe_addr + HPIPE_PHY_TEST_CONTROL_REG, data, mask);
 
 	mask = HPIPE_PHY_TEST_DATA_MASK;
-	data = 0x64 << HPIPE_PHY_TEST_DATA_OFFSET;
+	data = 0xc4 << HPIPE_PHY_TEST_DATA_OFFSET;
 	reg_set(hpipe_addr + HPIPE_PHY_TEST_DATA_REG, data, mask);
 
 	mask = HPIPE_PHY_TEST_EN_MASK;
 	data = 0x1 << HPIPE_PHY_TEST_EN_OFFSET;
 	reg_set(hpipe_addr + HPIPE_PHY_TEST_CONTROL_REG, data, mask);
 
-	mdelay(50);
+	mdelay(10);
+	debug("Enable TX/RX training\n\n");
 
-	/* Set the FFE value */
-	mask = HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_MASK;
-	data = ffe << HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_OFFSET;
-	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_3_REG, data, mask);
-
-	/* Start RX training */
-	mask = SD_EXTERNAL_STATUS_START_RX_TRAINING_MASK;
-	data = 1 << SD_EXTERNAL_STATUS_START_RX_TRAINING_OFFSET;
-	reg_set(sd_ip_addr + SD_EXTERNAL_STATUS_REG, data, mask);
+	mask = HPIPE_TRX_RX_TRAIN_EN_MASK;
+	data = 0x1 << HPIPE_TRX_RX_TRAIN_EN_OFFSET;
+	mask |= HPIPE_TRX_RX_ANA_IF_CLK_ENE_MASK;
+	data |= 0x1 << HPIPE_TRX_RX_ANA_IF_CLK_ENE_OFFSET;
+	mask |= HPIPE_TRX_TX_CTRL_CLK_EN_MASK;
+	data |= 0x1 << HPIPE_TRX_TX_CTRL_CLK_EN_OFFSET;
+	mask |= HPIPE_TRX_UPDATE_THEN_HOLD_MASK;
+	data |= 0x1 << HPIPE_TRX_UPDATE_THEN_HOLD_OFFSET;
+	mask |= HPIPE_TRX_TX_F0T_EO_BASED_MASK;
+	data |= 0x1 << HPIPE_TRX_TX_F0T_EO_BASED_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(sd_ip_addr + SD_EXTERNAL_STATAUS1_REG);
-		if (data & SD_EXTERNAL_STATAUS1_REG_RX_TRAIN_COMP_MASK)
+		data = mmio_read_32(hpipe_addr + HPIPE_INTERRUPT_1_REGISTER);
+		if (data & mask)
 			break;
 		mdelay(1);
 		timeout--;
 	}
 
-	if (timeout == 0)
+	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 & SD_EXTERNAL_STATAUS1_REG_RX_TRAIN_FAILED_MASK)
+	if (data & HPIPE_TRX_TRAIN_FAILED_MASK) {
+		ERROR("Rx training failed...\n");
 		return -EINVAL;
+	}
 
-	/* Stop RX training */
-	mask = SD_EXTERNAL_STATUS_START_RX_TRAINING_MASK;
-	data = 0 << SD_EXTERNAL_STATUS_START_RX_TRAINING_OFFSET;
-	reg_set(sd_ip_addr + SD_EXTERNAL_STATUS_REG, data, mask);
+	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);
 
-	/* Read the result */
-	data = mmio_read_32(hpipe_addr + HPIPE_SAVED_DFE_VALUES_REG);
-	data &= HPIPE_SAVED_DFE_VALUES_SAV_F0D_MASK;
-	data >>= HPIPE_SAVED_DFE_VALUES_SAV_F0D_OFFSET;
-	*result = data;
+	debug("Training done, reading results...\n\n");
 
-	mask = HPIPE_PHY_TEST_RESET_MASK;
-	data = 0x1 << HPIPE_PHY_TEST_RESET_OFFSET;
-	mask |= HPIPE_PHY_TEST_EN_MASK;
-	data |= 0x0 << HPIPE_PHY_TEST_EN_OFFSET;
-	reg_set(hpipe_addr + HPIPE_PHY_TEST_CONTROL_REG, data, mask);
+	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_PHY_TEST_RESET_MASK;
-	data = 0x0 << HPIPE_PHY_TEST_RESET_OFFSET;
-	reg_set(hpipe_addr + HPIPE_PHY_TEST_CONTROL_REG, data, mask);
+	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);
 
-	return 0;
-}
+	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);
 
-/* This function runs complete RX training sequence:
- *	- Run RX training for all possible Feed Forward Equalization values
- *	- Choose the FFE which gives the best result.
- *	- Run RX training again with the best result.
- *
- * Return '0' on success, error code in  a case of failure.
- */
-int mvebu_cp110_comphy_xfi_rx_training(uint64_t comphy_base,
-					      uint8_t comphy_index)
-{
-	uint32_t mask, data, max_rx_train = 0, max_rx_train_index = 0;
-	uintptr_t hpipe_addr;
-	uint32_t rx_train_result;
-	int ret, i;
+	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);
 
-	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
-				comphy_index);
+	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");
 
-	debug_enter();
+	/* 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);
 
-	/* Configure SQ threshold and CDR lock */
-	mask = HPIPE_SQUELCH_THRESH_IN_MASK;
-	data = 0xc << HPIPE_SQUELCH_THRESH_IN_OFFSET;
-	reg_set(hpipe_addr + HPIPE_SQUELCH_FFE_SETTING_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);
 
-	mask = HPIPE_SQ_DEGLITCH_WIDTH_P_MASK;
-	data = 0xf << HPIPE_SQ_DEGLITCH_WIDTH_P_OFFSET;
-	mask |= HPIPE_SQ_DEGLITCH_WIDTH_N_MASK;
-	data |= 0xf << HPIPE_SQ_DEGLITCH_WIDTH_N_OFFSET;
-	mask |= HPIPE_SQ_DEGLITCH_EN_MASK;
-	data |= 0x1 << HPIPE_SQ_DEGLITCH_EN_OFFSET;
-	reg_set(hpipe_addr + HPIPE_SQ_GLITCH_FILTER_CTRL, data, mask);
-
-	mask = HPIPE_CDR_LOCK_DET_EN_MASK;
-	data = 0x1 << HPIPE_CDR_LOCK_DET_EN_OFFSET;
-	reg_set(hpipe_addr + HPIPE_LOOPBACK_REG, data, mask);
-
-	udelay(100);
-
-	/* Determine if we have a cable attached to this comphy, if not,
-	 * we can't perform RX training.
+	/* Bypass the FFE table settings and use the FFE settings directly from
+	 * registers FFE_RES_SEL and FFE_CAP_SEL
 	 */
-	data = mmio_read_32(hpipe_addr + HPIPE_SQUELCH_FFE_SETTING_REG);
-	if (data & HPIPE_SQUELCH_DETECTED_MASK) {
-		ERROR("Squelsh is not detected, can't perform RX training\n");
-		return -EINVAL;
-	}
+	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);
 
-	data = mmio_read_32(hpipe_addr + HPIPE_LOOPBACK_REG);
-	if (!(data & HPIPE_CDR_LOCK_MASK)) {
-		ERROR("CDR is not locked, can't perform RX training\n");
-		return -EINVAL;
-	}
+	/* 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);
 
-	/* Do preparations for RX training */
+	/* Update align90 */
+	mask = HPIPE_CAL_OS_PH_EXT_MASK;
+	data = 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);
 
-	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_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);
+	/* 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);
 
-	/* Perform RX training for all possible FFE (Feed Forward
-	 * Equalization, possible values are 0-7).
-	 * We update the best value reached and the FFE which gave this value.
-	 */
-	for (i = 0; i < MAX_NUM_OF_FFE; i++) {
-		rx_train_result = 0;
-		ret = mvebu_cp110_comphy_test_single_ffe(comphy_base,
-							 comphy_index, i,
-							 &rx_train_result);
+	debug("PRBS31 loppback\n\n");
 
-		if ((!ret) && (rx_train_result > max_rx_train)) {
-			max_rx_train = rx_train_result;
-			max_rx_train_index = i;
-		}
-	}
+	mask = HPIPE_PHY_TEST_PT_TESTMODE_MASK;
+	data = 0x1 << HPIPE_PHY_TEST_PT_TESTMODE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHY_TEST_OOB_0_REGISTER, data, mask);
 
-	/* If we were able to determine which FFE gives the best value,
-	 * now we need to set it and run RX training again (only for this
-	 * FFE).
-	 */
-	if (max_rx_train) {
-		ret = mvebu_cp110_comphy_test_single_ffe(comphy_base,
-							 comphy_index,
-							 max_rx_train_index,
-							 &rx_train_result);
+	/* Configure PRBS counters */
+	mask = HPIPE_PHY_TEST_PATTERN_SEL_MASK;
+	data = 0xe << HPIPE_PHY_TEST_PATTERN_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHY_TEST_CONTROL_REG, data, mask);
 
-		if (ret == 0)
-			debug("RX Training passed (FFE = %d, result = 0x%x)\n",
-			       max_rx_train_index, rx_train_result);
-	} else {
-		ERROR("RX Training failed for comphy%d\n", comphy_index);
-		ret = -EINVAL;
-	}
+	mask = HPIPE_PHY_TEST_DATA_MASK;
+	data = 0xc4 << HPIPE_PHY_TEST_DATA_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHY_TEST_DATA_REG, data, mask);
 
-	debug_exit();
+	mask = HPIPE_PHY_TEST_EN_MASK;
+	data = 0x1 << HPIPE_PHY_TEST_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHY_TEST_CONTROL_REG, data, mask);
 
-	return ret;
+	/* Reset PRBS error counter */
+	mask = HPIPE_PHY_TEST_PATTERN_SEL_MASK;
+	data = 0x1 << HPIPE_PHY_TEST_RESET_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHY_TEST_CONTROL_REG, data, mask);
+
+	mask = HPIPE_PHY_TEST_PATTERN_SEL_MASK;
+	data = 0x0 << HPIPE_PHY_TEST_RESET_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHY_TEST_CONTROL_REG, data, mask);
+
+	mask = HPIPE_PHY_TEST_PT_TESTMODE_MASK;
+	data = 0x1 << HPIPE_PHY_TEST_PT_TESTMODE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHY_TEST_OOB_0_REGISTER, data, mask);
+
+	printf("########################################################\n");
+	printf("# To use trained values update the ATF sources:\n");
+	printf("# plat/marvell/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");
+
+	/* check */
+	debug("PRBS error counter[0x%lx] 0x%x\n\n",
+	      hpipe_addr + HPIPE_PHY_TEST_PRBS_ERROR_COUNTER_1_REG,
+	      mmio_read_32(hpipe_addr +
+			   HPIPE_PHY_TEST_PRBS_ERROR_COUNTER_1_REG));
+
+	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
@@ -2237,6 +2345,7 @@
 		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,
@@ -2261,9 +2370,18 @@
 {
 	uintptr_t sd_ip_addr, comphy_ip_addr;
 	uint32_t mask, data;
+	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;
+	}
+
 	sd_ip_addr = SD_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
 			     comphy_index);
 	comphy_ip_addr = COMPHY_ADDR(comphy_base, comphy_index);
diff --git a/drivers/marvell/comphy/phy-comphy-cp110.h b/drivers/marvell/comphy/phy-comphy-cp110.h
index 2461e5c..70dbfbf 100644
--- a/drivers/marvell/comphy/phy-comphy-cp110.h
+++ b/drivers/marvell/comphy/phy-comphy-cp110.h
@@ -5,7 +5,79 @@
  * https://spdx.org/licenses
  */
 
-/* Marvell CP110 SoC COMPHY unit driver */
+/* 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/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;
+
+	_Bool valid;
+};
 
 int mvebu_cp110_comphy_is_pll_locked(uint64_t comphy_base,
 				     uint8_t comphy_index);
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..39cd181
--- /dev/null
+++ b/drivers/marvell/comphy/phy-default-porting-layer.h
@@ -0,0 +1,51 @@
+/*
+ * 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,
+		.valid = 0x1
+	},
+};
+#endif /* __PHY_DEFAULT_PORTING_LAYER_H */
diff --git a/drivers/marvell/uart/a3700_console.S b/drivers/marvell/uart/a3700_console.S
new file mode 100644
index 0000000..6575af4
--- /dev/null
+++ b/drivers/marvell/uart/a3700_console.S
@@ -0,0 +1,168 @@
+/*
+ * Copyright (C) 2016 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <asm_macros.S>
+#include <a3700_console.h>
+
+	.globl	console_core_init
+	.globl	console_core_putc
+	.globl	console_core_getc
+	.globl	console_core_flush
+
+	/* -----------------------------------------------
+	 * int console_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
+	 * -----------------------------------------------
+	 */
+func console_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	w2, w2, #0x3ff
+
+	ldr	w3, [x0, #UART_BAUD_REG]
+	bic	w3, w3, 0x3ff
+	orr	w3, w3, w2
+	str	w3, [x0, #UART_BAUD_REG]/* set baud rate divisor */
+
+	/* Set UART to default 16X scheme */
+	mov	w3, #0
+	str	w3, [x0, #UART_POSSR_REG]
+
+	/*
+	 * Wait for the TX FIFO to be empty. If wait for 20ms, the TX FIFO is
+	 * still not empty, TX FIFO will reset by all means.
+	 */
+	mov	w1, #20				/* max time out 20ms */
+2:
+	/* Check whether TX FIFO is empty */
+	ldr	w3, [x0, #UART_STATUS_REG]
+	and	w3, w3, #UARTLSR_TXFIFOEMPTY
+	cmp	w3, #0
+	b.ne	4f
+
+	/* Delay */
+	mov	w2, #30000
+3:
+	sub     w2, w2, #1
+	cmp	w2, #0
+	b.ne	3b
+
+	/* Check whether 10ms is waited */
+	sub     w1, w1, #1
+	cmp	w1, #0
+	b.ne	2b
+
+4:
+	/* Reset FIFO */
+	mov	w3, #UART_CTRL_RXFIFO_RESET
+	orr	w3, w3, #UART_CTRL_TXFIFO_RESET
+	str	w3, [x0, #UART_CTRL_REG]
+
+	/* Delay */
+	mov	w2, #2000
+1:
+	sub     w2, w2, #1
+	cmp	w2, #0
+	b.ne	1b
+
+	/* 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_core_init
+
+	/* --------------------------------------------------------
+	 * int console_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_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_core_putc
+
+	/* ---------------------------------------------
+	 * int console_core_getc(void)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 on error.
+	 * In : w0 - console base address
+	 * Out : return -1 on error else return character.
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func console_core_getc
+	/* Check if the receive FIFO is empty */
+	ret
+getc_error:
+	mov	w0, #-1
+	ret
+endfunc console_core_getc
+
+	/* ---------------------------------------------
+	 * int console_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 console_core_flush
+	/* Placeholder */
+	mov	w0, #0
+	ret
+endfunc console_core_flush
diff --git a/drivers/marvell/uart/a3700_console.h b/drivers/marvell/uart/a3700_console.h
new file mode 100644
index 0000000..1831360
--- /dev/null
+++ b/drivers/marvell/uart/a3700_console.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2016 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef __A3700_CONSOLE_H__
+#define __A3700_CONSOLE_H__
+
+/* MVEBU UART Registers */
+#define UART_RX_REG		0x00
+#define UART_TX_REG		0x04
+#define UART_CTRL_REG		0x08
+#define UART_STATUS_REG		0x0c
+#define UART_BAUD_REG		0x10
+#define UART_POSSR_REG		0x14
+
+/* 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 */
+
+/* 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_TXFIFOFULL	(1 << 11)	/* Tx Fifo Full */
+
+/* UART Control Register bits */
+#define UART_CTRL_RXFIFO_RESET	(1 << 14)
+#define UART_CTRL_TXFIFO_RESET	(1 << 15)
+#define UARTLSR_TXFIFOEMPTY	(1 << 6)
+
+#endif	/* __A3700_CONSOLE_H__ */
diff --git a/include/common/ep_info.h b/include/common/ep_info.h
index bf3f782..db2355a 100644
--- a/include/common/ep_info.h
+++ b/include/common/ep_info.h
@@ -30,7 +30,7 @@
 #define PARAM_EP_SECURITY_MASK		U(0x1)
 
 /* Secure or Non-secure image */
-#define GET_SECURITY_STATE(x) (x & PARAM_EP_SECURITY_MASK)
+#define GET_SECURITY_STATE(x) ((x) & PARAM_EP_SECURITY_MASK)
 #define SET_SECURITY_STATE(x, security) \
 			((x) = ((x) & ~PARAM_EP_SECURITY_MASK) | (security))
 
diff --git a/include/drivers/allwinner/sunxi_rsb.h b/include/drivers/allwinner/sunxi_rsb.h
new file mode 100644
index 0000000..5a69d35
--- /dev/null
+++ b/include/drivers/allwinner/sunxi_rsb.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2017-2018 ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SUNXI_RSB_H
+#define SUNXI_RSB_H
+
+#include <stdint.h>
+
+int rsb_init_controller(void);
+int rsb_set_bus_speed(uint32_t source_freq, uint32_t bus_freq);
+int rsb_set_device_mode(uint32_t device_mode);
+int rsb_assign_runtime_address(uint16_t hw_addr, uint8_t rt_addr);
+
+int rsb_read(uint8_t rt_addr, uint8_t reg_addr);
+int rsb_write(uint8_t rt_addr, uint8_t reg_addr, uint8_t value);
+
+#endif
diff --git a/include/drivers/arm/tzc_common.h b/include/drivers/arm/tzc_common.h
index dac79aa..4b81547 100644
--- a/include/drivers/arm/tzc_common.h
+++ b/include/drivers/arm/tzc_common.h
@@ -73,10 +73,10 @@
 
 /* Macros for allowing Non-Secure access to a region based on NSAID */
 #define TZC_REGION_ACCESS_RD(nsaid)				\
-	((U(1) << (nsaid & TZC_REGION_ACCESS_ID_MASK)) <<	\
+	((U(1) << ((nsaid) & TZC_REGION_ACCESS_ID_MASK)) <<	\
 	 TZC_REGION_ACCESS_RD_EN_SHIFT)
 #define TZC_REGION_ACCESS_WR(nsaid)				\
-	((U(1) << (nsaid & TZC_REGION_ACCESS_ID_MASK)) <<	\
+	((U(1) << ((nsaid) & TZC_REGION_ACCESS_ID_MASK)) <<	\
 	 TZC_REGION_ACCESS_WR_EN_SHIFT)
 #define TZC_REGION_ACCESS_RDWR(nsaid)				\
 	(TZC_REGION_ACCESS_RD(nsaid) |				\
diff --git a/include/lib/aarch32/arch_helpers.h b/include/lib/aarch32/arch_helpers.h
index 5d9c1c1..03f0e86 100644
--- a/include/lib/aarch32/arch_helpers.h
+++ b/include/lib/aarch32/arch_helpers.h
@@ -4,8 +4,8 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef __ARCH_HELPERS_H__
-#define __ARCH_HELPERS_H__
+#ifndef ARCH_HELPERS_H
+#define ARCH_HELPERS_H
 
 #include <arch.h>	/* for additional register definitions */
 #include <cdefs.h>
@@ -381,4 +381,4 @@
 #define write_icc_sgi0r_el1(_v) \
 		write64_icc_sgi0r_el1(_v)
 
-#endif /* __ARCH_HELPERS_H__ */
+#endif /* ARCH_HELPERS_H */
diff --git a/include/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h
index d90061f..61f9830 100644
--- a/include/lib/aarch64/arch_helpers.h
+++ b/include/lib/aarch64/arch_helpers.h
@@ -4,11 +4,12 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef __ARCH_HELPERS_H__
-#define __ARCH_HELPERS_H__
+#ifndef ARCH_HELPERS_H
+#define ARCH_HELPERS_H
 
 #include <arch.h>	/* for additional register definitions */
 #include <cdefs.h>	/* For __dead2 */
+#include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
 
@@ -363,12 +364,22 @@
 }
 
 /*
- * Check if an EL is implemented from AA64PFR0 register fields. 'el' argument
- * must be one of 1, 2 or 3.
+ * Check if an EL is implemented from AA64PFR0 register fields.
  */
-#define EL_IMPLEMENTED(el) \
-	((read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL##el##_SHIFT) \
-		& ID_AA64PFR0_ELX_MASK)
+static inline uint64_t el_implemented(unsigned int el)
+{
+	if (el > 3U) {
+		return EL_IMPL_NONE;
+	} else {
+		unsigned int shift = ID_AA64PFR0_EL1_SHIFT * el;
+
+		return (read_id_aa64pfr0_el1() >> shift) & ID_AA64PFR0_ELX_MASK;
+	}
+}
+
+#if !ERROR_DEPRECATED
+#define EL_IMPLEMENTED(_el)	el_implemented(_el)
+#endif
 
 /* Previously defined accesor functions with incomplete register names  */
 
@@ -389,4 +400,4 @@
 #define read_cpacr()		read_cpacr_el1()
 #define write_cpacr(_v)		write_cpacr_el1(_v)
 
-#endif /* __ARCH_HELPERS_H__ */
+#endif /* ARCH_HELPERS_H */
diff --git a/include/lib/bakery_lock.h b/include/lib/bakery_lock.h
index c80082e..2d1612e 100644
--- a/include/lib/bakery_lock.h
+++ b/include/lib/bakery_lock.h
@@ -4,8 +4,8 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef __BAKERY_LOCK_H__
-#define __BAKERY_LOCK_H__
+#ifndef BAKERY_LOCK_H
+#define BAKERY_LOCK_H
 
 #include <platform_def.h>
 
@@ -13,21 +13,39 @@
 
 #ifndef __ASSEMBLY__
 #include <cdefs.h>
+#include <stdbool.h>
 #include <stdint.h>
+#include <utils_def.h>
 
 /*****************************************************************************
- * Internal helper macros used by the bakery lock implementation.
+ * Internal helpers used by the bakery lock implementation.
  ****************************************************************************/
+
 /* Convert a ticket to priority */
-#define PRIORITY(t, pos)	(((t) << 8) | (pos))
+static inline unsigned int bakery_get_priority(unsigned int t, unsigned int pos)
+{
+	return (t << 8) | pos;
+}
 
-#define CHOOSING_TICKET		0x1
-#define CHOSEN_TICKET		0x0
+#define CHOOSING_TICKET		U(0x1)
+#define CHOSEN_TICKET		U(0x0)
 
-#define bakery_is_choosing(info)	(info & 0x1)
-#define bakery_ticket_number(info)	((info >> 1) & 0x7FFF)
-#define make_bakery_data(choosing, number) \
-		(((choosing & 0x1) | (number << 1)) & 0xFFFF)
+static inline bool bakery_is_choosing(unsigned int info)
+{
+	return (info & 1U) == CHOOSING_TICKET;
+}
+
+static inline unsigned int bakery_ticket_number(unsigned int info)
+{
+	return (info >> 1) & 0x7FFFU;
+}
+
+static inline uint16_t make_bakery_data(unsigned int choosing, unsigned int num)
+{
+	unsigned int val = (choosing & 0x1U) | (num << 1);
+
+	return (uint16_t) val;
+}
 
 /*****************************************************************************
  * External bakery lock interface.
@@ -83,4 +101,4 @@
 
 
 #endif /* __ASSEMBLY__ */
-#endif /* __BAKERY_LOCK_H__ */
+#endif /* BAKERY_LOCK_H */
diff --git a/include/lib/el3_runtime/aarch32/context.h b/include/lib/el3_runtime/aarch32/context.h
index 6447360..1ea19ca 100644
--- a/include/lib/el3_runtime/aarch32/context.h
+++ b/include/lib/el3_runtime/aarch32/context.h
@@ -1,26 +1,28 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef __CONTEXT_H__
-#define __CONTEXT_H__
+#ifndef CONTEXT_H
+#define CONTEXT_H
+
+#include <utils_def.h>
 
 /*******************************************************************************
  * Constants that allow assembler code to access members of and the 'regs'
  * structure at their correct offsets.
  ******************************************************************************/
-#define CTX_REGS_OFFSET		0x0
-#define CTX_GPREG_R0		0x0
-#define CTX_GPREG_R1		0x4
-#define CTX_GPREG_R2		0x8
-#define CTX_GPREG_R3		0xC
-#define CTX_LR			0x10
-#define CTX_SCR			0x14
-#define CTX_SPSR		0x18
-#define CTX_NS_SCTLR		0x1C
-#define CTX_REGS_END		0x20
+#define CTX_REGS_OFFSET		U(0x0)
+#define CTX_GPREG_R0		U(0x0)
+#define CTX_GPREG_R1		U(0x4)
+#define CTX_GPREG_R2		U(0x8)
+#define CTX_GPREG_R3		U(0xC)
+#define CTX_LR			U(0x10)
+#define CTX_SCR			U(0x14)
+#define CTX_SPSR		U(0x18)
+#define CTX_NS_SCTLR		U(0x1C)
+#define CTX_REGS_END		U(0x20)
 
 #ifndef __ASSEMBLY__
 
@@ -31,7 +33,7 @@
  * Common constants to help define the 'cpu_context' structure and its
  * members below.
  */
-#define WORD_SHIFT		2
+#define WORD_SHIFT		U(2)
 #define DEFINE_REG_STRUCT(name, num_regs)	\
 	typedef struct name {			\
 		uint32_t _regs[num_regs];	\
@@ -64,4 +66,4 @@
 
 #endif /* __ASSEMBLY__ */
 
-#endif /* __CONTEXT_H__ */
+#endif /* CONTEXT_H */
diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index b990674..8c5f4c6 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h
@@ -4,8 +4,8 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef __CONTEXT_H__
-#define __CONTEXT_H__
+#ifndef CONTEXT_H
+#define CONTEXT_H
 
 #include <utils_def.h>
 
@@ -347,4 +347,4 @@
 
 #endif /* __ASSEMBLY__ */
 
-#endif /* __CONTEXT_H__ */
+#endif /* CONTEXT_H */
diff --git a/include/lib/el3_runtime/context_mgmt.h b/include/lib/el3_runtime/context_mgmt.h
index c5bbb2b..149ac3f 100644
--- a/include/lib/el3_runtime/context_mgmt.h
+++ b/include/lib/el3_runtime/context_mgmt.h
@@ -1,16 +1,15 @@
 /*
- * Copyright (c) 2013-2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2018, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef __CM_H__
-#define __CM_H__
+#ifndef CONTEXT_MGMT_H
+#define CONTEXT_MGMT_H
 
 #include <arch.h>
 #include <assert.h>
 #include <context.h>
-#include <context_mgmt.h>
 #include <stdint.h>
 
 /*******************************************************************************
@@ -80,4 +79,4 @@
 void cm_set_next_context(void *context);
 #endif /* AARCH32 */
 
-#endif /* __CM_H__ */
+#endif /* CONTEXT_MGMT_H */
diff --git a/include/lib/el3_runtime/cpu_data.h b/include/lib/el3_runtime/cpu_data.h
index b695950..561f8be 100644
--- a/include/lib/el3_runtime/cpu_data.h
+++ b/include/lib/el3_runtime/cpu_data.h
@@ -144,9 +144,9 @@
 void init_cpu_ops(void);
 
 #define get_cpu_data(_m)		   _cpu_data()->_m
-#define set_cpu_data(_m, _v)		   _cpu_data()->_m = _v
+#define set_cpu_data(_m, _v)		   _cpu_data()->_m = (_v)
 #define get_cpu_data_by_index(_ix, _m)	   _cpu_data_by_index(_ix)->_m
-#define set_cpu_data_by_index(_ix, _m, _v) _cpu_data_by_index(_ix)->_m = _v
+#define set_cpu_data_by_index(_ix, _m, _v) _cpu_data_by_index(_ix)->_m = (_v)
 /* ((cpu_data_t *)0)->_m is a dummy to get the sizeof the struct member _m */
 #define flush_cpu_data(_m)	   flush_dcache_range((uintptr_t)	  \
 						&(_cpu_data()->_m), \
diff --git a/include/lib/libfdt/libfdt.h b/include/lib/libfdt/libfdt.h
index 830b77e..c8c00fa 100644
--- a/include/lib/libfdt/libfdt.h
+++ b/include/lib/libfdt/libfdt.h
@@ -90,9 +90,8 @@
 
 /* Error codes: codes for bad device tree blobs */
 #define FDT_ERR_TRUNCATED	8
-	/* FDT_ERR_TRUNCATED: FDT or a sub-block is improperly
-	 * terminated (overflows, goes outside allowed bounds, or
-	 * isn't properly terminated).  */
+	/* FDT_ERR_TRUNCATED: Structure block of the given device tree
+	 * ends without an FDT_END tag. */
 #define FDT_ERR_BADMAGIC	9
 	/* FDT_ERR_BADMAGIC: Given "device tree" appears not to be a
 	 * device tree at all - it is missing the flattened device
@@ -154,29 +153,6 @@
 
 uint32_t fdt_next_tag(const void *fdt, int offset, int *nextoffset);
 
-/*
- * Alignment helpers:
- *     These helpers access words from a device tree blob.  They're
- *     built to work even with unaligned pointers on platforms (ike
- *     ARM) that don't like unaligned loads and stores
- */
-
-static inline uint32_t fdt32_ld(const fdt32_t *p)
-{
-	fdt32_t v;
-
-	memcpy(&v, p, sizeof(v));
-	return fdt32_to_cpu(v);
-}
-
-static inline uint64_t fdt64_ld(const fdt64_t *p)
-{
-	fdt64_t v;
-
-	memcpy(&v, p, sizeof(v));
-	return fdt64_to_cpu(v);
-}
-
 /**********************************************************************/
 /* Traversal functions                                                */
 /**********************************************************************/
@@ -237,7 +213,7 @@
 /* General functions                                                  */
 /**********************************************************************/
 #define fdt_get_header(fdt, field) \
-	(fdt32_ld(&((const struct fdt_header *)(fdt))->field))
+	(fdt32_to_cpu(((const struct fdt_header *)(fdt))->field))
 #define fdt_magic(fdt)			(fdt_get_header(fdt, magic))
 #define fdt_totalsize(fdt)		(fdt_get_header(fdt, totalsize))
 #define fdt_off_dt_struct(fdt)		(fdt_get_header(fdt, off_dt_struct))
@@ -268,31 +244,18 @@
 #undef fdt_set_hdr_
 
 /**
- * fdt_header_size - return the size of the tree's header
- * @fdt: pointer to a flattened device tree
- */
-size_t fdt_header_size_(uint32_t version);
-static inline size_t fdt_header_size(const void *fdt)
-{
-	return fdt_header_size_(fdt_version(fdt));
-}
-
-/**
- * fdt_check_header - sanity check a device tree header
-
+ * fdt_check_header - sanity check a device tree or possible device tree
  * @fdt: pointer to data which might be a flattened device tree
  *
  * fdt_check_header() checks that the given buffer contains what
- * appears to be a flattened device tree, and that the header contains
- * valid information (to the extent that can be determined from the
- * header alone).
+ * appears to be a flattened device tree with sane information in its
+ * header.
  *
  * returns:
  *     0, if the buffer appears to contain a valid device tree
  *     -FDT_ERR_BADMAGIC,
  *     -FDT_ERR_BADVERSION,
- *     -FDT_ERR_BADSTATE,
- *     -FDT_ERR_TRUNCATED, standard meanings, as above
+ *     -FDT_ERR_BADSTATE, standard meanings, as above
  */
 int fdt_check_header(const void *fdt);
 
@@ -321,24 +284,6 @@
 /* Read-only functions                                                */
 /**********************************************************************/
 
-int fdt_check_full(const void *fdt, size_t bufsize);
-
-/**
- * fdt_get_string - retrieve a string from the strings block of a device tree
- * @fdt: pointer to the device tree blob
- * @stroffset: offset of the string within the strings block (native endian)
- * @lenp: optional pointer to return the string's length
- *
- * fdt_get_string() retrieves a pointer to a single string from the
- * strings block of the device tree blob at fdt, and optionally also
- * returns the string's length in *lenp.
- *
- * returns:
- *     a pointer to the string, on success
- *     NULL, if stroffset is out of bounds, or doesn't point to a valid string
- */
-const char *fdt_get_string(const void *fdt, int stroffset, int *lenp);
-
 /**
  * fdt_string - retrieve a string from the strings block of a device tree
  * @fdt: pointer to the device tree blob
@@ -349,7 +294,7 @@
  *
  * returns:
  *     a pointer to the string, on success
- *     NULL, if stroffset is out of bounds, or doesn't point to a valid string
+ *     NULL, if stroffset is out of bounds
  */
 const char *fdt_string(const void *fdt, int stroffset);
 
@@ -1145,7 +1090,7 @@
  *
  * returns:
  *	0 <= n < FDT_MAX_NCELLS, on success
- *      2, if the node has no #size-cells property
+ *      2, if the node has no #address-cells property
  *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid
  *		#size-cells property
  *	-FDT_ERR_BADMAGIC,
@@ -1368,13 +1313,10 @@
 	fdt64_t tmp = cpu_to_fdt64(val);
 	return fdt_property(fdt, name, &tmp, sizeof(tmp));
 }
-
-#ifndef SWIG /* Not available in Python */
 static inline int fdt_property_cell(void *fdt, const char *name, uint32_t val)
 {
 	return fdt_property_u32(fdt, name, val);
 }
-#endif
 
 /**
  * fdt_property_placeholder - add a new property and return a ptr to its value
diff --git a/include/lib/libfdt/libfdt_env.h b/include/lib/libfdt/libfdt_env.h
index eb20538..bd24746 100644
--- a/include/lib/libfdt/libfdt_env.h
+++ b/include/lib/libfdt/libfdt_env.h
@@ -56,7 +56,6 @@
 #include <stdint.h>
 #include <stdlib.h>
 #include <string.h>
-#include <limits.h>
 
 #ifdef __CHECKER__
 #define FDT_FORCE __attribute__((force))
diff --git a/include/lib/mmio.h b/include/lib/mmio.h
index 880d2c5..38fdf0f 100644
--- a/include/lib/mmio.h
+++ b/include/lib/mmio.h
@@ -29,6 +29,13 @@
 	return *(volatile uint16_t*)addr;
 }
 
+static inline void mmio_clrsetbits_16(uintptr_t addr,
+				uint16_t clear,
+				uint16_t set)
+{
+	mmio_write_16(addr, (mmio_read_16(addr) & ~clear) | set);
+}
+
 static inline void mmio_write_32(uintptr_t addr, uint32_t value)
 {
 	*(volatile uint32_t*)addr = value;
diff --git a/include/lib/spinlock.h b/include/lib/spinlock.h
index 8aec707..fcd36e8 100644
--- a/include/lib/spinlock.h
+++ b/include/lib/spinlock.h
@@ -4,8 +4,8 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef __SPINLOCK_H__
-#define __SPINLOCK_H__
+#ifndef SPINLOCK_H
+#define SPINLOCK_H
 
 #ifndef __ASSEMBLY__
 
@@ -26,4 +26,4 @@
 
 #endif
 
-#endif /* __SPINLOCK_H__ */
+#endif /* SPINLOCK_H */
diff --git a/include/plat/arm/board/common/v2m_def.h b/include/plat/arm/board/common/v2m_def.h
index 4a1d43c..ed57fc9 100644
--- a/include/plat/arm/board/common/v2m_def.h
+++ b/include/plat/arm/board/common/v2m_def.h
@@ -48,9 +48,9 @@
 #define V2M_SYS_LED_EL_SHIFT		0x1
 #define V2M_SYS_LED_EC_SHIFT		0x3
 
-#define V2M_SYS_LED_SS_MASK		0x1
-#define V2M_SYS_LED_EL_MASK		0x3
-#define V2M_SYS_LED_EC_MASK		0x1f
+#define V2M_SYS_LED_SS_MASK		U(0x1)
+#define V2M_SYS_LED_EL_MASK		U(0x3)
+#define V2M_SYS_LED_EC_MASK		U(0x1f)
 
 /* V2M sysid register bits */
 #define V2M_SYS_ID_REV_SHIFT		28
@@ -59,28 +59,28 @@
 #define V2M_SYS_ID_ARCH_SHIFT		8
 #define V2M_SYS_ID_FPGA_SHIFT		0
 
-#define V2M_SYS_ID_REV_MASK		0xf
-#define V2M_SYS_ID_HBI_MASK		0xfff
-#define V2M_SYS_ID_BLD_MASK		0xf
-#define V2M_SYS_ID_ARCH_MASK		0xf
-#define V2M_SYS_ID_FPGA_MASK		0xff
+#define V2M_SYS_ID_REV_MASK		U(0xf)
+#define V2M_SYS_ID_HBI_MASK		U(0xfff)
+#define V2M_SYS_ID_BLD_MASK		U(0xf)
+#define V2M_SYS_ID_ARCH_MASK		U(0xf)
+#define V2M_SYS_ID_FPGA_MASK		U(0xff)
 
 #define V2M_SYS_ID_BLD_LENGTH		4
 
 
 /* NOR Flash */
-#define V2M_FLASH0_BASE			0x08000000
-#define V2M_FLASH0_SIZE			0x04000000
-#define V2M_FLASH_BLOCK_SIZE		0x00040000	/* 256 KB */
+#define V2M_FLASH0_BASE			UL(0x08000000)
+#define V2M_FLASH0_SIZE			UL(0x04000000)
+#define V2M_FLASH_BLOCK_SIZE		UL(0x00040000)	/* 256 KB */
 
-#define V2M_IOFPGA_BASE			0x1c000000
-#define V2M_IOFPGA_SIZE			0x03000000
+#define V2M_IOFPGA_BASE			UL(0x1c000000)
+#define V2M_IOFPGA_SIZE			UL(0x03000000)
 
 /* PL011 UART related constants */
-#define V2M_IOFPGA_UART0_BASE		0x1c090000
-#define V2M_IOFPGA_UART1_BASE		0x1c0a0000
-#define V2M_IOFPGA_UART2_BASE		0x1c0b0000
-#define V2M_IOFPGA_UART3_BASE		0x1c0c0000
+#define V2M_IOFPGA_UART0_BASE		UL(0x1c090000)
+#define V2M_IOFPGA_UART1_BASE		UL(0x1c0a0000)
+#define V2M_IOFPGA_UART2_BASE		UL(0x1c0b0000)
+#define V2M_IOFPGA_UART3_BASE		UL(0x1c0c0000)
 
 #define V2M_IOFPGA_UART0_CLK_IN_HZ	24000000
 #define V2M_IOFPGA_UART1_CLK_IN_HZ	24000000
@@ -88,15 +88,15 @@
 #define V2M_IOFPGA_UART3_CLK_IN_HZ	24000000
 
 /* SP804 timer related constants */
-#define V2M_SP804_TIMER0_BASE		0x1C110000
-#define V2M_SP804_TIMER1_BASE		0x1C120000
+#define V2M_SP804_TIMER0_BASE		UL(0x1C110000)
+#define V2M_SP804_TIMER1_BASE		UL(0x1C120000)
 
 /* SP810 controller */
-#define V2M_SP810_BASE			0x1c020000
-#define V2M_SP810_CTRL_TIM0_SEL		(1 << 15)
-#define V2M_SP810_CTRL_TIM1_SEL		(1 << 17)
-#define V2M_SP810_CTRL_TIM2_SEL		(1 << 19)
-#define V2M_SP810_CTRL_TIM3_SEL		(1 << 21)
+#define V2M_SP810_BASE			UL(0x1c020000)
+#define V2M_SP810_CTRL_TIM0_SEL		BIT_32(15)
+#define V2M_SP810_CTRL_TIM1_SEL		BIT_32(17)
+#define V2M_SP810_CTRL_TIM2_SEL		BIT_32(19)
+#define V2M_SP810_CTRL_TIM3_SEL		BIT_32(21)
 
 /*
  * The flash can be mapped either as read-only or read-write.
diff --git a/include/plat/arm/common/arm_def.h b/include/plat/arm/common/arm_def.h
index 0f5b57f..088d59d 100644
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h
@@ -21,7 +21,7 @@
  *****************************************************************************/
 
 /* Special value used to verify platform parameters from BL2 to BL31 */
-#define ARM_BL31_PLAT_PARAM_VAL		0x0f1e2d3c4b5a6978ULL
+#define ARM_BL31_PLAT_PARAM_VAL		ULL(0x0f1e2d3c4b5a6978)
 
 #define ARM_SYSTEM_COUNT		1
 
@@ -350,8 +350,8 @@
  * To enable TB_FW_CONFIG to be loaded by BL1, define the corresponding base
  * and limit. Leave enough space of BL2 meminfo.
  */
-#define ARM_TB_FW_CONFIG_BASE		ARM_BL_RAM_BASE + sizeof(meminfo_t)
-#define ARM_TB_FW_CONFIG_LIMIT		ARM_BL_RAM_BASE + PAGE_SIZE
+#define ARM_TB_FW_CONFIG_BASE		(ARM_BL_RAM_BASE + sizeof(meminfo_t))
+#define ARM_TB_FW_CONFIG_LIMIT		(ARM_BL_RAM_BASE + PAGE_SIZE)
 
 /*******************************************************************************
  * BL1 specific defines.
@@ -482,7 +482,7 @@
 #  define TSP_SEC_MEM_SIZE		PLAT_ARM_TRUSTED_DRAM_SIZE
 #  define BL32_BASE			PLAT_ARM_TRUSTED_DRAM_BASE
 #  define BL32_LIMIT			(PLAT_ARM_TRUSTED_DRAM_BASE	\
-						+ (1 << 21))
+						+ (UL(1) << 21))
 # elif ARM_TSP_RAM_LOCATION_ID == ARM_DRAM_ID
 #  define TSP_SEC_MEM_BASE		ARM_AP_TZC_DRAM1_BASE
 #  define TSP_SEC_MEM_SIZE		ARM_AP_TZC_DRAM1_SIZE
@@ -511,7 +511,7 @@
 #define BL2U_LIMIT			BL2_LIMIT
 
 #define NS_BL2U_BASE			ARM_NS_DRAM1_BASE
-#define NS_BL1U_BASE			(PLAT_ARM_NVM_BASE + 0x03EB8000)
+#define NS_BL1U_BASE			(PLAT_ARM_NVM_BASE + UL(0x03EB8000))
 
 /*
  * ID of the secure physical generic timer interrupt used by the TSP.
diff --git a/include/plat/arm/common/arm_dyn_cfg_helpers.h b/include/plat/arm/common/arm_dyn_cfg_helpers.h
index cb17c95..3ad6d54 100644
--- a/include/plat/arm/common/arm_dyn_cfg_helpers.h
+++ b/include/plat/arm/common/arm_dyn_cfg_helpers.h
@@ -3,8 +3,8 @@
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
-#ifndef __ARM_DYN_CFG_HELPERS_H__
-#define __ARM_DYN_CFG_HELPERS_H__
+#ifndef ARM_DYN_CFG_HELPERS_H
+#define ARM_DYN_CFG_HELPERS_H
 
 #include <stddef.h>
 #include <stdint.h>
@@ -19,4 +19,4 @@
 int arm_set_dtb_mbedtls_heap_info(void *dtb, void *heap_addr,
 	size_t heap_size);
 
-#endif /* __ARM_DYN_CFG_HELPERS_H__ */
+#endif /* ARM_DYN_CFG_HELPERS_H */
diff --git a/include/plat/arm/css/common/css_def.h b/include/plat/arm/css/common/css_def.h
index 048c58a..3853319 100644
--- a/include/plat/arm/css/common/css_def.h
+++ b/include/plat/arm/css/common/css_def.h
@@ -26,6 +26,11 @@
 #define SSC_REG_BASE			0x2a420000
 #define SSC_GPRETN			(SSC_REG_BASE + 0x030)
 
+/* System ID Registers Unit */
+#define SID_REG_BASE			0x2a4a0000
+#define SID_SYSTEM_ID_OFFSET		0x40
+#define SID_SYSTEM_CFG_OFFSET		0x70
+
 /* The slave_bootsecure controls access to GPU, DMC and CS. */
 #define CSS_NIC400_SLAVE_BOOTSECURE	8
 
@@ -123,6 +128,8 @@
 #define SSC_VERSION_DESIGNER_ID_MASK		0xff
 #define SSC_VERSION_PART_NUM_MASK		0xfff
 
+#define SID_SYSTEM_ID_PART_NUM_MASK		0xfff
+
 /* SSC debug configuration registers */
 #define SSC_DBGCFG_SET		0x14
 #define SSC_DBGCFG_CLR		0x18
diff --git a/include/plat/marvell/a3700/common/armada_common.h b/include/plat/marvell/a3700/common/armada_common.h
new file mode 100644
index 0000000..9fc4634
--- /dev/null
+++ b/include/plat/marvell/a3700/common/armada_common.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef __ARMADA_COMMON_H__
+#define __ARMADA_COMMON_H__
+
+#include <io_addr_dec.h>
+#include <stdint.h>
+
+int marvell_get_io_dec_win_conf(struct dec_win_config **win, uint32_t *size);
+
+#endif /* __ARMADA_COMMON_H__ */
diff --git a/include/plat/marvell/a3700/common/board_marvell_def.h b/include/plat/marvell/a3700/common/board_marvell_def.h
new file mode 100644
index 0000000..49a1a5c
--- /dev/null
+++ b/include/plat/marvell/a3700/common/board_marvell_def.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef __BOARD_MARVELL_DEF_H__
+#define __BOARD_MARVELL_DEF_H__
+
+/*
+ * Required platform porting definitions common to all ARM
+ * development platforms
+ */
+
+/* Size of cacheable stacks */
+#if IMAGE_BL1
+#if TRUSTED_BOARD_BOOT
+# define PLATFORM_STACK_SIZE 0x1000
+#else
+# define PLATFORM_STACK_SIZE 0x440
+#endif
+#elif IMAGE_BL2
+# if TRUSTED_BOARD_BOOT
+#  define PLATFORM_STACK_SIZE 0x1000
+# else
+#  define PLATFORM_STACK_SIZE 0x400
+# endif
+#elif IMAGE_BL31
+# define PLATFORM_STACK_SIZE 0x400
+#elif IMAGE_BL32
+# define PLATFORM_STACK_SIZE 0x440
+#endif
+
+/*
+ * PLAT_MARVELL_MMAP_ENTRIES depends on the number of entries in the
+ * plat_arm_mmap array defined for each BL stage.
+ */
+#if IMAGE_BLE
+#  define PLAT_MARVELL_MMAP_ENTRIES	3
+#endif
+#if IMAGE_BL1
+#  if TRUSTED_BOARD_BOOT
+#   define PLAT_MARVELL_MMAP_ENTRIES	7
+#  else
+#   define PLAT_MARVELL_MMAP_ENTRIES	6
+#  endif	/* TRUSTED_BOARD_BOOT */
+#endif
+#if IMAGE_BL2
+#  define PLAT_MARVELL_MMAP_ENTRIES	8
+#endif
+#if IMAGE_BL31
+#define PLAT_MARVELL_MMAP_ENTRIES	5
+#endif
+
+/*
+ * Platform specific page table and MMU setup constants
+ */
+#if IMAGE_BL1
+#define MAX_XLAT_TABLES			4
+#elif IMAGE_BLE
+#  define MAX_XLAT_TABLES		4
+#elif IMAGE_BL2
+#  define MAX_XLAT_TABLES		4
+#elif IMAGE_BL31
+# define MAX_XLAT_TABLES		4
+#elif IMAGE_BL32
+#  define MAX_XLAT_TABLES		4
+#endif
+
+#define MAX_IO_DEVICES			3
+#define MAX_IO_HANDLES			4
+
+#define PLAT_MARVELL_TRUSTED_SRAM_SIZE	0x80000	/* 512 KB */
+
+
+#endif /* __BOARD_MARVELL_DEF_H__ */
diff --git a/include/plat/marvell/a3700/common/marvell_def.h b/include/plat/marvell/a3700/common/marvell_def.h
new file mode 100644
index 0000000..6c3a524
--- /dev/null
+++ b/include/plat/marvell/a3700/common/marvell_def.h
@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef __MARVELL_DEF_H__
+#define __MARVELL_DEF_H__
+
+#include <arch.h>
+#include <common_def.h>
+#include <platform_def.h>
+#include <tbbr_img_def.h>
+#include <xlat_tables.h>
+
+
+/****************************************************************************
+ * Definitions common to all MARVELL standard platforms
+ ****************************************************************************
+ */
+/* Special value used to verify platform parameters from BL2 to BL31 */
+#define MARVELL_BL31_PLAT_PARAM_VAL		0x0f1e2d3c4b5a6978ULL
+
+#define PLAT_MARVELL_NORTHB_COUNT		1
+
+#define PLAT_MARVELL_CLUSTER_COUNT		1
+
+#define MARVELL_CACHE_WRITEBACK_SHIFT		6
+
+/*
+ * Macros mapping the MPIDR Affinity levels to MARVELL Platform Power levels.
+ * The power levels have a 1:1 mapping with the MPIDR affinity levels.
+ */
+#define MARVELL_PWR_LVL0		MPIDR_AFFLVL0
+#define MARVELL_PWR_LVL1		MPIDR_AFFLVL1
+#define MARVELL_PWR_LVL2		MPIDR_AFFLVL2
+
+/*
+ *  Macros for local power states in Marvell platforms encoded by State-ID field
+ *  within the power-state parameter.
+ */
+/* Local power state for power domains in Run state. */
+#define MARVELL_LOCAL_STATE_RUN	0
+/* Local power state for retention. Valid only for CPU power domains */
+#define MARVELL_LOCAL_STATE_RET	1
+/* Local power state for OFF/power-down.
+ * Valid for CPU and cluster power domains
+ */
+#define MARVELL_LOCAL_STATE_OFF	2
+
+/* The first 4KB of Trusted SRAM are used as shared memory */
+#define MARVELL_TRUSTED_SRAM_BASE	PLAT_MARVELL_ATF_BASE
+#define MARVELL_SHARED_RAM_BASE		MARVELL_TRUSTED_SRAM_BASE
+#define MARVELL_SHARED_RAM_SIZE		0x00001000	/* 4 KB */
+
+/* The remaining Trusted SRAM is used to load the BL images */
+#define MARVELL_BL_RAM_BASE		(MARVELL_SHARED_RAM_BASE + \
+					 MARVELL_SHARED_RAM_SIZE)
+#define MARVELL_BL_RAM_SIZE		(PLAT_MARVELL_TRUSTED_SRAM_SIZE - \
+					 MARVELL_SHARED_RAM_SIZE)
+
+#define MARVELL_DRAM_BASE		ULL(0x0)
+#define MARVELL_DRAM_SIZE		ULL(0x20000000)
+#define MARVELL_DRAM_END		(MARVELL_DRAM_BASE + \
+					 MARVELL_DRAM_SIZE - 1)
+
+#define MARVELL_IRQ_SEC_PHY_TIMER		29
+
+#define MARVELL_IRQ_SEC_SGI_0		8
+#define MARVELL_IRQ_SEC_SGI_1		9
+#define MARVELL_IRQ_SEC_SGI_2		10
+#define MARVELL_IRQ_SEC_SGI_3		11
+#define MARVELL_IRQ_SEC_SGI_4		12
+#define MARVELL_IRQ_SEC_SGI_5		13
+#define MARVELL_IRQ_SEC_SGI_6		14
+#define MARVELL_IRQ_SEC_SGI_7		15
+
+#define MARVELL_MAP_SHARED_RAM		MAP_REGION_FLAT(		 \
+						MARVELL_SHARED_RAM_BASE, \
+						MARVELL_SHARED_RAM_SIZE, \
+						MT_MEMORY | MT_RW | MT_SECURE)
+
+#define MARVELL_MAP_DRAM		MAP_REGION_FLAT(		\
+						MARVELL_DRAM_BASE,	\
+						MARVELL_DRAM_SIZE,	\
+						MT_MEMORY | MT_RW | MT_NS)
+
+
+/*
+ * The number of regions like RO(code), coherent and data required by
+ * different BL stages which need to be mapped in the MMU.
+ */
+#if USE_COHERENT_MEM
+#define MARVELL_BL_REGIONS		3
+#else
+#define MARVELL_BL_REGIONS		2
+#endif
+
+#define MAX_MMAP_REGIONS		(PLAT_MARVELL_MMAP_ENTRIES + \
+					 MARVELL_BL_REGIONS)
+
+#define MARVELL_CONSOLE_BAUDRATE	115200
+
+/****************************************************************************
+ * Required platform porting definitions common to all MARVELL std. platforms
+ ****************************************************************************
+ */
+#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
+#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)
+
+/*
+ * This macro defines the deepest retention state possible. A higher state
+ * id will represent an invalid or a power down state.
+ */
+#define PLAT_MAX_RET_STATE		MARVELL_LOCAL_STATE_RET
+
+/*
+ * This macro defines the deepest power down states possible. Any state ID
+ * higher than this is invalid.
+ */
+#define PLAT_MAX_OFF_STATE		MARVELL_LOCAL_STATE_OFF
+
+
+#define PLATFORM_CORE_COUNT		PLAT_MARVELL_CLUSTER_CORE_COUNT
+
+/*
+ * Some data must be aligned on the biggest cache line size in the platform.
+ * This is known only to the platform as it might have a combination of
+ * integrated and external caches.
+ */
+#define CACHE_WRITEBACK_GRANULE		(1 << MARVELL_CACHE_WRITEBACK_SHIFT)
+
+
+/*****************************************************************************
+ * BL1 specific defines.
+ * BL1 RW data is relocated from ROM to RAM at runtime so we need 2 sets of
+ * addresses.
+ *****************************************************************************
+ */
+#define BL1_RO_BASE		PLAT_MARVELL_TRUSTED_ROM_BASE
+#define BL1_RO_LIMIT		(PLAT_MARVELL_TRUSTED_ROM_BASE	\
+					+ PLAT_MARVELL_TRUSTED_ROM_SIZE)
+/*
+ * Put BL1 RW at the top of the Trusted SRAM.
+ */
+#define BL1_RW_BASE		(MARVELL_BL_RAM_BASE +		\
+					MARVELL_BL_RAM_SIZE -	\
+					PLAT_MARVELL_MAX_BL1_RW_SIZE)
+#define BL1_RW_LIMIT		(MARVELL_BL_RAM_BASE + MARVELL_BL_RAM_SIZE)
+
+/*****************************************************************************
+ * BL2 specific defines.
+ *****************************************************************************
+ */
+/*
+ * Put BL2 just below BL31.
+ */
+#define BL2_BASE		(BL31_BASE - PLAT_MARVELL_MAX_BL2_SIZE)
+#define BL2_LIMIT		BL31_BASE
+
+/*****************************************************************************
+ * BL31 specific defines.
+ *****************************************************************************
+ */
+/*
+ * Put BL31 at the top of the Trusted SRAM.
+ */
+#define BL31_BASE		(MARVELL_BL_RAM_BASE + \
+					MARVELL_BL_RAM_SIZE - \
+					PLAT_MARVEL_MAX_BL31_SIZE)
+#define BL31_PROGBITS_LIMIT	BL1_RW_BASE
+#define BL31_LIMIT			(MARVELL_BL_RAM_BASE +	\
+					 MARVELL_BL_RAM_SIZE)
+
+
+#endif /* __MARVELL_DEF_H__ */
diff --git a/include/plat/marvell/a3700/common/plat_marvell.h b/include/plat/marvell/a3700/common/plat_marvell.h
new file mode 100644
index 0000000..f733d04
--- /dev/null
+++ b/include/plat/marvell/a3700/common/plat_marvell.h
@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) 2016 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef __PLAT_MARVELL_H__
+#define __PLAT_MARVELL_H__
+
+#include <bl_common.h>
+#include <cassert.h>
+#include <cpu_data.h>
+#include <stdint.h>
+#include <xlat_tables.h>
+
+/*
+ * Extern declarations common to Marvell standard platforms
+ */
+extern const mmap_region_t plat_marvell_mmap[];
+
+#define MARVELL_CASSERT_MMAP						\
+	CASSERT((ARRAY_SIZE(plat_marvell_mmap) + MARVELL_BL_REGIONS)	\
+		<= MAX_MMAP_REGIONS,					\
+		assert_max_mmap_regions)
+
+/*
+ * Utility functions common to Marvell standard platforms
+ */
+void marvell_setup_page_tables(uintptr_t total_base,
+			       size_t total_size,
+			       uintptr_t code_start,
+			       uintptr_t code_limit,
+			       uintptr_t rodata_start,
+			       uintptr_t rodata_limit
+#if USE_COHERENT_MEM
+			     , uintptr_t coh_start,
+			       uintptr_t coh_limit
+#endif
+);
+
+/* IO storage utility functions */
+void marvell_io_setup(void);
+
+/* Systimer utility function */
+void marvell_configure_sys_timer(void);
+
+/* Topology utility function */
+int marvell_check_mpidr(u_register_t mpidr);
+
+/* BL1 utility functions */
+void marvell_bl1_early_platform_setup(void);
+void marvell_bl1_platform_setup(void);
+void marvell_bl1_plat_arch_setup(void);
+
+/* BL2 utility functions */
+void marvell_bl2_early_platform_setup(meminfo_t *mem_layout);
+void marvell_bl2_platform_setup(void);
+void marvell_bl2_plat_arch_setup(void);
+uint32_t marvell_get_spsr_for_bl32_entry(void);
+uint32_t marvell_get_spsr_for_bl33_entry(void);
+
+/* BL31 utility functions */
+void marvell_bl31_early_platform_setup(void *from_bl2,
+				       uintptr_t soc_fw_config,
+				       uintptr_t hw_config,
+				       void *plat_params_from_bl2);
+void marvell_bl31_platform_setup(void);
+void marvell_bl31_plat_runtime_setup(void);
+void marvell_bl31_plat_arch_setup(void);
+
+/* FIP TOC validity check */
+int marvell_io_is_toc_valid(void);
+
+/*
+ * PSCI functionality
+ */
+void marvell_psci_arch_init(int idx);
+void plat_marvell_system_reset(void);
+
+/*
+ * Optional functions required in Marvell standard platforms
+ */
+void plat_marvell_io_setup(void);
+int plat_marvell_get_alt_image_source(
+	unsigned int image_id,
+	uintptr_t *dev_handle,
+	uintptr_t *image_spec);
+unsigned int plat_marvell_calc_core_pos(u_register_t mpidr);
+
+void plat_marvell_interconnect_init(void);
+void plat_marvell_interconnect_enter_coherency(void);
+
+const mmap_region_t *plat_marvell_get_mmap(void);
+
+#endif /* __PLAT_MARVELL_H__ */
diff --git a/include/plat/marvell/a8k/common/armada_common.h b/include/plat/marvell/a8k/common/armada_common.h
index e727467..c1e37f5 100644
--- a/include/plat/marvell/a8k/common/armada_common.h
+++ b/include/plat/marvell/a8k/common/armada_common.h
@@ -5,8 +5,8 @@
  * https://spdx.org/licenses
  */
 
-#ifndef __A8K_COMMON_H__
-#define __A8K_COMMON_H__
+#ifndef __ARMADA_COMMON_H__
+#define __ARMADA_COMMON_H__
 
 #include <amb_adec.h>
 #include <io_win.h>
diff --git a/include/plat/marvell/common/aarch64/marvell_macros.S b/include/plat/marvell/common/aarch64/marvell_macros.S
index 0102af0..faf1070 100644
--- a/include/plat/marvell/common/aarch64/marvell_macros.S
+++ b/include/plat/marvell/common/aarch64/marvell_macros.S
@@ -45,7 +45,7 @@
 	 * Clobbers: x0 - x10, sp
 	 * ---------------------------------------------
 	 */
-	.macro arm_print_gic_regs
+	.macro marvell_print_gic_regs
 	/* Check for GICv3 system register access */
 	mrs	x7, id_aa64pfr0_el1
 	ubfx	x7, x7, #ID_AA64PFR0_GIC_SHIFT, #ID_AA64PFR0_GIC_WIDTH
diff --git a/include/plat/marvell/common/mvebu.h b/include/plat/marvell/common/mvebu.h
index a20e538..a7d6c3f 100644
--- a/include/plat/marvell/common/mvebu.h
+++ b/include/plat/marvell/common/mvebu.h
@@ -32,7 +32,8 @@
 #define ROUND_UP_TO_POW_OF_2(number)	(1 << \
 					(32 - __builtin_clz((number) - 1)))
 
-#define _1MB_				(1024ULL*1024ULL)
-#define _1GB_				(_1MB_*1024ULL)
+#define _1MB_				(1024ULL * 1024ULL)
+#define _1GB_				(_1MB_ * 1024ULL)
+#define _2GB_				(2 * _1GB_)
 
 #endif	/* MVEBU_H */
diff --git a/lib/el3_runtime/aarch32/context_mgmt.c b/lib/el3_runtime/aarch32/context_mgmt.c
index 80cea28..ad0a120 100644
--- a/lib/el3_runtime/aarch32/context_mgmt.c
+++ b/lib/el3_runtime/aarch32/context_mgmt.c
@@ -57,7 +57,7 @@
 	uint32_t scr, sctlr;
 	regs_t *reg_ctx;
 
-	assert(ctx);
+	assert(ctx != NULL);
 
 	security_state = GET_SECURITY_STATE(ep->h.attr);
 
@@ -97,7 +97,7 @@
 		assert(((ep->spsr >> SPSR_E_SHIFT) & SPSR_E_MASK) ==
 			(EP_GET_EE(ep->h.attr) >> EP_EE_SHIFT));
 
-		sctlr = EP_GET_EE(ep->h.attr) ? SCTLR_EE_BIT : 0;
+		sctlr = (EP_GET_EE(ep->h.attr) != 0U) ? SCTLR_EE_BIT : 0U;
 		sctlr |= (SCTLR_RESET_VAL & ~(SCTLR_TE_BIT | SCTLR_V_BIT));
 		write_ctx_reg(reg_ctx, CTX_NS_SCTLR, sctlr);
 	}
@@ -178,11 +178,11 @@
 	cpu_context_t *ctx = cm_get_context(security_state);
 	bool el2_unused = false;
 
-	assert(ctx);
+	assert(ctx != NULL);
 
 	if (security_state == NON_SECURE) {
 		scr = read_ctx_reg(get_regs_ctx(ctx), CTX_SCR);
-		if (scr & SCR_HCE_BIT) {
+		if ((scr & SCR_HCE_BIT) != 0U) {
 			/* Use SCTLR value to initialize HSCTLR */
 			hsctlr = read_ctx_reg(get_regs_ctx(ctx),
 						 CTX_NS_SCTLR);
@@ -199,8 +199,8 @@
 
 			write_scr(read_scr() & ~SCR_NS_BIT);
 			isb();
-		} else if (read_id_pfr1() &
-			(ID_PFR1_VIRTEXT_MASK << ID_PFR1_VIRTEXT_SHIFT)) {
+		} else if ((read_id_pfr1() &
+			(ID_PFR1_VIRTEXT_MASK << ID_PFR1_VIRTEXT_SHIFT)) != 0U) {
 			el2_unused = true;
 
 			/*
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index f037e18..39c27d0 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -68,7 +68,7 @@
 	gp_regs_t *gp_regs;
 	unsigned long sctlr_elx, actlr_elx;
 
-	assert(ctx);
+	assert(ctx != NULL);
 
 	security_state = GET_SECURITY_STATE(ep->h.attr);
 
@@ -84,7 +84,7 @@
 	 * the required value depending on the state of the SPSR_EL3 and the
 	 * Security state and entrypoint attributes of the next EL.
 	 */
-	scr_el3 = read_scr();
+	scr_el3 = (uint32_t)read_scr();
 	scr_el3 &= ~(SCR_NS_BIT | SCR_RW_BIT | SCR_FIQ_BIT | SCR_IRQ_BIT |
 			SCR_ST_BIT | SCR_HCE_BIT);
 	/*
@@ -103,7 +103,7 @@
 	 *  Secure timer registers to EL3, from AArch64 state only, if specified
 	 *  by the entrypoint attributes.
 	 */
-	if (EP_GET_ST(ep->h.attr))
+	if (EP_GET_ST(ep->h.attr) != 0U)
 		scr_el3 |= SCR_ST_BIT;
 
 #if !HANDLE_EA_EL3_FIRST
@@ -133,10 +133,9 @@
 	 * AArch64 and next EL is EL2, or if next execution state is AArch32 and
 	 * next mode is Hyp.
 	 */
-	if ((GET_RW(ep->spsr) == MODE_RW_64
-	     && GET_EL(ep->spsr) == MODE_EL2)
-	    || (GET_RW(ep->spsr) != MODE_RW_64
-		&& GET_M32(ep->spsr) == MODE32_hyp)) {
+	if (((GET_RW(ep->spsr) == MODE_RW_64) && (GET_EL(ep->spsr) == MODE_EL2))
+	    || ((GET_RW(ep->spsr) != MODE_RW_64)
+		&& (GET_M32(ep->spsr) == MODE32_hyp))) {
 		scr_el3 |= SCR_HCE_BIT;
 	}
 
@@ -151,7 +150,7 @@
 	 * SCTLR.M, SCTLR.C and SCTLR.I: These fields must be zero (as
 	 *  required by PSCI specification)
 	 */
-	sctlr_elx = EP_GET_EE(ep->h.attr) ? SCTLR_EE_BIT : 0;
+	sctlr_elx = (EP_GET_EE(ep->h.attr) != 0U) ? SCTLR_EE_BIT : 0U;
 	if (GET_RW(ep->spsr) == MODE_RW_64)
 		sctlr_elx |= SCTLR_EL1_RES1;
 	else {
@@ -291,20 +290,21 @@
 	uint32_t sctlr_elx, scr_el3, mdcr_el2;
 	cpu_context_t *ctx = cm_get_context(security_state);
 	bool el2_unused = false;
-	uint64_t hcr_el2 = 0;
+	uint64_t hcr_el2 = 0U;
 
-	assert(ctx);
+	assert(ctx != NULL);
 
 	if (security_state == NON_SECURE) {
-		scr_el3 = read_ctx_reg(get_el3state_ctx(ctx), CTX_SCR_EL3);
-		if (scr_el3 & SCR_HCE_BIT) {
+		scr_el3 = (uint32_t)read_ctx_reg(get_el3state_ctx(ctx),
+						 CTX_SCR_EL3);
+		if ((scr_el3 & SCR_HCE_BIT) != 0U) {
 			/* Use SCTLR_EL1.EE value to initialise sctlr_el2 */
-			sctlr_elx = read_ctx_reg(get_sysregs_ctx(ctx),
-						 CTX_SCTLR_EL1);
+			sctlr_elx = (uint32_t)read_ctx_reg(get_sysregs_ctx(ctx),
+							   CTX_SCTLR_EL1);
 			sctlr_elx &= SCTLR_EE_BIT;
 			sctlr_elx |= SCTLR_EL2_RES1;
 			write_sctlr_el2(sctlr_elx);
-		} else if (EL_IMPLEMENTED(2)) {
+		} else if (el_implemented(2) != EL_IMPL_NONE) {
 			el2_unused = true;
 
 			/*
@@ -314,7 +314,7 @@
 			 * Set EL2 register width appropriately: Set HCR_EL2
 			 * field to match SCR_EL3.RW.
 			 */
-			if (scr_el3 & SCR_RW_BIT)
+			if ((scr_el3 & SCR_RW_BIT) != 0U)
 				hcr_el2 |= HCR_RW_BIT;
 
 			/*
@@ -470,7 +470,7 @@
 	cpu_context_t *ctx;
 
 	ctx = cm_get_context(security_state);
-	assert(ctx);
+	assert(ctx != NULL);
 
 	el1_sysregs_context_save(get_sysregs_ctx(ctx));
 
@@ -487,7 +487,7 @@
 	cpu_context_t *ctx;
 
 	ctx = cm_get_context(security_state);
-	assert(ctx);
+	assert(ctx != NULL);
 
 	el1_sysregs_context_restore(get_sysregs_ctx(ctx));
 
@@ -509,7 +509,7 @@
 	el3_state_t *state;
 
 	ctx = cm_get_context(security_state);
-	assert(ctx);
+	assert(ctx != NULL);
 
 	/* Populate EL3 state so that ERET jumps to the correct entry */
 	state = get_el3state_ctx(ctx);
@@ -527,7 +527,7 @@
 	el3_state_t *state;
 
 	ctx = cm_get_context(security_state);
-	assert(ctx);
+	assert(ctx != NULL);
 
 	/* Populate EL3 state so that ERET jumps to the correct entry */
 	state = get_el3state_ctx(ctx);
@@ -549,21 +549,21 @@
 	uint32_t scr_el3;
 
 	ctx = cm_get_context(security_state);
-	assert(ctx);
+	assert(ctx != NULL);
 
 	/* Ensure that the bit position is a valid one */
-	assert((1 << bit_pos) & SCR_VALID_BIT_MASK);
+	assert(((1U << bit_pos) & SCR_VALID_BIT_MASK) != 0U);
 
 	/* Ensure that the 'value' is only a bit wide */
-	assert(value <= 1);
+	assert(value <= 1U);
 
 	/*
 	 * Get the SCR_EL3 value from the cpu context, clear the desired bit
 	 * and set it to its new value.
 	 */
 	state = get_el3state_ctx(ctx);
-	scr_el3 = read_ctx_reg(state, CTX_SCR_EL3);
-	scr_el3 &= ~(1 << bit_pos);
+	scr_el3 = (uint32_t)read_ctx_reg(state, CTX_SCR_EL3);
+	scr_el3 &= ~(1U << bit_pos);
 	scr_el3 |= value << bit_pos;
 	write_ctx_reg(state, CTX_SCR_EL3, scr_el3);
 }
@@ -578,11 +578,11 @@
 	el3_state_t *state;
 
 	ctx = cm_get_context(security_state);
-	assert(ctx);
+	assert(ctx != NULL);
 
 	/* Populate EL3 state so that ERET jumps to the correct entry */
 	state = get_el3state_ctx(ctx);
-	return read_ctx_reg(state, CTX_SCR_EL3);
+	return (uint32_t)read_ctx_reg(state, CTX_SCR_EL3);
 }
 
 /*******************************************************************************
@@ -595,7 +595,7 @@
 	cpu_context_t *ctx;
 
 	ctx = cm_get_context(security_state);
-	assert(ctx);
+	assert(ctx != NULL);
 
 	cm_set_next_context(ctx);
 }
diff --git a/lib/libfdt/fdt.c b/lib/libfdt/fdt.c
index ae03b11..7855a17 100644
--- a/lib/libfdt/fdt.c
+++ b/lib/libfdt/fdt.c
@@ -55,12 +55,7 @@
 
 #include "libfdt_internal.h"
 
-/*
- * Minimal sanity check for a read-only tree. fdt_ro_probe_() checks
- * that the given buffer contains what appears to be a flattened
- * device tree with sane information in its header.
- */
-int fdt_ro_probe_(const void *fdt)
+int fdt_check_header(const void *fdt)
 {
 	if (fdt_magic(fdt) == FDT_MAGIC) {
 		/* Complete tree */
@@ -79,78 +74,6 @@
 	return 0;
 }
 
-static int check_off_(uint32_t hdrsize, uint32_t totalsize, uint32_t off)
-{
-	return (off >= hdrsize) && (off <= totalsize);
-}
-
-static int check_block_(uint32_t hdrsize, uint32_t totalsize,
-			uint32_t base, uint32_t size)
-{
-	if (!check_off_(hdrsize, totalsize, base))
-		return 0; /* block start out of bounds */
-	if ((base + size) < base)
-		return 0; /* overflow */
-	if (!check_off_(hdrsize, totalsize, base + size))
-		return 0; /* block end out of bounds */
-	return 1;
-}
-
-size_t fdt_header_size_(uint32_t version)
-{
-	if (version <= 1)
-		return FDT_V1_SIZE;
-	else if (version <= 2)
-		return FDT_V2_SIZE;
-	else if (version <= 3)
-		return FDT_V3_SIZE;
-	else if (version <= 16)
-		return FDT_V16_SIZE;
-	else
-		return FDT_V17_SIZE;
-}
-
-int fdt_check_header(const void *fdt)
-{
-	size_t hdrsize;
-
-	if (fdt_magic(fdt) != FDT_MAGIC)
-		return -FDT_ERR_BADMAGIC;
-	hdrsize = fdt_header_size(fdt);
-	if ((fdt_version(fdt) < FDT_FIRST_SUPPORTED_VERSION)
-	    || (fdt_last_comp_version(fdt) > FDT_LAST_SUPPORTED_VERSION))
-		return -FDT_ERR_BADVERSION;
-	if (fdt_version(fdt) < fdt_last_comp_version(fdt))
-		return -FDT_ERR_BADVERSION;
-
-	if ((fdt_totalsize(fdt) < hdrsize)
-	    || (fdt_totalsize(fdt) > INT_MAX))
-		return -FDT_ERR_TRUNCATED;
-
-	/* Bounds check memrsv block */
-	if (!check_off_(hdrsize, fdt_totalsize(fdt), fdt_off_mem_rsvmap(fdt)))
-		return -FDT_ERR_TRUNCATED;
-
-	/* Bounds check structure block */
-	if (fdt_version(fdt) < 17) {
-		if (!check_off_(hdrsize, fdt_totalsize(fdt),
-				fdt_off_dt_struct(fdt)))
-			return -FDT_ERR_TRUNCATED;
-	} else {
-		if (!check_block_(hdrsize, fdt_totalsize(fdt),
-				  fdt_off_dt_struct(fdt),
-				  fdt_size_dt_struct(fdt)))
-			return -FDT_ERR_TRUNCATED;
-	}
-
-	/* Bounds check strings block */
-	if (!check_block_(hdrsize, fdt_totalsize(fdt),
-			  fdt_off_dt_strings(fdt), fdt_size_dt_strings(fdt)))
-		return -FDT_ERR_TRUNCATED;
-
-	return 0;
-}
-
 const void *fdt_offset_ptr(const void *fdt, int offset, unsigned int len)
 {
 	unsigned absoffset = offset + fdt_off_dt_struct(fdt);
@@ -321,7 +244,7 @@
 
 int fdt_move(const void *fdt, void *buf, int bufsize)
 {
-	FDT_RO_PROBE(fdt);
+	FDT_CHECK_HEADER(fdt);
 
 	if (fdt_totalsize(fdt) > bufsize)
 		return -FDT_ERR_NOSPACE;
diff --git a/lib/libfdt/fdt_addresses.c b/lib/libfdt/fdt_addresses.c
index 49537b5..eff4dbc 100644
--- a/lib/libfdt/fdt_addresses.c
+++ b/lib/libfdt/fdt_addresses.c
@@ -1,7 +1,6 @@
 /*
  * libfdt - Flat Device Tree manipulation
  * Copyright (C) 2014 David Gibson <david@gibson.dropbear.id.au>
- * Copyright (C) 2018 embedded brains GmbH
  *
  * libfdt is dual licensed: you can use it either under the terms of
  * the GPL, or the BSD license, at your option.
@@ -56,32 +55,42 @@
 
 #include "libfdt_internal.h"
 
-static int fdt_cells(const void *fdt, int nodeoffset, const char *name)
+int fdt_address_cells(const void *fdt, int nodeoffset)
 {
-	const fdt32_t *c;
+	const fdt32_t *ac;
 	int val;
 	int len;
 
-	c = fdt_getprop(fdt, nodeoffset, name, &len);
-	if (!c)
+	ac = fdt_getprop(fdt, nodeoffset, "#address-cells", &len);
+	if (!ac)
 		return 2;
 
-	if (len != sizeof(*c))
+	if (len != sizeof(*ac))
 		return -FDT_ERR_BADNCELLS;
 
-	val = fdt32_to_cpu(*c);
+	val = fdt32_to_cpu(*ac);
 	if ((val <= 0) || (val > FDT_MAX_NCELLS))
 		return -FDT_ERR_BADNCELLS;
 
 	return val;
 }
 
-int fdt_address_cells(const void *fdt, int nodeoffset)
-{
-	return fdt_cells(fdt, nodeoffset, "#address-cells");
-}
-
 int fdt_size_cells(const void *fdt, int nodeoffset)
 {
-	return fdt_cells(fdt, nodeoffset, "#size-cells");
+	const fdt32_t *sc;
+	int val;
+	int len;
+
+	sc = fdt_getprop(fdt, nodeoffset, "#size-cells", &len);
+	if (!sc)
+		return 2;
+
+	if (len != sizeof(*sc))
+		return -FDT_ERR_BADNCELLS;
+
+	val = fdt32_to_cpu(*sc);
+	if ((val < 0) || (val > FDT_MAX_NCELLS))
+		return -FDT_ERR_BADNCELLS;
+
+	return val;
 }
diff --git a/lib/libfdt/fdt_overlay.c b/lib/libfdt/fdt_overlay.c
index 5fdab6c..bf75388 100644
--- a/lib/libfdt/fdt_overlay.c
+++ b/lib/libfdt/fdt_overlay.c
@@ -697,7 +697,7 @@
 	int len = 0, namelen;
 	const char *name;
 
-	FDT_RO_PROBE(fdt);
+	FDT_CHECK_HEADER(fdt);
 
 	for (;;) {
 		name = fdt_get_name(fdt, nodeoffset, &namelen);
@@ -866,8 +866,8 @@
 	uint32_t delta = fdt_get_max_phandle(fdt);
 	int ret;
 
-	FDT_RO_PROBE(fdt);
-	FDT_RO_PROBE(fdto);
+	FDT_CHECK_HEADER(fdt);
+	FDT_CHECK_HEADER(fdto);
 
 	ret = overlay_adjust_local_phandles(fdto, delta);
 	if (ret)
diff --git a/lib/libfdt/fdt_ro.c b/lib/libfdt/fdt_ro.c
index eafc142..dfb3236 100644
--- a/lib/libfdt/fdt_ro.c
+++ b/lib/libfdt/fdt_ro.c
@@ -76,72 +76,17 @@
 		return 0;
 }
 
-const char *fdt_get_string(const void *fdt, int stroffset, int *lenp)
-{
-	uint32_t absoffset = stroffset + fdt_off_dt_strings(fdt);
-	size_t len;
-	int err;
-	const char *s, *n;
-
-	err = fdt_ro_probe_(fdt);
-	if (err != 0)
-		goto fail;
-
-	err = -FDT_ERR_BADOFFSET;
-	if (absoffset >= fdt_totalsize(fdt))
-		goto fail;
-	len = fdt_totalsize(fdt) - absoffset;
-
-	if (fdt_magic(fdt) == FDT_MAGIC) {
-		if (stroffset < 0)
-			goto fail;
-		if (fdt_version(fdt) >= 17) {
-			if (stroffset >= fdt_size_dt_strings(fdt))
-				goto fail;
-			if ((fdt_size_dt_strings(fdt) - stroffset) < len)
-				len = fdt_size_dt_strings(fdt) - stroffset;
-		}
-	} else if (fdt_magic(fdt) == FDT_SW_MAGIC) {
-		if ((stroffset >= 0)
-		    || (stroffset < -fdt_size_dt_strings(fdt)))
-			goto fail;
-		if ((-stroffset) < len)
-			len = -stroffset;
-	} else {
-		err = -FDT_ERR_INTERNAL;
-		goto fail;
-	}
-
-	s = (const char *)fdt + absoffset;
-	n = memchr(s, '\0', len);
-	if (!n) {
-		/* missing terminating NULL */
-		err = -FDT_ERR_TRUNCATED;
-		goto fail;
-	}
-
-	if (lenp)
-		*lenp = n - s;
-	return s;
-
-fail:
-	if (lenp)
-		*lenp = err;
-	return NULL;
-}
-
 const char *fdt_string(const void *fdt, int stroffset)
 {
-	return fdt_get_string(fdt, stroffset, NULL);
+	return (const char *)fdt + fdt_off_dt_strings(fdt) + stroffset;
 }
 
 static int fdt_string_eq_(const void *fdt, int stroffset,
 			  const char *s, int len)
 {
-	int slen;
-	const char *p = fdt_get_string(fdt, stroffset, &slen);
+	const char *p = fdt_string(fdt, stroffset);
 
-	return p && (slen == len) && (memcmp(p, s, len) == 0);
+	return (strlen(p) == len) && (memcmp(p, s, len) == 0);
 }
 
 uint32_t fdt_get_max_phandle(const void *fdt)
@@ -170,42 +115,21 @@
 	return 0;
 }
 
-static const struct fdt_reserve_entry *fdt_mem_rsv(const void *fdt, int n)
-{
-	int offset = n * sizeof(struct fdt_reserve_entry);
-	int absoffset = fdt_off_mem_rsvmap(fdt) + offset;
-
-	if (absoffset < fdt_off_mem_rsvmap(fdt))
-		return NULL;
-	if (absoffset > fdt_totalsize(fdt) - sizeof(struct fdt_reserve_entry))
-		return NULL;
-	return fdt_mem_rsv_(fdt, n);
-}
-
 int fdt_get_mem_rsv(const void *fdt, int n, uint64_t *address, uint64_t *size)
 {
-	const struct fdt_reserve_entry *re;
-
-	FDT_RO_PROBE(fdt);
-	re = fdt_mem_rsv(fdt, n);
-	if (!re)
-		return -FDT_ERR_BADOFFSET;
-
-	*address = fdt64_ld(&re->address);
-	*size = fdt64_ld(&re->size);
+	FDT_CHECK_HEADER(fdt);
+	*address = fdt64_to_cpu(fdt_mem_rsv_(fdt, n)->address);
+	*size = fdt64_to_cpu(fdt_mem_rsv_(fdt, n)->size);
 	return 0;
 }
 
 int fdt_num_mem_rsv(const void *fdt)
 {
-	int i;
-	const struct fdt_reserve_entry *re;
+	int i = 0;
 
-	for (i = 0; (re = fdt_mem_rsv(fdt, i)) != NULL; i++) {
-		if (fdt64_ld(&re->size) == 0)
-			return i;
-	}
-	return -FDT_ERR_TRUNCATED;
+	while (fdt64_to_cpu(fdt_mem_rsv_(fdt, i)->size) != 0)
+		i++;
+	return i;
 }
 
 static int nextprop_(const void *fdt, int offset)
@@ -237,7 +161,7 @@
 {
 	int depth;
 
-	FDT_RO_PROBE(fdt);
+	FDT_CHECK_HEADER(fdt);
 
 	for (depth = 0;
 	     (offset >= 0) && (depth >= 0);
@@ -263,7 +187,7 @@
 	const char *p = path;
 	int offset = 0;
 
-	FDT_RO_PROBE(fdt);
+	FDT_CHECK_HEADER(fdt);
 
 	/* see if we have an alias */
 	if (*path != '/') {
@@ -313,7 +237,7 @@
 	const char *nameptr;
 	int err;
 
-	if (((err = fdt_ro_probe_(fdt)) != 0)
+	if (((err = fdt_check_header(fdt)) != 0)
 	    || ((err = fdt_check_node_offset_(fdt, nodeoffset)) < 0))
 			goto fail;
 
@@ -379,7 +303,7 @@
 	prop = fdt_offset_ptr_(fdt, offset);
 
 	if (lenp)
-		*lenp = fdt32_ld(&prop->len);
+		*lenp = fdt32_to_cpu(prop->len);
 
 	return prop;
 }
@@ -416,7 +340,7 @@
 			offset = -FDT_ERR_INTERNAL;
 			break;
 		}
-		if (fdt_string_eq_(fdt, fdt32_ld(&prop->nameoff),
+		if (fdt_string_eq_(fdt, fdt32_to_cpu(prop->nameoff),
 				   name, namelen)) {
 			if (poffset)
 				*poffset = offset;
@@ -469,7 +393,7 @@
 
 	/* Handle realignment */
 	if (fdt_version(fdt) < 0x10 && (poffset + sizeof(*prop)) % 8 &&
-	    fdt32_ld(&prop->len) >= 8)
+	    fdt32_to_cpu(prop->len) >= 8)
 		return prop->data + 4;
 	return prop->data;
 }
@@ -482,22 +406,12 @@
 	prop = fdt_get_property_by_offset_(fdt, offset, lenp);
 	if (!prop)
 		return NULL;
-	if (namep) {
-		const char *name;
-		int namelen;
-		name = fdt_get_string(fdt, fdt32_ld(&prop->nameoff),
-				      &namelen);
-		if (!name) {
-			if (lenp)
-				*lenp = namelen;
-			return NULL;
-		}
-		*namep = name;
-	}
+	if (namep)
+		*namep = fdt_string(fdt, fdt32_to_cpu(prop->nameoff));
 
 	/* Handle realignment */
 	if (fdt_version(fdt) < 0x10 && (offset + sizeof(*prop)) % 8 &&
-	    fdt32_ld(&prop->len) >= 8)
+	    fdt32_to_cpu(prop->len) >= 8)
 		return prop->data + 4;
 	return prop->data;
 }
@@ -522,7 +436,7 @@
 			return 0;
 	}
 
-	return fdt32_ld(php);
+	return fdt32_to_cpu(*php);
 }
 
 const char *fdt_get_alias_namelen(const void *fdt,
@@ -548,7 +462,7 @@
 	int offset, depth, namelen;
 	const char *name;
 
-	FDT_RO_PROBE(fdt);
+	FDT_CHECK_HEADER(fdt);
 
 	if (buflen < 2)
 		return -FDT_ERR_NOSPACE;
@@ -600,7 +514,7 @@
 	int offset, depth;
 	int supernodeoffset = -FDT_ERR_INTERNAL;
 
-	FDT_RO_PROBE(fdt);
+	FDT_CHECK_HEADER(fdt);
 
 	if (supernodedepth < 0)
 		return -FDT_ERR_NOTFOUND;
@@ -659,7 +573,7 @@
 	const void *val;
 	int len;
 
-	FDT_RO_PROBE(fdt);
+	FDT_CHECK_HEADER(fdt);
 
 	/* FIXME: The algorithm here is pretty horrible: we scan each
 	 * property of a node in fdt_getprop(), then if that didn't
@@ -685,7 +599,7 @@
 	if ((phandle == 0) || (phandle == -1))
 		return -FDT_ERR_BADPHANDLE;
 
-	FDT_RO_PROBE(fdt);
+	FDT_CHECK_HEADER(fdt);
 
 	/* FIXME: The algorithm here is pretty horrible: we
 	 * potentially scan each property of a node in
@@ -838,7 +752,7 @@
 {
 	int offset, err;
 
-	FDT_RO_PROBE(fdt);
+	FDT_CHECK_HEADER(fdt);
 
 	/* FIXME: The algorithm here is pretty horrible: we scan each
 	 * property of a node in fdt_node_check_compatible(), then if
@@ -857,66 +771,3 @@
 
 	return offset; /* error from fdt_next_node() */
 }
-
-int fdt_check_full(const void *fdt, size_t bufsize)
-{
-	int err;
-	int num_memrsv;
-	int offset, nextoffset = 0;
-	uint32_t tag;
-	unsigned depth = 0;
-	const void *prop;
-	const char *propname;
-
-	if (bufsize < FDT_V1_SIZE)
-		return -FDT_ERR_TRUNCATED;
-	err = fdt_check_header(fdt);
-	if (err != 0)
-		return err;
-	if (bufsize < fdt_totalsize(fdt))
-		return -FDT_ERR_TRUNCATED;
-
-	num_memrsv = fdt_num_mem_rsv(fdt);
-	if (num_memrsv < 0)
-		return num_memrsv;
-
-	while (1) {
-		offset = nextoffset;
-		tag = fdt_next_tag(fdt, offset, &nextoffset);
-
-		if (nextoffset < 0)
-			return nextoffset;
-
-		switch (tag) {
-		case FDT_NOP:
-			break;
-
-		case FDT_END:
-			if (depth != 0)
-				return -FDT_ERR_BADSTRUCTURE;
-			return 0;
-
-		case FDT_BEGIN_NODE:
-			depth++;
-			if (depth > INT_MAX)
-				return -FDT_ERR_BADSTRUCTURE;
-			break;
-
-		case FDT_END_NODE:
-			if (depth == 0)
-				return -FDT_ERR_BADSTRUCTURE;
-			depth--;
-			break;
-
-		case FDT_PROP:
-			prop = fdt_getprop_by_offset(fdt, offset, &propname,
-						     &err);
-			if (!prop)
-				return err;
-			break;
-
-		default:
-			return -FDT_ERR_INTERNAL;
-		}
-	}
-}
diff --git a/lib/libfdt/fdt_rw.c b/lib/libfdt/fdt_rw.c
index 2e49855..9b82905 100644
--- a/lib/libfdt/fdt_rw.c
+++ b/lib/libfdt/fdt_rw.c
@@ -67,9 +67,9 @@
 		    (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt)));
 }
 
-static int fdt_rw_probe_(void *fdt)
+static int fdt_rw_check_header_(void *fdt)
 {
-	FDT_RO_PROBE(fdt);
+	FDT_CHECK_HEADER(fdt);
 
 	if (fdt_version(fdt) < 17)
 		return -FDT_ERR_BADVERSION;
@@ -82,10 +82,10 @@
 	return 0;
 }
 
-#define FDT_RW_PROBE(fdt) \
+#define FDT_RW_CHECK_HEADER(fdt) \
 	{ \
 		int err_; \
-		if ((err_ = fdt_rw_probe_(fdt)) != 0) \
+		if ((err_ = fdt_rw_check_header_(fdt)) != 0) \
 			return err_; \
 	}
 
@@ -176,7 +176,7 @@
 	struct fdt_reserve_entry *re;
 	int err;
 
-	FDT_RW_PROBE(fdt);
+	FDT_RW_CHECK_HEADER(fdt);
 
 	re = fdt_mem_rsv_w_(fdt, fdt_num_mem_rsv(fdt));
 	err = fdt_splice_mem_rsv_(fdt, re, 0, 1);
@@ -192,7 +192,7 @@
 {
 	struct fdt_reserve_entry *re = fdt_mem_rsv_w_(fdt, n);
 
-	FDT_RW_PROBE(fdt);
+	FDT_RW_CHECK_HEADER(fdt);
 
 	if (n >= fdt_num_mem_rsv(fdt))
 		return -FDT_ERR_NOTFOUND;
@@ -252,7 +252,7 @@
 	int oldlen, newlen;
 	int err;
 
-	FDT_RW_PROBE(fdt);
+	FDT_RW_CHECK_HEADER(fdt);
 
 	namep = (char *)(uintptr_t)fdt_get_name(fdt, nodeoffset, &oldlen);
 	if (!namep)
@@ -275,7 +275,7 @@
 	struct fdt_property *prop;
 	int err;
 
-	FDT_RW_PROBE(fdt);
+	FDT_RW_CHECK_HEADER(fdt);
 
 	err = fdt_resize_property_(fdt, nodeoffset, name, len, &prop);
 	if (err == -FDT_ERR_NOTFOUND)
@@ -308,7 +308,7 @@
 	struct fdt_property *prop;
 	int err, oldlen, newlen;
 
-	FDT_RW_PROBE(fdt);
+	FDT_RW_CHECK_HEADER(fdt);
 
 	prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
 	if (prop) {
@@ -334,7 +334,7 @@
 	struct fdt_property *prop;
 	int len, proplen;
 
-	FDT_RW_PROBE(fdt);
+	FDT_RW_CHECK_HEADER(fdt);
 
 	prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
 	if (!prop)
@@ -354,7 +354,7 @@
 	uint32_t tag;
 	fdt32_t *endtag;
 
-	FDT_RW_PROBE(fdt);
+	FDT_RW_CHECK_HEADER(fdt);
 
 	offset = fdt_subnode_offset_namelen(fdt, parentoffset, name, namelen);
 	if (offset >= 0)
@@ -394,7 +394,7 @@
 {
 	int endoffset;
 
-	FDT_RW_PROBE(fdt);
+	FDT_RW_CHECK_HEADER(fdt);
 
 	endoffset = fdt_node_end_offset_(fdt, nodeoffset);
 	if (endoffset < 0)
@@ -435,7 +435,7 @@
 	const char *fdtend = fdtstart + fdt_totalsize(fdt);
 	char *tmp;
 
-	FDT_RO_PROBE(fdt);
+	FDT_CHECK_HEADER(fdt);
 
 	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
 		* sizeof(struct fdt_reserve_entry);
@@ -494,7 +494,7 @@
 {
 	int mem_rsv_size;
 
-	FDT_RW_PROBE(fdt);
+	FDT_RW_CHECK_HEADER(fdt);
 
 	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
 		* sizeof(struct fdt_reserve_entry);
diff --git a/lib/libfdt/fdt_sw.c b/lib/libfdt/fdt_sw.c
index 9fa4a94..6d33cc2 100644
--- a/lib/libfdt/fdt_sw.c
+++ b/lib/libfdt/fdt_sw.c
@@ -55,77 +55,21 @@
 
 #include "libfdt_internal.h"
 
-static int fdt_sw_probe_(void *fdt)
+static int fdt_sw_check_header_(void *fdt)
 {
-	if (fdt_magic(fdt) == FDT_MAGIC)
-		return -FDT_ERR_BADSTATE;
-	else if (fdt_magic(fdt) != FDT_SW_MAGIC)
+	if (fdt_magic(fdt) != FDT_SW_MAGIC)
 		return -FDT_ERR_BADMAGIC;
+	/* FIXME: should check more details about the header state */
 	return 0;
 }
 
-#define FDT_SW_PROBE(fdt) \
+#define FDT_SW_CHECK_HEADER(fdt) \
 	{ \
 		int err; \
-		if ((err = fdt_sw_probe_(fdt)) != 0) \
+		if ((err = fdt_sw_check_header_(fdt)) != 0) \
 			return err; \
 	}
 
-/* 'memrsv' state:	Initial state after fdt_create()
- *
- * Allowed functions:
- *	fdt_add_reservmap_entry()
- *	fdt_finish_reservemap()		[moves to 'struct' state]
- */
-static int fdt_sw_probe_memrsv_(void *fdt)
-{
-	int err = fdt_sw_probe_(fdt);
-	if (err)
-		return err;
-
-	if (fdt_off_dt_strings(fdt) != 0)
-		return -FDT_ERR_BADSTATE;
-	return 0;
-}
-
-#define FDT_SW_PROBE_MEMRSV(fdt) \
-	{ \
-		int err; \
-		if ((err = fdt_sw_probe_memrsv_(fdt)) != 0) \
-			return err; \
-	}
-
-/* 'struct' state:	Enter this state after fdt_finish_reservemap()
- *
- * Allowed functions:
- *	fdt_begin_node()
- *	fdt_end_node()
- *	fdt_property*()
- *	fdt_finish()			[moves to 'complete' state]
- */
-static int fdt_sw_probe_struct_(void *fdt)
-{
-	int err = fdt_sw_probe_(fdt);
-	if (err)
-		return err;
-
-	if (fdt_off_dt_strings(fdt) != fdt_totalsize(fdt))
-		return -FDT_ERR_BADSTATE;
-	return 0;
-}
-
-#define FDT_SW_PROBE_STRUCT(fdt) \
-	{ \
-		int err; \
-		if ((err = fdt_sw_probe_struct_(fdt)) != 0) \
-			return err; \
-	}
-
-/* 'complete' state:	Enter this state after fdt_finish()
- *
- * Allowed functions: none
- */
-
 static void *fdt_grab_space_(void *fdt, size_t len)
 {
 	int offset = fdt_size_dt_struct(fdt);
@@ -143,11 +87,9 @@
 
 int fdt_create(void *buf, int bufsize)
 {
-	const size_t hdrsize = FDT_ALIGN(sizeof(struct fdt_header),
-					 sizeof(struct fdt_reserve_entry));
 	void *fdt = buf;
 
-	if (bufsize < hdrsize)
+	if (bufsize < sizeof(struct fdt_header))
 		return -FDT_ERR_NOSPACE;
 
 	memset(buf, 0, bufsize);
@@ -157,9 +99,10 @@
 	fdt_set_last_comp_version(fdt, FDT_FIRST_SUPPORTED_VERSION);
 	fdt_set_totalsize(fdt,  bufsize);
 
-	fdt_set_off_mem_rsvmap(fdt, hdrsize);
+	fdt_set_off_mem_rsvmap(fdt, FDT_ALIGN(sizeof(struct fdt_header),
+					      sizeof(struct fdt_reserve_entry)));
 	fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt));
-	fdt_set_off_dt_strings(fdt, 0);
+	fdt_set_off_dt_strings(fdt, bufsize);
 
 	return 0;
 }
@@ -169,14 +112,11 @@
 	size_t headsize, tailsize;
 	char *oldtail, *newtail;
 
-	FDT_SW_PROBE(fdt);
+	FDT_SW_CHECK_HEADER(fdt);
 
-	headsize = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
+	headsize = fdt_off_dt_struct(fdt);
 	tailsize = fdt_size_dt_strings(fdt);
 
-	if ((headsize + tailsize) > fdt_totalsize(fdt))
-		return -FDT_ERR_INTERNAL;
-
 	if ((headsize + tailsize) > bufsize)
 		return -FDT_ERR_NOSPACE;
 
@@ -193,9 +133,8 @@
 		memmove(buf, fdt, headsize);
 	}
 
+	fdt_set_off_dt_strings(buf, bufsize);
 	fdt_set_totalsize(buf, bufsize);
-	if (fdt_off_dt_strings(buf))
-		fdt_set_off_dt_strings(buf, bufsize);
 
 	return 0;
 }
@@ -205,7 +144,10 @@
 	struct fdt_reserve_entry *re;
 	int offset;
 
-	FDT_SW_PROBE_MEMRSV(fdt);
+	FDT_SW_CHECK_HEADER(fdt);
+
+	if (fdt_size_dt_struct(fdt))
+		return -FDT_ERR_BADSTATE;
 
 	offset = fdt_off_dt_struct(fdt);
 	if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
@@ -222,23 +164,16 @@
 
 int fdt_finish_reservemap(void *fdt)
 {
-	int err = fdt_add_reservemap_entry(fdt, 0, 0);
-
-	if (err)
-		return err;
-
-	fdt_set_off_dt_strings(fdt, fdt_totalsize(fdt));
-	return 0;
+	return fdt_add_reservemap_entry(fdt, 0, 0);
 }
 
 int fdt_begin_node(void *fdt, const char *name)
 {
 	struct fdt_node_header *nh;
-	int namelen;
+	int namelen = strlen(name) + 1;
 
-	FDT_SW_PROBE_STRUCT(fdt);
+	FDT_SW_CHECK_HEADER(fdt);
 
-	namelen = strlen(name) + 1;
 	nh = fdt_grab_space_(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
 	if (! nh)
 		return -FDT_ERR_NOSPACE;
@@ -252,7 +187,7 @@
 {
 	fdt32_t *en;
 
-	FDT_SW_PROBE_STRUCT(fdt);
+	FDT_SW_CHECK_HEADER(fdt);
 
 	en = fdt_grab_space_(fdt, FDT_TAGSIZE);
 	if (! en)
@@ -290,7 +225,7 @@
 	struct fdt_property *prop;
 	int nameoff;
 
-	FDT_SW_PROBE_STRUCT(fdt);
+	FDT_SW_CHECK_HEADER(fdt);
 
 	nameoff = fdt_find_add_string_(fdt, name);
 	if (nameoff == 0)
@@ -327,7 +262,7 @@
 	uint32_t tag;
 	int offset, nextoffset;
 
-	FDT_SW_PROBE_STRUCT(fdt);
+	FDT_SW_CHECK_HEADER(fdt);
 
 	/* Add terminator */
 	end = fdt_grab_space_(fdt, sizeof(*end));
diff --git a/lib/libfdt/libfdt_internal.h b/lib/libfdt/libfdt_internal.h
index 4109f89..7681e19 100644
--- a/lib/libfdt/libfdt_internal.h
+++ b/lib/libfdt/libfdt_internal.h
@@ -55,11 +55,10 @@
 #define FDT_ALIGN(x, a)		(((x) + (a) - 1) & ~((a) - 1))
 #define FDT_TAGALIGN(x)		(FDT_ALIGN((x), FDT_TAGSIZE))
 
-int fdt_ro_probe_(const void *fdt);
-#define FDT_RO_PROBE(fdt)			\
+#define FDT_CHECK_HEADER(fdt) \
 	{ \
 		int err_; \
-		if ((err_ = fdt_ro_probe_(fdt)) != 0)	\
+		if ((err_ = fdt_check_header(fdt)) != 0) \
 			return err_; \
 	}
 
diff --git a/lib/locks/bakery/bakery_lock_coherent.c b/lib/locks/bakery/bakery_lock_coherent.c
index 03277c3..8e86790 100644
--- a/lib/locks/bakery/bakery_lock_coherent.c
+++ b/lib/locks/bakery/bakery_lock_coherent.c
@@ -35,9 +35,9 @@
  */
 
 #define assert_bakery_entry_valid(_entry, _bakery) do {	\
-	assert(_bakery);					\
-	assert(_entry < BAKERY_LOCK_MAX_CPUS);		\
-} while (0)
+	assert((_bakery) != NULL);			\
+	assert((_entry) < BAKERY_LOCK_MAX_CPUS);	\
+} while (false)
 
 /* Obtain a ticket for a given CPU */
 static unsigned int bakery_get_ticket(bakery_lock_t *bakery, unsigned int me)
@@ -46,7 +46,7 @@
 	unsigned int they;
 
 	/* Prevent recursive acquisition */
-	assert(!bakery_ticket_number(bakery->lock_data[me]));
+	assert(bakery_ticket_number(bakery->lock_data[me]) == 0U);
 
 	/*
 	 * Flag that we're busy getting our ticket. All CPUs are iterated in the
@@ -58,9 +58,9 @@
 	 * ticket value. That's OK as the lock is acquired based on the priority
 	 * value, not the ticket value alone.
 	 */
-	my_ticket = 0;
+	my_ticket = 0U;
 	bakery->lock_data[me] = make_bakery_data(CHOOSING_TICKET, my_ticket);
-	for (they = 0; they < BAKERY_LOCK_MAX_CPUS; they++) {
+	for (they = 0U; they < BAKERY_LOCK_MAX_CPUS; they++) {
 		their_ticket = bakery_ticket_number(bakery->lock_data[they]);
 		if (their_ticket > my_ticket)
 			my_ticket = their_ticket;
@@ -105,8 +105,8 @@
 	 * Now that we got our ticket, compute our priority value, then compare
 	 * with that of others, and proceed to acquire the lock
 	 */
-	my_prio = PRIORITY(my_ticket, me);
-	for (they = 0; they < BAKERY_LOCK_MAX_CPUS; they++) {
+	my_prio = bakery_get_priority(my_ticket, me);
+	for (they = 0U; they < BAKERY_LOCK_MAX_CPUS; they++) {
 		if (me == they)
 			continue;
 
@@ -120,7 +120,8 @@
 		 * (valid) ticket value. If they do, compare priorities
 		 */
 		their_ticket = bakery_ticket_number(their_bakery_data);
-		if (their_ticket && (PRIORITY(their_ticket, they) < my_prio)) {
+		if ((their_ticket != 0U) &&
+		    (bakery_get_priority(their_ticket, they) < my_prio)) {
 			/*
 			 * They have higher priority (lower value). Wait for
 			 * their ticket value to change (either release the lock
@@ -148,7 +149,7 @@
 	unsigned int me = plat_my_core_pos();
 
 	assert_bakery_entry_valid(me, bakery);
-	assert(bakery_ticket_number(bakery->lock_data[me]));
+	assert(bakery_ticket_number(bakery->lock_data[me]) != 0U);
 
 	/*
 	 * Ensure that other observers see any stores in the critical section
@@ -156,7 +157,7 @@
 	 * Then signal other waiting contenders.
 	 */
 	dmbst();
-	bakery->lock_data[me] = 0;
+	bakery->lock_data[me] = 0U;
 	dsb();
 	sev();
 }
diff --git a/lib/locks/bakery/bakery_lock_normal.c b/lib/locks/bakery/bakery_lock_normal.c
index b947da9..beae63c 100644
--- a/lib/locks/bakery/bakery_lock_normal.c
+++ b/lib/locks/bakery/bakery_lock_normal.c
@@ -53,21 +53,31 @@
 IMPORT_SYM(uintptr_t, __PERCPU_BAKERY_LOCK_SIZE__, PERCPU_BAKERY_LOCK_SIZE);
 #endif
 
-#define get_bakery_info(_cpu_ix, _lock)	\
-	(bakery_info_t *)((uintptr_t)_lock + _cpu_ix * PERCPU_BAKERY_LOCK_SIZE)
+static inline bakery_lock_t *get_bakery_info(unsigned int cpu_ix,
+					     bakery_lock_t *lock)
+{
+	return (bakery_info_t *)((uintptr_t)lock +
+				cpu_ix * PERCPU_BAKERY_LOCK_SIZE);
+}
 
-#define write_cache_op(_addr, _cached)	\
-				do {	\
-					(_cached ? dccvac((uintptr_t)_addr) :\
-						dcivac((uintptr_t)_addr));\
-						dsbish();\
-				} while (0)
+static inline void write_cache_op(uintptr_t addr, bool cached)
+{
+	if (cached)
+		dccvac(addr);
+	else
+		dcivac(addr);
 
-#define read_cache_op(_addr, _cached)	if (_cached) \
-					    dccivac((uintptr_t)_addr)
+	dsbish();
+}
+
+static inline void read_cache_op(uintptr_t addr, bool cached)
+{
+	if (cached)
+		dccivac(addr);
+}
 
 /* Helper function to check if the lock is acquired */
-static inline int is_lock_acquired(const bakery_info_t *my_bakery_info,
+static inline bool is_lock_acquired(const bakery_info_t *my_bakery_info,
 							int is_cached)
 {
 	/*
@@ -78,8 +88,8 @@
 	 * operations were not propagated to all the caches in the system.
 	 * Hence do a `read_cache_op()` prior to read.
 	 */
-	read_cache_op(my_bakery_info, is_cached);
-	return !!(bakery_ticket_number(my_bakery_info->lock_data));
+	read_cache_op((uintptr_t)my_bakery_info, is_cached);
+	return bakery_ticket_number(my_bakery_info->lock_data) != 0U;
 }
 
 static unsigned int bakery_get_ticket(bakery_lock_t *lock,
@@ -94,7 +104,7 @@
 	 * it is not NULL.
 	 */
 	my_bakery_info = get_bakery_info(me, lock);
-	assert(my_bakery_info);
+	assert(my_bakery_info != NULL);
 
 	/* Prevent recursive acquisition.*/
 	assert(!is_lock_acquired(my_bakery_info, is_cached));
@@ -103,16 +113,16 @@
 	 * Tell other contenders that we are through the bakery doorway i.e.
 	 * going to allocate a ticket for this cpu.
 	 */
-	my_ticket = 0;
+	my_ticket = 0U;
 	my_bakery_info->lock_data = make_bakery_data(CHOOSING_TICKET, my_ticket);
 
-	write_cache_op(my_bakery_info, is_cached);
+	write_cache_op((uintptr_t)my_bakery_info, is_cached);
 
 	/*
 	 * Iterate through the bakery information of each contender to allocate
 	 * the highest ticket number for this cpu.
 	 */
-	for (they = 0; they < BAKERY_LOCK_MAX_CPUS; they++) {
+	for (they = 0U; they < BAKERY_LOCK_MAX_CPUS; they++) {
 		if (me == they)
 			continue;
 
@@ -121,9 +131,9 @@
 		 * ensure that a stale copy is not read.
 		 */
 		their_bakery_info = get_bakery_info(they, lock);
-		assert(their_bakery_info);
+		assert(their_bakery_info != NULL);
 
-		read_cache_op(their_bakery_info, is_cached);
+		read_cache_op((uintptr_t)their_bakery_info, is_cached);
 
 		/*
 		 * Update this cpu's ticket number if a higher ticket number is
@@ -141,7 +151,7 @@
 	++my_ticket;
 	my_bakery_info->lock_data = make_bakery_data(CHOSEN_TICKET, my_ticket);
 
-	write_cache_op(my_bakery_info, is_cached);
+	write_cache_op((uintptr_t)my_bakery_info, is_cached);
 
 	return my_ticket;
 }
@@ -167,8 +177,8 @@
 	 * Now that we got our ticket, compute our priority value, then compare
 	 * with that of others, and proceed to acquire the lock
 	 */
-	my_prio = PRIORITY(my_ticket, me);
-	for (they = 0; they < BAKERY_LOCK_MAX_CPUS; they++) {
+	my_prio = bakery_get_priority(my_ticket, me);
+	for (they = 0U; they < BAKERY_LOCK_MAX_CPUS; they++) {
 		if (me == they)
 			continue;
 
@@ -177,11 +187,11 @@
 		 * ensure that a stale copy is not read.
 		 */
 		their_bakery_info = get_bakery_info(they, lock);
-		assert(their_bakery_info);
+		assert(their_bakery_info != NULL);
 
 		/* Wait for the contender to get their ticket */
 		do {
-			read_cache_op(their_bakery_info, is_cached);
+			read_cache_op((uintptr_t)their_bakery_info, is_cached);
 			their_bakery_data = their_bakery_info->lock_data;
 		} while (bakery_is_choosing(their_bakery_data));
 
@@ -190,7 +200,7 @@
 		 * (valid) ticket value. If they do, compare priorities
 		 */
 		their_ticket = bakery_ticket_number(their_bakery_data);
-		if (their_ticket && (PRIORITY(their_ticket, they) < my_prio)) {
+		if (their_ticket && (bakery_get_priority(their_ticket, they) < my_prio)) {
 			/*
 			 * They have higher priority (lower value). Wait for
 			 * their ticket value to change (either release the lock
@@ -199,7 +209,7 @@
 			 */
 			do {
 				wfe();
-				read_cache_op(their_bakery_info, is_cached);
+				read_cache_op((uintptr_t)their_bakery_info, is_cached);
 			} while (their_ticket
 				== bakery_ticket_number(their_bakery_info->lock_data));
 		}
@@ -231,7 +241,7 @@
 	 * Then signal other waiting contenders.
 	 */
 	dmbst();
-	my_bakery_info->lock_data = 0;
-	write_cache_op(my_bakery_info, is_cached);
+	my_bakery_info->lock_data = 0U;
+	write_cache_op((uintptr_t)my_bakery_info, is_cached);
 	sev();
 }
diff --git a/maintainers.rst b/maintainers.rst
index 6d649c3..3122ecd 100644
--- a/maintainers.rst
+++ b/maintainers.rst
@@ -89,7 +89,7 @@
 :F: docs/plat/marvell/
 :F: plat/marvell/
 :F: drivers/marvell/
-:F: tools/doimage/
+:F: tools/marvell/
 
 NVidia platform ports
 ---------------------
diff --git a/plat/allwinner/common/allwinner-common.mk b/plat/allwinner/common/allwinner-common.mk
new file mode 100644
index 0000000..2dc058f
--- /dev/null
+++ b/plat/allwinner/common/allwinner-common.mk
@@ -0,0 +1,59 @@
+#
+# Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include lib/xlat_tables_v2/xlat_tables.mk
+
+AW_PLAT			:=	plat/allwinner
+
+PLAT_INCLUDES		:=	-Iinclude/plat/arm/common		\
+				-Iinclude/plat/arm/common/aarch64	\
+				-I${AW_PLAT}/common/include		\
+				-I${AW_PLAT}/${PLAT}/include
+
+include lib/libfdt/libfdt.mk
+
+PLAT_BL_COMMON_SOURCES	:=	drivers/console/${ARCH}/console.S	\
+				drivers/ti/uart/${ARCH}/16550_console.S	\
+				${XLAT_TABLES_LIB_SRCS}			\
+				${AW_PLAT}/common/plat_helpers.S	\
+				${AW_PLAT}/common/sunxi_common.c
+
+BL31_SOURCES		+=	drivers/arm/gic/common/gic_common.c	\
+				drivers/arm/gic/v2/gicv2_helpers.c	\
+				drivers/arm/gic/v2/gicv2_main.c		\
+				drivers/delay_timer/delay_timer.c	\
+				drivers/delay_timer/generic_delay_timer.c \
+				lib/cpus/${ARCH}/cortex_a53.S		\
+				plat/common/plat_gicv2.c		\
+				plat/common/plat_psci_common.c		\
+				${AW_PLAT}/common/sunxi_bl31_setup.c	\
+				${AW_PLAT}/common/sunxi_cpu_ops.c	\
+				${AW_PLAT}/common/sunxi_pm.c		\
+				${AW_PLAT}/${PLAT}/sunxi_power.c	\
+				${AW_PLAT}/common/sunxi_security.c	\
+				${AW_PLAT}/common/sunxi_topology.c
+
+# The bootloader is guaranteed to only run on CPU 0 by the boot ROM.
+COLD_BOOT_SINGLE_CPU		:=	1
+
+# Enable workarounds for Cortex-A53 errata. Allwinner uses at least r0p4.
+ERRATA_A53_835769		:=	1
+ERRATA_A53_843419		:=	1
+ERRATA_A53_855873		:=	1
+
+MULTI_CONSOLE_API		:=	1
+
+# The reset vector can be changed for each CPU.
+PROGRAMMABLE_RESET_ADDRESS	:=	1
+
+# Allow mapping read-only data as execute-never.
+SEPARATE_CODE_AND_RODATA	:=	1
+
+# BL31 gets loaded alongside BL33 (U-Boot) by U-Boot's SPL
+RESET_TO_BL31			:=	1
+
+# We are short on memory, so save 3.5KB by not having an extra coherent page.
+USE_COHERENT_MEM		:=	0
diff --git a/plat/allwinner/common/arisc_off.S b/plat/allwinner/common/arisc_off.S
new file mode 100644
index 0000000..ed10832
--- /dev/null
+++ b/plat/allwinner/common/arisc_off.S
@@ -0,0 +1,115 @@
+# turn_off_core.S
+#
+# Copyright (c) 2018, Andre Przywara <osp@andrep.de>
+# SPDX-License-Identifier: BSD-3-Clause
+#
+# OpenRISC assembly to turn off an ARM core on an Allwinner SoC from
+# the arisc management controller.
+# Generate a binary representation with:
+# $ or1k-elf-as -c -o turn_off_core.o turn_off_core.S
+# $ or1k-elf-objcopy -O binary --reverse-bytes=4 turn_off_core.o \
+#   turn_off_core.bin
+# The encoded instructions go into an array defined in
+# plat/allwinner/sun50i_*/include/core_off_arisc.h, to be handed off to
+# the arisc processor.
+#
+# This routine is meant to be called directly from arisc reset (put the
+# start address in the reset vector), to be actually triggered by that
+# very ARM core to be turned off.
+# It expects the core number presented as a mask in the upper half of
+# r3, so to be patched in the lower 16 bits of the first instruction,
+# overwriting the 0 in this code here.
+# The code will do the following:
+# - Read the C_CPU_STATUS register, which contains the status of the WFI
+#   lines of each of the four A53 cores.
+# - Loop until the core in question reaches WFI.
+# - Using that mask, activate the core output clamps by setting the
+#   respective core bit in CPUX_PWROFF_GATING_REG (0x1f01500).
+#   Note that the clamp for core 0 covers more than just the core, activating
+#   it hangs the whole system. So we skip this step for core 0.
+# - Using the negated mask, assert the core's reset line by clearing the
+#   respective bit in C_RST_CTRL (0x1f01c30).
+# - Finally turn off the core's power switch by writing 0xff to the
+#   respective CPUx_PWR_SWITCH_REG (0x1f01540 ff.)
+# - Assert the arisc's own reset to end execution.
+#   This also signals other arisc users that the chip is free again.
+# So in C this would look like:
+#	while (!(readl(0x1700030) & (1U << core_nr)))
+#		;
+#	if (core_nr != 0)
+#		writel(readl(0x1f01500) | (1U << core_nr), 0x1f01500);
+#	writel(readl(0x1f01c30) & ~(1U << core_nr), 0x1f01c30);
+#	writel(0xff, 0x1f01540 + (core_nr * 4));
+# (using A64/H5 addresses)
+
+.text
+_start:
+	l.movhi	r3, 0				# FIXUP! with core mask
+	l.movhi r0, 0				# clear r0
+	l.movhi	r13, 0x170			# r13: CPU_CFG_BASE=0x01700000
+wait_wfi:
+	l.lwz	r5, 0x30(r13)			# load C_CPU_STATUS
+	l.and	r5, r5, r3			# mask requested core
+	l.sfeq	r5, r0				# is it not yet in WFI?
+	l.bf	wait_wfi			# try again
+
+	l.srli	r6, r3, 16			# move mask to lower 16 bits
+	l.sfeqi	r6, 1				# core 0 is special
+	l.bf	1f				# don't touch the bit for core 0
+	l.movhi	r13, 0x1f0			# address of R_CPUCFG (delay)
+	l.lwz	r5, 0x1500(r13)			# core output clamps
+	l.or	r5, r5, r6			# set bit to ...
+	l.sw	0x1500(r13), r5			# ... activate for our core
+
+1:	l.lwz	r5, 0x1c30(r13)			# CPU power-on reset
+	l.xori	r6, r6, -1			# negate core mask
+	l.and	r5, r5, r6			# clear bit to ...
+	l.sw	0x1c30(r13), r5			# ... assert for our core
+
+	l.ff1	r6, r3				# get core number from high mask
+	l.addi	r6, r6, -17			# convert to 0-3
+	l.slli	r6, r6, 2			# r5: core number*4 (0-12)
+	l.add	r6, r6, r13			# add to base address
+	l.ori	r5, r0, 0xff			# 0xff means all switches off
+	l.sw	0x1540(r6), r5			# core power switch registers
+
+reset:	l.sw	0x1c00(r13),r0			# pull down our own reset line
+
+	l.j	reset				# just in case ....
+	l.nop	0x0				# (delay slot)
+
+# same as above, but with the MMIO addresses matching the H6 SoC
+_start_h6:
+	l.movhi	r3, 0				# FIXUP! with core mask
+	l.movhi r0, 0				# clear r0
+	l.movhi	r13, 0x901			# r13: CPU_CFG_BASE=0x09010000
+1:
+	l.lwz	r5, 0x80(r13)			# load C_CPU_STATUS
+	l.and	r5, r5, r3			# mask requested core
+	l.sfeq	r5, r0				# is it not yet in WFI?
+	l.bf	1b				# try again
+
+	l.srli	r6, r3, 16			# move mask to lower 16 bits(ds)
+	l.sfeqi	r6, 1				# core 0 is special
+	l.bf	1f				# don't touch the bit for core 0
+	l.movhi	r13, 0x700			# address of R_CPUCFG (ds)
+	l.lwz	r5, 0x0444(r13)			# core output clamps
+	l.or	r5, r5, r6			# set bit to ...
+	l.sw	0x0444(r13), r5			# ... activate for our core
+
+1:	l.lwz	r5, 0x0440(r13)			# CPU power-on reset
+	l.xori	r6, r6, -1			# negate core mask
+	l.and	r5, r5, r6			# clear bit to ...
+	l.sw	0x0440(r13), r5			# ... assert for our core
+
+	l.ff1	r6, r3				# get core number from high mask
+	l.addi	r6, r6, -17			# convert to 0-3
+	l.slli	r6, r6, 2			# r5: core number*4 (0-12)
+	l.add	r6, r6, r13			# add to base address
+	l.ori	r5, r0, 0xff			# 0xff means all switches off
+	l.sw	0x0450(r6), r5			# core power switch registers
+
+1:	l.sw	0x0400(r13),r0			# pull down our own reset line
+
+	l.j	1b				# just in case ...
+	l.nop	0x0				# (delay slot)
diff --git a/plat/allwinner/common/include/platform_def.h b/plat/allwinner/common/include/platform_def.h
index b46d410..08eb5cf 100644
--- a/plat/allwinner/common/include/platform_def.h
+++ b/plat/allwinner/common/include/platform_def.h
@@ -18,11 +18,17 @@
 /* The traditional U-Boot load address is 160MB into DRAM, so at 0x4a000000 */
 #define PLAT_SUNXI_NS_IMAGE_OFFSET	(SUNXI_DRAM_BASE + (160U << 20))
 
+/* How much memory to reserve as secure for BL32, if configured */
+#define SUNXI_DRAM_SEC_SIZE		(32U << 20)
+
+/* How much DRAM to map (to map BL33, for fetching the DTB from U-Boot) */
+#define SUNXI_DRAM_MAP_SIZE		(64U << 20)
+
 #define CACHE_WRITEBACK_SHIFT		6
 #define CACHE_WRITEBACK_GRANULE		(1 << CACHE_WRITEBACK_SHIFT)
 
-#define MAX_MMAP_REGIONS		(4 + PLATFORM_MMAP_REGIONS)
-#define MAX_XLAT_TABLES			2
+#define MAX_MMAP_REGIONS		(3 + PLATFORM_MMAP_REGIONS)
+#define MAX_XLAT_TABLES			1
 
 #define PLAT_MAX_PWR_LVL_STATES		U(2)
 #define PLAT_MAX_RET_STATE		U(1)
@@ -34,13 +40,13 @@
 					 PLATFORM_CORE_COUNT)
 
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
-#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)
+#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 28)
 
 #define PLATFORM_CLUSTER_COUNT		1
 #define PLATFORM_CORE_COUNT		(PLATFORM_CLUSTER_COUNT * \
 					 PLATFORM_MAX_CPUS_PER_CLUSTER)
 #define PLATFORM_MAX_CPUS_PER_CLUSTER	4
-#define PLATFORM_MMAP_REGIONS		3
+#define PLATFORM_MMAP_REGIONS		4
 #define PLATFORM_STACK_SIZE		(0x1000 / PLATFORM_CORE_COUNT)
 
 #ifndef SPD_none
diff --git a/plat/allwinner/common/include/sunxi_def.h b/plat/allwinner/common/include/sunxi_def.h
index e68fbe4..da87b23 100644
--- a/plat/allwinner/common/include/sunxi_def.h
+++ b/plat/allwinner/common/include/sunxi_def.h
@@ -14,4 +14,8 @@
 #define SUNXI_UART0_BAUDRATE		115200
 #define SUNXI_UART0_CLK_IN_HZ		SUNXI_OSC24M_CLK_IN_HZ
 
+#define SUNXI_SOC_A64			0x1689
+#define SUNXI_SOC_H5			0x1718
+#define SUNXI_SOC_H6			0x1728
+
 #endif /* __SUNXI_DEF_H__ */
diff --git a/plat/allwinner/common/include/sunxi_private.h b/plat/allwinner/common/include/sunxi_private.h
new file mode 100644
index 0000000..1e1b0a4
--- /dev/null
+++ b/plat/allwinner/common/include/sunxi_private.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SUNXI_PRIVATE_H
+#define SUNXI_PRIVATE_H
+
+void sunxi_configure_mmu_el3(int flags);
+
+void sunxi_cpu_on(unsigned int cluster, unsigned int core);
+void sunxi_cpu_off(unsigned int cluster, unsigned int core);
+void sunxi_disable_secondary_cpus(unsigned int primary_cpu);
+void __dead2 sunxi_power_down(void);
+
+int sunxi_pmic_setup(uint16_t socid, const void *fdt);
+void sunxi_security_setup(void);
+
+uint16_t sunxi_read_soc_id(void);
+void sunxi_set_gpio_out(char port, int pin, bool level_high);
+int sunxi_init_platform_r_twi(uint16_t socid, bool use_rsb);
+void sunxi_execute_arisc_code(uint32_t *code, size_t size,
+			      int patch_offset, uint16_t param);
+
+#endif /* SUNXI_PRIVATE_H */
diff --git a/plat/allwinner/common/sunxi_bl31_setup.c b/plat/allwinner/common/sunxi_bl31_setup.c
index 7e11cec..8f597c3 100644
--- a/plat/allwinner/common/sunxi_bl31_setup.c
+++ b/plat/allwinner/common/sunxi_bl31_setup.c
@@ -10,13 +10,14 @@
 #include <debug.h>
 #include <generic_delay_timer.h>
 #include <gicv2.h>
+#include <libfdt.h>
 #include <platform.h>
 #include <platform_def.h>
 #include <sunxi_def.h>
 #include <sunxi_mmap.h>
+#include <sunxi_private.h>
 #include <uart_16550.h>
 
-#include "sunxi_private.h"
 
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
@@ -28,6 +29,47 @@
 	.gicc_base = SUNXI_GICC_BASE,
 };
 
+/*
+ * Try to find a DTB loaded in memory by previous stages.
+ *
+ * At the moment we implement a heuristic to find the DTB attached to U-Boot:
+ * U-Boot appends its DTB to the end of the image. Assuming that BL33 is
+ * U-Boot, try to find the size of the U-Boot image to learn the DTB address.
+ * The generic ARMv8 U-Boot image contains the load address and its size
+ * as u64 variables at the beginning of the image. There might be padding
+ * or other headers before that data, so scan the first 2KB after the BL33
+ * entry point to find the load address, which should be followed by the
+ * size. Adding those together gives us the address of the DTB.
+ */
+static void *sunxi_find_dtb(void)
+{
+	uint64_t *u_boot_base;
+	int i;
+
+	u_boot_base = (void *)(SUNXI_DRAM_VIRT_BASE + SUNXI_DRAM_SEC_SIZE);
+
+	for (i = 0; i < 2048 / sizeof(uint64_t); i++) {
+		uint32_t *dtb_base;
+
+		if (u_boot_base[i] != PLAT_SUNXI_NS_IMAGE_OFFSET)
+			continue;
+
+		/* Does the suspected U-Boot size look anyhow reasonable? */
+		if (u_boot_base[i + 1] >= 256 * 1024 * 1024)
+			continue;
+
+		/* end of the image: base address + size */
+		dtb_base = (void *)((char *)u_boot_base + u_boot_base[i + 1]);
+
+		if (fdt_check_header(dtb_base) != 0)
+			continue;
+
+		return dtb_base;
+	}
+
+	return NULL;
+}
+
 void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 				u_register_t arg2, u_register_t arg3)
 {
@@ -66,15 +108,16 @@
 {
 	const char *soc_name;
 	uint16_t soc_id = sunxi_read_soc_id();
+	void *fdt;
 
 	switch (soc_id) {
-	case 0x1689:
+	case SUNXI_SOC_A64:
 		soc_name = "A64/H64/R18";
 		break;
-	case 0x1718:
+	case SUNXI_SOC_H5:
 		soc_name = "H5";
 		break;
-	case 0x1728:
+	case SUNXI_SOC_H6:
 		soc_name = "H6";
 		break;
 	default:
@@ -85,6 +128,18 @@
 
 	generic_delay_timer_init();
 
+	fdt = sunxi_find_dtb();
+	if (fdt) {
+		const char *model;
+		int length;
+
+		model = fdt_getprop(fdt, 0, "model", &length);
+		NOTICE("BL31: Found U-Boot DTB at %p, model: %s\n", fdt,
+		     model ?: "unknown");
+	} else {
+		NOTICE("BL31: No DTB found.\n");
+	}
+
 	/* Configure the interrupt controller */
 	gicv2_driver_init(&sunxi_gic_data);
 	gicv2_distif_init();
@@ -93,7 +148,7 @@
 
 	sunxi_security_setup();
 
-	sunxi_pmic_setup();
+	sunxi_pmic_setup(soc_id, fdt);
 
 	INFO("BL31: Platform setup done\n");
 }
diff --git a/plat/allwinner/common/sunxi_common.c b/plat/allwinner/common/sunxi_common.c
index fc9bf20..2eb26a9 100644
--- a/plat/allwinner/common/sunxi_common.c
+++ b/plat/allwinner/common/sunxi_common.c
@@ -4,21 +4,28 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <arch_helpers.h>
+#include <debug.h>
+#include <errno.h>
 #include <mmio.h>
 #include <platform.h>
 #include <platform_def.h>
 #include <sunxi_def.h>
+#include <sunxi_mmap.h>
+#include <sunxi_private.h>
 #include <xlat_tables_v2.h>
 
-#include "sunxi_private.h"
-
 static mmap_region_t sunxi_mmap[PLATFORM_MMAP_REGIONS + 1] = {
 	MAP_REGION_FLAT(SUNXI_SRAM_BASE, SUNXI_SRAM_SIZE,
 			MT_MEMORY | MT_RW | MT_SECURE),
 	MAP_REGION_FLAT(SUNXI_DEV_BASE, SUNXI_DEV_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE),
-	MAP_REGION_FLAT(SUNXI_DRAM_BASE, SUNXI_DRAM_SIZE,
-			MT_MEMORY | MT_RW | MT_NS),
+	MAP_REGION(SUNXI_DRAM_BASE, SUNXI_DRAM_VIRT_BASE, SUNXI_DRAM_SEC_SIZE,
+			MT_MEMORY | MT_RW | MT_SECURE),
+	MAP_REGION(PLAT_SUNXI_NS_IMAGE_OFFSET,
+		   SUNXI_DRAM_VIRT_BASE + SUNXI_DRAM_SEC_SIZE,
+		   SUNXI_DRAM_MAP_SIZE,
+		   MT_MEMORY | MT_RO | MT_NS),
 	{},
 };
 
@@ -47,9 +54,6 @@
 	mmap_add_region(BL_RO_DATA_BASE, BL_RO_DATA_BASE,
 			BL_RO_DATA_END - BL_RO_DATA_BASE,
 			MT_RO_DATA | MT_SECURE);
-	mmap_add_region(BL_COHERENT_RAM_BASE, BL_COHERENT_RAM_BASE,
-			BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE,
-			MT_DEVICE | MT_RW | MT_SECURE);
 	mmap_add(sunxi_mmap);
 	init_xlat_tables();
 
@@ -71,3 +75,136 @@
 
 	return reg >> 16;
 }
+
+/*
+ * Configure a given pin to the GPIO-OUT function and sets its level.
+ * The port is given as a capital letter, the pin is the number within
+ * this port group.
+ * So to set pin PC7 to high, use: sunxi_set_gpio_out('C', 7, true);
+ */
+void sunxi_set_gpio_out(char port, int pin, bool level_high)
+{
+	uintptr_t port_base;
+
+	if (port < 'A' || port > 'L')
+		return;
+	if (port == 'L')
+		port_base = SUNXI_R_PIO_BASE;
+	else
+		port_base = SUNXI_PIO_BASE + (port - 'A') * 0x24;
+
+	/* Set the new level first before configuring the pin. */
+	if (level_high)
+		mmio_setbits_32(port_base + 0x10, BIT(pin));
+	else
+		mmio_clrbits_32(port_base + 0x10, BIT(pin));
+
+	/* configure pin as GPIO out (4(3) bits per pin, 1: GPIO out */
+	mmio_clrsetbits_32(port_base + (pin / 8) * 4,
+			   0x7 << ((pin % 8) * 4),
+			   0x1 << ((pin % 8) * 4));
+}
+
+int sunxi_init_platform_r_twi(uint16_t socid, bool use_rsb)
+{
+	uint32_t pin_func = 0x77;
+	uint32_t device_bit;
+	unsigned int reset_offset = 0xb0;
+
+	switch (socid) {
+	case SUNXI_SOC_H5:
+		if (use_rsb)
+			return -ENODEV;
+		pin_func = 0x22;
+		device_bit = BIT(6);
+		break;
+	case SUNXI_SOC_H6:
+		if (use_rsb)
+			return -ENODEV;
+		pin_func = 0x33;
+		device_bit = BIT(16);
+		reset_offset = 0x19c;
+		break;
+	case SUNXI_SOC_A64:
+		pin_func = use_rsb ? 0x22 : 0x33;
+		device_bit = use_rsb ? BIT(3) : BIT(6);
+		break;
+	default:
+		INFO("R_I2C/RSB on Allwinner 0x%x SoC not supported\n", socid);
+		return -ENODEV;
+	}
+
+	/* un-gate R_PIO clock */
+	if (socid != SUNXI_SOC_H6)
+		mmio_setbits_32(SUNXI_R_PRCM_BASE + 0x28, BIT(0));
+
+	/* switch pins PL0 and PL1 to the desired function */
+	mmio_clrsetbits_32(SUNXI_R_PIO_BASE + 0x00, 0xffU, pin_func);
+
+	/* level 2 drive strength */
+	mmio_clrsetbits_32(SUNXI_R_PIO_BASE + 0x14, 0x0fU, 0xaU);
+
+	/* set both pins to pull-up */
+	mmio_clrsetbits_32(SUNXI_R_PIO_BASE + 0x1c, 0x0fU, 0x5U);
+
+	/* assert, then de-assert reset of I2C/RSB controller */
+	mmio_clrbits_32(SUNXI_R_PRCM_BASE + reset_offset, device_bit);
+	mmio_setbits_32(SUNXI_R_PRCM_BASE + reset_offset, device_bit);
+
+	/* un-gate clock */
+	if (socid != SUNXI_SOC_H6)
+		mmio_setbits_32(SUNXI_R_PRCM_BASE + 0x28, device_bit);
+	else
+		mmio_setbits_32(SUNXI_R_PRCM_BASE + 0x19c, device_bit | BIT(0));
+
+	return 0;
+}
+
+/* This lock synchronises access to the arisc management processor. */
+DEFINE_BAKERY_LOCK(arisc_lock);
+
+/*
+ * Tell the "arisc" SCP core (an OpenRISC core) to execute some code.
+ * We don't have any service running there, so we place some OpenRISC code
+ * in SRAM, put the address of that into the reset vector and release the
+ * arisc reset line. The SCP will execute that code and pull the line up again.
+ */
+void sunxi_execute_arisc_code(uint32_t *code, size_t size,
+			      int patch_offset, uint16_t param)
+{
+	uintptr_t arisc_reset_vec = SUNXI_SRAM_A2_BASE - 0x4000 + 0x100;
+
+	do {
+		bakery_lock_get(&arisc_lock);
+		/* Wait until the arisc is in reset state. */
+		if (!(mmio_read_32(SUNXI_R_CPUCFG_BASE) & BIT(0)))
+			break;
+
+		bakery_lock_release(&arisc_lock);
+	} while (1);
+
+	/* Patch up the code to feed in an input parameter. */
+	if (patch_offset >= 0 && patch_offset <= (size - 4))
+		code[patch_offset] = (code[patch_offset] & ~0xffff) | param;
+	clean_dcache_range((uintptr_t)code, size);
+
+	/*
+	 * The OpenRISC unconditional branch has opcode 0, the branch offset
+	 * is in the lower 26 bits, containing the distance to the target,
+	 * in instruction granularity (32 bits).
+	 */
+	mmio_write_32(arisc_reset_vec, ((uintptr_t)code - arisc_reset_vec) / 4);
+	clean_dcache_range(arisc_reset_vec, 4);
+
+	/* De-assert the arisc reset line to let it run. */
+	mmio_setbits_32(SUNXI_R_CPUCFG_BASE, BIT(0));
+
+	/*
+	 * We release the lock here, although the arisc is still busy.
+	 * But as long as it runs, the reset line is high, so other users
+	 * won't leave the loop above.
+	 * Once it has finished, the code is supposed to clear the reset line,
+	 * to signal this to other users.
+	 */
+	bakery_lock_release(&arisc_lock);
+}
diff --git a/plat/allwinner/common/sunxi_cpu_ops.c b/plat/allwinner/common/sunxi_cpu_ops.c
index aaee65c..3b732b5 100644
--- a/plat/allwinner/common/sunxi_cpu_ops.c
+++ b/plat/allwinner/common/sunxi_cpu_ops.c
@@ -4,15 +4,19 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <arch_helpers.h>
+#include <assert.h>
+#include <core_off_arisc.h>
 #include <debug.h>
+#include <delay_timer.h>
 #include <mmio.h>
+#include <platform.h>
 #include <platform_def.h>
-#include <sunxi_mmap.h>
 #include <sunxi_cpucfg.h>
+#include <sunxi_mmap.h>
+#include <sunxi_private.h>
 #include <utils_def.h>
 
-#include "sunxi_private.h"
-
 static void sunxi_cpu_disable_power(unsigned int cluster, unsigned int core)
 {
 	if (mmio_read_32(SUNXI_CPU_POWER_CLAMP_REG(cluster, core)) == 0xff)
@@ -40,16 +44,37 @@
 
 void sunxi_cpu_off(unsigned int cluster, unsigned int core)
 {
+	int corenr = cluster * PLATFORM_MAX_CPUS_PER_CLUSTER + core;
+
 	VERBOSE("PSCI: Powering off cluster %d core %d\n", cluster, core);
 
 	/* Deassert DBGPWRDUP */
 	mmio_clrbits_32(SUNXI_CPUCFG_DBG_REG0, BIT(core));
-	/* Activate the core output clamps */
-	mmio_setbits_32(SUNXI_POWEROFF_GATING_REG(cluster), BIT(core));
-	/* Assert CPU power-on reset */
-	mmio_clrbits_32(SUNXI_POWERON_RST_REG(cluster), BIT(core));
-	/* Remove power from the CPU */
-	sunxi_cpu_disable_power(cluster, core);
+
+	/* We can't turn ourself off like this, but it works for other cores. */
+	if (plat_my_core_pos() != corenr) {
+		/* Activate the core output clamps, but not for core 0. */
+		if (corenr != 0)
+			mmio_setbits_32(SUNXI_POWEROFF_GATING_REG(cluster),
+					BIT(core));
+		/* Assert CPU power-on reset */
+		mmio_clrbits_32(SUNXI_POWERON_RST_REG(cluster), BIT(core));
+		/* Remove power from the CPU */
+		sunxi_cpu_disable_power(cluster, core);
+
+		return;
+	}
+
+	/* Simplifies assembly, all SoCs so far are single cluster anyway. */
+	assert(cluster == 0);
+
+	/*
+	 * If we are supposed to turn ourself off, tell the arisc SCP
+	 * to do that work for us. The code expects the core mask to be
+	 * patched into the first instruction.
+	 */
+	sunxi_execute_arisc_code(arisc_core_off, sizeof(arisc_core_off),
+				 0, BIT_32(core));
 }
 
 void sunxi_cpu_on(unsigned int cluster, unsigned int core)
diff --git a/plat/allwinner/common/sunxi_pm.c b/plat/allwinner/common/sunxi_pm.c
index e4bb582..7d13cda 100644
--- a/plat/allwinner/common/sunxi_pm.c
+++ b/plat/allwinner/common/sunxi_pm.c
@@ -13,15 +13,14 @@
 #include <platform.h>
 #include <platform_def.h>
 #include <psci.h>
-#include <sunxi_mmap.h>
 #include <sunxi_cpucfg.h>
+#include <sunxi_mmap.h>
+#include <sunxi_private.h>
 
 #define SUNXI_WDOG0_CTRL_REG		(SUNXI_WDOG_BASE + 0x0010)
 #define SUNXI_WDOG0_CFG_REG		(SUNXI_WDOG_BASE + 0x0014)
 #define SUNXI_WDOG0_MODE_REG		(SUNXI_WDOG_BASE + 0x0018)
 
-#include "sunxi_private.h"
-
 #define mpidr_is_valid(mpidr) ( \
 	MPIDR_AFFLVL3_VAL(mpidr) == 0 && \
 	MPIDR_AFFLVL2_VAL(mpidr) == 0 && \
@@ -43,6 +42,16 @@
 	gicv2_cpuif_disable();
 }
 
+static void __dead2 sunxi_pwr_down_wfi(const psci_power_state_t *target_state)
+{
+	u_register_t mpidr = read_mpidr();
+
+	sunxi_cpu_off(MPIDR_AFFLVL1_VAL(mpidr), MPIDR_AFFLVL0_VAL(mpidr));
+
+	while (1)
+		wfi();
+}
+
 static void sunxi_pwr_domain_on_finish(const psci_power_state_t *target_state)
 {
 	gicv2_pcpu_distif_init();
@@ -83,6 +92,7 @@
 static plat_psci_ops_t sunxi_psci_ops = {
 	.pwr_domain_on			= sunxi_pwr_domain_on,
 	.pwr_domain_off			= sunxi_pwr_domain_off,
+	.pwr_domain_pwr_down_wfi	= sunxi_pwr_down_wfi,
 	.pwr_domain_on_finish		= sunxi_pwr_domain_on_finish,
 	.system_off			= sunxi_system_off,
 	.system_reset			= sunxi_system_reset,
diff --git a/plat/allwinner/common/sunxi_private.h b/plat/allwinner/common/sunxi_private.h
deleted file mode 100644
index 20fa23e..0000000
--- a/plat/allwinner/common/sunxi_private.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/*
- * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef __SUNXI_PRIVATE_H__
-#define __SUNXI_PRIVATE_H__
-
-void sunxi_configure_mmu_el3(int flags);
-void sunxi_cpu_off(unsigned int cluster, unsigned int core);
-void sunxi_cpu_on(unsigned int cluster, unsigned int core);
-void sunxi_disable_secondary_cpus(unsigned int primary_cpu);
-
-uint16_t sunxi_read_soc_id(void);
-
-void sunxi_pmic_setup(void);
-void sunxi_security_setup(void);
-
-void __dead2 sunxi_power_down(void);
-
-#endif /* __SUNXI_PRIVATE_H__ */
diff --git a/plat/allwinner/common/sunxi_security.c b/plat/allwinner/common/sunxi_security.c
index 80fed6a..9053728 100644
--- a/plat/allwinner/common/sunxi_security.c
+++ b/plat/allwinner/common/sunxi_security.c
@@ -7,6 +7,7 @@
 #include <debug.h>
 #include <mmio.h>
 #include <sunxi_mmap.h>
+#include <sunxi_private.h>
 
 #ifdef SUNXI_SPC_BASE
 #define SPC_DECPORT_STA_REG(p)	(SUNXI_SPC_BASE + ((p) * 0x0c) + 0x4)
diff --git a/plat/allwinner/sun50i_a64/include/core_off_arisc.h b/plat/allwinner/sun50i_a64/include/core_off_arisc.h
new file mode 100644
index 0000000..ae436ca
--- /dev/null
+++ b/plat/allwinner/sun50i_a64/include/core_off_arisc.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint32_t arisc_core_off[] = {
+	0x18600000, /* l.movhi	r3, <corenr>	*/
+	0x18000000, /* l.movhi	r0, 0x0		*/
+	0x19a00170, /* l.movhi	r13, 0x170	*/
+	0x84ad0030, /* l.lwz	r5, 0x30(r13)	*/
+	0xe0a51803, /* l.and	r5, r5, r3	*/
+	0xe4050000, /* l.sfeq	r5, r0		*/
+	0x13fffffd, /* l.bf	-12		*/
+
+	0xb8c30050, /* l.srli	r6, r3, 16	*/
+	0xbc060001, /* l.sfeqi	r6, 1		*/
+	0x10000005, /* l.bf	+20		*/
+	0x19a001f0, /* l.movhi	r13, 0x1f0	*/
+	0x84ad1500, /* l.lwz	r5, 0x1500(r13)	*/
+	0xe0a53004, /* l.or	r5, r5, r6	*/
+	0xd44d2d00, /* l.sw	0x1500(r13), r5	*/
+
+	0x84ad1c30, /* l.lwz	r5, 0x1c30(r13)	*/
+	0xacc6ffff, /* l.xori	r6, r6, -1	*/
+	0xe0a53003, /* l.and	r5, r5, r6	*/
+	0xd46d2c30, /* l.sw	0x1c30(r13), r5	*/
+
+	0xe0c3000f, /* l.ff1	r6, r3		*/
+	0x9cc6ffef, /* l.addi	r6, r6, -17	*/
+	0xb8c60002, /* l.slli	r6, r6, 2	*/
+	0xe0c66800, /* l.add	r6, r6, r13	*/
+	0xa8a000ff, /* l.ori	r5, r0, 0xff	*/
+	0xd4462d40, /* l.sw	0x1540(r6), r5	*/
+
+	0xd46d0400, /* l.sw	0x1c00(r13), r0	*/
+	0x03ffffff, /* l.j	-1		*/
+	0x15000000, /* l.nop			*/
+};
diff --git a/plat/allwinner/sun50i_a64/include/sunxi_mmap.h b/plat/allwinner/sun50i_a64/include/sunxi_mmap.h
index 7d46487..28b1dd3 100644
--- a/plat/allwinner/sun50i_a64/include/sunxi_mmap.h
+++ b/plat/allwinner/sun50i_a64/include/sunxi_mmap.h
@@ -21,7 +21,7 @@
 #define SUNXI_DEV_BASE			0x01000000
 #define SUNXI_DEV_SIZE			0x01000000
 #define SUNXI_DRAM_BASE			0x40000000
-#define SUNXI_DRAM_SIZE			0x80000000
+#define SUNXI_DRAM_VIRT_BASE		0x02000000
 
 /* Memory-mapped devices */
 #define SUNXI_CPU_MBIST_BASE		0x01502000
diff --git a/plat/allwinner/sun50i_a64/platform.mk b/plat/allwinner/sun50i_a64/platform.mk
index 2216654..b46fbc2 100644
--- a/plat/allwinner/sun50i_a64/platform.mk
+++ b/plat/allwinner/sun50i_a64/platform.mk
@@ -4,51 +4,7 @@
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
-include lib/xlat_tables_v2/xlat_tables.mk
+# The differences between the platform are covered by the include files.
+include plat/allwinner/common/allwinner-common.mk
 
-AW_PLAT			:=	plat/allwinner
-
-PLAT_INCLUDES		:=	-Iinclude/plat/arm/common/		\
-				-Iinclude/plat/arm/common/aarch64	\
-				-I${AW_PLAT}/common/include		\
-				-I${AW_PLAT}/${PLAT}/include
-
-PLAT_BL_COMMON_SOURCES	:=	drivers/console/${ARCH}/console.S	\
-				drivers/ti/uart/${ARCH}/16550_console.S	\
-				${XLAT_TABLES_LIB_SRCS}			\
-				${AW_PLAT}/common/plat_helpers.S	\
-				${AW_PLAT}/common/sunxi_common.c
-
-BL31_SOURCES		+=	drivers/arm/gic/common/gic_common.c	\
-				drivers/arm/gic/v2/gicv2_helpers.c	\
-				drivers/arm/gic/v2/gicv2_main.c		\
-				drivers/delay_timer/delay_timer.c	\
-				drivers/delay_timer/generic_delay_timer.c \
-				lib/cpus/${ARCH}/cortex_a53.S		\
-				plat/common/plat_gicv2.c		\
-				plat/common/plat_psci_common.c		\
-				${AW_PLAT}/common/sunxi_bl31_setup.c	\
-				${AW_PLAT}/common/sunxi_cpu_ops.c	\
-				${AW_PLAT}/common/sunxi_pm.c		\
-				${AW_PLAT}/sun50i_a64/sunxi_power.c	\
-				${AW_PLAT}/common/sunxi_security.c	\
-				${AW_PLAT}/common/sunxi_topology.c
-
-# The bootloader is guaranteed to only run on CPU 0 by the boot ROM.
-COLD_BOOT_SINGLE_CPU		:=	1
-
-# Enable workarounds for Cortex-A53 errata. Allwinner uses at least r0p4.
-ERRATA_A53_835769		:=	1
-ERRATA_A53_843419		:=	1
-ERRATA_A53_855873		:=	1
-
-MULTI_CONSOLE_API		:=	1
-
-# The reset vector can be changed for each CPU.
-PROGRAMMABLE_RESET_ADDRESS	:=	1
-
-# Allow mapping read-only data as execute-never.
-SEPARATE_CODE_AND_RODATA	:=	1
-
-# BL31 gets loaded alongside BL33 (U-Boot) by U-Boot's SPL
-RESET_TO_BL31			:=	1
+PLAT_BL_COMMON_SOURCES	+=	drivers/allwinner/sunxi_rsb.c
diff --git a/plat/allwinner/sun50i_a64/sunxi_power.c b/plat/allwinner/sun50i_a64/sunxi_power.c
index c1907d6..af30477 100644
--- a/plat/allwinner/sun50i_a64/sunxi_power.c
+++ b/plat/allwinner/sun50i_a64/sunxi_power.c
@@ -5,20 +5,350 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <allwinner/sunxi_rsb.h>
 #include <arch_helpers.h>
 #include <debug.h>
+#include <delay_timer.h>
+#include <errno.h>
+#include <libfdt.h>
+#include <mmio.h>
+#include <platform_def.h>
+#include <sunxi_def.h>
+#include <sunxi_mmap.h>
+#include <sunxi_private.h>
 
-int sunxi_pmic_setup(void)
+static enum pmic_type {
+	GENERIC_H5,
+	GENERIC_A64,
+	REF_DESIGN_H5,	/* regulators controlled by GPIO pins on port L */
+	AXP803_RSB,	/* PMIC connected via RSB on most A64 boards */
+} pmic;
+
+#define AXP803_HW_ADDR	0x3a3
+#define AXP803_RT_ADDR	0x2d
+
+/*
+ * On boards without a proper PMIC we struggle to turn off the system properly.
+ * Try to turn off as much off the system as we can, to reduce power
+ * consumption. This should be entered with only one core running and SMP
+ * disabled.
+ * This function only cares about peripherals.
+ */
+void sunxi_turn_off_soc(uint16_t socid)
 {
-	/* STUB */
-	NOTICE("BL31: STUB PMIC setup code called\n");
+	int i;
 
+	/** Turn off most peripherals, most importantly DRAM users. **/
+	/* Keep DRAM controller running for now. */
+	mmio_clrbits_32(SUNXI_CCU_BASE + 0x2c0, ~BIT_32(14));
+	mmio_clrbits_32(SUNXI_CCU_BASE + 0x60, ~BIT_32(14));
+	/* Contains msgbox (bit 21) and spinlock (bit 22) */
+	mmio_write_32(SUNXI_CCU_BASE + 0x2c4, 0);
+	mmio_write_32(SUNXI_CCU_BASE + 0x64, 0);
+	mmio_write_32(SUNXI_CCU_BASE + 0x2c8, 0);
+	/* Keep PIO controller running for now. */
+	mmio_clrbits_32(SUNXI_CCU_BASE + 0x68, ~(BIT_32(5)));
+	mmio_write_32(SUNXI_CCU_BASE + 0x2d0, 0);
+	/* Contains UART0 (bit 16) */
+	mmio_write_32(SUNXI_CCU_BASE + 0x2d8, 0);
+	mmio_write_32(SUNXI_CCU_BASE + 0x6c, 0);
+	mmio_write_32(SUNXI_CCU_BASE + 0x70, 0);
+
+	/** Turn off DRAM controller. **/
+	mmio_clrbits_32(SUNXI_CCU_BASE + 0x2c0, BIT_32(14));
+	mmio_clrbits_32(SUNXI_CCU_BASE + 0x60, BIT_32(14));
+
+	/** Migrate CPU and bus clocks away from the PLLs. **/
+	/* AHB1: use OSC24M/1, APB1 = AHB1 / 2 */
+	mmio_write_32(SUNXI_CCU_BASE + 0x54, 0x1000);
+	/* APB2: use OSC24M */
+	mmio_write_32(SUNXI_CCU_BASE + 0x58, 0x1000000);
+	/* AHB2: use AHB1 clock */
+	mmio_write_32(SUNXI_CCU_BASE + 0x5c, 0);
+	/* CPU: use OSC24M */
+	mmio_write_32(SUNXI_CCU_BASE + 0x50, 0x10000);
+
+	/** Turn off PLLs. **/
+	for (i = 0; i < 6; i++)
+		mmio_clrbits_32(SUNXI_CCU_BASE + i * 8, BIT(31));
+	switch (socid) {
+	case SUNXI_SOC_H5:
+		mmio_clrbits_32(SUNXI_CCU_BASE + 0x44, BIT(31));
+		break;
+	case SUNXI_SOC_A64:
+		mmio_clrbits_32(SUNXI_CCU_BASE + 0x2c, BIT(31));
+		mmio_clrbits_32(SUNXI_CCU_BASE + 0x4c, BIT(31));
+		break;
+	}
+}
+
+static int rsb_init(void)
+{
+	int ret;
+
+	ret = rsb_init_controller();
+	if (ret)
+		return ret;
+
+	/* Start with 400 KHz to issue the I2C->RSB switch command. */
+	ret = rsb_set_bus_speed(SUNXI_OSC24M_CLK_IN_HZ, 400000);
+	if (ret)
+		return ret;
+
+	/*
+	 * Initiate an I2C transaction to write 0x7c into register 0x3e,
+	 * switching the PMIC to RSB mode.
+	 */
+	ret = rsb_set_device_mode(0x7c3e00);
+	if (ret)
+		return ret;
+
+	/* Now in RSB mode, switch to the recommended 3 MHz. */
+	ret = rsb_set_bus_speed(SUNXI_OSC24M_CLK_IN_HZ, 3000000);
+	if (ret)
+		return ret;
+
+	/* Associate the 8-bit runtime address with the 12-bit bus address. */
+	return rsb_assign_runtime_address(AXP803_HW_ADDR,
+					  AXP803_RT_ADDR);
+}
+
+static int axp_write(uint8_t reg, uint8_t val)
+{
+	return rsb_write(AXP803_RT_ADDR, reg, val);
+}
+
+static int axp_setbits(uint8_t reg, uint8_t set_mask)
+{
+	uint8_t regval;
+	int ret;
+
+	ret = rsb_read(AXP803_RT_ADDR, reg);
+	if (ret < 0)
+		return ret;
+
+	regval = ret | set_mask;
+
+	return rsb_write(AXP803_RT_ADDR, reg, regval);
+}
+
+static bool should_enable_regulator(const void *fdt, int node)
+{
+	if (fdt_getprop(fdt, node, "phandle", NULL) != NULL)
+		return true;
+	if (fdt_getprop(fdt, node, "regulator-always-on", NULL) != NULL)
+		return true;
+	return false;
+}
+
+/*
+ * 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;
+}
+
+#define NO_SPLIT 0xff
+
+struct axp_regulator {
+	char *dt_name;
+	uint16_t min_volt;
+	uint16_t max_volt;
+	uint16_t step;
+	unsigned char split;
+	unsigned char volt_reg;
+	unsigned char switch_reg;
+	unsigned char switch_bit;
+} regulators[] = {
+	{"dcdc1", 1600, 3400, 100, NO_SPLIT, 0x20, 0xff, 9},
+	{"dcdc5",  800, 1840,  10,       32, 0x24, 0xff, 9},
+	{"dldo1",  700, 3300, 100, NO_SPLIT, 0x15, 0x12, 3},
+	{"dldo2",  700, 4200, 100,       27, 0x16, 0x12, 4},
+	{"dldo3",  700, 3300, 100, NO_SPLIT, 0x17, 0x12, 5},
+	{"fldo1",  700, 1450,  50, NO_SPLIT, 0x1c, 0x13, 2},
+	{}
+};
+
+static int setup_regulator(const void *fdt, int node,
+			   const struct axp_regulator *reg)
+{
+	int mvolt;
+	uint8_t regval;
+
+	if (!should_enable_regulator(fdt, node))
+		return -ENOENT;
+
+	mvolt = fdt_get_regulator_millivolt(fdt, node);
+	if (mvolt < reg->min_volt || mvolt > reg->max_volt)
+		return -EINVAL;
+
+	regval = (mvolt / reg->step) - (reg->min_volt / reg->step);
+	if (regval > reg->split)
+		regval = ((regval - reg->split) / 2) + reg->split;
+
+	axp_write(reg->volt_reg, regval);
+	if (reg->switch_reg < 0xff)
+		axp_setbits(reg->switch_reg, BIT(reg->switch_bit));
+
+	INFO("PMIC: AXP803: %s voltage: %d.%03dV\n", reg->dt_name,
+	     mvolt / 1000, mvolt % 1000);
+
+	return 0;
+}
+
+static void setup_axp803_rails(const void *fdt)
+{
+	int node;
+	bool dc1sw = false;
+
+	/* locate the PMIC DT node, bail out if not found */
+	node = fdt_node_offset_by_compatible(fdt, -1, "x-powers,axp803");
+	if (node == -FDT_ERR_NOTFOUND) {
+		WARN("BL31: PMIC: No AXP803 DT node, skipping initial setup.\n");
+		return;
+	}
+
+	if (fdt_getprop(fdt, node, "x-powers,drive-vbus-en", NULL))
+		axp_setbits(0x8f, BIT(4));
+
+	/* descend into the "regulators" subnode */
+	node = fdt_first_subnode(fdt, node);
+
+	/* iterate over all regulators to find used ones */
+	for (node = fdt_first_subnode(fdt, node);
+	     node != -FDT_ERR_NOTFOUND;
+	     node = fdt_next_subnode(fdt, node)) {
+		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);
+		for (reg = regulators; reg->dt_name; reg++) {
+			if (!strncmp(name, reg->dt_name, length)) {
+				setup_regulator(fdt, node, reg);
+				break;
+			}
+		}
+
+		if (!strncmp(name, "dc1sw", length)) {
+			/* Delay DC1SW enablement to avoid overheating. */
+			dc1sw = true;
+			continue;
+		}
+	}
+	/*
+	 * If DLDO2 is enabled after DC1SW, the PMIC overheats and shuts
+	 * down. So always enable DC1SW as the very last regulator.
+	 */
+	if (dc1sw) {
+		INFO("PMIC: AXP803: Enabling DC1SW\n");
+		axp_setbits(0x12, BIT(7));
+	}
+}
+
+int sunxi_pmic_setup(uint16_t socid, const void *fdt)
+{
+	int ret;
+
+	switch (socid) {
+	case SUNXI_SOC_H5:
+		pmic = REF_DESIGN_H5;
+		NOTICE("BL31: PMIC: Defaulting to PortL GPIO according to H5 reference design.\n");
+		break;
+	case SUNXI_SOC_A64:
+		pmic = GENERIC_A64;
+		ret = sunxi_init_platform_r_twi(socid, true);
+		if (ret)
+			return ret;
+
+		ret = rsb_init();
+		if (ret)
+			return ret;
+
+		pmic = AXP803_RSB;
+		NOTICE("BL31: PMIC: Detected AXP803 on RSB.\n");
+
+		if (fdt)
+			setup_axp803_rails(fdt);
+
+		break;
+	default:
+		NOTICE("BL31: PMIC: No support for Allwinner %x SoC.\n", socid);
+		return -ENODEV;
+	}
 	return 0;
 }
 
 void __dead2 sunxi_power_down(void)
 {
-	ERROR("PSCI: Full shutdown not implemented, halting\n");
+	switch (pmic) {
+	case GENERIC_H5:
+		/* Turn off as many peripherals and clocks as we can. */
+		sunxi_turn_off_soc(SUNXI_SOC_H5);
+		/* Turn off the pin controller now. */
+		mmio_write_32(SUNXI_CCU_BASE + 0x68, 0);
+		break;
+	case GENERIC_A64:
+		/* Turn off as many peripherals and clocks as we can. */
+		sunxi_turn_off_soc(SUNXI_SOC_A64);
+		/* Turn off the pin controller now. */
+		mmio_write_32(SUNXI_CCU_BASE + 0x68, 0);
+		break;
+	case REF_DESIGN_H5:
+		sunxi_turn_off_soc(SUNXI_SOC_H5);
+
+		/*
+		 * Switch PL pins to power off the board:
+		 * - PL5 (VCC_IO) -> high
+		 * - PL8 (PWR-STB = CPU power supply) -> low
+		 * - PL9 (PWR-DRAM) ->low
+		 * - PL10 (power LED) -> low
+		 * Note: Clearing PL8 will reset the board, so keep it up.
+		 */
+		sunxi_set_gpio_out('L', 5, 1);
+		sunxi_set_gpio_out('L', 9, 0);
+		sunxi_set_gpio_out('L', 10, 0);
+
+		/* Turn off pin controller now. */
+		mmio_write_32(SUNXI_CCU_BASE + 0x68, 0);
+
+		break;
+	case AXP803_RSB:
+		/* (Re-)init RSB in case the rich OS has disabled it. */
+		sunxi_init_platform_r_twi(SUNXI_SOC_A64, true);
+		rsb_init();
+
+		/* Set "power disable control" bit */
+		axp_setbits(0x32, BIT(7));
+		break;
+	default:
+		break;
+	}
+
+	udelay(1000);
+	ERROR("PSCI: Cannot turn off system, halting.\n");
 	wfi();
 	panic();
 }
diff --git a/plat/allwinner/sun50i_h6/include/core_off_arisc.h b/plat/allwinner/sun50i_h6/include/core_off_arisc.h
new file mode 100644
index 0000000..63a5d8d
--- /dev/null
+++ b/plat/allwinner/sun50i_h6/include/core_off_arisc.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+static uint32_t arisc_core_off[] = {
+	0x18600000, /* l.movhi	r3, <corenr>	*/
+	0x18000000, /* l.movhi	r0, 0x0		*/
+	0x19a00901, /* l.movhi	r13, 0x901	*/
+	0x84ad0080, /* l.lwz	r5, 0x80(r13)	*/
+	0xe0a51803, /* l.and	r5, r5, r3	*/
+	0xe4050000, /* l.sfeq	r5, r0		*/
+	0x13fffffd, /* l.bf	-12		*/
+	0xb8c30050, /* l.srli	r6, r3, 16	*/
+
+	0xbc060001, /* l.sfeqi	r6, 1		*/
+	0x10000005, /* l.bf	+20		*/
+	0x19a00700, /* l.movhi	r13, 0x700	*/
+	0x84ad0444, /* l.lwz	r5, 0x0444(r13)	*/
+	0xe0a53004, /* l.or	r5, r5, r6	*/
+	0xd40d2c44, /* l.sw	0x0444(r13), r5	*/
+
+	0x84ad0440, /* l.lwz	r5, 0x0440(r13)	*/
+	0xacc6ffff, /* l.xori	r6, r6, -1	*/
+	0xe0a53003, /* l.and	r5, r5, r6	*/
+	0xd40d2c40, /* l.sw	0x0440(r13), r5	*/
+
+	0xe0c3000f, /* l.ff1	r6, r3		*/
+	0x9cc6ffef, /* l.addi	r6, r6, -17	*/
+	0xb8c60002, /* l.slli	r6, r6, 2	*/
+	0xe0c66800, /* l.add	r6, r6, r13	*/
+	0xa8a000ff, /* l.ori	r5, r0, 0xff	*/
+	0xd4062c50, /* l.sw	0x0450(r6), r5	*/
+
+	0xd40d0400, /* l.sw	0x0400(r13), r0	*/
+	0x03ffffff, /* l.j	-1		*/
+	0x15000000, /* l.nop			*/
+};
diff --git a/plat/allwinner/sun50i_h6/include/sunxi_mmap.h b/plat/allwinner/sun50i_h6/include/sunxi_mmap.h
index f2d5aed..ff1eb61 100644
--- a/plat/allwinner/sun50i_h6/include/sunxi_mmap.h
+++ b/plat/allwinner/sun50i_h6/include/sunxi_mmap.h
@@ -11,7 +11,7 @@
 #define SUNXI_ROM_BASE			0x00000000
 #define SUNXI_ROM_SIZE			0x00010000
 #define SUNXI_SRAM_BASE			0x00020000
-#define SUNXI_SRAM_SIZE			0x00098000
+#define SUNXI_SRAM_SIZE			0x000f8000
 #define SUNXI_SRAM_A1_BASE		0x00020000
 #define SUNXI_SRAM_A1_SIZE		0x00008000
 #define SUNXI_SRAM_A2_BASE		0x00104000
@@ -21,7 +21,7 @@
 #define SUNXI_DEV_BASE			0x01000000
 #define SUNXI_DEV_SIZE			0x09000000
 #define SUNXI_DRAM_BASE			0x40000000
-#define SUNXI_DRAM_SIZE			0xc0000000
+#define SUNXI_DRAM_VIRT_BASE		0x0a000000
 
 /* Memory-mapped devices */
 #define SUNXI_SYSCON_BASE		0x03000000
diff --git a/plat/allwinner/sun50i_h6/platform.mk b/plat/allwinner/sun50i_h6/platform.mk
index 4fb8986..5c21ead 100644
--- a/plat/allwinner/sun50i_h6/platform.mk
+++ b/plat/allwinner/sun50i_h6/platform.mk
@@ -4,53 +4,7 @@
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
-include lib/xlat_tables_v2/xlat_tables.mk
+# The differences between the platform are covered by the include files.
+include plat/allwinner/common/allwinner-common.mk
 
-AW_PLAT			:=	plat/allwinner
-AW_DRIVERS		:=	drivers/allwinner
-
-PLAT_INCLUDES		:=	-Iinclude/plat/arm/common		\
-				-Iinclude/plat/arm/common/aarch64	\
-				-I${AW_PLAT}/common/include		\
-				-I${AW_PLAT}/${PLAT}/include
-
-PLAT_BL_COMMON_SOURCES	:=	drivers/console/${ARCH}/console.S	\
-				drivers/mentor/i2c/mi2cv.c		\
-				drivers/ti/uart/${ARCH}/16550_console.S	\
-				${XLAT_TABLES_LIB_SRCS}			\
-				${AW_PLAT}/common/plat_helpers.S	\
-				${AW_PLAT}/common/sunxi_common.c
-
-BL31_SOURCES		+=	drivers/arm/gic/common/gic_common.c	\
-				drivers/arm/gic/v2/gicv2_helpers.c	\
-				drivers/arm/gic/v2/gicv2_main.c		\
-				drivers/delay_timer/delay_timer.c	\
-				drivers/delay_timer/generic_delay_timer.c \
-				lib/cpus/${ARCH}/cortex_a53.S		\
-				plat/common/plat_gicv2.c		\
-				plat/common/plat_psci_common.c		\
-				${AW_PLAT}/common/sunxi_bl31_setup.c	\
-				${AW_PLAT}/common/sunxi_cpu_ops.c	\
-				${AW_PLAT}/common/sunxi_pm.c		\
-				${AW_PLAT}/sun50i_h6/sunxi_power.c	\
-				${AW_PLAT}/common/sunxi_security.c	\
-				${AW_PLAT}/common/sunxi_topology.c
-
-# The bootloader is guaranteed to only run on CPU 0 by the boot ROM.
-COLD_BOOT_SINGLE_CPU		:=	1
-
-# Enable workarounds for Cortex-A53 errata. Allwinner uses at least r0p4.
-ERRATA_A53_835769		:=	1
-ERRATA_A53_843419		:=	1
-ERRATA_A53_855873		:=	1
-
-MULTI_CONSOLE_API		:=	1
-
-# The reset vector can be changed for each CPU.
-PROGRAMMABLE_RESET_ADDRESS	:=	1
-
-# Allow mapping read-only data as execute-never.
-SEPARATE_CODE_AND_RODATA	:=	1
-
-# BL31 gets loaded alongside BL33 (U-Boot) by U-Boot's SPL
-RESET_TO_BL31			:=	1
+PLAT_BL_COMMON_SOURCES	+=	drivers/mentor/i2c/mi2cv.c
diff --git a/plat/allwinner/sun50i_h6/sunxi_power.c b/plat/allwinner/sun50i_h6/sunxi_power.c
index 12438b3..7bdac8a 100644
--- a/plat/allwinner/sun50i_h6/sunxi_power.c
+++ b/plat/allwinner/sun50i_h6/sunxi_power.c
@@ -12,7 +12,9 @@
 #include <mmio.h>
 #include <mentor/mi2cv.h>
 #include <string.h>
+#include <sunxi_def.h>
 #include <sunxi_mmap.h>
+#include <sunxi_private.h>
 
 #define AXP805_ADDR	0x36
 #define AXP805_ID	0x03
@@ -24,36 +26,6 @@
 
 enum pmic_type pmic;
 
-static int sunxi_init_r_i2c(void)
-{
-	uint32_t reg;
-
-	/* switch pins PL0 and PL1 to I2C */
-	reg = mmio_read_32(SUNXI_R_PIO_BASE + 0x00);
-	mmio_write_32(SUNXI_R_PIO_BASE + 0x00, (reg & ~0xff) | 0x33);
-
-	/* level 2 drive strength */
-	reg = mmio_read_32(SUNXI_R_PIO_BASE + 0x14);
-	mmio_write_32(SUNXI_R_PIO_BASE + 0x14, (reg & ~0x0f) | 0xa);
-
-	/* set both ports to pull-up */
-	reg = mmio_read_32(SUNXI_R_PIO_BASE + 0x1c);
-	mmio_write_32(SUNXI_R_PIO_BASE + 0x1c, (reg & ~0x0f) | 0x5);
-
-	/* assert & de-assert reset of R_I2C */
-	reg = mmio_read_32(SUNXI_R_PRCM_BASE + 0x19c);
-	mmio_write_32(SUNXI_R_PRCM_BASE + 0x19c, reg & ~BIT(16));
-	mmio_write_32(SUNXI_R_PRCM_BASE + 0x19c, reg | BIT(16));
-
-	/* un-gate R_I2C clock */
-	mmio_write_32(SUNXI_R_PRCM_BASE + 0x19c, reg | BIT(16) | BIT(0));
-
-	/* call mi2cv driver */
-	i2c_init((void *)SUNXI_R_I2C_BASE);
-
-	return 0;
-}
-
 int axp_i2c_read(uint8_t chip, uint8_t reg, uint8_t *val)
 {
 	int ret;
@@ -96,11 +68,13 @@
 	return 0;
 }
 
-int sunxi_pmic_setup(void)
+int sunxi_pmic_setup(uint16_t socid, const void *fdt)
 {
 	int ret;
 
-	sunxi_init_r_i2c();
+	sunxi_init_platform_r_twi(SUNXI_SOC_H6, false);
+	/* initialise mi2cv driver */
+	i2c_init((void *)SUNXI_R_I2C_BASE);
 
 	NOTICE("PMIC: Probing AXP805\n");
 	pmic = AXP805;
@@ -120,7 +94,10 @@
 
 	switch (pmic) {
 	case AXP805:
-		sunxi_init_r_i2c();
+		/* Re-initialise after rich OS might have used it. */
+		sunxi_init_platform_r_twi(SUNXI_SOC_H6, false);
+		/* initialise mi2cv driver */
+		i2c_init((void *)SUNXI_R_I2C_BASE);
 		axp_i2c_read(AXP805_ADDR, 0x32, &val);
 		axp_i2c_write(AXP805_ADDR, 0x32, val | 0x80);
 		break;
diff --git a/plat/arm/board/fvp/fvp_def.h b/plat/arm/board/fvp/fvp_def.h
index eb6f77f..a8ed9d3 100644
--- a/plat/arm/board/fvp/fvp_def.h
+++ b/plat/arm/board/fvp/fvp_def.h
@@ -31,70 +31,70 @@
  * FVP memory map related constants
  ******************************************************************************/
 
-#define FLASH1_BASE			0x0c000000
-#define FLASH1_SIZE			0x04000000
+#define FLASH1_BASE			UL(0x0c000000)
+#define FLASH1_SIZE			UL(0x04000000)
 
-#define PSRAM_BASE			0x14000000
-#define PSRAM_SIZE			0x04000000
+#define PSRAM_BASE			UL(0x14000000)
+#define PSRAM_SIZE			UL(0x04000000)
 
-#define VRAM_BASE			0x18000000
-#define VRAM_SIZE			0x02000000
+#define VRAM_BASE			UL(0x18000000)
+#define VRAM_SIZE			UL(0x02000000)
 
 /* Aggregate of all devices in the first GB */
-#define DEVICE0_BASE			0x20000000
-#define DEVICE0_SIZE			0x0c200000
+#define DEVICE0_BASE			UL(0x20000000)
+#define DEVICE0_SIZE			UL(0x0c200000)
 
 /*
  *  In case of FVP models with CCN, the CCN register space overlaps into
  *  the NSRAM area.
  */
 #if FVP_INTERCONNECT_DRIVER == FVP_CCN
-#define DEVICE1_BASE			0x2e000000
-#define DEVICE1_SIZE			0x1A00000
+#define DEVICE1_BASE			UL(0x2e000000)
+#define DEVICE1_SIZE			UL(0x1A00000)
 #else
-#define DEVICE1_BASE			0x2f000000
-#define DEVICE1_SIZE			0x200000
-#define NSRAM_BASE			0x2e000000
-#define NSRAM_SIZE			0x10000
+#define DEVICE1_BASE			UL(0x2f000000)
+#define DEVICE1_SIZE			UL(0x200000)
+#define NSRAM_BASE			UL(0x2e000000)
+#define NSRAM_SIZE			UL(0x10000)
 #endif
 /* Devices in the second GB */
-#define DEVICE2_BASE			0x7fe00000
-#define DEVICE2_SIZE			0x00200000
+#define DEVICE2_BASE			UL(0x7fe00000)
+#define DEVICE2_SIZE			UL(0x00200000)
 
-#define PCIE_EXP_BASE			0x40000000
-#define TZRNG_BASE			0x7fe60000
+#define PCIE_EXP_BASE			UL(0x40000000)
+#define TZRNG_BASE			UL(0x7fe60000)
 
 /* Non-volatile counters */
-#define TRUSTED_NVCTR_BASE		0x7fe70000
-#define TFW_NVCTR_BASE			(TRUSTED_NVCTR_BASE + 0x0000)
-#define TFW_NVCTR_SIZE			4
-#define NTFW_CTR_BASE			(TRUSTED_NVCTR_BASE + 0x0004)
-#define NTFW_CTR_SIZE			4
+#define TRUSTED_NVCTR_BASE		UL(0x7fe70000)
+#define TFW_NVCTR_BASE			(TRUSTED_NVCTR_BASE + UL(0x0000))
+#define TFW_NVCTR_SIZE			UL(4)
+#define NTFW_CTR_BASE			(TRUSTED_NVCTR_BASE + UL(0x0004))
+#define NTFW_CTR_SIZE			UL(4)
 
 /* Keys */
-#define SOC_KEYS_BASE			0x7fe80000
-#define TZ_PUB_KEY_HASH_BASE		(SOC_KEYS_BASE + 0x0000)
-#define TZ_PUB_KEY_HASH_SIZE		32
-#define HU_KEY_BASE			(SOC_KEYS_BASE + 0x0020)
-#define HU_KEY_SIZE			16
-#define END_KEY_BASE			(SOC_KEYS_BASE + 0x0044)
-#define END_KEY_SIZE			32
+#define SOC_KEYS_BASE			UL(0x7fe80000)
+#define TZ_PUB_KEY_HASH_BASE		(SOC_KEYS_BASE + UL(0x0000))
+#define TZ_PUB_KEY_HASH_SIZE		UL(32)
+#define HU_KEY_BASE			(SOC_KEYS_BASE + UL(0x0020))
+#define HU_KEY_SIZE			UL(16)
+#define END_KEY_BASE			(SOC_KEYS_BASE + UL(0x0044))
+#define END_KEY_SIZE			UL(32)
 
 /* Constants to distinguish FVP type */
-#define HBI_BASE_FVP			0x020
-#define REV_BASE_FVP_V0			0x0
-#define REV_BASE_FVP_REVC		0x2
+#define HBI_BASE_FVP			U(0x020)
+#define REV_BASE_FVP_V0			U(0x0)
+#define REV_BASE_FVP_REVC		U(0x2)
 
-#define HBI_FOUNDATION_FVP		0x010
-#define REV_FOUNDATION_FVP_V2_0		0x0
-#define REV_FOUNDATION_FVP_V2_1		0x1
-#define REV_FOUNDATION_FVP_v9_1		0x2
-#define REV_FOUNDATION_FVP_v9_6		0x3
+#define HBI_FOUNDATION_FVP		U(0x010)
+#define REV_FOUNDATION_FVP_V2_0		U(0x0)
+#define REV_FOUNDATION_FVP_V2_1		U(0x1)
+#define REV_FOUNDATION_FVP_v9_1		U(0x2)
+#define REV_FOUNDATION_FVP_v9_6		U(0x3)
 
-#define BLD_GIC_VE_MMAP			0x0
-#define BLD_GIC_A53A57_MMAP		0x1
+#define BLD_GIC_VE_MMAP			U(0x0)
+#define BLD_GIC_A53A57_MMAP		U(0x1)
 
-#define ARCH_MODEL			0x1
+#define ARCH_MODEL			U(0x1)
 
 /* FVP Power controller base address*/
 #define PWRC_BASE			UL(0x1c100000)
@@ -104,26 +104,26 @@
 #define SP804_TIMER_CLKDIV		35
 
 /* SP810 controller. FVP specific flags */
-#define FVP_SP810_CTRL_TIM0_OV		(1 << 16)
-#define FVP_SP810_CTRL_TIM1_OV		(1 << 18)
-#define FVP_SP810_CTRL_TIM2_OV		(1 << 20)
-#define FVP_SP810_CTRL_TIM3_OV		(1 << 22)
+#define FVP_SP810_CTRL_TIM0_OV		BIT_32(16)
+#define FVP_SP810_CTRL_TIM1_OV		BIT_32(18)
+#define FVP_SP810_CTRL_TIM2_OV		BIT_32(20)
+#define FVP_SP810_CTRL_TIM3_OV		BIT_32(22)
 
 /*******************************************************************************
  * GIC-400 & interrupt handling related constants
  ******************************************************************************/
 /* VE compatible GIC memory map */
-#define VE_GICD_BASE			0x2c001000
-#define VE_GICC_BASE			0x2c002000
-#define VE_GICH_BASE			0x2c004000
-#define VE_GICV_BASE			0x2c006000
+#define VE_GICD_BASE			UL(0x2c001000)
+#define VE_GICC_BASE			UL(0x2c002000)
+#define VE_GICH_BASE			UL(0x2c004000)
+#define VE_GICV_BASE			UL(0x2c006000)
 
 /* Base FVP compatible GIC memory map */
-#define BASE_GICD_BASE			0x2f000000
-#define BASE_GICR_BASE			0x2f100000
-#define BASE_GICC_BASE			0x2c000000
-#define BASE_GICH_BASE			0x2c010000
-#define BASE_GICV_BASE			0x2c02f000
+#define BASE_GICD_BASE			UL(0x2f000000)
+#define BASE_GICR_BASE			UL(0x2f100000)
+#define BASE_GICC_BASE			UL(0x2c000000)
+#define BASE_GICH_BASE			UL(0x2c010000)
+#define BASE_GICV_BASE			UL(0x2c02f000)
 
 #define FVP_IRQ_TZ_WDOG			56
 #define FVP_IRQ_SEC_SYS_TIMER		57
diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index ed1a302..44ab1b3 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h
@@ -4,8 +4,8 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef __PLATFORM_DEF_H__
-#define __PLATFORM_DEF_H__
+#ifndef PLATFORM_DEF_H
+#define PLATFORM_DEF_H
 
 /* Enable the dynamic translation tables library. */
 #ifdef AARCH32
@@ -44,26 +44,26 @@
  */
 #define PLAT_ARM_CLUSTER_COUNT		FVP_CLUSTER_COUNT
 
-#define PLAT_ARM_TRUSTED_SRAM_SIZE	0x00040000	/* 256 KB */
+#define PLAT_ARM_TRUSTED_SRAM_SIZE	UL(0x00040000)	/* 256 KB */
 
-#define PLAT_ARM_TRUSTED_ROM_BASE	0x00000000
-#define PLAT_ARM_TRUSTED_ROM_SIZE	0x04000000	/* 64 MB */
+#define PLAT_ARM_TRUSTED_ROM_BASE	UL(0x00000000)
+#define PLAT_ARM_TRUSTED_ROM_SIZE	UL(0x04000000)	/* 64 MB */
 
-#define PLAT_ARM_TRUSTED_DRAM_BASE	0x06000000
-#define PLAT_ARM_TRUSTED_DRAM_SIZE	0x02000000	/* 32 MB */
+#define PLAT_ARM_TRUSTED_DRAM_BASE	UL(0x06000000)
+#define PLAT_ARM_TRUSTED_DRAM_SIZE	UL(0x02000000)	/* 32 MB */
 
 /* virtual address used by dynamic mem_protect for chunk_base */
 #define PLAT_ARM_MEM_PROTEC_VA_FRAME	UL(0xc0000000)
 
 /* No SCP in FVP */
-#define PLAT_ARM_SCP_TZC_DRAM1_SIZE	ULL(0x0)
+#define PLAT_ARM_SCP_TZC_DRAM1_SIZE	UL(0x0)
 
-#define PLAT_ARM_DRAM2_SIZE		ULL(0x80000000)
+#define PLAT_ARM_DRAM2_SIZE		UL(0x80000000)
 
 /*
  * Load address of BL33 for this platform port
  */
-#define PLAT_ARM_NS_IMAGE_OFFSET	(ARM_DRAM1_BASE + U(0x8000000))
+#define PLAT_ARM_NS_IMAGE_OFFSET	(ARM_DRAM1_BASE + UL(0x8000000))
 
 /*
  * PLAT_ARM_MMAP_ENTRIES depends on the number of entries in the
@@ -94,18 +94,18 @@
  * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
  * plus a little space for growth.
  */
-#define PLAT_ARM_MAX_BL1_RW_SIZE	0xB000
+#define PLAT_ARM_MAX_BL1_RW_SIZE	UL(0xB000)
 
 /*
  * PLAT_ARM_MAX_ROMLIB_RW_SIZE is define to use a full page
  */
 
 #if USE_ROMLIB
-#define PLAT_ARM_MAX_ROMLIB_RW_SIZE	0x1000
-#define PLAT_ARM_MAX_ROMLIB_RO_SIZE	0xe000
+#define PLAT_ARM_MAX_ROMLIB_RW_SIZE	UL(0x1000)
+#define PLAT_ARM_MAX_ROMLIB_RO_SIZE	UL(0xe000)
 #else
-#define PLAT_ARM_MAX_ROMLIB_RW_SIZE	0
-#define PLAT_ARM_MAX_ROMLIB_RO_SIZE	0
+#define PLAT_ARM_MAX_ROMLIB_RW_SIZE	UL(0)
+#define PLAT_ARM_MAX_ROMLIB_RO_SIZE	UL(0)
 #endif
 
 /*
@@ -113,9 +113,9 @@
  * little space for growth.
  */
 #if TRUSTED_BOARD_BOOT
-# define PLAT_ARM_MAX_BL2_SIZE		0x1D000
+# define PLAT_ARM_MAX_BL2_SIZE		UL(0x1D000)
 #else
-# define PLAT_ARM_MAX_BL2_SIZE		0x11000
+# define PLAT_ARM_MAX_BL2_SIZE		UL(0x11000)
 #endif
 
 /*
@@ -123,7 +123,7 @@
  * calculated using the current BL31 PROGBITS debug size plus the sizes of
  * BL2 and BL1-RW
  */
-#define PLAT_ARM_MAX_BL31_SIZE		0x3B000
+#define PLAT_ARM_MAX_BL31_SIZE		UL(0x3B000)
 
 #ifdef AARCH32
 /*
@@ -131,7 +131,7 @@
  * calculated using the current SP_MIN PROGBITS debug size plus the sizes of
  * BL2 and BL1-RW
  */
-# define PLAT_ARM_MAX_BL32_SIZE		0x3B000
+# define PLAT_ARM_MAX_BL32_SIZE		UL(0x3B000)
 #endif
 
 /*
@@ -139,28 +139,28 @@
  */
 #if defined(IMAGE_BL1)
 # if TRUSTED_BOARD_BOOT
-#  define PLATFORM_STACK_SIZE 0x1000
+#  define PLATFORM_STACK_SIZE		UL(0x1000)
 # else
-#  define PLATFORM_STACK_SIZE 0x440
+#  define PLATFORM_STACK_SIZE		UL(0x440)
 # endif
 #elif defined(IMAGE_BL2)
 # if TRUSTED_BOARD_BOOT
-#  define PLATFORM_STACK_SIZE 0x1000
+#  define PLATFORM_STACK_SIZE		UL(0x1000)
 # else
-#  define PLATFORM_STACK_SIZE 0x400
+#  define PLATFORM_STACK_SIZE		UL(0x400)
 # endif
 #elif defined(IMAGE_BL2U)
-# define PLATFORM_STACK_SIZE 0x400
+# define PLATFORM_STACK_SIZE		UL(0x400)
 #elif defined(IMAGE_BL31)
 # if ENABLE_SPM
-#  define PLATFORM_STACK_SIZE 0x500
+#  define PLATFORM_STACK_SIZE		UL(0x500)
 # elif PLAT_XLAT_TABLES_DYNAMIC
-#  define PLATFORM_STACK_SIZE 0x800
+#  define PLATFORM_STACK_SIZE		UL(0x800)
 # else
-#  define PLATFORM_STACK_SIZE 0x400
+#  define PLATFORM_STACK_SIZE		UL(0x400)
 # endif
 #elif defined(IMAGE_BL32)
-# define PLATFORM_STACK_SIZE 0x440
+# define PLATFORM_STACK_SIZE		UL(0x440)
 #endif
 
 #define MAX_IO_DEVICES			3
@@ -191,20 +191,20 @@
 #define PLAT_ARM_TSP_UART_BASE		V2M_IOFPGA_UART2_BASE
 #define PLAT_ARM_TSP_UART_CLK_IN_HZ	V2M_IOFPGA_UART2_CLK_IN_HZ
 
-#define PLAT_FVP_SMMUV3_BASE		0x2b400000
+#define PLAT_FVP_SMMUV3_BASE		UL(0x2b400000)
 
 /* CCI related constants */
-#define PLAT_FVP_CCI400_BASE		0x2c090000
+#define PLAT_FVP_CCI400_BASE		UL(0x2c090000)
 #define PLAT_FVP_CCI400_CLUS0_SL_PORT	3
 #define PLAT_FVP_CCI400_CLUS1_SL_PORT	4
 
 /* CCI-500/CCI-550 on Base platform */
-#define PLAT_FVP_CCI5XX_BASE		0x2a000000
+#define PLAT_FVP_CCI5XX_BASE		UL(0x2a000000)
 #define PLAT_FVP_CCI5XX_CLUS0_SL_PORT	5
 #define PLAT_FVP_CCI5XX_CLUS1_SL_PORT	6
 
 /* CCN related constants. Only CCN 502 is currently supported */
-#define PLAT_ARM_CCN_BASE		0x2e000000
+#define PLAT_ARM_CCN_BASE		UL(0x2e000000)
 #define PLAT_ARM_CLUSTER_TO_CCN_ID_MAP	1, 5, 7, 11
 
 /* System timer related constants */
@@ -229,7 +229,7 @@
  * Give access to the CPUs and Virtio. Some devices
  * would normally use the default ID so allow that too.
  */
-#define PLAT_ARM_TZC_BASE		0x2a4a0000
+#define PLAT_ARM_TZC_BASE		UL(0x2a4a0000)
 #define PLAT_ARM_TZC_FILTERS		TZC_400_REGION_ATTR_FILTER_BIT(0)
 
 #define PLAT_ARM_TZC_NS_DEV_ACCESS	(				\
@@ -268,4 +268,4 @@
 #define PLAT_ARM_SP_IMAGE_STACK_BASE	(ARM_SP_IMAGE_NS_BUF_BASE +	\
 					 ARM_SP_IMAGE_NS_BUF_SIZE)
 
-#endif /* __PLATFORM_DEF_H__ */
+#endif /* PLATFORM_DEF_H */
diff --git a/plat/arm/board/juno/include/platform_def.h b/plat/arm/board/juno/include/platform_def.h
index d4a77f0..89e28ef 100644
--- a/plat/arm/board/juno/include/platform_def.h
+++ b/plat/arm/board/juno/include/platform_def.h
@@ -4,8 +4,8 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef __PLATFORM_DEF_H__
-#define __PLATFORM_DEF_H__
+#ifndef PLATFORM_DEF_H
+#define PLATFORM_DEF_H
 
 /* Enable the dynamic translation tables library. */
 #ifdef AARCH32
@@ -41,7 +41,7 @@
 					JUNO_CLUSTER1_CORE_COUNT)
 
 /* Cryptocell HW Base address */
-#define PLAT_CRYPTOCELL_BASE		0x60050000
+#define PLAT_CRYPTOCELL_BASE		UL(0x60050000)
 
 /*
  * Other platform porting definitions are provided by included headers
@@ -52,13 +52,13 @@
  */
 #define PLAT_ARM_CLUSTER_COUNT		JUNO_CLUSTER_COUNT
 
-#define PLAT_ARM_TRUSTED_SRAM_SIZE	0x00040000	/* 256 KB */
+#define PLAT_ARM_TRUSTED_SRAM_SIZE	UL(0x00040000)	/* 256 KB */
 
 /* Use the bypass address */
 #define PLAT_ARM_TRUSTED_ROM_BASE	V2M_FLASH0_BASE + BL1_ROM_BYPASS_OFFSET
 
-#define NSRAM_BASE			0x2e000000
-#define NSRAM_SIZE			0x00008000	/* 32KB */
+#define NSRAM_BASE			UL(0x2e000000)
+#define NSRAM_SIZE			UL(0x00008000)	/* 32KB */
 
 /* virtual address used by dynamic mem_protect for chunk_base */
 #define PLAT_ARM_MEM_PROTEC_VA_FRAME	UL(0xc0000000)
@@ -71,9 +71,9 @@
 #define PLAT_ARM_MAX_ROMLIB_RO_SIZE	0
 
 #if TRUSTED_BOARD_BOOT
-#define PLAT_ARM_TRUSTED_ROM_SIZE	0x00020000
+#define PLAT_ARM_TRUSTED_ROM_SIZE	UL(0x00020000)
 #else
-#define PLAT_ARM_TRUSTED_ROM_SIZE	0x00010000
+#define PLAT_ARM_TRUSTED_ROM_SIZE	UL(0x00010000)
 #endif /* TRUSTED_BOARD_BOOT */
 
 /*
@@ -115,18 +115,18 @@
  * plus a little space for growth.
  */
 #if TRUSTED_BOARD_BOOT
-# define PLAT_ARM_MAX_BL1_RW_SIZE	0xB000
+# define PLAT_ARM_MAX_BL1_RW_SIZE	UL(0xB000)
 #else
-# define PLAT_ARM_MAX_BL1_RW_SIZE	0x6000
+# define PLAT_ARM_MAX_BL1_RW_SIZE	UL(0x6000)
 #endif
 
 /*
  * PLAT_ARM_MAX_ROMLIB_RW_SIZE is define to use a full page
  */
 #if USE_ROMLIB
-#define PLAT_ARM_MAX_ROMLIB_RW_SIZE	0x1000
+#define PLAT_ARM_MAX_ROMLIB_RW_SIZE	UL(0x1000)
 #else
-#define PLAT_ARM_MAX_ROMLIB_RW_SIZE	0
+#define PLAT_ARM_MAX_ROMLIB_RW_SIZE	UL(0)
 #endif
 
 /*
@@ -135,14 +135,14 @@
  */
 #if TRUSTED_BOARD_BOOT
 #if TF_MBEDTLS_KEY_ALG_ID == TF_MBEDTLS_RSA_AND_ECDSA
-# define PLAT_ARM_MAX_BL2_SIZE		0x1F000
+# define PLAT_ARM_MAX_BL2_SIZE		UL(0x1F000)
 #elif TF_MBEDTLS_KEY_ALG_ID == TF_MBEDTLS_ECDSA
-# define PLAT_ARM_MAX_BL2_SIZE		0x1D000
+# define PLAT_ARM_MAX_BL2_SIZE		UL(0x1D000)
 #else
-# define PLAT_ARM_MAX_BL2_SIZE		0x1C000
+# define PLAT_ARM_MAX_BL2_SIZE		UL(0x1C000)
 #endif
 #else
-# define PLAT_ARM_MAX_BL2_SIZE		0xF000
+# define PLAT_ARM_MAX_BL2_SIZE		UL(0xF000)
 #endif
 
 /*
@@ -151,7 +151,7 @@
  * BL2 and BL1-RW.  SCP_BL2 image is loaded into the space BL31 -> BL2_BASE.
  * Hence the BL31 PROGBITS size should be >= PLAT_CSS_MAX_SCP_BL2_SIZE.
  */
-#define PLAT_ARM_MAX_BL31_SIZE		0x3E000
+#define PLAT_ARM_MAX_BL31_SIZE		UL(0x3E000)
 
 #if JUNO_AARCH32_EL3_RUNTIME
 /*
@@ -160,7 +160,7 @@
  * BL2 and BL1-RW.  SCP_BL2 image is loaded into the space BL32 -> BL2_BASE.
  * Hence the BL32 PROGBITS size should be >= PLAT_CSS_MAX_SCP_BL2_SIZE.
  */
-#define PLAT_ARM_MAX_BL32_SIZE		0x3E000
+#define PLAT_ARM_MAX_BL32_SIZE		UL(0x3E000)
 #endif
 
 /*
@@ -168,26 +168,26 @@
  */
 #if defined(IMAGE_BL1)
 # if TRUSTED_BOARD_BOOT
-#  define PLATFORM_STACK_SIZE 0x1000
+#  define PLATFORM_STACK_SIZE		UL(0x1000)
 # else
-#  define PLATFORM_STACK_SIZE 0x440
+#  define PLATFORM_STACK_SIZE		UL(0x440)
 # endif
 #elif defined(IMAGE_BL2)
 # if TRUSTED_BOARD_BOOT
-#  define PLATFORM_STACK_SIZE 0x1000
+#  define PLATFORM_STACK_SIZE		UL(0x1000)
 # else
-#  define PLATFORM_STACK_SIZE 0x400
+#  define PLATFORM_STACK_SIZE		UL(0x400)
 # endif
 #elif defined(IMAGE_BL2U)
-# define PLATFORM_STACK_SIZE 0x400
+# define PLATFORM_STACK_SIZE		UL(0x400)
 #elif defined(IMAGE_BL31)
 # if PLAT_XLAT_TABLES_DYNAMIC
-#  define PLATFORM_STACK_SIZE 0x800
+#  define PLATFORM_STACK_SIZE		UL(0x800)
 # else
-#  define PLATFORM_STACK_SIZE 0x400
+#  define PLATFORM_STACK_SIZE		UL(0x400)
 # endif
 #elif defined(IMAGE_BL32)
-# define PLATFORM_STACK_SIZE 0x440
+# define PLATFORM_STACK_SIZE		UL(0x440)
 #endif
 
 /*
@@ -197,7 +197,7 @@
 #define PLAT_LOG_LEVEL_ASSERT		40
 
 /* CCI related constants */
-#define PLAT_ARM_CCI_BASE		0x2c090000
+#define PLAT_ARM_CCI_BASE		UL(0x2c090000)
 #define PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX	4
 #define PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX	3
 
@@ -205,7 +205,7 @@
 #define PLAT_ARM_NSTIMER_FRAME_ID		1
 
 /* TZC related constants */
-#define PLAT_ARM_TZC_BASE		0x2a4a0000
+#define PLAT_ARM_TZC_BASE		UL(0x2a4a0000)
 #define PLAT_ARM_TZC_NS_DEV_ACCESS	(				\
 		TZC_REGION_ACCESS_RDWR(TZC400_NSAID_CCI400)	|	\
 		TZC_REGION_ACCESS_RDWR(TZC400_NSAID_PCIE)	|	\
@@ -223,13 +223,13 @@
  */
 
 /* GIC related constants (no GICR in GIC-400) */
-#define PLAT_ARM_GICD_BASE		0x2c010000
-#define PLAT_ARM_GICC_BASE		0x2c02f000
-#define PLAT_ARM_GICH_BASE		0x2c04f000
-#define PLAT_ARM_GICV_BASE		0x2c06f000
+#define PLAT_ARM_GICD_BASE		UL(0x2c010000)
+#define PLAT_ARM_GICC_BASE		UL(0x2c02f000)
+#define PLAT_ARM_GICH_BASE		UL(0x2c04f000)
+#define PLAT_ARM_GICV_BASE		UL(0x2c06f000)
 
 /* MHU related constants */
-#define PLAT_CSS_MHU_BASE		0x2b1f0000
+#define PLAT_CSS_MHU_BASE		UL(0x2b1f0000)
 
 /*
  * Base address of the first memory region used for communication between AP
@@ -243,7 +243,7 @@
  * 32-bit word on all CSS platforms. On Juno, part of this configuration is
  * which CPU is the primary, according to the shift and mask definitions below.
  */
-#define PLAT_CSS_SCP_COM_SHARED_MEM_BASE	(ARM_TRUSTED_SRAM_BASE + 0x80)
+#define PLAT_CSS_SCP_COM_SHARED_MEM_BASE	(ARM_TRUSTED_SRAM_BASE + UL(0x80))
 #define PLAT_CSS_PRIMARY_CPU_SHIFT		8
 #define PLAT_CSS_PRIMARY_CPU_BIT_WIDTH		4
 #endif
@@ -252,13 +252,13 @@
  * PLAT_CSS_MAX_SCP_BL2_SIZE is calculated using the current
  * SCP_BL2 size plus a little space for growth.
  */
-#define PLAT_CSS_MAX_SCP_BL2_SIZE	0x14000
+#define PLAT_CSS_MAX_SCP_BL2_SIZE	UL(0x14000)
 
 /*
  * PLAT_CSS_MAX_SCP_BL2U_SIZE is calculated using the current
  * SCP_BL2U size plus a little space for growth.
  */
-#define PLAT_CSS_MAX_SCP_BL2U_SIZE	0x14000
+#define PLAT_CSS_MAX_SCP_BL2U_SIZE	UL(0x14000)
 
 #define PLAT_ARM_G1S_IRQ_PROPS(grp) \
 	CSS_G1S_IRQ_PROPS(grp), \
@@ -287,9 +287,9 @@
  */
 
 /* CSS SoC NIC-400 Global Programmers View (GPV) */
-#define PLAT_SOC_CSS_NIC400_BASE	0x2a000000
+#define PLAT_SOC_CSS_NIC400_BASE	UL(0x2a000000)
 
 #define PLAT_ARM_PRIVATE_SDEI_EVENTS	ARM_SDEI_PRIVATE_EVENTS
 #define PLAT_ARM_SHARED_SDEI_EVENTS	ARM_SDEI_SHARED_EVENTS
 
-#endif /* __PLATFORM_DEF_H__ */
+#endif /* PLATFORM_DEF_H */
diff --git a/plat/arm/board/juno/juno_def.h b/plat/arm/board/juno/juno_def.h
index 95f2b39..5301886 100644
--- a/plat/arm/board/juno/juno_def.h
+++ b/plat/arm/board/juno/juno_def.h
@@ -14,20 +14,20 @@
  ******************************************************************************/
 
 /* Board revisions */
-#define REV_JUNO_R0			0x1	/* Rev B */
-#define REV_JUNO_R1			0x2	/* Rev C */
-#define REV_JUNO_R2			0x3	/* Rev D */
+#define REV_JUNO_R0			U(0x1)	/* Rev B */
+#define REV_JUNO_R1			U(0x2)	/* Rev C */
+#define REV_JUNO_R2			U(0x3)	/* Rev D */
 
 /* Bypass offset from start of NOR flash */
-#define BL1_ROM_BYPASS_OFFSET		0x03EC0000
+#define BL1_ROM_BYPASS_OFFSET		UL(0x03EC0000)
 
-#define EMMC_BASE			0x0c000000
-#define EMMC_SIZE			0x04000000
+#define EMMC_BASE			UL(0x0c000000)
+#define EMMC_SIZE			UL(0x04000000)
 
-#define PSRAM_BASE			0x14000000
-#define PSRAM_SIZE			0x02000000
+#define PSRAM_BASE			UL(0x14000000)
+#define PSRAM_SIZE			UL(0x02000000)
 
-#define JUNO_SSC_VER_PART_NUM		0x030
+#define JUNO_SSC_VER_PART_NUM		U(0x030)
 
 /*******************************************************************************
  * Juno topology related constants
@@ -54,19 +54,19 @@
 /*******************************************************************************
  * TRNG related constants
  ******************************************************************************/
-#define TRNG_BASE	0x7FE60000ULL
+#define TRNG_BASE	UL(0x7FE60000)
 #define TRNG_NOUTPUTS	4
-#define TRNG_STATUS	0x10
-#define TRNG_INTMASK	0x14
-#define TRNG_CONFIG	0x18
-#define TRNG_CONTROL	0x1C
+#define TRNG_STATUS	UL(0x10)
+#define TRNG_INTMASK	UL(0x14)
+#define TRNG_CONFIG	UL(0x18)
+#define TRNG_CONTROL	UL(0x1C)
 #define TRNG_NBYTES	16	/* Number of bytes generated per round. */
 
 /*******************************************************************************
  * MMU-401 related constants
  ******************************************************************************/
-#define MMU401_SSD_OFFSET		0x4000
-#define MMU401_DMA330_BASE		0x7fb00000
+#define MMU401_SSD_OFFSET		UL(0x4000)
+#define MMU401_DMA330_BASE		UL(0x7fb00000)
 
 /*******************************************************************************
  * Interrupt handling constants
diff --git a/plat/arm/board/juno/juno_topology.c b/plat/arm/board/juno/juno_topology.c
index 6d8fc05..c8a7598 100644
--- a/plat/arm/board/juno/juno_topology.c
+++ b/plat/arm/board/juno/juno_topology.c
@@ -12,6 +12,7 @@
 #include "../../css/drivers/scmi/scmi.h"
 #include "../../css/drivers/mhu/css_mhu_doorbell.h"
 
+#if CSS_USE_SCMI_SDS_DRIVER
 static scmi_channel_plat_info_t juno_scmi_plat_info = {
 		.scmi_mbx_mem = CSS_SCMI_PAYLOAD_BASE,
 		.db_reg_addr = PLAT_CSS_MHU_BASE + CSS_SCMI_MHU_DB_REG_OFF,
@@ -25,6 +26,7 @@
 	return &juno_scmi_plat_info;
 }
 
+#endif
 /*
  * On Juno, the system power level is the highest power level.
  * The first entry in the power domain descriptor specifies the
diff --git a/plat/arm/board/n1sdp/aarch64/n1sdp_helper.S b/plat/arm/board/n1sdp/aarch64/n1sdp_helper.S
new file mode 100644
index 0000000..6eb01aa
--- /dev/null
+++ b/plat/arm/board/n1sdp/aarch64/n1sdp_helper.S
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <cortex_ares.h>
+#include <cpu_macros.S>
+#include <platform_def.h>
+
+	.globl	plat_arm_calc_core_pos
+	.globl	plat_reset_handler
+
+	/* -----------------------------------------------------
+	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
+	 *
+	 * Helper function to calculate the core position.
+	 * (ClusterId * N1SDP_MAX_CPUS_PER_CLUSTER * N1SDP_MAX_PE_PER_CPU) +
+	 * (CPUId * N1SDP_MAX_PE_PER_CPU) +
+	 * ThreadId
+	 *
+	 * which can be simplified as:
+	 *
+	 * ((ClusterId * N1SDP_MAX_CPUS_PER_CLUSTER + CPUId) *
+	 * N1SDP_MAX_PE_PER_CPU) + ThreadId
+	 * ------------------------------------------------------
+	 */
+
+func plat_arm_calc_core_pos
+	mov	x3, x0
+
+	/*
+	 * The MT bit in MPIDR is always set for n1sdp and the
+	 * affinity level 0 corresponds to thread affinity level.
+	 */
+
+	/* Extract individual affinity fields from MPIDR */
+	ubfx	x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx	x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx	x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
+
+	/* Compute linear position */
+	mov	x4, #N1SDP_MAX_CPUS_PER_CLUSTER
+	madd	x1, x2, x4, x1
+	mov	x5, #N1SDP_MAX_PE_PER_CPU
+	madd	x0, x1, x5, x0
+	ret
+endfunc plat_arm_calc_core_pos
+
+	/* -----------------------------------------------------
+	 * void plat_reset_handler(void);
+	 *
+	 * Determine the CPU MIDR and disable power down bit for
+	 * that CPU.
+	 * -----------------------------------------------------
+	 */
+
+func plat_reset_handler
+	jump_if_cpu_midr CORTEX_ARES_MIDR, ARES
+	ret
+
+	/* -----------------------------------------------------
+	 * Disable CPU power down bit in power control register
+	 * -----------------------------------------------------
+	 */
+ARES:
+	mrs	x0, CORTEX_ARES_CPUPWRCTLR_EL1
+	bic	x0, x0, #CORTEX_ARES_CORE_PWRDN_EN_MASK
+	msr	CORTEX_ARES_CPUPWRCTLR_EL1, x0
+	isb
+	ret
+endfunc plat_reset_handler
diff --git a/plat/arm/board/n1sdp/include/plat_macros.S b/plat/arm/board/n1sdp/include/plat_macros.S
new file mode 100644
index 0000000..fe9a66c
--- /dev/null
+++ b/plat/arm/board/n1sdp/include/plat_macros.S
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef __PLAT_MACROS_S__
+#define __PLAT_MACROS_S__
+
+#include <css_macros.S>
+
+/* ---------------------------------------------
+ * The below required platform porting macro
+ * prints out relevant platform registers
+ * whenever an unhandled exception is taken in
+ * BL31.
+ *
+ * There are currently no platform specific regs
+ * to print.
+ * ---------------------------------------------
+ */
+	.macro plat_crash_print_regs
+	.endm
+#endif /* __PLAT_MACROS_S__ */
diff --git a/plat/arm/board/n1sdp/include/platform_def.h b/plat/arm/board/n1sdp/include/platform_def.h
new file mode 100644
index 0000000..d26f559
--- /dev/null
+++ b/plat/arm/board/n1sdp/include/platform_def.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __PLATFORM_DEF_H__
+#define __PLATFORM_DEF_H__
+
+#include <arm_def.h>
+#include <board_css_def.h>
+#include <css_def.h>
+
+#if CSS_USE_SCMI_SDS_DRIVER
+#define N1SDP_SCMI_PAYLOAD_BASE			0x45400000
+#else
+#define PLAT_CSS_SCP_COM_SHARED_MEM_BASE	0x45400000
+#endif
+
+#define PLAT_ARM_TRUSTED_SRAM_SIZE		0x00080000	/* 512 KB */
+#define PLAT_ARM_MAX_BL31_SIZE			0X20000
+
+
+/*******************************************************************************
+ * N1SDP topology related constants
+ ******************************************************************************/
+#define N1SDP_MAX_CPUS_PER_CLUSTER		2
+#define PLAT_ARM_CLUSTER_COUNT			2
+#define N1SDP_MAX_PE_PER_CPU			1
+
+#define PLATFORM_CORE_COUNT			(PLAT_ARM_CLUSTER_COUNT *	\
+						N1SDP_MAX_CPUS_PER_CLUSTER *	\
+						N1SDP_MAX_PE_PER_CPU)
+
+
+/*
+ * PLAT_ARM_MMAP_ENTRIES depends on the number of entries in the
+ * plat_arm_mmap array defined for each BL stage.
+ */
+#define PLAT_ARM_MMAP_ENTRIES			3
+#define MAX_XLAT_TABLES				4
+
+#define PLATFORM_STACK_SIZE			0x400
+
+#define PLAT_ARM_NSTIMER_FRAME_ID		0
+#define PLAT_CSS_MHU_BASE			0x45000000
+#define PLAT_MAX_PWR_LVL			1
+
+#define PLAT_ARM_G1S_IRQS			ARM_G1S_IRQS,			\
+						CSS_IRQ_MHU
+#define PLAT_ARM_G0_IRQS			ARM_G0_IRQS
+
+#define PLAT_ARM_G1S_IRQ_PROPS(grp)		CSS_G1S_IRQ_PROPS(grp)
+#define PLAT_ARM_G0_IRQ_PROPS(grp)		ARM_G0_IRQ_PROPS(grp)
+
+
+#define N1SDP_DEVICE_BASE			(0x20000000)
+#define N1SDP_DEVICE_SIZE			(0x20000000)
+#define N1SDP_MAP_DEVICE			MAP_REGION_FLAT(	\
+						N1SDP_DEVICE_BASE,	\
+						N1SDP_DEVICE_SIZE,	\
+						MT_DEVICE | MT_RW | MT_SECURE)
+
+/* GIC related constants */
+#define PLAT_ARM_GICD_BASE			0x30000000
+#define PLAT_ARM_GICC_BASE			0x2C000000
+#define PLAT_ARM_GICR_BASE			0x300C0000
+
+/* Platform ID address */
+#define SSC_VERSION				(SSC_REG_BASE + SSC_VERSION_OFFSET)
+#endif /* __PLATFORM_DEF_H__ */
diff --git a/plat/arm/board/n1sdp/n1sdp_bl31_setup.c b/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
new file mode 100644
index 0000000..65aad9c
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "../../css/drivers/scmi/scmi.h"
+#include "../../css/drivers/mhu/css_mhu_doorbell.h"
+#include <platform_def.h>
+
+static scmi_channel_plat_info_t n1sdp_scmi_plat_info = {
+		.scmi_mbx_mem = N1SDP_SCMI_PAYLOAD_BASE,
+		.db_reg_addr = PLAT_CSS_MHU_BASE + CSS_SCMI_MHU_DB_REG_OFF,
+		.db_preserve_mask = 0xfffffffe,
+		.db_modify_mask = 0x1,
+		.ring_doorbell = &mhu_ring_doorbell,
+};
+
+scmi_channel_plat_info_t *plat_css_get_scmi_info()
+{
+	return &n1sdp_scmi_plat_info;
+}
diff --git a/plat/arm/board/n1sdp/n1sdp_interconnect.c b/plat/arm/board/n1sdp/n1sdp_interconnect.c
new file mode 100644
index 0000000..908f41c
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_interconnect.c
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+
+/*
+ * For N1SDP which support FCM (with automatic interconnect enter/exit),
+ * we should not do anything in these interface functions.
+ * They are used to override the weak functions in cci drivers.
+ */
+
+/******************************************************************************
+ * Helper function to initialize ARM interconnect driver.
+ *****************************************************************************/
+void plat_arm_interconnect_init(void)
+{
+}
+
+/******************************************************************************
+ * Helper function to place current master into coherency
+ *****************************************************************************/
+void plat_arm_interconnect_enter_coherency(void)
+{
+}
+
+/******************************************************************************
+ * Helper function to remove current master from coherency
+ *****************************************************************************/
+void plat_arm_interconnect_exit_coherency(void)
+{
+}
diff --git a/plat/arm/board/n1sdp/n1sdp_plat.c b/plat/arm/board/n1sdp/n1sdp_plat.c
new file mode 100644
index 0000000..8c057c5
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_plat.c
@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arm_def.h>
+#include <bl_common.h>
+#include <debug.h>
+#include <plat_arm.h>
+#include <platform.h>
+#include <platform_def.h>
+
+/*
+ * Table of regions to map using the MMU.
+ * Replace or extend the below regions as required
+ */
+
+const mmap_region_t plat_arm_mmap[] = {
+	ARM_MAP_SHARED_RAM,
+	N1SDP_MAP_DEVICE,
+	SOC_CSS_MAP_DEVICE,
+	{0}
+};
+
diff --git a/plat/arm/board/n1sdp/n1sdp_security.c b/plat/arm/board/n1sdp/n1sdp_security.c
new file mode 100644
index 0000000..d2a187b
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_security.c
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * TZC programming is currently not done.
+ */
+void plat_arm_security_setup(void)
+{
+}
diff --git a/plat/arm/board/n1sdp/n1sdp_topology.c b/plat/arm/board/n1sdp/n1sdp_topology.c
new file mode 100644
index 0000000..c3b4550
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_topology.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat_arm.h>
+
+/* Topology */
+typedef struct n1sdp_topology {
+	const unsigned char *power_tree;
+	unsigned int plat_cluster_core_count;
+} n1sdp_topology_t;
+
+/*
+ * The power domain tree descriptor. The cluster power domains are
+ * arranged so that when the PSCI generic code creates the power domain tree,
+ * the indices of the CPU power domain nodes it allocates match the linear
+ * indices returned by plat_core_pos_by_mpidr().
+ */
+const unsigned char n1sdp_pd_tree_desc[] = {
+	PLAT_ARM_CLUSTER_COUNT,
+	N1SDP_MAX_CPUS_PER_CLUSTER,
+	N1SDP_MAX_CPUS_PER_CLUSTER
+};
+
+/* Topology configuration for n1sdp */
+const n1sdp_topology_t n1sdp_topology = {
+	.power_tree = n1sdp_pd_tree_desc,
+	.plat_cluster_core_count = N1SDP_MAX_CPUS_PER_CLUSTER
+};
+
+/*******************************************************************************
+ * This function returns the topology tree information.
+ ******************************************************************************/
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	return n1sdp_topology.power_tree;
+}
+
+/*******************************************************************************
+ * This function returns the core count within the cluster corresponding to
+ * `mpidr`.
+ ******************************************************************************/
+unsigned int plat_arm_get_cluster_core_count(u_register_t mpidr)
+{
+	return n1sdp_topology.plat_cluster_core_count;
+}
+
+/*******************************************************************************
+ * The array mapping platform core position (implemented by plat_my_core_pos())
+ * to the SCMI power domain ID implemented by SCP.
+ ******************************************************************************/
+const uint32_t plat_css_core_pos_to_scmi_dmn_id_map[PLATFORM_CORE_COUNT] = {
+	0, 1, 2, 3};
diff --git a/plat/arm/board/n1sdp/platform.mk b/plat/arm/board/n1sdp/platform.mk
new file mode 100644
index 0000000..c9acb49
--- /dev/null
+++ b/plat/arm/board/n1sdp/platform.mk
@@ -0,0 +1,67 @@
+#
+# Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+
+N1SDP_BASE		:=	plat/arm/board/n1sdp
+
+INTERCONNECT_SOURCES	:=	${N1SDP_BASE}/n1sdp_interconnect.c
+
+PLAT_INCLUDES		:=	-I${N1SDP_BASE}/include
+
+
+N1SDP_CPU_SOURCES	:=	lib/cpus/aarch64/cortex_ares.S
+
+
+N1SDP_GIC_SOURCES	:=	drivers/arm/gic/common/gic_common.c	\
+				drivers/arm/gic/v3/gicv3_main.c		\
+				drivers/arm/gic/v3/gicv3_helpers.c	\
+				plat/common/plat_gicv3.c		\
+				plat/arm/common/arm_gicv3.c		\
+				drivers/arm/gic/v3/gic600.c
+
+PLAT_BL_COMMON_SOURCES	:=	${N1SDP_BASE}/n1sdp_plat.c	        \
+				${N1SDP_BASE}/aarch64/n1sdp_helper.S
+
+
+BL31_SOURCES		:=	${N1SDP_CPU_SOURCES}			\
+				${INTERCONNECT_SOURCES}			\
+				${N1SDP_GIC_SOURCES}			\
+				${N1SDP_BASE}/n1sdp_bl31_setup.c	        \
+				${N1SDP_BASE}/n1sdp_topology.c	        \
+				${N1SDP_BASE}/n1sdp_security.c
+
+
+# TF-A not required to load the SCP Images
+override CSS_LOAD_SCP_IMAGES	  	:=	0
+
+# BL1/BL2 Image not a part of the capsule Image for n1sdp
+override NEED_BL1		  	:=	no
+override NEED_BL2		  	:=	no
+override NEED_BL2U		  	:=	no
+
+#TFA for n1sdp starts from BL31
+override RESET_TO_BL31            	:=	1
+
+# 32 bit mode not supported
+override CTX_INCLUDE_AARCH32_REGS 	:=	0
+
+override ARM_PLAT_MT              	:=	1
+
+# Select SCMI/SDS drivers instead of SCPI/BOM driver for communicating with the
+# SCP during power management operations and for SCP RAM Firmware transfer.
+CSS_USE_SCMI_SDS_DRIVER		  	:=	1
+
+# System coherency is managed in hardware
+HW_ASSISTED_COHERENCY			:=	1
+
+# When building for systems with hardware-assisted coherency, there's no need to
+# use USE_COHERENT_MEM. Require that USE_COHERENT_MEM must be set to 0 too.
+USE_COHERENT_MEM			:=	0
+include plat/arm/common/arm_common.mk
+include plat/arm/css/common/css_common.mk
+include plat/arm/soc/common/soc_css.mk
+include plat/arm/board/common/board_common.mk
+
diff --git a/plat/arm/board/sgiclarka/fdts/sgiclarka.dts b/plat/arm/board/sgiclarka/fdts/sgiclarka.dts
new file mode 100644
index 0000000..43bd856
--- /dev/null
+++ b/plat/arm/board/sgiclarka/fdts/sgiclarka.dts
@@ -0,0 +1,21 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/dts-v1/;
+/ {
+	/* compatible string */
+	compatible = "arm,sgi-clark";
+
+	/*
+	 * Place holder for system-id node with default values. The
+	 * value of platform-id and config-id will be set to the
+	 * correct values during the BL2 stage of boot.
+	 */
+	system-id {
+		platform-id = <0x0>;
+		config-id = <0x0>;
+	};
+};
diff --git a/plat/arm/board/sgiclarka/fdts/sgiclarka_tb_fw_config.dts b/plat/arm/board/sgiclarka/fdts/sgiclarka_tb_fw_config.dts
new file mode 100644
index 0000000..315fa69
--- /dev/null
+++ b/plat/arm/board/sgiclarka/fdts/sgiclarka_tb_fw_config.dts
@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/dts-v1/;
+
+/ {
+	/* Platform Config */
+	compatible = "arm,tb_fw";
+	hw_config_addr = <0x0 0xFEF00000>;
+	hw_config_max_size = <0x0100000>;
+	/*
+	 * The following two entries are placeholders for Mbed TLS
+	 * heap information. The default values don't matter since
+	 * they will be overwritten by BL1.
+	 * In case of having shared Mbed TLS heap between BL1 and BL2,
+	 * BL1 will populate these two properties with the respective
+	 * info about the shared heap. This info will be available for
+	 * BL2 in order to locate and re-use the heap.
+	 */
+	mbedtls_heap_addr = <0x0 0x0>;
+	mbedtls_heap_size = <0x0>;
+};
diff --git a/plat/arm/board/sgiclarka/include/platform_def.h b/plat/arm/board/sgiclarka/include/platform_def.h
new file mode 100644
index 0000000..abc48d8
--- /dev/null
+++ b/plat/arm/board/sgiclarka/include/platform_def.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLATFORM_DEF_H
+#define PLATFORM_DEF_H
+
+#include <sgi_base_platform_def.h>
+
+#define PLAT_ARM_CLUSTER_COUNT		2
+#define CSS_SGI_MAX_CPUS_PER_CLUSTER	4
+#define CSS_SGI_MAX_PE_PER_CPU		1
+
+#define PLAT_CSS_MHU_BASE		0x45400000
+
+#endif /* PLATFORM_DEF_H */
diff --git a/plat/arm/board/sgiclarka/platform.mk b/plat/arm/board/sgiclarka/platform.mk
new file mode 100644
index 0000000..fc2f766
--- /dev/null
+++ b/plat/arm/board/sgiclarka/platform.mk
@@ -0,0 +1,38 @@
+#
+# Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include plat/arm/css/sgi/sgi-common.mk
+
+SGICLARKA_BASE		=	plat/arm/board/sgiclarka
+
+PLAT_INCLUDES		+=	-I${SGICLARKA_BASE}/include/
+
+SGI_CPU_SOURCES		:=	lib/cpus/aarch64/cortex_ares.S
+
+BL1_SOURCES		+=	${SGI_CPU_SOURCES}
+
+BL2_SOURCES		+=	lib/utils/mem_region.c			\
+				plat/arm/common/arm_nor_psci_mem_protect.c
+
+BL31_SOURCES		+=	${SGI_CPU_SOURCES}			\
+				drivers/cfi/v2m/v2m_flash.c		\
+				lib/utils/mem_region.c			\
+				plat/arm/common/arm_nor_psci_mem_protect.c
+
+# Add the FDT_SOURCES and options for Dynamic Config
+FDT_SOURCES		+=	${SGICLARKA_BASE}/fdts/${PLAT}_tb_fw_config.dts
+TB_FW_CONFIG		:=	${BUILD_PLAT}/fdts/${PLAT}_tb_fw_config.dtb
+
+# Add the TB_FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${TB_FW_CONFIG},--tb-fw-config))
+
+FDT_SOURCES		+=	${SGICLARKA_BASE}/fdts/${PLAT}.dts
+HW_CONFIG		:=	${BUILD_PLAT}/fdts/${PLAT}.dtb
+
+# Add the HW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${HW_CONFIG},--hw-config))
+
+override CTX_INCLUDE_AARCH32_REGS	:= 0
diff --git a/plat/arm/common/arm_common.c b/plat/arm/common/arm_common.c
index 6b14785..49f0074 100644
--- a/plat/arm/common/arm_common.c
+++ b/plat/arm/common/arm_common.c
@@ -63,7 +63,7 @@
 	uint32_t spsr;
 
 	/* Figure out what mode we enter the non-secure world in */
-	mode = EL_IMPLEMENTED(2) ? MODE_EL2 : MODE_EL1;
+	mode = (el_implemented(2) != EL_IMPL_NONE) ? MODE_EL2 : MODE_EL1;
 
 	/*
 	 * TODO: Consider the possibility of specifying the SPSR in
diff --git a/plat/arm/common/arm_dyn_cfg.c b/plat/arm/common/arm_dyn_cfg.c
index 057d772..c2a4bb1 100644
--- a/plat/arm/common/arm_dyn_cfg.c
+++ b/plat/arm/common/arm_dyn_cfg.c
@@ -122,8 +122,8 @@
 void arm_load_tb_fw_config(void)
 {
 	int err;
-	uintptr_t config_base = 0;
-	image_desc_t *image_desc;
+	uintptr_t config_base = 0UL;
+	image_desc_t *desc;
 
 	image_desc_t arm_tb_fw_info = {
 		.image_id = TB_FW_CONFIG_ID,
@@ -148,12 +148,11 @@
 	tb_fw_cfg_dtb_size = (size_t)arm_tb_fw_info.image_info.image_max_size;
 
 	/* The BL2 ep_info arg0 is modified to point to TB_FW_CONFIG */
-	image_desc = bl1_plat_get_image_desc(BL2_IMAGE_ID);
-	assert(image_desc != NULL);
-	image_desc->ep_info.args.arg0 = config_base;
+	desc = bl1_plat_get_image_desc(BL2_IMAGE_ID);
+	assert(desc != NULL);
+	desc->ep_info.args.arg0 = config_base;
 
-	INFO("BL1: TB_FW_CONFIG loaded at address = %p\n",
-			(void *) config_base);
+	INFO("BL1: TB_FW_CONFIG loaded at address = 0x%lx\n", config_base);
 
 #if TRUSTED_BOARD_BOOT && defined(DYN_DISABLE_AUTH)
 	int tb_fw_node;
@@ -240,7 +239,7 @@
 		 */
 		if (config_ids[i] != HW_CONFIG_ID) {
 
-			if (check_uptr_overflow(image_base, image_size) != 0)
+			if (check_uptr_overflow(image_base, image_size))
 				continue;
 
 			/* Ensure the configs don't overlap with BL31 */
diff --git a/plat/arm/common/execution_state_switch.c b/plat/arm/common/execution_state_switch.c
index b12d82c..8fa864d 100644
--- a/plat/arm/common/execution_state_switch.c
+++ b/plat/arm/common/execution_state_switch.c
@@ -117,7 +117,7 @@
 		 * Switching from AArch64 to AArch32. Ensure this CPU implements
 		 * the target EL in AArch32.
 		 */
-		impl = from_el2 ? EL_IMPLEMENTED(2) : EL_IMPLEMENTED(1);
+		impl = from_el2 ? el_implemented(2) : el_implemented(1);
 		if (impl != EL_IMPL_A64_A32)
 			goto exec_denied;
 
diff --git a/plat/arm/css/sgi/aarch64/sgi_helper.S b/plat/arm/css/sgi/aarch64/sgi_helper.S
index 27bae43..d79f1aa 100644
--- a/plat/arm/css/sgi/aarch64/sgi_helper.S
+++ b/plat/arm/css/sgi/aarch64/sgi_helper.S
@@ -8,6 +8,7 @@
 #include <asm_macros.S>
 #include <platform_def.h>
 #include <cortex_a75.h>
+#include <cortex_ares.h>
 #include <cpu_macros.S>
 
 	.globl	plat_arm_calc_core_pos
@@ -58,6 +59,7 @@
 	 */
 func plat_reset_handler
 	jump_if_cpu_midr CORTEX_A75_MIDR, A75
+	jump_if_cpu_midr CORTEX_ARES_MIDR, ARES
 	ret
 
 	/* -----------------------------------------------------
@@ -70,4 +72,11 @@
 	msr	CORTEX_A75_CPUPWRCTLR_EL1, x0
 	isb
 	ret
+
+ARES:
+	mrs	x0, CORTEX_ARES_CPUPWRCTLR_EL1
+	bic	x0, x0, #CORTEX_ARES_CORE_PWRDN_EN_MASK
+	msr	CORTEX_ARES_CPUPWRCTLR_EL1, x0
+	isb
+	ret
 endfunc plat_reset_handler
diff --git a/plat/arm/css/sgi/include/sgi_variant.h b/plat/arm/css/sgi/include/sgi_variant.h
index 5698d0a..dea580b 100644
--- a/plat/arm/css/sgi/include/sgi_variant.h
+++ b/plat/arm/css/sgi/include/sgi_variant.h
@@ -4,10 +4,21 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef __SGI_VARIANT_H__
-#define __SGI_VARIANT_H__
+#ifndef SGI_VARIANT_H
+#define SGI_VARIANT_H
 
 /* SSC_VERSION values for SGI575 */
 #define SGI575_SSC_VER_PART_NUM		0x0783
 
-#endif /* __SGI_VARIANT_H__ */
+/* SID Version values for SGI-Clark */
+#define SGI_CLARK_SID_VER_PART_NUM		0x0786
+
+/* Structure containing SGI platform variant information */
+typedef struct sgi_platform_info {
+	unsigned int platform_id;	/* Part Number of the platform */
+	unsigned int config_id;		/* Config Id of the platform */
+} sgi_platform_info_t;
+
+extern sgi_platform_info_t sgi_plat_info;
+
+#endif /* SGI_VARIANT_H */
diff --git a/plat/arm/css/sgi/sgi_bl31_setup.c b/plat/arm/css/sgi/sgi_bl31_setup.c
index a16343c..ce85026 100644
--- a/plat/arm/css/sgi/sgi_bl31_setup.c
+++ b/plat/arm/css/sgi/sgi_bl31_setup.c
@@ -4,13 +4,18 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
 #include <bl_common.h>
 #include <debug.h>
+#include <libfdt.h>
 #include <plat_arm.h>
 #include <sgi_ras.h>
+#include <sgi_variant.h>
 #include "../../css/drivers/scmi/scmi.h"
 #include "../../css/drivers/mhu/css_mhu_doorbell.h"
 
+sgi_platform_info_t sgi_plat_info;
+
 static scmi_channel_plat_info_t sgi575_scmi_plat_info = {
 		.scmi_mbx_mem = CSS_SCMI_PAYLOAD_BASE,
 		.db_reg_addr = PLAT_CSS_MHU_BASE + CSS_SCMI_MHU_DB_REG_OFF,
@@ -19,14 +24,72 @@
 		.ring_doorbell = &mhu_ring_doorbell,
 };
 
+static scmi_channel_plat_info_t sgi_clark_scmi_plat_info = {
+		.scmi_mbx_mem = CSS_SCMI_PAYLOAD_BASE,
+		.db_reg_addr = PLAT_CSS_MHU_BASE + SENDER_REG_SET(0),
+		.db_preserve_mask = 0xfffffffe,
+		.db_modify_mask = 0x1,
+		.ring_doorbell = &mhuv2_ring_doorbell,
+};
+
 scmi_channel_plat_info_t *plat_css_get_scmi_info()
 {
-	return &sgi575_scmi_plat_info;
+	if (sgi_plat_info.platform_id == SGI_CLARK_SID_VER_PART_NUM)
+		return &sgi_clark_scmi_plat_info;
+	else if (sgi_plat_info.platform_id == SGI575_SSC_VER_PART_NUM)
+		return &sgi575_scmi_plat_info;
+	else
+		panic();
+};
+
+/*******************************************************************************
+ * This function sets the sgi_platform_id and sgi_config_id
+ ******************************************************************************/
+int sgi_identify_platform(unsigned long hw_config)
+{
+	void *fdt;
+	int nodeoffset;
+	const unsigned int *property;
+
+	fdt = (void *)hw_config;
+
+	/* Check the validity of the fdt */
+	assert(fdt_check_header(fdt) == 0);
+
+	nodeoffset = fdt_subnode_offset(fdt, 0, "system-id");
+	if (nodeoffset < 0) {
+		ERROR("Failed to get system-id node offset\n");
+		return -1;
+	}
+
+	property = fdt_getprop(fdt, nodeoffset, "platform-id", NULL);
+	if (property == NULL) {
+		ERROR("Failed to get platform-id property\n");
+		return -1;
+	}
+
+	sgi_plat_info.platform_id = fdt32_to_cpu(*property);
+
+	property = fdt_getprop(fdt, nodeoffset, "config-id", NULL);
+	if (property == NULL) {
+		ERROR("Failed to get config-id property\n");
+		return -1;
+	}
+
+	sgi_plat_info.config_id = fdt32_to_cpu(*property);
+
+	return 0;
 }
 
 void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 				u_register_t arg2, u_register_t arg3)
 {
+	int ret;
+
+	ret = sgi_identify_platform(arg2);
+	if (ret == -1)
+		panic();
+
 	arm_bl31_early_platform_setup((void *)arg0, arg1, arg2, (void *)arg3);
 }
 
diff --git a/plat/arm/css/sgi/sgi_image_load.c b/plat/arm/css/sgi/sgi_image_load.c
index 52dcf88..d97583e 100644
--- a/plat/arm/css/sgi/sgi_image_load.c
+++ b/plat/arm/css/sgi/sgi_image_load.c
@@ -50,6 +50,10 @@
 		platid = mmio_read_32(SSC_VERSION) & SSC_VERSION_PART_NUM_MASK;
 		platcfg = (mmio_read_32(SSC_VERSION) >> SSC_VERSION_CONFIG_SHIFT)
 				& SSC_VERSION_CONFIG_MASK;
+	} else if (strcmp(platform_name, "arm,sgi-clark") == 0) {
+		platid = mmio_read_32(SID_REG_BASE + SID_SYSTEM_ID_OFFSET)
+				& SID_SYSTEM_ID_PART_NUM_MASK;
+		platcfg = mmio_read_32(SID_REG_BASE + SID_SYSTEM_CFG_OFFSET);
 	} else {
 		WARN("Invalid platform\n");
 		return -1;
diff --git a/plat/hisilicon/hikey/hikey_bl2_setup.c b/plat/hisilicon/hikey/hikey_bl2_setup.c
index 4a27e1b..62cd1f2 100644
--- a/plat/hisilicon/hikey/hikey_bl2_setup.c
+++ b/plat/hisilicon/hikey/hikey_bl2_setup.c
@@ -100,7 +100,7 @@
 	uint32_t spsr;
 
 	/* Figure out what mode we enter the non-secure world in */
-	mode = EL_IMPLEMENTED(2) ? MODE_EL2 : MODE_EL1;
+	mode = (el_implemented(2) != EL_IMPL_NONE) ? MODE_EL2 : MODE_EL1;
 
 	/*
 	 * TODO: Consider the possibility of specifying the SPSR in
diff --git a/plat/hisilicon/hikey960/hikey960_bl2_setup.c b/plat/hisilicon/hikey960/hikey960_bl2_setup.c
index f57dd63..0e79e0a 100644
--- a/plat/hisilicon/hikey960/hikey960_bl2_setup.c
+++ b/plat/hisilicon/hikey960/hikey960_bl2_setup.c
@@ -191,7 +191,7 @@
 	uint32_t spsr;
 
 	/* Figure out what mode we enter the non-secure world in */
-	mode = EL_IMPLEMENTED(2) ? MODE_EL2 : MODE_EL1;
+	mode = (el_implemented(2) != EL_IMPL_NONE) ? MODE_EL2 : MODE_EL1;
 
 	/*
 	 * TODO: Consider the possibility of specifying the SPSR in
diff --git a/plat/layerscape/common/ls_common.c b/plat/layerscape/common/ls_common.c
index abf6525..afd5927 100644
--- a/plat/layerscape/common/ls_common.c
+++ b/plat/layerscape/common/ls_common.c
@@ -147,7 +147,7 @@
 	uint32_t spsr;
 
 	/* Figure out what mode we enter the non-secure world in */
-	mode = EL_IMPLEMENTED(2) ? MODE_EL2 : MODE_EL1;
+	mode = (el_implemented(2) != EL_IMPL_NONE) ? MODE_EL2 : MODE_EL1;
 
 	/*
 	 * TODO: Consider the possibility of specifying the SPSR in
diff --git a/plat/marvell/a3700/a3700/board/pm_src.c b/plat/marvell/a3700/a3700/board/pm_src.c
new file mode 100644
index 0000000..bc48ce8
--- /dev/null
+++ b/plat/marvell/a3700/a3700/board/pm_src.c
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+#include <a3700_pm.h>
+#include <plat_marvell.h>
+
+/* This struct provides the PM wake up src configuration */
+static struct pm_wake_up_src_config wake_up_src_cfg = {
+	.wake_up_src_num = 3,
+	.wake_up_src[0] = {
+		.wake_up_src_type = WAKE_UP_SRC_GPIO,
+		.wake_up_data = {
+			.gpio_data.bank_num = 0, /* North Bridge */
+			.gpio_data.gpio_num = 14
+		}
+	},
+	.wake_up_src[1] = {
+		.wake_up_src_type = WAKE_UP_SRC_GPIO,
+		.wake_up_data = {
+			.gpio_data.bank_num = 1, /* South Bridge */
+			.gpio_data.gpio_num = 2
+		}
+	},
+	.wake_up_src[2] = {
+		.wake_up_src_type = WAKE_UP_SRC_UART1,
+	}
+};
+
+struct pm_wake_up_src_config *mv_wake_up_src_config_get(void)
+{
+	return &wake_up_src_cfg;
+}
+
diff --git a/plat/marvell/a3700/a3700/mvebu_def.h b/plat/marvell/a3700/a3700/mvebu_def.h
new file mode 100644
index 0000000..c58f06b
--- /dev/null
+++ b/plat/marvell/a3700/a3700/mvebu_def.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef __MVEBU_DEF_H__
+#define __MVEBU_DEF_H__
+
+#include <a3700_plat_def.h>
+
+#endif /* __MVEBU_DEF_H__ */
diff --git a/plat/marvell/a3700/a3700/plat_bl31_setup.c b/plat/marvell/a3700/a3700/plat_bl31_setup.c
new file mode 100644
index 0000000..83db06f
--- /dev/null
+++ b/plat/marvell/a3700/a3700/plat_bl31_setup.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <armada_common.h>
+#include <dram_win.h>
+#include <io_addr_dec.h>
+#include <mmio.h>
+#include <marvell_plat_priv.h>
+#include <plat_marvell.h>
+
+/* This routine does MPP initialization */
+static void marvell_bl31_mpp_init(void)
+{
+	mmio_clrbits_32(MVEBU_NB_GPIO_SEL_REG, 1 << MVEBU_GPIO_TW1_GPIO_EN_OFF);
+
+	/* Set hidden GPIO setting for SPI.
+	 * In north_bridge_pin_out_en_high register 13804,
+	 * bit 28 is the one which enables CS, CLK pins to be
+	 * output, need to set it to 1.
+	 * The initial value of this bit is 1, but in UART boot mode
+	 * initialization, this bit is disabled and the SPI CS and CLK pins
+	 * are used for downloading image purpose; so after downloading,
+	 * we should set this bit to 1 again to enable SPI CS and CLK pins.
+	 * And anyway, this bit value should be 1 in all modes,
+	 * so here we does not judge boot mode and set this bit to 1 always.
+	 */
+	mmio_setbits_32(MVEBU_NB_GPIO_OUTPUT_EN_HIGH_REG,
+			1 << MVEBU_GPIO_NB_SPI_PIN_MODE_OFF);
+}
+
+/* This function overruns the same function in marvell_bl31_setup.c */
+void bl31_plat_arch_setup(void)
+{
+	struct dec_win_config *io_dec_map;
+	uint32_t dec_win_num;
+	struct dram_win_map dram_wins_map;
+
+	marvell_bl31_plat_arch_setup();
+
+	/* MPP init */
+	marvell_bl31_mpp_init();
+
+	/* initialize the timer for delay functionality */
+	plat_delay_timer_init();
+
+	/* CPU address decoder windows initialization. */
+	cpu_wins_init();
+
+	/* fetch CPU-DRAM window mapping information by reading
+	 * CPU-DRAM decode windows (only the enabled ones)
+	 */
+	dram_win_map_build(&dram_wins_map);
+
+	/* Get IO address decoder windows */
+	if (marvell_get_io_dec_win_conf(&io_dec_map, &dec_win_num)) {
+		printf("No IO address decoder windows configurations found!\n");
+		return;
+	}
+
+	/* IO address decoder init */
+	if (init_io_addr_dec(&dram_wins_map, io_dec_map, dec_win_num)) {
+		printf("IO address decoder windows initialization failed!\n");
+		return;
+	}
+}
diff --git a/plat/marvell/a3700/a3700/platform.mk b/plat/marvell/a3700/a3700/platform.mk
new file mode 100644
index 0000000..4f7ac08
--- /dev/null
+++ b/plat/marvell/a3700/a3700/platform.mk
@@ -0,0 +1,10 @@
+#
+# Copyright (C) 2018 Marvell International Ltd.
+#
+# SPDX-License-Identifier:	BSD-3-Clause
+# https://spdx.org/licenses
+#
+
+include plat/marvell/a3700/common/a3700_common.mk
+
+include plat/marvell/common/marvell_common.mk
diff --git a/plat/marvell/a3700/common/a3700_common.mk b/plat/marvell/a3700/common/a3700_common.mk
new file mode 100644
index 0000000..ff96394
--- /dev/null
+++ b/plat/marvell/a3700/common/a3700_common.mk
@@ -0,0 +1,176 @@
+#
+# Copyright (C) 2018 Marvell International Ltd.
+#
+# SPDX-License-Identifier:	BSD-3-Clause
+# https://spdx.org/licenses
+#
+
+MARVELL_PLAT_BASE		:= plat/marvell
+MARVELL_PLAT_INCLUDE_BASE	:= include/plat/marvell
+PLAT_FAMILY			:= a3700
+PLAT_FAMILY_BASE		:= $(MARVELL_PLAT_BASE)/$(PLAT_FAMILY)
+PLAT_INCLUDE_BASE		:= $(MARVELL_PLAT_INCLUDE_BASE)/$(PLAT_FAMILY)
+PLAT_COMMON_BASE		:= $(PLAT_FAMILY_BASE)/common
+MARVELL_DRV_BASE		:= drivers/marvell
+MARVELL_COMMON_BASE		:= $(MARVELL_PLAT_BASE)/common
+
+include $(MARVELL_PLAT_BASE)/marvell.mk
+
+#*********** A3700 *************
+DOIMAGEPATH	:= $(WTP)
+DOIMAGETOOL	:= $(DOIMAGEPATH)/wtptp/linux/tbb_linux
+
+ifeq ($(MARVELL_SECURE_BOOT),1)
+DOIMAGE_CFG	:= $(DOIMAGEPATH)/atf-tim.txt
+IMAGESPATH	:= $(DOIMAGEPATH)/tim/trusted
+
+TIMNCFG		:= $(DOIMAGEPATH)/atf-timN.txt
+TIMNSIG		:= $(IMAGESPATH)/timnsign.txt
+TIM2IMGARGS	:= -i $(DOIMAGE_CFG) -n $(TIMNCFG)
+TIMN_IMAGE	:= $$(grep "Image Filename:" -m 1 $(TIMNCFG) | cut -c 17-)
+else #MARVELL_SECURE_BOOT
+DOIMAGE_CFG	:= $(DOIMAGEPATH)/atf-ntim.txt
+IMAGESPATH	:= $(DOIMAGEPATH)/tim/untrusted
+TIM2IMGARGS	:= -i $(DOIMAGE_CFG)
+endif #MARVELL_SECURE_BOOT
+
+TIMBUILD	:= $(DOIMAGEPATH)/script/buildtim.sh
+TIM2IMG		:= $(DOIMAGEPATH)/script/tim2img.pl
+
+# WTMI_IMG is used to specify the customized RTOS image running over
+# Service CPU (CM3 processor). By the default, it points to a
+# baremetal binary of fuse programming in A3700_utils.
+WTMI_IMG	:= $(DOIMAGEPATH)/wtmi/fuse/build/fuse.bin
+
+# WTMI_SYSINIT_IMG is used for the system early initialization,
+# such as AVS settings, clock-tree setup and dynamic DDR PHY training.
+# After the initialization is done, this image will be wiped out
+# from the memory and CM3 will continue with RTOS image or other application.
+WTMI_SYSINIT_IMG	:= $(DOIMAGEPATH)/wtmi/sys_init/build/sys_init.bin
+
+# WTMI_MULTI_IMG is composed of CM3 RTOS image (WTMI_IMG)
+# and sys-init image (WTMI_SYSINIT_IMG).
+WTMI_MULTI_IMG		:= $(DOIMAGEPATH)/wtmi/build/wtmi.bin
+
+WTMI_ENC_IMG		:= $(DOIMAGEPATH)/wtmi/build/wtmi-enc.bin
+BUILD_UART		:= uart-images
+
+SRCPATH			:= $(dir $(BL33))
+
+CLOCKSPRESET		?= CPU_800_DDR_800
+
+DDR_TOPOLOGY		?= 0
+
+BOOTDEV			?= SPINOR
+PARTNUM			?= 0
+
+TIM_IMAGE		:= $$(grep "Image Filename:" -m 1 $(DOIMAGE_CFG) | cut -c 17-)
+TIMBLDARGS		:= $(MARVELL_SECURE_BOOT) $(BOOTDEV) $(IMAGESPATH) $(DOIMAGEPATH) $(CLOCKSPRESET) \
+				$(DDR_TOPOLOGY) $(PARTNUM) $(DEBUG) $(DOIMAGE_CFG) $(TIMNCFG) $(TIMNSIG) 1
+TIMBLDUARTARGS		:= $(MARVELL_SECURE_BOOT) UART $(IMAGESPATH) $(DOIMAGEPATH) $(CLOCKSPRESET) \
+				$(DDR_TOPOLOGY) 0 0 $(DOIMAGE_CFG) $(TIMNCFG) $(TIMNSIG) 0
+DOIMAGE_FLAGS		:= -r $(DOIMAGE_CFG) -v -D
+
+
+# GICV3
+$(eval $(call add_define,CONFIG_GICV3))
+
+# CCI-400
+$(eval $(call add_define,USE_CCI))
+
+MARVELL_GIC_SOURCES	:=	drivers/arm/gic/common/gic_common.c	\
+				drivers/arm/gic/v3/gicv3_main.c		\
+				drivers/arm/gic/v3/gicv3_helpers.c	\
+				drivers/arm/gic/v3/arm_gicv3_common.c	\
+				plat/common/plat_gicv3.c		\
+				drivers/arm/gic/v3/gic500.c
+
+ATF_INCLUDES		:=	-Iinclude/common/tbbr		\
+				-Iinclude/drivers
+
+PLAT_INCLUDES		:=	-I$(PLAT_FAMILY_BASE)/$(PLAT)		\
+				-I$(PLAT_COMMON_BASE)/include		\
+				-I$(PLAT_INCLUDE_BASE)/common		\
+				-I$(MARVELL_DRV_BASE)/uart		\
+				-I$(MARVELL_DRV_BASE)			\
+				-I$/drivers/arm/gic/common/		\
+				$(ATF_INCLUDES)
+
+PLAT_BL_COMMON_SOURCES	:=	$(PLAT_COMMON_BASE)/aarch64/a3700_common.c \
+				drivers/console/aarch64/console.S	   \
+				$(MARVELL_COMMON_BASE)/marvell_cci.c	   \
+				$(MARVELL_DRV_BASE)/uart/a3700_console.S
+
+BL1_SOURCES		+=	$(PLAT_COMMON_BASE)/aarch64/plat_helpers.S \
+				lib/cpus/aarch64/cortex_a53.S
+
+BL31_PORTING_SOURCES	:=	$(PLAT_FAMILY_BASE)/$(PLAT)/board/pm_src.c
+
+MARVELL_DRV		:=	$(MARVELL_DRV_BASE)/comphy/phy-comphy-3700.c
+
+BL31_SOURCES		+=	lib/cpus/aarch64/cortex_a53.S		\
+				$(PLAT_COMMON_BASE)/aarch64/plat_helpers.S \
+				$(PLAT_COMMON_BASE)/plat_pm.c		\
+				$(PLAT_COMMON_BASE)/dram_win.c		\
+				$(PLAT_COMMON_BASE)/io_addr_dec.c	\
+				$(PLAT_COMMON_BASE)/marvell_plat_config.c     \
+				$(PLAT_FAMILY_BASE)/$(PLAT)/plat_bl31_setup.c \
+				$(MARVELL_COMMON_BASE)/marvell_ddr_info.c	\
+				$(MARVELL_COMMON_BASE)/marvell_gicv3.c	\
+				$(MARVELL_GIC_SOURCES)			\
+				drivers/arm/cci/cci.c			\
+				$(BL31_PORTING_SOURCES)			\
+				$(PLAT_COMMON_BASE)/a3700_sip_svc.c	\
+				$(MARVELL_DRV)
+
+mrvl_flash: ${BUILD_PLAT}/${FIP_NAME} ${DOIMAGETOOL}
+	$(shell truncate -s %128K ${BUILD_PLAT}/bl1.bin)
+	$(shell cat ${BUILD_PLAT}/bl1.bin ${BUILD_PLAT}/${FIP_NAME} > ${BUILD_PLAT}/${BOOT_IMAGE})
+	$(shell truncate -s %4 ${BUILD_PLAT}/${BOOT_IMAGE})
+	$(shell truncate -s %4 $(WTMI_IMG))
+	@echo
+	@echo "Building uart images"
+	$(TIMBUILD) $(TIMBLDUARTARGS)
+	@sed -i 's|WTMI_IMG|$(WTMI_MULTI_IMG)|1' $(DOIMAGE_CFG)
+	@sed -i 's|BOOT_IMAGE|$(BUILD_PLAT)/$(BOOT_IMAGE)|1' $(DOIMAGE_CFG)
+ifeq ($(MARVELL_SECURE_BOOT),1)
+	@sed -i 's|WTMI_IMG|$(WTMI_MULTI_IMG)|1' $(TIMNCFG)
+	@sed -i 's|BOOT_IMAGE|$(BUILD_PLAT)/$(BOOT_IMAGE)|1' $(TIMNCFG)
+endif
+	$(DOIMAGETOOL) $(DOIMAGE_FLAGS)
+	@if [ -e "$(TIMNCFG)" ]; then $(DOIMAGETOOL) -r $(TIMNCFG); fi
+	@rm -rf $(BUILD_PLAT)/$(BUILD_UART)*
+	@mkdir $(BUILD_PLAT)/$(BUILD_UART)
+	@mv -t $(BUILD_PLAT)/$(BUILD_UART) $(TIM_IMAGE) $(DOIMAGE_CFG) $(TIMN_IMAGE) $(TIMNCFG)
+	@find . -name "*_h.*" |xargs cp -ut $(BUILD_PLAT)/$(BUILD_UART)
+	@mv $(subst .bin,_h.bin,$(WTMI_MULTI_IMG)) $(BUILD_PLAT)/$(BUILD_UART)/wtmi_h.bin
+	@tar czf $(BUILD_PLAT)/$(BUILD_UART).tgz -C $(BUILD_PLAT) ./$(BUILD_UART)
+	@echo
+	@echo "Building flash image"
+	$(TIMBUILD) $(TIMBLDARGS)
+	sed -i 's|WTMI_IMG|$(WTMI_MULTI_IMG)|1' $(DOIMAGE_CFG)
+	sed -i 's|BOOT_IMAGE|$(BUILD_PLAT)/$(BOOT_IMAGE)|1' $(DOIMAGE_CFG)
+ifeq ($(MARVELL_SECURE_BOOT),1)
+	@sed -i 's|WTMI_IMG|$(WTMI_MULTI_IMG)|1' $(TIMNCFG)
+	@sed -i 's|BOOT_IMAGE|$(BUILD_PLAT)/$(BOOT_IMAGE)|1' $(TIMNCFG)
+	@echo -e "\n\t=======================================================\n";
+	@echo -e "\t  Secure boot. Encrypting wtmi and boot-image \n";
+	@echo -e "\t=======================================================\n";
+	@truncate -s %16 $(WTMI_MULTI_IMG)
+	@openssl enc -aes-256-cbc -e -in $(WTMI_MULTI_IMG) \
+	-out $(WTMI_ENC_IMG) \
+	-K `cat $(IMAGESPATH)/aes-256.txt` -k 0 -nosalt \
+	-iv `cat $(IMAGESPATH)/iv.txt` -p
+	@truncate -s %16 $(BUILD_PLAT)/$(BOOT_IMAGE);
+	@openssl enc -aes-256-cbc -e -in $(BUILD_PLAT)/$(BOOT_IMAGE) \
+	-out $(BUILD_PLAT)/$(BOOT_ENC_IMAGE) \
+	-K `cat $(IMAGESPATH)/aes-256.txt` -k 0 -nosalt \
+	-iv `cat $(IMAGESPATH)/iv.txt` -p
+endif
+	$(DOIMAGETOOL) $(DOIMAGE_FLAGS)
+	@if [ -e "$(TIMNCFG)" ]; then $(DOIMAGETOOL) -r $(TIMNCFG); fi
+	@if [ "$(MARVELL_SECURE_BOOT)" = "1" ]; then sed -i 's|$(WTMI_MULTI_IMG)|$(WTMI_ENC_IMG)|1;s|$(BOOT_IMAGE)|$(BOOT_ENC_IMAGE)|1;' $(TIMNCFG); fi
+	$(TIM2IMG) $(TIM2IMGARGS) -o $(BUILD_PLAT)/$(FLASH_IMAGE)
+	@mv -t $(BUILD_PLAT) $(TIM_IMAGE) $(DOIMAGE_CFG) $(TIMN_IMAGE) $(TIMNCFG) $(WTMI_IMG) $(WTMI_SYSINIT_IMG) $(WTMI_MULTI_IMG)
+	@if [ "$(MARVELL_SECURE_BOOT)" = "1" ]; then mv -t $(BUILD_PLAT) $(WTMI_ENC_IMG) OtpHash.txt; fi
+	@find . -name "*.txt" | grep -E "CSK[[:alnum:]]_KeyHash.txt|Tim_msg.txt|TIMHash.txt" | xargs rm -f
diff --git a/plat/marvell/a3700/common/a3700_sip_svc.c b/plat/marvell/a3700/common/a3700_sip_svc.c
new file mode 100644
index 0000000..88d1fc2
--- /dev/null
+++ b/plat/marvell/a3700/common/a3700_sip_svc.c
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <debug.h>
+#include <marvell_plat_priv.h>
+#include <plat_marvell.h>
+#include <runtime_svc.h>
+#include <smccc.h>
+#include "comphy/phy-comphy-3700.h"
+
+/* Comphy related FID's */
+#define MV_SIP_COMPHY_POWER_ON	0x82000001
+#define MV_SIP_COMPHY_POWER_OFF	0x82000002
+#define MV_SIP_COMPHY_PLL_LOCK	0x82000003
+
+/* Miscellaneous FID's' */
+#define MV_SIP_DRAM_SIZE	0x82000010
+
+/* This macro is used to identify COMPHY related calls from SMC function ID */
+#define is_comphy_fid(fid)	\
+	((fid) >= MV_SIP_COMPHY_POWER_ON && (fid) <= MV_SIP_COMPHY_PLL_LOCK)
+
+uintptr_t mrvl_sip_smc_handler(uint32_t smc_fid,
+			       u_register_t x1,
+			       u_register_t x2,
+			       u_register_t x3,
+			       u_register_t x4,
+			       void *cookie,
+			       void *handle,
+			       u_register_t flags)
+{
+	u_register_t ret;
+
+	VERBOSE("%s: got SMC (0x%x) x1 0x%lx, x2 0x%lx\n",
+		__func__, smc_fid, x1, x2);
+	if (is_comphy_fid(smc_fid)) {
+		if (x1 >= MAX_LANE_NR) {
+			ERROR("%s: Wrong smc (0x%x) lane nr: %lx\n",
+			      __func__, smc_fid, x2);
+			SMC_RET1(handle, SMC_UNK);
+		}
+	}
+
+	switch (smc_fid) {
+	/* Comphy related FID's */
+	case MV_SIP_COMPHY_POWER_ON:
+		/* x1: comphy_index, x2: comphy_mode */
+		ret = mvebu_3700_comphy_power_on(x1, x2);
+		SMC_RET1(handle, ret);
+	case MV_SIP_COMPHY_POWER_OFF:
+		/* x1: comphy_index, x2: comphy_mode */
+		ret = mvebu_3700_comphy_power_off(x1, x2);
+		SMC_RET1(handle, ret);
+	case MV_SIP_COMPHY_PLL_LOCK:
+		/* x1: comphy_index, x2: comphy_mode */
+		ret = mvebu_3700_comphy_is_pll_locked(x1, x2);
+		SMC_RET1(handle, ret);
+	/* Miscellaneous FID's' */
+	case MV_SIP_DRAM_SIZE:
+		/* x1:  ap_base_addr */
+		ret = mvebu_get_dram_size(MVEBU_REGS_BASE);
+		SMC_RET1(handle, ret);
+
+	default:
+		ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
+		SMC_RET1(handle, SMC_UNK);
+	}
+}
+
+/* Define a runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	marvell_sip_svc,
+	OEN_SIP_START,
+	OEN_SIP_END,
+	SMC_TYPE_FAST,
+	NULL,
+	mrvl_sip_smc_handler
+);
diff --git a/plat/marvell/a3700/common/aarch64/a3700_common.c b/plat/marvell/a3700/common/aarch64/a3700_common.c
new file mode 100644
index 0000000..6351285
--- /dev/null
+++ b/plat/marvell/a3700/common/aarch64/a3700_common.c
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+#include <plat_marvell.h>
+
+/* MMU entry for internal (register) space access */
+#define MAP_DEVICE0	MAP_REGION_FLAT(DEVICE0_BASE,			\
+					DEVICE0_SIZE,			\
+					MT_DEVICE | MT_RW | MT_SECURE)
+
+/*
+ * Table of regions for various BL stages to map using the MMU.
+ */
+#if IMAGE_BL1
+const mmap_region_t plat_marvell_mmap[] = {
+	MARVELL_MAP_SHARED_RAM,
+	MAP_DEVICE0,
+	{0}
+};
+#endif
+#if IMAGE_BL2
+const mmap_region_t plat_marvell_mmap[] = {
+	MARVELL_MAP_SHARED_RAM,
+	MAP_DEVICE0,
+	MARVELL_MAP_DRAM,
+	{0}
+};
+#endif
+#if IMAGE_BL2U
+const mmap_region_t plat_marvell_mmap[] = {
+	MAP_DEVICE0,
+	{0}
+};
+#endif
+#if IMAGE_BL31
+const mmap_region_t plat_marvell_mmap[] = {
+	MARVELL_MAP_SHARED_RAM,
+	MAP_DEVICE0,
+	MARVELL_MAP_DRAM,
+	{0}
+};
+#endif
+#if IMAGE_BL32
+const mmap_region_t plat_marvell_mmap[] = {
+	MAP_DEVICE0,
+	{0}
+};
+#endif
+
+MARVELL_CASSERT_MMAP;
diff --git a/plat/marvell/a3700/common/aarch64/plat_helpers.S b/plat/marvell/a3700/common/aarch64/plat_helpers.S
new file mode 100644
index 0000000..90d76f0
--- /dev/null
+++ b/plat/marvell/a3700/common/aarch64/plat_helpers.S
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <asm_macros.S>
+#include <platform_def.h>
+
+	.globl	plat_secondary_cold_boot_setup
+	.globl	plat_get_my_entrypoint
+	.globl	plat_is_my_cpu_primary
+
+	/* -----------------------------------------------------
+	 * void plat_secondary_cold_boot_setup (void);
+	 *
+	 * This function performs any platform specific actions
+	 * needed for a secondary cpu after a cold reset. Right
+	 * now this is a stub function.
+	 * -----------------------------------------------------
+	 */
+func plat_secondary_cold_boot_setup
+	mov	x0, #0
+	ret
+endfunc plat_secondary_cold_boot_setup
+
+	/* ---------------------------------------------------------------------
+	 * unsigned long plat_get_my_entrypoint (void);
+	 *
+	 * Main job of this routine is to distinguish between cold and warm boot
+	 * For a cold boot, return 0.
+	 * For a warm boot, read the mailbox and return the address it contains.
+	 * A magic number is placed before entrypoint to avoid mistake caused by
+	 * uninitialized mailbox data area.
+	 * ---------------------------------------------------------------------
+	 */
+func plat_get_my_entrypoint
+	/* Read first word and compare it with magic num */
+	mov_imm x0, PLAT_MARVELL_MAILBOX_BASE
+	ldr	x1, [x0]
+	mov_imm x2, PLAT_MARVELL_MAILBOX_MAGIC_NUM
+	cmp	x1, x2
+	/* If compare failed, return 0, i.e. cold boot */
+	beq	entrypoint
+	mov	x0, #0
+	ret
+entrypoint:
+	/* Second word contains the jump address */
+	add	x0, x0, #8
+	ldr	x0, [x0]
+	ret
+endfunc plat_get_my_entrypoint
+
+	/* -----------------------------------------------------
+	 * unsigned int plat_is_my_cpu_primary (void);
+	 *
+	 * Find out whether the current cpu is the primary
+	 * cpu.
+	 * -----------------------------------------------------
+	 */
+func plat_is_my_cpu_primary
+	mrs	x0, mpidr_el1
+	and	x0, x0, #(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)
+	cmp	x0, #MVEBU_PRIMARY_CPU
+	cset	w0, eq
+	ret
+endfunc plat_is_my_cpu_primary
diff --git a/plat/marvell/a3700/common/dram_win.c b/plat/marvell/a3700/common/dram_win.c
new file mode 100644
index 0000000..fb236d8
--- /dev/null
+++ b/plat/marvell/a3700/common/dram_win.c
@@ -0,0 +1,276 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <dram_win.h>
+#include <marvell_plat_priv.h>
+#include <mmio.h>
+#include <mvebu.h>
+#include <plat_marvell.h>
+#include <string.h>
+
+/* Armada 3700 has 5 configurable windows */
+#define MV_CPU_WIN_NUM		5
+
+#define CPU_WIN_DISABLED	0
+#define CPU_WIN_ENABLED		1
+
+/*
+ * There are 2 different cpu decode window configuration cases:
+ * - DRAM size is not over 2GB;
+ * - DRAM size is 4GB.
+ */
+enum cpu_win_config_num {
+	CPU_WIN_CONFIG_DRAM_NOT_OVER_2GB = 0,
+	CPU_WIN_CONFIG_DRAM_4GB,
+	CPU_WIN_CONFIG_MAX
+};
+
+enum cpu_win_target {
+	CPU_WIN_TARGET_DRAM = 0,
+	CPU_WIN_TARGET_INTERNAL_REG,
+	CPU_WIN_TARGET_PCIE,
+	CPU_WIN_TARGET_PCIE_OVER_MCI,
+	CPU_WIN_TARGET_BOOT_ROM,
+	CPU_WIN_TARGET_MCI_EXTERNAL,
+	CPU_WIN_TARGET_RWTM_RAM = 7,
+	CPU_WIN_TARGET_CCI400_REG
+};
+
+struct cpu_win_configuration {
+	uint32_t		enabled;
+	enum cpu_win_target	target;
+	uint64_t		base_addr;
+	uint64_t		size;
+	uint64_t		remap_addr;
+};
+
+struct cpu_win_configuration mv_cpu_wins[CPU_WIN_CONFIG_MAX][MV_CPU_WIN_NUM] = {
+	/*
+	 * When total dram size is not over 2GB:
+	 * DDR window 0 is configured in tim header, its size may be not 512MB,
+	 * but the actual dram size, no need to configure it again;
+	 * other cpu windows are kept as default.
+	 */
+	{
+		/* enabled
+		 *	target
+		 *		base
+		 *			size
+		 *				remap
+		 */
+		{CPU_WIN_ENABLED,
+			CPU_WIN_TARGET_DRAM,
+				0x0,
+					0x08000000,
+						0x0},
+		{CPU_WIN_ENABLED,
+			CPU_WIN_TARGET_MCI_EXTERNAL,
+				0xe0000000,
+					0x08000000,
+						0xe0000000},
+		{CPU_WIN_ENABLED,
+			CPU_WIN_TARGET_PCIE,
+				0xe8000000,
+					0x08000000,
+						0xe8000000},
+		{CPU_WIN_ENABLED,
+			CPU_WIN_TARGET_RWTM_RAM,
+				0xf0000000,
+					0x00020000,
+						0x1fff0000},
+		{CPU_WIN_ENABLED,
+			CPU_WIN_TARGET_PCIE_OVER_MCI,
+				0x80000000,
+					0x10000000,
+						0x80000000},
+	},
+
+	/*
+	 * If total dram size is more than 2GB, now there is only one case - 4GB
+	 *  dram; we will use below cpu windows configurations:
+	 *  - Internal Regs, CCI-400, Boot Rom and PCIe windows are kept as
+	 *    default;
+	 *  - Use 4 CPU decode windows for DRAM, which cover 3.375GB DRAM;
+	 *    DDR window 0 is configured in tim header with 2GB size, no need to
+	 *    configure it again here;
+	 *
+	 *	0xFFFFFFFF ---> |-----------------------|
+	 *			|	  Boot ROM	| 64KB
+	 *	0xFFF00000 ---> +-----------------------+
+	 *			:			:
+	 *	0xF0000000 ---> |-----------------------|
+	 *			|	  PCIE		| 128 MB
+	 *	0xE8000000 ---> |-----------------------|
+	 *			|	  DDR window 3	| 128 MB
+	 *	0xE0000000 ---> +-----------------------+
+	 *			:			:
+	 *	0xD8010000 ---> |-----------------------|
+	 *			|	  CCI Regs	| 64 KB
+	 *	0xD8000000 ---> +-----------------------+
+	 *			:			:
+	 *			:			:
+	 *	0xD2000000 ---> +-----------------------+
+	 *			|	 Internal Regs	| 32MB
+	 *	0xD0000000 ---> |-----------------------|
+	 *			 |	  DDR window 2	| 256 MB
+	 *	0xC0000000 ---> |-----------------------|
+	 *			|			|
+	 *			|	 DDR window 1	| 1 GB
+	 *			|			|
+	 *	0x80000000 ---> |-----------------------|
+	 *			|			|
+	 *			|			|
+	 *			|	 DDR window 0	| 2 GB
+	 *			|			|
+	 *			|			|
+	 *	0x00000000 ---> +-----------------------+
+	 */
+	{
+		/* win_id
+		 *	target
+		 *		base
+		 *			size
+		 *				remap
+		 */
+		{CPU_WIN_ENABLED,
+			CPU_WIN_TARGET_DRAM,
+				0x0,
+					0x80000000,
+						0x0},
+		{CPU_WIN_ENABLED,
+			CPU_WIN_TARGET_DRAM,
+				0x80000000,
+					0x40000000,
+						0x80000000},
+		{CPU_WIN_ENABLED,
+			CPU_WIN_TARGET_DRAM,
+				0xc0000000,
+					0x10000000,
+						0xc0000000},
+		{CPU_WIN_ENABLED,
+			CPU_WIN_TARGET_DRAM,
+				0xe0000000,
+					0x08000000,
+						0xe0000000},
+		{CPU_WIN_ENABLED,
+			CPU_WIN_TARGET_PCIE,
+				0xe8000000,
+					0x08000000,
+						0xe8000000},
+	},
+};
+
+/*
+ * dram_win_map_build
+ *
+ * This function builds cpu dram windows mapping
+ * which includes base address and window size by
+ * reading cpu dram decode windows registers.
+ *
+ * @input: N/A
+ *
+ * @output:
+ *     - win_map: cpu dram windows mapping
+ *
+ * @return:  N/A
+ */
+void dram_win_map_build(struct dram_win_map *win_map)
+{
+	int32_t win_id;
+	struct dram_win *win;
+	uint32_t base_reg, ctrl_reg, size_reg, enabled, target;
+
+	memset(win_map, 0, sizeof(struct dram_win_map));
+	for (win_id = 0; win_id < DRAM_WIN_MAP_NUM_MAX; win_id++) {
+		ctrl_reg = mmio_read_32(CPU_DEC_WIN_CTRL_REG(win_id));
+		target = (ctrl_reg & CPU_DEC_CR_WIN_TARGET_MASK) >>
+			  CPU_DEC_CR_WIN_TARGET_OFFS;
+		enabled = ctrl_reg & CPU_DEC_CR_WIN_ENABLE;
+		/* Ignore invalid and non-dram windows*/
+		if ((enabled == 0) || (target != DRAM_CPU_DEC_TARGET_NUM))
+			continue;
+
+		win = win_map->dram_windows + win_map->dram_win_num;
+		base_reg = mmio_read_32(CPU_DEC_WIN_BASE_REG(win_id));
+		size_reg = mmio_read_32(CPU_DEC_WIN_SIZE_REG(win_id));
+		/* Base reg [15:0] corresponds to transaction address [39:16] */
+		win->base_addr = (base_reg & CPU_DEC_BR_BASE_MASK) >>
+				  CPU_DEC_BR_BASE_OFFS;
+		win->base_addr *= CPU_DEC_CR_WIN_SIZE_ALIGNMENT;
+		/*
+		 * Size reg [15:0] is programmed from LSB to MSB as a sequence
+		 * of 1s followed by a sequence of 0s and the number of 1s
+		 * specifies the size of the window in 64 KB granularity,
+		 * for example, a value of 00FFh specifies 256 x 64 KB = 16 MB
+		 */
+		win->win_size = (size_reg & CPU_DEC_CR_WIN_SIZE_MASK) >>
+				 CPU_DEC_CR_WIN_SIZE_OFFS;
+		win->win_size = (win->win_size + 1) *
+				 CPU_DEC_CR_WIN_SIZE_ALIGNMENT;
+
+		win_map->dram_win_num++;
+	}
+}
+
+static void cpu_win_set(uint32_t win_id, struct cpu_win_configuration *win_cfg)
+{
+	uint32_t base_reg, ctrl_reg, size_reg, remap_reg;
+
+	/* Disable window */
+	ctrl_reg = mmio_read_32(CPU_DEC_WIN_CTRL_REG(win_id));
+	ctrl_reg &= ~CPU_DEC_CR_WIN_ENABLE;
+	mmio_write_32(CPU_DEC_WIN_CTRL_REG(win_id), ctrl_reg);
+
+	/* For an disabled window, only disable it. */
+	if (!win_cfg->enabled)
+		return;
+
+	/* Set Base Register */
+	base_reg = (uint32_t)(win_cfg->base_addr /
+		   CPU_DEC_CR_WIN_SIZE_ALIGNMENT);
+	base_reg <<= CPU_DEC_BR_BASE_OFFS;
+	base_reg &= CPU_DEC_BR_BASE_MASK;
+	mmio_write_32(CPU_DEC_WIN_BASE_REG(win_id), base_reg);
+
+	/* Set Remap Register with the same value
+	 * as the <Base> field in Base Register
+	 */
+	remap_reg = (uint32_t)(win_cfg->remap_addr /
+		    CPU_DEC_CR_WIN_SIZE_ALIGNMENT);
+	remap_reg <<= CPU_DEC_RLR_REMAP_LOW_OFFS;
+	remap_reg &= CPU_DEC_RLR_REMAP_LOW_MASK;
+	mmio_write_32(CPU_DEC_REMAP_LOW_REG(win_id), remap_reg);
+
+	/* Set Size Register */
+	size_reg = (win_cfg->size / CPU_DEC_CR_WIN_SIZE_ALIGNMENT) - 1;
+	size_reg <<= CPU_DEC_CR_WIN_SIZE_OFFS;
+	size_reg &= CPU_DEC_CR_WIN_SIZE_MASK;
+	mmio_write_32(CPU_DEC_WIN_SIZE_REG(win_id), size_reg);
+
+	/* Set Control Register - set target id and enable window */
+	ctrl_reg &= ~CPU_DEC_CR_WIN_TARGET_MASK;
+	ctrl_reg |= (win_cfg->target << CPU_DEC_CR_WIN_TARGET_OFFS);
+	ctrl_reg |= CPU_DEC_CR_WIN_ENABLE;
+	mmio_write_32(CPU_DEC_WIN_CTRL_REG(win_id), ctrl_reg);
+}
+
+void cpu_wins_init(void)
+{
+	uint32_t cfg_idx, win_id;
+
+	if (mvebu_get_dram_size(MVEBU_REGS_BASE) <= _2GB_)
+		cfg_idx = CPU_WIN_CONFIG_DRAM_NOT_OVER_2GB;
+	else
+		cfg_idx = CPU_WIN_CONFIG_DRAM_4GB;
+
+	/* Window 0 is configured always for DRAM in tim header
+	 * already, no need to configure it again here
+	 */
+	for (win_id = 1; win_id < MV_CPU_WIN_NUM; win_id++)
+		cpu_win_set(win_id, &mv_cpu_wins[cfg_idx][win_id]);
+}
+
diff --git a/plat/marvell/a3700/common/include/a3700_plat_def.h b/plat/marvell/a3700/common/include/a3700_plat_def.h
new file mode 100644
index 0000000..6ae57b3
--- /dev/null
+++ b/plat/marvell/a3700/common/include/a3700_plat_def.h
@@ -0,0 +1,122 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef __A3700_PLAT_DEF_H__
+#define __A3700_PLAT_DEF_H__
+
+#include <marvell_def.h>
+
+
+#define MVEBU_MAX_CPUS_PER_CLUSTER	2
+
+#define MVEBU_PRIMARY_CPU		0x0
+
+/*
+ * The counter on A3700 is always fed from reference 25M clock (XTAL).
+ * However minimal CPU counter prescaler is 2, so the counter
+ * frequency will be divided by 2, the number is 12.5M
+ */
+#define COUNTER_FREQUENCY		12500000
+
+#define MVEBU_REGS_BASE			0xD0000000
+
+/*****************************************************************************
+ * MVEBU memory map related constants
+ *****************************************************************************
+ */
+/* Aggregate of all devices in the first GB */
+#define DEVICE0_BASE			MVEBU_REGS_BASE
+#define DEVICE0_SIZE			0x10000000
+
+/*****************************************************************************
+ * GIC-500 & interrupt handling related constants
+ *****************************************************************************
+ */
+/* Base MVEBU compatible GIC memory map */
+#define MVEBU_GICD_BASE			0x1D00000
+#define MVEBU_GICR_BASE			0x1D40000
+#define MVEBU_GICC_BASE			0x1D80000
+
+/* CCI-400 */
+#define MVEBU_CCI_BASE			0x8000000
+
+/*****************************************************************************
+ * North and south bridge register base
+ *****************************************************************************
+ */
+#define MVEBU_NB_REGS_BASE			(MVEBU_REGS_BASE + 0x13000)
+#define MVEBU_SB_REGS_BASE			(MVEBU_REGS_BASE + 0x18000)
+
+/*****************************************************************************
+ * GPIO registers related constants
+ *****************************************************************************
+ */
+/* North and south bridge GPIO register base address */
+#define MVEBU_NB_GPIO_REG_BASE			(MVEBU_NB_REGS_BASE + 0x800)
+#define MVEBU_NB_GPIO_IRQ_REG_BASE		(MVEBU_NB_REGS_BASE + 0xC00)
+#define MVEBU_SB_GPIO_REG_BASE			(MVEBU_SB_REGS_BASE + 0x800)
+#define MVEBU_SB_GPIO_IRQ_REG_BASE		(MVEBU_SB_REGS_BASE + 0xC00)
+#define MVEBU_NB_SB_IRQ_REG_BASE		(MVEBU_REGS_BASE + 0x8A00)
+
+/* North Bridge GPIO selection register */
+#define MVEBU_NB_GPIO_SEL_REG			(MVEBU_NB_GPIO_REG_BASE + 0x30)
+#define MVEBU_NB_GPIO_OUTPUT_EN_HIGH_REG	(MVEBU_NB_GPIO_REG_BASE + 0x04)
+/* I2C1 GPIO Enable bit offset */
+#define MVEBU_GPIO_TW1_GPIO_EN_OFF		(10)
+/* SPI pins mode bit offset */
+#define MVEBU_GPIO_NB_SPI_PIN_MODE_OFF		(28)
+
+/*****************************************************************************
+ * DRAM registers related constants
+ *****************************************************************************
+ */
+#define MVEBU_DRAM_REG_BASE			(MVEBU_REGS_BASE)
+
+/*****************************************************************************
+ * SB wake-up registers related constants
+ *****************************************************************************
+ */
+#define MVEBU_SB_WAKEUP_REG_BASE		(MVEBU_REGS_BASE + 0x19000)
+
+/*****************************************************************************
+ * PMSU registers related constants
+ *****************************************************************************
+ */
+#define MVEBU_PMSU_REG_BASE			(MVEBU_REGS_BASE + 0x14000)
+
+/*****************************************************************************
+ * North Bridge Step-Down Registers
+ *****************************************************************************
+ */
+#define MVEBU_NB_STEP_DOWN_REG_BASE		(MVEBU_REGS_BASE + 0x12800)
+
+/*****************************************************************************
+ * DRAM CS memory map register base
+ *****************************************************************************
+ */
+#define MVEBU_CS_MMAP_REG_BASE			(MVEBU_REGS_BASE + 0x200)
+
+/*****************************************************************************
+ * CPU decoder window registers related constants
+ *****************************************************************************
+ */
+#define MVEBU_CPU_DEC_WIN_REG_BASE		(MVEBU_REGS_BASE + 0xCF00)
+
+/*****************************************************************************
+ * AVS registers related constants
+ *****************************************************************************
+ */
+#define MVEBU_AVS_REG_BASE			(MVEBU_REGS_BASE + 0x11500)
+
+
+/*****************************************************************************
+ * AVS registers related constants
+ *****************************************************************************
+ */
+#define MVEBU_COMPHY_REG_BASE			(MVEBU_REGS_BASE + 0x18300)
+
+#endif /* __A3700_PLAT_DEF_H__ */
diff --git a/plat/marvell/a3700/common/include/a3700_pm.h b/plat/marvell/a3700/common/include/a3700_pm.h
new file mode 100644
index 0000000..bd8792a
--- /dev/null
+++ b/plat/marvell/a3700/common/include/a3700_pm.h
@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2016 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef __A3700_PM_H__
+#define __A3700_PM_H__
+
+#include <stdint.h>
+
+/* supported wake up sources */
+enum pm_wake_up_src_type {
+	WAKE_UP_SRC_GPIO,
+	/* FOLLOWING SRC NOT SUPPORTED YET */
+	WAKE_UP_SRC_TIMER,
+	WAKE_UP_SRC_UART0,
+	WAKE_UP_SRC_UART1,
+	WAKE_UP_SRC_MAX,
+};
+
+struct pm_gpio_data {
+	/*
+	 * bank 0: North bridge GPIO
+	 * bank 1: South bridge GPIO
+	 */
+	uint32_t bank_num;
+	uint32_t gpio_num;
+};
+
+union pm_wake_up_src_data {
+	struct pm_gpio_data gpio_data;
+	/* delay in seconds */
+	uint32_t timer_delay;
+};
+
+struct pm_wake_up_src {
+	enum pm_wake_up_src_type wake_up_src_type;
+
+	union pm_wake_up_src_data wake_up_data;
+};
+
+struct pm_wake_up_src_config {
+	uint32_t	wake_up_src_num;
+	struct pm_wake_up_src wake_up_src[WAKE_UP_SRC_MAX];
+};
+
+struct pm_wake_up_src_config *mv_wake_up_src_config_get(void);
+
+
+#endif /* __A3700_PM_H__ */
diff --git a/plat/marvell/a3700/common/include/ddr_info.h b/plat/marvell/a3700/common/include/ddr_info.h
new file mode 100644
index 0000000..1a9230d
--- /dev/null
+++ b/plat/marvell/a3700/common/include/ddr_info.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef _DDR_INFO_H_
+#define _DDR_INFO_H_
+
+#define DRAM_MAX_IFACE			1
+#define DRAM_CH0_MMAP_LOW_OFFSET	0x200
+
+#endif /* _DDR_INFO_H_ */
diff --git a/plat/marvell/a3700/common/include/dram_win.h b/plat/marvell/a3700/common/include/dram_win.h
new file mode 100644
index 0000000..7ff20be
--- /dev/null
+++ b/plat/marvell/a3700/common/include/dram_win.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef _DRAM_WIN_H_
+#define _DRAM_WIN_H_
+
+#include <bl_common.h>
+#include <io_addr_dec.h>
+
+void dram_win_map_build(struct dram_win_map *win_map);
+void cpu_wins_init(void);
+
+#endif /* _DRAM_WIN_H_ */
+
diff --git a/plat/marvell/a3700/common/include/io_addr_dec.h b/plat/marvell/a3700/common/include/io_addr_dec.h
new file mode 100644
index 0000000..79f6f2c
--- /dev/null
+++ b/plat/marvell/a3700/common/include/io_addr_dec.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef _IO_ADDR_DEC_H_
+#define _IO_ADDR_DEC_H_
+
+#include <stdint.h>
+
+/* There are 5 configurable cpu decoder windows. */
+#define DRAM_WIN_MAP_NUM_MAX	5
+/* Target number for dram in cpu decoder windows. */
+#define DRAM_CPU_DEC_TARGET_NUM	0
+
+/*
+ * Not all configurable decode windows could be used for dram, some units have
+ * to reserve one decode window for other unit they have to communicate with;
+ * for example, DMA engineer has 3 configurable windows, but only two could be
+ * for dram while the last one has to be for pcie, so for DMA, its max_dram_win
+ * is 2.
+ */
+struct dec_win_config {
+	uint32_t dec_reg_base; /* IO address decoder register base address */
+	uint32_t win_attr;	/* IO address decoder windows attributes */
+	/* How many configurable dram decoder windows that this unit has; */
+	uint32_t max_dram_win;
+	/* The decoder windows number including remapping that this unit has */
+	uint32_t max_remap;
+	/* The offset between continuous decode windows
+	 * within the same unit, typically 0x10
+	 */
+	uint32_t win_offset;
+};
+
+struct dram_win {
+	uintptr_t base_addr;
+	uintptr_t win_size;
+};
+
+struct  dram_win_map {
+	int dram_win_num;
+	struct dram_win dram_windows[DRAM_WIN_MAP_NUM_MAX];
+};
+
+/*
+ * init_io_addr_dec
+ *
+ * This function initializes io address decoder windows by
+ * cpu dram window mapping information
+ *
+ * @input: N/A
+ *     - dram_wins_map: cpu dram windows mapping
+ *     - io_dec_config: io address decoder windows configuration
+ *     - io_unit_num: io address decoder unit number
+ * @output: N/A
+ *
+ * @return:  0 on success and others on failure
+ */
+int init_io_addr_dec(struct dram_win_map *dram_wins_map,
+		     struct dec_win_config *io_dec_config,
+		     uint32_t io_unit_num);
+
+#endif /* _IO_ADDR_DEC_H_ */
+
diff --git a/plat/marvell/a3700/common/include/plat_macros.S b/plat/marvell/a3700/common/include/plat_macros.S
new file mode 100644
index 0000000..b52d1cf
--- /dev/null
+++ b/plat/marvell/a3700/common/include/plat_macros.S
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef __PLAT_MACROS_S__
+#define __PLAT_MACROS_S__
+
+#include <marvell_macros.S>
+
+/* ---------------------------------------------
+ * The below macro prints out relevant GIC and
+ * CCI registers registers whenever an unhandled
+ * exception is taken in BL31.
+ * ---------------------------------------------
+ */
+.macro plat_crash_print_regs
+	mov_imm	x17, MVEBU_GICC_BASE
+	mov_imm	x16, MVEBU_GICD_BASE
+	marvell_print_gic_regs
+	print_cci_regs
+.endm
+
+#endif /* __PLAT_MACROS_S__ */
diff --git a/plat/marvell/a3700/common/include/platform_def.h b/plat/marvell/a3700/common/include/platform_def.h
new file mode 100644
index 0000000..7c72cc5
--- /dev/null
+++ b/plat/marvell/a3700/common/include/platform_def.h
@@ -0,0 +1,231 @@
+/*
+ * Copyright (C) 2016 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef __PLATFORM_DEF_H__
+#define __PLATFORM_DEF_H__
+
+#include <board_marvell_def.h>
+#include <mvebu_def.h>
+#ifndef __ASSEMBLY__
+#include <stdio.h>
+#endif /* __ASSEMBLY__ */
+
+/*
+ * Most platform porting definitions provided by included headers
+ */
+
+/*
+ * DRAM Memory layout:
+ *		+-----------------------+
+ *		:			:
+ *		:	Linux		:
+ * 0x04X00000-->+-----------------------+
+ *		|	BL3-3(u-boot)	|>>}>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+ *		|-----------------------|  }				       |
+ *		|	BL3-[0,1, 2]	|  }---------------------------------> |
+ *		|-----------------------|  }				||     |
+ *		|	BL2		|  }->FIP (loaded by            ||     |
+ *		|-----------------------|  }       BootROM to DRAM)     ||     |
+ *		|	FIP_TOC		|  }                            ||     |
+ * 0x04120000-->|-----------------------|				||     |
+ *		|	BL1 (RO)	|				||     |
+ * 0x04100000-->+-----------------------+				||     |
+ *		:			:				||     |
+ *		: Trusted SRAM section	:				\/     |
+ * 0x04040000-->+-----------------------+  Replaced by BL2  +----------------+ |
+ *		|	BL1 (RW)	|  <<<<<<<<<<<<<<<< | BL3-1 NOBITS   | |
+ * 0x04037000-->|-----------------------|  <<<<<<<<<<<<<<<< |----------------| |
+ *		|			|  <<<<<<<<<<<<<<<< | BL3-1 PROGBITS | |
+ * 0x04023000-->|-----------------------|		    +----------------+ |
+ *		|	BL2		|				       |
+ *		|-----------------------|				       |
+ *		|			|				       |
+ * 0x04001000-->|-----------------------|				       |
+ *		|	Shared		|				       |
+ * 0x04000000-->+-----------------------+				       |
+ *		:			:				       |
+ *		:	Linux		:				       |
+ *		:			:				       |
+ *		|-----------------------|				       |
+ *		|			|	U-Boot(BL3-3) Loaded by BL2    |
+ *		|	U-Boot		|	<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+ * 0x00000000-->+-----------------------+
+ *
+ * Trusted SRAM section 0x4000000..0x4200000:
+ * ----------------------------------------
+ * SRAM_BASE		= 0x4001000
+ * BL2_BASE		= 0x4006000
+ * BL2_LIMIT		= BL31_BASE
+ * BL31_BASE		= 0x4023000 = (64MB + 256KB - 0x1D000)
+ * BL31_PROGBITS_LIMIT	= BL1_RW_BASE
+ * BL1_RW_BASE		= 0x4037000 = (64MB + 256KB - 0x9000)
+ * BL1_RW_LIMIT		= BL31_LIMIT = 0x4040000
+ *
+ *
+ * PLAT_MARVELL_FIP_BASE	= 0x4120000
+ */
+
+#define PLAT_MARVELL_ATF_BASE			0x4000000
+#define PLAT_MARVELL_ATF_LOAD_ADDR		\
+			(PLAT_MARVELL_ATF_BASE + 0x100000)
+
+#define PLAT_MARVELL_FIP_BASE			\
+			(PLAT_MARVELL_ATF_LOAD_ADDR + 0x20000)
+#define PLAT_MARVELL_FIP_MAX_SIZE		0x4000000
+
+#define PLAT_MARVELL_CLUSTER_CORE_COUNT		2
+/* DRAM[2MB..66MB] is used  as Trusted ROM */
+#define PLAT_MARVELL_TRUSTED_ROM_BASE		PLAT_MARVELL_ATF_LOAD_ADDR
+/* 64 MB TODO: reduce this to minimum needed according to fip image size*/
+#define PLAT_MARVELL_TRUSTED_ROM_SIZE		0x04000000
+/* Reserve 16M for SCP (Secure PayLoad) Trusted DRAM */
+#define PLAT_MARVELL_TRUSTED_DRAM_BASE		0x04400000
+#define PLAT_MARVELL_TRUSTED_DRAM_SIZE		0x01000000	/* 16 MB */
+
+/*
+ * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
+ * plus a little space for growth.
+ */
+#define PLAT_MARVELL_MAX_BL1_RW_SIZE		0xA000
+
+/*
+ * PLAT_ARM_MAX_BL2_SIZE is calculated using the current BL2 debug size plus a
+ * little space for growth.
+ */
+#define PLAT_MARVELL_MAX_BL2_SIZE		0xF000
+
+/*
+ * PLAT_ARM_MAX_BL31_SIZE is calculated using the current BL31 debug size plus a
+ * little space for growth.
+ */
+#define PLAT_MARVEL_MAX_BL31_SIZE		0x5D000
+
+#define PLAT_MARVELL_CPU_ENTRY_ADDR		BL1_RO_BASE
+
+/* GIC related definitions */
+#define PLAT_MARVELL_GICD_BASE		(MVEBU_REGS_BASE + MVEBU_GICD_BASE)
+#define PLAT_MARVELL_GICR_BASE		(MVEBU_REGS_BASE + MVEBU_GICR_BASE)
+#define PLAT_MARVELL_GICC_BASE		(MVEBU_REGS_BASE + MVEBU_GICC_BASE)
+
+#define PLAT_MARVELL_G0_IRQ_PROPS(grp) \
+	INTR_PROP_DESC(MARVELL_IRQ_SEC_SGI_0, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MARVELL_IRQ_SEC_SGI_6, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_LEVEL)
+
+#define PLAT_MARVELL_G1S_IRQ_PROPS(grp) \
+	INTR_PROP_DESC(MARVELL_IRQ_SEC_PHY_TIMER, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MARVELL_IRQ_SEC_SGI_1, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MARVELL_IRQ_SEC_SGI_2, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MARVELL_IRQ_SEC_SGI_3, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MARVELL_IRQ_SEC_SGI_4, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MARVELL_IRQ_SEC_SGI_5, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MARVELL_IRQ_SEC_SGI_7, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_LEVEL)
+
+
+#define PLAT_MARVELL_SHARED_RAM_CACHED		1
+
+/* CCI related constants */
+#define PLAT_MARVELL_CCI_BASE		(MVEBU_REGS_BASE + MVEBU_CCI_BASE)
+#define PLAT_MARVELL_CCI_CLUSTER0_SL_IFACE_IX	3
+#define PLAT_MARVELL_CCI_CLUSTER1_SL_IFACE_IX	4
+
+/*
+ * Load address of BL3-3 for this platform port
+ */
+#define PLAT_MARVELL_NS_IMAGE_OFFSET		0x0
+
+/* System Reference Clock*/
+#define PLAT_REF_CLK_IN_HZ			COUNTER_FREQUENCY
+
+/*
+ * PL011 related constants
+ */
+#define PLAT_MARVELL_BOOT_UART_BASE		(MVEBU_REGS_BASE + 0x12000)
+#define PLAT_MARVELL_BOOT_UART_CLK_IN_HZ	25804800
+
+#define PLAT_MARVELL_CRASH_UART_BASE		PLAT_MARVELL_BOOT_UART_BASE
+#define PLAT_MARVELL_CRASH_UART_CLK_IN_HZ	PLAT_MARVELL_BOOT_UART_CLK_IN_HZ
+
+#define PLAT_MARVELL_BL31_RUN_UART_BASE		PLAT_MARVELL_BOOT_UART_BASE
+#define PLAT_MARVELL_BL31_RUN_UART_CLK_IN_HZ	PLAT_MARVELL_BOOT_UART_CLK_IN_HZ
+
+/* Required platform porting definitions */
+#define PLAT_MAX_PWR_LVL			MPIDR_AFFLVL1
+
+/* System timer related constants */
+#define PLAT_MARVELL_NSTIMER_FRAME_ID		1
+
+/* Mailbox base address */
+#define PLAT_MARVELL_MAILBOX_BASE		\
+			(MARVELL_TRUSTED_SRAM_BASE + 0x400)
+#define PLAT_MARVELL_MAILBOX_SIZE		0x100
+#define PLAT_MARVELL_MAILBOX_MAGIC_NUM		0x6D72766C	/* mrvl */
+
+/* DRAM CS memory map registers related constants */
+#define MVEBU_CS_MMAP_LOW(cs_num)		\
+			(MVEBU_CS_MMAP_REG_BASE + (cs_num) * 0x8)
+#define MVEBU_CS_MMAP_ENABLE			0x1
+#define MVEBU_CS_MMAP_AREA_LEN_OFFS		16
+#define MVEBU_CS_MMAP_AREA_LEN_MASK		\
+			(0x1f << MVEBU_CS_MMAP_AREA_LEN_OFFS)
+#define MVEBU_CS_MMAP_START_ADDR_LOW_OFFS	23
+#define MVEBU_CS_MMAP_START_ADDR_LOW_MASK	\
+			(0x1ff << MVEBU_CS_MMAP_START_ADDR_LOW_OFFS)
+
+#define MVEBU_CS_MMAP_HIGH(cs_num)		\
+			(MVEBU_CS_MMAP_REG_BASE + 0x4 + (cs_num) * 0x8)
+
+/* DRAM max CS number */
+#define MVEBU_MAX_CS_MMAP_NUM			(2)
+
+/* CPU decoder window related constants */
+#define CPU_DEC_WIN_CTRL_REG(win_num)		\
+			(MVEBU_CPU_DEC_WIN_REG_BASE + (win_num) * 0x10)
+#define CPU_DEC_CR_WIN_ENABLE			0x1
+#define CPU_DEC_CR_WIN_TARGET_OFFS		4
+#define CPU_DEC_CR_WIN_TARGET_MASK		\
+			(0xf << CPU_DEC_CR_WIN_TARGET_OFFS)
+
+#define CPU_DEC_WIN_SIZE_REG(win_num)		\
+			(MVEBU_CPU_DEC_WIN_REG_BASE + 0x4 + (win_num) * 0x10)
+#define CPU_DEC_CR_WIN_SIZE_OFFS		0
+#define CPU_DEC_CR_WIN_SIZE_MASK		\
+			(0xffff << CPU_DEC_CR_WIN_SIZE_OFFS)
+#define CPU_DEC_CR_WIN_SIZE_ALIGNMENT		0x10000
+
+#define CPU_DEC_WIN_BASE_REG(win_num)		\
+			(MVEBU_CPU_DEC_WIN_REG_BASE + 0x8 + (win_num) * 0x10)
+#define CPU_DEC_BR_BASE_OFFS			0
+#define CPU_DEC_BR_BASE_MASK			\
+			(0xffff <<  CPU_DEC_BR_BASE_OFFS)
+
+#define CPU_DEC_REMAP_LOW_REG(win_num)		\
+			(MVEBU_CPU_DEC_WIN_REG_BASE + 0xC + (win_num) * 0x10)
+#define CPU_DEC_RLR_REMAP_LOW_OFFS		0
+#define CPU_DEC_RLR_REMAP_LOW_MASK		\
+			(0xffff <<  CPU_DEC_BR_BASE_OFFS)
+
+/* Securities */
+#define IRQ_SEC_OS_TICK_INT	MARVELL_IRQ_SEC_PHY_TIMER
+
+#define TRUSTED_DRAM_BASE	PLAT_MARVELL_TRUSTED_DRAM_BASE
+#define TRUSTED_DRAM_SIZE	PLAT_MARVELL_TRUSTED_DRAM_SIZE
+
+#ifdef BL32
+#define BL32_BASE		TRUSTED_DRAM_BASE
+#define BL32_LIMIT		TRUSTED_DRAM_SIZE
+#endif
+
+#endif /* __PLATFORM_DEF_H__ */
diff --git a/plat/marvell/a3700/common/io_addr_dec.c b/plat/marvell/a3700/common/io_addr_dec.c
new file mode 100644
index 0000000..f009594
--- /dev/null
+++ b/plat/marvell/a3700/common/io_addr_dec.c
@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2016 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+#include <debug.h>
+#include <io_addr_dec.h>
+#include <mmio.h>
+#include <plat_marvell.h>
+
+#define MVEBU_DEC_WIN_CTRL_REG(base, win, off)	(MVEBU_REGS_BASE + (base) + \
+						(win) * (off))
+#define MVEBU_DEC_WIN_BASE_REG(base, win, off)	(MVEBU_REGS_BASE + (base) + \
+						(win) * (off) + 0x4)
+#define MVEBU_DEC_WIN_REMAP_REG(base, win, off)	(MVEBU_REGS_BASE + (base) + \
+						(win) * (off) + 0x8)
+
+#define MVEBU_DEC_WIN_CTRL_SIZE_OFF		(16)
+#define MVEBU_DEC_WIN_ENABLE			(0x1)
+#define MVEBU_DEC_WIN_CTRL_ATTR_OFF		(8)
+#define MVEBU_DEC_WIN_CTRL_TARGET_OFF		(4)
+#define MVEBU_DEC_WIN_CTRL_EN_OFF		(0)
+#define MVEBU_DEC_WIN_BASE_OFF			(16)
+
+#define MVEBU_WIN_BASE_SIZE_ALIGNMENT		(0x10000)
+
+/* There are up to 14 IO unit which need address decode in Armada-3700 */
+#define IO_UNIT_NUM_MAX				(14)
+
+#define MVEBU_MAX_ADDRSS_4GB			(0x100000000ULL)
+
+
+static void set_io_addr_dec_win(int win_id, uintptr_t base_addr,
+				uintptr_t win_size,
+				struct dec_win_config *dec_win)
+{
+	uint32_t ctrl = 0;
+	uint32_t base = 0;
+
+	/* set size */
+	ctrl = ((win_size / MVEBU_WIN_BASE_SIZE_ALIGNMENT) - 1) <<
+	       MVEBU_DEC_WIN_CTRL_SIZE_OFF;
+	/* set attr according to IO decode window */
+	ctrl |= dec_win->win_attr << MVEBU_DEC_WIN_CTRL_ATTR_OFF;
+	/* set target */
+	ctrl |= DRAM_CPU_DEC_TARGET_NUM << MVEBU_DEC_WIN_CTRL_TARGET_OFF;
+	/* set base */
+	base = (base_addr / MVEBU_WIN_BASE_SIZE_ALIGNMENT) <<
+	       MVEBU_DEC_WIN_BASE_OFF;
+
+	/* set base address*/
+	mmio_write_32(MVEBU_DEC_WIN_BASE_REG(dec_win->dec_reg_base,
+		      win_id, dec_win->win_offset),
+		      base);
+	/* set remap window, some unit does not have remap window */
+	if (win_id < dec_win->max_remap)
+		mmio_write_32(MVEBU_DEC_WIN_REMAP_REG(dec_win->dec_reg_base,
+			      win_id, dec_win->win_offset), base);
+	/* set control register */
+	mmio_write_32(MVEBU_DEC_WIN_CTRL_REG(dec_win->dec_reg_base,
+		      win_id, dec_win->win_offset), ctrl);
+	/* enable the address decode window at last to make it effective */
+	ctrl |= MVEBU_DEC_WIN_ENABLE << MVEBU_DEC_WIN_CTRL_EN_OFF;
+	mmio_write_32(MVEBU_DEC_WIN_CTRL_REG(dec_win->dec_reg_base,
+		      win_id, dec_win->win_offset), ctrl);
+
+	INFO("set_io_addr_dec %d result: ctrl(0x%x) base(0x%x)",
+	     win_id, mmio_read_32(MVEBU_DEC_WIN_CTRL_REG(dec_win->dec_reg_base,
+	     win_id, dec_win->win_offset)),
+	     mmio_read_32(MVEBU_DEC_WIN_BASE_REG(dec_win->dec_reg_base,
+			  win_id, dec_win->win_offset)));
+	if (win_id < dec_win->max_remap)
+		INFO(" remap(%x)\n",
+		     mmio_read_32(MVEBU_DEC_WIN_REMAP_REG(dec_win->dec_reg_base,
+		     win_id, dec_win->win_offset)));
+	else
+		INFO("\n");
+}
+
+/* Set io decode window */
+static int set_io_addr_dec(struct dram_win_map *win_map,
+			   struct dec_win_config *dec_win)
+{
+	struct dram_win *win;
+	int id;
+
+	/* disable all windows first */
+	for (id = 0; id < dec_win->max_dram_win; id++)
+		mmio_write_32(MVEBU_DEC_WIN_CTRL_REG(dec_win->dec_reg_base, id,
+			      dec_win->win_offset), 0);
+
+	/* configure IO decode windows for DRAM, inheritate DRAM size,
+	 * base and target from CPU-DRAM decode window and others
+	 * from hard coded IO decode window settings array.
+	 */
+	if (win_map->dram_win_num > dec_win->max_dram_win) {
+		/*
+		 * If cpu dram windows number exceeds the io decode windows
+		 * max number, then fill the first io decode window
+		 * with base(0) and size(4GB).
+		 */
+		set_io_addr_dec_win(0, 0, MVEBU_MAX_ADDRSS_4GB, dec_win);
+
+		return 0;
+	}
+
+	for (id = 0; id < win_map->dram_win_num; id++, win++) {
+		win = &win_map->dram_windows[id];
+		set_io_addr_dec_win(id, win->base_addr, win->win_size, dec_win);
+	}
+
+	return 0;
+}
+
+/*
+ * init_io_addr_dec
+ *
+ * This function initializes io address decoder windows by
+ * cpu dram window mapping information
+ *
+ * @input: N/A
+ *     - dram_wins_map: cpu dram windows mapping
+ *     - io_dec_config: io address decoder windows configuration
+ *     - io_unit_num: io address decoder unit number
+ * @output: N/A
+ *
+ * @return:  0 on success and others on failure
+ */
+int init_io_addr_dec(struct dram_win_map *dram_wins_map,
+		     struct dec_win_config *io_dec_config, uint32_t io_unit_num)
+{
+	int32_t index;
+	struct dec_win_config *io_dec_win;
+	int32_t ret;
+
+	INFO("Initializing IO address decode windows\n");
+
+	if (io_dec_config == NULL || io_unit_num == 0) {
+		ERROR("No IO address decoder windows configurations!\n");
+		return -1;
+	}
+
+	if (io_unit_num > IO_UNIT_NUM_MAX) {
+		ERROR("IO address decoder windows number %d is over max %d\n",
+		      io_unit_num, IO_UNIT_NUM_MAX);
+		return -1;
+	}
+
+	if (dram_wins_map == NULL) {
+		ERROR("No cpu dram decoder windows map!\n");
+		return -1;
+	}
+
+	for (index = 0; index < dram_wins_map->dram_win_num; index++)
+		INFO("DRAM mapping %d base(0x%lx) size(0x%lx)\n",
+		     index, dram_wins_map->dram_windows[index].base_addr,
+		     dram_wins_map->dram_windows[index].win_size);
+
+	/* Set address decode window for each IO */
+	for (index = 0; index < io_unit_num; index++) {
+		io_dec_win = io_dec_config + index;
+		ret = set_io_addr_dec(dram_wins_map, io_dec_win);
+		if (ret) {
+			ERROR("Failed to set IO address decode\n");
+			return -1;
+		}
+		INFO("Set IO decode window successfully, base(0x%x)",
+		     io_dec_win->dec_reg_base);
+		INFO(" win_attr(%x) max_dram_win(%d) max_remap(%d)",
+		     io_dec_win->win_attr, io_dec_win->max_dram_win,
+		     io_dec_win->max_remap);
+		INFO(" win_offset(%d)\n", io_dec_win->win_offset);
+	}
+
+	return 0;
+}
diff --git a/plat/marvell/a3700/common/marvell_plat_config.c b/plat/marvell/a3700/common/marvell_plat_config.c
new file mode 100644
index 0000000..85a4201
--- /dev/null
+++ b/plat/marvell/a3700/common/marvell_plat_config.c
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <bl_common.h>
+#include <io_addr_dec.h>
+#include <mvebu_def.h>
+
+struct dec_win_config io_dec_win_conf[] = {
+	/* dec_reg_base  win_attr  max_dram_win  max_remap  win_offset */
+	{0xc000,         0x3d,     2,       0,        0x08}, /* USB */
+	{0xc100,         0x3d,     3,       0,        0x10}, /* USB3 */
+	{0xc200,         0x3d,     2,       0,        0x10}, /* DMA */
+	{0xc300,         0x3d,     2,       0,        0x10}, /* NETA0 */
+	{0xc400,         0x3d,     2,       0,        0x10}, /* NETA1 */
+	{0xc500,         0x3d,     2,       0,        0x10}, /* PCIe */
+	{0xc800,         0x3d,     3,       0,        0x10}, /* SATA */
+	{0xca00,         0x3d,     3,       0,        0x08}, /* SD */
+	{0xcb00,         0x3d,     3,       0,        0x10}, /* eMMC */
+	{0xce00,	 0x3d,	   2,	    0,        0x08}, /* EIP97 */
+};
+
+int marvell_get_io_dec_win_conf(struct dec_win_config **win, uint32_t *size)
+{
+	*win = io_dec_win_conf;
+	*size = sizeof(io_dec_win_conf)/sizeof(struct dec_win_config);
+
+	return 0;
+}
+
diff --git a/plat/marvell/a3700/common/plat_pm.c b/plat/marvell/a3700/common/plat_pm.c
new file mode 100644
index 0000000..5cebc92
--- /dev/null
+++ b/plat/marvell/a3700/common/plat_pm.c
@@ -0,0 +1,815 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:	BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <a3700_pm.h>
+#include <arch_helpers.h>
+#include <armada_common.h>
+#include <debug.h>
+#include <dram_win.h>
+#include <io_addr_dec.h>
+#include <mmio.h>
+#include <mvebu.h>
+#include <mvebu_def.h>
+#include <marvell_plat_priv.h>
+#include <platform.h>
+#include <plat_marvell.h>
+#include <psci.h>
+#ifdef USE_CCI
+#include <cci.h>
+#endif
+
+/* Warm reset register */
+#define MVEBU_WARM_RESET_REG		(MVEBU_NB_REGS_BASE + 0x840)
+#define MVEBU_WARM_RESET_MAGIC		0x1D1E
+
+/* North Bridge GPIO1 SEL register */
+#define MVEBU_NB_GPIO1_SEL_REG		(MVEBU_NB_REGS_BASE + 0x830)
+ #define MVEBU_NB_GPIO1_UART1_SEL	BIT(19)
+ #define MVEBU_NB_GPIO1_GPIO_25_26_EN	BIT(17)
+ #define MVEBU_NB_GPIO1_GPIO_19_EN	BIT(14)
+ #define MVEBU_NB_GPIO1_GPIO_18_EN	BIT(13)
+
+/* CPU 1 reset register */
+#define MVEBU_CPU_1_RESET_VECTOR	(MVEBU_REGS_BASE + 0x14044)
+#define MVEBU_CPU_1_RESET_REG		(MVEBU_REGS_BASE + 0xD00C)
+#define MVEBU_CPU_1_RESET_BIT		31
+
+/* IRQ register */
+#define MVEBU_NB_IRQ_STATUS_1_REG		(MVEBU_NB_SB_IRQ_REG_BASE)
+#define MVEBU_NB_IRQ_STATUS_2_REG		(MVEBU_NB_SB_IRQ_REG_BASE + \
+						0x10)
+#define MVEBU_NB_IRQ_MASK_2_REG			(MVEBU_NB_SB_IRQ_REG_BASE + \
+						0x18)
+#define MVEBU_SB_IRQ_STATUS_1_REG		(MVEBU_NB_SB_IRQ_REG_BASE + \
+						0x40)
+#define MVEBU_SB_IRQ_STATUS_2_REG		(MVEBU_NB_SB_IRQ_REG_BASE + \
+						0x50)
+#define MVEBU_NB_GPIO_IRQ_MASK_1_REG		(MVEBU_NB_SB_IRQ_REG_BASE + \
+						0xC8)
+#define MVEBU_NB_GPIO_IRQ_MASK_2_REG		(MVEBU_NB_SB_IRQ_REG_BASE + \
+						0xD8)
+#define MVEBU_SB_GPIO_IRQ_MASK_REG		(MVEBU_NB_SB_IRQ_REG_BASE + \
+						0xE8)
+#define MVEBU_NB_GPIO_IRQ_EN_LOW_REG		(MVEBU_NB_GPIO_IRQ_REG_BASE)
+#define MVEBU_NB_GPIO_IRQ_EN_HIGH_REG		(MVEBU_NB_GPIO_IRQ_REG_BASE + \
+						0x04)
+#define MVEBU_NB_GPIO_IRQ_STATUS_LOW_REG	(MVEBU_NB_GPIO_IRQ_REG_BASE + \
+						0x10)
+#define MVEBU_NB_GPIO_IRQ_STATUS_HIGH_REG	(MVEBU_NB_GPIO_IRQ_REG_BASE + \
+						0x14)
+#define MVEBU_NB_GPIO_IRQ_WK_LOW_REG		(MVEBU_NB_GPIO_IRQ_REG_BASE + \
+						0x18)
+#define MVEBU_NB_GPIO_IRQ_WK_HIGH_REG		(MVEBU_NB_GPIO_IRQ_REG_BASE + \
+						0x1C)
+#define MVEBU_SB_GPIO_IRQ_EN_REG		(MVEBU_SB_GPIO_IRQ_REG_BASE)
+#define MVEBU_SB_GPIO_IRQ_STATUS_REG		(MVEBU_SB_GPIO_IRQ_REG_BASE + \
+						0x10)
+#define MVEBU_SB_GPIO_IRQ_WK_REG		(MVEBU_SB_GPIO_IRQ_REG_BASE + \
+						0x18)
+
+/* PMU registers */
+#define MVEBU_PM_NB_PWR_CTRL_REG	(MVEBU_PMSU_REG_BASE)
+ #define MVEBU_PM_PWR_DN_CNT_SEL	BIT(28)
+ #define MVEBU_PM_SB_PWR_DWN		BIT(4)
+ #define MVEBU_PM_INTERFACE_IDLE	BIT(0)
+#define MVEBU_PM_NB_CPU_PWR_CTRL_REG	(MVEBU_PMSU_REG_BASE + 0x4)
+ #define MVEBU_PM_L2_FLUSH_EN		BIT(22)
+#define MVEBU_PM_NB_PWR_OPTION_REG	(MVEBU_PMSU_REG_BASE + 0x8)
+ #define MVEBU_PM_DDR_SR_EN		BIT(29)
+ #define MVEBU_PM_DDR_CLK_DIS_EN	BIT(28)
+ #define MVEBU_PM_WARM_RESET_EN		BIT(27)
+ #define MVEBU_PM_DDRPHY_PWRDWN_EN	BIT(23)
+ #define MVEBU_PM_DDRPHY_PAD_PWRDWN_EN	BIT(22)
+ #define MVEBU_PM_OSC_OFF_EN		BIT(21)
+ #define MVEBU_PM_TBG_OFF_EN		BIT(20)
+ #define MVEBU_PM_CPU_VDDV_OFF_EN	BIT(19)
+ #define MVEBU_PM_AVS_DISABLE_MODE	BIT(14)
+ #define MVEBU_PM_AVS_VDD2_MODE		BIT(13)
+ #define MVEBU_PM_AVS_HOLD_MODE		BIT(12)
+ #define MVEBU_PM_L2_SRAM_LKG_PD_EN	BIT(8)
+ #define MVEBU_PM_EIP_SRAM_LKG_PD_EN	BIT(7)
+ #define MVEBU_PM_DDRMC_SRAM_LKG_PD_EN	BIT(6)
+ #define MVEBU_PM_MCI_SRAM_LKG_PD_EN	BIT(5)
+ #define MVEBU_PM_MMC_SRAM_LKG_PD_EN	BIT(4)
+ #define MVEBU_PM_SATA_SRAM_LKG_PD_EN	BIT(3)
+ #define MVEBU_PM_DMA_SRAM_LKG_PD_EN	BIT(2)
+ #define MVEBU_PM_SEC_SRAM_LKG_PD_EN	BIT(1)
+ #define MVEBU_PM_CPU_SRAM_LKG_PD_EN	BIT(0)
+ #define MVEBU_PM_NB_SRAM_LKG_PD_EN	(MVEBU_PM_L2_SRAM_LKG_PD_EN |\
+	MVEBU_PM_EIP_SRAM_LKG_PD_EN | MVEBU_PM_DDRMC_SRAM_LKG_PD_EN |\
+	MVEBU_PM_MCI_SRAM_LKG_PD_EN | MVEBU_PM_MMC_SRAM_LKG_PD_EN |\
+	MVEBU_PM_SATA_SRAM_LKG_PD_EN | MVEBU_PM_DMA_SRAM_LKG_PD_EN |\
+	MVEBU_PM_SEC_SRAM_LKG_PD_EN | MVEBU_PM_CPU_SRAM_LKG_PD_EN)
+#define MVEBU_PM_NB_PWR_DEBUG_REG	(MVEBU_PMSU_REG_BASE + 0xC)
+ #define MVEBU_PM_NB_FORCE_CLK_ON	BIT(30)
+ #define MVEBU_PM_IGNORE_CM3_SLEEP	BIT(21)
+ #define MVEBU_PM_IGNORE_CM3_DEEP	BIT(20)
+#define MVEBU_PM_NB_WAKE_UP_EN_REG	(MVEBU_PMSU_REG_BASE + 0x2C)
+ #define MVEBU_PM_SB_WKP_NB_EN		BIT(31)
+ #define MVEBU_PM_NB_GPIO_WKP_EN	BIT(27)
+ #define MVEBU_PM_SOC_TIMER_WKP_EN	BIT(26)
+ #define MVEBU_PM_UART_WKP_EN		BIT(25)
+ #define MVEBU_PM_UART2_WKP_EN		BIT(19)
+ #define MVEBU_PM_CPU_TIMER_WKP_EN	BIT(17)
+ #define MVEBU_PM_NB_WKP_EN		BIT(16)
+ #define MVEBU_PM_CORE1_FIQ_IRQ_WKP_EN	BIT(13)
+ #define MVEBU_PM_CORE0_FIQ_IRQ_WKP_EN	BIT(12)
+#define MVEBU_PM_CPU_0_PWR_CTRL_REG	(MVEBU_PMSU_REG_BASE + 0x34)
+#define MVEBU_PM_CPU_1_PWR_CTRL_REG	(MVEBU_PMSU_REG_BASE + 0x38)
+ #define MVEBU_PM_CORE_SOC_PD		BIT(2)
+ #define MVEBU_PM_CORE_PROC_PD		BIT(1)
+ #define MVEBU_PM_CORE_PD		BIT(0)
+#define MVEBU_PM_CORE_1_RETURN_ADDR_REG	(MVEBU_PMSU_REG_BASE + 0x44)
+#define MVEBU_PM_CPU_VDD_OFF_INFO_1_REG	(MVEBU_PMSU_REG_BASE + 0x48)
+#define MVEBU_PM_CPU_VDD_OFF_INFO_2_REG	(MVEBU_PMSU_REG_BASE + 0x4C)
+ #define MVEBU_PM_LOW_POWER_STATE	BIT(0)
+#define MVEBU_PM_CPU_WAKE_UP_CONF_REG	(MVEBU_PMSU_REG_BASE + 0x54)
+ #define MVEBU_PM_CORE1_WAKEUP		BIT(13)
+ #define MVEBU_PM_CORE0_WAKEUP		BIT(12)
+#define MVEBU_PM_WAIT_DDR_RDY_VALUE	(0x15)
+#define MVEBU_PM_SB_CPU_PWR_CTRL_REG	(MVEBU_SB_WAKEUP_REG_BASE)
+  #define MVEBU_PM_SB_PM_START		BIT(0)
+#define MVEBU_PM_SB_PWR_OPTION_REG	(MVEBU_SB_WAKEUP_REG_BASE + 0x4)
+  #define MVEBU_PM_SDIO_PHY_PDWN_EN	BIT(17)
+  #define MVEBU_PM_SB_VDDV_OFF_EN	BIT(16)
+  #define MVEBU_PM_EBM_SRAM_LKG_PD_EN		BIT(11)
+  #define MVEBU_PM_PCIE_SRAM_LKG_PD_EN		BIT(10)
+  #define MVEBU_PM_GBE1_TX_SRAM_LKG_PD_EN	BIT(9)
+  #define MVEBU_PM_GBE1_RX_SRAM_LKG_PD_EN	BIT(8)
+  #define MVEBU_PM_GBE1_MIB_SRAM_LKG_PD_EN	BIT(7)
+  #define MVEBU_PM_GBE0_TX_SRAM_LKG_PD_EN	BIT(6)
+  #define MVEBU_PM_GBE0_RX_SRAM_LKG_PD_EN	BIT(5)
+  #define MVEBU_PM_GBE0_MIB_SRAM_LKG_PD_EN	BIT(4)
+  #define MVEBU_PM_SDIO_SRAM_LKG_PD_EN		BIT(3)
+  #define MVEBU_PM_USB2_SRAM_LKG_PD_EN		BIT(2)
+  #define MVEBU_PM_USB3_H_SRAM_LKG_PD_EN	BIT(1)
+  #define MVEBU_PM_SB_SRAM_LKG_PD_EN	(MVEBU_PM_EBM_SRAM_LKG_PD_EN |\
+	MVEBU_PM_PCIE_SRAM_LKG_PD_EN | MVEBU_PM_GBE1_TX_SRAM_LKG_PD_EN |\
+	MVEBU_PM_GBE1_RX_SRAM_LKG_PD_EN | MVEBU_PM_GBE1_MIB_SRAM_LKG_PD_EN |\
+	MVEBU_PM_GBE0_TX_SRAM_LKG_PD_EN | MVEBU_PM_GBE0_RX_SRAM_LKG_PD_EN |\
+	MVEBU_PM_GBE0_MIB_SRAM_LKG_PD_EN | MVEBU_PM_SDIO_SRAM_LKG_PD_EN |\
+	MVEBU_PM_USB2_SRAM_LKG_PD_EN | MVEBU_PM_USB3_H_SRAM_LKG_PD_EN)
+#define MVEBU_PM_SB_WK_EN_REG		(MVEBU_SB_WAKEUP_REG_BASE + 0x10)
+  #define MVEBU_PM_SB_GPIO_WKP_EN	BIT(24)
+  #define MVEBU_PM_SB_WKP_EN		BIT(20)
+
+/* DRAM registers */
+#define MVEBU_DRAM_STATS_CH0_REG	(MVEBU_DRAM_REG_BASE + 0x4)
+ #define MVEBU_DRAM_WCP_EMPTY		BIT(19)
+#define MVEBU_DRAM_CMD_0_REG		(MVEBU_DRAM_REG_BASE + 0x20)
+ #define MVEBU_DRAM_CH0_CMD0		BIT(28)
+ #define MVEBU_DRAM_CS_CMD0		BIT(24)
+ #define MVEBU_DRAM_WCB_DRAIN_REQ	BIT(1)
+#define MVEBU_DRAM_PWR_CTRL_REG		(MVEBU_DRAM_REG_BASE + 0x54)
+ #define MVEBU_DRAM_PHY_CLK_GATING_EN	BIT(1)
+ #define MVEBU_DRAM_PHY_AUTO_AC_OFF_EN	BIT(0)
+
+/* AVS registers */
+#define MVEBU_AVS_CTRL_2_REG		(MVEBU_AVS_REG_BASE + 0x8)
+ #define MVEBU_LOW_VDD_MODE_EN		BIT(6)
+
+/* Clock registers */
+#define MVEBU_NB_CLOCK_SEL_REG		(MVEBU_NB_REGS_BASE + 0x10)
+ #define MVEBU_A53_CPU_CLK_SEL		BIT(15)
+
+/* North Bridge Step-Down Registers */
+#define MVEBU_NB_STEP_DOWN_INT_EN_REG	MVEBU_NB_STEP_DOWN_REG_BASE
+ #define MVEBU_NB_GPIO_INT_WAKE_WCPU_CLK	BIT(8)
+
+#define MVEBU_NB_GPIO_18	18
+#define MVEBU_NB_GPIO_19	19
+#define MVEBU_NB_GPIO_25	25
+#define MVEBU_NB_GPIO_26	26
+
+typedef int (*wake_up_src_func)(union pm_wake_up_src_data *);
+
+struct wake_up_src_func_map {
+	enum pm_wake_up_src_type type;
+	wake_up_src_func func;
+};
+
+void marvell_psci_arch_init(int die_index)
+{
+}
+
+static void a3700_pm_ack_irq(void)
+{
+	uint32_t reg;
+
+	reg = mmio_read_32(MVEBU_NB_IRQ_STATUS_1_REG);
+	if (reg)
+		mmio_write_32(MVEBU_NB_IRQ_STATUS_1_REG, reg);
+
+	reg = mmio_read_32(MVEBU_NB_IRQ_STATUS_2_REG);
+	if (reg)
+		mmio_write_32(MVEBU_NB_IRQ_STATUS_2_REG, reg);
+
+	reg = mmio_read_32(MVEBU_SB_IRQ_STATUS_1_REG);
+	if (reg)
+		mmio_write_32(MVEBU_SB_IRQ_STATUS_1_REG, reg);
+
+	reg = mmio_read_32(MVEBU_SB_IRQ_STATUS_2_REG);
+	if (reg)
+		mmio_write_32(MVEBU_SB_IRQ_STATUS_2_REG, reg);
+
+	reg = mmio_read_32(MVEBU_NB_GPIO_IRQ_STATUS_LOW_REG);
+	if (reg)
+		mmio_write_32(MVEBU_NB_GPIO_IRQ_STATUS_LOW_REG, reg);
+
+	reg = mmio_read_32(MVEBU_NB_GPIO_IRQ_STATUS_HIGH_REG);
+	if (reg)
+		mmio_write_32(MVEBU_NB_GPIO_IRQ_STATUS_HIGH_REG, reg);
+
+	reg = mmio_read_32(MVEBU_SB_GPIO_IRQ_STATUS_REG);
+	if (reg)
+		mmio_write_32(MVEBU_SB_GPIO_IRQ_STATUS_REG, reg);
+}
+
+/*****************************************************************************
+ * A3700 handler called to check the validity of the power state
+ * parameter.
+ *****************************************************************************
+ */
+int a3700_validate_power_state(unsigned int power_state,
+			       psci_power_state_t *req_state)
+{
+	ERROR("%s needs to be implemented\n", __func__);
+	panic();
+}
+
+/*****************************************************************************
+ * A3700 handler called when a CPU is about to enter standby.
+ *****************************************************************************
+ */
+void a3700_cpu_standby(plat_local_state_t cpu_state)
+{
+	ERROR("%s needs to be implemented\n", __func__);
+	panic();
+}
+
+/*****************************************************************************
+ * A3700 handler called when a power domain is about to be turned on. The
+ * mpidr determines the CPU to be turned on.
+ *****************************************************************************
+ */
+int a3700_pwr_domain_on(u_register_t mpidr)
+{
+	/* Set barrier */
+	dsbsy();
+
+	/* Set the cpu start address to BL1 entry point */
+	mmio_write_32(MVEBU_CPU_1_RESET_VECTOR,
+		      PLAT_MARVELL_CPU_ENTRY_ADDR >> 2);
+
+	/* Get the cpu out of reset */
+	mmio_clrbits_32(MVEBU_CPU_1_RESET_REG, BIT(MVEBU_CPU_1_RESET_BIT));
+	mmio_setbits_32(MVEBU_CPU_1_RESET_REG, BIT(MVEBU_CPU_1_RESET_BIT));
+
+	return 0;
+}
+
+/*****************************************************************************
+ * A3700 handler called to validate the entry point.
+ *****************************************************************************
+ */
+int a3700_validate_ns_entrypoint(uintptr_t entrypoint)
+{
+	return PSCI_E_SUCCESS;
+}
+
+/*****************************************************************************
+ * A3700 handler called when a power domain is about to be turned off. The
+ * target_state encodes the power state that each level should transition to.
+ *****************************************************************************
+ */
+void a3700_pwr_domain_off(const psci_power_state_t *target_state)
+{
+	uint32_t cpu_idx = plat_my_core_pos();
+
+	/* Prevent interrupts from spuriously waking up this cpu */
+	plat_marvell_gic_cpuif_disable();
+
+	/*
+	 * Enable Core VDD OFF, core is supposed to be powered
+	 * off by PMU when WFI command is issued.
+	 */
+	mmio_setbits_32(MVEBU_PM_CPU_0_PWR_CTRL_REG + 4 * cpu_idx,
+			MVEBU_PM_CORE_PD);
+
+	/* Core can not be powered down with pending IRQ,
+	 * acknowledge all the pending IRQ
+	 */
+	a3700_pm_ack_irq();
+}
+
+static void a3700_set_gen_pwr_off_option(void)
+{
+	/* Enable L2 flush -> processor state-machine option */
+	mmio_setbits_32(MVEBU_PM_NB_CPU_PWR_CTRL_REG, MVEBU_PM_L2_FLUSH_EN);
+
+	/*
+	 * North bridge cannot be VDD off (always ON).
+	 * The NB state machine support low power mode by its state machine.
+	 * This bit MUST be set for north bridge power down, e.g.,
+	 * OSC input cutoff(NOT TEST), SRAM power down, PMIC, etc.
+	 * It is not related to CPU VDD OFF!!
+	 */
+	mmio_clrbits_32(MVEBU_PM_NB_PWR_OPTION_REG, MVEBU_PM_CPU_VDDV_OFF_EN);
+
+	/*
+	 * MUST: Switch CPU/AXI clock to OSC
+	 * NB state machine clock is always connected to OSC (slow clock).
+	 * But Core0/1/processor state machine's clock are connected to AXI
+	 *  clock. Now, AXI clock takes the TBG as clock source.
+	 * If using AXI clock, Core0/1/processor state machine may much faster
+	 * than NB state machine. It will cause problem in this case if cores
+	 * are released before north bridge gets ready.
+	 */
+	mmio_clrbits_32(MVEBU_NB_CLOCK_SEL_REG, MVEBU_A53_CPU_CLK_SEL);
+
+	/*
+	 * These register bits will trigger north bridge
+	 * power-down state machine regardless CM3 status.
+	 */
+	mmio_setbits_32(MVEBU_PM_NB_PWR_DEBUG_REG, MVEBU_PM_IGNORE_CM3_SLEEP);
+	mmio_setbits_32(MVEBU_PM_NB_PWR_DEBUG_REG, MVEBU_PM_IGNORE_CM3_DEEP);
+
+	/*
+	 * SRAM => controlled by north bridge state machine.
+	 * Core VDD OFF is not related to CPU SRAM power down.
+	 */
+	mmio_setbits_32(MVEBU_PM_NB_PWR_OPTION_REG, MVEBU_PM_NB_SRAM_LKG_PD_EN);
+
+	/*
+	 * Idle AXI interface in order to get L2_WFI
+	 * L2 WFI is only asserted after CORE-0 and CORE-1 WFI asserted.
+	 * (only both core-0/1in WFI, L2 WFI will be issued by CORE.)
+	 * Once L2 WFI asserted, this bit is used for signalling assertion
+	 * to AXI IO masters.
+	 */
+	mmio_setbits_32(MVEBU_PM_NB_PWR_CTRL_REG, MVEBU_PM_INTERFACE_IDLE);
+
+	/* Enable core0 and core1 VDD_OFF */
+	mmio_setbits_32(MVEBU_PM_CPU_0_PWR_CTRL_REG, MVEBU_PM_CORE_PD);
+	mmio_setbits_32(MVEBU_PM_CPU_1_PWR_CTRL_REG, MVEBU_PM_CORE_PD);
+
+	/* Enable North bridge power down -
+	 * Both Cores MUST enable this bit to power down north bridge!
+	 */
+	mmio_setbits_32(MVEBU_PM_CPU_0_PWR_CTRL_REG, MVEBU_PM_CORE_SOC_PD);
+	mmio_setbits_32(MVEBU_PM_CPU_1_PWR_CTRL_REG, MVEBU_PM_CORE_SOC_PD);
+
+	/* CA53 (processor domain) power down */
+	mmio_setbits_32(MVEBU_PM_CPU_0_PWR_CTRL_REG, MVEBU_PM_CORE_PROC_PD);
+	mmio_setbits_32(MVEBU_PM_CPU_1_PWR_CTRL_REG, MVEBU_PM_CORE_PROC_PD);
+}
+
+static void a3700_en_ddr_self_refresh(void)
+{
+	/*
+	 * Both count is 16 bits and configurable. By default, osc stb cnt
+	 * is 0xFFF for lower 12 bits.
+	 * Thus, powerdown count is smaller than osc count.
+	 * This count is used for exiting DDR SR mode on wakeup event.
+	 * The powerdown count also has impact on the following
+	 * state changes: idle -> count-down -> ... (power-down, vdd off, etc)
+	 * Here, make stable counter shorter
+	 * Use power down count value instead of osc_stb_cnt to speed up
+	 * DDR self refresh exit
+	 */
+	mmio_setbits_32(MVEBU_PM_NB_PWR_CTRL_REG, MVEBU_PM_PWR_DN_CNT_SEL);
+
+	/*
+	 * Enable DDR SR mode => controlled by north bridge state machine
+	 * Therefore, we must powerdown north bridge to trigger the DDR SR
+	 * mode switching.
+	 */
+	mmio_setbits_32(MVEBU_PM_NB_PWR_OPTION_REG, MVEBU_PM_DDR_SR_EN);
+	/* Disable DDR clock, otherwise DDR will not enter into SR mode. */
+	mmio_setbits_32(MVEBU_PM_NB_PWR_OPTION_REG, MVEBU_PM_DDR_CLK_DIS_EN);
+	/* Power down DDR PHY (PAD) */
+	mmio_setbits_32(MVEBU_PM_NB_PWR_OPTION_REG, MVEBU_PM_DDRPHY_PWRDWN_EN);
+	mmio_setbits_32(MVEBU_PM_NB_PWR_OPTION_REG,
+			MVEBU_PM_DDRPHY_PAD_PWRDWN_EN);
+
+	/* Set wait time for DDR ready in ROM code */
+	mmio_write_32(MVEBU_PM_CPU_VDD_OFF_INFO_1_REG,
+		      MVEBU_PM_WAIT_DDR_RDY_VALUE);
+
+	/* DDR flush write buffer - mandatory */
+	mmio_write_32(MVEBU_DRAM_CMD_0_REG, MVEBU_DRAM_CH0_CMD0 |
+		      MVEBU_DRAM_CS_CMD0 | MVEBU_DRAM_WCB_DRAIN_REQ);
+	while ((mmio_read_32(MVEBU_DRAM_STATS_CH0_REG) &
+			     MVEBU_DRAM_WCP_EMPTY) != MVEBU_DRAM_WCP_EMPTY)
+		;
+
+	/* Trigger PHY reset after ddr out of self refresh =>
+	 * supply reset pulse for DDR phy after wake up
+	 */
+	mmio_setbits_32(MVEBU_DRAM_PWR_CTRL_REG, MVEBU_DRAM_PHY_CLK_GATING_EN |
+						 MVEBU_DRAM_PHY_AUTO_AC_OFF_EN);
+}
+
+static void a3700_pwr_dn_avs(void)
+{
+	/*
+	 * AVS power down - controlled by north bridge statemachine
+	 * Enable AVS power down by clear the AVS disable bit.
+	 */
+	mmio_clrbits_32(MVEBU_PM_NB_PWR_OPTION_REG, MVEBU_PM_AVS_DISABLE_MODE);
+	/*
+	 * Should set BIT[12:13] to powerdown AVS.
+	 * 1. Enable AVS VDD2 mode
+	 * 2. After power down AVS, we must hold AVS output voltage.
+	 * 3. We can choose the lower VDD for AVS power down.
+	 */
+	mmio_setbits_32(MVEBU_PM_NB_PWR_OPTION_REG, MVEBU_PM_AVS_VDD2_MODE);
+	mmio_setbits_32(MVEBU_PM_NB_PWR_OPTION_REG, MVEBU_PM_AVS_HOLD_MODE);
+
+	/* Enable low VDD mode, AVS will set CPU to lowest core VDD 747mV */
+	mmio_setbits_32(MVEBU_AVS_CTRL_2_REG, MVEBU_LOW_VDD_MODE_EN);
+}
+
+static void a3700_pwr_dn_tbg(void)
+{
+	/* Power down TBG */
+	mmio_setbits_32(MVEBU_PM_NB_PWR_OPTION_REG, MVEBU_PM_TBG_OFF_EN);
+}
+
+static void a3700_pwr_dn_sb(void)
+{
+	/* Enable south bridge power down option */
+	mmio_setbits_32(MVEBU_PM_NB_PWR_CTRL_REG, MVEBU_PM_SB_PWR_DWN);
+
+	/* Enable SDIO_PHY_PWRDWN */
+	mmio_setbits_32(MVEBU_PM_SB_PWR_OPTION_REG, MVEBU_PM_SDIO_PHY_PDWN_EN);
+
+	/* Enable SRAM LRM on SB */
+	mmio_setbits_32(MVEBU_PM_SB_PWR_OPTION_REG, MVEBU_PM_SB_SRAM_LKG_PD_EN);
+
+	/* Enable SB Power Off */
+	mmio_setbits_32(MVEBU_PM_SB_PWR_OPTION_REG, MVEBU_PM_SB_VDDV_OFF_EN);
+
+	/* Kick off South Bridge Power Off */
+	mmio_setbits_32(MVEBU_PM_SB_CPU_PWR_CTRL_REG, MVEBU_PM_SB_PM_START);
+}
+
+static void a3700_set_pwr_off_option(void)
+{
+	/* Set general power off option */
+	a3700_set_gen_pwr_off_option();
+
+	/* Enable DDR self refresh in low power mode */
+	a3700_en_ddr_self_refresh();
+
+	/* Power down AVS */
+	a3700_pwr_dn_avs();
+
+	/* Power down TBG */
+	a3700_pwr_dn_tbg();
+
+	/* Power down south bridge, pay attention south bridge setting
+	 * should be done before
+	 */
+	a3700_pwr_dn_sb();
+}
+
+static void a3700_set_wake_up_option(void)
+{
+	/*
+	 * Enable the wakeup event for NB SOC => north-bridge
+	 * state-machine enablement on wake-up event
+	 */
+	mmio_setbits_32(MVEBU_PM_NB_WAKE_UP_EN_REG, MVEBU_PM_NB_WKP_EN);
+
+	 /* Enable both core0 and core1 wakeup on demand */
+	mmio_setbits_32(MVEBU_PM_CPU_WAKE_UP_CONF_REG,
+			MVEBU_PM_CORE1_WAKEUP | MVEBU_PM_CORE0_WAKEUP);
+
+	/* Enable warm reset in low power mode */
+	mmio_setbits_32(MVEBU_PM_NB_PWR_OPTION_REG, MVEBU_PM_WARM_RESET_EN);
+}
+
+static void a3700_pm_en_nb_gpio(uint32_t gpio)
+{
+	/* For GPIO1 interrupt -- North bridge only */
+	if (gpio >= 32) {
+		/* GPIO int mask */
+		mmio_clrbits_32(MVEBU_NB_GPIO_IRQ_MASK_2_REG, BIT(gpio - 32));
+
+		/* NB_CPU_WAKE-up ENABLE GPIO int */
+		mmio_setbits_32(MVEBU_NB_GPIO_IRQ_EN_HIGH_REG, BIT(gpio - 32));
+	} else {
+		/* GPIO int mask */
+		mmio_clrbits_32(MVEBU_NB_GPIO_IRQ_MASK_1_REG, BIT(gpio));
+
+		/* NB_CPU_WAKE-up ENABLE GPIO int */
+		mmio_setbits_32(MVEBU_NB_GPIO_IRQ_EN_LOW_REG, BIT(gpio));
+	}
+
+	mmio_setbits_32(MVEBU_NB_STEP_DOWN_INT_EN_REG,
+			MVEBU_NB_GPIO_INT_WAKE_WCPU_CLK);
+
+	/* Enable using GPIO as wakeup event
+	 * (actually not only for north bridge)
+	 */
+	mmio_setbits_32(MVEBU_PM_NB_WAKE_UP_EN_REG, MVEBU_PM_NB_GPIO_WKP_EN |
+		MVEBU_PM_NB_WKP_EN | MVEBU_PM_CORE1_FIQ_IRQ_WKP_EN |
+		MVEBU_PM_CORE0_FIQ_IRQ_WKP_EN);
+}
+
+static void a3700_pm_en_sb_gpio(uint32_t gpio)
+{
+	/* Enable using GPIO as wakeup event */
+	mmio_setbits_32(MVEBU_PM_NB_WAKE_UP_EN_REG, MVEBU_PM_SB_WKP_NB_EN |
+		MVEBU_PM_NB_WKP_EN | MVEBU_PM_CORE1_FIQ_IRQ_WKP_EN |
+		MVEBU_PM_CORE0_FIQ_IRQ_WKP_EN);
+
+	/* SB GPIO Wake UP | South Bridge Wake Up Enable */
+	mmio_setbits_32(MVEBU_PM_SB_WK_EN_REG, MVEBU_PM_SB_GPIO_WKP_EN |
+			MVEBU_PM_SB_GPIO_WKP_EN);
+
+	/* GPIO int mask */
+	mmio_clrbits_32(MVEBU_SB_GPIO_IRQ_MASK_REG, BIT(gpio));
+
+	/* NB_CPU_WAKE-up ENABLE GPIO int */
+	mmio_setbits_32(MVEBU_SB_GPIO_IRQ_EN_REG, BIT(gpio));
+}
+
+int a3700_pm_src_gpio(union pm_wake_up_src_data *src_data)
+{
+	if (src_data->gpio_data.bank_num == 0)
+		/* North Bridge GPIO */
+		a3700_pm_en_nb_gpio(src_data->gpio_data.gpio_num);
+	else
+		a3700_pm_en_sb_gpio(src_data->gpio_data.gpio_num);
+	return 0;
+}
+
+int a3700_pm_src_uart1(union pm_wake_up_src_data *src_data)
+{
+	/* Clear Uart1 select */
+	mmio_clrbits_32(MVEBU_NB_GPIO1_SEL_REG, MVEBU_NB_GPIO1_UART1_SEL);
+	/* set pin 19 gpio usage*/
+	mmio_setbits_32(MVEBU_NB_GPIO1_SEL_REG, MVEBU_NB_GPIO1_GPIO_19_EN);
+	/* Enable gpio wake-up*/
+	a3700_pm_en_nb_gpio(MVEBU_NB_GPIO_19);
+	/* set pin 18 gpio usage*/
+	mmio_setbits_32(MVEBU_NB_GPIO1_SEL_REG, MVEBU_NB_GPIO1_GPIO_18_EN);
+	/* Enable gpio wake-up*/
+	a3700_pm_en_nb_gpio(MVEBU_NB_GPIO_18);
+
+	return 0;
+}
+
+int a3700_pm_src_uart0(union pm_wake_up_src_data *src_data)
+{
+	/* set pin 25/26 gpio usage*/
+	mmio_setbits_32(MVEBU_NB_GPIO1_SEL_REG, MVEBU_NB_GPIO1_GPIO_25_26_EN);
+	/* Enable gpio wake-up*/
+	a3700_pm_en_nb_gpio(MVEBU_NB_GPIO_25);
+	/* Enable gpio wake-up*/
+	a3700_pm_en_nb_gpio(MVEBU_NB_GPIO_26);
+
+	return 0;
+}
+
+struct wake_up_src_func_map src_func_table[WAKE_UP_SRC_MAX] = {
+	{WAKE_UP_SRC_GPIO, a3700_pm_src_gpio},
+	{WAKE_UP_SRC_UART1, a3700_pm_src_uart1},
+	{WAKE_UP_SRC_UART0, a3700_pm_src_uart0},
+	/* FOLLOWING SRC NOT SUPPORTED YET */
+	{WAKE_UP_SRC_TIMER, NULL}
+};
+
+static wake_up_src_func a3700_get_wake_up_src_func(
+						  enum pm_wake_up_src_type type)
+{
+	uint32_t loop;
+
+	for (loop = 0; loop < WAKE_UP_SRC_MAX; loop++) {
+		if (src_func_table[loop].type == type)
+			return src_func_table[loop].func;
+	}
+	return NULL;
+}
+
+static void a3700_set_wake_up_source(void)
+{
+	struct pm_wake_up_src_config *wake_up_src;
+	uint32_t loop;
+	wake_up_src_func src_func = NULL;
+
+	wake_up_src = mv_wake_up_src_config_get();
+	for (loop = 0; loop < wake_up_src->wake_up_src_num; loop++) {
+		src_func = a3700_get_wake_up_src_func(
+			   wake_up_src->wake_up_src[loop].wake_up_src_type);
+		if (src_func)
+			src_func(
+				&(wake_up_src->wake_up_src[loop].wake_up_data));
+	}
+}
+
+static void a3700_pm_save_lp_flag(void)
+{
+	/* Save the flag for enter the low power mode */
+	mmio_setbits_32(MVEBU_PM_CPU_VDD_OFF_INFO_2_REG,
+			MVEBU_PM_LOW_POWER_STATE);
+}
+
+static void a3700_pm_clear_lp_flag(void)
+{
+	/* Clear the flag for enter the low power mode */
+	mmio_clrbits_32(MVEBU_PM_CPU_VDD_OFF_INFO_2_REG,
+			MVEBU_PM_LOW_POWER_STATE);
+}
+
+static uint32_t a3700_pm_get_lp_flag(void)
+{
+	/* Get the flag for enter the low power mode */
+	return mmio_read_32(MVEBU_PM_CPU_VDD_OFF_INFO_2_REG) &
+			    MVEBU_PM_LOW_POWER_STATE;
+}
+
+/*****************************************************************************
+ * A3700 handler called when a power domain is about to be suspended. The
+ * target_state encodes the power state that each level should transition to.
+ *****************************************************************************
+ */
+void a3700_pwr_domain_suspend(const psci_power_state_t *target_state)
+{
+	/* Prevent interrupts from spuriously waking up this cpu */
+	plat_marvell_gic_cpuif_disable();
+
+	/* Save IRQ states */
+	plat_marvell_gic_irq_save();
+
+	/* Set wake up options */
+	a3700_set_wake_up_option();
+
+	/* Set wake up sources */
+	a3700_set_wake_up_source();
+
+	/* SoC can not be powered down with pending IRQ,
+	 * acknowledge all the pending IRQ
+	 */
+	a3700_pm_ack_irq();
+
+	/* Set power off options */
+	a3700_set_pwr_off_option();
+
+	/* Save the flag for enter the low power mode */
+	a3700_pm_save_lp_flag();
+
+	isb();
+}
+
+/*****************************************************************************
+ * A3700 handler called when a power domain has just been powered on after
+ * being turned off earlier. The target_state encodes the low power state that
+ * each level has woken up from.
+ *****************************************************************************
+ */
+void a3700_pwr_domain_on_finish(const psci_power_state_t *target_state)
+{
+	/* arch specific configuration */
+	marvell_psci_arch_init(0);
+
+	/* Per-CPU interrupt initialization */
+	plat_marvell_gic_pcpu_init();
+	plat_marvell_gic_cpuif_enable();
+
+	/* Restore the per-cpu IRQ state */
+	if (a3700_pm_get_lp_flag())
+		plat_marvell_gic_irq_pcpu_restore();
+}
+
+/*****************************************************************************
+ * A3700 handler called when a power domain has just been powered on after
+ * having been suspended earlier. The target_state encodes the low power state
+ * that each level has woken up from.
+ * TODO: At the moment we reuse the on finisher and reinitialize the secure
+ * context. Need to implement a separate suspend finisher.
+ *****************************************************************************
+ */
+void a3700_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
+{
+	struct dec_win_config *io_dec_map;
+	uint32_t dec_win_num;
+	struct dram_win_map dram_wins_map;
+
+	/* arch specific configuration */
+	marvell_psci_arch_init(0);
+
+	/* Interrupt initialization */
+	plat_marvell_gic_init();
+
+	/* Restore IRQ states */
+	plat_marvell_gic_irq_restore();
+
+	/*
+	 * Initialize CCI for this cluster after resume from suspend state.
+	 * No need for locks as no other CPU is active.
+	 */
+	plat_marvell_interconnect_init();
+	/*
+	 * Enable CCI coherency for the primary CPU's cluster.
+	 * Platform specific PSCI code will enable coherency for other
+	 * clusters.
+	 */
+	plat_marvell_interconnect_enter_coherency();
+
+	/* CPU address decoder windows initialization. */
+	cpu_wins_init();
+
+	/* fetch CPU-DRAM window mapping information by reading
+	 * CPU-DRAM decode windows (only the enabled ones)
+	 */
+	dram_win_map_build(&dram_wins_map);
+
+	/* Get IO address decoder windows */
+	if (marvell_get_io_dec_win_conf(&io_dec_map, &dec_win_num)) {
+		printf("No IO address decoder windows configurations found!\n");
+		return;
+	}
+
+	/* IO address decoder init */
+	if (init_io_addr_dec(&dram_wins_map, io_dec_map, dec_win_num)) {
+		printf("IO address decoder windows initialization failed!\n");
+		return;
+	}
+
+	/* Clear low power mode flag */
+	a3700_pm_clear_lp_flag();
+}
+
+/*****************************************************************************
+ * This handler is called by the PSCI implementation during the `SYSTEM_SUSPEND
+ * call to get the `power_state` parameter. This allows the platform to encode
+ * the appropriate State-ID field within the `power_state` parameter which can
+ * be utilized in `pwr_domain_suspend()` to suspend to system affinity level.
+ *****************************************************************************
+ */
+void a3700_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	/* lower affinities use PLAT_MAX_OFF_STATE */
+	for (int i = MPIDR_AFFLVL0; i <= PLAT_MAX_PWR_LVL; i++)
+		req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
+}
+
+/*****************************************************************************
+ * A3700 handlers to shutdown/reboot the system
+ *****************************************************************************
+ */
+static void __dead2 a3700_system_off(void)
+{
+	ERROR("%s needs to be implemented\n", __func__);
+	panic();
+}
+
+/*****************************************************************************
+ * A3700 handlers to reset the system
+ *****************************************************************************
+ */
+static void __dead2 a3700_system_reset(void)
+{
+	/* Clean the mailbox magic number to let it as act like cold boot */
+	mmio_write_32(PLAT_MARVELL_MAILBOX_BASE, 0x0);
+
+	dsbsy();
+
+	/* Flush data cache if the mail box shared RAM is cached */
+#if PLAT_MARVELL_SHARED_RAM_CACHED
+	flush_dcache_range((uintptr_t)PLAT_MARVELL_MAILBOX_BASE,
+			   2 * sizeof(uint64_t));
+#endif
+
+	/* Trigger the warm reset */
+	mmio_write_32(MVEBU_WARM_RESET_REG, MVEBU_WARM_RESET_MAGIC);
+
+	/* Shouldn't get to this point */
+	panic();
+}
+
+/*****************************************************************************
+ * Export the platform handlers via plat_arm_psci_pm_ops. The ARM Standard
+ * platform layer will take care of registering the handlers with PSCI.
+ *****************************************************************************
+ */
+const plat_psci_ops_t plat_arm_psci_pm_ops = {
+	.cpu_standby = a3700_cpu_standby,
+	.pwr_domain_on = a3700_pwr_domain_on,
+	.pwr_domain_off = a3700_pwr_domain_off,
+	.pwr_domain_suspend = a3700_pwr_domain_suspend,
+	.pwr_domain_on_finish = a3700_pwr_domain_on_finish,
+	.pwr_domain_suspend_finish = a3700_pwr_domain_suspend_finish,
+	.get_sys_suspend_power_state = a3700_get_sys_suspend_power_state,
+	.system_off = a3700_system_off,
+	.system_reset = a3700_system_reset,
+	.validate_power_state = a3700_validate_power_state,
+	.validate_ns_entrypoint = a3700_validate_ns_entrypoint
+};
diff --git a/plat/marvell/a8k/a70x0/platform.mk b/plat/marvell/a8k/a70x0/platform.mk
index 29dfd95..d3a0167 100644
--- a/plat/marvell/a8k/a70x0/platform.mk
+++ b/plat/marvell/a8k/a70x0/platform.mk
@@ -7,6 +7,9 @@
 
 PCI_EP_SUPPORT		:= 0
 
+CP_NUM			:= 1
+$(eval $(call add_define,CP_NUM))
+
 DOIMAGE_SEC     	:=	tools/doimage/secure/sec_img_7K.cfg
 
 MARVELL_MOCHI_DRV	:=	drivers/marvell/mochi/apn806_setup.c
diff --git a/plat/marvell/a8k/a70x0_amc/platform.mk b/plat/marvell/a8k/a70x0_amc/platform.mk
index 29dfd95..d3a0167 100644
--- a/plat/marvell/a8k/a70x0_amc/platform.mk
+++ b/plat/marvell/a8k/a70x0_amc/platform.mk
@@ -7,6 +7,9 @@
 
 PCI_EP_SUPPORT		:= 0
 
+CP_NUM			:= 1
+$(eval $(call add_define,CP_NUM))
+
 DOIMAGE_SEC     	:=	tools/doimage/secure/sec_img_7K.cfg
 
 MARVELL_MOCHI_DRV	:=	drivers/marvell/mochi/apn806_setup.c
diff --git a/plat/marvell/a8k/a80x0/board/phy-porting-layer.h b/plat/marvell/a8k/a80x0/board/phy-porting-layer.h
new file mode 100644
index 0000000..da391eb
--- /dev/null
+++ b/plat/marvell/a8k/a80x0/board/phy-porting-layer.h
@@ -0,0 +1,167 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#ifndef __PHY_PORTING_LAYER_H
+#define __PHY_PORTING_LAYER_H
+
+#define MAX_LANE_NR		6
+
+static const struct xfi_params
+	xfi_static_values_tab[AP_NUM][CP_NUM][MAX_LANE_NR] = {
+	/* AP0 */
+	{
+		/* CP 0 */
+		{
+			{ 0 }, /* Comphy0 */
+			{ 0 }, /* Comphy1 */
+			{ .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 = 0x1 }, /* Comphy2 */
+			{ 0 }, /* Comphy3 */
+			{ 0 }, /* Comphy4 */
+			{ 0 }, /* Comphy5 */
+		},
+
+		/* CP 1 */
+		{
+			{ 0 }, /* Comphy0 */
+			{ 0 }, /* Comphy1 */
+			{ .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 = 0x1 }, /* Comphy2 */
+			{ 0 }, /* Comphy3 */
+			{ 0 }, /* Comphy4 */
+			{ 0 }, /* Comphy5 */
+		},
+	},
+};
+
+static const struct sata_params
+	sata_static_values_tab[AP_NUM][CP_NUM][MAX_LANE_NR] = {
+	/* AP0 */
+	{
+		/* CP 0 */
+		{
+			{ 0 }, /* Comphy0 */
+			{ .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,
+			  .valid = 0x1
+			}, /* Comphy1 */
+			{ 0 }, /* Comphy2 */
+			{ .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,
+			 .valid = 0x1
+			}, /* Comphy3 */
+			{ 0 }, /* Comphy4 */
+			{ 0 }, /* Comphy5 */
+		},
+
+		/* CP 1 */
+		{
+			{ 0 }, /* Comphy0 */
+			{ .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,
+			  .valid = 0x1
+			}, /* Comphy1 */
+			{ 0 }, /* Comphy2 */
+			{ .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,
+			  .valid = 0x1
+			}, /* Comphy3 */
+			{ 0 }, /* Comphy4 */
+			{ 0 }, /* Comphy5 */
+
+		},
+	},
+};
+#endif /* __PHY_PORTING_LAYER_H */
diff --git a/plat/marvell/a8k/a80x0/platform.mk b/plat/marvell/a8k/a80x0/platform.mk
index 0fe235b..00d24b2 100644
--- a/plat/marvell/a8k/a80x0/platform.mk
+++ b/plat/marvell/a8k/a80x0/platform.mk
@@ -7,6 +7,9 @@
 
 PCI_EP_SUPPORT		:= 0
 
+CP_NUM			:= 2
+$(eval $(call add_define,CP_NUM))
+
 DOIMAGE_SEC     	:=	tools/doimage/secure/sec_img_8K.cfg
 
 MARVELL_MOCHI_DRV	:=	drivers/marvell/mochi/apn806_setup.c
@@ -14,3 +17,4 @@
 include plat/marvell/a8k/common/a8k_common.mk
 
 include plat/marvell/common/marvell_common.mk
+PLAT_INCLUDES		+=	-Iplat/marvell/a8k/a80x0/board
diff --git a/plat/marvell/a8k/a80x0_mcbin/platform.mk b/plat/marvell/a8k/a80x0_mcbin/platform.mk
index 0fe235b..3749c37 100644
--- a/plat/marvell/a8k/a80x0_mcbin/platform.mk
+++ b/plat/marvell/a8k/a80x0_mcbin/platform.mk
@@ -7,6 +7,9 @@
 
 PCI_EP_SUPPORT		:= 0
 
+CP_NUM			:= 2
+$(eval $(call add_define,CP_NUM))
+
 DOIMAGE_SEC     	:=	tools/doimage/secure/sec_img_8K.cfg
 
 MARVELL_MOCHI_DRV	:=	drivers/marvell/mochi/apn806_setup.c
diff --git a/plat/marvell/a8k/common/a8k_common.mk b/plat/marvell/a8k/common/a8k_common.mk
index 49745ce..a589004 100644
--- a/plat/marvell/a8k/common/a8k_common.mk
+++ b/plat/marvell/a8k/common/a8k_common.mk
@@ -4,7 +4,7 @@
 # SPDX-License-Identifier:     BSD-3-Clause
 # https://spdx.org/licenses
 
-include tools/doimage/doimage.mk
+include tools/marvell/doimage/doimage.mk
 
 PLAT_FAMILY		:= a8k
 PLAT_FAMILY_BASE	:= plat/marvell/$(PLAT_FAMILY)
@@ -25,7 +25,11 @@
 $(eval $(call add_define,PCI_EP_SUPPORT))
 $(eval $(call assert_boolean,PCI_EP_SUPPORT))
 
-DOIMAGEPATH		?=	tools/doimage
+
+AP_NUM			:= 1
+$(eval $(call add_define,AP_NUM))
+
+DOIMAGEPATH		?=	tools/marvell/doimage
 DOIMAGETOOL		?=	${DOIMAGEPATH}/doimage
 
 ROM_BIN_EXT ?= $(BUILD_PLAT)/ble.bin
diff --git a/plat/marvell/a8k/common/include/a8k_plat_def.h b/plat/marvell/a8k/common/include/a8k_plat_def.h
index 1b7e954..55ad002 100644
--- a/plat/marvell/a8k/common/include/a8k_plat_def.h
+++ b/plat/marvell/a8k/common/include/a8k_plat_def.h
@@ -28,7 +28,10 @@
 #define MVEBU_REGS_BASE			0xF0000000
 #define MVEBU_REGS_BASE_MASK		0xF0000000
 #define MVEBU_REGS_BASE_AP(ap)		MVEBU_REGS_BASE
-#define MVEBU_CP_REGS_BASE(cp_index)	(0xF2000000 + (cp_index) * 0x2000000)
+#define MVEBU_AP_IO_BASE(ap)		0xF2000000
+#define MVEBU_CP_OFFSET			0x2000000
+#define MVEBU_CP_REGS_BASE(cp_index)	(MVEBU_AP_IO_BASE(0) + \
+						(cp_index) * MVEBU_CP_OFFSET)
 #define MVEBU_RFU_BASE			(MVEBU_REGS_BASE + 0x6F0000)
 #define MVEBU_IO_WIN_BASE(ap_index)	(MVEBU_RFU_BASE)
 #define MVEBU_IO_WIN_GCR_OFFSET		(0x70)
diff --git a/plat/marvell/common/marvell_gicv3.c b/plat/marvell/common/marvell_gicv3.c
new file mode 100644
index 0000000..7cfefaf
--- /dev/null
+++ b/plat/marvell/common/marvell_gicv3.c
@@ -0,0 +1,208 @@
+/*
+ * Copyright (C) 2018 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <debug.h>
+#include <gicv3.h>
+#include <interrupt_props.h>
+#include <marvell_def.h>
+#include <plat_marvell.h>
+#include <platform.h>
+#include <platform_def.h>
+
+/******************************************************************************
+ * The following functions are defined as weak to allow a platform to override
+ * the way the GICv3 driver is initialised and used.
+ ******************************************************************************
+ */
+#pragma weak plat_marvell_gic_driver_init
+#pragma weak plat_marvell_gic_init
+#pragma weak plat_marvell_gic_cpuif_enable
+#pragma weak plat_marvell_gic_cpuif_disable
+#pragma weak plat_marvell_gic_pcpu_init
+
+/* The GICv3 driver only needs to be initialized in EL3 */
+static uintptr_t rdistif_base_addrs[PLATFORM_CORE_COUNT];
+
+static const interrupt_prop_t marvell_interrupt_props[] = {
+	PLAT_MARVELL_G1S_IRQ_PROPS(INTR_GROUP1S),
+	PLAT_MARVELL_G0_IRQ_PROPS(INTR_GROUP0)
+};
+
+/*
+ * We save and restore the GICv3 context on system suspend. Allocate the
+ * data in the designated EL3 Secure carve-out memory
+ */
+static gicv3_redist_ctx_t rdist_ctx __section("arm_el3_tzc_dram");
+static gicv3_dist_ctx_t dist_ctx __section("arm_el3_tzc_dram");
+
+/*
+ * MPIDR hashing function for translating MPIDRs read from GICR_TYPER register
+ * to core position.
+ *
+ * Calculating core position is dependent on MPIDR_EL1.MT bit. However, affinity
+ * values read from GICR_TYPER don't have an MT field. To reuse the same
+ * translation used for CPUs, we insert MT bit read from the PE's MPIDR into
+ * that read from GICR_TYPER.
+ *
+ * Assumptions:
+ *
+ *   - All CPUs implemented in the system have MPIDR_EL1.MT bit set;
+ *   - No CPUs implemented in the system use affinity level 3.
+ */
+static unsigned int marvell_gicv3_mpidr_hash(u_register_t mpidr)
+{
+	mpidr |= (read_mpidr_el1() & MPIDR_MT_MASK);
+	return plat_marvell_calc_core_pos(mpidr);
+}
+
+const gicv3_driver_data_t marvell_gic_data = {
+	.gicd_base = PLAT_MARVELL_GICD_BASE,
+	.gicr_base = PLAT_MARVELL_GICR_BASE,
+	.interrupt_props = marvell_interrupt_props,
+	.interrupt_props_num = ARRAY_SIZE(marvell_interrupt_props),
+	.rdistif_num = PLATFORM_CORE_COUNT,
+	.rdistif_base_addrs = rdistif_base_addrs,
+	.mpidr_to_core_pos = marvell_gicv3_mpidr_hash
+};
+
+void plat_marvell_gic_driver_init(void)
+{
+	/*
+	 * The GICv3 driver is initialized in EL3 and does not need
+	 * to be initialized again in SEL1. This is because the S-EL1
+	 * can use GIC system registers to manage interrupts and does
+	 * not need GIC interface base addresses to be configured.
+	 */
+#if IMAGE_BL31
+	gicv3_driver_init(&marvell_gic_data);
+#endif
+}
+
+/******************************************************************************
+ * Marvell common helper to initialize the GIC. Only invoked by BL31
+ ******************************************************************************
+ */
+void plat_marvell_gic_init(void)
+{
+	/* Initialize GIC-600 Multi Chip feature,
+	 * only if the maximum number of north bridges
+	 * is more than 1 - otherwise no need for multi
+	 * chip feature initialization
+	 */
+#if (PLAT_MARVELL_NORTHB_COUNT > 1)
+	if (gic600_multi_chip_init())
+		ERROR("GIC-600 Multi Chip initialization failed\n");
+#endif
+	gicv3_distif_init();
+	gicv3_rdistif_init(plat_my_core_pos());
+	gicv3_cpuif_enable(plat_my_core_pos());
+}
+
+/******************************************************************************
+ * Marvell common helper to enable the GIC CPU interface
+ ******************************************************************************
+ */
+void plat_marvell_gic_cpuif_enable(void)
+{
+	gicv3_cpuif_enable(plat_my_core_pos());
+}
+
+/******************************************************************************
+ * Marvell common helper to disable the GIC CPU interface
+ ******************************************************************************
+ */
+void plat_marvell_gic_cpuif_disable(void)
+{
+	gicv3_cpuif_disable(plat_my_core_pos());
+}
+
+/******************************************************************************
+ * Marvell common helper to init. the per-cpu redistributor interface in GICv3
+ ******************************************************************************
+ */
+void plat_marvell_gic_pcpu_init(void)
+{
+	gicv3_rdistif_init(plat_my_core_pos());
+}
+
+/******************************************************************************
+ * Marvell common helper to save SPI irq states in GICv3
+ ******************************************************************************
+ */
+void plat_marvell_gic_irq_save(void)
+{
+
+	/*
+	 * If an ITS is available, save its context before
+	 * the Redistributor using:
+	 * gicv3_its_save_disable(gits_base, &its_ctx[i])
+	 * Additionally, an implementation-defined sequence may
+	 * be required to save the whole ITS state.
+	 */
+
+	/*
+	 * Save the GIC Redistributors and ITS contexts before the
+	 * Distributor context. As we only handle SYSTEM SUSPEND API,
+	 * we only need to save the context of the CPU that is issuing
+	 * the SYSTEM SUSPEND call, i.e. the current CPU.
+	 */
+	gicv3_rdistif_save(plat_my_core_pos(), &rdist_ctx);
+
+	/* Save the GIC Distributor context */
+	gicv3_distif_save(&dist_ctx);
+
+	/*
+	 * From here, all the components of the GIC can be safely powered down
+	 * as long as there is an alternate way to handle wakeup interrupt
+	 * sources.
+	 */
+}
+
+/******************************************************************************
+ * Marvell common helper to restore SPI irq states in GICv3
+ ******************************************************************************
+ */
+void plat_marvell_gic_irq_restore(void)
+{
+	/* Restore the GIC Distributor context */
+	gicv3_distif_init_restore(&dist_ctx);
+
+	/*
+	 * Restore the GIC Redistributor and ITS contexts after the
+	 * Distributor context. As we only handle SYSTEM SUSPEND API,
+	 * we only need to restore the context of the CPU that issued
+	 * the SYSTEM SUSPEND call.
+	 */
+	gicv3_rdistif_init_restore(plat_my_core_pos(), &rdist_ctx);
+
+	/*
+	 * If an ITS is available, restore its context after
+	 * the Redistributor using:
+	 * gicv3_its_restore(gits_base, &its_ctx[i])
+	 * An implementation-defined sequence may be required to
+	 * restore the whole ITS state. The ITS must also be
+	 * re-enabled after this sequence has been executed.
+	 */
+}
+
+/******************************************************************************
+ * Marvell common helper to save per-cpu PPI irq states in GICv3
+ ******************************************************************************
+ */
+void plat_marvell_gic_irq_pcpu_save(void)
+{
+	gicv3_rdistif_save(plat_my_core_pos(), &rdist_ctx);
+}
+
+/******************************************************************************
+ * Marvell common helper to restore per-cpu PPI irq states in GICv3
+ ******************************************************************************
+ */
+void plat_marvell_gic_irq_pcpu_restore(void)
+{
+	gicv3_rdistif_init_restore(plat_my_core_pos(), &rdist_ctx);
+}
diff --git a/plat/marvell/marvell.mk b/plat/marvell/marvell.mk
index 2a2da3b..d8be0dd 100644
--- a/plat/marvell/marvell.mk
+++ b/plat/marvell/marvell.mk
@@ -49,6 +49,6 @@
 
 ${DOIMAGETOOL}: mrvl_clean
 	@$(DOIMAGE_LIBS_CHECK)
-	${Q}${MAKE} --no-print-directory -C ${DOIMAGEPATH} WTMI_IMG=$(WTMI_IMG)
+	${Q}${MAKE} --no-print-directory -C ${DOIMAGEPATH} VERSION=$(SUBVERSION) WTMI_IMG=$(WTMI_IMG)
 
 
diff --git a/plat/mediatek/mt6795/bl31_plat_setup.c b/plat/mediatek/mt6795/bl31_plat_setup.c
index 96a0bd8..8df7dad 100644
--- a/plat/mediatek/mt6795/bl31_plat_setup.c
+++ b/plat/mediatek/mt6795/bl31_plat_setup.c
@@ -339,7 +339,7 @@
 	next_image_info = &bl33_image_ep_info;
 
 	/* Figure out what mode we enter the non-secure world in */
-	if (EL_IMPLEMENTED(2)) {
+	if (el_implemented(2) != EL_IMPL_NONE) {
 		INFO("Kernel_EL2\n");
 		mode = MODE_EL2;
 	} else{
diff --git a/plat/qemu/qemu_bl2_setup.c b/plat/qemu/qemu_bl2_setup.c
index d76621d..b3c3960 100644
--- a/plat/qemu/qemu_bl2_setup.c
+++ b/plat/qemu/qemu_bl2_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -118,7 +118,7 @@
 	unsigned int mode;
 
 	/* Figure out what mode we enter the non-secure world in */
-	mode = EL_IMPLEMENTED(2) ? MODE_EL2 : MODE_EL1;
+	mode = (el_implemented(2) != EL_IMPL_NONE) ? MODE_EL2 : MODE_EL1;
 
 	/*
 	 * TODO: Consider the possibility of specifying the SPSR in
diff --git a/plat/socionext/uniphier/uniphier.h b/plat/socionext/uniphier/uniphier.h
index 7ff1464..89ed362 100644
--- a/plat/socionext/uniphier/uniphier.h
+++ b/plat/socionext/uniphier/uniphier.h
@@ -41,7 +41,6 @@
 int uniphier_usb_init(unsigned int soc, uintptr_t *block_dev_spec);
 
 int uniphier_io_setup(unsigned int soc);
-int uniphier_check_image(unsigned int image_id);
 
 struct image_info;
 struct image_info *uniphier_get_image_info(unsigned int image_id);
diff --git a/plat/socionext/uniphier/uniphier_bl2_setup.c b/plat/socionext/uniphier/uniphier_bl2_setup.c
index f7ae426..fb7997c 100644
--- a/plat/socionext/uniphier/uniphier_bl2_setup.c
+++ b/plat/socionext/uniphier/uniphier_bl2_setup.c
@@ -88,16 +88,6 @@
 		break;
 	}
 
-	if (!skip_scp) {
-		ret = uniphier_check_image(SCP_BL2_IMAGE_ID);
-		if (ret) {
-			WARN("SCP_BL2 image not found. SCP_BL2 load will be skipped.\n");
-			WARN("You must setup SCP by other means.\n");
-			skip_scp = 1;
-			uniphier_bl2_kick_scp = 0;
-		}
-	}
-
 	if (skip_scp) {
 		struct image_info *image_info;
 
diff --git a/plat/socionext/uniphier/uniphier_io_storage.c b/plat/socionext/uniphier/uniphier_io_storage.c
index 1fd835b..5d841ac 100644
--- a/plat/socionext/uniphier/uniphier_io_storage.c
+++ b/plat/socionext/uniphier/uniphier_io_storage.c
@@ -335,21 +335,3 @@
 
 	return io_dev_init(*dev_handle, init_params);
 }
-
-int uniphier_check_image(unsigned int image_id)
-{
-	uintptr_t dev_handle, image_spec, image_handle;
-	int ret;
-
-	ret = plat_get_image_source(image_id, &dev_handle, &image_spec);
-	if (ret)
-		return ret;
-
-	ret = io_open(dev_handle, image_spec, &image_handle);
-	if (ret)
-		return ret;
-
-	io_close(image_handle);
-
-	return 0;
-}
diff --git a/plat/socionext/uniphier/uniphier_scp.c b/plat/socionext/uniphier/uniphier_scp.c
index 9a921c4..58eb72e 100644
--- a/plat/socionext/uniphier/uniphier_scp.c
+++ b/plat/socionext/uniphier/uniphier_scp.c
@@ -12,7 +12,9 @@
 #define UNIPHIER_ROM_RSV3		0x5980120c
 
 #define UNIPHIER_STMBE2COM		0x5f800030
+#define UNIPHIER_STMTOBEIRQ		0x5f800060
 #define UNIPHIER_BETOSTMIRQ0PT		0x5f800070
+#define UNIPHIER_BEIRQCLRPT		0x5f800072
 
 #define UNIPHIER_SCP_READY_MAGIC	0x0000b6a5
 
@@ -59,6 +61,10 @@
 	}
 
 	mmio_write_8(UNIPHIER_BETOSTMIRQ0PT, 0x55);
+
+	while (!(mmio_read_32(UNIPHIER_STMTOBEIRQ) & BIT(1)))
+		;
+	mmio_write_8(UNIPHIER_BEIRQCLRPT, BIT(1) | BIT(0));
 }
 
 static void uniphier_scp_send_cmd(const uint8_t *cmd, int cmd_len)
diff --git a/plat/ti/k3/common/k3_bl31_setup.c b/plat/ti/k3/common/k3_bl31_setup.c
index e438dc3..c0cf5e2 100644
--- a/plat/ti/k3/common/k3_bl31_setup.c
+++ b/plat/ti/k3/common/k3_bl31_setup.c
@@ -10,14 +10,14 @@
 #include <bl_common.h>
 #include <debug.h>
 #include <k3_console.h>
-#include <plat_arm.h>
-#include <platform_def.h>
 #include <k3_gicv3.h>
+#include <platform_def.h>
 #include <string.h>
 #include <ti_sci.h>
+#include <xlat_tables_v2.h>
 
 /* Table of regions to map using the MMU */
-const mmap_region_t plat_arm_mmap[] = {
+const mmap_region_t plat_k3_mmap[] = {
 	MAP_REGION_FLAT(SHARED_RAM_BASE, SHARED_RAM_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
 	MAP_REGION_FLAT(K3_USART_BASE_ADDRESS, K3_USART_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
 	MAP_REGION_FLAT(K3_GICD_BASE, K3_GICD_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
@@ -97,7 +97,6 @@
 
 void bl31_plat_arch_setup(void)
 {
-
 	const mmap_region_t bl_regions[] = {
 		MAP_REGION_FLAT(BL31_BASE, BL31_END - BL31_BASE,
 				MT_MEMORY | MT_RW | MT_SECURE),
@@ -108,7 +107,7 @@
 		{0}
 	};
 
-	setup_page_tables(bl_regions, plat_arm_get_mmap());
+	setup_page_tables(bl_regions, plat_k3_mmap);
 	enable_mmu_el3(0);
 }
 
diff --git a/plat/ti/k3/common/plat_common.mk b/plat/ti/k3/common/plat_common.mk
index d835436..fbd79b4 100644
--- a/plat/ti/k3/common/plat_common.mk
+++ b/plat/ti/k3/common/plat_common.mk
@@ -35,8 +35,6 @@
 
 PLAT_INCLUDES		+=	\
 				-I${PLAT_PATH}/include			\
-				-Iinclude/plat/arm/common/		\
-				-Iinclude/plat/arm/common/aarch64/	\
 				-I${PLAT_PATH}/common/drivers/sec_proxy	\
 				-I${PLAT_PATH}/common/drivers/ti_sci	\
 
@@ -63,7 +61,6 @@
 				${PLAT_PATH}/common/drivers/ti_sci/ti_sci.c \
 
 PLAT_BL_COMMON_SOURCES	+=	\
-				plat/arm/common/arm_common.c		\
 				lib/cpus/aarch64/cortex_a53.S		\
 				${XLAT_TABLES_LIB_SRCS}			\
 				${K3_CONSOLE_SOURCES}			\
diff --git a/services/std_svc/sdei/sdei_private.h b/services/std_svc/sdei/sdei_private.h
index f5197c6..8212667 100644
--- a/services/std_svc/sdei/sdei_private.h
+++ b/services/std_svc/sdei/sdei_private.h
@@ -8,6 +8,7 @@
 #define SDEI_PRIVATE_H
 
 #include <arch_helpers.h>
+#include <context.h>
 #include <context_mgmt.h>
 #include <debug.h>
 #include <errno.h>
diff --git a/tools/doimage/Makefile b/tools/marvell/doimage/Makefile
similarity index 100%
rename from tools/doimage/Makefile
rename to tools/marvell/doimage/Makefile
diff --git a/tools/doimage/doimage.c b/tools/marvell/doimage/doimage.c
similarity index 100%
rename from tools/doimage/doimage.c
rename to tools/marvell/doimage/doimage.c
diff --git a/tools/doimage/doimage.mk b/tools/marvell/doimage/doimage.mk
similarity index 100%
rename from tools/doimage/doimage.mk
rename to tools/marvell/doimage/doimage.mk
diff --git a/tools/doimage/secure/aes_key.txt b/tools/marvell/doimage/secure/aes_key.txt
similarity index 100%
rename from tools/doimage/secure/aes_key.txt
rename to tools/marvell/doimage/secure/aes_key.txt
diff --git a/tools/doimage/secure/csk_priv_pem0.key b/tools/marvell/doimage/secure/csk_priv_pem0.key
similarity index 100%
rename from tools/doimage/secure/csk_priv_pem0.key
rename to tools/marvell/doimage/secure/csk_priv_pem0.key
diff --git a/tools/doimage/secure/csk_priv_pem1.key b/tools/marvell/doimage/secure/csk_priv_pem1.key
similarity index 100%
rename from tools/doimage/secure/csk_priv_pem1.key
rename to tools/marvell/doimage/secure/csk_priv_pem1.key
diff --git a/tools/doimage/secure/csk_priv_pem2.key b/tools/marvell/doimage/secure/csk_priv_pem2.key
similarity index 100%
rename from tools/doimage/secure/csk_priv_pem2.key
rename to tools/marvell/doimage/secure/csk_priv_pem2.key
diff --git a/tools/doimage/secure/csk_priv_pem3.key b/tools/marvell/doimage/secure/csk_priv_pem3.key
similarity index 100%
rename from tools/doimage/secure/csk_priv_pem3.key
rename to tools/marvell/doimage/secure/csk_priv_pem3.key
diff --git a/tools/doimage/secure/kak_priv_pem.key b/tools/marvell/doimage/secure/kak_priv_pem.key
similarity index 100%
rename from tools/doimage/secure/kak_priv_pem.key
rename to tools/marvell/doimage/secure/kak_priv_pem.key
diff --git a/tools/doimage/secure/sec_img_7K.cfg b/tools/marvell/doimage/secure/sec_img_7K.cfg
similarity index 100%
rename from tools/doimage/secure/sec_img_7K.cfg
rename to tools/marvell/doimage/secure/sec_img_7K.cfg
diff --git a/tools/doimage/secure/sec_img_8K.cfg b/tools/marvell/doimage/secure/sec_img_8K.cfg
similarity index 100%
rename from tools/doimage/secure/sec_img_8K.cfg
rename to tools/marvell/doimage/secure/sec_img_8K.cfg