diff --git a/src/rsn_supp/tdls.c b/src/rsn_supp/tdls.c index 8eb74ac22..5f7607b46 100644 --- a/src/rsn_supp/tdls.c +++ b/src/rsn_supp/tdls.c @@ -159,6 +159,8 @@ struct wpa_tdls_peer { /* channel switch currently enabled */ int chan_switch_enabled; + + int mld_link_id; }; @@ -737,6 +739,7 @@ static void wpa_tdls_peer_clear(struct wpa_sm *sm, struct wpa_tdls_peer *peer) os_memset(&peer->tpk, 0, sizeof(peer->tpk)); os_memset(peer->inonce, 0, WPA_NONCE_LEN); os_memset(peer->rnonce, 0, WPA_NONCE_LEN); + peer->mld_link_id = -1; } @@ -1075,6 +1078,7 @@ wpa_tdls_add_peer(struct wpa_sm *sm, const u8 *addr, int *existing) return NULL; os_memcpy(peer->addr, addr, ETH_ALEN); + peer->mld_link_id = -1; peer->next = sm->tdls; sm->tdls = peer; @@ -1544,6 +1548,38 @@ skip_ies: } +static bool wpa_tdls_is_lnkid_bss_valid(struct wpa_sm *sm, + const struct wpa_tdls_lnkid *lnkid, + int *link_id) +{ + *link_id = -1; + + if (!sm->mlo.valid_links) { + if (os_memcmp(sm->bssid, lnkid->bssid, ETH_ALEN) != 0) + return false; + } else { + int i; + + for (i = 0; i < MAX_NUM_MLD_LINKS; i++) { + if ((sm->mlo.valid_links & BIT(i)) && + os_memcmp(lnkid->bssid, sm->mlo.links[i].bssid, + ETH_ALEN) == 0) { + *link_id = i; + break; + } + } + if (*link_id < 0) { + wpa_printf(MSG_DEBUG, + "TDLS: MLD link not found for linkid BSS " + MACSTR, MAC2STR(lnkid->bssid)); + return false; + } + } + + return true; +} + + static int wpa_tdls_process_discovery_request(struct wpa_sm *sm, const u8 *addr, const u8 *buf, size_t len) @@ -3120,6 +3156,63 @@ int wpa_tdls_is_external_setup(struct wpa_sm *sm) } +int wpa_tdls_process_discovery_response(struct wpa_sm *sm, const u8 *addr, + const u8 *buf, size_t len) +{ + struct ieee802_11_elems elems; + struct wpa_tdls_lnkid lnkid; + struct wpa_tdls_peer *peer; + size_t min_req_len = 1 /* Dialog Token */ + 2 /* Capability */ + + sizeof(struct wpa_tdls_lnkid); + int link_id = -1; + + wpa_printf(MSG_DEBUG, "TDLS: Process Discovery Response from " MACSTR, + MAC2STR(addr)); + + if (len < min_req_len) { + wpa_printf(MSG_DEBUG, "TDLS Discovery Resp is too short: %zu", + len); + return -1; + } + + /* Elements start after the three octets of fixed field (one octet for + * the Dialog Token field and two octets for the Capability field. */ + if (ieee802_11_parse_elems(buf + 3, len - 3, &elems, 1) == + ParseFailed) { + wpa_printf(MSG_DEBUG, + "TDLS: Failed to parse IEs in Discovery Response"); + return -1; + } + + if (!elems.link_id) { + wpa_printf(MSG_DEBUG, + "TDLS: Link Identifier element not found in Discovery Response"); + return -1; + } + + os_memcpy(&lnkid.bssid[0], elems.link_id, sizeof(lnkid) - 2); + + if (!wpa_tdls_is_lnkid_bss_valid(sm, &lnkid, &link_id)) { + wpa_printf(MSG_DEBUG, + "TDLS: Discovery Response from different BSS " + MACSTR, MAC2STR(lnkid.bssid)); + return -1; + } + + peer = wpa_tdls_add_peer(sm, addr, NULL); + if (!peer) { + wpa_printf(MSG_DEBUG, "TDLS: Could not add peer entry"); + return -1; + } + + peer->mld_link_id = link_id; + wpa_printf(MSG_DEBUG, "TDLS: Link identifier BSS: " MACSTR + " , link id: %u", MAC2STR(lnkid.bssid), link_id); + + return 0; +} + + int wpa_tdls_enable_chan_switch(struct wpa_sm *sm, const u8 *addr, u8 oper_class, struct hostapd_freq_params *freq_params) diff --git a/src/rsn_supp/wpa.h b/src/rsn_supp/wpa.h index 36ddab3fe..dfcc3aa02 100644 --- a/src/rsn_supp/wpa.h +++ b/src/rsn_supp/wpa.h @@ -574,6 +574,8 @@ int wpa_tdls_enable_chan_switch(struct wpa_sm *sm, const u8 *addr, u8 oper_class, struct hostapd_freq_params *freq_params); int wpa_tdls_disable_chan_switch(struct wpa_sm *sm, const u8 *addr); +int wpa_tdls_process_discovery_response(struct wpa_sm *sm, const u8 *addr, + const u8 *buf, size_t len); #ifdef CONFIG_TDLS_TESTING extern unsigned int tdls_testing; #endif /* CONFIG_TDLS_TESTING */ diff --git a/wpa_supplicant/events.c b/wpa_supplicant/events.c index 41e25f077..32cf24c79 100644 --- a/wpa_supplicant/events.c +++ b/wpa_supplicant/events.c @@ -4771,6 +4771,12 @@ static void wpas_event_rx_mgmt_action(struct wpa_supplicant *wpa_s, wpa_dbg(wpa_s, MSG_DEBUG, "TDLS: Received Discovery Response from " MACSTR, MAC2STR(mgmt->sa)); + if (wpa_s->valid_links && + wpa_tdls_process_discovery_response(wpa_s->wpa, mgmt->sa, + &payload[1], plen - 1)) + wpa_dbg(wpa_s, MSG_ERROR, + "TDLS: Discovery Response process failed for " + MACSTR, MAC2STR(mgmt->sa)); return; } #endif /* CONFIG_TDLS */