CSI port: SmartRG/MTK CSI patch (Bo Jiao, MTK) rebased onto mt76 eb567bc7 (24.10.5 pin) — 4 rejects hand-merged (enum 0xc2 coexists with SPR_SET_PARAM; Makefile/vendor.h anchors); build via filogic SDK

.gitignore

@@ -13,3 +13,8 @@ __pycache__/
 *.gpr
 *.id*
 ghidra-proj/
+
+firmware-build/
+sdk/
+openwrt/
+mt76/

tools/patches/9999-mt76-csi-24.10.5.patch

@@ -0,0 +1,896 @@
+diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
+index 8d59cf43..574838f1 100644
+--- a/mt76_connac_mcu.h
++++ b/mt76_connac_mcu.h
+@@ -1046,6 +1046,7 @@ enum {
+ 	MCU_EXT_EVENT_BCC_NOTIFY = 0x75,
+ 	MCU_EXT_EVENT_WF_RF_PIN_CTRL = 0x9a,
+ 	MCU_EXT_EVENT_MURU_CTRL = 0x9f,
++	MCU_EXT_EVENT_CSI_REPORT = 0xc2,
+ };
+ 
+ /* unified event table */
+@@ -1262,6 +1263,7 @@ enum {
+ 	MCU_EXT_CMD_DPD_PRE_CAL_INFO = 0xac,
+ 	MCU_EXT_CMD_PHY_STAT_INFO = 0xad,
+ 	MCU_EXT_CMD_WF_RF_PIN_CTRL = 0xbd,
++	MCU_EXT_CMD_CSI_CTRL = 0xc2,
+ };
+ 
+ enum {
+diff --git a/mt7915/Makefile b/mt7915/Makefile
+index 6b0058ca..ad936178 100644
+--- a/mt7915/Makefile
++++ b/mt7915/Makefile
+@@ -1,10 +1,10 @@
+ # SPDX-License-Identifier: BSD-3-Clause-Clear
+ 
+-EXTRA_CFLAGS += -DCONFIG_MT76_LEDS
++ccflags-y += -DCONFIG_MT76_LEDS -DCONFIG_MTK_VENDOR
+ obj-$(CONFIG_MT7915E) += mt7915e.o
+ 
+ mt7915e-y := pci.o init.o dma.o eeprom.o main.o mcu.o mac.o \
+-	     debugfs.o mmio.o
++	     debugfs.o mmio.o vendor.o
+ 
+ mt7915e-$(CONFIG_NL80211_TESTMODE) += testmode.o
+ mt7915e-$(CONFIG_MT798X_WMAC) += soc.o
+diff --git a/mt7915/init.c b/mt7915/init.c
+index 2da6ea91..56e722e1 100644
+--- a/mt7915/init.c
++++ b/mt7915/init.c
+@@ -719,6 +719,12 @@ mt7915_register_ext_phy(struct mt7915_dev *dev, struct mt7915_phy *phy)
+ 	/* init wiphy according to mphy and phy */
+ 	mt7915_init_wiphy(phy);
+ 
++#ifdef CONFIG_MTK_VENDOR
++	INIT_LIST_HEAD(&phy->csi.csi_list);
++	spin_lock_init(&phy->csi.csi_lock);
++	mt7915_vendor_register(phy);
++#endif
++
+ 	ret = mt76_register_phy(mphy, true, mt76_rates,
+ 				ARRAY_SIZE(mt76_rates));
+ 	if (ret)
+@@ -1196,6 +1202,25 @@ void mt7915_set_stream_he_caps(struct mt7915_phy *phy)
+ 	}
+ }
+ 
++#ifdef CONFIG_MTK_VENDOR
++static int mt7915_unregister_features(struct mt7915_phy *phy)
++{
++	struct csi_data *c, *tmp_c;
++
++	spin_lock_bh(&phy->csi.csi_lock);
++	phy->csi.enable = 0;
++
++	list_for_each_entry_safe(c, tmp_c, &phy->csi.csi_list, node) {
++		list_del(&c->node);
++		kfree(c);
++	}
++	spin_unlock_bh(&phy->csi.csi_lock);
++
++
++	return 0;
++}
++#endif
++
+ static void mt7915_unregister_ext_phy(struct mt7915_dev *dev)
+ {
+ 	struct mt7915_phy *phy = mt7915_ext_phy(dev);
+@@ -1204,6 +1229,10 @@ static void mt7915_unregister_ext_phy(struct mt7915_dev *dev)
+ 	if (!phy)
+ 		return;
+ 
++#ifdef CONFIG_MTK_VENDOR
++	mt7915_unregister_features(phy);
++#endif
++
+ 	mt7915_unregister_thermal(phy);
+ 	mt76_unregister_phy(mphy);
+ 	ieee80211_free_hw(mphy->hw);
+@@ -1216,6 +1245,10 @@ static void mt7915_stop_hardware(struct mt7915_dev *dev)
+ 	mt7915_dma_cleanup(dev);
+ 	tasklet_disable(&dev->mt76.irq_tasklet);
+ 
++#ifdef CONFIG_MTK_VENDOR
++	mt7915_unregister_features(&dev->phy);
++#endif
++
+ 	if (is_mt798x(&dev->mt76))
+ 		mt7986_wmac_disable(dev);
+ }
+@@ -1254,6 +1287,12 @@ int mt7915_register_device(struct mt7915_dev *dev)
+ 	dev->mt76.test_ops = &mt7915_testmode_ops;
+ #endif
+ 
++#ifdef CONFIG_MTK_VENDOR
++	INIT_LIST_HEAD(&dev->phy.csi.csi_list);
++	spin_lock_init(&dev->phy.csi.csi_lock);
++	mt7915_vendor_register(&dev->phy);
++#endif
++
+ 	ret = mt76_register_device(&dev->mt76, true, mt76_rates,
+ 				   ARRAY_SIZE(mt76_rates));
+ 	if (ret)
+diff --git a/mt7915/mcu.c b/mt7915/mcu.c
+index 40364eea..9d00b7be 100644
+--- a/mt7915/mcu.c
++++ b/mt7915/mcu.c
+@@ -39,6 +39,10 @@ static bool sr_scene_detect = true;
+ module_param(sr_scene_detect, bool, 0644);
+ MODULE_PARM_DESC(sr_scene_detect, "Enable firmware scene detection algorithm");
+ 
++#ifdef CONFIG_MTK_VENDOR
++static int mt7915_mcu_report_csi(struct mt7915_dev *dev, struct sk_buff *skb);
++#endif
++
+ static u8
+ mt7915_mcu_get_sta_nss(u16 mcs_map)
+ {
+@@ -421,6 +425,11 @@ mt7915_mcu_rx_ext_event(struct mt7915_dev *dev, struct sk_buff *skb)
+ 	case MCU_EXT_EVENT_FW_LOG_2_HOST:
+ 		mt7915_mcu_rx_log_message(dev, skb);
+ 		break;
++#ifdef CONFIG_MTK_VENDOR
++	case MCU_EXT_EVENT_CSI_REPORT:
++		mt7915_mcu_report_csi(dev, skb);
++		break;
++#endif
+ 	case MCU_EXT_EVENT_BCC_NOTIFY:
+ 		mt7915_mcu_rx_bcc_notify(dev, skb);
+ 		break;
+@@ -4139,3 +4148,106 @@ int mt7915_mcu_rf_regval(struct mt7915_dev *dev, u32 regidx, u32 *val, bool set)
+ 
+ 	return 0;
+ }
++
++#ifdef CONFIG_MTK_VENDOR
++int mt7915_mcu_set_csi(struct mt7915_phy *phy, u8 mode,
++			u8 cfg, u8 v1, u32 v2, u8 *mac_addr)
++{
++	struct mt7915_dev *dev = phy->dev;
++	struct mt7915_mcu_csi req = {
++		.band = phy != &dev->phy,
++		.mode = mode,
++		.cfg = cfg,
++		.v1 = v1,
++		.v2 = cpu_to_le32(v2),
++	};
++
++	if (is_valid_ether_addr(mac_addr))
++		ether_addr_copy(req.mac_addr, mac_addr);
++
++	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(CSI_CTRL), &req,
++				 sizeof(req), false);
++}
++
++static int
++mt7915_mcu_report_csi(struct mt7915_dev *dev, struct sk_buff *skb)
++{
++	struct mt76_connac2_mcu_rxd *rxd = (struct mt76_connac2_mcu_rxd *)skb->data;
++	struct mt7915_phy *phy = &dev->phy;
++	struct mt7915_mcu_csi_report *cr;
++	struct csi_data *csi;
++	int len, i;
++
++	skb_pull(skb, sizeof(struct mt76_connac2_mcu_rxd));
++
++	len = le16_to_cpu(rxd->len) - sizeof(struct mt76_connac2_mcu_rxd) + 24;
++	if (len < sizeof(*cr))
++		return -EINVAL;
++
++	cr = (struct mt7915_mcu_csi_report *)skb->data;
++
++	if (phy->csi.interval &&
++	    le32_to_cpu(cr->ts) < phy->csi.last_record + phy->csi.interval)
++		return 0;
++
++	csi = kzalloc(sizeof(*csi), GFP_KERNEL);
++	if (!csi)
++		return -ENOMEM;
++
++#define SET_CSI_DATA(_field)	csi->_field = le32_to_cpu(cr->_field)
++	SET_CSI_DATA(ch_bw);
++	SET_CSI_DATA(rssi);
++	SET_CSI_DATA(snr);
++	SET_CSI_DATA(data_num);
++	SET_CSI_DATA(data_bw);
++	SET_CSI_DATA(pri_ch_idx);
++	SET_CSI_DATA(info);
++	SET_CSI_DATA(rx_mode);
++	SET_CSI_DATA(h_idx);
++	SET_CSI_DATA(ts);
++
++	SET_CSI_DATA(band);
++	if (csi->band && !phy->mt76->band_idx)
++		phy = mt7915_ext_phy(dev);
++#undef SET_CSI_DATA
++
++	for (i = 0; i < csi->data_num; i++) {
++		csi->data_i[i] = le16_to_cpu(cr->data_i[i]);
++		csi->data_q[i] = le16_to_cpu(cr->data_q[i]);
++	}
++
++	memcpy(csi->ta, cr->ta, ETH_ALEN);
++	csi->tx_idx = le32_get_bits(cr->trx_idx, GENMASK(31, 16));
++	csi->rx_idx = le32_get_bits(cr->trx_idx, GENMASK(15, 0));
++
++	INIT_LIST_HEAD(&csi->node);
++	spin_lock_bh(&phy->csi.csi_lock);
++
++	if (!phy->csi.enable) {
++		kfree(csi);
++		spin_unlock_bh(&phy->csi.csi_lock);
++		return 0;
++	}
++
++	list_add_tail(&csi->node, &phy->csi.csi_list);
++	phy->csi.count++;
++
++	if (phy->csi.count > CSI_MAX_BUF_NUM) {
++		struct csi_data *old;
++
++		old = list_first_entry(&phy->csi.csi_list,
++				       struct csi_data, node);
++
++		list_del(&old->node);
++		kfree(old);
++		phy->csi.count--;
++	}
++
++	if (csi->h_idx & BIT(15)) /* last chain */
++		phy->csi.last_record = csi->ts;
++	spin_unlock_bh(&phy->csi.csi_lock);
++
++	return 0;
++}
++#endif
++
+diff --git a/mt7915/mcu.h b/mt7915/mcu.h
+index 3be105bc..c3c906fc 100644
+--- a/mt7915/mcu.h
++++ b/mt7915/mcu.h
+@@ -522,4 +522,82 @@ enum {
+ 					 sizeof(struct bss_info_bmc_rate) +\
+ 					 sizeof(struct bss_info_ext_bss))
+ 
++
++#ifdef CONFIG_MTK_VENDOR
++struct mt7915_mcu_csi {
++	u8 band;
++	u8 mode;
++	u8 cfg;
++	u8 v1;
++	__le32 v2;
++	u8 mac_addr[ETH_ALEN];
++	u8 _rsv[34];
++} __packed;
++
++struct csi_tlv {
++	__le32 tag;
++	__le32 len;
++} __packed;
++
++#define CSI_MAX_COUNT	256
++#define CSI_MAX_BUF_NUM	3000
++
++struct mt7915_mcu_csi_report {
++	struct csi_tlv _t0;
++	__le32 ver;
++	struct csi_tlv _t1;
++	__le32 ch_bw;
++	struct csi_tlv _t2;
++	__le32 rssi;
++	struct csi_tlv _t3;
++	__le32 snr;
++	struct csi_tlv _t4;
++	__le32 band;
++	struct csi_tlv _t5;
++	__le32 data_num;
++	struct csi_tlv _t6;
++	__le16 data_i[CSI_MAX_COUNT];
++	struct csi_tlv _t7;
++	__le16 data_q[CSI_MAX_COUNT];
++	struct csi_tlv _t8;
++	__le32 data_bw;
++	struct csi_tlv _t9;
++	__le32 pri_ch_idx;
++	struct csi_tlv _t10;
++	u8 ta[8];
++	struct csi_tlv _t11;
++	__le32 info;
++	struct csi_tlv _t12;
++	__le32 rx_mode;
++	struct csi_tlv _t17;
++	__le32 h_idx;
++	struct csi_tlv _t18;
++	__le32 trx_idx;
++	struct csi_tlv _t19;
++	__le32 ts;
++} __packed;
++
++struct csi_data {
++	u8 ch_bw;
++	u16 data_num;
++	s16 data_i[CSI_MAX_COUNT];
++	s16 data_q[CSI_MAX_COUNT];
++	u8 band;
++	s8 rssi;
++	u8 snr;
++	u32 ts;
++	u8 data_bw;
++	u8 pri_ch_idx;
++	u8 ta[ETH_ALEN];
++	u32 info;
++	u8 rx_mode;
++	u32 h_idx;
++	u16 tx_idx;
++	u16 rx_idx;
++
++	struct list_head node;
++};
++#endif
++
++
+ #endif
+diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
+index 7d98a42d..071600c2 100644
+--- a/mt7915/mt7915.h
++++ b/mt7915/mt7915.h
+@@ -240,6 +240,20 @@ struct mt7915_phy {
+ 		u8 spe_idx;
+ 	} test;
+ #endif
++
++#ifdef CONFIG_MTK_VENDOR
++	struct {
++		struct list_head csi_list;
++		spinlock_t csi_lock;
++		u32 count;
++		bool mask;
++		bool reorder;
++		bool enable;
++
++		u32 interval;
++		u32 last_record;
++	} csi;
++#endif
+ };
+ 
+ struct mt7915_dev {
+@@ -628,4 +642,10 @@ void mt7915_sta_add_debugfs(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+ int mt7915_mmio_wed_init(struct mt7915_dev *dev, void *pdev_ptr,
+ 			 bool pci, int *irq);
+ 
++#ifdef CONFIG_MTK_VENDOR
++void mt7915_vendor_register(struct mt7915_phy *phy);
++int mt7915_mcu_set_csi(struct mt7915_phy *phy, u8 mode,
++			u8 cfg, u8 v1, u32 v2, u8 *mac_addr);
++#endif
++
+ #endif
+diff --git a/mt7915/vendor.c b/mt7915/vendor.c
+new file mode 100644
+index 00000000..98fd9c2d
+--- /dev/null
++++ b/mt7915/vendor.c
+@@ -0,0 +1,452 @@
++// SPDX-License-Identifier: ISC
++/*
++ * Copyright (C) 2020, MediaTek Inc. All rights reserved.
++ */
++
++#include <net/netlink.h>
++
++#include "mt7915.h"
++#include "mcu.h"
++#include "vendor.h"
++
++static const 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_U8 },
++	[MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR] = { .type = NLA_NESTED },
++	[MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL] = { .type = NLA_U32 },
++	[MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM] = { .type = NLA_U16 },
++	[MTK_VENDOR_ATTR_CSI_CTRL_DATA] = { .type = NLA_NESTED },
++};
++
++struct csi_null_tone {
++	u8 start;
++	u8 end;
++};
++
++struct csi_reorder{
++	u8 dest;
++	u8 start;
++	u8 end;
++};
++
++struct csi_mask {
++	struct csi_null_tone null[10];
++	u8 pilot[8];
++	struct csi_reorder ro[3];
++};
++
++static const struct csi_mask csi_mask_groups[] = {
++	/* OFDM */
++	{ .null = { { 0 }, { 27, 37 } },
++	  .ro = { {0, 0, 63} },
++	},
++	{ .null = { { 0, 69 }, { 96 }, { 123, 127 } },
++	  .ro = { { 0, 96 }, { 38, 70, 95 }, { 1, 97, 122 } },
++	},
++	{ .null = { { 0, 5 }, { 32 }, { 59, 127 } },
++	  .ro = { { 0, 32 }, { 38, 6, 31 }, { 1, 33, 58 } },
++	},
++	{ .null = { { 0, 5 }, { 32 }, { 59, 69 }, { 96 }, { 123, 127 } },
++	  .ro = { { 0, 0, 127 } },
++	},
++	{ .null = { { 0, 133 }, { 160 }, { 187, 255 } },
++	  .ro = { { 0, 160 }, { 1, 161, 186 }, { 38, 134, 159 } },
++	},
++	{ .null = { { 0, 197 }, { 224 }, { 251, 255 } },
++	  .ro = { { 0, 224 }, { 1, 225, 250 }, { 38, 198, 223 } },
++	},
++	{ .null = { { 0, 5 }, { 32 }, { 59, 255 } },
++	  .ro = { { 0, 32 }, { 1, 33, 58 }, { 38, 6, 31 } },
++	},
++	{ .null = { { 0, 69 }, { 96 }, { 123, 255 } },
++	  .ro = { { 0, 96 }, { 1, 97, 122 }, { 38, 70, 95 } },
++	},
++	{ .null = { { 0, 133 }, { 160 }, { 187, 197 }, { 224 }, { 251, 255 } },
++	  .ro = { { 0, 192 }, { 2, 198, 250 }, { 74, 134, 186 } },
++	},
++	{ .null = { { 0, 5 }, { 32 }, { 59, 69 }, { 96 }, { 123, 255 } },
++	  .ro = { { 0, 64 }, { 2, 70, 122 }, { 74, 6, 58 } },
++	},
++	{ .null = { { 0, 5 }, { 32 }, { 59, 69 }, { 96 }, { 123, 133 },
++		    { 160 }, { 187, 197 }, { 224 }, { 251, 255 } },
++	  .ro = { { 0, 0, 255 } },
++	},
++
++	/* HT/VHT */
++	{ .null = { { 0 }, { 29, 35 } },
++	  .pilot = { 7, 21, 43, 57 },
++	  .ro = { { 0, 0, 63 } },
++	},
++	{ .null = { { 0, 67 }, { 96 }, { 125, 127 } },
++	  .pilot = { 75, 89, 103, 117 },
++	  .ro = { { 0, 96 }, { 36, 68, 95 }, { 1, 97, 124 } },
++	},
++	{ .null = { { 0, 3 }, { 32 }, { 61, 127 } },
++	  .pilot = { 11, 25, 39, 53 },
++	  .ro = { { 0, 32 }, { 36, 4, 31 }, { 1, 33, 60 } },
++	},
++	{ .null = { { 0, 1 }, { 59, 69 }, { 127 } },
++	  .pilot = { 11, 25, 53, 75, 103, 117 },
++	  .ro = { { 0, 0, 127 } },
++	},
++	{ .null = { { 0, 131 }, { 160 }, { 189, 255 } },
++	  .pilot = { 139, 153, 167, 181 },
++	  .ro = { { 0, 160 }, { 1, 161, 188 }, { 36, 132, 159 } },
++	},
++	{ .null = { { 0, 195 }, { 224 }, { 253 }, { 255 } },
++	  .pilot = { 203, 217, 231, 245 },
++	  .ro = { { 0, 224 }, { 1, 225, 252 }, { 36, 196, 223 } },
++	},
++	{ .null = { { 0, 3 }, { 32 }, { 61, 255 } },
++	  .pilot = { 11, 25, 39, 53 },
++	  .ro = { { 0, 32 }, { 1, 33, 60 }, { 36, 4, 31 } },
++	},
++	{ .null = { { 0, 67 }, { 96 }, { 125, 255 } },
++	  .pilot = { 75, 89, 103, 117 },
++	  .ro = { { 0, 96 }, { 1, 97, 124 }, { 36, 68, 95 } },
++	},
++	{ .null = { { 0, 133 }, { 191, 193 }, { 251, 255 } },
++	  .pilot = { 139, 167, 181, 203, 217, 245 },
++	  .ro = { { 0, 192 }, { 2, 194, 250 }, { 70, 134, 190 } },
++	},
++	{ .null = { { 0, 5 }, { 63, 65 }, { 123, 127 } },
++	  .pilot = { 11, 39, 53, 75, 89, 117 },
++	  .ro = { { 0, 64 }, { 2, 66, 122 }, { 70, 6, 62 } },
++	},
++	{ .null = { { 0, 1 }, { 123, 133 }, { 255 } },
++	  .pilot = { 11, 39, 75, 103, 153, 181, 217, 245 },
++	  .ro = { { 0, 0, 255 } },
++	},
++
++	/* HE */
++	{ .null = { { 0 }, { 31, 33 } },
++	  .pilot = { 12, 29, 35, 52 },
++	  .ro = { { 0, 0, 63 } },
++	},
++	{ .null = { { 30, 34 }, { 96 } },
++	  .pilot = { 4, 21, 43, 60, 70, 87, 105, 122 },
++	  .ro = { { 0, 96 }, { 34, 66, 95 }, { 1, 97, 126 } },
++	},
++	{ .null = { { 32 }, { 94, 98 } },
++	  .pilot = { 6, 23, 41, 58, 68, 85, 107, 124 },
++	  .ro = { { 0, 32 }, { 34, 2, 31 }, { 1, 31, 62 } },
++	},
++	{ .null = { { 0 }, { 62, 66 } },
++	  .pilot = { 9, 26, 36, 53, 75, 92, 102, 119 },
++	  .ro = { { 0, 0, 127 } },
++	},
++	{ .null = { { 30, 34 }, { 160 } },
++	  .pilot = { 4, 21, 43, 60, 137, 154, 166, 183 },
++	  .ro = { { 0, 160 }, { 1, 161, 190 }, { 34, 130, 159 } },
++	},
++	{ .null = { { 94, 98 }, { 224 } },
++	  .pilot = { 68, 85, 107, 124, 201, 218, 230, 247 },
++	  .ro = { { 0, 224 }, { 1, 225, 254 }, { 34, 194, 223 } },
++	},
++	{ .null = { { 32 }, { 158, 162 } },
++	  .pilot = { 9, 26, 38, 55, 132, 149, 171, 188 },
++	  .ro = { { 0, 32 }, { 1, 33, 62 }, { 34, 2, 31 } },
++	},
++	{ .null = { { 96 }, { 222, 226 } },
++	  .pilot = { 73, 90, 102, 119, 196, 213, 235, 252 },
++	  .ro = { { 0, 96 }, { 1, 97, 126 }, { 34, 66, 95 } },
++	},
++	{ .null = { { 62, 66 }, { 192 } },
++	  .pilot = { 36, 53, 75, 92, 169, 186, 198, 215 },
++	  .ro = { { 0, 192 }, { 1, 193, 253 }, { 67, 131, 191 } },
++	},
++	{ .null = { { 64 }, { 190, 194 } },
++	  .pilot = { 41, 58, 70, 87, 164, 181, 203, 220 },
++	  .ro = { { 0, 64 }, { 1, 65, 125 }, { 67, 3, 63 } },
++	},
++	{ .null = { { 0 }, { 126, 130 } },
++	  .pilot = { 6, 23, 100, 117, 139, 156, 233, 250 },
++	  .ro = { { 0, 0, 255 } },
++	},
++};
++
++static inline u8 csi_group_idx(u8 mode, u8 ch_bw, u8 data_bw, u8 pri_ch_idx)
++{
++	if (ch_bw < 2 || data_bw < 1)
++		return mode * 11 + ch_bw * ch_bw + pri_ch_idx;
++	else
++		return mode * 11 + ch_bw * ch_bw + (data_bw + 1) * 2 + pri_ch_idx;
++}
++
++static int mt7915_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 mt7915_phy *phy = mt7915_hw_phy(hw);
++	struct nlattr *tb[NUM_MTK_VENDOR_ATTRS_CSI_CTRL];
++	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_CFG]) {
++		u8 mode = 0, type = 0, v1 = 0, 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_u8(cur);
++				break;
++			default:
++				return -EINVAL;
++			};
++		}
++
++		if (tb[MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR]) {
++			int idx = 0;
++
++			nla_for_each_nested(cur, tb[MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR], rem) {
++				mac_addr[idx++] = nla_get_u8(cur);
++			}
++		}
++
++		mt7915_mcu_set_csi(phy, mode, type, v1, v2, mac_addr);
++
++		spin_lock_bh(&phy->csi.csi_lock);
++
++		phy->csi.enable = !!mode;
++
++		if (mode == 2 && type == 5) {
++			if (v1 >= 1)
++				phy->csi.mask = 1;
++			if (v1 == 2)
++				phy->csi.reorder = 1;
++		}
++
++		/* clean up old csi stats */
++		if ((mode == 0 || mode == 2) && !list_empty(&phy->csi.csi_list)) {
++			struct csi_data *c, *tmp_c;
++
++			list_for_each_entry_safe(c, tmp_c, &phy->csi.csi_list,
++						 node) {
++				list_del(&c->node);
++				kfree(c);
++				phy->csi.count--;
++			}
++		} else if (mode == 1) {
++			phy->csi.last_record = 0;
++		}
++
++		spin_unlock_bh(&phy->csi.csi_lock);
++	}
++
++	if (tb[MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL])
++		phy->csi.interval = nla_get_u32(tb[MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL]);
++
++	return 0;
++}
++
++static void
++mt7915_vendor_csi_tone_mask(struct mt7915_phy *phy, struct csi_data *csi)
++{
++	static const u8 mode_map[] = {
++		[MT_PHY_TYPE_OFDM] = 0,
++		[MT_PHY_TYPE_HT] = 1,
++		[MT_PHY_TYPE_VHT] = 1,
++		[MT_PHY_TYPE_HE_SU] = 2,
++	};
++	const struct csi_mask *cmask;
++	int i;
++
++	if (csi->rx_mode == MT_PHY_TYPE_CCK || !phy->csi.mask)
++		return;
++
++	if (csi->data_bw == IEEE80211_STA_RX_BW_40)
++		csi->pri_ch_idx /= 2;
++
++	cmask = &csi_mask_groups[csi_group_idx(mode_map[csi->rx_mode],
++					       csi->ch_bw,
++					       csi->data_bw,
++					       csi->pri_ch_idx)];
++
++	for (i = 0; i < 10; i++) {
++		const struct csi_null_tone *ntone = &cmask->null[i];
++		u8 start = ntone->start;
++		u8 end = ntone->end;
++		int j;
++
++		if (!start && !end && i > 0)
++			break;
++
++		if (!end)
++			end = start;
++
++		for (j = start; j <= end; j++) {
++			csi->data_i[j] = 0;
++			csi->data_q[j] = 0;
++		}
++	}
++
++	for (i = 0; i < 8; i++) {
++		u8 pilot = cmask->pilot[i];
++
++		if (!pilot)
++			break;
++
++		csi->data_i[pilot] = 0;
++		csi->data_q[pilot] = 0;
++	}
++
++	if (!phy->csi.reorder)
++		return;
++
++	for (i = 0; i < 3; i++) {
++		const struct csi_reorder *ro = &cmask->ro[i];
++		u8 dest = ro->dest;
++		u8 start = ro->start;
++		u8 end = ro->end;
++
++		if (!dest && !start && !end)
++			break;
++
++		if (dest == start)
++			continue;
++
++		if (end) {
++			memmove(&csi->data_i[dest], &csi->data_i[start],
++				end - start + 1);
++			memmove(&csi->data_q[dest], &csi->data_q[start],
++				end - start + 1);
++		} else {
++			csi->data_i[dest] = csi->data_i[start];
++			csi->data_q[dest] = csi->data_q[start];
++		}
++	}
++}
++
++static int
++mt7915_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 mt7915_phy *phy = mt7915_hw_phy(hw);
++	struct nlattr *tb[NUM_MTK_VENDOR_ATTRS_CSI_CTRL];
++	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 (!(*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.csi_lock);
++
++	if (!list_empty(&phy->csi.csi_list)) {
++		struct csi_data *csi;
++		void *a, *b;
++		int i;
++
++		csi = list_first_entry(&phy->csi.csi_list, struct csi_data, node);
++
++		mt7915_vendor_csi_tone_mask(phy, csi);
++
++		a = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_CTRL_DATA);
++
++		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->info) ||
++		    nla_put_u32(skb, MTK_VENDOR_ATTR_CSI_DATA_H_IDX, csi->h_idx) ||
++		    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);
++			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);
++
++		b = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_DATA_I);
++			for (i = 0; i < ARRAY_SIZE(csi->data_i); 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);
++			for (i = 0; i < ARRAY_SIZE(csi->data_q); 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.csi_lock);
++
++	return err;
++}
++
++static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
++	{
++		.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 = mt7915_vendor_csi_ctrl,
++		.dumpit = mt7915_vendor_csi_ctrl_dump,
++		.policy = csi_ctrl_policy,
++		.maxattr = MTK_VENDOR_ATTR_CSI_CTRL_MAX,
++	}
++};
++
++void mt7915_vendor_register(struct mt7915_phy *phy)
++{
++	phy->mt76->hw->wiphy->vendor_commands = mt7915_vendor_commands;
++	phy->mt76->hw->wiphy->n_vendor_commands = ARRAY_SIZE(mt7915_vendor_commands);
++}
+diff --git a/mt7915/vendor.h b/mt7915/vendor.h
+new file mode 100644
+index 00000000..9d3db2a7
+--- /dev/null
++++ b/mt7915/vendor.h
+@@ -0,0 +1,60 @@
++#ifndef __MT7915_VENDOR_H
++#define __MT7915_VENDOR_H
++
++#define MTK_NL80211_VENDOR_ID	0x0ce7
++
++enum mtk_nl80211_vendor_subcmds {
++	MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL = 0xc2,
++};
++
++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_INTERVAL,
++
++	MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM,
++
++	MTK_VENDOR_ATTR_CSI_CTRL_DATA,
++
++	/* 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_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_H_IDX,
++
++	/* keep last */
++	NUM_MTK_VENDOR_ATTRS_CSI_DATA,
++	MTK_VENDOR_ATTR_CSI_DATA_MAX =
++		NUM_MTK_VENDOR_ATTRS_CSI_DATA - 1
++};
++
++#endif