openwrt/target/linux/generic/pending-6.18/739-03-net-pcs-pcs-mtk-lynxi-add-platform-driver-for-MT7988.patch

From e11b399cedf08e0ca3046c856708daffb6f7888a Mon Sep 17 00:00:00 2001
From: Daniel Golle <daniel@makrotopia.org>
Date: Tue, 12 Dec 2023 03:47:18 +0000
Subject: [PATCH] net: pcs: pcs-mtk-lynxi: add platform driver for MT7988

Introduce a full platform MFD driver for the LynxI (H)SGMII PCS which
is going to initially be used for the MT7988 SoC.

Signed-off-by: Daniel Golle <daniel@makrotopia.org>
---
 drivers/net/pcs/pcs-mtk-lynxi.c | 290 +++++++++++++++++++++++++++++---
 1 file changed, 262 insertions(+), 28 deletions(-)

--- a/drivers/net/pcs/pcs-mtk-lynxi.c
+++ b/drivers/net/pcs/pcs-mtk-lynxi.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (c) 2018-2019 MediaTek Inc.
-/* A library for MediaTek SGMII circuit
+/* A library and platform driver for the MediaTek LynxI SGMII circuit
  *
  * Author: Sean Wang <sean.wang@mediatek.com>
  * Author: Alexander Couzens <lynxis@fe80.eu>
@@ -8,12 +8,22 @@
  *
  */
 
+#include <linux/clk.h>
 #include <linux/mdio.h>
+#include <linux/mfd/syscon.h>
+#include <linux/mutex.h>
 #include <linux/of.h>
+#include <linux/workqueue.h>
+#include <linux/of_platform.h>
 #include <linux/pcs/pcs-mtk-lynxi.h>
 #include <linux/phy/phy-common-props.h>
+#include <linux/pcs/pcs-provider.h>
+#include <linux/phy/phy.h>
 #include <linux/phylink.h>
+#include <linux/platform_device.h>
 #include <linux/regmap.h>
+#include <linux/reset.h>
+#include <linux/rtnetlink.h>
 
 /* SGMII subsystem config registers */
 /* BMCR (low 16) BMSR (high 16) */
@@ -67,6 +77,8 @@
 #define SGMII_PN_SWAP_TX		BIT(0)
 
 
+#define MTK_NETSYS_V3_AMA_RGC3		0x128
+
 /* struct mtk_pcs_lynxi -  This structure holds each sgmii regmap andassociated
  *                         data
  * @regmap:                The register map pointing at the range used to setup
@@ -74,16 +86,37 @@
  * @dev:                   Pointer to device owning the PCS
  * @ana_rgc3:              The offset of register ANA_RGC3 relative to regmap
  * @interface:             Currently configured interface mode
+ * @neg_mode:              Currently configured negotiation mode
+ * @advertising:           Currently configured advertisement
  * @pcs:                   Phylink PCS structure
  * @flags:                 Flags indicating hardware properties
+ * @last_get_state:        Jiffies at last pcs_get_state call
+ * @link_poll:             Delayed work for link polling when phylink does
+ *                         not poll the PCS
+ * @rstc:                  Reset controller
+ * @sgmii_sel:             SGMII Register Clock
+ * @sgmii_rx:              SGMII RX Clock
+ * @sgmii_tx:              SGMII TX Clock
+ * @node:                  List node
  */
 struct mtk_pcs_lynxi {
 	struct regmap		*regmap;
+	struct device		*dev;
 	u32			ana_rgc3;
 	phy_interface_t		interface;
+	unsigned int		neg_mode;
+	__ETHTOOL_DECLARE_LINK_MODE_MASK(advertising);
 	struct			phylink_pcs pcs;
 	u32			flags;
 	struct fwnode_handle	*fwnode;
+	unsigned long		last_get_state;
+	struct delayed_work	link_poll;
+	struct reset_control	*rstc;
+	struct clk		*sgmii_sel;
+	struct clk		*sgmii_rx;
+	struct clk		*sgmii_tx;
+	struct phy		*xfi_tphy;
+	struct list_head	node;
 };
 
 static struct mtk_pcs_lynxi *pcs_to_mtk_pcs_lynxi(struct phylink_pcs *pcs)
@@ -105,22 +138,6 @@ static unsigned int mtk_pcs_lynxi_inband
 	}
 }
 
-static void mtk_pcs_lynxi_get_state(struct phylink_pcs *pcs,
-				    unsigned int neg_mode,
-				    struct phylink_link_state *state)
-{
-	struct mtk_pcs_lynxi *mpcs = pcs_to_mtk_pcs_lynxi(pcs);
-	unsigned int bm, adv;
-
-	/* Read the BMSR and LPA */
-	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &bm);
-	regmap_read(mpcs->regmap, SGMSYS_PCS_ADVERTISE, &adv);
-
-	phylink_mii_c22_pcs_decode_state(state, neg_mode,
-					 FIELD_GET(SGMII_BMSR, bm),
-					 FIELD_GET(SGMII_LPA, adv));
-}
-
 static int mtk_pcs_config_polarity(struct mtk_pcs_lynxi *mpcs,
 				   phy_interface_t interface)
 {
@@ -160,6 +177,17 @@ static int mtk_pcs_config_polarity(struc
 				  SGMII_PN_SWAP_RX | SGMII_PN_SWAP_TX, val);
 }
 
+static void mtk_sgmii_reset(struct mtk_pcs_lynxi *mpcs)
+{
+	if (!mpcs->rstc)
+		return;
+
+	reset_control_assert(mpcs->rstc);
+	udelay(100);
+	reset_control_deassert(mpcs->rstc);
+	mdelay(1);
+}
+
 static int mtk_pcs_lynxi_config(struct phylink_pcs *pcs, unsigned int neg_mode,
 				phy_interface_t interface,
 				const unsigned long *advertising,
@@ -171,8 +199,13 @@ static int mtk_pcs_lynxi_config(struct p
 	int advertise, link_timer;
 	int ret;
 
+	mpcs->neg_mode = neg_mode;
+
+	if (advertising)
+		linkmode_copy(mpcs->advertising, advertising);
+
 	advertise = phylink_mii_c22_pcs_encode_advertisement(interface,
-							     advertising);
+							     mpcs->advertising);
 	if (advertise < 0)
 		return advertise;
 
@@ -206,6 +239,10 @@ static int mtk_pcs_lynxi_config(struct p
 				SGMII_PHYA_PWD);
 
 		/* Reset SGMII PCS state */
+		if (mpcs->xfi_tphy)
+			phy_reset(mpcs->xfi_tphy);
+
+		mtk_sgmii_reset(mpcs);
 		regmap_set_bits(mpcs->regmap, SGMSYS_RESERVED_0,
 				SGMII_SW_RESET);
 
@@ -255,9 +292,41 @@ static int mtk_pcs_lynxi_config(struct p
 	usleep_range(50, 100);
 	regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL, 0);
 
+	/* Setup PMA/PMD */
+	if (mpcs->xfi_tphy)
+		phy_set_mode_ext(mpcs->xfi_tphy, PHY_MODE_ETHERNET, interface);
+
 	return changed || mode_changed;
 }
 
+static void mtk_pcs_lynxi_get_state(struct phylink_pcs *pcs,
+				    unsigned int neg_mode,
+				    struct phylink_link_state *state)
+{
+	struct mtk_pcs_lynxi *mpcs = pcs_to_mtk_pcs_lynxi(pcs);
+	unsigned int bm, adv;
+
+	/* Read the BMSR and LPA */
+	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &bm);
+	regmap_read(mpcs->regmap, SGMSYS_PCS_ADVERTISE, &adv);
+
+	phylink_mii_c22_pcs_decode_state(state, neg_mode,
+					 FIELD_GET(SGMII_BMSR, bm),
+					 FIELD_GET(SGMII_LPA, adv));
+
+	mpcs->last_get_state = jiffies;
+
+	/* The XFI T-PHY's analog calibration may settle at a bad
+	 * operating point after hard reset, leaving the TX output
+	 * non-functional. Re-initialize to get another chance.
+	 */
+	if (!state->link && mpcs->xfi_tphy) {
+		mpcs->interface = PHY_INTERFACE_MODE_NA;
+		mtk_pcs_lynxi_config(pcs, mpcs->neg_mode,
+				     state->interface, NULL, false);
+	}
+}
+
 static void mtk_pcs_lynxi_restart_an(struct phylink_pcs *pcs)
 {
 	struct mtk_pcs_lynxi *mpcs = pcs_to_mtk_pcs_lynxi(pcs);
@@ -273,6 +342,16 @@ static void mtk_pcs_lynxi_link_up(struct
 	struct mtk_pcs_lynxi *mpcs = pcs_to_mtk_pcs_lynxi(pcs);
 	unsigned int sgm_mode;
 
+	/* In force mode the PCS provides no link status feedback,
+	 * so re-init the T-PHY here as the only chance to recover
+	 * from a bad analog calibration. In-band modes are handled
+	 * by pcs_get_state and the link_poll worker instead.
+	 */
+	if (mpcs->xfi_tphy && neg_mode != PHYLINK_PCS_NEG_INBAND_ENABLED) {
+		mpcs->interface = PHY_INTERFACE_MODE_NA;
+		mtk_pcs_lynxi_config(pcs, neg_mode, interface, NULL, false);
+	}
+
 	if (neg_mode != PHYLINK_PCS_NEG_INBAND_ENABLED) {
 		/* Force the speed and duplex setting */
 		if (speed == SPEED_10)
@@ -291,11 +370,71 @@ static void mtk_pcs_lynxi_link_up(struct
 	}
 }
 
+/* Fallback link poll worker for cases where phylink does not poll the
+ * PCS (e.g. when an SFP module has a PHY and phylink uses MLO_AN_PHY).
+ * Defers to pcs_get_state when phylink IS polling the PCS.
+ */
+static void mtk_pcs_lynxi_link_poll(struct work_struct *work)
+{
+	struct mtk_pcs_lynxi *mpcs = container_of(work,
+						  struct mtk_pcs_lynxi,
+						  link_poll.work);
+	phy_interface_t interface;
+	unsigned int bm;
+
+	/* pcs_get_state handles re-init when phylink polls the PCS */
+	if (time_is_after_jiffies(mpcs->last_get_state + 2 * HZ))
+		goto reschedule;
+
+	interface = mpcs->interface;
+	if (interface == PHY_INTERFACE_MODE_NA)
+		goto reschedule;
+
+	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &bm);
+	if (FIELD_GET(SGMII_BMSR, bm) & BMSR_LSTATUS)
+		goto reschedule;
+
+	mpcs->interface = PHY_INTERFACE_MODE_NA;
+	mtk_pcs_lynxi_config(&mpcs->pcs, mpcs->neg_mode,
+			     interface, NULL, false);
+
+reschedule:
+	schedule_delayed_work(&mpcs->link_poll, HZ);
+}
+
+static int mtk_pcs_lynxi_enable(struct phylink_pcs *pcs)
+{
+	struct mtk_pcs_lynxi *mpcs = pcs_to_mtk_pcs_lynxi(pcs);
+
+	if (mpcs->sgmii_tx && mpcs->sgmii_rx) {
+		clk_prepare_enable(mpcs->sgmii_rx);
+		clk_prepare_enable(mpcs->sgmii_tx);
+	}
+
+	if (mpcs->xfi_tphy) {
+		phy_power_on(mpcs->xfi_tphy);
+		schedule_delayed_work(&mpcs->link_poll, HZ);
+	}
+
+	return 0;
+}
+
 static void mtk_pcs_lynxi_disable(struct phylink_pcs *pcs)
 {
 	struct mtk_pcs_lynxi *mpcs = pcs_to_mtk_pcs_lynxi(pcs);
 
+	regmap_set_bits(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL, SGMII_PHYA_PWD);
+
+	if (mpcs->sgmii_tx && mpcs->sgmii_rx) {
+		clk_disable_unprepare(mpcs->sgmii_tx);
+		clk_disable_unprepare(mpcs->sgmii_rx);
+	}
+
 	mpcs->interface = PHY_INTERFACE_MODE_NA;
+	if (mpcs->xfi_tphy) {
+		cancel_delayed_work_sync(&mpcs->link_poll);
+		phy_power_off(mpcs->xfi_tphy);
+	}
 }
 
 static const struct phylink_pcs_ops mtk_pcs_lynxi_ops = {
@@ -305,11 +444,13 @@ static const struct phylink_pcs_ops mtk_
 	.pcs_an_restart = mtk_pcs_lynxi_restart_an,
 	.pcs_link_up = mtk_pcs_lynxi_link_up,
 	.pcs_disable = mtk_pcs_lynxi_disable,
+	.pcs_enable = mtk_pcs_lynxi_enable,
 };
 
-struct phylink_pcs *mtk_pcs_lynxi_create(struct device *dev,
-					 struct fwnode_handle *fwnode,
-					 struct regmap *regmap, u32 ana_rgc3)
+static struct phylink_pcs *mtk_pcs_lynxi_init(struct device *dev,
+					      struct fwnode_handle *fwnode,
+					      struct regmap *regmap, u32 ana_rgc3,
+					      struct mtk_pcs_lynxi *prealloc)
 {
 	struct mtk_pcs_lynxi *mpcs;
 	u32 id, ver;
@@ -317,29 +458,33 @@ struct phylink_pcs *mtk_pcs_lynxi_create
 
 	ret = regmap_read(regmap, SGMSYS_PCS_DEVICE_ID, &id);
 	if (ret < 0)
-		return NULL;
+		return ERR_PTR(ret);
 
 	if (id != SGMII_LYNXI_DEV_ID) {
 		dev_err(dev, "unknown PCS device id %08x\n", id);
-		return NULL;
+		return ERR_PTR(-ENODEV);
 	}
 
 	ret = regmap_read(regmap, SGMSYS_PCS_SCRATCH, &ver);
 	if (ret < 0)
-		return NULL;
+		return ERR_PTR(ret);
 
 	ver = FIELD_GET(SGMII_DEV_VERSION, ver);
 	if (ver != 0x1) {
 		dev_err(dev, "unknown PCS device version %04x\n", ver);
-		return NULL;
+		return ERR_PTR(-ENODEV);
 	}
 
 	dev_dbg(dev, "MediaTek LynxI SGMII PCS (id 0x%08x, ver 0x%04x)\n", id,
 		ver);
 
-	mpcs = kzalloc(sizeof(*mpcs), GFP_KERNEL);
-	if (!mpcs)
-		return NULL;
+	if (prealloc) {
+		mpcs = prealloc;
+	} else {
+		mpcs = kzalloc(sizeof(*mpcs), GFP_KERNEL);
+		if (!mpcs)
+			return ERR_PTR(-ENOMEM);
+	};
 
 	mpcs->ana_rgc3 = ana_rgc3;
 	mpcs->regmap = regmap;
@@ -347,12 +492,20 @@ struct phylink_pcs *mtk_pcs_lynxi_create
 	mpcs->pcs.poll = true;
 	mpcs->interface = PHY_INTERFACE_MODE_NA;
 	mpcs->fwnode = fwnode_handle_get(fwnode);
+	INIT_DELAYED_WORK(&mpcs->link_poll, mtk_pcs_lynxi_link_poll);
 
 	__set_bit(PHY_INTERFACE_MODE_SGMII, mpcs->pcs.supported_interfaces);
 	__set_bit(PHY_INTERFACE_MODE_1000BASEX, mpcs->pcs.supported_interfaces);
 	__set_bit(PHY_INTERFACE_MODE_2500BASEX, mpcs->pcs.supported_interfaces);
 
 	return &mpcs->pcs;
+};
+
+struct phylink_pcs *mtk_pcs_lynxi_create(struct device *dev,
+					 struct fwnode_handle *fwnode,
+					 struct regmap *regmap, u32 ana_rgc3)
+{
+	return mtk_pcs_lynxi_init(dev, fwnode, regmap, ana_rgc3, NULL);
 }
 EXPORT_SYMBOL(mtk_pcs_lynxi_create);
 
@@ -369,5 +522,86 @@ void mtk_pcs_lynxi_destroy(struct phylin
 }
 EXPORT_SYMBOL(mtk_pcs_lynxi_destroy);
 
+#ifdef CONFIG_FWNODE_PCS
+static int mtk_pcs_lynxi_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct mtk_pcs_lynxi *mpcs;
+	struct phylink_pcs *pcs;
+	struct regmap *regmap;
+
+	mpcs = devm_kzalloc(dev, sizeof(*mpcs), GFP_KERNEL);
+	if (!mpcs)
+		return -ENOMEM;
+
+	mpcs->dev = dev;
+	regmap = syscon_node_to_regmap(np->parent);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	mpcs->rstc = of_reset_control_get_shared(np->parent, NULL);
+	if (IS_ERR(mpcs->rstc))
+		return PTR_ERR(mpcs->rstc);
+
+	reset_control_deassert(mpcs->rstc);
+	mpcs->sgmii_sel = devm_clk_get_enabled(dev, "sgmii_sel");
+	if (IS_ERR(mpcs->sgmii_sel))
+		return PTR_ERR(mpcs->sgmii_sel);
+
+	mpcs->sgmii_rx = devm_clk_get(dev, "sgmii_rx");
+	if (IS_ERR(mpcs->sgmii_rx))
+		return PTR_ERR(mpcs->sgmii_rx);
+
+	mpcs->sgmii_tx = devm_clk_get(dev, "sgmii_tx");
+	if (IS_ERR(mpcs->sgmii_tx))
+		return PTR_ERR(mpcs->sgmii_tx);
+
+	mpcs->xfi_tphy = devm_of_phy_optional_get(mpcs->dev, np, NULL);
+	if (IS_ERR(mpcs->xfi_tphy))
+		return PTR_ERR(mpcs->xfi_tphy);
+
+	pcs = mtk_pcs_lynxi_init(dev, of_fwnode_handle(np), regmap,
+				 (uintptr_t)of_device_get_match_data(dev),
+				 mpcs);
+	if (IS_ERR(pcs))
+		return PTR_ERR(pcs);
+
+	regmap_set_bits(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL, SGMII_PHYA_PWD);
+
+	platform_set_drvdata(pdev, mpcs);
+
+	return fwnode_pcs_add_provider(of_fwnode_handle(np), fwnode_pcs_simple_get, &mpcs->pcs);
+}
+
+static void mtk_pcs_lynxi_remove(struct platform_device *pdev)
+{
+	struct mtk_pcs_lynxi *mpcs = platform_get_drvdata(pdev);
+
+	fwnode_pcs_del_provider(dev_fwnode(&pdev->dev));
+
+	rtnl_lock();
+	phylink_release_pcs(&mpcs->pcs);
+	rtnl_unlock();
+};
+
+static const struct of_device_id mtk_pcs_lynxi_of_match[] = {
+	{ .compatible = "mediatek,mt7988-sgmii", .data = (void *)MTK_NETSYS_V3_AMA_RGC3 },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, mtk_pcs_lynxi_of_match);
+
+static struct platform_driver mtk_pcs_lynxi_driver = {
+	.driver = {
+		.name			= "mtk-pcs-lynxi",
+		.of_match_table		= mtk_pcs_lynxi_of_match,
+	},
+	.probe = mtk_pcs_lynxi_probe,
+	.remove = mtk_pcs_lynxi_remove,
+};
+module_platform_driver(mtk_pcs_lynxi_driver);
+#endif
+
+MODULE_AUTHOR("Daniel Golle <daniel@makrotopia.org>");
 MODULE_DESCRIPTION("MediaTek SGMII library for LynxI");
 MODULE_LICENSE("GPL");