[RFC PATCH net-next 4/5] net: macb: Configure High Speed Mac for given speed.

[Vineeth Karumanchi <vineeth.karumanchi@xxxxxxx>]

HS Mac configuration steps:
- Configure speed and serdes rate bits of USX_CONTROL register from
  user specified speed in the device-tree.
- Enable HS Mac for 5G and 10G speeds.
- Reset RX receive path to achieve USX block lock for the
  configured serdes rate.
- Wait for USX block lock synchronization.

Move the initialization instances to macb_usx_pcs_link_up().

Signed-off-by: Vineeth Karumanchi <vineeth.karumanchi@xxxxxxx>
---
 drivers/net/ethernet/cadence/macb.h      |  1 +
 drivers/net/ethernet/cadence/macb_main.c | 57 ++++++++++++++++++++----
 2 files changed, 50 insertions(+), 8 deletions(-)

diff --git a/drivers/net/ethernet/cadence/macb.h b/drivers/net/ethernet/cadence/macb.h
index 47e80fa72865..ed4edeac3a59 100644
--- a/drivers/net/ethernet/cadence/macb.h
+++ b/drivers/net/ethernet/cadence/macb.h
@@ -825,6 +825,7 @@
 	})
 
 #define MACB_READ_NSR(bp)	macb_readl(bp, NSR)
+#define MACB_READ_USX_STATUS(bp)	gem_readl(bp, USX_STATUS)
 
 /* struct macb_dma_desc - Hardware DMA descriptor
  * @addr: DMA address of data buffer
diff --git a/drivers/net/ethernet/cadence/macb_main.c b/drivers/net/ethernet/cadence/macb_main.c
index 3f9dc0b037c0..7beb775a0bd7 100644
--- a/drivers/net/ethernet/cadence/macb_main.c
+++ b/drivers/net/ethernet/cadence/macb_main.c
@@ -94,6 +94,7 @@ struct sifive_fu540_macb_mgmt {
 #define MACB_PM_TIMEOUT  100 /* ms */
 
 #define MACB_MDIO_TIMEOUT	1000000 /* in usecs */
+#define GEM_SYNC_TIMEOUT	2500000 /* in usecs */
 
 /* DMA buffer descriptor might be different size
  * depends on hardware configuration:
@@ -564,14 +565,59 @@ static void macb_usx_pcs_link_up(struct phylink_pcs *pcs, unsigned int neg_mode,
 				 int duplex)
 {
 	struct macb *bp = container_of(pcs, struct macb, phylink_usx_pcs);
-	u32 config;
+	u32 speed_val, serdes_rate, config;
+	bool hs_mac = false;
+
+	switch (speed) {
+	case SPEED_1000:
+		speed_val = HS_SPEED_1000M;
+		serdes_rate = MACB_SERDES_RATE_1G;
+		break;
+	case SPEED_2500:
+		speed_val = HS_SPEED_2500M;
+		serdes_rate = MACB_SERDES_RATE_2_5G;
+		break;
+	case SPEED_5000:
+		speed_val = HS_SPEED_5000M;
+		serdes_rate = MACB_SERDES_RATE_5G;
+		hs_mac = true;
+		break;
+	case SPEED_10000:
+		speed_val = HS_SPEED_10000M;
+		serdes_rate = MACB_SERDES_RATE_10G;
+		hs_mac = true;
+		break;
+	default:
+		netdev_err(bp->dev, "Specified speed not supported\n");
+		return;
+	}
+
+	/* Enable HS MAC for high speeds */
+	if (hs_mac) {
+		config = macb_or_gem_readl(bp, NCR);
+		config |= GEM_BIT(ENABLE_HS_MAC);
+		macb_or_gem_writel(bp, NCR, config);
+	}
+
+	/* Configure HS MAC for specified speed */
+	config = gem_readl(bp, HS_MAC_CONFIG);
+	config = GEM_BFINS(HS_MAC_SPEED, speed_val, config);
+	gem_writel(bp, HS_MAC_CONFIG, config);
 
 	config = gem_readl(bp, USX_CONTROL);
-	config = GEM_BFINS(SERDES_RATE, MACB_SERDES_RATE_10G, config);
-	config = GEM_BFINS(USX_CTRL_SPEED, HS_SPEED_10000M, config);
+	config = GEM_BFINS(SERDES_RATE, serdes_rate, config);
+	config = GEM_BFINS(USX_CTRL_SPEED, speed_val, config);
 	config &= ~(GEM_BIT(TX_SCR_BYPASS) | GEM_BIT(RX_SCR_BYPASS));
+	config |= GEM_BIT(RX_SYNC_RESET);
+	gem_writel(bp, USX_CONTROL, config);
+	mdelay(250);
+	config &= ~GEM_BIT(RX_SYNC_RESET);
 	config |= GEM_BIT(TX_EN);
 	gem_writel(bp, USX_CONTROL, config);
+
+	if (readx_poll_timeout(MACB_READ_USX_STATUS, bp, config, config & GEM_BIT(USX_BLOCK_LOCK),
+			       1, GEM_SYNC_TIMEOUT))
+		netdev_err(bp->dev, "USX PCS block lock not achieved\n");
 }
 
 static void macb_usx_pcs_get_state(struct phylink_pcs *pcs,
@@ -662,7 +708,6 @@ static void macb_mac_config(struct phylink_config *config, unsigned int mode,
 			ctrl |= GEM_BIT(SGMIIEN) | GEM_BIT(PCSSEL);
 		} else if (state->interface == PHY_INTERFACE_MODE_10GBASER) {
 			ctrl |= GEM_BIT(PCSSEL);
-			ncr |= GEM_BIT(ENABLE_HS_MAC);
 		} else if (bp->caps & MACB_CAPS_MIIONRGMII &&
 			   bp->phy_interface == PHY_INTERFACE_MODE_MII) {
 			ncr |= MACB_BIT(MIIONRGMII);
@@ -766,10 +811,6 @@ static void macb_mac_link_up(struct phylink_config *config,
 
 	macb_or_gem_writel(bp, NCFGR, ctrl);
 
-	if (bp->phy_interface == PHY_INTERFACE_MODE_10GBASER)
-		gem_writel(bp, HS_MAC_CONFIG, GEM_BFINS(HS_MAC_SPEED, HS_SPEED_10000M,
-							gem_readl(bp, HS_MAC_CONFIG)));
-
 	spin_unlock_irqrestore(&bp->lock, flags);
 
 	if (!(bp->caps & MACB_CAPS_MACB_IS_EMAC))
-- 
2.34.1