With the FPE preemption-class mapping error now propagated from stmmac_fpe_map_preemption_class(), tc_setup_dwmac510_mqprio() can fail on the mapping step. The error path used to call stmmac_reset_tc_mqprio(), which resets the number of real TX queues to priv->plat->tx_queues_to_use (the platform maximum), overwriting the value that was active before the offload was attempted (for example a lower count left over from a previous mqprio configuration). The issue can be triggered using the following configuration: # First mqprio config lowers the hw queue count. $tc qdisc add dev eth0 root handle 1: mqprio queues 4@0 4@4 # Replace mqprio configuration with a second one that fails FPE # preemption-class mapping. stmmac driver resets the real_num_tx_queues # to the platform maximum, losing the previous configuration. $tc qdisc replace dev eth0 root handle 2: mqprio queues 2@0 2@2 fp E P Save ndev->real_num_tx_queues before lowering it and restore it, together with the TC-to-queue and priority-to-TC mappings, when the FPE preemption-class mapping fails, instead of resetting the queue count to the platform maximum. Note that a failed setup makes the qdisc layer run mqprio_destroy() on the new qdisc, which resets dev->num_tc to 0; that is qdisc-layer handling, out of the driver's control, and the driver-side restore remains correct for the still-active previous configuration. Fixes: 195e4f409a40 ("net: stmmac: support fp parameter of tc-mqprio") Signed-off-by: Lorenzo Bianconi --- drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c | 80 ++++++++++++++++++------- 1 file changed, 60 insertions(+), 20 deletions(-) diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c index 5398616fcdfe..7d73b0c16f50 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c @@ -1237,6 +1237,30 @@ static int tc_query_caps(struct stmmac_priv *priv, } } +static int stmmac_set_ndev_tcs(struct net_device *ndev, u8 ntc, + struct netdev_tc_txq *tc_to_txq) +{ + int i, err; + + netdev_reset_tc(ndev); + if (!ntc) + return 0; + + err = netdev_set_num_tc(ndev, ntc); + if (err) + return err; + + for (i = 0; i < ntc; i++) { + u16 count, offset; + + count = tc_to_txq[i].count; + offset = tc_to_txq[i].offset; + netdev_set_tc_queue(ndev, i, count, offset); + } + + return 0; +} + static int stmmac_reset_tc_mqprio(struct net_device *ndev, struct netlink_ext_ack *extack) { @@ -1251,43 +1275,59 @@ static int stmmac_reset_tc_mqprio(struct net_device *ndev, static int tc_setup_dwmac510_mqprio(struct stmmac_priv *priv, struct tc_mqprio_qopt_offload *mqprio) { + unsigned int ndev_num_tx_queues, num_tx_queues = 0; + struct netdev_tc_txq ndev_tc_to_txq[TC_MAX_QUEUE]; + struct netdev_tc_txq tc_to_txq[TC_MAX_QUEUE] = {}; struct netlink_ext_ack *extack = mqprio->extack; 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; + u8 ndev_prio_tc_map[TC_BITMASK + 1]; + int i, err, ndev_ntc; - if (!num_tc) + if (!qopt->num_tc) return stmmac_reset_tc_mqprio(ndev, extack); - err = netdev_set_num_tc(ndev, num_tc); - if (err) - return err; - - for (u32 tc = 0; tc < num_tc; tc++) { - offset = qopt->offset[tc]; - count = qopt->count[tc]; - num_stack_tx_queues += count; + if (qopt->num_tc > ARRAY_SIZE(tc_to_txq)) + return -EINVAL; - err = netdev_set_tc_queue(ndev, tc, count, offset); - if (err) - goto err_reset_tc; + /* save current tc values for reset */ + ndev_ntc = netdev_get_num_tc(ndev); + for (i = 0; i < ARRAY_SIZE(ndev->tc_to_txq); i++) + ndev_tc_to_txq[i].combined = + READ_ONCE(ndev->tc_to_txq[i].combined); + for (i = 0; i < ARRAY_SIZE(ndev_prio_tc_map); i++) + ndev_prio_tc_map[i] = READ_ONCE(ndev->prio_tc_map[i]); + + for (i = 0; i < qopt->num_tc; i++) { + tc_to_txq[i] = (struct netdev_tc_txq) { + .count = qopt->count[i], + .offset = qopt->offset[i], + }; + num_tx_queues += qopt->count[i]; } - err = netif_set_real_num_tx_queues(ndev, num_stack_tx_queues); + err = stmmac_set_ndev_tcs(ndev, qopt->num_tc, tc_to_txq); + if (err) + goto error_reset_tc; + + ndev_num_tx_queues = ndev->real_num_tx_queues; + err = netif_set_real_num_tx_queues(ndev, num_tx_queues); if (err) - goto err_reset_tc; + goto error_reset_tc; err = stmmac_fpe_map_preemption_class(priv, ndev, extack, mqprio->preemptible_tcs); if (err) - goto err_reset_tc; + goto error_reset_num_tx_queues; return 0; -err_reset_tc: - stmmac_reset_tc_mqprio(ndev, extack); +error_reset_num_tx_queues: + netif_set_real_num_tx_queues(ndev, ndev_num_tx_queues); +error_reset_tc: + stmmac_set_ndev_tcs(ndev, ndev_ntc, ndev_tc_to_txq); + for (i = 0; i < ARRAY_SIZE(ndev_prio_tc_map); i++) + netdev_set_prio_tc_map(ndev, i, ndev_prio_tc_map[i]); return err; } -- 2.55.0