[PATCH] mt76: mt7921u: add suspend/resume support

[Lorenzo Bianconi <lorenzo@xxxxxxxxxx>]

Introduce suspend/resume callbacks for mt7921u driver.

Tested-by: Deren Wu <deren.wu@xxxxxxxxxxxx>
Signed-off-by: Lorenzo Bianconi <lorenzo@xxxxxxxxxx>
---
 .../wireless/mediatek/mt76/mt7921/mt7921.h    |  2 +-
 .../net/wireless/mediatek/mt76/mt7921/regs.h  |  5 ++
 .../net/wireless/mediatek/mt76/mt7921/usb.c   | 62 ++++++++++++++++++-
 .../wireless/mediatek/mt76/mt7921/usb_mac.c   |  7 ++-
 4 files changed, 72 insertions(+), 4 deletions(-)

diff --git a/drivers/net/wireless/mediatek/mt76/mt7921/mt7921.h b/drivers/net/wireless/mediatek/mt76/mt7921/mt7921.h
index 7690364bc079..459226c5bb11 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7921/mt7921.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7921/mt7921.h
@@ -467,7 +467,7 @@ bool mt7921_usb_sdio_tx_status_data(struct mt76_dev *mdev, u8 *update);
 
 int mt7921u_mcu_power_on(struct mt7921_dev *dev);
 int mt7921u_wfsys_reset(struct mt7921_dev *dev);
-int mt7921u_dma_init(struct mt7921_dev *dev);
+int mt7921u_dma_init(struct mt7921_dev *dev, bool resume);
 int mt7921u_init_reset(struct mt7921_dev *dev);
 int mt7921u_mac_reset(struct mt7921_dev *dev);
 #endif
diff --git a/drivers/net/wireless/mediatek/mt76/mt7921/regs.h b/drivers/net/wireless/mediatek/mt76/mt7921/regs.h
index 6712ff60c722..ea643260ceb6 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7921/regs.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7921/regs.h
@@ -516,4 +516,9 @@
 #define MT_TOP_MISC2_FW_PWR_ON		BIT(0)
 #define MT_TOP_MISC2_FW_N9_RDY		GENMASK(1, 0)
 
+#define MT_WF_SW_DEF_CR(ofs)		(0x401a00 + (ofs))
+#define MT_WF_SW_DEF_CR_USB_MCU_EVENT	MT_WF_SW_DEF_CR(0x028)
+#define MT_WF_SW_SER_TRIGGER_SUSPEND	BIT(6)
+#define MT_WF_SW_SER_DONE_SUSPEND	BIT(7)
+
 #endif
diff --git a/drivers/net/wireless/mediatek/mt76/mt7921/usb.c b/drivers/net/wireless/mediatek/mt76/mt7921/usb.c
index b7771e9f1fcd..dc38baef273a 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7921/usb.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7921/usb.c
@@ -246,7 +246,7 @@ static int mt7921u_probe(struct usb_interface *usb_intf,
 	if (ret)
 		goto error;
 
-	ret = mt7921u_dma_init(dev);
+	ret = mt7921u_dma_init(dev, false);
 	if (ret)
 		return ret;
 
@@ -288,6 +288,61 @@ static void mt7921u_disconnect(struct usb_interface *usb_intf)
 	mt76_free_device(&dev->mt76);
 }
 
+#ifdef CONFIG_PM
+static int mt7921u_suspend(struct usb_interface *intf, pm_message_t state)
+{
+	struct mt7921_dev *dev = usb_get_intfdata(intf);
+	int err;
+
+	err = mt76_connac_mcu_set_hif_suspend(&dev->mt76, true);
+	if (err)
+		return err;
+
+	mt76u_stop_rx(&dev->mt76);
+	mt76u_stop_tx(&dev->mt76);
+
+	set_bit(MT76_STATE_SUSPEND, &dev->mphy.state);
+
+	return 0;
+}
+
+static int mt7921u_resume(struct usb_interface *intf)
+{
+	struct mt7921_dev *dev = usb_get_intfdata(intf);
+	bool reinit = true;
+	int err, i;
+
+	for (i = 0; i < 10; i++) {
+		u32 val = mt76_rr(dev, MT_WF_SW_DEF_CR_USB_MCU_EVENT);
+
+		if (!(val & MT_WF_SW_SER_TRIGGER_SUSPEND)) {
+			reinit = false;
+			break;
+		}
+		if (val & MT_WF_SW_SER_DONE_SUSPEND) {
+			mt76_wr(dev, MT_WF_SW_DEF_CR_USB_MCU_EVENT, 0);
+			break;
+		}
+
+		msleep(20);
+	}
+
+	if (reinit || mt7921_dma_need_reinit(dev)) {
+		err = mt7921u_dma_init(dev, true);
+		if (err)
+			return err;
+	}
+
+	clear_bit(MT76_STATE_SUSPEND, &dev->mphy.state);
+
+	err = mt76u_resume_rx(&dev->mt76);
+	if (err < 0)
+		return err;
+
+	return mt76_connac_mcu_set_hif_suspend(&dev->mt76, false);
+}
+#endif /* CONFIG_PM */
+
 MODULE_DEVICE_TABLE(usb, mt7921u_device_table);
 MODULE_FIRMWARE(MT7921_FIRMWARE_WM);
 MODULE_FIRMWARE(MT7921_ROM_PATCH);
@@ -297,6 +352,11 @@ static struct usb_driver mt7921u_driver = {
 	.id_table	= mt7921u_device_table,
 	.probe		= mt7921u_probe,
 	.disconnect	= mt7921u_disconnect,
+#ifdef CONFIG_PM
+	.suspend	= mt7921u_suspend,
+	.resume		= mt7921u_resume,
+	.reset_resume	= mt7921u_resume,
+#endif /* CONFIG_PM */
 	.soft_unbind	= 1,
 	.disable_hub_initiated_lpm = 1,
 };
diff --git a/drivers/net/wireless/mediatek/mt76/mt7921/usb_mac.c b/drivers/net/wireless/mediatek/mt76/mt7921/usb_mac.c
index 99bcbd858b65..cd2f09743d2f 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7921/usb_mac.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7921/usb_mac.c
@@ -121,7 +121,7 @@ static void mt7921u_epctl_rst_opt(struct mt7921_dev *dev, bool reset)
 	mt7921u_uhw_wr(&dev->mt76, MT_SSUSB_EPCTL_CSR_EP_RST_OPT, val);
 }
 
-int mt7921u_dma_init(struct mt7921_dev *dev)
+int mt7921u_dma_init(struct mt7921_dev *dev, bool resume)
 {
 	int err;
 
@@ -136,6 +136,9 @@ int mt7921u_dma_init(struct mt7921_dev *dev)
 		   MT_WL_RX_AGG_TO | MT_WL_RX_AGG_LMT);
 	mt76_clear(dev, MT_UDMA_WLCFG_1, MT_WL_RX_AGG_PKT_LMT);
 
+	if (resume)
+		return 0;
+
 	err = mt7921u_dma_rx_evt_ep4(dev);
 	if (err)
 		return err;
@@ -221,7 +224,7 @@ int mt7921u_mac_reset(struct mt7921_dev *dev)
 	if (err)
 		goto out;
 
-	err = mt7921u_dma_init(dev);
+	err = mt7921u_dma_init(dev, false);
 	if (err)
 		goto out;
 
-- 
2.35.1