[PATCH net-next v2 03/10] net: stmmac: xgmac: Implement tx_queue_prio()

[Jose Abreu]

Implement the TX Queue Priority callback in XGMAC core.

Signed-off-by: Jose Abreu <joab...@synopsys.com>

---
Cc: Giuseppe Cavallaro <peppe.cavall...@st.com>
Cc: Alexandre Torgue <alexandre.tor...@st.com>
Cc: Jose Abreu <joab...@synopsys.com>
Cc: "David S. Miller" <da...@davemloft.net>
Cc: Maxime Coquelin <mcoquelin.st...@gmail.com>
Cc: net...@vger.kernel.org
Cc: linux-st...@st-md-mailman.stormreply.com
Cc: linux-arm-ker...@lists.infradead.org
Cc: linux-kernel@vger.kernel.org
---
 drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h      |  4 ++++
 drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c | 19 ++++++++++++++++++-
 2 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h 
b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
index 2fe6cd5a8ea0..95c9019567f4 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
@@ -124,6 +124,10 @@
 #define XGMAC_MTL_RXQ_DMA_MAP1         0x00001034
 #define XGMAC_QxMDMACH(x)              GENMASK((x) * 8 + 3, (x) * 8)
 #define XGMAC_QxMDMACH_SHIFT(x)                ((x) * 8)
+#define XGMAC_TC_PRTY_MAP0             0x00001040
+#define XGMAC_TC_PRTY_MAP1             0x00001044
+#define XGMAC_PSTC(x)                  GENMASK((x) * 8 + 7, (x) * 8)
+#define XGMAC_PSTC_SHIFT(x)            ((x) * 8)
 #define XGMAC_MTL_TXQ_OPMODE(x)                (0x00001100 + (0x80 * (x)))
 #define XGMAC_TQS                      GENMASK(25, 16)
 #define XGMAC_TQS_SHIFT                        16
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c 
b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
index 44d728c5c4e4..03ccb3fe48ba 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
@@ -114,6 +114,23 @@ static void dwxgmac2_rx_queue_prio(struct mac_device_info 
*hw, u32 prio,
        writel(value, ioaddr + reg);
 }
 
+static void dwxgmac2_tx_queue_prio(struct mac_device_info *hw, u32 prio,
+                                  u32 queue)
+{
+       void __iomem *ioaddr = hw->pcsr;
+       u32 value, reg;
+
+       reg = (queue < 4) ? XGMAC_TC_PRTY_MAP0 : XGMAC_TC_PRTY_MAP1;
+       if (queue >= 4)
+               queue -= 4;
+
+       value = readl(ioaddr + reg);
+       value &= ~XGMAC_PSTC(queue);
+       value |= (prio << XGMAC_PSTC_SHIFT(queue)) & XGMAC_PSTC(queue);
+
+       writel(value, ioaddr + reg);
+}
+
 static void dwxgmac2_prog_mtl_rx_algorithms(struct mac_device_info *hw,
                                            u32 rx_alg)
 {
@@ -359,7 +376,7 @@ const struct stmmac_ops dwxgmac210_ops = {
        .rx_ipc = dwxgmac2_rx_ipc,
        .rx_queue_enable = dwxgmac2_rx_queue_enable,
        .rx_queue_prio = dwxgmac2_rx_queue_prio,
-       .tx_queue_prio = NULL,
+       .tx_queue_prio = dwxgmac2_tx_queue_prio,
        .rx_queue_routing = NULL,
        .prog_mtl_rx_algorithms = dwxgmac2_prog_mtl_rx_algorithms,
        .prog_mtl_tx_algorithms = dwxgmac2_prog_mtl_tx_algorithms,
-- 
2.7.4