recipes-wifi/linux-mt76/files/patches-3.x/0109-wifi-mt76-mt7996-add-beacon-monitoring-in-driver-for.patch

From 184ecc9c9c16bbc9b93a69c994194d8bf1e41e72 Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Tue, 20 Feb 2024 10:08:01 +0800
Subject: [PATCH 109/116] wifi: mt76: mt7996: add beacon monitoring in driver
 for mlo

Add beacon monitoring in driver since mac80211 does not
support connect monitoring if WIPHY_FLAG_SUPPORTS_MLO is set.
(IEEE80211_HW_CONNECTION_MONITOR should be set)

CR-Id: WCNCR00274293
Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Change-Id: I52211987abd6309bbb23dc648af3f377adf8982a
---
 mt7996/mac.c    | 56 +++++++++++++++++++++++++++++++++++++++++++++++++
 mt7996/main.c   | 50 +++++++++++++++++++++++++++++++++++++++++++
 mt7996/mt7996.h |  7 +++++++
 3 files changed, 113 insertions(+)

diff --git a/mt7996/mac.c b/mt7996/mac.c
index 5967b6aeb..2a45fc03d 100644
--- a/mt7996/mac.c
+++ b/mt7996/mac.c
@@ -565,6 +565,21 @@ mt7996_mac_fill_rx(struct mt7996_dev *dev, enum mt76_rxq_id q,
 			 */
 			if (ieee80211_has_a4(fc) && is_mesh && status->amsdu)
 				*qos &= ~IEEE80211_QOS_CTL_A_MSDU_PRESENT;
+		} else if (ieee80211_is_beacon(fc)) {
+			struct ieee80211_hw *hw = phy->mt76->hw;
+			struct ieee80211_sta *sta;
+			struct mt7996_sta *msta;
+			unsigned int link_id;
+
+			sta = ieee80211_find_sta_by_link_addrs(hw, hdr->addr2, NULL, &link_id);
+			if (!sta)
+				sta = ieee80211_find_sta_by_ifaddr(hw, hdr->addr2, NULL);
+
+			if (sta) {
+				msta = (struct mt7996_sta *)sta->drv_priv;
+				if (msta && msta->vif)
+					msta->vif->beacon_received_time[band_idx] = jiffies;
+			}
 		}
 #ifdef CONFIG_MTK_VENDOR
 		if (phy->amnt_ctrl.enable && !ieee80211_is_beacon(fc))
@@ -2956,6 +2971,47 @@ void mt7996_scan_work(struct work_struct *work)
 	ieee80211_queue_delayed_work(hw, &phy->scan_work, duration);
 }
 
+void mt7996_beacon_mon_work(struct work_struct *work)
+{
+	struct mt7996_vif *mvif = container_of(work, struct mt7996_vif, beacon_mon_work.work);
+	struct ieee80211_vif *vif = container_of((void *)mvif, struct ieee80211_vif, drv_priv);
+	struct ieee80211_hw *hw = mvif->hw;
+	struct mt7996_dev *dev = mt7996_hw_dev(hw);
+	unsigned long next_time = ULONG_MAX, valid_links = vif->valid_links ?: BIT(0);
+	unsigned int link_id;
+
+	mutex_lock(&dev->mt76.mutex);
+
+	for_each_set_bit(link_id, &valid_links, IEEE80211_MLD_MAX_NUM_LINKS) {
+		struct ieee80211_bss_conf *conf;
+		struct mt7996_bss_conf *mconf;
+		struct mt7996_phy *phy;
+		unsigned long timeout, loss_duration;
+
+		conf = link_conf_dereference_protected(vif, link_id);
+		mconf = mconf_dereference_protected(mvif, link_id);
+		if (!conf || !mconf)
+			continue;
+
+		phy = mconf->phy;
+		loss_duration = msecs_to_jiffies(MT7996_MAX_BEACON_LOSS * conf->beacon_int);
+		timeout = mvif->beacon_received_time[phy->mt76->band_idx] + loss_duration;
+		if (time_after_eq(jiffies, timeout)) {
+			mutex_unlock(&dev->mt76.mutex);
+			wiphy_info(hw->wiphy,
+				   "link %d: detected beacon loss, start disconnecting\n",
+				   link_id);
+			/* TODO: disconnect single link & handle link reconfiguration for MLD */
+			ieee80211_connection_loss(vif);
+			return;
+		}
+		next_time = min(next_time, timeout - jiffies);
+	}
+	mutex_unlock(&dev->mt76.mutex);
+
+	ieee80211_queue_delayed_work(hw, &mvif->beacon_mon_work, next_time);
+}
+
 void mt7996_get_hw(struct mt76_dev *dev, struct mt76_wcid *wcid, u8 phy_idx,
 		   struct ieee80211_hw **hw)
 {
diff --git a/mt7996/main.c b/mt7996/main.c
index 6943b28ba..e37f0c013 100644
--- a/mt7996/main.c
+++ b/mt7996/main.c
@@ -450,6 +450,7 @@ static int mt7996_add_interface(struct ieee80211_hw *hw,
 	    is_zero_ether_addr(vif->addr))
 		phy->monitor_vif = vif;
 
+	INIT_DELAYED_WORK(&mvif->beacon_mon_work, mt7996_beacon_mon_work);
 	mvif->dev = dev;
 	mvif->hw = hw;
 	mvif->sta.vif = mvif;
@@ -2565,6 +2566,54 @@ out:
 	return ret;
 }
 
+static void
+mt7996_event_callback(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		      const struct ieee80211_event *event)
+{
+	struct mt7996_dev *dev = mt7996_hw_dev(hw);
+	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
+
+	mutex_lock(&dev->mt76.mutex);
+
+	switch (event->type) {
+	case MLME_EVENT:
+		if (event->u.mlme.data == ASSOC_EVENT &&
+		    event->u.mlme.status == MLME_SUCCESS) {
+			struct ieee80211_bss_conf *conf;
+			struct mt7996_bss_conf *mconf;
+			struct mt7996_phy *phy;
+			unsigned long cur, valid_links = vif->valid_links ?: BIT(0);
+			unsigned int link_id;
+			int next_time = INT_MAX;
+
+			cur = jiffies;
+			for_each_set_bit(link_id, &valid_links, IEEE80211_MLD_MAX_NUM_LINKS) {
+				conf = link_conf_dereference_protected(vif, link_id);
+				mconf = mconf_dereference_protected(mvif, link_id);
+				if (conf && mconf) {
+					phy = mconf->phy;
+					mvif->beacon_received_time[phy->mt76->band_idx] = cur;
+					next_time = min(next_time,
+							MT7996_MAX_BEACON_LOSS *
+							conf->beacon_int);
+				}
+			}
+
+			ieee80211_queue_delayed_work(hw, &mvif->beacon_mon_work,
+						     msecs_to_jiffies(next_time));
+			break;
+		}
+
+		cancel_delayed_work_sync(&mvif->beacon_mon_work);
+		break;
+	default:
+		break;
+	}
+
+	mutex_unlock(&dev->mt76.mutex);
+	return;
+}
+
 const struct ieee80211_ops mt7996_ops = {
 	.tx = mt7996_tx,
 	.start = mt7996_start,
@@ -2617,6 +2666,7 @@ const struct ieee80211_ops mt7996_ops = {
 	.net_fill_forward_path = mt7996_net_fill_forward_path,
 	.net_setup_tc = mt76_wed_net_setup_tc,
 #endif
+	.event_callback = mt7996_event_callback,
 	.add_chanctx = mt7996_add_chanctx,
 	.remove_chanctx = mt7996_remove_chanctx,
 	.change_chanctx = mt7996_change_chanctx,
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index dc3cacc3a..0412d7375 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -128,6 +128,8 @@
 
 #define to_rssi(field, rcpi)	((FIELD_GET(field, rcpi) - 220) / 2)
 
+#define MT7996_MAX_BEACON_LOSS		50
+
 struct mt7996_vif;
 struct mt7996_sta;
 struct mt7996_dfs_pulse;
@@ -360,6 +362,10 @@ struct mt7996_vif {
 	u8 mld_remap_id;
 
 	u8 band_to_link[__MT_MAX_BAND];
+
+	/* for beacon monitoring */
+	struct delayed_work beacon_mon_work;
+	unsigned long beacon_received_time[__MT_MAX_BAND];
 };
 
 /* crash-dump */
@@ -1113,6 +1119,7 @@ bool mt7996_rx_check(struct mt76_dev *mdev, void *data, int len);
 void mt7996_stats_work(struct work_struct *work);
 void mt7996_scan_work(struct work_struct *work);
 void mt7996_scan_complete(struct mt7996_phy *phy, bool aborted);
+void mt7996_beacon_mon_work(struct work_struct *work);
 int mt76_dfs_start_rdd(struct mt7996_dev *dev, bool force);
 int mt7996_dfs_init_radar_detector(struct mt7996_phy *phy);
 void mt7996_set_stream_he_eht_caps(struct mt7996_phy *phy);
-- 
2.39.2