[PATCH net-next 04/26] net: stmmac: xgmac: Implement set_mtl_tx_queue_weight()

[Jose Abreu]

Implement the TX Queue Weight callback. In order for this to be active
we also need to set ETS algorithm when configuring Queue.

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
---
 .../net/ethernet/stmicro/stmmac/dwxgmac2_core.c    | 22 +++++++++++++++++++++-
 1 file changed, 21 insertions(+), 1 deletion(-)

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c 
b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
index 85c68b7ee8c6..ce6503dfc86d 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
@@ -144,7 +144,9 @@ static void dwxgmac2_prog_mtl_tx_algorithms(struct 
mac_device_info *hw,
                                            u32 tx_alg)
 {
        void __iomem *ioaddr = hw->pcsr;
+       bool ets = true;
        u32 value;
+       int i;
 
        value = readl(ioaddr + XGMAC_MTL_OPMODE);
        value &= ~XGMAC_ETSALG;
@@ -160,10 +162,28 @@ static void dwxgmac2_prog_mtl_tx_algorithms(struct 
mac_device_info *hw,
                value |= XGMAC_DWRR;
                break;
        default:
+               ets = false;
                break;
        }
 
        writel(value, ioaddr + XGMAC_MTL_OPMODE);
+
+       /* Set ETS if desired */
+       for (i = 0; i < MTL_MAX_TX_QUEUES; i++) {
+               value = readl(ioaddr + XGMAC_MTL_TCx_ETS_CONTROL(i));
+               value &= ~XGMAC_TSA;
+               if (ets)
+                       value |= XGMAC_ETS;
+               writel(value, ioaddr + XGMAC_MTL_TCx_ETS_CONTROL(i));
+       }
+}
+
+static void dwxgmac2_set_mtl_tx_queue_weight(struct mac_device_info *hw,
+                                            u32 weight, u32 queue)
+{
+       void __iomem *ioaddr = hw->pcsr;
+
+       writel(weight, ioaddr + XGMAC_MTL_TCx_QUANTUM_WEIGHT(queue));
 }
 
 static void dwxgmac2_map_mtl_to_dma(struct mac_device_info *hw, u32 queue,
@@ -412,7 +432,7 @@ const struct stmmac_ops dwxgmac210_ops = {
        .rx_queue_routing = NULL,
        .prog_mtl_rx_algorithms = dwxgmac2_prog_mtl_rx_algorithms,
        .prog_mtl_tx_algorithms = dwxgmac2_prog_mtl_tx_algorithms,
-       .set_mtl_tx_queue_weight = NULL,
+       .set_mtl_tx_queue_weight = dwxgmac2_set_mtl_tx_queue_weight,
        .map_mtl_to_dma = dwxgmac2_map_mtl_to_dma,
        .config_cbs = dwxgmac2_config_cbs,
        .dump_regs = NULL,
-- 
2.7.4