mvebu: cp110: add support for digital reset

Change-Id: Iaed7504ccd349456e3e16ac96e7f3a423bfb4f94
Signed-off-by: Grzegorz Jaszczyk <jaz@semihalf.com>
Reviewed-on: http://vgitil04.il.marvell.com:8080/52902
Reviewed-by: Kostya Porotchkin <kostap@marvell.com>
Tested-by: Kostya Porotchkin <kostap@marvell.com>

3 files changed, 44 insertions, 0 deletions

diff --git a/drivers/marvell/comphy/phy-comphy-cp110.c b/drivers/marvell/comphy/phy-comphy-cp110.c
index 8b446183..01f7c6a9 100644
--- a/drivers/marvell/comphy/phy-comphy-cp110.c
+++ b/drivers/marvell/comphy/phy-comphy-cp110.c
@@ -87,6 +87,10 @@
 /* The  default speed for IO with fixed known speed */
 #define COMPHY_SPEED_DEFAULT		COMPHY_SPEED_MAX
 
+/* Commands for comphy driver */
+#define COMPHY_COMMAND_DIGITAL_PWR_OFF		0x00000001
+#define COMPHY_COMMAND_DIGITAL_PWR_ON		0x00000002
+
 #define COMPHY_PIPE_FROM_COMPHY_ADDR(x)	((x & ~0xffffff) + 0x120000)
 
 /* System controler registers */
@@ -2086,6 +2090,40 @@ static int mvebu_cp110_comphy_ap_power_on(uint64_t comphy_base,
 	return 0;
 }
 
+/*
+ * This function allows to reset the digital synchronizers between
+ * the MAC and the PHY, it is required when the MAC changes its state.
+ */
+int mvebu_cp110_comphy_digital_reset(uint64_t comphy_base,
+				     uint8_t comphy_index,
+				     uint32_t comphy_mode, uint32_t command)
+{
+	int mode = COMPHY_GET_MODE(comphy_mode);
+	uintptr_t sd_ip_addr;
+	uint32_t mask, data;
+
+	sd_ip_addr = SD_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base), comphy_index);
+
+	switch (mode) {
+	case (COMPHY_SGMII_MODE):
+	case (COMPHY_HS_SGMII_MODE):
+	case (COMPHY_XFI_MODE):
+	case (COMPHY_SFI_MODE):
+	case (COMPHY_RXAUI_MODE):
+		mask = SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+		data = ((command == COMPHY_COMMAND_DIGITAL_PWR_OFF) ?
+			0x0 : 0x1) << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET;
+		reg_set(sd_ip_addr + SD_EXTERNAL_CONFIG1_REG, data, mask);
+		break;
+	default:
+		ERROR("comphy%d: COMPHY_COMMAND_DIGITAL_PWR_ON/OFF is not supported\n",
+			comphy_index);
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
 int mvebu_cp110_comphy_power_on(uint64_t comphy_base, uint64_t comphy_index, uint64_t comphy_mode)
 {
 	int mode = COMPHY_GET_MODE(comphy_mode);
diff --git a/drivers/marvell/comphy/phy-comphy-cp110.h b/drivers/marvell/comphy/phy-comphy-cp110.h
index 6476f8ce..98f05ec6 100644
--- a/drivers/marvell/comphy/phy-comphy-cp110.h
+++ b/drivers/marvell/comphy/phy-comphy-cp110.h
@@ -9,3 +9,4 @@ int mvebu_cp110_comphy_is_pll_locked(uint64_t comphy_base, uint64_t comphy_index
 int mvebu_cp110_comphy_power_off(uint64_t comphy_base, uint64_t comphy_index);
 int mvebu_cp110_comphy_power_on(uint64_t comphy_base, uint64_t comphy_index, uint64_t comphy_mode);
 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);
diff --git a/plat/marvell/common/mrvl_sip_svc.c b/plat/marvell/common/mrvl_sip_svc.c
index 066ff87d..f1f5e8be 100644
--- a/plat/marvell/common/mrvl_sip_svc.c
+++ b/plat/marvell/common/mrvl_sip_svc.c
@@ -21,6 +21,7 @@
 #define MV_SIP_COMPHY_POWER_OFF	0x82000002
 #define MV_SIP_COMPHY_PLL_LOCK	0x82000003
 #define MV_SIP_COMPHY_XFI_TRAIN	0x82000004
+#define MV_SIP_COMPHY_DIG_RESET	0x82000005
 
 #define MAX_LANE_NR		6
 #define MVEBU_COMPHY_OFFSET	0x441000
@@ -73,6 +74,10 @@ uint64_t mrvl_sip_smc_handler(uint32_t smc_fid,
 		/* x1:  comphy_base, x2: comphy_index */
 		ret = mvebu_cp110_comphy_xfi_rx_training(x1, x2);
 		SMC_RET1(handle, ret);
+	case MV_SIP_COMPHY_DIG_RESET:
+		/* x1:  comphy_base, x2: comphy_index */
+		ret = mvebu_cp110_comphy_digital_reset(x1, x2, x3);
+		SMC_RET1(handle, ret);
 	default:
 		ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
 		SMC_RET1(handle, SMC_UNK);