[V4] multiple_bssid: add support

diff --git a/hostapd/config_file.c b/hostapd/config_file.c
index 542c8cd0d..2e3318184 100644
--- a/hostapd/config_file.c
+++ b/hostapd/config_file.c
@@ -4569,6 +4569,10 @@  static int hostapd_config_fill(struct hostapd_config *conf,
 		}
 		bss->mka_psk_set |= MKA_PSK_SET_CKN;
 #endif /* CONFIG_MACSEC */
+	} else if (os_strcmp(buf, "multiple_bssid") == 0) {
+		conf->multiple_bssid = atoi(pos);
+	} else if (os_strcmp(buf, "ema_beacon") == 0) {
+		conf->ema_beacon = atoi(pos);
 	} else {
 		wpa_printf(MSG_ERROR,
 			   "Line %d: unknown configuration item '%s'",
diff --git a/hostapd/ctrl_iface.c b/hostapd/ctrl_iface.c
index 5b71a7bc4..1c5ce51da 100644
--- a/hostapd/ctrl_iface.c
+++ b/hostapd/ctrl_iface.c
@@ -2681,6 +2681,9 @@  static int hostapd_ctrl_iface_chan_switch(struct hostapd_iface *iface,
 			 * submitting multi-BSS CSA requests? */
 			return ret;
 		}
+
+		if (iface->bss[i]->iconf->multiple_bssid)
+			break;
 	}
 
 	return 0;
diff --git a/src/ap/ap_config.h b/src/ap/ap_config.h
index bada04c3e..b5b25767b 100644
--- a/src/ap/ap_config.h
+++ b/src/ap/ap_config.h
@@ -987,6 +987,9 @@  struct hostapd_config {
 	u8 vht_oper_centr_freq_seg1_idx;
 	u8 ht40_plus_minus_allowed;
 
+	u8 multiple_bssid;
+	u8 ema_beacon;
+
 	/* Use driver-generated interface addresses when adding multiple BSSs */
 	u8 use_driver_iface_addr;
 
diff --git a/src/ap/beacon.c b/src/ap/beacon.c
index b3b33b7fa..8a80dbe79 100644
--- a/src/ap/beacon.c
+++ b/src/ap/beacon.c
@@ -293,7 +293,7 @@  static const u8 * hostapd_vendor_wpa_ie(struct hostapd_data *hapd,
 }
 
 
-static u8 * hostapd_get_rsne(struct hostapd_data *hapd, u8 *pos, size_t len)
+u8 * hostapd_get_rsne(struct hostapd_data *hapd, u8 *pos, size_t len)
 {
 	const u8 *ie;
 
@@ -436,6 +436,9 @@  static u8 * hostapd_gen_probe_resp(struct hostapd_data *hapd,
 	u8 *pos, *epos, *csa_pos;
 	size_t buflen;
 
+	if (hapd->iconf->multiple_bssid)
+		hapd = hostapd_get_primary_bss(hapd);
+
 #define MAX_PROBERESP_LEN 768
 	buflen = MAX_PROBERESP_LEN;
 #ifdef CONFIG_WPS
@@ -472,6 +475,8 @@  static u8 * hostapd_gen_probe_resp(struct hostapd_data *hapd,
 	buflen += hostapd_mbo_ie_len(hapd);
 	buflen += hostapd_eid_owe_trans_len(hapd);
 	buflen += hostapd_eid_dpp_cc_len(hapd);
+	if (hapd->iconf->multiple_bssid)
+		buflen += hostapd_eid_multiple_bssid_len(hapd);
 
 	resp = os_zalloc(buflen);
 	if (resp == NULL)
@@ -527,6 +532,9 @@  static u8 * hostapd_gen_probe_resp(struct hostapd_data *hapd,
 	pos = hostapd_eid_rm_enabled_capab(hapd, pos, epos - pos);
 	pos = hostapd_get_mde(hapd, pos, epos - pos);
 
+        if (hapd->iconf->multiple_bssid)
+		pos = hostapd_eid_multiple_bssid(hapd, pos, epos, 0, NULL, 0, 0);
+
 	/* eCSA IE */
 	csa_pos = hostapd_eid_ecsa(hapd, pos);
 	if (csa_pos != pos)
@@ -818,6 +826,10 @@  void handle_probe_req(struct hostapd_data *hapd,
 	size_t csa_offs_len;
 	struct radius_sta rad_info;
 
+        if (hapd->iconf->multiple_bssid &&
+	    hapd != hostapd_get_primary_bss(hapd))
+		return;
+
 	if (len < IEEE80211_HDRLEN)
 		return;
 	ie = ((const u8 *) mgmt) + IEEE80211_HDRLEN;
@@ -1061,7 +1073,7 @@  void handle_probe_req(struct hostapd_data *hapd,
 				hapd->cs_c_off_ecsa_proberesp;
 	}
 
-	ret = hostapd_drv_send_mlme(hapd, resp, resp_len, noack,
+	ret = hostapd_drv_send_mlme(hostapd_get_primary_bss(hapd), resp, resp_len, noack,
 				    csa_offs_len ? csa_offs : NULL,
 				    csa_offs_len, 0);
 
@@ -1461,6 +1473,8 @@  void ieee802_11_free_ap_params(struct wpa_driver_ap_params *params)
 	params->head = NULL;
 	os_free(params->proberesp);
 	params->proberesp = NULL;
+	os_free(params->multiple_bssid_ies);
+	params->multiple_bssid_ies = NULL;
 }
 
 
@@ -1506,6 +1520,25 @@  int ieee802_11_set_beacon(struct hostapd_data *hapd)
 	params.twt_responder = hostapd_get_he_twt_responder(hapd,
 							    IEEE80211_MODE_AP);
 #endif /* CONFIG_IEEE80211AX */
+        if (hapd->iconf->multiple_bssid) {
+		int len = hostapd_eid_multiple_bssid_len(hapd);
+		u8 *end;
+
+		params.multiple_bssid_index = hostapd_get_bss_index(hapd);
+		params.multiple_bssid_count = iface->num_bss;
+		params.ema_beacon = hapd->iconf->ema_beacon;
+		if (hapd != hapd->iface->bss[0])
+			params.multiple_bssid_parent = hapd->iface->bss[0]->conf->iface;
+		params.multiple_bssid_ies = os_zalloc(len);
+		if (params.multiple_bssid_ies == NULL)
+			goto fail;
+		end = hostapd_eid_multiple_bssid(hapd, params.multiple_bssid_ies,
+						 params.multiple_bssid_ies + len,
+						 1, params.multiple_bssid_ie_offsets,
+						 &params.multiple_bssid_ie_count,
+						 MULTIPLE_BSSID_IE_MAX);
+		params.multiple_bssid_ie_len = end - params.multiple_bssid_ies;
+	}
 	hapd->reenable_beacon = 0;
 
 	if (cmode &&
diff --git a/src/ap/hostapd.c b/src/ap/hostapd.c
index b37f49f9a..c442b82db 100644
--- a/src/ap/hostapd.c
+++ b/src/ap/hostapd.c
@@ -87,6 +87,26 @@  int hostapd_for_each_interface(struct hapd_interfaces *interfaces,
 }
 
 
+int hostapd_get_bss_index(struct hostapd_data *hapd)
+{
+	size_t i;
+
+	for (i = 1; i < hapd->iface->num_bss; i++)
+		if (hapd->iface->bss[i] == hapd)
+			return i;
+	return 0;
+}
+
+
+struct hostapd_data * hostapd_get_primary_bss(struct hostapd_data *hapd)
+{
+	if (hapd->iconf->multiple_bssid)
+		return hapd->iface->bss[0];
+
+	return hapd;
+}
+
+
 void hostapd_reconfig_encryption(struct hostapd_data *hapd)
 {
 	if (hapd->wpa_auth)
diff --git a/src/ap/hostapd.h b/src/ap/hostapd.h
index b70d13fba..44d2c6068 100644
--- a/src/ap/hostapd.h
+++ b/src/ap/hostapd.h
@@ -595,6 +595,8 @@  struct hostapd_iface {
 int hostapd_for_each_interface(struct hapd_interfaces *interfaces,
 			       int (*cb)(struct hostapd_iface *iface,
 					 void *ctx), void *ctx);
+int hostapd_get_bss_index(struct hostapd_data *hapd);
+struct hostapd_data * hostapd_get_primary_bss(struct hostapd_data *hapd);
 int hostapd_reload_config(struct hostapd_iface *iface);
 void hostapd_reconfig_encryption(struct hostapd_data *hapd);
 struct hostapd_data *
diff --git a/src/ap/ieee802_11.c b/src/ap/ieee802_11.c
index 568ecacb0..89ca76eb2 100644
--- a/src/ap/ieee802_11.c
+++ b/src/ap/ieee802_11.c
@@ -6,6 +6,7 @@ 
  * See README for more details.
  */
 
+#include <math.h>
 #include "utils/includes.h"
 
 #ifndef CONFIG_NATIVE_WINDOWS
@@ -2700,7 +2701,11 @@  int hostapd_get_aid(struct hostapd_data *hapd, struct sta_info *sta)
 	}
 	if (j == 32)
 		return -1;
-	aid = i * 32 + j + 1;
+	aid = i * 32 + j;
+	if (hapd->iconf->multiple_bssid)
+		aid += hapd->iface->num_bss;
+	else
+		aid += 1;
 	if (aid > 2007)
 		return -1;
 
@@ -4371,7 +4376,7 @@  static void handle_assoc(struct hostapd_data *hapd,
 		goto fail;
 	omit_rsnxe = !get_ie(pos, left, WLAN_EID_RSNX);
 
-	if (hostapd_get_aid(hapd, sta) < 0) {
+	if (hostapd_get_aid(hostapd_get_primary_bss(hapd), sta) < 0) {
 		hostapd_logger(hapd, mgmt->sa, HOSTAPD_MODULE_IEEE80211,
 			       HOSTAPD_LEVEL_INFO, "No room for more AIDs");
 		resp = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
@@ -5621,4 +5626,127 @@  u8 * hostapd_eid_wb_chsw_wrapper(struct hostapd_data *hapd, u8 *eid)
 	return eid;
 }
 
+
+static int hostapd_eid_multiple_bssid_chunk_len(struct hostapd_data *hapd,
+						int *count)
+{
+	/* ID + size + count */
+	int i, len = 3;
+
+	for (i = *count; i < hapd->iface->num_bss; i++) {
+		struct hostapd_data *bss = hapd->iface->bss[i];
+		/* 11 mbssid + ssid len + 32 RSN */
+		int ssid_len = 11 + bss->conf->ssid.ssid_len + 32;
+
+		if (len + ssid_len > 255) {
+			goto multiple_bssid_too_big;
+		}
+		len += ssid_len;
+	}
+
+multiple_bssid_too_big:
+	*count = i;
+
+	return len;
+}
+
+
+int hostapd_eid_multiple_bssid_len(struct hostapd_data *hapd)
+{
+	int count = 1, len = 0;
+
+	while (count < hapd->iface->num_bss)
+		len += hostapd_eid_multiple_bssid_chunk_len(hapd, &count);
+
+	return len;
+}
+
+
+static u8 * hostapd_eid_multiple_bssid_chunk(struct hostapd_data *hapd,
+					     u8 *eid, u8 *end, int *count,
+					     u8 is_beacon)
+{
+	u8 *size_offset, *num_offset, num = 0;
+	int i;
+
+	*eid++ = WLAN_EID_MULTIPLE_BSSID;
+	size_offset = eid++;
+	num_offset = eid++;
+
+	for (i = *count; i < hapd->iface->num_bss; i++) {
+		struct hostapd_data *bss = hapd->iface->bss[i];
+		u8 *bss_size_offset, *index_size_offset, *pos = eid, *rsn;
+		u16 capab_info;
+
+		*eid++ = WLAN_EID_SUBELEMENT_NONTRANSMITTED_BSSID_PROFILE;
+		bss_size_offset = eid++;
+
+		*eid++ = WLAN_EID_NONTRANSMITTED_BSSID_CAPA;
+		*eid++ = sizeof(capab_info);
+		capab_info = host_to_le16(hostapd_own_capab_info(bss));
+		os_memcpy(eid, (const void*)&capab_info, sizeof(capab_info));
+		eid += sizeof(capab_info);
+
+		*eid++ = WLAN_EID_SSID;
+		*eid++ = bss->conf->ssid.ssid_len;
+		os_memcpy(eid, bss->conf->ssid.ssid, bss->conf->ssid.ssid_len);
+		eid += bss->conf->ssid.ssid_len;
+
+		*eid++ = WLAN_EID_MULTIPLE_BSSID_INDEX;
+		index_size_offset = eid++;
+		*eid++ = i;
+		if (is_beacon) {
+			*eid++ = bss->conf->dtim_period;
+			*eid++ = 0xFF;
+		}
+		*index_size_offset = (eid - index_size_offset) - 1;
+
+		rsn = hostapd_get_rsne(bss, eid, end - eid);
+		if (rsn == eid) {
+			/* add RSN non-inheritance IE */
+			*eid++ = WLAN_EID_EXTENSION;
+			*eid++ = 3;
+			*eid++ = WLAN_EID_EXT_NON_INHERITANCE;
+			*eid++ = 1;
+			*eid++ = WLAN_EID_RSN;
+		} else {
+			eid = rsn;
+		}
+		*bss_size_offset = (eid - bss_size_offset) - 1;
+
+		if ((eid - size_offset) - 1 > 255) {
+			eid = pos;
+			goto multiple_bssid_too_big;
+		}
+		num++;
+	}
+
+multiple_bssid_too_big:
+	*count = i;
+	*num_offset = (u8)ceil(log2(hapd->iface->num_bss));
+	if (*num_offset < 1)
+		*num_offset = 1;
+	*size_offset = (eid - size_offset) - 1;
+
+	return eid;
+}
+
+
+u8 * hostapd_eid_multiple_bssid(struct hostapd_data *hapd, u8 *eid, u8 *end,
+				u8 is_beacon, u8 **eid_offsets, int *eid_count,
+				int eid_max)
+{
+	int count = 1;
+
+	while (count < hapd->iface->num_bss) {
+		if (eid_offsets && *eid_count < eid_max) {
+			eid_offsets[*eid_count] = eid;
+			*eid_count = *eid_count + 1;
+		}
+		eid = hostapd_eid_multiple_bssid_chunk(hapd, eid, end, &count,
+						       is_beacon);
+	}
+	return eid;
+}
+
 #endif /* CONFIG_NATIVE_WINDOWS */
diff --git a/src/ap/ieee802_11.h b/src/ap/ieee802_11.h
index ea8c60846..11fd1245c 100644
--- a/src/ap/ieee802_11.h
+++ b/src/ap/ieee802_11.h
@@ -119,6 +119,10 @@  u8 * hostapd_eid_time_zone(struct hostapd_data *hapd, u8 *eid);
 int hostapd_update_time_adv(struct hostapd_data *hapd);
 void hostapd_client_poll_ok(struct hostapd_data *hapd, const u8 *addr);
 u8 * hostapd_eid_bss_max_idle_period(struct hostapd_data *hapd, u8 *eid);
+u8 * hostapd_eid_multiple_bssid(struct hostapd_data *hapd, u8 *eid, u8 *end,
+				u8 is_beacon, u8 **eid_offsets, int *eid_count,
+				int eid_max);
+int hostapd_eid_multiple_bssid_len(struct hostapd_data *hapd);
 
 int auth_sae_init_committed(struct hostapd_data *hapd, struct sta_info *sta);
 #ifdef CONFIG_SAE
@@ -194,5 +198,6 @@  int get_tx_parameters(struct sta_info *sta, int ap_max_chanwidth,
 
 void auth_sae_process_commit(void *eloop_ctx, void *user_ctx);
 u8 * hostapd_eid_rsnxe(struct hostapd_data *hapd, u8 *eid, size_t len);
+u8 * hostapd_get_rsne(struct hostapd_data *hapd, u8 *pos, size_t len);
 
 #endif /* IEEE802_11_H */
diff --git a/src/ap/ieee802_11_shared.c b/src/ap/ieee802_11_shared.c
index 17003d506..508e74184 100644
--- a/src/ap/ieee802_11_shared.c
+++ b/src/ap/ieee802_11_shared.c
@@ -357,6 +357,8 @@  static void hostapd_ext_capab_byte(struct hostapd_data *hapd, u8 *pos, int idx)
 			*pos |= 0x02; /* Bit 17 - WNM-Sleep Mode */
 		if (hapd->conf->bss_transition)
 			*pos |= 0x08; /* Bit 19 - BSS Transition */
+		if (hapd->iconf->multiple_bssid)
+                        *pos |= 0x40; /* Bit 22 - Multiple BSSID */
 		break;
 	case 3: /* Bits 24-31 */
 #ifdef CONFIG_WNM_AP
diff --git a/src/common/ieee802_11_defs.h b/src/common/ieee802_11_defs.h
index 86d71c156..238a55a32 100644
--- a/src/common/ieee802_11_defs.h
+++ b/src/common/ieee802_11_defs.h
@@ -452,6 +452,8 @@ 
 #define WLAN_EID_RSNX 244
 #define WLAN_EID_EXTENSION 255
 
+#define WLAN_EID_SUBELEMENT_NONTRANSMITTED_BSSID_PROFILE 0
+
 /* Element ID Extension (EID 255) values */
 #define WLAN_EID_EXT_ASSOC_DELAY_INFO 1
 #define WLAN_EID_EXT_FILS_REQ_PARAMS 2
@@ -474,6 +476,7 @@ 
 #define WLAN_EID_EXT_HE_MU_EDCA_PARAMS 38
 #define WLAN_EID_EXT_SPATIAL_REUSE 39
 #define WLAN_EID_EXT_OCV_OCI 54
+#define WLAN_EID_EXT_NON_INHERITANCE 56
 #define WLAN_EID_EXT_SHORT_SSID_LIST 58
 #define WLAN_EID_EXT_HE_6GHZ_BAND_CAP 59
 #define WLAN_EID_EXT_EDMG_CAPABILITIES 61
diff --git a/src/drivers/driver.h b/src/drivers/driver.h
index e8defaba2..aa1c938d4 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -1212,6 +1212,8 @@  struct wowlan_triggers {
 	u8 rfkill_release;
 };
 
+#define MULTIPLE_BSSID_IE_MAX	8
+
 struct wpa_driver_ap_params {
 	/**
 	 * head - Beacon head from IEEE 802.11 header to IEs before TIM IE
@@ -1508,6 +1510,46 @@  struct wpa_driver_ap_params {
 	 * twt_responder - Whether Target Wait Time responder is enabled
 	 */
 	int twt_responder;
+
+	/**
+	 * multiple_bssid_parent - The transmitting iface
+	 */
+	const char *multiple_bssid_parent;
+
+	/**
+	 * multiple_bssid_index - The index of this BSS in the group
+	 */
+	unsigned int multiple_bssid_index;
+
+	/**
+	 * multiple_bssid_count - The number of BSSs in the group
+	 */
+	unsigned int multiple_bssid_count;
+
+	/**
+	 * multiple_bssid_ies - This buffer contains all of the IEs
+	 */
+	u8 *multiple_bssid_ies;
+
+	/**
+	 * multiple_bssid_ie_len - The IE buffer length
+	 */
+	int multiple_bssid_ie_len;
+
+	/**
+	 * multiple_bssid_ie_offsets - The offsets to the IEs inside multiple_bssid_ies
+	 */
+	u8 *multiple_bssid_ie_offsets[MULTIPLE_BSSID_IE_MAX];
+
+	/**
+	 * multiple_bssid_ie_count - The the number of offsets inside multiple_bssid_ie_offsets
+	 */
+	int multiple_bssid_ie_count;
+
+	/**
+	 * ema_beacon - should the multiple_bssid beacons be sent out in EMA mode
+	 */
+	int ema_beacon;
 };
 
 struct wpa_driver_mesh_bss_params {
diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index 2ee34d11d..c37e99fdb 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -4561,6 +4561,37 @@  static int wpa_driver_nl80211_set_ap(void *priv,
 	}
 #endif /* CONFIG_IEEE80211AX */
 
+	if (params->multiple_bssid_count) {
+		nla_put_u8(msg, NL80211_ATTR_MULTIPLE_BSSID_INDEX,
+			   params->multiple_bssid_index);
+		nla_put_u8(msg, NL80211_ATTR_MULTIPLE_BSSID_COUNT,
+			   params->multiple_bssid_count);
+		if (params->ema_beacon)
+			nla_put_flag(msg, NL80211_ATTR_MULTIPLE_BSSID_EMA);
+	}
+
+	if (params->multiple_bssid_parent) {
+		int ifidx = if_nametoindex(params->multiple_bssid_parent);
+		if (ifidx <= 0)
+			goto fail;
+		nla_put_u32(msg, NL80211_ATTR_MULTIPLE_BSSID_PARENT,
+			    ifidx);
+	}
+
+	if (params->multiple_bssid_ie_len) {
+		struct nlattr *ies = nla_nest_start(msg, NL80211_ATTR_MULTIPLE_BSSID_IES);
+		u8 **offs = params->multiple_bssid_ie_offsets;
+		int i;
+
+		for (i = 0; i < params->multiple_bssid_ie_count - 1; i++)
+			nla_put(msg, i + 1,
+				offs[i + 1] - offs[i], offs[i]);
+		nla_put(msg, i + 1,
+			*offs + params->multiple_bssid_ie_len - offs[i],
+			offs[i]);
+		nla_nest_end(msg, ies);
+	}
+
 	ret = send_and_recv_msgs_owner(drv, msg, get_connect_handle(bss), 1,
 				       NULL, NULL, NULL, NULL);
 	if (ret) {
@@ -5150,7 +5181,6 @@  const char * nl80211_iftype_str(enum nl80211_iftype mode)
 	}
 }
 
-
 static int nl80211_create_iface_once(struct wpa_driver_nl80211_data *drv,
 				     const char *ifname,
 				     enum nl80211_iftype iftype,
diff --git a/src/drivers/nl80211_copy.h b/src/drivers/nl80211_copy.h
index dad8c8f85..e1f4144dd 100644
--- a/src/drivers/nl80211_copy.h
+++ b/src/drivers/nl80211_copy.h
@@ -2505,6 +2505,19 @@  enum nl80211_commands {
  * @NL80211_ATTR_HE_6GHZ_CAPABILITY: HE 6 GHz Band Capability element (from
  *	association request when used with NL80211_CMD_NEW_STATION).
  *
+ * @NL80211_ATTR_MULTIPLE_BSSID_PARENT: If this is a Non-Transmitted BSSID, define
+ *      the parent (transmitting) interface.
+ *
+ * @NL80211_ATTR_MULTIPLE_BSSID_INDEX: The index of this BSS inside the multi bssid
+ *      element.
+ *
+ * @NL80211_ATTR_MULTIPLE_BSSID_COUNT: The number of BSSs inside the multi bssid element.
+ *
+ * @NL80211_ATTR_MULTIPLE_BSSID_IES: The Elements that describe our multiple BSS group.
+ *      these get passed separately as the kernel might need to split them up for EMA VAP.
+ *
+ * @NL80211_ATTR_MULTIPLE_BSSID_EMA: Shall the multiple BSS beacons be sent out in EMA mode. 
+ *
  * @NUM_NL80211_ATTR: total number of nl80211_attrs available
  * @NL80211_ATTR_MAX: highest attribute number currently defined
  * @__NL80211_ATTR_AFTER_LAST: internal use
@@ -2987,6 +3000,12 @@  enum nl80211_attrs {
 
 	NL80211_ATTR_HE_6GHZ_CAPABILITY,
 
+	NL80211_ATTR_MULTIPLE_BSSID_PARENT,
+	NL80211_ATTR_MULTIPLE_BSSID_INDEX,
+	NL80211_ATTR_MULTIPLE_BSSID_COUNT,
+	NL80211_ATTR_MULTIPLE_BSSID_IES,
+	NL80211_ATTR_MULTIPLE_BSSID_EMA,
+
 	/* add attributes here, update the policy in nl80211.c */
 
 	__NL80211_ATTR_AFTER_LAST,