From a9856ee394084946e0fb59c6b56dfd66108761ba Mon Sep 17 00:00:00 2001
From: Bo Jiao <Bo.Jiao@mediatek.com>
Date: Wed, 22 Jun 2022 10:51:59 +0800
Subject: [PATCH] mt76: mt7915: add L0.5 system error recovery support

---
 mt7915/debugfs.c |  88 ++++++++++++---
 mt7915/dma.c     |  48 ++++++++
 mt7915/init.c    |   8 +-
 mt7915/mac.c     | 283 +++++++++++++++++++++++++++++++++++++----------
 mt7915/main.c    |  19 +++-
 mt7915/mcu.c     |  95 ++++++++++++++--
 mt7915/mcu.h     |   3 +-
 mt7915/mmio.c    |   8 +-
 mt7915/mt7915.h  |  23 ++++
 mt7915/regs.h    |  16 +++
 10 files changed, 492 insertions(+), 99 deletions(-)

diff --git a/mt7915/debugfs.c b/mt7915/debugfs.c
index 0cfb6068..07de2eef 100644
--- a/mt7915/debugfs.c
+++ b/mt7915/debugfs.c
@@ -52,12 +52,17 @@ static ssize_t
 mt7915_fw_ser_set(struct file *file, const char __user *user_buf,
 		  size_t count, loff_t *ppos)
 {
+#define SER_LEVEL	GENMASK(3, 0)
+#define SER_ACTION	GENMASK(11, 8)
+
 	struct mt7915_phy *phy = file->private_data;
 	struct mt7915_dev *dev = phy->dev;
-	bool ext_phy = phy != &dev->phy;
+	u8 ser_action, ser_set, set_val;
+	u8 band_idx = phy->band_idx;
 	char buf[16];
 	int ret = 0;
 	u16 val;
+	u32 intr;
 
 	if (count >= sizeof(buf))
 		return -EINVAL;
@@ -73,28 +78,71 @@ mt7915_fw_ser_set(struct file *file, const char __user *user_buf,
 	if (kstrtou16(buf, 0, &val))
 		return -EINVAL;
 
-	switch (val) {
+	ser_action = FIELD_GET(SER_ACTION, val);
+	ser_set = set_val = FIELD_GET(SER_LEVEL, val);
+
+	switch (ser_action) {
 	case SER_QUERY:
 		/* grab firmware SER stats */
-		ret = mt7915_mcu_set_ser(dev, 0, 0, ext_phy);
+		ser_set = 0;
 		break;
-	case SER_SET_RECOVER_L1:
-	case SER_SET_RECOVER_L2:
-	case SER_SET_RECOVER_L3_RX_ABORT:
-	case SER_SET_RECOVER_L3_TX_ABORT:
-	case SER_SET_RECOVER_L3_TX_DISABLE:
-	case SER_SET_RECOVER_L3_BF:
-		ret = mt7915_mcu_set_ser(dev, SER_ENABLE, BIT(val), ext_phy);
-		if (ret)
-			return ret;
-
-		ret = mt7915_mcu_set_ser(dev, SER_RECOVER, val, ext_phy);
+	case SER_SET:
+		/*
+		 * 0x100: disable system error recovery function.
+		 * 0x101: enable system error recovery function.
+		 * 0x103: enable l0.5 recover function.
+		 */
+		ser_set = !!set_val;
+
+		dev->ser.reset_enable = ser_set;
+		intr = mt76_rr(dev, MT_WFDMA0_MCU_HOST_INT_ENA);
+		if (dev->ser.reset_enable)
+			intr |= MT_MCU_CMD_WDT_MASK;
+		else
+			intr &= ~MT_MCU_CMD_WDT_MASK;
+		mt76_set(dev, MT_WFDMA0_MCU_HOST_INT_ENA, intr);
 		break;
-	default:
+	case SER_ENABLE:
+		/*
+		 * 0x200: enable system error tracking.
+		 * 0x201: enable system error L1 recover.
+		 * 0x202: enable system error L2 recover.
+		 * 0x203: enable system error L3 rx abort.
+		 * 0x204: enable system error L3 tx abort.
+		 * 0x205: enable system error L3 tx disable.
+		 * 0x206: enable system error L3 bf recover.
+		 * 0x207: enable system error all recover.
+		 */
+		ser_set = set_val > 7 ? 0x7f : BIT(set_val);
+		break;
+	case SER_RECOVER:
+		/*
+		 * 0x300: trigger L0.5 recover.
+		 * 0x301: trigger L1 recover.
+		 * 0x302: trigger L2 recover.
+		 * 0x303: trigger L3 rx abort.
+		 * 0x304: trigger L3 tx abort
+		 * 0x305: trigger L3 tx disable.
+		 * 0x306: trigger L3 bf recover.
+		 */
+		if (!ser_set) {
+			if (dev->ser.reset_enable) {
+				dev->reset_state |= MT_MCU_CMD_WDT_MASK;
+				mt7915_reset(dev);
+			} else {
+				dev_info(dev->mt76.dev, "SER: chip full recovery not enable\n");
+			}
+			goto out;
+		}
 		break;
+	default:
+		goto out;
 	}
-
-	return ret ? ret : count;
+	ret = mt7915_mcu_set_ser(dev, ser_action, ser_set, band_idx);
+	if (ret)
+		return ret;
+out:
+	return count;
 }
 
 static ssize_t
@@ -143,6 +191,12 @@ mt7915_fw_ser_get(struct file *file, char __user *user_buf,
 			  "::E  R , SER_LMAC_WISR7_B1 = 0x%08x\n",
 			  mt76_rr(dev, MT_SWDEF_LAMC_WISR7_BN1_STATS));
 
+	desc += scnprintf(buff + desc, bufsz - desc,
+			  "\nWF RESET STATUS: EN %d, WM %d, WA %d\n",
+			  dev->ser.reset_enable,
+			  dev->ser.wf_reset_wm_count,
+			  dev->ser.wf_reset_wa_count);
+
 	ret = simple_read_from_buffer(user_buf, count, ppos, buff, desc);
 	kfree(buff);
 	return ret;
diff --git a/mt7915/dma.c b/mt7915/dma.c
index f7e6bb10..4b594a53 100644
--- a/mt7915/dma.c
+++ b/mt7915/dma.c
@@ -479,6 +479,54 @@ int mt7915_dma_init(struct mt7915_dev *dev, struct mt7915_phy *phy2)
 	return 0;
 }
 
+int mt7915_dma_reset(struct mt7915_dev *dev, bool force)
+{
+	struct mt76_phy *mphy_ext = dev->mt76.phys[MT_BAND1];
+	int i;
+
+	/* clean up hw queues */
+	for (i = 0; i < ARRAY_SIZE(dev->mt76.phy.q_tx); i++) {
+		mt76_queue_tx_cleanup(dev, dev->mphy.q_tx[i], true);
+		if (mphy_ext)
+			mt76_queue_tx_cleanup(dev, mphy_ext->q_tx[i], true);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(dev->mt76.q_mcu); i++)
+		mt76_queue_tx_cleanup(dev, dev->mt76.q_mcu[i], true);
+
+	mt76_for_each_q_rx(&dev->mt76, i)
+		mt76_queue_rx_cleanup(dev, &dev->mt76.q_rx[i]);
+
+	/* reset wfsys */
+	if (force)
+		mt7915_wfsys_reset(dev);
+
+	/* disable wfdma */
+	mt7915_dma_disable(dev, force);
+
+	/* reset hw queues */
+	for (i = 0; i < __MT_TXQ_MAX; i++) {
+		mt76_queue_reset(dev, dev->mphy.q_tx[i]);
+		if (mphy_ext)
+			mt76_queue_reset(dev, mphy_ext->q_tx[i]);
+	}
+
+	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_tx_status_check(&dev->mt76, true);
+
+	mt7915_dma_enable(dev);
+
+	mt76_for_each_q_rx(&dev->mt76, i)
+		mt76_queue_rx_reset(dev, i);
+
+	return 0;
+}
+
 void mt7915_dma_cleanup(struct mt7915_dev *dev)
 {
 	mt7915_dma_disable(dev, true);
diff --git a/mt7915/init.c b/mt7915/init.c
index 37b7b54a..141c5ad8 100644
--- a/mt7915/init.c
+++ b/mt7915/init.c
@@ -262,7 +262,7 @@ static void mt7915_led_set_brightness(struct led_classdev *led_cdev,
 		mt7915_led_set_config(led_cdev, 0xff, 0);
 }
 
-static void
+void
 mt7915_init_txpower(struct mt7915_dev *dev,
 		    struct ieee80211_supported_band *sband)
 {
@@ -449,7 +449,7 @@ mt7915_mac_init_band(struct mt7915_dev *dev, u8 band)
 	mt76_clear(dev, MT_DMA_DCR0(band), MT_DMA_DCR0_RXD_G5_EN);
 }
 
-static void mt7915_mac_init(struct mt7915_dev *dev)
+void mt7915_mac_init(struct mt7915_dev *dev)
 {
 	int i;
 	u32 rx_len = is_mt7915(&dev->mt76) ? 0x400 : 0x680;
@@ -479,7 +479,7 @@ static void mt7915_mac_init(struct mt7915_dev *dev)
 	}
 }
 
-static int mt7915_txbf_init(struct mt7915_dev *dev)
+int mt7915_txbf_init(struct mt7915_dev *dev)
 {
 	int ret;
 
@@ -1162,6 +1162,8 @@ int mt7915_register_device(struct mt7915_dev *dev)
 
 	mt7915_init_debugfs(&dev->phy);
 
+	dev->ser.hw_init_done = true;
+
 	return 0;
 
 unreg_thermal:
diff --git a/mt7915/mac.c b/mt7915/mac.c
index 6b3fa4ce..e5409413 100644
--- a/mt7915/mac.c
+++ b/mt7915/mac.c
@@ -3,6 +3,7 @@
 
 #include <linux/etherdevice.h>
 #include <linux/timekeeping.h>
+#include <linux/pci.h>
 #include "mt7915.h"
 #include "../dma.h"
 #include "mac.h"
@@ -1315,85 +1316,187 @@ mt7915_update_beacons(struct mt7915_dev *dev)
 		mt7915_update_vif_beacon, mphy_ext->hw);
 }
 
-static void
-mt7915_dma_reset(struct mt7915_dev *dev)
+void mt7915_tx_token_put(struct mt7915_dev *dev)
 {
-	struct mt76_phy *mphy_ext = dev->mt76.phys[MT_BAND1];
-	u32 hif1_ofs = MT_WFDMA0_PCIE1(0) - MT_WFDMA0(0);
-	int i;
+	struct mt76_txwi_cache *txwi;
+	int id;
+
+	spin_lock_bh(&dev->mt76.token_lock);
+	idr_for_each_entry(&dev->mt76.token, txwi, id) {
+		mt7915_txwi_free(dev, txwi, NULL, NULL);
+		dev->mt76.token_count--;
+	}
+	spin_unlock_bh(&dev->mt76.token_lock);
+	idr_destroy(&dev->mt76.token);
+}
+
+static int
+mt7915_mac_reset(struct mt7915_dev *dev)
+{
+	struct mt7915_phy *phy2;
+	struct mt76_phy *ext_phy;
+	struct mt76_dev *mdev = &dev->mt76;
+	int i, ret;
+	u32 irq_mask;
 
-	mt76_clear(dev, MT_WFDMA0_GLO_CFG,
-		   MT_WFDMA0_GLO_CFG_TX_DMA_EN |
-		   MT_WFDMA0_GLO_CFG_RX_DMA_EN);
+	ext_phy = dev->mt76.phys[MT_BAND1];
+	phy2 = ext_phy ? ext_phy->priv : NULL;
 
-	if (is_mt7915(&dev->mt76))
-		mt76_clear(dev, MT_WFDMA1_GLO_CFG,
-			   MT_WFDMA1_GLO_CFG_TX_DMA_EN |
-			   MT_WFDMA1_GLO_CFG_RX_DMA_EN);
 	if (dev->hif2) {
-		mt76_clear(dev, MT_WFDMA0_GLO_CFG + hif1_ofs,
-			   MT_WFDMA0_GLO_CFG_TX_DMA_EN |
-			   MT_WFDMA0_GLO_CFG_RX_DMA_EN);
-
-		if (is_mt7915(&dev->mt76))
-			mt76_clear(dev, MT_WFDMA1_GLO_CFG + hif1_ofs,
-				   MT_WFDMA1_GLO_CFG_TX_DMA_EN |
-				   MT_WFDMA1_GLO_CFG_RX_DMA_EN);
+		mt76_wr(dev, MT_INT1_MASK_CSR, 0x0);
+		mt76_wr(dev, MT_INT1_SOURCE_CSR, ~0);
 	}
 
-	usleep_range(1000, 2000);
+	if (dev_is_pci(mdev->dev)) {
+		mt76_wr(dev, MT_PCIE_MAC_INT_ENABLE, 0x0);
+		if (dev->hif2)
+			mt76_wr(dev, MT_PCIE1_MAC_INT_ENABLE, 0x0);
+	}
 
-	for (i = 0; i < __MT_TXQ_MAX; i++) {
-		mt76_queue_tx_cleanup(dev, dev->mphy.q_tx[i], true);
-		if (mphy_ext)
-			mt76_queue_tx_cleanup(dev, mphy_ext->q_tx[i], true);
+	set_bit(MT76_RESET, &dev->mphy.state);
+	set_bit(MT76_MCU_RESET, &dev->mphy.state);
+	wake_up(&dev->mt76.mcu.wait);
+	if (ext_phy) {
+		set_bit(MT76_RESET, &ext_phy->state);
+		set_bit(MT76_MCU_RESET, &ext_phy->state);
 	}
 
-	for (i = 0; i < __MT_MCUQ_MAX; i++)
-		mt76_queue_tx_cleanup(dev, dev->mt76.q_mcu[i], true);
+	/* lock/unlock all queues to ensure that no tx is pending */
+	mt76_txq_schedule_all(&dev->mphy);
+	if (ext_phy)
+		mt76_txq_schedule_all(ext_phy);
 
-	mt76_for_each_q_rx(&dev->mt76, i)
-		mt76_queue_rx_reset(dev, i);
+	/* disable all tx/rx napi */
+	mt76_worker_disable(&dev->mt76.tx_worker);
+	mt76_for_each_q_rx(mdev, i) {
+		if (mdev->q_rx[i].ndesc)
+			napi_disable(&dev->mt76.napi[i]);
+	}
+	napi_disable(&dev->mt76.tx_napi);
 
-	mt76_tx_status_check(&dev->mt76, true);
+	/* token reinit */
+	mt7915_tx_token_put(dev);
+	idr_init(&dev->mt76.token);
 
-	/* re-init prefetch settings after reset */
-	mt7915_dma_prefetch(dev);
+	mt7915_dma_reset(dev, true);
+
+	local_bh_disable();
+	mt76_for_each_q_rx(mdev, i) {
+		if (mdev->q_rx[i].ndesc) {
+			napi_enable(&dev->mt76.napi[i]);
+			napi_schedule(&dev->mt76.napi[i]);
+		}
+	}
+	local_bh_enable();
+	clear_bit(MT76_MCU_RESET, &dev->mphy.state);
+	clear_bit(MT76_STATE_MCU_RUNNING, &dev->mphy.state);
+
+	mt76_wr(dev, MT_INT_MASK_CSR, dev->mt76.mmio.irqmask);
+	mt76_wr(dev, MT_INT_SOURCE_CSR, ~0);
 
-	mt76_set(dev, MT_WFDMA0_GLO_CFG,
-		 MT_WFDMA0_GLO_CFG_TX_DMA_EN | MT_WFDMA0_GLO_CFG_RX_DMA_EN);
-	if (is_mt7915(&dev->mt76))
-		mt76_set(dev, MT_WFDMA1_GLO_CFG,
-			 MT_WFDMA1_GLO_CFG_TX_DMA_EN |
-			 MT_WFDMA1_GLO_CFG_RX_DMA_EN |
-			 MT_WFDMA1_GLO_CFG_OMIT_TX_INFO |
-			 MT_WFDMA1_GLO_CFG_OMIT_RX_INFO);
 	if (dev->hif2) {
-		mt76_set(dev, MT_WFDMA0_GLO_CFG + hif1_ofs,
-			 MT_WFDMA0_GLO_CFG_TX_DMA_EN |
-			 MT_WFDMA0_GLO_CFG_RX_DMA_EN);
-
-		if (is_mt7915(&dev->mt76))
-			mt76_set(dev, MT_WFDMA1_GLO_CFG + hif1_ofs,
-				 MT_WFDMA1_GLO_CFG_TX_DMA_EN |
-				 MT_WFDMA1_GLO_CFG_RX_DMA_EN |
-				 MT_WFDMA1_GLO_CFG_OMIT_TX_INFO |
-				 MT_WFDMA1_GLO_CFG_OMIT_RX_INFO);
+		mt76_wr(dev, MT_INT1_MASK_CSR, irq_mask);
+		mt76_wr(dev, MT_INT1_SOURCE_CSR, ~0);
+	}
+	if (dev_is_pci(mdev->dev)) {
+		mt76_wr(dev, MT_PCIE_MAC_INT_ENABLE, 0xff);
+		if (dev->hif2)
+			mt76_wr(dev, MT_PCIE1_MAC_INT_ENABLE, 0xff);
+	}
+
+	/* load firmware */
+	ret = mt7915_run_firmware(dev);
+	if (ret)
+		goto out;
+
+	/* set the necessary init items */
+	ret = mt7915_mcu_set_eeprom(dev, dev->flash_mode);
+	if (ret)
+		goto out;
+
+	mt7915_mac_init(dev);
+	mt7915_init_txpower(dev, &dev->mphy.sband_2g.sband);
+	mt7915_init_txpower(dev, &dev->mphy.sband_5g.sband);
+	ret = mt7915_txbf_init(dev);
+
+	if (test_bit(MT76_STATE_RUNNING, &dev->mphy.state)) {
+		ret = __mt7915_start(dev->mphy.hw);
+		if (ret)
+			goto out;
 	}
+
+	if (ext_phy && test_bit(MT76_STATE_RUNNING, &ext_phy->state)) {
+		ret = __mt7915_start(ext_phy->hw);
+		if (ret)
+			goto out;
+	}
+
+out:
+	/* reset done */
+	clear_bit(MT76_RESET, &dev->mphy.state);
+	if (phy2)
+		clear_bit(MT76_RESET, &phy2->mt76->state);
+
+	local_bh_disable();
+	napi_enable(&dev->mt76.tx_napi);
+	napi_schedule(&dev->mt76.tx_napi);
+	local_bh_enable();
+
+	mt76_worker_enable(&dev->mt76.tx_worker);
+
+	return ret;
 }
 
-void mt7915_tx_token_put(struct mt7915_dev *dev)
+static void
+mt7915_mac_full_reset(struct mt7915_dev *dev)
 {
-	struct mt76_txwi_cache *txwi;
-	int id;
+	struct mt7915_phy *phy2;
+	struct mt76_phy *ext_phy;
+	int i;
 
-	spin_lock_bh(&dev->mt76.token_lock);
-	idr_for_each_entry(&dev->mt76.token, txwi, id) {
-		mt7915_txwi_free(dev, txwi, NULL, NULL);
-		dev->mt76.token_count--;
-	}
-	spin_unlock_bh(&dev->mt76.token_lock);
-	idr_destroy(&dev->mt76.token);
+	ext_phy = dev->mt76.phys[MT_BAND1];
+	phy2 = ext_phy ? ext_phy->priv : NULL;
+
+	dev->ser.hw_full_reset = true;
+	if (READ_ONCE(dev->reset_state) & MT_MCU_CMD_WA_WDT)
+		dev->ser.wf_reset_wa_count++;
+	else
+		dev->ser.wf_reset_wm_count++;
+
+	wake_up(&dev->mt76.mcu.wait);
+	ieee80211_stop_queues(mt76_hw(dev));
+	if (ext_phy)
+		ieee80211_stop_queues(ext_phy->hw);
+
+	cancel_delayed_work_sync(&dev->mphy.mac_work);
+	if (ext_phy)
+		cancel_delayed_work_sync(&ext_phy->mac_work);
+
+	mutex_lock(&dev->mt76.mutex);
+	for (i = 0; i < 10; i++) {
+		if (!mt7915_mac_reset(dev))
+			break;
+ 	}
+	mutex_unlock(&dev->mt76.mutex);
+
+	if (i == 10)
+		dev_err(dev->mt76.dev, "chip full reset failed\n");
+
+	ieee80211_restart_hw(mt76_hw(dev));
+	if (ext_phy)
+		ieee80211_restart_hw(ext_phy->hw);
+
+	ieee80211_wake_queues(mt76_hw(dev));
+	if (ext_phy)
+		ieee80211_wake_queues(ext_phy->hw);
+
+	dev->ser.hw_full_reset = false;
+	ieee80211_queue_delayed_work(mt76_hw(dev), &dev->mphy.mac_work,
+				     MT7915_WATCHDOG_TIME);
+	if (ext_phy)
+		ieee80211_queue_delayed_work(ext_phy->hw,
+					     &ext_phy->mac_work,
+					     MT7915_WATCHDOG_TIME);
 }
 
 /* system error recovery */
@@ -1408,6 +1511,36 @@ void mt7915_mac_reset_work(struct work_struct *work)
 	ext_phy = dev->mt76.phys[MT_BAND1];
 	phy2 = ext_phy ? ext_phy->priv : NULL;
 
+	/* chip full reset */
+	if (dev->ser.reset_type == SER_TYPE_FULL_RESET) {
+		u32 intr;
+
+		/* disable WA/WM WDT */
+		intr = mt76_rr(dev, MT_WFDMA0_MCU_HOST_INT_ENA);
+		intr &= ~MT_MCU_CMD_WDT_MASK;
+		mt76_set(dev, MT_WFDMA0_MCU_HOST_INT_ENA, intr);
+
+		mt7915_mac_full_reset(dev);
+
+		/* enable the mcu irq*/
+		mt7915_irq_enable(dev, MT_INT_MCU_CMD);
+		mt7915_irq_disable(dev, 0);
+
+		/* re-enable WA/WM WDT */
+		intr = mt76_rr(dev, MT_WFDMA0_MCU_HOST_INT_ENA);
+		intr |= MT_MCU_CMD_WDT_MASK;
+		mt76_set(dev, MT_WFDMA0_MCU_HOST_INT_ENA, intr);
+
+		dev->reset_state = MT_MCU_CMD_NORMAL_STATE;
+		dev->ser.reset_type = SER_TYPE_NONE;
+		dev_info(dev->mt76.dev, "SER: chip full reset completed, WM %d, WA %d\n",
+			 dev->ser.wf_reset_wm_count,
+			 dev->ser.wf_reset_wa_count);
+		return;
+	}
+
+	/* chip partial reset */
+	dev->ser.reset_type = SER_TYPE_NONE;
 	if (!(READ_ONCE(dev->reset_state) & MT_MCU_CMD_STOP_DMA))
 		return;
 
@@ -1433,7 +1566,7 @@ void mt7915_mac_reset_work(struct work_struct *work)
 	mt76_wr(dev, MT_MCU_INT_EVENT, MT_MCU_INT_EVENT_DMA_STOPPED);
 
 	if (mt7915_wait_reset_state(dev, MT_MCU_CMD_RESET_DONE)) {
-		mt7915_dma_reset(dev);
+		mt7915_dma_reset(dev, false);
 
 		mt7915_tx_token_put(dev);
 		idr_init(&dev->mt76.token);
@@ -1482,6 +1615,34 @@ void mt7915_mac_reset_work(struct work_struct *work)
 					     MT7915_WATCHDOG_TIME);
 }
 
+void mt7915_reset(struct mt7915_dev *dev)
+{
+	if (!dev->ser.hw_init_done)
+		return;
+
+	if (dev->ser.hw_full_reset)
+		return;
+
+	/* wm/wa exception: do full recovery */
+	if (READ_ONCE(dev->reset_state) & MT_MCU_CMD_WDT_MASK) {
+		dev_info(dev->mt76.dev, "SER: chip full recovery start, WM %d, WA %d\n",
+			 dev->ser.wf_reset_wm_count,
+			 dev->ser.wf_reset_wa_count);
+
+		dev->ser.reset_type = SER_TYPE_FULL_RESET;
+
+		mt7915_irq_disable(dev, MT_INT_MCU_CMD);
+		queue_work(dev->mt76.wq, &dev->reset_work);
+		return;
+	}
+
+	dev_info(dev->mt76.dev, "SER: chip partial recovery, reset_state(0x%08X)\n",
+		READ_ONCE(dev->reset_state));
+	dev->ser.reset_type = SER_TYPE_PARTIAL_RESET;
+	queue_work(dev->mt76.wq, &dev->reset_work);
+	wake_up(&dev->reset_wait);
+}
+
 void mt7915_mac_update_stats(struct mt7915_phy *phy)
 {
 	struct mt7915_dev *dev = phy->dev;
diff --git a/mt7915/main.c b/mt7915/main.c
index 4e82904f..7a7ff86a 100644
--- a/mt7915/main.c
+++ b/mt7915/main.c
@@ -20,17 +20,13 @@ static bool mt7915_dev_running(struct mt7915_dev *dev)
 	return phy && test_bit(MT76_STATE_RUNNING, &phy->mt76->state);
 }
 
-static int mt7915_start(struct ieee80211_hw *hw)
+int __mt7915_start(struct ieee80211_hw *hw)
 {
 	struct mt7915_dev *dev = mt7915_hw_dev(hw);
 	struct mt7915_phy *phy = mt7915_hw_phy(hw);
 	bool running;
 	int ret;
 
-	flush_work(&dev->init_work);
-
-	mutex_lock(&dev->mt76.mutex);
-
 	running = mt7915_dev_running(dev);
 
 	if (!running) {
@@ -84,6 +80,18 @@ static int mt7915_start(struct ieee80211_hw *hw)
 		mt7915_mac_reset_counters(phy);
 
 out:
+	return ret;
+}
+
+static int mt7915_start(struct ieee80211_hw *hw)
+{
+	struct mt7915_dev *dev = mt7915_hw_dev(hw);
+	int ret;
+
+	flush_work(&dev->init_work);
+
+	mutex_lock(&dev->mt76.mutex);
+	ret = __mt7915_start(hw);
 	mutex_unlock(&dev->mt76.mutex);
 
 	return ret;
@@ -95,6 +103,7 @@ static void mt7915_stop(struct ieee80211_hw *hw)
 	struct mt7915_phy *phy = mt7915_hw_phy(hw);
 
 	cancel_delayed_work_sync(&phy->mt76->mac_work);
+	cancel_work_sync(&dev->reset_work);
 
 	mutex_lock(&dev->mt76.mutex);
 
diff --git a/mt7915/mcu.c b/mt7915/mcu.c
index f692e520..5eac6ed6 100644
--- a/mt7915/mcu.c
+++ b/mt7915/mcu.c
@@ -150,19 +150,90 @@ mt7915_mcu_set_sta_ht_mcs(struct ieee80211_sta *sta, u8 *ht_mcs,
 		ht_mcs[nss] = sta->ht_cap.mcs.rx_mask[nss] & mask[nss];
 }
 
+static int
+mt7915_fw_exception_chk(struct mt7915_dev *dev)
+{
+	u32 reg_val;
+
+	reg_val = mt76_rr(dev, MT_EXCEPTION_ADDR);
+
+	if (is_mt7915(&dev->mt76))
+		reg_val >>= 8;
+
+	return !!(reg_val & 0xff);
+}
+
+static void
+mt7915_fw_heart_beat_chk(struct mt7915_dev *dev)
+{
+#define WM_TIMEOUT_COUNT_CHECK 5
+#define WM_HANG_COUNT_CHECK 9
+	static u32 cidx_rec[5], didx_rec[5];
+	u32 cnt, cidx, didx, queue;
+	u32 idx, i;
+
+	if (dev->ser.hw_full_reset)
+		return;
+
+	if (dev->ser.cmd_fail_cnt >= WM_TIMEOUT_COUNT_CHECK) {
+		cnt = mt76_rr(dev, WF_WFDMA_MEM_DMA_RX_RING_CTL + 4);
+		cidx = mt76_rr(dev, WF_WFDMA_MEM_DMA_RX_RING_CTL + 8);
+		didx = mt76_rr(dev, WF_WFDMA_MEM_DMA_RX_RING_CTL + 12);
+		queue = (didx > cidx) ?
+			(didx - cidx - 1) : (didx - cidx + cnt - 1);
+
+		idx = (dev->ser.cmd_fail_cnt - WM_TIMEOUT_COUNT_CHECK) % 5;
+		cidx_rec[idx] = cidx;
+		didx_rec[idx] = didx;
+
+		if ((cnt - 1) == queue &&
+		    dev->ser.cmd_fail_cnt >= WM_HANG_COUNT_CHECK) {
+
+			for (i = 0; i < 5; i++) {
+				if (cidx_rec[i] != cidx ||
+				    didx_rec[i] != didx)
+					return;
+			}
+			dev_err(dev->mt76.dev, "detect mem dma hang!\n");
+			if (dev->ser.reset_enable) {
+				dev->reset_state |= MT_MCU_CMD_WDT_MASK;
+				mt7915_reset(dev);
+			}
+			dev->ser.cmd_fail_cnt = 0;
+		}
+	}
+}
+
 static int
 mt7915_mcu_parse_response(struct mt76_dev *mdev, int cmd,
 			  struct sk_buff *skb, int seq)
 {
+	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
 	struct mt76_connac2_mcu_rxd *rxd;
 	int ret = 0;
 
 	if (!skb) {
 		dev_err(mdev->dev, "Message %08x (seq %d) timeout\n",
 			cmd, seq);
+
+		dev->ser.cmd_fail_cnt++;
+
+		if (dev->ser.cmd_fail_cnt < 5) {
+			int exp_type = mt7915_fw_exception_chk(dev);
+
+			dev_err(mdev->dev, "Fw is status(%d)\n", exp_type);
+			if (exp_type && dev->ser.reset_enable) {
+				dev->reset_state |= MT_MCU_CMD_WDT_MASK;
+				mt7915_reset(dev);
+			}
+		}
+		mt7915_fw_heart_beat_chk(dev);
+
 		return -ETIMEDOUT;
 	}
 
+	dev->ser.cmd_fail_cnt = 0;
+
 	rxd = (struct mt76_connac2_mcu_rxd *)skb->data;
 	if (seq != rxd->seq)
 		return -EAGAIN;
@@ -2266,18 +2337,10 @@ mt7915_mcu_init_rx_airtime(struct mt7915_dev *dev)
 				 sizeof(req), true);
 }
 
-int mt7915_mcu_init(struct mt7915_dev *dev)
+int mt7915_run_firmware(struct mt7915_dev *dev)
 {
-	static const struct mt76_mcu_ops mt7915_mcu_ops = {
-		.headroom = sizeof(struct mt76_connac2_mcu_txd),
-		.mcu_skb_send_msg = mt7915_mcu_send_message,
-		.mcu_parse_response = mt7915_mcu_parse_response,
-		.mcu_restart = mt76_connac_mcu_restart,
-	};
 	int ret;
 
-	dev->mt76.mcu_ops = &mt7915_mcu_ops;
-
 	/* force firmware operation mode into normal state,
 	 * which should be set before firmware download stage.
 	 */
@@ -2326,6 +2389,20 @@ int mt7915_mcu_init(struct mt7915_dev *dev)
 				 MCU_WA_PARAM_RED, 0, 0);
 }
 
+int mt7915_mcu_init(struct mt7915_dev *dev)
+{
+	static const struct mt76_mcu_ops mt7915_mcu_ops = {
+		.headroom = sizeof(struct mt76_connac2_mcu_txd),
+		.mcu_skb_send_msg = mt7915_mcu_send_message,
+		.mcu_parse_response = mt7915_mcu_parse_response,
+		.mcu_restart = mt76_connac_mcu_restart,
+	};
+
+	dev->mt76.mcu_ops = &mt7915_mcu_ops;
+
+	return mt7915_run_firmware(dev);
+}
+
 void mt7915_mcu_exit(struct mt7915_dev *dev)
 {
 	__mt76_mcu_restart(&dev->mt76);
diff --git a/mt7915/mcu.h b/mt7915/mcu.h
index aab1a6a3..c4850644 100644
--- a/mt7915/mcu.h
+++ b/mt7915/mcu.h
@@ -453,8 +453,9 @@ enum {
 
 enum {
 	SER_QUERY,
+	SER_SET,
 	/* recovery */
-	SER_SET_RECOVER_L1,
+	SER_SET_RECOVER_L1 = 1,
 	SER_SET_RECOVER_L2,
 	SER_SET_RECOVER_L3_RX_ABORT,
 	SER_SET_RECOVER_L3_TX_ABORT,
diff --git a/mt7915/mmio.c b/mt7915/mmio.c
index c81fa091..8469d8fb 100644
--- a/mt7915/mmio.c
+++ b/mt7915/mmio.c
@@ -24,6 +24,7 @@ static const u32 mt7915_reg[] = {
 	[INFRA_MCU_ADDR_END]	= 0x7c3fffff,
 	[FW_EXCEPTION_ADDR]	= 0x219848,
 	[SWDEF_BASE_ADDR]	= 0x41f200,
+	[EXCEPTION_BASE_ADDR]	= 0x219848,
 };
 
 static const u32 mt7916_reg[] = {
@@ -40,6 +41,7 @@ static const u32 mt7916_reg[] = {
 	[INFRA_MCU_ADDR_END]	= 0x7c085fff,
 	[FW_EXCEPTION_ADDR]	= 0x022050bc,
 	[SWDEF_BASE_ADDR]	= 0x411400,
+	[EXCEPTION_BASE_ADDR]	= 0x022050BC,
 };
 
 static const u32 mt7986_reg[] = {
@@ -56,6 +58,7 @@ static const u32 mt7986_reg[] = {
 	[INFRA_MCU_ADDR_END]	= 0x7c085fff,
 	[FW_EXCEPTION_ADDR]	= 0x02204ffc,
 	[SWDEF_BASE_ADDR]	= 0x411400,
+	[EXCEPTION_BASE_ADDR]	= 0x02204FFC,
 };
 
 static const u32 mt7915_offs[] = {
@@ -613,10 +616,9 @@ static void mt7915_irq_tasklet(struct tasklet_struct *t)
 		u32 val = mt76_rr(dev, MT_MCU_CMD);
 
 		mt76_wr(dev, MT_MCU_CMD, val);
-		if (val & MT_MCU_CMD_ERROR_MASK) {
+		if (val & (MT_MCU_CMD_ERROR_MASK | MT_MCU_CMD_WDT_MASK)) {
 			dev->reset_state = val;
-			queue_work(dev->mt76.wq, &dev->reset_work);
-			wake_up(&dev->reset_wait);
+			mt7915_reset(dev);
 		}
 	}
 }
diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index 267fa7eb..d214b181 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -352,6 +352,15 @@ struct mt7915_dev {
 	struct work_struct reset_work;
 	wait_queue_head_t reset_wait;
 	u32 reset_state;
+	struct {
+		bool hw_full_reset:1;
+		bool hw_init_done:1;
+		bool reset_enable:1;
+		u32 reset_type;
+		u32 cmd_fail_cnt;
+		u32 wf_reset_wm_count;
+		u32 wf_reset_wa_count;
+	}ser;
 
 	struct list_head sta_rc_list;
 	struct list_head sta_poll_list;
@@ -416,6 +425,12 @@ enum {
 	__MT_WFDMA_MAX,
 };
 
+enum {
+	SER_TYPE_NONE,
+	SER_TYPE_PARTIAL_RESET,
+	SER_TYPE_FULL_RESET,
+};
+
 enum {
 	MT_RX_SEL0,
 	MT_RX_SEL1,
@@ -514,6 +529,14 @@ s8 mt7915_eeprom_get_power_delta(struct mt7915_dev *dev, int band);
 int mt7915_dma_init(struct mt7915_dev *dev, struct mt7915_phy *phy2);
 void mt7915_dma_prefetch(struct mt7915_dev *dev);
 void mt7915_dma_cleanup(struct mt7915_dev *dev);
+void mt7915_reset(struct mt7915_dev *dev);
+int mt7915_dma_reset(struct mt7915_dev *dev, bool force);
+int __mt7915_start(struct ieee80211_hw *hw);
+void mt7915_init_txpower(struct mt7915_dev *dev,
+		    struct ieee80211_supported_band *sband);
+int mt7915_txbf_init(struct mt7915_dev *dev);
+void mt7915_mac_init(struct mt7915_dev *dev);
+int mt7915_run_firmware(struct mt7915_dev *dev);
 int mt7915_mcu_init(struct mt7915_dev *dev);
 int mt7915_mcu_twt_agrt_update(struct mt7915_dev *dev,
 			       struct mt7915_vif *mvif,
diff --git a/mt7915/regs.h b/mt7915/regs.h
index 16875680..3d2c15be 100644
--- a/mt7915/regs.h
+++ b/mt7915/regs.h
@@ -26,6 +26,7 @@ enum reg_rev {
 	INFRA_MCU_ADDR_END,
 	FW_EXCEPTION_ADDR,
 	SWDEF_BASE_ADDR,
+	EXCEPTION_BASE_ADDR,
 	__MT_REG_MAX,
 };
 
@@ -107,6 +108,11 @@ enum offs_rev {
 #define __REG(id)			(dev->reg.reg_rev[(id)])
 #define __OFFS(id)			(dev->reg.offs_rev[(id)])
 
+/* MEM WFDMA */
+#define WF_WFDMA_MEM_DMA		0x58000000
+
+#define WF_WFDMA_MEM_DMA_RX_RING_CTL	(WF_WFDMA_MEM_DMA + (0x510))
+
 /* MCU WFDMA0 */
 #define MT_MCU_WFDMA0_BASE		0x2000
 #define MT_MCU_WFDMA0(ofs)		(MT_MCU_WFDMA0_BASE + (ofs))
@@ -559,6 +565,10 @@ enum offs_rev {
 #define MT_WFDMA0_PRI_DLY_INT_CFG1	MT_WFDMA0(0x2f4)
 #define MT_WFDMA0_PRI_DLY_INT_CFG2	MT_WFDMA0(0x2f8)
 
+#define MT_WFDMA0_MCU_HOST_INT_ENA	MT_WFDMA0(0x1f4)
+#define MT_WFDMA0_MT_WA_WDT_INT		BIT(31)
+#define MT_WFDMA0_MT_WM_WDT_INT		BIT(30)
+
 /* WFDMA1 */
 #define MT_WFDMA1_BASE			0xd5000
 #define MT_WFDMA1(ofs)			(MT_WFDMA1_BASE + (ofs))
@@ -704,6 +714,10 @@ enum offs_rev {
 #define MT_MCU_CMD_NORMAL_STATE		BIT(5)
 #define MT_MCU_CMD_ERROR_MASK		GENMASK(5, 1)
 
+#define MT_MCU_CMD_WA_WDT		BIT(31)
+#define MT_MCU_CMD_WM_WDT		BIT(30)
+#define MT_MCU_CMD_WDT_MASK		GENMASK(31, 30)
+
 /* TOP RGU */
 #define MT_TOP_RGU_BASE			0x18000000
 #define MT_TOP_PWR_CTRL			(MT_TOP_RGU_BASE + (0x0))
@@ -979,6 +993,8 @@ enum offs_rev {
 #define MT_CPU_UTIL_PEAK_IDLE_CNT	MT_CPU_UTIL(0x0c)
 #define MT_CPU_UTIL_CTRL		MT_CPU_UTIL(0x1c)
 
+#define MT_EXCEPTION_ADDR		__REG(EXCEPTION_BASE_ADDR)
+
 /* LED */
 #define MT_LED_TOP_BASE			0x18013000
 #define MT_LED_PHYS(_n)			(MT_LED_TOP_BASE + (_n))
-- 
2.18.0