nl80211: Get all requested MLO links information from (re)association events

Currently only accepted MLO links information is getting parsed from
(re)association events. Add support to parse all the requested MLO links
information including rejected links. Get the rejected MLO links
information from netlink attributes if the kernel supports indicating
per link status. Otherwise get the rejected MLO links information by
parsing (Re)association Request and Response frame elements.

Signed-off-by: Veerendranath Jakkam <quic_vjakkam@quicinc.com>

src/drivers/driver.h

@@ -2741,7 +2741,8 @@ struct weighted_pcl {
 };
 
 struct driver_sta_mlo_info {
-	u16 valid_links; /* bitmap of valid link IDs */
+	u16 req_links; /* bitmap of requested link IDs */
+	u16 valid_links; /* bitmap of accepted link IDs */
 	u8 assoc_link_id;
 	u8 ap_mld_addr[ETH_ALEN];
 	struct {

src/drivers/driver_nl80211_event.c

@@ -482,12 +482,16 @@ static void nl80211_parse_mlo_link_info(struct driver_sta_mlo_info *mlo,
 		if (link_id >= MAX_NUM_MLD_LINKS)
 			continue;
 
-		if (tb[NL80211_ATTR_STATUS_CODE] &&
+		if (tb[NL80211_ATTR_STATUS_CODE]) {
-		    nla_get_u16(tb[NL80211_ATTR_STATUS_CODE]) !=
+			/* Set requested links only when status indicated */
-		    WLAN_STATUS_SUCCESS)
+			mlo->req_links |= BIT(link_id);
-			continue;
+			if (nla_get_u16(tb[NL80211_ATTR_STATUS_CODE]) ==
+			    WLAN_STATUS_SUCCESS)
+				mlo->valid_links |= BIT(link_id);
+		} else {
+			mlo->valid_links |= BIT(link_id);
+		}
 
-		mlo->valid_links |= BIT(link_id);
 		os_memcpy(mlo->links[link_id].addr,
 			  nla_data(tb[NL80211_ATTR_MAC]), ETH_ALEN);
 		os_memcpy(mlo->links[link_id].bssid,
@@ -500,6 +504,140 @@ static void nl80211_parse_mlo_link_info(struct driver_sta_mlo_info *mlo,
 }
 
 
+struct links_info {
+	/* bitmap of link IDs in Per-STA profile subelements */
+	u16 non_assoc_links;
+	u8 addr[MAX_NUM_MLD_LINKS][ETH_ALEN];
+};
+
+
+static void nl80211_get_basic_mle_links_info(const u8 *mle, size_t mle_len,
+					     struct links_info *info)
+{
+	size_t rem_len;
+	const u8 *pos;
+
+	if (mle_len < MULTI_LINK_CONTROL_LEN + 1 ||
+	    mle_len - MULTI_LINK_CONTROL_LEN < mle[MULTI_LINK_CONTROL_LEN])
+		return;
+
+	/* Skip Common Info */
+	pos = mle + MULTI_LINK_CONTROL_LEN + mle[MULTI_LINK_CONTROL_LEN];
+	rem_len = mle_len -
+		(MULTI_LINK_CONTROL_LEN + mle[MULTI_LINK_CONTROL_LEN]);
+
+	/* Parse Subelements */
+	while (rem_len > 2) {
+		size_t ie_len = 2 + pos[1];
+
+		if (rem_len < ie_len)
+			break;
+
+		if (pos[0] == MULTI_LINK_SUB_ELEM_ID_PER_STA_PROFILE) {
+			u8 link_id;
+			const u8 *sta_profile;
+
+			if (pos[1] < BASIC_MLE_STA_PROF_STA_MAC_IDX + ETH_ALEN)
+				goto next_subelem;
+
+			sta_profile = &pos[2];
+			link_id = sta_profile[0] &
+				BASIC_MLE_STA_CTRL0_LINK_ID_MASK;
+			if (link_id >= MAX_NUM_MLD_LINKS)
+				goto next_subelem;
+
+			if (!(sta_profile[0] &
+			      BASIC_MLE_STA_CTRL0_PRES_STA_MAC))
+				goto next_subelem;
+
+			info->non_assoc_links |= BIT(link_id);
+			os_memcpy(info->addr[link_id],
+				  &sta_profile[BASIC_MLE_STA_PROF_STA_MAC_IDX],
+				  ETH_ALEN);
+		}
+next_subelem:
+		pos += ie_len;
+		rem_len -= ie_len;
+	}
+}
+
+
+static int nl80211_update_rejected_links_info(struct driver_sta_mlo_info *mlo,
+					      struct nlattr *req_ie,
+					      struct nlattr *resp_ie)
+{
+	int i;
+	struct wpabuf *mle;
+	struct ieee802_11_elems req_elems, resp_elems;
+	struct links_info req_info, resp_info;
+
+	if (!req_ie || !resp_ie) {
+		wpa_printf(MSG_INFO,
+			   "nl80211: MLO: (Re)Association Request/Response frame elements not available");
+		return -1;
+	}
+
+	if (ieee802_11_parse_elems(nla_data(req_ie), nla_len(req_ie),
+				   &req_elems, 0) == ParseFailed ||
+	    ieee802_11_parse_elems(nla_data(resp_ie), nla_len(resp_ie),
+				   &resp_elems, 0) == ParseFailed) {
+		wpa_printf(MSG_INFO,
+			   "nl80211: MLO: Failed to parse (Re)Association Request/Response elements");
+		return -1;
+	}
+
+	mle = ieee802_11_defrag_mle(&req_elems, MULTI_LINK_CONTROL_TYPE_BASIC);
+	if (!mle) {
+		wpa_printf(MSG_INFO,
+			   "nl80211: MLO: Basic Multi-Link element not found in Association Request");
+		return -1;
+	}
+	os_memset(&req_info, 0, sizeof(req_info));
+	nl80211_get_basic_mle_links_info(wpabuf_head(mle), wpabuf_len(mle),
+					 &req_info);
+	wpabuf_free(mle);
+
+	mle = ieee802_11_defrag_mle(&resp_elems, MULTI_LINK_CONTROL_TYPE_BASIC);
+	if (!mle) {
+		wpa_printf(MSG_ERROR,
+			   "nl80211: MLO: Basic Multi-Link element not found in Association Response");
+		return -1;
+	}
+	os_memset(&resp_info, 0, sizeof(resp_info));
+	nl80211_get_basic_mle_links_info(wpabuf_head(mle), wpabuf_len(mle),
+					 &resp_info);
+	wpabuf_free(mle);
+
+	if (req_info.non_assoc_links != resp_info.non_assoc_links) {
+		wpa_printf(MSG_ERROR,
+			   "nl80211: MLO: Association Request and Response links bitmaps not equal (0x%x != 0x%x)",
+			   req_info.non_assoc_links,
+			   resp_info.non_assoc_links);
+		return -1;
+	}
+
+	mlo->req_links = BIT(mlo->assoc_link_id) | req_info.non_assoc_links;
+	if ((mlo->req_links & mlo->valid_links) != mlo->valid_links) {
+		wpa_printf(MSG_ERROR,
+			   "nl80211: MLO: Accepted links are not a subset of requested links (req_links=0x%x valid_links=0x%x non_assoc_links=0x%x assoc_link_id=0x%x)",
+			   mlo->req_links, mlo->valid_links,
+			   req_info.non_assoc_links, BIT(mlo->assoc_link_id));
+		return -1;
+	}
+
+	/* Get MLO links info for rejected links */
+	for (i = 0; i < MAX_NUM_MLD_LINKS; i++) {
+		if (!((mlo->req_links & ~mlo->valid_links) & BIT(i)))
+			continue;
+
+		os_memcpy(mlo->links[i].bssid, resp_info.addr[i], ETH_ALEN);
+		os_memcpy(mlo->links[i].addr, req_info.addr[i], ETH_ALEN);
+	}
+
+	return 0;
+}
+
+
 static int nl80211_get_assoc_link_id(const u8 *data, u8 len)
 {
 	if (!(data[0] & BASIC_MULTI_LINK_CTRL0_PRES_LINK_ID))
@@ -520,6 +658,7 @@ static void nl80211_parse_mlo_info(struct wpa_driver_nl80211_data *drv,
 				   bool qca_roam_auth,
 				   struct nlattr *addr,
 				   struct nlattr *mlo_links,
+				   struct nlattr *req_ie,
 				   struct nlattr *resp_ie)
 {
 	const u8 *ml_ie;
@@ -554,9 +693,10 @@ static void nl80211_parse_mlo_info(struct wpa_driver_nl80211_data *drv,
 		nl80211_parse_qca_vendor_mlo_link_info(mlo, mlo_links);
 #endif /* CONFIG_DRIVER_NL80211_QCA */
 
-	if (!(mlo->valid_links & BIT(drv->sta_mlo_info.assoc_link_id))) {
+	if (!(mlo->valid_links & BIT(mlo->assoc_link_id)) ||
-		wpa_printf(MSG_ERROR, "nl80211: Invalid MLO assoc link ID %d",
+	    (!mlo->req_links &&
-			   drv->sta_mlo_info.assoc_link_id);
+	     nl80211_update_rejected_links_info(mlo, req_ie, resp_ie))) {
+		wpa_printf(MSG_INFO, "nl80211: Invalid MLO connection info");
 		mlo->valid_links = 0;
 		return;
 	}
@@ -675,7 +815,8 @@ static void mlme_event_connect(struct wpa_driver_nl80211_data *drv,
 
 	drv->associated = 1;
 	drv->sta_mlo_info.valid_links = 0;
-	nl80211_parse_mlo_info(drv, qca_roam_auth, addr, mlo_links, resp_ie);
+	nl80211_parse_mlo_info(drv, qca_roam_auth, addr, mlo_links, req_ie,
+			       resp_ie);
 	if (!drv->sta_mlo_info.valid_links && addr) {
 		os_memcpy(drv->bssid, nla_data(addr), ETH_ALEN);
 		os_memcpy(drv->prev_bssid, drv->bssid, ETH_ALEN);