From c125d2615df25bd9a5a4801abfbcc91f21482e02 Mon Sep 17 00:00:00 2001
From: mtk20656 <chank.chen@mediatek.com>
Date: Tue, 6 Feb 2024 15:46:05 +0800
Subject: [PATCH 098/104] mtk: hostapd: add connac3 csi control interface

1. add hostapd_cli interface
2. add csi set/dump flow
3. add csi raw data to json

CR-Id: WCNCR00364748
Change-Id: If16d4b39ebb7942e47ed4bf4459589d6191983ac
Signed-off-by: mtk20656 <chank.chen@mediatek.com>
---
 hostapd/ctrl_iface.c              | 193 ++++++++++++++++++++++++
 hostapd/hostapd_cli.c             |  16 ++
 src/ap/ap_drv_ops.c               |  13 ++
 src/ap/ap_drv_ops.h               |   2 +
 src/common/mtk_vendor.h           |  31 +++-
 src/drivers/driver.h              |  16 ++
 src/drivers/driver_nl80211.c      | 235 ++++++++++++++++++++++++++++++
 src/drivers/driver_nl80211.h      |   1 +
 src/drivers/driver_nl80211_capa.c |   3 +
 9 files changed, 503 insertions(+), 7 deletions(-)

diff --git a/hostapd/ctrl_iface.c b/hostapd/ctrl_iface.c
index f0c990314..b5f6431bf 100644
--- a/hostapd/ctrl_iface.c
+++ b/hostapd/ctrl_iface.c
@@ -4925,6 +4925,193 @@ hostapd_ctrl_iface_disable_beacon(struct hostapd_data *hapd, char *value,
 
 }
 
+static int
+hostapd_ctrl_iface_set_csi(struct hostapd_data *hapd, char *cmd,
+					char *buf, size_t buflen)
+{
+	char *tmp;
+	u8 sta_mac[ETH_ALEN] = {0};
+	u32 csi_para[4] = {0};
+	char mac_str[18] = {0};
+	u8 csi_para_cnt = 0;
+
+	tmp = strtok_r(cmd, ",", &cmd);
+
+	while (tmp) {
+		csi_para_cnt++;
+
+		if (csi_para_cnt <= 4)
+			csi_para[csi_para_cnt - 1] = strtol(tmp, &tmp, 10);
+		else if (csi_para_cnt == 5) {
+			memcpy(mac_str, tmp, sizeof(mac_str) - 1);
+			break;
+		}
+
+		tmp = strtok_r(NULL, ",", &cmd);
+	}
+
+	if (strlen(mac_str)) {	/* user input mac string */
+		if (hwaddr_aton(mac_str, sta_mac) < 0) {
+			wpa_printf(MSG_ERROR, "station mac is not right.\n");
+			return -1;
+		}
+
+		if (hostapd_drv_csi_set(hapd, csi_para[0], csi_para[1], csi_para[2], csi_para[3], sta_mac)) {
+			wpa_printf(MSG_ERROR, "Not able to set csi, %d,%d,%d,%d,%s\n",
+					csi_para[0], csi_para[1], csi_para[2], csi_para[3], mac_str);
+			return -1;
+		}
+	} else {
+		if (hostapd_drv_csi_set(hapd, csi_para[0], csi_para[1], csi_para[2], csi_para[3], NULL)) {
+			wpa_printf(MSG_ERROR, "Not able to set csi, %d,%d,%d,%d\n",
+					csi_para[0], csi_para[1], csi_para[2], csi_para[3]);
+			return -1;
+		}
+	}
+
+	return os_snprintf(buf, buflen, "OK\n");
+}
+
+static int mt76_csi_to_json(char *fname, struct csi_resp_data *resp_buf)
+{
+#define MAX_BUF_SIZE	10000
+	FILE *f;
+	int i;
+
+	if (!fname) {
+		wpa_printf(MSG_ERROR, "csi dump file name is null!\n");
+		return -1;
+	}
+
+	f = fopen(fname, "a+");
+	if (!f) {
+		wpa_printf(MSG_ERROR, "open csi dump file %s failed\n", fname);
+		return -1;
+	}
+
+	if (fwrite("[", 1, 1, f) != 1) {
+		fclose(f);
+		return -1;
+	}
+
+	for (i = 0; i < resp_buf->buf_cnt; i++) {
+		struct csi_data *c = &resp_buf->csi_buf[i];
+		char *pos, *buf;
+		int j;
+
+		buf = malloc(MAX_BUF_SIZE);
+		if (!buf) {
+			fclose(f);
+			return -1;
+		}
+
+		pos = buf;
+		pos += snprintf(pos, MAX_BUF_SIZE, "%c", '[');
+
+		pos += snprintf(pos, MAX_BUF_SIZE, "%d,", c->ts);
+		pos += snprintf(pos, MAX_BUF_SIZE, "\"%02x%02x%02x%02x%02x%02x\",", c->ta[0], c->ta[1], c->ta[2], c->ta[3], c->ta[4], c->ta[5]);
+
+		pos += snprintf(pos, MAX_BUF_SIZE, "%d,", c->rssi);
+		pos += snprintf(pos, MAX_BUF_SIZE, "%u,", c->snr);
+		pos += snprintf(pos, MAX_BUF_SIZE, "%u,", c->data_bw);
+		pos += snprintf(pos, MAX_BUF_SIZE, "%u,", c->pri_ch_idx);
+		pos += snprintf(pos, MAX_BUF_SIZE, "%u,", c->rx_mode);
+		pos += snprintf(pos, MAX_BUF_SIZE, "%d,", c->tx_idx);
+		pos += snprintf(pos, MAX_BUF_SIZE, "%d,", c->rx_idx);
+		pos += snprintf(pos, MAX_BUF_SIZE, "%d,", c->chain_info);
+		pos += snprintf(pos, MAX_BUF_SIZE, "%d,", c->ext_info);
+
+		pos += snprintf(pos, MAX_BUF_SIZE, "%c", '[');
+		for (j = 0; j < c->data_num; j++) {
+			pos += snprintf(pos, MAX_BUF_SIZE, "%d", c->data_i[j]);
+			if (j != (c->data_num - 1))
+				pos += snprintf(pos, MAX_BUF_SIZE, ",");
+		}
+		pos += snprintf(pos, MAX_BUF_SIZE, "%c,", ']');
+
+		pos += snprintf(pos, MAX_BUF_SIZE, "%c", '[');
+		for (j = 0; j < c->data_num; j++) {
+			pos += snprintf(pos, MAX_BUF_SIZE, "%d", c->data_q[j]);
+			if (j != (c->data_num - 1))
+				pos += snprintf(pos, MAX_BUF_SIZE, ",");
+		}
+		pos += snprintf(pos, MAX_BUF_SIZE, "%c", ']');
+
+		pos += snprintf(pos, MAX_BUF_SIZE, "%c", ']');
+		if (i != resp_buf->buf_cnt - 1)
+			pos += snprintf(pos, MAX_BUF_SIZE, ",");
+
+		if (fwrite(buf, 1, pos - buf, f) != (pos - buf)) {
+			perror("fwrite");
+			free(buf);
+			fclose(f);
+			return -1;
+		}
+
+		free(buf);
+	}
+
+	if (fwrite("]", 1, 1, f) != 1) {
+		fclose(f);
+		return -1;
+	}
+
+	fclose(f);
+
+	return 0;
+}
+
+static int
+hostapd_ctrl_iface_dump_csi(struct hostapd_data *hapd, char *cmd,
+				char *buf, size_t buflen)
+{
+	char *tmp, *fname;
+	int data_cnt = 0, ret = 0;
+	struct csi_resp_data resp_buf;
+
+	tmp = strtok_r(cmd, ",", &cmd);
+
+	if (!tmp) {
+		wpa_printf(MSG_ERROR, "Error in command format\n");
+		return -1;
+	}
+
+	data_cnt = strtoul(tmp, &tmp, 0);
+
+	if (data_cnt > 3000) {
+		wpa_printf(MSG_ERROR, "Wrong input csi data cnt\n");
+		return -1;
+	}
+
+	fname = strtok_r(NULL, ",", &cmd);
+
+	if (!fname) {
+		wpa_printf(MSG_ERROR, "Error in command format, csi_filename.\n");
+		return -1;
+	}
+
+	resp_buf.csi_buf = (struct csi_data *)os_zalloc(sizeof(struct csi_data) * data_cnt);
+
+	if (resp_buf.csi_buf == NULL) {
+		wpa_printf(MSG_ERROR, "Error in memory allocation\n");
+		return -1;
+	}
+
+	resp_buf.usr_need_cnt = data_cnt;
+	resp_buf.buf_cnt = 0;
+
+	if (hostapd_drv_csi_dump(hapd, (void *)&resp_buf)) {
+		wpa_printf(MSG_ERROR, "Not able to set csi dump\n");
+		os_free(resp_buf.csi_buf);
+		return -1;
+	}
+
+	mt76_csi_to_json(fname, &resp_buf);
+
+	os_free(resp_buf.csi_buf);
+	return 0;
+}
+
 static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 					      char *buf, char *reply,
 					      int reply_size,
@@ -5578,6 +5765,12 @@ static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 	} else if (os_strncmp(buf, "NO_BEACON ", 10) == 0) {
 		reply_len = hostapd_ctrl_iface_disable_beacon(hapd, buf + 10, reply,
 							      reply_size);
+	} else if (os_strncmp(buf, "SET_CSI ", 7) == 0) {
+		reply_len = hostapd_ctrl_iface_set_csi(hapd, buf + 8,
+							reply, reply_size);
+	} else if (os_strncmp(buf, "DUMP_CSI ", 8) == 0) {
+		reply_len = hostapd_ctrl_iface_dump_csi(hapd, buf + 9,
+							reply, reply_size);
 	} else {
 		os_memcpy(reply, "UNKNOWN COMMAND\n", 16);
 		reply_len = 16;
diff --git a/hostapd/hostapd_cli.c b/hostapd/hostapd_cli.c
index acfa3b1d1..81b74d6de 100644
--- a/hostapd/hostapd_cli.c
+++ b/hostapd/hostapd_cli.c
@@ -1719,6 +1719,18 @@ static int hostapd_cli_cmd_dump_amnt(struct wpa_ctrl *ctrl, int argc,
 	return hostapd_cli_cmd(ctrl, "DUMP_AMNT", 1, argc, argv);
 }
 
+static int hostapd_cli_cmd_set_csi(struct wpa_ctrl *ctrl, int argc,
+					   char *argv[])
+{
+	return hostapd_cli_cmd(ctrl, "SET_CSI", 1, argc, argv);
+}
+
+static int hostapd_cli_cmd_dump_csi(struct wpa_ctrl *ctrl, int argc,
+					   char *argv[])
+{
+	return hostapd_cli_cmd(ctrl, "DUMP_CSI", 1, argc, argv);
+}
+
 struct hostapd_cli_cmd {
 	const char *cmd;
 	int (*handler)(struct wpa_ctrl *ctrl, int argc, char *argv[]);
@@ -1960,6 +1972,10 @@ static const struct hostapd_cli_cmd hostapd_cli_commands[] = {
 		" = Set Station index and mac to monitor"},
 	{ "dump_amnt", hostapd_cli_cmd_dump_amnt, NULL,
 		" = Dump RSSI of monitoring Station"},
+	{ "set_csi", hostapd_cli_cmd_set_csi, NULL,
+		" = Set csi configuaration"},
+	{ "dump_csi", hostapd_cli_cmd_dump_csi, NULL,
+		" = Dump csi data to a json file"},
 	{ NULL, NULL, NULL, NULL }
 };
 
diff --git a/src/ap/ap_drv_ops.c b/src/ap/ap_drv_ops.c
index 3c1609656..cb782fa31 100644
--- a/src/ap/ap_drv_ops.c
+++ b/src/ap/ap_drv_ops.c
@@ -1409,3 +1409,16 @@ int hostapd_drv_beacon_ctrl(struct hostapd_data *hapd, u8 beacon_mode)
 	return hapd->driver->beacon_ctrl(hapd->drv_priv, beacon_mode);
 }
 
+int hostapd_drv_csi_set(struct hostapd_data *hapd, u8 mode, u8 cfg, u8 v1, u32 v2, u8 *mac)
+{
+	if (!hapd->driver || !hapd->driver->csi_set)
+		return 0;
+	return hapd->driver->csi_set(hapd->drv_priv, mode, cfg, v1, v2, mac);
+}
+
+int hostapd_drv_csi_dump(struct hostapd_data *hapd, void *dump_buf)
+{
+	if (!hapd->driver || !hapd->driver->csi_dump)
+		return 0;
+	return hapd->driver->csi_dump(hapd->drv_priv, dump_buf);
+}
diff --git a/src/ap/ap_drv_ops.h b/src/ap/ap_drv_ops.h
index 5830705a3..7abc58377 100644
--- a/src/ap/ap_drv_ops.h
+++ b/src/ap/ap_drv_ops.h
@@ -171,6 +171,8 @@ int hostapd_drv_amnt_dump(struct hostapd_data *hapd, u8 amnt_idx, u8 *amnt_dump_
 int hostapd_drv_background_radar_mode(struct hostapd_data *hapd);
 int hostapd_drv_pp_mode_set(struct hostapd_data *hapd);
 int hostapd_drv_beacon_ctrl(struct hostapd_data *hapd, u8 beacon_mode);
+int hostapd_drv_csi_set(struct hostapd_data *hapd, u8 mode, u8 cfg, u8 v1, u32 v2, u8 *mac);
+int hostapd_drv_csi_dump(struct hostapd_data *hapd, void *dump_buf);
 
 #include "drivers/driver.h"
 
diff --git a/src/common/mtk_vendor.h b/src/common/mtk_vendor.h
index 1e4c3670e..c290e72a7 100644
--- a/src/common/mtk_vendor.h
+++ b/src/common/mtk_vendor.h
@@ -73,7 +73,6 @@ enum mtk_vendor_attr_csi_ctrl {
 	MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL1,
 	MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL2,
 	MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR,
-	MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL,
 
 	MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM,
 
@@ -96,6 +95,7 @@ enum mtk_vendor_attr_csi_data {
 	MTK_VENDOR_ATTR_CSI_DATA_BW,
 	MTK_VENDOR_ATTR_CSI_DATA_CH_IDX,
 	MTK_VENDOR_ATTR_CSI_DATA_TA,
+	MTK_VENDOR_ATTR_CSI_DATA_NUM,
 	MTK_VENDOR_ATTR_CSI_DATA_I,
 	MTK_VENDOR_ATTR_CSI_DATA_Q,
 	MTK_VENDOR_ATTR_CSI_DATA_INFO,
@@ -106,7 +106,7 @@ enum mtk_vendor_attr_csi_data {
 	MTK_VENDOR_ATTR_CSI_DATA_TX_ANT,
 	MTK_VENDOR_ATTR_CSI_DATA_RX_ANT,
 	MTK_VENDOR_ATTR_CSI_DATA_MODE,
-	MTK_VENDOR_ATTR_CSI_DATA_H_IDX,
+	MTK_VENDOR_ATTR_CSI_DATA_CHAIN_INFO,
 
 	/* keep last */
 	NUM_MTK_VENDOR_ATTRS_CSI_DATA,
@@ -281,23 +281,40 @@ enum mtk_vendor_attr_beacon_ctrl {
 		NUM_MTK_VENDOR_ATTRS_BEACON_CTRL - 1
 };
 
-#define CSI_MAX_COUNT 256
+#define CSI_BW20_DATA_COUNT	64
+#define CSI_BW40_DATA_COUNT	128
+#define CSI_BW80_DATA_COUNT	256
+#define CSI_BW160_DATA_COUNT	512
+#define CSI_BW320_DATA_COUNT	1024
 #define ETH_ALEN 6
 
 struct csi_data {
-	s16 data_i[CSI_MAX_COUNT];
-	s16 data_q[CSI_MAX_COUNT];
+	u8 ch_bw;
+	u16 data_num;
+	s16 data_i[CSI_BW320_DATA_COUNT];
+	s16 data_q[CSI_BW320_DATA_COUNT];
+	u8 band;
 	s8 rssi;
 	u8 snr;
 	u32 ts;
 	u8 data_bw;
 	u8 pri_ch_idx;
 	u8 ta[ETH_ALEN];
-	u32 info;
+	u32 ext_info;
 	u8 rx_mode;
-	u32 h_idx;
+	u32 chain_info;
 	u16 tx_idx;
 	u16 rx_idx;
+	u32 segment_num;
+	u8 remain_last;
+	u16 pkt_sn;
+	u8 tr_stream;
+};
+
+struct csi_resp_data {
+	u16 usr_need_cnt;
+	u16 buf_cnt;
+	struct csi_data *csi_buf;
 };
 
 #define AIR_MONITOR_MAX_ENTRY 16
diff --git a/src/drivers/driver.h b/src/drivers/driver.h
index ba61f5842..7efb5e342 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -5342,6 +5342,22 @@ struct wpa_driver_ops {
 #ifdef CONFIG_IEEE80211BE
 	int (*get_mld_addr)(void *priv, u8 *addr);
 #endif
+	/**
+	 * csi_set - Set csi related mode and parameter
+	 * @priv: Private driver interface data
+	 * @mode: Csi mode parameter
+	 * @cfg: Csi config parameter
+	 * @v1: Value1
+	 * @v2: Value2
+	 * @mac: Station mac for station filter
+	 */
+	int (*csi_set)(void *priv, u8 mode, u8 cfg, u8 v1, u32 v2, u8 *mac);
+	/**
+	* csi_dump - Dump csi data to json file
+	* @priv: Private driver interface data
+	* @dump_buf: Dump_struct that store csi data and related info
+	*/
+	int (*csi_dump)(void *priv, void *dump_buf);
 };
 
 /**
diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index df4a7ec41..39b45ef4b 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -161,6 +161,35 @@ pp_ctrl_policy[NUM_MTK_VENDOR_ATTRS_PP_CTRL] = {
 	[MTK_VENDOR_ATTR_PP_MODE] = { .type = NLA_U8 },
 };
 
+static struct nla_policy csi_ctrl_policy[NUM_MTK_VENDOR_ATTRS_CSI_CTRL] = {
+	[MTK_VENDOR_ATTR_CSI_CTRL_CFG] = { .type = NLA_NESTED },
+	[MTK_VENDOR_ATTR_CSI_CTRL_CFG_MODE] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_CSI_CTRL_CFG_TYPE] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL1] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL2] = { .type = NLA_U32 },
+	[MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR] = { .type = NLA_NESTED },
+	[MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM] = { .type = NLA_U16 },
+	[MTK_VENDOR_ATTR_CSI_CTRL_DATA] = { .type = NLA_NESTED },
+};
+
+static struct nla_policy csi_data_policy[NUM_MTK_VENDOR_ATTRS_CSI_DATA] = {
+	[MTK_VENDOR_ATTR_CSI_DATA_VER] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_CSI_DATA_TS] = { .type = NLA_U32 },
+	[MTK_VENDOR_ATTR_CSI_DATA_RSSI] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_CSI_DATA_SNR] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_CSI_DATA_BW] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_CSI_DATA_CH_IDX] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_CSI_DATA_TA] = { .type = NLA_NESTED },
+	[MTK_VENDOR_ATTR_CSI_DATA_NUM] = { .type = NLA_U32 },
+	[MTK_VENDOR_ATTR_CSI_DATA_I] = { .type = NLA_NESTED },
+	[MTK_VENDOR_ATTR_CSI_DATA_Q] = { .type = NLA_NESTED },
+	[MTK_VENDOR_ATTR_CSI_DATA_INFO] = { .type = NLA_U32 },
+	[MTK_VENDOR_ATTR_CSI_DATA_TX_ANT] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_CSI_DATA_RX_ANT] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_CSI_DATA_MODE] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_CSI_DATA_CHAIN_INFO] = { .type = NLA_U32 },
+};
+
 static struct nl_sock * nl_create_handle(struct nl_cb *cb, const char *dbg)
 {
 	struct nl_sock *handle;
@@ -15192,6 +15221,210 @@ static int nl80211_get_mld_addr(void *priv, u8 *addr)
 }
 #endif
 
+static int
+nl80211_csi_set(void *priv, u8 mode, u8 cfg, u8 v1, u32 v2, u8 *mac)
+{
+	struct i802_bss *bss = priv;
+	struct wpa_driver_nl80211_data *drv = bss->drv;
+	struct nl_msg *msg;
+	struct nlattr *data;
+	void *tb1, *tb2;
+	int ret, i;
+
+	if (!drv->mtk_csi_vendor_cmd_avail) {
+		wpa_printf(MSG_ERROR,
+			"nl80211: Driver does not support csi");
+		return 0;
+	}
+
+	msg = nl80211_drv_msg(drv, 0, NL80211_CMD_VENDOR);
+	if (!msg)
+		goto fail;
+
+	if (nla_put_u32(msg, NL80211_ATTR_VENDOR_ID, OUI_MTK) ||
+			nla_put_u32(msg, NL80211_ATTR_VENDOR_SUBCMD,
+			MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL))
+		goto fail;
+
+	data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA | NLA_F_NESTED);
+	if (!data)
+		goto fail;
+
+	tb1 = nla_nest_start(msg, MTK_VENDOR_ATTR_CSI_CTRL_CFG | NLA_F_NESTED);
+	if (!tb1)
+		goto fail;
+
+	nla_put_u8(msg, MTK_VENDOR_ATTR_CSI_CTRL_CFG_MODE, mode);
+	nla_put_u8(msg, MTK_VENDOR_ATTR_CSI_CTRL_CFG_TYPE, cfg);
+	nla_put_u8(msg, MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL1, v1);
+	nla_put_u32(msg, MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL2, v2);
+
+	nla_nest_end(msg, tb1);
+
+	if (mac) {
+		tb2 = nla_nest_start(msg, MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR | NLA_F_NESTED);
+		if (!tb2)
+			goto fail;
+
+		for (i = 0; i < ETH_ALEN; i++)
+			nla_put_u8(msg, i, mac[i]);
+
+		nla_nest_end(msg, tb2);
+	}
+
+	nla_nest_end(msg, data);
+
+	ret = send_and_recv_cmd(drv, msg);
+
+	if (ret)
+		wpa_printf(MSG_ERROR, "Failed to set csi. ret=%d (%s)",
+			ret, strerror(-ret));
+
+	return ret;
+
+fail:
+	nlmsg_free(msg);
+	return -ENOBUFS;
+
+}
+
+static int
+mt76_csi_dump_cb(struct nl_msg *msg, void *arg)
+{
+	struct nlattr *tb[NL80211_ATTR_MAX + 1];
+	struct nlattr *tb1[NUM_MTK_VENDOR_ATTRS_CSI_CTRL];
+	struct nlattr *tb2[NUM_MTK_VENDOR_ATTRS_CSI_DATA];
+	struct nlattr *attr, *cur, *data;
+	int len = 0, rem, idx;
+	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
+	struct csi_resp_data *csi_resp = (struct csi_resp_data *)arg;
+	struct csi_data *c = csi_resp->csi_buf;
+
+	c += csi_resp->buf_cnt;
+
+	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
+		genlmsg_attrlen(gnlh, 0), NULL);
+
+	attr = tb[NL80211_ATTR_VENDOR_DATA];
+	if (!attr)
+		return NL_SKIP;
+
+	nla_parse_nested(tb1, MTK_VENDOR_ATTR_CSI_CTRL_MAX,
+			attr, csi_ctrl_policy);
+
+	if (!tb1[MTK_VENDOR_ATTR_CSI_CTRL_DATA])
+		return NL_SKIP;
+
+	nla_parse_nested(tb2, MTK_VENDOR_ATTR_CSI_DATA_MAX,
+			tb1[MTK_VENDOR_ATTR_CSI_CTRL_DATA], csi_data_policy);
+
+	if (!(tb2[MTK_VENDOR_ATTR_CSI_DATA_VER] &&
+	      tb2[MTK_VENDOR_ATTR_CSI_DATA_TS] &&
+	      tb2[MTK_VENDOR_ATTR_CSI_DATA_RSSI] &&
+	      tb2[MTK_VENDOR_ATTR_CSI_DATA_SNR] &&
+	      tb2[MTK_VENDOR_ATTR_CSI_DATA_BW] &&
+	      tb2[MTK_VENDOR_ATTR_CSI_DATA_CH_IDX] &&
+	      tb2[MTK_VENDOR_ATTR_CSI_DATA_TA] &&
+	      tb2[MTK_VENDOR_ATTR_CSI_DATA_I] &&
+	      tb2[MTK_VENDOR_ATTR_CSI_DATA_Q] &&
+	      tb2[MTK_VENDOR_ATTR_CSI_DATA_INFO] &&
+	      tb2[MTK_VENDOR_ATTR_CSI_DATA_MODE] &&
+	      tb2[MTK_VENDOR_ATTR_CSI_DATA_CHAIN_INFO])) {
+		fprintf(stderr, "Attributes error for CSI data\n");
+		return NL_SKIP;
+	}
+
+	c->rssi = nla_get_u8(tb2[MTK_VENDOR_ATTR_CSI_DATA_RSSI]);
+	c->snr = nla_get_u8(tb2[MTK_VENDOR_ATTR_CSI_DATA_SNR]);
+	c->data_bw = nla_get_u8(tb2[MTK_VENDOR_ATTR_CSI_DATA_BW]);
+	c->pri_ch_idx = nla_get_u8(tb2[MTK_VENDOR_ATTR_CSI_DATA_CH_IDX]);
+	c->rx_mode = nla_get_u8(tb2[MTK_VENDOR_ATTR_CSI_DATA_MODE]);
+
+	c->tx_idx = nla_get_u16(tb2[MTK_VENDOR_ATTR_CSI_DATA_TX_ANT]);
+	c->rx_idx = nla_get_u16(tb2[MTK_VENDOR_ATTR_CSI_DATA_RX_ANT]);
+
+	c->ext_info = nla_get_u32(tb2[MTK_VENDOR_ATTR_CSI_DATA_INFO]);
+	c->chain_info = nla_get_u32(tb2[MTK_VENDOR_ATTR_CSI_DATA_CHAIN_INFO]);
+
+	c->ts = nla_get_u32(tb2[MTK_VENDOR_ATTR_CSI_DATA_TS]);
+
+	c->data_num = nla_get_u32(tb2[MTK_VENDOR_ATTR_CSI_DATA_NUM]);
+
+	idx = 0;
+	nla_for_each_nested(cur, tb2[MTK_VENDOR_ATTR_CSI_DATA_TA], rem) {
+		if (idx < ETH_ALEN)
+			c->ta[idx++] = nla_get_u8(cur);
+	}
+
+	idx = 0;
+	nla_for_each_nested(cur, tb2[MTK_VENDOR_ATTR_CSI_DATA_I], rem) {
+		if (idx < c->data_num)
+			c->data_i[idx++] = nla_get_u16(cur);
+	}
+
+	idx = 0;
+	nla_for_each_nested(cur, tb2[MTK_VENDOR_ATTR_CSI_DATA_Q], rem) {
+		if (idx < c->data_num)
+			c->data_q[idx++] = nla_get_u16(cur);
+	}
+
+	csi_resp->buf_cnt++;
+
+	return NL_SKIP;
+}
+
+static int
+nl80211_csi_dump(void *priv, void *dump_buf)
+{
+	struct i802_bss *bss = priv;
+	struct wpa_driver_nl80211_data *drv = bss->drv;
+	struct nl_msg *msg;
+	struct nlattr *data;
+	int ret;
+	struct csi_resp_data *csi_resp;
+	u16 pkt_num, i;
+
+	if (!drv->mtk_csi_vendor_cmd_avail) {
+		wpa_printf(MSG_INFO,
+			"nl80211: Driver does not support csi");
+		return 0;
+	}
+
+	csi_resp = (struct csi_resp_data *)dump_buf;
+	pkt_num =  csi_resp->usr_need_cnt;
+
+	if (pkt_num > 3000)
+		return -EINVAL;
+
+#define CSI_DUMP_PER_NUM	3
+	for (i = 0; i < pkt_num / CSI_DUMP_PER_NUM; i++) {
+		msg = nl80211_drv_msg(drv, NLM_F_DUMP, NL80211_CMD_VENDOR);
+		if (!msg)
+			goto fail;
+
+		if (nla_put_u32(msg, NL80211_ATTR_VENDOR_ID, OUI_MTK) ||
+				nla_put_u32(msg, NL80211_ATTR_VENDOR_SUBCMD,
+				MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL))
+			goto fail;
+
+		data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA | NLA_F_NESTED);
+		if (!data)
+			goto fail;
+
+		nla_put_u16(msg, MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM, CSI_DUMP_PER_NUM);
+
+		nla_nest_end(msg, data);
+
+		ret = send_and_recv_resp(drv, msg, mt76_csi_dump_cb, dump_buf);
+	}
+
+	return ret;
+
+fail:
+	nlmsg_free(msg);
+	return -ENOBUFS;
+}
+
 const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.name = "nl80211",
 	.desc = "Linux nl80211/cfg80211",
@@ -15375,4 +15608,6 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 #ifdef CONFIG_IEEE80211BE
 	.get_mld_addr = nl80211_get_mld_addr,
 #endif
+	.csi_set = nl80211_csi_set,
+	.csi_dump = nl80211_csi_dump,
 };
diff --git a/src/drivers/driver_nl80211.h b/src/drivers/driver_nl80211.h
index a0a62e536..b710b50cd 100644
--- a/src/drivers/driver_nl80211.h
+++ b/src/drivers/driver_nl80211.h
@@ -212,6 +212,7 @@ struct wpa_driver_nl80211_data {
 	unsigned int mtk_background_radar_vendor_cmd_avail:1;
 	unsigned int mtk_pp_vendor_cmd_avail:1;
 	unsigned int mtk_beacon_ctrl_vendor_cmd_avail:1;
+	unsigned int mtk_csi_vendor_cmd_avail:1;
 
 	u32 ignore_next_local_disconnect;
 	u32 ignore_next_local_deauth;
diff --git a/src/drivers/driver_nl80211_capa.c b/src/drivers/driver_nl80211_capa.c
index f3e3d52e2..8688b849f 100644
--- a/src/drivers/driver_nl80211_capa.c
+++ b/src/drivers/driver_nl80211_capa.c
@@ -1173,6 +1173,9 @@ static int wiphy_info_handler(struct nl_msg *msg, void *arg)
 				case MTK_NL80211_VENDOR_SUBCMD_BEACON_CTRL :
 					drv->mtk_beacon_ctrl_vendor_cmd_avail = 1;
 					break;
+				case MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL:
+					drv->mtk_csi_vendor_cmd_avail = 1;
+					break;
 				}
 			}
 
-- 
2.39.2