[PATCH v3 5/7] multiple_bssid: add the new IE

[John Crispin]

For multiple bssid to work we need to add a new IE to the beacon and probe
responses that includes the information on the non-transmitting BSSs. If
all the information does not fit into the 255 bytes, we need to split it
up over multiple IEs of the same type.

Signed-off-by: John Crispin <john at phrozen.org>
---
 src/ap/ieee802_11.c          | 114 +++++++++++++++++++++++++++++++++++
 src/ap/ieee802_11.h          |   5 ++
 src/common/ieee802_11_defs.h |   2 +
 3 files changed, 121 insertions(+)

diff --git a/src/ap/ieee802_11.c b/src/ap/ieee802_11.c
index b91640070..06286378f 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
@@ -5617,4 +5618,117 @@ 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;
+		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;
+
+		eid = hostapd_get_rsne(bss, eid, end - eid);
+		*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/common/ieee802_11_defs.h b/src/common/ieee802_11_defs.h
index 4d3037eee..7d1287763 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
-- 
2.25.1