[net v2 PATCH] net: stmmac: Update CBS parameters when speed changes after linking up

[Xiaolei Wang <xiaolei.wang@windriver.com>]

When the port is relinked, if the speed changes, the CBS parameters
should be updated, so saving the user transmission parameters so
that idle_slope and send_slope can be recalculated after the speed
changes after linking up can help reconfigure CBS after the speed
changes.

Fixes: 1f705bc61aee ("net: stmmac: Add support for CBS QDISC")
Signed-off-by: Xiaolei Wang <xiaolei.wang@windriver.com>
---
v1 -> v2
 - Update CBS parameters when speed changes

 drivers/net/ethernet/stmicro/stmmac/stmmac.h  |  4 ++
 .../net/ethernet/stmicro/stmmac/stmmac_main.c | 45 ++++++++++++++++++-
 .../net/ethernet/stmicro/stmmac/stmmac_tc.c   |  6 +++
 3 files changed, 53 insertions(+), 2 deletions(-)

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index b23b920eedb1..7a386b43f117 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -356,6 +356,10 @@ struct stmmac_priv {
 	unsigned int rfs_entries_total;
 	struct stmmac_rfs_entry *rfs_entries;
 
+	/* Save CBS configuration to adjust parameters when port link up speed changes */
+	s32 old_idleslope[MTL_MAX_TX_QUEUES];
+	s32 old_sendslope[MTL_MAX_TX_QUEUES];
+
 	/* Pulse Per Second output */
 	struct stmmac_pps_cfg pps[STMMAC_PPS_MAX];
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index b3afc7cb7d72..44db35a7ca6a 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -138,6 +138,7 @@ static void stmmac_tx_timer_arm(struct stmmac_priv *priv, u32 queue);
 static void stmmac_flush_tx_descriptors(struct stmmac_priv *priv, int queue);
 static void stmmac_set_dma_operation_mode(struct stmmac_priv *priv, u32 txmode,
 					  u32 rxmode, u32 chan);
+static void stmmac_configure_cbs(struct stmmac_priv *priv);
 
 #ifdef CONFIG_DEBUG_FS
 static const struct net_device_ops stmmac_netdev_ops;
@@ -1075,7 +1076,11 @@ static void stmmac_mac_link_up(struct phylink_config *config,
 		}
 	}
 
-	priv->speed = speed;
+	/* Update speed and CBS parameters when speed changes */
+	if (speed != priv->speed) {
+		priv->speed = speed;
+		stmmac_configure_cbs(priv);
+	}
 
 	if (priv->plat->fix_mac_speed)
 		priv->plat->fix_mac_speed(priv->plat->bsp_priv, speed, mode);
@@ -1115,6 +1120,7 @@ static void stmmac_mac_link_up(struct phylink_config *config,
 
 	if (priv->plat->flags & STMMAC_FLAG_HWTSTAMP_CORRECT_LATENCY)
 		stmmac_hwtstamp_correct_latency(priv, priv);
+
 }
 
 static const struct phylink_mac_ops stmmac_phylink_mac_ops = {
@@ -3182,13 +3188,42 @@ static void stmmac_set_tx_queue_weight(struct stmmac_priv *priv)
 /**
  *  stmmac_configure_cbs - Configure CBS in TX queue
  *  @priv: driver private structure
- *  Description: It is used for configuring CBS in AVB TX queues
+ *  Description: It is used for configuring CBS in AVB TX queues,
+ *  and when the speed changes, update CBS parameters to reconfigure
  */
 static void stmmac_configure_cbs(struct stmmac_priv *priv)
 {
 	u32 tx_queues_count = priv->plat->tx_queues_to_use;
 	u32 mode_to_use;
 	u32 queue;
+	u32 ptr, speed_div;
+	u64 value;
+
+	/* Port Transmit Rate and Speed Divider */
+	switch (priv->speed) {
+	case SPEED_10000:
+		ptr = 32;
+		speed_div = 10000000;
+		break;
+	case SPEED_5000:
+		ptr = 32;
+		speed_div = 5000000;
+		break;
+	case SPEED_2500:
+		ptr = 8;
+		speed_div = 2500000;
+		break;
+	case SPEED_1000:
+		ptr = 8;
+		speed_div = 1000000;
+		break;
+	case SPEED_100:
+		ptr = 4;
+		speed_div = 100000;
+		break;
+	default:
+		netdev_dbg(priv->dev, "link speed is not known\n");
+	}
 
 	/* queue 0 is reserved for legacy traffic */
 	for (queue = 1; queue < tx_queues_count; queue++) {
@@ -3196,6 +3231,12 @@ static void stmmac_configure_cbs(struct stmmac_priv *priv)
 		if (mode_to_use == MTL_QUEUE_DCB)
 			continue;
 
+		value = div_s64(priv->old_idleslope[queue] * 1024ll * ptr, speed_div);
+		priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0);
+
+		value = div_s64(-priv->old_sendslope[queue] * 1024ll * ptr, speed_div);
+		priv->plat->tx_queues_cfg[queue].send_slope = value & GENMASK(31, 0);
+
 		stmmac_config_cbs(priv, priv->hw,
 				priv->plat->tx_queues_cfg[queue].send_slope,
 				priv->plat->tx_queues_cfg[queue].idle_slope,
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
index 222540b55480..d3526ad91aff 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
@@ -355,6 +355,9 @@ static int tc_setup_cbs(struct stmmac_priv *priv,
 	if (!priv->dma_cap.av)
 		return -EOPNOTSUPP;
 
+	if (!netif_carrier_ok(priv->dev))
+		return -ENETDOWN;
+
 	/* Port Transmit Rate and Speed Divider */
 	switch (priv->speed) {
 	case SPEED_10000:
@@ -397,6 +400,9 @@ static int tc_setup_cbs(struct stmmac_priv *priv,
 		priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_DCB;
 	}
 
+	priv->old_idleslope[queue] = qopt->idleslope;
+	priv->old_sendslope[queue] = qopt->sendslope;
+
 	/* Final adjustments for HW */
 	value = div_s64(qopt->idleslope * 1024ll * ptr, speed_div);
 	priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0);
-- 
2.25.1