openwrt/target/linux/ipq806x/patches-5.15/101-dwmac-ipq806x-qsgmii-pcs-all-ch-ctl.patch

--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-ipq806x.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-ipq806x.c
@@ -64,6 +64,17 @@
 #define NSS_COMMON_CLK_DIV_SGMII_100		4
 #define NSS_COMMON_CLK_DIV_SGMII_10		49
 
+#define QSGMII_PCS_ALL_CH_CTL			0x80
+#define QSGMII_PCS_CH_SPEED_FORCE		0x2
+#define QSGMII_PCS_CH_SPEED_10			0x0
+#define QSGMII_PCS_CH_SPEED_100			0x4
+#define QSGMII_PCS_CH_SPEED_1000		0x8
+#define QSGMII_PCS_CH_SPEED_MASK		(QSGMII_PCS_CH_SPEED_FORCE | \
+						 QSGMII_PCS_CH_SPEED_10 | \
+						 QSGMII_PCS_CH_SPEED_100 | \
+						 QSGMII_PCS_CH_SPEED_1000)
+#define QSGMII_PCS_CH_SPEED_SHIFT(x)		(x * 4)
+
 #define QSGMII_PCS_CAL_LCKDT_CTL		0x120
 #define QSGMII_PCS_CAL_LCKDT_CTL_RST		BIT(19)
 
@@ -242,6 +253,36 @@ static void ipq806x_gmac_fix_mac_speed(v
 	ipq806x_gmac_set_speed(gmac, speed);
 }
 
+static int
+ipq806x_gmac_get_qsgmii_pcs_speed_val(struct platform_device *pdev) {
+	struct device_node *fixed_link_node;
+	int rv;
+	int fixed_link_speed;
+
+	if (!of_phy_is_fixed_link(pdev->dev.of_node))
+		return 0;
+
+	fixed_link_node = of_get_child_by_name(pdev->dev.of_node, "fixed-link");
+	if (!fixed_link_node)
+		return -1;
+
+	rv = of_property_read_u32(fixed_link_node, "speed", &fixed_link_speed);
+	of_node_put(fixed_link_node);
+	if (rv)
+		return -1;
+
+	switch (fixed_link_speed) {
+	case SPEED_1000:
+		return QSGMII_PCS_CH_SPEED_FORCE | QSGMII_PCS_CH_SPEED_1000;
+	case SPEED_100:
+		return QSGMII_PCS_CH_SPEED_FORCE | QSGMII_PCS_CH_SPEED_100;
+	case SPEED_10:
+		return QSGMII_PCS_CH_SPEED_FORCE | QSGMII_PCS_CH_SPEED_10;
+	}
+
+	return -1;
+}
+
 static int ipq806x_gmac_probe(struct platform_device *pdev)
 {
 	struct plat_stmmacenet_data *plat_dat;
@@ -250,6 +291,7 @@ static int ipq806x_gmac_probe(struct pla
 	struct ipq806x_gmac *gmac;
 	int val;
 	int err;
+	int qsgmii_pcs_speed;
 
 	val = stmmac_get_platform_resources(pdev, &stmmac_res);
 	if (val)
@@ -339,6 +381,17 @@ static int ipq806x_gmac_probe(struct pla
 			     0x1ul << QSGMII_PHY_RX_INPUT_EQU_OFFSET |
 			     0x2ul << QSGMII_PHY_CDR_PI_SLEW_OFFSET |
 			     0xCul << QSGMII_PHY_TX_DRV_AMP_OFFSET);
+
+		qsgmii_pcs_speed = ipq806x_gmac_get_qsgmii_pcs_speed_val(pdev);
+		if (qsgmii_pcs_speed != -1) {
+			regmap_update_bits(
+			    gmac->qsgmii_csr,
+			    QSGMII_PCS_ALL_CH_CTL,
+			    QSGMII_PCS_CH_SPEED_MASK <<
+				QSGMII_PCS_CH_SPEED_SHIFT(gmac->id),
+			    qsgmii_pcs_speed <<
+				QSGMII_PCS_CH_SPEED_SHIFT(gmac->id));
+		}
 	}
 
 	plat_dat->has_gmac = true;