Merge branch 'aosp/upstream-master' into HEAD am: faa476c0caHEADandroid-s-beta-5android-s-beta-4android-s-beta-3android-s-beta-2android-s-beta-1mastermain-cg-testing-releaseandroid-s-beta-5android-s-beta-4

Original change: https://android-review.googlesource.com/c/platform/external/arm-trusted-firmware/+/1589611

MUST ONLY BE SUBMITTED BY AUTOMERGER

Change-Id: I3a25534ceed4f8e188510641080d8b8ed49b8f62

14 files changed, 624 insertions, 205 deletions

diff --git a/drivers/marvell/ap807_clocks_init.c b/drivers/marvell/ap807_clocks_init.c
index 04c256b61..c1f861909 100644
--- a/drivers/marvell/ap807_clocks_init.c
+++ b/drivers/marvell/ap807_clocks_init.c
@@ -39,19 +39,29 @@
 #define AP807_CPU_PLL_PARAM(cluster)	AP807_CPU_PLL_CTRL(cluster)
 #define AP807_CPU_PLL_CFG(cluster)	(AP807_CPU_PLL_CTRL(cluster) + 0x4)
 #define AP807_CPU_PLL_CFG_BYPASS_MODE	(0x1)
+#define AP807_CPU_PLL_FRC_DSCHG		(0x2)
 #define AP807_CPU_PLL_CFG_USE_REG_FILE	(0x1 << 9)
 
 static void pll_set_freq(unsigned int freq_val)
 {
 	int i;
 
+	if (freq_val != PLL_FREQ_2200)
+		return;
+
 	for (i = 0 ; i < AP807_CLUSTER_NUM ; i++) {
+		/* Set parameter of cluster i PLL to 2.2GHz */
+		mmio_write_32(AP807_CPU_PLL_PARAM(i), freq_val);
+		/* Set apll_lpf_frc_dschg - Control
+		 * voltage of internal VCO is discharged
+		 */
 		mmio_write_32(AP807_CPU_PLL_CFG(i),
-			      AP807_CPU_PLL_CFG_USE_REG_FILE);
+				AP807_CPU_PLL_FRC_DSCHG);
+		/* Set use_rf_conf  load PLL parameter from register */
 		mmio_write_32(AP807_CPU_PLL_CFG(i),
-			      AP807_CPU_PLL_CFG_USE_REG_FILE |
-			      AP807_CPU_PLL_CFG_BYPASS_MODE);
-		mmio_write_32(AP807_CPU_PLL_PARAM(i), freq_val);
+				AP807_CPU_PLL_FRC_DSCHG |
+				AP807_CPU_PLL_CFG_USE_REG_FILE);
+		/* Un-set apll_lpf_frc_dschg */
 		mmio_write_32(AP807_CPU_PLL_CFG(i),
 			      AP807_CPU_PLL_CFG_USE_REG_FILE);
 	}
@@ -84,19 +94,16 @@ static void aro_to_pll(void)
  */
 void ap807_clocks_init(unsigned int freq_option)
 {
-	/* Switch from ARO to PLL */
-	aro_to_pll();
-
 	/* Modifications in frequency table:
 	 * 0x0: 764x: change to 2000 MHz.
 	 * 0x2: 744x change to 1800 MHz, 764x change to 2200/2400.
 	 * 0x3: 3900/744x/764x change to 1200 MHz.
 	 */
-	switch (freq_option) {
-	case CPU_2000_DDR_1200_RCLK_1200:
-		pll_set_freq(PLL_FREQ_2000);
-		break;
-	default:
-		break;
-	}
+
+	if (freq_option == CPU_2200_DDR_1200_RCLK_1200)
+		pll_set_freq(PLL_FREQ_2200);
+
+	/* Switch from ARO to PLL */
+	aro_to_pll();
+
 }
diff --git a/drivers/marvell/cache_llc.c b/drivers/marvell/cache_llc.c
index 3df93a43b..4b06b4752 100644
--- a/drivers/marvell/cache_llc.c
+++ b/drivers/marvell/cache_llc.c
@@ -31,19 +31,19 @@ void llc_cache_sync(int ap_index)
 
 void llc_flush_all(int ap_index)
 {
-	mmio_write_32(L2X0_CLEAN_INV_WAY(ap_index), LLC_WAY_MASK);
+	mmio_write_32(LLC_CLEAN_INV_WAY(ap_index), LLC_ALL_WAYS_MASK);
 	llc_cache_sync(ap_index);
 }
 
 void llc_clean_all(int ap_index)
 {
-	mmio_write_32(L2X0_CLEAN_WAY(ap_index), LLC_WAY_MASK);
+	mmio_write_32(LLC_CLEAN_WAY(ap_index), LLC_ALL_WAYS_MASK);
 	llc_cache_sync(ap_index);
 }
 
 void llc_inv_all(int ap_index)
 {
-	mmio_write_32(L2X0_INV_WAY(ap_index), LLC_WAY_MASK);
+	mmio_write_32(LLC_INV_WAY(ap_index), LLC_ALL_WAYS_MASK);
 	llc_cache_sync(ap_index);
 }
 
@@ -109,3 +109,81 @@ void llc_runtime_enable(int ap_index)
 	reg |= (0x1 << CCU_SET_POC_OFFSET);
 	mmio_write_32(CCU_HTC_CR(ap_index), reg);
 }
+
+#if LLC_SRAM
+int llc_sram_enable(int ap_index, int size)
+{
+	uint32_t tc, way, ways_to_allocate;
+	uint32_t way_addr;
+
+	if ((size <= 0) || (size > LLC_SIZE) || (size % LLC_WAY_SIZE))
+		return -1;
+
+	llc_enable(ap_index, 1);
+	llc_inv_all(ap_index);
+
+	ways_to_allocate = size / LLC_WAY_SIZE;
+
+	/* Lockdown all available ways for all traffic classes */
+	for (tc = 0; tc < LLC_TC_NUM; tc++)
+		mmio_write_32(LLC_TCN_LOCK(ap_index, tc), LLC_ALL_WAYS_MASK);
+
+	/* Clear the high bits of SRAM address */
+	mmio_write_32(LLC_BANKED_MNT_AHR(ap_index), 0);
+
+	way_addr = PLAT_MARVELL_TRUSTED_RAM_BASE;
+	for (way = 0; way < ways_to_allocate; way++) {
+		/* Trigger allocation block command */
+		mmio_write_32(LLC_BLK_ALOC(ap_index),
+			      LLC_BLK_ALOC_BASE_ADDR(way_addr) |
+			      LLC_BLK_ALOC_WAY_DATA_SET |
+			      LLC_BLK_ALOC_WAY_ID(way));
+		way_addr += LLC_WAY_SIZE;
+	}
+	return 0;
+}
+
+void llc_sram_disable(int ap_index)
+{
+	uint32_t tc;
+
+	/* Disable the line lockings */
+	for (tc = 0; tc < LLC_TC_NUM; tc++)
+		mmio_write_32(LLC_TCN_LOCK(ap_index, tc), 0);
+
+	/* Invalidate all ways */
+	llc_inv_all(ap_index);
+}
+
+int llc_sram_test(int ap_index, int size, char *msg)
+{
+	uintptr_t addr, end_addr;
+	uint32_t data = 0;
+
+	if ((size <= 0) || (size > LLC_SIZE))
+		return -1;
+
+	INFO("=== LLC SRAM WRITE test %s\n", msg);
+	for (addr = PLAT_MARVELL_TRUSTED_RAM_BASE,
+	     end_addr = PLAT_MARVELL_TRUSTED_RAM_BASE + size;
+	     addr < end_addr; addr += 4) {
+		mmio_write_32(addr, addr);
+	}
+	INFO("=== LLC SRAM WRITE test %s PASSED\n", msg);
+	INFO("=== LLC SRAM READ test %s\n", msg);
+	for (addr = PLAT_MARVELL_TRUSTED_RAM_BASE,
+	     end_addr = PLAT_MARVELL_TRUSTED_RAM_BASE + size;
+	     addr < end_addr; addr += 4) {
+		data = mmio_read_32(addr);
+		if (data != addr) {
+			INFO("=== LLC SRAM READ test %s FAILED @ 0x%08lx)\n",
+			     msg, addr);
+			return -1;
+		}
+	}
+	INFO("=== LLC SRAM READ test %s PASSED (last read = 0x%08x)\n",
+	     msg, data);
+	return 0;
+}
+
+#endif /* LLC_SRAM */
diff --git a/drivers/marvell/ccu.c b/drivers/marvell/ccu.c
index 1e4ab44da..b4251f49b 100644
--- a/drivers/marvell/ccu.c
+++ b/drivers/marvell/ccu.c
@@ -26,10 +26,40 @@
 #define ADDRESS_MASK			(0xFFFFFFF0)
 #define CCU_WIN_ALIGNMENT		(0x100000)
 
+/*
+ * Physical address of the highest address of window bits[31:19] = 0x6FF
+ * Physical address of the lowest address of window bits[18:6] = 0x6E0
+ * Unit Id bits [5:2] = 2
+ * RGF Window Enable bit[0] = 1
+ * 0x37f9b809 - 11011111111 0011011100000 0010 0 1
+ */
+#define ERRATA_WA_CCU_WIN4	0x37f9b809U
+
+/*
+ * Physical address of the highest address of window bits[31:19] = 0xFFF
+ * Physical address of the lowest address of window bits[18:6] = 0x800
+ * Unit Id bits [5:2] = 2
+ * RGF Window Enable bit[0] = 1
+ * 0x7ffa0009 - 111111111111 0100000000000 0010 0 1
+ */
+#define ERRATA_WA_CCU_WIN5	0x7ffa0009U
+
+/*
+ * Physical address of the highest address of window bits[31:19] = 0x1FFF
+ * Physical address of the lowest address of window bits[18:6] = 0x1000
+ * Unit Id bits [5:2] = 2
+ * RGF Window Enable bit[0] = 1
+ * 0xfffc000d - 1111111111111 1000000000000 0011 0 1
+ */
+#define ERRATA_WA_CCU_WIN6	0xfffc000dU
+
 #define IS_DRAM_TARGET(tgt)		((((tgt) == DRAM_0_TID) || \
 					((tgt) == DRAM_1_TID) || \
 					((tgt) == RAR_TID)) ? 1 : 0)
 
+#define CCU_RGF(win)			(MVEBU_CCU_BASE(MVEBU_AP0) + \
+					 0x90 + 4 * (win))
+
 /* For storage of CR, SCR, ALR, AHR abd GCR */
 static uint32_t ccu_regs_save[MVEBU_CCU_MAX_WINS * 4 + 1];
 
@@ -54,8 +84,8 @@ static void dump_ccu(int ap_index)
 							      win_id));
 			start = ((uint64_t)alr << ADDRESS_SHIFT);
 			end = (((uint64_t)ahr + 0x10) << ADDRESS_SHIFT);
-			printf("\tccu    %02x     0x%016llx 0x%016llx\n",
-			       target_id, start, end);
+			printf("\tccu%d    %02x     0x%016llx 0x%016llx\n",
+			       win_id, target_id, start, end);
 		}
 	}
 	win_cr = mmio_read_32(CCU_WIN_GCR_OFFSET(ap_index));
@@ -81,6 +111,12 @@ void ccu_win_check(struct addr_map_win *win)
 	}
 }
 
+int ccu_is_win_enabled(int ap_index, uint32_t win_id)
+{
+	return mmio_read_32(CCU_WIN_CR_OFFSET(ap_index, win_id)) &
+			    WIN_ENABLE_BIT;
+}
+
 void ccu_enable_win(int ap_index, struct addr_map_win *win, uint32_t win_id)
 {
 	uint32_t ccu_win_reg;
@@ -360,3 +396,19 @@ int init_ccu(int ap_index)
 
 	return 0;
 }
+
+void errata_wa_init(void)
+{
+	/*
+	 * EERATA ID: RES-3033912 - Internal Address Space Init state causes
+	 * a hang upon accesses to [0xf070_0000, 0xf07f_ffff]
+	 * Workaround: Boot Firmware (ATF) should configure CCU_RGF_WIN(4) to
+	 * split [0x6e_0000, 0x1ff_ffff] to values [0x6e_0000, 0x6f_ffff] and
+	 * [0x80_0000, 0xff_ffff] and [0x100_0000, 0x1ff_ffff],that cause
+	 * accesses to the segment of [0xf070_0000, 0xf1ff_ffff]
+	 * to act as RAZWI.
+	 */
+	mmio_write_32(CCU_RGF(4), ERRATA_WA_CCU_WIN4);
+	mmio_write_32(CCU_RGF(5), ERRATA_WA_CCU_WIN5);
+	mmio_write_32(CCU_RGF(6), ERRATA_WA_CCU_WIN6);
+}
diff --git a/drivers/marvell/comphy/comphy-cp110.h b/drivers/marvell/comphy/comphy-cp110.h
index 6eb7fd0d2..9b10619ed 100644
--- a/drivers/marvell/comphy/comphy-cp110.h
+++ b/drivers/marvell/comphy/comphy-cp110.h
@@ -116,6 +116,9 @@
 			(0x1 << SD_EXTERNAL_CONFIG0_MEDIA_MODE_OFFSET)
 
 #define SD_EXTERNAL_CONFIG1_REG			0x4
+#define SD_EXTERNAL_CONFIG1_TX_IDLE_OFFSET	2
+#define SD_EXTERNAL_CONFIG1_TX_IDLE_MASK	\
+			(0x1 << SD_EXTERNAL_CONFIG1_TX_IDLE_OFFSET)
 #define SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET	3
 #define SD_EXTERNAL_CONFIG1_RESET_IN_MASK	\
 			(0x1 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET)
@@ -352,6 +355,14 @@
 #define HPIPE_CDR_LOCK_DET_EN_MASK		\
 			(0x1 << HPIPE_CDR_LOCK_DET_EN_OFFSET)
 
+#define HPIPE_SYNC_PATTERN_REG			0x090
+#define HPIPE_SYNC_PATTERN_TXD_INV_OFFSET	10
+#define HPIPE_SYNC_PATTERN_TXD_INV_MASK	\
+	(0x1 << HPIPE_SYNC_PATTERN_TXD_INV_OFFSET)
+#define HPIPE_SYNC_PATTERN_RXD_INV_OFFSET	11
+#define HPIPE_SYNC_PATTERN_RXD_INV_MASK	\
+	(0x1 << HPIPE_SYNC_PATTERN_RXD_INV_OFFSET)
+
 #define HPIPE_INTERFACE_REG			0x94
 #define HPIPE_INTERFACE_GEN_MAX_OFFSET		10
 #define HPIPE_INTERFACE_GEN_MAX_MASK		\
@@ -659,18 +670,32 @@
 			(0x3f << HPIPE_SAVED_DFE_VALUES_SAV_F0D_OFFSET)
 
 #define HPIPE_CDR_CONTROL_REG			0x418
-#define HPIPE_CDR_RX_MAX_DFE_ADAPT_0_OFFSET	14
-#define HPIPE_CDR_RX_MAX_DFE_ADAPT_0_MASK	\
-			(0x3 << HPIPE_CDR_RX_MAX_DFE_ADAPT_0_OFFSET)
-#define HPIPE_CDR_RX_MAX_DFE_ADAPT_1_OFFSET	12
-#define HPIPE_CDR_RX_MAX_DFE_ADAPT_1_MASK	\
-			(0x3 << HPIPE_CDR_RX_MAX_DFE_ADAPT_1_OFFSET)
-#define HPIPE_CDR_MAX_DFE_ADAPT_0_OFFSET	9
-#define HPIPE_CDR_MAX_DFE_ADAPT_0_MASK		\
-			(0x7 << HPIPE_CDR_MAX_DFE_ADAPT_0_OFFSET)
+#define HPIPE_CRD_MIDPOINT_PHASE_OS_OFFSET	0
+#define HPIPE_CRD_MIDPOINT_PHASE_OS_MASK	\
+			(0x3f << HPIPE_CRD_MIDPOINT_PHASE_OS_OFFSET)
 #define HPIPE_CDR_MAX_DFE_ADAPT_1_OFFSET	6
 #define HPIPE_CDR_MAX_DFE_ADAPT_1_MASK		\
 			(0x7 << HPIPE_CDR_MAX_DFE_ADAPT_1_OFFSET)
+#define HPIPE_CDR_MAX_DFE_ADAPT_0_OFFSET	9
+#define HPIPE_CDR_MAX_DFE_ADAPT_0_MASK		\
+			(0x7 << HPIPE_CDR_MAX_DFE_ADAPT_0_OFFSET)
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_1_OFFSET	12
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_1_MASK	\
+			(0x3 << HPIPE_CDR_RX_MAX_DFE_ADAPT_1_OFFSET)
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_0_OFFSET	14
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_0_MASK	\
+			(0x3 << HPIPE_CDR_RX_MAX_DFE_ADAPT_0_OFFSET)
+
+
+#define HPIPE_CDR_CONTROL1_REG			0x41c
+#define HPIPE_CRD2_CRD_MIDPOINT_SMALL_THRES_K_OFF	12
+#define HPIPE_CRD2_CRD_MIDPOINT_SMALL_THRES_K_MASK	\
+			(0xf << HPIPE_CRD2_CRD_MIDPOINT_SMALL_THRES_K_OFF)
+
+#define HPIPE_CDR_CONTROL2_REG			0x420
+#define HPIPE_CRD2_CRD_MIDPOINT_LARGE_THRES_K_OFF	12
+#define HPIPE_CRD2_CRD_MIDPOINT_LARGE_THRES_K_MASK	\
+			(0xf << HPIPE_CRD2_CRD_MIDPOINT_LARGE_THRES_K_OFF)
 
 #define HPIPE_TX_TRAIN_CTRL_11_REG		0x438
 #define HPIPE_TX_STATUS_CHECK_MODE_OFFSET	6
@@ -749,6 +774,30 @@
 #define HPIPE_DFE_CTRL_28_PIPE4_MASK		\
 			(0x1 << HPIPE_DFE_CTRL_28_PIPE4_OFFSET)
 
+#define HPIPE_TRX0_REG				0x4cc /*in doc 0x133*4*/
+#define HPIPE_TRX0_GAIN_TRAIN_WITH_SAMPLER_OFF	2
+#define HPIPE_TRX0_GAIN_TRAIN_WITH_SAMPLER_MASK \
+			(0x1 << HPIPE_TRX0_GAIN_TRAIN_WITH_SAMPLER_OFF)
+#define HPIPE_TRX0_GAIN_TRAIN_WITH_C_OFF	0
+#define HPIPE_TRX0_GAIN_TRAIN_WITH_C_MASK	\
+			(0x1 << HPIPE_TRX0_GAIN_TRAIN_WITH_C_OFF)
+
+#define HPIPE_TRX_REG1				0x4d0 /*in doc 0x134*4*/
+#define HPIPE_TRX_REG1_MIN_BOOST_MODE_OFF	3
+#define HPIPE_TRX_REG1_MIN_BOOST_MODE_MASK	\
+			(0x1 << HPIPE_TRX_REG1_MIN_BOOST_MODE_OFF)
+#define HPIPE_TRX_REG1_SUMFTAP_EN_OFF		10
+#define HPIPE_TRX_REG1_SUMFTAP_EN_MASK		\
+			(0x3f << HPIPE_TRX_REG1_SUMFTAP_EN_OFF)
+
+#define HPIPE_TRX_REG2				0x4d8 /*in doc 0x136*4*/
+#define HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_OFF	11
+#define HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_MASK	\
+			(0x1f << HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_OFF)
+#define HPIPE_TRX_REG2_SUMF_BOOST_TARGET_K_OFF	7
+#define HPIPE_TRX_REG2_SUMF_BOOST_TARGET_K_MASK	\
+			(0xf << HPIPE_TRX_REG2_SUMF_BOOST_TARGET_K_OFF)
+
 #define HPIPE_G1_SETTING_5_REG			0x538
 #define HPIPE_G1_SETTING_5_G1_ICP_OFFSET	0
 #define HPIPE_G1_SETTING_5_G1_ICP_MASK		\
diff --git a/drivers/marvell/comphy/phy-comphy-cp110.c b/drivers/marvell/comphy/phy-comphy-cp110.c
index 384dd39f2..d1c26f8d3 100644
--- a/drivers/marvell/comphy/phy-comphy-cp110.c
+++ b/drivers/marvell/comphy/phy-comphy-cp110.c
@@ -11,6 +11,7 @@
 
 #include <common/debug.h>
 #include <drivers/delay_timer.h>
+#include <mg_conf_cm3/mg_conf_cm3.h>
 #include <lib/mmio.h>
 #include <lib/spinlock.h>
 
@@ -209,8 +210,10 @@ static void mvebu_cp110_comphy_set_phy_selector(uint64_t comphy_base,
 			   * as SFI1/XFI1 available only for CP115.
 			   */
 			if ((mode == COMPHY_SGMII_MODE ||
-			    mode == COMPHY_HS_SGMII_MODE ||
-			    mode == COMPHY_SFI_MODE || mode == COMPHY_XFI_MODE)
+			     mode == COMPHY_HS_SGMII_MODE ||
+			     mode == COMPHY_SFI_MODE ||
+			     mode == COMPHY_XFI_MODE ||
+			     mode == COMPHY_AP_MODE)
 			    && COMPHY_GET_ID(comphy_mode) == 1)
 				reg |= COMMON_SELECTOR_COMPHY4_PORT1 <<
 					comphy_offset;
@@ -320,12 +323,33 @@ int mvebu_cp110_comphy_is_pll_locked(uint64_t comphy_base, uint8_t comphy_index)
 	return ret;
 }
 
+static void mvebu_cp110_polarity_invert(uintptr_t addr, uint8_t phy_polarity_invert)
+{
+	uint32_t mask, data;
+
+	/* Set RX / TX polarity */
+	data = mask = 0x0U;
+	if ((phy_polarity_invert & COMPHY_POLARITY_TXD_INVERT) != 0) {
+		data |= (1 << HPIPE_SYNC_PATTERN_TXD_INV_OFFSET);
+		mask |= HPIPE_SYNC_PATTERN_TXD_INV_MASK;
+		debug("%s: inverting TX polarity\n", __func__);
+	}
+
+	if ((phy_polarity_invert & COMPHY_POLARITY_RXD_INVERT) != 0) {
+		data |= (1 << HPIPE_SYNC_PATTERN_RXD_INV_OFFSET);
+		mask |= HPIPE_SYNC_PATTERN_RXD_INV_MASK;
+		debug("%s: inverting RX polarity\n", __func__);
+	}
+
+	reg_set(addr, data, mask);
+}
+
 static int mvebu_cp110_comphy_sata_power_on(uint64_t comphy_base,
 				     uint8_t comphy_index, uint32_t comphy_mode)
 {
 	uintptr_t hpipe_addr, sd_ip_addr, comphy_addr;
 	uint32_t mask, data;
-	uint8_t ap_nr, cp_nr;
+	uint8_t ap_nr, cp_nr, phy_polarity_invert;
 	int ret = 0;
 
 	debug_enter();
@@ -335,6 +359,7 @@ static int mvebu_cp110_comphy_sata_power_on(uint64_t comphy_base,
 	const struct sata_params *sata_static_values =
 			&sata_static_values_tab[ap_nr][cp_nr][comphy_index];
 
+	phy_polarity_invert = sata_static_values->polarity_invert;
 
 	/* configure phy selector for SATA */
 	mvebu_cp110_comphy_set_phy_selector(comphy_base,
@@ -626,6 +651,11 @@ static int mvebu_cp110_comphy_sata_power_on(uint64_t comphy_base,
 	reg_set(hpipe_addr + HPIPE_PWR_CTR_REG,
 		0x0 << HPIPE_PWR_CTR_RST_DFE_OFFSET,
 		HPIPE_PWR_CTR_RST_DFE_MASK);
+
+	if (phy_polarity_invert != 0)
+		mvebu_cp110_polarity_invert(hpipe_addr + HPIPE_SYNC_PATTERN_REG,
+					    phy_polarity_invert);
+
 	/* SW reset for interrupt logic */
 	reg_set(hpipe_addr + HPIPE_PWR_CTR_REG,
 		0x1 << HPIPE_PWR_CTR_SFT_RST_OFFSET,
@@ -895,11 +925,21 @@ static int mvebu_cp110_comphy_xfi_power_on(uint64_t comphy_base,
 	data = 0x1 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET;
 	mask |= SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
 	data |= 0x1 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET;
+	mask |= SD_EXTERNAL_CONFIG1_TX_IDLE_MASK;
+	data |= 0x1 << SD_EXTERNAL_CONFIG1_TX_IDLE_OFFSET;
 	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
 
 	/* Wait 1ms - until band gap and ref clock ready */
 	mdelay(1);
 
+	/*
+	 * Erratum IPCE_COMPHY-1353: toggle TX_IDLE bit in
+	 * addition to the PHY reset
+	 */
+	mask = SD_EXTERNAL_CONFIG1_TX_IDLE_MASK;
+	data = 0x0U;
+	reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+
 	/* Start comphy Configuration */
 	debug("stage: Comphy configuration\n");
 	/* set reference clock */
@@ -1882,6 +1922,7 @@ static int mvebu_cp110_comphy_usb3_power_on(uint64_t comphy_base,
 {
 	uintptr_t hpipe_addr, comphy_addr, addr;
 	uint32_t mask, data;
+	uint8_t ap_nr, cp_nr, phy_polarity_invert;
 	int ret = 0;
 
 	debug_enter();
@@ -1890,6 +1931,13 @@ static int mvebu_cp110_comphy_usb3_power_on(uint64_t comphy_base,
 	mvebu_cp110_comphy_set_pipe_selector(comphy_base, comphy_index,
 					     comphy_mode);
 
+	mvebu_cp110_get_ap_and_cp_nr(&ap_nr, &cp_nr, comphy_base);
+
+	const struct usb_params *usb_static_values =
+			&usb_static_values_tab[ap_nr][cp_nr][comphy_index];
+
+	phy_polarity_invert = usb_static_values->polarity_invert;
+
 	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
 				comphy_index);
 	comphy_addr = COMPHY_ADDR(comphy_base, comphy_index);
@@ -1969,6 +2017,13 @@ static int mvebu_cp110_comphy_usb3_power_on(uint64_t comphy_base,
 		0x1 << HPIPE_TST_MODE_CTRL_MODE_MARGIN_OFFSET,
 		HPIPE_TST_MODE_CTRL_MODE_MARGIN_MASK);
 
+	/* The polarity inversion for USB was not tested due to lack of hw
+	 * design which requires it. Support is added for customer needs.
+	 */
+	if (phy_polarity_invert)
+		mvebu_cp110_polarity_invert(hpipe_addr + HPIPE_SYNC_PATTERN_REG,
+					    phy_polarity_invert);
+
 	/* Start analog parameters from ETP(HW) */
 	debug("stage: Analog parameters from ETP(HW)\n");
 	/* Set Pin DFE_PAT_DIS -> Bit[1]: PIN_DFE_PAT_DIS = 0x0 */
@@ -2012,12 +2067,58 @@ static int mvebu_cp110_comphy_usb3_power_on(uint64_t comphy_base,
 	return ret;
 }
 
+static void rx_pre_train(uint64_t comphy_base, uint8_t comphy_index)
+{
+	uintptr_t hpipe_addr;
+	uint32_t mask, data;
+
+	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+				comphy_index);
+
+	debug("rx_training preparation\n\n");
+
+	mask = HPIPE_TRX0_GAIN_TRAIN_WITH_C_MASK;
+	data = (0x1 << HPIPE_TRX0_GAIN_TRAIN_WITH_C_OFF);
+	mask |= HPIPE_TRX0_GAIN_TRAIN_WITH_SAMPLER_MASK;
+	data |= (0x0 << HPIPE_TRX0_GAIN_TRAIN_WITH_SAMPLER_OFF);
+	reg_set(hpipe_addr + HPIPE_TRX0_REG, data, mask);
+
+
+	mask = HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_MASK;
+	data = (0x1e << HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_OFF);
+	mask |= HPIPE_TRX_REG2_SUMF_BOOST_TARGET_K_MASK;
+	data |= (0x0 << HPIPE_TRX_REG2_SUMF_BOOST_TARGET_K_OFF);
+	reg_set(hpipe_addr + HPIPE_TRX_REG2, data, mask);
+
+	mask = HPIPE_TRX_REG1_MIN_BOOST_MODE_MASK;
+	data = (0x1 << HPIPE_TRX_REG1_MIN_BOOST_MODE_OFF);
+	reg_set(hpipe_addr + HPIPE_TRX_REG1, data, mask);
+
+	mask = HPIPE_CRD2_CRD_MIDPOINT_SMALL_THRES_K_MASK;
+	data = (0x8 << HPIPE_CRD2_CRD_MIDPOINT_SMALL_THRES_K_OFF);
+	reg_set(hpipe_addr + HPIPE_CDR_CONTROL1_REG, data, mask);
+
+	mask = HPIPE_CRD2_CRD_MIDPOINT_LARGE_THRES_K_MASK;
+	data = (0x8 << HPIPE_CRD2_CRD_MIDPOINT_LARGE_THRES_K_OFF);
+	reg_set(hpipe_addr + HPIPE_CDR_CONTROL2_REG, data, mask);
+
+	mask = HPIPE_CRD_MIDPOINT_PHASE_OS_MASK;
+	data = (0x0 << HPIPE_CRD_MIDPOINT_PHASE_OS_OFFSET);
+	reg_set(hpipe_addr + HPIPE_CDR_CONTROL_REG, data, mask);
+
+	mask = HPIPE_TRX_REG1_SUMFTAP_EN_MASK;
+	data = (0x38 << HPIPE_TRX_REG1_SUMFTAP_EN_OFF);
+	mask |= HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_MASK;
+	data |= (0x1e << HPIPE_TRX_REG2_SUMF_BOOST_TARGET_C_OFF);
+	reg_set(hpipe_addr + HPIPE_TRX_REG1, data, mask);
+}
+
 int mvebu_cp110_comphy_xfi_rx_training(uint64_t comphy_base,
 					      uint8_t comphy_index)
 {
 	uint32_t mask, data, timeout;
 	uint32_t g1_ffe_cap_sel, g1_ffe_res_sel, align90, g1_dfe_res;
-	uintptr_t hpipe_addr, sd_ip_addr;
+	uintptr_t hpipe_addr;
 
 	uint8_t ap_nr, cp_nr;
 
@@ -2025,30 +2126,10 @@ int mvebu_cp110_comphy_xfi_rx_training(uint64_t 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);
+	rx_pre_train(comphy_base, comphy_index);
 
 	debug("Preparation for rx_training\n\n");
 
@@ -2068,34 +2149,10 @@ int mvebu_cp110_comphy_xfi_rx_training(uint64_t comphy_base,
 	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 = 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(10);
-	debug("Enable TX/RX training\n\n");
+	debug("Enable RX training\n\n");
 
 	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 */
@@ -2180,21 +2237,9 @@ int mvebu_cp110_comphy_xfi_rx_training(uint64_t comphy_base,
 	data = 1 << 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 = 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;
+	data = 0x1 << HPIPE_DFE_RES_FORCE_OFFSET;
 	reg_set(hpipe_addr + HPIPE_DFE_REG0, data, mask);
 
 	/* 0x111-G1 DFE_Setting_4 */
@@ -2202,41 +2247,9 @@ int mvebu_cp110_comphy_xfi_rx_training(uint64_t comphy_base,
 	data = g1_dfe_res << HPIPE_G1_SETTINGS_4_G1_DFE_RES_OFFSET;
 	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_4_REG, data, mask);
 
-	debug("PRBS31 loppback\n\n");
-
-	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);
-
-	/* 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 = 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);
-
-	/* 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("# plat/marvell/armada/a8k/<board_type>/board/phy-porting-layer.h ");
 	printf("file\n# with new values as below (for appropriate AP nr %d",
 	       ap_nr);
 	printf("and CP nr: %d comphy_index %d\n\n",
@@ -2252,12 +2265,6 @@ int mvebu_cp110_comphy_xfi_rx_training(uint64_t comphy_base,
 	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;
@@ -2273,12 +2280,17 @@ int mvebu_cp110_comphy_xfi_rx_training(uint64_t comphy_base,
  * the network registers like: MG, AP, MAC, PCS, Serdes etc.)
  */
 static int mvebu_cp110_comphy_ap_power_on(uint64_t comphy_base,
-					  uint8_t comphy_index)
+					  uint8_t comphy_index,
+					  uint32_t comphy_mode)
 {
 	uint32_t mask, data;
+	uint8_t ap_nr, cp_nr;
 	uintptr_t comphy_addr = comphy_addr =
 				COMPHY_ADDR(comphy_base, comphy_index);
 
+	/* configure phy selector for XFI/SFI */
+	mvebu_cp110_comphy_set_phy_selector(comphy_base, comphy_index,
+					    comphy_mode);
 	debug_enter();
 	debug("stage: RFU configurations - hard reset comphy\n");
 	/* RFU configurations - hard reset comphy */
@@ -2289,6 +2301,10 @@ static int mvebu_cp110_comphy_ap_power_on(uint64_t comphy_base,
 	reg_set(comphy_addr + COMMON_PHY_CFG1_REG, data, mask);
 	debug_exit();
 
+	/* Start AP Firmware */
+	mvebu_cp110_get_ap_and_cp_nr(&ap_nr, &cp_nr, comphy_base);
+	mg_start_ap_fw(cp_nr, comphy_index);
+
 	return 0;
 }
 
@@ -2371,7 +2387,8 @@ int mvebu_cp110_comphy_power_on(uint64_t comphy_base, uint8_t comphy_index,
 						       comphy_mode);
 		break;
 	case (COMPHY_AP_MODE):
-		err = mvebu_cp110_comphy_ap_power_on(comphy_base, comphy_index);
+		err = mvebu_cp110_comphy_ap_power_on(comphy_base, comphy_index,
+						     comphy_mode);
 		break;
 	default:
 		ERROR("comphy%d: unsupported comphy mode\n", comphy_index);
diff --git a/drivers/marvell/comphy/phy-comphy-cp110.h b/drivers/marvell/comphy/phy-comphy-cp110.h
index 407909bf7..b4a210242 100644
--- a/drivers/marvell/comphy/phy-comphy-cp110.h
+++ b/drivers/marvell/comphy/phy-comphy-cp110.h
@@ -7,7 +7,7 @@
 
 /* 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
+ * the plat/marvell/armada/a8k/<board_type>/board/phy-porting-layer.h
  */
 struct xfi_params {
 	uint8_t g1_ffe_res_sel;
@@ -76,9 +76,15 @@ struct sata_params {
 	uint8_t g2_rx_selmupi;
 	uint8_t g3_rx_selmupi;
 
+	uint8_t polarity_invert;
+
 	_Bool valid;
 };
 
+struct usb_params {
+	uint8_t polarity_invert;
+};
+
 int mvebu_cp110_comphy_is_pll_locked(uint64_t comphy_base,
 				     uint8_t comphy_index);
 int mvebu_cp110_comphy_power_off(uint64_t comphy_base,
@@ -89,3 +95,7 @@ int mvebu_cp110_comphy_xfi_rx_training(uint64_t comphy_base,
 				       uint8_t comphy_index);
 int mvebu_cp110_comphy_digital_reset(uint64_t comphy_base, uint8_t comphy_index,
 				     uint32_t comphy_mode, uint32_t command);
+
+#define COMPHY_POLARITY_NO_INVERT	0
+#define COMPHY_POLARITY_TXD_INVERT	1
+#define COMPHY_POLARITY_RXD_INVERT	2
diff --git a/drivers/marvell/comphy/phy-default-porting-layer.h b/drivers/marvell/comphy/phy-default-porting-layer.h
index b3ad7eb14..3c63c64b8 100644
--- a/drivers/marvell/comphy/phy-default-porting-layer.h
+++ b/drivers/marvell/comphy/phy-default-porting-layer.h
@@ -45,7 +45,15 @@ static const struct sata_params
 		.g3_rx_selmupf = 0x2,
 		.g1_rx_selmupi = 0x0, .g2_rx_selmupi = 0x0,
 		.g3_rx_selmupi = 0x2,
+		.polarity_invert = COMPHY_POLARITY_NO_INVERT,
 		.valid = 0x1
 	},
 };
+
+static const struct usb_params
+	usb_static_values_tab[AP_NUM][CP_NUM][MAX_LANE_NR] = {
+	[0 ... AP_NUM-1][0 ... CP_NUM-1][0 ... MAX_LANE_NR-1] = {
+		.polarity_invert = COMPHY_POLARITY_NO_INVERT
+	},
+};
 #endif /* PHY_DEFAULT_PORTING_LAYER_H */
diff --git a/drivers/marvell/mci.c b/drivers/marvell/mci.c
index 06fe88e13..2b5470042 100644
--- a/drivers/marvell/mci.c
+++ b/drivers/marvell/mci.c
@@ -571,21 +571,21 @@ static int mci_enable_simultaneous_transactions(int mci_index)
 
 	debug_enter();
 	/* ID assignment (assigning global ID offset to CP) */
-	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(0),
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
 			  MCI_DID_GLOBAL_ASSIGN_REQ_MCI_LOCAL_ID(2) |
 			  MCI_DID_GLOBAL_ASSIGN_REQ_MCI_COUNT(2) |
 			  MCI_DID_GLOBAL_ASSIGN_REQ_HOPS_NUM(2));
-	mci_mmio_write_32(MCI_ACCESS_CMD_REG(0),
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
 			  MCI_INDIRECT_REG_CTRL_ADDR(
 				MCI_DID_GLOBAL_ASSIGNMENT_REQUEST_REG) |
 			  MCI_INDIRECT_CTRL_ASSIGN_CMD);
 	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
 
 	/* Assigning dest. ID=3 to all transactions entering from AXI at AP */
-	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(0),
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
 			  MCI_HB_CTRL_WIN0_DEST_VALID_FLAG(1) |
 			  MCI_HB_CTRL_WIN0_DEST_ID(3));
-	mci_mmio_write_32(MCI_ACCESS_CMD_REG(0),
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
 			  MCI_INDIRECT_REG_CTRL_ADDR(
 				MCI_HB_CTRL_WIN0_DESTINATION_REG_NUM) |
 			  MCI_INDIRECT_CTRL_HOPID(GID_AXI_HB) |
@@ -593,10 +593,10 @@ static int mci_enable_simultaneous_transactions(int mci_index)
 	ret |= mci_poll_command_completion(mci_index, MCI_CMD_WRITE);
 
 	/* Assigning dest. ID=1 to all transactions entering from AXI at CP */
-	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(0),
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index),
 			  MCI_HB_CTRL_WIN0_DEST_VALID_FLAG(1) |
 			  MCI_HB_CTRL_WIN0_DEST_ID(1));
-	mci_mmio_write_32(MCI_ACCESS_CMD_REG(0),
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
 			  MCI_INDIRECT_REG_CTRL_ADDR(
 				MCI_HB_CTRL_WIN0_DESTINATION_REG_NUM) |
 			  MCI_INDIRECT_CTRL_HOPID(GID_IHB_EXT) |
@@ -607,8 +607,8 @@ static int mci_enable_simultaneous_transactions(int mci_index)
 	 * This will lead to get match for any AXI address
 	 * and receive destination ID=3
 	 */
-	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(0), 0xffffffff);
-	mci_mmio_write_32(MCI_ACCESS_CMD_REG(0),
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index), 0xffffffff);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
 			  MCI_INDIRECT_REG_CTRL_ADDR(
 				MCI_HB_CTRL_WIN0_ADDRESS_MASK_REG_NUM) |
 			  MCI_INDIRECT_CTRL_HOPID(GID_AXI_HB) |
@@ -619,8 +619,8 @@ static int mci_enable_simultaneous_transactions(int mci_index)
 	 * This will lead to get match for any AXI address
 	 * and receive destination ID=1
 	 */
-	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(0), 0xffffffff);
-	mci_mmio_write_32(MCI_ACCESS_CMD_REG(0),
+	mci_mmio_write_32(MCI_WRITE_READ_DATA_REG(mci_index), 0xffffffff);
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
 			  MCI_INDIRECT_REG_CTRL_ADDR(
 				MCI_HB_CTRL_WIN0_ADDRESS_MASK_REG_NUM) |
 			  MCI_INDIRECT_CTRL_HOPID(GID_IHB_EXT) |
@@ -653,7 +653,7 @@ static _Bool mci_simulatenous_trans_missing(int mci_index)
 	 * performed by BootROM.
 	 */
 	debug_enter();
-	mci_mmio_write_32(MCI_ACCESS_CMD_REG(0),
+	mci_mmio_write_32(MCI_ACCESS_CMD_REG(mci_index),
 			  MCI_INDIRECT_REG_CTRL_ADDR(
 				MCI_HB_CTRL_WIN0_DESTINATION_REG_NUM) |
 			  MCI_INDIRECT_CTRL_HOPID(GID_AXI_HB) |
@@ -697,7 +697,8 @@ int mci_configure(int mci_index)
 	 * wasn't already enabled in bootrom.
 	 */
 	if (mci_simulatenous_trans_missing(mci_index)) {
-		VERBOSE("Enabling MCI simultaneous transaction\n");
+		VERBOSE("Enabling MCI simultaneous transaction for mci%d\n",
+		       mci_index);
 		/* set MCI to support read/write transactions
 		 * to arrive at the same time
 		 */
@@ -819,7 +820,7 @@ void mci_turn_link_on(void)
 }
 
 /* Initialize MCI for performance improvements */
-int mci_initialize(int mci_index)
+int mci_link_tune(int mci_index)
 {
 	int ret;
 
diff --git a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
new file mode 100644
index 000000000..98e189687
--- /dev/null
+++ b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2019 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <a8k_plat_def.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <mss_scp_bl2_format.h>
+
+/* CONFI REGISTERS */
+#define MG_CM3_CONFI_BASE(CP)		(MVEBU_CP_REGS_BASE(CP) + 0x100000)
+#define MG_CM3_SRAM_BASE(CP)		MG_CM3_CONFI_BASE(CP)
+#define MG_CM3_CONFI_GLOB_CFG_REG(CP)	(MG_CM3_CONFI_BASE(CP) + 0x2B500)
+#define CM3_CPU_EN_BIT			BIT(28)
+#define MG_CM3_MG_INT_MFX_REG(CP)	(MG_CM3_CONFI_BASE(CP) + 0x2B054)
+#define CM3_SYS_RESET_BIT		BIT(0)
+
+#define MG_CM3_SHARED_MEM_BASE(CP)	(MG_CM3_SRAM_BASE(CP) + 0x1FC00ULL)
+
+#define MG_SRAM_SIZE	0x20000 /* 128KB */
+
+#define MG_ACK_TIMEOUT 10
+
+/**
+ * struct ap_sharedmem_ctrl - used to pass information between the HOST and CM3
+ * @init_done:	Set by CM3 when ap_proces initialzied. Host check if CM3 set
+ *		this flag to confirm that the process is running
+ * @lane_nr:	Set by Host to mark which comphy lane should be configure. E.g.:
+ *		- A8K development board uses comphy lane 2 for eth0
+ *		- CN913x development board uses comphy lane 4 for eth0
+ */
+struct ap_sharedmem_ctrl {
+	uint32_t init_done;
+	uint32_t lane_nr;
+};
+
+int mg_image_load(uintptr_t src_addr, uint32_t size, int cp_index)
+{
+	uintptr_t mg_regs = MG_CM3_SRAM_BASE(cp_index);
+
+	if (size > MG_SRAM_SIZE) {
+		ERROR("image is too big to fit into MG CM3 memory\n");
+		return 1;
+	}
+
+	NOTICE("Loading MG image from address 0x%lx Size 0x%x to MG at 0x%lx\n",
+	       src_addr, size, mg_regs);
+
+	/* Copy image to MG CM3 SRAM */
+	memcpy((void *)mg_regs, (void *)src_addr, size);
+
+	/* Don't release MG CM3 from reset - it will be done by next step
+	 * bootloader (e.g. U-Boot), when appriopriate device-tree setup (which
+	 * has enabeld 802.3. auto-neg) will be choosen.
+	 */
+
+	return 0;
+}
+
+void mg_start_ap_fw(int cp_nr, uint8_t comphy_index)
+{
+	volatile struct ap_sharedmem_ctrl *ap_shared_ctrl =
+					(void *)MG_CM3_SHARED_MEM_BASE(cp_nr);
+	int timeout = MG_ACK_TIMEOUT;
+
+	if (mmio_read_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr)) & CM3_CPU_EN_BIT) {
+		VERBOSE("cm3 already running\n");
+		return;  /* cm3 already running */
+	}
+
+	/*
+	 * Mark which comphy lane should be used - it will be read via shared
+	 * mem by ap process
+	 */
+	ap_shared_ctrl->lane_nr = comphy_index;
+	/* Make sure it took place before enabling cm3 */
+	dmbst();
+
+	mmio_setbits_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr), CM3_CPU_EN_BIT);
+	mmio_setbits_32(MG_CM3_MG_INT_MFX_REG(cp_nr), CM3_SYS_RESET_BIT);
+
+	/* Check for ap process initialization by fw */
+	while (ap_shared_ctrl->init_done != 1 && timeout--)
+		VERBOSE("Waiting for ap process ack, timeout %d\n", timeout);
+
+	if (timeout == 0) {
+		ERROR("AP process failed, disabling cm3\n");
+		mmio_clrbits_32(MG_CM3_MG_INT_MFX_REG(cp_nr),
+				CM3_SYS_RESET_BIT);
+		mmio_clrbits_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr),
+				CM3_CPU_EN_BIT);
+	}
+}
diff --git a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
new file mode 100644
index 000000000..e2756de9c
--- /dev/null
+++ b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
@@ -0,0 +1,9 @@
+/*
+ * Copyright (C) 2019 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+void mg_start_ap_fw(int cp_nr, uint8_t comphy_index);
+int mg_image_load(uintptr_t src_addr, uint32_t size, int cp_index);
diff --git a/drivers/marvell/mochi/ap807_setup.c b/drivers/marvell/mochi/ap807_setup.c
index 864c9230a..1069f8cef 100644
--- a/drivers/marvell/mochi/ap807_setup.c
+++ b/drivers/marvell/mochi/ap807_setup.c
@@ -11,6 +11,7 @@
 #include <drivers/marvell/cache_llc.h>
 #include <drivers/marvell/ccu.h>
 #include <drivers/marvell/io_win.h>
+#include <drivers/marvell/iob.h>
 #include <drivers/marvell/mci.h>
 #include <drivers/marvell/mochi/ap_setup.h>
 #include <lib/mmio.h>
@@ -31,6 +32,11 @@
 #define DSS_CR0					(MVEBU_RFU_BASE + 0x100)
 #define DVM_48BIT_VA_ENABLE			(1 << 21)
 
+
+/* SoC RFU / IHBx4 Control */
+#define MCIX4_807_REG_START_ADDR_REG(unit_id)	(MVEBU_RFU_BASE + \
+						0x4258 + (unit_id * 0x4))
+
 /* Secure MoChi incoming access */
 #define SEC_MOCHI_IN_ACC_REG			(MVEBU_RFU_BASE + 0x4738)
 #define SEC_MOCHI_IN_ACC_IHB0_EN		(1)
@@ -41,6 +47,14 @@
 						 SEC_MOCHI_IN_ACC_IHB1_EN | \
 						 SEC_MOCHI_IN_ACC_IHB2_EN | \
 						 SEC_MOCHI_IN_ACC_PIDI_EN)
+#define MOCHI_IN_ACC_LEVEL_FORCE_NONSEC		(0)
+#define MOCHI_IN_ACC_LEVEL_FORCE_SEC		(1)
+#define MOCHI_IN_ACC_LEVEL_LEAVE_ORIG		(2)
+#define MOCHI_IN_ACC_LEVEL_MASK_ALL		(3)
+#define SEC_MOCHI_IN_ACC_IHB0_LEVEL(l)		((l) << 1)
+#define SEC_MOCHI_IN_ACC_IHB1_LEVEL(l)		((l) << 4)
+#define SEC_MOCHI_IN_ACC_PIDI_LEVEL(l)		((l) << 10)
+
 
 /* SYSRST_OUTn Config definitions */
 #define MVEBU_SYSRST_OUT_CONFIG_REG		(MVEBU_MISC_SOC_BASE + 0x4)
@@ -65,19 +79,36 @@ enum axi_attr {
 
 static void ap_sec_masters_access_en(uint32_t enable)
 {
-	uint32_t reg;
-
 	/* Open/Close incoming access for all masters.
 	 * The access is disabled in trusted boot mode
 	 * Could only be done in EL3
 	 */
-	reg = mmio_read_32(SEC_MOCHI_IN_ACC_REG);
-	if (enable)
-		mmio_write_32(SEC_MOCHI_IN_ACC_REG, reg |
-			      SEC_IN_ACCESS_ENA_ALL_MASTERS);
-	else
-		mmio_write_32(SEC_MOCHI_IN_ACC_REG,
-			      reg & ~SEC_IN_ACCESS_ENA_ALL_MASTERS);
+	if (enable != 0) {
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG, 0x0U, /* no clear */
+				   SEC_IN_ACCESS_ENA_ALL_MASTERS);
+#if LLC_SRAM
+		/* Do not change access security level
+		 * for PIDI masters
+		 */
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG,
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_MASK_ALL),
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_LEAVE_ORIG));
+#endif
+	} else {
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG,
+				   SEC_IN_ACCESS_ENA_ALL_MASTERS,
+				   0x0U /* no set */);
+#if LLC_SRAM
+		/* Return PIDI access level to the default */
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG,
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_MASK_ALL),
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_FORCE_NONSEC));
+#endif
+	}
 }
 
 static void setup_smmu(void)
@@ -111,6 +142,8 @@ static void init_aurora2(void)
 	reg |= (0x1 << CCU_SET_POC_OFFSET);
 	mmio_write_32(CCU_HTC_CR, reg);
 #endif /* LLC_ENABLE */
+
+	errata_wa_init();
 }
 
 
@@ -124,7 +157,7 @@ static void mci_remap_indirect_access_base(void)
 	uint32_t mci;
 
 	for (mci = 0; mci < MCI_MAX_UNIT_ID; mci++)
-		mmio_write_32(MCIX4_REG_START_ADDRESS_REG(mci),
+		mmio_write_32(MCIX4_807_REG_START_ADDR_REG(mci),
 				  MVEBU_MCI_REG_BASE_REMAP(mci) >>
 				  MCI_REMAP_OFF_SHIFT);
 }
@@ -186,6 +219,38 @@ static void misc_soc_configurations(void)
 	mmio_write_32(MVEBU_SYSRST_OUT_CONFIG_REG, reg);
 }
 
+/*
+ * By default all external CPs start with configuration address space set to
+ * 0xf200_0000. To overcome this issue, go in the loop and initialize the
+ * CP one by one, using temporary window configuration which allows to access
+ * each CP and update its configuration space according to decoding
+ * windows scheme defined for each platform.
+ */
+void update_cp110_default_win(int cp_id)
+{
+	int mci_id = cp_id - 1;
+	uintptr_t cp110_base, cp110_temp_base;
+
+	/* CP110 default configuration address space */
+	cp110_temp_base = MVEBU_AP_IO_BASE(MVEBU_AP0);
+
+	struct addr_map_win iowin_temp_win = {
+		.base_addr = cp110_temp_base,
+		.win_size = MVEBU_CP_OFFSET,
+	};
+
+	iowin_temp_win.target_id = mci_id;
+	iow_temp_win_insert(0, &iowin_temp_win, 1);
+
+	/* Calculate the new CP110 - base address */
+	cp110_base = MVEBU_CP_REGS_BASE(cp_id);
+	/* Go and update the CP110 configuration address space */
+	iob_cfg_space_update(0, cp_id, cp110_temp_base, cp110_base);
+
+	/* Remove the temporary IO-WIN window */
+	iow_temp_win_remove(0, &iowin_temp_win, 1);
+}
+
 void ap_init(void)
 {
 	/* Setup Aurora2. */
diff --git a/drivers/marvell/mochi/apn806_setup.c b/drivers/marvell/mochi/apn806_setup.c
index 1e91c4317..8c3ba9296 100644
--- a/drivers/marvell/mochi/apn806_setup.c
+++ b/drivers/marvell/mochi/apn806_setup.c
@@ -28,9 +28,6 @@
 							0x200)
 #define CCU_SET_POC_OFFSET			5
 
-#define CCU_RGF(win)				(MVEBU_CCU_BASE(MVEBU_AP0) + \
-							0x90 + 4 * (win))
-
 #define DSS_CR0					(MVEBU_RFU_BASE + 0x100)
 #define DVM_48BIT_VA_ENABLE			(1 << 21)
 
@@ -44,6 +41,14 @@
 						 SEC_MOCHI_IN_ACC_IHB1_EN | \
 						 SEC_MOCHI_IN_ACC_IHB2_EN | \
 						 SEC_MOCHI_IN_ACC_PIDI_EN)
+#define MOCHI_IN_ACC_LEVEL_FORCE_NONSEC		(0)
+#define MOCHI_IN_ACC_LEVEL_FORCE_SEC		(1)
+#define MOCHI_IN_ACC_LEVEL_LEAVE_ORIG		(2)
+#define MOCHI_IN_ACC_LEVEL_MASK_ALL		(3)
+#define SEC_MOCHI_IN_ACC_IHB0_LEVEL(l)		((l) << 1)
+#define SEC_MOCHI_IN_ACC_IHB1_LEVEL(l)		((l) << 4)
+#define SEC_MOCHI_IN_ACC_PIDI_LEVEL(l)		((l) << 10)
+
 
 /* SYSRST_OUTn Config definitions */
 #define MVEBU_SYSRST_OUT_CONFIG_REG		(MVEBU_MISC_SOC_BASE + 0x4)
@@ -70,19 +75,36 @@ enum axi_attr {
 
 static void apn_sec_masters_access_en(uint32_t enable)
 {
-	uint32_t reg;
-
 	/* Open/Close incoming access for all masters.
 	 * The access is disabled in trusted boot mode
 	 * Could only be done in EL3
 	 */
-	reg = mmio_read_32(SEC_MOCHI_IN_ACC_REG);
-	if (enable)
-		mmio_write_32(SEC_MOCHI_IN_ACC_REG, reg |
+	if (enable != 0) {
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG, 0x0U, /* no clear */
 			      SEC_IN_ACCESS_ENA_ALL_MASTERS);
-	else
-		mmio_write_32(SEC_MOCHI_IN_ACC_REG, reg &
-			      ~SEC_IN_ACCESS_ENA_ALL_MASTERS);
+#if LLC_SRAM
+		/* Do not change access security level
+		 * for PIDI masters
+		 */
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG,
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_MASK_ALL),
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_LEAVE_ORIG));
+#endif
+	} else {
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG,
+				   SEC_IN_ACCESS_ENA_ALL_MASTERS,
+				   0x0U /* no set */);
+#if LLC_SRAM
+		/* Return PIDI access level to the default */
+		mmio_clrsetbits_32(SEC_MOCHI_IN_ACC_REG,
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_MASK_ALL),
+				   SEC_MOCHI_IN_ACC_PIDI_LEVEL(
+					  MOCHI_IN_ACC_LEVEL_FORCE_NONSEC));
+#endif
+	}
 }
 
 static void setup_smmu(void)
@@ -95,20 +117,6 @@ static void setup_smmu(void)
 	mmio_write_32(SMMU_sACR, reg);
 }
 
-static void apn806_errata_wa_init(void)
-{
-	/*
-	 * ERRATA ID: RES-3033912 - Internal Address Space Init state causes
-	 * a hang upon accesses to [0xf070_0000, 0xf07f_ffff]
-	 * Workaround: Boot Firmware (ATF) should configure CCU_RGF_WIN(4) to
-	 * split [0x6e_0000, 0xff_ffff] to values [0x6e_0000, 0x6f_ffff] and
-	 * [0x80_0000, 0xff_ffff] that cause accesses to the
-	 * segment of [0xf070_0000, 0xf07f_ffff] to act as RAZWI.
-	 */
-	mmio_write_32(CCU_RGF(4), 0x37f9b809);
-	mmio_write_32(CCU_RGF(5), 0x7ffa0009);
-}
-
 static void init_aurora2(void)
 {
 	uint32_t reg;
@@ -131,7 +139,7 @@ static void init_aurora2(void)
 	mmio_write_32(CCU_HTC_CR, reg);
 #endif /* LLC_ENABLE */
 
-	apn806_errata_wa_init();
+	errata_wa_init();
 }
 
 
@@ -250,3 +258,6 @@ int ap_get_count(void)
 	return 1;
 }
 
+void update_cp110_default_win(int cp_id)
+{
+}
diff --git a/drivers/marvell/mochi/cp110_setup.c b/drivers/marvell/mochi/cp110_setup.c
index b4b4e0c82..0fa049764 100644
--- a/drivers/marvell/mochi/cp110_setup.c
+++ b/drivers/marvell/mochi/cp110_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018 Marvell International Ltd.
+ * Copyright (C) 2018-2020 Marvell International Ltd.
  *
  * SPDX-License-Identifier:     BSD-3-Clause
  * https://spdx.org/licenses
@@ -130,6 +130,7 @@ enum axi_attr {
 #define USB3H_1_STREAM_ID_REG	(RFU_STREAM_ID_BASE + 0x10)
 #define SATA_0_STREAM_ID_REG	(RFU_STREAM_ID_BASE + 0x14)
 #define SATA_1_STREAM_ID_REG	(RFU_STREAM_ID_BASE + 0x18)
+#define SDIO_0_STREAM_ID_REG	(RFU_STREAM_ID_BASE + 0x28)
 
 #define CP_DMA_0_STREAM_ID_REG  (0x6B0010)
 #define CP_DMA_1_STREAM_ID_REG  (0x6D0010)
@@ -144,6 +145,7 @@ uintptr_t stream_id_reg[] = {
 	CP_DMA_1_STREAM_ID_REG,
 	SATA_0_STREAM_ID_REG,
 	SATA_1_STREAM_ID_REG,
+	SDIO_0_STREAM_ID_REG,
 	0
 };
 
@@ -303,7 +305,7 @@ static void cp110_axi_attr_init(uintptr_t base)
 			      DOMAIN_SYSTEM_SHAREABLE);
 }
 
-static void amb_bridge_init(uintptr_t base)
+void cp110_amb_init(uintptr_t base)
 {
 	uint32_t reg;
 
@@ -399,7 +401,7 @@ void cp110_init(uintptr_t cp110_base, uint32_t stream_id)
 	cp110_stream_id_init(cp110_base, stream_id);
 
 	/* Open AMB bridge for comphy for CP0 & CP1*/
-	amb_bridge_init(cp110_base);
+	cp110_amb_init(cp110_base);
 
 	/* Reset RTC if needed */
 	cp110_rtc_init(cp110_base);
@@ -411,7 +413,7 @@ void cp110_ble_init(uintptr_t cp110_base)
 #if PCI_EP_SUPPORT
 	INFO("%s: Initialize CPx - base = %lx\n", __func__, cp110_base);
 
-	amb_bridge_init(cp110_base);
+	cp110_amb_init(cp110_base);
 
 	/* Configure PCIe clock */
 	cp110_pcie_clk_cfg(cp110_base);
diff --git a/drivers/marvell/uart/a3700_console.S b/drivers/marvell/uart/a3700_console.S
index da1ce351c..58dad7aa5 100644
--- a/drivers/marvell/uart/a3700_console.S
+++ b/drivers/marvell/uart/a3700_console.S
@@ -60,14 +60,14 @@ func console_a3700_core_init
 	str	w3, [x0, #UART_POSSR_REG]
 
 	/*
-	 * Wait for the TX FIFO to be empty. If wait for 20ms, the TX FIFO is
+	 * Wait for the TX (THR and TSR) 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 */
+	/* Check whether TX (THR and TSR) is empty */
 	ldr	w3, [x0, #UART_STATUS_REG]
-	and	w3, w3, #UARTLSR_TXFIFOEMPTY
+	and	w3, w3, #UARTLSR_TXEMPTY
 	cmp	w3, #0
 	b.ne	4f
 
@@ -110,7 +110,7 @@ endfunc console_a3700_core_init
 	.globl console_a3700_register
 
 	/* -----------------------------------------------
-	 * int console_a3700_register(console_16550_t *console,
+	 * int console_a3700_register(console_t *console,
 		uintptr_t base, uint32_t clk, uint32_t baud)
 	 * Function to initialize and register a new a3700
 	 * console. Storage passed in for the console struct
@@ -118,7 +118,7 @@ endfunc console_a3700_core_init
 	 * In: x0 - UART register base address
 	 *     w1 - UART clock in Hz
 	 *     w2 - Baud rate
-	 *     x3 - pointer to empty console_a3700_t struct
+	 *     x3 - pointer to empty console_t struct
 	 * Out: return 1 on success, 0 on error
 	 * Clobber list : x0, x1, x2, x6, x7, x14
 	 * -----------------------------------------------
@@ -127,7 +127,7 @@ func console_a3700_register
 	mov	x7, x30
 	mov	x6, x3
 	cbz	x6, register_fail
-	str	x0, [x6, #CONSOLE_T_A3700_BASE]
+	str	x0, [x6, #CONSOLE_T_BASE]
 
 	bl	console_a3700_core_init
 	cbz	x0, register_fail
@@ -178,7 +178,7 @@ putc_error:
 endfunc console_a3700_core_putc
 
 	/* --------------------------------------------------------
-	 * int console_a3700_putc(int c, console_a3700_t *console)
+	 * int console_a3700_putc(int c, console_t *console)
 	 * Function to output a character over the console. It
 	 * returns the character printed on success or -1 on error.
 	 * In : w0 - character to be printed
@@ -188,7 +188,7 @@ endfunc console_a3700_core_putc
 	 * --------------------------------------------------------
 	 */
 func console_a3700_putc
-	ldr	x1, [x1, #CONSOLE_T_A3700_BASE]
+	ldr	x1, [x1, #CONSOLE_T_BASE]
 	b	console_a3700_core_putc
 endfunc console_a3700_putc
 
@@ -196,19 +196,28 @@ endfunc console_a3700_putc
 	 * int console_a3700_core_getc(void)
 	 * Function to get a character from the console.
 	 * It returns the character grabbed on success
-	 * or -1 on error.
+	 * or -1 if no character is available.
 	 * In : w0 - console base address
-	 * Out : return -1 on error else return character.
+	 * Out : w0 - character if available, else -1
 	 * Clobber list : x0, x1
 	 * ---------------------------------------------
 	 */
 func console_a3700_core_getc
-	mov	w0, #-1
+	/* Check if there is a pending character */
+	ldr	w1, [x0, #UART_STATUS_REG]
+	and	w1, w1, #UARTLSR_RXRDY
+	cmp	w1, #UARTLSR_RXRDY
+	b.ne	getc_no_char
+	ldr	w0, [x0, #UART_RX_REG]
+	and	w0, w0, #0xff
+	ret
+getc_no_char:
+	mov	w0, #ERROR_NO_PENDING_CHAR
 	ret
 endfunc console_a3700_core_getc
 
 	/* ---------------------------------------------
-	 * int console_a3700_getc(console_a3700_t *console)
+	 * int console_a3700_getc(console_t *console)
 	 * Function to get a character from the console.
 	 * It returns the character grabbed on success
 	 * or -1 on if no character is available.
@@ -218,35 +227,39 @@ endfunc console_a3700_core_getc
 	 * ---------------------------------------------
 	 */
 func console_a3700_getc
-	ldr	x0, [x0, #CONSOLE_T_A3700_BASE]
+	ldr	x0, [x0, #CONSOLE_T_BASE]
 	b	console_a3700_core_getc
 endfunc console_a3700_getc
 
 	/* ---------------------------------------------
-	 * int console_a3700_core_flush(uintptr_t base_addr)
+	 * void console_a3700_core_flush(uintptr_t base_addr)
 	 * Function to force a write of all buffered
 	 * data that hasn't been output.
 	 * In : x0 - console base address
-	 * Out : return -1 on error else return 0.
+	 * Out : void.
 	 * Clobber list : x0, x1
 	 * ---------------------------------------------
 	 */
 func console_a3700_core_flush
-	mov	w0, #0
+	/* Wait for the TX (THR and TSR) to be empty */
+1:	ldr	w1, [x0, #UART_STATUS_REG]
+	and	w1, w1, #UARTLSR_TXEMPTY
+	cmp	w1, #UARTLSR_TXEMPTY
+	b.ne	1b
 	ret
 endfunc console_a3700_core_flush
 
 	/* ---------------------------------------------
-	 * int console_a3700_flush(console_a3700_t *console)
+	 * void console_a3700_flush(console_t *console)
 	 * Function to force a write of all buffered
 	 * data that hasn't been output.
 	 * In : x0 - pointer to console_t structure
-	 * Out : return -1 on error else return 0.
+	 * Out : void.
 	 * Clobber list : x0, x1
 	 * ---------------------------------------------
 	 */
 func console_a3700_flush
-	ldr	x0, [x0, #CONSOLE_T_A3700_BASE]
+	ldr	x0, [x0, #CONSOLE_T_BASE]
 	b	console_a3700_core_flush
 endfunc console_a3700_flush