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/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-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/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..a42481f 100644
--- a/plat/marvell/a8k/common/a8k_common.mk
+++ b/plat/marvell/a8k/common/a8k_common.mk
@@ -25,6 +25,10 @@
 $(eval $(call add_define,PCI_EP_SUPPORT))
 $(eval $(call assert_boolean,PCI_EP_SUPPORT))
 
+
+AP_NUM			:= 1
+$(eval $(call add_define,AP_NUM))
+
 DOIMAGEPATH		?=	tools/doimage
 DOIMAGETOOL		?=	${DOIMAGEPATH}/doimage
 
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)