From a58aa02645052470e6bacb9e6c1eb7adfb9fc0d8 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] mac80211: 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       | 21 ++++++++++++++++
 include/uapi/linux/nl80211.h |  2 +-
 net/mac80211/cfg.c           | 48 ++++++++++++++++++++++++++++++++++++
 net/mac80211/chan.c          |  2 +-
 net/wireless/chan.c          |  6 ++---
 net/wireless/nl80211.c       |  8 ++++++
 net/wireless/rdev-ops.h      | 16 ++++++++++++
 net/wireless/trace.h         | 15 +++++++++++
 8 files changed, 113 insertions(+), 5 deletions(-)

diff --git a/include/net/cfg80211.h b/include/net/cfg80211.h
index f594914..0af069d 100644
--- a/include/net/cfg80211.h
+++ b/include/net/cfg80211.h
@@ -800,6 +800,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
  *
@@ -4401,6 +4419,8 @@ struct cfg80211_ops {
 				struct cfg80211_color_change_settings *params);
 	int	(*set_radar_background)(struct wiphy *wiphy,
 					struct cfg80211_chan_def *chandef);
+	void	(*check_cac_skip)(struct wiphy *wiphy,
+				  struct cfg80211_chan_def *chandef);
 };
 
 /*
@@ -5554,6 +5574,7 @@ struct wireless_dev {
 	struct work_struct pmsr_free_wk;
 
 	unsigned long unprot_beacon_reported;
+	bool start_disabled;
 };
 
 static inline u8 *wdev_address(struct wireless_dev *wdev)
diff --git a/include/uapi/linux/nl80211.h b/include/uapi/linux/nl80211.h
index e674aa7..ada8288 100644
--- a/include/uapi/linux/nl80211.h
+++ b/include/uapi/linux/nl80211.h
@@ -3129,7 +3129,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 bf71594..73fb432 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -4504,6 +4504,53 @@ ieee80211_set_radar_background(struct wiphy *wiphy,
 	return local->ops->set_radar_background(&local->hw, chandef);
 }
 
+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->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)) {
+					cac_time_ms = wdev->cac_time_ms;
+					wdev->cac_start_time = jiffies -
+							       msecs_to_jiffies(cac_time_ms + 1);
+					cfg80211_cac_event(wdev->netdev, &wdev->chandef,
+							   NL80211_RADAR_CAC_FINISHED, GFP_KERNEL);
+					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,
@@ -4610,4 +4657,5 @@ const struct cfg80211_ops mac80211_config_ops = {
 	.set_sar_specs = ieee80211_set_sar_specs,
 	.color_change = ieee80211_color_change,
 	.set_radar_background = ieee80211_set_radar_background,
+	.check_cac_skip = ieee80211_check_cac_skip,
 };
diff --git a/net/mac80211/chan.c b/net/mac80211/chan.c
index 63e15f5..5e57e4a 100644
--- a/net/mac80211/chan.c
+++ b/net/mac80211/chan.c
@@ -505,7 +505,7 @@ bool ieee80211_is_radar_required(struct ieee80211_local *local)
 
 	rcu_read_lock();
 	list_for_each_entry_rcu(sdata, &local->interfaces, list) {
-		if (sdata->radar_required) {
+		if (sdata->radar_required && sdata->wdev.cac_started) {
 			rcu_read_unlock();
 			return true;
 		}
diff --git a/net/wireless/chan.c b/net/wireless/chan.c
index 5f50ac4..067ed79 100644
--- a/net/wireless/chan.c
+++ b/net/wireless/chan.c
@@ -664,13 +664,13 @@ bool cfg80211_beaconing_iface_active(struct wireless_dev *wdev)
 	switch (wdev->iftype) {
 	case NL80211_IFTYPE_AP:
 	case NL80211_IFTYPE_P2P_GO:
-		active = wdev->beacon_interval != 0;
+		active = wdev->beacon_interval != 0 || wdev->start_disabled;
 		break;
 	case NL80211_IFTYPE_ADHOC:
-		active = wdev->ssid_len != 0;
+		active = wdev->ssid_len != 0 || wdev->start_disabled;
 		break;
 	case NL80211_IFTYPE_MESH_POINT:
-		active = wdev->mesh_id_len != 0;
+		active = wdev->mesh_id_len != 0 || wdev->start_disabled;
 		break;
 	case NL80211_IFTYPE_STATION:
 	case NL80211_IFTYPE_OCB:
diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index a20aba5..8dc928d 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -803,6 +803,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_WIPHY_ANTENNA_GAIN] = { .type = NLA_U32 },
 };
 
@@ -5547,6 +5548,12 @@ static int nl80211_start_ap(struct sk_buff *skb, struct genl_info *info)
 
 	memset(&params, 0, sizeof(params));
 
+	if (info->attrs[NL80211_ATTR_START_DISABLED]) {
+		wdev->start_disabled = nla_get_flag(info->attrs[NL80211_ATTR_START_DISABLED]);
+		err = 0;
+		goto out;
+	}
+
 	/* these are required for START_AP */
 	if (!info->attrs[NL80211_ATTR_BEACON_INTERVAL] ||
 	    !info->attrs[NL80211_ATTR_DTIM_PERIOD] ||
@@ -9393,6 +9400,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->chandef);
 	}
 unlock:
 	wiphy_unlock(wiphy);
diff --git a/net/wireless/rdev-ops.h b/net/wireless/rdev-ops.h
index 8555468..e7d0064 100644
--- a/net/wireless/rdev-ops.h
+++ b/net/wireless/rdev-ops.h
@@ -1398,4 +1398,20 @@ rdev_set_radar_background(struct cfg80211_registered_device *rdev,
 	return ret;
 }
 
+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 97a2937..e3f1c79 100644
--- a/net/wireless/trace.h
+++ b/net/wireless/trace.h
@@ -3665,6 +3665,21 @@ TRACE_EVENT(rdev_set_radar_background,
 		  WIPHY_PR_ARG, CHAN_DEF_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.18.0