AP: Add parsing of ML probe requests

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

Signed-off-by: Benjamin Berg <benjamin.berg@intel.com>
Signed-off-by: Andrei Otcheretianski <andrei.otcheretianski@intel.com>

diff --git a/src/ap/beacon.c b/src/ap/beacon.c
index 02f024103c9d94bd708458a6e6f42f36ebe0d4b7..d6fe2e34211a278272103e56df2c212ed651e15e 100644 (file)
--- a/src/ap/beacon.c
+++ b/src/ap/beacon.c
@@ -1081,6 +1081,90 @@ void sta_track_claim_taxonomy_info(struct hostapd_iface *iface, const u8 *addr,
 #endif /* CONFIG_TAXONOMY */
 
 
+#ifdef CONFIG_IEEE80211BE
+static 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;
+
+       if (ml_len < sizeof(struct ieee80211_eht_ml))
+               return false;
+
+       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 subelements (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;
+}
+#endif /* CONFIG_IEEE80211BE */
+
+
 void handle_probe_req(struct hostapd_data *hapd,
                      const struct ieee80211_mgmt *mgmt, size_t len,
                      int ssi_signal)
@@ -1096,6 +1180,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 /* CONFIG_IEEE80211BE */
 
        if (hapd->iconf->rssi_ignore_probe_request && ssi_signal &&
            ssi_signal < hapd->iconf->rssi_ignore_probe_request)
@@ -1327,6 +1415,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 */
+
        os_memset(&params, 0, sizeof(params));
        params.req = mgmt;
        params.is_p2p = !!elems.p2p;