From 5fd8ba4e856b94c8753a468b9e8a7dbece63cfec Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Fri, 7 Oct 2022 10:46:29 +0800
Subject: [PATCH] Add DFS and ZWDFS support

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
---
 hostapd/config_file.c        |  4 ++
 hostapd/ctrl_iface.c         | 95 ++++++++++++++++++++++++++++++++++++
 src/ap/ap_config.h           | 13 +++++
 src/ap/dfs.c                 | 35 +++++++------
 src/ap/dfs.h                 | 15 ++++++
 src/ap/hostapd.c             |  4 +-
 src/drivers/driver.h         |  7 +++
 src/drivers/driver_nl80211.c | 29 +++++++++++
 src/drivers/nl80211_copy.h   |  1 +
 9 files changed, 186 insertions(+), 17 deletions(-)

diff --git a/hostapd/config_file.c b/hostapd/config_file.c
index 310f97d..a2ee418 100644
--- a/hostapd/config_file.c
+++ b/hostapd/config_file.c
@@ -4807,6 +4807,10 @@ static int hostapd_config_fill(struct hostapd_config *conf,
 			return 1;
 		}
 		conf->amsdu = val;
+	} else if (os_strcmp(buf, "dfs_detect_mode") == 0) { /*bypass channel switch*/
+		u8 en = strtol(pos, NULL, 10);
+
+		conf->dfs_detect_mode = en;
 	} else {
 		wpa_printf(MSG_ERROR,
 			   "Line %d: unknown configuration item '%s'",
diff --git a/hostapd/ctrl_iface.c b/hostapd/ctrl_iface.c
index 0c40175..296e42f 100644
--- a/hostapd/ctrl_iface.c
+++ b/hostapd/ctrl_iface.c
@@ -3537,6 +3537,96 @@ hostapd_ctrl_iface_get_aggregation(struct hostapd_data *hapd, char *buf,
 }
 
 
+static int
+hostapd_ctrl_iface_set_dfs_detect_mode(struct hostapd_data *hapd, char *value,
+				       char *buf, size_t buflen)
+{
+	u8 dfs_detect_mode;
+
+	if (!value)
+		return -1;
+
+	dfs_detect_mode = strtol(value, NULL, 10);
+	if (dfs_detect_mode > DFS_DETECT_MODE_MAX) {
+		wpa_printf(MSG_ERROR, "Invalid value for dfs detect mode");
+		return -1;
+	}
+	hapd->iconf->dfs_detect_mode = dfs_detect_mode;
+
+	return os_snprintf(buf, buflen, "OK\n");
+}
+
+
+static int
+hostapd_ctrl_iface_set_offchan_ctrl(struct hostapd_data *hapd, char *cmd,
+				    char *buf, size_t buflen)
+{
+	struct hostapd_iface *iface = hapd->iface;
+	char *pos, *param;
+	enum hostapd_hw_mode hw_mode;
+	bool chan_found = false;
+	int i, num_available_chandefs, channel, chan_width, sec = 0;
+	int sec_chan_idx_80p80 = -1;
+	u8 oper_centr_freq_seg0_idx, oper_centr_freq_seg1_idx;
+	struct hostapd_channel_data *chan;
+	enum dfs_channel_type type = DFS_NO_CAC_YET;
+
+	param = os_strchr(cmd, ' ');
+	if (!param)
+		return -1;
+	*param++ = '\0';
+
+	pos = os_strstr(param, "chan=");
+	if (pos)
+		channel = strtol(pos + 5, NULL, 10);
+	else
+		return -1;
+
+	num_available_chandefs = dfs_find_channel(iface, NULL, 0, type);
+	for (i = 0; i < num_available_chandefs; i++) {
+		dfs_find_channel(iface, &chan, i, type);
+		if (chan->chan == channel) {
+			chan_found = true;
+			break;
+		}
+	}
+
+	if (!chan_found)
+		return -1;
+
+	if (iface->conf->secondary_channel)
+		sec = 1;
+
+	dfs_adjust_center_freq(iface, chan,
+			       sec,
+			       sec_chan_idx_80p80,
+			       &oper_centr_freq_seg0_idx,
+			       &oper_centr_freq_seg1_idx);
+
+	if (hostapd_start_dfs_cac(iface, iface->conf->hw_mode,
+				  chan->freq, chan->chan,
+				  iface->conf->ieee80211n,
+				  iface->conf->ieee80211ac,
+				  iface->conf->ieee80211ax,
+				  iface->conf->ieee80211be,
+				  sec, hostapd_get_oper_chwidth(iface->conf),
+				  oper_centr_freq_seg0_idx,
+				  oper_centr_freq_seg1_idx, true)) {
+		wpa_printf(MSG_ERROR, "DFS failed to start CAC offchannel");
+		iface->radar_background.channel = -1;
+		return -1;
+	}
+
+	iface->radar_background.channel = chan->chan;
+	iface->radar_background.freq = chan->freq;
+	iface->radar_background.secondary_channel = sec;
+	iface->radar_background.centr_freq_seg0_idx = oper_centr_freq_seg0_idx;
+	iface->radar_background.centr_freq_seg1_idx = oper_centr_freq_seg1_idx;
+
+	return os_snprintf(buf, buflen, "OK\n");
+}
+
+
 static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 					      char *buf, char *reply,
 					      int reply_size,
@@ -4099,6 +4189,11 @@ static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 		reply_len = hostapd_ctrl_iface_get_ibf(hapd, reply, reply_size);
 	} else if (os_strncmp(buf, "GET_AGGR", 8) == 0) {
 		reply_len = hostapd_ctrl_iface_get_aggregation(hapd, reply, reply_size);
+	} else if (os_strncmp(buf, "DFS_DETECT_MODE ", 16) == 0) {
+		reply_len = hostapd_ctrl_iface_set_dfs_detect_mode(hapd, buf + 16,
+								   reply, reply_size);
+	} else if (os_strncmp(buf, "SET_OFFCHAN_CTRL", 16) == 0) {
+		reply_len = hostapd_ctrl_iface_set_offchan_ctrl(hapd, buf + 16, reply, reply_size);
 	} else {
 		os_memcpy(reply, "UNKNOWN COMMAND\n", 16);
 		reply_len = 16;
diff --git a/src/ap/ap_config.h b/src/ap/ap_config.h
index 01b051d..f567c37 100644
--- a/src/ap/ap_config.h
+++ b/src/ap/ap_config.h
@@ -1160,6 +1160,7 @@ struct hostapd_config {
 	u8 ibf_enable;
 	u8 ampdu;
 	u8 amsdu;
+	u8 dfs_detect_mode;
 };
 
 enum three_wire_mode {
@@ -1174,6 +1175,18 @@ enum three_wire_mode {
 		NUM_THREE_WIRE_MODE - 1
 };
 
+enum dfs_mode {
+	DFS_DETECT_MODE_DISABLE,
+	DFS_DETECT_MODE_AP_ENABLE,
+	DFS_DETECT_MODE_BACKGROUND_ENABLE,
+	DFS_DETECT_MODE_ALL_ENABLE,
+
+	/* keep last */
+	NUM_DFS_DETECT_MODE,
+	DFS_DETECT_MODE_MAX =
+		NUM_DFS_DETECT_MODE - 1
+};
+
 enum edcca_mode {
 	EDCCA_MODE_FORCE_DISABLE = 0,
 	EDCCA_MODE_AUTO = 1,
diff --git a/src/ap/dfs.c b/src/ap/dfs.c
index b5d105d..1c3f678 100644
--- a/src/ap/dfs.c
+++ b/src/ap/dfs.c
@@ -19,13 +19,6 @@
 #include "dfs.h"
 #include "crypto/crypto.h"
 
-
-enum dfs_channel_type {
-	DFS_ANY_CHANNEL,
-	DFS_AVAILABLE, /* non-radar or radar-available */
-	DFS_NO_CAC_YET, /* radar-not-yet-available */
-};
-
 static struct hostapd_channel_data *
 dfs_downgrade_bandwidth(struct hostapd_iface *iface, int *secondary_channel,
 			u8 *oper_centr_freq_seg0_idx,
@@ -238,9 +231,9 @@ static int is_in_chanlist(struct hostapd_iface *iface,
  *  - hapd->vht/he_oper_centr_freq_seg0_idx
  *  - hapd->vht/he_oper_centr_freq_seg1_idx
  */
-static int dfs_find_channel(struct hostapd_iface *iface,
-			    struct hostapd_channel_data **ret_chan,
-			    int idx, enum dfs_channel_type type)
+int dfs_find_channel(struct hostapd_iface *iface,
+		     struct hostapd_channel_data **ret_chan,
+		     int idx, enum dfs_channel_type type)
 {
 	struct hostapd_hw_modes *mode;
 	struct hostapd_channel_data *chan;
@@ -299,12 +292,12 @@ static int dfs_find_channel(struct hostapd_iface *iface,
 }
 
 
-static void dfs_adjust_center_freq(struct hostapd_iface *iface,
-				   struct hostapd_channel_data *chan,
-				   int secondary_channel,
-				   int sec_chan_idx_80p80,
-				   u8 *oper_centr_freq_seg0_idx,
-				   u8 *oper_centr_freq_seg1_idx)
+void dfs_adjust_center_freq(struct hostapd_iface *iface,
+			    struct hostapd_channel_data *chan,
+			    int secondary_channel,
+			    int sec_chan_idx_80p80,
+			    u8 *oper_centr_freq_seg0_idx,
+			    u8 *oper_centr_freq_seg1_idx)
 {
 	if (!iface->conf->ieee80211ac && !iface->conf->ieee80211ax)
 		return;
@@ -1317,6 +1310,11 @@ hostapd_dfs_background_start_channel_switch(struct hostapd_iface *iface,
 		   __func__, iface->radar_background.cac_started ? "yes" : "no",
 		   hostapd_csa_in_progress(iface) ? "yes" : "no");
 
+	/* Skip channel switch when background dfs detect mode is on */
+	if (iface->conf->dfs_detect_mode == DFS_DETECT_MODE_BACKGROUND_ENABLE ||
+	    iface->conf->dfs_detect_mode == DFS_DETECT_MODE_ALL_ENABLE)
+		return 0;
+
 	/* Check if CSA in progress */
 	if (hostapd_csa_in_progress(iface))
 		return 0;
@@ -1365,6 +1363,11 @@ static int hostapd_dfs_start_channel_switch(struct hostapd_iface *iface)
 		   __func__, iface->cac_started ? "yes" : "no",
 		   hostapd_csa_in_progress(iface) ? "yes" : "no");
 
+	/* Skip channel switch when dfs detect mode is on */
+	if (iface->conf->dfs_detect_mode == DFS_DETECT_MODE_AP_ENABLE ||
+	    iface->conf->dfs_detect_mode == DFS_DETECT_MODE_ALL_ENABLE)
+		return 0;
+
 	/* Check if CSA in progress */
 	if (hostapd_csa_in_progress(iface))
 		return 0;
diff --git a/src/ap/dfs.h b/src/ap/dfs.h
index 606c1b3..c2556d2 100644
--- a/src/ap/dfs.h
+++ b/src/ap/dfs.h
@@ -9,6 +9,12 @@
 #ifndef DFS_H
 #define DFS_H
 
+enum dfs_channel_type {
+	DFS_ANY_CHANNEL,
+	DFS_AVAILABLE, /* non-radar or radar-available */
+	DFS_NO_CAC_YET, /* radar-not-yet-available */
+};
+
 int hostapd_handle_dfs(struct hostapd_iface *iface);
 
 int hostapd_dfs_complete_cac(struct hostapd_iface *iface, int success, int freq,
@@ -32,5 +38,14 @@ int hostapd_dfs_start_cac(struct hostapd_iface *iface, int freq,
 int hostapd_handle_dfs_offload(struct hostapd_iface *iface);
 int hostapd_is_dfs_overlap(struct hostapd_iface *iface, enum chan_width width,
 			   int center_freq);
+int dfs_find_channel(struct hostapd_iface *iface,
+		     struct hostapd_channel_data **ret_chan,
+		     int idx, enum dfs_channel_type type);
+void dfs_adjust_center_freq(struct hostapd_iface *iface,
+			    struct hostapd_channel_data *chan,
+			    int secondary_channel,
+			    int sec_chan_idx_80p80,
+			    u8 *oper_centr_freq_seg0_idx,
+			    u8 *oper_centr_freq_seg1_idx);
 
 #endif /* DFS_H */
diff --git a/src/ap/hostapd.c b/src/ap/hostapd.c
index 5fc998e..ed1dfdb 100644
--- a/src/ap/hostapd.c
+++ b/src/ap/hostapd.c
@@ -1463,7 +1463,9 @@ static int hostapd_setup_bss(struct hostapd_data *hapd, int first)
 		return -1;
 	}
 
-	if (!conf->start_disabled && ieee802_11_set_beacon(hapd) < 0)
+	if (conf->start_disabled)
+		hapd->driver->start_disabled(hapd->drv_priv);
+	else if (ieee802_11_set_beacon(hapd) < 0)
 		return -1;
 
 	if (flush_old_stations && !conf->start_disabled &&
diff --git a/src/drivers/driver.h b/src/drivers/driver.h
index e34903b..6ad6d84 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -4722,6 +4722,13 @@ struct wpa_driver_ops {
 	int (*ampdu_ctrl)(void *priv, u8 ampdu);
 	int (*amsdu_ctrl)(void *priv, u8 amsdu);
 	int (*aggregation_dump)(void *priv, u8 *aggr);
+
+	/**
+	 * start_disabled - set start_disabled to cfg80211
+	 * @priv: Private driver interface data
+	 *
+	 */
+	int (*start_disabled)(void *priv);
 };
 
 /**
diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index a06ac58..af064c0 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -12812,6 +12812,34 @@ fail:
 	return -ENOBUFS;
 }
 
+static int nl80211_start_disabled(void *priv)
+{
+	struct i802_bss *bss = priv;
+	struct wpa_driver_nl80211_data *drv = bss->drv;
+	struct nl_msg *msg;
+	struct nlattr *data;
+	int ret;
+
+	msg = nl80211_bss_msg(bss, 0, NL80211_CMD_NEW_BEACON);
+	if (!msg)
+		goto fail;
+
+	if (nla_put_flag(msg, NL80211_ATTR_START_DISABLED))
+		goto fail;
+
+	ret = send_and_recv_msgs_connect_handle(drv, msg, bss, 1);
+
+	if (ret)
+		wpa_printf(MSG_ERROR, "Failed to set start_disabled. ret=%d (%s)",
+			   ret, strerror(-ret));
+
+	return ret;
+
+fail:
+	nlmsg_free(msg);
+	return ret;
+}
+
 const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.name = "nl80211",
 	.desc = "Linux nl80211/cfg80211",
@@ -12967,4 +12995,5 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.ampdu_ctrl = nl80211_enable_ampdu,
 	.amsdu_ctrl = nl80211_enable_amsdu,
 	.aggregation_dump = nl80211_dump_aggregation,
+	.start_disabled = nl80211_start_disabled,
 };
diff --git a/src/drivers/nl80211_copy.h b/src/drivers/nl80211_copy.h
index c4bf3ad..79bc76c 100644
--- a/src/drivers/nl80211_copy.h
+++ b/src/drivers/nl80211_copy.h
@@ -3176,6 +3176,7 @@ enum nl80211_attrs {
 	NL80211_ATTR_EHT_CAPABILITY,
 
 	/* 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,
-- 
2.18.0