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 (dev->mt76.test.state != MT76_TM_STATE_OFF)
  74                 tx_power = dev->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_dev *dev, bool en, u32 val)
  89 {
  90         struct mt7915_tm_cmd req = {
  91                 .testmode_en = en,
  92                 .param_idx = MCU_ATE_SET_FREQ_OFFSET,
  93                 .param.freq.freq_offset = cpu_to_le32(val),
  94         };
  95
  96         return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD_ATE_CTRL, &req,
  97                                  sizeof(req), false);
  98 }
  99
 100 static int
 101 mt7915_tm_mode_ctrl(struct mt7915_dev *dev, bool enable)
 102 {
 103         struct {
 104                 u8 format_id;
 105                 bool enable;
 106                 u8 rsv[2];
 107         } __packed req = {
 108                 .format_id = 0x6,
 109                 .enable = enable,
 110         };
 111
 112         return mt76_mcu_send_msg(&dev->mt76,
 113                                  MCU_EXT_CMD_TX_POWER_FEATURE_CTRL,
 114                                  &req, sizeof(req), false);
 115 }
 116
 117 static int
 118 mt7915_tm_set_trx(struct mt7915_dev *dev, struct mt7915_phy *phy,
 119                   int type, bool en)
 120 {
 121         struct mt7915_tm_cmd req = {
 122                 .testmode_en = 1,
 123                 .param_idx = MCU_ATE_SET_TRX,
 124                 .param.trx.type = type,
 125                 .param.trx.enable = en,
 126                 .param.trx.band = phy != &dev->phy,
 127         };
 128
 129         return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD_ATE_CTRL, &req,
 130                                  sizeof(req), false);
 131 }
 132
 133 static void
 134 mt7915_tm_reg_backup_restore(struct mt7915_dev *dev, struct mt7915_phy *phy)
 135 {
 136         int n_regs = ARRAY_SIZE(reg_backup_list);
 137         bool ext_phy = phy != &dev->phy;
 138         u32 *b = dev->test.reg_backup;
 139         int i;
 140
 141         if (dev->mt76.test.state == MT76_TM_STATE_OFF) {
 142                 for (i = 0; i < n_regs; i++)
 143                         mt76_wr(dev, reg_backup_list[i].band[ext_phy], b[i]);
 144                 return;
 145         }
 146
 147         if (b)
 148                 return;
 149
 150         b = devm_kzalloc(dev->mt76.dev, 4 * n_regs, GFP_KERNEL);
 151         if (!b)
 152                 return;
 153
 154         dev->test.reg_backup = b;
 155         for (i = 0; i < n_regs; i++)
 156                 b[i] = mt76_rr(dev, reg_backup_list[i].band[ext_phy]);
 157
 158         mt76_clear(dev, MT_AGG_PCR0(ext_phy, 0), MT_AGG_PCR0_MM_PROT |
 159                    MT_AGG_PCR0_GF_PROT | MT_AGG_PCR0_ERP_PROT |
 160                    MT_AGG_PCR0_VHT_PROT | MT_AGG_PCR0_BW20_PROT |
 161                    MT_AGG_PCR0_BW40_PROT | MT_AGG_PCR0_BW80_PROT);
 162         mt76_set(dev, MT_AGG_PCR0(ext_phy, 0), MT_AGG_PCR0_PTA_WIN_DIS);
 163
 164         mt76_wr(dev, MT_AGG_PCR0(ext_phy, 1), MT_AGG_PCR1_RTS0_NUM_THRES |
 165                 MT_AGG_PCR1_RTS0_LEN_THRES);
 166
 167         mt76_clear(dev, MT_AGG_MRCR(ext_phy), MT_AGG_MRCR_BAR_CNT_LIMIT |
 168                    MT_AGG_MRCR_LAST_RTS_CTS_RN | MT_AGG_MRCR_RTS_FAIL_LIMIT |
 169                    MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT);
 170
 171         mt76_rmw(dev, MT_AGG_MRCR(ext_phy), MT_AGG_MRCR_RTS_FAIL_LIMIT |
 172                  MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT,
 173                  FIELD_PREP(MT_AGG_MRCR_RTS_FAIL_LIMIT, 1) |
 174                  FIELD_PREP(MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT, 1));
 175
 176         mt76_wr(dev, MT_TMAC_TFCR0(ext_phy), 0);
 177         mt76_clear(dev, MT_TMAC_TCR0(ext_phy), MT_TMAC_TCR0_TBTT_STOP_CTRL);
 178
 179         /* config rx filter for testmode rx */
 180         mt76_wr(dev, MT_WF_RFCR(ext_phy), 0xcf70a);
 181         mt76_wr(dev, MT_WF_RFCR1(ext_phy), 0);
 182 }
 183
 184 static void
 185 mt7915_tm_init(struct mt7915_dev *dev)
 186 {
 187         bool en = !(dev->mt76.test.state == MT76_TM_STATE_OFF);
 188
 189         if (!test_bit(MT76_STATE_RUNNING, &dev->phy.mt76->state))
 190                 return;
 191
 192         mt7915_tm_mode_ctrl(dev, en);
 193         mt7915_tm_reg_backup_restore(dev, &dev->phy);
 194         mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_TXRX, !en);
 195 }
 196
 197 static void
 198 mt7915_tm_set_tx_frames(struct mt7915_dev *dev, bool en)
 199 {
 200         static const u8 spe_idx_map[] = {0, 0, 1, 0, 3, 2, 4, 0,
 201                                          9, 8, 6, 10, 16, 12, 18, 0};
 202         struct sk_buff *skb = dev->mt76.test.tx_skb;
 203         struct ieee80211_tx_info *info;
 204
 205         mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_RX_RXV, false);
 206
 207         if (en) {
 208                 u8 tx_ant = dev->mt76.test.tx_antenna_mask;
 209
 210                 mutex_unlock(&dev->mt76.mutex);
 211                 mt7915_set_channel(&dev->phy);
 212                 mutex_lock(&dev->mt76.mutex);
 213
 214                 mt7915_mcu_set_chan_info(&dev->phy, MCU_EXT_CMD_SET_RX_PATH);
 215                 dev->test.spe_idx = spe_idx_map[tx_ant];
 216         }
 217
 218         mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_TX, en);
 219
 220         if (!en || !skb)
 221                 return;
 222
 223         info = IEEE80211_SKB_CB(skb);
 224         info->control.vif = dev->phy.monitor_vif;
 225 }
 226
 227 static void
 228 mt7915_tm_set_rx_frames(struct mt7915_dev *dev, bool en)
 229 {
 230         if (en) {
 231                 mutex_unlock(&dev->mt76.mutex);
 232                 mt7915_set_channel(&dev->phy);
 233                 mutex_lock(&dev->mt76.mutex);
 234
 235                 mt7915_mcu_set_chan_info(&dev->phy, MCU_EXT_CMD_SET_RX_PATH);
 236         }
 237
 238         mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_RX_RXV, en);
 239 }
 240
 241 static void
 242 mt7915_tm_update_params(struct mt7915_dev *dev, u32 changed)
 243 {
 244         struct mt76_testmode_data *td = &dev->mt76.test;
 245         bool en = dev->mt76.test.state != MT76_TM_STATE_OFF;
 246
 247         if (changed & BIT(TM_CHANGED_FREQ_OFFSET))
 248                 mt7915_tm_set_freq_offset(dev, en, en ? td->freq_offset : 0);
 249         if (changed & BIT(TM_CHANGED_TXPOWER))
 250                 mt7915_tm_set_tx_power(&dev->phy);
 251 }
 252
 253 static int
 254 mt7915_tm_set_state(struct mt76_dev *mdev, enum mt76_testmode_state state)
 255 {
 256         struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
 257         struct mt76_testmode_data *td = &mdev->test;
 258         enum mt76_testmode_state prev_state = td->state;
 259
 260         mdev->test.state = state;
 261
 262         if (prev_state == MT76_TM_STATE_TX_FRAMES)
 263                 mt7915_tm_set_tx_frames(dev, false);
 264         else if (state == MT76_TM_STATE_TX_FRAMES)
 265                 mt7915_tm_set_tx_frames(dev, true);
 266         else if (prev_state == MT76_TM_STATE_RX_FRAMES)
 267                 mt7915_tm_set_rx_frames(dev, false);
 268         else if (state == MT76_TM_STATE_RX_FRAMES)
 269                 mt7915_tm_set_rx_frames(dev, true);
 270         else if (prev_state == MT76_TM_STATE_OFF || state == MT76_TM_STATE_OFF)
 271                 mt7915_tm_init(dev);
 272
 273         if ((state == MT76_TM_STATE_IDLE &&
 274              prev_state == MT76_TM_STATE_OFF) ||
 275             (state == MT76_TM_STATE_OFF &&
 276              prev_state == MT76_TM_STATE_IDLE)) {
 277                 u32 changed = 0;
 278                 int i;
 279
 280                 for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
 281                         u16 cur = tm_change_map[i];
 282
 283                         if (td->param_set[cur / 32] & BIT(cur % 32))
 284                                 changed |= BIT(i);
 285                 }
 286
 287                 mt7915_tm_update_params(dev, changed);
 288         }
 289
 290         return 0;
 291 }
 292
 293 static int
 294 mt7915_tm_set_params(struct mt76_dev *mdev, struct nlattr **tb,
 295                      enum mt76_testmode_state new_state)
 296 {
 297         struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
 298         struct mt76_testmode_data *td = &dev->mt76.test;
 299         u32 changed = 0;
 300         int i;
 301
 302         BUILD_BUG_ON(NUM_TM_CHANGED >= 32);
 303
 304         if (new_state == MT76_TM_STATE_OFF ||
 305             td->state == MT76_TM_STATE_OFF)
 306                 return 0;
 307
 308         if (td->tx_antenna_mask & ~dev->phy.chainmask)
 309                 return -EINVAL;
 310
 311         for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
 312                 if (tb[tm_change_map[i]])
 313                         changed |= BIT(i);
 314         }
 315
 316         mt7915_tm_update_params(dev, changed);
 317
 318         return 0;
 319 }
 320
 321 static int
 322 mt7915_tm_dump_stats(struct mt76_dev *mdev, struct sk_buff *msg)
 323 {
 324         struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
 325         void *rx, *rssi;
 326         int i;
 327
 328         rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
 329         if (!rx)
 330                 return -ENOMEM;
 331
 332         if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, dev->test.last_freq_offset))
 333                 return -ENOMEM;
 334
 335         rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI);
 336         if (!rssi)
 337                 return -ENOMEM;
 338
 339         for (i = 0; i < ARRAY_SIZE(dev->test.last_rcpi); i++)
 340                 if (nla_put_u8(msg, i, dev->test.last_rcpi[i]))
 341                         return -ENOMEM;
 342
 343         nla_nest_end(msg, rssi);
 344
 345         rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_IB_RSSI);
 346         if (!rssi)
 347                 return -ENOMEM;
 348
 349         for (i = 0; i < ARRAY_SIZE(dev->test.last_ib_rssi); i++)
 350                 if (nla_put_s8(msg, i, dev->test.last_ib_rssi[i]))
 351                         return -ENOMEM;
 352
 353         nla_nest_end(msg, rssi);
 354
 355         rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_WB_RSSI);
 356         if (!rssi)
 357                 return -ENOMEM;
 358
 359         for (i = 0; i < ARRAY_SIZE(dev->test.last_wb_rssi); i++)
 360                 if (nla_put_s8(msg, i, dev->test.last_wb_rssi[i]))
 361                         return -ENOMEM;
 362
 363         nla_nest_end(msg, rssi);
 364
 365         if (nla_put_u8(msg, MT76_TM_RX_ATTR_SNR, dev->test.last_snr))
 366                 return -ENOMEM;
 367
 368         nla_nest_end(msg, rx);
 369
 370         return 0;
 371 }
 372
 373 const struct mt76_testmode_ops mt7915_testmode_ops = {
 374         .set_state = mt7915_tm_set_state,
 375         .set_params = mt7915_tm_set_params,
 376         .dump_stats = mt7915_tm_dump_stats,
 377 };