drivers/net/wireless/mediatek/mt76/testmode.c

   1 // SPDX-License-Identifier: ISC
   2 /* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */
   3
   4 #include <linux/random.h>
   5 #include "mt76.h"
   6
   7 const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS] = {
   8         [MT76_TM_ATTR_RESET] = { .type = NLA_FLAG },
   9         [MT76_TM_ATTR_STATE] = { .type = NLA_U8 },
  10         [MT76_TM_ATTR_TX_COUNT] = { .type = NLA_U32 },
  11         [MT76_TM_ATTR_TX_LENGTH] = { .type = NLA_U32 },
  12         [MT76_TM_ATTR_TX_RATE_MODE] = { .type = NLA_U8 },
  13         [MT76_TM_ATTR_TX_RATE_NSS] = { .type = NLA_U8 },
  14         [MT76_TM_ATTR_TX_RATE_IDX] = { .type = NLA_U8 },
  15         [MT76_TM_ATTR_TX_RATE_SGI] = { .type = NLA_U8 },
  16         [MT76_TM_ATTR_TX_RATE_LDPC] = { .type = NLA_U8 },
  17         [MT76_TM_ATTR_TX_RATE_STBC] = { .type = NLA_U8 },
  18         [MT76_TM_ATTR_TX_LTF] = { .type = NLA_U8 },
  19         [MT76_TM_ATTR_TX_ANTENNA] = { .type = NLA_U8 },
  20         [MT76_TM_ATTR_TX_SPE_IDX] = { .type = NLA_U8 },
  21         [MT76_TM_ATTR_TX_POWER_CONTROL] = { .type = NLA_U8 },
  22         [MT76_TM_ATTR_TX_POWER] = { .type = NLA_NESTED },
  23         [MT76_TM_ATTR_TX_DUTY_CYCLE] = { .type = NLA_U8 },
  24         [MT76_TM_ATTR_TX_IPG] = { .type = NLA_U32 },
  25         [MT76_TM_ATTR_TX_TIME] = { .type = NLA_U32 },
  26         [MT76_TM_ATTR_FREQ_OFFSET] = { .type = NLA_U32 },
  27         [MT76_TM_ATTR_DRV_DATA] = { .type = NLA_NESTED },
  28 };
  29 EXPORT_SYMBOL_GPL(mt76_tm_policy);
  30
  31 void mt76_testmode_tx_pending(struct mt76_phy *phy)
  32 {
  33         struct mt76_testmode_data *td = &phy->test;
  34         struct mt76_dev *dev = phy->dev;
  35         struct mt76_wcid *wcid = &dev->global_wcid;
  36         struct sk_buff *skb = td->tx_skb;
  37         struct mt76_queue *q;
  38         u16 tx_queued_limit;
  39         int qid;
  40
  41         if (!skb || !td->tx_pending)
  42                 return;
  43
  44         qid = skb_get_queue_mapping(skb);
  45         q = phy->q_tx[qid];
  46
  47         tx_queued_limit = td->tx_queued_limit ? td->tx_queued_limit : 1000;
  48
  49         spin_lock_bh(&q->lock);
  50
  51         while (td->tx_pending > 0 &&
  52                td->tx_queued - td->tx_done < tx_queued_limit &&
  53                q->queued < q->ndesc / 2) {
  54                 int ret;
  55
  56                 ret = dev->queue_ops->tx_queue_skb(dev, q, qid, skb_get(skb),
  57                                                    wcid, NULL);
  58                 if (ret < 0)
  59                         break;
  60
  61                 td->tx_pending--;
  62                 td->tx_queued++;
  63         }
  64
  65         dev->queue_ops->kick(dev, q);
  66
  67         spin_unlock_bh(&q->lock);
  68 }
  69
  70 static u32
  71 mt76_testmode_max_mpdu_len(struct mt76_phy *phy, u8 tx_rate_mode)
  72 {
  73         switch (tx_rate_mode) {
  74         case MT76_TM_TX_MODE_HT:
  75                 return IEEE80211_MAX_MPDU_LEN_HT_7935;
  76         case MT76_TM_TX_MODE_VHT:
  77         case MT76_TM_TX_MODE_HE_SU:
  78         case MT76_TM_TX_MODE_HE_EXT_SU:
  79         case MT76_TM_TX_MODE_HE_TB:
  80         case MT76_TM_TX_MODE_HE_MU:
  81                 if (phy->sband_5g.sband.vht_cap.cap &
  82                     IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991)
  83                         return IEEE80211_MAX_MPDU_LEN_VHT_7991;
  84                 return IEEE80211_MAX_MPDU_LEN_VHT_11454;
  85         case MT76_TM_TX_MODE_CCK:
  86         case MT76_TM_TX_MODE_OFDM:
  87         default:
  88                 return IEEE80211_MAX_FRAME_LEN;
  89         }
  90 }
  91
  92 static void
  93 mt76_testmode_free_skb(struct mt76_phy *phy)
  94 {
  95         struct mt76_testmode_data *td = &phy->test;
  96
  97         dev_kfree_skb(td->tx_skb);
  98         td->tx_skb = NULL;
  99 }
 100
 101 int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len)
 102 {
 103 #define MT_TXP_MAX_LEN  4095
 104         u16 fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA |
 105                  IEEE80211_FCTL_FROMDS;
 106         struct mt76_testmode_data *td = &phy->test;
 107         struct sk_buff **frag_tail, *head;
 108         struct ieee80211_tx_info *info;
 109         struct ieee80211_hdr *hdr;
 110         u32 max_len, head_len;
 111         int nfrags, i;
 112
 113         max_len = mt76_testmode_max_mpdu_len(phy, td->tx_rate_mode);
 114         if (len > max_len)
 115                 len = max_len;
 116         else if (len < sizeof(struct ieee80211_hdr))
 117                 len = sizeof(struct ieee80211_hdr);
 118
 119         nfrags = len / MT_TXP_MAX_LEN;
 120         head_len = nfrags ? MT_TXP_MAX_LEN : len;
 121
 122         if (len > IEEE80211_MAX_FRAME_LEN)
 123                 fc |= IEEE80211_STYPE_QOS_DATA;
 124
 125         head = alloc_skb(head_len, GFP_KERNEL);
 126         if (!head)
 127                 return -ENOMEM;
 128
 129         hdr = __skb_put_zero(head, sizeof(*hdr));
 130         hdr->frame_control = cpu_to_le16(fc);
 131         memcpy(hdr->addr1, td->addr[0], ETH_ALEN);
 132         memcpy(hdr->addr2, td->addr[1], ETH_ALEN);
 133         memcpy(hdr->addr3, td->addr[2], ETH_ALEN);
 134         skb_set_queue_mapping(head, IEEE80211_AC_BE);
 135         get_random_bytes(__skb_put(head, head_len - sizeof(*hdr)),
 136                          head_len - sizeof(*hdr));
 137
 138         info = IEEE80211_SKB_CB(head);
 139         info->flags = IEEE80211_TX_CTL_INJECTED |
 140                       IEEE80211_TX_CTL_NO_ACK |
 141                       IEEE80211_TX_CTL_NO_PS_BUFFER;
 142
 143         info->hw_queue |= FIELD_PREP(MT_TX_HW_QUEUE_PHY, phy->band_idx);
 144         frag_tail = &skb_shinfo(head)->frag_list;
 145
 146         for (i = 0; i < nfrags; i++) {
 147                 struct sk_buff *frag;
 148                 u16 frag_len;
 149
 150                 if (i == nfrags - 1)
 151                         frag_len = len % MT_TXP_MAX_LEN;
 152                 else
 153                         frag_len = MT_TXP_MAX_LEN;
 154
 155                 frag = alloc_skb(frag_len, GFP_KERNEL);
 156                 if (!frag) {
 157                         mt76_testmode_free_skb(phy);
 158                         dev_kfree_skb(head);
 159                         return -ENOMEM;
 160                 }
 161
 162                 get_random_bytes(__skb_put(frag, frag_len), frag_len);
 163                 head->len += frag->len;
 164                 head->data_len += frag->len;
 165
 166                 *frag_tail = frag;
 167                 frag_tail = &(*frag_tail)->next;
 168         }
 169
 170         mt76_testmode_free_skb(phy);
 171         td->tx_skb = head;
 172
 173         return 0;
 174 }
 175 EXPORT_SYMBOL(mt76_testmode_alloc_skb);
 176
 177 static int
 178 mt76_testmode_tx_init(struct mt76_phy *phy)
 179 {
 180         struct mt76_testmode_data *td = &phy->test;
 181         struct ieee80211_tx_info *info;
 182         struct ieee80211_tx_rate *rate;
 183         u8 max_nss = hweight8(phy->antenna_mask);
 184         int ret;
 185
 186         ret = mt76_testmode_alloc_skb(phy, td->tx_mpdu_len);
 187         if (ret)
 188                 return ret;
 189
 190         if (td->tx_rate_mode > MT76_TM_TX_MODE_VHT)
 191                 goto out;
 192
 193         if (td->tx_antenna_mask)
 194                 max_nss = min_t(u8, max_nss, hweight8(td->tx_antenna_mask));
 195
 196         info = IEEE80211_SKB_CB(td->tx_skb);
 197         rate = &info->control.rates[0];
 198         rate->count = 1;
 199         rate->idx = td->tx_rate_idx;
 200
 201         switch (td->tx_rate_mode) {
 202         case MT76_TM_TX_MODE_CCK:
 203                 if (phy->chandef.chan->band != NL80211_BAND_2GHZ)
 204                         return -EINVAL;
 205
 206                 if (rate->idx > 4)
 207                         return -EINVAL;
 208                 break;
 209         case MT76_TM_TX_MODE_OFDM:
 210                 if (phy->chandef.chan->band != NL80211_BAND_2GHZ)
 211                         break;
 212
 213                 if (rate->idx > 8)
 214                         return -EINVAL;
 215
 216                 rate->idx += 4;
 217                 break;
 218         case MT76_TM_TX_MODE_HT:
 219                 if (rate->idx > 8 * max_nss &&
 220                         !(rate->idx == 32 &&
 221                           phy->chandef.width >= NL80211_CHAN_WIDTH_40))
 222                         return -EINVAL;
 223
 224                 rate->flags |= IEEE80211_TX_RC_MCS;
 225                 break;
 226         case MT76_TM_TX_MODE_VHT:
 227                 if (rate->idx > 9)
 228                         return -EINVAL;
 229
 230                 if (td->tx_rate_nss > max_nss)
 231                         return -EINVAL;
 232
 233                 ieee80211_rate_set_vht(rate, td->tx_rate_idx, td->tx_rate_nss);
 234                 rate->flags |= IEEE80211_TX_RC_VHT_MCS;
 235                 break;
 236         default:
 237                 break;
 238         }
 239
 240         if (td->tx_rate_sgi)
 241                 rate->flags |= IEEE80211_TX_RC_SHORT_GI;
 242
 243         if (td->tx_rate_ldpc)
 244                 info->flags |= IEEE80211_TX_CTL_LDPC;
 245
 246         if (td->tx_rate_stbc)
 247                 info->flags |= IEEE80211_TX_CTL_STBC;
 248
 249         if (td->tx_rate_mode >= MT76_TM_TX_MODE_HT) {
 250                 switch (phy->chandef.width) {
 251                 case NL80211_CHAN_WIDTH_40:
 252                         rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
 253                         break;
 254                 case NL80211_CHAN_WIDTH_80:
 255                         rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
 256                         break;
 257                 case NL80211_CHAN_WIDTH_80P80:
 258                 case NL80211_CHAN_WIDTH_160:
 259                         rate->flags |= IEEE80211_TX_RC_160_MHZ_WIDTH;
 260                         break;
 261                 default:
 262                         break;
 263                 }
 264         }
 265 out:
 266         return 0;
 267 }
 268
 269 static void
 270 mt76_testmode_tx_start(struct mt76_phy *phy)
 271 {
 272         struct mt76_testmode_data *td = &phy->test;
 273         struct mt76_dev *dev = phy->dev;
 274
 275         td->tx_queued = 0;
 276         td->tx_done = 0;
 277         td->tx_pending = td->tx_count;
 278         mt76_worker_schedule(&dev->tx_worker);
 279 }
 280
 281 static void
 282 mt76_testmode_tx_stop(struct mt76_phy *phy)
 283 {
 284         struct mt76_testmode_data *td = &phy->test;
 285         struct mt76_dev *dev = phy->dev;
 286
 287         mt76_worker_disable(&dev->tx_worker);
 288
 289         td->tx_pending = 0;
 290
 291         mt76_worker_enable(&dev->tx_worker);
 292
 293         wait_event_timeout(dev->tx_wait, td->tx_done == td->tx_queued,
 294                            MT76_TM_TIMEOUT * HZ);
 295
 296         mt76_testmode_free_skb(phy);
 297 }
 298
 299 static inline void
 300 mt76_testmode_param_set(struct mt76_testmode_data *td, u16 idx)
 301 {
 302         td->param_set[idx / 32] |= BIT(idx % 32);
 303 }
 304
 305 static inline bool
 306 mt76_testmode_param_present(struct mt76_testmode_data *td, u16 idx)
 307 {
 308         return td->param_set[idx / 32] & BIT(idx % 32);
 309 }
 310
 311 static void
 312 mt76_testmode_init_defaults(struct mt76_phy *phy)
 313 {
 314         struct mt76_testmode_data *td = &phy->test;
 315
 316         if (td->tx_mpdu_len > 0)
 317                 return;
 318
 319         td->tx_mpdu_len = 1024;
 320         td->tx_count = 1;
 321         td->tx_rate_mode = MT76_TM_TX_MODE_OFDM;
 322         td->tx_rate_nss = 1;
 323
 324         memcpy(td->addr[0], phy->macaddr, ETH_ALEN);
 325         memcpy(td->addr[1], phy->macaddr, ETH_ALEN);
 326         memcpy(td->addr[2], phy->macaddr, ETH_ALEN);
 327 }
 328
 329 static int
 330 __mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state)
 331 {
 332         enum mt76_testmode_state prev_state = phy->test.state;
 333         struct mt76_dev *dev = phy->dev;
 334         int err;
 335
 336         if (prev_state == MT76_TM_STATE_TX_FRAMES)
 337                 mt76_testmode_tx_stop(phy);
 338
 339         if (state == MT76_TM_STATE_TX_FRAMES) {
 340                 err = mt76_testmode_tx_init(phy);
 341                 if (err)
 342                         return err;
 343         }
 344
 345         err = dev->test_ops->set_state(phy, state);
 346         if (err) {
 347                 if (state == MT76_TM_STATE_TX_FRAMES)
 348                         mt76_testmode_tx_stop(phy);
 349
 350                 return err;
 351         }
 352
 353         if (state == MT76_TM_STATE_TX_FRAMES)
 354                 mt76_testmode_tx_start(phy);
 355         else if (state == MT76_TM_STATE_RX_FRAMES) {
 356                 memset(&phy->test.rx_stats, 0, sizeof(phy->test.rx_stats));
 357         }
 358
 359         phy->test.state = state;
 360
 361         return 0;
 362 }
 363
 364 int mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state)
 365 {
 366         struct mt76_testmode_data *td = &phy->test;
 367         struct ieee80211_hw *hw = phy->hw;
 368
 369         if (state == td->state && state == MT76_TM_STATE_OFF)
 370                 return 0;
 371
 372         if (state > MT76_TM_STATE_OFF &&
 373             (!test_bit(MT76_STATE_RUNNING, &phy->state) ||
 374              !(hw->conf.flags & IEEE80211_CONF_MONITOR)))
 375                 return -ENOTCONN;
 376
 377         if (state != MT76_TM_STATE_IDLE &&
 378             td->state != MT76_TM_STATE_IDLE) {
 379                 int ret;
 380
 381                 ret = __mt76_testmode_set_state(phy, MT76_TM_STATE_IDLE);
 382                 if (ret)
 383                         return ret;
 384         }
 385
 386         return __mt76_testmode_set_state(phy, state);
 387
 388 }
 389 EXPORT_SYMBOL(mt76_testmode_set_state);
 390
 391 static int
 392 mt76_tm_get_u8(struct nlattr *attr, u8 *dest, u8 min, u8 max)
 393 {
 394         u8 val;
 395
 396         if (!attr)
 397                 return 0;
 398
 399         val = nla_get_u8(attr);
 400         if (val < min || val > max)
 401                 return -EINVAL;
 402
 403         *dest = val;
 404         return 0;
 405 }
 406
 407 int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 408                       void *data, int len)
 409 {
 410         struct mt76_phy *phy = hw->priv;
 411         struct mt76_dev *dev = phy->dev;
 412         struct mt76_testmode_data *td = &phy->test;
 413         struct nlattr *tb[NUM_MT76_TM_ATTRS];
 414         u32 state;
 415         int err;
 416         int i;
 417
 418         if (!dev->test_ops)
 419                 return -EOPNOTSUPP;
 420
 421         err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
 422                                    mt76_tm_policy, NULL);
 423         if (err)
 424                 return err;
 425
 426         err = -EINVAL;
 427
 428         mutex_lock(&dev->mutex);
 429
 430         if (tb[MT76_TM_ATTR_RESET]) {
 431                 mt76_testmode_set_state(phy, MT76_TM_STATE_OFF);
 432                 memset(td, 0, sizeof(*td));
 433         }
 434
 435         mt76_testmode_init_defaults(phy);
 436
 437         if (tb[MT76_TM_ATTR_TX_COUNT])
 438                 td->tx_count = nla_get_u32(tb[MT76_TM_ATTR_TX_COUNT]);
 439
 440         if (tb[MT76_TM_ATTR_TX_RATE_IDX])
 441                 td->tx_rate_idx = nla_get_u8(tb[MT76_TM_ATTR_TX_RATE_IDX]);
 442
 443         if (mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_MODE], &td->tx_rate_mode,
 444                            0, MT76_TM_TX_MODE_MAX) ||
 445             mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_NSS], &td->tx_rate_nss,
 446                            1, hweight8(phy->antenna_mask)) ||
 447             mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_SGI], &td->tx_rate_sgi, 0, 2) ||
 448             mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_LDPC], &td->tx_rate_ldpc, 0, 1) ||
 449             mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_STBC], &td->tx_rate_stbc, 0, 1) ||
 450             mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_LTF], &td->tx_ltf, 0, 2) ||
 451             mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_ANTENNA],
 452                            &td->tx_antenna_mask, 0, 0xff) ||
 453             mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_SPE_IDX], &td->tx_spe_idx, 0, 27) ||
 454             mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_DUTY_CYCLE],
 455                            &td->tx_duty_cycle, 0, 99) ||
 456             mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_POWER_CONTROL],
 457                            &td->tx_power_control, 0, 1))
 458                 goto out;
 459
 460         if (tb[MT76_TM_ATTR_TX_LENGTH]) {
 461                 u32 val = nla_get_u32(tb[MT76_TM_ATTR_TX_LENGTH]);
 462
 463                 if (val > mt76_testmode_max_mpdu_len(phy, td->tx_rate_mode) ||
 464                     val < sizeof(struct ieee80211_hdr))
 465                         goto out;
 466
 467                 td->tx_mpdu_len = val;
 468         }
 469
 470         if (tb[MT76_TM_ATTR_TX_IPG])
 471                 td->tx_ipg = nla_get_u32(tb[MT76_TM_ATTR_TX_IPG]);
 472
 473         if (tb[MT76_TM_ATTR_TX_TIME])
 474                 td->tx_time = nla_get_u32(tb[MT76_TM_ATTR_TX_TIME]);
 475
 476         if (tb[MT76_TM_ATTR_FREQ_OFFSET])
 477                 td->freq_offset = nla_get_u32(tb[MT76_TM_ATTR_FREQ_OFFSET]);
 478
 479         if (tb[MT76_TM_ATTR_STATE]) {
 480                 state = nla_get_u32(tb[MT76_TM_ATTR_STATE]);
 481                 if (state > MT76_TM_STATE_MAX)
 482                         goto out;
 483         } else {
 484                 state = td->state;
 485         }
 486
 487         if (tb[MT76_TM_ATTR_TX_POWER]) {
 488                 struct nlattr *cur;
 489                 int idx = 0;
 490                 int rem;
 491
 492                 nla_for_each_nested(cur, tb[MT76_TM_ATTR_TX_POWER], rem) {
 493                         if (nla_len(cur) != 1 ||
 494                             idx >= ARRAY_SIZE(td->tx_power))
 495                                 goto out;
 496
 497                         td->tx_power[idx++] = nla_get_u8(cur);
 498                 }
 499         }
 500
 501         if (tb[MT76_TM_ATTR_MAC_ADDRS]) {
 502                 struct nlattr *cur;
 503                 int idx = 0;
 504                 int rem;
 505
 506                 nla_for_each_nested(cur, tb[MT76_TM_ATTR_MAC_ADDRS], rem) {
 507                         if (nla_len(cur) != ETH_ALEN || idx >= 3)
 508                                 goto out;
 509
 510                         memcpy(td->addr[idx], nla_data(cur), ETH_ALEN);
 511                         idx++;
 512                 }
 513         }
 514
 515         if (dev->test_ops->set_params) {
 516                 err = dev->test_ops->set_params(phy, tb, state);
 517                 if (err)
 518                         goto out;
 519         }
 520
 521         for (i = MT76_TM_ATTR_STATE; i < ARRAY_SIZE(tb); i++)
 522                 if (tb[i])
 523                         mt76_testmode_param_set(td, i);
 524
 525         err = 0;
 526         if (tb[MT76_TM_ATTR_STATE])
 527                 err = mt76_testmode_set_state(phy, state);
 528
 529 out:
 530         mutex_unlock(&dev->mutex);
 531
 532         return err;
 533 }
 534 EXPORT_SYMBOL(mt76_testmode_cmd);
 535
 536 static int
 537 mt76_testmode_dump_stats(struct mt76_phy *phy, struct sk_buff *msg)
 538 {
 539         struct mt76_testmode_data *td = &phy->test;
 540         struct mt76_dev *dev = phy->dev;
 541         u64 rx_packets = 0;
 542         u64 rx_fcs_error = 0;
 543         int i;
 544
 545         if (dev->test_ops->dump_stats) {
 546                 int ret;
 547
 548                 ret = dev->test_ops->dump_stats(phy, msg);
 549                 if (ret)
 550                         return ret;
 551         }
 552
 553         for (i = 0; i < ARRAY_SIZE(td->rx_stats.packets); i++) {
 554                 rx_packets += td->rx_stats.packets[i];
 555                 rx_fcs_error += td->rx_stats.fcs_error[i];
 556         }
 557
 558         if (nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_PENDING, td->tx_pending) ||
 559             nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_QUEUED, td->tx_queued) ||
 560             nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_DONE, td->tx_done) ||
 561             nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_PACKETS, rx_packets,
 562                               MT76_TM_STATS_ATTR_PAD) ||
 563             nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_FCS_ERROR, rx_fcs_error,
 564                               MT76_TM_STATS_ATTR_PAD))
 565                 return -EMSGSIZE;
 566
 567         return 0;
 568 }
 569
 570 int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
 571                        struct netlink_callback *cb, void *data, int len)
 572 {
 573         struct mt76_phy *phy = hw->priv;
 574         struct mt76_dev *dev = phy->dev;
 575         struct mt76_testmode_data *td = &phy->test;
 576         struct nlattr *tb[NUM_MT76_TM_ATTRS] = {};
 577         int err = 0;
 578         void *a;
 579         int i;
 580
 581         if (!dev->test_ops)
 582                 return -EOPNOTSUPP;
 583
 584         if (cb->args[2]++ > 0)
 585                 return -ENOENT;
 586
 587         if (data) {
 588                 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
 589                                            mt76_tm_policy, NULL);
 590                 if (err)
 591                         return err;
 592         }
 593
 594         mutex_lock(&dev->mutex);
 595
 596         if (tb[MT76_TM_ATTR_STATS]) {
 597                 err = -EINVAL;
 598
 599                 a = nla_nest_start(msg, MT76_TM_ATTR_STATS);
 600                 if (a) {
 601                         err = mt76_testmode_dump_stats(phy, msg);
 602                         nla_nest_end(msg, a);
 603                 }
 604
 605                 goto out;
 606         }
 607
 608         mt76_testmode_init_defaults(phy);
 609
 610         err = -EMSGSIZE;
 611         if (nla_put_u32(msg, MT76_TM_ATTR_STATE, td->state))
 612                 goto out;
 613
 614         if (dev->test_mtd.name &&
 615             (nla_put_string(msg, MT76_TM_ATTR_MTD_PART, dev->test_mtd.name) ||
 616              nla_put_u32(msg, MT76_TM_ATTR_MTD_OFFSET, dev->test_mtd.offset)))
 617                 goto out;
 618
 619         if (nla_put_u32(msg, MT76_TM_ATTR_TX_COUNT, td->tx_count) ||
 620             nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH, td->tx_mpdu_len) ||
 621             nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_MODE, td->tx_rate_mode) ||
 622             nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_NSS, td->tx_rate_nss) ||
 623             nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, td->tx_rate_idx) ||
 624             nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_SGI, td->tx_rate_sgi) ||
 625             nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_LDPC, td->tx_rate_ldpc) ||
 626             nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_STBC, td->tx_rate_stbc) ||
 627             (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_LTF) &&
 628              nla_put_u8(msg, MT76_TM_ATTR_TX_LTF, td->tx_ltf)) ||
 629             (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_ANTENNA) &&
 630              nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, td->tx_antenna_mask)) ||
 631             (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_SPE_IDX) &&
 632              nla_put_u8(msg, MT76_TM_ATTR_TX_SPE_IDX, td->tx_spe_idx)) ||
 633             (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_DUTY_CYCLE) &&
 634              nla_put_u8(msg, MT76_TM_ATTR_TX_DUTY_CYCLE, td->tx_duty_cycle)) ||
 635             (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_IPG) &&
 636              nla_put_u32(msg, MT76_TM_ATTR_TX_IPG, td->tx_ipg)) ||
 637             (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_TIME) &&
 638              nla_put_u32(msg, MT76_TM_ATTR_TX_TIME, td->tx_time)) ||
 639             (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER_CONTROL) &&
 640              nla_put_u8(msg, MT76_TM_ATTR_TX_POWER_CONTROL, td->tx_power_control)) ||
 641             (mt76_testmode_param_present(td, MT76_TM_ATTR_FREQ_OFFSET) &&
 642              nla_put_u8(msg, MT76_TM_ATTR_FREQ_OFFSET, td->freq_offset)))
 643                 goto out;
 644
 645         if (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER)) {
 646                 a = nla_nest_start(msg, MT76_TM_ATTR_TX_POWER);
 647                 if (!a)
 648                         goto out;
 649
 650                 for (i = 0; i < ARRAY_SIZE(td->tx_power); i++)
 651                         if (nla_put_u8(msg, i, td->tx_power[i]))
 652                                 goto out;
 653
 654                 nla_nest_end(msg, a);
 655         }
 656
 657         if (mt76_testmode_param_present(td, MT76_TM_ATTR_MAC_ADDRS)) {
 658                 a = nla_nest_start(msg, MT76_TM_ATTR_MAC_ADDRS);
 659                 if (!a)
 660                         goto out;
 661
 662                 for (i = 0; i < 3; i++)
 663                         if (nla_put(msg, i, ETH_ALEN, td->addr[i]))
 664                                 goto out;
 665
 666                 nla_nest_end(msg, a);
 667         }
 668
 669         err = 0;
 670
 671 out:
 672         mutex_unlock(&dev->mutex);
 673
 674         return err;
 675 }
 676 EXPORT_SYMBOL(mt76_testmode_dump);