blob: beada05fcc605267303d6c320eb8e66bc7988d51 [file] [log] [blame]
developer3abe1ad2022-01-24 11:13:32 +08001/* Copyright (C) 2021-2022 Mediatek Inc. */
2#define _GNU_SOURCE
3
4#include <unl.h>
5
6#include "atenl.h"
7
8#define to_rssi(_rcpi) ((_rcpi - 220) / 2)
9
10struct atenl_nl_priv {
11 struct atenl *an;
12 struct unl unl;
13 struct nl_msg *msg;
14 int attr;
15 void *res;
16};
17
18struct atenl_nl_ops {
19 int set;
20 int dump;
21 int (*ops)(struct atenl *an, struct atenl_data *data,
22 struct atenl_nl_priv *nl_priv);
23};
24
25static struct nla_policy testdata_policy[NUM_MT76_TM_ATTRS] = {
26 [MT76_TM_ATTR_STATE] = { .type = NLA_U8 },
27 [MT76_TM_ATTR_MTD_PART] = { .type = NLA_STRING },
28 [MT76_TM_ATTR_MTD_OFFSET] = { .type = NLA_U32 },
developerf90c9af2022-12-28 22:40:23 +080029 [MT76_TM_ATTR_BAND_IDX] = { .type = NLA_U8 },
developer3abe1ad2022-01-24 11:13:32 +080030 [MT76_TM_ATTR_TX_COUNT] = { .type = NLA_U32 },
31 [MT76_TM_ATTR_TX_LENGTH] = { .type = NLA_U32 },
32 [MT76_TM_ATTR_TX_RATE_MODE] = { .type = NLA_U8 },
33 [MT76_TM_ATTR_TX_RATE_NSS] = { .type = NLA_U8 },
34 [MT76_TM_ATTR_TX_RATE_IDX] = { .type = NLA_U8 },
35 [MT76_TM_ATTR_TX_RATE_SGI] = { .type = NLA_U8 },
36 [MT76_TM_ATTR_TX_RATE_LDPC] = { .type = NLA_U8 },
37 [MT76_TM_ATTR_TX_RATE_STBC] = { .type = NLA_U8 },
38 [MT76_TM_ATTR_TX_LTF] = { .type = NLA_U8 },
39 [MT76_TM_ATTR_TX_POWER_CONTROL] = { .type = NLA_U8 },
40 [MT76_TM_ATTR_TX_ANTENNA] = { .type = NLA_U8 },
41 [MT76_TM_ATTR_FREQ_OFFSET] = { .type = NLA_U32 },
42 [MT76_TM_ATTR_STATS] = { .type = NLA_NESTED },
developer071927d2022-08-31 20:39:29 +080043 [MT76_TM_ATTR_PRECAL] = { .type = NLA_NESTED },
44 [MT76_TM_ATTR_PRECAL_INFO] = { .type = NLA_NESTED },
developer3abe1ad2022-01-24 11:13:32 +080045};
46
47static struct nla_policy stats_policy[NUM_MT76_TM_STATS_ATTRS] = {
48 [MT76_TM_STATS_ATTR_TX_PENDING] = { .type = NLA_U32 },
49 [MT76_TM_STATS_ATTR_TX_QUEUED] = { .type = NLA_U32 },
50 [MT76_TM_STATS_ATTR_TX_DONE] = { .type = NLA_U32 },
51 [MT76_TM_STATS_ATTR_RX_PACKETS] = { .type = NLA_U64 },
52 [MT76_TM_STATS_ATTR_RX_FCS_ERROR] = { .type = NLA_U64 },
53};
54
55static struct nla_policy rx_policy[NUM_MT76_TM_RX_ATTRS] = {
56 [MT76_TM_RX_ATTR_FREQ_OFFSET] = { .type = NLA_U32 },
57 [MT76_TM_RX_ATTR_RCPI] = { .type = NLA_NESTED },
58 [MT76_TM_RX_ATTR_IB_RSSI] = { .type = NLA_NESTED },
59 [MT76_TM_RX_ATTR_WB_RSSI] = { .type = NLA_NESTED },
60 [MT76_TM_RX_ATTR_SNR] = { .type = NLA_U8 },
61};
62
63struct he_sgi {
64 enum mt76_testmode_tx_mode tx_mode;
65 u8 sgi;
66 u8 tx_ltf;
67};
68
69#define HE_SGI_GROUP(_tx_mode, _sgi, _tx_ltf) \
70 { .tx_mode = MT76_TM_TX_MODE_##_tx_mode, .sgi = _sgi, .tx_ltf = _tx_ltf }
71static const struct he_sgi he_sgi_groups[] = {
72 HE_SGI_GROUP(HE_SU, 0, 0),
73 HE_SGI_GROUP(HE_SU, 0, 1),
74 HE_SGI_GROUP(HE_SU, 1, 1),
75 HE_SGI_GROUP(HE_SU, 2, 2),
76 HE_SGI_GROUP(HE_SU, 0, 2),
77 HE_SGI_GROUP(HE_EXT_SU, 0, 0),
78 HE_SGI_GROUP(HE_EXT_SU, 0, 1),
79 HE_SGI_GROUP(HE_EXT_SU, 1, 1),
80 HE_SGI_GROUP(HE_EXT_SU, 2, 2),
81 HE_SGI_GROUP(HE_EXT_SU, 0, 2),
82 HE_SGI_GROUP(HE_TB, 1, 0),
83 HE_SGI_GROUP(HE_TB, 1, 1),
84 HE_SGI_GROUP(HE_TB, 2, 2),
85 HE_SGI_GROUP(HE_MU, 0, 2),
86 HE_SGI_GROUP(HE_MU, 0, 1),
87 HE_SGI_GROUP(HE_MU, 1, 1),
88 HE_SGI_GROUP(HE_MU, 2, 2),
89};
90#undef HE_SGI_LTF_GROUP
91
92static u8 phy_type_to_attr(u8 phy_type)
93{
94 static const u8 phy_type_to_attr[] = {
95 [ATENL_PHY_TYPE_CCK] = MT76_TM_TX_MODE_CCK,
96 [ATENL_PHY_TYPE_OFDM] = MT76_TM_TX_MODE_OFDM,
97 [ATENL_PHY_TYPE_HT] = MT76_TM_TX_MODE_HT,
98 [ATENL_PHY_TYPE_HT_GF] = MT76_TM_TX_MODE_HT,
99 [ATENL_PHY_TYPE_VHT] = MT76_TM_TX_MODE_VHT,
100 [ATENL_PHY_TYPE_HE_SU] = MT76_TM_TX_MODE_HE_SU,
101 [ATENL_PHY_TYPE_HE_EXT_SU] = MT76_TM_TX_MODE_HE_EXT_SU,
102 [ATENL_PHY_TYPE_HE_TB] = MT76_TM_TX_MODE_HE_TB,
103 [ATENL_PHY_TYPE_HE_MU] = MT76_TM_TX_MODE_HE_MU,
104 };
105
106 if (phy_type >= ARRAY_SIZE(phy_type_to_attr))
107 return 0;
108
109 return phy_type_to_attr[phy_type];
110}
111
112static void
113atenl_set_attr_state(struct atenl *an, struct nl_msg *msg,
114 u8 band, enum mt76_testmode_state state)
115{
116 if (get_band_val(an, band, cur_state) == state)
117 return;
118
119 nla_put_u8(msg, MT76_TM_ATTR_STATE, state);
120 set_band_val(an, band, cur_state, state);
121}
122
123static void
124atenl_set_attr_antenna(struct atenl *an, struct nl_msg *msg, u8 tx_antenna)
125{
126 if (!tx_antenna)
127 return;
developer67630e02022-12-06 14:35:28 +0800128
129 nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, tx_antenna);
developer3abe1ad2022-01-24 11:13:32 +0800130}
131
132static int
133atenl_nl_set_attr(struct atenl *an, struct atenl_data *data,
134 struct atenl_nl_priv *nl_priv)
135{
136 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
137 struct nl_msg *msg = nl_priv->msg;
138 u32 val = ntohl(*(u32 *)hdr->data);
139 int attr = nl_priv->attr;
140 void *ptr, *a;
141
142 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
143 if (!ptr)
144 return -ENOMEM;
145
146 switch (attr) {
147 case MT76_TM_ATTR_TX_ANTENNA:
148 atenl_set_attr_antenna(an, msg, val);
149 break;
150 case MT76_TM_ATTR_FREQ_OFFSET:
151 nla_put_u32(msg, attr, val);
152 break;
153 case MT76_TM_ATTR_TX_POWER:
154 a = nla_nest_start(msg, MT76_TM_ATTR_TX_POWER);
155 if (!a)
156 return -ENOMEM;
157 nla_put_u8(msg, 0, val);
158 nla_nest_end(msg, a);
159 break;
160 default:
161 nla_put_u8(msg, attr, val);
162 break;
163 }
164
165 nla_nest_end(msg, ptr);
166
167 return unl_genl_request(&nl_priv->unl, msg, NULL, NULL);
168}
169
170static int
171atenl_nl_set_cfg(struct atenl *an, struct atenl_data *data,
172 struct atenl_nl_priv *nl_priv)
173{
174 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
175 struct nl_msg *msg = nl_priv->msg;
176 enum atenl_cmd cmd = data->cmd;
177 u32 *v = (u32 *)hdr->data;
178 u8 type = ntohl(v[0]);
179 u8 enable = ntohl(v[1]);
180 void *ptr, *cfg;
181
182 if (cmd == HQA_CMD_SET_TSSI) {
183 type = 0;
184 enable = 1;
185 }
186
187 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
188 if (!ptr)
189 return -ENOMEM;
190
191 cfg = nla_nest_start(msg, MT76_TM_ATTR_CFG);
192 if (!cfg)
193 return -ENOMEM;
194
195 if (nla_put_u8(msg, 0, type) ||
196 nla_put_u8(msg, 1, enable))
197 return -EINVAL;
198
199 nla_nest_end(msg, cfg);
200
201 nla_nest_end(msg, ptr);
202
203 return unl_genl_request(&nl_priv->unl, msg, NULL, NULL);
204}
205
206static int
207atenl_nl_set_tx(struct atenl *an, struct atenl_data *data,
208 struct atenl_nl_priv *nl_priv)
209{
210 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
211 struct nl_msg *msg = nl_priv->msg;
212 u32 *v = (u32 *)hdr->data;
213 u8 *addr1 = hdr->data + 36;
214 u8 *addr2 = addr1 + ETH_ALEN;
215 u8 *addr3 = addr2 + ETH_ALEN;
developer5698c9c2022-05-30 16:40:23 +0800216 u8 def_mac[ETH_ALEN] = {0x00, 0x11, 0x22, 0x33, 0x44, 0x55};
developer3abe1ad2022-01-24 11:13:32 +0800217 void *ptr, *a;
218
developer5698c9c2022-05-30 16:40:23 +0800219 if (get_band_val(an, an->cur_band, use_tx_time))
220 set_band_val(an, an->cur_band, tx_time, ntohl(v[7]));
221 else
222 set_band_val(an, an->cur_band, tx_mpdu_len, ntohl(v[7]));
223
developer3abe1ad2022-01-24 11:13:32 +0800224 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
225 if (!ptr)
226 return -ENOMEM;
227
developer3abe1ad2022-01-24 11:13:32 +0800228 a = nla_nest_start(msg, MT76_TM_ATTR_MAC_ADDRS);
229 if (!a)
230 return -ENOMEM;
231
developer5698c9c2022-05-30 16:40:23 +0800232 nla_put(msg, 0, ETH_ALEN, use_default_addr(addr1) ? def_mac : addr1);
233 nla_put(msg, 1, ETH_ALEN, use_default_addr(addr2) ? def_mac : addr2);
234 nla_put(msg, 2, ETH_ALEN, use_default_addr(addr3) ? def_mac : addr3);
developer3abe1ad2022-01-24 11:13:32 +0800235
236 nla_nest_end(msg, a);
237
238 nla_nest_end(msg, ptr);
239
240 *(u32 *)(hdr->data + 2) = data->ext_id;
241
242 return unl_genl_request(&nl_priv->unl, msg, NULL, NULL);
243}
244
245static int
246atenl_nl_tx(struct atenl *an, struct atenl_data *data, struct atenl_nl_priv *nl_priv)
247{
248#define USE_SPE_IDX BIT(31)
249 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
250 struct nl_msg *msg = nl_priv->msg;
251 u32 *v = (u32 *)hdr->data;
252 u8 band = ntohl(v[2]);
253 void *ptr;
254 int ret;
255
256 if (band >= MAX_BAND_NUM)
257 return -EINVAL;
258
259 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
260 if (!ptr)
261 return -ENOMEM;
262
263 if (data->ext_cmd == HQA_EXT_CMD_STOP_TX) {
264 atenl_set_attr_state(an, msg, band, MT76_TM_STATE_IDLE);
265 } else {
266 u32 tx_count = ntohl(v[3]);
267 u8 tx_rate_mode = phy_type_to_attr(ntohl(v[4]));
268 u8 aid = ntohl(v[11]);
269 u8 sgi = ntohl(v[13]);
270 u32 tx_antenna = ntohl(v[14]);
271 void *a;
272
273 if (sgi > 5)
274 return -EINVAL;
275
276 if (!tx_count)
277 tx_count = 10000000;
278
279 nla_put_u32(msg, MT76_TM_ATTR_TX_COUNT, tx_count);
280 nla_put_u32(msg, MT76_TM_ATTR_TX_IPG, ntohl(v[12]));
281 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_MODE, tx_rate_mode);
282 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, ntohl(v[5]));
283 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_STBC, ntohl(v[7]));
284 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_LDPC, ntohl(v[8]));
285 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_NSS, ntohl(v[15]));
286
developer5698c9c2022-05-30 16:40:23 +0800287 if (get_band_val(an, band, use_tx_time))
288 nla_put_u32(msg, MT76_TM_ATTR_TX_TIME,
289 get_band_val(an, band, tx_time));
290 else
developer93dadcc2022-07-13 10:25:35 +0800291 nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH,
292 get_band_val(an, band, tx_mpdu_len));
developer5698c9c2022-05-30 16:40:23 +0800293
developer3abe1ad2022-01-24 11:13:32 +0800294 /* for chips after 7915, tx need to use at least wcid = 1 */
295 if (!is_mt7915(an) && !aid)
296 aid = 1;
297 nla_put_u8(msg, MT76_TM_ATTR_AID, aid);
298
299 if (tx_antenna & USE_SPE_IDX) {
300 nla_put_u8(msg, MT76_TM_ATTR_TX_SPE_IDX,
301 tx_antenna & ~USE_SPE_IDX);
302 } else {
303 nla_put_u8(msg, MT76_TM_ATTR_TX_SPE_IDX, 0);
304 atenl_set_attr_antenna(an, msg, tx_antenna);
305 }
306
307 if (tx_rate_mode >= MT76_TM_TX_MODE_HE_SU) {
308 u8 ofs = sgi;
309 size_t i;
310
311 for (i = 0; i < ARRAY_SIZE(he_sgi_groups); i++)
312 if (he_sgi_groups[i].tx_mode == tx_rate_mode)
313 break;
314
315 if ((i + ofs) >= ARRAY_SIZE(he_sgi_groups))
316 return -EINVAL;
317
318 sgi = he_sgi_groups[i + ofs].sgi;
319 nla_put_u8(msg, MT76_TM_ATTR_TX_LTF,
320 he_sgi_groups[i + ofs].tx_ltf);
321 }
322 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_SGI, sgi);
323
324 a = nla_nest_start(msg, MT76_TM_ATTR_TX_POWER);
325 if (!a)
326 return -ENOMEM;
327 nla_put_u8(msg, 0, ntohl(v[6]));
328 nla_nest_end(msg, a);
329
330 atenl_set_attr_state(an, msg, band, MT76_TM_STATE_TX_FRAMES);
331 }
332
333 nla_nest_end(msg, ptr);
334
335 ret = unl_genl_request(&nl_priv->unl, msg, NULL, NULL);
336 if (ret)
337 return ret;
338
339 *(u32 *)(hdr->data + 2) = data->ext_id;
340
341 return 0;
342}
343
344static int
345atenl_nl_rx(struct atenl *an, struct atenl_data *data, struct atenl_nl_priv *nl_priv)
346{
347 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
348 struct atenl_band *anb = &an->anb[an->cur_band];
349 struct nl_msg *msg = nl_priv->msg;
350 u32 *v = (u32 *)hdr->data;
351 u8 band = ntohl(v[2]);
352 void *ptr;
353
354 if (band >= MAX_BAND_NUM)
355 return -EINVAL;
356
357 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
358 if (!ptr)
359 return -ENOMEM;
360
361 if (data->ext_cmd == HQA_EXT_CMD_STOP_RX) {
362 atenl_set_attr_state(an, msg, band, MT76_TM_STATE_IDLE);
363 } else {
364 v = (u32 *)(hdr->data + 18);
365
366 atenl_set_attr_antenna(an, msg, ntohl(v[0]));
367 nla_put_u8(msg, MT76_TM_ATTR_AID, ntohl(v[1]));
368 atenl_set_attr_state(an, msg, band, MT76_TM_STATE_RX_FRAMES);
369
370 anb->reset_rx_cnt = false;
371
372 /* clear history buffer */
373 memset(&anb->rx_stat, 0, sizeof(anb->rx_stat));
374 }
375
376 nla_nest_end(msg, ptr);
377
378 *(u32 *)(hdr->data + 2) = data->ext_id;
379
380 return unl_genl_request(&nl_priv->unl, msg, NULL, NULL);
381}
382
383static int
384atenl_off_ch_scan(struct atenl *an, struct atenl_data *data,
385 struct atenl_nl_priv *nl_priv)
386{
387 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
388 struct nl_msg *msg = nl_priv->msg;
389 u32 *v = (u32 *)hdr->data;
390 u8 ch = ntohl(v[2]);
391 u8 bw = ntohl(v[4]);
392 u8 tx_path = ntohl(v[5]);
393 u8 status = ntohl(v[6]);
394 void *ptr;
395
396 if (!status)
397 ch = 0; /* stop */
398
399 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
400 if (!ptr)
401 return -ENOMEM;
402
403 nla_put_u8(msg, MT76_TM_ATTR_OFF_CH_SCAN_CH, ch);
404 nla_put_u8(msg, MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH,
405 atenl_get_center_channel(bw, CH_BAND_5GHZ, ch));
406 nla_put_u8(msg, MT76_TM_ATTR_OFF_CH_SCAN_BW, bw);
407 nla_put_u8(msg, MT76_TM_ATTR_OFF_CH_SCAN_PATH, tx_path);
408
409 nla_nest_end(msg, ptr);
410
developer5698c9c2022-05-30 16:40:23 +0800411 *(u32 *)(hdr->data + 2) = data->ext_id;
developer3abe1ad2022-01-24 11:13:32 +0800412
413 return 0;
414}
415
416static int atenl_nl_dump_cb(struct nl_msg *msg, void *arg)
417{
418 struct atenl_nl_priv *nl_priv = (struct atenl_nl_priv *)arg;
419 struct nlattr *tb1[NUM_MT76_TM_ATTRS];
420 struct nlattr *tb2[NUM_MT76_TM_STATS_ATTRS];
421 struct nlattr *nl_attr;
422 int attr = nl_priv->attr;
423 u64 *res = nl_priv->res;
424
425 nl_attr = unl_find_attr(&nl_priv->unl, msg, NL80211_ATTR_TESTDATA);
426 if (!nl_attr) {
developer5698c9c2022-05-30 16:40:23 +0800427 atenl_err("Testdata attribute not found\n");
developer3abe1ad2022-01-24 11:13:32 +0800428 return NL_SKIP;
429 }
430
431 nla_parse_nested(tb1, MT76_TM_ATTR_MAX, nl_attr, testdata_policy);
432 nla_parse_nested(tb2, MT76_TM_STATS_ATTR_MAX,
433 tb1[MT76_TM_ATTR_STATS], stats_policy);
434
435 if (attr == MT76_TM_STATS_ATTR_TX_DONE)
436 *res = nla_get_u32(tb2[MT76_TM_STATS_ATTR_TX_DONE]);
437
438 return NL_SKIP;
439}
440
441static int
442atenl_nl_dump_attr(struct atenl *an, struct atenl_data *data,
443 struct atenl_nl_priv *nl_priv)
444{
445 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
446 struct nl_msg *msg = nl_priv->msg;
447 void *ptr;
448 u64 res = 0;
449
450 nl_priv->res = (void *)&res;
451
452 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
453 if (!ptr)
454 return -ENOMEM;
455 nla_put_flag(msg, MT76_TM_ATTR_STATS);
456 nla_nest_end(msg, ptr);
457
458 unl_genl_request(&nl_priv->unl, msg, atenl_nl_dump_cb, (void *)nl_priv);
459
460 if (nl_priv->attr == MT76_TM_STATS_ATTR_TX_DONE)
461 *(u32 *)(hdr->data + 2 + 4 * an->cur_band) = htonl(res);
462
463 return 0;
464}
465
466static int atenl_nl_continuous_tx(struct atenl *an,
467 struct atenl_data *data,
468 struct atenl_nl_priv *nl_priv)
469{
470 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
471 struct nl_msg *msg = nl_priv->msg;
472 u32 *v = (u32 *)hdr->data;
473 u8 band = ntohl(v[0]);
474 bool enable = ntohl(v[1]);
475 void *ptr;
476
477 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
478 if (!ptr)
479 return -ENOMEM;
480
481 if (band >= MAX_BAND_NUM)
482 return -EINVAL;
483
484 if (!enable) {
485 int phy = get_band_val(an, band, phy_idx);
486 char cmd[64];
487
488 atenl_set_attr_state(an, msg, band, MT76_TM_STATE_IDLE);
489 nla_nest_end(msg, ptr);
490 unl_genl_request(&nl_priv->unl, msg, NULL, NULL);
491
492 sprintf(cmd, "iw dev mon%d del", phy);
493 system(cmd);
494 sprintf(cmd, "iw phy phy%d interface add mon%d type monitor", phy, phy);
495 system(cmd);
496 sprintf(cmd, "ifconfig mon%d up", phy);
497 system(cmd);
498
499 return 0;
500 }
501
502 if (get_band_val(an, band, rf_mode) != ATENL_RF_MODE_TEST)
503 return 0;
504
505 nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, ntohl(v[2]));
506 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_MODE, phy_type_to_attr(ntohl(v[3])));
507 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, ntohl(v[6]));
508
509 atenl_dbg("%s: enable = %d, ant=%u, tx_rate_mode=%u, rate_idx=%u\n",
510 __func__, enable, ntohl(v[2]), ntohl(v[3]), ntohl(v[6]));
511
512 atenl_set_attr_state(an, msg, band, MT76_TM_STATE_TX_CONT);
513
514 nla_nest_end(msg, ptr);
515
516 return unl_genl_request(&nl_priv->unl, msg, NULL, NULL);
517}
518
519static int atenl_nl_get_rx_info_cb(struct nl_msg *msg, void *arg)
520{
521 struct atenl_nl_priv *nl_priv = (struct atenl_nl_priv *)arg;
522 struct atenl *an = nl_priv->an;
523 struct atenl_band *anb = &an->anb[an->cur_band];
524 struct atenl_data *data = nl_priv->res;
525 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
526 struct atenl_rx_info_hdr *rx_hdr;
527 struct atenl_rx_info_band *rx_band;
528 struct atenl_rx_info_user *rx_user;
529 struct atenl_rx_info_path *rx_path;
530 struct atenl_rx_info_comm *rx_comm;
531 struct nlattr *tb1[NUM_MT76_TM_ATTRS];
532 struct nlattr *tb2[NUM_MT76_TM_STATS_ATTRS];
533 struct nlattr *tb3[NUM_MT76_TM_RX_ATTRS];
534 struct nlattr *nl_attr, *cur;
535 struct atenl_rx_stat rx_cur, rx_diff = {};
536 u32 rcpi[4] = {};
537 u32 type_num = htonl(4);
538 s32 ib_rssi[4] = {}, wb_rssi[4] = {};
539 u8 path = an->anb[an->cur_band].chainmask;
540 u8 path_num = __builtin_popcount(path);
541 u8 *buf = hdr->data + 2;
542 int i, rem;
543
544 *(u32 *)buf = type_num;
545 buf += sizeof(type_num);
546
547#define RX_PUT_HDR(_hdr, _type, _val, _size) do { \
548 _hdr->type = htonl(_type); \
549 _hdr->val = htonl(_val); \
550 _hdr->len = htonl(_size); \
551 buf += sizeof(*_hdr); \
552 } while (0)
553
554 rx_hdr = (struct atenl_rx_info_hdr *)buf;
555 RX_PUT_HDR(rx_hdr, 0, BIT(an->cur_band), sizeof(*rx_band));
556 rx_band = (struct atenl_rx_info_band *)buf;
557 buf += sizeof(*rx_band);
558
559 rx_hdr = (struct atenl_rx_info_hdr *)buf;
560 RX_PUT_HDR(rx_hdr, 1, path, path_num * sizeof(*rx_path));
561 rx_path = (struct atenl_rx_info_path *)buf;
562 buf += path_num * sizeof(*rx_path);
563
564 rx_hdr = (struct atenl_rx_info_hdr *)buf;
565 RX_PUT_HDR(rx_hdr, 2, GENMASK(15, 0), 16 * sizeof(*rx_user));
566 rx_user = (struct atenl_rx_info_user *)buf;
567 buf += 16 * sizeof(*rx_user);
568
569 rx_hdr = (struct atenl_rx_info_hdr *)buf;
570 RX_PUT_HDR(rx_hdr, 3, BIT(0), sizeof(*rx_comm));
571 rx_comm = (struct atenl_rx_info_comm *)buf;
572 buf += sizeof(*rx_comm);
573
574 hdr->len = htons(buf - hdr->data);
575
576 nl_attr = unl_find_attr(&nl_priv->unl, msg, NL80211_ATTR_TESTDATA);
577 if (!nl_attr) {
developer5698c9c2022-05-30 16:40:23 +0800578 atenl_err("Testdata attribute not found\n");
developer3abe1ad2022-01-24 11:13:32 +0800579 return NL_SKIP;
580 }
581
582 nla_parse_nested(tb1, MT76_TM_ATTR_MAX, nl_attr, testdata_policy);
583 nla_parse_nested(tb2, MT76_TM_STATS_ATTR_MAX,
584 tb1[MT76_TM_ATTR_STATS], stats_policy);
585
586 rx_cur.total = nla_get_u64(tb2[MT76_TM_STATS_ATTR_RX_PACKETS]);
587 rx_cur.err_cnt = nla_get_u64(tb2[MT76_TM_STATS_ATTR_RX_FCS_ERROR]);
588 rx_cur.len_mismatch = nla_get_u64(tb2[MT76_TM_STATS_ATTR_RX_LEN_MISMATCH]);
589 rx_cur.ok_cnt = rx_cur.total - rx_cur.err_cnt - rx_cur.len_mismatch;
590
developer5698c9c2022-05-30 16:40:23 +0800591 if (!anb->reset_rx_cnt ||
592 get_band_val(an, an->cur_band, cur_state) == MT76_TM_STATE_RX_FRAMES) {
developer3abe1ad2022-01-24 11:13:32 +0800593#define RX_COUNT_DIFF(_field) \
developer5698c9c2022-05-30 16:40:23 +0800594 rx_diff._field = (rx_cur._field) - (anb->rx_stat._field);
developer3abe1ad2022-01-24 11:13:32 +0800595 RX_COUNT_DIFF(total);
596 RX_COUNT_DIFF(err_cnt);
597 RX_COUNT_DIFF(len_mismatch);
598 RX_COUNT_DIFF(ok_cnt);
599#undef RX_COUNT_DIFF
600
601 memcpy(&anb->rx_stat, &rx_cur, sizeof(anb->rx_stat));
602 }
603
604 rx_band->mac_rx_mdrdy_cnt = htonl((u32)rx_diff.total);
605 rx_band->mac_rx_fcs_err_cnt = htonl((u32)rx_diff.err_cnt);
606 rx_band->mac_rx_fcs_ok_cnt = htonl((u32)rx_diff.ok_cnt);
607 rx_band->mac_rx_len_mismatch = htonl((u32)rx_diff.len_mismatch);
608 rx_user->fcs_error_cnt = htonl((u32)rx_diff.err_cnt);
609
610 nla_parse_nested(tb3, MT76_TM_RX_ATTR_MAX,
611 tb2[MT76_TM_STATS_ATTR_LAST_RX], rx_policy);
612
613 rx_user->freq_offset = htonl(nla_get_u32(tb3[MT76_TM_RX_ATTR_FREQ_OFFSET]));
614 rx_user->snr = htonl(nla_get_u8(tb3[MT76_TM_RX_ATTR_SNR]));
615
616 i = 0;
617 nla_for_each_nested(cur, tb3[MT76_TM_RX_ATTR_RCPI], rem) {
618 if (nla_len(cur) != 1 || i >= 4)
619 break;
620
621 rcpi[i++] = nla_get_u8(cur);
622 }
623
624 i = 0;
625 nla_for_each_nested(cur, tb3[MT76_TM_RX_ATTR_IB_RSSI], rem) {
626 if (nla_len(cur) != 1 || i >= 4)
627 break;
628
629 ib_rssi[i++] = (s8)nla_get_u8(cur);
630 }
631
632 i = 0;
633 nla_for_each_nested(cur, tb3[MT76_TM_RX_ATTR_WB_RSSI], rem) {
634 if (nla_len(cur) != 1 || i >= 4)
635 break;
636
637 wb_rssi[i++] = (s8)nla_get_u8(cur);
638 }
639
640 for (i = 0; i < 4; i++) {
641 struct atenl_rx_info_path *path = &rx_path[i];
642
643 path->rcpi = htonl(rcpi[i]);
644 path->rssi = htonl(to_rssi((u8)rcpi[i]));
645 path->fagc_ib_rssi = htonl(ib_rssi[i]);
646 path->fagc_wb_rssi = htonl(wb_rssi[i]);
647 }
648
649 return NL_SKIP;
650}
651
652static int atenl_nl_get_rx_info(struct atenl *an, struct atenl_data *data,
653 struct atenl_nl_priv *nl_priv)
654{
655 struct nl_msg *msg = nl_priv->msg;
656 void *ptr;
657
658 nl_priv->an = an;
659 nl_priv->res = (void *)data;
660
661 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
662 if (!ptr)
663 return -ENOMEM;
664
665 nla_put_flag(msg, MT76_TM_ATTR_STATS);
666
667 nla_nest_end(msg, ptr);
668
669 return unl_genl_request(&nl_priv->unl, msg, atenl_nl_get_rx_info_cb,
670 (void *)nl_priv);
671}
672
673static int
674atenl_nl_set_ru(struct atenl *an, struct atenl_data *data,
675 struct atenl_nl_priv *nl_priv)
676{
677 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
678 struct nl_msg *msg;
679 u32 *v = (u32 *)(hdr->data + 4);
680 u32 seg0_num = ntohl(v[0]); /* v[1] seg1_num unused */
681 void *ptr;
682 int i, ret;
683
684 if (seg0_num > 8)
685 return -EINVAL;
686
687 for (i = 0, v = &v[2]; i < seg0_num; i++, v += 11) {
688 u32 ru_alloc = ntohl(v[1]);
689 u32 aid = ntohl(v[2]);
690 u32 ru_idx = ntohl(v[3]);
691 u32 mcs = ntohl(v[4]);
692 u32 ldpc = ntohl(v[5]);
693 u32 nss = ntohl(v[6]);
694 u32 tx_length = ntohl(v[8]);
695 char buf[10];
696
697 if (unl_genl_init(&nl_priv->unl, "nl80211") < 0) {
developer5698c9c2022-05-30 16:40:23 +0800698 atenl_err("Failed to connect to nl80211\n");
developer3abe1ad2022-01-24 11:13:32 +0800699 return 2;
700 }
701
702 msg = unl_genl_msg(&nl_priv->unl, NL80211_CMD_TESTMODE, false);
703 nla_put_u32(msg, NL80211_ATTR_WIPHY, get_band_val(an, an->cur_band, phy_idx));
704
705 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
706 if (!ptr)
707 return -ENOMEM;
708
709 if (i == 0)
710 atenl_set_attr_state(an, msg, an->cur_band, MT76_TM_STATE_IDLE);
711
712 nla_put_u8(msg, MT76_TM_ATTR_AID, aid);
713 nla_put_u8(msg, MT76_TM_ATTR_RU_IDX, ru_idx);
714 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, mcs);
715 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_LDPC, ldpc);
716 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_NSS, nss);
developer93dadcc2022-07-13 10:25:35 +0800717 nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH, tx_length);
developer3abe1ad2022-01-24 11:13:32 +0800718
719 ret = snprintf(buf, sizeof(buf), "%x", ru_alloc);
720 if (snprintf_error(sizeof(buf), ret))
721 return -EINVAL;
722
723 nla_put_u8(msg, MT76_TM_ATTR_RU_ALLOC, strtol(buf, NULL, 2));
724
725 nla_nest_end(msg, ptr);
726
727 unl_genl_request(&nl_priv->unl, msg, NULL, NULL);
728
729 unl_free(&nl_priv->unl);
730 }
731
732 return 0;
733}
734
developer5698c9c2022-05-30 16:40:23 +0800735static int
736atenl_nl_ibf_init(struct atenl *an, u8 band)
737{
738 struct atenl_nl_priv nl_priv = {};
739 struct nl_msg *msg;
740 void *ptr, *a;
741 int ret;
742
743 if (unl_genl_init(&nl_priv.unl, "nl80211") < 0) {
744 atenl_err("Failed to connect to nl80211\n");
745 return 2;
746 }
747
748 msg = unl_genl_msg(&nl_priv.unl, NL80211_CMD_TESTMODE, false);
749 nla_put_u32(msg, NL80211_ATTR_WIPHY, get_band_val(an, band, phy_idx));
750
751 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
752 if (!ptr) {
753 ret = -ENOMEM;
754 goto out;
755 }
756
757 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_MODE, MT76_TM_TX_MODE_HT);
758 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, an->ibf_mcs);
759 nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, an->ibf_ant);
760 nla_put_u8(msg, MT76_TM_ATTR_TXBF_ACT, MT76_TM_TXBF_ACT_INIT);
761
762 a = nla_nest_start(msg, MT76_TM_ATTR_TXBF_PARAM);
763 if (!a) {
764 ret = -ENOMEM;
765 goto out;
766 }
767 nla_put_u16(msg, 0, 1);
768 nla_nest_end(msg, a);
769
770 nla_nest_end(msg, ptr);
771
772 ret = unl_genl_request(&nl_priv.unl, msg, NULL, NULL);
773
774out:
775 unl_free(&nl_priv.unl);
776 return ret;
777}
778
779static int
780atenl_nl_ibf_e2p_update(struct atenl *an)
781{
782 struct atenl_nl_priv nl_priv = {};
783 struct nl_msg *msg;
784 void *ptr, *a;
785 int ret;
786
787 if (unl_genl_init(&nl_priv.unl, "nl80211") < 0) {
788 atenl_err("Failed to connect to nl80211\n");
789 return 2;
790 }
791
792 msg = unl_genl_msg(&nl_priv.unl, NL80211_CMD_TESTMODE, false);
793 nla_put_u32(msg, NL80211_ATTR_WIPHY, get_band_val(an, an->cur_band, phy_idx));
794
795 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
796 if (!ptr) {
797 ret = -ENOMEM;
798 goto out;
799 }
800
801 nla_put_u8(msg, MT76_TM_ATTR_TXBF_ACT, MT76_TM_TXBF_ACT_E2P_UPDATE);
802 a = nla_nest_start(msg, MT76_TM_ATTR_TXBF_PARAM);
803 if (!a) {
804 ret = -ENOMEM;
805 goto out;
806 }
807 nla_put_u16(msg, 0, 0);
808 nla_nest_end(msg, a);
809
810 nla_nest_end(msg, ptr);
811
812 ret = unl_genl_request(&nl_priv.unl, msg, NULL, NULL);
813
814out:
815 unl_free(&nl_priv.unl);
816 return ret;
817}
818
819static void
820atenl_get_ibf_cal_result(struct atenl *an)
821{
822 u16 offset;
823
824 if (an->adie_id == 0x7975)
825 offset = 0x651;
826 else if (an->adie_id == 0x7976)
827 offset = 0x60a;
828
829 /* per group size = 40, for group 0-8 */
830 atenl_eeprom_read_from_driver(an, offset, 40 * 9);
831}
832
833static int
834atenl_nl_ibf_set_val(struct atenl *an, struct atenl_data *data,
835 struct atenl_nl_priv *nl_priv)
developer3abe1ad2022-01-24 11:13:32 +0800836{
837#define MT_IBF(_act) MT76_TM_TXBF_ACT_##_act
838 static const u8 bf_act_map[] = {
839 [TXBF_ACT_IBF_PHASE_COMP] = MT_IBF(PHASE_COMP),
840 [TXBF_ACT_IBF_PROF_UPDATE] = MT_IBF(IBF_PROF_UPDATE),
841 [TXBF_ACT_EBF_PROF_UPDATE] = MT_IBF(EBF_PROF_UPDATE),
842 [TXBF_ACT_IBF_PHASE_CAL] = MT_IBF(PHASE_CAL),
843 };
844#undef MT_IBF
845 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
846 struct nl_msg *msg = nl_priv->msg;
847 u32 *v = (u32 *)(hdr->data + 4);
848 u32 action = ntohl(v[0]);
developerf9843e22022-09-13 10:57:15 +0800849 u16 val[8], is_atenl = 1;
developer5698c9c2022-05-30 16:40:23 +0800850 u8 tmp_ant;
developer3abe1ad2022-01-24 11:13:32 +0800851 void *ptr, *a;
852 char cmd[64];
853 int i;
854
855 for (i = 0; i < 8; i++)
856 val[i] = ntohl(v[i + 1]);
857
858 atenl_dbg("%s: action = %u, val = %u, %u, %u, %u, %u\n",
859 __func__, action, val[0], val[1], val[2], val[3], val[4]);
860
861 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
862 if (!ptr)
863 return -ENOMEM;
864
865 switch (action) {
developer3abe1ad2022-01-24 11:13:32 +0800866 case TXBF_ACT_CHANNEL:
developer5698c9c2022-05-30 16:40:23 +0800867 an->cur_band = val[1];
868 /* a sanity to prevent script band idx error */
869 if (val[0] > 14)
870 an->cur_band = 1;
871 atenl_nl_ibf_init(an, an->cur_band);
872 atenl_set_channel(an, 0, an->cur_band, val[0], 0, 0);
873
874 nla_put_u8(msg, MT76_TM_ATTR_AID, 0);
developer3abe1ad2022-01-24 11:13:32 +0800875 nla_put_u8(msg, MT76_TM_ATTR_TXBF_ACT, MT76_TM_TXBF_ACT_UPDATE_CH);
876 a = nla_nest_start(msg, MT76_TM_ATTR_TXBF_PARAM);
877 if (!a)
878 return -ENOMEM;
879 nla_put_u16(msg, 0, 0);
880 nla_nest_end(msg, a);
881 break;
882 case TXBF_ACT_MCS:
developer5698c9c2022-05-30 16:40:23 +0800883 tmp_ant = (1 << DIV_ROUND_UP(val[0], 8)) - 1 ?: 1;
884 /* sometimes the correct band idx will be set after this action,
885 * so maintain a temp variable to allow mcs update in anthor action.
886 */
887 an->ibf_mcs = val[0];
888 an->ibf_ant = tmp_ant;
889 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, an->ibf_mcs);
890 nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, an->ibf_ant);
developer3abe1ad2022-01-24 11:13:32 +0800891 break;
892 case TXBF_ACT_TX_ANT:
893 nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, val[0]);
894 break;
895 case TXBF_ACT_RX_START:
896 atenl_set_attr_state(an, msg, an->cur_band, MT76_TM_STATE_RX_FRAMES);
897 break;
898 case TXBF_ACT_RX_ANT:
899 nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, val[0]);
900 break;
901 case TXBF_ACT_TX_PKT:
902 nla_put_u8(msg, MT76_TM_ATTR_AID, val[1]);
developer3abe1ad2022-01-24 11:13:32 +0800903 nla_put_u8(msg, MT76_TM_ATTR_TXBF_ACT, MT76_TM_TXBF_ACT_TX_PREP);
developer5698c9c2022-05-30 16:40:23 +0800904 nla_put_u32(msg, MT76_TM_ATTR_TX_COUNT, 10000000);
developer93dadcc2022-07-13 10:25:35 +0800905 nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH, 1024);
developer3abe1ad2022-01-24 11:13:32 +0800906 a = nla_nest_start(msg, MT76_TM_ATTR_TXBF_PARAM);
907 if (!a)
908 return -ENOMEM;
909
910 for (i = 0; i < 5; i++)
911 nla_put_u16(msg, i, val[i]);
912 nla_nest_end(msg, a);
913
914 atenl_set_attr_state(an, msg, an->cur_band, MT76_TM_STATE_TX_FRAMES);
915 break;
916 case TXBF_ACT_IBF_PHASE_COMP:
developer5698c9c2022-05-30 16:40:23 +0800917 nla_put_u8(msg, MT76_TM_ATTR_AID, 1);
developer3abe1ad2022-01-24 11:13:32 +0800918 case TXBF_ACT_IBF_PROF_UPDATE:
919 case TXBF_ACT_EBF_PROF_UPDATE:
920 case TXBF_ACT_IBF_PHASE_CAL:
921 nla_put_u8(msg, MT76_TM_ATTR_TXBF_ACT, bf_act_map[action]);
922 a = nla_nest_start(msg, MT76_TM_ATTR_TXBF_PARAM);
923 if (!a)
924 return -ENOMEM;
developerf9843e22022-09-13 10:57:15 +0800925 /* Note: litepoint may send random number for lna_gain_level, reset to 0 */
926 if (action == TXBF_ACT_IBF_PHASE_CAL)
927 val[4] = 0;
developer3abe1ad2022-01-24 11:13:32 +0800928 for (i = 0; i < 5; i++)
929 nla_put_u16(msg, i, val[i]);
developerf9843e22022-09-13 10:57:15 +0800930 /* Used to distinguish between command mode and HQADLL mode */
931 nla_put_u16(msg, 5, is_atenl);
developer3abe1ad2022-01-24 11:13:32 +0800932 nla_nest_end(msg, a);
933 break;
934 case TXBF_ACT_IBF_PHASE_E2P_UPDATE:
developer5698c9c2022-05-30 16:40:23 +0800935 atenl_nl_ibf_e2p_update(an);
936 atenl_get_ibf_cal_result(an);
developer3abe1ad2022-01-24 11:13:32 +0800937
938 nla_put_u8(msg, MT76_TM_ATTR_AID, 0);
939 nla_put_u8(msg, MT76_TM_ATTR_TXBF_ACT, MT76_TM_TXBF_ACT_INIT);
940
941 a = nla_nest_start(msg, MT76_TM_ATTR_TXBF_PARAM);
942 if (!a)
943 return -ENOMEM;
944 nla_put_u16(msg, 0, 0);
945 nla_nest_end(msg, a);
946 break;
developer5698c9c2022-05-30 16:40:23 +0800947 case TXBF_ACT_INIT:
948 case TXBF_ACT_POWER:
developer3abe1ad2022-01-24 11:13:32 +0800949 default:
950 break;
951 }
952
953 nla_nest_end(msg, ptr);
954
developer5698c9c2022-05-30 16:40:23 +0800955 *(u32 *)(hdr->data + 2) = data->ext_id;
developer3abe1ad2022-01-24 11:13:32 +0800956
957 return unl_genl_request(&nl_priv->unl, msg, NULL, NULL);
958}
959
960static int
961atenl_nl_ibf_get_status(struct atenl *an, struct atenl_data *data,
962 struct atenl_nl_priv *nl_priv)
963{
964 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
965 u32 status = htonl(1);
966
developer5698c9c2022-05-30 16:40:23 +0800967 *(u32 *)(hdr->data + 2) = data->ext_id;
developer3abe1ad2022-01-24 11:13:32 +0800968 memcpy(hdr->data + 6, &status, 4);
969
970 return 0;
971}
972
973static int
974atenl_nl_ibf_profile_update_all(struct atenl *an, struct atenl_data *data,
975 struct atenl_nl_priv *nl_priv)
976{
977 struct atenl_cmd_hdr *hdr = atenl_hdr(data);
978 struct nl_msg *msg;
979 void *ptr, *a;
980 u32 *v = (u32 *)(hdr->data + 4);
981 u16 pfmu_idx = ntohl(v[0]);
982 int i;
983
984 for (i = 0, v = &v[5]; i < 64; i++, v += 5) {
985 int j;
986
987 if (unl_genl_init(&nl_priv->unl, "nl80211") < 0) {
developer5698c9c2022-05-30 16:40:23 +0800988 atenl_err("Failed to connect to nl80211\n");
developer3abe1ad2022-01-24 11:13:32 +0800989 return 2;
990 }
991
992 msg = unl_genl_msg(&nl_priv->unl, NL80211_CMD_TESTMODE, false);
993 nla_put_u32(msg, NL80211_ATTR_WIPHY,
994 get_band_val(an, an->cur_band, phy_idx));
995
996 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
997 if (!ptr)
998 return -ENOMEM;
999
1000 nla_put_u8(msg, MT76_TM_ATTR_TXBF_ACT, MT76_TM_TXBF_ACT_PROF_UPDATE_ALL);
1001 a = nla_nest_start(msg, MT76_TM_ATTR_TXBF_PARAM);
1002 if (!a)
1003 return -ENOMEM;
1004 nla_put_u16(msg, 0, pfmu_idx);
1005
1006 for (j = 0; j < 5; j++)
1007 nla_put_u16(msg, j + 1, ntohl(v[j]));
1008 nla_nest_end(msg, a);
1009
1010 nla_nest_end(msg, ptr);
1011
1012 unl_genl_request(&nl_priv->unl, msg, NULL, NULL);
1013
1014 unl_free(&nl_priv->unl);
1015 }
1016
developer5698c9c2022-05-30 16:40:23 +08001017 *(u32 *)(hdr->data + 2) = data->ext_id;
developer3abe1ad2022-01-24 11:13:32 +08001018
1019 return 0;
1020}
1021
1022#define NL_OPS_GROUP(cmd, ...) [HQA_CMD_##cmd] = { __VA_ARGS__ }
1023static const struct atenl_nl_ops nl_ops[] = {
1024 NL_OPS_GROUP(SET_TX_PATH, .set=MT76_TM_ATTR_TX_ANTENNA),
1025 NL_OPS_GROUP(SET_TX_POWER, .set=MT76_TM_ATTR_TX_POWER),
1026 NL_OPS_GROUP(SET_RX_PATH, .set=MT76_TM_ATTR_TX_ANTENNA),
1027 NL_OPS_GROUP(SET_FREQ_OFFSET, .set=MT76_TM_ATTR_FREQ_OFFSET),
1028 NL_OPS_GROUP(SET_CFG, .ops=atenl_nl_set_cfg),
1029 NL_OPS_GROUP(SET_TSSI, .ops=atenl_nl_set_cfg),
1030 NL_OPS_GROUP(CONTINUOUS_TX, .ops=atenl_nl_continuous_tx),
1031 NL_OPS_GROUP(GET_TX_INFO, .dump=MT76_TM_STATS_ATTR_TX_DONE),
1032 NL_OPS_GROUP(GET_RX_INFO, .ops=atenl_nl_get_rx_info, .dump=true),
1033 NL_OPS_GROUP(SET_RU, .ops=atenl_nl_set_ru),
1034};
1035#undef NL_OPS_GROUP
1036
1037#define NL_OPS_EXT(cmd, ...) [HQA_EXT_CMD_##cmd] = { __VA_ARGS__ }
1038static const struct atenl_nl_ops nl_ops_ext[] = {
1039 NL_OPS_EXT(SET_TX, .ops=atenl_nl_set_tx),
1040 NL_OPS_EXT(START_TX, .ops=atenl_nl_tx),
1041 NL_OPS_EXT(STOP_TX, .ops=atenl_nl_tx),
1042 NL_OPS_EXT(START_RX, .ops=atenl_nl_rx),
1043 NL_OPS_EXT(STOP_RX, .ops=atenl_nl_rx),
1044 NL_OPS_EXT(OFF_CH_SCAN, .ops=atenl_off_ch_scan),
1045 NL_OPS_EXT(IBF_SET_VAL, .ops=atenl_nl_ibf_set_val),
1046 NL_OPS_EXT(IBF_GET_STATUS, .ops=atenl_nl_ibf_get_status),
1047 NL_OPS_EXT(IBF_PROF_UPDATE_ALL, .ops=atenl_nl_ibf_profile_update_all),
1048};
1049#undef NL_OPS_EXT
1050
1051int atenl_nl_process(struct atenl *an, struct atenl_data *data)
1052{
1053 struct atenl_nl_priv nl_priv = {};
1054 const struct atenl_nl_ops *ops;
1055 struct nl_msg *msg;
1056 int ret = 0;
1057
1058 if (data->ext_cmd != 0)
1059 ops = &nl_ops_ext[data->ext_cmd];
1060 else
1061 ops = &nl_ops[data->cmd];
1062
1063 if (unl_genl_init(&nl_priv.unl, "nl80211") < 0) {
developer5698c9c2022-05-30 16:40:23 +08001064 atenl_err("Failed to connect to nl80211\n");
developer3abe1ad2022-01-24 11:13:32 +08001065 return -1;
1066 }
1067
1068 msg = unl_genl_msg(&nl_priv.unl, NL80211_CMD_TESTMODE, !!ops->dump);
1069 nla_put_u32(msg, NL80211_ATTR_WIPHY, get_band_val(an, an->cur_band, phy_idx));
1070 nl_priv.msg = msg;
1071
1072 if (ops->ops) {
1073 ret = ops->ops(an, data, &nl_priv);
1074 } else if (ops->dump) {
1075 nl_priv.attr = ops->dump;
1076 ret = atenl_nl_dump_attr(an, data, &nl_priv);
1077 } else {
1078 nl_priv.attr = ops->set;
1079 ret = atenl_nl_set_attr(an, data, &nl_priv);
1080 }
1081
1082 if (ret)
developer5698c9c2022-05-30 16:40:23 +08001083 atenl_err("command process error: 0x%x (0x%x)\n", data->cmd_id, data->ext_id);
developer3abe1ad2022-01-24 11:13:32 +08001084
1085 unl_free(&nl_priv.unl);
1086
1087 return ret;
1088}
1089
1090int atenl_nl_process_many(struct atenl *an, struct atenl_data *data)
1091{
1092 struct atenl_nl_priv nl_priv = {};
1093 const struct atenl_nl_ops *ops;
1094 int ret = 0;
1095
1096 if (data->ext_cmd != 0)
1097 ops = &nl_ops_ext[data->ext_cmd];
1098 else
1099 ops = &nl_ops[data->cmd];
1100
1101 if (ops->ops)
1102 ret = ops->ops(an, data, &nl_priv);
1103
1104 return ret;
1105}
1106
1107int atenl_nl_set_state(struct atenl *an, u8 band,
1108 enum mt76_testmode_state state)
1109{
1110 struct atenl_nl_priv nl_priv = {};
1111 struct nl_msg *msg;
1112 void *ptr;
1113
1114 if (unl_genl_init(&nl_priv.unl, "nl80211") < 0) {
developer5698c9c2022-05-30 16:40:23 +08001115 atenl_err("Failed to connect to nl80211\n");
developer3abe1ad2022-01-24 11:13:32 +08001116 return 2;
1117 }
1118
1119 msg = unl_genl_msg(&nl_priv.unl, NL80211_CMD_TESTMODE, false);
1120 nla_put_u32(msg, NL80211_ATTR_WIPHY, get_band_val(an, band, phy_idx));
1121
1122 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
1123 if (!ptr)
1124 return -ENOMEM;
1125
1126 atenl_set_attr_state(an, msg, band, state);
1127
1128 nla_nest_end(msg, ptr);
1129
1130 unl_genl_request(&nl_priv.unl, msg, NULL, NULL);
1131
1132 unl_free(&nl_priv.unl);
1133
1134 return 0;
1135}
1136
developer5698c9c2022-05-30 16:40:23 +08001137int atenl_nl_set_aid(struct atenl *an, u8 band, u8 aid)
1138{
1139 struct atenl_nl_priv nl_priv = {};
1140 struct nl_msg *msg;
1141 void *ptr;
1142
1143 if (unl_genl_init(&nl_priv.unl, "nl80211") < 0) {
1144 atenl_err("Failed to connect to nl80211\n");
1145 return 2;
1146 }
1147
1148 msg = unl_genl_msg(&nl_priv.unl, NL80211_CMD_TESTMODE, false);
1149 nla_put_u32(msg, NL80211_ATTR_WIPHY, get_band_val(an, band, phy_idx));
1150
1151 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
1152 if (!ptr)
1153 return -ENOMEM;
1154
1155 nla_put_u8(msg, MT76_TM_ATTR_AID, aid);
1156
1157 nla_nest_end(msg, ptr);
1158
1159 unl_genl_request(&nl_priv.unl, msg, NULL, NULL);
1160
1161 unl_free(&nl_priv.unl);
1162
1163 return 0;
1164}
1165
developer3abe1ad2022-01-24 11:13:32 +08001166static int atenl_nl_check_mtd_cb(struct nl_msg *msg, void *arg)
1167{
1168 struct atenl_nl_priv *nl_priv = (struct atenl_nl_priv *)arg;
1169 struct atenl *an = nl_priv->an;
1170 struct nlattr *tb[NUM_MT76_TM_ATTRS];
1171 struct nlattr *attr;
1172
1173 attr = unl_find_attr(&nl_priv->unl, msg, NL80211_ATTR_TESTDATA);
1174 if (!attr)
1175 return NL_SKIP;
1176
1177 nla_parse_nested(tb, MT76_TM_ATTR_MAX, attr, testdata_policy);
1178 if (!tb[MT76_TM_ATTR_MTD_PART] || !tb[MT76_TM_ATTR_MTD_OFFSET])
1179 return NL_SKIP;
1180
1181 an->mtd_part = strdup(nla_get_string(tb[MT76_TM_ATTR_MTD_PART]));
1182 an->mtd_offset = nla_get_u32(tb[MT76_TM_ATTR_MTD_OFFSET]);
developerf90c9af2022-12-28 22:40:23 +08001183 an->band_idx = nla_get_u32(tb[MT76_TM_ATTR_BAND_IDX]);
developer3abe1ad2022-01-24 11:13:32 +08001184
1185 return NL_SKIP;
1186}
1187
1188int atenl_nl_check_mtd(struct atenl *an)
1189{
1190 struct atenl_nl_priv nl_priv = { .an = an };
1191 struct nl_msg *msg;
1192
1193 if (unl_genl_init(&nl_priv.unl, "nl80211") < 0) {
developer5698c9c2022-05-30 16:40:23 +08001194 atenl_err("Failed to connect to nl80211\n");
developer3abe1ad2022-01-24 11:13:32 +08001195 return 2;
1196 }
1197
1198 msg = unl_genl_msg(&nl_priv.unl, NL80211_CMD_TESTMODE, true);
1199 nla_put_u32(msg, NL80211_ATTR_WIPHY, get_band_val(an, 0, phy_idx));
1200 unl_genl_request(&nl_priv.unl, msg, atenl_nl_check_mtd_cb, (void *)&nl_priv);
1201
1202 unl_free(&nl_priv.unl);
1203
1204 return 0;
1205}
1206
1207int atenl_nl_write_eeprom(struct atenl *an, u32 offset, u8 *val, int len)
1208{
1209 struct atenl_nl_priv nl_priv = {};
1210 struct nl_msg *msg;
1211 void *ptr, *a;
1212 int i;
1213
1214 if (unl_genl_init(&nl_priv.unl, "nl80211") < 0) {
developer5698c9c2022-05-30 16:40:23 +08001215 atenl_err("Failed to connect to nl80211\n");
developer3abe1ad2022-01-24 11:13:32 +08001216 return 2;
1217 }
1218
1219 if (len > 16)
1220 return -EINVAL;
1221
1222 msg = unl_genl_msg(&nl_priv.unl, NL80211_CMD_TESTMODE, false);
1223 nla_put_u32(msg, NL80211_ATTR_WIPHY, get_band_val(an, 0, phy_idx));
1224
1225 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
1226 if (!ptr)
1227 return -ENOMEM;
1228
1229 nla_put_u8(msg, MT76_TM_ATTR_EEPROM_ACTION,
1230 MT76_TM_EEPROM_ACTION_UPDATE_DATA);
1231 nla_put_u32(msg, MT76_TM_ATTR_EEPROM_OFFSET, offset);
1232
1233 a = nla_nest_start(msg, MT76_TM_ATTR_EEPROM_VAL);
1234 if (!a)
1235 return -ENOMEM;
1236
1237 for (i = 0; i < len; i++)
1238 if (nla_put_u8(msg, i, val[i]))
1239 goto out;
1240
1241 nla_nest_end(msg, a);
1242
1243 nla_nest_end(msg, ptr);
1244
1245 unl_genl_request(&nl_priv.unl, msg, NULL, NULL);
1246
1247 unl_free(&nl_priv.unl);
1248
1249out:
1250 return 0;
1251}
1252
developer9b7cdad2022-03-10 14:24:55 +08001253int atenl_nl_write_efuse_all(struct atenl *an)
developer3abe1ad2022-01-24 11:13:32 +08001254{
1255 struct atenl_nl_priv nl_priv = {};
1256 struct nl_msg *msg;
1257 void *ptr;
1258
1259 if (unl_genl_init(&nl_priv.unl, "nl80211") < 0) {
developer5698c9c2022-05-30 16:40:23 +08001260 atenl_err("Failed to connect to nl80211\n");
developer3abe1ad2022-01-24 11:13:32 +08001261 return 2;
1262 }
1263
1264 msg = unl_genl_msg(&nl_priv.unl, NL80211_CMD_TESTMODE, false);
1265 nla_put_u32(msg, NL80211_ATTR_WIPHY, get_band_val(an, 0, phy_idx));
1266
1267 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
1268 if (!ptr)
1269 return -ENOMEM;
1270
1271 nla_put_u8(msg, MT76_TM_ATTR_EEPROM_ACTION,
1272 MT76_TM_EEPROM_ACTION_WRITE_TO_EFUSE);
1273
1274 nla_nest_end(msg, ptr);
1275
1276 unl_genl_request(&nl_priv.unl, msg, NULL, NULL);
1277
1278 unl_free(&nl_priv.unl);
1279
1280 return 0;
1281}
1282
1283int atenl_nl_update_buffer_mode(struct atenl *an)
1284{
1285 struct atenl_nl_priv nl_priv = {};
1286 struct nl_msg *msg;
1287 void *ptr;
1288
1289 if (unl_genl_init(&nl_priv.unl, "nl80211") < 0) {
developer5698c9c2022-05-30 16:40:23 +08001290 atenl_err("Failed to connect to nl80211\n");
developer3abe1ad2022-01-24 11:13:32 +08001291 return 2;
1292 }
1293
1294 msg = unl_genl_msg(&nl_priv.unl, NL80211_CMD_TESTMODE, false);
1295 nla_put_u32(msg, NL80211_ATTR_WIPHY, get_band_val(an, 0, phy_idx));
1296
1297 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
1298 if (!ptr)
1299 return -ENOMEM;
1300
1301 nla_put_u8(msg, MT76_TM_ATTR_EEPROM_ACTION,
1302 MT76_TM_EEPROM_ACTION_UPDATE_BUFFER_MODE);
1303
1304 nla_nest_end(msg, ptr);
1305
1306 unl_genl_request(&nl_priv.unl, msg, NULL, NULL);
1307
1308 unl_free(&nl_priv.unl);
1309
1310 return 0;
1311}
1312
developer071927d2022-08-31 20:39:29 +08001313static int atenl_nl_precal_sync_from_driver_cb(struct nl_msg *msg, void *arg)
1314{
1315 struct atenl_nl_priv *nl_priv = (struct atenl_nl_priv *)arg;
1316 struct atenl *an = nl_priv->an;
1317 struct nlattr *tb[NUM_MT76_TM_ATTRS];
1318 struct nlattr *attr, *cur;
1319 int i, rem, prek_offset = nl_priv->attr;
1320
1321
1322 attr = unl_find_attr(&nl_priv->unl, msg, NL80211_ATTR_TESTDATA);
1323 if (!attr)
1324 return NL_SKIP;
1325
1326 nla_parse_nested(tb, MT76_TM_ATTR_MAX, attr, testdata_policy);
1327
1328 if (!tb[MT76_TM_ATTR_PRECAL_INFO] && !tb[MT76_TM_ATTR_PRECAL]) {
1329 atenl_info("No Pre cal data or info!\n");
1330 return NL_SKIP;
1331 }
1332
1333 if (tb[MT76_TM_ATTR_PRECAL_INFO]) {
1334 i = 0;
1335 nla_for_each_nested(cur, tb[MT76_TM_ATTR_PRECAL_INFO], rem) {
1336 an->cal_info[i] = (u32) nla_get_u32(cur);
1337 i++;
1338 }
1339 return NL_SKIP;
1340 }
1341
1342 if (tb[MT76_TM_ATTR_PRECAL] && an->cal) {
1343 i = prek_offset;
1344 nla_for_each_nested(cur, tb[MT76_TM_ATTR_PRECAL], rem) {
1345 an->cal[i] = (u8) nla_get_u8(cur);
1346 i++;
1347 }
1348 return NL_SKIP;
1349 }
1350 atenl_info("No data found for pre-cal!\n");
1351
1352 return NL_SKIP;
1353}
1354
1355static int
1356atenl_nl_precal_sync_partition(struct atenl_nl_priv *nl_priv, enum mt76_testmode_attr attr,
1357 int prek_type, int prek_offset)
1358{
1359 int ret;
1360 void *ptr;
1361 struct nl_msg *msg;
1362 struct atenl *an = nl_priv->an;
1363
1364 msg = unl_genl_msg(&(nl_priv->unl), NL80211_CMD_TESTMODE, true);
1365 nla_put_u32(msg, NL80211_ATTR_WIPHY, get_band_val(an, an->cur_band, phy_idx));
1366 nl_priv->msg = msg;
1367 nl_priv->attr = prek_offset;
1368
1369 ptr = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
1370 if (!ptr)
1371 return -ENOMEM;
1372
1373 nla_put_flag(msg, attr);
1374 if (attr == MT76_TM_ATTR_PRECAL)
1375 nla_put_u8(msg, MT76_TM_ATTR_PRECAL_INFO, prek_type);
1376 nla_nest_end(msg, ptr);
1377
1378 ret = unl_genl_request(&(nl_priv->unl), msg, atenl_nl_precal_sync_from_driver_cb, (void *)nl_priv);
1379
1380 if (ret) {
1381 atenl_err("command process error!\n");
1382 return ret;
1383 }
1384
1385 return 0;
1386}
1387
1388int atenl_nl_precal_sync_from_driver(struct atenl *an, enum prek_ops ops)
1389{
1390#define GROUP_IND_MASK BIT(0)
1391#define DPD_IND_MASK GENMASK(3, 1)
1392 int ret;
1393 u32 i, times, group_size, dpd_size, total_size, transmit_size, offs;
1394 u32 dpd_per_chan_size, dpd_chan_num[3], total_chan_num;
1395 u32 size, base, base_idx, *size_ptr;
1396 u8 cal_indicator, *precal_info;
1397 struct atenl_nl_priv nl_priv = { .an = an };
1398
1399 offs = an->eeprom_prek_offs;
1400 cal_indicator = an->eeprom_data[offs];
1401
1402 if (cal_indicator) {
1403 precal_info = an->eeprom_data + an->eeprom_size;
1404 memcpy(an->cal_info, precal_info, PRE_CAL_INFO);
1405 group_size = an->cal_info[0];
1406 dpd_size = an->cal_info[1];
1407 total_size = group_size + dpd_size;
1408 dpd_chan_num[0] = (an->cal_info[2] >> DPD_INFO_6G_SHIFT) & DPD_INFO_MASK;
1409 dpd_chan_num[1] = (an->cal_info[2] >> DPD_INFO_5G_SHIFT) & DPD_INFO_MASK;
1410 dpd_chan_num[2] = (an->cal_info[2] >> DPD_INFO_2G_SHIFT) & DPD_INFO_MASK;
1411 dpd_per_chan_size = (an->cal_info[2] >> DPD_INFO_CH_SHIFT) & DPD_INFO_MASK;
1412 total_chan_num = dpd_chan_num[0] + dpd_chan_num[1] + dpd_chan_num[2];
1413 }
1414
1415 switch (ops){
1416 case PREK_SYNC_ALL:
1417 size_ptr = &total_size;
1418 base_idx = 0;
1419 goto start;
1420 case PREK_SYNC_GROUP:
1421 size_ptr = &group_size;
1422 base_idx = 0;
1423 goto start;
1424 case PREK_SYNC_DPD_6G:
1425 size_ptr = &dpd_size;
1426 base_idx = 0;
1427 goto start;
1428 case PREK_SYNC_DPD_5G:
1429 size_ptr = &dpd_size;
1430 base_idx = 1;
1431 goto start;
1432 case PREK_SYNC_DPD_2G:
1433 size_ptr = &dpd_size;
1434 base_idx = 2;
1435
1436start:
1437 if (unl_genl_init(&nl_priv.unl, "nl80211") < 0) {
1438 atenl_err("Failed to connect to nl80211\n");
1439 return 2;
1440 }
1441
1442 ret = atenl_nl_precal_sync_partition(&nl_priv, MT76_TM_ATTR_PRECAL_INFO, 0, 0);
1443 if (ret || !an->cal_info)
1444 goto out;
1445
1446 group_size = an->cal_info[0];
1447 dpd_size = an->cal_info[1];
1448 total_size = group_size + dpd_size;
1449 dpd_chan_num[0] = (an->cal_info[2] >> DPD_INFO_6G_SHIFT) & DPD_INFO_MASK;
1450 dpd_chan_num[1] = (an->cal_info[2] >> DPD_INFO_5G_SHIFT) & DPD_INFO_MASK;
1451 dpd_chan_num[2] = (an->cal_info[2] >> DPD_INFO_2G_SHIFT) & DPD_INFO_MASK;
1452 dpd_per_chan_size = (an->cal_info[2] >> DPD_INFO_CH_SHIFT) & DPD_INFO_MASK;
1453 total_chan_num = dpd_chan_num[0] + dpd_chan_num[1] + dpd_chan_num[2];
1454 transmit_size = an->cal_info[3];
1455
1456 size = *size_ptr;
1457 size = (size_ptr == &dpd_size) ? (size / total_chan_num * dpd_chan_num[base_idx]) :
1458 size;
1459 base = 0;
1460 for (i = 0; i < base_idx; i++) {
1461 base += dpd_chan_num[i] * dpd_per_chan_size * MT_EE_CAL_UNIT;
1462 }
1463 base += (size_ptr == &dpd_size) ? group_size : 0;
1464
1465 if (!an->cal)
1466 an->cal = (u8 *) calloc(size, sizeof(u8));
1467 times = size / transmit_size + 1;
1468 for (i = 0; i < times; i++) {
1469 ret = atenl_nl_precal_sync_partition(&nl_priv, MT76_TM_ATTR_PRECAL, ops,
1470 i * transmit_size);
1471 if (ret)
1472 goto out;
1473 }
1474
1475 ret = atenl_eeprom_update_precal(an, base, size);
1476 break;
1477 case PREK_CLEAN_GROUP:
1478 if (!(cal_indicator & GROUP_IND_MASK))
1479 return 0;
1480 an->cal_info[4] = cal_indicator & (u8) ~GROUP_IND_MASK;
1481 ret = atenl_eeprom_update_precal(an, 0, group_size);
1482 break;
1483 case PREK_CLEAN_DPD:
1484 if (!(cal_indicator & DPD_IND_MASK))
1485 return 0;
1486 an->cal_info[4] = cal_indicator & (u8) ~DPD_IND_MASK;
1487 ret = atenl_eeprom_update_precal(an, group_size, dpd_size);
1488 break;
1489 default:
1490 break;
1491 }
1492
1493out:
1494 unl_free(&nl_priv.unl);
1495 return ret;
1496}