From 33e8b84542a2d58af377c6e74eaeefbd7f29146c Mon Sep 17 00:00:00 2001
From: Sujuan Chen <sujuan.chen@mediatek.com>
Date: Sat, 10 Sep 2022 17:09:21 +0800
Subject: [PATCH 3002/3007] add-wed-tx-wds-support-on-mt7986

Signed-off-by: Sujuan Chen <sujuan.chen@mediatek.com>
---
 mac80211.c      |  5 ++++-
 mt76.h          |  2 ++
 mt7915/init.c   |  9 +++++++++
 mt7915/main.c   | 52 +++++++++++++++++++++++++++++++++++++++++++------
 mt7915/mcu.c    | 12 ++++++++++--
 mt7915/mcu.h    |  1 +
 mt7915/mmio.c   |  3 +++
 mt7915/mt7915.h |  4 ++++
 8 files changed, 79 insertions(+), 9 deletions(-)

diff --git a/mac80211.c b/mac80211.c
index 4eaf317b..c477d625 100644
--- a/mac80211.c
+++ b/mac80211.c
@@ -1360,7 +1360,10 @@ void __mt76_sta_remove(struct mt76_dev *dev, struct ieee80211_vif *vif,
 
 	mt76_packet_id_flush(dev, wcid);
 
-	mt76_wcid_mask_clear(dev->wcid_mask, idx);
+	if (dev->drv->wed_wds_check(dev, sta))
+		mt76_wcid_mask_clear(dev->wcid_wds_mask, idx);
+	else
+		mt76_wcid_mask_clear(dev->wcid_mask, idx);
 	mt76_wcid_mask_clear(dev->wcid_phy_mask, idx);
 }
 EXPORT_SYMBOL_GPL(__mt76_sta_remove);
diff --git a/mt76.h b/mt76.h
index e997908a..ec9bd59d 100644
--- a/mt76.h
+++ b/mt76.h
@@ -453,6 +453,7 @@ struct mt76_driver_ops {
 
 	void (*sta_remove)(struct mt76_dev *dev, struct ieee80211_vif *vif,
 			   struct ieee80211_sta *sta);
+	bool (*wed_wds_check)(struct mt76_dev *dev, struct ieee80211_sta *sta);
 };
 
 struct mt76_channel_state {
@@ -826,6 +827,7 @@ struct mt76_dev {
 	spinlock_t status_lock;
 
 	u32 wcid_mask[DIV_ROUND_UP(MT76_N_WCIDS, 32)];
+	u32 wcid_wds_mask[DIV_ROUND_UP(MT76_N_WCIDS, 32)];
 	u32 wcid_phy_mask[DIV_ROUND_UP(MT76_N_WCIDS, 32)];
 
 	u64 vif_mask;
diff --git a/mt7915/init.c b/mt7915/init.c
index 538ff5c3..cd9d846d 100644
--- a/mt7915/init.c
+++ b/mt7915/init.c
@@ -695,6 +695,15 @@ mt7915_init_hardware(struct mt7915_dev *dev, struct mt7915_phy *phy2)
 			return ret;
 	}
 
+	/* wds workaround for mt7986 */
+	if (mtk_wed_device_active(&dev->mt76.mmio.wed) && is_mt7986(&dev->mt76)) {
+		for(idx = MT7915_WTBL_WDS_START; idx < MT7915_WTBL_WDS_END; idx++)
+			mt76_wcid_mask_set(dev->mt76.wcid_mask, idx);
+
+		for (idx = 0; idx < DIV_ROUND_UP(MT7915_WTBL_STA, 32); idx++)
+			dev->mt76.wcid_wds_mask[idx] = ~dev->mt76.wcid_mask[idx];
+	}
+
 	/* Beacon and mgmt frames should occupy wcid 0 */
 	idx = mt76_wcid_alloc(dev->mt76.wcid_mask, MT7915_WTBL_STA);
 	if (idx)
diff --git a/mt7915/main.c b/mt7915/main.c
index 3a09f3f5..b1fb3ddf 100644
--- a/mt7915/main.c
+++ b/mt7915/main.c
@@ -658,6 +658,24 @@ mt7915_channel_switch_beacon(struct ieee80211_hw *hw,
 	mutex_unlock(&dev->mt76.mutex);
 }
 
+bool
+mt7915_wed_wds_check(struct mt76_dev *mdev, struct ieee80211_sta *sta)
+{
+	struct mt7915_sta *msta = (struct mt7915_sta *)sta->drv_priv;
+
+	if (!mtk_wed_device_active(&mdev->mmio.wed))
+		return false;
+
+	if(!is_mt7986(mdev))
+		return false;
+
+	if((msta->wcid.idx < MT7915_WTBL_WDS_START ||
+	     msta->wcid.idx > MT7915_WTBL_WDS_END))
+	     return false;
+
+	return true;
+}
+
 int mt7915_mac_sta_add(struct mt76_dev *mdev, struct ieee80211_vif *vif,
 		       struct ieee80211_sta *sta)
 {
@@ -670,8 +688,18 @@ int mt7915_mac_sta_add(struct mt76_dev *mdev, struct ieee80211_vif *vif,
 #endif
 	int ret, idx;
 	u32 addr;
+	bool wed_wds = false;
 
-	idx = mt76_wcid_alloc(dev->mt76.wcid_mask, MT7915_WTBL_STA);
+	if (mtk_wed_device_active(&mdev->mmio.wed) && is_mt7986(mdev))
+		wed_wds = !!test_bit(MT_WCID_FLAG_4ADDR, &msta->wcid.flags);
+
+	if (wed_wds)
+		idx = mt76_wcid_alloc(mdev->wcid_wds_mask, MT7915_WTBL_STA);
+	else {
+		idx = mt76_wcid_alloc(mdev->wcid_mask, MT7915_WTBL_STA);
+		if (idx < 0)
+			idx = mt76_wcid_alloc(mdev->wcid_wds_mask, MT7915_WTBL_STA);
+	}
 	if (idx < 0)
 		return -ENOSPC;
 
@@ -1124,6 +1152,14 @@ static void mt7915_sta_set_4addr(struct ieee80211_hw *hw,
 	else
 		clear_bit(MT_WCID_FLAG_4ADDR, &msta->wcid.flags);
 
+	if (mtk_wed_device_active(&dev->mt76.mmio.wed) &&
+	    is_mt7986(&dev->mt76) &&
+	    (msta->wcid.idx < MT7915_WTBL_WDS_START ||
+	     msta->wcid.idx > MT7915_WTBL_WDS_END)) {
+		mt7915_sta_remove(hw, vif, sta);
+		mt7915_sta_add(hw, vif, sta);
+	 }
+
 	mt76_connac_mcu_wtbl_update_hdr_trans(&dev->mt76, vif, sta);
 }
 
@@ -1463,12 +1499,16 @@ mt7915_net_fill_forward_path(struct ieee80211_hw *hw,
 	path->dev = ctx->dev;
 	path->mtk_wdma.wdma_idx = wed->wdma_idx;
 	path->mtk_wdma.bss = mvif->mt76.idx;
-	/* fw will find the wcid by dest addr */
-	if(is_mt7915(&dev->mt76))
-		path->mtk_wdma.wcid = 0xff;
-	else
-		path->mtk_wdma.wcid = 0x3ff;
 
+	if (test_bit(MT_WCID_FLAG_4ADDR, &msta->wcid.flags)) {
+		path->mtk_wdma.wcid = msta->wcid.idx;
+	} else {
+		/* fw will find the wcid by dest addr */
+		if(is_mt7915(&dev->mt76))
+			path->mtk_wdma.wcid = 0xff;
+		else
+			path->mtk_wdma.wcid = 0x3ff;
+	}
 	path->mtk_wdma.queue = phy != &dev->phy;
 
 	ctx->dev = NULL;
diff --git a/mt7915/mcu.c b/mt7915/mcu.c
index f5caa326..21167f00 100644
--- a/mt7915/mcu.c
+++ b/mt7915/mcu.c
@@ -2350,6 +2350,7 @@ mt7915_mcu_init_rx_airtime(struct mt7915_dev *dev)
 int mt7915_run_firmware(struct mt7915_dev *dev)
 {
 	int ret;
+	struct mtk_wed_device *wed = &dev->mt76.mmio.wed;
 
 	/* force firmware operation mode into normal state,
 	 * which should be set before firmware download stage.
@@ -2379,8 +2380,15 @@ int mt7915_run_firmware(struct mt7915_dev *dev)
 	if (ret)
 		return ret;
 
-	if (mtk_wed_device_active(&dev->mt76.mmio.wed) && is_mt7915(&dev->mt76))
-		mt7915_mcu_wa_cmd(dev, MCU_WA_PARAM_CMD(CAPABILITY), 0, 0, 0);
+	if (mtk_wed_device_active(wed)) {
+		if (is_mt7915(&dev->mt76))
+			mt7915_mcu_wa_cmd(dev, MCU_WA_PARAM_CMD(CAPABILITY),
+					  0, 0, 0);
+		else
+			mt7915_mcu_wa_cmd(dev, MCU_WA_PARAM_CMD(SET),
+					  MCU_WA_PARAM_WED_VERSION,
+					  wed->rev_id, 0);
+	}
 
 	ret = mt7915_mcu_set_mwds(dev, 1);
 	if (ret)
diff --git a/mt7915/mcu.h b/mt7915/mcu.h
index 9d0fac47..1f56db6b 100644
--- a/mt7915/mcu.h
+++ b/mt7915/mcu.h
@@ -268,6 +268,7 @@ enum {
 	MCU_WA_PARAM_RED_SHOW_STA = 0xf,
 	MCU_WA_PARAM_RED_TARGET_DELAY = 0x10,
 #endif
+	MCU_WA_PARAM_WED_VERSION = 0x32,
 };
 
 enum mcu_mmps_mode {
diff --git a/mt7915/mmio.c b/mt7915/mmio.c
index 11c90772..6df039a7 100644
--- a/mt7915/mmio.c
+++ b/mt7915/mmio.c
@@ -736,6 +736,8 @@ mt7915_pci_wed_init(struct mt7915_dev *dev, struct device *pdev, int *irq)
 	/* disable dynamic tx token */
 	wed->wlan.offload_enable = mt7915_wed_offload_enable;
 	wed->wlan.offload_disable = mt7915_wed_offload_disable;
+	if (!is_mt7915(mdev))
+		wed->wlan.wcid_512 = true;
 
 	if (mtk_wed_device_attach(wed) != 0)
 		return 0;
@@ -776,6 +778,7 @@ struct mt7915_dev *mt7915_mmio_probe(struct device *pdev,
 		.sta_add = mt7915_mac_sta_add,
 		.sta_remove = mt7915_mac_sta_remove,
 		.update_survey = mt7915_update_channel,
+		.wed_wds_check = mt7915_wed_wds_check,
 	};
 	struct mt7915_dev *dev;
 	struct mt76_dev *mdev;
diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index 1c78b882..486c203c 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -18,6 +18,9 @@
 #define MT7915_WTBL_STA			(MT7915_WTBL_RESERVED - \
 					 MT7915_MAX_INTERFACES)
 
+#define MT7915_WTBL_WDS_START		256
+#define MT7915_WTBL_WDS_END		271
+
 #define MT7915_WATCHDOG_TIME		(HZ / 10)
 #define MT7915_RESET_TIMEOUT		(30 * HZ)
 
@@ -697,6 +700,7 @@ void mt7915_tx_token_put(struct mt7915_dev *dev);
 void mt7915_queue_rx_skb(struct mt76_dev *mdev, enum mt76_rxq_id q,
 			 struct sk_buff *skb);
 bool mt7915_rx_check(struct mt76_dev *mdev, void *data, int len);
+bool mt7915_wed_wds_check(struct mt76_dev *mdev, struct ieee80211_sta *sta);
 void mt7915_sta_ps(struct mt76_dev *mdev, struct ieee80211_sta *sta, bool ps);
 void mt7915_stats_work(struct work_struct *work);
 int mt76_dfs_start_rdd(struct mt7915_dev *dev, bool force);
-- 
2.25.1