qcacld-3.0: Add support for T2LM vendor interface implementation

Add support for T2LM vendor interface implementation

Change-Id: I2150bdb90369b3ed57b0fbbbd67604b068b01aa7
CRs-Fixed: 3505252

core/hdd/inc/wlan_hdd_mlo.h

@@ -219,6 +219,17 @@ wlan_handle_mlo_link_state_operation(struct wiphy *wiphy,
 extern const struct nla_policy
 ml_link_state_request_policy[QCA_WLAN_VENDOR_ATTR_LINK_STATE_MAX + 1];
 
+/**
+ * wlan_hdd_send_t2lm_event() - Send t2lm info to userspace
+ * @vdev: vdev handler
+ * @t2lm: tid to link mapping info
+ *
+ * This function is called when driver needs to send vendor specific
+ * t2lm info to userspace
+ */
+QDF_STATUS wlan_hdd_send_t2lm_event(struct wlan_objmgr_vdev *vdev,
+				    struct wlan_t2lm_info *t2lm);
+
 /**
  * wlan_hdd_cfg80211_process_ml_link_state() - process ml link state
  * @wiphy: wiphy pointer

core/hdd/src/wlan_hdd_cfg80211.c

@@ -1955,6 +1955,10 @@ static const struct nl80211_vendor_cmd_info wlan_hdd_cfg80211_vendor_events[] =
 	FEATURE_GREEN_AP_LOW_LATENCY_PWR_SAVE_EVENT
 	FEATURE_ROAM_STATS_EVENTS
 #ifdef WLAN_FEATURE_11BE_MLO
+	[QCA_NL80211_VENDOR_SUBCMD_TID_TO_LINK_MAP_INDEX] = {
+		.vendor_id = QCA_NL80211_VENDOR_ID,
+		.subcmd = QCA_NL80211_VENDOR_SUBCMD_TID_TO_LINK_MAP,
+	},
 	[QCA_NL80211_VENDOR_SUBCMD_LINK_RECONFIG_INDEX] = {
 		.vendor_id = QCA_NL80211_VENDOR_ID,
 		.subcmd = QCA_NL80211_VENDOR_SUBCMD_LINK_RECONFIG,
@@ -27165,7 +27169,8 @@ wlan_hdd_cfg80211_del_intf_link(struct wiphy *wiphy, struct wireless_dev *wdev,
 }
 #endif
 
-#if defined(WLAN_FEATURE_11BE_MLO) && defined(WLAN_TID_LINK_MAP_SUPPORT)
+#if defined(WLAN_FEATURE_11BE_MLO)
+#if defined(WLAN_TID_LINK_MAP_SUPPORT)
 #define MAX_T2LM_INFO 2
 
 static void wlan_hdd_print_t2lm_info(struct cfg80211_mlo_tid_map *map)
@@ -27331,13 +27336,82 @@ wlan_hdd_cfg80211_get_t2lm_mapping_status(struct wiphy *wiphy,
 	return errno;
 }
 
+QDF_STATUS hdd_tid_to_link_map(struct wlan_objmgr_vdev *vdev,
+			       struct wlan_t2lm_info *t2lm)
+{
+	struct cfg80211_mlo_tid_map map;
+	bool found = false;
+
+	qdf_mem_zero(&map, sizeof(map));
+	wlan_hdd_fill_map(t2lm, &map, &found);
+	if (!found) {
+		hdd_debug("Failed to get t2lm info");
+		return QDF_STATUS_E_FAILURE;
+	}
+
+	wlan_hdd_print_t2lm_info(&map);
+	cfg80211_tid_to_link_map_change(dev, &map);
+	return QDF_STATUS_SUCCESS;
+}
+
+#else
+static void wlan_hdd_print_vendor_t2lm_info(struct wlan_t2lm_info *t2lm)
+{
+	int tid, value = 0;
+
+	hdd_debug("default mapping: %d", t2lm->default_link_mapping);
+
+	if (t2lm->direction == WLAN_T2LM_INVALID_DIRECTION)
+		return;
+
+	switch (t2lm->direction) {
+	case WLAN_T2LM_BIDI_DIRECTION:
+		for (tid = 0; tid < T2LM_MAX_NUM_TIDS; tid++) {
+			hdd_debug("TID[%d]: Downlink: %d Uplink: %d",
+				  tid, t2lm->ieee_link_map_tid[tid],
+				  t2lm->ieee_link_map_tid[tid]);
+		}
+		break;
+	case WLAN_T2LM_DL_DIRECTION:
+		for (tid = 0; tid < T2LM_MAX_NUM_TIDS; tid++) {
+			/* Keep uplink info as 0 for downlink direction */
+			hdd_debug("TID[%d]: Downlink: %d Uplink: %d",
+				  tid, t2lm->ieee_link_map_tid[tid], value);
+		}
+		break;
+	case WLAN_T2LM_UL_DIRECTION:
+		for (tid = 0; tid < T2LM_MAX_NUM_TIDS; tid++) {
+			/* Keep downlinklink info as 0 for downlink direction */
+			hdd_debug("TID[%d]: Downlink: %d Uplink: %d",
+				  tid, value, t2lm->ieee_link_map_tid[tid]);
+		}
+		break;
+	default:
+		return;
+	}
+}
+
+QDF_STATUS hdd_tid_to_link_map(struct wlan_objmgr_vdev *vdev,
+			       struct wlan_t2lm_info *t2lm)
+{
+	uint8_t ret;
+
+	wlan_hdd_print_vendor_t2lm_info(t2lm);
+	ret = wlan_hdd_send_t2lm_event(vdev, t2lm);
+	if (QDF_IS_STATUS_ERROR(ret)) {
+		hdd_debug("failed to send t2lm info to userspace");
+		return QDF_STATUS_E_FAILURE;
+	}
+	return QDF_STATUS_SUCCESS;
+}
+#endif
+
 QDF_STATUS hdd_mlo_dev_t2lm_notify_link_update(struct wlan_objmgr_vdev *vdev,
 					       struct wlan_t2lm_info *t2lm)
 {
-	struct cfg80211_mlo_tid_map map;
 	struct wlan_hdd_link_info *link_info;
 	struct net_device *dev;
-	bool found = false;
+	uint8_t ret;
 
 	link_info = wlan_hdd_get_link_info_from_objmgr(vdev);
 	if (!link_info) {
@@ -27348,17 +27422,12 @@ QDF_STATUS hdd_mlo_dev_t2lm_notify_link_update(struct wlan_objmgr_vdev *vdev,
 	dev = link_info->adapter->dev;
 	hdd_enter_dev(dev);
 
-	qdf_mem_zero(&map, sizeof(map));
-
-	wlan_hdd_fill_map(t2lm, &map, &found);
-	if (!found) {
-		hdd_debug("Failed to get t2lm info");
+	ret = hdd_tid_to_link_map(vdev, t2lm);
+	if (QDF_IS_STATUS_ERROR(ret)) {
+		hdd_debug("tid to link map change failed ");
 		return QDF_STATUS_E_FAILURE;
 	}
 
-	wlan_hdd_print_t2lm_info(&map);
-	cfg80211_tid_to_link_map_change(dev, &map);
-
 	hdd_exit();
 
 	return QDF_STATUS_SUCCESS;

core/hdd/src/wlan_hdd_cfg80211.h

@@ -1074,7 +1074,17 @@ struct wlan_objmgr_vdev *wlan_key_get_link_vdev(struct hdd_adapter *adapter,
 void wlan_key_put_link_vdev(struct wlan_objmgr_vdev *link_vdev,
 			    wlan_objmgr_ref_dbgid id);
 
-#if defined(WLAN_FEATURE_11BE_MLO) && defined(WLAN_TID_LINK_MAP_SUPPORT)
+#if defined(WLAN_FEATURE_11BE_MLO)
+/**
+ * hdd_tid_to_link_map() - to get t2lm info
+ * @vdev: Pointer to vdev
+ * @t2lm: T2LM info
+ *
+ * Return: QDF_STATUS
+ */
+QDF_STATUS hdd_tid_to_link_map(struct wlan_objmgr_vdev *vdev,
+			       struct wlan_t2lm_info *t2lm);
+
 /**
  * hdd_mlo_dev_t2lm_notify_link_update() - Send update T2LM info event
  * @vdev: Pointer to vdev

core/hdd/src/wlan_hdd_mlo.c

@@ -665,4 +665,176 @@ wlan_handle_mlo_link_state_operation(struct wiphy *wiphy,
 	return ret;
 }
 
+static uint32_t
+hdd_get_t2lm_setup_event_len(void)
+{
+	uint32_t len = 0;
+	uint32_t info_len = 0;
+
+	len = NLMSG_HDRLEN;
+
+	/* QCA_WLAN_VENDOR_ATTR_TID_TO_LINK_MAP_AP_MLD_ADDR */
+	len += nla_total_size(QDF_MAC_ADDR_SIZE);
+
+	/* nest */
+	info_len = NLA_HDRLEN;
+	/* QCA_WLAN_VENDOR_ATTR_LINK_TID_MAP_STATUS_UPLINK */
+	info_len += NLA_HDRLEN + sizeof(u16);
+	/* QCA_WLAN_VENDOR_ATTR_LINK_TID_MAP_STATUS_DOWNLINK */
+	info_len += NLA_HDRLEN + sizeof(u16);
+
+	/* QCA_WLAN_VENDOR_ATTR_TID_TO_LINK_MAP_STATUS */
+	len += NLA_HDRLEN + (info_len * T2LM_MAX_NUM_TIDS);
+
+	return len;
+}
+
+static QDF_STATUS
+hdd_t2lm_pack_nl_response(struct sk_buff *skb,
+			  struct wlan_objmgr_vdev *vdev,
+			  struct wlan_t2lm_info *t2lm,
+			  struct qdf_mac_addr mld_addr)
+{
+	struct nlattr *config_attr, *config_params;
+	uint32_t i = 0, attr, attr1;
+	int errno;
+	uint32_t value;
+	uint8_t tid_num;
+
+	attr = QCA_WLAN_VENDOR_ATTR_TID_TO_LINK_MAP_AP_MLD_ADDR;
+	if (nla_put(skb, attr, QDF_MAC_ADDR_SIZE, mld_addr.bytes)) {
+		hdd_err("Failed to put mac_addr");
+		return QDF_STATUS_E_INVAL;
+	}
+
+	if (t2lm->default_link_mapping) {
+		hdd_debug("update mld addr for default mapping");
+		return QDF_STATUS_SUCCESS;
+	}
+
+	attr = QCA_WLAN_VENDOR_ATTR_TID_TO_LINK_MAP_STATUS;
+	config_attr = nla_nest_start(skb, attr);
+	if (!config_attr) {
+		hdd_err("nla_nest_start error");
+		return QDF_STATUS_E_INVAL;
+	}
+
+	switch (t2lm->direction) {
+	case WLAN_T2LM_UL_DIRECTION:
+		for (tid_num = 0; tid_num < T2LM_MAX_NUM_TIDS; tid_num++) {
+			config_params = nla_nest_start(skb, tid_num + 1);
+			if (!config_params)
+				return -EINVAL;
+
+			attr1 = QCA_WLAN_VENDOR_ATTR_LINK_TID_MAP_STATUS_UPLINK;
+			value = t2lm->ieee_link_map_tid[i];
+			errno = nla_put_u16(skb, attr1, value);
+			if (errno)
+				return errno;
+
+			attr1 = QCA_WLAN_VENDOR_ATTR_LINK_TID_MAP_STATUS_DOWNLINK;
+			value = 0;
+			errno = nla_put_u16(skb, attr1, value);
+			if (errno)
+				return errno;
+			nla_nest_end(skb, config_params);
+		}
+		break;
+	case WLAN_T2LM_DL_DIRECTION:
+		for (tid_num = 0; tid_num < T2LM_MAX_NUM_TIDS; tid_num++) {
+			config_params = nla_nest_start(skb, tid_num + 1);
+			if (!config_params)
+				return -EINVAL;
+			attr1 = QCA_WLAN_VENDOR_ATTR_LINK_TID_MAP_STATUS_DOWNLINK;
+			value = t2lm->ieee_link_map_tid[i];
+			errno = nla_put_u16(skb, attr1, value);
+			if (errno)
+				return errno;
+
+			attr1 = QCA_WLAN_VENDOR_ATTR_LINK_TID_MAP_STATUS_UPLINK;
+			value = 0;
+			errno = nla_put_u16(skb, attr1, value);
+			if (errno)
+				return errno;
+			nla_nest_end(skb, config_params);
+		}
+		break;
+	case WLAN_T2LM_BIDI_DIRECTION:
+		for (tid_num = 0; tid_num < T2LM_MAX_NUM_TIDS; tid_num++) {
+			config_params = nla_nest_start(skb, tid_num + 1);
+			if (!config_params)
+				return -EINVAL;
+
+			attr1 = QCA_WLAN_VENDOR_ATTR_LINK_TID_MAP_STATUS_UPLINK;
+			value = t2lm->ieee_link_map_tid[i];
+			errno = nla_put_u16(skb, attr1, value);
+			if (errno)
+				return errno;
+
+			attr1 = QCA_WLAN_VENDOR_ATTR_LINK_TID_MAP_STATUS_DOWNLINK;
+			value = t2lm->ieee_link_map_tid[i];
+			errno = nla_put_u16(skb, attr1, value);
+			if (errno)
+				return errno;
+			nla_nest_end(skb, config_params);
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	nla_nest_end(skb, config_attr);
+	return QDF_STATUS_SUCCESS;
+}
+
+QDF_STATUS wlan_hdd_send_t2lm_event(struct wlan_objmgr_vdev *vdev,
+				    struct wlan_t2lm_info *t2lm)
+{
+	struct sk_buff *skb;
+	size_t data_len;
+	QDF_STATUS status;
+	struct qdf_mac_addr mld_addr;
+	struct hdd_adapter *adapter;
+	struct wlan_hdd_link_info *link_info;
+
+	enum qca_nl80211_vendor_subcmds_index index =
+		QCA_NL80211_VENDOR_SUBCMD_TID_TO_LINK_MAP_INDEX;
+
+	link_info = wlan_hdd_get_link_info_from_objmgr(vdev);
+	if (!link_info) {
+		hdd_err("Invalid VDEV");
+		return QDF_STATUS_E_FAILURE;
+	}
+
+	adapter = link_info->adapter;
+	data_len = hdd_get_t2lm_setup_event_len();
+	skb = wlan_cfg80211_vendor_event_alloc(adapter->hdd_ctx->wiphy,
+					       NULL,
+					       data_len,
+					       index, GFP_KERNEL);
+	if (!skb) {
+		hdd_err("wlan_cfg80211_vendor_event_alloc failed");
+		return -EINVAL;
+	}
+
+	/* get mld addr */
+	status = wlan_vdev_get_bss_peer_mld_mac(vdev, &mld_addr);
+	if (QDF_IS_STATUS_ERROR(status)) {
+		hdd_err("Failed to get mld address");
+		goto free_skb;
+	}
+
+	status = hdd_t2lm_pack_nl_response(skb, vdev, t2lm, mld_addr);
+	if (QDF_IS_STATUS_ERROR(status)) {
+		hdd_err("Failed to pack nl response");
+		goto free_skb;
+	}
+
+	wlan_cfg80211_vendor_event(skb, GFP_KERNEL);
+
+	return status;
+free_skb:
+	wlan_cfg80211_vendor_free_skb(skb);
+
+	return status;
+}
 #endif