From 8e2d3c2456fddf1d5a3fc015ff25c4d79e496bcd 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] cfg80211: mtk: implement DFS status show, cac and nop skip
 command via debugfs

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
---
 include/net/cfg80211.h  |   1 +
 net/mac80211/cfg.c      |  19 +++
 net/wireless/core.h     |   3 +
 net/wireless/debugfs.c  | 270 ++++++++++++++++++++++++++++++++++++++--
 net/wireless/mlme.c     |   6 +
 net/wireless/rdev-ops.h |  14 +++
 net/wireless/trace.h    |  12 ++
 7 files changed, 318 insertions(+), 7 deletions(-)

diff --git a/include/net/cfg80211.h b/include/net/cfg80211.h
index ae2af09..b97ddbd 100644
--- a/include/net/cfg80211.h
+++ b/include/net/cfg80211.h
@@ -4401,6 +4401,7 @@ struct cfg80211_ops {
 				struct cfg80211_color_change_settings *params);
 	int	(*set_radar_background)(struct wiphy *wiphy,
 					struct cfg80211_chan_def *chandef);
+	void	(*skip_cac)(struct wireless_dev *wdev);
 };
 
 /*
diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index 544dcf5..0a6257d 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -4501,6 +4501,24 @@ ieee80211_set_radar_background(struct wiphy *wiphy,
 	return local->ops->set_radar_background(&local->hw, chandef);
 }
 
+static void
+ieee80211_skip_cac(struct wireless_dev *wdev)
+{
+	struct net_device *dev = wdev->netdev;
+	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
+	unsigned int cac_time_ms;
+
+	cancel_delayed_work(&sdata->dfs_cac_timer_work);
+	if (wdev->cac_started) {
+		ieee80211_vif_release_channel(sdata);
+		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);
+	}
+}
+
 const struct cfg80211_ops mac80211_config_ops = {
 	.add_virtual_intf = ieee80211_add_iface,
 	.del_virtual_intf = ieee80211_del_iface,
@@ -4607,4 +4625,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,
+	.skip_cac = ieee80211_skip_cac,
 };
diff --git a/net/wireless/core.h b/net/wireless/core.h
index b588e5a..10e8981 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 0878b16..d3a0aae 100644
--- a/net/wireless/debugfs.c
+++ b/net/wireless/debugfs.c
@@ -9,6 +9,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,	\
@@ -96,16 +97,271 @@ 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 = &wdev->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;
+	int i, remain_time = 0, wait_time_ms = 0;
+	bool is_background;
+
+	offset += scnprintf(buf + offset, buf_size - offset, "DFS Channel:\n");
+
+	for (band = 0; band < NUM_NL80211_BANDS; band++) {
+		sband = wiphy->bands[band];
+		if (!sband)
+			continue;
+		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)) {
+					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)) {
+					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))
+					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 *chandef;
+	unsigned int skip_mode = val;
+	unsigned long cac_time;
+	struct ieee80211_channel *chan;
+
+	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) && wdev->chandef.chan) {
+			chandef = &wdev->chandef;
+			chan = chandef->chan;
+
+			if ((cfg80211_chandef_dfs_required(wiphy, chandef, wdev->iftype) > 0) &&
+			    cfg80211_chandef_dfs_usable(wiphy, chandef) && wdev->cac_started) {
+				rdev_skip_cac(rdev, wdev);
+			}
+		}
+
+		if ((skip_mode & CAC_SKIP_BACKGROUND_MASK) &&
+		    rdev->background_radar_wdev == wdev &&
+		    rdev->background_radar_chandef.chan) {
+			chandef = &rdev->background_radar_chandef;
+			chan = chandef->chan;
+
+			if ((cfg80211_chandef_dfs_required(wiphy, chandef, wdev->iftype) > 0) &&
+			    cfg80211_chandef_dfs_usable(wiphy, chandef) &&
+			    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");
+
+#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);
 }
diff --git a/net/wireless/mlme.c b/net/wireless/mlme.c
index 00370ca..205c1bc 100644
--- a/net/wireless/mlme.c
+++ b/net/wireless/mlme.c
@@ -998,13 +998,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;
@@ -1024,6 +1027,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)
 {
@@ -1085,8 +1089,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 8555468..26f4604 100644
--- a/net/wireless/rdev-ops.h
+++ b/net/wireless/rdev-ops.h
@@ -1398,4 +1398,18 @@ rdev_set_radar_background(struct cfg80211_registered_device *rdev,
 	return ret;
 }
 
+static inline int
+rdev_skip_cac(struct cfg80211_registered_device *rdev,
+	      struct wireless_dev *wdev)
+{
+	if (!rdev->ops->skip_cac)
+		return -EOPNOTSUPP;
+
+	trace_rdev_skip_cac(wdev);
+	rdev->ops->skip_cac(wdev);
+	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 97a2937..eadabfa 100644
--- a/net/wireless/trace.h
+++ b/net/wireless/trace.h
@@ -3665,6 +3665,18 @@ TRACE_EVENT(rdev_set_radar_background,
 		  WIPHY_PR_ARG, CHAN_DEF_PR_ARG)
 );
 
+TRACE_EVENT(rdev_skip_cac,
+	    TP_PROTO(struct wireless_dev *wdev),
+
+	    TP_ARGS(wdev),
+
+	    TP_STRUCT__entry(WDEV_ENTRY),
+
+	    TP_fast_assign(WDEV_ASSIGN;),
+
+	    TP_printk(WDEV_PR_FMT, WDEV_PR_ARG)
+);
+
 #endif /* !__RDEV_OPS_TRACE || TRACE_HEADER_MULTI_READ */
 
 #undef TRACE_INCLUDE_PATH
-- 
2.18.0