net: microchip: sparx5: add support for offloading tbf qdisc

Add support for offloading tbf qdisc to sparx5 qdisc.

The tbf qdisc makes it possible to attach a shaper on traffic egressing
from a port or a queue. Per-port tbf qdiscs are attached as a root qdisc
directly and queue tbf qdiscs are attached to one of the classes of a
parent qdisc (such as mqprio).

Signed-off-by: Daniel Machon <daniel.machon@microchip.com>
Signed-off-by: Steen Hegelund <steen.hegelund@microchip.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

1 files changed, 39 insertions, 0 deletions

diff --git a/drivers/net/ethernet/microchip/sparx5/sparx5_tc.c b/drivers/net/ethernet/microchip/sparx5/sparx5_tc.c
index 6e01a7c7c821..42b102009e7e 100644
--- a/drivers/net/ethernet/microchip/sparx5/sparx5_tc.c
+++ b/drivers/net/ethernet/microchip/sparx5/sparx5_tc.c
@@ -10,6 +10,19 @@
 #include "sparx5_main.h"
 #include "sparx5_qos.h"
 
+static void sparx5_tc_get_layer_and_idx(u32 parent, u32 portno, u32 *layer,
+					u32 *idx)
+{
+	if (parent == TC_H_ROOT) {
+		*layer = 2;
+		*idx = portno;
+	} else {
+		u32 queue = TC_H_MIN(parent) - 1;
+		*layer = 0;
+		*idx = SPX5_HSCH_L0_GET_IDX(portno, queue);
+	}
+}
+
 static int sparx5_tc_setup_qdisc_mqprio(struct net_device *ndev,
 					struct tc_mqprio_qopt_offload *m)
 {
@@ -21,12 +34,38 @@ static int sparx5_tc_setup_qdisc_mqprio(struct net_device *ndev,
 		return sparx5_tc_mqprio_add(ndev, m->qopt.num_tc);
 }
 
+static int sparx5_tc_setup_qdisc_tbf(struct net_device *ndev,
+				     struct tc_tbf_qopt_offload *qopt)
+{
+	struct sparx5_port *port = netdev_priv(ndev);
+	u32 layer, se_idx;
+
+	sparx5_tc_get_layer_and_idx(qopt->parent, port->portno, &layer,
+				    &se_idx);
+
+	switch (qopt->command) {
+	case TC_TBF_REPLACE:
+		return sparx5_tc_tbf_add(port, &qopt->replace_params, layer,
+					 se_idx);
+	case TC_TBF_DESTROY:
+		return sparx5_tc_tbf_del(port, layer, se_idx);
+	case TC_TBF_STATS:
+		return -EOPNOTSUPP;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return -EOPNOTSUPP;
+}
+
 int sparx5_port_setup_tc(struct net_device *ndev, enum tc_setup_type type,
 			 void *type_data)
 {
 	switch (type) {
 	case TC_SETUP_QDISC_MQPRIO:
 		return sparx5_tc_setup_qdisc_mqprio(ndev, type_data);
+	case TC_SETUP_QDISC_TBF:
+		return sparx5_tc_setup_qdisc_tbf(ndev, type_data);
 	default:
 		return -EOPNOTSUPP;
 	}