qcacld-3.0: Process QCA_WLAN_VENDOR_ATTR_LINK_STATE_CONFIG

1.Host receives QCA_NL80211_VENDOR_SUBCMD_MLO_LINK_STATE
  vendor command.

2.Parse the new attribute
  QCA_WLAN_VENDOR_ATTR_LINK_STATE_CONFIG and get the
  link and corresponding link state.

3.Send it to FW via WMI command
  WMI_MLO_LINK_SET_ACTIVE_CMDID

Change-Id: I78a055f4a9fb60ccce106f9e4761fbf6d4d2f262
CRs-Fixed: 3529456

components/cmn_services/policy_mgr/inc/wlan_policy_mgr_api.h

@@ -4880,6 +4880,24 @@ void policy_mgr_handle_emlsr_sta_concurrency(struct wlan_objmgr_psoc *psoc,
 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);
+
+/**
+ * policy_mgr_update_mlo_links_based_on_linkid() - Force active ML links based
+ * on user requested coming via QCA_NL80211_VENDOR_SUBCMD_MLO_LINK_STATE
+ * @psoc: objmgr psoc
+ * @vdev_id: vdev id
+ * @num_links: number of links to be forced active
+ * @link_id_list: link id(s) list coming from user space
+ * @config_state_list: config state list coming from user space
+ *
+ * Return: success if the command gets processed successfully
+ */
+QDF_STATUS
+policy_mgr_update_mlo_links_based_on_linkid(struct wlan_objmgr_psoc *psoc,
+					    uint8_t vdev_id,
+					    uint8_t num_links,
+					    uint8_t *link_id_list,
+					    uint32_t *config_state_list);
 #else
 
 static inline bool policy_mgr_is_mlo_sap_concurrency_allowed(

components/cmn_services/policy_mgr/src/wlan_policy_mgr_get_set_utils.c

@@ -7444,6 +7444,181 @@ void policy_mgr_activate_mlo_links(struct wlan_objmgr_psoc *psoc,
 done:
 	wlan_objmgr_vdev_release_ref(vdev, WLAN_POLICY_MGR_ID);
 }
+
+QDF_STATUS
+policy_mgr_update_mlo_links_based_on_linkid(struct wlan_objmgr_psoc *psoc,
+					    uint8_t vdev_id,
+					    uint8_t num_links,
+					    uint8_t *link_id_list,
+					    uint32_t *config_state_list)
+{
+	uint8_t idx, link, active_vdev_cnt = 0, inactive_vdev_cnt = 0;
+	uint16_t ml_vdev_cnt = 0;
+	struct wlan_objmgr_vdev *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_id, num_links_to_disable = 0, num_matched_linkid = 0;
+	QDF_STATUS status = QDF_STATUS_E_FAILURE;
+	uint8_t num_ml_sta = 0, num_disabled_ml_sta = 0, num_non_ml = 0;
+	uint8_t ml_sta_vdev_lst[MAX_NUMBER_OF_CONC_CONNECTIONS] = {0};
+	qdf_freq_t ml_freq_lst[MAX_NUMBER_OF_CONC_CONNECTIONS] = {0};
+	struct policy_mgr_psoc_priv_obj *pm_ctx;
+
+	for (idx = 0; idx < num_links; idx++) {
+		if (config_state_list[idx] == 0)
+			num_links_to_disable++;
+	}
+
+	if (num_links_to_disable == num_links) {
+		policy_mgr_debug("vdev: %d num_links_to_disable: %d", vdev_id,
+				 num_links_to_disable);
+		return QDF_STATUS_E_FAILURE;
+	}
+
+	vdev = wlan_objmgr_get_vdev_by_id_from_psoc(psoc, vdev_id,
+						    WLAN_POLICY_MGR_ID);
+	if (!vdev) {
+		policy_mgr_err("vdev: %d vdev not found", vdev_id);
+		return QDF_STATUS_E_FAILURE;
+	}
+
+	if (!wlan_cm_is_vdev_connected(vdev)) {
+		policy_mgr_err("vdev: %d is not in connected state", vdev_id);
+		goto release_vdev_ref;
+	}
+
+	if (!wlan_vdev_mlme_is_mlo_vdev(vdev)) {
+		policy_mgr_err("vdev:%d is not mlo vdev", vdev_id);
+		goto release_vdev_ref;
+	}
+
+	pm_ctx = policy_mgr_get_context(psoc);
+	if (!pm_ctx) {
+		policy_mgr_err("Invalid pm context");
+		goto release_vdev_ref;
+	}
+
+	mlo_get_ml_vdev_list(vdev, &ml_vdev_cnt, vdev_lst);
+	for (idx = 0; idx < ml_vdev_cnt; idx++) {
+		link_id = wlan_vdev_get_link_id(vdev_lst[idx]);
+		for (link = 0; link < num_links; link++) {
+			if (link_id_list[link] == link_id) {
+				num_matched_linkid++;
+				policy_mgr_debug("link id:%d match", link_id);
+				if (config_state_list[link]) {
+					active_vdev_lst[active_vdev_cnt] =
+						wlan_vdev_get_id(vdev_lst[idx]);
+					active_vdev_cnt++;
+				} else {
+					inactive_vdev_lst[inactive_vdev_cnt] =
+						wlan_vdev_get_id(vdev_lst[idx]);
+					inactive_vdev_cnt++;
+				}
+			}
+		}
+	}
+
+	policy_mgr_debug("vdev: %d, active links: %d, ml count: %d, active count: %d, inactive count: %d",
+			 vdev_id, num_links, ml_vdev_cnt, active_vdev_cnt,
+			 inactive_vdev_cnt);
+
+	if (num_links != num_matched_linkid) {
+		policy_mgr_debug("invalid link id(s), num_matched_linkid: %d",
+				 num_matched_linkid);
+		goto release_ml_vdev_ref;
+	}
+
+	if (active_vdev_cnt &&
+	    policy_mgr_is_emlsr_sta_concurrency_present(psoc)) {
+		policy_mgr_debug("vdev: %d emlsr sta conn present", vdev_id);
+		if (active_vdev_cnt == 1)
+			status = QDF_STATUS_SUCCESS;
+		goto release_ml_vdev_ref;
+	}
+
+	policy_mgr_get_ml_sta_info(pm_ctx, &num_ml_sta, &num_disabled_ml_sta,
+				   ml_sta_vdev_lst, ml_freq_lst, &num_non_ml,
+				   NULL, NULL);
+	policy_mgr_debug("vdev %d: num_ml_sta %d disabled %d num_non_ml: %d",
+			 vdev_id, num_ml_sta, num_disabled_ml_sta, num_non_ml);
+
+	/*
+	 * No ML STA is present or sinle link ML is present or
+	 * more no.of links are active than supported concurrent connections
+	 */
+	if (!num_ml_sta || num_ml_sta < 2 ||
+	    num_ml_sta > MAX_NUMBER_OF_CONC_CONNECTIONS)
+		goto release_ml_vdev_ref;
+
+	if (!num_disabled_ml_sta) {
+		/*
+		 * both link are already enabled and received set link req to
+		 * enable both again
+		 */
+		if (active_vdev_cnt && !inactive_vdev_cnt) {
+			status = QDF_STATUS_SUCCESS;
+			goto release_ml_vdev_ref;
+		}
+
+		/*
+		 * both link are already enabled and received set link req
+		 * disable one link, disable any
+		 */
+		if (active_vdev_cnt && inactive_vdev_cnt) {
+			status = policy_mgr_mlo_sta_set_link_ext(psoc,
+					MLO_LINK_FORCE_REASON_CONNECT,
+					MLO_LINK_FORCE_MODE_ACTIVE_INACTIVE,
+					active_vdev_cnt, active_vdev_lst,
+					inactive_vdev_cnt, inactive_vdev_lst);
+			goto release_ml_vdev_ref;
+		}
+	} else {
+		/*
+		 * one link is enable and one link is disabled, If disabled
+		 * link can not be allowed to enable then send status failure
+		 * to upper layer.
+		 */
+		if (active_vdev_cnt &&
+		    !policy_mgr_sta_ml_link_enable_allowed(psoc,
+							   num_disabled_ml_sta,
+							   num_ml_sta,
+							   ml_freq_lst,
+							   ml_sta_vdev_lst)) {
+			policy_mgr_debug("vdev %d: link enable not allowed",
+					 vdev_id);
+			goto release_ml_vdev_ref;
+		}
+
+		/*
+		 * If there are both active and inactive vdev count, then
+		 * issue a single WMI with force mode
+		 * MLO_LINK_FORCE_MODE_ACTIVE_INACTIVE, else if there is only
+		 * active vdev count, send single WMI for all active vdevs
+		 * with force mode MLO_LINK_FORCE_MODE_ACTIVE.
+		 */
+		if (active_vdev_cnt && inactive_vdev_cnt)
+			status = policy_mgr_mlo_sta_set_link_ext(psoc,
+					MLO_LINK_FORCE_REASON_CONNECT,
+					MLO_LINK_FORCE_MODE_ACTIVE_INACTIVE,
+					active_vdev_cnt, active_vdev_lst,
+					inactive_vdev_cnt, inactive_vdev_lst);
+		else if (active_vdev_cnt && !inactive_vdev_cnt)
+			status = policy_mgr_mlo_sta_set_link(psoc,
+					MLO_LINK_FORCE_REASON_DISCONNECT,
+					MLO_LINK_FORCE_MODE_ACTIVE,
+					active_vdev_cnt, active_vdev_lst);
+	}
+
+release_ml_vdev_ref:
+	for (idx = 0; idx < ml_vdev_cnt; idx++)
+		mlo_release_vdev_ref(vdev_lst[idx]);
+release_vdev_ref:
+	wlan_objmgr_vdev_release_ref(vdev, WLAN_POLICY_MGR_ID);
+
+	return status;
+}
+
 #else
 static bool
 policy_mgr_allow_sta_concurrency(struct wlan_objmgr_psoc *psoc,

core/hdd/inc/wlan_hdd_mlo.h

@@ -205,6 +205,7 @@ void hdd_mlo_t2lm_unregister_callback(struct wlan_objmgr_vdev *vdev);
  * wlan_handle_mlo_link_state_operation() - mlo link state operation
  * @wiphy: wiphy pointer
  * @vdev: vdev handler
+ * @hdd_ctx: hdd context
  * @data: pointer to incoming NL vendor data
  * @data_len: length of @data
  *
@@ -212,10 +213,10 @@ void hdd_mlo_t2lm_unregister_callback(struct wlan_objmgr_vdev *vdev);
  *
  * Return: 0 on success and error number otherwise.
  */
-int
-wlan_handle_mlo_link_state_operation(struct wiphy *wiphy,
-				     struct wlan_objmgr_vdev *vdev,
-				     const void *data, int data_len);
+int wlan_handle_mlo_link_state_operation(struct wiphy *wiphy,
+					 struct wlan_objmgr_vdev *vdev,
+					 struct hdd_context *hdd_ctx,
+					 const void *data, int data_len);
 
 extern const struct nla_policy
 ml_link_state_request_policy[QCA_WLAN_VENDOR_ATTR_LINK_STATE_MAX + 1];
@@ -322,6 +323,7 @@ void hdd_mlo_t2lm_unregister_callback(struct wlan_objmgr_vdev *vdev)
 static inline int
 wlan_handle_mlo_link_state_operation(struct wiphy *wiphy,
 				     struct wlan_objmgr_vdev *vdev,
+				     struct hdd_context *hdd_ctx,
 				     const void *data, int data_len)
 {
 	return 0;

core/hdd/src/wlan_hdd_mlo.c

@@ -391,12 +391,17 @@ __wlan_hdd_cfg80211_process_ml_link_state(struct wiphy *wiphy,
 	struct net_device *dev = wdev->netdev;
 	struct hdd_adapter *adapter = WLAN_HDD_GET_PRIV_PTR(dev);
 	struct wlan_objmgr_vdev *vdev;
+	struct hdd_context *hdd_ctx = NULL;
 
 	hdd_enter_dev(wdev->netdev);
 
 	if (hdd_validate_adapter(adapter))
 		return -EINVAL;
 
+	hdd_ctx = WLAN_HDD_GET_CTX(adapter);
+	if (!hdd_ctx)
+		return -EINVAL;
+
 	if (adapter->device_mode != QDF_STA_MODE)
 		return -EINVAL;
 
@@ -405,7 +410,8 @@ __wlan_hdd_cfg80211_process_ml_link_state(struct wiphy *wiphy,
 	if (!vdev)
 		return -EINVAL;
 
-	ret = wlan_handle_mlo_link_state_operation(wiphy, vdev, data, data_len);
+	ret = wlan_handle_mlo_link_state_operation(wiphy, vdev, hdd_ctx,
+						   data, data_len);
 
 	hdd_objmgr_put_vdev_by_user(vdev, WLAN_OSIF_ID);
 
@@ -635,26 +641,26 @@ free_event:
 
 #define MLD_MAX_SUPPORTED_LINKS 2
 
-int
-wlan_handle_mlo_link_state_operation(struct wiphy *wiphy,
-				     struct wlan_objmgr_vdev *vdev,
-				     const void *data, int data_len)
+int wlan_handle_mlo_link_state_operation(struct wiphy *wiphy,
+					 struct wlan_objmgr_vdev *vdev,
+					 struct hdd_context *hdd_ctx,
+					 const void *data, int data_len)
 {
 	struct nlattr *tb[QCA_WLAN_VENDOR_ATTR_LINK_STATE_MAX + 1];
 	struct nlattr *tb2[QCA_WLAN_VENDOR_ATTR_LINK_STATE_CONFIG_MAX + 1];
 	enum qca_wlan_vendor_link_state_op_types ml_link_op;
 	struct nlattr *link_oper_attr, *mode_attr, *curr_attr, *num_link_attr;
-	int rem_len = 0, rc = 0;
-	uint32_t attr_id, ml_config_state = 0;
-	uint8_t ml_active_num_links = 0, ml_link_control_mode = 0;
-	uint8_t ml_config_link_id = 0, num_links = 0;
+	int rem_len = 0, rc;
+	uint32_t attr_id, ml_config_state;
+	uint8_t ml_active_num_links, ml_link_control_mode;
+	uint8_t ml_config_link_id, num_links = 0;
 	uint8_t vdev_id = vdev->vdev_objmgr.vdev_id;
 	uint8_t link_id_list[MLD_MAX_SUPPORTED_LINKS] = {0};
 	uint32_t config_state_list[MLD_MAX_SUPPORTED_LINKS] = {0};
+	QDF_STATUS status;
 
 	if (wlan_cfg80211_nla_parse(tb, QCA_WLAN_VENDOR_ATTR_LINK_STATE_MAX,
-				    data,
-				    data_len,
+				    data, data_len,
 				    ml_link_state_request_policy)) {
 		hdd_debug("vdev %d: invalid mlo link state attr", vdev_id);
 		return -EINVAL;
@@ -736,7 +742,14 @@ wlan_handle_mlo_link_state_operation(struct wiphy *wiphy,
 			if (num_links >= MLD_MAX_SUPPORTED_LINKS)
 				break;
 		}
-		/* TODO: send link(s) enable/disable command to FW */
+
+		status = policy_mgr_update_mlo_links_based_on_linkid(
+						hdd_ctx->psoc,
+						vdev_id, num_links,
+						link_id_list,
+						config_state_list);
+		if (QDF_IS_STATUS_ERROR(status))
+			return -EINVAL;
 		break;
 	case QCA_WLAN_VENDOR_LINK_STATE_CONTROL_MODE_MIXED:
 		attr_id =
@@ -759,6 +772,8 @@ wlan_handle_mlo_link_state_operation(struct wiphy *wiphy,
 		return -EINVAL;
 	}
 
+	hdd_debug("vdev: %d, processed link state command successfully",
+		  vdev_id);
 	return 0;
 }