From 8c9d9f2b8da1b0e3e0832e7d7b02d75c4c0a4f3e Mon Sep 17 00:00:00 2001
From: MeiChia Chiu <meichia.chiu@mediatek.com>
Date: Thu, 24 May 2022 21:48:21 +0800
Subject: [PATCH] hostapd:v2 add support for runtime set in-band discovery-v2

Usage:
hostapd_cli unsolic_probe_resp [tx_type] [interval]

0: disable all in-band discovery
1: enable unsolicited probe response
2: enable FILS discovery

Signed-off-by: MeiChia Chiu <MeiChia.Chiu@mediatek.com>
---
 hostapd/ctrl_iface.c         | 62 ++++++++++++++++++++++++++++++++++++
 hostapd/hostapd_cli.c        | 20 ++++++++++++
 src/ap/beacon.c              |  5 ++-
 src/drivers/driver_nl80211.c |  8 +++--
 src/drivers/nl80211_copy.h   |  1 +
 5 files changed, 92 insertions(+), 4 deletions(-)

diff --git a/hostapd/ctrl_iface.c b/hostapd/ctrl_iface.c
index 86adf18e5..41740cfd5 100644
--- a/hostapd/ctrl_iface.c
+++ b/hostapd/ctrl_iface.c
@@ -769,6 +769,65 @@ static int hostapd_ctrl_iface_send_qos_map_conf(struct hostapd_data *hapd,
 
 #endif /* CONFIG_INTERWORKING */
 
+static int hostapd_ctrl_iface_inband_discovery(struct hostapd_data *hapd,
+					       const char *cmd)
+{
+	struct hostapd_bss_config *conf = hapd->conf;
+	const char *pos = cmd;
+	int tx_type, interval, ret;
+
+	tx_type = atoi(pos);
+	if (tx_type < 0 || tx_type > 2) {
+		wpa_printf(MSG_ERROR, "Invalid tx type\n");
+		return -1;
+	}
+
+	pos = os_strchr(pos, ' ');
+	if(!pos)
+		return -1;
+	pos++;
+	interval = atoi(pos);
+	if (interval < 0 || interval > 20) {
+		wpa_printf(MSG_ERROR, "Invalid interval value\n");
+		return -1;
+	}
+
+	wpa_printf(MSG_ERROR, "Set inband discovery type:%d, interval:%d\n",
+			      tx_type, interval);
+
+#define DISABLE_INBAND_DISC 0
+#define UNSOL_PROBE_RESP 1
+#define FILS_DISCOVERY 2
+
+	conf->fils_discovery_max_int = 0;
+	conf->fils_discovery_min_int = 0;
+	conf->unsol_bcast_probe_resp_interval = 0;
+
+	switch (tx_type) {
+	case DISABLE_INBAND_DISC:
+	default:
+		/* Disable both Unsolicited probe response and FILS discovery*/
+		break;
+	case UNSOL_PROBE_RESP:
+		/* Enable Unsolicited probe response */
+		conf->unsol_bcast_probe_resp_interval = interval;
+		break;
+	case FILS_DISCOVERY:
+		/* Enable FILS discovery */
+		conf->fils_discovery_min_int = interval;
+		conf->fils_discovery_max_int = interval;
+		break;
+	}
+
+	ret = ieee802_11_update_beacons(hapd->iface);
+	if(ret) {
+		wpa_printf(MSG_DEBUG,
+			"Failed to update with inband discovery parameters\n");
+		return -1;
+	}
+
+	return 0;
+}
 
 #ifdef CONFIG_WNM_AP
 
@@ -3673,6 +3732,9 @@ static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 		if (hostapd_ctrl_iface_coloc_intf_req(hapd, buf + 15))
 			reply_len = -1;
 #endif /* CONFIG_WNM_AP */
+	} else if (os_strncmp(buf, "INBAND_DISCOVERY ", 17) == 0) {
+		if (hostapd_ctrl_iface_inband_discovery(hapd, buf + 17))
+			reply_len = -1;
 	} else if (os_strcmp(buf, "GET_CONFIG") == 0) {
 		reply_len = hostapd_ctrl_iface_get_config(hapd, reply,
 							  reply_size);
diff --git a/hostapd/hostapd_cli.c b/hostapd/hostapd_cli.c
index 260912111..e30c4e7c1 100644
--- a/hostapd/hostapd_cli.c
+++ b/hostapd/hostapd_cli.c
@@ -646,6 +646,24 @@ static int hostapd_cli_cmd_wps_config(struct wpa_ctrl *ctrl, int argc,
 }
 #endif /* CONFIG_WPS */
 
+static int hostapd_cli_cmd_inband_discovery(struct wpa_ctrl *ctrl, int argc,
+					    char *argv[])
+{
+	char buf[300];
+	int res;
+
+	if (argc < 2) {
+		printf("Invalid 'inband_discovery' command - two arguments"
+		       "tx_type interval\n");
+		return -1;
+	}
+
+	res = os_snprintf(buf, sizeof(buf), "INBAND_DISCOVERY %s %s",
+			  argv[0], argv[1]);
+	if (os_snprintf_error(sizeof(buf), res))
+		return -1;
+	return wpa_ctrl_command(ctrl, buf);
+}
 
 static int hostapd_cli_cmd_disassoc_imminent(struct wpa_ctrl *ctrl, int argc,
 					     char *argv[])
@@ -1744,6 +1762,8 @@ static const struct hostapd_cli_cmd hostapd_cli_commands[] = {
 	{ "driver", hostapd_cli_cmd_driver, NULL,
 	  "<driver sub command> [<hex formatted data>] = send driver command data" },
 #endif /* ANDROID */
+	{ "inband_discovery", hostapd_cli_cmd_inband_discovery, NULL,
+          "<tx type(0/1/2)> <interval> = runtime set inband discovery" },
 	{ NULL, NULL, NULL, NULL }
 };
 
diff --git a/src/ap/beacon.c b/src/ap/beacon.c
index 3c49653cc..367e32611 100644
--- a/src/ap/beacon.c
+++ b/src/ap/beacon.c
@@ -1406,6 +1406,8 @@ static u8 * hostapd_fils_discovery(struct hostapd_data *hapd,
 				   struct wpa_driver_ap_params *params)
 {
 	params->fd_max_int = hapd->conf->fils_discovery_max_int;
+	params->unsol_bcast_probe_resp_interval =
+		hapd->conf->unsol_bcast_probe_resp_interval;
 	if (is_6ghz_op_class(hapd->iconf->op_class) &&
 	    params->fd_max_int > FD_MAX_INTERVAL_6GHZ)
 		params->fd_max_int = FD_MAX_INTERVAL_6GHZ;
@@ -1414,7 +1416,8 @@ static u8 * hostapd_fils_discovery(struct hostapd_data *hapd,
 	if (params->fd_min_int > params->fd_max_int)
 		params->fd_min_int = params->fd_max_int;
 
-	if (params->fd_max_int)
+	if (params->fd_max_int ||
+	    !params->unsol_bcast_probe_resp_interval)
 		return hostapd_gen_fils_discovery(hapd,
 						  &params->fd_frame_tmpl_len);
 
diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index aec179ac3..6113aff0b 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -4491,9 +4491,10 @@ static int nl80211_fils_discovery(struct i802_bss *bss, struct nl_msg *msg,
 			params->fd_max_int) ||
 	    (params->fd_frame_tmpl &&
 	     nla_put(msg, NL80211_FILS_DISCOVERY_ATTR_TMPL,
-		     params->fd_frame_tmpl_len, params->fd_frame_tmpl)))
+		     params->fd_frame_tmpl_len, params->fd_frame_tmpl)) ||
+	    nla_put_u32(msg, NL80211_UNSOL_BCAST_PROBE_RESP_ATTR_INTE,
+			params->unsol_bcast_probe_resp_interval))
 		return -1;
-
 	nla_nest_end(msg, attr);
 	return 0;
 }
@@ -4823,7 +4824,8 @@ static int wpa_driver_nl80211_set_ap(void *priv,
 #endif /* CONFIG_SAE */
 
 #ifdef CONFIG_FILS
-	if (params->fd_max_int && nl80211_fils_discovery(bss, msg, params) < 0)
+	if ((params->fd_max_int || !(params->unsol_bcast_probe_resp_interval)) &&
+	     nl80211_fils_discovery(bss, msg, params) < 0)
 		goto fail;
 #endif /* CONFIG_FILS */
 
diff --git a/src/drivers/nl80211_copy.h b/src/drivers/nl80211_copy.h
index f962c06e9..6fb7b7fcf 100644
--- a/src/drivers/nl80211_copy.h
+++ b/src/drivers/nl80211_copy.h
@@ -7150,6 +7150,7 @@ enum nl80211_fils_discovery_attributes {
 	NL80211_FILS_DISCOVERY_ATTR_INT_MIN,
 	NL80211_FILS_DISCOVERY_ATTR_INT_MAX,
 	NL80211_FILS_DISCOVERY_ATTR_TMPL,
+	NL80211_UNSOL_BCAST_PROBE_RESP_ATTR_INTE,
 
 	/* keep last */
 	__NL80211_FILS_DISCOVERY_ATTR_LAST,
-- 
2.29.2