net: stmmac: xgmac: RX queue routing configuration

Commit abe80fdc6ee6 ("net: stmmac: RX queue routing configuration")
introduced RX queue routing to DWMAC4 core.
This patch extend the support to XGMAC2 core.

Signed-off-by: Furong Xu <0x1207@gmail.com>
Reviewed-by: Simon Horman <horms@kernel.org>
Link: https://lore.kernel.org/r/20230809020238.1136732-1-0x1207@gmail.com
Signed-off-by: Jakub Kicinski <kuba@kernel.org>

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
index 153321f..ce67b17 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
@@ -74,8 +74,20 @@
 #define XGMAC_RXQEN(x)                 GENMASK((x) * 2 + 1, (x) * 2)
 #define XGMAC_RXQEN_SHIFT(x)           ((x) * 2)
 #define XGMAC_RXQ_CTRL1                        0x000000a4
+#define XGMAC_AVCPQ                    GENMASK(31, 28)
+#define XGMAC_AVCPQ_SHIFT              28
+#define XGMAC_PTPQ                     GENMASK(27, 24)
+#define XGMAC_PTPQ_SHIFT               24
+#define XGMAC_TACPQE                   BIT(23)
+#define XGMAC_DCBCPQ                   GENMASK(19, 16)
+#define XGMAC_DCBCPQ_SHIFT             16
+#define XGMAC_MCBCQEN                  BIT(15)
+#define XGMAC_MCBCQ                    GENMASK(11, 8)
+#define XGMAC_MCBCQ_SHIFT              8
 #define XGMAC_RQ                       GENMASK(7, 4)
 #define XGMAC_RQ_SHIFT                 4
+#define XGMAC_UPQ                      GENMASK(3, 0)
+#define XGMAC_UPQ_SHIFT                        0
 #define XGMAC_RXQ_CTRL2                        0x000000a8
 #define XGMAC_RXQ_CTRL3                        0x000000ac
 #define XGMAC_PSRQ(x)                  GENMASK((x) * 8 + 7, (x) * 8)

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
index a0c2ef8..3878266 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
@@ -127,6 +127,36 @@ static void dwxgmac2_tx_queue_prio(struct mac_device_info *hw, u32 prio,
        writel(value, ioaddr + reg);
 }
 
+static void dwxgmac2_rx_queue_routing(struct mac_device_info *hw,
+                                     u8 packet, u32 queue)
+{
+       void __iomem *ioaddr = hw->pcsr;
+       u32 value;
+
+       static const struct stmmac_rx_routing dwxgmac2_route_possibilities[] = {
+               { XGMAC_AVCPQ, XGMAC_AVCPQ_SHIFT },
+               { XGMAC_PTPQ, XGMAC_PTPQ_SHIFT },
+               { XGMAC_DCBCPQ, XGMAC_DCBCPQ_SHIFT },
+               { XGMAC_UPQ, XGMAC_UPQ_SHIFT },
+               { XGMAC_MCBCQ, XGMAC_MCBCQ_SHIFT },
+       };
+
+       value = readl(ioaddr + XGMAC_RXQ_CTRL1);
+
+       /* routing configuration */
+       value &= ~dwxgmac2_route_possibilities[packet - 1].reg_mask;
+       value |= (queue << dwxgmac2_route_possibilities[packet - 1].reg_shift) &
+                dwxgmac2_route_possibilities[packet - 1].reg_mask;
+
+       /* some packets require extra ops */
+       if (packet == PACKET_AVCPQ)
+               value |= FIELD_PREP(XGMAC_TACPQE, 1);
+       else if (packet == PACKET_MCBCQ)
+               value |= FIELD_PREP(XGMAC_MCBCQEN, 1);
+
+       writel(value, ioaddr + XGMAC_RXQ_CTRL1);
+}
+
 static void dwxgmac2_prog_mtl_rx_algorithms(struct mac_device_info *hw,
                                            u32 rx_alg)
 {
@@ -1463,7 +1493,7 @@ const struct stmmac_ops dwxgmac210_ops = {
        .rx_queue_enable = dwxgmac2_rx_queue_enable,
        .rx_queue_prio = dwxgmac2_rx_queue_prio,
        .tx_queue_prio = dwxgmac2_tx_queue_prio,
-       .rx_queue_routing = NULL,
+       .rx_queue_routing = dwxgmac2_rx_queue_routing,
        .prog_mtl_rx_algorithms = dwxgmac2_prog_mtl_rx_algorithms,
        .prog_mtl_tx_algorithms = dwxgmac2_prog_mtl_tx_algorithms,
        .set_mtl_tx_queue_weight = dwxgmac2_set_mtl_tx_queue_weight,
@@ -1524,7 +1554,7 @@ const struct stmmac_ops dwxlgmac2_ops = {
        .rx_queue_enable = dwxlgmac2_rx_queue_enable,
        .rx_queue_prio = dwxgmac2_rx_queue_prio,
        .tx_queue_prio = dwxgmac2_tx_queue_prio,
-       .rx_queue_routing = NULL,
+       .rx_queue_routing = dwxgmac2_rx_queue_routing,
        .prog_mtl_rx_algorithms = dwxgmac2_prog_mtl_rx_algorithms,
        .prog_mtl_tx_algorithms = dwxgmac2_prog_mtl_tx_algorithms,
        .set_mtl_tx_queue_weight = dwxgmac2_set_mtl_tx_queue_weight,