From b4ef49a905052f9f765d3e1b1e18244202de3083 Mon Sep 17 00:00:00 2001
From: MarkLee <Mark-MC.Lee@mediatek.com>
Date: Tue, 21 Jan 2020 19:31:57 +0800
Subject: [PATCH] eth: mtk-eth: add sgmii mode support in mediatek eth driver

This patch add sgmii init part for the mediatek SoC that
support sgmii mode. It is a must for mt7622.

Signed-off-by: MarkLee <Mark-MC.Lee@mediatek.com>
---
 drivers/net/mtk_eth.c | 44 +++++++++++++++++++++++++++++++++++++++++++
 drivers/net/mtk_eth.h | 15 +++++++++++++++
 2 files changed, 59 insertions(+)

diff --git a/drivers/net/mtk_eth.c b/drivers/net/mtk_eth.c
index c22e590387..8354e82244 100644
--- a/drivers/net/mtk_eth.c
+++ b/drivers/net/mtk_eth.c
@@ -151,6 +151,7 @@ struct mtk_eth_priv {
 	void __iomem *fe_base;
 	void __iomem *gmac_base;
 	void __iomem *ethsys_base;
+	void __iomem *sgmii_base;
 
 	struct mii_dev *mdio_bus;
 	int (*mii_read)(struct mtk_eth_priv *priv, u8 phy, u8 reg);
@@ -750,6 +751,24 @@ static int mtk_phy_probe(struct udevice *dev)
 	return 0;
 }
 
+static void mtk_sgmii_init(struct mtk_eth_priv *priv)
+{
+	/* Set SGMII GEN2 speed(2.5G) */
+	clrsetbits_le32(priv->sgmii_base + SGMSYS_GEN2_SPEED,
+			SGMSYS_SPEED_2500, SGMSYS_SPEED_2500);
+
+	/* Disable SGMII AN */
+	clrsetbits_le32(priv->sgmii_base + SGMSYS_PCS_CONTROL_1,
+			SGMII_AN_ENABLE, 0);
+
+	/* SGMII force mode setting */
+	writel(SGMII_FORCE_MODE, priv->sgmii_base + SGMSYS_SGMII_MODE);
+
+	/* Release PHYA power down state */
+	clrsetbits_le32(priv->sgmii_base + SGMSYS_QPHY_PWR_STATE_CTRL,
+			SGMII_PHYA_PWD, 0);
+}
+
 static void mtk_mac_init(struct mtk_eth_priv *priv)
 {
 	int i, ge_mode = 0;
@@ -758,8 +777,13 @@ static void mtk_mac_init(struct mtk_eth_priv *priv)
 	switch (priv->phy_interface) {
 	case PHY_INTERFACE_MODE_RGMII_RXID:
 	case PHY_INTERFACE_MODE_RGMII:
+		ge_mode = GE_MODE_RGMII;
+		break;
 	case PHY_INTERFACE_MODE_SGMII:
 		ge_mode = GE_MODE_RGMII;
+		mtk_ethsys_rmw(priv, ETHSYS_SYSCFG0_REG, SYSCFG0_SGMII_SEL_M,
+			       SYSCFG0_SGMII_SEL(priv->gmac_id));
+		mtk_sgmii_init(priv);
 		break;
 	case PHY_INTERFACE_MODE_MII:
 	case PHY_INTERFACE_MODE_GMII:
@@ -1104,6 +1128,26 @@ static int mtk_eth_ofdata_to_platdata(struct udevice *dev)
 		}
 	}
 
+	if (priv->phy_interface == PHY_INTERFACE_MODE_SGMII) {
+		/* get corresponding sgmii phandle */
+		ret = dev_read_phandle_with_args(dev, "mediatek,sgmiisys",
+						 NULL, 0, 0, &args);
+		if (ret)
+			return ret;
+
+		regmap = syscon_node_to_regmap(args.node);
+
+		if (IS_ERR(regmap))
+			return PTR_ERR(regmap);
+
+		priv->sgmii_base = regmap_get_range(regmap, 0);
+
+		if (!priv->sgmii_base) {
+			dev_err(dev, "Unable to find sgmii\n");
+			return -ENODEV;
+		}
+	}
+
 	/* check for switch first, otherwise phy will be used */
 	priv->sw = SW_NONE;
 	priv->switch_init = NULL;
diff --git a/drivers/net/mtk_eth.h b/drivers/net/mtk_eth.h
index fe89a03739..9bb037d440 100644
--- a/drivers/net/mtk_eth.h
+++ b/drivers/net/mtk_eth.h
@@ -20,6 +20,8 @@
 #define ETHSYS_SYSCFG0_REG		0x14
 #define SYSCFG0_GE_MODE_S(n)		(12 + ((n) * 2))
 #define SYSCFG0_GE_MODE_M		0x3
+#define SYSCFG0_SGMII_SEL_M		(0x3 << 8)
+#define SYSCFG0_SGMII_SEL(gmac)		((!(gmac)) ? BIT(9) : BIT(8))
 
 #define ETHSYS_CLKCFG0_REG		0x2c
 #define ETHSYS_TRGMII_CLK_SEL362_5	BIT(11)
@@ -30,6 +32,19 @@
 #define GE_MODE_MII_PHY			2
 #define GE_MODE_RMII			3
 
+/* SGMII subsystem config registers */
+#define SGMSYS_PCS_CONTROL_1		0x0
+#define SGMII_AN_ENABLE			BIT(12)
+
+#define SGMSYS_SGMII_MODE		0x20
+#define SGMII_FORCE_MODE		0x31120019
+
+#define SGMSYS_QPHY_PWR_STATE_CTRL	0xe8
+#define SGMII_PHYA_PWD			BIT(4)
+
+#define SGMSYS_GEN2_SPEED		0x2028
+#define SGMSYS_SPEED_2500		BIT(2)
+
 /* Frame Engine Registers */
 
 /* PDMA */
-- 
2.25.1