[PATCH 19/42] AP: Add parsing of ML probe requests

[Andrei Otcheretianski]

From: Benjamin Berg <benjamin.berg at intel.com>

This adds the parsing of ML probe requests. Handling will be added by
a later patch.

Signed-off-by: Benjamin Berg <benjamin.berg at intel.com>
Signed-off-by: Andrei Otcheretianski <andrei.otcheretianski at intel.com>
---
 src/ap/beacon.c                | 14 ++++++
 src/common/ieee802_11_common.c | 79 ++++++++++++++++++++++++++++++++++
 src/common/ieee802_11_common.h |  2 +
 3 files changed, 95 insertions(+)

diff --git a/src/ap/beacon.c b/src/ap/beacon.c
index 232098328d..e8acedd32b 100644
--- a/src/ap/beacon.c
+++ b/src/ap/beacon.c
@@ -1095,6 +1095,10 @@ void handle_probe_req(struct hostapd_data *hapd,
 	size_t csa_offs_len;
 	struct radius_sta rad_info;
 	struct probe_resp_params params;
+#ifdef CONFIG_IEEE80211BE
+	int mld_id;
+	u16 links;
+#endif
 
 	if (hapd->iconf->rssi_ignore_probe_request && ssi_signal &&
 	    ssi_signal < hapd->iconf->rssi_ignore_probe_request)
@@ -1326,6 +1330,16 @@ void handle_probe_req(struct hostapd_data *hapd,
 	wpa_msg_ctrl(hapd->msg_ctx, MSG_INFO, RX_PROBE_REQUEST "sa=" MACSTR
 		     " signal=%d", MAC2STR(mgmt->sa), ssi_signal);
 
+#ifdef CONFIG_IEEE80211BE
+	if (hapd->conf->mld_ap && elems.probe_req_mle &&
+	    parse_ml_probe_req((struct ieee80211_eht_ml *)elems.probe_req_mle,
+			       elems.probe_req_mle_len, &mld_id, &links)) {
+		wpa_printf(MSG_DEBUG,
+			   "MLD: Got ML probe request with AP MLD ID %d for links %04x",
+			   mld_id, links);
+	}
+#endif /* CONFIG_IEEE80211BE */
+
 	params.req = mgmt;
 	params.is_p2p = !!elems.p2p;
 	params.known_bss = elems.mbssid_known_bss;
diff --git a/src/common/ieee802_11_common.c b/src/common/ieee802_11_common.c
index 06f82ce303..27d3ff41e5 100644
--- a/src/common/ieee802_11_common.c
+++ b/src/common/ieee802_11_common.c
@@ -3212,6 +3212,85 @@ struct wpabuf * ieee802_11_defrag(const u8 *data, size_t len, bool ext_elem)
 }
 
 
+bool parse_ml_probe_req(const struct ieee80211_eht_ml *ml, size_t ml_len,
+			int *mld_id, u16 *links)
+{
+	u16 ml_control;
+	const struct element *sub;
+	const u8 *pos;
+	size_t len;
+
+	*mld_id = -1;
+	*links = 0xffff;
+
+	ml_control = le_to_host16(ml->ml_control);
+
+	if ((ml_control & MULTI_LINK_CONTROL_TYPE_MASK) !=
+	    MULTI_LINK_CONTROL_TYPE_PROBE_REQ) {
+		wpa_printf(MSG_DEBUG, "MLD: not an ML probe req");
+		return false;
+	}
+
+	if (sizeof(struct ieee80211_eht_ml) + 1 > ml_len) {
+		wpa_printf(MSG_DEBUG, "MLD: ML probe req too short");
+		return false;
+	}
+
+	pos = ml->variable;
+	len = pos[0];
+	if (len < 1 || sizeof(struct ieee80211_eht_ml) + len > ml_len) {
+		wpa_printf(MSG_DEBUG, "MLD: ML probe request with invalid length");
+		return false;
+	}
+
+	if (ml_control & EHT_ML_PRES_BM_PROBE_REQ_AP_MLD_ID) {
+		if (len < 2) {
+			wpa_printf(MSG_DEBUG,
+				   "MLD: ML probe req too short for MLD ID");
+			return false;
+		}
+
+		*mld_id = pos[1];
+	}
+	pos += len;
+
+	/* Parse sub-elements (if there are any) */
+	len = ml_len - len - sizeof(struct ieee80211_eht_ml);
+	for_each_element_id(sub, 0, pos, len) {
+		const struct ieee80211_eht_per_sta_profile *sta;
+		u16 sta_control;
+
+		if (*links == 0xffff)
+			*links = 0;
+
+		if (sub->datalen <
+		    sizeof(struct ieee80211_eht_per_sta_profile)) {
+			wpa_printf(MSG_DEBUG,
+				   "MLD: ML probe req %d too short for sta profile",
+				   sub->datalen);
+			return false;
+		}
+
+		sta = (struct ieee80211_eht_per_sta_profile *)sub->data;
+
+		/*
+		 * Extract the link ID, do not return whether a complete or
+		 * partial profile was requested.
+		 */
+		sta_control = le_to_host16(sta->sta_control);
+		*links |= BIT(sta_control & EHT_PER_STA_CTRL_LINK_ID_MSK);
+	}
+
+	if (!for_each_element_completed(sub, pos, len)) {
+		wpa_printf(MSG_DEBUG,
+			   "MLD: ML probe req sub-elements parsing error");
+		return false;
+	}
+
+	return true;
+}
+
+
 const u8 * get_ml_ie(const u8 *ies, size_t len, u8 type)
 {
 	const struct element *elem;
diff --git a/src/common/ieee802_11_common.h b/src/common/ieee802_11_common.h
index a7d407b65d..6e7d770c0b 100644
--- a/src/common/ieee802_11_common.h
+++ b/src/common/ieee802_11_common.h
@@ -350,6 +350,8 @@ int ieee802_edmg_is_allowed(struct ieee80211_edmg_config allowed,
 			    struct ieee80211_edmg_config requested);
 
 struct wpabuf * ieee802_11_defrag(const u8 *data, size_t len, bool ext_elem);
+bool parse_ml_probe_req(const struct ieee80211_eht_ml *ml, size_t ml_len,
+			int *mld_id, u16 *links);
 const u8 * get_ml_ie(const u8 *ies, size_t len, u8 type);
 const u8 * get_basic_mle_mld_addr(const u8 *buf, size_t len);
 
-- 
2.38.1