openwrt/target/linux/mvebu/patches-5.10/701-v5.14-net-ethernet-marvell-mvnetaMQPrio.patch

From 4906887a8ae5f1296f8079bcf4565a6092a8e402 Mon Sep 17 00:00:00 2001
From: Maxime Chevallier <maxime.chevallier@bootlin.com>
Date: Tue, 16 Feb 2021 10:25:36 +0100
Subject: net: mvneta: Implement mqprio support

Implement a basic MQPrio support, inserting rules in RX that translate
the TC to prio mapping into vlan prio to queues.

The TX logic stays the same as when we don't offload the qdisc.

Signed-off-by: Maxime Chevallier <maxime.chevallier@bootlin.com>
Signed-off-by: David S. Miller <davem@davemloft.net>
---
 drivers/net/ethernet/marvell/mvneta.c | 61 +++++++++++++++++++++++++++++++++++
 1 file changed, 61 insertions(+)

(limited to 'drivers/net/ethernet/marvell/mvneta.c')

--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -102,6 +102,8 @@
 #define      MVNETA_TX_NO_DATA_SWAP              BIT(5)
 #define      MVNETA_DESC_SWAP                    BIT(6)
 #define      MVNETA_TX_BRST_SZ_MASK(burst)       ((burst) << 22)
+#define	MVNETA_VLAN_PRIO_TO_RXQ			 0x2440
+#define      MVNETA_VLAN_PRIO_RXQ_MAP(prio, rxq) ((rxq) << ((prio) * 3))
 #define MVNETA_PORT_STATUS                       0x2444
 #define      MVNETA_TX_IN_PRGRS                  BIT(0)
 #define      MVNETA_TX_FIFO_EMPTY                BIT(8)
@@ -490,6 +492,7 @@ struct mvneta_port {
 	u8 mcast_count[256];
 	u16 tx_ring_size;
 	u16 rx_ring_size;
+	u8 prio_tc_map[8];
 
 	phy_interface_t phy_interface;
 	struct device_node *dn;
@@ -4913,6 +4916,63 @@ static u16 mvneta_select_queue(struct ne
 }
 #endif
 
+static void mvneta_clear_rx_prio_map(struct mvneta_port *pp)
+{
+	mvreg_write(pp, MVNETA_VLAN_PRIO_TO_RXQ, 0);
+}
+
+static void mvneta_setup_rx_prio_map(struct mvneta_port *pp)
+{
+	u32 val = 0;
+	int i;
+
+	for (i = 0; i < rxq_number; i++)
+		val |= MVNETA_VLAN_PRIO_RXQ_MAP(i, pp->prio_tc_map[i]);
+
+	mvreg_write(pp, MVNETA_VLAN_PRIO_TO_RXQ, val);
+}
+
+static int mvneta_setup_mqprio(struct net_device *dev,
+			       struct tc_mqprio_qopt *qopt)
+{
+	struct mvneta_port *pp = netdev_priv(dev);
+	u8 num_tc;
+	int i;
+
+	qopt->hw = TC_MQPRIO_HW_OFFLOAD_TCS;
+	num_tc = qopt->num_tc;
+
+	if (num_tc > rxq_number)
+		return -EINVAL;
+
+	if (!num_tc) {
+		mvneta_clear_rx_prio_map(pp);
+		netdev_reset_tc(dev);
+		return 0;
+	}
+
+	memcpy(pp->prio_tc_map, qopt->prio_tc_map, sizeof(pp->prio_tc_map));
+
+	mvneta_setup_rx_prio_map(pp);
+
+	netdev_set_num_tc(dev, qopt->num_tc);
+	for (i = 0; i < qopt->num_tc; i++)
+		netdev_set_tc_queue(dev, i, qopt->count[i], qopt->offset[i]);
+
+	return 0;
+}
+
+static int mvneta_setup_tc(struct net_device *dev, enum tc_setup_type type,
+			   void *type_data)
+{
+	switch (type) {
+	case TC_SETUP_QDISC_MQPRIO:
+		return mvneta_setup_mqprio(dev, type_data);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
 static const struct net_device_ops mvneta_netdev_ops = {
 	.ndo_open            = mvneta_open,
 	.ndo_stop            = mvneta_stop,
@@ -4928,6 +4988,7 @@ static const struct net_device_ops mvnet
 #endif
 	.ndo_bpf	     = mvneta_xdp,
 	.ndo_xdp_xmit        = mvneta_xdp_xmit,
+	.ndo_setup_tc	     = mvneta_setup_tc,
 };
 
 static const struct ethtool_ops mvneta_eth_tool_ops = {