[19/50] AP: Support building basic Multi-Link Element

diff --git a/src/ap/beacon.c b/src/ap/beacon.c
index c7ebc55347..94d68f6e70 100644
--- a/src/ap/beacon.c
+++ b/src/ap/beacon.c
@@ -585,6 +585,14 @@  static u8 * hostapd_gen_probe_resp(struct hostapd_data *hapd,
 	if (hapd->iconf->ieee80211be && !hapd->conf->disable_11be) {
 		buflen += hostapd_eid_eht_capab_len(hapd, IEEE80211_MODE_AP);
 		buflen += 3 + sizeof(struct ieee80211_eht_operation);
+
+		/*
+		 * TODO: multi link AP has variable length and can be
+		 * long based on the common info and number of per
+		 * station profiles. For now use 256.
+		 */
+		if (hapd->conf->mld_ap)
+			buflen += 256;
 	}
 #endif /* CONFIG_IEEE80211BE */
 
@@ -727,6 +735,9 @@  static u8 * hostapd_gen_probe_resp(struct hostapd_data *hapd,
 
 #ifdef CONFIG_IEEE80211BE
 	if (hapd->iconf->ieee80211be && !hapd->conf->disable_11be) {
+		if (hapd->conf->mld_ap)
+			pos = hostapd_eid_eht_basic_ml(hapd, pos, NULL, true);
+
 		pos = hostapd_eid_eht_capab(hapd, pos, IEEE80211_MODE_AP);
 		pos = hostapd_eid_eht_operation(hapd, pos);
 	}
@@ -1655,6 +1666,14 @@  int ieee802_11_build_ap_params(struct hostapd_data *hapd,
 	if (hapd->iconf->ieee80211be && !hapd->conf->disable_11be) {
 		tail_len += hostapd_eid_eht_capab_len(hapd, IEEE80211_MODE_AP);
 		tail_len += 3 + sizeof(struct ieee80211_eht_operation);
+
+		/*
+		 * TODO: multi link AP has variable length and can be
+		 * long based on the common info and number of per
+		 * station profiles. For now use 256.
+		 */
+		if (hapd->conf->mld_ap)
+			tail_len += 256;
 	}
 #endif /* CONFIG_IEEE80211BE */
 
@@ -1825,6 +1844,10 @@  int ieee802_11_build_ap_params(struct hostapd_data *hapd,
 
 #ifdef CONFIG_IEEE80211BE
 	if (hapd->iconf->ieee80211be && !hapd->conf->disable_11be) {
+		if (hapd->conf->mld_ap)
+			tailpos = hostapd_eid_eht_basic_ml(hapd, tailpos, NULL,
+							   true);
+
 		tailpos = hostapd_eid_eht_capab(hapd, tailpos,
 						IEEE80211_MODE_AP);
 		tailpos = hostapd_eid_eht_operation(hapd, tailpos);
diff --git a/src/ap/ieee802_11.h b/src/ap/ieee802_11.h
index 5f443fcb8a..a5b33e7a89 100644
--- a/src/ap/ieee802_11.h
+++ b/src/ap/ieee802_11.h
@@ -83,6 +83,8 @@  void hostapd_get_eht_capab(struct hostapd_data *hapd,
 			   const struct ieee80211_eht_capabilities *src,
 			   struct ieee80211_eht_capabilities *dest,
 			   size_t len);
+u8 *hostapd_eid_eht_basic_ml(struct hostapd_data *hapd, u8 *eid,
+			     struct sta_info *info, bool include_mld_id);
 int hostapd_get_aid(struct hostapd_data *hapd, struct sta_info *sta);
 u16 copy_sta_ht_capab(struct hostapd_data *hapd, struct sta_info *sta,
 		      const u8 *ht_capab);
diff --git a/src/ap/ieee802_11_eht.c b/src/ap/ieee802_11_eht.c
index caaadcecf0..d5d37995ce 100644
--- a/src/ap/ieee802_11_eht.c
+++ b/src/ap/ieee802_11_eht.c
@@ -408,3 +408,195 @@  void hostapd_get_eht_capab(struct hostapd_data *hapd,
 	os_memset(dest, 0, sizeof(*dest));
 	os_memcpy(dest, src, len);
 }
+
+u8 *hostapd_eid_eht_basic_ml(struct hostapd_data *hapd, u8 *eid,
+			     struct sta_info *info, bool include_mld_id)
+{
+	struct wpabuf *buf;
+	u16 control;
+	u8 *pos = eid;
+	const u8 *ptr;
+	size_t len, slice_len;
+	u8 link_id;
+	u8 common_info_len;
+
+	/*
+	 * As the ML element can exceed the size of 244 bytes need to first
+	 * build it and then handle defragmentation
+	 */
+	buf = wpabuf_alloc(1024);
+	if (!buf)
+		return pos;
+
+	/* set the multi-link control field */
+	control = MULTI_LINK_CONTROL_TYPE_BASIC |
+		  BASIC_MULTI_LINK_CTRL_PRES_LINK_ID |
+		  BASIC_MULTI_LINK_CTRL_PRES_BSS_PARAM_CH_COUNT |
+		  BASIC_MULTI_LINK_CTRL_PRES_EML_CAPA |
+		  BASIC_MULTI_LINK_CTRL_PRES_MLD_CAPA;
+
+	/*
+	 * set the basic multi-link common information. Hard code the common
+	 * info length to 13 based on the length of the present fields:
+	 * Length (1) + MLD address (6) + Link ID (1) +
+	 * BSS change parameter (1) + MLD EML capabilities (2) +
+	 * MLD MLD capabilities (2)
+	 */
+	common_info_len = 13;
+
+	if (include_mld_id) {
+		control |= BASIC_MULTI_LINK_CTRL_PRES_AP_MLD_ID;
+		common_info_len++;
+	}
+
+	wpabuf_put_le16(buf, control);
+
+	wpabuf_put_u8(buf, common_info_len);
+
+	/* own MLD address */
+	wpabuf_put_data(buf, hapd->mld_addr, ETH_ALEN);
+
+	/* own link ID */
+	wpabuf_put_u8(buf, hapd->conf->mld_link_id);
+
+	/* currently hard code the BSS change parameters to 0x1 */
+	wpabuf_put_u8(buf, 0x1);
+
+	wpa_printf(MSG_DEBUG, "MLD: EML capabilities=0x%x",
+		   hapd->conf->mld_eml_capa);
+
+	wpabuf_put_le16(buf, hapd->conf->mld_eml_capa);
+
+	wpa_printf(MSG_DEBUG, "MLD: MLD capabilities=0x%x",
+		   hapd->conf->mld_mld_capa);
+
+	wpabuf_put_le16(buf, hapd->conf->mld_mld_capa);
+
+	if (include_mld_id) {
+		wpa_printf(MSG_DEBUG, "MLD: MLD ID=0x%x", hapd->conf->mld_id);
+		wpabuf_put_u8(buf, hapd->conf->mld_id);
+	}
+
+	if (!info)
+		goto out;
+
+	/* Add link info for the other links */
+	for (link_id = 0; link_id < MAX_NUM_MLD_LINKS; link_id++) {
+		struct mld_link_info *link = &info->mld_info.links[link_id];
+		/*
+		 * control (2) + station info length (1) + MAC address (6) +
+		 * beacon interval (2) + TSF offset (8) + DTIM info (2) + BSS
+		 * parameters change counter (1) + station profile length.
+		 */
+		const size_t fixed_len = 22;
+		size_t total_len = fixed_len + link->resp_sta_profile_len;
+
+		/* skip the local one */
+		if (link_id == hapd->conf->mld_link_id || !link->valid)
+			continue;
+
+		wpabuf_put_u8(buf, EHT_ML_SUB_ELEM_PER_STA_PROFILE);
+
+		if (total_len <= 255)
+			wpabuf_put_u8(buf, total_len);
+		else
+			wpabuf_put_u8(buf, 255);
+
+		control = (link_id & 0xf) |
+			EHT_PER_STA_CTRL_MAC_ADDR_PRESENT_MSK |
+			EHT_PER_STA_CTRL_COMPLETE_PROFILE_MSK |
+			EHT_PER_STA_CTRL_TSF_OFFSET_PRESENT_MSK |
+			EHT_PER_STA_CTRL_BEACON_INTERVAL_PRESENT_MSK |
+			EHT_PER_STA_CTRL_DTIM_INFO_PRESENT_MSK |
+			EHT_PER_STA_CTRL_BSS_PARAM_CNT_PRESENT_MSK;
+
+		wpabuf_put_le16(buf, control);
+
+		/* STA info length */
+		wpabuf_put_u8(buf, fixed_len - 2);
+
+		wpabuf_put_data(buf, link->local_addr, ETH_ALEN);
+
+		/* TODO: currently assume same beacon interval */
+		wpabuf_put_le16(buf, hapd->iconf->beacon_int);
+
+		/*
+		 * TODO: currently setting TSF offset to zero. However this
+		 * information needs to come from the driver
+		 */
+		wpabuf_put_le32(buf, 0);
+		wpabuf_put_le32(buf, 0);
+
+		/* TODO: currently assume same DTIM information */
+		wpabuf_put_le16(buf, hapd->conf->dtim_period);
+
+		/* TODO: currently hard code the BSS change parameters to 0x1 */
+		wpabuf_put_u8(buf, 0x1);
+
+		/* Fragment the sub element if needed */
+		if (total_len <= 255) {
+			wpabuf_put_data(buf, link->resp_sta_profile,
+					link->resp_sta_profile_len);
+		} else {
+			ptr = link->resp_sta_profile;
+			len = link->resp_sta_profile_len;
+
+			slice_len = 255 - fixed_len;
+
+			wpabuf_put_data(buf, ptr, slice_len);
+			len -= slice_len;
+			ptr += slice_len;
+
+			while (len) {
+				if (len <= 255)
+					slice_len = len;
+				else
+					slice_len = 255;
+
+				wpabuf_put_u8(buf, EHT_ML_SUB_ELEM_FRAGMENT);
+				wpabuf_put_u8(buf, slice_len);
+				wpabuf_put_data(buf, ptr, slice_len);
+
+				len -= slice_len;
+				ptr += slice_len;
+			}
+		}
+	}
+
+out:
+	/* start the fragmentation */
+	len = wpabuf_len(buf);
+	ptr = wpabuf_head(buf);
+
+	if (len <= 254)
+		slice_len = len;
+	else
+		slice_len = 254;
+
+	*pos++ = WLAN_EID_EXTENSION;
+	*pos++ = slice_len + 1;
+	*pos++ = WLAN_EID_EXT_MULTI_LINK;
+	os_memcpy(pos, ptr, slice_len);
+
+	ptr += slice_len;
+	pos += slice_len;
+	len -= slice_len;
+
+	while (len) {
+		if (len <= 255)
+			slice_len = len;
+		else
+			slice_len = 255;
+
+		*pos++ = WLAN_EID_FRAGMENT;
+		*pos++ = slice_len;
+		os_memcpy(pos, ptr, slice_len);
+
+		ptr += slice_len;
+		pos += slice_len;
+		len -= slice_len;
+	}
+
+	wpabuf_free(buf);
+	return pos;
+}
diff --git a/src/ap/sta_info.h b/src/ap/sta_info.h
index b59b7584b3..06e68133bb 100644
--- a/src/ap/sta_info.h
+++ b/src/ap/sta_info.h
@@ -69,6 +69,35 @@  struct pending_eapol_rx {
 	enum frame_encryption encrypted;
 };
 
+#define EHT_ML_MAX_STA_PROF_LEN 1024
+struct mld_info {
+	bool mld_sta;
+
+	struct ml_common_info {
+		u8 mld_addr[ETH_ALEN];
+		u16 medium_sync_delay;
+		u16 eml_capa;
+		u16 mld_capa;
+	} common_info;
+
+	struct mld_link_info {
+		u8 valid;
+		u8 local_addr[ETH_ALEN];
+		u8 peer_addr[ETH_ALEN];
+
+		size_t nstr_bitmap_len;
+		u8 nstr_bitmap[2];
+
+		u16 capability;
+
+		u16 status;
+		size_t resp_sta_profile_len;
+		u8 resp_sta_profile[EHT_ML_MAX_STA_PROF_LEN];
+
+		const u8 *rsne, *rsnxe;
+	} links[MAX_NUM_MLD_LINKS];
+};
+
 struct sta_info {
 	struct sta_info *next; /* next entry in sta list */
 	struct sta_info *hnext; /* next entry in hash table list */
@@ -299,6 +328,11 @@  struct sta_info {
 #ifdef CONFIG_PASN
 	struct pasn_data *pasn;
 #endif /* CONFIG_PASN */
+
+#ifdef CONFIG_IEEE80211BE
+	struct mld_info mld_info;
+	u8 mld_assoc_link_id;
+#endif /* CONFIG_IEEE80211BE */
 };
 
 
diff --git a/src/common/ieee802_11_defs.h b/src/common/ieee802_11_defs.h
index 12ddad466f..45f03a8351 100644
--- a/src/common/ieee802_11_defs.h
+++ b/src/common/ieee802_11_defs.h
@@ -2623,6 +2623,16 @@  struct eht_ml_basic_common_info {
 #define EHT_ML_MLD_CAPA_FREQ_SEP_FOR_STR_MASK         0x0f80
 #define EHT_ML_MLD_CAPA_AAR_SUPP                      0x1000
 
+#define EHT_PER_STA_CTRL_LINK_ID_MSK                  0x000f
+#define EHT_PER_STA_CTRL_COMPLETE_PROFILE_MSK         0x0010
+#define EHT_PER_STA_CTRL_MAC_ADDR_PRESENT_MSK         0x0020
+#define EHT_PER_STA_CTRL_BEACON_INTERVAL_PRESENT_MSK  0x0040
+#define EHT_PER_STA_CTRL_TSF_OFFSET_PRESENT_MSK       0x0080
+#define EHT_PER_STA_CTRL_DTIM_INFO_PRESENT_MSK        0x0100
+#define EHT_PER_STA_CTRL_NSTR_LINK_PAIR_PRESENT_MSK   0x0200
+#define EHT_PER_STA_CTRL_NSTR_BM_SIZE_MSK             0x0400
+#define EHT_PER_STA_CTRL_BSS_PARAM_CNT_PRESENT_MSK    0x0800
+
 /* IEEE P802.11be/D2.0, 9.4.2.312.2.4 - Per-STA Profile subelement format */
 struct ieee80211_eht_per_sta_profile {
 	le16 sta_control;