qcacld-3.0: Force active ML links based on link mac address

Add support to force enable/disable MLO links in MLSR mode
based on link mac address. The input will be an array of
active link mac addresses. Force enable the links associated
with all the mentioned link addresses and force disable the
links associated with mac addresses that are not given in
the input.

Change-Id: I14660f14ba6381da04460b641f971734c03aa9a7
CRs-Fixed: 3391992

components/cmn_services/policy_mgr/inc/wlan_policy_mgr_api.h

@@ -4555,7 +4555,7 @@ bool policy_mgr_is_mlo_in_mode_dbs(struct wlan_objmgr_psoc *psoc,
 bool policy_mgr_is_mlo_in_mode_emlsr(struct wlan_objmgr_psoc *psoc,
 				     uint8_t *mlo_vdev_lst, uint8_t *num_mlo);
 
-/*
+/**
  * policy_mgr_handle_ml_sta_links_on_vdev_up_csa() - Handle enable/disable
  * link on vdev UP and channel change
  * @psoc: objmgr psoc
@@ -4569,7 +4569,7 @@ policy_mgr_handle_ml_sta_links_on_vdev_up_csa(struct wlan_objmgr_psoc *psoc,
 					      enum QDF_OPMODE mode,
 					      uint8_t vdev_id);
 
-/*
+/**
  * policy_mgr_handle_ml_sta_link_on_traffic_type_change() - Handle
  * enable/disable link on vdev traffic type change on SAP/P2P vdev
  * @psoc: objmgr psoc
@@ -4584,7 +4584,7 @@ void policy_mgr_handle_ml_sta_link_on_traffic_type_change(
 						struct wlan_objmgr_psoc *psoc,
 						struct wlan_objmgr_vdev *vdev);
 
-/*
+/**
  * policy_mgr_handle_ml_sta_links_on_vdev_down() - Handle enable
  * link on any vdev down
  * @psoc: objmgr psoc
@@ -4597,9 +4597,35 @@ void policy_mgr_handle_ml_sta_links_on_vdev_down(struct wlan_objmgr_psoc *psoc,
 						 enum QDF_OPMODE mode,
 						 uint8_t vdev_id);
 
+/**
+ * policy_mgr_handle_emlsr_sta_concurrency() - Handle EMLSR STA concurrency
+ * cases
+ *
+ * @psoc: objmgr psoc
+ * @vdev: pointer to vdev on which new connection is coming up
+ * @is_ap_up: flag to check if new connection is SAP/P2P GO
+ * @sta_coming_up: flag to check if new connection is STA/P2P client
+ *
+ * Return: void
+ */
 void policy_mgr_handle_emlsr_sta_concurrency(struct wlan_objmgr_psoc *psoc,
 					     struct wlan_objmgr_vdev *vdev,
 					     bool is_ap_up, bool sta_coming_up);
+
+/**
+ * policy_mgr_activate_mlo_links() - Force active ML links based on user
+ * requested link mac address
+ *
+ * @psoc: objmgr psoc
+ * @session_id: session id
+ * @num_links: number of links to be forced active
+ * @active_link_addr: link mac address of links to be forced active
+ *
+ * Return: void
+ */
+void policy_mgr_activate_mlo_links(struct wlan_objmgr_psoc *psoc,
+				   uint8_t session_id, uint8_t num_links,
+				   struct qdf_mac_addr *active_link_addr);
 #else
 
 static inline bool policy_mgr_is_mlo_sap_concurrency_allowed(

components/cmn_services/policy_mgr/src/wlan_policy_mgr_get_set_utils.c

@@ -6652,6 +6652,90 @@ policy_mgr_is_restart_sap_required_with_mlo_sta(struct wlan_objmgr_psoc *psoc,
 
 	return restart_required;
 }
+
+void policy_mgr_activate_mlo_links(struct wlan_objmgr_psoc *psoc,
+				   uint8_t session_id, uint8_t num_links,
+				   struct qdf_mac_addr active_link_addr[2])
+{
+	uint8_t idx, link, active_vdev_cnt = 0, inactive_vdev_cnt = 0;
+	uint16_t ml_vdev_cnt = 0;
+	struct wlan_objmgr_vdev *tmp_vdev_lst[WLAN_UMAC_MLO_MAX_VDEVS] = {0};
+	uint8_t active_vdev_lst[WLAN_UMAC_MLO_MAX_VDEVS] = {0};
+	uint8_t inactive_vdev_lst[WLAN_UMAC_MLO_MAX_VDEVS] = {0};
+	struct wlan_objmgr_vdev *vdev;
+	uint8_t *link_mac_addr;
+	bool active_vdev_present = false;
+
+	vdev = wlan_objmgr_get_vdev_by_id_from_psoc(psoc, session_id,
+						    WLAN_POLICY_MGR_ID);
+	if (!vdev) {
+		policy_mgr_err("vdev_id: %d vdev not found", session_id);
+		return;
+	}
+
+	if (!wlan_cm_is_vdev_connected(vdev)) {
+		policy_mgr_err("vdev is not in connected state");
+		goto done;
+	}
+
+	if (!wlan_vdev_mlme_is_mlo_vdev(vdev)) {
+		policy_mgr_err("vdev is not mlo vdev");
+		goto done;
+	}
+
+	mlo_get_ml_vdev_list(vdev, &ml_vdev_cnt, tmp_vdev_lst);
+	policy_mgr_debug("Num active links: %d, ML vdev cnt: %d", num_links,
+			 ml_vdev_cnt);
+	for (idx = 0; idx < ml_vdev_cnt; idx++) {
+		link_mac_addr = wlan_vdev_mlme_get_macaddr(tmp_vdev_lst[idx]);
+		policy_mgr_debug("link addr: " QDF_MAC_ADDR_FMT,
+				 QDF_MAC_ADDR_REF(link_mac_addr));
+		for (link = 0; link < num_links; link++) {
+			policy_mgr_debug("active addr: " QDF_MAC_ADDR_FMT,
+					 QDF_MAC_ADDR_REF(&active_link_addr[link]));
+			if (!qdf_mem_cmp(link_mac_addr,
+					 &active_link_addr[link].bytes[0],
+					 QDF_MAC_ADDR_SIZE)) {
+				active_vdev_lst[active_vdev_cnt] =
+					wlan_vdev_get_id(tmp_vdev_lst[idx]);
+				active_vdev_cnt++;
+				active_vdev_present = true;
+				policy_mgr_debug("Link address match");
+			}
+		}
+		if (!active_vdev_present) {
+			inactive_vdev_lst[inactive_vdev_cnt] =
+					wlan_vdev_get_id(tmp_vdev_lst[idx]);
+			inactive_vdev_cnt++;
+			policy_mgr_err("No link address match");
+		}
+		active_vdev_present = false;
+	}
+
+	policy_mgr_debug("active vdev cnt: %d, inactive vdev cnt: %d",
+			 active_vdev_cnt, inactive_vdev_cnt);
+	/*
+	 * Invoke Force active link cmd first, followed by Force inactive link
+	 * cmd. This ensures that there is atleast 1 link active at any given
+	 * time.
+	 */
+	if (active_vdev_cnt)
+		policy_mgr_mlo_sta_set_link(psoc,
+					    MLO_LINK_FORCE_REASON_DISCONNECT,
+					    MLO_LINK_FORCE_MODE_ACTIVE,
+					    active_vdev_cnt,
+					    active_vdev_lst);
+	if (inactive_vdev_cnt)
+		policy_mgr_mlo_sta_set_link(psoc, MLO_LINK_FORCE_REASON_CONNECT,
+					    MLO_LINK_FORCE_MODE_INACTIVE,
+					    inactive_vdev_cnt,
+					    inactive_vdev_lst);
+
+	for (idx = 0; idx < ml_vdev_cnt; idx++)
+		mlo_release_vdev_ref(tmp_vdev_lst[idx]);
+done:
+	wlan_objmgr_vdev_release_ref(vdev, WLAN_POLICY_MGR_ID);
+}
 #else
 static bool
 policy_mgr_allow_sta_concurrency(struct wlan_objmgr_psoc *psoc,

core/sme/inc/sme_api.h

@@ -3734,6 +3734,21 @@ void sme_set_mlo_max_simultaneous_links(mac_handle_t mac_handle,
  */
 void sme_set_mlo_assoc_link_band(mac_handle_t mac_handle, uint8_t vdev_id,
 				 uint8_t val);
+
+/**
+ * sme_activate_mlo_links() - Force active ML links based on user
+ * requested link mac address
+ *
+ * @mac_handle: Opaque handle to the global MAC context
+ * @session_id: session id
+ * @num_links: number of links to be forced active
+ * @active_link_addr: link mac address of (up to 2) links to be forced active
+ *
+ * Return: void
+ */
+void sme_activate_mlo_links(mac_handle_t mac_handle, uint8_t session_id,
+			    uint8_t num_links,
+			    struct qdf_mac_addr active_link_addr[2]);
 #else
 static inline void sme_set_eht_testbed_def(mac_handle_t mac_handle,
 					   uint8_t vdev_id)

core/sme/src/common/sme_api.c

@@ -15144,6 +15144,24 @@ void sme_update_eht_cap_mcs(mac_handle_t mac_handle, uint8_t vdev_id,
 	qdf_mem_copy(&mac_ctx->eht_cap_5g, mlme_eht_cap,
 		     sizeof(tDot11fIEeht_cap));
 }
+
+void sme_activate_mlo_links(mac_handle_t mac_handle, uint8_t session_id,
+			    uint8_t num_links,
+			    struct qdf_mac_addr active_link_addr[2])
+{
+	struct mac_context *mac_ctx = MAC_CONTEXT(mac_handle);
+	struct csr_roam_session *session;
+
+	session = CSR_GET_SESSION(mac_ctx, session_id);
+
+	if (!session) {
+		sme_err("No session for id %d", session_id);
+		return;
+	}
+
+	policy_mgr_activate_mlo_links(mac_ctx->psoc, session_id, num_links,
+				      active_link_addr);
+}
 #endif
 
 void sme_set_nss_capability(mac_handle_t mac_handle, uint8_t vdev_id,