qcacld-3.0: Generate link specific Probe response

Add support to generate link specific probe response
and add to the scan database.

Change-Id: I30a68df5496bf3e81b4fe71734253555512b82ea
CRs-Fixed: 3158761

core/mac/src/pe/include/lim_api.h

@@ -566,6 +566,25 @@ lim_fill_pe_session(struct mac_context *mac_ctx,
 		    struct pe_session *session,
 		    struct bss_description *bss_desc);
 
+#ifdef WLAN_FEATURE_11BE_MLO
+QDF_STATUS
+lim_gen_link_specific_probe_rsp(struct mac_context *mac_ctx,
+				struct pe_session *session_entry,
+				uint8_t *probe_rsp,
+				uint32_t probe_rsp_len,
+				int32_t rssi);
+#else
+static inline QDF_STATUS
+lim_gen_link_specific_probe_rsp(struct mac_context *mac_ctx,
+				struct pe_session *session_entry,
+				uint8_t *probe_rsp,
+				uint32_t probe_rsp_len,
+				int32_t rssi)
+{
+	return QDF_STATUS_E_NOSUPPORT;
+}
+#endif
+
 #if defined(WLAN_FEATURE_ROAM_OFFLOAD) && defined(WLAN_FEATURE_11BE_MLO)
 /**
  * lim_cm_roam_create_session() - Create pe session for legacy to MLO roaming

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

@@ -3461,3 +3461,132 @@ lim_mlo_roam_delete_link_peer(struct pe_session *pe_session,
 	wlan_objmgr_peer_release_ref(peer, WLAN_LEGACY_MAC_ID);
 }
 #endif
+
+#ifdef WLAN_FEATURE_11BE_MLO
+static void
+lim_add_bcn_probe(struct wlan_objmgr_vdev *vdev, uint8_t *bcn_probe,
+		  uint32_t len, qdf_freq_t freq, int32_t rssi)
+{
+	qdf_nbuf_t buf;
+	struct wlan_objmgr_pdev *pdev;
+	uint8_t *data, i, vdev_id;
+	struct mgmt_rx_event_params rx_param = {0};
+	struct wlan_frame_hdr *hdr;
+	enum mgmt_frame_type frm_type = MGMT_BEACON;
+
+	vdev_id = wlan_vdev_get_id(vdev);
+	if (!bcn_probe || !len || (len < sizeof(*hdr))) {
+		pe_err("bcn_probe is null or invalid len %d",
+		       len);
+		return;
+	}
+
+	pdev = wlan_vdev_get_pdev(vdev);
+	if (!pdev) {
+		pe_err("Failed to find pdev");
+		return;
+	}
+
+	hdr = (struct wlan_frame_hdr *)bcn_probe;
+	if ((hdr->i_fc[0] & QDF_IEEE80211_FC0_SUBTYPE_MASK) ==
+	    MGMT_SUBTYPE_PROBE_RESP)
+		frm_type = MGMT_PROBE_RESP;
+
+	rx_param.pdev_id = wlan_objmgr_pdev_get_pdev_id(pdev);
+	rx_param.chan_freq = freq;
+	rx_param.rssi = rssi;
+
+	/* Set all per chain rssi as invalid */
+	for (i = 0; i < WLAN_MGMT_TXRX_HOST_MAX_ANTENNA; i++)
+		rx_param.rssi_ctl[i] = WLAN_INVALID_PER_CHAIN_RSSI;
+
+	buf = qdf_nbuf_alloc(NULL, qdf_roundup(len, 4), 0, 4, false);
+	if (!buf)
+		return;
+
+	qdf_nbuf_put_tail(buf, len);
+	qdf_nbuf_set_protocol(buf, ETH_P_CONTROL);
+
+	data = qdf_nbuf_data(buf);
+	qdf_mem_copy(data, bcn_probe, len);
+
+	pe_debug("MLO: add prb rsp to scan db");
+	/* buf will be freed by scan module in error or success case */
+	wlan_scan_process_bcn_probe_rx_sync(wlan_pdev_get_psoc(pdev), buf,
+					    &rx_param, frm_type);
+}
+
+QDF_STATUS
+lim_gen_link_specific_probe_rsp(struct mac_context *mac_ctx,
+				struct pe_session *session_entry,
+				uint8_t *probe_rsp,
+				uint32_t probe_rsp_len,
+				int32_t rssi)
+{
+	struct element_info link_probe_rsp;
+	struct qdf_mac_addr sta_link_addr;
+	QDF_STATUS status = QDF_STATUS_SUCCESS;
+	struct mlo_link_info *link_info = NULL;
+	struct mlo_partner_info *partner_info;
+	uint8_t chan;
+	uint8_t op_class;
+	uint16_t chan_freq;
+
+	link_probe_rsp.ptr = qdf_mem_malloc(probe_rsp_len);
+	if (!link_probe_rsp.ptr)
+		return QDF_STATUS_E_NOMEM;
+
+	qdf_mem_copy(&sta_link_addr, session_entry->self_mac_addr,
+		     QDF_MAC_ADDR_SIZE);
+
+	link_probe_rsp.len = probe_rsp_len;
+	status = util_gen_link_probe_rsp(probe_rsp,
+					 probe_rsp_len,
+					 sta_link_addr,
+					 link_probe_rsp.ptr,
+					 probe_rsp_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;
+	}
+
+	partner_info = &session_entry->lim_join_req->partner_info;
+	if (!partner_info->num_partner_links) {
+		pe_err("Partner link info not available");
+		status = QDF_STATUS_E_FAILURE;
+		goto end;
+	}
+
+	/* 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));
+		status = QDF_STATUS_E_FAILURE;
+		goto end;
+	}
+	chan_freq = wlan_reg_chan_opclass_to_freq(chan, op_class,
+						  true);
+
+	lim_add_bcn_probe(session_entry->vdev, probe_rsp, probe_rsp_len,
+			  chan_freq, rssi);
+
+end:
+	qdf_mem_free(link_probe_rsp.ptr);
+	link_probe_rsp.len = 0;
+	return status;
+}
+#endif

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

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2011-2021 The Linux Foundation. All rights reserved.
+ * Copyright (c) 2021-2022 Qualcomm Innovation Center, Inc. All rights reserved.
  *
  * Permission to use, copy, modify, and/or distribute this software for
  * any purpose with or without fee is hereby granted, provided that the
@@ -154,6 +155,10 @@ lim_process_probe_rsp_frame(struct mac_context *mac_ctx, uint8_t *rx_Packet_info
 		return;
 	}
 
+	lim_gen_link_specific_probe_rsp(mac_ctx, session_entry, body,
+					frame_len,
+					mac_ctx->lim.bss_rssi);
+
 	if (session_entry->limMlmState ==
 			eLIM_MLM_WT_JOIN_BEACON_STATE) {
 		/*

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

@@ -163,12 +163,6 @@ lim_populate_ml_probe_req(struct mac_context *mac,
 		return QDF_STATUS_E_NULL_VALUE;
 	}
 
-	/* if num partner links is > 0
-	 * means complete profiles were sent by AP
-	 */
-	if (session->lim_join_req->partner_info.num_partner_links)
-		return QDF_STATUS_SUCCESS;
-
 	ml_prb_req = qdf_mem_malloc(sizeof(struct wlan_ml_probe_req));
 	if (!ml_prb_req)
 		return QDF_STATUS_E_NULL_VALUE;
@@ -181,6 +175,7 @@ lim_populate_ml_probe_req(struct mac_context *mac,
 	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
@@ -195,12 +190,11 @@ lim_populate_ml_probe_req(struct mac_context *mac,
 		     WLAN_ML_CTRL_PBM_IDX,
 		     WLAN_ML_CTRL_PBM_BITS,
 		     1);
-	ml_probe_len += sizeof(struct wlan_ie_multilink);
-	ml_prb_req->common_info_len = 1;
-	ml_probe_len++;
+	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;
-	ml_probe_len++;
 
 	stacontrol = htole16(stacontrol);
 	partner_info = session->lim_join_req->partner_info;
@@ -210,7 +204,7 @@ lim_populate_ml_probe_req(struct mac_context *mac,
 	     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 + 2;
+			WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE;
 		ml_probe_len += 2;
 
 		QDF_SET_BITS(stacontrol,
@@ -228,9 +222,9 @@ lim_populate_ml_probe_req(struct mac_context *mac,
 	ml_prb_req->ml_ie_ff.elem_len = ml_probe_len;
 	*ml_probe_req_len = ml_probe_len;
 
-	pe_nofl_debug("Send ML probe req");
+	pe_nofl_debug("Send ML probe req %d", ml_probe_len);
 	QDF_TRACE_HEX_DUMP(QDF_MODULE_ID_PE, QDF_TRACE_LEVEL_DEBUG,
-			   ml_probe, ml_probe_len);
+			   ml_probe, ml_probe_len + 2);
 
 	return QDF_STATUS_SUCCESS;
 }