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 }; 378