qcacld-3.0: Handle TDLS discovery response logic

For MLD case, it may receive two discovery response frames if
peer is MLD device. In this case, it needs to check the response
frame and make a decision about which vdev is used for TDLS
vdev.

Change-Id: I3d7c3343aef2d6843b362fca191caef8b982d3f8
CRs-Fixed: 3439577

components/tdls/core/src/wlan_tdls_ct.c

@@ -26,6 +26,8 @@
 #include "wlan_tdls_main.h"
 #include "wlan_tdls_peer.h"
 #include "wlan_tdls_ct.h"
+#include "wlan_tdls_mgmt.h"
+#include "wlan_mlo_mgr_sta.h"
 #include "wlan_tdls_cmds_process.h"
 #include "wlan_reg_services_api.h"
 #include "wlan_policy_mgr_api.h"
@@ -88,18 +90,59 @@ void tdls_discovery_timeout_peer_cb(void *user_data)
 	struct tdls_peer *peer;
 	QDF_STATUS status;
 	struct tdls_vdev_priv_obj *tdls_vdev;
+	struct tdls_soc_priv_obj *tdls_soc;
 	struct wlan_objmgr_vdev *vdev;
+	struct wlan_objmgr_vdev *select_vdev;
+	struct wlan_objmgr_vdev *tdls_link_vdev;
+	struct tdls_rx_mgmt_frame *rx_mgmt;
+	uint8_t *mac;
 
 	if (!user_data) {
 		tdls_err("discovery time out data is null");
 		return;
 	}
 
-	vdev = tdls_get_vdev(user_data, WLAN_TDLS_NB_ID);
-	if (!vdev)
-		return;
-	tdls_vdev = wlan_vdev_get_tdls_vdev_obj(vdev);
+	vdev = (struct wlan_objmgr_vdev *)user_data;
+	tdls_soc = wlan_vdev_get_tdls_soc_obj(vdev);
+	if (wlan_vdev_mlme_is_mlo_vdev(vdev) &&
+	    qdf_atomic_dec_and_test(&tdls_soc->timer_cnt)) {
+		tdls_process_mlo_cal_tdls_link_score(vdev);
+		select_vdev = tdls_process_mlo_choice_tdls_vdev(vdev);
+		tdls_link_vdev = tdls_mlo_get_tdls_link_vdev(vdev);
+		if (select_vdev) {
+			tdls_vdev =
+			      wlan_objmgr_vdev_get_comp_private_obj(select_vdev,
+							   WLAN_UMAC_COMP_TDLS);
+			rx_mgmt = tdls_vdev->rx_mgmt;
+			if (tdls_link_vdev && tdls_link_vdev != select_vdev) {
+				tdls_debug("tdls link created on vdev %d",
+					   wlan_vdev_get_id(tdls_link_vdev));
+			} else {
+				mac =
+				     &rx_mgmt->buf[TDLS_80211_PEER_ADDR_OFFSET];
+				tdls_notice("[TDLS] TDLS Discovery Response,"
+					    "QDF_MAC_ADDR_FMT RSSI[%d]<---OTA",
+					    rx_mgmt->rx_rssi);
+				tdls_recv_discovery_resp(tdls_vdev, mac);
+				tdls_set_rssi(tdls_vdev->vdev, mac,
+					      rx_mgmt->rx_rssi);
+				if (tdls_soc && tdls_soc->tdls_rx_cb)
+					tdls_soc->tdls_rx_cb(
+						     tdls_soc->tdls_rx_cb_data,
+						     rx_mgmt);
+			}
+
+			qdf_mem_free(tdls_vdev->rx_mgmt);
+			tdls_vdev->rx_mgmt = NULL;
+			tdls_vdev->link_score = 0;
+
+			return;
+		}
 
+		tdls_debug("no discovery response");
+	}
+
+	tdls_vdev = wlan_vdev_get_tdls_vdev_obj(vdev);
 	for (i = 0; i < WLAN_TDLS_PEER_LIST_SIZE; i++) {
 		head = &tdls_vdev->peer_list[i];
 		status = qdf_list_peek_front(head, &p_node);
@@ -118,9 +161,8 @@ void tdls_discovery_timeout_peer_cb(void *user_data)
 						  TDLS_LINK_NOT_SUPPORTED);
 		}
 	}
-	tdls_vdev->discovery_sent_cnt = 0;
-	wlan_objmgr_vdev_release_ref(vdev, WLAN_TDLS_NB_ID);
 
+	tdls_vdev->discovery_sent_cnt = 0;
 	/* add tdls power save prohibited */
 
 	return;
@@ -440,17 +482,18 @@ void tdls_implicit_send_discovery_request(
 	tdls_psoc->tdls_event_cb(tdls_psoc->tdls_evt_cb_data,
 				 TDLS_EVENT_DISCOVERY_REQ, &tdls_ind);
 
-	tdls_vdev_obj->discovery_sent_cnt++;
-
-	tdls_timer_restart(tdls_vdev_obj->vdev,
-				&tdls_vdev_obj->peer_discovery_timer,
-				tdls_vdev_obj->threshold_config.tx_period_t -
-				TDLS_DISCOVERY_TIMEOUT_ERE_UPDATE);
-
-	tdls_debug("discovery count %u timeout %u msec",
-		 tdls_vdev_obj->discovery_sent_cnt,
-		 tdls_vdev_obj->threshold_config.tx_period_t -
-		 TDLS_DISCOVERY_TIMEOUT_ERE_UPDATE);
+	if (!wlan_vdev_mlme_is_mlo_vdev(tdls_vdev_obj->vdev)) {
+		tdls_vdev_obj->discovery_sent_cnt++;
+		tdls_timer_restart(tdls_vdev_obj->vdev,
+				   &tdls_vdev_obj->peer_discovery_timer,
+				   tdls_vdev_obj->threshold_config.tx_period_t -
+				   TDLS_DISCOVERY_TIMEOUT_ERE_UPDATE);
+
+		tdls_debug("discovery count %u timeout %u msec",
+			   tdls_vdev_obj->discovery_sent_cnt,
+			   tdls_vdev_obj->threshold_config.tx_period_t -
+			   TDLS_DISCOVERY_TIMEOUT_ERE_UPDATE);
+	}
 done:
 	tdls_vdev_obj->curr_candidate = NULL;
 	tdls_vdev_obj->magic = 0;
@@ -481,11 +524,13 @@ int tdls_recv_discovery_resp(struct tdls_vdev_priv_obj *tdls_vdev,
 		return -EINVAL;
 	}
 
-	if (tdls_vdev->discovery_sent_cnt)
-		tdls_vdev->discovery_sent_cnt--;
+	if (!wlan_vdev_mlme_is_mlo_vdev(tdls_vdev->vdev)) {
+		if (tdls_vdev->discovery_sent_cnt)
+			tdls_vdev->discovery_sent_cnt--;
 
-	if (0 == tdls_vdev->discovery_sent_cnt)
-		qdf_mc_timer_stop(&tdls_vdev->peer_discovery_timer);
+		if (tdls_vdev->discovery_sent_cnt == 0)
+			qdf_mc_timer_stop(&tdls_vdev->peer_discovery_timer);
+	}
 
 	tdls_debug("Discovery(%u) Response from " QDF_MAC_ADDR_FMT
 		   " link_status %d", tdls_vdev->discovery_sent_cnt,
@@ -1474,6 +1519,10 @@ void tdls_teardown_connections(struct tdls_link_teardown *tdls_teardown)
 		goto fail_tdls_vdev;
 	}
 
+	tdls_debug("tdls teardown connections");
+	wlan_vdev_mlme_feat_ext2_cap_clear(tdls_vdev,
+					   WLAN_VDEV_FEXT2_MLO_STA_TDLS);
+
 	tdls_disable_offchan_and_teardown_links(tdls_vdev);
 	qdf_event_set(&tdls_vdev_obj->tdls_teardown_comp);
 fail_tdls_vdev:

components/tdls/core/src/wlan_tdls_main.c

@@ -223,7 +223,7 @@ static QDF_STATUS tdls_vdev_init(struct tdls_vdev_priv_obj *vdev_obj)
 	qdf_mc_timer_init(&vdev_obj->peer_update_timer, QDF_TIMER_TYPE_SW,
 			  tdls_ct_handler, soc_obj->soc);
 	qdf_mc_timer_init(&vdev_obj->peer_discovery_timer, QDF_TIMER_TYPE_SW,
-			  tdls_discovery_timeout_peer_cb, soc_obj->soc);
+			  tdls_discovery_timeout_peer_cb, vdev_obj->vdev);
 
 	return QDF_STATUS_SUCCESS;
 }
@@ -746,7 +746,8 @@ void tdls_timer_restart(struct wlan_objmgr_vdev *vdev,
  */
 static void tdls_monitor_timers_stop(struct tdls_vdev_priv_obj *tdls_vdev)
 {
-	qdf_mc_timer_stop(&tdls_vdev->peer_discovery_timer);
+	if (!wlan_vdev_mlme_is_mlo_vdev(tdls_vdev->vdev))
+		qdf_mc_timer_stop(&tdls_vdev->peer_discovery_timer);
 }
 
 /**
@@ -1090,6 +1091,7 @@ void tdls_set_ct_mode(struct wlan_objmgr_psoc *psoc,
 		goto set_state;
 	}
 
+	qdf_atomic_set(&tdls_soc_obj->timer_cnt, 0);
 	tdls_feature_flags = tdls_soc_obj->tdls_configs.tdls_feature_flags;
 	if (TDLS_SUPPORT_DISABLED == tdls_soc_obj->tdls_current_mode ||
 	    TDLS_SUPPORT_SUSPENDED == tdls_soc_obj->tdls_current_mode ||
@@ -1614,6 +1616,9 @@ tdls_process_sta_disconnect(struct tdls_sta_notify_params *notify)
 					     WLAN_TDLS_NB_ID);
 	}
 
+	wlan_vdev_mlme_feat_ext2_cap_clear(notify->vdev,
+					   WLAN_VDEV_FEXT2_MLO_STA_TDLS);
+
 	return status;
 }
 

components/tdls/core/src/wlan_tdls_main.h

@@ -279,6 +279,8 @@ struct tdls_soc_priv_obj {
  * @curr_candidate: current candidate
  * @ct_peer_table: linear mac address table for counting the packets
  * @valid_mac_entries: number of valid mac entry in @ct_peer_mac_table
+ * @rx_mgmt: the pointer of rx mgmt info
+ * @link_score: select tdls vdev per the score
  * @magic: magic
  * @session_id: vdev ID
  * @tx_queue: tx frame queue
@@ -296,6 +298,8 @@ struct tdls_vdev_priv_obj {
 	struct tdls_conn_tracker_mac_table
 			ct_peer_table[WLAN_TDLS_CT_TABLE_SIZE];
 	uint8_t valid_mac_entries;
+	struct tdls_rx_mgmt_frame *rx_mgmt;
+	uint32_t link_score;
 	uint32_t magic;
 	uint8_t session_id;
 	qdf_list_t tx_queue;

components/tdls/core/src/wlan_tdls_mgmt.c

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2017-2020 The Linux Foundation. All rights reserved.
+ * Copyright (c) 2023 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
@@ -30,6 +31,9 @@
 #include "wlan_tdls_ct.h"
 #include "wlan_tdls_cmds_process.h"
 #include "wlan_tdls_mgmt.h"
+#include "wlan_policy_mgr_api.h"
+#include <wlan_reg_services_api.h>
+#include <wlan_mlo_mgr_sta.h>
 
 static
 const char *const tdls_action_frames_type[] = { "TDLS Setup Request",
@@ -77,6 +81,347 @@ QDF_STATUS tdls_set_rssi(struct wlan_objmgr_vdev *vdev,
 	return QDF_STATUS_SUCCESS;
 }
 
+#ifdef WLAN_FEATURE_11BE_MLO
+struct wlan_objmgr_vdev *
+tdls_mlo_get_tdls_link_vdev(struct wlan_objmgr_vdev *vdev)
+{
+	return wlan_mlo_get_tdls_link_vdev(vdev);
+}
+
+QDF_STATUS
+tdls_process_mlo_cal_tdls_link_score(struct wlan_objmgr_vdev *vdev)
+{
+	struct wlan_mlo_dev_context *mlo_dev_ctx;
+	struct wlan_objmgr_vdev *mlo_vdev;
+	struct tdls_vdev_priv_obj *tdls_vdev;
+	struct tdls_rx_mgmt_frame *rx_mgmt;
+	uint32_t score;
+	int i;
+	QDF_STATUS status = QDF_STATUS_E_FAILURE;
+
+	mlo_dev_ctx = vdev->mlo_dev_ctx;
+	for (i =  0; i < WLAN_UMAC_MLO_MAX_VDEVS; i++) {
+		score = 0;
+		mlo_vdev = mlo_dev_ctx->wlan_vdev_list[i];
+		if (!mlo_vdev)
+			continue;
+		tdls_vdev =
+		     wlan_objmgr_vdev_get_comp_private_obj(mlo_vdev,
+							   WLAN_UMAC_COMP_TDLS);
+		tdls_vdev->link_score = 0;
+		rx_mgmt = tdls_vdev->rx_mgmt;
+		if (!rx_mgmt)
+			continue;
+
+		switch (wlan_reg_freq_to_band(rx_mgmt->rx_freq)) {
+		case REG_BAND_2G:
+			score += 50;
+			break;
+		case REG_BAND_5G:
+			score += 70;
+			break;
+		case REG_BAND_6G:
+			score += 80;
+			break;
+		default:
+			score += 40;
+			break;
+		}
+
+		tdls_vdev->link_score = score;
+		status = QDF_STATUS_SUCCESS;
+	}
+
+	return status;
+}
+
+struct wlan_objmgr_vdev *
+tdls_process_mlo_choice_tdls_vdev(struct wlan_objmgr_vdev *vdev)
+{
+	struct wlan_mlo_dev_context *mlo_dev_ctx;
+	struct wlan_objmgr_vdev *mlo_vdev;
+	struct wlan_objmgr_vdev *select_vdev = NULL;
+	struct tdls_vdev_priv_obj *tdls_vdev;
+	uint32_t score = 0;
+	int i;
+
+	mlo_dev_ctx = vdev->mlo_dev_ctx;
+	for (i =  0; i < WLAN_UMAC_MLO_MAX_VDEVS; i++) {
+		mlo_vdev = mlo_dev_ctx->wlan_vdev_list[i];
+		if (!mlo_vdev)
+			continue;
+		tdls_vdev =
+		     wlan_objmgr_vdev_get_comp_private_obj(mlo_vdev,
+							   WLAN_UMAC_COMP_TDLS);
+		if (score < tdls_vdev->link_score) {
+			select_vdev = mlo_vdev;
+			score = tdls_vdev->link_score;
+		}
+	}
+
+	/* free the memory except the choice one */
+	for (i =  0; i < WLAN_UMAC_MLO_MAX_VDEVS; i++) {
+		mlo_vdev = mlo_dev_ctx->wlan_vdev_list[i];
+		if (!mlo_vdev || mlo_vdev == select_vdev)
+			continue;
+
+		tdls_vdev =
+		     wlan_objmgr_vdev_get_comp_private_obj(mlo_vdev,
+							   WLAN_UMAC_COMP_TDLS);
+		qdf_mem_free(tdls_vdev->rx_mgmt);
+		tdls_vdev->rx_mgmt = NULL;
+		tdls_vdev->link_score = 0;
+	}
+
+	return select_vdev;
+}
+
+static struct tdls_vdev_priv_obj
+*tdls_get_correct_vdev(struct tdls_vdev_priv_obj *tdls_vdev,
+		       struct tdls_rx_mgmt_frame *rx_mgmt)
+{
+	struct wlan_objmgr_pdev *pdev;
+	struct wlan_objmgr_vdev *vdev;
+	struct tdls_link_identifier *linkid_ie;
+	uint8_t vdev_id;
+	uint8_t *ies;
+	uint32_t ie_len;
+	uint8_t elem_id_param = WLAN_ELEMID_LINK_IDENTIFIER;
+
+	if (!tdls_vdev || !rx_mgmt)
+		return NULL;
+
+	ies = &rx_mgmt->buf[TDLS_PUBLIC_ACTION_FRAME_TDLS_IE_OFFSET];
+	ie_len = rx_mgmt->frame_len - TDLS_PUBLIC_ACTION_FRAME_TDLS_IE_OFFSET;
+
+	linkid_ie =
+	  (struct tdls_link_identifier *)wlan_get_ie_ptr_from_eid(elem_id_param,
+								  ies, ie_len);
+	if (!linkid_ie)
+		return tdls_vdev;
+
+	pdev = wlan_vdev_get_pdev(tdls_vdev->vdev);
+	if (!pdev)
+		return tdls_vdev;
+
+	if (!wlan_get_connected_vdev_by_bssid(pdev,
+					      linkid_ie->bssid, &vdev_id))
+		return tdls_vdev;
+
+	vdev = wlan_objmgr_get_vdev_by_id_from_pdev(pdev, vdev_id,
+						    WLAN_TDLS_NB_ID);
+	if (!vdev)
+		return tdls_vdev;
+
+	tdls_vdev = wlan_objmgr_vdev_get_comp_private_obj(vdev,
+							  WLAN_UMAC_COMP_TDLS);
+	rx_mgmt->vdev_id = vdev_id;
+
+	wlan_objmgr_vdev_release_ref(vdev, WLAN_TDLS_NB_ID);
+	tdls_debug("received discovery response on vdev %d", rx_mgmt->vdev_id);
+
+	return tdls_vdev;
+}
+
+static bool tdls_check_peer_mlo_dev(struct wlan_objmgr_vdev *vdev,
+				    struct tdls_rx_mgmt_frame *rx_mgmt)
+{
+	uint8_t *ies;
+	const uint8_t *ie;
+	uint32_t ie_len;
+	const uint8_t ext_id_param = WLAN_EXTN_ELEMID_MULTI_LINK;
+	uint8_t elem_id_param = WLAN_ELEMID_LINK_IDENTIFIER;
+
+	if (!vdev || !rx_mgmt)
+		return QDF_STATUS_E_INVAL;
+
+	ies = &rx_mgmt->buf[TDLS_PUBLIC_ACTION_FRAME_TDLS_IE_OFFSET];
+	ie_len = rx_mgmt->frame_len - TDLS_PUBLIC_ACTION_FRAME_TDLS_IE_OFFSET;
+
+	ie = wlan_get_ie_ptr_from_eid(elem_id_param, ies, ie_len);
+	if (ie)
+		qdf_trace_hex_dump(QDF_MODULE_ID_TDLS, QDF_TRACE_LEVEL_DEBUG,
+				   (void *)&ie[0], ie[1] + 2);
+
+	ie = wlan_get_ext_ie_ptr_from_ext_id(&ext_id_param,
+					     1, ies, ie_len);
+	if (ie)
+		return true;
+
+	return false;
+}
+
+static QDF_STATUS
+tdls_process_mlo_rx_mgmt_sync(struct tdls_soc_priv_obj *tdls_soc,
+			      struct tdls_vdev_priv_obj *tdls_vdev,
+			      struct tdls_rx_mgmt_frame *rx_mgmt)
+{
+	QDF_STATUS status;
+	struct wlan_objmgr_vdev *vdev;
+	struct wlan_objmgr_vdev *mlo_vdev;
+	struct wlan_mlo_dev_context *mlo_dev_ctx;
+	bool peer_mlo;
+	uint8_t i;
+
+	vdev = tdls_vdev->vdev;
+	peer_mlo = tdls_check_peer_mlo_dev(vdev, rx_mgmt);
+	if (!peer_mlo) {
+		mlo_dev_ctx = vdev->mlo_dev_ctx;
+		/* stop all timers */
+		for (i =  0; i < WLAN_UMAC_MLO_MAX_VDEVS; i++) {
+			mlo_vdev = mlo_dev_ctx->wlan_vdev_list[i];
+			if (!mlo_vdev)
+				continue;
+
+			tdls_vdev =
+			       wlan_objmgr_vdev_get_comp_private_obj(mlo_vdev,
+							   WLAN_UMAC_COMP_TDLS);
+			tdls_vdev->discovery_sent_cnt = 0;
+			if (QDF_TIMER_STATE_RUNNING ==
+			    qdf_mc_timer_get_current_state(
+					    &tdls_vdev->peer_discovery_timer)) {
+				qdf_mc_timer_stop(
+					      &tdls_vdev->peer_discovery_timer);
+				qdf_atomic_dec(&tdls_soc->timer_cnt);
+			}
+		}
+		tdls_debug("peer is legacy device, timer_cnt %d",
+			   qdf_atomic_read(&tdls_soc->timer_cnt));
+
+		status = QDF_STATUS_E_INVAL;
+	} else {
+		tdls_debug("peer is mlo device, timer_cnt %d",
+			   qdf_atomic_read(&tdls_soc->timer_cnt));
+
+		/* If peer is MLD, it uses ml mac address to send the
+		 * disconvery response, it needs to find out the
+		 * corresponding vdev per the bssid in link identifier ie.
+		 */
+		tdls_vdev = tdls_get_correct_vdev(tdls_vdev, rx_mgmt);
+		status = QDF_STATUS_TDLS_MLO_SYNC;
+		if (!tdls_vdev || tdls_vdev->rx_mgmt) {
+			tdls_err("rx dup tdls discovery resp on same vdev.");
+			goto exit;
+		}
+
+		tdls_vdev->discovery_sent_cnt = 0;
+		qdf_mc_timer_stop(&tdls_vdev->peer_discovery_timer);
+		qdf_atomic_dec(&tdls_soc->timer_cnt);
+
+		tdls_vdev->rx_mgmt = qdf_mem_malloc_atomic(sizeof(*rx_mgmt) +
+							   rx_mgmt->frame_len);
+		if (tdls_vdev->rx_mgmt) {
+			tdls_vdev->rx_mgmt->frame_len = rx_mgmt->frame_len;
+			tdls_vdev->rx_mgmt->rx_freq = rx_mgmt->rx_freq;
+			tdls_vdev->rx_mgmt->vdev_id = rx_mgmt->vdev_id;
+			tdls_vdev->rx_mgmt->frm_type = rx_mgmt->frm_type;
+			tdls_vdev->rx_mgmt->rx_rssi = rx_mgmt->rx_rssi;
+			qdf_mem_copy(tdls_vdev->rx_mgmt->buf,
+				     rx_mgmt->buf, rx_mgmt->frame_len);
+		} else {
+			tdls_err("alloc rx mgmt buf error");
+		}
+
+		if (qdf_atomic_read(&tdls_soc->timer_cnt) == 0) {
+			tdls_process_mlo_cal_tdls_link_score(vdev);
+			status = QDF_STATUS_SUCCESS;
+		}
+	}
+
+exit:
+	return status;
+}
+
+void tdls_set_no_force_vdev(struct wlan_objmgr_vdev *vdev, bool flag)
+{
+	uint8_t i, count = 0;
+	struct wlan_objmgr_psoc *psoc;
+	struct wlan_objmgr_vdev *mlo_vdev;
+	struct wlan_mlo_dev_context *mlo_dev_ctx;
+	uint8_t mlo_vdev_lst[WLAN_UMAC_MLO_MAX_VDEVS];
+	bool is_mlo_vdev;
+
+	if (!vdev)
+		return;
+
+	is_mlo_vdev = wlan_vdev_mlme_is_mlo_vdev(vdev);
+	if (!is_mlo_vdev)
+		return;
+
+	psoc = wlan_vdev_get_psoc(vdev);
+	if (!psoc)
+		return;
+
+	mlo_dev_ctx = vdev->mlo_dev_ctx;
+	for (i = 0; i < WLAN_UMAC_MLO_MAX_VDEVS; i++) {
+		mlo_vdev = mlo_dev_ctx->wlan_vdev_list[i];
+
+		/* flag: true means no force all vdevs,
+		 * false means except the current one
+		 */
+		if (!flag && (mlo_vdev == vdev))
+			continue;
+		mlo_vdev_lst[count] = wlan_vdev_get_id(mlo_vdev);
+		count++;
+	}
+
+	policy_mgr_mlo_sta_set_link(psoc, MLO_LINK_FORCE_REASON_TDLS,
+				    MLO_LINK_FORCE_MODE_NO_FORCE,
+				    count, mlo_vdev_lst);
+}
+#else
+struct wlan_objmgr_vdev *
+tdls_mlo_get_tdls_link_vdev(struct wlan_objmgr_vdev *vdev)
+{
+	return NULL;
+}
+
+QDF_STATUS
+tdls_process_mlo_cal_tdls_link_score(struct wlan_objmgr_vdev *vdev)
+{
+	return QDF_STATUS_SUCCESS;
+}
+
+struct wlan_objmgr_vdev *
+tdls_process_mlo_choice_tdls_vdev(struct wlan_objmgr_vdev *vdev)
+{
+	return NULL;
+}
+
+static struct tdls_vdev_priv_obj
+*tdls_get_correct_vdev(struct tdls_vdev_priv_obj *tdls_vdev,
+		       struct tdls_rx_mgmt_frame *rx_mgmt)
+{
+	return NULL;
+}
+
+static QDF_STATUS
+tdls_process_mlo_rx_mgmt_sync(struct tdls_soc_priv_obj *tdls_soc,
+			      struct tdls_vdev_priv_obj *tdls_vdev,
+			      struct tdls_rx_mgmt_frame *rx_mgmt)
+{
+	return QDF_STATUS_SUCCESS;
+}
+
+void tdls_set_no_force_vdev(struct wlan_objmgr_vdev *vdev, bool flag)
+{
+}
+#endif
+
+static bool
+tdls_needs_wait_discovery_response(struct wlan_objmgr_vdev *vdev,
+				   struct tdls_soc_priv_obj *tdls_soc)
+{
+	bool is_mlo_vdev;
+	bool wait = false;
+
+	is_mlo_vdev = wlan_vdev_mlme_is_mlo_vdev(vdev);
+	if (is_mlo_vdev && qdf_atomic_read(&tdls_soc->timer_cnt))
+		wait = true;
+
+	return wait;
+}
+
 /**
  * tdls_process_rx_mgmt() - process tdls rx mgmt frames
  * @rx_mgmt_event: tdls rx mgmt event
@@ -92,6 +437,10 @@ static QDF_STATUS tdls_process_rx_mgmt(
 	struct tdls_soc_priv_obj *tdls_soc_obj;
 	uint8_t *mac;
 	enum tdls_actioncode action_frame_type;
+	struct wlan_objmgr_vdev *vdev;
+	struct wlan_objmgr_vdev *tdls_link_vdev;
+	bool tdls_vdev_select = false;
+	QDF_STATUS status = QDF_STATUS_E_INVAL;
 
 	if (!rx_mgmt_event)
 		return QDF_STATUS_E_INVAL;
@@ -104,19 +453,66 @@ static QDF_STATUS tdls_process_rx_mgmt(
 		return QDF_STATUS_E_INVAL;
 	}
 
+	vdev = tdls_vdev->vdev;
+	tdls_debug("received mgmt on vdev %d", wlan_vdev_get_id(vdev));
 	tdls_debug("soc:%pK, frame_len:%d, rx_freq:%d, vdev_id:%d, frm_type:%d, rx_rssi:%d, buf:%pK",
 		   tdls_soc_obj->soc, rx_mgmt->frame_len,
 		   rx_mgmt->rx_freq, rx_mgmt->vdev_id, rx_mgmt->frm_type,
 		   rx_mgmt->rx_rssi, rx_mgmt->buf);
 
 	if (rx_mgmt->buf[TDLS_PUBLIC_ACTION_FRAME_OFFSET + 1] ==
-						TDLS_PUBLIC_ACTION_DISC_RESP) {
+	    TDLS_PUBLIC_ACTION_DISC_RESP) {
+		if (tdls_needs_wait_discovery_response(vdev, tdls_soc_obj)) {
+			status = tdls_process_mlo_rx_mgmt_sync(tdls_soc_obj,
+							       tdls_vdev,
+							       rx_mgmt);
+			if (status == QDF_STATUS_TDLS_MLO_SYNC) {
+				return QDF_STATUS_SUCCESS;
+			} else if (status == QDF_STATUS_SUCCESS) {
+				vdev = tdls_process_mlo_choice_tdls_vdev(vdev);
+				tdls_vdev =
+				     wlan_objmgr_vdev_get_comp_private_obj(vdev,
+							   WLAN_UMAC_COMP_TDLS);
+				rx_mgmt = tdls_vdev->rx_mgmt;
+				tdls_vdev_select = true;
+				tdls_debug("choice vdev %d as tdls vdev",
+					   wlan_vdev_get_id(vdev));
+			} else {
+				tdls_vdev = tdls_get_correct_vdev(tdls_vdev,
+								  rx_mgmt);
+				if (!tdls_vdev)
+					return QDF_STATUS_SUCCESS;
+				vdev = tdls_vdev->vdev;
+			}
+		} else {
+			if (wlan_vdev_mlme_is_mlo_vdev(vdev)) {
+				tdls_link_vdev =
+					      tdls_mlo_get_tdls_link_vdev(vdev);
+				tdls_vdev =
+				      tdls_get_correct_vdev(tdls_vdev, rx_mgmt);
+				if (!tdls_link_vdev || !tdls_vdev) {
+					tdls_debug("not expected frame");
+					return QDF_STATUS_SUCCESS;
+				}
+				if (tdls_link_vdev != tdls_vdev->vdev) {
+					tdls_debug("not forward to userspace");
+					return QDF_STATUS_SUCCESS;
+				}
+				rx_mgmt->vdev_id =
+					       wlan_vdev_get_id(tdls_link_vdev);
+				vdev = tdls_link_vdev;
+			}
+		}
+
+		/* this is mld mac address for mlo case*/
 		mac = &rx_mgmt->buf[TDLS_80211_PEER_ADDR_OFFSET];
 		tdls_notice("[TDLS] TDLS Discovery Response,"
-		       QDF_MAC_ADDR_FMT " RSSI[%d] <--- OTA",
-		       QDF_MAC_ADDR_REF(mac), rx_mgmt->rx_rssi);
-			tdls_recv_discovery_resp(tdls_vdev, mac);
-			tdls_set_rssi(tdls_vdev->vdev, mac, rx_mgmt->rx_rssi);
+			    QDF_MAC_ADDR_FMT " RSSI[%d] <--- OTA",
+			    QDF_MAC_ADDR_REF(mac), rx_mgmt->rx_rssi);
+
+		tdls_debug("discovery resp on vdev %d", wlan_vdev_get_id(vdev));
+		tdls_recv_discovery_resp(tdls_vdev, mac);
+		tdls_set_rssi(tdls_vdev->vdev, mac, rx_mgmt->rx_rssi);
 	}
 
 	if (rx_mgmt->buf[TDLS_PUBLIC_ACTION_FRAME_OFFSET] ==
@@ -139,6 +535,12 @@ static QDF_STATUS tdls_process_rx_mgmt(
 	else
 		tdls_debug("rx mgmt, but no valid up layer callback");
 
+	if (tdls_vdev_select && tdls_vdev->rx_mgmt) {
+		qdf_mem_free(tdls_vdev->rx_mgmt);
+		tdls_vdev->rx_mgmt = NULL;
+		tdls_vdev->link_score = 0;
+	}
+
 	return QDF_STATUS_SUCCESS;
 }
 

components/tdls/core/src/wlan_tdls_mgmt.h

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2017-2019 The Linux Foundation. All rights reserved.
+ * Copyright (c) 2023 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
@@ -26,6 +27,10 @@
 #define _WLAN_TDLS_MGMT_H_
 
 #define TDLS_PUBLIC_ACTION_FRAME_OFFSET 24
+/* TDLS_PUBLIC_ACTION_FRAME_OFFSET(category[1]) + action[1] + dialog[1]
+ * + cap[2]
+ */
+#define TDLS_PUBLIC_ACTION_FRAME_TDLS_IE_OFFSET 29
 #define TDLS_PUBLIC_ACTION_FRAME 4
 #define TDLS_PUBLIC_ACTION_DISC_RESP 14
 #define TDLS_ACTION_FRAME 12
@@ -33,6 +38,22 @@
 				     QDF_MAC_ADDR_SIZE)
 #define TDLS_ACTION_FRAME_TYPE_MAX 11
 
+/**
+ * struct tdls_link_identifier - tdls link identifier ie
+ * @id: element id
+ * @len: length
+ * @bssid: bssid
+ * @initsta: the mac address of initiator
+ * @respsta: the mac address of responder
+ */
+struct tdls_link_identifier {
+	uint8_t id;
+	uint8_t len;
+	uint8_t bssid[6];
+	uint8_t initsta[6];
+	uint8_t respsta[6];
+};
+
 /**
  * struct tdls_rx_mgmt_event - tdls rx mgmt frame event
  * @tdls_soc_obj: tdls soc private object
@@ -55,6 +76,27 @@ struct tdls_rx_mgmt_event {
 QDF_STATUS tdls_process_mgmt_req(
 			struct tdls_action_frame_request *tdls_mgmt_req);
 
+/**
+ * tdls_process_mlo_cal_tdls_link_score() - process mlo cal tdls link
+ * @vdev: object manager vdev
+ *
+ * Converts rx tdls frame freq to a link score and stores the score
+ * in relative tdls_vdev object.
+ *
+ *Return: QDF_STATUS
+ */
+QDF_STATUS
+tdls_process_mlo_cal_tdls_link_score(struct wlan_objmgr_vdev *vdev);
+
+/**
+ * tdls_mlo_get_tdls_link_vdev() - wrapper function
+ * @vdev: vdev object
+ *
+ * Return: tdls vdev
+ */
+struct wlan_objmgr_vdev *
+tdls_mlo_get_tdls_link_vdev(struct wlan_objmgr_vdev *vdev);
+
 /**
  * tdls_mgmt_rx_ops() - register or unregister rx callback
  * @psoc: psoc object
@@ -88,5 +130,23 @@ QDF_STATUS tdls_process_rx_frame(struct scheduler_msg *msg);
  */
 QDF_STATUS tdls_set_rssi(struct wlan_objmgr_vdev *vdev,
 			 uint8_t *mac, int8_t rssi);
+
+/**
+ * tdls_process_mlo_choice_tdls_vdev() - choice one vdev for tdls vdev
+ * @vdev: object manager vdev
+ *
+ * Return: pointer of vdev object
+ */
+struct wlan_objmgr_vdev *
+tdls_process_mlo_choice_tdls_vdev(struct wlan_objmgr_vdev *vdev);
+
+/**
+ * tdls_set_no_force_vdev() - set no force for the vdev
+ * @vdev: object manager vdev
+ * @flag: true, set all vdev as no force; false, except the current one.
+ *
+ * Return: void
+ */
+void tdls_set_no_force_vdev(struct wlan_objmgr_vdev *vdev, bool flag);
 #endif
 

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

@@ -1985,6 +1985,14 @@ void lim_process_action_frame(struct mac_context *mac_ctx,
 			lim_process_ext_channel_switch_action_frame(mac_ctx,
 							rx_pkt_info, session);
 			break;
+		case TDLS_DISCOVERY_RESPONSE:
+			/* do not forward the tdls discovery response frame,
+			 * it is handled by
+			 * tgt_mgmt_txrx_rx_frame_handler ->
+			 * tgt_tdls_mgmt_frame_rx_cb ->
+			 * tdls_process_rx_frame
+			 */
+			break;
 		case SIR_MAC_ACTION_VENDOR_SPECIFIC:
 			pub_action =
 				(tpSirMacVendorSpecificPublicActionFrameHdr)