From f6cd955e1c042a00691fa80d231495024cfcf7a5 Mon Sep 17 00:00:00 2001
From: Yi-Chia Hsieh <yi-chia.hsieh@mediatek.com>
Date: Tue, 15 Nov 2022 11:03:22 +0800
Subject: [PATCH] mac80211: mtk: ageout color bitmap

Adding a periodic work which runs once per second to check BSS color.
OBSS BSS Color will be ageout if not seen for 10 seconds.
---
 include/net/cfg80211.h     |  1 +
 net/mac80211/cfg.c         | 25 +++++++++++++++++++++++++
 net/mac80211/ieee80211_i.h |  4 ++++
 net/mac80211/iface.c       |  5 +++++
 net/mac80211/rx.c          |  1 +
 5 files changed, 36 insertions(+)

diff --git a/include/net/cfg80211.h b/include/net/cfg80211.h
index e5175c2..3e826f0 100644
--- a/include/net/cfg80211.h
+++ b/include/net/cfg80211.h
@@ -308,6 +308,7 @@ struct cfg80211_he_bss_color {
 	bool enabled;
 	bool partial;
 	u64 used_color_bitmap;
+	u64 color_last_seen[64];
 };
 
 /**
diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index a7b6284..f084912 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -4425,6 +4425,31 @@ unlock:
 	sdata_unlock(sdata);
 }
 
+void ieee80211_color_aging_work(struct work_struct *work)
+{
+	struct ieee80211_sub_if_data *sdata =
+		container_of(work, struct ieee80211_sub_if_data,
+			     color_aging_work.work);
+	struct cfg80211_he_bss_color *he_bss_color = &sdata->vif.bss_conf.he_bss_color;
+	int i = 0;
+
+	sdata_lock(sdata);
+
+	for (i = 0; i < IEEE80211_BSS_COLOR_MAX; i++) {
+		/* ageout if not seen for a period */
+		if ((he_bss_color->used_color_bitmap & BIT_ULL(i)) &&
+		    time_before(he_bss_color->color_last_seen[i],
+			jiffies - IEEE80211_BSS_COLOR_AGEOUT_TIME * HZ)) {
+			he_bss_color->used_color_bitmap &= ~BIT_ULL(i);
+		}
+	}
+
+	ieee80211_queue_delayed_work(&sdata->local->hw,
+				     &sdata->color_aging_work, HZ);
+
+	sdata_unlock(sdata);
+}
+
 void ieee80211_color_change_finish(struct ieee80211_vif *vif)
 {
 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index 6edabd8..415f202 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -1033,6 +1033,8 @@ struct ieee80211_sub_if_data {
 	} debugfs;
 #endif
 
+	struct delayed_work color_aging_work;
+
 	/* must be last, dynamically sized area in this! */
 	struct ieee80211_vif vif;
 };
@@ -1811,8 +1813,11 @@ void ieee80211_csa_finalize_work(struct work_struct *work);
 int ieee80211_channel_switch(struct wiphy *wiphy, struct net_device *dev,
 			     struct cfg80211_csa_settings *params);
 
+#define IEEE80211_BSS_COLOR_AGEOUT_TIME	10
+#define IEEE80211_BSS_COLOR_MAX		64
 /* color change handling */
 void ieee80211_color_change_finalize_work(struct work_struct *work);
+void ieee80211_color_aging_work(struct work_struct *work);
 
 /* interface handling */
 #define MAC80211_SUPPORTED_FEATURES_TX	(NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM | \
diff --git a/net/mac80211/iface.c b/net/mac80211/iface.c
index 43f6cb0..7f26828 100644
--- a/net/mac80211/iface.c
+++ b/net/mac80211/iface.c
@@ -466,6 +466,8 @@ static void ieee80211_do_stop(struct ieee80211_sub_if_data *sdata, bool going_do
 	cancel_work_sync(&sdata->color_change_finalize_work);
 
 	cancel_delayed_work_sync(&sdata->dfs_cac_timer_work);
+	if (sdata->vif.type == NL80211_IFTYPE_AP)
+		cancel_delayed_work_sync(&sdata->color_aging_work);
 
 	if (sdata->wdev.cac_started) {
 		chandef = sdata->vif.bss_conf.chandef;
@@ -1804,6 +1806,9 @@ static void ieee80211_setup_sdata(struct ieee80211_sub_if_data *sdata,
 		skb_queue_head_init(&sdata->u.ap.ps.bc_buf);
 		INIT_LIST_HEAD(&sdata->u.ap.vlans);
 		sdata->vif.bss_conf.bssid = sdata->vif.addr;
+		INIT_DELAYED_WORK(&sdata->color_aging_work, ieee80211_color_aging_work);
+		ieee80211_queue_delayed_work(&sdata->local->hw,
+					     &sdata->color_aging_work, 0);
 		break;
 	case NL80211_IFTYPE_P2P_CLIENT:
 		type = NL80211_IFTYPE_STATION;
diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index 789a41b..4a6491c 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -3221,6 +3221,7 @@ ieee80211_rx_check_bss_color_collision(struct ieee80211_rx_data *rx)
 				      IEEE80211_HE_OPERATION_BSS_COLOR_MASK);
 
 		bss_conf->he_bss_color.used_color_bitmap |= BIT_ULL(color);
+		bss_conf->he_bss_color.color_last_seen[color] = jiffies;
 
 		trace_bss_color_bitmap(color,
 				       bss_conf->he_bss_color.used_color_bitmap);
-- 
2.39.0