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_tm_mode_ctrl(dev, en);
 406         mt7915_tm_reg_backup_restore(phy);
 407         mt7915_tm_set_trx(phy, TM_MAC_TXRX, !en);
 408
 409         mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en);
 410 }
 411
 412 static void
 413 mt7915_tm_update_channel(struct mt7915_phy *phy)
 414 {
 415         mutex_unlock(&phy->dev->mt76.mutex);
 416         mt7915_set_channel(phy);
 417         mutex_lock(&phy->dev->mt76.mutex);
 418
 419         mt7915_mcu_set_chan_info(phy, MCU_EXT_CMD_SET_RX_PATH);
 420 }
 421
 422 static void
 423 mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en)
 424 {
 425         static const u8 spe_idx_map[] = {0, 0, 1, 0, 3, 2, 4, 0,
 426                                          9, 8, 6, 10, 16, 12, 18, 0};
 427         struct mt76_testmode_data *td = &phy->mt76->test;
 428         struct mt7915_dev *dev = phy->dev;
 429         struct ieee80211_tx_info *info;
 430         u8 duty_cycle = td->tx_duty_cycle;
 431         u32 tx_time = td->tx_time;
 432         u32 ipg = td->tx_ipg;
 433
 434         mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
 435         mt7915_tm_clean_hwq(phy, dev->mt76.global_wcid.idx);
 436
 437         if (en) {
 438                 mt7915_tm_update_channel(phy);
 439
 440                 if (td->tx_spe_idx) {
 441                         phy->test.spe_idx = td->tx_spe_idx;
 442                 } else {
 443                         u8 tx_ant = td->tx_antenna_mask;
 444
 445                         if (phy != &dev->phy)
 446                                 tx_ant >>= 2;
 447                         phy->test.spe_idx = spe_idx_map[tx_ant];
 448                 }
 449         }
 450
 451         /* if all three params are set, duty_cycle will be ignored */
 452         if (duty_cycle && tx_time && !ipg) {
 453                 ipg = tx_time * 100 / duty_cycle - tx_time;
 454         } else if (duty_cycle && !tx_time && ipg) {
 455                 if (duty_cycle < 100)
 456                         tx_time = duty_cycle * ipg / (100 - duty_cycle);
 457         }
 458
 459         mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode);
 460         mt7915_tm_set_tx_len(phy, tx_time);
 461
 462         if (ipg)
 463                 td->tx_queued_limit = MT76_TM_TIMEOUT * 1000000 / ipg / 2;
 464
 465         if (!en || !td->tx_skb)
 466                 return;
 467
 468         info = IEEE80211_SKB_CB(td->tx_skb);
 469         info->control.vif = phy->monitor_vif;
 470
 471         mt7915_tm_set_trx(phy, TM_MAC_TX, en);
 472 }
 473
 474 static void
 475 mt7915_tm_set_rx_frames(struct mt7915_phy *phy, bool en)
 476 {
 477         if (en)
 478                 mt7915_tm_update_channel(phy);
 479
 480         mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, en);
 481 }
 482
 483 static int
 484 mt7915_tm_rf_switch_mode(struct mt7915_dev *dev, u32 oper)
 485 {
 486         struct mt7915_tm_rf_test req = {
 487                 .op.op_mode = cpu_to_le32(oper),
 488         };
 489
 490         return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD_RF_TEST, &req,
 491                                  sizeof(req), true);
 492 }
 493
 494 static int
 495 mt7915_tm_set_tx_cont(struct mt7915_phy *phy, bool en)
 496 {
 497 #define TX_CONT_START   0x05
 498 #define TX_CONT_STOP    0x06
 499         struct mt7915_dev *dev = phy->dev;
 500         struct cfg80211_chan_def *chandef = &phy->mt76->chandef;
 501         int freq1 = ieee80211_frequency_to_channel(chandef->center_freq1);
 502         struct mt76_testmode_data *td = &phy->mt76->test;
 503         u32 func_idx = en ? TX_CONT_START : TX_CONT_STOP;
 504         u8 rate_idx = td->tx_rate_idx, mode;
 505         u16 rateval;
 506         struct mt7915_tm_rf_test req = {
 507                 .action = 1,
 508                 .icap_len = 120,
 509                 .op.rf.func_idx = cpu_to_le32(func_idx),
 510         };
 511         struct tm_tx_cont *tx_cont = &req.op.rf.param.tx_cont;
 512
 513         tx_cont->control_ch = chandef->chan->hw_value;
 514         tx_cont->center_ch = freq1;
 515         tx_cont->tx_ant = td->tx_antenna_mask;
 516         tx_cont->band = phy != &dev->phy;
 517
 518         switch (chandef->width) {
 519         case NL80211_CHAN_WIDTH_40:
 520                 tx_cont->bw = CMD_CBW_40MHZ;
 521                 break;
 522         case NL80211_CHAN_WIDTH_80:
 523                 tx_cont->bw = CMD_CBW_80MHZ;
 524                 break;
 525         case NL80211_CHAN_WIDTH_80P80:
 526                 tx_cont->bw = CMD_CBW_8080MHZ;
 527                 break;
 528         case NL80211_CHAN_WIDTH_160:
 529                 tx_cont->bw = CMD_CBW_160MHZ;
 530                 break;
 531         case NL80211_CHAN_WIDTH_5:
 532                 tx_cont->bw = CMD_CBW_5MHZ;
 533                 break;
 534         case NL80211_CHAN_WIDTH_10:
 535                 tx_cont->bw = CMD_CBW_10MHZ;
 536                 break;
 537         case NL80211_CHAN_WIDTH_20:
 538                 tx_cont->bw = CMD_CBW_20MHZ;
 539                 break;
 540         case NL80211_CHAN_WIDTH_20_NOHT:
 541                 tx_cont->bw = CMD_CBW_20MHZ;
 542                 break;
 543         default:
 544                 break;
 545         }
 546
 547         if (!en) {
 548                 req.op.rf.param.func_data = cpu_to_le32(phy != &dev->phy);
 549                 goto out;
 550         }
 551
 552         if (td->tx_rate_mode <= MT76_TM_TX_MODE_OFDM) {
 553                 struct ieee80211_supported_band *sband;
 554                 u8 idx = rate_idx;
 555
 556                 if (chandef->chan->band == NL80211_BAND_5GHZ)
 557                         sband = &phy->mt76->sband_5g.sband;
 558                 else
 559                         sband = &phy->mt76->sband_2g.sband;
 560
 561                 if (td->tx_rate_mode == MT76_TM_TX_MODE_OFDM)
 562                         idx += 4;
 563                 rate_idx = sband->bitrates[idx].hw_value & 0xff;
 564         }
 565
 566         switch (td->tx_rate_mode) {
 567         case MT76_TM_TX_MODE_CCK:
 568                 mode = MT_PHY_TYPE_CCK;
 569                 break;
 570         case MT76_TM_TX_MODE_OFDM:
 571                 mode = MT_PHY_TYPE_OFDM;
 572                 break;
 573         case MT76_TM_TX_MODE_HT:
 574                 mode = MT_PHY_TYPE_HT;
 575                 break;
 576         case MT76_TM_TX_MODE_VHT:
 577                 mode = MT_PHY_TYPE_VHT;
 578                 break;
 579         case MT76_TM_TX_MODE_HE_SU:
 580                 mode = MT_PHY_TYPE_HE_SU;
 581                 break;
 582         case MT76_TM_TX_MODE_HE_EXT_SU:
 583                 mode = MT_PHY_TYPE_HE_EXT_SU;
 584                 break;
 585         case MT76_TM_TX_MODE_HE_TB:
 586                 mode = MT_PHY_TYPE_HE_TB;
 587                 break;
 588         case MT76_TM_TX_MODE_HE_MU:
 589                 mode = MT_PHY_TYPE_HE_MU;
 590                 break;
 591         default:
 592                 break;
 593         }
 594
 595         rateval =  mode << 6 | rate_idx;
 596         tx_cont->rateval = cpu_to_le16(rateval);
 597
 598 out:
 599         if (!en) {
 600                 int ret;
 601
 602                 ret = mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD_RF_TEST, &req,
 603                                         sizeof(req), true);
 604                 if (ret)
 605                         return ret;
 606
 607                 return mt7915_tm_rf_switch_mode(dev, RF_OPER_NORMAL);
 608         }
 609
 610         mt7915_tm_rf_switch_mode(dev, RF_OPER_RF_TEST);
 611         mt7915_tm_update_channel(phy);
 612
 613         return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD_RF_TEST, &req,
 614                                  sizeof(req), true);
 615 }
 616
 617 static void
 618 mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed)
 619 {
 620         struct mt76_testmode_data *td = &phy->mt76->test;
 621         bool en = phy->mt76->test.state != MT76_TM_STATE_OFF;
 622
 623         if (changed & BIT(TM_CHANGED_FREQ_OFFSET))
 624                 mt7915_tm_set_freq_offset(phy, en, en ? td->freq_offset : 0);
 625         if (changed & BIT(TM_CHANGED_TXPOWER))
 626                 mt7915_tm_set_tx_power(phy);
 627 }
 628
 629 static int
 630 mt7915_tm_set_state(struct mt76_phy *mphy, enum mt76_testmode_state state)
 631 {
 632         struct mt76_testmode_data *td = &mphy->test;
 633         struct mt7915_phy *phy = mphy->priv;
 634         enum mt76_testmode_state prev_state = td->state;
 635
 636         mphy->test.state = state;
 637
 638         if (prev_state == MT76_TM_STATE_TX_FRAMES ||
 639             state == MT76_TM_STATE_TX_FRAMES)
 640                 mt7915_tm_set_tx_frames(phy, state == MT76_TM_STATE_TX_FRAMES);
 641         else if (prev_state == MT76_TM_STATE_RX_FRAMES ||
 642                  state == MT76_TM_STATE_RX_FRAMES)
 643                 mt7915_tm_set_rx_frames(phy, state == MT76_TM_STATE_RX_FRAMES);
 644         else if (prev_state == MT76_TM_STATE_TX_CONT ||
 645                  state == MT76_TM_STATE_TX_CONT)
 646                 mt7915_tm_set_tx_cont(phy, state == MT76_TM_STATE_TX_CONT);
 647         else if (prev_state == MT76_TM_STATE_OFF ||
 648                  state == MT76_TM_STATE_OFF)
 649                 mt7915_tm_init(phy, !(state == MT76_TM_STATE_OFF));
 650
 651         if ((state == MT76_TM_STATE_IDLE &&
 652              prev_state == MT76_TM_STATE_OFF) ||
 653             (state == MT76_TM_STATE_OFF &&
 654              prev_state == MT76_TM_STATE_IDLE)) {
 655                 u32 changed = 0;
 656                 int i;
 657
 658                 for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
 659                         u16 cur = tm_change_map[i];
 660
 661                         if (td->param_set[cur / 32] & BIT(cur % 32))
 662                                 changed |= BIT(i);
 663                 }
 664
 665                 mt7915_tm_update_params(phy, changed);
 666         }
 667
 668         return 0;
 669 }
 670
 671 static int
 672 mt7915_tm_set_params(struct mt76_phy *mphy, struct nlattr **tb,
 673                      enum mt76_testmode_state new_state)
 674 {
 675         struct mt76_testmode_data *td = &mphy->test;
 676         struct mt7915_phy *phy = mphy->priv;
 677         u32 changed = 0;
 678         int i;
 679
 680         BUILD_BUG_ON(NUM_TM_CHANGED >= 32);
 681
 682         if (new_state == MT76_TM_STATE_OFF ||
 683             td->state == MT76_TM_STATE_OFF)
 684                 return 0;
 685
 686         if (td->tx_antenna_mask & ~mphy->chainmask)
 687                 return -EINVAL;
 688
 689         for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
 690                 if (tb[tm_change_map[i]])
 691                         changed |= BIT(i);
 692         }
 693
 694         mt7915_tm_update_params(phy, changed);
 695
 696         return 0;
 697 }
 698
 699 static int
 700 mt7915_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
 701 {
 702         struct mt7915_phy *phy = mphy->priv;
 703         void *rx, *rssi;
 704         int i;
 705
 706         rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
 707         if (!rx)
 708                 return -ENOMEM;
 709
 710         if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, phy->test.last_freq_offset))
 711                 return -ENOMEM;
 712
 713         rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI);
 714         if (!rssi)
 715                 return -ENOMEM;
 716
 717         for (i = 0; i < ARRAY_SIZE(phy->test.last_rcpi); i++)
 718                 if (nla_put_u8(msg, i, phy->test.last_rcpi[i]))
 719                         return -ENOMEM;
 720
 721         nla_nest_end(msg, rssi);
 722
 723         rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_IB_RSSI);
 724         if (!rssi)
 725                 return -ENOMEM;
 726
 727         for (i = 0; i < ARRAY_SIZE(phy->test.last_ib_rssi); i++)
 728                 if (nla_put_s8(msg, i, phy->test.last_ib_rssi[i]))
 729                         return -ENOMEM;
 730
 731         nla_nest_end(msg, rssi);
 732
 733         rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_WB_RSSI);
 734         if (!rssi)
 735                 return -ENOMEM;
 736
 737         for (i = 0; i < ARRAY_SIZE(phy->test.last_wb_rssi); i++)
 738                 if (nla_put_s8(msg, i, phy->test.last_wb_rssi[i]))
 739                         return -ENOMEM;
 740
 741         nla_nest_end(msg, rssi);
 742
 743         if (nla_put_u8(msg, MT76_TM_RX_ATTR_SNR, phy->test.last_snr))
 744                 return -ENOMEM;
 745
 746         nla_nest_end(msg, rx);
 747
 748         return 0;
 749 }
 750
 751 const struct mt76_testmode_ops mt7915_testmode_ops = {
 752         .set_state = mt7915_tm_set_state,
 753         .set_params = mt7915_tm_set_params,
 754         .dump_stats = mt7915_tm_dump_stats,
 755 };