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