[PATCH net-next 3/4] net: macb: Add MQPRIO qdisc hardware offload support

Add support for TC_SETUP_QDISC_MQPRIO hardware offload, allowing
traffic class to queue mapping via the mqprio qdisc.

Implement macb_setup_mqprio() which configures TC-to-queue mappings
through netdev_set_num_tc() and netdev_set_tc_queue(), and resets
them when num_tc is zero. The driver advertises TC_MQPRIO_HW_OFFLOAD_TCS
offload level.

Add macb_tc_query_caps() to report mqprio capabilities. Setting
validate_queue_counts to true delegates queue count and overlap
validation to the mqprio core via mqprio_validate_queue_counts(),
avoiding redundant checks in the driver.

Signed-off-by: Vineeth Karumanchi <[email protected]>
---
 drivers/net/ethernet/cadence/macb_main.c | 59 ++++++++++++++++++++++++
 1 file changed, 59 insertions(+)

diff --git a/drivers/net/ethernet/cadence/macb_main.c b/drivers/net/ethernet/cadence/macb_main.c
index 508d952e2ae7..00355cc4b434 100644
--- a/drivers/net/ethernet/cadence/macb_main.c
+++ b/drivers/net/ethernet/cadence/macb_main.c
@@ -4515,6 +4515,60 @@ static int macb_setup_taprio(struct net_device *ndev,
 	return err;
 }
 
+static int macb_setup_mqprio(struct net_device *ndev,
+			     struct tc_mqprio_qopt_offload *mqprio)
+{
+	struct tc_mqprio_qopt *qopt = &mqprio->qopt;
+	u8 num_tc = qopt->num_tc;
+	int err;
+	u8 i;
+
+	/* Handle reset case early */
+	if (!num_tc) {
+		netdev_reset_tc(ndev);
+		return 0;
+	}
+
+	/* Configure traffic classes */
+	qopt->hw = TC_MQPRIO_HW_OFFLOAD_TCS;
+
+	err = netdev_set_num_tc(ndev, num_tc);
+	if (err)
+		return err;
+
+	for (i = 0; i < num_tc; i++) {
+		err = netdev_set_tc_queue(ndev, i, qopt->count[i],
+					  qopt->offset[i]);
+		if (err)
+			goto err_reset_tc;
+
+		netdev_dbg(ndev, "MQPRIO: TC%d -> queue %u (count=%u)\n",
+			   i, qopt->offset[i], qopt->count[i]);
+	}
+
+	return 0;
+
+err_reset_tc:
+	netdev_reset_tc(ndev);
+	return err;
+}
+
+static int macb_tc_query_caps(struct net_device *dev,
+			      struct tc_query_caps_base *base)
+{
+	switch (base->type) {
+	case TC_SETUP_QDISC_MQPRIO: {
+		struct tc_mqprio_caps *caps = base->caps;
+
+		caps->validate_queue_counts = true;
+
+		return 0;
+	}
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
 static int macb_setup_tc(struct net_device *dev, enum tc_setup_type type,
 			 void *type_data)
 {
@@ -4523,6 +4577,9 @@ static int macb_setup_tc(struct net_device *dev, enum tc_setup_type type,
 	if (!dev || !type_data)
 		return -EINVAL;
 
+	if (type == TC_QUERY_CAPS)
+		return macb_tc_query_caps(dev, type_data);
+
 	bp = netdev_priv(dev);
 
 	if (unlikely(!(dev->hw_features & NETIF_F_HW_TC)))
@@ -4535,6 +4592,8 @@ static int macb_setup_tc(struct net_device *dev, enum tc_setup_type type,
 	}
 
 	switch (type) {
+	case TC_SETUP_QDISC_MQPRIO:
+		return macb_setup_mqprio(dev, type_data);
 	case TC_SETUP_QDISC_TAPRIO:
 		return macb_setup_taprio(dev, type_data);
 	default:
-- 
2.44.4