1 files changed, 150 insertions, 3 deletions

diff --git a/drivers/net/wireless/realtek/rtw89/ps.c b/drivers/net/wireless/realtek/rtw89/ps.c
index 96ea04d90cd3..8e4fe73e7d77 100644
--- a/drivers/net/wireless/realtek/rtw89/ps.c
+++ b/drivers/net/wireless/realtek/rtw89/ps.c
@@ -113,7 +113,7 @@ static void __rtw89_leave_lps(struct rtw89_dev *rtwdev,
 
 void rtw89_leave_ps_mode(struct rtw89_dev *rtwdev)
 {
-	lockdep_assert_held(&rtwdev->mutex);
+	lockdep_assert_wiphy(rtwdev->hw->wiphy);
 
 	__rtw89_leave_ps_mode(rtwdev);
 }
@@ -125,7 +125,7 @@ void rtw89_enter_lps(struct rtw89_dev *rtwdev, struct rtw89_vif *rtwvif,
 	bool can_ps_mode = true;
 	unsigned int link_id;
 
-	lockdep_assert_held(&rtwdev->mutex);
+	lockdep_assert_wiphy(rtwdev->hw->wiphy);
 
 	if (test_and_set_bit(RTW89_FLAG_LEISURE_PS, rtwdev->flags))
 		return;
@@ -162,7 +162,7 @@ void rtw89_leave_lps(struct rtw89_dev *rtwdev)
 	struct rtw89_vif *rtwvif;
 	unsigned int link_id;
 
-	lockdep_assert_held(&rtwdev->mutex);
+	lockdep_assert_wiphy(rtwdev->hw->wiphy);
 
 	if (!test_and_clear_bit(RTW89_FLAG_LEISURE_PS, rtwdev->flags))
 		return;
@@ -382,3 +382,150 @@ u8 rtw89_p2p_noa_fetch(struct rtw89_vif_link *rtwvif_link, void **data)
 	tail = ie->noa_desc + setter->noa_count;
 	return tail - *data;
 }
+
+static void rtw89_ps_noa_once_set_work(struct wiphy *wiphy, struct wiphy_work *work)
+{
+	struct rtw89_ps_noa_once_handler *noa_once =
+		container_of(work, struct rtw89_ps_noa_once_handler, set_work.work);
+
+	lockdep_assert_wiphy(wiphy);
+
+	noa_once->in_duration = true;
+}
+
+static void rtw89_ps_noa_once_clr_work(struct wiphy *wiphy, struct wiphy_work *work)
+{
+	struct rtw89_ps_noa_once_handler *noa_once =
+		container_of(work, struct rtw89_ps_noa_once_handler, clr_work.work);
+	struct rtw89_vif_link *rtwvif_link =
+		container_of(noa_once, struct rtw89_vif_link, noa_once);
+	struct rtw89_dev *rtwdev = rtwvif_link->rtwvif->rtwdev;
+
+	lockdep_assert_wiphy(wiphy);
+
+	rtw89_fw_h2c_set_bcn_fltr_cfg(rtwdev, rtwvif_link, true);
+	noa_once->in_duration = false;
+}
+
+void rtw89_p2p_noa_once_init(struct rtw89_vif_link *rtwvif_link)
+{
+	struct rtw89_ps_noa_once_handler *noa_once = &rtwvif_link->noa_once;
+
+	noa_once->in_duration = false;
+	noa_once->tsf_begin = 0;
+	noa_once->tsf_end = 0;
+
+	wiphy_delayed_work_init(&noa_once->set_work, rtw89_ps_noa_once_set_work);
+	wiphy_delayed_work_init(&noa_once->clr_work, rtw89_ps_noa_once_clr_work);
+}
+
+static void rtw89_p2p_noa_once_cancel(struct rtw89_vif_link *rtwvif_link)
+{
+	struct rtw89_ps_noa_once_handler *noa_once = &rtwvif_link->noa_once;
+	struct rtw89_dev *rtwdev = rtwvif_link->rtwvif->rtwdev;
+	struct wiphy *wiphy = rtwdev->hw->wiphy;
+
+	wiphy_delayed_work_cancel(wiphy, &noa_once->set_work);
+	wiphy_delayed_work_cancel(wiphy, &noa_once->clr_work);
+}
+
+void rtw89_p2p_noa_once_deinit(struct rtw89_vif_link *rtwvif_link)
+{
+	rtw89_p2p_noa_once_cancel(rtwvif_link);
+	rtw89_p2p_noa_once_init(rtwvif_link);
+}
+
+void rtw89_p2p_noa_once_recalc(struct rtw89_vif_link *rtwvif_link)
+{
+	struct rtw89_ps_noa_once_handler *noa_once = &rtwvif_link->noa_once;
+	struct rtw89_dev *rtwdev = rtwvif_link->rtwvif->rtwdev;
+	const struct ieee80211_p2p_noa_desc *noa_desc;
+	struct wiphy *wiphy = rtwdev->hw->wiphy;
+	struct ieee80211_bss_conf *bss_conf;
+	u64 tsf_begin = U64_MAX, tsf_end;
+	u64 set_delay_us = 0;
+	u64 clr_delay_us = 0;
+	u32 start_time;
+	u32 interval;
+	u32 duration;
+	u64 tsf;
+	int ret;
+	int i;
+
+	lockdep_assert_wiphy(wiphy);
+
+	ret = rtw89_mac_port_get_tsf(rtwdev, rtwvif_link, &tsf);
+	if (ret) {
+		rtw89_warn(rtwdev, "%s: failed to get tsf\n", __func__);
+		return;
+	}
+
+	rcu_read_lock();
+
+	bss_conf = rtw89_vif_rcu_dereference_link(rtwvif_link, true);
+
+	for (i = 0; i < ARRAY_SIZE(bss_conf->p2p_noa_attr.desc); i++) {
+		bool first = tsf_begin == U64_MAX;
+		u64 tmp;
+
+		noa_desc = &bss_conf->p2p_noa_attr.desc[i];
+		if (noa_desc->count == 0 || noa_desc->count == 255)
+			continue;
+
+		start_time = le32_to_cpu(noa_desc->start_time);
+		interval = le32_to_cpu(noa_desc->interval);
+		duration = le32_to_cpu(noa_desc->duration);
+
+		if (unlikely(duration == 0 ||
+			     (noa_desc->count > 1 && interval == 0)))
+			continue;
+
+		tmp = start_time + interval * (noa_desc->count - 1) + duration;
+		tmp = (tsf & GENMASK_ULL(63, 32)) + tmp;
+		if (unlikely(tmp <= tsf))
+			continue;
+		tsf_end = first ? tmp : max(tsf_end, tmp);
+
+		tmp = (tsf & GENMASK_ULL(63, 32)) | start_time;
+		tsf_begin = first ? tmp : min(tsf_begin, tmp);
+	}
+
+	rcu_read_unlock();
+
+	if (tsf_begin == U64_MAX)
+		return;
+
+	rtw89_p2p_noa_once_cancel(rtwvif_link);
+
+	if (noa_once->tsf_end > tsf) {
+		tsf_begin = min(tsf_begin, noa_once->tsf_begin);
+		tsf_end = max(tsf_end, noa_once->tsf_end);
+	}
+
+	clr_delay_us = min_t(u64, tsf_end - tsf, UINT_MAX);
+
+	if (tsf_begin <= tsf) {
+		noa_once->in_duration = true;
+		goto out;
+	}
+
+	set_delay_us = tsf_begin - tsf;
+	if (unlikely(set_delay_us > UINT_MAX)) {
+		rtw89_warn(rtwdev, "%s: unhandled begin\n", __func__);
+		set_delay_us = 0;
+		clr_delay_us = 0;
+		rtw89_fw_h2c_set_bcn_fltr_cfg(rtwdev, rtwvif_link, true);
+		noa_once->in_duration = false;
+	}
+
+out:
+	if (set_delay_us)
+		wiphy_delayed_work_queue(wiphy, &noa_once->set_work,
+					 usecs_to_jiffies(set_delay_us));
+	if (clr_delay_us)
+		wiphy_delayed_work_queue(wiphy, &noa_once->clr_work,
+					 usecs_to_jiffies(clr_delay_us));
+
+	noa_once->tsf_begin = tsf_begin;
+	noa_once->tsf_end = tsf_end;
+}