From 7dd3aacd15e5678a617001dc329cf942ed7f4298 Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Wed, 5 Oct 2022 19:13:43 +0800
Subject: [PATCH 11/16] mac80211: mtk: fix the issue of AP and STA starting on
 DFS channel concurrently

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
---
 include/net/cfg80211.h       | 22 ++++++++++++++++++
 include/uapi/linux/nl80211.h |  2 +-
 net/mac80211/cfg.c           | 44 ++++++++++++++++++++++++++++++++++++
 net/mac80211/chan.c          |  2 +-
 net/wireless/chan.c          |  6 ++---
 net/wireless/nl80211.c       |  7 ++++++
 net/wireless/rdev-ops.h      | 16 +++++++++++++
 net/wireless/trace.h         | 15 ++++++++++++
 8 files changed, 109 insertions(+), 5 deletions(-)

diff --git a/include/net/cfg80211.h b/include/net/cfg80211.h
index f9c2cc3..84e769b 100644
--- a/include/net/cfg80211.h
+++ b/include/net/cfg80211.h
@@ -888,6 +888,24 @@ cfg80211_chandef_identical(const struct cfg80211_chan_def *chandef1,
 		chandef1->center_freq2 == chandef2->center_freq2);
 }
 
+/**
+ * cfg80211_chan_fully_overlap - check if two channel are fully overlapped
+ * @chandef1: first channel definition
+ * @chandef2: second channel definition
+ *
+ * Return: %true if the channels are valid and fully overlapped, %false otherwise.
+ */
+static inline bool
+cfg80211_chan_fully_overlap(const struct cfg80211_chan_def *chandef1,
+			    const struct cfg80211_chan_def *chandef2)
+{
+	return (chandef1->center_freq1 != 0 &&
+		chandef1->center_freq1 == chandef2->center_freq1 &&
+		chandef1->width == chandef2->width &&
+		chandef1->freq1_offset == chandef2->freq1_offset &&
+		chandef1->center_freq2 == chandef2->center_freq2);
+}
+
 /**
  * cfg80211_chandef_is_edmg - check if chandef represents an EDMG channel
  *
@@ -4642,6 +4660,8 @@ struct cfg80211_ops {
 	int	(*del_link_station)(struct wiphy *wiphy, struct net_device *dev,
 				    struct link_station_del_parameters *params);
 	void	(*skip_cac)(struct wireless_dev *wdev);
+	void	(*check_cac_skip)(struct wiphy *wiphy,
+				  struct cfg80211_chan_def *chandef);
 };
 
 /*
@@ -5847,6 +5867,8 @@ struct wireless_dev {
 		};
 	} links[IEEE80211_MLD_MAX_NUM_LINKS];
 	u16 valid_links;
+
+	bool start_disabled;
 };
 
 static inline const u8 *wdev_address(struct wireless_dev *wdev)
diff --git a/include/uapi/linux/nl80211.h b/include/uapi/linux/nl80211.h
index 1293d30..db9d0a8 100644
--- a/include/uapi/linux/nl80211.h
+++ b/include/uapi/linux/nl80211.h
@@ -3283,7 +3283,7 @@ enum nl80211_attrs {
 	NL80211_ATTR_WIPHY_ANTENNA_GAIN,
 
 	/* add attributes here, update the policy in nl80211.c */
-
+	NL80211_ATTR_START_DISABLED = 999,
 	__NL80211_ATTR_AFTER_LAST,
 	NUM_NL80211_ATTR = __NL80211_ATTR_AFTER_LAST,
 	NL80211_ATTR_MAX = __NL80211_ATTR_AFTER_LAST - 1
diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index 5718c56..9da02d9 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -4889,6 +4889,49 @@ ieee80211_skip_cac(struct wireless_dev *wdev)
 	}
 }
 
+static void
+ieee80211_check_cac_skip(struct wiphy *wiphy,
+			 struct cfg80211_chan_def *chandef)
+{
+	struct ieee80211_local *local = wiphy_priv(wiphy);
+	struct ieee80211_sub_if_data *s1;
+	struct ieee80211_sub_if_data *s2;
+	struct ieee80211_sub_if_data *sdata_sta;
+	struct ieee80211_if_managed *ifmgd;
+	struct ieee80211_channel *chan;
+	struct wireless_dev *wdev;
+	unsigned int cac_time_ms;
+
+	mutex_lock(&local->mtx);
+	/* Bypass AP's cac if there is a STA associated to the same DFS channel */
+	list_for_each_entry(s1, &local->interfaces, list) {
+		ifmgd = &s1->u.mgd;
+
+		if (s1->vif.type == NL80211_IFTYPE_STATION && ifmgd->associated)
+			sdata_sta = s1;
+		else
+			continue;
+
+		list_for_each_entry(s2, &local->interfaces, list) {
+			wdev = &s2->wdev;
+			chan = wdev->links[0].ap.chandef.chan;
+			if (chan) {
+				if (!(chan->flags & IEEE80211_CHAN_RADAR))
+					continue;
+
+				if (wdev->identifier != sdata_sta->wdev.identifier &&
+				    chan->dfs_state == NL80211_DFS_USABLE && wdev->cac_started &&
+				    cfg80211_chan_fully_overlap(&sdata_sta->vif.bss_conf.chandef,
+								&s2->vif.bss_conf.chandef)) {
+					ieee80211_skip_cac(wdev);
+					sdata_info(s2, "Skip CAC on the associated STA's chan\n");
+				}
+			}
+		}
+	}
+	mutex_unlock(&local->mtx);
+}
+
 const struct cfg80211_ops mac80211_config_ops = {
 	.add_virtual_intf = ieee80211_add_iface,
 	.del_virtual_intf = ieee80211_del_iface,
@@ -5001,4 +5044,5 @@ const struct cfg80211_ops mac80211_config_ops = {
 	.mod_link_station = ieee80211_mod_link_station,
 	.del_link_station = ieee80211_del_link_station,
 	.skip_cac = ieee80211_skip_cac,
+	.check_cac_skip = ieee80211_check_cac_skip,
 };
diff --git a/net/mac80211/chan.c b/net/mac80211/chan.c
index e72cf07..94496d7 100644
--- a/net/mac80211/chan.c
+++ b/net/mac80211/chan.c
@@ -567,7 +567,7 @@ bool ieee80211_is_radar_required(struct ieee80211_local *local)
 
 			link = rcu_dereference(sdata->link[link_id]);
 
-			if (link && link->radar_required) {
+			if (link && link->radar_required && sdata->wdev.cac_started) {
 				rcu_read_unlock();
 				return true;
 			}
diff --git a/net/wireless/chan.c b/net/wireless/chan.c
index 29b5c2f..bf21f99 100644
--- a/net/wireless/chan.c
+++ b/net/wireless/chan.c
@@ -719,16 +719,16 @@ bool cfg80211_beaconing_iface_active(struct wireless_dev *wdev)
 	case NL80211_IFTYPE_AP:
 	case NL80211_IFTYPE_P2P_GO:
 		for_each_valid_link(wdev, link) {
-			if (wdev->links[link].ap.beacon_interval)
+			if (wdev->links[link].ap.beacon_interval || wdev->start_disabled)
 				return true;
 		}
 		break;
 	case NL80211_IFTYPE_ADHOC:
-		if (wdev->u.ibss.ssid_len)
+		if (wdev->u.ibss.ssid_len || wdev->start_disabled)
 			return true;
 		break;
 	case NL80211_IFTYPE_MESH_POINT:
-		if (wdev->u.mesh.id_len)
+		if (wdev->u.mesh.id_len || wdev->start_disabled)
 			return true;
 		break;
 	case NL80211_IFTYPE_STATION:
diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index 202f802..cc8d4b2 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -789,6 +789,7 @@ static const struct nla_policy nl80211_policy[NUM_NL80211_ATTR] = {
 			NLA_POLICY_NESTED(nl80211_mbssid_config_policy),
 	[NL80211_ATTR_MBSSID_ELEMS] = { .type = NLA_NESTED },
 	[NL80211_ATTR_RADAR_BACKGROUND] = { .type = NLA_FLAG },
+	[NL80211_ATTR_START_DISABLED] = { .type = NLA_FLAG },
 	[NL80211_ATTR_AP_SETTINGS_FLAGS] = { .type = NLA_U32 },
 	[NL80211_ATTR_EHT_CAPABILITY] =
 		NLA_POLICY_BINARY_RANGE(NL80211_EHT_MIN_CAPABILITY_LEN, NL80211_EHT_MAX_CAPABILITY_LEN),
@@ -5803,6 +5804,11 @@ static int nl80211_start_ap(struct sk_buff *skb, struct genl_info *info)
 	if (wdev->links[link_id].ap.beacon_interval)
 		return -EALREADY;
 
+	if (info->attrs[NL80211_ATTR_START_DISABLED]) {
+		wdev->start_disabled = nla_get_flag(info->attrs[NL80211_ATTR_START_DISABLED]);
+		return 0;
+	}
+
 	/* these are required for START_AP */
 	if (!info->attrs[NL80211_ATTR_BEACON_INTERVAL] ||
 	    !info->attrs[NL80211_ATTR_DTIM_PERIOD] ||
@@ -9846,6 +9852,7 @@ static int nl80211_start_radar_detection(struct sk_buff *skb,
 		wdev->cac_started = true;
 		wdev->cac_start_time = jiffies;
 		wdev->cac_time_ms = cac_time_ms;
+		err = rdev_check_cac_skip(rdev, &wdev->links[0].ap.chandef);
 	}
 unlock:
 	wiphy_unlock(wiphy);
diff --git a/net/wireless/rdev-ops.h b/net/wireless/rdev-ops.h
index ce83152..ffa504a 100644
--- a/net/wireless/rdev-ops.h
+++ b/net/wireless/rdev-ops.h
@@ -1508,4 +1508,20 @@ rdev_skip_cac(struct cfg80211_registered_device *rdev,
 	return 0;
 }
 
+static inline int
+rdev_check_cac_skip(struct cfg80211_registered_device *rdev,
+		    struct cfg80211_chan_def *chandef)
+{
+	struct wiphy *wiphy = &rdev->wiphy;
+
+	if (!rdev->ops->check_cac_skip)
+		return -EOPNOTSUPP;
+
+	trace_rdev_check_cac_skip(wiphy, chandef);
+	rdev->ops->check_cac_skip(wiphy, chandef);
+	trace_rdev_return_void(wiphy);
+
+	return 0;
+}
+
 #endif /* __CFG80211_RDEV_OPS */
diff --git a/net/wireless/trace.h b/net/wireless/trace.h
index d3a98e8..72c8f0e 100644
--- a/net/wireless/trace.h
+++ b/net/wireless/trace.h
@@ -3913,6 +3913,21 @@ TRACE_EVENT(rdev_skip_cac,
 	    TP_printk(WDEV_PR_FMT, WDEV_PR_ARG)
 );
 
+TRACE_EVENT(rdev_check_cac_skip,
+	    TP_PROTO(struct wiphy *wiphy, struct cfg80211_chan_def *chandef),
+
+	    TP_ARGS(wiphy, chandef),
+
+	    TP_STRUCT__entry(WIPHY_ENTRY
+			     CHAN_DEF_ENTRY),
+
+	    TP_fast_assign(WIPHY_ASSIGN;
+			   CHAN_DEF_ASSIGN(chandef)),
+
+	    TP_printk(WIPHY_PR_FMT ", " CHAN_DEF_PR_FMT,
+		      WIPHY_PR_ARG, CHAN_DEF_PR_ARG)
+);
+
 #endif /* !__RDEV_OPS_TRACE || TRACE_HEADER_MULTI_READ */
 
 #undef TRACE_INCLUDE_PATH
-- 
2.25.1