From 9ee127de900a433833746e2cf386145e863b37a9 Mon Sep 17 00:00:00 2001 From: Zachery Aaron Shores-Chmielewski Date: Sat, 22 Aug 2026 00:08:54 +0400 Subject: [PATCH] =?UTF-8?q?CSI=20port:=20SmartRG/MTK=20CSI=20patch=20(Bo?= =?UTF-8?q?=20Jiao,=20MTK)=20rebased=20onto=20mt76=20eb567bc7=20(24.10.5?= =?UTF-8?q?=20pin)=20=E2=80=94=204=20rejects=20hand-merged=20(enum=200xc2?= =?UTF-8?q?=20coexists=20with=20SPR=5FSET=5FPARAM;=20Makefile/vendor.h=20a?= =?UTF-8?q?nchors);=20build=20via=20filogic=20SDK?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 5 + tools/patches/9999-mt76-csi-24.10.5.patch | 896 ++++++++++++++++++++++ 2 files changed, 901 insertions(+) create mode 100644 tools/patches/9999-mt76-csi-24.10.5.patch diff --git a/.gitignore b/.gitignore index 3718155..499ab5b 100644 --- a/.gitignore +++ b/.gitignore @@ -13,3 +13,8 @@ __pycache__/ *.gpr *.id* ghidra-proj/ + +firmware-build/ +sdk/ +openwrt/ +mt76/ diff --git a/tools/patches/9999-mt76-csi-24.10.5.patch b/tools/patches/9999-mt76-csi-24.10.5.patch new file mode 100644 index 0000000..45a6aac --- /dev/null +++ b/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 ++ ++#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