1 files changed, 143 insertions, 54 deletions

diff --git a/drivers/net/wireless/realtek/rtw88/ps.c b/drivers/net/wireless/realtek/rtw88/ps.c
index 7a189a9926fe..add5a20b8432 100644
--- a/drivers/net/wireless/realtek/rtw88/ps.c
+++ b/drivers/net/wireless/realtek/rtw88/ps.c
@@ -18,15 +18,16 @@ static int rtw_ips_pwr_up(struct rtw_dev *rtwdev)
 	if (ret)
 		rtw_err(rtwdev, "leave idle state failed\n");
 
+	rtw_coex_ips_notify(rtwdev, COEX_IPS_LEAVE);
 	rtw_set_channel(rtwdev);
-	clear_bit(RTW_FLAG_INACTIVE_PS, rtwdev->flags);
 
 	return ret;
 }
 
 int rtw_enter_ips(struct rtw_dev *rtwdev)
 {
-	set_bit(RTW_FLAG_INACTIVE_PS, rtwdev->flags);
+	if (!test_bit(RTW_FLAG_POWERON, rtwdev->flags))
+		return 0;
 
 	rtw_coex_ips_notify(rtwdev, COEX_IPS_ENTER);
 
@@ -36,8 +37,7 @@ int rtw_enter_ips(struct rtw_dev *rtwdev)
 	return 0;
 }
 
-static void rtw_restore_port_cfg_iter(void *data, u8 *mac,
-				      struct ieee80211_vif *vif)
+static void rtw_restore_port_cfg_iter(void *data, struct ieee80211_vif *vif)
 {
 	struct rtw_dev *rtwdev = data;
 	struct rtw_vif *rtwvif = (struct rtw_vif *)vif->drv_priv;
@@ -50,6 +50,9 @@ int rtw_leave_ips(struct rtw_dev *rtwdev)
 {
 	int ret;
 
+	if (test_bit(RTW_FLAG_POWERON, rtwdev->flags))
+		return 0;
+
 	rtw_hci_link_ps(rtwdev, false);
 
 	ret = rtw_ips_pwr_up(rtwdev);
@@ -58,9 +61,7 @@ int rtw_leave_ips(struct rtw_dev *rtwdev)
 		return ret;
 	}
 
-	rtw_iterate_vifs_atomic(rtwdev, rtw_restore_port_cfg_iter, rtwdev);
-
-	rtw_coex_ips_notify(rtwdev, COEX_IPS_LEAVE);
+	rtw_iterate_vifs(rtwdev, rtw_restore_port_cfg_iter, rtwdev);
 
 	return 0;
 }
@@ -68,48 +69,43 @@ int rtw_leave_ips(struct rtw_dev *rtwdev)
 void rtw_power_mode_change(struct rtw_dev *rtwdev, bool enter)
 {
 	u8 request, confirm, polling;
-	u8 polling_cnt;
-	u8 retry_cnt = 0;
-
-	for (retry_cnt = 0; retry_cnt < 3; retry_cnt++) {
-		request = rtw_read8(rtwdev, rtwdev->hci.rpwm_addr);
-		confirm = rtw_read8(rtwdev, rtwdev->hci.cpwm_addr);
-
-		/* toggle to request power mode, others remain 0 */
-		request ^= request | BIT_RPWM_TOGGLE;
-		if (!enter) {
-			request |= POWER_MODE_ACK;
-		} else {
-			request |= POWER_MODE_LCLK;
-			if (rtw_fw_lps_deep_mode == LPS_DEEP_MODE_PG)
-				request |= POWER_MODE_PG;
-		}
-
-		rtw_write8(rtwdev, rtwdev->hci.rpwm_addr, request);
-
-		if (enter)
-			return;
-
-		/* check confirm power mode has left power save state */
-		for (polling_cnt = 0; polling_cnt < 50; polling_cnt++) {
-			polling = rtw_read8(rtwdev, rtwdev->hci.cpwm_addr);
-			if ((polling ^ confirm) & BIT_RPWM_TOGGLE)
-				return;
-			udelay(100);
-		}
-
-		/* in case of fw/hw missed the request, retry */
-		rtw_warn(rtwdev, "failed to leave deep PS, retry=%d\n",
-			 retry_cnt);
+	int ret;
+
+	request = rtw_read8(rtwdev, rtwdev->hci.rpwm_addr);
+	confirm = rtw_read8(rtwdev, rtwdev->hci.cpwm_addr);
+
+	/* toggle to request power mode, others remain 0 */
+	request ^= request | BIT_RPWM_TOGGLE;
+	if (enter) {
+		request |= POWER_MODE_LCLK;
+		if (rtw_get_lps_deep_mode(rtwdev) == LPS_DEEP_MODE_PG)
+			request |= POWER_MODE_PG;
 	}
+	/* Each request require an ack from firmware */
+	request |= POWER_MODE_ACK;
 
-	/* Hit here means that driver failed to change hardware power mode to
-	 * active state after retry 3 times. If the power state is locked at
-	 * Deep sleep, most of the hardware circuits is not working, even
-	 * register read/write. It should be treated as fatal error and
-	 * requires an entire analysis about the firmware/hardware
-	 */
-	WARN(1, "Hardware power state locked\n");
+	if (rtw_fw_feature_check(&rtwdev->fw, FW_FEATURE_TX_WAKE))
+		request |= POWER_TX_WAKE;
+
+	rtw_write8(rtwdev, rtwdev->hci.rpwm_addr, request);
+
+	/* Check firmware get the power requset and ack via cpwm register */
+	ret = read_poll_timeout_atomic(rtw_read8, polling,
+				       (polling ^ confirm) & BIT_RPWM_TOGGLE,
+				       100, 15000, true, rtwdev,
+				       rtwdev->hci.cpwm_addr);
+	if (ret) {
+		/* Hit here means that driver failed to get an ack from firmware.
+		 * The reason could be that hardware is locked at Deep sleep,
+		 * so most of the hardware circuits are not working, even
+		 * register read/write; or firmware is locked in some state and
+		 * cannot get the request. It should be treated as fatal error
+		 * and requires an entire analysis about the firmware/hardware.
+		 */
+		WARN(1, "firmware failed to ack driver for %s Deep Power mode\n",
+		     enter ? "entering" : "leaving");
+		rtw_fw_dump_dbg_info(rtwdev);
+	}
 }
 EXPORT_SYMBOL(rtw_power_mode_change);
 
@@ -118,7 +114,7 @@ static void __rtw_leave_lps_deep(struct rtw_dev *rtwdev)
 	rtw_hci_deep_ps(rtwdev, false);
 }
 
-static void rtw_fw_leave_lps_state_check(struct rtw_dev *rtwdev)
+static int __rtw_fw_leave_lps_check_reg(struct rtw_dev *rtwdev)
 {
 	int i;
 
@@ -136,12 +132,54 @@ static void rtw_fw_leave_lps_state_check(struct rtw_dev *rtwdev)
 	 */
 	for (i = 0 ; i < LEAVE_LPS_TRY_CNT; i++) {
 		if (rtw_read32_mask(rtwdev, REG_TCR, BIT_PWRMGT_HWDATA_EN) == 0)
-			return;
+			return 0;
 		msleep(20);
 	}
 
-	rtw_write32_mask(rtwdev, REG_TCR, BIT_PWRMGT_HWDATA_EN, 0);
-	rtw_warn(rtwdev, "firmware failed to restore hardware setting\n");
+	return -EBUSY;
+}
+
+static  int __rtw_fw_leave_lps_check_c2h(struct rtw_dev *rtwdev)
+{
+	if (wait_for_completion_timeout(&rtwdev->lps_leave_check,
+					LEAVE_LPS_TIMEOUT))
+		return 0;
+	return -EBUSY;
+}
+
+static void rtw_fw_leave_lps_check(struct rtw_dev *rtwdev)
+{
+	bool ret = false;
+	struct rtw_fw_state *fw;
+
+	if (test_bit(RTW_FLAG_WOWLAN, rtwdev->flags))
+		fw = &rtwdev->wow_fw;
+	else
+		fw = &rtwdev->fw;
+
+	if (rtw_fw_feature_check(fw, FW_FEATURE_LPS_C2H))
+		ret = __rtw_fw_leave_lps_check_c2h(rtwdev);
+	else
+		ret = __rtw_fw_leave_lps_check_reg(rtwdev);
+
+	if (ret) {
+		rtw_write32_clr(rtwdev, REG_TCR, BIT_PWRMGT_HWDATA_EN);
+		rtw_warn(rtwdev, "firmware failed to leave lps state\n");
+		rtw_fw_dump_dbg_info(rtwdev);
+	}
+}
+
+static void rtw_fw_leave_lps_check_prepare(struct rtw_dev *rtwdev)
+{
+	struct rtw_fw_state *fw;
+
+	if (test_bit(RTW_FLAG_WOWLAN, rtwdev->flags))
+		fw = &rtwdev->wow_fw;
+	else
+		fw = &rtwdev->fw;
+
+	if (rtw_fw_feature_check(fw, FW_FEATURE_LPS_C2H))
+		reinit_completion(&rtwdev->lps_leave_check);
 }
 
 static void rtw_leave_lps_core(struct rtw_dev *rtwdev)
@@ -154,17 +192,26 @@ static void rtw_leave_lps_core(struct rtw_dev *rtwdev)
 	conf->smart_ps = 0;
 
 	rtw_hci_link_ps(rtwdev, false);
+	rtw_fw_leave_lps_check_prepare(rtwdev);
 	rtw_fw_set_pwr_mode(rtwdev);
-	rtw_fw_leave_lps_state_check(rtwdev);
+	rtw_fw_leave_lps_check(rtwdev);
 
 	clear_bit(RTW_FLAG_LEISURE_PS, rtwdev->flags);
 
 	rtw_coex_lps_notify(rtwdev, COEX_LPS_DISABLE);
 }
 
+enum rtw_lps_deep_mode rtw_get_lps_deep_mode(struct rtw_dev *rtwdev)
+{
+	if (test_bit(RTW_FLAG_WOWLAN, rtwdev->flags))
+		return rtwdev->lps_conf.wow_deep_mode;
+	else
+		return rtwdev->lps_conf.deep_mode;
+}
+
 static void __rtw_enter_lps_deep(struct rtw_dev *rtwdev)
 {
-	if (rtwdev->lps_conf.deep_mode == LPS_DEEP_MODE_NONE)
+	if (rtw_get_lps_deep_mode(rtwdev) == LPS_DEEP_MODE_NONE)
 		return;
 
 	if (!test_bit(RTW_FLAG_LEISURE_PS, rtwdev->flags)) {
@@ -173,7 +220,7 @@ static void __rtw_enter_lps_deep(struct rtw_dev *rtwdev)
 		return;
 	}
 
-	if (rtw_fw_lps_deep_mode == LPS_DEEP_MODE_PG)
+	if (rtw_get_lps_deep_mode(rtwdev) == LPS_DEEP_MODE_PG)
 		rtw_fw_set_pg_info(rtwdev);
 
 	rtw_hci_deep_ps(rtwdev, true);
@@ -252,3 +299,45 @@ void rtw_leave_lps_deep(struct rtw_dev *rtwdev)
 
 	__rtw_leave_lps_deep(rtwdev);
 }
+
+struct rtw_vif_recalc_lps_iter_data {
+	struct rtw_dev *rtwdev;
+	struct ieee80211_vif *found_vif;
+	int count;
+};
+
+static void __rtw_vif_recalc_lps(struct rtw_vif_recalc_lps_iter_data *data,
+				 struct ieee80211_vif *vif)
+{
+	if (data->count < 0)
+		return;
+
+	if (vif->type != NL80211_IFTYPE_STATION) {
+		data->count = -1;
+		return;
+	}
+
+	data->count++;
+	data->found_vif = vif;
+}
+
+static void rtw_vif_recalc_lps_iter(void *data, struct ieee80211_vif *vif)
+{
+	__rtw_vif_recalc_lps(data, vif);
+}
+
+void rtw_recalc_lps(struct rtw_dev *rtwdev, struct ieee80211_vif *new_vif)
+{
+	struct rtw_vif_recalc_lps_iter_data data = { .rtwdev = rtwdev };
+
+	if (new_vif)
+		__rtw_vif_recalc_lps(&data, new_vif);
+	rtw_iterate_vifs(rtwdev, rtw_vif_recalc_lps_iter, &data);
+
+	if (data.count == 1 && data.found_vif->cfg.ps) {
+		rtwdev->ps_enabled = true;
+	} else {
+		rtwdev->ps_enabled = false;
+		rtw_leave_lps(rtwdev);
+	}
+}