qcacld-3.0: Add Vendor command support for MLO link state

Add vendor support for MLO link state.
Change-Id: Ifebb53917cca08952439d2cd21d0564326b62a5e
CRs-Fixed: 3432249

core/hdd/inc/wlan_hdd_mlo.h

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2021, The Linux Foundation. All rights reserved.
- * Copyright (c) 2021-2022 Qualcomm Innovation Center, Inc. All rights reserved.
+ * Copyright (c) 2021-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 above
@@ -54,6 +54,18 @@ struct hdd_adapter_create_param {
 			   ((x)->mlo_adapter_info.associate_with_ml_adapter)
 #define hdd_adapter_get_mlo_adapter_from_link(x) \
 			   ((x)->mlo_adapter_info.ml_adapter)
+/* MLO_STATE_COMMANDS */
+#define FEATURE_ML_LINK_STATE_COMMANDS					\
+	{								\
+		.info.vendor_id = QCA_NL80211_VENDOR_ID,		\
+		.info.subcmd = QCA_NL80211_VENDOR_SUBCMD_MLO_LINK_STATE,\
+		.flags = WIPHY_VENDOR_CMD_NEED_WDEV |			\
+			 WIPHY_VENDOR_CMD_NEED_NETDEV |			\
+			 WIPHY_VENDOR_CMD_NEED_RUNNING,			\
+		.doit = wlan_hdd_cfg80211_process_ml_link_state,	\
+		vendor_command_policy(ml_link_state_request_policy,	\
+				QCA_WLAN_VENDOR_ATTR_LINK_STATE_MAX)	\
+	},
 #else
 #define hdd_adapter_is_link_adapter(x) (0)
 #define hdd_adapter_is_ml_adapter(x)   (0)
@@ -158,6 +170,55 @@ struct hdd_adapter *hdd_get_ml_adapter(struct hdd_context *hdd_ctx);
  * Return: adapter or NULL
  */
 struct hdd_adapter *hdd_get_assoc_link_adapter(struct hdd_adapter *ml_adapter);
+
+/**
+ * hdd_mlo_t2lm_register_callback() - Register T2LM callback
+ * @vdev: Pointer to vdev
+ *
+ * Return: None
+ */
+void hdd_mlo_t2lm_register_callback(struct wlan_objmgr_vdev *vdev);
+
+/**
+ * hdd_mlo_t2lm_unregister_callback() - Unregister T2LM callback
+ * @vdev: Pointer to vdev
+ *
+ * Return: None
+ */
+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
+ * @data: pointer to incoming NL vendor data
+ * @data_len: length of @data
+ *
+ * Based on the data get or set the mlo link state
+ * Return: QDF_STATUS
+ */
+QDF_STATUS
+wlan_handle_mlo_link_state_operation(struct wiphy *wiphy,
+				     struct wlan_objmgr_vdev *vdev,
+				     const void *data, int data_len);
+
+extern const struct nla_policy
+ml_link_state_request_policy[QCA_WLAN_VENDOR_ATTR_LINK_STATE_MAX + 1];
+
+/**
+ * wlan_hdd_cfg80211_process_ml_link_state() - process ml link state
+ * @wiphy: wiphy pointer
+ * @wdev: pointer to struct wireless_dev
+ * @data: pointer to incoming NL vendor data
+ * @data_len: length of @data
+ *
+ * Set (or) get the ml link state.
+ *
+ * Return:  0 on success; error number otherwise.
+ */
+int wlan_hdd_cfg80211_process_ml_link_state(struct wiphy *wiphy,
+					    struct wireless_dev *wdev,
+					    const void *data, int data_len);
 #else
 static inline
 QDF_STATUS hdd_wlan_unregister_mlo_interfaces(struct hdd_adapter *adapter,
@@ -199,5 +260,33 @@ struct hdd_adapter *hdd_get_assoc_link_adapter(struct hdd_adapter *ml_adapter)
 {
 	return NULL;
 }
+
+static inline
+void hdd_mlo_t2lm_register_callback(struct wlan_objmgr_vdev *vdev)
+{
+}
+
+static inline
+void hdd_mlo_t2lm_unregister_callback(struct wlan_objmgr_vdev *vdev)
+{
+}
+
+static inline QDF_STATUS
+wlan_handle_mlo_link_state_operation(struct wiphy *wiphy,
+				     struct wlan_objmgr_vdev *vdev,
+				     const void *data, int data_len)
+{
+	return QDF_STATUS_SUCCESS;
+}
+
+static inline
+int wlan_hdd_cfg80211_process_ml_link_state(struct wiphy *wiphy,
+					    struct wireless_dev *wdev,
+					    const void *data, int data_len)
+{
+	return -ENOTSUPP;
+}
+
+#define FEATURE_ML_LINK_STATE_COMMANDS
 #endif
 #endif

core/hdd/src/wlan_hdd_cfg80211.c

@@ -200,6 +200,7 @@
 #include "wlan_psoc_mlme_api.h"
 #include <utils_mlo.h>
 #include "wlan_mlo_mgr_roam.h"
+#include "wlan_hdd_mlo.h"
 
 /*
  * A value of 100 (milliseconds) can be sent to FW.
@@ -18973,6 +18974,7 @@ const struct wiphy_vendor_command hdd_wiphy_vendor_commands[] = {
 		vendor_command_policy(VENDOR_CMD_RAW_DATA, 0)
 	},
 	FEATURE_COAP_OFFLOAD_COMMANDS
+	FEATURE_ML_LINK_STATE_COMMANDS
 };
 
 struct hdd_context *hdd_cfg80211_wiphy_alloc(void)

core/hdd/src/wlan_hdd_mlo.c

@@ -28,9 +28,15 @@
 #include "wlan_osif_features.h"
 #include "wlan_dp_ucfg_api.h"
 #include "wlan_psoc_mlme_ucfg_api.h"
+#include "wlan_osif_request_manager.h"
+#include "wlan_hdd_object_manager.h"
+
+/*max time in ms, caller may wait for link state request get serviced */
+#define WLAN_WAIT_TIME_LINK_STATE 800
 
 #if defined(CFG80211_11BE_BASIC)
 #ifdef CFG80211_IFTYPE_MLO_LINK_SUPPORT
+
 static
 void wlan_hdd_register_ml_link(struct hdd_adapter *sta_adapter,
 			       struct hdd_adapter *link_adapter)
@@ -277,4 +283,303 @@ int hdd_update_vdev_mac_address(struct hdd_context *hdd_ctx,
 	return ret;
 }
 #endif
+
+const struct nla_policy
+ml_link_state_request_policy[QCA_WLAN_VENDOR_ATTR_LINK_STATE_MAX + 1] = {
+	[QCA_WLAN_VENDOR_ATTR_LINK_STATE_OP_TYPE] = {.type = NLA_U32},
+};
+
+static int
+__wlan_hdd_cfg80211_process_ml_link_state(struct wiphy *wiphy,
+					  struct wireless_dev *wdev,
+					  const void *data, int data_len)
+{
+	int ret = 0;
+	struct net_device *dev = wdev->netdev;
+	struct hdd_adapter *adapter = WLAN_HDD_GET_PRIV_PTR(dev);
+	struct wlan_objmgr_vdev *vdev;
+
+	hdd_enter_dev(wdev->netdev);
+
+	if (hdd_validate_adapter(adapter))
+		return -EINVAL;
+
+	vdev = hdd_objmgr_get_vdev_by_user(adapter, WLAN_OSIF_ID);
+
+	if (!vdev)
+		return -EINVAL;
+
+	wlan_handle_mlo_link_state_operation(wiphy, vdev, data, data_len);
+
+	hdd_objmgr_put_vdev_by_user(vdev, WLAN_OSIF_ID);
+
+	return ret;
+}
+
+int wlan_hdd_cfg80211_process_ml_link_state(struct wiphy *wiphy,
+					    struct wireless_dev *wdev,
+					    const void *data, int data_len)
+{
+	int errno;
+	struct osif_vdev_sync *vdev_sync;
+
+	errno = osif_vdev_sync_op_start(wdev->netdev, &vdev_sync);
+	if (errno)
+		return errno;
+
+	errno = __wlan_hdd_cfg80211_process_ml_link_state(wiphy, wdev, data,
+							  data_len);
+
+	osif_vdev_sync_op_stop(vdev_sync);
+
+	return errno;
+}
+
+static inline void ml_link_state_resp_cb(struct ml_link_state_info_event *ev,
+					 void *cookie)
+{
+	struct ml_link_state_info_event *priv;
+	struct osif_request *request;
+
+	request = osif_request_get(cookie);
+
+	if (!request) {
+		hdd_err("Obsolete request");
+		return;
+	}
+
+	priv = osif_request_priv(request);
+
+	qdf_mem_copy(priv, ev, sizeof(*priv));
+	osif_request_complete(request);
+	osif_request_put(request);
+}
+
+static uint32_t
+hdd_get_ml_link_state_response_len(const struct ml_link_state_info_event *event)
+{
+	uint32_t len = 0;
+	uint32_t info_len = 0;
+
+	len = NLMSG_HDRLEN;
+	/* QCA_WLAN_VENDOR_ATTR_LINK_STATE_CONTROL_MODE */
+	len += NLA_HDRLEN + sizeof(u32);
+
+	/* QCA_WLAN_VENDOR_ATTR_LINK_STATE_OPERATION_MODE */
+	len += NLA_HDRLEN + sizeof(u32);
+
+	/* nest */
+	info_len = NLA_HDRLEN;
+	/* QCA_WLAN_VENDOR_ATTR_LINK_STATE_CONFIG_LINK_ID */
+	info_len += NLA_HDRLEN + sizeof(u8);
+	/* QCA_WLAN_VENDOR_ATTR_LINK_STATE_CONFIG_STATE */
+	info_len += NLA_HDRLEN + sizeof(u32);
+
+	/* QCA_WLAN_VENDOR_ATTR_LINK_STATE_CONFIG */
+	len += NLA_HDRLEN + (info_len * event->num_mlo_vdev_link_info);
+
+	return len;
+}
+
+static int
+hdd_ml_generate_link_state_resp_nlmsg(struct sk_buff *skb,
+				      struct ml_link_state_info_event *params,
+				      uint32_t num_link_info)
+{
+	struct nlattr *nla_config_attr, *nla_config_params;
+	uint32_t i = 0, attr;
+	int errno;
+	uint32_t value;
+
+	attr = QCA_WLAN_VENDOR_ATTR_LINK_STATE_CONTROL_MODE;
+
+	/* Default control mode is only supported */
+	value = QCA_WLAN_VENDOR_LINK_STATE_CONTROL_MODE_DEFAULT;
+	errno = nla_put_u32(skb, attr, value);
+	if (errno)
+		return errno;
+
+	attr = QCA_WLAN_VENDOR_ATTR_LINK_STATE_OPERATION_MODE;
+
+	/* Default link state operation mode is only supported */
+	value = QCA_WLAN_VENDOR_LINK_STATE_OPERATION_MODE_DEFAULT;
+	errno = nla_put_u32(skb, attr, value);
+	if (errno)
+		return errno;
+
+	attr = QCA_WLAN_VENDOR_ATTR_LINK_STATE_CONFIG;
+	nla_config_attr = nla_nest_start(skb, attr);
+
+	if (!nla_config_attr)
+		return -EINVAL;
+
+	for (i = 0; i < num_link_info; i++) {
+		nla_config_params = nla_nest_start(skb, attr);
+		if (!nla_config_params)
+			return -EINVAL;
+
+		attr = QCA_WLAN_VENDOR_ATTR_LINK_STATE_CONFIG_LINK_ID;
+		value = params->link_info[i].link_id;
+		errno = nla_put_u8(skb, attr, value);
+		if (errno)
+			return errno;
+
+		attr = QCA_WLAN_VENDOR_ATTR_LINK_STATE_CONFIG_STATE;
+		value = params->link_info[i].link_status;
+		errno = nla_put_u32(skb, attr, value);
+
+		if (errno)
+			return errno;
+
+		nla_nest_end(skb, nla_config_params);
+	}
+
+	nla_nest_end(skb, nla_config_attr);
+
+	return 0;
+}
+
+static QDF_STATUS wlan_hdd_link_state_request(struct wiphy *wiphy,
+					      struct wlan_objmgr_vdev *vdev)
+{
+	int errno;
+	int skb_len;
+	struct sk_buff *reply_skb = NULL;
+	QDF_STATUS status = QDF_STATUS_E_INVAL;
+	void *cookie;
+	struct ml_link_state_info_event *link_state_event = NULL;
+	struct osif_request *request;
+	struct ml_link_state_cmd_info info = {0};
+	int num_info = 0;
+	static const struct osif_request_params params = {
+		.priv_size = sizeof(*link_state_event),
+		.timeout_ms = WLAN_WAIT_TIME_LINK_STATE,
+		.dealloc = NULL,
+	};
+
+	if (!wiphy || !vdev)
+		return status;
+
+	request = osif_request_alloc(&params);
+	if (!request)
+		return QDF_STATUS_E_NOMEM;
+
+	cookie = osif_request_cookie(request);
+	link_state_event = osif_request_priv(request);
+
+	info.request_cookie = cookie;
+	info.ml_link_state_resp_cb = ml_link_state_resp_cb;
+
+	status = mlo_get_link_state_register_resp_cb(vdev,
+						     &info);
+	if (QDF_IS_STATUS_ERROR(status)) {
+		hdd_err("Failed to register resp callback: %d", status);
+		status = qdf_status_to_os_return(status);
+		goto free_event;
+	}
+
+	status = ml_post_get_link_state_msg(vdev);
+	if (QDF_IS_STATUS_ERROR(status)) {
+		hdd_err("Failed to post scheduler msg");
+		goto free_event;
+		return status;
+	}
+
+	status = osif_request_wait_for_response(request);
+	if (status) {
+		hdd_err("wait failed or timed out ret: %d", status);
+		goto free_event;
+	}
+
+	hdd_debug("ml_link_state_resp: vdev id %d status %d num %d MAC addr " QDF_MAC_ADDR_FMT,
+		  link_state_event->vdev_id, link_state_event->status,
+		  link_state_event->num_mlo_vdev_link_info,
+		  QDF_MAC_ADDR_REF(link_state_event->mldaddr.bytes));
+
+	for (num_info = 0; num_info < link_state_event->num_mlo_vdev_link_info;
+	     num_info++) {
+		hdd_debug("ml_link_state_resp: chan_freq %d vdev_id %d link_id %d link_status %d",
+			  link_state_event->link_info[num_info].chan_freq,
+			  link_state_event->link_info[num_info].vdev_id,
+			  link_state_event->link_info[num_info].link_id,
+			  link_state_event->link_info[num_info].link_status);
+	}
+
+	skb_len = hdd_get_ml_link_state_response_len(link_state_event);
+
+	reply_skb = wlan_cfg80211_vendor_cmd_alloc_reply_skb(
+						wiphy,
+						skb_len);
+	if (!reply_skb) {
+		hdd_err("Get stats - alloc reply_skb failed");
+		status = QDF_STATUS_E_NOMEM;
+		goto free_event;
+	}
+
+	status = hdd_ml_generate_link_state_resp_nlmsg(
+			reply_skb, link_state_event,
+			link_state_event->num_mlo_vdev_link_info);
+	if (QDF_IS_STATUS_ERROR(status)) {
+		hdd_err("Failed to pack nl response");
+		goto free_skb;
+	}
+
+	osif_request_put(request);
+
+	errno = wlan_cfg80211_vendor_cmd_reply(reply_skb);
+	return qdf_status_from_os_return(errno);
+
+free_skb:
+	wlan_cfg80211_vendor_free_skb(reply_skb);
+free_event:
+	osif_request_put(request);
+
+	return status;
+}
+
+QDF_STATUS
+wlan_handle_mlo_link_state_operation(struct wiphy *wiphy,
+				     struct wlan_objmgr_vdev *vdev,
+				     const void *data, int data_len)
+{
+	struct nlattr *tb[QCA_WLAN_VENDOR_ATTR_LINK_STATE_MAX + 1];
+	enum qca_wlan_vendor_link_state_op_types ml_link_op;
+	struct nlattr *link_oper_attr;
+	uint32_t id;
+	int ret = 0;
+
+	if (wlan_cfg80211_nla_parse(tb, QCA_WLAN_VENDOR_ATTR_LINK_STATE_MAX,
+				    data,
+				    data_len,
+				    ml_link_state_request_policy)) {
+		hdd_err_rl("invalid twt attr");
+		return -EINVAL;
+	}
+
+	id = QCA_WLAN_VENDOR_ATTR_LINK_STATE_OP_TYPE;
+	link_oper_attr = tb[id];
+	if (!link_oper_attr) {
+		hdd_err_rl("link state operation NOT specified");
+		return -EINVAL;
+	}
+
+	ml_link_op = nla_get_u8(link_oper_attr);
+
+	hdd_debug("ml link state request:%d", ml_link_op);
+	switch (ml_link_op) {
+	case QCA_WLAN_VENDOR_LINK_STATE_OP_GET:
+		ret = wlan_hdd_link_state_request(wiphy, vdev);
+		break;
+	case QCA_WLAN_VENDOR_LINK_STATE_OP_SET:
+		hdd_debug_rl("ml link SET state not supported");
+		break;
+	default:
+		hdd_err_rl("Invalid link state operation");
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
 #endif