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