From dd9c922fc167858afc7ac23dd180cc45821a3501 Mon Sep 17 00:00:00 2001
From: Bo Jiao <Bo.Jiao@mediatek.com>
Date: Mon, 28 Mar 2022 09:28:30 +0800
Subject: [PATCH] mt76: mt7915: add L0.5 SER for mt7986

---
 mt7915/debugfs.c | 168 +++++++++++++++++++++++++++++++++-
 mt7915/dma.c     |  49 ++++++++++
 mt7915/init.c    |   8 +-
 mt7915/mac.c     | 231 ++++++++++++++++++++++++++++++++++++++++++++++-
 mt7915/main.c    |  16 +++-
 mt7915/mcu.c     |  52 ++++++++---
 mt7915/mmio.c    |  12 ++-
 mt7915/mt7915.h  |  27 ++++++
 mt7915/regs.h    |  37 +++++++-
 9 files changed, 572 insertions(+), 28 deletions(-)

diff --git a/mt7915/debugfs.c b/mt7915/debugfs.c
index 6df148c5..57754105 100644
--- a/mt7915/debugfs.c
+++ b/mt7915/debugfs.c
@@ -47,7 +47,8 @@ mt7915_implicit_txbf_get(void *data, u64 *val)
 DEFINE_DEBUGFS_ATTRIBUTE(fops_implicit_txbf, mt7915_implicit_txbf_get,
 			 mt7915_implicit_txbf_set, "%lld\n");
 
-/* test knob of system layer 1/2 error recovery */
+/* test knob of system layer 0.5/1/2 error recovery */
+/*
 static int mt7915_ser_trigger_set(void *data, u64 val)
 {
 	enum {
@@ -74,9 +75,172 @@ static int mt7915_ser_trigger_set(void *data, u64 val)
 	return ret;
 }
 
+*/
+static int mt7915_ser_trigger_set(void *data, u64 val)
+{
+#define SER_SET		GENMASK(3, 0)
+#define SER_BAND	GENMASK(7, 4)
+#define SER_ACTION	GENMASK(11, 8)
+	enum {
+		SER_ACTION_SET = 1,
+		SER_ACTION_SET_MASK = 2,
+		SER_ACTION_TRIGGER = 3,
+	};
+
+	struct mt7915_dev *dev = data;
+	u8 ser_action, ser_band, ser_set, set_val;
+
+	ser_action = FIELD_GET(SER_ACTION, val);
+	ser_set = set_val = FIELD_GET(SER_SET, val);
+	ser_band = (ser_action == SER_ACTION_TRIGGER) ?
+		   FIELD_GET(SER_BAND, val) : 0;
+
+	if (ser_band > 1)
+		return -1;
+
+	switch (ser_action) {
+	case SER_ACTION_SET:
+		/*
+		 * 0x100: disable system error recovery function.
+		 * 0x101: enable system error recovery function.
+		 */
+		ser_set = !!set_val;
+		break;
+	case SER_ACTION_SET_MASK:
+		/*
+		 * 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_ACTION_TRIGGER:
+		/*
+		 * 0x300: trigger L0 recover.
+		 * 0x301/0x311: trigger L1 recover for band0/band1.
+		 * 0x302/0x312: trigger L2 recover for band0/band1.
+		 * 0x303/0x313: trigger L3 rx abort for band0/band1.
+		 * 0x304/0x314: trigger L3 tx abort for band0/band1.
+		 * 0x305/0x315: trigger L3 tx disable for band0/band1.
+		 * 0x306/0x316: trigger L3 bf recover for band0/band1.
+		 */
+		if (0x300 == val || 0x310 == val) {
+			mt7915_reset(dev, SER_TYPE_FULL_RESET);
+			return 0;
+		}
+
+		if (ser_set > 6)
+			return -1;
+		break;
+	default:
+		return -1;
+	}
+
+	return mt7915_mcu_set_ser(dev, ser_action, ser_set, ser_band);
+}
+
 DEFINE_DEBUGFS_ATTRIBUTE(fops_ser_trigger, NULL,
 			 mt7915_ser_trigger_set, "%lld\n");
 
+static int
+mt7915_ser_stats_show(struct seq_file *s, void *data)
+{
+#define	SER_ACTION_QUERY	0
+	struct mt7915_dev *dev = dev_get_drvdata(s->private);
+	int ret = 0;
+
+	/* get more info from firmware */
+	ret = mt7915_mcu_set_ser(dev, SER_ACTION_QUERY, 0, 0);
+	msleep(100);
+
+	seq_printf(s, "::E  R , SER_STATUS        = 0x%08X\n",
+		   MT_SWDEF_SER_STATUS);
+	seq_printf(s, "::E  R , SER_PLE_ERR       = 0x%08X\n",
+		   MT_SWDEF_PLE_STATUS);
+	seq_printf(s, "::E  R , SER_PLE_ERR_1     = 0x%08X\n",
+		   MT_SWDEF_PLE1_STATUS);
+	seq_printf(s, "::E  R , SER_PLE_ERR_AMSDU = 0x%08X\n",
+		   MT_SWDEF_PLE_AMSDU_STATUS);
+	seq_printf(s, "::E  R , SER_PSE_ERR       = 0x%08X\n",
+		   MT_SWDEF_PSE_STATUS);
+	seq_printf(s, "::E  R , SER_PSE_ERR_1     = 0x%08X\n",
+		   MT_SWDEF_PSE1_STATUS);
+	seq_printf(s, "::E  R , SER_LMAC_WISR6_B0 = 0x%08X\n",
+		   MT_SWDEF_LAMC_WISR6_BN0_STATUS);
+	seq_printf(s, "::E  R , SER_LMAC_WISR6_B1 = 0x%08X\n",
+		   MT_SWDEF_LAMC_WISR6_BN1_STATUS);
+	seq_printf(s, "::E  R , SER_LMAC_WISR7_B0 = 0x%08X\n",
+		   MT_SWDEF_LAMC_WISR7_BN0_STATUS);
+	seq_printf(s, "::E  R , SER_LMAC_WISR7_B1 = 0x%08X\n",
+		   MT_SWDEF_LAMC_WISR7_BN1_STATUS);
+
+	seq_printf(s, "\nWF RESET STATUS: WM %d, WA %d, WO %d\n",
+		   dev->ser.wf_reset_wm_count,
+		   dev->ser.wf_reset_wa_count,
+		   dev->ser.wf_reset_wo_count);
+
+	return ret;
+}
+
+
+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);
+}
+
+void mt7915_fw_heart_beat_chk(struct mt7915_dev *dev)
+{
+#define WM_TIMEOUT_COUNT_CHECK 5
+#define WM_HANG_COUNT_CHECK 9
+	u32 cnt, cidx, didx, queue;
+	u32 idx, i;
+	static struct {
+		u32 cidx;
+		u32 didx;
+	} dma_rec[5];
+
+	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;
+		dma_rec[idx].cidx = cidx;
+		dma_rec[idx].cidx = didx;
+
+		if (((cnt - 1) == queue) &&
+		    (dev->ser.cmd_fail_cnt >= WM_HANG_COUNT_CHECK)) {
+
+			for (i = 0; i < 5; i++) {
+				if ((dma_rec[i].cidx != cidx) ||
+				    (dma_rec[i].didx != didx))
+					return;
+			}
+
+			mt7915_reset(dev, SER_TYPE_FULL_RESET);
+			dev->ser.cmd_fail_cnt = 0;
+		}
+	}
+}
+
 static int
 mt7915_radar_trigger(void *data, u64 val)
 {
@@ -927,6 +1091,8 @@ int mt7915_init_debugfs(struct mt7915_phy *phy)
 	debugfs_create_devm_seqfile(dev->mt76.dev, "twt_stats", dir,
 				    mt7915_twt_stats);
 	debugfs_create_file("ser_trigger", 0200, dir, dev, &fops_ser_trigger);
+	debugfs_create_devm_seqfile(dev->mt76.dev, "ser_show", dir,
+				    mt7915_ser_stats_show);
 	if (!dev->dbdc_support || phy->band_idx) {
 		debugfs_create_u32("dfs_hw_pattern", 0400, dir,
 				   &dev->hw_pattern);
diff --git a/mt7915/dma.c b/mt7915/dma.c
index 66a312a9..60b4368a 100644
--- a/mt7915/dma.c
+++ b/mt7915/dma.c
@@ -453,6 +453,55 @@ 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.phy2;
+	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);
+
+	for (i = 0; i < __MT_RXQ_MAX; 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]);
+
+	for (i = 0; i < __MT_RXQ_MAX; i++)
+		mt76_queue_reset(dev, &dev->mt76.q_rx[i]);
+
+	mt76_tx_status_check(&dev->mt76, true);
+
+	mt7915_dma_enable(dev);
+
+	for (i = 0; i < __MT_RXQ_MAX; 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 4d462c3a..03bcb40e 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)
 {
@@ -446,7 +446,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;
@@ -476,7 +476,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;
 
@@ -1157,6 +1157,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 d8578d12..aa312b75 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"
@@ -1970,9 +1971,9 @@ mt7915_update_beacons(struct mt7915_dev *dev)
 		IEEE80211_IFACE_ITER_RESUME_ALL,
 		mt7915_update_vif_beacon, dev->mt76.phy2->hw);
 }
-
+#if 0
 static void
-mt7915_dma_reset(struct mt7915_dev *dev)
+mt7915_dma_reset(struct mt7915_dev *dev, bool force)
 {
 	struct mt76_phy *mphy_ext = dev->mt76.phy2;
 	u32 hif1_ofs = MT_WFDMA0_PCIE1(0) - MT_WFDMA0(0);
@@ -2037,6 +2038,7 @@ mt7915_dma_reset(struct mt7915_dev *dev)
 				 MT_WFDMA1_GLO_CFG_OMIT_RX_INFO);
 	}
 }
+#endif
 
 void mt7915_tx_token_put(struct mt7915_dev *dev)
 {
@@ -2052,6 +2054,172 @@ void mt7915_tx_token_put(struct mt7915_dev *dev)
 	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;
+
+	ext_phy = dev->mt76.phy2;
+	phy2 = ext_phy ? ext_phy->priv : NULL;
+
+	/* irq disable */
+	mt76_wr(dev, MT_INT_MASK_CSR, 0x0);
+	mt76_wr(dev, MT_INT_SOURCE_CSR, ~0);
+	if (dev->hif2) {
+		mt76_wr(dev, MT_INT1_MASK_CSR, 0x0);
+		mt76_wr(dev, MT_INT1_SOURCE_CSR, ~0);
+	}
+	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);
+	}
+
+	set_bit(MT76_RESET, &dev->mphy.state);
+	wake_up(&dev->mt76.mcu.wait);
+	if (ext_phy)
+		set_bit(MT76_RESET, &ext_phy->state);
+
+	/* 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);
+
+	/* 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);
+
+	/* token reinit */
+	mt7915_tx_token_put(dev);
+	idr_init(&dev->mt76.token);
+
+	mt7915_dma_reset(dev, true);
+	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]);
+		}
+	}
+	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);
+	if (dev->hif2) {
+		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);
+
+out:
+	/* reset done */
+	clear_bit(MT76_RESET, &dev->mphy.state);
+	if (phy2)
+		clear_bit(MT76_RESET, &phy2->mt76->state);
+
+	napi_enable(&dev->mt76.tx_napi);
+	napi_schedule(&dev->mt76.tx_napi);
+	mt76_worker_enable(&dev->mt76.tx_worker);
+
+	return ret;
+}
+
+static void
+mt7915_mac_full_reset(struct mt7915_dev *dev)
+{
+	struct mt7915_phy *phy2;
+	struct mt76_phy *ext_phy;
+	int i;
+	struct cfg80211_scan_info info = {
+		.aborted = true,
+	};
+
+	ext_phy = dev->mt76.phy2;
+	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");
+
+	if (test_and_clear_bit(MT76_HW_SCANNING, &dev->mphy.state))
+		ieee80211_scan_completed(dev->mphy.hw, &info);
+
+	if (test_and_clear_bit(MT76_HW_SCANNING, &ext_phy->state))
+		ieee80211_scan_completed(ext_phy->hw, &info);
+
+	ieee80211_wake_queues(mt76_hw(dev));
+	if (ext_phy)
+		ieee80211_wake_queues(ext_phy->hw);
+
+	if (test_bit(MT76_STATE_RUNNING, &dev->mphy.state))
+		__mt7915_start(dev->mphy.hw);
+
+	if (ext_phy &&
+	    test_bit(MT76_STATE_RUNNING, &ext_phy->state))
+		__mt7915_start(ext_phy->hw);
+
+	dev->ser.hw_full_reset = false;
+
+	ieee80211_restart_hw(mt76_hw(dev));
+	if (ext_phy)
+		ieee80211_restart_hw(ext_phy->hw);
+
+	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 */
 void mt7915_mac_reset_work(struct work_struct *work)
 {
@@ -2063,6 +2231,25 @@ void mt7915_mac_reset_work(struct work_struct *work)
 	ext_phy = dev->mt76.phy2;
 	phy2 = ext_phy ? ext_phy->priv : NULL;
 
+	/* chip full reset */
+	if (dev->ser.reset_type == SER_TYPE_FULL_RESET) {
+		u32 val;
+
+		/* disable wa/wm watchdog interuppt */
+		val = mt76_rr(dev, MT_WFDMA0_MCU_HOST_INT_ENA);
+		val &= ~(MT_MCU_CMD_WA_WDT | MT_MCU_CMD_WM_WDT);
+		mt76_set(dev, MT_WFDMA0_MCU_HOST_INT_ENA, val);
+
+		mt7915_mac_full_reset(dev);
+
+		/* enable the mcu irq*/
+		mt7915_irq_enable(dev, MT_INT_MCU_CMD);
+		mt7915_irq_disable(dev, 0);
+		return;
+	}
+
+	/* chip partial reset */
+	dev->ser.reset_type = SER_TYPE_NONE;
 	if (!(READ_ONCE(dev->reset_state) & MT_MCU_CMD_STOP_DMA))
 		return;
 
@@ -2089,7 +2276,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);
@@ -2140,6 +2327,44 @@ void mt7915_mac_reset_work(struct work_struct *work)
 					     MT7915_WATCHDOG_TIME);
 }
 
+
+void mt7915_reset(struct mt7915_dev *dev, u8 type)
+{
+	struct mt76_phy *ext_phy = dev->mt76.phy2;
+
+	if (!dev->ser.hw_init_done)
+		return;
+
+	if (dev->ser.hw_full_reset)
+		return;
+
+	dev_err(dev->mt76.dev, "SER: reset_state(0x%08X) type(0x%08X)\n",
+		READ_ONCE(dev->reset_state), type);
+
+	/* WM/WA exception: do full recovery */
+	if ((READ_ONCE(dev->reset_state) & MT_MCU_CMD_WDT_MASK) ||
+	    type == SER_TYPE_FULL_RESET) {
+
+		if (!is_mt7986(&dev->mt76))
+			return;
+
+		dev->ser.reset_type = SER_TYPE_FULL_RESET;
+		set_bit(MT76_MCU_RESET, &dev->mphy.state);
+		if (ext_phy)
+			set_bit(MT76_MCU_RESET, &ext_phy->state);
+
+		ieee80211_queue_work(mt76_hw(dev), &dev->reset_work);
+		return;
+	}
+
+	/* do partial recovery */
+	mt7915_irq_enable(dev, MT_INT_MCU_CMD);
+	mt7915_irq_disable(dev, 0);
+	dev->ser.reset_type = SER_TYPE_PARTIAL_RESET;
+	ieee80211_queue_work(mt76_hw(dev), &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 0816398a..9de5f152 100644
--- a/mt7915/main.c
+++ b/mt7915/main.c
@@ -20,7 +20,7 @@ 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);
@@ -29,8 +29,6 @@ static int mt7915_start(struct ieee80211_hw *hw)
 
 	flush_work(&dev->init_work);
 
-	mutex_lock(&dev->mt76.mutex);
-
 	running = mt7915_dev_running(dev);
 
 	if (!running) {
@@ -95,6 +93,18 @@ out:
 	return ret;
 }
 
+static int mt7915_start(struct ieee80211_hw *hw)
+{
+	struct mt7915_dev *dev = mt7915_hw_dev(hw);
+	int ret;
+
+	mutex_lock(&dev->mt76.mutex);
+	ret = __mt7915_start(hw);
+	mutex_unlock(&dev->mt76.mutex);
+
+	return ret;
+}
+
 static void mt7915_stop(struct ieee80211_hw *hw)
 {
 	struct mt7915_dev *dev = mt7915_hw_dev(hw);
diff --git a/mt7915/mcu.c b/mt7915/mcu.c
index 987e5f3f..e7c0ab1d 100644
--- a/mt7915/mcu.c
+++ b/mt7915/mcu.c
@@ -212,14 +212,31 @@ mt7915_mcu_parse_response(struct mt76_dev *mdev, int cmd,
 			  struct sk_buff *skb, int seq)
 {
 	struct mt7915_mcu_rxd *rxd;
+	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
 	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_type = SER_TYPE_FULL_RESET;
+				mt7915_reset(dev, SER_TYPE_FULL_RESET);
+			}
+		}
+		mt7915_fw_heart_beat_chk(dev);
+
 		return -ETIMEDOUT;
 	}
 
+	dev->ser.cmd_fail_cnt = 0;
+
 	rxd = (struct mt7915_mcu_rxd *)skb->data;
 	if (seq != rxd->seq)
 		return -EAGAIN;
@@ -243,11 +260,17 @@ mt7915_mcu_send_message(struct mt76_dev *mdev, struct sk_buff *skb,
 {
 	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
 	struct mt7915_mcu_txd *mcu_txd;
+	//struct mt76_phy *ext_phy = mdev->phy2;
 	enum mt76_mcuq_id qid;
 	__le32 *txd;
 	u32 val;
 	u8 seq;
 
+	if (test_bit(MT76_MCU_RESET, &mdev->phy.state)) {
+		dev_err(mdev->dev, "%s assert\n", __func__);
+		return -EBUSY;
+	}
+
 	/* TODO: make dynamic based on msg type */
 	mdev->mcu.timeout = 20 * HZ;
 
@@ -2472,25 +2495,14 @@ 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 mt7915_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.
 	 */
-	if (is_mt7915(&dev->mt76))
-		mt76_wr(dev, MT_SWDEF_MODE, MT_SWDEF_NORMAL_MODE);
-	else
-		mt76_wr(dev, MT_SWDEF_MODE_MT7916, MT_SWDEF_NORMAL_MODE);
+	mt76_wr(dev, MT_SWDEF_MODE, MT_SWDEF_NORMAL_MODE);
 
 	ret = mt7915_driver_own(dev, 0);
 	if (ret)
@@ -2532,6 +2544,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 mt7915_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/mmio.c b/mt7915/mmio.c
index 2466907e..561ac650 100644
--- a/mt7915/mmio.c
+++ b/mt7915/mmio.c
@@ -22,6 +22,8 @@ static const u32 mt7915_reg[] = {
 	[WFDMA_EXT_CSR_ADDR]	= 0xd7000,
 	[CBTOP1_PHY_END]	= 0x77ffffff,
 	[INFRA_MCU_ADDR_END]	= 0x7c3fffff,
+	[EXCEPTION_BASE_ADDR]	= 0x219848,
+	[SWDEF_BASE_ADDR]	= 0x41f200,
 };
 
 static const u32 mt7916_reg[] = {
@@ -36,6 +38,8 @@ static const u32 mt7916_reg[] = {
 	[WFDMA_EXT_CSR_ADDR]	= 0xd7000,
 	[CBTOP1_PHY_END]	= 0x7fffffff,
 	[INFRA_MCU_ADDR_END]	= 0x7c085fff,
+	[EXCEPTION_BASE_ADDR]	= 0x022050BC,
+	[SWDEF_BASE_ADDR]	= 0x411400,
 };
 
 static const u32 mt7986_reg[] = {
@@ -50,6 +54,8 @@ static const u32 mt7986_reg[] = {
 	[WFDMA_EXT_CSR_ADDR]	= 0x27000,
 	[CBTOP1_PHY_END]	= 0x7fffffff,
 	[INFRA_MCU_ADDR_END]	= 0x7c085fff,
+	[EXCEPTION_BASE_ADDR]	= 0x02204FFC,
+	[SWDEF_BASE_ADDR]	= 0x411400,
 };
 
 static const u32 mt7915_offs[] = {
@@ -601,10 +607,10 @@ 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) {
+		mt7915_irq_disable(dev, MT_INT_MCU_CMD);
+		if (val & MT_MCU_CMD_ALL_ERROR_MASK) {
 			dev->reset_state = val;
-			ieee80211_queue_work(mt76_hw(dev), &dev->reset_work);
-			wake_up(&dev->reset_wait);
+			mt7915_reset(dev, SER_TYPE_UNKNOWN);
 		}
 	}
 }
diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index b4d3981e..87557da8 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -343,6 +343,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;
+		u32 reset_type;
+		u32 cmd_fail_cnt;
+		u32 wf_reset_wm_count;
+		u32 wf_reset_wa_count;
+		u32 wf_reset_wo_count;
+	}ser;
 
 	struct list_head sta_rc_list;
 	struct list_head sta_poll_list;
@@ -439,6 +448,13 @@ enum mt7915_rdd_cmd {
 	RDD_IRQ_OFF,
 };
 
+enum {
+	SER_TYPE_NONE,
+	SER_TYPE_PARTIAL_RESET,
+	SER_TYPE_FULL_RESET,
+	SER_TYPE_UNKNOWN,
+};
+
 static inline struct mt7915_phy *
 mt7915_hw_phy(struct ieee80211_hw *hw)
 {
@@ -656,6 +672,17 @@ int mt7915_mcu_muru_debug_get(struct mt7915_phy *phy, void *ms);
 int mt7915_init_debugfs(struct mt7915_phy *phy);
 void mt7915_debugfs_rx_fw_monitor(struct mt7915_dev *dev, const void *data, int len);
 bool mt7915_debugfs_rx_log(struct mt7915_dev *dev, const void *data, int len);
+int mt7915_fw_exception_chk(struct mt7915_dev *dev);
+void mt7915_fw_heart_beat_chk(struct mt7915_dev *dev);
+void mt7915_wfsys_reset(struct mt7915_dev *dev);
+void mt7915_reset(struct mt7915_dev *dev, u8 type);
+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);
 #ifdef CONFIG_MAC80211_DEBUGFS
 void mt7915_sta_add_debugfs(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			    struct ieee80211_sta *sta, struct dentry *dir);
diff --git a/mt7915/regs.h b/mt7915/regs.h
index 6ddfa48f..27f682fe 100644
--- a/mt7915/regs.h
+++ b/mt7915/regs.h
@@ -30,6 +30,8 @@ enum reg_rev {
 	WFDMA_EXT_CSR_ADDR,
 	CBTOP1_PHY_END,
 	INFRA_MCU_ADDR_END,
+	EXCEPTION_BASE_ADDR,
+	SWDEF_BASE_ADDR,
 	__MT_REG_MAX,
 };
 
@@ -111,6 +113,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))
@@ -555,6 +562,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))
@@ -687,6 +698,12 @@ 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)
+#define MT_MCU_CMD_ALL_ERROR_MASK	(MT_MCU_CMD_ERROR_MASK |	\
+					 MT_MCU_CMD_WDT_MASK)
+
 /* TOP RGU */
 #define MT_TOP_RGU_BASE			0x18000000
 #define MT_TOP_PWR_CTRL			(MT_TOP_RGU_BASE + (0x0))
@@ -929,12 +946,25 @@ enum offs_rev {
 #define MT_ADIE_TYPE_MASK		BIT(1)
 
 /* FW MODE SYNC */
-#define MT_SWDEF_MODE			0x41f23c
-#define MT_SWDEF_MODE_MT7916		0x41143c
+#define MT_SWDEF_BASE			__REG(SWDEF_BASE_ADDR)
+
+#define MT_SWDEF(ofs)			(MT_SWDEF_BASE + (ofs))
+#define MT_SWDEF_MODE			MT_SWDEF(0x3c)
 #define MT_SWDEF_NORMAL_MODE		0
 #define MT_SWDEF_ICAP_MODE		1
 #define MT_SWDEF_SPECTRUM_MODE		2
 
+#define MT_SWDEF_SER_STATUS		MT_SWDEF(0x040)
+#define MT_SWDEF_PLE_STATUS		MT_SWDEF(0x044)
+#define MT_SWDEF_PLE1_STATUS		MT_SWDEF(0x048)
+#define MT_SWDEF_PLE_AMSDU_STATUS	MT_SWDEF(0x04C)
+#define MT_SWDEF_PSE_STATUS		MT_SWDEF(0x050)
+#define MT_SWDEF_PSE1_STATUS		MT_SWDEF(0x054)
+#define MT_SWDEF_LAMC_WISR6_BN0_STATUS	MT_SWDEF(0x058)
+#define MT_SWDEF_LAMC_WISR6_BN1_STATUS	MT_SWDEF(0x05C)
+#define MT_SWDEF_LAMC_WISR7_BN0_STATUS	MT_SWDEF(0x060)
+#define MT_SWDEF_LAMC_WISR7_BN1_STATUS	MT_SWDEF(0x064)
+
 #define MT_DIC_CMD_REG_BASE		0x41f000
 #define MT_DIC_CMD_REG(ofs)		(MT_DIC_CMD_REG_BASE + (ofs))
 #define MT_DIC_CMD_REG_CMD		MT_DIC_CMD_REG(0x10)
@@ -1016,6 +1046,9 @@ enum offs_rev {
 
 #define MT_MCU_BUS_REMAP		MT_MCU_BUS(0x120)
 
+/* MISC */
+#define MT_EXCEPTION_ADDR		__REG(EXCEPTION_BASE_ADDR)
+
 /* TOP CFG */
 #define MT_TOP_CFG_BASE			0x184b0000
 #define MT_TOP_CFG(ofs)			(MT_TOP_CFG_BASE + (ofs))
-- 
2.18.0