From 4bae67631956c6878e1f055e1cb0e3dd2154f7b7 Mon Sep 17 00:00:00 2001
From: Sujuan Chen <sujuan.chen@mediatek.com>
Date: Thu, 28 Jul 2022 11:16:15 +0800
Subject: [PATCH] mt76 add ser spport when wed on

Signed-off-by: Sujuan Chen <sujuan.chen@mediatek.com>
---
 dma.c           | 29 ++++++++++++++++++++---------
 dma.h           |  2 +-
 mt76.h          |  1 +
 mt7915/dma.c    | 36 +++++++++++++++++++++++++++++++-----
 mt7915/mac.c    | 20 ++++++++++++++++++++
 mt7915/mmio.c   |  2 ++
 mt7915/mt7915.h |  1 +
 7 files changed, 76 insertions(+), 15 deletions(-)

diff --git a/dma.c b/dma.c
index 3317d2b9..fa56ccfb 100644
--- a/dma.c
+++ b/dma.c
@@ -169,7 +169,7 @@ mt76_free_pending_txwi(struct mt76_dev *dev)
 	local_bh_enable();
 }
 
-static void
+void
 mt76_free_pending_rxwi(struct mt76_dev *dev)
 {
 	struct mt76_txwi_cache *r;
@@ -183,6 +183,7 @@ mt76_free_pending_rxwi(struct mt76_dev *dev)
 	}
 	local_bh_enable();
 }
+EXPORT_SYMBOL_GPL(mt76_free_pending_rxwi);
 
 static void
 mt76_dma_sync_idx(struct mt76_dev *dev, struct mt76_queue *q)
@@ -624,14 +625,18 @@ mt76_dma_rx_fill(struct mt76_dev *dev, struct mt76_queue *q)
 	return frames;
 }
 
-static int
-mt76_dma_wed_setup(struct mt76_dev *dev, struct mt76_queue *q)
+int
+mt76_dma_wed_setup(struct mt76_dev *dev, struct mt76_queue *q, bool reset)
 {
 #ifdef CONFIG_NET_MEDIATEK_SOC_WED
 	struct mtk_wed_device *wed = &dev->mmio.wed;
 	int ret, type, ring;
-	u8 flags = q->flags;
+	u8 flags;
 
+	if (!q || !q->ndesc)
+		return -EINVAL;
+
+	flags = q->flags;
 	if (!mtk_wed_device_active(wed))
 		q->flags &= ~MT_QFLAG_WED;
 
@@ -643,7 +648,7 @@ mt76_dma_wed_setup(struct mt76_dev *dev, struct mt76_queue *q)
 
 	switch (type) {
 	case MT76_WED_Q_TX:
-		ret = mtk_wed_device_tx_ring_setup(wed, ring, q->regs);
+		ret = mtk_wed_device_tx_ring_setup(wed, ring, q->regs, reset);
 		if (!ret)
 			q->wed_regs = wed->tx_ring[ring].reg_base;
 		break;
@@ -659,7 +664,7 @@ mt76_dma_wed_setup(struct mt76_dev *dev, struct mt76_queue *q)
 			q->wed_regs = wed->txfree_ring.reg_base;
 		break;
 	case MT76_WED_Q_RX:
-		ret = mtk_wed_device_rx_ring_setup(wed, ring, q->regs);
+		ret = mtk_wed_device_rx_ring_setup(wed, ring, q->regs, reset);
 		if (!ret)
 			q->wed_regs = wed->rx_ring[ring].reg_base;
 		break;
@@ -672,6 +677,7 @@ mt76_dma_wed_setup(struct mt76_dev *dev, struct mt76_queue *q)
 	return 0;
 #endif
 }
+EXPORT_SYMBOL_GPL(mt76_dma_wed_setup);
 
 static int
 mt76_dma_alloc_queue(struct mt76_dev *dev, struct mt76_queue *q,
@@ -704,7 +710,7 @@ mt76_dma_alloc_queue(struct mt76_dev *dev, struct mt76_queue *q,
 	if (!q->entry)
 		return -ENOMEM;
 
-	ret = mt76_dma_wed_setup(dev, q);
+	ret = mt76_dma_wed_setup(dev, q, false);
 	if (ret)
 		return ret;
 
@@ -755,8 +761,13 @@ mt76_dma_rx_reset(struct mt76_dev *dev, enum mt76_rxq_id qid)
 		q->desc[i].ctrl = cpu_to_le32(MT_DMA_CTL_DMA_DONE);
 
 	mt76_dma_rx_cleanup(dev, q);
-	mt76_dma_sync_idx(dev, q);
-	mt76_dma_rx_fill(dev, q);
+
+	mt76_dma_wed_setup(dev, q, true);
+
+	if (q->flags != MT_WED_Q_TXFREE) {
+		mt76_dma_sync_idx(dev, q);
+		mt76_dma_rx_fill(dev, q);
+	}
 
 	if (!q->rx_head)
 		return;
diff --git a/dma.h b/dma.h
index 90370d12..083cbca4 100644
--- a/dma.h
+++ b/dma.h
@@ -58,5 +58,5 @@ enum mt76_mcu_evt_type {
 int mt76_dma_rx_poll(struct napi_struct *napi, int budget);
 void mt76_dma_attach(struct mt76_dev *dev);
 void mt76_dma_cleanup(struct mt76_dev *dev);
-
+int mt76_dma_wed_setup(struct mt76_dev *dev, struct mt76_queue *q, bool reset);
 #endif
diff --git a/mt76.h b/mt76.h
index 2903b625..831a47a9 100644
--- a/mt76.h
+++ b/mt76.h
@@ -1371,6 +1371,7 @@ mt76_tx_status_get_hw(struct mt76_dev *dev, struct sk_buff *skb)
 void mt76_put_txwi(struct mt76_dev *dev, struct mt76_txwi_cache *t);
 void mt76_put_rxwi(struct mt76_dev *dev, struct mt76_txwi_cache *t);
 struct mt76_txwi_cache *mt76_get_rxwi(struct mt76_dev *dev);
+void mt76_free_pending_rxwi(struct mt76_dev *dev);
 void mt76_rx_complete(struct mt76_dev *dev, struct sk_buff_head *frames,
 		      struct napi_struct *napi);
 void mt76_rx_poll_complete(struct mt76_dev *dev, enum mt76_rxq_id q,
diff --git a/mt7915/dma.c b/mt7915/dma.c
index 197a0169..e0a51316 100644
--- a/mt7915/dma.c
+++ b/mt7915/dma.c
@@ -522,6 +522,7 @@ int mt7915_dma_init(struct mt7915_dev *dev, struct mt7915_phy *phy2)
 int mt7915_dma_reset(struct mt7915_dev *dev, bool force)
 {
 	struct mt76_phy *mphy_ext = dev->mt76.phys[MT_BAND1];
+	struct mtk_wed_device *wed = &dev->mt76.mmio.wed;
 	int i;
 
 	/* clean up hw queues */
@@ -542,28 +543,53 @@ int mt7915_dma_reset(struct mt7915_dev *dev, bool force)
 		mt7915_wfsys_reset(dev);
 
 	/* disable wfdma */
+	if (mtk_wed_device_active(&dev->mt76.mmio.wed))
+		mtk_wed_device_dma_reset(&dev->mt76.mmio.wed);
 	mt7915_dma_disable(dev, force);
 
+	/* set wifi reset done, wait FE reset */
+	if (mtk_wed_device_active(wed) && atomic_read(&wed->fe_reset)) {
+		atomic_set(&wed->fe_reset, 0);
+		rtnl_lock();
+		complete(&wed->wlan_reset_done);
+		rtnl_unlock();
+		wait_for_completion(&wed->fe_reset_done);
+	}
+
 	/* reset hw queues */
 	for (i = 0; i < __MT_TXQ_MAX; i++) {
 		mt76_queue_reset(dev, dev->mphy.q_tx[i]);
-		if (mphy_ext)
+		if (mphy_ext) {
 			mt76_queue_reset(dev, mphy_ext->q_tx[i]);
+			if (mtk_wed_device_active(wed))
+				mt76_dma_wed_setup(&dev->mt76,
+						   mphy_ext->q_tx[i],
+						   true);
+		}
+		if (mtk_wed_device_active(wed))
+			mt76_dma_wed_setup(&dev->mt76, dev->mphy.q_tx[i],
+					   true);
 	}
 
 	for (i = 0; i < __MT_MCUQ_MAX; i++)
 		mt76_queue_reset(dev, dev->mt76.q_mcu[i]);
 
-	mt76_for_each_q_rx(&dev->mt76, i)
-		mt76_queue_reset(dev, &dev->mt76.q_rx[i]);
+	mt76_for_each_q_rx(&dev->mt76, i) {
+		if (dev->mt76.q_rx[i].flags != MT_WED_Q_TXFREE)
+			mt76_queue_reset(dev, &dev->mt76.q_rx[i]);
+	}
 
 	mt76_tx_status_check(&dev->mt76, true);
 
-	mt7915_dma_enable(dev);
-
 	mt76_for_each_q_rx(&dev->mt76, i)
 		mt76_queue_rx_reset(dev, i);
 
+	if(mtk_wed_device_active(wed) && is_mt7915(&dev->mt76))
+		mt76_rmw(dev, MT_WFDMA0_EXT0_CFG, MT_WFDMA0_EXT0_RXWB_KEEP,
+			 MT_WFDMA0_EXT0_RXWB_KEEP);
+
+	mt7915_dma_enable(dev);
+
 	return 0;
 }
 
diff --git a/mt7915/mac.c b/mt7915/mac.c
index 1a2cadc5..11b73c8f 100644
--- a/mt7915/mac.c
+++ b/mt7915/mac.c
@@ -916,6 +916,8 @@ void mt7915_wed_release_rx_buf(struct mtk_wed_device *wed)
 		mt76_put_rxwi(&dev->mt76, rxwi);
 	}
 
+	mt76_free_pending_rxwi(&dev->mt76);
+
 	if (wed->rx_page.va)
 		return;
 
@@ -926,6 +928,18 @@ void mt7915_wed_release_rx_buf(struct mtk_wed_device *wed)
 	return;
 }
 
+void mt7915_wed_trigger_ser(struct mtk_wed_device *wed)
+{
+	struct mt7915_dev *dev;
+	u8 band_idx;
+	dev = container_of(wed, struct mt7915_dev, mt76.mmio.wed);
+	band_idx = dev->phy.band_idx;
+
+	mt7915_mcu_set_ser(dev, SER_RECOVER, 1, band_idx);
+
+	return;
+}
+
 static void
 mt7915_tx_check_aggr(struct ieee80211_sta *sta, __le32 *txwi)
 {
@@ -1662,6 +1676,12 @@ void mt7915_mac_reset_work(struct work_struct *work)
 	if (!(READ_ONCE(dev->reset_state) & MT_MCU_CMD_STOP_DMA))
 		return;
 
+	if (mtk_wed_device_active(&dev->mt76.mmio.wed)) {
+		mtk_wed_device_stop(&dev->mt76.mmio.wed, true);
+		if (!is_mt7986(&dev->mt76))
+			mt76_wr(dev, MT_INT_WED_MASK_CSR, 0);
+	}
+
 	ieee80211_stop_queues(mt76_hw(dev));
 	if (ext_phy)
 		ieee80211_stop_queues(ext_phy->hw);
diff --git a/mt7915/mmio.c b/mt7915/mmio.c
index 111444d7..1ccec14f 100644
--- a/mt7915/mmio.c
+++ b/mt7915/mmio.c
@@ -757,6 +757,8 @@ mt7915_pci_wed_init(struct mt7915_dev *dev, struct device *pdev, int *irq)
 	wed->wlan.init_rx_buf = mt7915_wed_init_rx_buf;
 	wed->wlan.release_rx_buf = mt7915_wed_release_rx_buf;
 
+	wed->wlan.ser_trigger = mt7915_wed_trigger_ser;
+
 	dev->mt76.rx_token_size = wed->wlan.rx_pkt;
 	if (mtk_wed_device_attach(wed) != 0)
 		return 0;
diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index d23416ca..01b2f681 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -547,6 +547,7 @@ u32 mt7915_wed_init_buf(void *ptr, dma_addr_t phys, int token_id);
 u32 mt7915_wed_init_rx_buf(struct mtk_wed_device *wed,
 				int pkt_num);
 void mt7915_wed_release_rx_buf(struct mtk_wed_device *wed);
+void mt7915_wed_trigger_ser(struct mtk_wed_device *wed);
 int mt7915_register_device(struct mt7915_dev *dev);
 void mt7915_unregister_device(struct mt7915_dev *dev);
 int mt7915_eeprom_init(struct mt7915_dev *dev);
-- 
2.18.0