blob: 654e632702ceb207cc3a633567367f90972164ee [file] [log] [blame]
developer1a173672023-12-21 14:49:33 +08001From 799948d6f5dc16251034621b69a48bdf6f30a197 Mon Sep 17 00:00:00 2001
developer73e5a572022-04-19 10:21:20 +08002From: Bo Jiao <Bo.Jiao@mediatek.com>
developer6caa5e22022-06-16 13:33:13 +08003Date: Mon, 6 Jun 2022 20:13:02 +0800
developer1a173672023-12-21 14:49:33 +08004Subject: [PATCH 13/76] wifi: mt76: mt7915: csi: implement csi support
developer73e5a572022-04-19 10:21:20 +08005
6---
developer28b11e22022-09-05 19:09:45 +08007 mt76_connac_mcu.h | 2 +
8 mt7915/Makefile | 4 +-
developer1a173672023-12-21 14:49:33 +08009 mt7915/init.c | 38 ++++
10 mt7915/main.c | 4 +
11 mt7915/mcu.c | 203 ++++++++++++++++++++
12 mt7915/mcu.h | 74 ++++++++
13 mt7915/mt7915.h | 60 ++++++
14 mt7915/vendor.c | 470 ++++++++++++++++++++++++++++++++++++++++++++++
15 mt7915/vendor.h | 63 +++++++
16 9 files changed, 916 insertions(+), 2 deletions(-)
developer28b11e22022-09-05 19:09:45 +080017 create mode 100644 mt7915/vendor.c
18 create mode 100644 mt7915/vendor.h
developer73e5a572022-04-19 10:21:20 +080019
20diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
developer1a173672023-12-21 14:49:33 +080021index d28ee91..b31f19a 100644
developer73e5a572022-04-19 10:21:20 +080022--- a/mt76_connac_mcu.h
23+++ b/mt76_connac_mcu.h
developer1a173672023-12-21 14:49:33 +080024@@ -1017,6 +1017,7 @@ enum {
developer47efbdb2023-06-29 20:33:22 +080025 MCU_EXT_EVENT_WA_TX_STAT = 0x74,
developer73e5a572022-04-19 10:21:20 +080026 MCU_EXT_EVENT_BCC_NOTIFY = 0x75,
27 MCU_EXT_EVENT_MURU_CTRL = 0x9f,
28+ MCU_EXT_EVENT_CSI_REPORT = 0xc2,
29 };
30
developer3609d782022-11-29 18:07:22 +080031 /* unified event table */
developer1a173672023-12-21 14:49:33 +080032@@ -1228,6 +1229,7 @@ enum {
developer73e5a572022-04-19 10:21:20 +080033 MCU_EXT_CMD_GROUP_PRE_CAL_INFO = 0xab,
34 MCU_EXT_CMD_DPD_PRE_CAL_INFO = 0xac,
35 MCU_EXT_CMD_PHY_STAT_INFO = 0xad,
36+ MCU_EXT_CMD_CSI_CTRL = 0xc2,
37 };
38
39 enum {
40diff --git a/mt7915/Makefile b/mt7915/Makefile
developerbd9fa1e2023-10-16 11:04:00 +080041index fd71141..65129b4 100644
developer73e5a572022-04-19 10:21:20 +080042--- a/mt7915/Makefile
43+++ b/mt7915/Makefile
developer60a3d662023-02-07 15:24:34 +080044@@ -1,10 +1,10 @@
developer73e5a572022-04-19 10:21:20 +080045 # SPDX-License-Identifier: ISC
developer60a3d662023-02-07 15:24:34 +080046
47-EXTRA_CFLAGS += -DCONFIG_MT76_LEDS
48+EXTRA_CFLAGS += -DCONFIG_MT76_LEDS -DCONFIG_MTK_VENDOR
developer73e5a572022-04-19 10:21:20 +080049 obj-$(CONFIG_MT7915E) += mt7915e.o
50
51 mt7915e-y := pci.o init.o dma.o eeprom.o main.o mcu.o mac.o \
52- debugfs.o mmio.o mtk_debugfs.o mtk_mcu.o
53+ debugfs.o mmio.o mtk_debugfs.o mtk_mcu.o vendor.o
54
55 mt7915e-$(CONFIG_NL80211_TESTMODE) += testmode.o
developer7af0f762023-05-22 15:16:16 +080056 mt7915e-$(CONFIG_MT798X_WMAC) += soc.o
developer73e5a572022-04-19 10:21:20 +080057diff --git a/mt7915/init.c b/mt7915/init.c
developer1a173672023-12-21 14:49:33 +080058index d5d9cbf..d4bf09c 100644
developer73e5a572022-04-19 10:21:20 +080059--- a/mt7915/init.c
60+++ b/mt7915/init.c
developerbd9fa1e2023-10-16 11:04:00 +080061@@ -696,6 +696,12 @@ mt7915_register_ext_phy(struct mt7915_dev *dev, struct mt7915_phy *phy)
developer73e5a572022-04-19 10:21:20 +080062 /* init wiphy according to mphy and phy */
developer17bb0a82022-12-13 15:52:04 +080063 mt7915_init_wiphy(phy);
developer73e5a572022-04-19 10:21:20 +080064
65+#ifdef CONFIG_MTK_VENDOR
66+ INIT_LIST_HEAD(&phy->csi.csi_list);
67+ spin_lock_init(&phy->csi.csi_lock);
68+ mt7915_vendor_register(phy);
69+#endif
70+
71 ret = mt76_register_phy(mphy, true, mt76_rates,
72 ARRAY_SIZE(mt76_rates));
73 if (ret)
developer1a173672023-12-21 14:49:33 +080074@@ -1173,6 +1179,24 @@ void mt7915_set_stream_he_caps(struct mt7915_phy *phy)
developer73e5a572022-04-19 10:21:20 +080075 }
76 }
77
78+#ifdef CONFIG_MTK_VENDOR
79+static int mt7915_unregister_features(struct mt7915_phy *phy)
80+{
81+ struct csi_data *c, *tmp_c;
82+
83+ spin_lock_bh(&phy->csi.csi_lock);
84+ phy->csi.enable = 0;
85+
86+ list_for_each_entry_safe(c, tmp_c, &phy->csi.csi_list, node) {
87+ list_del(&c->node);
88+ kfree(c);
89+ }
90+ spin_unlock_bh(&phy->csi.csi_lock);
91+
developer73e5a572022-04-19 10:21:20 +080092+ 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);
developer1a173672023-12-21 14:49:33 +080099@@ -1181,6 +1205,10 @@ static void mt7915_unregister_ext_phy(struct mt7915_dev *dev)
developer73e5a572022-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);
developer1a173672023-12-21 14:49:33 +0800110@@ -1193,6 +1221,10 @@ static void mt7915_stop_hardware(struct mt7915_dev *dev)
developer73e5a572022-04-19 10:21:20 +0800111 mt7915_dma_cleanup(dev);
developer7af0f762023-05-22 15:16:16 +0800112 tasklet_disable(&dev->mt76.irq_tasklet);
developer73e5a572022-04-19 10:21:20 +0800113
114+#ifdef CONFIG_MTK_VENDOR
115+ mt7915_unregister_features(&dev->phy);
116+#endif
117+
developer7af0f762023-05-22 15:16:16 +0800118 if (is_mt798x(&dev->mt76))
developer73e5a572022-04-19 10:21:20 +0800119 mt7986_wmac_disable(dev);
120 }
developer1a173672023-12-21 14:49:33 +0800121@@ -1233,6 +1265,12 @@ int mt7915_register_device(struct mt7915_dev *dev)
developer73e5a572022-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+
developer17bb0a82022-12-13 15:52:04 +0800131 ret = mt76_register_device(&dev->mt76, true, mt76_rates,
132 ARRAY_SIZE(mt76_rates));
133 if (ret)
developer1a173672023-12-21 14:49:33 +0800134diff --git a/mt7915/main.c b/mt7915/main.c
135index 38ffd90..a4baa3d 100644
136--- a/mt7915/main.c
137+++ b/mt7915/main.c
138@@ -784,6 +784,10 @@ void mt7915_mac_sta_remove(struct mt76_dev *mdev, struct ieee80211_vif *vif,
139 struct mt7915_phy *phy = msta->vif->phy;
140 int i;
141
142+#ifdef CONFIG_MTK_VENDOR
143+ mt7915_mcu_set_csi(&dev->phy, 2, 8, 1, 0, sta->addr, 0);
144+#endif
145+
146 mt7915_mcu_add_sta(dev, vif, sta, false);
147
148 mt7915_mac_wtbl_update(dev, msta->wcid.idx,
developer73e5a572022-04-19 10:21:20 +0800149diff --git a/mt7915/mcu.c b/mt7915/mcu.c
developer1a173672023-12-21 14:49:33 +0800150index 1cb462f..3202c4e 100644
developer73e5a572022-04-19 10:21:20 +0800151--- a/mt7915/mcu.c
152+++ b/mt7915/mcu.c
developer7af0f762023-05-22 15:16:16 +0800153@@ -40,6 +40,10 @@ static bool sr_scene_detect = true;
developer3609d782022-11-29 18:07:22 +0800154 module_param(sr_scene_detect, bool, 0644);
155 MODULE_PARM_DESC(sr_scene_detect, "Enable firmware scene detection algorithm");
developer73e5a572022-04-19 10:21:20 +0800156
157+#ifdef CONFIG_MTK_VENDOR
158+static int mt7915_mcu_report_csi(struct mt7915_dev *dev, struct sk_buff *skb);
159+#endif
160+
161 static u8
162 mt7915_mcu_get_sta_nss(u16 mcs_map)
163 {
developerbd9fa1e2023-10-16 11:04:00 +0800164@@ -379,6 +383,11 @@ mt7915_mcu_rx_ext_event(struct mt7915_dev *dev, struct sk_buff *skb)
developer73e5a572022-04-19 10:21:20 +0800165 case MCU_EXT_EVENT_FW_LOG_2_HOST:
166 mt7915_mcu_rx_log_message(dev, skb);
167 break;
168+#ifdef CONFIG_MTK_VENDOR
169+ case MCU_EXT_EVENT_CSI_REPORT:
170+ mt7915_mcu_report_csi(dev, skb);
171+ break;
172+#endif
173 case MCU_EXT_EVENT_BCC_NOTIFY:
174 mt7915_mcu_rx_bcc_notify(dev, skb);
175 break;
developer1a173672023-12-21 14:49:33 +0800176@@ -4024,6 +4033,200 @@ out:
developer47efbdb2023-06-29 20:33:22 +0800177 return ret;
developer73e5a572022-04-19 10:21:20 +0800178 }
179
180+#ifdef CONFIG_MTK_VENDOR
181+int mt7915_mcu_set_csi(struct mt7915_phy *phy, u8 mode,
developer1a173672023-12-21 14:49:33 +0800182+ u8 cfg, u8 v1, u32 v2, u8 *mac_addr, u32 sta_interval)
developer73e5a572022-04-19 10:21:20 +0800183+{
184+ struct mt7915_dev *dev = phy->dev;
185+ struct mt7915_mcu_csi req = {
186+ .band = phy != &dev->phy,
187+ .mode = mode,
188+ .cfg = cfg,
189+ .v1 = v1,
190+ .v2 = cpu_to_le32(v2),
191+ };
192+
developer1a173672023-12-21 14:49:33 +0800193+ if (is_valid_ether_addr(mac_addr)) {
developer73e5a572022-04-19 10:21:20 +0800194+ ether_addr_copy(req.mac_addr, mac_addr);
195+
developer1a173672023-12-21 14:49:33 +0800196+ if (req.v2 == 1 && sta_interval)
197+ req.sta_interval = sta_interval;
198+ }
199+
developer73e5a572022-04-19 10:21:20 +0800200+ return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(CSI_CTRL), &req,
developer1a173672023-12-21 14:49:33 +0800201+ sizeof(req), true);
developer73e5a572022-04-19 10:21:20 +0800202+}
203+
developer1a173672023-12-21 14:49:33 +0800204+static int csi_integret_segment_data(struct mt7915_phy *phy, struct csi_data *csi)
205+{
206+ struct csi_data *csi_temp = NULL;
207+
208+ if (csi->segment_num == 0 &&
209+ csi->remain_last == 0)
210+ return CSI_CHAIN_COMPLETE;
211+ else if (csi->segment_num == 0 &&
212+ csi->remain_last == 1) {
213+ memcpy(&phy->csi.buffered_csi,
214+ csi, sizeof(struct csi_data));
215+
216+ return CSI_CHAIN_SEGMENT_FIRST;
217+ } else if (csi->segment_num != 0) {
218+ csi_temp = &phy->csi.buffered_csi;
219+ if (csi->chain_info !=
220+ csi_temp->chain_info ||
221+ csi->segment_num !=
222+ (csi_temp->segment_num + 1))
223+ return CSI_CHAIN_SEGMENT_ERR;
224+
225+ memcpy(&csi_temp->data_i[csi_temp->data_num],
226+ csi->data_i, csi->data_num * sizeof(s16));
227+
228+ memcpy(&csi_temp->data_q[csi_temp->data_num],
229+ csi->data_q, csi->data_num * sizeof(s16));
230+
231+ csi_temp->data_num += csi->data_num;
232+ csi_temp->segment_num = csi->segment_num;
233+ csi_temp->remain_last = csi->remain_last;
234+
235+ if (csi->remain_last == 0)
236+ return CSI_CHAIN_SEGMENT_LAST;
237+ else if (csi->remain_last == 1)
238+ return CSI_CHAIN_SEGMENT_MIDDLE;
239+ }
240+
241+ return CSI_CHAIN_ERR;
242+}
243+
developer73e5a572022-04-19 10:21:20 +0800244+static int
245+mt7915_mcu_report_csi(struct mt7915_dev *dev, struct sk_buff *skb)
246+{
developer7c3a5082022-06-24 13:40:42 +0800247+ struct mt76_connac2_mcu_rxd *rxd = (struct mt76_connac2_mcu_rxd *)skb->data;
developer73e5a572022-04-19 10:21:20 +0800248+ struct mt7915_phy *phy = &dev->phy;
developer73e5a572022-04-19 10:21:20 +0800249+ int len, i;
developer1a173672023-12-21 14:49:33 +0800250+ struct mt7915_mcu_csi_report *cr;
251+ int ret;
252+ struct csi_data *current_csi = NULL;
253+ struct csi_data *target_csi = NULL;
developer73e5a572022-04-19 10:21:20 +0800254+
developer7c3a5082022-06-24 13:40:42 +0800255+ skb_pull(skb, sizeof(struct mt76_connac2_mcu_rxd));
developer73e5a572022-04-19 10:21:20 +0800256+
developer7c3a5082022-06-24 13:40:42 +0800257+ len = le16_to_cpu(rxd->len) - sizeof(struct mt76_connac2_mcu_rxd) + 24;
developer73e5a572022-04-19 10:21:20 +0800258+
259+ cr = (struct mt7915_mcu_csi_report *)skb->data;
260+
261+ if (phy->csi.interval &&
262+ le32_to_cpu(cr->ts) < phy->csi.last_record + phy->csi.interval)
263+ return 0;
264+
developer1a173672023-12-21 14:49:33 +0800265+ current_csi = kzalloc(sizeof(*current_csi), GFP_KERNEL);
266+
267+ if (!current_csi)
developer73e5a572022-04-19 10:21:20 +0800268+ return -ENOMEM;
269+
developer1a173672023-12-21 14:49:33 +0800270+ memset(current_csi, 0, sizeof(struct csi_data));
271+
272+#define SET_CSI_DATA(_field) (current_csi->_field = le32_to_cpu((cr->_field)))
developer73e5a572022-04-19 10:21:20 +0800273+ SET_CSI_DATA(ch_bw);
274+ SET_CSI_DATA(rssi);
275+ SET_CSI_DATA(snr);
276+ SET_CSI_DATA(data_num);
277+ SET_CSI_DATA(data_bw);
278+ SET_CSI_DATA(pri_ch_idx);
developer1a173672023-12-21 14:49:33 +0800279+ SET_CSI_DATA(ext_info);
developer73e5a572022-04-19 10:21:20 +0800280+ SET_CSI_DATA(rx_mode);
developer1a173672023-12-21 14:49:33 +0800281+ SET_CSI_DATA(chain_info);
developer73e5a572022-04-19 10:21:20 +0800282+ SET_CSI_DATA(ts);
283+
developer1a173672023-12-21 14:49:33 +0800284+ if (is_mt798x(&dev->mt76) || is_mt7916(&dev->mt76)) {
285+ SET_CSI_DATA(segment_num);
286+ SET_CSI_DATA(remain_last);
287+ SET_CSI_DATA(pkt_sn);
288+ SET_CSI_DATA(tr_stream);
289+ }
290+
developer73e5a572022-04-19 10:21:20 +0800291+ SET_CSI_DATA(band);
developer1a173672023-12-21 14:49:33 +0800292+ if (current_csi->band && !phy->mt76->band_idx)
developer73e5a572022-04-19 10:21:20 +0800293+ phy = mt7915_ext_phy(dev);
294+#undef SET_CSI_DATA
295+
developer1a173672023-12-21 14:49:33 +0800296+ switch (current_csi->ch_bw) {
297+ case CSI_BW20:
298+ if (is_mt798x(&dev->mt76) || is_mt7916(&dev->mt76))
299+ current_csi->data_num = CSI_BW20_DATA_COUNT;
300+ break;
301+ case CSI_BW40:
302+ if (is_mt798x(&dev->mt76) || is_mt7916(&dev->mt76))
303+ current_csi->data_num = CSI_BW40_DATA_COUNT;
304+ break;
305+ default:
306+ break;
developer73e5a572022-04-19 10:21:20 +0800307+ }
308+
developer1a173672023-12-21 14:49:33 +0800309+ for (i = 0; i < current_csi->data_num; i++) {
310+ current_csi->data_i[i] = le16_to_cpu(cr->data_i[i]);
311+ current_csi->data_q[i] = le16_to_cpu(cr->data_q[i]);
312+ }
313+
314+ memcpy(current_csi->ta, cr->ta, ETH_ALEN);
315+ current_csi->tx_idx = le32_get_bits(cr->trx_idx, GENMASK(31, 16));
316+ current_csi->rx_idx = le32_get_bits(cr->trx_idx, GENMASK(15, 0));
317+
318+ /* integret the bw80 segment */
319+ if ((is_mt798x(&dev->mt76) || is_mt7916(&dev->mt76)) && current_csi->ch_bw >= CSI_BW80) {
320+ ret = csi_integret_segment_data(phy, current_csi);
developer73e5a572022-04-19 10:21:20 +0800321+
developer1a173672023-12-21 14:49:33 +0800322+ /* event data error or event drop */
323+ if (ret == CSI_CHAIN_ERR || ret == CSI_CHAIN_SEGMENT_ERR) {
324+ kfree(current_csi);
325+ return -EINVAL;
326+ }
327+
328+ if (ret == CSI_CHAIN_SEGMENT_FIRST || ret == CSI_CHAIN_SEGMENT_MIDDLE) {
329+ kfree(current_csi);
330+ return 0;
331+ } else if (ret == CSI_CHAIN_COMPLETE) {
332+ target_csi = current_csi;
333+ } else if (ret == CSI_CHAIN_SEGMENT_LAST) {
334+ target_csi = current_csi;
335+ memcpy(target_csi, &phy->csi.buffered_csi, sizeof(struct csi_data));
336+ memset(&phy->csi.buffered_csi, 0, sizeof(struct csi_data));
337+ }
338+ } else {
339+ target_csi = current_csi;
340+ }
341+
342+ /* put the csi data into list */
343+ INIT_LIST_HEAD(&target_csi->node);
developer73e5a572022-04-19 10:21:20 +0800344+ spin_lock_bh(&phy->csi.csi_lock);
345+
346+ if (!phy->csi.enable) {
developer1a173672023-12-21 14:49:33 +0800347+ kfree(target_csi);
developer73e5a572022-04-19 10:21:20 +0800348+ spin_unlock_bh(&phy->csi.csi_lock);
349+ return 0;
350+ }
351+
developer1a173672023-12-21 14:49:33 +0800352+ list_add_tail(&target_csi->node, &phy->csi.csi_list);
developer73e5a572022-04-19 10:21:20 +0800353+ phy->csi.count++;
354+
355+ if (phy->csi.count > CSI_MAX_BUF_NUM) {
356+ struct csi_data *old;
357+
358+ old = list_first_entry(&phy->csi.csi_list,
359+ struct csi_data, node);
360+
361+ list_del(&old->node);
362+ kfree(old);
363+ phy->csi.count--;
364+ }
365+
developer1a173672023-12-21 14:49:33 +0800366+ if (target_csi->chain_info & BIT(15)) /* last chain */
367+ phy->csi.last_record = target_csi->ts;
developer73e5a572022-04-19 10:21:20 +0800368+ spin_unlock_bh(&phy->csi.csi_lock);
369+
370+ return 0;
371+}
372+#endif
373+
374 #ifdef MTK_DEBUG
375 int mt7915_dbg_mcu_wa_cmd(struct mt7915_dev *dev, int cmd, u32 a1, u32 a2, u32 a3, bool wait_resp)
376 {
377diff --git a/mt7915/mcu.h b/mt7915/mcu.h
developer1a173672023-12-21 14:49:33 +0800378index dd3b506..09aa7ec 100644
developer73e5a572022-04-19 10:21:20 +0800379--- a/mt7915/mcu.h
380+++ b/mt7915/mcu.h
developer1a173672023-12-21 14:49:33 +0800381@@ -589,4 +589,78 @@ mt7915_get_power_bound(struct mt7915_phy *phy, s8 txpower)
developer8effbd32023-04-17 15:57:28 +0800382 enum {
383 MCU_GET_TX_RATE = 4
384 };
385+
developer73e5a572022-04-19 10:21:20 +0800386+#ifdef CONFIG_MTK_VENDOR
387+struct mt7915_mcu_csi {
388+ u8 band;
389+ u8 mode;
390+ u8 cfg;
391+ u8 v1;
392+ __le32 v2;
393+ u8 mac_addr[ETH_ALEN];
developer1a173672023-12-21 14:49:33 +0800394+ u8 _rsv1[2];
395+ u32 sta_interval;
396+ u8 _rsv2[28];
developer73e5a572022-04-19 10:21:20 +0800397+} __packed;
398+
399+struct csi_tlv {
400+ __le32 tag;
401+ __le32 len;
402+} __packed;
403+
developer73e5a572022-04-19 10:21:20 +0800404+#define CSI_MAX_BUF_NUM 3000
405+
406+struct mt7915_mcu_csi_report {
407+ struct csi_tlv _t0;
408+ __le32 ver;
409+ struct csi_tlv _t1;
410+ __le32 ch_bw;
411+ struct csi_tlv _t2;
412+ __le32 rssi;
413+ struct csi_tlv _t3;
414+ __le32 snr;
415+ struct csi_tlv _t4;
416+ __le32 band;
417+ struct csi_tlv _t5;
418+ __le32 data_num;
419+ struct csi_tlv _t6;
developer1a173672023-12-21 14:49:33 +0800420+ __le16 data_i[CSI_BW80_DATA_COUNT];
developer73e5a572022-04-19 10:21:20 +0800421+ struct csi_tlv _t7;
developer1a173672023-12-21 14:49:33 +0800422+ __le16 data_q[CSI_BW80_DATA_COUNT];
developer73e5a572022-04-19 10:21:20 +0800423+ struct csi_tlv _t8;
424+ __le32 data_bw;
425+ struct csi_tlv _t9;
426+ __le32 pri_ch_idx;
427+ struct csi_tlv _t10;
428+ u8 ta[8];
429+ struct csi_tlv _t11;
developer1a173672023-12-21 14:49:33 +0800430+ __le32 ext_info;
developer73e5a572022-04-19 10:21:20 +0800431+ struct csi_tlv _t12;
432+ __le32 rx_mode;
433+ struct csi_tlv _t17;
developer1a173672023-12-21 14:49:33 +0800434+ __le32 chain_info;
developer73e5a572022-04-19 10:21:20 +0800435+ struct csi_tlv _t18;
436+ __le32 trx_idx;
437+ struct csi_tlv _t19;
438+ __le32 ts;
developer1a173672023-12-21 14:49:33 +0800439+ struct csi_tlv _t20;
440+ __le32 pkt_sn;
441+ struct csi_tlv _t21;
442+ __le32 segment_num;
443+ struct csi_tlv _t22;
444+ __le32 remain_last;
445+ struct csi_tlv _t23;
446+ __le32 tr_stream;
developer73e5a572022-04-19 10:21:20 +0800447+} __packed;
448+
developer1a173672023-12-21 14:49:33 +0800449+enum CSI_CHAIN_TYPE {
450+ CSI_CHAIN_ERR,
451+ CSI_CHAIN_COMPLETE,
452+ CSI_CHAIN_SEGMENT_FIRST,
453+ CSI_CHAIN_SEGMENT_MIDDLE,
454+ CSI_CHAIN_SEGMENT_LAST,
455+ CSI_CHAIN_SEGMENT_ERR,
456+};
457+#endif
458+
459 #endif
460diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
461index 887c4a5..475fcf7 100644
462--- a/mt7915/mt7915.h
463+++ b/mt7915/mt7915.h
464@@ -195,6 +195,45 @@ struct mt7915_hif {
465 int irq;
466 };
467
468+#ifdef CONFIG_MTK_VENDOR
469+enum csi_bw {
470+ CSI_BW20,
471+ CSI_BW40,
472+ CSI_BW80,
473+ CSI_BW160
474+};
475+
476+#define CSI_BW20_DATA_COUNT 64
477+#define CSI_BW40_DATA_COUNT 128
478+#define CSI_BW80_DATA_COUNT 256
479+#define CSI_BW160_DATA_COUNT 512
480+
developer73e5a572022-04-19 10:21:20 +0800481+struct csi_data {
482+ u8 ch_bw;
483+ u16 data_num;
developer1a173672023-12-21 14:49:33 +0800484+ s16 data_i[CSI_BW160_DATA_COUNT];
485+ s16 data_q[CSI_BW160_DATA_COUNT];
developer73e5a572022-04-19 10:21:20 +0800486+ u8 band;
487+ s8 rssi;
488+ u8 snr;
489+ u32 ts;
490+ u8 data_bw;
491+ u8 pri_ch_idx;
492+ u8 ta[ETH_ALEN];
developer1a173672023-12-21 14:49:33 +0800493+ u32 ext_info;
developer73e5a572022-04-19 10:21:20 +0800494+ u8 rx_mode;
developer1a173672023-12-21 14:49:33 +0800495+ u32 chain_info;
developer73e5a572022-04-19 10:21:20 +0800496+ u16 tx_idx;
497+ u16 rx_idx;
developer1a173672023-12-21 14:49:33 +0800498+ u32 segment_num;
499+ u8 remain_last;
500+ u16 pkt_sn;
501+ u8 tr_stream;
developer73e5a572022-04-19 10:21:20 +0800502+
503+ struct list_head node;
504+};
505+#endif
506+
developer1a173672023-12-21 14:49:33 +0800507 struct mt7915_phy {
508 struct mt76_phy *mt76;
509 struct mt7915_dev *dev;
510@@ -243,6 +282,21 @@ struct mt7915_phy {
developer73e5a572022-04-19 10:21:20 +0800511 u8 spe_idx;
512 } test;
513 #endif
514+
515+#ifdef CONFIG_MTK_VENDOR
516+ struct {
517+ struct list_head csi_list;
developer1a173672023-12-21 14:49:33 +0800518+ spinlock_t csi_lock; /* used for csi data push/pop */
developer73e5a572022-04-19 10:21:20 +0800519+ u32 count;
520+ bool mask;
521+ bool reorder;
522+ bool enable;
523+
developer1a173672023-12-21 14:49:33 +0800524+ struct csi_data buffered_csi;
developer73e5a572022-04-19 10:21:20 +0800525+ u32 interval;
526+ u32 last_record;
527+ } csi;
528+#endif
529 };
530
531 struct mt7915_dev {
developer1a173672023-12-21 14:49:33 +0800532@@ -624,6 +678,12 @@ void mt7915_sta_add_debugfs(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
developerec567112022-10-11 11:02:55 +0800533 int mt7915_mmio_wed_init(struct mt7915_dev *dev, void *pdev_ptr,
534 bool pci, int *irq);
developer73e5a572022-04-19 10:21:20 +0800535
536+#ifdef CONFIG_MTK_VENDOR
537+void mt7915_vendor_register(struct mt7915_phy *phy);
538+int mt7915_mcu_set_csi(struct mt7915_phy *phy, u8 mode,
developer1a173672023-12-21 14:49:33 +0800539+ u8 cfg, u8 v1, u32 v2, u8 *mac_addr, u32 sta_interval);
developer73e5a572022-04-19 10:21:20 +0800540+#endif
541+
542 #ifdef MTK_DEBUG
543 int mt7915_mtk_init_debugfs(struct mt7915_phy *phy, struct dentry *dir);
544 int mt7915_dbg_mcu_wa_cmd(struct mt7915_dev *dev, int cmd, u32 a1, u32 a2, u32 a3, bool wait_resp);
545diff --git a/mt7915/vendor.c b/mt7915/vendor.c
546new file mode 100644
developer1a173672023-12-21 14:49:33 +0800547index 0000000..55da60a
developer73e5a572022-04-19 10:21:20 +0800548--- /dev/null
549+++ b/mt7915/vendor.c
developer1a173672023-12-21 14:49:33 +0800550@@ -0,0 +1,470 @@
developer73e5a572022-04-19 10:21:20 +0800551+// SPDX-License-Identifier: ISC
552+/*
553+ * Copyright (C) 2020, MediaTek Inc. All rights reserved.
554+ */
555+
556+#include <net/netlink.h>
557+
558+#include "mt7915.h"
559+#include "mcu.h"
560+#include "vendor.h"
561+
562+static const struct nla_policy
563+csi_ctrl_policy[NUM_MTK_VENDOR_ATTRS_CSI_CTRL] = {
564+ [MTK_VENDOR_ATTR_CSI_CTRL_CFG] = {.type = NLA_NESTED },
565+ [MTK_VENDOR_ATTR_CSI_CTRL_CFG_MODE] = { .type = NLA_U8 },
566+ [MTK_VENDOR_ATTR_CSI_CTRL_CFG_TYPE] = { .type = NLA_U8 },
567+ [MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL1] = { .type = NLA_U8 },
568+ [MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL2] = { .type = NLA_U8 },
569+ [MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR] = { .type = NLA_NESTED },
570+ [MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL] = { .type = NLA_U32 },
developer1a173672023-12-21 14:49:33 +0800571+ [MTK_VENDOR_ATTR_CSI_CTRL_STA_INTERVAL] = { .type = NLA_U32 },
developer73e5a572022-04-19 10:21:20 +0800572+ [MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM] = { .type = NLA_U16 },
573+ [MTK_VENDOR_ATTR_CSI_CTRL_DATA] = { .type = NLA_NESTED },
574+};
575+
576+struct csi_null_tone {
577+ u8 start;
578+ u8 end;
579+};
580+
developer1a173672023-12-21 14:49:33 +0800581+struct csi_reorder {
developer73e5a572022-04-19 10:21:20 +0800582+ u8 dest;
583+ u8 start;
584+ u8 end;
585+};
586+
587+struct csi_mask {
588+ struct csi_null_tone null[10];
589+ u8 pilot[8];
590+ struct csi_reorder ro[3];
591+};
592+
593+static const struct csi_mask csi_mask_groups[] = {
594+ /* OFDM */
595+ { .null = { { 0 }, { 27, 37 } },
596+ .ro = { {0, 0, 63} },
597+ },
598+ { .null = { { 0, 69 }, { 96 }, { 123, 127 } },
599+ .ro = { { 0, 96 }, { 38, 70, 95 }, { 1, 97, 122 } },
600+ },
601+ { .null = { { 0, 5 }, { 32 }, { 59, 127 } },
602+ .ro = { { 0, 32 }, { 38, 6, 31 }, { 1, 33, 58 } },
603+ },
604+ { .null = { { 0, 5 }, { 32 }, { 59, 69 }, { 96 }, { 123, 127 } },
605+ .ro = { { 0, 0, 127 } },
606+ },
607+ { .null = { { 0, 133 }, { 160 }, { 187, 255 } },
608+ .ro = { { 0, 160 }, { 1, 161, 186 }, { 38, 134, 159 } },
609+ },
610+ { .null = { { 0, 197 }, { 224 }, { 251, 255 } },
611+ .ro = { { 0, 224 }, { 1, 225, 250 }, { 38, 198, 223 } },
612+ },
613+ { .null = { { 0, 5 }, { 32 }, { 59, 255 } },
614+ .ro = { { 0, 32 }, { 1, 33, 58 }, { 38, 6, 31 } },
615+ },
616+ { .null = { { 0, 69 }, { 96 }, { 123, 255 } },
617+ .ro = { { 0, 96 }, { 1, 97, 122 }, { 38, 70, 95 } },
618+ },
619+ { .null = { { 0, 133 }, { 160 }, { 187, 197 }, { 224 }, { 251, 255 } },
620+ .ro = { { 0, 192 }, { 2, 198, 250 }, { 74, 134, 186 } },
621+ },
622+ { .null = { { 0, 5 }, { 32 }, { 59, 69 }, { 96 }, { 123, 255 } },
623+ .ro = { { 0, 64 }, { 2, 70, 122 }, { 74, 6, 58 } },
624+ },
625+ { .null = { { 0, 5 }, { 32 }, { 59, 69 }, { 96 }, { 123, 133 },
626+ { 160 }, { 187, 197 }, { 224 }, { 251, 255 } },
627+ .ro = { { 0, 0, 255 } },
628+ },
629+
630+ /* HT/VHT */
631+ { .null = { { 0 }, { 29, 35 } },
632+ .pilot = { 7, 21, 43, 57 },
633+ .ro = { { 0, 0, 63 } },
634+ },
635+ { .null = { { 0, 67 }, { 96 }, { 125, 127 } },
636+ .pilot = { 75, 89, 103, 117 },
637+ .ro = { { 0, 96 }, { 36, 68, 95 }, { 1, 97, 124 } },
638+ },
639+ { .null = { { 0, 3 }, { 32 }, { 61, 127 } },
640+ .pilot = { 11, 25, 39, 53 },
641+ .ro = { { 0, 32 }, { 36, 4, 31 }, { 1, 33, 60 } },
642+ },
643+ { .null = { { 0, 1 }, { 59, 69 }, { 127 } },
644+ .pilot = { 11, 25, 53, 75, 103, 117 },
645+ .ro = { { 0, 0, 127 } },
646+ },
647+ { .null = { { 0, 131 }, { 160 }, { 189, 255 } },
648+ .pilot = { 139, 153, 167, 181 },
649+ .ro = { { 0, 160 }, { 1, 161, 188 }, { 36, 132, 159 } },
650+ },
651+ { .null = { { 0, 195 }, { 224 }, { 253 }, { 255 } },
652+ .pilot = { 203, 217, 231, 245 },
653+ .ro = { { 0, 224 }, { 1, 225, 252 }, { 36, 196, 223 } },
654+ },
655+ { .null = { { 0, 3 }, { 32 }, { 61, 255 } },
656+ .pilot = { 11, 25, 39, 53 },
657+ .ro = { { 0, 32 }, { 1, 33, 60 }, { 36, 4, 31 } },
658+ },
659+ { .null = { { 0, 67 }, { 96 }, { 125, 255 } },
660+ .pilot = { 75, 89, 103, 117 },
661+ .ro = { { 0, 96 }, { 1, 97, 124 }, { 36, 68, 95 } },
662+ },
663+ { .null = { { 0, 133 }, { 191, 193 }, { 251, 255 } },
664+ .pilot = { 139, 167, 181, 203, 217, 245 },
665+ .ro = { { 0, 192 }, { 2, 194, 250 }, { 70, 134, 190 } },
666+ },
667+ { .null = { { 0, 5 }, { 63, 65 }, { 123, 127 } },
668+ .pilot = { 11, 39, 53, 75, 89, 117 },
669+ .ro = { { 0, 64 }, { 2, 66, 122 }, { 70, 6, 62 } },
670+ },
671+ { .null = { { 0, 1 }, { 123, 133 }, { 255 } },
672+ .pilot = { 11, 39, 75, 103, 153, 181, 217, 245 },
673+ .ro = { { 0, 0, 255 } },
674+ },
675+
676+ /* HE */
677+ { .null = { { 0 }, { 31, 33 } },
678+ .pilot = { 12, 29, 35, 52 },
679+ .ro = { { 0, 0, 63 } },
680+ },
681+ { .null = { { 30, 34 }, { 96 } },
682+ .pilot = { 4, 21, 43, 60, 70, 87, 105, 122 },
683+ .ro = { { 0, 96 }, { 34, 66, 95 }, { 1, 97, 126 } },
684+ },
685+ { .null = { { 32 }, { 94, 98 } },
686+ .pilot = { 6, 23, 41, 58, 68, 85, 107, 124 },
687+ .ro = { { 0, 32 }, { 34, 2, 31 }, { 1, 31, 62 } },
688+ },
689+ { .null = { { 0 }, { 62, 66 } },
690+ .pilot = { 9, 26, 36, 53, 75, 92, 102, 119 },
691+ .ro = { { 0, 0, 127 } },
692+ },
693+ { .null = { { 30, 34 }, { 160 } },
694+ .pilot = { 4, 21, 43, 60, 137, 154, 166, 183 },
695+ .ro = { { 0, 160 }, { 1, 161, 190 }, { 34, 130, 159 } },
696+ },
697+ { .null = { { 94, 98 }, { 224 } },
698+ .pilot = { 68, 85, 107, 124, 201, 218, 230, 247 },
699+ .ro = { { 0, 224 }, { 1, 225, 254 }, { 34, 194, 223 } },
700+ },
701+ { .null = { { 32 }, { 158, 162 } },
702+ .pilot = { 9, 26, 38, 55, 132, 149, 171, 188 },
703+ .ro = { { 0, 32 }, { 1, 33, 62 }, { 34, 2, 31 } },
704+ },
705+ { .null = { { 96 }, { 222, 226 } },
706+ .pilot = { 73, 90, 102, 119, 196, 213, 235, 252 },
707+ .ro = { { 0, 96 }, { 1, 97, 126 }, { 34, 66, 95 } },
708+ },
709+ { .null = { { 62, 66 }, { 192 } },
710+ .pilot = { 36, 53, 75, 92, 169, 186, 198, 215 },
711+ .ro = { { 0, 192 }, { 1, 193, 253 }, { 67, 131, 191 } },
712+ },
713+ { .null = { { 64 }, { 190, 194 } },
714+ .pilot = { 41, 58, 70, 87, 164, 181, 203, 220 },
715+ .ro = { { 0, 64 }, { 1, 65, 125 }, { 67, 3, 63 } },
716+ },
717+ { .null = { { 0 }, { 126, 130 } },
718+ .pilot = { 6, 23, 100, 117, 139, 156, 233, 250 },
719+ .ro = { { 0, 0, 255 } },
720+ },
721+};
722+
723+static inline u8 csi_group_idx(u8 mode, u8 ch_bw, u8 data_bw, u8 pri_ch_idx)
724+{
725+ if (ch_bw < 2 || data_bw < 1)
726+ return mode * 11 + ch_bw * ch_bw + pri_ch_idx;
727+ else
728+ return mode * 11 + ch_bw * ch_bw + (data_bw + 1) * 2 + pri_ch_idx;
729+}
730+
731+static int mt7915_vendor_csi_ctrl(struct wiphy *wiphy,
732+ struct wireless_dev *wdev,
733+ const void *data,
734+ int data_len)
735+{
736+ struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
737+ struct mt7915_phy *phy = mt7915_hw_phy(hw);
738+ struct nlattr *tb[NUM_MTK_VENDOR_ATTRS_CSI_CTRL];
739+ int err;
740+
741+ err = nla_parse(tb, MTK_VENDOR_ATTR_CSI_CTRL_MAX, data, data_len,
742+ csi_ctrl_policy, NULL);
743+ if (err)
744+ return err;
745+
developer1a173672023-12-21 14:49:33 +0800746+ if (is_mt7915(phy->mt76->dev) && phy->mt76->chandef.width > NL80211_CHAN_WIDTH_80) {
747+ err = -EINVAL;
748+ return err;
749+ }
750+
developer73e5a572022-04-19 10:21:20 +0800751+ if (tb[MTK_VENDOR_ATTR_CSI_CTRL_CFG]) {
752+ u8 mode = 0, type = 0, v1 = 0, v2 = 0;
753+ u8 mac_addr[ETH_ALEN] = {};
754+ struct nlattr *cur;
755+ int rem;
developer1a173672023-12-21 14:49:33 +0800756+ u32 sta_interval = 0;
developer73e5a572022-04-19 10:21:20 +0800757+
758+ nla_for_each_nested(cur, tb[MTK_VENDOR_ATTR_CSI_CTRL_CFG], rem) {
developer1a173672023-12-21 14:49:33 +0800759+ switch (nla_type(cur)) {
developer73e5a572022-04-19 10:21:20 +0800760+ case MTK_VENDOR_ATTR_CSI_CTRL_CFG_MODE:
761+ mode = nla_get_u8(cur);
762+ break;
763+ case MTK_VENDOR_ATTR_CSI_CTRL_CFG_TYPE:
764+ type = nla_get_u8(cur);
765+ break;
766+ case MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL1:
767+ v1 = nla_get_u8(cur);
768+ break;
769+ case MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL2:
770+ v2 = nla_get_u8(cur);
771+ break;
772+ default:
773+ return -EINVAL;
774+ };
775+ }
776+
777+ if (tb[MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR]) {
778+ int idx = 0;
779+
780+ nla_for_each_nested(cur, tb[MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR], rem) {
781+ mac_addr[idx++] = nla_get_u8(cur);
782+ }
developer1a173672023-12-21 14:49:33 +0800783+
784+ /* when configure mac filter, add interval for report interval per sta */
785+ if (tb[MTK_VENDOR_ATTR_CSI_CTRL_STA_INTERVAL])
786+ sta_interval =
787+ nla_get_u32(tb[MTK_VENDOR_ATTR_CSI_CTRL_STA_INTERVAL]);
developer73e5a572022-04-19 10:21:20 +0800788+ }
789+
developer1a173672023-12-21 14:49:33 +0800790+ err = mt7915_mcu_set_csi(phy, mode, type, v1, v2, mac_addr, sta_interval);
791+
792+ if (err < 0)
793+ return err;
developer73e5a572022-04-19 10:21:20 +0800794+
795+ spin_lock_bh(&phy->csi.csi_lock);
796+
797+ phy->csi.enable = !!mode;
798+
799+ if (mode == 2 && type == 5) {
800+ if (v1 >= 1)
801+ phy->csi.mask = 1;
802+ if (v1 == 2)
803+ phy->csi.reorder = 1;
804+ }
805+
806+ /* clean up old csi stats */
807+ if ((mode == 0 || mode == 2) && !list_empty(&phy->csi.csi_list)) {
808+ struct csi_data *c, *tmp_c;
809+
810+ list_for_each_entry_safe(c, tmp_c, &phy->csi.csi_list,
811+ node) {
812+ list_del(&c->node);
813+ kfree(c);
814+ phy->csi.count--;
815+ }
816+ } else if (mode == 1) {
817+ phy->csi.last_record = 0;
818+ }
819+
820+ spin_unlock_bh(&phy->csi.csi_lock);
821+ }
822+
823+ if (tb[MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL])
824+ phy->csi.interval = nla_get_u32(tb[MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL]);
825+
826+ return 0;
827+}
828+
829+static void
830+mt7915_vendor_csi_tone_mask(struct mt7915_phy *phy, struct csi_data *csi)
831+{
832+ static const u8 mode_map[] = {
833+ [MT_PHY_TYPE_OFDM] = 0,
834+ [MT_PHY_TYPE_HT] = 1,
835+ [MT_PHY_TYPE_VHT] = 1,
836+ [MT_PHY_TYPE_HE_SU] = 2,
837+ };
838+ const struct csi_mask *cmask;
839+ int i;
840+
841+ if (csi->rx_mode == MT_PHY_TYPE_CCK || !phy->csi.mask)
842+ return;
843+
844+ if (csi->data_bw == IEEE80211_STA_RX_BW_40)
845+ csi->pri_ch_idx /= 2;
846+
847+ cmask = &csi_mask_groups[csi_group_idx(mode_map[csi->rx_mode],
848+ csi->ch_bw,
849+ csi->data_bw,
850+ csi->pri_ch_idx)];
851+
852+ for (i = 0; i < 10; i++) {
853+ const struct csi_null_tone *ntone = &cmask->null[i];
854+ u8 start = ntone->start;
855+ u8 end = ntone->end;
856+ int j;
857+
858+ if (!start && !end && i > 0)
859+ break;
860+
861+ if (!end)
862+ end = start;
863+
864+ for (j = start; j <= end; j++) {
865+ csi->data_i[j] = 0;
866+ csi->data_q[j] = 0;
867+ }
868+ }
869+
870+ for (i = 0; i < 8; i++) {
871+ u8 pilot = cmask->pilot[i];
872+
873+ if (!pilot)
874+ break;
875+
876+ csi->data_i[pilot] = 0;
877+ csi->data_q[pilot] = 0;
878+ }
879+
880+ if (!phy->csi.reorder)
881+ return;
882+
883+ for (i = 0; i < 3; i++) {
884+ const struct csi_reorder *ro = &cmask->ro[i];
885+ u8 dest = ro->dest;
886+ u8 start = ro->start;
887+ u8 end = ro->end;
888+
889+ if (!dest && !start && !end)
890+ break;
891+
892+ if (dest == start)
893+ continue;
894+
895+ if (end) {
896+ memmove(&csi->data_i[dest], &csi->data_i[start],
897+ end - start + 1);
898+ memmove(&csi->data_q[dest], &csi->data_q[start],
899+ end - start + 1);
900+ } else {
901+ csi->data_i[dest] = csi->data_i[start];
902+ csi->data_q[dest] = csi->data_q[start];
903+ }
904+ }
905+}
906+
907+static int
908+mt7915_vendor_csi_ctrl_dump(struct wiphy *wiphy, struct wireless_dev *wdev,
909+ struct sk_buff *skb, const void *data, int data_len,
910+ unsigned long *storage)
911+{
912+#define RESERVED_SET BIT(31)
913+ struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
914+ struct mt7915_phy *phy = mt7915_hw_phy(hw);
915+ struct nlattr *tb[NUM_MTK_VENDOR_ATTRS_CSI_CTRL];
916+ int err = 0;
917+
918+ if (*storage & RESERVED_SET) {
919+ if ((*storage & GENMASK(15, 0)) == 0)
920+ return -ENOENT;
921+ (*storage)--;
922+ }
923+
924+ if (data) {
925+ err = nla_parse(tb, MTK_VENDOR_ATTR_CSI_CTRL_MAX, data, data_len,
926+ csi_ctrl_policy, NULL);
927+ if (err)
928+ return err;
929+ }
930+
931+ if (!(*storage & RESERVED_SET) && tb[MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM]) {
932+ *storage = nla_get_u16(tb[MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM]);
933+ *storage |= RESERVED_SET;
934+ }
935+
936+ spin_lock_bh(&phy->csi.csi_lock);
937+
938+ if (!list_empty(&phy->csi.csi_list)) {
939+ struct csi_data *csi;
940+ void *a, *b;
941+ int i;
942+
943+ csi = list_first_entry(&phy->csi.csi_list, struct csi_data, node);
944+
945+ mt7915_vendor_csi_tone_mask(phy, csi);
946+
947+ a = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_CTRL_DATA);
948+
949+ if (nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_VER, 1) ||
950+ nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_RSSI, csi->rssi) ||
951+ nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_SNR, csi->snr) ||
952+ nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_BW, csi->data_bw) ||
953+ nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_CH_IDX, csi->pri_ch_idx) ||
954+ nla_put_u8(skb, MTK_VENDOR_ATTR_CSI_DATA_MODE, csi->rx_mode))
955+ goto out;
956+
957+ if (nla_put_u16(skb, MTK_VENDOR_ATTR_CSI_DATA_TX_ANT, csi->tx_idx) ||
958+ nla_put_u16(skb, MTK_VENDOR_ATTR_CSI_DATA_RX_ANT, csi->rx_idx))
959+ goto out;
960+
developer1a173672023-12-21 14:49:33 +0800961+ if (nla_put_u32(skb, MTK_VENDOR_ATTR_CSI_DATA_INFO, csi->ext_info) ||
962+ nla_put_u32(skb, MTK_VENDOR_ATTR_CSI_DATA_CHAIN_INFO, csi->chain_info) ||
developer73e5a572022-04-19 10:21:20 +0800963+ nla_put_u32(skb, MTK_VENDOR_ATTR_CSI_DATA_TS, csi->ts))
964+ goto out;
965+
966+ b = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_DATA_TA);
developer1a173672023-12-21 14:49:33 +0800967+ for (i = 0; i < ARRAY_SIZE(csi->ta); i++)
968+ if (nla_put_u8(skb, i, csi->ta[i]))
969+ goto out;
developer73e5a572022-04-19 10:21:20 +0800970+ nla_nest_end(skb, b);
971+
developer1a173672023-12-21 14:49:33 +0800972+ if (nla_put_u32(skb, MTK_VENDOR_ATTR_CSI_DATA_NUM, csi->data_num))
973+ goto out;
974+
developer73e5a572022-04-19 10:21:20 +0800975+ b = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_DATA_I);
developer1a173672023-12-21 14:49:33 +0800976+ for (i = 0; i < csi->data_num; i++)
977+ if (nla_put_u16(skb, i, csi->data_i[i]))
978+ goto out;
developer73e5a572022-04-19 10:21:20 +0800979+ nla_nest_end(skb, b);
980+
981+ b = nla_nest_start(skb, MTK_VENDOR_ATTR_CSI_DATA_Q);
developer1a173672023-12-21 14:49:33 +0800982+ for (i = 0; i < csi->data_num; i++)
983+ if (nla_put_u16(skb, i, csi->data_q[i]))
984+ goto out;
developer73e5a572022-04-19 10:21:20 +0800985+ nla_nest_end(skb, b);
986+
987+ nla_nest_end(skb, a);
988+
989+ list_del(&csi->node);
990+ kfree(csi);
991+ phy->csi.count--;
992+
993+ err = phy->csi.count;
994+ }
995+out:
996+ spin_unlock_bh(&phy->csi.csi_lock);
997+
998+ return err;
999+}
1000+
1001+static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
1002+ {
1003+ .info = {
1004+ .vendor_id = MTK_NL80211_VENDOR_ID,
1005+ .subcmd = MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL,
1006+ },
1007+ .flags = WIPHY_VENDOR_CMD_NEED_NETDEV |
1008+ WIPHY_VENDOR_CMD_NEED_RUNNING,
1009+ .doit = mt7915_vendor_csi_ctrl,
1010+ .dumpit = mt7915_vendor_csi_ctrl_dump,
1011+ .policy = csi_ctrl_policy,
1012+ .maxattr = MTK_VENDOR_ATTR_CSI_CTRL_MAX,
1013+ }
1014+};
1015+
1016+void mt7915_vendor_register(struct mt7915_phy *phy)
1017+{
1018+ phy->mt76->hw->wiphy->vendor_commands = mt7915_vendor_commands;
1019+ phy->mt76->hw->wiphy->n_vendor_commands = ARRAY_SIZE(mt7915_vendor_commands);
1020+}
1021diff --git a/mt7915/vendor.h b/mt7915/vendor.h
1022new file mode 100644
developer1a173672023-12-21 14:49:33 +08001023index 0000000..e1f5fd3
developer73e5a572022-04-19 10:21:20 +08001024--- /dev/null
1025+++ b/mt7915/vendor.h
developer1a173672023-12-21 14:49:33 +08001026@@ -0,0 +1,63 @@
1027+/* SPDX-License-Identifier: ISC */
developer73e5a572022-04-19 10:21:20 +08001028+#ifndef __MT7915_VENDOR_H
1029+#define __MT7915_VENDOR_H
1030+
1031+#define MTK_NL80211_VENDOR_ID 0x0ce7
1032+
1033+enum mtk_nl80211_vendor_subcmds {
1034+ MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL = 0xc2,
1035+};
1036+
1037+enum mtk_vendor_attr_csi_ctrl {
1038+ MTK_VENDOR_ATTR_CSI_CTRL_UNSPEC,
1039+
1040+ MTK_VENDOR_ATTR_CSI_CTRL_CFG,
1041+ MTK_VENDOR_ATTR_CSI_CTRL_CFG_MODE,
1042+ MTK_VENDOR_ATTR_CSI_CTRL_CFG_TYPE,
1043+ MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL1,
1044+ MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL2,
1045+ MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR,
1046+ MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL,
developer1a173672023-12-21 14:49:33 +08001047+ MTK_VENDOR_ATTR_CSI_CTRL_STA_INTERVAL,
developer73e5a572022-04-19 10:21:20 +08001048+
1049+ MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM,
1050+
1051+ MTK_VENDOR_ATTR_CSI_CTRL_DATA,
1052+
1053+ /* keep last */
1054+ NUM_MTK_VENDOR_ATTRS_CSI_CTRL,
1055+ MTK_VENDOR_ATTR_CSI_CTRL_MAX =
1056+ NUM_MTK_VENDOR_ATTRS_CSI_CTRL - 1
1057+};
1058+
1059+enum mtk_vendor_attr_csi_data {
1060+ MTK_VENDOR_ATTR_CSI_DATA_UNSPEC,
1061+ MTK_VENDOR_ATTR_CSI_DATA_PAD,
1062+
1063+ MTK_VENDOR_ATTR_CSI_DATA_VER,
1064+ MTK_VENDOR_ATTR_CSI_DATA_TS,
1065+ MTK_VENDOR_ATTR_CSI_DATA_RSSI,
1066+ MTK_VENDOR_ATTR_CSI_DATA_SNR,
1067+ MTK_VENDOR_ATTR_CSI_DATA_BW,
1068+ MTK_VENDOR_ATTR_CSI_DATA_CH_IDX,
1069+ MTK_VENDOR_ATTR_CSI_DATA_TA,
developer1a173672023-12-21 14:49:33 +08001070+ MTK_VENDOR_ATTR_CSI_DATA_NUM,
developer73e5a572022-04-19 10:21:20 +08001071+ MTK_VENDOR_ATTR_CSI_DATA_I,
1072+ MTK_VENDOR_ATTR_CSI_DATA_Q,
1073+ MTK_VENDOR_ATTR_CSI_DATA_INFO,
1074+ MTK_VENDOR_ATTR_CSI_DATA_RSVD1,
1075+ MTK_VENDOR_ATTR_CSI_DATA_RSVD2,
1076+ MTK_VENDOR_ATTR_CSI_DATA_RSVD3,
1077+ MTK_VENDOR_ATTR_CSI_DATA_RSVD4,
1078+ MTK_VENDOR_ATTR_CSI_DATA_TX_ANT,
1079+ MTK_VENDOR_ATTR_CSI_DATA_RX_ANT,
1080+ MTK_VENDOR_ATTR_CSI_DATA_MODE,
developer1a173672023-12-21 14:49:33 +08001081+ MTK_VENDOR_ATTR_CSI_DATA_CHAIN_INFO,
developer73e5a572022-04-19 10:21:20 +08001082+
1083+ /* keep last */
1084+ NUM_MTK_VENDOR_ATTRS_CSI_DATA,
1085+ MTK_VENDOR_ATTR_CSI_DATA_MAX =
1086+ NUM_MTK_VENDOR_ATTRS_CSI_DATA - 1
1087+};
1088+
1089+#endif
1090--
developerbd9fa1e2023-10-16 11:04:00 +080010912.18.0
developer73e5a572022-04-19 10:21:20 +08001092