[v2,2/3] AP: Add FILS discovery support

diff --git a/hostapd/config_file.c b/hostapd/config_file.c
index 1861b5203f0f..b9e41951ce06 100644
--- a/hostapd/config_file.c
+++ b/hostapd/config_file.c
@@ -4353,6 +4353,10 @@  static int hostapd_config_fill(struct hostapd_config *conf,
 		bss->dhcp_server_port = atoi(pos);
 	} else if (os_strcmp(buf, "dhcp_relay_port") == 0) {
 		bss->dhcp_relay_port = atoi(pos);
+	} else if (os_strcmp(buf, "fils_discovery_min_interval") == 0) {
+		bss->fils_discovery_min_int = atoi(pos);
+	} else if (os_strcmp(buf, "fils_discovery_max_interval") == 0) {
+		bss->fils_discovery_max_int = atoi(pos);
 #endif /* CONFIG_FILS */
 	} else if (os_strcmp(buf, "multicast_to_unicast") == 0) {
 		bss->multicast_to_unicast = atoi(pos);
diff --git a/hostapd/hostapd.conf b/hostapd/hostapd.conf
index 25b4e49276c0..75687dd7a9aa 100644
--- a/hostapd/hostapd.conf
+++ b/hostapd/hostapd.conf
@@ -1908,6 +1908,11 @@  own_ip_addr=127.0.0.1
 # default: 30 TUs (= 30.72 milliseconds)
 #fils_hlp_wait_time=30
 
+# FILS discovery transmission minimum and maximum packet interval settings.
+# If maximum interval is non-zero, the AP schedules FILS discovery transmission
+#fils_discovery_max_interval=0 to 10000 (in TUs).
+#fils_discovery_min_interval=0 to 10000 (in TUs).
+
 # Transition Disable indication
 # The AP can notify authenticated stations to disable transition mode in their
 # network profiles when the network has completed transition steps, i.e., once
diff --git a/src/ap/ap_config.c b/src/ap/ap_config.c
index 769f7fab6d55..a8747b1d29e8 100644
--- a/src/ap/ap_config.c
+++ b/src/ap/ap_config.c
@@ -131,6 +131,7 @@  void hostapd_config_defaults_bss(struct hostapd_bss_config *bss)
 	bss->fils_hlp_wait_time = 30;
 	bss->dhcp_server_port = DHCP_SERVER_PORT;
 	bss->dhcp_relay_port = DHCP_SERVER_PORT;
+	bss->fils_discovery_min_int = 20;
 #endif /* CONFIG_FILS */
 
 	bss->broadcast_deauth = 1;
diff --git a/src/ap/ap_config.h b/src/ap/ap_config.h
index b705c378fe28..9661a4b8e123 100644
--- a/src/ap/ap_config.h
+++ b/src/ap/ap_config.h
@@ -728,6 +728,8 @@  struct hostapd_bss_config {
 	unsigned int fils_hlp_wait_time;
 	u16 dhcp_server_port;
 	u16 dhcp_relay_port;
+	u32 fils_discovery_min_int;
+	u32 fils_discovery_max_int;
 #endif /* CONFIG_FILS */
 
 	int multicast_to_unicast;
diff --git a/src/ap/beacon.c b/src/ap/beacon.c
index b3b33b7faf1d..21f3f83c6b94 100644
--- a/src/ap/beacon.c
+++ b/src/ap/beacon.c
@@ -1124,6 +1124,261 @@  void sta_track_del(struct hostapd_sta_info *info)
 }
 
 
+#ifdef CONFIG_FILS
+static u8 hostapd_fils_discovery_rsn(struct hostapd_data *hapd, u16 *cap,
+				     u32 *suite_selector_ptr)
+{
+	const u8 *ie, *start;
+	u8 len;
+	u16 cnt;
+	u32 suite_selector = 0;
+
+	ie = hostapd_wpa_ie(hapd, WLAN_EID_RSN);
+	if (!ie || ie[1] < 6)
+		return 0;
+
+	len = ie[1];
+	start = &ie[2];
+	ie += 4;
+	do {
+		/*  Group Data Cipher Suite Selector */
+		suite_selector = ie[3];
+		ie += 4;
+
+		/*  Pairwise Cipher Suite Selector */
+		if ((ie - start) >= len)
+			break;
+		os_memcpy((u8 *) &cnt, ie, 2);
+		ie += 2;
+		if (cnt) {
+			suite_selector |= (((u32) ie[3]) << 12);
+			ie += (4 * cnt);
+		}
+
+		/*  AKM Cipher Suite Selector */
+		if ((ie - start) >= len)
+			break;
+		os_memcpy((u8 *) &cnt, ie, 2);
+		ie += 2;
+		if (cnt) {
+			suite_selector |= (((u32) ie[3]) << 18);
+			ie += (4 * cnt);
+		}
+
+		/* RSN capabilities */
+		if ((ie - start) >= len)
+			break;
+		os_memcpy((u8 *) cap, ie, 2);
+		ie += 2;
+
+		/*  Group Management Cipher Suite Selector */
+		if ((ie - start) < len)
+			suite_selector |= (((u32) ie[3]) << 6);
+	} while (0);
+
+	*suite_selector_ptr = suite_selector;
+	return 1;
+}
+
+
+u16 hostapd_fils_discovery_cap(struct hostapd_data *hapd)
+{
+	u16 cap_info = 0, nss_mask = 0x0003;
+	u8 nss = 0, chwidth = 0;
+	int i;
+
+	cap_info = FILS_DISCOVERY_CAP_ESS |
+		   (hapd->conf->wpa ? FILS_DISCOVERY_CAP_PRIVACY : 0);
+
+	if (is_6ghz_op_class(hapd->iconf->op_class)) {
+		cap_info |= (FILS_DISCOVERY_CAP_PHY_INDEX_HE <<
+			     FILS_DISCOVERY_CAP_PHY_INDEX_SHIFT);
+
+		switch (hapd->iconf->op_class) {
+		case 132:
+			chwidth = FILS_DISCOVERY_CAP_BSS_CHWIDTH_40;
+			break;
+		case 133:
+			chwidth = FILS_DISCOVERY_CAP_BSS_CHWIDTH_80;
+			break;
+		case 134:
+		case 135:
+			chwidth = FILS_DISCOVERY_CAP_BSS_CHWIDTH_160_8080;
+			break;
+		}
+	} else {
+		switch (hostapd_get_oper_chwidth(hapd->iconf)) {
+		case CHANWIDTH_80P80MHZ:
+		case CHANWIDTH_160MHZ:
+			chwidth = FILS_DISCOVERY_CAP_BSS_CHWIDTH_160_8080;
+			break;
+		case CHANWIDTH_80MHZ:
+			chwidth = FILS_DISCOVERY_CAP_BSS_CHWIDTH_80;
+			break;
+		case CHANWIDTH_USE_HT:
+			if (hapd->iconf->secondary_channel)
+				chwidth = FILS_DISCOVERY_CAP_BSS_CHWIDTH_40;
+			break;
+		}
+
+		if (hapd->iconf->ieee80211ax) {
+#ifdef CONFIG_IEEE80211AX
+			cap_info |= (FILS_DISCOVERY_CAP_PHY_INDEX_HE <<
+				     FILS_DISCOVERY_CAP_PHY_INDEX_SHIFT);
+#endif /* CONFIG_IEEE80211AX */
+		} else if (hapd->conf->vendor_vht) {
+#ifdef CONFIG_IEEE80211AC
+			cap_info |= (FILS_DISCOVERY_CAP_PHY_INDEX_VHT <<
+				     FILS_DISCOVERY_CAP_PHY_INDEX_SHIFT);
+#endif /* CONFIG_IEEE80211AC */
+		} else if (hapd->iconf->ieee80211n &&
+			   !hapd->conf->disable_11n) {
+			cap_info |= (FILS_DISCOVERY_CAP_PHY_INDEX_HT <<
+				     FILS_DISCOVERY_CAP_PHY_INDEX_SHIFT);
+		}
+	}
+
+	cap_info |= (chwidth << FILS_DISCOVERY_CAP_BSS_CHWIDTH_SHIFT);
+
+	for (i = 0; i < 8; i++) {
+		if ((hapd->iconf->he_op.he_basic_mcs_nss_set & nss_mask) !=
+		    nss_mask)
+			nss++;
+		nss_mask = nss_mask << 2;
+	}
+	if (nss > 4)
+		cap_info |= (4 << FILS_DISCOVERY_CAP_NSS_SHIFT);
+	else
+		cap_info |= ((nss - 1) << FILS_DISCOVERY_CAP_NSS_SHIFT);
+
+	/* TODO: FILS minimum rate */
+	return cap_info;
+}
+
+
+static u8 * hostapd_gen_fils_discovery(struct hostapd_data *hapd, u32 *len)
+{
+	struct ieee80211_mgmt *head;
+	const u8 *mobility_domain;
+	u8 *pos, *length_pos, rsn = 0, buf[200], buf_len;
+	u16 frm_cntl = 0, rsn_cap = 0;
+	u32 suite_selectors = 0, total_len;
+
+#define FILS_DISOVERY_TMPL_HEAD_LEN 26
+#define FILS_DISOVERY_TMPL_MIN_LEN 19
+
+	total_len = FILS_DISOVERY_TMPL_HEAD_LEN + FILS_DISOVERY_TMPL_MIN_LEN;
+
+	/* FILS discovery frame control: 2 bytes */
+	frm_cntl = (sizeof(hapd->conf->ssid.short_ssid) - 1) |
+		   FILS_DISCOVERY_FRM_CNTL_CAP_PRESENT |
+		   FILS_DISCOVERY_FRM_CNTL_SHORT_SSID_PRESENT |
+		   FILS_DISCOVERY_FRM_CNTL_LENGTH_PRESENT;
+
+	/* Check for optional subfields and calculate length */
+	rsn = hostapd_fils_discovery_rsn(hapd, &rsn_cap, &suite_selectors);
+	if (rsn) {
+		frm_cntl |= FILS_DISCOVERY_FRM_CNTL_RSN_INFO_PRESENT;
+		total_len += 5;
+	}
+
+	mobility_domain = hostapd_wpa_ie(hapd, WLAN_EID_MOBILITY_DOMAIN);
+	if (mobility_domain) {
+		frm_cntl |= FILS_DISCOVERY_FRM_CNTL_MOBILITY_DOMAIN_PRESENT;
+		total_len += 3;
+	}
+
+	pos = hostapd_eid_fils_indic(hapd, buf, 0);
+	buf_len = pos - buf;
+	total_len += buf_len;
+
+	head = os_zalloc(total_len);
+	if (!head)
+		return NULL;
+
+	head->frame_control = IEEE80211_FC(WLAN_FC_TYPE_MGMT,
+					   WLAN_FC_STYPE_ACTION);
+	os_memset(head->da, 0xff, ETH_ALEN);
+	os_memcpy(head->sa, hapd->own_addr, ETH_ALEN);
+	os_memcpy(head->bssid, hapd->own_addr, ETH_ALEN);
+
+	head->u.action.category = WLAN_ACTION_PUBLIC;
+	head->u.action.u.public_action.action = WLAN_PA_FILS_DISCOVERY;
+
+	pos = &head->u.action.u.public_action.variable[0];
+	*((u16 *) pos) = host_to_le16(frm_cntl);
+	pos += 2;
+
+	/* hardware or low-level driver will setup timestamp */
+	pos += 8;
+
+	/* Beacon interval */
+	*((u16 *) pos) = host_to_le16(hapd->iconf->beacon_int);
+	pos += 2;
+
+	/* Short SSID */
+	*((u32 *) pos) = host_to_le32(hapd->conf->ssid.short_ssid);
+	pos += sizeof(hapd->conf->ssid.short_ssid);
+
+	/* Store position of FILS discovery information element length field */
+	length_pos = pos++;
+
+	/* FD Capability : total 2 bytes */
+	*((u16 *) pos) = host_to_le16(hostapd_fils_discovery_cap(hapd));
+	pos += 2;
+
+	/* RSN */
+	if (frm_cntl & FILS_DISCOVERY_FRM_CNTL_RSN_INFO_PRESENT) {
+		os_memcpy(pos, &rsn_cap, 2);
+		os_memcpy(&pos[2], &suite_selectors, 3);
+		pos += 5;
+	}
+
+	/* Mobility Domain */
+	if (frm_cntl & FILS_DISCOVERY_FRM_CNTL_MOBILITY_DOMAIN_PRESENT) {
+		os_memcpy(pos, &mobility_domain[2], 3);
+		pos += 3;
+	}
+
+	/* Fill the FILS discovery information element length */
+	*length_pos = pos - (length_pos + 1);
+
+	/* FILS indication element */
+	if (buf_len) {
+		os_memcpy(pos, buf, buf_len);
+		pos += buf_len;
+	}
+
+	*len = pos - (u8 *) head;
+	return ((u8 *) head);
+}
+
+
+/* Configure FILS discovery transmission */
+static u8 * hostapd_fils_discovery(struct hostapd_data *hapd,
+				  struct wpa_driver_ap_params *params)
+{
+#define VALID_INTERVAL(x,y) { x = (x > y) ? y : x; }
+
+	params->fils_discovery_max_int = hapd->conf->fils_discovery_max_int;
+	if (is_6ghz_op_class(hapd->iconf->op_class))
+		VALID_INTERVAL(params->fils_discovery_max_int,
+			       FILS_DISCOVERY_MAX_INTERVAL_6GHZ);
+
+	params->fils_discovery_min_int = hapd->conf->fils_discovery_min_int;
+	VALID_INTERVAL(params->fils_discovery_min_int,
+		       params->fils_discovery_max_int);
+#undef VALID_INTERVAL
+
+	if (params->fils_discovery_max_int)
+		return hostapd_gen_fils_discovery(hapd,
+						  &params->fils_discovery_tmpl_len);
+
+	return NULL;
+}
+#endif /* CONFIG_FILS */
+
+
 int ieee802_11_build_ap_params(struct hostapd_data *hapd,
 			       struct wpa_driver_ap_params *params)
 {
@@ -1461,6 +1716,10 @@  void ieee802_11_free_ap_params(struct wpa_driver_ap_params *params)
 	params->head = NULL;
 	os_free(params->proberesp);
 	params->proberesp = NULL;
+#ifdef CONFIG_FILS
+	os_free(params->fils_discovery_tmpl);
+	params->fils_discovery_tmpl = NULL;
+#endif /* CONFIG_FILS */
 }
 
 
@@ -1508,6 +1767,10 @@  int ieee802_11_set_beacon(struct hostapd_data *hapd)
 #endif /* CONFIG_IEEE80211AX */
 	hapd->reenable_beacon = 0;
 
+#ifdef CONFIG_FILS
+	params.fils_discovery_tmpl = hostapd_fils_discovery(hapd, &params);
+#endif /* CONFIG_FILS */
+
 	if (cmode &&
 	    hostapd_set_freq_params(&freq, iconf->hw_mode, iface->freq,
 				    iconf->channel, iconf->enable_edmg,
diff --git a/src/drivers/driver.h b/src/drivers/driver.h
index e8defaba2118..91659f3afcf1 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -1508,6 +1508,26 @@  struct wpa_driver_ap_params {
 	 * twt_responder - Whether Target Wait Time responder is enabled
 	 */
 	int twt_responder;
+
+	/**
+	 * FILS discovery minimum interval
+	 */
+	u32 fils_discovery_min_int;
+
+	/**
+	 * FILS discovery maximum interval
+	 */
+	u32 fils_discovery_max_int;
+
+	/**
+	 * FILS discovery template data
+	 */
+	u8 *fils_discovery_tmpl;
+
+	/**
+	 * FILS discovery template length
+	 */
+	size_t fils_discovery_tmpl_len;
 };
 
 struct wpa_driver_mesh_bss_params {