net: stmmac: Add support for CBS QDISC

This adds support for CBS reconfiguration using the TC application.

A new callback was added to TC ops struct and another one to DMA ops to
reconfigure the channel mode.

Tested in GMAC5.10.

Signed-off-by: Jose Abreu <joabreu@synopsys.com>
Cc: David S. Miller <davem@davemloft.net>
Cc: Joao Pinto <jpinto@synopsys.com>
Cc: Vitor Soares <soares@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/dwmac4_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c
index d37f17ca62fecf66a6b5af1c9aa105923310a341..6e32f8a3710be1767d9b0e493230cb022b08956a 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c
@@ -407,6 +407,19 @@ static void dwmac4_enable_tso(void __iomem *ioaddr, bool en, u32 chan)
        }
 }
 
+static void dwmac4_qmode(void __iomem *ioaddr, u32 channel, u8 qmode)
+{
+       u32 mtl_tx_op = readl(ioaddr + MTL_CHAN_TX_OP_MODE(channel));
+
+       mtl_tx_op &= ~MTL_OP_MODE_TXQEN_MASK;
+       if (qmode != MTL_QUEUE_AVB)
+               mtl_tx_op |= MTL_OP_MODE_TXQEN;
+       else
+               mtl_tx_op |= MTL_OP_MODE_TXQEN_AV;
+
+       writel(mtl_tx_op, ioaddr +  MTL_CHAN_TX_OP_MODE(channel));
+}
+
 const struct stmmac_dma_ops dwmac4_dma_ops = {
        .reset = dwmac4_dma_reset,
        .init = dwmac4_dma_init,
@@ -431,6 +444,7 @@ const struct stmmac_dma_ops dwmac4_dma_ops = {
        .set_rx_tail_ptr = dwmac4_set_rx_tail_ptr,
        .set_tx_tail_ptr = dwmac4_set_tx_tail_ptr,
        .enable_tso = dwmac4_enable_tso,
+       .qmode = dwmac4_qmode,
 };
 
 const struct stmmac_dma_ops dwmac410_dma_ops = {
@@ -457,4 +471,5 @@ const struct stmmac_dma_ops dwmac410_dma_ops = {
        .set_rx_tail_ptr = dwmac4_set_rx_tail_ptr,
        .set_tx_tail_ptr = dwmac4_set_tx_tail_ptr,
        .enable_tso = dwmac4_enable_tso,
+       .qmode = dwmac4_qmode,
 };

diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index e44e7b26ce829be0eff000c6a68b064139d532b8..e2a965790648c574b749a86c363347fcd8711bd1 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -183,6 +183,7 @@ struct stmmac_dma_ops {
        void (*set_rx_tail_ptr)(void __iomem *ioaddr, u32 tail_ptr, u32 chan);
        void (*set_tx_tail_ptr)(void __iomem *ioaddr, u32 tail_ptr, u32 chan);
        void (*enable_tso)(void __iomem *ioaddr, bool en, u32 chan);
+       void (*qmode)(void __iomem *ioaddr, u32 channel, u8 qmode);
 };
 
 #define stmmac_reset(__priv, __args...) \
@@ -235,6 +236,8 @@ struct stmmac_dma_ops {
        stmmac_do_void_callback(__priv, dma, set_tx_tail_ptr, __args)
 #define stmmac_enable_tso(__priv, __args...) \
        stmmac_do_void_callback(__priv, dma, enable_tso, __args)
+#define stmmac_dma_qmode(__priv, __args...) \
+       stmmac_do_void_callback(__priv, dma, qmode, __args)
 
 struct mac_device_info;
 struct net_device;
@@ -441,17 +444,22 @@ struct stmmac_mode_ops {
 
 struct stmmac_priv;
 struct tc_cls_u32_offload;
+struct tc_cbs_qopt_offload;
 
 struct stmmac_tc_ops {
        int (*init)(struct stmmac_priv *priv);
        int (*setup_cls_u32)(struct stmmac_priv *priv,
                             struct tc_cls_u32_offload *cls);
+       int (*setup_cbs)(struct stmmac_priv *priv,
+                        struct tc_cbs_qopt_offload *qopt);
 };
 
 #define stmmac_tc_init(__priv, __args...) \
        stmmac_do_callback(__priv, tc, init, __args)
 #define stmmac_tc_setup_cls_u32(__priv, __args...) \
        stmmac_do_callback(__priv, tc, setup_cls_u32, __args)
+#define stmmac_tc_setup_cbs(__priv, __args...) \
+       stmmac_do_callback(__priv, tc, setup_cbs, __args)
 
 struct stmmac_regs_off {
        u32 ptp_off;

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 2354e30caa78cfc6d2c711c3819d45408da14840..93a3bea8576e159ce73b56be1f43482330a603da 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -3793,6 +3793,8 @@ static int stmmac_setup_tc(struct net_device *ndev, enum tc_setup_type type,
        switch (type) {
        case TC_SETUP_BLOCK:
                return stmmac_setup_tc_block(priv, type_data);
+       case TC_SETUP_QDISC_CBS:
+               return stmmac_tc_setup_cbs(priv, priv, type_data);
        default:
                return -EOPNOTSUPP;
        }

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
index 2258cd8cc84413f22c19a9339a3b6193427c464e..0b0fca0200b201004d7428c26df430fa5b24c74d 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
@@ -289,7 +289,69 @@ static int tc_init(struct stmmac_priv *priv)
        return 0;
 }
 
+static int tc_setup_cbs(struct stmmac_priv *priv,
+                       struct tc_cbs_qopt_offload *qopt)
+{
+       u32 tx_queues_count = priv->plat->tx_queues_to_use;
+       u32 queue = qopt->queue;
+       u32 ptr, speed_div;
+       u32 mode_to_use;
+       u64 value;
+       int ret;
+
+       /* Queue 0 is not AVB capable */
+       if (queue <= 0 || queue >= tx_queues_count)
+               return -EINVAL;
+       if (priv->speed != SPEED_100 && priv->speed != SPEED_1000)
+               return -EOPNOTSUPP;
+
+       mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
+       if (mode_to_use == MTL_QUEUE_DCB && qopt->enable) {
+               ret = stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_AVB);
+               if (ret)
+                       return ret;
+
+               priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB;
+       } else if (!qopt->enable) {
+               return stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_DCB);
+       }
+
+       /* Port Transmit Rate and Speed Divider */
+       ptr = (priv->speed == SPEED_100) ? 4 : 8;
+       speed_div = (priv->speed == SPEED_100) ? 100000 : 1000000;
+
+       /* Final adjustments for HW */
+       value = qopt->idleslope * 1024 * ptr;
+       do_div(value, speed_div);
+       priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0);
+
+       value = -qopt->sendslope * 1024UL * ptr;
+       do_div(value, speed_div);
+       priv->plat->tx_queues_cfg[queue].send_slope = value & GENMASK(31, 0);
+
+       value = qopt->hicredit * 1024 * 8;
+       priv->plat->tx_queues_cfg[queue].high_credit = value & GENMASK(31, 0);
+
+       value = qopt->locredit * 1024 * 8;
+       priv->plat->tx_queues_cfg[queue].low_credit = value & GENMASK(31, 0);
+
+       ret = stmmac_config_cbs(priv, priv->hw,
+                               priv->plat->tx_queues_cfg[queue].send_slope,
+                               priv->plat->tx_queues_cfg[queue].idle_slope,
+                               priv->plat->tx_queues_cfg[queue].high_credit,
+                               priv->plat->tx_queues_cfg[queue].low_credit,
+                               queue);
+       if (ret)
+               return ret;
+
+       dev_info(priv->device, "CBS queue %d: send %d, idle %d, hi %d, lo %d\n",
+                       queue, qopt->sendslope, qopt->idleslope,
+                       qopt->hicredit, qopt->locredit);
+       return 0;
+}
+
 const struct stmmac_tc_ops dwmac510_tc_ops = {
        .init = tc_init,
        .setup_cls_u32 = tc_setup_cls_u32,
+       .setup_cbs = tc_setup_cbs,
 };