From 5ae8afb5643493ef89362e42c3855c2b9a69a369 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E5=91=89=E7=9C=9F?= <kuretru@gmail.com>
Date: Sun, 14 Nov 2021 13:18:43 +0800
Subject: [PATCH 2/4] net: phy led fixup

Change-Id: I1ee517f541a2933b5d622222780aefed7d2959f6
---
 .../net/ethernet/stmicro/stmmac/stmmac_main.c | 148 ++++++++++++++++++
 1 file changed, 148 insertions(+)

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index cff46c49b72f..a179ac1be97d 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -4260,6 +4260,140 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
 	return 0;
 }
 
+#define RTL_8211E_PHY_ID  0x001cc915
+#define RTL_8211F_PHY_ID  0x001cc916
+#define RTL_8201F_PHY_ID  0x001cc816
+#define DP_83848_PHY_ID   0x20005c90
+
+static int phy_rtl8211e_led_fixup(struct phy_device *phydev)
+{
+	int val;
+
+	printk("%s in\n", __func__);
+
+	/*switch to extension page44*/
+	phy_write(phydev, 31, 0x07);
+	phy_write(phydev, 30, 0x2c);
+
+	/*set led1(yellow) act*/
+	val = phy_read(phydev, 26);
+	val &= (~(1<<4));// bit4=0
+	val |= (1<<5);// bit5=1
+	val &= (~(1<<6));// bit6=0
+	phy_write(phydev, 26, val);
+
+	/*set led0(green) link*/
+	val = phy_read(phydev, 28);
+	val |= (7<<0);// bit0,1,2=1
+	val &= (~(7<<4));// bit4,5,6=0
+	val &= (~(7<<8));// bit8,9,10=0
+	phy_write(phydev, 28, val);
+	/*switch back to page0*/
+	phy_write(phydev,31,0x00);
+
+	return 0;
+}
+
+static int phy_rtl8211f_led_fixup(struct phy_device *phydev)
+{
+	int value;
+
+	printk("%s in\n", __func__);
+
+	// Switch page to 0xd04
+	phy_write(phydev, 31, 0xd04);
+
+	// Mode select
+	value = phy_read(phydev, 16);
+	value &= 0x7fff; // Mode A
+	// value |= 0x8000; // Mode B
+	phy_write(phydev, 16, value);
+
+	// Set LED0
+	value = phy_read(phydev, 16);
+	value &= 0xffe0; // clean bits
+	// value |= 0x0008; // link
+	// value |= 0x0010; // active
+	phy_write(phydev, 16, value);
+
+	// Set LED1(green)
+	value = phy_read(phydev, 16);
+	value &= 0xfc1f; // clean bits
+	value |= 0x0160; // link
+	// value |= 0x0200; // active
+	phy_write(phydev, 16, value);
+
+	// Set LED2(yellow)
+	value = phy_read(phydev, 16);
+	value &= 0x83ff; // clean bits
+	value |= 0x2C00; // link
+	value |= 0x4000; // active
+	phy_write(phydev, 16, value);
+ 
+	// Set EEE LED Function
+	value = phy_read(phydev, 11);
+	value &= 0xfff1; // clean bits
+	// value |= 0x0002; // LED0
+	// value |= 0x0004; // LED1
+	value |= 0x0008; // LED2
+	phy_write(phydev, 11, value);
+
+	// Switch back to page0
+	phy_write(phydev, 31, 0xa42);
+
+	return 0;
+}
+
+static int phy_rtl8201f_led_fixup(struct phy_device *phydev)
+{
+	int value;
+
+	printk("%s in\n", __func__);
+
+	/* switch to page 7 */
+	value = phy_read(phydev, 31);
+	value &= 0xff00;
+	value |= 0x0007;
+	value = phy_write(phydev, 31, value);
+
+	/* set customized led enable */
+	value = phy_read(phydev, 19);
+	value |= (0x1<<3);
+	phy_write(phydev, 19, value);
+
+	value &= 0x0000;
+	value |= (0x1<<3);
+	value |= (0x1<<5);
+	phy_write(phydev, 17, value);
+
+	/* back to page 0 */
+	value = phy_read(phydev, 31);
+	value &= 0x0000;
+	value = phy_write(phydev, 31, value);
+
+	return 0;
+}
+
+static int phy_dp83848_led_fixup(struct phy_device *phydev)
+{
+	int value;
+
+	if (phydev->phy_id != DP_83848_PHY_ID)
+		return 0;
+
+	printk("%s in\n", __func__);
+
+	value = phy_read(phydev, 0x18);
+	value &= ~(1<<2);
+	phy_write(phydev, 0x18, value);
+
+	value = phy_read(phydev, 0x19);
+	value &= ~(1<<5);
+	phy_write(phydev, 0x19, value);
+
+	return 0;
+}
+
 /**
  * stmmac_dvr_probe
  * @device: device pointer
@@ -4453,6 +4587,20 @@ int stmmac_dvr_probe(struct device *device,
 			    __func__);
 #endif
 
+	/* register the PHY board fixup */
+	ret = phy_register_fixup_for_uid(RTL_8211E_PHY_ID, 0xffffffff, phy_rtl8211e_led_fixup);
+	if (ret)
+		pr_warn("Cannot register PHY board fixup.\n");
+	ret = phy_register_fixup_for_uid(RTL_8211F_PHY_ID, 0xffffffff, phy_rtl8211f_led_fixup);
+	if (ret)
+		pr_warn("Cannot register PHY board fixup.\n");
+	ret = phy_register_fixup_for_uid(RTL_8201F_PHY_ID, 0xffffffff, phy_rtl8201f_led_fixup);
+	if (ret)
+		pr_warn("Cannot register PHY board fixup.\n");
+	ret = phy_register_fixup_for_uid(DP_83848_PHY_ID, 0xffffffff, phy_dp83848_led_fixup);
+	if (ret)
+		pr_warn("Cannot register PHY board fixup.\n");
+	
 	return ret;
 
 error_netdev_register:
-- 
2.25.1