[rtw-next 7/8] wifi: rtw89: restart hardware to recover firmware if power-save becomes abnormal

[Ping-Ke Shih <pkshih@realtek.com>]

From: Chin-Yen Lee <timlee@realtek.com>

Somehow power-save related functions get failure, such as failed to
send null packet, or no response form firmware, and then WiFi will
become unstable. Trigger SER function actively to reset firmware/driver
to recover from abnormal states, including

 - firmware failed to ACK for entering PS mode
 - firmware failed to ACK for leaving PS mode
 - check PS H2C command received by firmware fail
 - failed to leave PS state

Signed-off-by: Chin-Yen Lee <timlee@realtek.com>
Signed-off-by: Ping-Ke Shih <pkshih@realtek.com>
---
 drivers/net/wireless/realtek/rtw89/core.c |  1 +
 drivers/net/wireless/realtek/rtw89/core.h |  2 ++
 drivers/net/wireless/realtek/rtw89/mac.c  |  7 +++++--
 drivers/net/wireless/realtek/rtw89/ps.c   | 23 +++++++++++++++++++++--
 4 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/drivers/net/wireless/realtek/rtw89/core.c b/drivers/net/wireless/realtek/rtw89/core.c
index a3f156bf708e..1b5a40e9821c 100644
--- a/drivers/net/wireless/realtek/rtw89/core.c
+++ b/drivers/net/wireless/realtek/rtw89/core.c
@@ -5538,6 +5538,7 @@ int rtw89_core_start(struct rtw89_dev *rtwdev)
 	rtw89_fw_h2c_fw_log(rtwdev, rtwdev->fw.log.enable);
 	rtw89_fw_h2c_init_ba_cam(rtwdev);
 	rtw89_tas_fw_timer_enable(rtwdev, true);
+	rtwdev->ps_hang_cnt = 0;
 
 	return 0;
 }
diff --git a/drivers/net/wireless/realtek/rtw89/core.h b/drivers/net/wireless/realtek/rtw89/core.h
index ae98d6866b30..f8b443894db9 100644
--- a/drivers/net/wireless/realtek/rtw89/core.h
+++ b/drivers/net/wireless/realtek/rtw89/core.h
@@ -42,6 +42,7 @@ extern const struct ieee80211_ops rtw89_ops;
 #define RTW89_TRACK_WORK_PERIOD	round_jiffies_relative(HZ * 2)
 #define RTW89_TRACK_PS_WORK_PERIOD msecs_to_jiffies(100)
 #define RTW89_FORBID_BA_TIMER round_jiffies_relative(HZ * 4)
+#define RTW89_PS_HANG_MAX_CNT 3
 #define CFO_TRACK_MAX_USER 64
 #define MAX_RSSI 110
 #define RSSI_FACTOR 1
@@ -6082,6 +6083,7 @@ struct rtw89_dev {
 	struct rtw89_btc btc;
 	enum rtw89_ps_mode ps_mode;
 	bool lps_enabled;
+	u8 ps_hang_cnt;
 
 	struct rtw89_wow_param wow;
 
diff --git a/drivers/net/wireless/realtek/rtw89/mac.c b/drivers/net/wireless/realtek/rtw89/mac.c
index fd11b8fb3c89..d837513f4e92 100644
--- a/drivers/net/wireless/realtek/rtw89/mac.c
+++ b/drivers/net/wireless/realtek/rtw89/mac.c
@@ -12,6 +12,7 @@
 #include "phy.h"
 #include "ps.h"
 #include "reg.h"
+#include "ser.h"
 #include "util.h"
 
 static const u32 rtw89_mac_mem_base_addrs_ax[RTW89_MAC_MEM_NUM] = {
@@ -1423,13 +1424,15 @@ void rtw89_mac_power_mode_change(struct rtw89_dev *rtwdev, bool enter)
 		if (!ret)
 			break;
 
-		if (i == RPWM_TRY_CNT - 1)
+		if (i == RPWM_TRY_CNT - 1) {
 			rtw89_err(rtwdev, "firmware failed to ack for %s ps mode\n",
 				  enter ? "entering" : "leaving");
-		else
+			rtw89_ser_notify(rtwdev, MAC_AX_ERR_ASSERTION);
+		} else {
 			rtw89_debug(rtwdev, RTW89_DBG_UNEXP,
 				    "%d time firmware failed to ack for %s ps mode\n",
 				    i + 1, enter ? "entering" : "leaving");
+		}
 	}
 }
 
diff --git a/drivers/net/wireless/realtek/rtw89/ps.c b/drivers/net/wireless/realtek/rtw89/ps.c
index cf58121eb541..3f69dd4361c3 100644
--- a/drivers/net/wireless/realtek/rtw89/ps.c
+++ b/drivers/net/wireless/realtek/rtw89/ps.c
@@ -11,6 +11,7 @@
 #include "phy.h"
 #include "ps.h"
 #include "reg.h"
+#include "ser.h"
 #include "util.h"
 
 static int rtw89_fw_receive_lps_h2c_check(struct rtw89_dev *rtwdev, u8 macid)
@@ -26,16 +27,27 @@ static int rtw89_fw_receive_lps_h2c_check(struct rtw89_dev *rtwdev, u8 macid)
 	c2h_info.id = RTW89_FWCMD_C2HREG_FUNC_PS_LEAVE_ACK;
 	ret = rtw89_fw_msg_reg(rtwdev, NULL, &c2h_info);
 	if (ret)
-		return ret;
+		goto fw_fail;
 
 	c2hreg_macid = u32_get_bits(c2h_info.u.c2hreg[0],
 				    RTW89_C2HREG_PS_LEAVE_ACK_MACID);
 	c2hreg_ret = u32_get_bits(c2h_info.u.c2hreg[1], RTW89_C2HREG_PS_LEAVE_ACK_RET);
 
-	if (macid != c2hreg_macid || c2hreg_ret)
+	if (macid != c2hreg_macid || c2hreg_ret) {
 		rtw89_warn(rtwdev, "rtw89: check lps h2c received by firmware fail\n");
+		ret = -EINVAL;
+		goto fw_fail;
+	}
+	rtwdev->ps_hang_cnt = 0;
 
 	return 0;
+
+fw_fail:
+	rtwdev->ps_hang_cnt++;
+	if (rtwdev->ps_hang_cnt >= RTW89_PS_HANG_MAX_CNT)
+		rtw89_ser_notify(rtwdev, MAC_AX_ERR_ASSERTION);
+
+	return ret;
 }
 
 static int rtw89_fw_leave_lps_check(struct rtw89_dev *rtwdev, u8 macid)
@@ -51,9 +63,16 @@ static int rtw89_fw_leave_lps_check(struct rtw89_dev *rtwdev, u8 macid)
 				       mac->ps_status, chk_msk);
 	if (ret) {
 		rtw89_info(rtwdev, "rtw89: failed to leave lps state\n");
+
+		rtwdev->ps_hang_cnt++;
+		if (rtwdev->ps_hang_cnt >= RTW89_PS_HANG_MAX_CNT)
+			rtw89_ser_notify(rtwdev, MAC_AX_ERR_ASSERTION);
+
 		return -EBUSY;
 	}
 
+	rtwdev->ps_hang_cnt = 0;
+
 	return 0;
 }
 
-- 
2.25.1