recipes-wifi/linux-mt76/files/patches-3.x/0106-mtk-mt76-mt7996-Add-connac3-csi-feature.patch

From 3420fc6b8b3645808456e49f55180c23e3260c46 Mon Sep 17 00:00:00 2001
From: mtk20656 <chank.chen@mediatek.com>
Date: Sat, 20 Jan 2024 12:03:24 +0800
Subject: [PATCH 106/193] mtk: mt76: mt7996: Add connac3 csi feature.

1. format align to wifi6.
2. add bw320 support.
3. add active mode.

Fix csi bug with single wiphy design.

Change-Id: If37ac6de4781c3673671707ee3ee243dda8163f8
Change-Id: I7761e2883bb08939917432df075062fb78de3bee
Signed-off-by: mtk20656 <chank.chen@mediatek.com>
---
 mt76_connac_mcu.h |   2 +
 mt7996/init.c     |  22 +++
 mt7996/main.c     |   3 +
 mt7996/mcu.c      | 465 ++++++++++++++++++++++++++++++++++++++++++++++
 mt7996/mcu.h      | 105 +++++++++++
 mt7996/mt7996.h   |  55 ++++++
 mt7996/vendor.c   | 247 ++++++++++++++++++++++++
 mt7996/vendor.h   |  52 ++++++
 8 files changed, 951 insertions(+)

diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index f6b472e..11c147c 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -1066,6 +1066,7 @@ enum {
 	MCU_UNI_EVENT_THERMAL = 0x35,
 	MCU_UNI_EVENT_NIC_CAPAB = 0x43,
 	MCU_UNI_EVENT_TESTMODE_CTRL = 0x46,
+	MCU_UNI_EVENT_CSI_REPORT = 0x4A,
 	MCU_UNI_EVENT_WED_RRO = 0x57,
 	MCU_UNI_EVENT_PER_STA_INFO = 0x6d,
 	MCU_UNI_EVENT_ALL_STA_INFO = 0x6e,
@@ -1303,6 +1304,7 @@ enum {
 	MCU_UNI_CMD_TESTMODE_TRX_PARAM = 0x42,
 	MCU_UNI_CMD_TESTMODE_CTRL = 0x46,
 	MCU_UNI_CMD_PRECAL_RESULT = 0x47,
+	MCU_UNI_CMD_CSI_CTRL = 0x4A,
 	MCU_UNI_CMD_THERMAL_CAL = 0x4c,
 	MCU_UNI_CMD_RRO = 0x57,
 	MCU_UNI_CMD_OFFCH_SCAN_CTRL = 0x58,
diff --git a/mt7996/init.c b/mt7996/init.c
index 25ffd69..c501d19 100644
--- a/mt7996/init.c
+++ b/mt7996/init.c
@@ -804,6 +804,24 @@ error:
 	return ret;
 }
 
+#ifdef CONFIG_MTK_VENDOR
+static int mt7996_unregister_csi(struct mt7996_phy *phy)
+{
+	struct csi_data *c, *tmp_c;
+
+	spin_lock_bh(&phy->csi.lock);
+	phy->csi.enable = 0;
+
+	list_for_each_entry_safe(c, tmp_c, &phy->csi.list, node) {
+		list_del(&c->node);
+		kfree(c);
+	}
+	spin_unlock_bh(&phy->csi.lock);
+
+	return 0;
+}
+#endif
+
 static void
 mt7996_unregister_phy(struct mt7996_phy *phy, enum mt76_band_id band)
 {
@@ -815,6 +833,10 @@ mt7996_unregister_phy(struct mt7996_phy *phy, enum mt76_band_id band)
 	/* TODO: FIXME: temp for single wiphy support */
 	phy->mt76->hw = phy->mt76->ori_hw;
 
+#ifdef CONFIG_MTK_VENDOR
+	mt7996_unregister_csi(phy);
+#endif
+
 	mt7996_unregister_thermal(phy);
 
 	mphy = phy->dev->mt76.phys[band];
diff --git a/mt7996/main.c b/mt7996/main.c
index 408f848..574b0b5 100644
--- a/mt7996/main.c
+++ b/mt7996/main.c
@@ -1305,6 +1305,9 @@ void mt7996_mac_sta_remove(struct mt76_dev *mdev, struct ieee80211_vif *vif,
 	struct mt7996_dev *dev = container_of(mdev, struct mt7996_dev, mt76);
 	unsigned long rem = sta->valid_links ?: BIT(0);
 
+#ifdef CONFIG_MTK_VENDOR
+	mt7996_mcu_set_csi(&dev->phy, 2, 8, 1, 0, sta->addr);
+#endif
 	mt7996_mac_sta_remove_links(dev, vif, sta, rem);
 }
 
diff --git a/mt7996/mcu.c b/mt7996/mcu.c
index efc0d54..56ad93a 100644
--- a/mt7996/mcu.c
+++ b/mt7996/mcu.c
@@ -658,6 +658,263 @@ mt7996_mcu_rx_all_sta_info_event(struct mt7996_dev *dev, struct sk_buff *skb)
 	}
 }
 
+static int
+csi_integrate_segment_data(struct mt7996_phy *phy, struct csi_data *csi)
+{
+	struct csi_data *csi_temp = NULL;
+
+	if (csi->segment_num == 0 && csi->remain_last == 0)
+		return CSI_CHAIN_COMPLETE;
+	else if (csi->segment_num == 0 && csi->remain_last == 1) {
+		memcpy(&phy->csi.buffered_csi,
+		       csi, sizeof(struct csi_data));
+
+		return CSI_CHAIN_SEGMENT_FIRST;
+	} else if (csi->segment_num != 0) {
+		csi_temp = &phy->csi.buffered_csi;
+		if (csi->chain_info != csi_temp->chain_info ||
+		csi->segment_num != (csi_temp->segment_num + 1))
+			return CSI_CHAIN_SEGMENT_ERR;
+
+		memcpy(&csi_temp->data_i[csi_temp->data_num],
+		       csi->data_i, csi->data_num * sizeof(s16));
+
+		memcpy(&csi_temp->data_q[csi_temp->data_num],
+		       csi->data_q, csi->data_num * sizeof(s16));
+
+		csi_temp->data_num += csi->data_num;
+		csi_temp->segment_num = csi->segment_num;
+		csi_temp->remain_last = csi->remain_last;
+
+		if (csi->remain_last == 0)
+			return CSI_CHAIN_SEGMENT_LAST;
+		else if (csi->remain_last == 1)
+			return CSI_CHAIN_SEGMENT_MIDDLE;
+	}
+
+	return CSI_CHAIN_ERR;
+}
+
+static int
+mt7996_mcu_csi_report_data(struct mt7996_phy *phy, u8 *tlv_buf, u32 len)
+{
+	int ret, i;
+	struct csi_data *current_csi;
+	struct csi_data *target_csi;
+	struct csi_tlv *tlv_data;
+	u8 *buf_tmp;
+	u32 rx_info, tx_rx_idx;
+	u32 buf_len_last, offset;
+
+	buf_tmp = tlv_buf;
+	buf_len_last = len;
+	offset = sizeof(((struct csi_tlv *)0)->basic);
+
+	current_csi = kzalloc(sizeof(*current_csi), GFP_KERNEL);
+	if (!current_csi)
+		return -ENOMEM;
+
+	while (buf_len_last >= offset) {
+		u32 tag, len;
+		s16 *data_tmp = NULL;
+
+		tlv_data = (struct csi_tlv *)buf_tmp;
+		tag = le32_to_cpu(tlv_data->basic.tag);
+		len = le32_to_cpu(tlv_data->basic.len);
+
+		switch (tag) {
+		case CSI_EVENT_FW_VER:
+			current_csi->fw_ver = le32_to_cpu(tlv_data->info);
+			break;
+		case CSI_EVENT_CBW:
+			current_csi->ch_bw = le32_to_cpu(tlv_data->info);
+			break;
+		case CSI_EVENT_RSSI:
+			current_csi->rssi = le32_to_cpu(tlv_data->info);
+			break;
+		case CSI_EVENT_SNR:
+			current_csi->snr = le32_to_cpu(tlv_data->info);
+			break;
+		case CSI_EVENT_BAND:
+			current_csi->band = le32_to_cpu(tlv_data->info);
+
+			if (current_csi->band != phy->mt76->band_idx) {
+				kfree(current_csi);
+				return -EINVAL;
+			}
+
+			break;
+		case CSI_EVENT_CSI_NUM:
+			current_csi->data_num = le32_to_cpu(tlv_data->info);
+
+			if (current_csi->data_num > CSI_BW80_DATA_COUNT) {
+				kfree(current_csi);
+				return -EINVAL;
+			}
+
+			break;
+		case CSI_EVENT_CSI_I_DATA:
+			if (len != sizeof(s16) * current_csi->data_num) {
+				kfree(current_csi);
+				return -EINVAL;
+			}
+
+			data_tmp = tlv_data->data;
+			for (i = 0; i < current_csi->data_num; i++)
+				current_csi->data_i[i] = le16_to_cpu(*(data_tmp + i));
+			break;
+		case CSI_EVENT_CSI_Q_DATA:
+			if (len != sizeof(s16) * current_csi->data_num) {
+				kfree(current_csi);
+				return -EINVAL;
+			}
+
+			data_tmp = tlv_data->data;
+			for (i = 0; i < current_csi->data_num; i++)
+				current_csi->data_q[i] = le16_to_cpu(*(data_tmp + i));
+			break;
+		case CSI_EVENT_DBW:
+			current_csi->data_bw = le32_to_cpu(tlv_data->info);
+			break;
+		case CSI_EVENT_CH_IDX:
+			current_csi->pri_ch_idx = le32_to_cpu(tlv_data->info);
+			break;
+		case CSI_EVENT_TA:
+			memcpy(current_csi->ta, tlv_data->mac, ETH_ALEN);
+			break;
+		case CSI_EVENT_EXTRA_INFO:
+			current_csi->ext_info = le32_to_cpu(tlv_data->info);
+			break;
+		case CSI_EVENT_RX_MODE:
+			rx_info = le32_to_cpu(tlv_data->info);
+			current_csi->rx_mode = u32_get_bits(rx_info, GENMASK(15, 0));
+			current_csi->rx_rate = u32_get_bits(rx_info, GENMASK(31, 16));
+			break;
+		case CSI_EVENT_H_IDX:
+			current_csi->chain_info = le32_to_cpu(tlv_data->info);
+			break;
+		case CSI_EVENT_TX_RX_IDX:
+			tx_rx_idx = le32_to_cpu(tlv_data->info);
+			current_csi->tx_idx = u32_get_bits(tx_rx_idx, GENMASK(31, 16));
+			current_csi->rx_idx = u32_get_bits(tx_rx_idx, GENMASK(15, 0));
+			break;
+		case CSI_EVENT_TS:
+			current_csi->ts = le32_to_cpu(tlv_data->info);
+
+			if (phy->csi.interval &&
+				current_csi->ts < phy->csi.last_record + phy->csi.interval) {
+				kfree(current_csi);
+				return 0;
+			}
+
+			break;
+		case CSI_EVENT_PKT_SN:
+			current_csi->pkt_sn = le32_to_cpu(tlv_data->info);
+			break;
+		case CSI_EVENT_BW_SEG:
+			current_csi->segment_num = le32_to_cpu(tlv_data->info);
+			break;
+		case CSI_EVENT_REMAIN_LAST:
+			current_csi->remain_last = le32_to_cpu(tlv_data->info);
+			break;
+		case CSI_EVENT_TR_STREAM:
+			current_csi->tr_stream = le32_to_cpu(tlv_data->info);
+			break;
+		default:
+			break;
+		};
+
+		buf_len_last -= (offset + len);
+
+		if (buf_len_last >= offset)
+			buf_tmp += (offset + len);
+	}
+
+	/* integret the bw80 segment */
+	if (current_csi->ch_bw >= CSI_BW80) {
+		ret = csi_integrate_segment_data(phy, current_csi);
+
+		switch (ret) {
+		case CSI_CHAIN_ERR:
+		case CSI_CHAIN_SEGMENT_ERR:
+			kfree(current_csi);
+			return -EINVAL;
+			break;
+		case CSI_CHAIN_SEGMENT_FIRST:
+		case CSI_CHAIN_SEGMENT_MIDDLE:
+			kfree(current_csi);
+			return 0;
+			break;
+		case CSI_CHAIN_COMPLETE:
+			target_csi = current_csi;
+			break;
+		case CSI_CHAIN_SEGMENT_LAST:
+			target_csi = current_csi;
+			memcpy(target_csi, &phy->csi.buffered_csi, sizeof(struct csi_data));
+			memset(&phy->csi.buffered_csi, 0, sizeof(struct csi_data));
+			break;
+		default:
+			break;
+		}
+	} else {
+		target_csi = current_csi;
+	}
+
+	/* put the csi data into list */
+	INIT_LIST_HEAD(&target_csi->node);
+	spin_lock_bh(&phy->csi.lock);
+
+	if (!phy->csi.enable) {
+		kfree(target_csi);
+		goto out;
+	}
+
+	list_add_tail(&target_csi->node, &phy->csi.list);
+	phy->csi.count++;
+
+	if (phy->csi.count > CSI_MAX_BUF_NUM) {
+		struct csi_data *old;
+
+		old = list_first_entry(&phy->csi.list,
+				       struct csi_data, node);
+
+		list_del(&old->node);
+		kfree(old);
+		phy->csi.count--;
+	}
+
+	if (target_csi->chain_info & BIT(15)) /* last chain */
+		phy->csi.last_record = target_csi->ts;
+
+out:
+	spin_unlock_bh(&phy->csi.lock);
+	return 0;
+}
+
+void
+mt7996_mcu_csi_report_event(struct mt7996_dev *dev, struct sk_buff *skb)
+{
+	struct mt7996_mcu_csi_event *event;
+	struct mt76_phy *mphy;
+	struct mt7996_phy *phy;
+
+	event = (struct mt7996_mcu_csi_event *)skb->data;
+
+	mphy = dev->mt76.phys[event->band_idx];
+	if (!mphy)
+		return;
+
+	phy = mphy->priv;
+
+	switch (le16_to_cpu(event->tag)) {
+	case UNI_EVENT_CSI_DATA:
+		mt7996_mcu_csi_report_data(phy, event->tlv_buf, le16_to_cpu(event->len) - 4);
+		break;
+	default:
+		break;
+	}
+}
+
 static void
 mt7996_mcu_rx_thermal_notify(struct mt7996_dev *dev, struct sk_buff *skb)
 {
@@ -901,6 +1158,11 @@ mt7996_mcu_uni_rx_unsolicited_event(struct mt7996_dev *dev, struct sk_buff *skb)
 	case MCU_UNI_EVENT_BF:
 		mt7996_mcu_rx_bf_event(dev, skb);
 		break;
+#endif
+#ifdef CONFIG_MTK_VENDOR
+	case MCU_UNI_EVENT_CSI_REPORT:
+		mt7996_mcu_csi_report_event(dev, skb);
+		break;
 #endif
 	default:
 		break;
@@ -6006,4 +6268,207 @@ void mt7996_set_beacon_vif(void *data, u8 *mac, struct ieee80211_vif *vif)
 
 	mt7996_mcu_add_beacon(hw, &vif->bss_conf, &mvif->deflink, val);
 }
+
+static int mt7996_mcu_set_csi_enable(struct mt7996_phy *phy, u16 tag)
+{
+	struct {
+		u8 band;
+		u8 rsv1[3];
+
+		__le16 tag;
+		__le16 len;
+	} __packed req = {
+		.band = phy->mt76->band_idx,
+		.tag = cpu_to_le16(tag),
+		.len = cpu_to_le16(sizeof(req) - 4),
+	};
+
+	return mt76_mcu_send_msg(&phy->dev->mt76, MCU_WM_UNI_CMD(CSI_CTRL), &req,
+				sizeof(req), false);
+}
+
+static int mt7996_mcu_set_csi_frame_type(struct mt7996_phy *phy, u16 tag, u8 type_idx, u32 type)
+{
+	struct {
+		u8 band;
+		u8 rsv1[3];
+
+		__le16 tag;
+		__le16 len;
+		u8 frame_type_idx;
+		u8 frame_type;
+		u8 rsv2[2];
+	} __packed req = {
+		.band = phy->mt76->band_idx,
+		.tag = cpu_to_le16(tag),
+		.len = cpu_to_le16(sizeof(req) - 4),
+		.frame_type_idx = type_idx,
+		.frame_type = type,
+	};
+
+	return mt76_mcu_send_msg(&phy->dev->mt76, MCU_WM_UNI_CMD(CSI_CTRL), &req,
+				sizeof(req), false);
+}
+
+static int mt7996_mcu_set_csi_chain_filter(struct mt7996_phy *phy, u16 tag, u8 func, u32 value)
+{
+	struct {
+		u8 band;
+		u8 rsv1[3];
+
+		__le16 tag;
+		__le16 len;
+		u8 function;
+		u8 chain_value;
+		u8 rsv2[2];
+	} __packed req = {
+		.band = phy->mt76->band_idx,
+		.tag = cpu_to_le16(tag),
+		.len = cpu_to_le16(sizeof(req) - 4),
+		.function = func,
+		.chain_value = value,
+	};
+
+	return mt76_mcu_send_msg(&phy->dev->mt76, MCU_WM_UNI_CMD(CSI_CTRL), &req,
+				sizeof(req), false);
+}
+
+static int mt7996_mcu_set_csi_sta_filter(struct mt7996_phy *phy, u16 tag, u32 op, u8 *sta_mac)
+{
+	struct {
+		u8 band;
+		u8 rsv1[3];
+
+		__le16 tag;
+		__le16 len;
+		u8 operation;
+		u8 rsv2[1];
+		u8 mac[6];
+	} __packed req = {
+		.band = phy->mt76->band_idx,
+		.tag = cpu_to_le16(tag),
+		.len = cpu_to_le16(sizeof(req) - 4),
+		.operation = op,
+	};
+
+	memcpy(req.mac, sta_mac, ETH_ALEN);
+
+	return mt76_mcu_send_msg(&phy->dev->mt76, MCU_WM_UNI_CMD(CSI_CTRL), &req,
+				sizeof(req), false);
+}
+
+static int mt7996_mcu_set_csi_active_mode(struct mt7996_phy *phy, u16 tag,
+					  u32 interval, u8 frame_idx, u8 subframe_idx, u32 bitmap)
+{
+	struct {
+		u8 band;
+		u8 rsv1[3];
+
+		__le16 tag;
+		__le16 len;
+		__le16 interval; /* uint: ms */
+		u8 frame_type_idx;
+		u8 subframe_type_idx;
+		__le32 bitmap; /* sta wcid bitmap */
+		u8 rsv2[4];
+	} __packed req = {
+		.band = phy->mt76->band_idx,
+		.tag = cpu_to_le16(tag),
+		.len = cpu_to_le16(sizeof(req) - 4),
+		.interval = cpu_to_le16(interval),
+		.frame_type_idx = frame_idx,
+		.subframe_type_idx = subframe_idx,
+		.bitmap = cpu_to_le32(bitmap),
+	};
+
+	return mt76_mcu_send_msg(&phy->dev->mt76, MCU_WM_UNI_CMD(CSI_CTRL), &req,
+				sizeof(req), false);
+}
+
+void mt7996_csi_wcid_bitmap_update(void *data, struct ieee80211_sta *sta)
+{
+	struct mt7996_sta *msta = (struct mt7996_sta *)sta->drv_priv;
+	struct mt7996_phy *phy = msta->vif->deflink.phy;
+	struct csi_bitmap_info_update *sta_info = (struct csi_bitmap_info_update *)data;
+	u16 wcid = 0;
+
+#define CSI_ACTIVE_MODE_ADD 1
+#define CSI_ACTIVE_MODE_REMOVE 0
+
+	if (!memcmp(sta_info->addr, sta->addr, ETH_ALEN)) {
+		wcid = msta->deflink.wcid.idx;
+
+		/* active mode: only support station with wcid less than 32 */
+		if (wcid > 32)
+			return;
+
+		if (sta_info->action == CSI_ACTIVE_MODE_ADD)
+			phy->csi.active_bitmap |= BIT(wcid);
+		else if (sta_info->action == CSI_ACTIVE_MODE_REMOVE)
+			phy->csi.active_bitmap &= ~(BIT(wcid));
+	}
+}
+
+int mt7996_mcu_set_csi(struct mt7996_phy *phy, u8 mode,
+			u8 cfg, u8 v1, u32 v2, u8 *mac_addr)
+{
+	switch (mode) {
+	case CSI_CONTROL_MODE_STOP:
+		return mt7996_mcu_set_csi_enable(phy, UNI_CMD_CSI_STOP);
+	case CSI_CONTROL_MODE_START:
+		return mt7996_mcu_set_csi_enable(phy, UNI_CMD_CSI_START);
+	case CSI_CONTROL_MODE_SET:
+		switch (cfg) {
+		case CSI_CONFIG_FRAME_TYPE:
+			if (v2 > 255)
+				return -EINVAL;
+
+			return mt7996_mcu_set_csi_frame_type(phy,
+					UNI_CMD_CSI_SET_FRAME_TYPE, v1, v2);
+		case CSI_CONFIG_CHAIN_FILTER:
+			if (v2 > 255)
+				return -EINVAL;
+
+			return mt7996_mcu_set_csi_chain_filter(phy,
+					UNI_CMD_CSI_SET_CHAIN_FILTER, v1, v2);
+		case CSI_CONFIG_STA_FILTER:
+			if (!is_valid_ether_addr(mac_addr))
+				return -EINVAL;
+
+			if (v2 > 255)
+				return -EINVAL;
+
+			return mt7996_mcu_set_csi_sta_filter(phy,
+					UNI_CMD_CSI_SET_STA_FILTER, v2, mac_addr);
+		case CSI_CONFIG_ACTIVE_MODE:
+			if (is_valid_ether_addr(mac_addr)) {
+				struct csi_bitmap_info_update sta_info;
+
+				if (v2 > 255)
+					return -EINVAL;
+
+				memcpy(sta_info.addr, mac_addr, ETH_ALEN);
+				sta_info.action = v2;
+
+				ieee80211_iterate_stations_atomic(phy->mt76->hw,
+								mt7996_csi_wcid_bitmap_update, &sta_info);
+				return 0;
+			} else {
+				u8 frame_type = v1 & 0x3;
+				u8 frame_subtype = (v1 & 0x3c) >> 2;
+
+					/* active mode: max interval is 3000ms */
+					if (v2 > 3000)
+						return -EINVAL;
+
+				return mt7996_mcu_set_csi_active_mode(phy, UNI_CMD_CSI_SET_ACTIVE_MODE,
+						v2, frame_type, frame_subtype, phy->csi.active_bitmap);
+			}
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+}
 #endif
diff --git a/mt7996/mcu.h b/mt7996/mcu.h
index f9f0468..42e9f52 100644
--- a/mt7996/mcu.h
+++ b/mt7996/mcu.h
@@ -1153,4 +1153,109 @@ struct fixed_rate_table_ctrl {
 	u8 _rsv2;
 } __packed;
 
+#ifdef CONFIG_MTK_VENDOR
+struct mt7996_mcu_csi_event {
+	struct mt7996_mcu_rxd rxd;
+
+	u8 band_idx;
+	u8 _rsv[3];
+
+	__le16 tag;
+	__le16 len;
+	u8 tlv_buf[0];
+};
+
+enum UNI_EVENT_CSI_TAG_T {
+	UNI_EVENT_CSI_DATA = 0,
+	UNI_EVENT_CSI_MAX_NUM
+};
+
+struct csi_tlv {
+	struct {
+		__le32 tag;
+		__le32 len;
+	} basic;
+	union {
+		u8 mac[ETH_ALEN];
+		__le32 info;
+		s16 data[0];
+	};
+} __packed;
+
+struct csi_bitmap_info_update {
+	u8 action;
+	u8 addr[ETH_ALEN];
+};
+
+#define CSI_MAX_BUF_NUM	3000
+
+enum CSI_EVENT_TLV_TAG {
+	CSI_EVENT_FW_VER,
+	CSI_EVENT_CBW,
+	CSI_EVENT_RSSI,
+	CSI_EVENT_SNR,
+	CSI_EVENT_BAND,
+	CSI_EVENT_CSI_NUM,
+	CSI_EVENT_CSI_I_DATA,
+	CSI_EVENT_CSI_Q_DATA,
+	CSI_EVENT_DBW,
+	CSI_EVENT_CH_IDX,
+	CSI_EVENT_TA,
+	CSI_EVENT_EXTRA_INFO,
+	CSI_EVENT_RX_MODE,
+	CSI_EVENT_RSVD1,
+	CSI_EVENT_RSVD2,
+	CSI_EVENT_RSVD3,
+	CSI_EVENT_RSVD4,
+	CSI_EVENT_H_IDX,
+	CSI_EVENT_TX_RX_IDX,
+	CSI_EVENT_TS,
+	CSI_EVENT_PKT_SN,
+	CSI_EVENT_BW_SEG,
+	CSI_EVENT_REMAIN_LAST,
+	CSI_EVENT_TR_STREAM,
+	CSI_EVENT_TLV_TAG_NUM,
+};
+
+enum CSI_CHAIN_TYPE {
+	CSI_CHAIN_ERR,
+	CSI_CHAIN_COMPLETE,
+	CSI_CHAIN_SEGMENT_FIRST,
+	CSI_CHAIN_SEGMENT_MIDDLE,
+	CSI_CHAIN_SEGMENT_LAST,
+	CSI_CHAIN_SEGMENT_ERR,
+};
+
+enum CSI_CONTROL_MODE_T {
+	CSI_CONTROL_MODE_STOP,
+	CSI_CONTROL_MODE_START,
+	CSI_CONTROL_MODE_SET,
+	CSI_CONTROL_MODE_NUM
+};
+
+enum CSI_CONFIG_ITEM_T {
+	CSI_CONFIG_RSVD1,
+	CSI_CONFIG_WF,
+	CSI_CONFIG_RSVD2,
+	CSI_CONFIG_FRAME_TYPE,
+	CSI_CONFIG_TX_PATH,
+	CSI_CONFIG_OUTPUT_FORMAT,
+	CSI_CONFIG_INFO,
+	CSI_CONFIG_CHAIN_FILTER,
+	CSI_CONFIG_STA_FILTER,
+	CSI_CONFIG_ACTIVE_MODE,
+	CSI_CONFIG_ITEM_NUM
+};
+
+/* CSI config Tag */
+enum UNI_CMD_CSI_TAG_T {
+	UNI_CMD_CSI_STOP = 0,
+	UNI_CMD_CSI_START = 1,
+	UNI_CMD_CSI_SET_FRAME_TYPE = 2,
+	UNI_CMD_CSI_SET_CHAIN_FILTER = 3,
+	UNI_CMD_CSI_SET_STA_FILTER = 4,
+	UNI_CMD_CSI_SET_ACTIVE_MODE = 5,
+};
+#endif
+
 #endif
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index 2a638a4..eb38427 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -446,6 +446,47 @@ struct mt7996_air_monitor_ctrl {
 	struct mt7996_air_monitor_group group[MT7996_AIR_MONITOR_MAX_GROUP];
 	struct mt7996_air_monitor_entry entry[MT7996_AIR_MONITOR_MAX_ENTRY];
 };
+
+enum {
+	CSI_BW20,
+	CSI_BW40,
+	CSI_BW80,
+	CSI_BW160,
+	CSI_BW320
+};
+
+#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
+
+struct csi_data {
+	u8 fw_ver;
+	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 ext_info;
+	u16 rx_mode;
+	u16 rx_rate;
+	u32 chain_info;
+	u16 tx_idx;
+	u16 rx_idx;
+	u32 segment_num;
+	u8 remain_last;
+	u16 pkt_sn;
+	u8 tr_stream;
+
+	struct list_head node;
+};
 #endif
 
 struct mt7996_rro_ba_session {
@@ -542,6 +583,18 @@ struct mt7996_phy {
 	u8 rts_bw_sig;
 	spinlock_t amnt_lock;
 	struct mt7996_air_monitor_ctrl amnt_ctrl;
+
+	struct {
+		struct list_head list;
+		spinlock_t lock;
+		u32 count;
+		bool enable;
+
+		struct csi_data buffered_csi;
+		u32 active_bitmap;
+		u32 interval;
+		u32 last_record;
+	} csi;
 #endif
 #ifdef CONFIG_MTK_DEBUG
 	bool sr_enable:1;
@@ -1180,6 +1233,8 @@ void mt7996_mcu_set_mimo(struct mt7996_phy *phy);
 int mt7996_set_muru_cfg(struct mt7996_phy *phy, u8 action, u8 val);
 int mt7996_mcu_set_muru_cfg(struct mt7996_phy *phy, void *data);
 void mt7996_set_beacon_vif(void *data, u8 *mac, struct ieee80211_vif *vif);
+int mt7996_mcu_set_csi(struct mt7996_phy *phy, u8 mode,
+		       u8 cfg, u8 v1, u32 v2, u8 *mac_addr);
 #endif
 
 int mt7996_mcu_edcca_enable(struct mt7996_phy *phy, bool enable);
diff --git a/mt7996/vendor.c b/mt7996/vendor.c
index 64ef551..84b50ab 100644
--- a/mt7996/vendor.c
+++ b/mt7996/vendor.c
@@ -119,6 +119,19 @@ beacon_ctrl_policy[NUM_MTK_VENDOR_ATTRS_BEACON_CTRL] = {
 	[MTK_VENDOR_ATTR_BEACON_CTRL_MODE] = { .type = NLA_U8 },
 };
 
+static const struct nla_policy
+csi_ctrl_policy[NUM_MTK_VENDOR_ATTRS_CSI_CTRL] = {
+	[MTK_VENDOR_ATTR_CSI_CTRL_BAND_IDX] = { .type = NLA_U8 },
+	[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 },
+};
+
 struct mt7996_amnt_data {
 	u8 idx;
 	u8 addr[ETH_ALEN];
@@ -1000,7 +1013,226 @@ static int mt7996_vendor_beacon_ctrl(struct wiphy *wiphy,
 
 	return 0;
 }
+static int mt7996_vendor_csi_ctrl(struct wiphy *wiphy,
+				  struct wireless_dev *wdev,
+				  const void *data,
+				  int data_len)
+{
+	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+	struct mt7996_dev *dev = mt7996_hw_dev(hw);
+	struct mt7996_phy *phy;
+	struct mt76_phy *mphy;
+	struct nlattr *tb[NUM_MTK_VENDOR_ATTRS_CSI_CTRL];
+	u8 band_idx = 0;
+	int err;
+
+	err = nla_parse(tb, MTK_VENDOR_ATTR_CSI_CTRL_MAX, data, data_len,
+			csi_ctrl_policy, NULL);
+	if (err)
+		return err;
+
+	if (tb[MTK_VENDOR_ATTR_CSI_CTRL_BAND_IDX])
+		band_idx = nla_get_u8(tb[MTK_VENDOR_ATTR_CSI_CTRL_BAND_IDX]);
+
+	if (!mt7996_band_valid(dev, band_idx))
+		goto error;
+
+	mphy = dev->mt76.phys[band_idx];
+	if (!mphy)
+		goto error;
+
+	phy = (struct mt7996_phy *)mphy->priv;
+	if (!phy)
+		goto error;
+
+	if (tb[MTK_VENDOR_ATTR_CSI_CTRL_CFG]) {
+		u8 mode = 0, type = 0, v1 = 0;
+		u32 v2 = 0;
+		u8 mac_addr[ETH_ALEN] = {};
+		struct nlattr *cur;
+		int rem;
+
+		nla_for_each_nested(cur, tb[MTK_VENDOR_ATTR_CSI_CTRL_CFG], rem) {
+			switch (nla_type(cur)) {
+			case MTK_VENDOR_ATTR_CSI_CTRL_CFG_MODE:
+				mode = nla_get_u8(cur);
+				break;
+			case MTK_VENDOR_ATTR_CSI_CTRL_CFG_TYPE:
+				type = nla_get_u8(cur);
+				break;
+			case MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL1:
+				v1 = nla_get_u8(cur);
+				break;
+			case MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL2:
+				v2 = nla_get_u32(cur);
+				break;
+			default:
+				return -EINVAL;
+			};
+		}
+
+		if (tb[MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR]) {
+			u8 idx = 0;
+
+			nla_for_each_nested(cur, tb[MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR], rem) {
+				mac_addr[idx++] = nla_get_u8(cur);
+			}
+		}
+
+		err = mt7996_mcu_set_csi(phy, mode, type, v1, v2, mac_addr);
+		if (err < 0)
+			return err;
+
+		spin_lock_bh(&phy->csi.lock);
+
+		phy->csi.enable = !!mode;
+
+		/* clean up old csi stats */
+		if ((mode == CSI_CONTROL_MODE_STOP || mode == CSI_CONTROL_MODE_SET)
+			&& !list_empty(&phy->csi.list)) {
+			struct csi_data *c, *tmp_c;
+
+			list_for_each_entry_safe(c, tmp_c, &phy->csi.list, node) {
+				list_del(&c->node);
+				kfree(c);
+				phy->csi.count--;
+			}
+		} else if (mode == CSI_CONTROL_MODE_START) {
+			phy->csi.last_record = 0;
+		}
+
+		spin_unlock_bh(&phy->csi.lock);
+
+		if (mode == CSI_CONTROL_MODE_SET && type == CSI_CONFIG_STA_FILTER && v1 == 2)
+			phy->csi.interval = v2;
+	}
+
+	return 0;
+
+error:
+	dev_err(dev->mt76.dev, "Invalid band idx: %d\n", band_idx);
+	return -EINVAL;
+}
+
+static int
+mt7996_vendor_csi_ctrl_dump(struct wiphy *wiphy, struct wireless_dev *wdev,
+			    struct sk_buff *skb, const void *data, int data_len,
+			    unsigned long *storage)
+{
+#define RESERVED_SET	BIT(31)
+	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+	struct mt7996_dev *dev = mt7996_hw_dev(hw);
+	struct mt7996_phy *phy;
+	struct mt76_phy *mphy;
+	struct nlattr *tb[NUM_MTK_VENDOR_ATTRS_CSI_CTRL] = {0};
+	u8 band_idx = 0;
+	int err = 0;
+
+	if (*storage & RESERVED_SET) {
+		if ((*storage & GENMASK(15, 0)) == 0)
+			return -ENOENT;
+		(*storage)--;
+	}
+
+	if (data) {
+		err = nla_parse(tb, MTK_VENDOR_ATTR_CSI_CTRL_MAX, data, data_len,
+				csi_ctrl_policy, NULL);
+		if (err)
+			return err;
+	}
+
+	if (tb[MTK_VENDOR_ATTR_CSI_CTRL_BAND_IDX])
+		band_idx = nla_get_u8(tb[MTK_VENDOR_ATTR_CSI_CTRL_BAND_IDX]);
+
+	if (!mt7996_band_valid(dev, band_idx))
+		return -EINVAL;
 
+	mphy = dev->mt76.phys[band_idx];
+	if (!mphy)
+		return -EINVAL;
+
+	phy = (struct mt7996_phy *)mphy->priv;
+	if (!phy)
+		return -EINVAL;
+
+	if (!(*storage & RESERVED_SET) && tb[MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM]) {
+		*storage = nla_get_u16(tb[MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM]);
+		*storage |= RESERVED_SET;
+	}
+
+	spin_lock_bh(&phy->csi.lock);
+
+	if (!list_empty(&phy->csi.list)) {
+		struct csi_data *csi;
+		void *a, *b;
+		int i;
+
+		csi = list_first_entry(&phy->csi.list, struct csi_data, node);
+
+		a = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_CTRL_DATA);
+		if (!a)
+			goto out;
+
+		if (nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_VER, 1) ||
+		    nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_RSSI, csi->rssi) ||
+		    nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_SNR, csi->snr) ||
+		    nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_BW, csi->data_bw) ||
+		    nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_CH_IDX, csi->pri_ch_idx) ||
+		    nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_MODE, csi->rx_mode))
+			goto out;
+
+		if (nla_put_u16(skb, MTK_VENDOR_ATTR_CSI_DATA_TX_ANT, csi->tx_idx) ||
+		    nla_put_u16(skb, MTK_VENDOR_ATTR_CSI_DATA_RX_ANT, csi->rx_idx))
+			goto out;
+
+		if (nla_put_u32(skb, MTK_VENDOR_ATTR_CSI_DATA_INFO, csi->ext_info) ||
+		    nla_put_u32(skb, MTK_VENDOR_ATTR_CSI_DATA_CHAIN_INFO, csi->chain_info) ||
+		    nla_put_u32(skb, MTK_VENDOR_ATTR_CSI_DATA_TS, csi->ts))
+			goto out;
+
+		b = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_DATA_TA);
+		if (!b)
+			goto out;
+
+		for (i = 0; i < ARRAY_SIZE(csi->ta); i++)
+			if (nla_put_u8(skb, i, csi->ta[i]))
+				goto out;
+		nla_nest_end(skb, b);
+
+		if (nla_put_u32(skb, MTK_VENDOR_ATTR_CSI_DATA_NUM, csi->data_num))
+			goto out;
+
+		b = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_DATA_I);
+		if (!b)
+			goto out;
+
+		for (i = 0; i < csi->data_num; i++)
+			if (nla_put_u16(skb, i, csi->data_i[i]))
+				goto out;
+		nla_nest_end(skb, b);
+
+		b = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_DATA_Q);
+		if (!b)
+			goto out;
+
+		for (i = 0; i < csi->data_num; i++)
+			if (nla_put_u16(skb, i, csi->data_q[i]))
+				goto out;
+		nla_nest_end(skb, b);
+
+		nla_nest_end(skb, a);
+
+		list_del(&csi->node);
+		kfree(csi);
+		phy->csi.count--;
+
+		err = phy->csi.count;
+	}
+out:
+	spin_unlock_bh(&phy->csi.lock);
+
+	return err;
+}
 
 static const struct wiphy_vendor_command mt7996_vendor_commands[] = {
 	{
@@ -1129,6 +1361,18 @@ static const struct wiphy_vendor_command mt7996_vendor_commands[] = {
 		.policy = beacon_ctrl_policy,
 		.maxattr = MTK_VENDOR_ATTR_BEACON_CTRL_MAX,
 	},
+	{
+		.info = {
+			.vendor_id = MTK_NL80211_VENDOR_ID,
+			.subcmd = MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL,
+		},
+		.flags = WIPHY_VENDOR_CMD_NEED_NETDEV |
+			 WIPHY_VENDOR_CMD_NEED_RUNNING,
+		.doit = mt7996_vendor_csi_ctrl,
+		.dumpit = mt7996_vendor_csi_ctrl_dump,
+		.policy = csi_ctrl_policy,
+		.maxattr = MTK_VENDOR_ATTR_CSI_CTRL_MAX,
+	},
 };
 
 void mt7996_vendor_register(struct mt7996_phy *phy)
@@ -1136,6 +1380,9 @@ void mt7996_vendor_register(struct mt7996_phy *phy)
 	phy->mt76->hw->wiphy->vendor_commands = mt7996_vendor_commands;
 	phy->mt76->hw->wiphy->n_vendor_commands = ARRAY_SIZE(mt7996_vendor_commands);
 
+	INIT_LIST_HEAD(&phy->csi.list);
+	spin_lock_init(&phy->csi.lock);
+
 	spin_lock_init(&phy->amnt_lock);
 }
 #endif
diff --git a/mt7996/vendor.h b/mt7996/vendor.h
index 3234677..834b3d0 100644
--- a/mt7996/vendor.h
+++ b/mt7996/vendor.h
@@ -7,6 +7,7 @@
 
 enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_AMNT_CTRL = 0xae,
+	MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL = 0xc2,
 	MTK_NL80211_VENDOR_SUBCMD_RFEATURE_CTRL = 0xc3,
 	MTK_NL80211_VENDOR_SUBCMD_WIRELESS_CTRL = 0xc4,
 	MTK_NL80211_VENDOR_SUBCMD_MU_CTRL = 0xc5,
@@ -240,6 +241,57 @@ enum mtk_vendor_attr_beacon_ctrl {
 		NUM_MTK_VENDOR_ATTRS_BEACON_CTRL - 1
 };
 
+enum mtk_vendor_attr_csi_ctrl {
+	MTK_VENDOR_ATTR_CSI_CTRL_UNSPEC,
+
+	MTK_VENDOR_ATTR_CSI_CTRL_CFG,
+	MTK_VENDOR_ATTR_CSI_CTRL_CFG_MODE,
+	MTK_VENDOR_ATTR_CSI_CTRL_CFG_TYPE,
+	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_DUMP_NUM,
+
+	MTK_VENDOR_ATTR_CSI_CTRL_DATA,
+
+	MTK_VENDOR_ATTR_CSI_CTRL_BAND_IDX,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_CSI_CTRL,
+	MTK_VENDOR_ATTR_CSI_CTRL_MAX =
+		NUM_MTK_VENDOR_ATTRS_CSI_CTRL - 1
+};
+
+enum mtk_vendor_attr_csi_data {
+	MTK_VENDOR_ATTR_CSI_DATA_UNSPEC,
+	MTK_VENDOR_ATTR_CSI_DATA_PAD,
+
+	MTK_VENDOR_ATTR_CSI_DATA_VER,
+	MTK_VENDOR_ATTR_CSI_DATA_TS,
+	MTK_VENDOR_ATTR_CSI_DATA_RSSI,
+	MTK_VENDOR_ATTR_CSI_DATA_SNR,
+	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,
+	MTK_VENDOR_ATTR_CSI_DATA_RSVD1,
+	MTK_VENDOR_ATTR_CSI_DATA_RSVD2,
+	MTK_VENDOR_ATTR_CSI_DATA_RSVD3,
+	MTK_VENDOR_ATTR_CSI_DATA_RSVD4,
+	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_CHAIN_INFO,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_CSI_DATA,
+	MTK_VENDOR_ATTR_CSI_DATA_MAX =
+		NUM_MTK_VENDOR_ATTRS_CSI_DATA - 1
+};
 #endif
 
 #endif
-- 
2.45.2