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

   1 // SPDX-License-Identifier: ISC
   2 /* Copyright (C) 2020 MediaTek Inc. */
   3
   4 #include "mt7915.h"
   5 #include "mac.h"
   6 #include "mcu.h"
   7 #include "testmode.h"
   8
   9 enum {
  10         TM_CHANGED_TXPOWER,
  11         TM_CHANGED_FREQ_OFFSET,
  12
  13         /* must be last */
  14         NUM_TM_CHANGED
  15 };
  16
  17 static const u8 tm_change_map[] = {
  18         [TM_CHANGED_TXPOWER] = MT76_TM_ATTR_TX_POWER,
  19         [TM_CHANGED_FREQ_OFFSET] = MT76_TM_ATTR_FREQ_OFFSET,
  20 };
  21
  22 struct reg_band {
  23         u32 band[2];
  24 };
  25
  26 #define REG_BAND(_reg) \
  27         { .band[0] = MT_##_reg(0), .band[1] = MT_##_reg(1) }
  28 #define REG_BAND_IDX(_reg, _idx) \
  29         { .band[0] = MT_##_reg(0, _idx), .band[1] = MT_##_reg(1, _idx) }
  30
  31 static const struct reg_band reg_backup_list[] = {
  32         REG_BAND_IDX(AGG_PCR0, 0),
  33         REG_BAND_IDX(AGG_PCR0, 1),
  34         REG_BAND_IDX(AGG_AWSCR0, 0),
  35         REG_BAND_IDX(AGG_AWSCR0, 1),
  36         REG_BAND_IDX(AGG_AWSCR0, 2),
  37         REG_BAND_IDX(AGG_AWSCR0, 3),
  38         REG_BAND(AGG_MRCR),
  39         REG_BAND(TMAC_TFCR0),
  40         REG_BAND(TMAC_TCR0),
  41         REG_BAND(AGG_ATCR1),
  42         REG_BAND(AGG_ATCR3),
  43         REG_BAND(TMAC_TRCR0),
  44         REG_BAND(TMAC_ICR0),
  45         REG_BAND_IDX(ARB_DRNGR0, 0),
  46         REG_BAND_IDX(ARB_DRNGR0, 1),
  47         REG_BAND(WF_RFCR),
  48         REG_BAND(WF_RFCR1),
  49 };
  50
  51 static int
  52 mt7915_tm_set_tx_power(struct mt7915_phy *phy)
  53 {
  54         struct mt7915_dev *dev = phy->dev;
  55         struct mt76_phy *mphy = phy->mt76;
  56         struct cfg80211_chan_def *chandef = &mphy->chandef;
  57         int freq = chandef->center_freq1;
  58         int ret;
  59         struct {
  60                 u8 format_id;
  61                 u8 dbdc_idx;
  62                 s8 tx_power;
  63                 u8 ant_idx;     /* Only 0 is valid */
  64                 u8 center_chan;
  65                 u8 rsv[3];
  66         } __packed req = {
  67                 .format_id = 0xf,
  68                 .dbdc_idx = phy != &dev->phy,
  69                 .center_chan = ieee80211_frequency_to_channel(freq),
  70         };
  71         u8 *tx_power = NULL;
  72
  73         if (phy->mt76->test.state != MT76_TM_STATE_OFF)
  74                 tx_power = phy->mt76->test.tx_power;
  75
  76         /* Tx power of the other antennas are the same as antenna 0 */
  77         if (tx_power && tx_power[0])
  78                 req.tx_power = tx_power[0];
  79
  80         ret = mt76_mcu_send_msg(&dev->mt76,
  81                                 MCU_EXT_CMD(TX_POWER_FEATURE_CTRL),
  82                                 &req, sizeof(req), false);
  83
  84         return ret;
  85 }
  86
  87 static int
  88 mt7915_tm_set_freq_offset(struct mt7915_phy *phy, bool en, u32 val)
  89 {
  90         struct mt7915_dev *dev = phy->dev;
  91         struct mt7915_tm_cmd req = {
  92                 .testmode_en = en,
  93                 .param_idx = MCU_ATE_SET_FREQ_OFFSET,
  94                 .param.freq.band = phy != &dev->phy,
  95                 .param.freq.freq_offset = cpu_to_le32(val),
  96         };
  97
  98         return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
  99                                  sizeof(req), false);
 100 }
 101
 102 static int
 103 mt7915_tm_mode_ctrl(struct mt7915_dev *dev, bool enable)
 104 {
 105         struct {
 106                 u8 format_id;
 107                 bool enable;
 108                 u8 rsv[2];
 109         } __packed req = {
 110                 .format_id = 0x6,
 111                 .enable = enable,
 112         };
 113
 114         return mt76_mcu_send_msg(&dev->mt76,
 115                                  MCU_EXT_CMD(TX_POWER_FEATURE_CTRL),
 116                                  &req, sizeof(req), false);
 117 }
 118
 119 static int
 120 mt7915_tm_set_trx(struct mt7915_phy *phy, int type, bool en)
 121 {
 122         struct mt7915_dev *dev = phy->dev;
 123         struct mt7915_tm_cmd req = {
 124                 .testmode_en = 1,
 125                 .param_idx = MCU_ATE_SET_TRX,
 126                 .param.trx.type = type,
 127                 .param.trx.enable = en,
 128                 .param.trx.band = phy != &dev->phy,
 129         };
 130
 131         return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
 132                                  sizeof(req), false);
 133 }
 134
 135 static int
 136 mt7915_tm_clean_hwq(struct mt7915_phy *phy, u8 wcid)
 137 {
 138         struct mt7915_dev *dev = phy->dev;
 139         struct mt7915_tm_cmd req = {
 140                 .testmode_en = 1,
 141                 .param_idx = MCU_ATE_CLEAN_TXQUEUE,
 142                 .param.clean.wcid = wcid,
 143                 .param.clean.band = phy != &dev->phy,
 144         };
 145
 146         return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
 147                                  sizeof(req), false);
 148 }
 149
 150 static int
 151 mt7915_tm_set_slot_time(struct mt7915_phy *phy, u8 slot_time, u8 sifs)
 152 {
 153         struct mt7915_dev *dev = phy->dev;
 154         struct mt7915_tm_cmd req = {
 155                 .testmode_en = !(phy->mt76->test.state == MT76_TM_STATE_OFF),
 156                 .param_idx = MCU_ATE_SET_SLOT_TIME,
 157                 .param.slot.slot_time = slot_time,
 158                 .param.slot.sifs = sifs,
 159                 .param.slot.rifs = 2,
 160                 .param.slot.eifs = cpu_to_le16(60),
 161                 .param.slot.band = phy != &dev->phy,
 162         };
 163
 164         return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
 165                                  sizeof(req), false);
 166 }
 167
 168 static int
 169 mt7915_tm_set_wmm_qid(struct mt7915_dev *dev, u8 qid, u8 aifs, u8 cw_min,
 170                       u16 cw_max, u16 txop)
 171 {
 172         struct mt7915_mcu_tx req = { .total = 1 };
 173         struct edca *e = &req.edca[0];
 174
 175         e->queue = qid;
 176         e->set = WMM_PARAM_SET;
 177
 178         e->aifs = aifs;
 179         e->cw_min = cw_min;
 180         e->cw_max = cpu_to_le16(cw_max);
 181         e->txop = cpu_to_le16(txop);
 182
 183         return mt7915_mcu_update_edca(dev, &req);
 184 }
 185
 186 static int
 187 mt7915_tm_set_ipg_params(struct mt7915_phy *phy, u32 ipg, u8 mode)
 188 {
 189 #define TM_DEFAULT_SIFS 10
 190 #define TM_MAX_SIFS     127
 191 #define TM_MAX_AIFSN    0xf
 192 #define TM_MIN_AIFSN    0x1
 193 #define BBP_PROC_TIME   1500
 194         struct mt7915_dev *dev = phy->dev;
 195         u8 sig_ext = (mode == MT76_TM_TX_MODE_CCK) ? 0 : 6;
 196         u8 slot_time = 9, sifs = TM_DEFAULT_SIFS;
 197         u8 aifsn = TM_MIN_AIFSN;
 198         u32 i2t_time, tr2t_time, txv_time;
 199         bool ext_phy = phy != &dev->phy;
 200         u16 cw = 0;
 201
 202         if (ipg < sig_ext + slot_time + sifs)
 203                 ipg = 0;
 204
 205         if (!ipg)
 206                 goto done;
 207
 208         ipg -= sig_ext;
 209
 210         if (ipg <= (TM_MAX_SIFS + slot_time)) {
 211                 sifs = ipg - slot_time;
 212         } else {
 213                 u32 val = (ipg + slot_time) / slot_time;
 214
 215                 while (val >>= 1)
 216                         cw++;
 217
 218                 if (cw > 16)
 219                         cw = 16;
 220
 221                 ipg -= ((1 << cw) - 1) * slot_time;
 222
 223                 aifsn = ipg / slot_time;
 224                 if (aifsn > TM_MAX_AIFSN)
 225                         aifsn = TM_MAX_AIFSN;
 226
 227                 ipg -= aifsn * slot_time;
 228
 229                 if (ipg > TM_DEFAULT_SIFS) {
 230                         if (ipg < TM_MAX_SIFS)
 231                                 sifs = ipg;
 232                         else
 233                                 sifs = TM_MAX_SIFS;
 234                 }
 235         }
 236 done:
 237         txv_time = mt76_get_field(dev, MT_TMAC_ATCR(ext_phy),
 238                                   MT_TMAC_ATCR_TXV_TOUT);
 239         txv_time *= 50; /* normal clock time */
 240
 241         i2t_time = (slot_time * 1000 - txv_time - BBP_PROC_TIME) / 50;
 242         tr2t_time = (sifs * 1000 - txv_time - BBP_PROC_TIME) / 50;
 243
 244         mt76_set(dev, MT_TMAC_TRCR0(ext_phy),
 245                  FIELD_PREP(MT_TMAC_TRCR0_TR2T_CHK, tr2t_time) |
 246                  FIELD_PREP(MT_TMAC_TRCR0_I2T_CHK, i2t_time));
 247
 248         mt7915_tm_set_slot_time(phy, slot_time, sifs);
 249
 250         return mt7915_tm_set_wmm_qid(dev,
 251                                      mt7915_lmac_mapping(dev, IEEE80211_AC_BE),
 252                                      aifsn, cw, cw, 0);
 253 }
 254
 255 static int
 256 mt7915_tm_set_tx_len(struct mt7915_phy *phy, u32 tx_time)
 257 {
 258         struct mt76_phy *mphy = phy->mt76;
 259         struct mt76_testmode_data *td = &mphy->test;
 260         struct sk_buff *old = td->tx_skb, *new;
 261         struct ieee80211_supported_band *sband;
 262         struct rate_info rate = {};
 263         u16 flags = 0, tx_len;
 264         u32 bitrate;
 265
 266         if (!tx_time || !old)
 267                 return 0;
 268
 269         rate.mcs = td->tx_rate_idx;
 270         rate.nss = td->tx_rate_nss;
 271
 272         switch (td->tx_rate_mode) {
 273         case MT76_TM_TX_MODE_CCK:
 274         case MT76_TM_TX_MODE_OFDM:
 275                 if (mphy->chandef.chan->band == NL80211_BAND_5GHZ)
 276                         sband = &mphy->sband_5g.sband;
 277                 else
 278                         sband = &mphy->sband_2g.sband;
 279
 280                 rate.legacy = sband->bitrates[rate.mcs].bitrate;
 281                 break;
 282         case MT76_TM_TX_MODE_HT:
 283                 rate.mcs += rate.nss * 8;
 284                 flags |= RATE_INFO_FLAGS_MCS;
 285
 286                 if (td->tx_rate_sgi)
 287                         flags |= RATE_INFO_FLAGS_SHORT_GI;
 288                 break;
 289         case MT76_TM_TX_MODE_VHT:
 290                 flags |= RATE_INFO_FLAGS_VHT_MCS;
 291
 292                 if (td->tx_rate_sgi)
 293                         flags |= RATE_INFO_FLAGS_SHORT_GI;
 294                 break;
 295         case MT76_TM_TX_MODE_HE_SU:
 296         case MT76_TM_TX_MODE_HE_EXT_SU:
 297         case MT76_TM_TX_MODE_HE_TB:
 298         case MT76_TM_TX_MODE_HE_MU:
 299                 rate.he_gi = td->tx_rate_sgi;
 300                 flags |= RATE_INFO_FLAGS_HE_MCS;
 301                 break;
 302         default:
 303                 break;
 304         }
 305         rate.flags = flags;
 306
 307         switch (mphy->chandef.width) {
 308         case NL80211_CHAN_WIDTH_160:
 309         case NL80211_CHAN_WIDTH_80P80:
 310                 rate.bw = RATE_INFO_BW_160;
 311                 break;
 312         case NL80211_CHAN_WIDTH_80:
 313                 rate.bw = RATE_INFO_BW_80;
 314                 break;
 315         case NL80211_CHAN_WIDTH_40:
 316                 rate.bw = RATE_INFO_BW_40;
 317                 break;
 318         default:
 319                 rate.bw = RATE_INFO_BW_20;
 320                 break;
 321         }
 322
 323         bitrate = cfg80211_calculate_bitrate(&rate);
 324         tx_len = bitrate * tx_time / 10 / 8;
 325
 326         if (tx_len < sizeof(struct ieee80211_hdr))
 327                 tx_len = sizeof(struct ieee80211_hdr);
 328         else if (tx_len > IEEE80211_MAX_FRAME_LEN)
 329                 tx_len = IEEE80211_MAX_FRAME_LEN;
 330
 331         new = alloc_skb(tx_len, GFP_KERNEL);
 332         if (!new)
 333                 return -ENOMEM;
 334
 335         skb_copy_header(new, old);
 336         __skb_put_zero(new, tx_len);
 337         memcpy(new->data, old->data, sizeof(struct ieee80211_hdr));
 338
 339         dev_kfree_skb(old);
 340         td->tx_skb = new;
 341
 342         return 0;
 343 }
 344
 345 static void
 346 mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
 347 {
 348         int n_regs = ARRAY_SIZE(reg_backup_list);
 349         struct mt7915_dev *dev = phy->dev;
 350         bool ext_phy = phy != &dev->phy;
 351         u32 *b = phy->test.reg_backup;
 352         int i;
 353
 354         if (phy->mt76->test.state == MT76_TM_STATE_OFF) {
 355                 for (i = 0; i < n_regs; i++)
 356                         mt76_wr(dev, reg_backup_list[i].band[ext_phy], b[i]);
 357                 return;
 358         }
 359
 360         if (b)
 361                 return;
 362
 363         b = devm_kzalloc(dev->mt76.dev, 4 * n_regs, GFP_KERNEL);
 364         if (!b)
 365                 return;
 366
 367         phy->test.reg_backup = b;
 368         for (i = 0; i < n_regs; i++)
 369                 b[i] = mt76_rr(dev, reg_backup_list[i].band[ext_phy]);
 370
 371         mt76_clear(dev, MT_AGG_PCR0(ext_phy, 0), MT_AGG_PCR0_MM_PROT |
 372                    MT_AGG_PCR0_GF_PROT | MT_AGG_PCR0_ERP_PROT |
 373                    MT_AGG_PCR0_VHT_PROT | MT_AGG_PCR0_BW20_PROT |
 374                    MT_AGG_PCR0_BW40_PROT | MT_AGG_PCR0_BW80_PROT);
 375         mt76_set(dev, MT_AGG_PCR0(ext_phy, 0), MT_AGG_PCR0_PTA_WIN_DIS);
 376
 377         mt76_wr(dev, MT_AGG_PCR0(ext_phy, 1), MT_AGG_PCR1_RTS0_NUM_THRES |
 378                 MT_AGG_PCR1_RTS0_LEN_THRES);
 379
 380         mt76_clear(dev, MT_AGG_MRCR(ext_phy), MT_AGG_MRCR_BAR_CNT_LIMIT |
 381                    MT_AGG_MRCR_LAST_RTS_CTS_RN | MT_AGG_MRCR_RTS_FAIL_LIMIT |
 382                    MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT);
 383
 384         mt76_rmw(dev, MT_AGG_MRCR(ext_phy), MT_AGG_MRCR_RTS_FAIL_LIMIT |
 385                  MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT,
 386                  FIELD_PREP(MT_AGG_MRCR_RTS_FAIL_LIMIT, 1) |
 387                  FIELD_PREP(MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT, 1));
 388
 389         mt76_wr(dev, MT_TMAC_TFCR0(ext_phy), 0);
 390         mt76_clear(dev, MT_TMAC_TCR0(ext_phy), MT_TMAC_TCR0_TBTT_STOP_CTRL);
 391
 392         /* config rx filter for testmode rx */
 393         mt76_wr(dev, MT_WF_RFCR(ext_phy), 0xcf70a);
 394         mt76_wr(dev, MT_WF_RFCR1(ext_phy), 0);
 395 }
 396
 397 static void
 398 mt7915_tm_init(struct mt7915_phy *phy, bool en)
 399 {
 400         struct mt7915_dev *dev = phy->dev;
 401
 402         if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
 403                 return;
 404
 405         mt7915_mcu_set_sku_en(phy, !en);
 406
 407         mt7915_tm_mode_ctrl(dev, en);
 408         mt7915_tm_reg_backup_restore(phy);
 409         mt7915_tm_set_trx(phy, TM_MAC_TXRX, !en);
 410
 411         mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en);
 412 }
 413
 414 static void
 415 mt7915_tm_update_channel(struct mt7915_phy *phy)
 416 {
 417         mutex_unlock(&phy->dev->mt76.mutex);
 418         mt7915_set_channel(phy);
 419         mutex_lock(&phy->dev->mt76.mutex);
 420
 421         mt7915_mcu_set_chan_info(phy, MCU_EXT_CMD(SET_RX_PATH));
 422 }
 423
 424 static void
 425 mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en)
 426 {
 427         static const u8 spe_idx_map[] = {0, 0, 1, 0, 3, 2, 4, 0,
 428                                          9, 8, 6, 10, 16, 12, 18, 0};
 429         struct mt76_testmode_data *td = &phy->mt76->test;
 430         struct mt7915_dev *dev = phy->dev;
 431         struct ieee80211_tx_info *info;
 432         u8 duty_cycle = td->tx_duty_cycle;
 433         u32 tx_time = td->tx_time;
 434         u32 ipg = td->tx_ipg;
 435
 436         mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
 437         mt7915_tm_clean_hwq(phy, dev->mt76.global_wcid.idx);
 438
 439         if (en) {
 440                 mt7915_tm_update_channel(phy);
 441
 442                 if (td->tx_spe_idx) {
 443                         phy->test.spe_idx = td->tx_spe_idx;
 444                 } else {
 445                         u8 tx_ant = td->tx_antenna_mask;
 446
 447                         if (phy != &dev->phy)
 448                                 tx_ant >>= 2;
 449                         phy->test.spe_idx = spe_idx_map[tx_ant];
 450                 }
 451         }
 452
 453         /* if all three params are set, duty_cycle will be ignored */
 454         if (duty_cycle && tx_time && !ipg) {
 455                 ipg = tx_time * 100 / duty_cycle - tx_time;
 456         } else if (duty_cycle && !tx_time && ipg) {
 457                 if (duty_cycle < 100)
 458                         tx_time = duty_cycle * ipg / (100 - duty_cycle);
 459         }
 460
 461         mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode);
 462         mt7915_tm_set_tx_len(phy, tx_time);
 463
 464         if (ipg)
 465                 td->tx_queued_limit = MT76_TM_TIMEOUT * 1000000 / ipg / 2;
 466
 467         if (!en || !td->tx_skb)
 468                 return;
 469
 470         info = IEEE80211_SKB_CB(td->tx_skb);
 471         info->control.vif = phy->monitor_vif;
 472
 473         mt7915_tm_set_trx(phy, TM_MAC_TX, en);
 474 }
 475
 476 static void
 477 mt7915_tm_set_rx_frames(struct mt7915_phy *phy, bool en)
 478 {
 479         if (en)
 480                 mt7915_tm_update_channel(phy);
 481
 482         mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, en);
 483 }
 484
 485 static int
 486 mt7915_tm_rf_switch_mode(struct mt7915_dev *dev, u32 oper)
 487 {
 488         struct mt7915_tm_rf_test req = {
 489                 .op.op_mode = cpu_to_le32(oper),
 490         };
 491
 492         return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(RF_TEST), &req,
 493                                  sizeof(req), true);
 494 }
 495
 496 static int
 497 mt7915_tm_set_tx_cont(struct mt7915_phy *phy, bool en)
 498 {
 499 #define TX_CONT_START   0x05
 500 #define TX_CONT_STOP    0x06
 501         struct mt7915_dev *dev = phy->dev;
 502         struct cfg80211_chan_def *chandef = &phy->mt76->chandef;
 503         int freq1 = ieee80211_frequency_to_channel(chandef->center_freq1);
 504         struct mt76_testmode_data *td = &phy->mt76->test;
 505         u32 func_idx = en ? TX_CONT_START : TX_CONT_STOP;
 506         u8 rate_idx = td->tx_rate_idx, mode;
 507         u16 rateval;
 508         struct mt7915_tm_rf_test req = {
 509                 .action = 1,
 510                 .icap_len = 120,
 511                 .op.rf.func_idx = cpu_to_le32(func_idx),
 512         };
 513         struct tm_tx_cont *tx_cont = &req.op.rf.param.tx_cont;
 514
 515         tx_cont->control_ch = chandef->chan->hw_value;
 516         tx_cont->center_ch = freq1;
 517         tx_cont->tx_ant = td->tx_antenna_mask;
 518         tx_cont->band = phy != &dev->phy;
 519
 520         switch (chandef->width) {
 521         case NL80211_CHAN_WIDTH_40:
 522                 tx_cont->bw = CMD_CBW_40MHZ;
 523                 break;
 524         case NL80211_CHAN_WIDTH_80:
 525                 tx_cont->bw = CMD_CBW_80MHZ;
 526                 break;
 527         case NL80211_CHAN_WIDTH_80P80:
 528                 tx_cont->bw = CMD_CBW_8080MHZ;
 529                 break;
 530         case NL80211_CHAN_WIDTH_160:
 531                 tx_cont->bw = CMD_CBW_160MHZ;
 532                 break;
 533         case NL80211_CHAN_WIDTH_5:
 534                 tx_cont->bw = CMD_CBW_5MHZ;
 535                 break;
 536         case NL80211_CHAN_WIDTH_10:
 537                 tx_cont->bw = CMD_CBW_10MHZ;
 538                 break;
 539         case NL80211_CHAN_WIDTH_20:
 540                 tx_cont->bw = CMD_CBW_20MHZ;
 541                 break;
 542         case NL80211_CHAN_WIDTH_20_NOHT:
 543                 tx_cont->bw = CMD_CBW_20MHZ;
 544                 break;
 545         default:
 546                 return -EINVAL;
 547         }
 548
 549         if (!en) {
 550                 req.op.rf.param.func_data = cpu_to_le32(phy != &dev->phy);
 551                 goto out;
 552         }
 553
 554         if (td->tx_rate_mode <= MT76_TM_TX_MODE_OFDM) {
 555                 struct ieee80211_supported_band *sband;
 556                 u8 idx = rate_idx;
 557
 558                 if (chandef->chan->band == NL80211_BAND_5GHZ)
 559                         sband = &phy->mt76->sband_5g.sband;
 560                 else
 561                         sband = &phy->mt76->sband_2g.sband;
 562
 563                 if (td->tx_rate_mode == MT76_TM_TX_MODE_OFDM)
 564                         idx += 4;
 565                 rate_idx = sband->bitrates[idx].hw_value & 0xff;
 566         }
 567
 568         switch (td->tx_rate_mode) {
 569         case MT76_TM_TX_MODE_CCK:
 570                 mode = MT_PHY_TYPE_CCK;
 571                 break;
 572         case MT76_TM_TX_MODE_OFDM:
 573                 mode = MT_PHY_TYPE_OFDM;
 574                 break;
 575         case MT76_TM_TX_MODE_HT:
 576                 mode = MT_PHY_TYPE_HT;
 577                 break;
 578         case MT76_TM_TX_MODE_VHT:
 579                 mode = MT_PHY_TYPE_VHT;
 580                 break;
 581         case MT76_TM_TX_MODE_HE_SU:
 582                 mode = MT_PHY_TYPE_HE_SU;
 583                 break;
 584         case MT76_TM_TX_MODE_HE_EXT_SU:
 585                 mode = MT_PHY_TYPE_HE_EXT_SU;
 586                 break;
 587         case MT76_TM_TX_MODE_HE_TB:
 588                 mode = MT_PHY_TYPE_HE_TB;
 589                 break;
 590         case MT76_TM_TX_MODE_HE_MU:
 591                 mode = MT_PHY_TYPE_HE_MU;
 592                 break;
 593         default:
 594                 return -EINVAL;
 595         }
 596
 597         rateval =  mode << 6 | rate_idx;
 598         tx_cont->rateval = cpu_to_le16(rateval);
 599
 600 out:
 601         if (!en) {
 602                 int ret;
 603
 604                 ret = mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(RF_TEST), &req,
 605                                         sizeof(req), true);
 606                 if (ret)
 607                         return ret;
 608
 609                 return mt7915_tm_rf_switch_mode(dev, RF_OPER_NORMAL);
 610         }
 611
 612         mt7915_tm_rf_switch_mode(dev, RF_OPER_RF_TEST);
 613         mt7915_tm_update_channel(phy);
 614
 615         return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(RF_TEST), &req,
 616                                  sizeof(req), true);
 617 }
 618
 619 static void
 620 mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed)
 621 {
 622         struct mt76_testmode_data *td = &phy->mt76->test;
 623         bool en = phy->mt76->test.state != MT76_TM_STATE_OFF;
 624
 625         if (changed & BIT(TM_CHANGED_FREQ_OFFSET))
 626                 mt7915_tm_set_freq_offset(phy, en, en ? td->freq_offset : 0);
 627         if (changed & BIT(TM_CHANGED_TXPOWER))
 628                 mt7915_tm_set_tx_power(phy);
 629 }
 630
 631 static int
 632 mt7915_tm_set_state(struct mt76_phy *mphy, enum mt76_testmode_state state)
 633 {
 634         struct mt76_testmode_data *td = &mphy->test;
 635         struct mt7915_phy *phy = mphy->priv;
 636         enum mt76_testmode_state prev_state = td->state;
 637
 638         mphy->test.state = state;
 639
 640         if (prev_state == MT76_TM_STATE_TX_FRAMES ||
 641             state == MT76_TM_STATE_TX_FRAMES)
 642                 mt7915_tm_set_tx_frames(phy, state == MT76_TM_STATE_TX_FRAMES);
 643         else if (prev_state == MT76_TM_STATE_RX_FRAMES ||
 644                  state == MT76_TM_STATE_RX_FRAMES)
 645                 mt7915_tm_set_rx_frames(phy, state == MT76_TM_STATE_RX_FRAMES);
 646         else if (prev_state == MT76_TM_STATE_TX_CONT ||
 647                  state == MT76_TM_STATE_TX_CONT)
 648                 mt7915_tm_set_tx_cont(phy, state == MT76_TM_STATE_TX_CONT);
 649         else if (prev_state == MT76_TM_STATE_OFF ||
 650                  state == MT76_TM_STATE_OFF)
 651                 mt7915_tm_init(phy, !(state == MT76_TM_STATE_OFF));
 652
 653         if ((state == MT76_TM_STATE_IDLE &&
 654              prev_state == MT76_TM_STATE_OFF) ||
 655             (state == MT76_TM_STATE_OFF &&
 656              prev_state == MT76_TM_STATE_IDLE)) {
 657                 u32 changed = 0;
 658                 int i;
 659
 660                 for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
 661                         u16 cur = tm_change_map[i];
 662
 663                         if (td->param_set[cur / 32] & BIT(cur % 32))
 664                                 changed |= BIT(i);
 665                 }
 666
 667                 mt7915_tm_update_params(phy, changed);
 668         }
 669
 670         return 0;
 671 }
 672
 673 static int
 674 mt7915_tm_set_params(struct mt76_phy *mphy, struct nlattr **tb,
 675                      enum mt76_testmode_state new_state)
 676 {
 677         struct mt76_testmode_data *td = &mphy->test;
 678         struct mt7915_phy *phy = mphy->priv;
 679         u32 changed = 0;
 680         int i;
 681
 682         BUILD_BUG_ON(NUM_TM_CHANGED >= 32);
 683
 684         if (new_state == MT76_TM_STATE_OFF ||
 685             td->state == MT76_TM_STATE_OFF)
 686                 return 0;
 687
 688         if (td->tx_antenna_mask & ~mphy->chainmask)
 689                 return -EINVAL;
 690
 691         for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
 692                 if (tb[tm_change_map[i]])
 693                         changed |= BIT(i);
 694         }
 695
 696         mt7915_tm_update_params(phy, changed);
 697
 698         return 0;
 699 }
 700
 701 static int
 702 mt7915_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
 703 {
 704         struct mt7915_phy *phy = mphy->priv;
 705         void *rx, *rssi;
 706         int i;
 707
 708         rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
 709         if (!rx)
 710                 return -ENOMEM;
 711
 712         if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, phy->test.last_freq_offset))
 713                 return -ENOMEM;
 714
 715         rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI);
 716         if (!rssi)
 717                 return -ENOMEM;
 718
 719         for (i = 0; i < ARRAY_SIZE(phy->test.last_rcpi); i++)
 720                 if (nla_put_u8(msg, i, phy->test.last_rcpi[i]))
 721                         return -ENOMEM;
 722
 723         nla_nest_end(msg, rssi);
 724
 725         rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_IB_RSSI);
 726         if (!rssi)
 727                 return -ENOMEM;
 728
 729         for (i = 0; i < ARRAY_SIZE(phy->test.last_ib_rssi); i++)
 730                 if (nla_put_s8(msg, i, phy->test.last_ib_rssi[i]))
 731                         return -ENOMEM;
 732
 733         nla_nest_end(msg, rssi);
 734
 735         rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_WB_RSSI);
 736         if (!rssi)
 737                 return -ENOMEM;
 738
 739         for (i = 0; i < ARRAY_SIZE(phy->test.last_wb_rssi); i++)
 740                 if (nla_put_s8(msg, i, phy->test.last_wb_rssi[i]))
 741                         return -ENOMEM;
 742
 743         nla_nest_end(msg, rssi);
 744
 745         if (nla_put_u8(msg, MT76_TM_RX_ATTR_SNR, phy->test.last_snr))
 746                 return -ENOMEM;
 747
 748         nla_nest_end(msg, rx);
 749
 750         return 0;
 751 }
 752
 753 const struct mt76_testmode_ops mt7915_testmode_ops = {
 754         .set_state = mt7915_tm_set_state,
 755         .set_params = mt7915_tm_set_params,
 756         .dump_stats = mt7915_tm_dump_stats,
 757 };