1 // SPDX-License-Identifier: ISC 2 /* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */ 3 4 #include "mt7615.h" 5 #include "eeprom.h" 6 #include "mcu.h" 7 8 enum { 9 TM_CHANGED_TXPOWER_CTRL, 10 TM_CHANGED_TXPOWER, 11 TM_CHANGED_FREQ_OFFSET, 12 13 /* must be last */ 14 NUM_TM_CHANGED 15 }; 16 17 18 static const u8 tm_change_map[] = { 19 [TM_CHANGED_TXPOWER_CTRL] = MT76_TM_ATTR_TX_POWER_CONTROL, 20 [TM_CHANGED_TXPOWER] = MT76_TM_ATTR_TX_POWER, 21 [TM_CHANGED_FREQ_OFFSET] = MT76_TM_ATTR_FREQ_OFFSET, 22 }; 23 24 static const u32 reg_backup_list[] = { 25 MT_WF_PHY_RFINTF3_0(0), 26 MT_WF_PHY_RFINTF3_0(1), 27 MT_WF_PHY_RFINTF3_0(2), 28 MT_WF_PHY_RFINTF3_0(3), 29 MT_ANT_SWITCH_CON(2), 30 MT_ANT_SWITCH_CON(3), 31 MT_ANT_SWITCH_CON(4), 32 MT_ANT_SWITCH_CON(6), 33 MT_ANT_SWITCH_CON(7), 34 MT_ANT_SWITCH_CON(8), 35 }; 36 37 static const struct { 38 u16 wf; 39 u16 reg; 40 } rf_backup_list[] = { 41 { 0, 0x48 }, 42 { 1, 0x48 }, 43 { 2, 0x48 }, 44 { 3, 0x48 }, 45 }; 46 47 static int 48 mt7615_tm_set_tx_power(struct mt7615_phy *phy) 49 { 50 struct mt7615_dev *dev = phy->dev; 51 struct mt76_phy *mphy = phy->mt76; 52 int i, ret, n_chains = hweight8(mphy->antenna_mask); 53 struct cfg80211_chan_def *chandef = &mphy->chandef; 54 int freq = chandef->center_freq1, len, target_chains; 55 u8 *data, *eep = (u8 *)dev->mt76.eeprom.data; 56 enum nl80211_band band = chandef->chan->band; 57 struct sk_buff *skb; 58 struct { 59 u8 center_chan; 60 u8 dbdc_idx; 61 u8 band; 62 u8 rsv; 63 } __packed req_hdr = { 64 .center_chan = ieee80211_frequency_to_channel(freq), 65 .band = band, 66 .dbdc_idx = phy != &dev->phy, 67 }; 68 u8 *tx_power = NULL; 69 70 if (mphy->test.state != MT76_TM_STATE_OFF) 71 tx_power = mphy->test.tx_power; 72 73 len = MT7615_EE_MAX - MT_EE_NIC_CONF_0; 74 skb = mt76_mcu_msg_alloc(&dev->mt76, NULL, sizeof(req_hdr) + len); 75 if (!skb) 76 return -ENOMEM; 77 78 skb_put_data(skb, &req_hdr, sizeof(req_hdr)); 79 data = skb_put_data(skb, eep + MT_EE_NIC_CONF_0, len); 80 81 target_chains = mt7615_ext_pa_enabled(dev, band) ? 1 : n_chains; 82 for (i = 0; i < target_chains; i++) { 83 ret = mt7615_eeprom_get_target_power_index(dev, chandef->chan, i); 84 if (ret < 0) { 85 dev_kfree_skb(skb); 86 return -EINVAL; 87 } 88 89 if (tx_power && tx_power[i]) 90 data[ret - MT_EE_NIC_CONF_0] = tx_power[i]; 91 } 92 93 return mt76_mcu_skb_send_msg(&dev->mt76, skb, 94 MCU_EXT_CMD_SET_TX_POWER_CTRL, false); 95 } 96 97 static void 98 mt7615_tm_reg_backup_restore(struct mt7615_phy *phy) 99 { 100 struct mt7615_dev *dev = phy->dev; 101 u32 *b = phy->test.reg_backup; 102 int n_regs = ARRAY_SIZE(reg_backup_list); 103 int n_rf_regs = ARRAY_SIZE(rf_backup_list); 104 int i; 105 106 if (phy->mt76->test.state == MT76_TM_STATE_OFF) { 107 for (i = 0; i < n_regs; i++) 108 mt76_wr(dev, reg_backup_list[i], b[i]); 109 110 for (i = 0; i < n_rf_regs; i++) 111 mt7615_rf_wr(dev, rf_backup_list[i].wf, 112 rf_backup_list[i].reg, b[n_regs + i]); 113 return; 114 } 115 116 if (b) 117 return; 118 119 b = devm_kzalloc(dev->mt76.dev, 4 * (n_regs + n_rf_regs), 120 GFP_KERNEL); 121 if (!b) 122 return; 123 124 phy->test.reg_backup = b; 125 for (i = 0; i < n_regs; i++) 126 b[i] = mt76_rr(dev, reg_backup_list[i]); 127 for (i = 0; i < n_rf_regs; i++) 128 b[n_regs + i] = mt7615_rf_rr(dev, rf_backup_list[i].wf, 129 rf_backup_list[i].reg); 130 } 131 132 static void 133 mt7615_tm_init(struct mt7615_phy *phy) 134 { 135 struct mt7615_dev *dev = phy->dev; 136 unsigned int total_flags = ~0; 137 138 if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state)) 139 return; 140 141 mt7615_mcu_set_sku_en(phy, phy->mt76->test.state == MT76_TM_STATE_OFF); 142 143 mutex_unlock(&dev->mt76.mutex); 144 mt7615_set_channel(phy); 145 mt7615_ops.configure_filter(phy->mt76->hw, 0, &total_flags, 0); 146 mutex_lock(&dev->mt76.mutex); 147 148 mt7615_tm_reg_backup_restore(phy); 149 } 150 151 static void 152 mt7615_tm_set_rx_enable(struct mt7615_dev *dev, bool en) 153 { 154 u32 rqcr_mask = (MT_ARB_RQCR_RX_START | 155 MT_ARB_RQCR_RXV_START | 156 MT_ARB_RQCR_RXV_R_EN | 157 MT_ARB_RQCR_RXV_T_EN) * 158 (BIT(0) | BIT(MT_ARB_RQCR_BAND_SHIFT)); 159 160 if (en) { 161 mt76_clear(dev, MT_ARB_SCR, 162 MT_ARB_SCR_RX0_DISABLE | MT_ARB_SCR_RX1_DISABLE); 163 mt76_set(dev, MT_ARB_RQCR, rqcr_mask); 164 } else { 165 mt76_set(dev, MT_ARB_SCR, 166 MT_ARB_SCR_RX0_DISABLE | MT_ARB_SCR_RX1_DISABLE); 167 mt76_clear(dev, MT_ARB_RQCR, rqcr_mask); 168 } 169 } 170 171 static void 172 mt7615_tm_set_tx_antenna(struct mt7615_phy *phy, bool en) 173 { 174 struct mt7615_dev *dev = phy->dev; 175 struct mt76_testmode_data *td = &phy->mt76->test; 176 u8 mask = td->tx_antenna_mask; 177 int i; 178 179 if (!mask) 180 return; 181 182 if (!en) 183 mask = phy->mt76->chainmask; 184 185 for (i = 0; i < 4; i++) { 186 mt76_rmw_field(dev, MT_WF_PHY_RFINTF3_0(i), 187 MT_WF_PHY_RFINTF3_0_ANT, 188 (td->tx_antenna_mask & BIT(i)) ? 0 : 0xa); 189 190 } 191 192 /* 2.4 GHz band */ 193 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(3), MT_ANT_SWITCH_CON_MODE(0), 194 (td->tx_antenna_mask & BIT(0)) ? 0x8 : 0x1b); 195 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(4), MT_ANT_SWITCH_CON_MODE(2), 196 (td->tx_antenna_mask & BIT(1)) ? 0xe : 0x1b); 197 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(6), MT_ANT_SWITCH_CON_MODE1(0), 198 (td->tx_antenna_mask & BIT(2)) ? 0x0 : 0xf); 199 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(7), MT_ANT_SWITCH_CON_MODE1(2), 200 (td->tx_antenna_mask & BIT(3)) ? 0x6 : 0xf); 201 202 /* 5 GHz band */ 203 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(4), MT_ANT_SWITCH_CON_MODE(1), 204 (td->tx_antenna_mask & BIT(0)) ? 0xd : 0x1b); 205 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(2), MT_ANT_SWITCH_CON_MODE(3), 206 (td->tx_antenna_mask & BIT(1)) ? 0x13 : 0x1b); 207 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(7), MT_ANT_SWITCH_CON_MODE1(1), 208 (td->tx_antenna_mask & BIT(2)) ? 0x5 : 0xf); 209 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(8), MT_ANT_SWITCH_CON_MODE1(3), 210 (td->tx_antenna_mask & BIT(3)) ? 0xb : 0xf); 211 212 for (i = 0; i < 4; i++) { 213 u32 val; 214 215 val = mt7615_rf_rr(dev, i, 0x48); 216 val &= ~(0x3ff << 20); 217 if (td->tx_antenna_mask & BIT(i)) 218 val |= 3 << 20; 219 else 220 val |= (2 << 28) | (2 << 26) | (8 << 20); 221 mt7615_rf_wr(dev, i, 0x48, val); 222 } 223 } 224 225 static void 226 mt7615_tm_set_tx_frames(struct mt7615_phy *phy, bool en) 227 { 228 struct mt7615_dev *dev = phy->dev; 229 struct ieee80211_tx_info *info; 230 struct sk_buff *skb = phy->mt76->test.tx_skb; 231 232 mt7615_mcu_set_chan_info(phy, MCU_EXT_CMD_SET_RX_PATH); 233 mt7615_tm_set_tx_antenna(phy, en); 234 mt7615_tm_set_rx_enable(dev, !en); 235 if (!en || !skb) 236 return; 237 238 info = IEEE80211_SKB_CB(skb); 239 info->control.vif = phy->monitor_vif; 240 } 241 242 static void 243 mt7615_tm_update_params(struct mt7615_phy *phy, u32 changed) 244 { 245 struct mt7615_dev *dev = phy->dev; 246 struct mt76_testmode_data *td = &phy->mt76->test; 247 bool en = phy->mt76->test.state != MT76_TM_STATE_OFF; 248 249 if (changed & BIT(TM_CHANGED_TXPOWER_CTRL)) 250 mt7615_mcu_set_test_param(dev, MCU_ATE_SET_TX_POWER_CONTROL, 251 en, en && td->tx_power_control); 252 if (changed & BIT(TM_CHANGED_FREQ_OFFSET)) 253 mt7615_mcu_set_test_param(dev, MCU_ATE_SET_FREQ_OFFSET, 254 en, en ? td->freq_offset : 0); 255 if (changed & BIT(TM_CHANGED_TXPOWER)) 256 mt7615_tm_set_tx_power(phy); 257 } 258 259 static int 260 mt7615_tm_set_state(struct mt76_phy *mphy, enum mt76_testmode_state state) 261 { 262 struct mt7615_phy *phy = mphy->priv; 263 struct mt76_testmode_data *td = &mphy->test; 264 enum mt76_testmode_state prev_state = td->state; 265 266 mphy->test.state = state; 267 268 if (prev_state == MT76_TM_STATE_TX_FRAMES) 269 mt7615_tm_set_tx_frames(phy, false); 270 else if (state == MT76_TM_STATE_TX_FRAMES) 271 mt7615_tm_set_tx_frames(phy, true); 272 273 if (state <= MT76_TM_STATE_IDLE) 274 mt7615_tm_init(phy); 275 276 if ((state == MT76_TM_STATE_IDLE && 277 prev_state == MT76_TM_STATE_OFF) || 278 (state == MT76_TM_STATE_OFF && 279 prev_state == MT76_TM_STATE_IDLE)) { 280 u32 changed = 0; 281 int i; 282 283 for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) { 284 u16 cur = tm_change_map[i]; 285 286 if (td->param_set[cur / 32] & BIT(cur % 32)) 287 changed |= BIT(i); 288 } 289 290 mt7615_tm_update_params(phy, changed); 291 } 292 293 return 0; 294 } 295 296 static int 297 mt7615_tm_set_params(struct mt76_phy *mphy, struct nlattr **tb, 298 enum mt76_testmode_state new_state) 299 { 300 struct mt76_testmode_data *td = &mphy->test; 301 struct mt7615_phy *phy = mphy->priv; 302 u32 changed = 0; 303 int i; 304 305 BUILD_BUG_ON(NUM_TM_CHANGED >= 32); 306 307 if (new_state == MT76_TM_STATE_OFF || 308 td->state == MT76_TM_STATE_OFF) 309 return 0; 310 311 if (td->tx_antenna_mask & ~mphy->chainmask) 312 return -EINVAL; 313 314 for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) { 315 if (tb[tm_change_map[i]]) 316 changed |= BIT(i); 317 } 318 319 mt7615_tm_update_params(phy, changed); 320 321 return 0; 322 } 323 324 static int 325 mt7615_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg) 326 { 327 struct mt7615_phy *phy = mphy->priv; 328 void *rx, *rssi; 329 int i; 330 331 rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX); 332 if (!rx) 333 return -ENOMEM; 334 335 if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, phy->test.last_freq_offset)) 336 return -ENOMEM; 337 338 rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI); 339 if (!rssi) 340 return -ENOMEM; 341 342 for (i = 0; i < ARRAY_SIZE(phy->test.last_rcpi); i++) 343 if (nla_put_u8(msg, i, phy->test.last_rcpi[i])) 344 return -ENOMEM; 345 346 nla_nest_end(msg, rssi); 347 348 rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_IB_RSSI); 349 if (!rssi) 350 return -ENOMEM; 351 352 for (i = 0; i < ARRAY_SIZE(phy->test.last_ib_rssi); i++) 353 if (nla_put_s8(msg, i, phy->test.last_ib_rssi[i])) 354 return -ENOMEM; 355 356 nla_nest_end(msg, rssi); 357 358 rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_WB_RSSI); 359 if (!rssi) 360 return -ENOMEM; 361 362 for (i = 0; i < ARRAY_SIZE(phy->test.last_wb_rssi); i++) 363 if (nla_put_s8(msg, i, phy->test.last_wb_rssi[i])) 364 return -ENOMEM; 365 366 nla_nest_end(msg, rssi); 367 368 nla_nest_end(msg, rx); 369 370 return 0; 371 } 372 373 const struct mt76_testmode_ops mt7615_testmode_ops = { 374 .set_state = mt7615_tm_set_state, 375 .set_params = mt7615_tm_set_params, 376 .dump_stats = mt7615_tm_dump_stats, 377 }; 378