phy: marvell: cp110: add 5G XFI mode

This patch adds the option to configure a comphy to 5G XFI mode.

In order to configure the comphy to 5G XFI, update
the comphy node in the device-tree:
	phy2 {
		phy-type = <PHY_TYPE_SFI>;
		phy-speed = <PHY_SPEED_5_15625G>;
	};

Signed-off-by: Igal Liberman <igall@marvell.com>
Signed-off-by: Stefan Roese <sr@denx.de>
Reviewed-by: Stefan Roese <sr@denx.de>

3 files changed, 56 insertions, 11 deletions

diff --git a/drivers/phy/marvell/comphy_cp110.c b/drivers/phy/marvell/comphy_cp110.c
index 6a6083b..056de83 100644
--- a/drivers/phy/marvell/comphy_cp110.c
+++ b/drivers/phy/marvell/comphy_cp110.c
@@ -1074,7 +1074,7 @@ static int comphy_sgmii_power_up(u32 lane, u32 sgmii_speed,
 }
 
 static int comphy_sfi_power_up(u32 lane, void __iomem *hpipe_base,
-			       void __iomem *comphy_base)
+			       void __iomem *comphy_base, u32 speed)
 {
 	u32 mask, data, ret = 1;
 	void __iomem *hpipe_addr = HPIPE_ADDR(hpipe_base, lane);
@@ -1129,7 +1129,9 @@ static int comphy_sfi_power_up(u32 lane, void __iomem *hpipe_base,
 	debug("stage: Comphy configuration\n");
 	/* set reference clock */
 	mask = HPIPE_MISC_ICP_FORCE_MASK;
-	data = 0x1 << HPIPE_MISC_ICP_FORCE_OFFSET;
+	data = (speed == PHY_SPEED_5_15625G) ?
+		(0x0 << HPIPE_MISC_ICP_FORCE_OFFSET) :
+		(0x1 << HPIPE_MISC_ICP_FORCE_OFFSET);
 	mask |= HPIPE_MISC_REFCLK_SEL_MASK;
 	data |= 0x0 << HPIPE_MISC_REFCLK_SEL_OFFSET;
 	reg_set(hpipe_addr + HPIPE_MISC_REG, data, mask);
@@ -1154,6 +1156,19 @@ static int comphy_sfi_power_up(u32 lane, void __iomem *hpipe_base,
 	data = 0x1 << HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET;
 	reg_set(hpipe_addr + HPIPE_PWR_CTR_DTL_REG, data, mask);
 
+	/* Transmitter/Receiver Speed Divider Force */
+	if (speed == PHY_SPEED_5_15625G) {
+		mask = HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_MASK;
+		data = 1 << HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_OFFSET;
+		mask |= HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_MASK;
+		data |= 1 << HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_OFFSET;
+		mask |= HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_MASK;
+		data |= 1 << HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_OFFSET;
+		mask |= HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_MASK;
+		data |= 1 << HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_OFFSET;
+		reg_set(hpipe_addr + HPIPE_SPD_DIV_FORCE_REG, data, mask);
+	}
+
 	/* Set analog paramters from ETP(HW) */
 	debug("stage: Analog paramters from ETP(HW)\n");
 	/* SERDES External Configuration 2 */
@@ -1165,10 +1180,15 @@ static int comphy_sfi_power_up(u32 lane, void __iomem *hpipe_base,
 	data = 0x1 << HPIPE_DFE_RES_FORCE_OFFSET;
 	reg_set(hpipe_addr + HPIPE_DFE_REG0, data, mask);
 	/* 0xd-G1_Setting_0 */
-	mask = HPIPE_G1_SET_0_G1_TX_AMP_MASK;
-	data = 0x1c << 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;
+	if (speed == PHY_SPEED_5_15625G) {
+		mask = HPIPE_G1_SET_0_G1_TX_EMPH1_MASK;
+		data = 0x6 << HPIPE_G1_SET_0_G1_TX_EMPH1_OFFSET;
+	} else {
+		mask = HPIPE_G1_SET_0_G1_TX_AMP_MASK;
+		data = 0x1c << 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;
+	}
 	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;
@@ -1214,6 +1234,15 @@ static int comphy_sfi_power_up(u32 lane, void __iomem *hpipe_base,
 	/* Genration 1 setting 3 (G1_Setting_3) */
 	mask = HPIPE_G1_SETTINGS_3_G1_FBCK_SEL_MASK;
 	data = 0x1 << HPIPE_G1_SETTINGS_3_G1_FBCK_SEL_OFFSET;
+	if (speed == PHY_SPEED_5_15625G) {
+		/* Force FFE (Feed Forward Equalization) to 5G */
+		mask |= HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_MASK;
+		data |= 0xf << HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_OFFSET;
+		mask |= HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_MASK;
+		data |= 0x4 << HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_OFFSET;
+		mask |= HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_MASK;
+		data |= 0x1 << HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_OFFSET;
+	}
 	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_3_REG, data, mask);
 
 	debug("stage: RFU configurations- Power Up PLL,Tx,Rx\n");
@@ -1867,7 +1896,8 @@ int comphy_cp110_init(struct chip_serdes_phy_config *ptr_chip_cfg,
 			break;
 		case PHY_TYPE_SFI:
 			ret = comphy_sfi_power_up(lane, hpipe_base_addr,
-						  comphy_base_addr);
+						  comphy_base_addr,
+						  ptr_comphy_map->speed);
 			break;
 		case PHY_TYPE_RXAUI0:
 		case PHY_TYPE_RXAUI1:
diff --git a/drivers/phy/marvell/comphy_hpipe.h b/drivers/phy/marvell/comphy_hpipe.h
index 1857ffd..1e294fb 100644
--- a/drivers/phy/marvell/comphy_hpipe.h
+++ b/drivers/phy/marvell/comphy_hpipe.h
@@ -300,6 +300,20 @@
 #define HPIPE_PWR_CTR_SFT_RST_MASK		\
 	(0x1 << HPIPE_PWR_CTR_SFT_RST_OFFSET)
 
+#define HPIPE_SPD_DIV_FORCE_REG				0x154
+#define HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_OFFSET		8
+#define HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_MASK		\
+	(0x3 << HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_OFFSET)
+#define HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_OFFSET	10
+#define HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_MASK	\
+	(0x1 << HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_OFFSET)
+#define HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_OFFSET		13
+#define HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_MASK		\
+	(0x3 << HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_OFFSET)
+#define HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_OFFSET	15
+#define HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_MASK	\
+	(0x1 << HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_OFFSET)
+
 #define HPIPE_PLLINTP_REG1			0x150
 
 #define HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG	0x16C
diff --git a/include/dt-bindings/comphy/comphy_data.h b/include/dt-bindings/comphy/comphy_data.h
index 5eb32ef..0983116 100644
--- a/include/dt-bindings/comphy/comphy_data.h
+++ b/include/dt-bindings/comphy/comphy_data.h
@@ -13,10 +13,11 @@
 #define PHY_SPEED_3G			3
 #define PHY_SPEED_3_125G		4
 #define PHY_SPEED_5G			5
-#define PHY_SPEED_6G			6
-#define PHY_SPEED_6_25G			7
-#define PHY_SPEED_10_3125G		8
-#define PHY_SPEED_MAX			9
+#define PHY_SPEED_5_15625G		6
+#define PHY_SPEED_6G			7
+#define PHY_SPEED_6_25G			8
+#define PHY_SPEED_10_3125G		9
+#define PHY_SPEED_MAX			10
 #define PHY_SPEED_INVALID		0xff
 
 #define PHY_TYPE_UNCONNECTED		0