From 5bcd4472062750b192c98d944b74e07b14ab3af5 Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Wed, 2 Aug 2023 19:00:34 +0800
Subject: [PATCH 1005/1005] hostapd: mtk: add zwdfs mode ctrl for eagle efem
 hwits

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
---
 hostapd/config_file.c             |  2 ++
 hostapd/ctrl_iface.c              | 30 +++++++++++++++++++++++++++
 src/ap/ap_config.h                |  6 ++++++
 src/ap/ap_drv_ops.c               | 14 +++++++++++++
 src/ap/dfs.c                      |  6 ++++++
 src/common/mtk_vendor.h           | 12 +++++++++++
 src/drivers/driver.h              |  7 +++++++
 src/drivers/driver_nl80211.c      | 34 +++++++++++++++++++++++++++++++
 src/drivers/driver_nl80211.h      |  1 +
 src/drivers/driver_nl80211_capa.c |  3 +++
 10 files changed, 115 insertions(+)

diff --git a/hostapd/config_file.c b/hostapd/config_file.c
index 9e3dbb2..a751993 100644
--- a/hostapd/config_file.c
+++ b/hostapd/config_file.c
@@ -3183,6 +3183,8 @@ static int hostapd_config_fill(struct hostapd_config *conf,
 		conf->acs_exclude_6ghz_non_psc = atoi(pos);
 	} else if (os_strcmp(buf, "enable_background_radar") == 0) {
 		conf->enable_background_radar = atoi(pos);
+	} else if (os_strcmp(buf, "background_radar_mode") == 0) {
+		conf->background_radar_mode = atoi(pos);
 	} else if (os_strcmp(buf, "min_tx_power") == 0) {
 		int val = atoi(pos);
 
diff --git a/hostapd/ctrl_iface.c b/hostapd/ctrl_iface.c
index c288352..517ebd6 100644
--- a/hostapd/ctrl_iface.c
+++ b/hostapd/ctrl_iface.c
@@ -4156,6 +4156,33 @@ hostapd_ctrl_iface_dump_amnt(struct hostapd_data *hapd, char *cmd,
 		return pos - buf;
 }
 
+static int
+hostapd_ctrl_iface_set_background_radar_mode(struct hostapd_data *hapd, char *cmd,
+					     char *buf, size_t buflen)
+{
+	struct hostapd_iface *iface = hapd->iface;
+	char *pos, *param;
+
+	param = os_strchr(cmd, ' ');
+	if (!param)
+		return -1;
+	*param++ = '\0';
+
+	pos = os_strstr(param, "mode=");
+	if (!pos)
+		return -1;
+
+	if (os_strncmp(pos + 5, "cert", 4) == 0)
+		iface->conf->background_radar_mode = BACKGROUND_RADAR_CERT_MODE;
+	else if (os_strncmp(pos + 5, "normal", 6) == 0)
+		iface->conf->background_radar_mode = BACKGROUND_RADAR_NORMAL_MODE;
+
+	if (hostapd_drv_background_radar_mode(hapd) < 0)
+		return -1;
+
+	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,
@@ -4750,6 +4777,9 @@ static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 		if (pos)
 			*pos = ' ';
 		reply_len = hostapd_ctrl_iface_set_mu(hapd, buf + 23, reply, reply_size);
+	} else if (os_strncmp(buf, "SET_BACKGROUND_RADAR_MODE", 25) == 0) {
+		reply_len = hostapd_ctrl_iface_set_background_radar_mode(hapd, buf + 25,
+									 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 7c0d12a..b6f05e7 100644
--- a/src/ap/ap_config.h
+++ b/src/ap/ap_config.h
@@ -1025,6 +1025,7 @@ struct hostapd_config {
 	bool hw_mode_set;
 	int acs_exclude_6ghz_non_psc;
 	int enable_background_radar;
+	int background_radar_mode;
 	enum {
 		LONG_PREAMBLE = 0,
 		SHORT_PREAMBLE = 1
@@ -1218,6 +1219,11 @@ enum three_wire_mode {
 		NUM_THREE_WIRE_MODE - 1
 };
 
+enum background_radar_mode {
+	BACKGROUND_RADAR_NORMAL_MODE,
+	BACKGROUND_RADAR_CERT_MODE,
+};
+
 enum dfs_mode {
 	DFS_DETECT_MODE_DISABLE,
 	DFS_DETECT_MODE_AP_ENABLE,
diff --git a/src/ap/ap_drv_ops.c b/src/ap/ap_drv_ops.c
index 721bfa0..5b93ea6 100644
--- a/src/ap/ap_drv_ops.c
+++ b/src/ap/ap_drv_ops.c
@@ -1257,3 +1257,17 @@ int hostapd_drv_amnt_dump(struct hostapd_data *hapd, u8 amnt_idx, u8 *amnt_dump_
 		return 0;
 	return hapd->driver->amnt_dump(hapd->drv_priv, amnt_idx, amnt_dump_buf);
 }
+
+int hostapd_drv_background_radar_mode(struct hostapd_data *hapd)
+{
+	if (!hapd->driver || !hapd->driver->background_radar_mode ||
+	    !(hapd->iface->drv_flags2 & WPA_DRIVER_RADAR_BACKGROUND) ||
+	    !hapd->iface->conf->enable_background_radar)
+		return 0;
+	if (hapd->iconf->background_radar_mode > BACKGROUND_RADAR_CERT_MODE) {
+		wpa_printf(MSG_INFO, "Invalid value for background radar mode\n");
+		return 0;
+	}
+	return hapd->driver->background_radar_mode(hapd->drv_priv,
+						   hapd->iconf->background_radar_mode);
+}
diff --git a/src/ap/dfs.c b/src/ap/dfs.c
index 008596b..2564168 100644
--- a/src/ap/dfs.c
+++ b/src/ap/dfs.c
@@ -983,6 +983,9 @@ int hostapd_handle_dfs(struct hostapd_iface *iface)
 		if (res < 0)
 			return res;
 
+		if (hostapd_drv_background_radar_mode(iface->bss[0]) < 0)
+			return -1;
+
 		iface->radar_background.temp_ch = 1;
 		return 1;
 	} else if (dfs_use_radar_background(iface)) {
@@ -1023,6 +1026,9 @@ int hostapd_handle_dfs(struct hostapd_iface *iface)
 		iface->radar_background.secondary_channel = sec;
 		iface->radar_background.centr_freq_seg0_idx = cf1;
 		iface->radar_background.centr_freq_seg1_idx = cf2;
+
+		if (hostapd_drv_background_radar_mode(iface->bss[0]) < 0)
+			return -1;
 	}
 
 	return 0;
diff --git a/src/common/mtk_vendor.h b/src/common/mtk_vendor.h
index e140de6..5bc1e04 100644
--- a/src/common/mtk_vendor.h
+++ b/src/common/mtk_vendor.h
@@ -16,6 +16,7 @@ enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_3WIRE_CTRL = 0xc8,
 	MTK_NL80211_VENDOR_SUBCMD_IBF_CTRL = 0xc9,
 	MTK_NL80211_VENDOR_SUBCMD_BSS_COLOR_CTRL = 0xca,
+	MTK_NL80211_VENDOR_SUBCMD_BACKGROUND_RADAR_CTRL = 0xcb,
 };
 
 enum mtk_vendor_attr_edcca_ctrl {
@@ -244,6 +245,17 @@ enum mtk_vendor_attr_bss_color_ctrl {
 		NUM_MTK_VENDOR_ATTRS_BSS_COLOR_CTRL - 1
 };
 
+enum mtk_vendor_attr_background_radar_ctrl {
+	MTK_VENDOR_ATTR_BACKGROUND_RADAR_CTRL_UNSPEC,
+
+	MTK_VENDOR_ATTR_BACKGROUND_RADAR_CTRL_MODE,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_BACKGROUND_RADAR_CTRL,
+	MTK_VENDOR_ATTR_BACKGROUND_RADAR_CTRL_MAX =
+		NUM_MTK_VENDOR_ATTRS_BACKGROUND_RADAR_CTRL - 1
+};
+
 #define CSI_MAX_COUNT 256
 #define ETH_ALEN 6
 
diff --git a/src/drivers/driver.h b/src/drivers/driver.h
index a9f48a1..bc82d28 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -5201,6 +5201,13 @@ struct wpa_driver_ops {
 	* @amnt_dump_buf: Buffer to print
 	*/
 	int (*amnt_dump)(void *priv, u8 amnt_idx, u8 *amnt_dump_buf);
+
+	/**
+	 * background_radar_mode - set background radar mode
+	 * @priv: Private driver interface data
+	 * @background_radar_mode: background radar mode
+	 */
+	int (*background_radar_mode)(void *priv, u8 background_radar_mode);
 };
 
 /**
diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index 25e5910..73401fd 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -14723,6 +14723,39 @@ fail:
 	return -ENOBUFS;
 }
 
+static int nl80211_background_radar_mode(void *priv, const u8 background_radar_mode)
+{
+	struct i802_bss *bss = priv;
+	struct wpa_driver_nl80211_data *drv = bss->drv;
+	/* Prepare nl80211 cmd */
+	struct nl_msg *msg;
+	struct nlattr *data;
+	int ret;
+
+	if (!drv->mtk_background_radar_vendor_cmd_avail) {
+		wpa_printf(MSG_INFO,
+			   "nl80211: Driver does not support setting background radar mode");
+		return 0;
+	}
+
+	if (!(msg = nl80211_drv_msg(drv, 0, NL80211_CMD_VENDOR)) ||
+	    nla_put_u32(msg, NL80211_ATTR_VENDOR_ID, OUI_MTK) ||
+	    nla_put_u32(msg, NL80211_ATTR_VENDOR_SUBCMD,
+			MTK_NL80211_VENDOR_SUBCMD_BACKGROUND_RADAR_CTRL) ||
+	    !(data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA)) ||
+	    nla_put_u8(msg, MTK_VENDOR_ATTR_BACKGROUND_RADAR_CTRL_MODE, background_radar_mode)) {
+		nlmsg_free(msg);
+		return -ENOBUFS;
+	}
+	nla_nest_end(msg, data);
+	ret = send_and_recv_msgs(drv, msg, NULL, NULL, NULL, NULL);
+	if (ret) {
+		wpa_printf(MSG_ERROR, "Failed to set background radar mode. ret=%d (%s) ",
+			   ret, strerror(-ret));
+	}
+	return ret;
+}
+
 const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.name = "nl80211",
 	.desc = "Linux nl80211/cfg80211",
@@ -14895,4 +14928,5 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.ap_trigtype = nl80211_ap_trigtype,
 	.amnt_set = nl80211_amnt_set,
 	.amnt_dump = nl80211_amnt_dump,
+	.background_radar_mode = nl80211_background_radar_mode,
 };
diff --git a/src/drivers/driver_nl80211.h b/src/drivers/driver_nl80211.h
index 0d85adf..74ee9b1 100644
--- a/src/drivers/driver_nl80211.h
+++ b/src/drivers/driver_nl80211.h
@@ -210,6 +210,7 @@ struct wpa_driver_nl80211_data {
 	unsigned int mtk_bss_color_vendor_cmd_avail:1;
 	unsigned int mtk_rfeatures_vendor_cmd_avail:1;
 	unsigned int mtk_amnt_vendor_cmd_avail:1;
+	unsigned int mtk_background_radar_vendor_cmd_avail:1;
 
 	u64 vendor_scan_cookie;
 	u64 remain_on_chan_cookie;
diff --git a/src/drivers/driver_nl80211_capa.c b/src/drivers/driver_nl80211_capa.c
index 8c8b84e..90711b4 100644
--- a/src/drivers/driver_nl80211_capa.c
+++ b/src/drivers/driver_nl80211_capa.c
@@ -1133,6 +1133,9 @@ static int wiphy_info_handler(struct nl_msg *msg, void *arg)
 				case MTK_NL80211_VENDOR_SUBCMD_RFEATURE_CTRL:
 					drv->mtk_rfeatures_vendor_cmd_avail = 1;
 					break;
+				case MTK_NL80211_VENDOR_SUBCMD_BACKGROUND_RADAR_CTRL:
+					drv->mtk_background_radar_vendor_cmd_avail = 1;
+					break;
 				}
 			}
 
-- 
2.18.0