qcacld-3.0: Enhance logic to support mlo 2+ links

Enhance the code logic to support mlo 2+ links.

Change-Id: I618d789a502f0577d62d62243a25e7324e685315
CRs-Fixed: 3193628

components/umac/mlme/connection_mgr/core/src/wlan_cm_vdev_connect.c

@@ -1122,7 +1122,7 @@ cm_get_ml_partner_info(struct scan_cache_entry *scan_entry,
 		return QDF_STATUS_E_INVAL;
 	}
 	mlo_support_link_num = wlan_mlme_get_sta_mlo_conn_max_num(psoc);
-	mlme_debug("mlo support link num: %d", mlo_support_link_num);
+	mlme_debug("sta mlo support link num: %d", mlo_support_link_num);
 
 	/* TODO: Make sure that scan_entry->ml_info->link_info is a sorted
 	 * list.
@@ -1132,9 +1132,10 @@ cm_get_ml_partner_info(struct scan_cache_entry *scan_entry,
 	 * entry should be cleared to not affect next connect request.
 	 */
 	for (i = 0; i < scan_entry->ml_info.num_links; i++) {
-		mlme_debug("freq: %d, link id: %d "QDF_MAC_ADDR_FMT,
+		mlme_debug("freq: %d, link id: %d is valid %d "QDF_MAC_ADDR_FMT,
 			   scan_entry->ml_info.link_info[i].freq,
 			   scan_entry->ml_info.link_info[i].link_id,
+			   scan_entry->ml_info.link_info[i].is_valid_link,
 			   QDF_MAC_ADDR_REF(
 			   scan_entry->ml_info.link_info[i].link_addr.bytes));
 		if (j >= mlo_support_link_num - 1)
@@ -1152,7 +1153,7 @@ cm_get_ml_partner_info(struct scan_cache_entry *scan_entry,
 		}
 	}
 	partner_info->num_partner_links = j;
-	mlme_debug("partner link num: %d", j);
+	mlme_debug("sta and ap integrate link num: %d", j);
 
 	wlan_objmgr_psoc_release_ref(psoc, WLAN_MLME_CM_ID);
 

core/hdd/src/wlan_hdd_mlo.c

@@ -172,13 +172,15 @@ void hdd_wlan_register_mlo_interfaces(struct hdd_context *hdd_ctx)
 		/* if target supports MLO create a new dev */
 		params.only_wdev_register = true;
 		params.associate_with_ml_adapter = false;
-		status = hdd_open_adapter_no_trans(hdd_ctx, QDF_STA_MODE,
-						   "null", mac_addr, &params);
+		status = hdd_open_adapter_no_trans(hdd_ctx,
+						   QDF_STA_MODE,
+						   "null", mac_addr,
+						   &params);
 		if (QDF_IS_STATUS_ERROR(status))
 			hdd_err("Failed to register link adapter:%d", status);
 	}
 
-	qdf_mem_zero(&params, sizeof(struct hdd_adapter_create_param));
+	qdf_mem_zero(&params, sizeof(params));
 	params.only_wdev_register  = true;
 	params.associate_with_ml_adapter = true;
 	mac_addr = wlan_hdd_get_intf_addr(hdd_ctx, QDF_STA_MODE);

core/mac/src/include/parser_api.h

@@ -1435,6 +1435,16 @@ QDF_STATUS populate_dot11f_assoc_rsp_mlo_ie(struct mac_context *mac_ctx,
 QDF_STATUS populate_dot11f_bcn_mlo_ie(struct mac_context *mac_ctx,
 				      struct pe_session *session);
 
+/**
+ * populate_dot11f_probe_req_mlo_ie() - populate mlo ie for probe req
+ * @mac_ctx: Global MAC context
+ * @session: PE session
+ *
+ * Return: QDF_STATUS_SUCCESS of no error
+ */
+QDF_STATUS populate_dot11f_probe_req_mlo_ie(struct mac_context *mac_ctx,
+					    struct pe_session *session);
+
 /**
  * populate_dot11f_mlo_rnr() - populate rnr for mlo
  * @mac_ctx: Global MAC context

core/mac/src/pe/lim/lim_api.c

@@ -2648,6 +2648,29 @@ lim_check_ft_initial_im_association(struct roam_offload_synch_ind *roam_synch,
 	}
 }
 
+#ifdef WLAN_FEATURE_11BE_MLO
+static void
+lim_mlo_roam_copy_partner_info_to_session(struct pe_session *session,
+					  struct roam_offload_synch_ind *sync_ind)
+{
+	uint8_t i;
+	struct mlo_partner_info partner_info;
+
+	if (!sync_ind)
+		return;
+
+	for (i = 0; i < sync_ind->num_setup_links; i++) {
+		partner_info.partner_link_info[i].link_id =
+			sync_ind->ml_link[i].link_id;
+		qdf_copy_macaddr(
+			&partner_info.partner_link_info[i].link_addr,
+			&sync_ind->ml_link[i].link_addr);
+	}
+
+	session->ml_partner_info = partner_info;
+	session->ml_partner_info.num_partner_links = sync_ind->num_setup_links;
+}
+
 static QDF_STATUS
 lim_gen_link_specific_assoc_rsp(struct mac_context *mac_ctx,
 				struct pe_session *session_entry,
@@ -2657,6 +2680,8 @@ lim_gen_link_specific_assoc_rsp(struct mac_context *mac_ctx,
 	struct element_info link_reassoc_rsp;
 	struct qdf_mac_addr sta_link_addr;
 	QDF_STATUS status = QDF_STATUS_SUCCESS;
+	uint8_t idx = 0;
+	uint8_t link_id;
 
 	link_reassoc_rsp.ptr = qdf_mem_malloc(reassoc_rsp_len);
 	if (!link_reassoc_rsp.ptr)
@@ -2667,28 +2692,53 @@ lim_gen_link_specific_assoc_rsp(struct mac_context *mac_ctx,
 
 	link_reassoc_rsp.len = reassoc_rsp_len;
 
-	status = util_gen_link_assoc_rsp(reassoc_rsp + WLAN_MAC_HDR_LEN_3A,
+	for (idx = 0;
+	     idx < session_entry->ml_partner_info.num_partner_links;
+	     idx++) {
+		link_id =
+		 session_entry->ml_partner_info.partner_link_info[idx].link_id;
+		status =
+		 util_gen_link_assoc_rsp(reassoc_rsp + WLAN_MAC_HDR_LEN_3A,
 					 reassoc_rsp_len - WLAN_MAC_HDR_LEN_3A,
 					 true,
+					 link_id,
 					 sta_link_addr,
 					 link_reassoc_rsp.ptr,
 					 reassoc_rsp_len,
 					 (qdf_size_t *)&link_reassoc_rsp.len);
 
-	if (QDF_IS_STATUS_ERROR(status)) {
-		pe_err("MLO ROAM: Link reassoc generation failed %d", status);
-		goto end;
-	}
+		if (QDF_IS_STATUS_ERROR(status)) {
+			pe_err("MLO ROAM: Link reassoc generation failed %d",
+			       status);
+			goto end;
+		}
 
-	lim_process_assoc_rsp_frame(mac_ctx, link_reassoc_rsp.ptr,
+		lim_process_assoc_rsp_frame(mac_ctx, link_reassoc_rsp.ptr,
 				    link_reassoc_rsp.len - SIR_MAC_HDR_LEN_3A,
 				    LIM_REASSOC, session_entry);
+	}
 end:
 	qdf_mem_free(link_reassoc_rsp.ptr);
 	link_reassoc_rsp.len = 0;
 	return status;
 }
 
+#else
+static inline void
+lim_mlo_roam_copy_partner_info_to_session(struct pe_session *session,
+					struct roam_offload_synch_ind *sync_ind)
+{}
+
+static QDF_STATUS
+lim_gen_link_specific_assoc_rsp(struct mac_context *mac_ctx,
+				struct pe_session *session_entry,
+				uint8_t *reassoc_rsp,
+				uint32_t reassoc_rsp_len)
+{
+	return QDF_STATUS_E_NOSUPPORT;
+}
+#endif
+
 #ifdef WLAN_FEATURE_11AX
 static void pe_roam_fill_obss_scan_param(struct pe_session *src_session,
 					 struct pe_session *dst_session)
@@ -2991,6 +3041,9 @@ pe_roam_synch_callback(struct mac_context *mac_ctx,
 	if (roam_sync_ind_ptr->auth_status == ROAM_AUTH_STATUS_AUTHENTICATED)
 		curr_sta_ds->is_key_installed = true;
 
+	lim_mlo_roam_copy_partner_info_to_session(ft_session_ptr,
+						  roam_sync_ind_ptr);
+
 	reassoc_resp = (uint8_t *)roam_sync_ind_ptr +
 			roam_sync_ind_ptr->reassoc_resp_offset;
 
@@ -3779,24 +3832,6 @@ lim_validate_probe_rsp_link_info(struct pe_session *session_entry,
 		}
 	}
 
-	/* WAR: currently util_gen_link_reqrsp_cmn generates only the first
-	 * partner link and DUT only 2 mlo link is supported, so let's just
-	 * comparing the first partner info to check whether mlo is possible.
-	 */
-	if (ml_partner_info.num_partner_links == 1) {
-		if (partner_info.partner_link_info[0].link_id ==
-		    ml_partner_info.partner_link_info[0].link_id &&
-		    (qdf_is_macaddr_equal(&ml_partner_info.partner_link_info[0].link_addr,
-					  &partner_info.partner_link_info[0].link_addr)))
-			status = QDF_STATUS_SUCCESS;
-		else
-			status = QDF_STATUS_E_PROTO;
-
-		pe_debug("DUT num partner: %d, AP num partner: %d, status: %d",
-			 ml_partner_info.num_partner_links,
-			 partner_info.num_partner_links, status);
-	}
-
 	return status;
 }
 
@@ -3984,6 +4019,8 @@ lim_gen_link_specific_probe_rsp(struct mac_context *mac_ctx,
 	uint8_t chan;
 	uint8_t op_class;
 	uint16_t chan_freq, gen_frame_len;
+	uint8_t idx;
+	uint8_t req_link_id;
 
 	if (!session_entry)
 		return QDF_STATUS_E_NULL_VALUE;
@@ -3999,13 +4036,17 @@ lim_gen_link_specific_probe_rsp(struct mac_context *mac_ctx,
 
 	if (session_entry->lim_join_req->is_ml_probe_req_sent &&
 	    rcvd_probe_resp->mlo_ie.mlo_ie_present) {
-
 		session_entry->lim_join_req->is_ml_probe_req_sent = false;
 
+		partner_info = &session_entry->lim_join_req->partner_info;
+		if (!partner_info->num_partner_links) {
+			pe_err("STA doesn't have any partner link information");
+			return QDF_STATUS_E_FAILURE;
+		}
+
 		status = lim_validate_probe_rsp_link_info(session_entry,
 							  probe_rsp,
 							  probe_rsp_len);
-
 		if (QDF_IS_STATUS_ERROR(status)) {
 			if(QDF_IS_STATUS_ERROR(
 				lim_check_scan_db_for_join_req_partner_info(
@@ -4040,77 +4081,84 @@ lim_gen_link_specific_probe_rsp(struct mac_context *mac_ctx,
 			return QDF_STATUS_E_NOMEM;
 		}
 
+		link_probe_rsp.len = gen_frame_len;
 		qdf_mem_copy(&sta_link_addr, session_entry->self_mac_addr,
 			     QDF_MAC_ADDR_SIZE);
 
-		link_probe_rsp.len = gen_frame_len;
-		status = util_gen_link_probe_rsp(probe_rsp,
-				probe_rsp_len, sta_link_addr,
-				link_probe_rsp.ptr, gen_frame_len,
-				(qdf_size_t *)&link_probe_rsp.len);
-
-		if (QDF_IS_STATUS_ERROR(status)) {
-			pe_err("MLO: Link probe response generation failed %d",
-					status);
-			if(QDF_IS_STATUS_ERROR(
-				lim_check_scan_db_for_join_req_partner_info(
-					session_entry,
-					mac_ctx)))
-				lim_clear_ml_partner_info(session_entry);
-
-			goto end;
-		}
-
-		pe_debug("MLO: link probe rsp size:%u original probe rsp :%u",
-			 link_probe_rsp.len, probe_rsp_len);
-
-		/* Currently only 2 link mlo is supported */
-		link_info = &partner_info->partner_link_info[0];
-		wlan_get_chan_by_bssid_from_rnr(session_entry->vdev,
-						session_entry->cm_id,
-						&link_info->link_addr,
-						&chan, &op_class);
-		if (!chan)
-			wlan_get_chan_by_link_id_from_rnr(session_entry->vdev,
-							  session_entry->cm_id,
-							  link_info->link_id,
-							  &chan, &op_class);
-		if (!chan) {
-			pe_err("Invalid link id %d link mac: " QDF_MAC_ADDR_FMT,
-			       link_info->link_id,
-			       QDF_MAC_ADDR_REF(link_info->link_addr.bytes));
-			if(QDF_IS_STATUS_ERROR(
-				lim_check_scan_db_for_join_req_partner_info(
-					session_entry,
-					mac_ctx)))
-				lim_clear_ml_partner_info(session_entry);
-
-			status = QDF_STATUS_E_FAILURE;
-			goto end;
-		}
-		chan_freq = wlan_reg_chan_opclass_to_freq(chan, op_class,
-							  true);
-
-		status = lim_add_bcn_probe(session_entry->vdev,
-				link_probe_rsp.ptr,
-				link_probe_rsp.len,
-				chan_freq, rssi);
-		if (QDF_IS_STATUS_ERROR(status)) {
-			pe_err("failed to add bcn probe %d", status);
-			if(QDF_IS_STATUS_ERROR(
-				lim_check_scan_db_for_join_req_partner_info(
-					session_entry,
-					mac_ctx)))
-				lim_clear_ml_partner_info(session_entry);
+		for (idx = 0; idx < partner_info->num_partner_links; idx++) {
+			req_link_id =
+				partner_info->partner_link_info[idx].link_id;
+			status = util_gen_link_probe_rsp(probe_rsp,
+					probe_rsp_len, req_link_id,
+					sta_link_addr, link_probe_rsp.ptr,
+					gen_frame_len,
+					(qdf_size_t *)&link_probe_rsp.len);
+
+			if (QDF_IS_STATUS_ERROR(status)) {
+				pe_err("MLO: Link %d probe resp gen failed %d",
+				       req_link_id, status);
+				status =
+				   lim_check_scan_db_for_join_req_partner_info(
+							session_entry, mac_ctx);
+				if (QDF_IS_STATUS_ERROR(status))
+				       lim_clear_ml_partner_info(session_entry);
+
+				goto end;
+			}
 
-			goto end;
+			pe_debug("MLO:link probe rsp size:%u orig probe rsp:%u",
+				 link_probe_rsp.len, probe_rsp_len);
+
+			link_info = &partner_info->partner_link_info[idx];
+			wlan_get_chan_by_bssid_from_rnr(session_entry->vdev,
+							session_entry->cm_id,
+							&link_info->link_addr,
+							&chan, &op_class);
+			if (!chan)
+				wlan_get_chan_by_link_id_from_rnr(
+							session_entry->vdev,
+							session_entry->cm_id,
+							link_info->link_id,
+							&chan, &op_class);
+			if (!chan) {
+				pe_err("Invalid link id %d link mac: " QDF_MAC_ADDR_FMT,
+				  link_info->link_id,
+				  QDF_MAC_ADDR_REF(link_info->link_addr.bytes));
+				status =
+				   lim_check_scan_db_for_join_req_partner_info(
+					session_entry, mac_ctx);
+				if (QDF_IS_STATUS_ERROR(status))
+				       lim_clear_ml_partner_info(session_entry);
+
+				status = QDF_STATUS_E_FAILURE;
+				goto end;
+			}
+			chan_freq =
+				wlan_reg_chan_opclass_to_freq(chan, op_class,
+							      true);
+
+			status = lim_add_bcn_probe(session_entry->vdev,
+						   link_probe_rsp.ptr,
+						   link_probe_rsp.len,
+						   chan_freq, rssi);
+			if (QDF_IS_STATUS_ERROR(status)) {
+				pe_err("failed to add bcn probe %d", status);
+				status =
+				   lim_check_scan_db_for_join_req_partner_info(
+					session_entry, mac_ctx);
+				if (QDF_IS_STATUS_ERROR(status))
+				       lim_clear_ml_partner_info(session_entry);
+
+				goto end;
+			}
 		}
 	} else if (session_entry->lim_join_req->is_ml_probe_req_sent &&
-			!rcvd_probe_resp->mlo_ie.mlo_ie_present) {
-		if(QDF_IS_STATUS_ERROR(lim_check_scan_db_for_join_req_partner_info(
-						session_entry,
-						mac_ctx)))
-				lim_clear_ml_partner_info(session_entry);
+		   !rcvd_probe_resp->mlo_ie.mlo_ie_present) {
+		status =
+			lim_check_scan_db_for_join_req_partner_info(
+						session_entry, mac_ctx);
+		if (QDF_IS_STATUS_ERROR(status))
+			lim_clear_ml_partner_info(session_entry);
 
 		status = QDF_STATUS_E_FAILURE;
 		return status;
@@ -4120,6 +4168,7 @@ lim_gen_link_specific_probe_rsp(struct mac_context *mac_ctx,
 end:
 	if (link_probe_rsp.ptr)
 		qdf_mem_free(link_probe_rsp.ptr);
+	link_probe_rsp.ptr = NULL;
 	link_probe_rsp.len = 0;
 	return status;
 }
@@ -4137,6 +4186,7 @@ lim_gen_link_probe_rsp_roam(struct mac_context *mac_ctx,
 	uint32_t frame_len;
 	struct wlan_frame_hdr *hdr;
 	uint16_t gen_frame_len;
+	uint32_t idx, link_id;
 
 	if (!session || !roam_sync_ind)
 		return QDF_STATUS_E_NULL_VALUE;
@@ -4201,37 +4251,41 @@ lim_gen_link_probe_rsp_roam(struct mac_context *mac_ctx,
 			     QDF_MAC_ADDR_SIZE);
 
 		link_probe_rsp.len = gen_frame_len;
-		status = util_gen_link_probe_rsp(frame, frame_len,
-				sta_link_addr, link_probe_rsp.ptr,
-				gen_frame_len,
-				(qdf_size_t *)&link_probe_rsp.len);
-		if (QDF_IS_STATUS_ERROR(status)) {
-			pe_err("MLO: Link probe response generation failed %d",
-			       status);
-			status = QDF_STATUS_E_FAILURE;
-			goto end;
-		}
-		pe_debug("MLO: link probe rsp size:%u original probe rsp :%u",
-			 link_probe_rsp.len, frame_len);
+		for (idx = 0; idx < roam_sync_ind->num_setup_links; idx++) {
+			link_id =  roam_sync_ind->ml_link[idx].link_id;
+			status = util_gen_link_probe_rsp(frame, frame_len,
+					     link_id, sta_link_addr,
+					     link_probe_rsp.ptr, gen_frame_len,
+					     (qdf_size_t *)&link_probe_rsp.len);
+			if (QDF_IS_STATUS_ERROR(status)) {
+				pe_err("MLO: Link %d probe resp gen failed %d",
+				       link_id, status);
+				status = QDF_STATUS_E_FAILURE;
+				continue;
+			}
+			pe_debug("MLO: link probe rsp size:%u orig probe rsp :%u",
+				 link_probe_rsp.len, frame_len);
 
-		src_addr = lim_get_src_addr_from_frame(&link_probe_rsp);
-		if (!src_addr) {
-			pe_err("MLO: Failed to fetch src address");
-			status = QDF_STATUS_E_FAILURE;
-			goto end;
+			src_addr = lim_get_src_addr_from_frame(&link_probe_rsp);
+			if (!src_addr) {
+				pe_err("MLO: Failed to fetch src address");
+				status = QDF_STATUS_E_FAILURE;
+				continue;
+			}
+			lim_add_bcn_probe(session->vdev, link_probe_rsp.ptr,
+					  link_probe_rsp.len,
+					  mlo_roam_get_link_freq_from_mac_addr(
+						       roam_sync_ind, src_addr),
+					  roam_sync_ind->rssi);
 		}
-		lim_add_bcn_probe(session->vdev, link_probe_rsp.ptr,
-				  link_probe_rsp.len,
-				  mlo_roam_get_link_freq_from_mac_addr(
-						 roam_sync_ind, src_addr),
-				  roam_sync_ind->rssi);
 	} else {
 		qdf_mem_free(probe_rsp);
 		return status;
 	}
-end:
+
 	if (link_probe_rsp.ptr)
 		qdf_mem_free(link_probe_rsp.ptr);
+	link_probe_rsp.ptr = NULL;
 	link_probe_rsp.len = 0;
 	qdf_mem_free(probe_rsp);
 	return status;
@@ -4329,7 +4383,7 @@ lim_process_cu_for_probe_rsp(struct mac_context *mac_ctx,
 		 * updated.
 		 */
 		status =
-		     util_gen_link_probe_rsp(probe_rsp, probe_rsp_len,
+		     util_gen_link_probe_rsp(probe_rsp, probe_rsp_len, link_id,
 					     sta_link_addr, link_probe_rsp.ptr,
 					     probe_rsp_len,
 					     (qdf_size_t *)&link_probe_rsp.len);

core/mac/src/pe/lim/lim_mlo.c

@@ -626,6 +626,7 @@ QDF_STATUS lim_mlo_proc_assoc_req_frm(struct wlan_objmgr_vdev *vdev,
 	qdf_copy_macaddr(&link_bssid, (struct qdf_mac_addr *)session->bssId);
 	status = util_gen_link_assoc_req(
 				frm_body, frame_len, sub_type == LIM_REASSOC,
+				0,
 				link_bssid,
 				qdf_nbuf_data(assoc_req->assoc_req_buf),
 				qdf_nbuf_len(assoc_req->assoc_req_buf),
@@ -1264,6 +1265,22 @@ lim_send_bcn_frame_mlo(struct mac_context *mac_ctx,
 	return session->mlo_ie_total_len;
 }
 
+uint16_t
+lim_send_probe_req_frame_mlo(struct mac_context *mac_ctx,
+			     struct pe_session *session)
+{
+	QDF_STATUS status;
+
+	session->mlo_ie_total_len = 0;
+	qdf_mem_zero(&session->mlo_ie, sizeof(session->mlo_ie));
+	status = populate_dot11f_probe_req_mlo_ie(mac_ctx, session);
+	if (QDF_IS_STATUS_SUCCESS(status))
+		session->mlo_ie_total_len =
+				lim_caculate_mlo_ie_length(&session->mlo_ie);
+
+	return session->mlo_ie_total_len;
+}
+
 uint16_t
 lim_get_frame_mlo_ie_len(struct pe_session *session)
 {

core/mac/src/pe/lim/lim_mlo.h

@@ -302,6 +302,17 @@ lim_send_assoc_rsp_mgmt_frame_mlo(struct mac_context *mac_ctx,
 uint16_t
 lim_send_bcn_frame_mlo(struct mac_context *mac_ctx, struct pe_session *session);
 
+/**
+ * lim_send_probe_req_frame_mlo() - Prepare ML IE for probe req frame
+ * @mac_ctx: pointer to mac_context
+ * @session: pointer to pe_session
+ *
+ * Return: the actual ML IE length
+ */
+uint16_t
+lim_send_probe_req_frame_mlo(struct mac_context *mac_ctx,
+			     struct pe_session *session);
+
 /**
  * lim_get_frame_mlo_ie_len() - get ML IE length
  * @session: pointer to pe_session
@@ -470,6 +481,13 @@ lim_send_assoc_rsp_mgmt_frame_mlo(struct mac_context *mac_ctx,
 	return 0;
 }
 
+static inline uint16_t
+lim_send_probe_req_frame_mlo(struct mac_context *mac_ctx,
+			     struct pe_session *session)
+{
+	return 0;
+}
+
 static inline uint16_t
 lim_send_bcn_frame_mlo(struct mac_context *mac_ctx, struct pe_session *session)
 {

core/mac/src/pe/lim/lim_process_mlm_rsp_messages.c

@@ -2770,18 +2770,21 @@ static QDF_STATUS
 lim_process_switch_channel_join_mlo(struct pe_session *session_entry,
 				    struct mac_context *mac_ctx)
 {
-	QDF_STATUS status;
-
+	QDF_STATUS status = QDF_STATUS_SUCCESS;
+	struct mlo_partner_info *partner_info;
 	struct element_info assoc_rsp;
 	struct qdf_mac_addr sta_link_addr;
+	uint8_t link_id = 0;
 
 	assoc_rsp.len = 0;
 	mlo_get_assoc_rsp(session_entry->vdev, &assoc_rsp);
 
-	if (!session_entry->lim_join_req->partner_info.num_partner_links) {
+	partner_info = &session_entry->lim_join_req->partner_info;
+	if (!partner_info->num_partner_links) {
 		pe_debug("MLO: num_partner_links is 0");
 		return QDF_STATUS_E_INVAL;
 	}
+
 	/* Todo: update the sta addr by matching link id */
 	qdf_mem_copy(&sta_link_addr, session_entry->self_mac_addr,
 		     QDF_MAC_ADDR_SIZE);
@@ -2808,23 +2811,23 @@ lim_process_switch_channel_join_mlo(struct pe_session *session_entry,
 
 		link_assoc_rsp.len = assoc_rsp.len + 24;
 		session_entry->limMlmState = eLIM_MLM_WT_ASSOC_RSP_STATE;
-		pe_debug("MLO: Generate and process assoc rsp for link vdev");
 
+		link_id = wlan_vdev_get_link_id(session_entry->vdev);
+		pe_debug("MLO: Generate and process assoc rsp for link vdev %d",
+			 link_id);
 		status = util_gen_link_assoc_rsp(assoc_rsp.ptr,
 						 assoc_rsp.len - 24,
-						 false, sta_link_addr,
+						 false, link_id, sta_link_addr,
 						 link_assoc_rsp.ptr,
 						 assoc_rsp.len,
 						 (qdf_size_t *)&link_assoc_rsp.len);
-
 		if (QDF_IS_STATUS_SUCCESS(status)) {
 			pe_debug("MLO: process assoc rsp for link vdev");
 			lim_process_assoc_rsp_frame(mac_ctx,
-						    link_assoc_rsp.ptr,
-						    (link_assoc_rsp.len - SIR_MAC_HDR_LEN_3A),
-						    LIM_ASSOC,
-						    session_entry);
-			qdf_mem_free(link_assoc_rsp.ptr);
+					    link_assoc_rsp.ptr,
+					    (link_assoc_rsp.len - SIR_MAC_HDR_LEN_3A),
+					    LIM_ASSOC,
+					    session_entry);
 		} else {
 			pe_debug("MLO: link vdev assoc rsp generation failed");
 			assoc_cnf.resultCode = eSIR_SME_INVALID_PARAMETERS;
@@ -2833,11 +2836,14 @@ lim_process_switch_channel_join_mlo(struct pe_session *session_entry,
 			assoc_cnf.sessionId = session_entry->peSessionId;
 			lim_post_sme_message(mac_ctx, LIM_MLM_ASSOC_CNF,
 					(uint32_t *)&assoc_cnf);
-			qdf_mem_free(link_assoc_rsp.ptr);
 		}
+
+		qdf_mem_free(link_assoc_rsp.ptr);
+		link_assoc_rsp.ptr = NULL;
+		link_assoc_rsp.len = 0;
 	}
 
-	return QDF_STATUS_SUCCESS;
+	return status;
 }
 
 #else /* WLAN_FEATURE_11BE_MLO */
@@ -2857,6 +2863,7 @@ lim_process_switch_channel_join_mlo_roam(struct pe_session *session_entry,
 	QDF_STATUS status;
 	struct element_info assoc_rsp = {};
 	struct qdf_mac_addr sta_link_addr;
+	uint8_t link_id = 0;
 
 	assoc_rsp.len = 0;
 	mlo_get_assoc_rsp(session_entry->vdev, &assoc_rsp);
@@ -2896,11 +2903,13 @@ lim_process_switch_channel_join_mlo_roam(struct pe_session *session_entry,
 		session_entry->limMlmState = eLIM_MLM_WT_REASSOC_RSP_STATE;
 		mlo_get_link_mac_addr_from_reassoc_rsp(session_entry->vdev, &bssid);
 		sir_copy_mac_addr(session_entry->limReAssocbssId, bssid.bytes);
-		pe_debug("MLO_ROAM: Generate and process reassoc rsp for link vdev");
+		link_id = wlan_vdev_get_link_id(session_entry->vdev);
+		pe_debug("MLO ROAM: Generate and process assoc rsp for link vdev %d",
+			 link_id);
 
 		status = util_gen_link_assoc_rsp(assoc_rsp.ptr,
 						 assoc_rsp.len,
-						 true, sta_link_addr,
+						 true, link_id, sta_link_addr,
 						 link_assoc_rsp.ptr,
 						 assoc_rsp.len,
 						 (qdf_size_t *)&link_assoc_rsp.len);

core/mac/src/pe/lim/lim_send_management_frames.c

@@ -144,111 +144,6 @@ void lim_populate_mac_header(struct mac_context *mac_ctx, uint8_t *buf,
 		mac_hdr->seqControl.seqNumHi, mac_ctx->mgmtSeqNum);
 }
 
-#ifdef WLAN_FEATURE_11BE_MLO
-static QDF_STATUS
-lim_populate_ml_probe_req(struct mac_context *mac,
-			  struct pe_session *session,
-			  uint8_t **ml_prb_req_ie,
-			  uint16_t *ml_probe_req_len)
-{
-	qdf_size_t ml_probe_len = 0;
-	struct wlan_ml_probe_req *ml_prb_req = NULL;
-	uint8_t *ml_probe = NULL;
-	uint8_t link = 0;
-	uint16_t stacontrol = 0;
-	struct mlo_partner_info partner_info;
-
-	if (!session || !session->vdev || !session->vdev->mlo_dev_ctx) {
-		pe_err("Null value");
-		return QDF_STATUS_E_NULL_VALUE;
-	}
-
-	ml_prb_req = qdf_mem_malloc(sizeof(struct wlan_ml_probe_req));
-	if (!ml_prb_req)
-		return QDF_STATUS_E_NULL_VALUE;
-
-	qdf_mem_zero(ml_prb_req, sizeof(struct wlan_ml_probe_req));
-
-	ml_probe = (uint8_t *)ml_prb_req;
-	*ml_prb_req_ie = (uint8_t *)ml_prb_req;
-	/* Fill the Element ID IE Type (0xFF) */
-	ml_prb_req->ml_ie_ff.elem_id = WLAN_ELEMID_EXTN_ELEM;
-	/* Fill the Multi link extn Element ID IE Type (0x6B) */
-	ml_prb_req->ml_ie_ff.elem_id_ext = WLAN_EXTN_ELEMID_MULTI_LINK;
-	ml_probe_len++;
-
-	/* Set ML IE multi link control bitmap:
-	 * ML probe variant type = 1
-	 * In presence bitmap, set MLD ID presence bit = 1
-	 */
-	QDF_SET_BITS(ml_prb_req->ml_ie_ff.mlcontrol,
-		     WLAN_ML_CTRL_TYPE_IDX,
-		     WLAN_ML_CTRL_TYPE_BITS,
-		     WLAN_ML_VARIANT_PROBEREQ);
-
-	QDF_SET_BITS(ml_prb_req->ml_ie_ff.mlcontrol,
-		     WLAN_ML_CTRL_PBM_IDX,
-		     WLAN_ML_CTRL_PBM_BITS,
-		     1);
-	ml_probe_len += WLAN_ML_CTRL_SIZE;
-	ml_prb_req->common_info_len = 2;
-	ml_probe_len += ml_prb_req->common_info_len;
-	/* mld id is always 0 for tx link for SAP or AP */
-	ml_prb_req->mld_id = 0;
-
-	if (wlan_vdev_mlme_cap_get(session->vdev,
-				   WLAN_VDEV_C_EXCL_STA_PROF_PRB_REQ)) {
-		pe_debug("Do not populate sta profile in MLO IE");
-		goto no_sta_prof;
-	}
-	pe_debug("Populate sta profile in MLO IE");
-
-	stacontrol = htole16(stacontrol);
-	partner_info = session->lim_join_req->partner_info;
-
-	for (link = 0;
-	     link < partner_info.num_partner_links;
-	     link++) {
-		ml_prb_req->sta_profile[link].sub_elem_id = 0;
-		ml_prb_req->sta_profile[link].per_sta_len =
-			WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE;
-		ml_probe_len += 2;
-
-		QDF_SET_BITS(stacontrol,
-			     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_LINKID_IDX,
-			     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_LINKID_BITS,
-			     partner_info.partner_link_info[link].link_id);
-
-		QDF_SET_BITS(stacontrol,
-			     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_IDX,
-			     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_BITS,
-			     1);
-		ml_prb_req->sta_profile[link].sta_control = stacontrol;
-		ml_probe_len += WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE;
-	}
-no_sta_prof:
-	ml_prb_req->ml_ie_ff.elem_len = ml_probe_len;
-	*ml_probe_req_len = ml_probe_len + MIN_IE_LEN;
-
-	pe_nofl_debug("Send ML probe req %zu", ml_probe_len);
-	QDF_TRACE_HEX_DUMP(QDF_MODULE_ID_PE, QDF_TRACE_LEVEL_DEBUG,
-			   ml_probe, ml_probe_len + MIN_IE_LEN);
-
-	session->lim_join_req->is_ml_probe_req_sent = true;
-
-	return QDF_STATUS_SUCCESS;
-}
-#else
-static QDF_STATUS
-lim_populate_ml_probe_req(struct mac_context *mac,
-			  struct pe_session *session,
-			  uint8_t **ml_prb_req_ie,
-			  uint16_t *ml_probe_req_len)
-{
-	return QDF_STATUS_E_NOSUPPORT;
-}
-#endif
-
 /**
  * lim_send_probe_req_mgmt_frame() - send probe request management frame
  * @mac_ctx: Pointer to Global MAC structure
@@ -301,12 +196,10 @@ lim_send_probe_req_mgmt_frame(struct mac_context *mac_ctx,
 	QDF_STATUS sir_status;
 	const uint8_t *qcn_ie = NULL;
 	uint8_t channel;
-	uint8_t *ml_probe_req_ie = NULL;
-	uint16_t ml_probe_req_ie_len = 0;
-	int ret;
 	uint8_t *eht_cap_ie = NULL, eht_cap_ie_len = 0;
 	bool is_band_2g;
 	uint16_t ie_buf_size;
+	uint16_t mlo_ie_len = 0;
 
 	if (additional_ielen)
 		addn_ielen = *additional_ielen;
@@ -446,10 +339,9 @@ lim_send_probe_req_mgmt_frame(struct mac_context *mac_ctx,
 	if (IS_DOT11_MODE_EHT(dot11mode) && pesession &&
 			pesession->lim_join_req) {
 		lim_update_session_eht_capable(mac_ctx, pesession);
-		lim_populate_ml_probe_req(mac_ctx, pesession,
-					  &ml_probe_req_ie,
-					  &ml_probe_req_ie_len);
+		mlo_ie_len = lim_send_probe_req_frame_mlo(mac_ctx, pesession);
 	}
+
 	populate_dot11f_eht_caps(mac_ctx, pesession, &pr->eht_cap);
 
 	if (addn_ielen && additional_ie) {
@@ -500,20 +392,6 @@ lim_send_probe_req_mgmt_frame(struct mac_context *mac_ctx,
 	if (extracted_ext_cap_flag)
 		lim_merge_extcap_struct(&pr->ExtCap, &extracted_ext_cap, true);
 
-	/*
-	 * Do unpack to populate the add_ie buffer to frm structure
-	 * before packing the frm structure. In this way, the IE ordering
-	 * which the latest 802.11 spec mandates is maintained.
-	 */
-	if (ml_probe_req_ie_len) {
-		ret = dot11f_unpack_probe_request(mac_ctx, ml_probe_req_ie,
-						  ml_probe_req_ie_len,
-						  pr, true);
-		if (DOT11F_FAILED(ret)) {
-			pe_err("unpack failed, ret: 0x%x", ret);
-			goto end;
-		}
-	}
 
 	/* That's it-- now we pack it.  First, how much space are we going to */
 	status = dot11f_get_packed_probe_request_size(mac_ctx, pr, &payload);
@@ -527,8 +405,7 @@ lim_send_probe_req_mgmt_frame(struct mac_context *mac_ctx,
 			status);
 	}
 
-	bytes = payload + sizeof(tSirMacMgmtHdr) + addn_ielen +
-		ml_probe_req_ie_len;
+	bytes = payload + sizeof(tSirMacMgmtHdr) + addn_ielen + mlo_ie_len;
 
 	/* Ok-- try to allocate some memory: */
 	qdf_status = cds_packet_alloc((uint16_t) bytes, (void **)&frame,
@@ -600,10 +477,15 @@ skip_eht_ie_update:
 		payload += addn_ielen;
 	}
 
-	if (ml_probe_req_ie_len) {
-		qdf_mem_copy(frame + sizeof(tSirMacMgmtHdr) + payload,
-			     ml_probe_req_ie, ml_probe_req_ie_len);
-		payload += ml_probe_req_ie_len;
+	if (mlo_ie_len) {
+		qdf_status = lim_fill_complete_mlo_ie(pesession, mlo_ie_len,
+				      frame + sizeof(tSirMacMgmtHdr) + payload);
+		if (QDF_IS_STATUS_ERROR(qdf_status)) {
+			pe_debug("assemble ml ie error, status %d", qdf_status);
+			mlo_ie_len = 0;
+		}
+
+		payload += mlo_ie_len;
 	}
 
 	pe_nofl_debug("Probe req TX: vdev %d seq num %d to " QDF_MAC_ADDR_FMT " len %d",
@@ -637,8 +519,6 @@ skip_eht_ie_update:
 		return QDF_STATUS_E_FAILURE;
 	}
 
-end:
-	qdf_mem_free(ml_probe_req_ie);
 	return QDF_STATUS_SUCCESS;
 } /* End lim_send_probe_req_mgmt_frame. */
 

core/mac/src/sys/legacy/src/utils/src/parser_api.c

@@ -9279,6 +9279,104 @@ QDF_STATUS populate_dot11f_eht_operation(struct mac_context *mac_ctx,
 #endif /* WLAN_FEATURE_11BE */
 
 #ifdef WLAN_FEATURE_11BE_MLO
+QDF_STATUS
+populate_dot11f_probe_req_mlo_ie(struct mac_context *mac,
+				 struct pe_session *session)
+{
+	struct wlan_mlo_ie *mlo_ie;
+	uint8_t *p_ml_ie, *sta_data;
+	uint16_t len_remaining, sta_len_left;
+	struct wlan_mlo_sta_profile *sta_pro;
+	int num_sta_pro = 0;
+	struct mlo_partner_info partner_info;
+	uint8_t link;
+
+	if (!session || !session->vdev || !session->vdev->mlo_dev_ctx) {
+		pe_err("Null value");
+		return QDF_STATUS_E_NULL_VALUE;
+	}
+
+	mlo_ie = &session->mlo_ie;
+	p_ml_ie = mlo_ie->data;
+	len_remaining = sizeof(mlo_ie->data);
+
+	*p_ml_ie++ = WLAN_ELEMID_EXTN_ELEM;
+	len_remaining--;
+
+	/* set length later */
+	*p_ml_ie++ = 0;
+	len_remaining--;
+
+	*p_ml_ie++ = WLAN_EXTN_ELEMID_MULTI_LINK;
+	len_remaining--;
+
+	/* Set ML IE multi link control bitmap:
+	 * ML probe variant type = 1
+	 * In presence bitmap, set MLD ID presence bit = 1
+	 */
+	mlo_ie->type = WLAN_ML_VARIANT_PROBEREQ;
+	QDF_SET_BITS(*(uint16_t *)p_ml_ie, WLAN_ML_CTRL_TYPE_IDX,
+		     WLAN_ML_CTRL_TYPE_BITS, mlo_ie->type);
+	QDF_SET_BITS(*(uint16_t *)p_ml_ie, WLAN_ML_CTRL_PBM_IDX,
+		     WLAN_ML_CTRL_PBM_BITS, 1);
+
+	p_ml_ie += WLAN_ML_CTRL_SIZE;
+	len_remaining -= WLAN_ML_CTRL_SIZE;
+
+	/* common info length is 2 */
+	*p_ml_ie++ = 2;
+	len_remaining--;
+
+	/* mld id is always 0 for tx link for SAP or AP */
+	*p_ml_ie++ = 0;
+	len_remaining--;
+
+	mlo_ie->num_data = p_ml_ie - mlo_ie->data;
+
+	if (wlan_vdev_mlme_cap_get(session->vdev,
+				   WLAN_VDEV_C_EXCL_STA_PROF_PRB_REQ)) {
+		pe_debug("Do not populate sta profile in MLO IE");
+		goto no_sta_prof;
+	}
+	pe_debug("Populate sta profile in MLO IE");
+
+	partner_info = session->lim_join_req->partner_info;
+	for (link = 0; link < partner_info.num_partner_links; link++) {
+		sta_pro = &mlo_ie->sta_profile[num_sta_pro];
+		sta_data = sta_pro->data;
+		sta_len_left = sizeof(sta_pro->data);
+
+		*sta_data++ = WLAN_ML_LINFO_SUBELEMID_PERSTAPROFILE;
+		sta_len_left--;
+		/* length of subelement, filled at last */
+		*sta_data++ = 0;
+		sta_len_left--;
+
+		QDF_SET_BITS(*(uint16_t *)sta_data,
+			     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_LINKID_IDX,
+			     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_LINKID_BITS,
+			     partner_info.partner_link_info[link].link_id);
+
+		QDF_SET_BITS(*(uint16_t *)sta_data,
+			     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_IDX,
+			     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_BITS,
+			     1);
+		sta_data += WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE;
+		sta_len_left -= WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE;
+
+		sta_pro->num_data = sta_data - sta_pro->data;
+		sta_pro->data[TAG_LEN_POS] = sta_pro->num_data - MIN_IE_LEN;
+
+		num_sta_pro++;
+	}
+
+no_sta_prof:
+	mlo_ie->num_sta_profile = num_sta_pro;
+	session->lim_join_req->is_ml_probe_req_sent = true;
+
+	return QDF_STATUS_SUCCESS;
+}
+
 QDF_STATUS populate_dot11f_assoc_rsp_mlo_ie(struct mac_context *mac_ctx,
 					    struct pe_session *session,
 					    tpDphHashNode sta,