net: stmmac: Add support for CBS QDISC

[thirdparty/kernel/stable.git] / drivers / net / ethernet / stmicro / stmmac / dwmac4_dma.c

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,
 };