[PATCH net-next v2 1/3] net/stmmac/dwxgmac: Modify DMA functions for future hardware

[Jakub Raczynski <j.raczynski@samsung.com>]

Datasheet for newer hardware of XGMAC (3.20a and 3.40a) list support for
up to 16 DMA/MTL queues. Currently maximum amount of queues in 8 set by
STMMAC_CH_MAX, MTL_MAX_TX_QUEUES, MTL_MAX_RX_QUEUES macros.
But before we set these to higher value there are changes to be made.

While newer hardware supports up to 16 MTL/DMA queues, there is support only
for 8 TC's (traffic control) entries. Current source assumes these are equal,
which might not be true. While in some cases it would be just incorrect value,
there are possible wrong memory accesses.

Fix this by saving number of TC supported by hardware in mac_device_info and
verify it in related functions. Also use TC count rather than MTL_MAX values
where it applies.

Co-developed-by: Chang-Sub Lee <cs0617.lee@samsung.com>
Signed-off-by: Chang-Sub Lee <cs0617.lee@samsung.com>
Signed-off-by: Jakub Raczynski <j.raczynski@samsung.com>
---
 drivers/net/ethernet/stmicro/stmmac/common.h  |  1 +
 .../ethernet/stmicro/stmmac/dwxgmac2_core.c   | 46 ++++++++++++++++++-
 .../ethernet/stmicro/stmmac/dwxgmac2_dma.c    | 15 +++++-
 drivers/net/ethernet/stmicro/stmmac/hwif.c    |  3 ++
 .../net/ethernet/stmicro/stmmac/stmmac_main.c |  2 +-
 5 files changed, 63 insertions(+), 4 deletions(-)

diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index 927ea6230073..359a76d3658c 100644
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -620,6 +620,7 @@ struct mac_device_info {
 	const struct stmmac_mmc_ops *mmc;
 	const struct stmmac_est_ops *est;
 	const struct stmmac_vlan_ops *vlan;
+	struct stmmac_priv *priv_data;
 	struct dw_xpcs *xpcs;
 	struct phylink_pcs *phylink_pcs;
 	struct mii_regs mii;	/* MII register Addresses */
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
index f02b434bbd50..dbedf31bb2ab 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
@@ -97,11 +97,19 @@ static void dwxgmac2_rx_queue_enable(struct mac_device_info *hw, u8 mode,
 static void dwxgmac2_rx_queue_prio(struct mac_device_info *hw, u32 prio,
 				   u32 queue)
 {
+	const unsigned int numtc = hw->priv_data->dma_cap.numtc;
+	struct net_device *ndev = hw->priv_data->dev;
 	void __iomem *ioaddr = hw->pcsr;
 	u32 clear_mask = 0;
 	u32 ctrl2, ctrl3;
 	int i;
 
+	if (queue >= numtc) {
+		netdev_err(ndev, "%s: invalid TC queue %d, supported %d\n",
+			   __func__, queue, numtc);
+		return;
+	}
+
 	ctrl2 = readl(ioaddr + XGMAC_RXQ_CTRL2);
 	ctrl3 = readl(ioaddr + XGMAC_RXQ_CTRL3);
 
@@ -138,9 +146,17 @@ static void dwxgmac2_rx_queue_prio(struct mac_device_info *hw, u32 prio,
 static void dwxgmac2_tx_queue_prio(struct mac_device_info *hw, u32 prio,
 				   u32 queue)
 {
+	const unsigned int numtc = hw->priv_data->dma_cap.numtc;
+	struct net_device *ndev = hw->priv_data->dev;
 	void __iomem *ioaddr = hw->pcsr;
 	u32 value, reg;
 
+	if (queue >= numtc) {
+		netdev_err(ndev, "%s: invalid TC queue %d, supported %d\n",
+			   __func__, queue, numtc);
+		return;
+	}
+
 	reg = (queue < 4) ? XGMAC_TC_PRTY_MAP0 : XGMAC_TC_PRTY_MAP1;
 	if (queue >= 4)
 		queue -= 4;
@@ -207,6 +223,7 @@ static void dwxgmac2_prog_mtl_rx_algorithms(struct mac_device_info *hw,
 static void dwxgmac2_prog_mtl_tx_algorithms(struct mac_device_info *hw,
 					    u32 tx_alg)
 {
+	const unsigned int numtc = hw->priv_data->dma_cap.numtc;
 	void __iomem *ioaddr = hw->pcsr;
 	bool ets = true;
 	u32 value;
@@ -233,7 +250,7 @@ static void dwxgmac2_prog_mtl_tx_algorithms(struct mac_device_info *hw,
 	writel(value, ioaddr + XGMAC_MTL_OPMODE);
 
 	/* Set ETS if desired */
-	for (i = 0; i < MTL_MAX_TX_QUEUES; i++) {
+	for (i = 0; i < numtc; i++) {
 		value = readl(ioaddr + XGMAC_MTL_TCx_ETS_CONTROL(i));
 		value &= ~XGMAC_TSA;
 		if (ets)
@@ -246,8 +263,15 @@ static void dwxgmac2_set_mtl_tx_queue_weight(struct stmmac_priv *priv,
 					     struct mac_device_info *hw,
 					     u32 weight, u32 queue)
 {
+	const unsigned int numtc = priv->dma_cap.numtc;
 	void __iomem *ioaddr = hw->pcsr;
 
+	if (queue >= numtc) {
+		netdev_err(priv->dev, "%s: invalid TC queue %d, supported %d\n",
+			   __func__, queue, numtc);
+		return;
+	}
+
 	writel(weight, ioaddr + XGMAC_MTL_TCx_QUANTUM_WEIGHT(queue));
 }
 
@@ -273,9 +297,16 @@ static void dwxgmac2_config_cbs(struct stmmac_priv *priv,
 				u32 send_slope, u32 idle_slope,
 				u32 high_credit, u32 low_credit, u32 queue)
 {
+	const unsigned int numtc = priv->dma_cap.numtc;
 	void __iomem *ioaddr = hw->pcsr;
 	u32 value;
 
+	if (queue >= numtc) {
+		netdev_err(priv->dev, "%s: invalid TC queue %d, supported %d\n",
+			   __func__, queue, numtc);
+		return;
+	}
+
 	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));
@@ -357,6 +388,8 @@ static void dwxgmac2_flow_ctrl(struct mac_device_info *hw, unsigned int duplex,
 			       unsigned int fc, unsigned int pause_time,
 			       u8 tx_cnt)
 {
+	const unsigned int numtc = hw->priv_data->dma_cap.numtc;
+	struct net_device *ndev = hw->priv_data->dev;
 	void __iomem *ioaddr = hw->pcsr;
 	u8 i;
 
@@ -366,6 +399,17 @@ static void dwxgmac2_flow_ctrl(struct mac_device_info *hw, unsigned int duplex,
 		for (i = 0; i < tx_cnt; i++) {
 			u32 value = XGMAC_TFE;
 
+			if (i >= numtc) {
+				netdev_err(ndev,
+					   "%s: invalid TC queue %d, supported %d\n",
+					   __func__, i, numtc);
+				/* This will skip all other wrong channels,
+				 * but triggering this is preceded by warnings
+				 * from other functions, so limit the spam.
+				 */
+				break;
+			}
+
 			if (duplex)
 				value |= FIELD_PREP(XGMAC_PT, pause_time);
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c
index 03437f1cf3df..f70eaa181dad 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c
@@ -204,8 +204,19 @@ static void dwxgmac2_dma_tx_mode(struct stmmac_priv *priv, void __iomem *ioaddr,
 		value = u32_replace_bits(value, ttc, XGMAC_TTC);
 	}
 
-	/* Use static TC to Queue mapping */
-	value |= FIELD_PREP(XGMAC_Q2TCMAP, channel);
+	/* Newer XGMAC hardware does support up to 16 MTL/DMA queues but
+	 * only 8 traffic class queues. Redirect these, but this is error in
+	 * configuration.
+	 */
+	if (channel >= priv->dma_cap.numtc) {
+		netdev_err(priv->dev,
+			   "%s: Wrong channel set for TX mode redirecting to TC 0\n",
+			   __func__);
+		value |= FIELD_PREP(XGMAC_Q2TCMAP, 0);
+	} else {
+		/* Use static TC to Queue mapping */
+		value |= FIELD_PREP(XGMAC_Q2TCMAP, channel);
+	}
 
 	if (qmode != MTL_QUEUE_AVB)
 		txqen = 0x2;
diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.c b/drivers/net/ethernet/stmicro/stmmac/hwif.c
index 511b0fd5e834..52842366ae0c 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.c
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.c
@@ -368,6 +368,9 @@ int stmmac_hwif_init(struct stmmac_priv *priv)
 		return -EINVAL;
 	}
 
+	/* Assign priv struct internally for easier processing */
+	mac->priv_data = priv;
+
 	/* Only use generic HW helpers if needed */
 	mac->desc = mac->desc ? : entry->desc;
 	mac->dma = mac->dma ? : entry->dma;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 3591755ea30b..5917bf47c7de 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -4140,7 +4140,7 @@ static int __stmmac_open(struct net_device *dev,
 	u8 chan;
 	int ret;
 
-	for (int i = 0; i < MTL_MAX_TX_QUEUES; i++)
+	for (int i = 0; i < priv->plat->tx_queues_to_use; i++)
 		if (priv->dma_conf.tx_queue[i].tbs & STMMAC_TBS_EN)
 			dma_conf->tx_queue[i].tbs = priv->dma_conf.tx_queue[i].tbs;
 	memcpy(&priv->dma_conf, dma_conf, sizeof(*dma_conf));
-- 
2.34.1