[PATCH net v2 4/5] net: mvneta: unwind a failed mqprio setup

[Josef Schlehofer <pepe.schlehofer@gmail.com>]

A failed mqprio offload setup leaves part of its configuration in the
hardware. For example, when one max_rate is not a multiple of
10 kbit/s, the command fails but the TX queues set up before it stay
rate limited.

mvneta_setup_mqprio() returns on an invalid queue range or rate after
it has already mapped RX queues, switched the rate limit mode and set
the rates of some TX queues. The qdisc then fails to attach and the
driver is not called again, so nothing removes them.

Undo the RX queue mapping, the rate limits and the traffic classes
when the setup fails.

Tested on a Turris Omnia (Armada 385). After mqprio failed on a
max_rate of 10001kbit, TX queues 0-3 stayed limited to 10 Mbit/s and
the VLAN priority map stayed set. With this patch neither is left
behind.

Fixes: e9f7099d0730 ("net: mvneta: Allow having more than one queue per TC")
Fixes: 2551dc9e398c ("net: mvneta: Add TC traffic shaping offload")
Signed-off-by: Josef Schlehofer <pepe.schlehofer@gmail.com>
---
 drivers/net/ethernet/marvell/mvneta.c | 28 +++++++++++++++++++--------
 1 file changed, 20 insertions(+), 8 deletions(-)

diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index 75d46349df37..d3a6bf8f383e 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -5299,8 +5299,10 @@ static int mvneta_setup_mqprio(struct net_device *dev,
 		for (rxq = mqprio->qopt.offset[tc];
 		     rxq < mqprio->qopt.count[tc] + mqprio->qopt.offset[tc];
 		     rxq++) {
-			if (rxq >= rxq_number)
-				return -EINVAL;
+			if (rxq >= rxq_number) {
+				ret = -EINVAL;
+				goto err;
+			}
 
 			mvneta_map_vlan_prio_to_rxq(pp, tc, rxq);
 		}
@@ -5311,29 +5313,39 @@ static int mvneta_setup_mqprio(struct net_device *dev,
 		return 0;
 	}
 
-	if (mqprio->qopt.num_tc > txq_number)
-		return -EINVAL;
+	if (mqprio->qopt.num_tc > txq_number) {
+		ret = -EINVAL;
+		goto err;
+	}
 
 	ret = mvneta_enable_per_queue_rate_limit(pp);
 	if (ret)
-		return ret;
+		goto err;
 
 	for (tc = 0; tc < mqprio->qopt.num_tc; tc++) {
 		for (txq = mqprio->qopt.offset[tc];
 		     txq < mqprio->qopt.count[tc] + mqprio->qopt.offset[tc];
 		     txq++) {
-			if (txq >= txq_number)
-				return -EINVAL;
+			if (txq >= txq_number) {
+				ret = -EINVAL;
+				goto err;
+			}
 
 			ret = mvneta_setup_queue_rates(pp, txq,
 						       mqprio->min_rate[tc],
 						       mqprio->max_rate[tc]);
 			if (ret)
-				return ret;
+				goto err;
 		}
 	}
 
 	return 0;
+
+err:
+	mvneta_clear_rx_prio_map(pp);
+	mvneta_disable_per_queue_rate_limit(pp);
+	netdev_reset_tc(dev);
+	return ret;
 }
 
 static int mvneta_setup_tc(struct net_device *dev, enum tc_setup_type type,
-- 
2.54.0 (Apple Git-157)