net: stmmac: Add CBS support in XGMAC2

XGMAC2 uses the same CBS mechanism as GMAC5, only registers offset
changes. Lets use the same TC callbacks and implement the .config_cbs
callback in XGMAC2 core.

Signed-off-by: Jose Abreu <joabreu@synopsys.com>
Cc: David S. Miller <davem@davemloft.net>
Cc: Joao Pinto <jpinto@synopsys.com>
Cc: Giuseppe Cavallaro <peppe.cavallaro@st.com>
Cc: Alexandre Torgue <alexandre.torgue@st.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

4 files changed, 50 insertions, 2 deletions

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
index 0a80fa25afe3..d6bb953685fa 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
@@ -119,11 +119,23 @@
 #define XGMAC_MTL_TXQ_OPMODE(x)         (0x00001100 + (0x80 * (x)))
 #define XGMAC_TQS                       GENMASK(25, 16)
 #define XGMAC_TQS_SHIFT                 16
+#define XGMAC_Q2TCMAP                   GENMASK(10, 8)
+#define XGMAC_Q2TCMAP_SHIFT             8
 #define XGMAC_TTC                       GENMASK(6, 4)
 #define XGMAC_TTC_SHIFT                 4
 #define XGMAC_TXQEN                     GENMASK(3, 2)
 #define XGMAC_TXQEN_SHIFT               2
 #define XGMAC_TSF                       BIT(1)
+#define XGMAC_MTL_TCx_ETS_CONTROL(x)    (0x00001110 + (0x80 * (x)))
+#define XGMAC_MTL_TCx_QUANTUM_WEIGHT(x) (0x00001118 + (0x80 * (x)))
+#define XGMAC_MTL_TCx_SENDSLOPE(x)      (0x0000111c + (0x80 * (x)))
+#define XGMAC_MTL_TCx_HICREDIT(x)       (0x00001120 + (0x80 * (x)))
+#define XGMAC_MTL_TCx_LOCREDIT(x)       (0x00001124 + (0x80 * (x)))
+#define XGMAC_CC                        BIT(3)
+#define XGMAC_TSA                       GENMASK(1, 0)
+#define XGMAC_SP                        (0x0 << 0)
+#define XGMAC_CBS                       (0x1 << 0)
+#define XGMAC_ETS                       (0x2 << 0)
 #define XGMAC_MTL_RXQ_OPMODE(x)         (0x00001140 + (0x80 * (x)))
 #define XGMAC_RQS                       GENMASK(25, 16)
 #define XGMAC_RQS_SHIFT                 16
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
index d182f82f7b58..64b8cb88ea45 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
@@ -177,6 +177,23 @@ static void dwxgmac2_map_mtl_to_dma(struct mac_device_info *hw, u32 queue,
         writel(value, ioaddr + reg);
 }
 
+static void dwxgmac2_config_cbs(struct mac_device_info *hw,
+                                u32 send_slope, u32 idle_slope,
+                                u32 high_credit, u32 low_credit, u32 queue)
+{
+        void __iomem *ioaddr = hw->pcsr;
+        u32 value;
+
+        writel(send_slope, ioaddr + XGMAC_MTL_TCx_SENDSLOPE(queue));
+        writel(idle_slope, ioaddr + XGMAC_MTL_TCx_QUANTUM_WEIGHT(queue));
+        writel(high_credit, ioaddr + XGMAC_MTL_TCx_HICREDIT(queue));
+        writel(low_credit, ioaddr + XGMAC_MTL_TCx_LOCREDIT(queue));
+
+        value = readl(ioaddr + XGMAC_MTL_TCx_ETS_CONTROL(queue));
+        value |= XGMAC_CC | XGMAC_CBS;
+        writel(value, ioaddr + XGMAC_MTL_TCx_ETS_CONTROL(queue));
+}
+
 static int dwxgmac2_host_irq_status(struct mac_device_info *hw,
                                     struct stmmac_extra_stats *x)
 {
@@ -316,7 +333,7 @@ const struct stmmac_ops dwxgmac210_ops = {
         .prog_mtl_tx_algorithms = dwxgmac2_prog_mtl_tx_algorithms,
         .set_mtl_tx_queue_weight = NULL,
         .map_mtl_to_dma = dwxgmac2_map_mtl_to_dma,
-        .config_cbs = NULL,
+        .config_cbs = dwxgmac2_config_cbs,
         .dump_regs = NULL,
         .host_irq_status = dwxgmac2_host_irq_status,
         .host_mtl_irq_status = dwxgmac2_host_mtl_irq_status,
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c
index 20909036e002..6c5092e7771c 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c
@@ -182,6 +182,9 @@ static void dwxgmac2_dma_tx_mode(void __iomem *ioaddr, int mode,
                         value |= 0x7 << XGMAC_TTC_SHIFT;
         }
 
+        /* Use static TC to Queue mapping */
+        value |= (channel << XGMAC_Q2TCMAP_SHIFT) & XGMAC_Q2TCMAP;
+
         value &= ~XGMAC_TXQEN;
         if (qmode != MTL_QUEUE_AVB)
                 value |= 0x2 << XGMAC_TXQEN_SHIFT;
@@ -374,6 +377,21 @@ static void dwxgmac2_enable_tso(void __iomem *ioaddr, bool en, u32 chan)
         writel(value, ioaddr + XGMAC_DMA_CH_TX_CONTROL(chan));
 }
 
+static void dwxgmac2_qmode(void __iomem *ioaddr, u32 channel, u8 qmode)
+{
+        u32 value = readl(ioaddr + XGMAC_MTL_TXQ_OPMODE(channel));
+
+        value &= ~XGMAC_TXQEN;
+        if (qmode != MTL_QUEUE_AVB) {
+                value |= 0x2 << XGMAC_TXQEN_SHIFT;
+                writel(0, ioaddr + XGMAC_MTL_TCx_ETS_CONTROL(channel));
+        } else {
+                value |= 0x1 << XGMAC_TXQEN_SHIFT;
+        }
+
+        writel(value, ioaddr +  XGMAC_MTL_TXQ_OPMODE(channel));
+}
+
 static void dwxgmac2_set_bfsize(void __iomem *ioaddr, int bfsize, u32 chan)
 {
         u32 value;
@@ -407,5 +425,6 @@ const struct stmmac_dma_ops dwxgmac210_dma_ops = {
         .set_rx_tail_ptr = dwxgmac2_set_rx_tail_ptr,
         .set_tx_tail_ptr = dwxgmac2_set_tx_tail_ptr,
         .enable_tso = dwxgmac2_enable_tso,
+        .qmode = dwxgmac2_qmode,
         .set_bfsize = dwxgmac2_set_bfsize,
 };
diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.c b/drivers/net/ethernet/stmicro/stmmac/hwif.c
index 357309a6d6a5..d9a34a4d08b3 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.c
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.c
@@ -201,7 +201,7 @@ static const struct stmmac_hwif_entry {
                 .mac = &dwxgmac210_ops,
                 .hwtimestamp = &stmmac_ptp,
                 .mode = NULL,
-                .tc = NULL,
+                .tc = &dwmac510_tc_ops,
                 .setup = dwxgmac2_setup,
                 .quirks = NULL,
         },