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>

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
index 0a80fa2..d6bb953 100644 (file)
--- 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 d182f82..64b8cb8 100644 (file)
--- 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 2090903..6c5092e 100644 (file)
--- 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 357309a..d9a34a4 100644 (file)
--- 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,
        },