blob: 10e5f574413e34ef30f75ed2e2a59c019a36858a [file] [log] [blame]
developer699cda22022-12-17 15:21:57 +08001From fb6a20679c1b404d3e06e79f1c83e8014ead7dc4 Mon Sep 17 00:00:00 2001
developere2cc0fa2022-03-29 17:31:03 +08002From: Bo Jiao <Bo.Jiao@mediatek.com>
developerbd398d52022-06-06 20:53:24 +08003Date: Mon, 6 Jun 2022 20:13:02 +0800
developer9851a292022-12-15 17:33:43 +08004Subject: [PATCH 1002/1009] mt76: mt7915: csi: implement csi support
developere2cc0fa2022-03-29 17:31:03 +08005
6---
developer5ce5ea42022-08-31 14:12:29 +08007 mt76_connac_mcu.h | 2 +
8 mt7915/Makefile | 4 +-
9 mt7915/init.c | 39 ++++
10 mt7915/mcu.c | 111 ++++++++++++
11 mt7915/mcu.h | 76 ++++++++
12 mt7915/mt7915.h | 20 ++
13 mt7915/vendor.c | 452 ++++++++++++++++++++++++++++++++++++++++++++++
14 mt7915/vendor.h | 60 ++++++
developere2cc0fa2022-03-29 17:31:03 +080015 8 files changed, 762 insertions(+), 2 deletions(-)
developer5ce5ea42022-08-31 14:12:29 +080016 create mode 100644 mt7915/vendor.c
17 create mode 100644 mt7915/vendor.h
developere2cc0fa2022-03-29 17:31:03 +080018
19diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
developer699cda22022-12-17 15:21:57 +080020index 9d0d613..26e7cf6 100644
developere2cc0fa2022-03-29 17:31:03 +080021--- a/mt76_connac_mcu.h
22+++ b/mt76_connac_mcu.h
developereb6a0182022-12-12 18:53:32 +080023@@ -998,6 +998,7 @@ enum {
developere2cc0fa2022-03-29 17:31:03 +080024 MCU_EXT_EVENT_CSA_NOTIFY = 0x4f,
25 MCU_EXT_EVENT_BCC_NOTIFY = 0x75,
26 MCU_EXT_EVENT_MURU_CTRL = 0x9f,
27+ MCU_EXT_EVENT_CSI_REPORT = 0xc2,
28 };
29
developer144824b2022-11-25 21:27:43 +080030 /* unified event table */
developereb6a0182022-12-12 18:53:32 +080031@@ -1191,6 +1192,7 @@ enum {
developere2cc0fa2022-03-29 17:31:03 +080032 MCU_EXT_CMD_GROUP_PRE_CAL_INFO = 0xab,
33 MCU_EXT_CMD_DPD_PRE_CAL_INFO = 0xac,
34 MCU_EXT_CMD_PHY_STAT_INFO = 0xad,
35+ MCU_EXT_CMD_CSI_CTRL = 0xc2,
36 };
37
38 enum {
39diff --git a/mt7915/Makefile b/mt7915/Makefile
developer699cda22022-12-17 15:21:57 +080040index a42866e..14ce98a 100644
developere2cc0fa2022-03-29 17:31:03 +080041--- a/mt7915/Makefile
42+++ b/mt7915/Makefile
43@@ -1,9 +1,9 @@
44 # SPDX-License-Identifier: ISC
45-
46+EXTRA_CFLAGS += -DCONFIG_MTK_VENDOR
47 obj-$(CONFIG_MT7915E) += mt7915e.o
48
49 mt7915e-y := pci.o init.o dma.o eeprom.o main.o mcu.o mac.o \
50- debugfs.o mmio.o mtk_debugfs.o mtk_mcu.o
51+ debugfs.o mmio.o mtk_debugfs.o mtk_mcu.o vendor.o
52
53 mt7915e-$(CONFIG_NL80211_TESTMODE) += testmode.o
developerbbedb242022-11-15 15:02:35 +080054 mt7915e-$(CONFIG_MT7986_WMAC) += soc.o
developere2cc0fa2022-03-29 17:31:03 +080055diff --git a/mt7915/init.c b/mt7915/init.c
developer699cda22022-12-17 15:21:57 +080056index b23a2fb..844a92f 100644
developere2cc0fa2022-03-29 17:31:03 +080057--- a/mt7915/init.c
58+++ b/mt7915/init.c
developer9851a292022-12-15 17:33:43 +080059@@ -658,6 +658,12 @@ mt7915_register_ext_phy(struct mt7915_dev *dev, struct mt7915_phy *phy)
developere2cc0fa2022-03-29 17:31:03 +080060 /* init wiphy according to mphy and phy */
developereb6a0182022-12-12 18:53:32 +080061 mt7915_init_wiphy(phy);
developere2cc0fa2022-03-29 17:31:03 +080062
63+#ifdef CONFIG_MTK_VENDOR
64+ INIT_LIST_HEAD(&phy->csi.csi_list);
65+ spin_lock_init(&phy->csi.csi_lock);
66+ mt7915_vendor_register(phy);
67+#endif
68+
69 ret = mt76_register_phy(mphy, true, mt76_rates,
70 ARRAY_SIZE(mt76_rates));
71 if (ret)
developer9851a292022-12-15 17:33:43 +080072@@ -1164,6 +1170,25 @@ void mt7915_set_stream_he_caps(struct mt7915_phy *phy)
developere2cc0fa2022-03-29 17:31:03 +080073 }
74 }
75
76+#ifdef CONFIG_MTK_VENDOR
77+static int mt7915_unregister_features(struct mt7915_phy *phy)
78+{
79+ struct csi_data *c, *tmp_c;
80+
81+ spin_lock_bh(&phy->csi.csi_lock);
82+ phy->csi.enable = 0;
83+
84+ list_for_each_entry_safe(c, tmp_c, &phy->csi.csi_list, node) {
85+ list_del(&c->node);
86+ kfree(c);
87+ }
88+ spin_unlock_bh(&phy->csi.csi_lock);
89+
90+
91+ return 0;
92+}
93+#endif
94+
95 static void mt7915_unregister_ext_phy(struct mt7915_dev *dev)
96 {
97 struct mt7915_phy *phy = mt7915_ext_phy(dev);
developer9851a292022-12-15 17:33:43 +080098@@ -1172,6 +1197,10 @@ static void mt7915_unregister_ext_phy(struct mt7915_dev *dev)
developere2cc0fa2022-03-29 17:31:03 +080099 if (!phy)
100 return;
101
102+#ifdef CONFIG_MTK_VENDOR
103+ mt7915_unregister_features(phy);
104+#endif
105+
106 mt7915_unregister_thermal(phy);
107 mt76_unregister_phy(mphy);
108 ieee80211_free_hw(mphy->hw);
developer9851a292022-12-15 17:33:43 +0800109@@ -1184,6 +1213,10 @@ static void mt7915_stop_hardware(struct mt7915_dev *dev)
developere2cc0fa2022-03-29 17:31:03 +0800110 mt7915_dma_cleanup(dev);
111 tasklet_disable(&dev->irq_tasklet);
112
113+#ifdef CONFIG_MTK_VENDOR
114+ mt7915_unregister_features(&dev->phy);
115+#endif
116+
117 if (is_mt7986(&dev->mt76))
118 mt7986_wmac_disable(dev);
119 }
developer9851a292022-12-15 17:33:43 +0800120@@ -1224,6 +1257,12 @@ int mt7915_register_device(struct mt7915_dev *dev)
developere2cc0fa2022-03-29 17:31:03 +0800121 dev->mt76.test_ops = &mt7915_testmode_ops;
122 #endif
123
124+#ifdef CONFIG_MTK_VENDOR
125+ INIT_LIST_HEAD(&dev->phy.csi.csi_list);
126+ spin_lock_init(&dev->phy.csi.csi_lock);
127+ mt7915_vendor_register(&dev->phy);
128+#endif
129+
developereb6a0182022-12-12 18:53:32 +0800130 ret = mt76_register_device(&dev->mt76, true, mt76_rates,
131 ARRAY_SIZE(mt76_rates));
132 if (ret)
developere2cc0fa2022-03-29 17:31:03 +0800133diff --git a/mt7915/mcu.c b/mt7915/mcu.c
developer699cda22022-12-17 15:21:57 +0800134index 644e6a8..788f3fb 100644
developere2cc0fa2022-03-29 17:31:03 +0800135--- a/mt7915/mcu.c
136+++ b/mt7915/mcu.c
developer144824b2022-11-25 21:27:43 +0800137@@ -36,6 +36,10 @@ static bool sr_scene_detect = true;
138 module_param(sr_scene_detect, bool, 0644);
139 MODULE_PARM_DESC(sr_scene_detect, "Enable firmware scene detection algorithm");
developere2cc0fa2022-03-29 17:31:03 +0800140
141+#ifdef CONFIG_MTK_VENDOR
142+static int mt7915_mcu_report_csi(struct mt7915_dev *dev, struct sk_buff *skb);
143+#endif
144+
145 static u8
146 mt7915_mcu_get_sta_nss(u16 mcs_map)
147 {
developereb6a0182022-12-12 18:53:32 +0800148@@ -371,6 +375,11 @@ mt7915_mcu_rx_ext_event(struct mt7915_dev *dev, struct sk_buff *skb)
developere2cc0fa2022-03-29 17:31:03 +0800149 case MCU_EXT_EVENT_FW_LOG_2_HOST:
150 mt7915_mcu_rx_log_message(dev, skb);
151 break;
152+#ifdef CONFIG_MTK_VENDOR
153+ case MCU_EXT_EVENT_CSI_REPORT:
154+ mt7915_mcu_report_csi(dev, skb);
155+ break;
156+#endif
157 case MCU_EXT_EVENT_BCC_NOTIFY:
158 mt7915_mcu_rx_bcc_notify(dev, skb);
159 break;
developer9851a292022-12-15 17:33:43 +0800160@@ -3775,6 +3784,108 @@ int mt7915_mcu_twt_agrt_update(struct mt7915_dev *dev,
developere2cc0fa2022-03-29 17:31:03 +0800161 &req, sizeof(req), true);
162 }
163
164+#ifdef CONFIG_MTK_VENDOR
165+int mt7915_mcu_set_csi(struct mt7915_phy *phy, u8 mode,
166+ u8 cfg, u8 v1, u32 v2, u8 *mac_addr)
167+{
168+ struct mt7915_dev *dev = phy->dev;
169+ struct mt7915_mcu_csi req = {
170+ .band = phy != &dev->phy,
171+ .mode = mode,
172+ .cfg = cfg,
173+ .v1 = v1,
174+ .v2 = cpu_to_le32(v2),
175+ };
176+
177+ if (is_valid_ether_addr(mac_addr))
178+ ether_addr_copy(req.mac_addr, mac_addr);
179+
180+ return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(CSI_CTRL), &req,
181+ sizeof(req), false);
182+}
183+
184+static int
185+mt7915_mcu_report_csi(struct mt7915_dev *dev, struct sk_buff *skb)
186+{
developerf64861f2022-06-22 11:44:53 +0800187+ struct mt76_connac2_mcu_rxd *rxd = (struct mt76_connac2_mcu_rxd *)skb->data;
developere2cc0fa2022-03-29 17:31:03 +0800188+ struct mt7915_phy *phy = &dev->phy;
189+ struct mt7915_mcu_csi_report *cr;
190+ struct csi_data *csi;
191+ int len, i;
192+
developerf64861f2022-06-22 11:44:53 +0800193+ skb_pull(skb, sizeof(struct mt76_connac2_mcu_rxd));
developere2cc0fa2022-03-29 17:31:03 +0800194+
developerf64861f2022-06-22 11:44:53 +0800195+ len = le16_to_cpu(rxd->len) - sizeof(struct mt76_connac2_mcu_rxd) + 24;
developere2cc0fa2022-03-29 17:31:03 +0800196+ if (len < sizeof(*cr))
197+ return -EINVAL;
198+
199+ cr = (struct mt7915_mcu_csi_report *)skb->data;
200+
201+ if (phy->csi.interval &&
202+ le32_to_cpu(cr->ts) < phy->csi.last_record + phy->csi.interval)
203+ return 0;
204+
205+ csi = kzalloc(sizeof(*csi), GFP_KERNEL);
206+ if (!csi)
207+ return -ENOMEM;
208+
209+#define SET_CSI_DATA(_field) csi->_field = le32_to_cpu(cr->_field)
210+ SET_CSI_DATA(ch_bw);
211+ SET_CSI_DATA(rssi);
212+ SET_CSI_DATA(snr);
213+ SET_CSI_DATA(data_num);
214+ SET_CSI_DATA(data_bw);
215+ SET_CSI_DATA(pri_ch_idx);
216+ SET_CSI_DATA(info);
217+ SET_CSI_DATA(rx_mode);
218+ SET_CSI_DATA(h_idx);
219+ SET_CSI_DATA(ts);
220+
221+ SET_CSI_DATA(band);
developereb6a0182022-12-12 18:53:32 +0800222+ if (csi->band && !phy->mt76->band_idx)
developere2cc0fa2022-03-29 17:31:03 +0800223+ phy = mt7915_ext_phy(dev);
224+#undef SET_CSI_DATA
225+
226+ for (i = 0; i < csi->data_num; i++) {
227+ csi->data_i[i] = le16_to_cpu(cr->data_i[i]);
228+ csi->data_q[i] = le16_to_cpu(cr->data_q[i]);
229+ }
230+
231+ memcpy(csi->ta, cr->ta, ETH_ALEN);
232+ csi->tx_idx = le32_get_bits(cr->trx_idx, GENMASK(31, 16));
233+ csi->rx_idx = le32_get_bits(cr->trx_idx, GENMASK(15, 0));
234+
235+ INIT_LIST_HEAD(&csi->node);
236+ spin_lock_bh(&phy->csi.csi_lock);
237+
238+ if (!phy->csi.enable) {
239+ kfree(csi);
240+ spin_unlock_bh(&phy->csi.csi_lock);
241+ return 0;
242+ }
243+
244+ list_add_tail(&csi->node, &phy->csi.csi_list);
245+ phy->csi.count++;
246+
247+ if (phy->csi.count > CSI_MAX_BUF_NUM) {
248+ struct csi_data *old;
249+
250+ old = list_first_entry(&phy->csi.csi_list,
251+ struct csi_data, node);
252+
253+ list_del(&old->node);
254+ kfree(old);
255+ phy->csi.count--;
256+ }
257+
258+ if (csi->h_idx & BIT(15)) /* last chain */
259+ phy->csi.last_record = csi->ts;
260+ spin_unlock_bh(&phy->csi.csi_lock);
261+
262+ return 0;
263+}
264+#endif
265+
266 #ifdef MTK_DEBUG
267 int mt7915_dbg_mcu_wa_cmd(struct mt7915_dev *dev, int cmd, u32 a1, u32 a2, u32 a3, bool wait_resp)
268 {
269diff --git a/mt7915/mcu.h b/mt7915/mcu.h
developer699cda22022-12-17 15:21:57 +0800270index aa89106..f3dedd5 100644
developere2cc0fa2022-03-29 17:31:03 +0800271--- a/mt7915/mcu.h
272+++ b/mt7915/mcu.h
developereb6a0182022-12-12 18:53:32 +0800273@@ -532,4 +532,80 @@ mt7915_get_power_bound(struct mt7915_phy *phy, s8 txpower)
274 return txpower;
275 }
developere2cc0fa2022-03-29 17:31:03 +0800276
277+#ifdef CONFIG_MTK_VENDOR
278+struct mt7915_mcu_csi {
279+ u8 band;
280+ u8 mode;
281+ u8 cfg;
282+ u8 v1;
283+ __le32 v2;
284+ u8 mac_addr[ETH_ALEN];
285+ u8 _rsv[34];
286+} __packed;
287+
288+struct csi_tlv {
289+ __le32 tag;
290+ __le32 len;
291+} __packed;
292+
293+#define CSI_MAX_COUNT 256
294+#define CSI_MAX_BUF_NUM 3000
295+
296+struct mt7915_mcu_csi_report {
297+ struct csi_tlv _t0;
298+ __le32 ver;
299+ struct csi_tlv _t1;
300+ __le32 ch_bw;
301+ struct csi_tlv _t2;
302+ __le32 rssi;
303+ struct csi_tlv _t3;
304+ __le32 snr;
305+ struct csi_tlv _t4;
306+ __le32 band;
307+ struct csi_tlv _t5;
308+ __le32 data_num;
309+ struct csi_tlv _t6;
310+ __le16 data_i[CSI_MAX_COUNT];
311+ struct csi_tlv _t7;
312+ __le16 data_q[CSI_MAX_COUNT];
313+ struct csi_tlv _t8;
314+ __le32 data_bw;
315+ struct csi_tlv _t9;
316+ __le32 pri_ch_idx;
317+ struct csi_tlv _t10;
318+ u8 ta[8];
319+ struct csi_tlv _t11;
320+ __le32 info;
321+ struct csi_tlv _t12;
322+ __le32 rx_mode;
323+ struct csi_tlv _t17;
324+ __le32 h_idx;
325+ struct csi_tlv _t18;
326+ __le32 trx_idx;
327+ struct csi_tlv _t19;
328+ __le32 ts;
329+} __packed;
330+
331+struct csi_data {
332+ u8 ch_bw;
333+ u16 data_num;
334+ s16 data_i[CSI_MAX_COUNT];
335+ s16 data_q[CSI_MAX_COUNT];
336+ u8 band;
337+ s8 rssi;
338+ u8 snr;
339+ u32 ts;
340+ u8 data_bw;
341+ u8 pri_ch_idx;
342+ u8 ta[ETH_ALEN];
343+ u32 info;
344+ u8 rx_mode;
345+ u32 h_idx;
346+ u16 tx_idx;
347+ u16 rx_idx;
348+
349+ struct list_head node;
350+};
351+#endif
352+
353 #endif
354diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
developer699cda22022-12-17 15:21:57 +0800355index afe2ec7..53e1315 100644
developere2cc0fa2022-03-29 17:31:03 +0800356--- a/mt7915/mt7915.h
357+++ b/mt7915/mt7915.h
developer9851a292022-12-15 17:33:43 +0800358@@ -293,6 +293,20 @@ struct mt7915_phy {
developere2cc0fa2022-03-29 17:31:03 +0800359 u8 spe_idx;
360 } test;
361 #endif
362+
363+#ifdef CONFIG_MTK_VENDOR
364+ struct {
365+ struct list_head csi_list;
366+ spinlock_t csi_lock;
367+ u32 count;
368+ bool mask;
369+ bool reorder;
370+ bool enable;
371+
372+ u32 interval;
373+ u32 last_record;
374+ } csi;
375+#endif
376 };
377
378 struct mt7915_dev {
developer9851a292022-12-15 17:33:43 +0800379@@ -674,6 +688,12 @@ void mt7915_sta_add_debugfs(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
developerc226de82022-10-03 12:24:57 +0800380 int mt7915_mmio_wed_init(struct mt7915_dev *dev, void *pdev_ptr,
381 bool pci, int *irq);
developere2cc0fa2022-03-29 17:31:03 +0800382
383+#ifdef CONFIG_MTK_VENDOR
384+void mt7915_vendor_register(struct mt7915_phy *phy);
385+int mt7915_mcu_set_csi(struct mt7915_phy *phy, u8 mode,
386+ u8 cfg, u8 v1, u32 v2, u8 *mac_addr);
387+#endif
388+
389 #ifdef MTK_DEBUG
390 int mt7915_mtk_init_debugfs(struct mt7915_phy *phy, struct dentry *dir);
391 int mt7915_dbg_mcu_wa_cmd(struct mt7915_dev *dev, int cmd, u32 a1, u32 a2, u32 a3, bool wait_resp);
392diff --git a/mt7915/vendor.c b/mt7915/vendor.c
393new file mode 100644
developer699cda22022-12-17 15:21:57 +0800394index 0000000..98fd9c2
developere2cc0fa2022-03-29 17:31:03 +0800395--- /dev/null
396+++ b/mt7915/vendor.c
397@@ -0,0 +1,452 @@
398+// SPDX-License-Identifier: ISC
399+/*
400+ * Copyright (C) 2020, MediaTek Inc. All rights reserved.
401+ */
402+
403+#include <net/netlink.h>
404+
405+#include "mt7915.h"
406+#include "mcu.h"
407+#include "vendor.h"
408+
409+static const struct nla_policy
410+csi_ctrl_policy[NUM_MTK_VENDOR_ATTRS_CSI_CTRL] = {
411+ [MTK_VENDOR_ATTR_CSI_CTRL_CFG] = {.type = NLA_NESTED },
412+ [MTK_VENDOR_ATTR_CSI_CTRL_CFG_MODE] = { .type = NLA_U8 },
413+ [MTK_VENDOR_ATTR_CSI_CTRL_CFG_TYPE] = { .type = NLA_U8 },
414+ [MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL1] = { .type = NLA_U8 },
415+ [MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL2] = { .type = NLA_U8 },
416+ [MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR] = { .type = NLA_NESTED },
417+ [MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL] = { .type = NLA_U32 },
418+ [MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM] = { .type = NLA_U16 },
419+ [MTK_VENDOR_ATTR_CSI_CTRL_DATA] = { .type = NLA_NESTED },
420+};
421+
422+struct csi_null_tone {
423+ u8 start;
424+ u8 end;
425+};
426+
427+struct csi_reorder{
428+ u8 dest;
429+ u8 start;
430+ u8 end;
431+};
432+
433+struct csi_mask {
434+ struct csi_null_tone null[10];
435+ u8 pilot[8];
436+ struct csi_reorder ro[3];
437+};
438+
439+static const struct csi_mask csi_mask_groups[] = {
440+ /* OFDM */
441+ { .null = { { 0 }, { 27, 37 } },
442+ .ro = { {0, 0, 63} },
443+ },
444+ { .null = { { 0, 69 }, { 96 }, { 123, 127 } },
445+ .ro = { { 0, 96 }, { 38, 70, 95 }, { 1, 97, 122 } },
446+ },
447+ { .null = { { 0, 5 }, { 32 }, { 59, 127 } },
448+ .ro = { { 0, 32 }, { 38, 6, 31 }, { 1, 33, 58 } },
449+ },
450+ { .null = { { 0, 5 }, { 32 }, { 59, 69 }, { 96 }, { 123, 127 } },
451+ .ro = { { 0, 0, 127 } },
452+ },
453+ { .null = { { 0, 133 }, { 160 }, { 187, 255 } },
454+ .ro = { { 0, 160 }, { 1, 161, 186 }, { 38, 134, 159 } },
455+ },
456+ { .null = { { 0, 197 }, { 224 }, { 251, 255 } },
457+ .ro = { { 0, 224 }, { 1, 225, 250 }, { 38, 198, 223 } },
458+ },
459+ { .null = { { 0, 5 }, { 32 }, { 59, 255 } },
460+ .ro = { { 0, 32 }, { 1, 33, 58 }, { 38, 6, 31 } },
461+ },
462+ { .null = { { 0, 69 }, { 96 }, { 123, 255 } },
463+ .ro = { { 0, 96 }, { 1, 97, 122 }, { 38, 70, 95 } },
464+ },
465+ { .null = { { 0, 133 }, { 160 }, { 187, 197 }, { 224 }, { 251, 255 } },
466+ .ro = { { 0, 192 }, { 2, 198, 250 }, { 74, 134, 186 } },
467+ },
468+ { .null = { { 0, 5 }, { 32 }, { 59, 69 }, { 96 }, { 123, 255 } },
469+ .ro = { { 0, 64 }, { 2, 70, 122 }, { 74, 6, 58 } },
470+ },
471+ { .null = { { 0, 5 }, { 32 }, { 59, 69 }, { 96 }, { 123, 133 },
472+ { 160 }, { 187, 197 }, { 224 }, { 251, 255 } },
473+ .ro = { { 0, 0, 255 } },
474+ },
475+
476+ /* HT/VHT */
477+ { .null = { { 0 }, { 29, 35 } },
478+ .pilot = { 7, 21, 43, 57 },
479+ .ro = { { 0, 0, 63 } },
480+ },
481+ { .null = { { 0, 67 }, { 96 }, { 125, 127 } },
482+ .pilot = { 75, 89, 103, 117 },
483+ .ro = { { 0, 96 }, { 36, 68, 95 }, { 1, 97, 124 } },
484+ },
485+ { .null = { { 0, 3 }, { 32 }, { 61, 127 } },
486+ .pilot = { 11, 25, 39, 53 },
487+ .ro = { { 0, 32 }, { 36, 4, 31 }, { 1, 33, 60 } },
488+ },
489+ { .null = { { 0, 1 }, { 59, 69 }, { 127 } },
490+ .pilot = { 11, 25, 53, 75, 103, 117 },
491+ .ro = { { 0, 0, 127 } },
492+ },
493+ { .null = { { 0, 131 }, { 160 }, { 189, 255 } },
494+ .pilot = { 139, 153, 167, 181 },
495+ .ro = { { 0, 160 }, { 1, 161, 188 }, { 36, 132, 159 } },
496+ },
497+ { .null = { { 0, 195 }, { 224 }, { 253 }, { 255 } },
498+ .pilot = { 203, 217, 231, 245 },
499+ .ro = { { 0, 224 }, { 1, 225, 252 }, { 36, 196, 223 } },
500+ },
501+ { .null = { { 0, 3 }, { 32 }, { 61, 255 } },
502+ .pilot = { 11, 25, 39, 53 },
503+ .ro = { { 0, 32 }, { 1, 33, 60 }, { 36, 4, 31 } },
504+ },
505+ { .null = { { 0, 67 }, { 96 }, { 125, 255 } },
506+ .pilot = { 75, 89, 103, 117 },
507+ .ro = { { 0, 96 }, { 1, 97, 124 }, { 36, 68, 95 } },
508+ },
509+ { .null = { { 0, 133 }, { 191, 193 }, { 251, 255 } },
510+ .pilot = { 139, 167, 181, 203, 217, 245 },
511+ .ro = { { 0, 192 }, { 2, 194, 250 }, { 70, 134, 190 } },
512+ },
513+ { .null = { { 0, 5 }, { 63, 65 }, { 123, 127 } },
514+ .pilot = { 11, 39, 53, 75, 89, 117 },
515+ .ro = { { 0, 64 }, { 2, 66, 122 }, { 70, 6, 62 } },
516+ },
517+ { .null = { { 0, 1 }, { 123, 133 }, { 255 } },
518+ .pilot = { 11, 39, 75, 103, 153, 181, 217, 245 },
519+ .ro = { { 0, 0, 255 } },
520+ },
521+
522+ /* HE */
523+ { .null = { { 0 }, { 31, 33 } },
524+ .pilot = { 12, 29, 35, 52 },
525+ .ro = { { 0, 0, 63 } },
526+ },
527+ { .null = { { 30, 34 }, { 96 } },
528+ .pilot = { 4, 21, 43, 60, 70, 87, 105, 122 },
529+ .ro = { { 0, 96 }, { 34, 66, 95 }, { 1, 97, 126 } },
530+ },
531+ { .null = { { 32 }, { 94, 98 } },
532+ .pilot = { 6, 23, 41, 58, 68, 85, 107, 124 },
533+ .ro = { { 0, 32 }, { 34, 2, 31 }, { 1, 31, 62 } },
534+ },
535+ { .null = { { 0 }, { 62, 66 } },
536+ .pilot = { 9, 26, 36, 53, 75, 92, 102, 119 },
537+ .ro = { { 0, 0, 127 } },
538+ },
539+ { .null = { { 30, 34 }, { 160 } },
540+ .pilot = { 4, 21, 43, 60, 137, 154, 166, 183 },
541+ .ro = { { 0, 160 }, { 1, 161, 190 }, { 34, 130, 159 } },
542+ },
543+ { .null = { { 94, 98 }, { 224 } },
544+ .pilot = { 68, 85, 107, 124, 201, 218, 230, 247 },
545+ .ro = { { 0, 224 }, { 1, 225, 254 }, { 34, 194, 223 } },
546+ },
547+ { .null = { { 32 }, { 158, 162 } },
548+ .pilot = { 9, 26, 38, 55, 132, 149, 171, 188 },
549+ .ro = { { 0, 32 }, { 1, 33, 62 }, { 34, 2, 31 } },
550+ },
551+ { .null = { { 96 }, { 222, 226 } },
552+ .pilot = { 73, 90, 102, 119, 196, 213, 235, 252 },
553+ .ro = { { 0, 96 }, { 1, 97, 126 }, { 34, 66, 95 } },
554+ },
555+ { .null = { { 62, 66 }, { 192 } },
556+ .pilot = { 36, 53, 75, 92, 169, 186, 198, 215 },
557+ .ro = { { 0, 192 }, { 1, 193, 253 }, { 67, 131, 191 } },
558+ },
559+ { .null = { { 64 }, { 190, 194 } },
560+ .pilot = { 41, 58, 70, 87, 164, 181, 203, 220 },
561+ .ro = { { 0, 64 }, { 1, 65, 125 }, { 67, 3, 63 } },
562+ },
563+ { .null = { { 0 }, { 126, 130 } },
564+ .pilot = { 6, 23, 100, 117, 139, 156, 233, 250 },
565+ .ro = { { 0, 0, 255 } },
566+ },
567+};
568+
569+static inline u8 csi_group_idx(u8 mode, u8 ch_bw, u8 data_bw, u8 pri_ch_idx)
570+{
571+ if (ch_bw < 2 || data_bw < 1)
572+ return mode * 11 + ch_bw * ch_bw + pri_ch_idx;
573+ else
574+ return mode * 11 + ch_bw * ch_bw + (data_bw + 1) * 2 + pri_ch_idx;
575+}
576+
577+static int mt7915_vendor_csi_ctrl(struct wiphy *wiphy,
578+ struct wireless_dev *wdev,
579+ const void *data,
580+ int data_len)
581+{
582+ struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
583+ struct mt7915_phy *phy = mt7915_hw_phy(hw);
584+ struct nlattr *tb[NUM_MTK_VENDOR_ATTRS_CSI_CTRL];
585+ int err;
586+
587+ err = nla_parse(tb, MTK_VENDOR_ATTR_CSI_CTRL_MAX, data, data_len,
588+ csi_ctrl_policy, NULL);
589+ if (err)
590+ return err;
591+
592+ if (tb[MTK_VENDOR_ATTR_CSI_CTRL_CFG]) {
593+ u8 mode = 0, type = 0, v1 = 0, v2 = 0;
594+ u8 mac_addr[ETH_ALEN] = {};
595+ struct nlattr *cur;
596+ int rem;
597+
598+ nla_for_each_nested(cur, tb[MTK_VENDOR_ATTR_CSI_CTRL_CFG], rem) {
599+ switch(nla_type(cur)) {
600+ case MTK_VENDOR_ATTR_CSI_CTRL_CFG_MODE:
601+ mode = nla_get_u8(cur);
602+ break;
603+ case MTK_VENDOR_ATTR_CSI_CTRL_CFG_TYPE:
604+ type = nla_get_u8(cur);
605+ break;
606+ case MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL1:
607+ v1 = nla_get_u8(cur);
608+ break;
609+ case MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL2:
610+ v2 = nla_get_u8(cur);
611+ break;
612+ default:
613+ return -EINVAL;
614+ };
615+ }
616+
617+ if (tb[MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR]) {
618+ int idx = 0;
619+
620+ nla_for_each_nested(cur, tb[MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR], rem) {
621+ mac_addr[idx++] = nla_get_u8(cur);
622+ }
623+ }
624+
625+ mt7915_mcu_set_csi(phy, mode, type, v1, v2, mac_addr);
626+
627+ spin_lock_bh(&phy->csi.csi_lock);
628+
629+ phy->csi.enable = !!mode;
630+
631+ if (mode == 2 && type == 5) {
632+ if (v1 >= 1)
633+ phy->csi.mask = 1;
634+ if (v1 == 2)
635+ phy->csi.reorder = 1;
636+ }
637+
638+ /* clean up old csi stats */
639+ if ((mode == 0 || mode == 2) && !list_empty(&phy->csi.csi_list)) {
640+ struct csi_data *c, *tmp_c;
641+
642+ list_for_each_entry_safe(c, tmp_c, &phy->csi.csi_list,
643+ node) {
644+ list_del(&c->node);
645+ kfree(c);
646+ phy->csi.count--;
647+ }
648+ } else if (mode == 1) {
649+ phy->csi.last_record = 0;
650+ }
651+
652+ spin_unlock_bh(&phy->csi.csi_lock);
653+ }
654+
655+ if (tb[MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL])
656+ phy->csi.interval = nla_get_u32(tb[MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL]);
657+
658+ return 0;
659+}
660+
661+static void
662+mt7915_vendor_csi_tone_mask(struct mt7915_phy *phy, struct csi_data *csi)
663+{
664+ static const u8 mode_map[] = {
665+ [MT_PHY_TYPE_OFDM] = 0,
666+ [MT_PHY_TYPE_HT] = 1,
667+ [MT_PHY_TYPE_VHT] = 1,
668+ [MT_PHY_TYPE_HE_SU] = 2,
669+ };
670+ const struct csi_mask *cmask;
671+ int i;
672+
673+ if (csi->rx_mode == MT_PHY_TYPE_CCK || !phy->csi.mask)
674+ return;
675+
676+ if (csi->data_bw == IEEE80211_STA_RX_BW_40)
677+ csi->pri_ch_idx /= 2;
678+
679+ cmask = &csi_mask_groups[csi_group_idx(mode_map[csi->rx_mode],
680+ csi->ch_bw,
681+ csi->data_bw,
682+ csi->pri_ch_idx)];
683+
684+ for (i = 0; i < 10; i++) {
685+ const struct csi_null_tone *ntone = &cmask->null[i];
686+ u8 start = ntone->start;
687+ u8 end = ntone->end;
688+ int j;
689+
690+ if (!start && !end && i > 0)
691+ break;
692+
693+ if (!end)
694+ end = start;
695+
696+ for (j = start; j <= end; j++) {
697+ csi->data_i[j] = 0;
698+ csi->data_q[j] = 0;
699+ }
700+ }
701+
702+ for (i = 0; i < 8; i++) {
703+ u8 pilot = cmask->pilot[i];
704+
705+ if (!pilot)
706+ break;
707+
708+ csi->data_i[pilot] = 0;
709+ csi->data_q[pilot] = 0;
710+ }
711+
712+ if (!phy->csi.reorder)
713+ return;
714+
715+ for (i = 0; i < 3; i++) {
716+ const struct csi_reorder *ro = &cmask->ro[i];
717+ u8 dest = ro->dest;
718+ u8 start = ro->start;
719+ u8 end = ro->end;
720+
721+ if (!dest && !start && !end)
722+ break;
723+
724+ if (dest == start)
725+ continue;
726+
727+ if (end) {
728+ memmove(&csi->data_i[dest], &csi->data_i[start],
729+ end - start + 1);
730+ memmove(&csi->data_q[dest], &csi->data_q[start],
731+ end - start + 1);
732+ } else {
733+ csi->data_i[dest] = csi->data_i[start];
734+ csi->data_q[dest] = csi->data_q[start];
735+ }
736+ }
737+}
738+
739+static int
740+mt7915_vendor_csi_ctrl_dump(struct wiphy *wiphy, struct wireless_dev *wdev,
741+ struct sk_buff *skb, const void *data, int data_len,
742+ unsigned long *storage)
743+{
744+#define RESERVED_SET BIT(31)
745+ struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
746+ struct mt7915_phy *phy = mt7915_hw_phy(hw);
747+ struct nlattr *tb[NUM_MTK_VENDOR_ATTRS_CSI_CTRL];
748+ int err = 0;
749+
750+ if (*storage & RESERVED_SET) {
751+ if ((*storage & GENMASK(15, 0)) == 0)
752+ return -ENOENT;
753+ (*storage)--;
754+ }
755+
756+ if (data) {
757+ err = nla_parse(tb, MTK_VENDOR_ATTR_CSI_CTRL_MAX, data, data_len,
758+ csi_ctrl_policy, NULL);
759+ if (err)
760+ return err;
761+ }
762+
763+ if (!(*storage & RESERVED_SET) && tb[MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM]) {
764+ *storage = nla_get_u16(tb[MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM]);
765+ *storage |= RESERVED_SET;
766+ }
767+
768+ spin_lock_bh(&phy->csi.csi_lock);
769+
770+ if (!list_empty(&phy->csi.csi_list)) {
771+ struct csi_data *csi;
772+ void *a, *b;
773+ int i;
774+
775+ csi = list_first_entry(&phy->csi.csi_list, struct csi_data, node);
776+
777+ mt7915_vendor_csi_tone_mask(phy, csi);
778+
779+ a = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_CTRL_DATA);
780+
781+ if (nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_VER, 1) ||
782+ nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_RSSI, csi->rssi) ||
783+ nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_SNR, csi->snr) ||
784+ nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_BW, csi->data_bw) ||
785+ nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_CH_IDX, csi->pri_ch_idx) ||
786+ nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_MODE, csi->rx_mode))
787+ goto out;
788+
789+ if (nla_put_u16(skb, MTK_VENDOR_ATTR_CSI_DATA_TX_ANT, csi->tx_idx) ||
790+ nla_put_u16(skb, MTK_VENDOR_ATTR_CSI_DATA_RX_ANT, csi->rx_idx))
791+ goto out;
792+
793+ if (nla_put_u32(skb, MTK_VENDOR_ATTR_CSI_DATA_INFO, csi->info) ||
794+ nla_put_u32(skb, MTK_VENDOR_ATTR_CSI_DATA_H_IDX, csi->h_idx) ||
795+ nla_put_u32(skb, MTK_VENDOR_ATTR_CSI_DATA_TS, csi->ts))
796+ goto out;
797+
798+ b = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_DATA_TA);
799+ for (i = 0; i < ARRAY_SIZE(csi->ta); i++)
800+ if (nla_put_u8(skb, i, csi->ta[i]))
801+ goto out;
802+ nla_nest_end(skb, b);
803+
804+ b = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_DATA_I);
805+ for (i = 0; i < ARRAY_SIZE(csi->data_i); i++)
806+ if (nla_put_u16(skb, i, csi->data_i[i]))
807+ goto out;
808+ nla_nest_end(skb, b);
809+
810+ b = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_DATA_Q);
811+ for (i = 0; i < ARRAY_SIZE(csi->data_q); i++)
812+ if (nla_put_u16(skb, i, csi->data_q[i]))
813+ goto out;
814+ nla_nest_end(skb, b);
815+
816+ nla_nest_end(skb, a);
817+
818+ list_del(&csi->node);
819+ kfree(csi);
820+ phy->csi.count--;
821+
822+ err = phy->csi.count;
823+ }
824+out:
825+ spin_unlock_bh(&phy->csi.csi_lock);
826+
827+ return err;
828+}
829+
830+static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
831+ {
832+ .info = {
833+ .vendor_id = MTK_NL80211_VENDOR_ID,
834+ .subcmd = MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL,
835+ },
836+ .flags = WIPHY_VENDOR_CMD_NEED_NETDEV |
837+ WIPHY_VENDOR_CMD_NEED_RUNNING,
838+ .doit = mt7915_vendor_csi_ctrl,
839+ .dumpit = mt7915_vendor_csi_ctrl_dump,
840+ .policy = csi_ctrl_policy,
841+ .maxattr = MTK_VENDOR_ATTR_CSI_CTRL_MAX,
842+ }
843+};
844+
845+void mt7915_vendor_register(struct mt7915_phy *phy)
846+{
847+ phy->mt76->hw->wiphy->vendor_commands = mt7915_vendor_commands;
848+ phy->mt76->hw->wiphy->n_vendor_commands = ARRAY_SIZE(mt7915_vendor_commands);
849+}
850diff --git a/mt7915/vendor.h b/mt7915/vendor.h
851new file mode 100644
developer699cda22022-12-17 15:21:57 +0800852index 0000000..9d3db2a
developere2cc0fa2022-03-29 17:31:03 +0800853--- /dev/null
854+++ b/mt7915/vendor.h
855@@ -0,0 +1,60 @@
856+#ifndef __MT7915_VENDOR_H
857+#define __MT7915_VENDOR_H
858+
859+#define MTK_NL80211_VENDOR_ID 0x0ce7
860+
861+enum mtk_nl80211_vendor_subcmds {
862+ MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL = 0xc2,
863+};
864+
865+enum mtk_vendor_attr_csi_ctrl {
866+ MTK_VENDOR_ATTR_CSI_CTRL_UNSPEC,
867+
868+ MTK_VENDOR_ATTR_CSI_CTRL_CFG,
869+ MTK_VENDOR_ATTR_CSI_CTRL_CFG_MODE,
870+ MTK_VENDOR_ATTR_CSI_CTRL_CFG_TYPE,
871+ MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL1,
872+ MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL2,
873+ MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR,
874+ MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL,
875+
876+ MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM,
877+
878+ MTK_VENDOR_ATTR_CSI_CTRL_DATA,
879+
880+ /* keep last */
881+ NUM_MTK_VENDOR_ATTRS_CSI_CTRL,
882+ MTK_VENDOR_ATTR_CSI_CTRL_MAX =
883+ NUM_MTK_VENDOR_ATTRS_CSI_CTRL - 1
884+};
885+
886+enum mtk_vendor_attr_csi_data {
887+ MTK_VENDOR_ATTR_CSI_DATA_UNSPEC,
888+ MTK_VENDOR_ATTR_CSI_DATA_PAD,
889+
890+ MTK_VENDOR_ATTR_CSI_DATA_VER,
891+ MTK_VENDOR_ATTR_CSI_DATA_TS,
892+ MTK_VENDOR_ATTR_CSI_DATA_RSSI,
893+ MTK_VENDOR_ATTR_CSI_DATA_SNR,
894+ MTK_VENDOR_ATTR_CSI_DATA_BW,
895+ MTK_VENDOR_ATTR_CSI_DATA_CH_IDX,
896+ MTK_VENDOR_ATTR_CSI_DATA_TA,
897+ MTK_VENDOR_ATTR_CSI_DATA_I,
898+ MTK_VENDOR_ATTR_CSI_DATA_Q,
899+ MTK_VENDOR_ATTR_CSI_DATA_INFO,
900+ MTK_VENDOR_ATTR_CSI_DATA_RSVD1,
901+ MTK_VENDOR_ATTR_CSI_DATA_RSVD2,
902+ MTK_VENDOR_ATTR_CSI_DATA_RSVD3,
903+ MTK_VENDOR_ATTR_CSI_DATA_RSVD4,
904+ MTK_VENDOR_ATTR_CSI_DATA_TX_ANT,
905+ MTK_VENDOR_ATTR_CSI_DATA_RX_ANT,
906+ MTK_VENDOR_ATTR_CSI_DATA_MODE,
907+ MTK_VENDOR_ATTR_CSI_DATA_H_IDX,
908+
909+ /* keep last */
910+ NUM_MTK_VENDOR_ATTRS_CSI_DATA,
911+ MTK_VENDOR_ATTR_CSI_DATA_MAX =
912+ NUM_MTK_VENDOR_ATTRS_CSI_DATA - 1
913+};
914+
915+#endif
916--
developer699cda22022-12-17 15:21:57 +08009172.36.1
developere2cc0fa2022-03-29 17:31:03 +0800918