From 6c1136f7cdafe9cd4a6517afa60227f5ffa86b0a Mon Sep 17 00:00:00 2001
From: Peter Chiu <chui-hao.chiu@mediatek.com>
Date: Mon, 29 Jan 2024 15:33:24 +0800
Subject: [PATCH 13/14] wifi: mt76: mt7915: limit per-band token count

Add a threshold for per-band token count to prevent a band from interfering
with the other band.

Signed-off-by: Peter Chiu <chui-hao.chiu@mediatek.com>
---
 mt76.h           |  8 +++++++-
 mt7915/init.c    |  3 +++
 mt7915/mac.c     |  3 ++-
 mt7921/pci_mac.c |  2 +-
 mt7925/pci_mac.c |  2 +-
 mt7996/init.c    |  5 +++++
 mt7996/mac.c     |  3 ++-
 tx.c             | 20 ++++++++++++++++++--
 8 files changed, 39 insertions(+), 7 deletions(-)

diff --git a/mt76.h b/mt76.h
index ee14425..b83456b 100644
--- a/mt76.h
+++ b/mt76.h
@@ -407,6 +407,8 @@ struct mt76_txwi_cache {
 	struct list_head list;
 	dma_addr_t dma_addr;
 
+	u8 phy_idx;
+
 	union {
 		struct sk_buff *skb;
 		void *ptr;
@@ -819,6 +821,7 @@ struct mt76_phy {
 		bool al;
 		u8 pin;
 	} leds;
+	int tokens;
 };
 
 struct mt76_dev {
@@ -872,6 +875,8 @@ struct mt76_dev {
 	u16 wed_token_count;
 	u16 token_count;
 	u16 token_size;
+	u16 token_threshold;
+	u8 num_phy;
 
 	spinlock_t rx_token_lock;
 	struct idr rx_token;
@@ -1662,7 +1667,8 @@ static inline bool mt76_queue_is_wed_rx(struct mt76_queue *q)
 
 struct mt76_txwi_cache *
 mt76_token_release(struct mt76_dev *dev, int token, bool *wake);
-int mt76_token_consume(struct mt76_dev *dev, struct mt76_txwi_cache **ptxwi);
+int mt76_token_consume(struct mt76_dev *dev, struct mt76_txwi_cache **ptxwi,
+		       u8 phy_idx);
 void __mt76_set_tx_blocked(struct mt76_dev *dev, bool blocked);
 struct mt76_txwi_cache *mt76_rx_token_release(struct mt76_dev *dev, int token);
 int mt76_rx_token_consume(struct mt76_dev *dev, void *ptr,
diff --git a/mt7915/init.c b/mt7915/init.c
index f81a2f2..3ec9eab 100644
--- a/mt7915/init.c
+++ b/mt7915/init.c
@@ -1225,6 +1225,8 @@ int mt7915_register_device(struct mt7915_dev *dev)
 
 	dev->dbdc_support = mt7915_band_config(dev);
 
+	dev->mt76.num_phy = 1 + !!dev->dbdc_support;
+
 	phy2 = mt7915_alloc_ext_phy(dev);
 	if (IS_ERR(phy2))
 		return PTR_ERR(phy2);
@@ -1257,6 +1259,7 @@ int mt7915_register_device(struct mt7915_dev *dev)
 	}
 
 	dev->recovery.hw_init_done = true;
+	dev->mt76.token_threshold = dev->mt76.token_size / dev->mt76.num_phy;
 
 	ret = mt7915_init_debugfs(&dev->phy);
 	if (ret)
diff --git a/mt7915/mac.c b/mt7915/mac.c
index a5d0b09..4604a68 100644
--- a/mt7915/mac.c
+++ b/mt7915/mac.c
@@ -738,6 +738,7 @@ int mt7915_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 	struct mt76_connac_fw_txp *txp;
 	struct mt76_txwi_cache *t;
 	int id, i, nbuf = tx_info->nbuf - 1;
+	u8 phy_idx = (info->hw_queue & MT_TX_HW_QUEUE_PHY) >> 2;
 	u8 *txwi = (u8 *)txwi_ptr;
 	int pid;
 
@@ -761,7 +762,7 @@ int mt7915_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 	t = (struct mt76_txwi_cache *)(txwi + mdev->drv->txwi_size);
 	t->skb = tx_info->skb;
 
-	id = mt76_token_consume(mdev, &t);
+	id = mt76_token_consume(mdev, &t, phy_idx);
 	if (id < 0)
 		return id;
 
diff --git a/mt7921/pci_mac.c b/mt7921/pci_mac.c
index 031ba9a..4c69c55 100644
--- a/mt7921/pci_mac.c
+++ b/mt7921/pci_mac.c
@@ -27,7 +27,7 @@ int mt7921e_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 	t = (struct mt76_txwi_cache *)(txwi + mdev->drv->txwi_size);
 	t->skb = tx_info->skb;
 
-	id = mt76_token_consume(mdev, &t);
+	id = mt76_token_consume(mdev, &t, 0);
 	if (id < 0)
 		return id;
 
diff --git a/mt7925/pci_mac.c b/mt7925/pci_mac.c
index 9fca887..f1d615c 100644
--- a/mt7925/pci_mac.c
+++ b/mt7925/pci_mac.c
@@ -27,7 +27,7 @@ int mt7925e_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 	t = (struct mt76_txwi_cache *)(txwi + mdev->drv->txwi_size);
 	t->skb = tx_info->skb;
 
-	id = mt76_token_consume(mdev, &t);
+	id = mt76_token_consume(mdev, &t, 0);
 	if (id < 0)
 		return id;
 
diff --git a/mt7996/init.c b/mt7996/init.c
index 9aa97e4..7549a10 100644
--- a/mt7996/init.c
+++ b/mt7996/init.c
@@ -634,6 +634,8 @@ static int mt7996_register_phy(struct mt7996_dev *dev, struct mt7996_phy *phy,
 		mtk_wed_device_start(&dev->mt76.mmio.wed_hif2, irq_mask);
 	}
 
+	dev->mt76.num_phy++;
+
 	return 0;
 
 error:
@@ -1330,6 +1332,8 @@ int mt7996_register_device(struct mt7996_dev *dev)
 	if (ret)
 		return ret;
 
+	dev->mt76.num_phy = 1;
+
 	ret = mt7996_register_phy(dev, mt7996_phy2(dev), MT_BAND1);
 	if (ret)
 		return ret;
@@ -1342,6 +1346,7 @@ int mt7996_register_device(struct mt7996_dev *dev)
 
 	dev->recovery.hw_init_done = true;
 
+	dev->mt76.token_threshold = dev->mt76.token_size / dev->mt76.num_phy;
 	ret = mt7996_init_debugfs(&dev->phy);
 	if (ret)
 		goto error;
diff --git a/mt7996/mac.c b/mt7996/mac.c
index bc7111a..aa19120 100644
--- a/mt7996/mac.c
+++ b/mt7996/mac.c
@@ -922,6 +922,7 @@ int mt7996_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 	struct mt76_txwi_cache *t;
 	int id, i, pid, nbuf = tx_info->nbuf - 1;
 	bool is_8023 = info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP;
+	u8 phy_idx = (info->hw_queue & MT_TX_HW_QUEUE_PHY) >> 2;
 	u8 *txwi = (u8 *)txwi_ptr;
 
 	if (unlikely(tx_info->skb->len <= ETH_HLEN))
@@ -933,7 +934,7 @@ int mt7996_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 	t = (struct mt76_txwi_cache *)(txwi + mdev->drv->txwi_size);
 	t->skb = tx_info->skb;
 
-	id = mt76_token_consume(mdev, &t);
+	id = mt76_token_consume(mdev, &t, phy_idx);
 	if (id < 0)
 		return id;
 
diff --git a/tx.c b/tx.c
index ab42f69..0fdf7d8 100644
--- a/tx.c
+++ b/tx.c
@@ -825,16 +825,30 @@ void __mt76_set_tx_blocked(struct mt76_dev *dev, bool blocked)
 }
 EXPORT_SYMBOL_GPL(__mt76_set_tx_blocked);
 
-int mt76_token_consume(struct mt76_dev *dev, struct mt76_txwi_cache **ptxwi)
+int mt76_token_consume(struct mt76_dev *dev, struct mt76_txwi_cache **ptxwi,
+		       u8 phy_idx)
 {
+	struct mt76_phy *phy = phy_idx < __MT_MAX_BAND ? dev->phys[phy_idx] : NULL;
 	int token;
 
 	spin_lock_bh(&dev->token_lock);
 
+	if (dev->num_phy > 1 && phy && phy->tokens > dev->token_threshold) {
+		spin_unlock_bh(&dev->token_lock);
+
+		return -EINVAL;
+	}
+
 	token = idr_alloc(&dev->token, *ptxwi, 0, dev->token_size, GFP_ATOMIC);
-	if (token >= 0)
+	if (token >= 0) {
 		dev->token_count++;
 
+		if (dev->num_phy > 1 && phy) {
+			(*ptxwi)->phy_idx = phy_idx;
+			phy->tokens++;
+		}
+	}
+
 #ifdef CONFIG_NET_MEDIATEK_SOC_WED
 	if (mtk_wed_device_active(&dev->mmio.wed) &&
 	    token >= dev->mmio.wed.wlan.token_start)
@@ -878,6 +892,8 @@ mt76_token_release(struct mt76_dev *dev, int token, bool *wake)
 	txwi = idr_remove(&dev->token, token);
 	if (txwi) {
 		dev->token_count--;
+		if (dev->num_phy > 1 && dev->phys[txwi->phy_idx])
+			dev->phys[txwi->phy_idx]->tokens--;
 
 #ifdef CONFIG_NET_MEDIATEK_SOC_WED
 		if (mtk_wed_device_active(&dev->mmio.wed) &&
-- 
2.18.0