[PATCH net-next v2 2/2] net: stmmac: add tc-mqprio qdisc offload

Implement offload of the tc-mqprio qdisc in the stmmac driver. The MTL
TX scheduler is switched to strict priority, and the priority (PSTQX)
of each TX queue is programmed according to the requested traffic class
mapping: queues assigned to a higher tc are given a higher priority and
are served first, while queues not mapped to any tc keep the lowest
priority.

The per-queue priorities are stored in the unified qdisc state
(priv->qdisc.prio) so they can be reprogrammed on device reopen, and
are applied through the MTL TX queue priority map registers. Export
stmmac_mac_config_tx_queues_prio() so the mqprio path can reprogram the
queue priorities. Destroying the mqprio qdisc, or failing its setup,
restores the devicetree configured scheduling algorithm, TX queue
priorities and TX queue weights.

Signed-off-by: Lorenzo Bianconi <[email protected]>
---
 drivers/net/ethernet/stmicro/stmmac/stmmac.h      |  2 ++
 drivers/net/ethernet/stmicro/stmmac/stmmac_main.c | 13 ++++++++-----
 drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c   | 22 +++++++++++++++++++++-
 3 files changed, 31 insertions(+), 6 deletions(-)

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index 823b40212233..1bb261fdcb0e 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -305,6 +305,7 @@ struct stmmac_priv {
 		bool enable;
 		u8 algo;
 		u32 quanta[MTL_MAX_TX_QUEUES];
+		u32 prio[MTL_MAX_TX_QUEUES];
 	} qdisc;
 	struct dma_features dma_cap;
 	struct stmmac_counters mmc;
@@ -400,6 +401,7 @@ enum stmmac_state {
 extern const struct dev_pm_ops stmmac_simple_pm_ops;
 
 void stmmac_set_tx_queue_weight(struct stmmac_priv *priv);
+void stmmac_mac_config_tx_queues_prio(struct stmmac_priv *priv);
 int stmmac_mdio_unregister(struct net_device *ndev);
 int stmmac_mdio_register(struct net_device *ndev);
 int stmmac_mdio_reset(struct mii_bus *mii);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 7d1473fbde54..c2786ecca530 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -3503,17 +3503,20 @@ static void stmmac_mac_config_rx_queues_prio(struct stmmac_priv *priv)
  *  @priv: driver private structure
  *  Description: It is used for configuring the TX Queue Priority
  */
-static void stmmac_mac_config_tx_queues_prio(struct stmmac_priv *priv)
+void stmmac_mac_config_tx_queues_prio(struct stmmac_priv *priv)
 {
 	u8 tx_queues_count = priv->plat->tx_queues_to_use;
 	u8 queue;
-	u32 prio;
 
 	for (queue = 0; queue < tx_queues_count; queue++) {
-		if (!priv->plat->tx_queues_cfg[queue].use_prio)
-			continue;
+		u32 prio = 0;
+
+		if (priv->qdisc.enable &&
+		    priv->qdisc.algo == MTL_TX_ALGORITHM_SP)
+			prio = priv->qdisc.prio[queue];
+		else if (priv->plat->tx_queues_cfg[queue].use_prio)
+			prio = priv->plat->tx_queues_cfg[queue].prio;
 
-		prio = priv->plat->tx_queues_cfg[queue].prio;
 		stmmac_tx_queue_prio(priv, priv->hw, prio, queue);
 	}
 }
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
index 5dcd18b5808e..3ba71be24c64 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
@@ -1218,6 +1218,7 @@ static void stmmac_qdisc_restore_dt_config(struct stmmac_priv *priv)
 	/* reset to the dt configured algorithm. */
 	priv->qdisc.enable = false;
 	stmmac_set_tx_queue_weight(priv);
+	stmmac_mac_config_tx_queues_prio(priv);
 	stmmac_prog_mtl_tx_algorithms(priv, priv->hw,
 				      priv->plat->tx_sched_algorithm);
 }
@@ -1321,17 +1322,19 @@ static void stmmac_reset_tc_mqprio(struct net_device *ndev,
 	netdev_reset_tc(ndev);
 	netif_set_real_num_tx_queues(ndev, priv->plat->tx_queues_to_use);
 	stmmac_fpe_map_preemption_class(priv, ndev, extack, 0);
+	stmmac_qdisc_restore_dt_config(priv);
 }
 
 static int tc_setup_dwmac510_mqprio(struct stmmac_priv *priv,
 				    struct tc_mqprio_qopt_offload *mqprio)
 {
 	struct netlink_ext_ack *extack = mqprio->extack;
+	struct plat_stmmacenet_data *pdata = priv->plat;
 	struct tc_mqprio_qopt *qopt = &mqprio->qopt;
 	u32 offset, count, num_stack_tx_queues = 0;
 	struct net_device *ndev = priv->dev;
 	u32 num_tc = qopt->num_tc;
-	int err;
+	int i, tc, err;
 
 	if (!num_tc) {
 		stmmac_reset_tc_mqprio(ndev, extack);
@@ -1361,6 +1364,23 @@ static int tc_setup_dwmac510_mqprio(struct stmmac_priv *priv,
 	if (err)
 		goto err_reset_tc;
 
+	/* Queues not mapped to any TC default to the lowest priority */
+	for (i = 0; i < pdata->tx_queues_to_use; i++) {
+		priv->qdisc.prio[i] = 0;
+		stmmac_tx_queue_prio(priv, priv->hw, 0, i);
+	}
+
+	for (tc = 0; tc < num_tc; tc++) {
+		for (i = qopt->offset[tc];
+		     i < qopt->offset[tc] + qopt->count[tc]; i++) {
+			priv->qdisc.prio[i] = tc;
+			stmmac_tx_queue_prio(priv, priv->hw, tc, i);
+		}
+	}
+	stmmac_prog_mtl_tx_algorithms(priv, priv->hw, MTL_TX_ALGORITHM_SP);
+	priv->qdisc.algo = MTL_TX_ALGORITHM_SP;
+	priv->qdisc.enable = true;
+
 	return 0;
 
 err_reset_tc:

-- 
2.55.0