From 60adbabe8df3bdbab9bd3c2146f19b4c83b69def Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Thu, 22 Sep 2022 14:27:41 +0800
Subject: [PATCH 11/61] mtk: cfg80211: implement DFS status show, cac and nop
 skip command via debugfs

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>

Refactor DFS debugfs command for MLO

CR-Id: WCNCR00274293
Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Change-Id: I5e50523684bdc5086a18d36177e0d81530ba0e3f
---
 include/net/cfg80211.h  |   1 +
 net/mac80211/cfg.c      |  25 +++
 net/wireless/core.h     |   3 +
 net/wireless/debugfs.c  | 326 +++++++++++++++++++++++++++++++++++++++-
 net/wireless/mlme.c     |   6 +
 net/wireless/rdev-ops.h |  14 ++
 net/wireless/trace.h    |  13 ++
 7 files changed, 381 insertions(+), 7 deletions(-)

diff --git a/include/net/cfg80211.h b/include/net/cfg80211.h
index d987b62..c55028f 100644
--- a/include/net/cfg80211.h
+++ b/include/net/cfg80211.h
@@ -4965,6 +4965,7 @@ struct cfg80211_ops {
 				    struct cfg80211_set_hw_timestamp *hwts);
 	int	(*set_ttlm)(struct wiphy *wiphy, struct net_device *dev,
 			    struct cfg80211_ttlm_params *params);
+	void	(*skip_cac)(struct wireless_dev *wdev, unsigned int link_id);
 };
 
 /*
diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index 72e64be..3f4c129 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -5055,6 +5055,30 @@ ieee80211_set_ttlm(struct wiphy *wiphy, struct net_device *dev,
 	return ieee80211_req_neg_ttlm(sdata, params);
 }
 
+static void
+ieee80211_skip_cac(struct wireless_dev *wdev, unsigned int link_id)
+{
+	struct net_device *dev = wdev->netdev;
+	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
+	struct ieee80211_link_data *link;
+	unsigned int cac_time_ms;
+
+	link = sdata_dereference(sdata->link[link_id], sdata);
+	if (!link)
+		return;
+
+	wiphy_delayed_work_cancel(sdata->local->hw.wiphy,
+				  &link->dfs_cac_timer_work);
+	if (wdev->cac_started) {
+		ieee80211_link_release_channel(link);
+		cac_time_ms = wdev->cac_time_ms;
+		wdev->cac_start_time = jiffies -
+				       msecs_to_jiffies(cac_time_ms + 1);
+		cfg80211_cac_event(wdev->netdev, &link->conf->chanreq.oper,
+				   NL80211_RADAR_CAC_FINISHED, GFP_KERNEL);
+	}
+}
+
 const struct cfg80211_ops mac80211_config_ops = {
 	.add_virtual_intf = ieee80211_add_iface,
 	.del_virtual_intf = ieee80211_del_iface,
@@ -5168,4 +5192,5 @@ const struct cfg80211_ops mac80211_config_ops = {
 	.del_link_station = ieee80211_del_link_station,
 	.set_hw_timestamp = ieee80211_set_hw_timestamp,
 	.set_ttlm = ieee80211_set_ttlm,
+	.skip_cac = ieee80211_skip_cac,
 };
diff --git a/net/wireless/core.h b/net/wireless/core.h
index 7bef6b0..ae6b7fa 100644
--- a/net/wireless/core.h
+++ b/net/wireless/core.h
@@ -86,6 +86,9 @@ struct cfg80211_registered_device {
 
 	struct wireless_dev *background_radar_wdev;
 	struct cfg80211_chan_def background_radar_chandef;
+	bool background_cac_started;
+	unsigned long background_cac_start_time;
+	unsigned int background_cac_time_ms;
 	struct delayed_work background_cac_done_wk;
 	struct work_struct background_cac_abort_wk;
 
diff --git a/net/wireless/debugfs.c b/net/wireless/debugfs.c
index 7c59a25..a246b2c 100644
--- a/net/wireless/debugfs.c
+++ b/net/wireless/debugfs.c
@@ -10,6 +10,7 @@
 #include <linux/slab.h>
 #include "core.h"
 #include "debugfs.h"
+#include "rdev-ops.h"
 
 #define DEBUGFS_READONLY_FILE(name, buflen, fmt, value...)		\
 static ssize_t name## _read(struct file *file, char __user *userbuf,	\
@@ -97,18 +98,329 @@ static const struct file_operations ht40allow_map_ops = {
 	.llseek = default_llseek,
 };
 
-#define DEBUGFS_ADD(name)						\
-	debugfs_create_file(#name, 0444, phyd, &rdev->wiphy, &name## _ops)
+static int dfs_print_chan(struct ieee80211_channel *chan, int remain_time, int wait_time,
+			  char *buf, int buf_size, int offset, bool is_background)
+{
+	if (WARN_ON(offset > buf_size))
+		return 0;
+
+	if (chan->dfs_state == NL80211_DFS_UNAVAILABLE) {
+		offset += scnprintf(buf + offset, buf_size - offset,
+				    "	Channel = %d, DFS_state = Unavailable",
+				    chan->hw_value);
+		if (remain_time > 0)
+			offset += scnprintf(buf + offset, buf_size - offset,
+					    ", Non-occupancy Remain Time = %d / %d [sec]",
+					    remain_time, wait_time);
+		else
+			offset += scnprintf(buf + offset, buf_size - offset,
+					    ", Changing state...");
+	} else if (chan->dfs_state == NL80211_DFS_USABLE) {
+		offset += scnprintf(buf + offset, buf_size - offset,
+				    "	Channel = %d, DFS_state = Usable",
+				    chan->hw_value);
+		if (remain_time > 0)
+			offset += scnprintf(buf + offset, buf_size - offset,
+					    ", CAC Remain Time = %d / %d [sec]",
+					    remain_time, wait_time);
+	} else if (chan->dfs_state == NL80211_DFS_AVAILABLE) {
+		offset += scnprintf(buf + offset, buf_size - offset,
+				    "	Channel = %d, DFS_state = Available",
+				    chan->hw_value);
+	} else {
+		offset += scnprintf(buf + offset, buf_size - offset,
+				    "	Channel = %d, DFS_state = Unknown",
+				    chan->hw_value);
+	}
+
+	if (is_background)
+		offset += scnprintf(buf + offset, buf_size - offset,
+				    " (background chain)");
+	offset += scnprintf(buf + offset, buf_size - offset, "\n");
+
+	return offset;
+}
+
+static int dfs_status_read_wdev(struct wiphy *wiphy, struct wireless_dev *wdev, char *buf,
+				unsigned int buf_size, unsigned int offset)
+{
+	struct cfg80211_registered_device *rdev = wiphy_to_rdev(wiphy);
+	struct cfg80211_chan_def *chandef;
+	struct cfg80211_chan_def *background_chandef = &rdev->background_radar_chandef;
+	enum nl80211_band band;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *chan;
+	unsigned long jiffies_passed;
+	unsigned int link_id;
+	int i, remain_time = 0, wait_time_ms = 0;
+	bool is_background;
+
+	for (band = 0; band < NUM_NL80211_BANDS; band++)
+		if (wiphy->bands[band] &&
+		    wiphy->bands[band]->band == NL80211_BAND_5GHZ)
+			sband = wiphy->bands[band];
+
+	if (!sband) {
+		offset += scnprintf(buf + offset, buf_size - offset, "No 5G band\n");
+		return offset;
+	}
+
+	for_each_valid_link(wdev, link_id) {
+		chandef = wdev_chandef(wdev, link_id);
+		if (!chandef || !chandef->chan ||
+		    chandef->chan->band != NL80211_BAND_5GHZ)
+			continue;
+
+		offset += scnprintf(buf + offset, buf_size - offset,
+				    "Link %d DFS channel:\n", link_id);
+		for (i = 0; i < sband->n_channels; i++) {
+			is_background = false;
+			chan = &sband->channels[i];
+
+			if (!(chan->flags & IEEE80211_CHAN_RADAR))
+				continue;
+
+			if (chan->dfs_state == NL80211_DFS_UNAVAILABLE) {
+				jiffies_passed = jiffies - chan->dfs_state_entered;
+				wait_time_ms = IEEE80211_DFS_MIN_NOP_TIME_MS;
+				remain_time = (wait_time_ms - jiffies_to_msecs(jiffies_passed));
+				if (remain_time > wait_time_ms)
+					remain_time = 0;
+			} else if (chan->dfs_state == NL80211_DFS_USABLE) {
+				if (wdev->cac_started &&
+				    cfg80211_is_sub_chan(chandef, chan, false)) {
+					jiffies_passed = jiffies - wdev->cac_start_time;
+					wait_time_ms = wdev->cac_time_ms;
+					remain_time = (wait_time_ms -
+						       jiffies_to_msecs(jiffies_passed));
+				}
+
+				if (rdev->background_radar_wdev == wdev &&
+				    rdev->background_cac_started &&
+				    cfg80211_is_sub_chan(background_chandef, chan, false)) {
+					jiffies_passed = jiffies - rdev->background_cac_start_time;
+					wait_time_ms = rdev->background_cac_time_ms;
+					remain_time = (wait_time_ms -
+						       jiffies_to_msecs(jiffies_passed));
+					is_background = true;
+				}
+
+				if (remain_time > wait_time_ms)
+					remain_time = 0;
+
+			} else {
+				if (rdev->background_radar_wdev == wdev &&
+				    cfg80211_is_sub_chan(background_chandef, chan, false))
+					is_background = true;
+			}
+
+			offset = dfs_print_chan(chan, remain_time / 1000, wait_time_ms / 1000,
+						buf, buf_size, offset, is_background);
+			remain_time = 0;
+		}
+	}
+
+	return offset;
+}
+
+static ssize_t dfs_status_read(struct file *file, char __user *user_buf,
+			       size_t count, loff_t *ppos)
+{
+	struct wiphy *wiphy = file->private_data;
+	struct wireless_dev *wdev;
+	char *buf;
+	unsigned int offset = 0, buf_size = PAGE_SIZE, r;
+	const char * const iftype_str[] = {
+		[NL80211_IFTYPE_UNSPECIFIED] = "unspecified",
+		[NL80211_IFTYPE_ADHOC] = "adhoc",
+		[NL80211_IFTYPE_STATION] = "station",
+		[NL80211_IFTYPE_AP] = "ap",
+		[NL80211_IFTYPE_AP_VLAN] = "ap vlan",
+		[NL80211_IFTYPE_WDS] = "wds",
+		[NL80211_IFTYPE_MONITOR] = "monitor",
+		[NL80211_IFTYPE_MESH_POINT] = "mesh point",
+		[NL80211_IFTYPE_P2P_CLIENT] = "p2p client",
+		[NL80211_IFTYPE_P2P_GO] = "p2p go",
+		[NL80211_IFTYPE_P2P_DEVICE] = "p2p device",
+		[NL80211_IFTYPE_OCB] = "ocb",
+		[NL80211_IFTYPE_NAN] = "nan",
+	};
+
+	buf = kzalloc(buf_size, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	list_for_each_entry(wdev, &wiphy->wdev_list, list) {
+		offset += scnprintf(buf + offset, buf_size - offset,
+				    "wdev 0x%x\n"
+				    "interface type %s\n",
+				    wdev->identifier, iftype_str[wdev->iftype]);
+		offset = dfs_status_read_wdev(wiphy, wdev, buf, buf_size, offset);
+	}
+
+	r = simple_read_from_buffer(user_buf, count, ppos, buf, offset);
+
+	kfree(buf);
+
+	return r;
+}
+
+static const struct file_operations dfs_status_ops = {
+	.read = dfs_status_read,
+	.open = simple_open,
+	.llseek = default_llseek,
+};
+
+static int
+dfs_nop_skip(void *data, u64 val)
+{
+	struct wiphy *wiphy = data;
+	struct cfg80211_registered_device *rdev = wiphy_to_rdev(wiphy);
+	bool en = !!val;
+	enum nl80211_band band;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *chan;
+	u32 nop_time = IEEE80211_DFS_MIN_NOP_TIME_MS;
+	int i;
+
+	if (!en)
+		return 0;
+
+	for (band = 0; band < NUM_NL80211_BANDS; band++) {
+		sband = wiphy->bands[band];
+		if (!sband)
+			continue;
+		for (i = 0; i < sband->n_channels; i++) {
+			chan = &sband->channels[i];
+
+			if (!(chan->flags & IEEE80211_CHAN_RADAR))
+				continue;
+
+			if (chan->dfs_state == NL80211_DFS_UNAVAILABLE) {
+				// Let current jiffies > dfs_state_entered_jiffies + NOP time
+				chan->dfs_state_entered = jiffies -
+						       msecs_to_jiffies(nop_time + 1);
+			}
+		}
+	}
+
+	cfg80211_sched_dfs_chan_update(rdev);
+
+	return 0;
+}
+
+DEFINE_DEBUGFS_ATTRIBUTE(dfs_skip_nop_ops, NULL,
+			 dfs_nop_skip, "0x%08llx\n");
+
+static int
+dfs_cac_skip(void *data, u64 val)
+{
+#define CAC_SKIP_MASK			BIT(0)
+#define CAC_SKIP_BACKGROUND_MASK	BIT(1)
+	struct wiphy *wiphy = data;
+	struct cfg80211_registered_device *rdev = wiphy_to_rdev(wiphy);
+	struct wireless_dev *wdev;
+	struct cfg80211_chan_def *c;
+	unsigned int link_id, skip_mode = val;
+	unsigned long cac_time;
+
+	if (!skip_mode || skip_mode > (CAC_SKIP_MASK | CAC_SKIP_BACKGROUND_MASK))
+		return 0;
+
+	list_for_each_entry(wdev, &wiphy->wdev_list, list) {
+		if (skip_mode & CAC_SKIP_MASK) {
+			for_each_valid_link(wdev, link_id) {
+				c = wdev_chandef(wdev, link_id);
+				if (!c || !c->chan ||
+				    c->chan->band != NL80211_BAND_5GHZ)
+					continue;
+
+				if (cfg80211_chandef_dfs_required(wiphy, c, wdev->iftype) > 0 &&
+				    cfg80211_chandef_dfs_usable(wiphy, c) && wdev->cac_started) {
+					rdev_skip_cac(rdev, wdev, link_id);
+				}
+			}
+		}
+
+		if ((skip_mode & CAC_SKIP_BACKGROUND_MASK) &&
+		    rdev->background_radar_wdev == wdev &&
+		    rdev->background_radar_chandef.chan) {
+			c = &rdev->background_radar_chandef;
+
+			if ((cfg80211_chandef_dfs_required(wiphy, c, wdev->iftype) > 0) &&
+			    cfg80211_chandef_dfs_usable(wiphy, c) &&
+			    rdev->background_cac_started) {
+				// Let current jiffies > dfs_state_entered_jiffies + CAC time
+				cac_time = rdev->background_cac_time_ms;
+				rdev->background_cac_start_time = jiffies -
+								  msecs_to_jiffies(cac_time + 1);
+				cancel_delayed_work(&rdev->background_cac_done_wk);
+				queue_delayed_work(cfg80211_wq, &rdev->background_cac_done_wk, 0);
+			}
+		}
+	}
+
+	return 0;
+}
+
+DEFINE_DEBUGFS_ATTRIBUTE(dfs_skip_cac_ops, NULL,
+			 dfs_cac_skip, "0x%08llx\n");
+
+static int
+dfs_available_reset(void *data, u64 val)
+{
+	struct wiphy *wiphy = data;
+	struct cfg80211_registered_device *rdev = wiphy_to_rdev(wiphy);
+	bool en = !!val;
+	enum nl80211_band band;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *chan;
+	int i;
+
+	if (!en)
+		return 0;
+
+	for (band = 0; band < NUM_NL80211_BANDS; band++) {
+		sband = wiphy->bands[band];
+		if (!sband)
+			continue;
+		for (i = 0; i < sband->n_channels; i++) {
+			chan = &sband->channels[i];
+
+			if (!(chan->flags & IEEE80211_CHAN_RADAR))
+				continue;
+
+			if (chan->dfs_state == NL80211_DFS_AVAILABLE) {
+				chan->dfs_state = NL80211_DFS_USABLE;
+				chan->dfs_state_entered = jiffies;
+			}
+		}
+	}
+
+	cfg80211_sched_dfs_chan_update(rdev);
+
+	return 0;
+}
+
+DEFINE_DEBUGFS_ATTRIBUTE(dfs_available_reset_ops, NULL,
+			 dfs_available_reset, "0x%08llx\n");
+
+#define DEBUGFS_ADD(name, chmod)						\
+	debugfs_create_file(#name, chmod, phyd, &rdev->wiphy, &name## _ops)
 
 void cfg80211_debugfs_rdev_add(struct cfg80211_registered_device *rdev)
 {
 	struct dentry *phyd = rdev->wiphy.debugfsdir;
 
-	DEBUGFS_ADD(rts_threshold);
-	DEBUGFS_ADD(fragmentation_threshold);
-	DEBUGFS_ADD(short_retry_limit);
-	DEBUGFS_ADD(long_retry_limit);
-	DEBUGFS_ADD(ht40allow_map);
+	DEBUGFS_ADD(rts_threshold, 0444);
+	DEBUGFS_ADD(fragmentation_threshold, 0444);
+	DEBUGFS_ADD(short_retry_limit, 0444);
+	DEBUGFS_ADD(long_retry_limit, 0444);
+	DEBUGFS_ADD(ht40allow_map, 0444);
+	DEBUGFS_ADD(dfs_status, 0444);
+	DEBUGFS_ADD(dfs_skip_nop, 0600);
+	DEBUGFS_ADD(dfs_skip_cac, 0600);
+	DEBUGFS_ADD(dfs_available_reset, 0600);
 }
 
 struct debugfs_read_work {
diff --git a/net/wireless/mlme.c b/net/wireless/mlme.c
index c7e62eb..d42b65b 100644
--- a/net/wireless/mlme.c
+++ b/net/wireless/mlme.c
@@ -1177,13 +1177,16 @@ __cfg80211_background_cac_event(struct cfg80211_registered_device *rdev,
 		queue_work(cfg80211_wq, &rdev->propagate_cac_done_wk);
 		cfg80211_sched_dfs_chan_update(rdev);
 		wdev = rdev->background_radar_wdev;
+		rdev->background_cac_started = false;
 		break;
 	case NL80211_RADAR_CAC_ABORTED:
 		if (!cancel_delayed_work(&rdev->background_cac_done_wk))
 			return;
 		wdev = rdev->background_radar_wdev;
+		rdev->background_cac_started = false;
 		break;
 	case NL80211_RADAR_CAC_STARTED:
+		rdev->background_cac_started = true;
 		break;
 	default:
 		return;
@@ -1203,6 +1206,7 @@ cfg80211_background_cac_event(struct cfg80211_registered_device *rdev,
 					chandef, event);
 	wiphy_unlock(&rdev->wiphy);
 }
+EXPORT_SYMBOL(cfg80211_background_cac_event);
 
 void cfg80211_background_cac_done_wk(struct work_struct *work)
 {
@@ -1264,8 +1268,10 @@ cfg80211_start_background_radar_detection(struct cfg80211_registered_device *rde
 	if (!cac_time_ms)
 		cac_time_ms = IEEE80211_DFS_MIN_CAC_TIME_MS;
 
+	rdev->background_cac_time_ms = cac_time_ms;
 	rdev->background_radar_chandef = *chandef;
 	rdev->background_radar_wdev = wdev; /* Get offchain ownership */
+	rdev->background_cac_start_time = jiffies;
 
 	__cfg80211_background_cac_event(rdev, wdev, chandef,
 					NL80211_RADAR_CAC_STARTED);
diff --git a/net/wireless/rdev-ops.h b/net/wireless/rdev-ops.h
index 466828f..2ae7fc5 100644
--- a/net/wireless/rdev-ops.h
+++ b/net/wireless/rdev-ops.h
@@ -1542,4 +1542,18 @@ rdev_set_ttlm(struct cfg80211_registered_device *rdev,
 
 	return ret;
 }
+
+static inline int
+rdev_skip_cac(struct cfg80211_registered_device *rdev,
+	      struct wireless_dev *wdev, unsigned int link_id)
+{
+	if (!rdev->ops->skip_cac)
+		return -EOPNOTSUPP;
+
+	trace_rdev_skip_cac(wdev, link_id);
+	rdev->ops->skip_cac(wdev, link_id);
+	trace_rdev_return_void(&rdev->wiphy);
+
+	return 0;
+}
 #endif /* __CFG80211_RDEV_OPS */
diff --git a/net/wireless/trace.h b/net/wireless/trace.h
index 7073a70..aa3284f 100644
--- a/net/wireless/trace.h
+++ b/net/wireless/trace.h
@@ -4005,6 +4005,19 @@ TRACE_EVENT(rdev_set_ttlm,
 		  WIPHY_PR_ARG, NETDEV_PR_ARG)
 );
 
+TRACE_EVENT(rdev_skip_cac,
+	TP_PROTO(struct wireless_dev *wdev, unsigned int link_id),
+	TP_ARGS(wdev, link_id),
+	TP_STRUCT__entry(
+		WDEV_ENTRY
+		__field(unsigned int, link_id)
+	),
+	TP_fast_assign(
+		WDEV_ASSIGN;
+		__entry->link_id = link_id;
+	),
+	TP_printk(WDEV_PR_FMT ", link_id: %d", WDEV_PR_ARG, __entry->link_id)
+);
 #endif /* !__RDEV_OPS_TRACE || TRACE_HEADER_MULTI_READ */
 
 #undef TRACE_INCLUDE_PATH
-- 
2.39.2