From d62fc50ff42223df2bd82c1ea0f1786ea26d3f08 Mon Sep 17 00:00:00 2001
From: Daniel Golle <daniel@makrotopia.org>
Date: Sun, 5 Oct 2025 03:39:52 +0100
Subject: [PATCH] net: phy: mediatek: add 2.5G PHY support for MT7987

Add MT7987 PHY ID, firmware file paths (PMB and DSPBitTb blobs), and the
necessary PMD/XBZ register definitions. Extend the driver probe path to
load and initialise the MT7987-specific firmware and configure the
internal 2.5G PHY hardware.

Signed-off-by: Daniel Golle <daniel@makrotopia.org>
---
--- a/drivers/net/phy/mediatek/mtk-2p5ge.c
+++ b/drivers/net/phy/mediatek/mtk-2p5ge.c
@@ -9,17 +9,76 @@
 
 #include "mtk.h"
 
+#define MTK_2P5GPHY_ID_MT7987		0x00339c91
 #define MTK_2P5GPHY_ID_MT7988		0x00339c11
 
+#define MT7987_2P5GE_PMB_FW		"mediatek/mt7987/i2p5ge-phy-pmb.bin"
+#define MT7987_2P5GE_PMB_FW_SIZE	0x18000
+#define MT7987_2P5GE_DSPBITTB		"mediatek/mt7987/i2p5ge-phy-DSPBitTb.bin"
+#define MT7987_2P5GE_DSPBITTB_SIZE	0x7000
+
 #define MT7988_2P5GE_PMB_FW		"mediatek/mt7988/i2p5ge-phy-pmb.bin"
 #define MT7988_2P5GE_PMB_FW_SIZE	0x20000
 #define MT7988_2P5GE_PMB_FW_BASE	0x0f100000
 #define MT7988_2P5GE_PMB_FW_LEN		0x20000
+
+#define MTK_2P5GPHY_PMD_REG_BASE	0x0f010000
+#define MTK_2P5GPHY_PMD_REG_LEN		0x210
+#define DO_NOT_RESET			0x28
+#define DO_NOT_RESET_XBZ		BIT(0)
+#define DO_NOT_RESET_PMA		BIT(3)
+#define DO_NOT_RESET_RX			BIT(5)
+#define FNPLL_PWR_CTRL1			0x208
+#define RG_SPEED_MASK			GENMASK(3, 0)
+#define RG_SPEED_2500			BIT(3)
+#define RG_SPEED_100			BIT(0)
+#define FNPLL_PWR_CTRL_STATUS		0x20c
+#define RG_STABLE_MASK			GENMASK(3, 0)
+#define RG_SPEED_2500_STABLE		BIT(3)
+#define RG_SPEED_100_STABLE		BIT(0)
+
+#define MTK_2P5GPHY_XBZ_PCS_REG_BASE	0x0f030000
+#define MTK_2P5GPHY_XBZ_PCS_REG_LEN	0x844
+#define PHY_CTRL_CONFIG			0x200
+#define PMU_WP				0x800
+#define WRITE_PROTECT_KEY		0xCAFEF00D
+#define PMU_PMA_AUTO_CFG		0x820
+#define POWER_ON_AUTO_MODE		BIT(16)
+#define PMU_AUTO_MODE_EN		BIT(0)
+#define PMU_PMA_STATUS			0x840
+#define CLK_IS_DISABLED			BIT(3)
+
+#define MTK_2P5GPHY_XBZ_PMA_RX_BASE	0x0f080000
+#define MTK_2P5GPHY_XBZ_PMA_RX_LEN	0x5228
+#define SMEM_WDAT0			0x5000
+#define SMEM_WDAT1			0x5004
+#define SMEM_WDAT2			0x5008
+#define SMEM_WDAT3			0x500c
+#define SMEM_CTRL			0x5024
+#define SMEM_HW_RDATA_ZERO		BIT(24)
+#define SMEM_ADDR_REF_ADDR		0x502c
+#define CM_CTRL_P01			0x5100
+#define CM_CTRL_P23			0x5124
+#define DM_CTRL_P01			0x5200
+#define DM_CTRL_P23			0x5224
+
+#define MTK_2P5GPHY_CHIP_SCU_BASE	0x0f0cf800
+#define MTK_2P5GPHY_CHIP_SCU_LEN	0x12c
+#define SYS_SW_RESET			0x128
+#define RESET_RST_CNT			BIT(0)
+
 #define MTK_2P5GPHY_MCU_CSR_BASE	0x0f0f0000
 #define MTK_2P5GPHY_MCU_CSR_LEN		0x20
 #define MD32_EN_CFG			0x18
 #define   MD32_EN			BIT(0)
 
+#define MTK_2P5GPHY_PMB_FW_BASE		0x0f100000
+
+#define MTK_2P5GPHY_APB_BASE		0x11c30000
+#define MTK_2P5GPHY_APB_LEN		0x9c
+#define SW_RESET			0x94
+#define MD32_RESTART_EN_CLEAR		BIT(9)
+
 #define BASE100T_STATUS_EXTEND		0x10
 #define BASE1000T_STATUS_EXTEND		0x11
 #define EXTEND_CTRL_AND_STATUS		0x16
@@ -32,6 +91,14 @@
 #define MTK_PHY_LPI_PCS_DSP_CTRL		0x121
 #define   MTK_PHY_LPI_SIG_EN_LO_THRESH100_MASK	GENMASK(12, 8)
 
+#define MTK_PHY_LINK_STATUS_RELATED		(0x147)
+#define   MTK_PHY_BYPASS_LINK_STATUS_OK		BIT(4)
+#define   MTK_PHY_FORCE_LINK_STATUS_HCD		BIT(3)
+
+#define MTK_PHY_AN_FORCE_SPEED_REG		(0x313)
+#define   MTK_PHY_MASTER_FORCE_SPEED_SEL_EN	BIT(7)
+#define   MTK_PHY_MASTER_FORCE_SPEED_SEL_MASK	GENMASK(6, 0)
+
 #define MTK_PHY_HOST_CMD1		0x800e
 #define MTK_PHY_HOST_CMD2		0x800f
 /* Registers on Token Ring debug nodes */
@@ -45,7 +112,243 @@ enum {
 	PHY_AUX_SPD_2500,
 };
 
-static int mt798x_2p5ge_phy_load_fw(struct phy_device *phydev)
+static int mt7987_2p5ge_phy_load_fw(struct phy_device *phydev)
+{
+	struct device *dev = &phydev->mdio.dev;
+	void __iomem *xbz_pcs_reg_base;
+	void __iomem *xbz_pma_rx_base;
+	void __iomem *chip_scu_base;
+	void __iomem *pmd_reg_base;
+	void __iomem *mcu_csr_base;
+	const struct firmware *fw;
+	void __iomem *apb_base;
+	void __iomem *pmb_addr;
+	int ret, i;
+	u32 reg;
+
+	apb_base = ioremap(MTK_2P5GPHY_APB_BASE,
+			   MTK_2P5GPHY_APB_LEN);
+	if (!apb_base)
+		return -ENOMEM;
+
+	pmd_reg_base = ioremap(MTK_2P5GPHY_PMD_REG_BASE,
+			       MTK_2P5GPHY_PMD_REG_LEN);
+	if (!pmd_reg_base) {
+		ret = -ENOMEM;
+		goto free_apb;
+	}
+
+	xbz_pcs_reg_base = ioremap(MTK_2P5GPHY_XBZ_PCS_REG_BASE,
+				   MTK_2P5GPHY_XBZ_PCS_REG_LEN);
+	if (!xbz_pcs_reg_base) {
+		ret = -ENOMEM;
+		goto free_pmd;
+	}
+
+	xbz_pma_rx_base = ioremap(MTK_2P5GPHY_XBZ_PMA_RX_BASE,
+				  MTK_2P5GPHY_XBZ_PMA_RX_LEN);
+	if (!xbz_pma_rx_base) {
+		ret = -ENOMEM;
+		goto free_pcs;
+	}
+
+	chip_scu_base = ioremap(MTK_2P5GPHY_CHIP_SCU_BASE,
+				MTK_2P5GPHY_CHIP_SCU_LEN);
+	if (!chip_scu_base) {
+		ret = -ENOMEM;
+		goto free_pma;
+	}
+
+	mcu_csr_base = ioremap(MTK_2P5GPHY_MCU_CSR_BASE,
+			       MTK_2P5GPHY_MCU_CSR_LEN);
+	if (!mcu_csr_base) {
+		ret = -ENOMEM;
+		goto free_chip_scu;
+	}
+
+	pmb_addr = ioremap(MTK_2P5GPHY_PMB_FW_BASE, MT7987_2P5GE_PMB_FW_SIZE);
+	if (!pmb_addr) {
+		return -ENOMEM;
+		goto free_mcu_csr;
+	}
+
+	ret = request_firmware(&fw, MT7987_2P5GE_PMB_FW, dev);
+	if (ret) {
+		dev_err(dev, "failed to load firmware: %s, ret: %d\n",
+			MT7987_2P5GE_PMB_FW, ret);
+		goto free_pmb_addr;
+	}
+
+	if (fw->size != MT7987_2P5GE_PMB_FW_SIZE) {
+		dev_err(dev, "PMb firmware size 0x%zx != 0x%x\n",
+			fw->size, MT7987_2P5GE_PMB_FW_SIZE);
+		ret = -EINVAL;
+		goto release_fw;
+	}
+
+	/* Force 2.5Gphy back to AN state */
+	phy_set_bits(phydev, MII_BMCR, BMCR_RESET);
+	usleep_range(5000, 6000);
+	phy_set_bits(phydev, MII_BMCR, BMCR_PDOWN);
+
+	reg = readw(apb_base + SW_RESET);
+	writew(reg & ~MD32_RESTART_EN_CLEAR, apb_base + SW_RESET);
+	writew(reg | MD32_RESTART_EN_CLEAR, apb_base + SW_RESET);
+	writew(reg & ~MD32_RESTART_EN_CLEAR, apb_base + SW_RESET);
+
+	reg = readw(mcu_csr_base + MD32_EN_CFG);
+	writew(reg & ~MD32_EN, mcu_csr_base + MD32_EN_CFG);
+
+	for (i = 0; i < MT7987_2P5GE_PMB_FW_SIZE - 1; i += 4)
+		writel(*((uint32_t *)(fw->data + i)), pmb_addr + i);
+	dev_info(dev, "Firmware date code: %x/%x/%x, version: %x.%x\n",
+		 be16_to_cpu(*((__be16 *)(fw->data +
+					  MT7987_2P5GE_PMB_FW_SIZE - 8))),
+		 *(fw->data + MT7987_2P5GE_PMB_FW_SIZE - 6),
+		 *(fw->data + MT7987_2P5GE_PMB_FW_SIZE - 5),
+		 *(fw->data + MT7987_2P5GE_PMB_FW_SIZE - 2),
+		 *(fw->data + MT7987_2P5GE_PMB_FW_SIZE - 1));
+	release_firmware(fw);
+
+	/* Enable 100Mbps module clock. */
+	writel(FIELD_PREP(RG_SPEED_MASK, RG_SPEED_100),
+	       pmd_reg_base + FNPLL_PWR_CTRL1);
+
+	/* Check if 100Mbps module clock is ready. */
+	ret = readl_poll_timeout(pmd_reg_base + FNPLL_PWR_CTRL_STATUS, reg,
+				 reg & RG_SPEED_100_STABLE, 1, 10000);
+	if (ret)
+		dev_err(dev, "Fail to enable 100Mbps module clock: %d\n", ret);
+
+	/* Enable 2.5Gbps module clock. */
+	writel(FIELD_PREP(RG_SPEED_MASK, RG_SPEED_2500),
+	       pmd_reg_base + FNPLL_PWR_CTRL1);
+
+	/* Check if 2.5Gbps module clock is ready. */
+	ret = readl_poll_timeout(pmd_reg_base + FNPLL_PWR_CTRL_STATUS, reg,
+				 reg & RG_SPEED_2500_STABLE, 1, 10000);
+
+	if (ret)
+		dev_err(dev, "Fail to enable 2.5Gbps module clock: %d\n", ret);
+
+	/* Disable AN */
+	phy_clear_bits(phydev, MII_BMCR, BMCR_ANENABLE);
+
+	/* Force to run at 2.5G speed */
+	phy_modify_mmd(phydev, MDIO_MMD_VEND1, MTK_PHY_AN_FORCE_SPEED_REG,
+		       MTK_PHY_MASTER_FORCE_SPEED_SEL_MASK,
+		       MTK_PHY_MASTER_FORCE_SPEED_SEL_EN |
+		       FIELD_PREP(MTK_PHY_MASTER_FORCE_SPEED_SEL_MASK, 0x1b));
+
+	phy_set_bits_mmd(phydev, MDIO_MMD_VEND1, MTK_PHY_LINK_STATUS_RELATED,
+			 MTK_PHY_BYPASS_LINK_STATUS_OK |
+			 MTK_PHY_FORCE_LINK_STATUS_HCD);
+
+	/* Set xbz, pma and rx as "do not reset" in order to input DSP code. */
+	reg = readl(pmd_reg_base + DO_NOT_RESET);
+	reg |= DO_NOT_RESET_XBZ | DO_NOT_RESET_PMA | DO_NOT_RESET_RX;
+	writel(reg, pmd_reg_base + DO_NOT_RESET);
+
+	reg = readl(chip_scu_base + SYS_SW_RESET);
+	writel(reg & ~RESET_RST_CNT, chip_scu_base + SYS_SW_RESET);
+
+	writel(WRITE_PROTECT_KEY, xbz_pcs_reg_base + PMU_WP);
+
+	reg = readl(xbz_pcs_reg_base + PMU_PMA_AUTO_CFG);
+	reg |= PMU_AUTO_MODE_EN | POWER_ON_AUTO_MODE;
+	writel(reg, xbz_pcs_reg_base + PMU_PMA_AUTO_CFG);
+
+	/* Check if clock in auto mode is disabled. */
+	ret = readl_poll_timeout(xbz_pcs_reg_base + PMU_PMA_STATUS, reg,
+				 (reg & CLK_IS_DISABLED) == 0x0, 1, 100000);
+	if (ret)
+		dev_err(dev, "Clock isn't disabled in auto mode: %d\n", ret);
+
+	reg = readl(xbz_pma_rx_base + SMEM_CTRL);
+	writel(reg | SMEM_HW_RDATA_ZERO, xbz_pma_rx_base + SMEM_CTRL);
+
+	reg = readl(xbz_pcs_reg_base + PHY_CTRL_CONFIG);
+	writel(reg | BIT(16), xbz_pcs_reg_base + PHY_CTRL_CONFIG);
+
+	/* Initialize data memory */
+	reg = readl(xbz_pma_rx_base + DM_CTRL_P01);
+	writel(reg | BIT(28), xbz_pma_rx_base + DM_CTRL_P01);
+	reg = readl(xbz_pma_rx_base + DM_CTRL_P23);
+	writel(reg | BIT(28), xbz_pma_rx_base + DM_CTRL_P23);
+
+	/* Initialize coefficient memory */
+	reg = readl(xbz_pma_rx_base + CM_CTRL_P01);
+	writel(reg | BIT(28), xbz_pma_rx_base + CM_CTRL_P01);
+	reg = readl(xbz_pma_rx_base + CM_CTRL_P23);
+	writel(reg | BIT(28), xbz_pma_rx_base + CM_CTRL_P23);
+
+	/* Initilize PM offset */
+	writel(0, xbz_pma_rx_base + SMEM_ADDR_REF_ADDR);
+
+	ret = request_firmware(&fw, MT7987_2P5GE_DSPBITTB, dev);
+	if (ret) {
+		dev_err(dev, "failed to load firmware: %s, ret: %d\n",
+			MT7987_2P5GE_DSPBITTB, ret);
+		goto free_pmb_addr;
+	}
+	if (fw->size != MT7987_2P5GE_DSPBITTB_SIZE) {
+		dev_err(dev, "DSPBITTB size 0x%zx != 0x%x\n",
+			fw->size, MT7987_2P5GE_DSPBITTB_SIZE);
+		ret = -EINVAL;
+		goto release_fw;
+	}
+
+	for (i = 0; i < fw->size - 1; i += 16) {
+		writel(*((uint32_t *)(fw->data + i)),
+		       xbz_pma_rx_base + SMEM_WDAT0);
+		writel(*((uint32_t *)(fw->data + i + 0x4)),
+		       xbz_pma_rx_base + SMEM_WDAT1);
+		writel(*((uint32_t *)(fw->data + i + 0x8)),
+		       xbz_pma_rx_base + SMEM_WDAT2);
+		writel(*((uint32_t *)(fw->data + i + 0xc)),
+		       xbz_pma_rx_base + SMEM_WDAT3);
+	}
+
+	reg = readl(xbz_pma_rx_base + DM_CTRL_P01);
+	writel(reg & ~BIT(28), xbz_pma_rx_base + DM_CTRL_P01);
+
+	reg = readl(xbz_pma_rx_base + DM_CTRL_P23);
+	writel(reg & ~BIT(28), xbz_pma_rx_base + DM_CTRL_P23);
+
+	reg = readl(xbz_pma_rx_base + CM_CTRL_P01);
+	writel(reg & ~BIT(28), xbz_pma_rx_base + CM_CTRL_P01);
+
+	reg = readl(xbz_pma_rx_base + CM_CTRL_P23);
+	writel(reg & ~BIT(28), xbz_pma_rx_base + CM_CTRL_P23);
+
+	reg = readw(mcu_csr_base + MD32_EN_CFG);
+	writew(reg | MD32_EN, mcu_csr_base + MD32_EN_CFG);
+	phy_set_bits(phydev, MII_BMCR, BMCR_RESET);
+	/* We need a delay here to stabilize initialization of MCU */
+	usleep_range(7000, 8000);
+	dev_info(dev, "Firmware loading/trigger ok.\n");
+
+release_fw:
+	release_firmware(fw);
+free_pmb_addr:
+	iounmap(pmb_addr);
+free_mcu_csr:
+	iounmap(mcu_csr_base);
+free_chip_scu:
+	iounmap(chip_scu_base);
+free_pma:
+	iounmap(xbz_pma_rx_base);
+free_pcs:
+	iounmap(xbz_pcs_reg_base);
+free_pmd:
+	iounmap(pmd_reg_base);
+free_apb:
+	iounmap(apb_base);
+
+	return ret;
+}
+
+static int mt7988_2p5ge_phy_load_fw(struct phy_device *phydev)
 {
 	struct device *dev = &phydev->mdio.dev;
 	void __iomem *mcu_csr_base, *pmb_addr;
@@ -326,7 +629,15 @@ static int mt798x_2p5ge_phy_probe(struct
 	struct pinctrl *pinctrl;
 	int ret;
 
+	priv = devm_kzalloc(&phydev->mdio.dev, sizeof(struct mtk_socphy_priv),
+			    GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	phydev->priv = priv;
+
 	switch (phydev->drv->phy_id) {
+	case MTK_2P5GPHY_ID_MT7987:
 	case MTK_2P5GPHY_ID_MT7988:
 		/* This built-in 2.5GbE hardware only sets MDIO_DEVS_PMAPMD.
 		 * Set the rest by this driver since PCS/AN/VEND1/VEND2 MDIO
@@ -341,15 +652,36 @@ static int mt798x_2p5ge_phy_probe(struct
 		return -EINVAL;
 	}
 
-	ret = mt798x_2p5ge_phy_load_fw(phydev);
+	switch (phydev->drv->phy_id) {
+	case MTK_2P5GPHY_ID_MT7987:
+		ret = mt7987_2p5ge_phy_load_fw(phydev);
+		break;
+	case MTK_2P5GPHY_ID_MT7988:
+		ret = mt7988_2p5ge_phy_load_fw(phydev);
+		break;
+	default:
+		return -EINVAL;
+	}
+
 	if (ret < 0)
 		return ret;
 
+	switch (phydev->drv->phy_id) {
+	case MTK_2P5GPHY_ID_MT7987:
+		phy_clear_bits_mmd(phydev, MDIO_MMD_VEND2, MTK_PHY_LED0_ON_CTRL,
+				   MTK_PHY_LED_ON_POLARITY);
+		break;
+	case MTK_2P5GPHY_ID_MT7988:
+		phy_set_bits_mmd(phydev, MDIO_MMD_VEND2, MTK_PHY_LED0_ON_CTRL,
+				 MTK_PHY_LED_ON_POLARITY);
+		break;
+	default:
+		return -EINVAL;
+	}
+
 	/* Setup LED. On default, LED0 is on/off when link is up/down. As for
 	 * LED1, it blinks as tx/rx transmission takes place.
 	 */
-	phy_set_bits_mmd(phydev, MDIO_MMD_VEND2, MTK_PHY_LED0_ON_CTRL,
-			 MTK_PHY_LED_ON_POLARITY | MTK_2P5GPHY_LED_ON_SET);
 	phy_clear_bits_mmd(phydev, MDIO_MMD_VEND2, MTK_PHY_LED0_BLINK_CTRL,
 			   MTK_2P5GPHY_LED_TX_BLINK_SET |
 			   MTK_2P5GPHY_LED_RX_BLINK_SET);
@@ -365,12 +697,6 @@ static int mt798x_2p5ge_phy_probe(struct
 	if (IS_ERR(pinctrl))
 		dev_err(&phydev->mdio.dev, "Fail to set LED pins!\n");
 
-	priv = devm_kzalloc(&phydev->mdio.dev, sizeof(struct mtk_socphy_priv),
-			    GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-	phydev->priv = priv;
-
 	mtk_phy_leds_state_init(phydev);
 
 	return 0;
@@ -378,6 +704,25 @@ static int mt798x_2p5ge_phy_probe(struct
 
 static struct phy_driver mtk_2p5gephy_driver[] = {
 	{
+		PHY_ID_MATCH_MODEL(MTK_2P5GPHY_ID_MT7987),
+		.name = "MediaTek MT7987 2.5GbE PHY",
+		.probe = mt798x_2p5ge_phy_probe,
+		.config_init = mt798x_2p5ge_phy_config_init,
+		.config_aneg = mt798x_2p5ge_phy_config_aneg,
+		.get_features = mt798x_2p5ge_phy_get_features,
+		.read_status = mt798x_2p5ge_phy_read_status,
+		.get_rate_matching = mt798x_2p5ge_phy_get_rate_matching,
+		.suspend = genphy_suspend,
+		.resume = genphy_resume,
+		.read_page = mtk_phy_read_page,
+		.write_page = mtk_phy_write_page,
+		.led_blink_set = mt798x_2p5ge_phy_led_blink_set,
+		.led_brightness_set = mt798x_2p5ge_phy_led_brightness_set,
+		.led_hw_is_supported = mt798x_2p5ge_phy_led_hw_is_supported,
+		.led_hw_control_get = mt798x_2p5ge_phy_led_hw_control_get,
+		.led_hw_control_set = mt798x_2p5ge_phy_led_hw_control_set,
+	},
+	{
 		PHY_ID_MATCH_MODEL(MTK_2P5GPHY_ID_MT7988),
 		.name = "MediaTek MT7988 2.5GbE PHY",
 		.probe = mt798x_2p5ge_phy_probe,
