From bbfc52d9e15334a0e72884754be04b8df335de39 Mon Sep 17 00:00:00 2001
From: Evelyn Tsai <evelyn.tsai@mediatek.com>
Date: Fri, 6 May 2022 15:53:07 +0800
Subject: [PATCH 1114/1116] mt76: mt7915: drop packetes when TWT stations use
 more token then 128

---
 mt7915/mac.c    | 21 ++++++++++++++++++---
 mt7915/mt7915.h |  2 ++
 2 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/mt7915/mac.c b/mt7915/mac.c
index b899697c..1935acff 100644
--- a/mt7915/mac.c
+++ b/mt7915/mac.c
@@ -1303,6 +1303,7 @@ int mt7915_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_info->skb);
 	struct ieee80211_key_conf *key = info->control.hw_key;
 	struct ieee80211_vif *vif = info->control.vif;
+	struct mt7915_sta *msta = NULL;
 	struct mt76_txwi_cache *t;
 	struct mt7915_txp *txp;
 	int id, i, nbuf = tx_info->nbuf - 1;
@@ -1316,8 +1317,6 @@ int mt7915_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 		wcid = &dev->mt76.global_wcid;
 
 	if (sta) {
-		struct mt7915_sta *msta;
-
 		msta = (struct mt7915_sta *)sta->drv_priv;
 
 		if (time_after(jiffies, msta->jiffies + HZ / 4)) {
@@ -1329,10 +1328,22 @@ 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;
 
+	spin_lock_bh(&mdev->token_lock);
+	if (msta && msta->twt.flowid_mask && msta->token_count > 128) {
+		spin_unlock_bh(&mdev->token_lock);
+		return -1;
+	}
+	spin_unlock_bh(&mdev->token_lock);
+
 	id = mt76_token_consume(mdev, &t);
 	if (id < 0)
 		return id;
 
+	spin_lock_bh(&mdev->token_lock);
+	if (msta)
+		msta->token_count++;
+	spin_unlock_bh(&mdev->token_lock);
+
 	pid = mt76_tx_status_skb_add(mdev, wcid, tx_info->skb);
 	mt7915_mac_write_txwi(dev, txwi_ptr, tx_info->skb, wcid, pid, key,
 			      false);
@@ -1524,6 +1535,7 @@ mt7915_mac_tx_free(struct mt7915_dev *dev, void *data, int len)
 	struct mt76_dev *mdev = &dev->mt76;
 	struct mt76_txwi_cache *txwi;
 	struct ieee80211_sta *sta = NULL;
+	struct mt7915_sta *msta;
 	LIST_HEAD(free_list);
 	void *end = data + len;
 	bool v3, wake = false;
@@ -1547,7 +1559,6 @@ mt7915_mac_tx_free(struct mt7915_dev *dev, void *data, int len)
 		 * 1'b0: msdu_id with the same 'wcid pair' as above.
 		 */
 		if (info & MT_TX_FREE_PAIR) {
-			struct mt7915_sta *msta;
 			struct mt76_wcid *wcid;
 			u16 idx;
 
@@ -1583,6 +1594,10 @@ mt7915_mac_tx_free(struct mt7915_dev *dev, void *data, int len)
 			txwi = mt76_token_release(mdev, msdu, &wake);
 			if (!txwi)
 				continue;
+			spin_lock(&mdev->token_lock);
+			if (msta)
+				msta->token_count--;
+			spin_unlock(&mdev->token_lock);
 
 			mt7915_txwi_free(dev, txwi, sta, &free_list);
 		}
diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index 24276da5..0feccb17 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -136,6 +136,8 @@ struct mt7915_sta {
 		u8 flowid_mask;
 		struct mt7915_twt_flow flow[MT7915_MAX_STA_TWT_AGRT];
 	} twt;
+
+	int token_count;
 };
 
 struct mt7915_vif_cap {
-- 
2.18.0