developer | 20d6771 | 2022-03-02 14:09:32 +0800 | [diff] [blame] | 1 | From 8027e94f1564089d719a6fb0eab7d29bb2981bf0 Mon Sep 17 00:00:00 2001 |
| 2 | From: Shayne Chen <shayne.chen@mediatek.com> |
| 3 | Date: Tue, 11 May 2021 16:24:09 +0800 |
| 4 | Subject: [PATCH 1110/1112] mt76: mt7915: implement aid support in testmode |
| 5 | |
| 6 | Add support for virtual stations in mt7915 testmode. |
| 7 | |
| 8 | Signed-off-by: Shayne Chen <shayne.chen@mediatek.com> |
| 9 | --- |
| 10 | .../wireless/mediatek/mt76/mt76_connac_mcu.c | 5 + |
| 11 | .../net/wireless/mediatek/mt76/mt7915/mac.c | 25 +- |
| 12 | .../wireless/mediatek/mt76/mt7915/testmode.c | 231 +++++++++++++++--- |
| 13 | 3 files changed, 216 insertions(+), 45 deletions(-) |
| 14 | |
| 15 | diff --git a/mt76_connac_mcu.c b/mt76_connac_mcu.c |
| 16 | index eac096c..a361ab6 100644 |
| 17 | --- a/mt76_connac_mcu.c |
| 18 | +++ b/mt76_connac_mcu.c |
| 19 | @@ -389,6 +389,7 @@ void mt76_connac_mcu_sta_basic_tlv(struct sk_buff *skb, |
| 20 | switch (vif->type) { |
| 21 | case NL80211_IFTYPE_MESH_POINT: |
| 22 | case NL80211_IFTYPE_AP: |
| 23 | + case NL80211_IFTYPE_MONITOR: |
| 24 | if (vif->p2p) |
| 25 | conn_type = CONNECTION_P2P_GC; |
| 26 | else |
| 27 | @@ -577,6 +578,10 @@ void mt76_connac_mcu_wtbl_generic_tlv(struct mt76_dev *dev, |
| 28 | wtbl_tlv, sta_wtbl); |
| 29 | spe = (struct wtbl_spe *)tlv; |
| 30 | spe->spe_idx = 24; |
| 31 | + |
| 32 | + /* check */ |
| 33 | + if (vif->type == NL80211_IFTYPE_MONITOR) |
| 34 | + rx->rca1 = 0; |
| 35 | } |
| 36 | EXPORT_SYMBOL_GPL(mt76_connac_mcu_wtbl_generic_tlv); |
| 37 | |
| 38 | diff --git a/mt7915/mac.c b/mt7915/mac.c |
| 39 | index fb42446..2ad4cb1 100644 |
| 40 | --- a/mt7915/mac.c |
| 41 | +++ b/mt7915/mac.c |
| 42 | @@ -906,16 +906,28 @@ mt7915_mac_write_txwi_tm(struct mt7915_phy *phy, __le32 *txwi, |
| 43 | { |
| 44 | #ifdef CONFIG_NL80211_TESTMODE |
| 45 | struct mt76_testmode_data *td = &phy->mt76->test; |
| 46 | + struct mt76_testmode_sta_data *sd = &td->sd; |
| 47 | const struct ieee80211_rate *r; |
| 48 | - u8 bw, mode, nss = td->tx_rate_nss; |
| 49 | - u8 rate_idx = td->tx_rate_idx; |
| 50 | + u8 bw, mode, nss, rate_idx; |
| 51 | u16 rateval = 0; |
| 52 | u32 val; |
| 53 | bool cck = false; |
| 54 | int band; |
| 55 | |
| 56 | - if (skb != phy->mt76->test.tx_skb) |
| 57 | - return; |
| 58 | + if (mt76_testmode_has_sta(phy->mt76)) { |
| 59 | + struct mt76_testmode_sta *tm_sta; |
| 60 | + int i; |
| 61 | + |
| 62 | + mt76_testmode_for_each_sta(phy->mt76, i, tm_sta) { |
| 63 | + if (tm_sta->tx_skb == skb) { |
| 64 | + sd = &tm_sta->sd; |
| 65 | + break; |
| 66 | + } |
| 67 | + } |
| 68 | + } |
| 69 | + |
| 70 | + nss = sd->tx_rate_nss; |
| 71 | + rate_idx = sd->tx_rate_idx; |
| 72 | |
| 73 | switch (td->tx_rate_mode) { |
| 74 | case MT76_TM_TX_MODE_HT: |
| 75 | @@ -1005,7 +1017,7 @@ mt7915_mac_write_txwi_tm(struct mt7915_phy *phy, __le32 *txwi, |
| 76 | if (mode >= MT_PHY_TYPE_HE_SU) |
| 77 | val |= FIELD_PREP(MT_TXD6_HELTF, td->tx_ltf); |
| 78 | |
| 79 | - if (td->tx_rate_ldpc || (bw > 0 && mode >= MT_PHY_TYPE_HE_SU)) |
| 80 | + if (sd->tx_rate_ldpc || (bw > 0 && mode >= MT_PHY_TYPE_HE_SU)) |
| 81 | val |= MT_TXD6_LDPC; |
| 82 | |
| 83 | txwi[1] &= ~cpu_to_le32(MT_TXD1_VTA); |
| 84 | @@ -1474,6 +1486,9 @@ mt7915_mac_tx_free(struct mt7915_dev *dev, void *data, int len) |
| 85 | continue; |
| 86 | |
| 87 | msta = container_of(wcid, struct mt7915_sta, wcid); |
| 88 | + if (mt76_testmode_enabled(msta->vif->phy->mt76)) |
| 89 | + continue; |
| 90 | + |
| 91 | spin_lock_bh(&dev->sta_poll_lock); |
| 92 | if (list_empty(&msta->poll_list)) |
| 93 | list_add_tail(&msta->poll_list, &dev->sta_poll_list); |
| 94 | diff --git a/mt7915/testmode.c b/mt7915/testmode.c |
| 95 | index 054829e..29c173d 100644 |
| 96 | --- a/mt7915/testmode.c |
| 97 | +++ b/mt7915/testmode.c |
| 98 | @@ -11,6 +11,7 @@ enum { |
| 99 | TM_CHANGED_FREQ_OFFSET, |
| 100 | TM_CHANGED_CFG, |
| 101 | TM_CHANGED_OFF_CH_SCAN_CH, |
| 102 | + TM_CHANGED_AID, |
| 103 | |
| 104 | /* must be last */ |
| 105 | NUM_TM_CHANGED |
| 106 | @@ -21,6 +22,7 @@ static const u8 tm_change_map[] = { |
| 107 | [TM_CHANGED_FREQ_OFFSET] = MT76_TM_ATTR_FREQ_OFFSET, |
| 108 | [TM_CHANGED_CFG] = MT76_TM_ATTR_CFG, |
| 109 | [TM_CHANGED_OFF_CH_SCAN_CH] = MT76_TM_ATTR_OFF_CH_SCAN_CH, |
| 110 | + [TM_CHANGED_AID] = MT76_TM_ATTR_AID, |
| 111 | }; |
| 112 | |
| 113 | struct reg_band { |
| 114 | @@ -142,18 +144,33 @@ mt7915_tm_set_trx(struct mt7915_phy *phy, int type, bool en) |
| 115 | } |
| 116 | |
| 117 | static int |
| 118 | -mt7915_tm_clean_hwq(struct mt7915_phy *phy, u8 wcid) |
| 119 | +mt7915_tm_clean_hwq(struct mt7915_phy *phy) |
| 120 | { |
| 121 | struct mt7915_dev *dev = phy->dev; |
| 122 | struct mt7915_tm_cmd req = { |
| 123 | .testmode_en = 1, |
| 124 | .param_idx = MCU_ATE_CLEAN_TXQUEUE, |
| 125 | - .param.clean.wcid = wcid, |
| 126 | .param.clean.band = phy != &dev->phy, |
| 127 | }; |
| 128 | + struct mt76_testmode_sta *tm_sta; |
| 129 | + int ret, i; |
| 130 | |
| 131 | - return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req, |
| 132 | - sizeof(req), false); |
| 133 | + if (!mt76_testmode_has_sta(phy->mt76)) { |
| 134 | + req.param.clean.wcid = dev->mt76.global_wcid.idx; |
| 135 | + |
| 136 | + return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), |
| 137 | + &req, sizeof(req), false); |
| 138 | + } |
| 139 | + |
| 140 | + mt76_testmode_for_each_sta(phy->mt76, i, tm_sta) { |
| 141 | + req.param.clean.wcid = phy->mt76->test.tm_wcid[i]->idx; |
| 142 | + ret = mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), |
| 143 | + &req, sizeof(req), false); |
| 144 | + if (ret) |
| 145 | + return ret; |
| 146 | + } |
| 147 | + |
| 148 | + return 0; |
| 149 | } |
| 150 | |
| 151 | static int |
| 152 | @@ -530,27 +547,109 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy) |
| 153 | } |
| 154 | } |
| 155 | |
| 156 | +static int |
| 157 | +mt7915_tm_sta_add(struct mt7915_phy *phy, u8 aid, |
| 158 | + struct mt76_testmode_sta_data *sd) |
| 159 | +{ |
| 160 | + struct mt76_testmode_data *td = &phy->mt76->test; |
| 161 | + struct mt76_testmode_sta *tm_sta; |
| 162 | + |
| 163 | + if (!aid) |
| 164 | + return 0; |
| 165 | + |
| 166 | + if (!td->tm_wcid[aid]) { |
| 167 | + struct ieee80211_vif *vif = phy->monitor_vif; |
| 168 | + struct ieee80211_sband_iftype_data *data; |
| 169 | + struct ieee80211_supported_band *sband; |
| 170 | + struct ieee80211_sta *sta; |
| 171 | + struct mt7915_sta *msta; |
| 172 | + int ret; |
| 173 | + |
| 174 | + sta = kzalloc(sizeof(*sta) + phy->mt76->hw->sta_data_size + |
| 175 | + sizeof(*tm_sta), GFP_KERNEL); |
| 176 | + if (!sta) |
| 177 | + return -ENOMEM; |
| 178 | + |
| 179 | + if (phy->mt76->chandef.chan->band == NL80211_BAND_5GHZ) { |
| 180 | + sband = &phy->mt76->sband_5g.sband; |
| 181 | + data = phy->iftype[NL80211_BAND_5GHZ]; |
| 182 | + } else { |
| 183 | + sband = &phy->mt76->sband_2g.sband; |
| 184 | + data = phy->iftype[NL80211_BAND_2GHZ]; |
| 185 | + } |
| 186 | + |
| 187 | + ether_addr_copy(sta->addr, phy->mt76->macaddr); |
| 188 | + sta->addr[0] += aid * 4; |
| 189 | + memcpy(&sta->ht_cap, &sband->ht_cap, sizeof(sta->ht_cap)); |
| 190 | + memcpy(&sta->vht_cap, &sband->vht_cap, sizeof(sta->vht_cap)); |
| 191 | + memcpy(&sta->he_cap, &data[NL80211_IFTYPE_STATION].he_cap, |
| 192 | + sizeof(sta->he_cap)); |
| 193 | + sta->aid = aid; |
| 194 | + sta->wme = 1; |
| 195 | + |
| 196 | + ret = mt7915_mac_sta_add(&phy->dev->mt76, vif, sta); |
| 197 | + if (ret) { |
| 198 | + kfree(sta); |
| 199 | + return ret; |
| 200 | + } |
| 201 | + |
| 202 | + msta = (struct mt7915_sta *)sta->drv_priv; |
| 203 | + td->tm_wcid[aid] = &msta->wcid; |
| 204 | + td->tm_sta_mask |= BIT(aid - 1); |
| 205 | + } |
| 206 | + |
| 207 | + tm_sta = mt76_testmode_aid_get_sta(phy->mt76, aid); |
| 208 | + memcpy(&tm_sta->sd, sd, sizeof(tm_sta->sd)); |
| 209 | + |
| 210 | + return 0; |
| 211 | +} |
| 212 | + |
| 213 | static void |
| 214 | -mt7915_tm_init(struct mt7915_phy *phy, bool en) |
| 215 | +mt7915_tm_sta_remove(struct mt7915_phy *phy, u8 aid) |
| 216 | { |
| 217 | + struct mt76_testmode_data *td = &phy->mt76->test; |
| 218 | + struct mt76_wcid *wcid = td->tm_wcid[aid]; |
| 219 | struct mt7915_dev *dev = phy->dev; |
| 220 | + struct ieee80211_sta *sta = wcid_to_sta(wcid); |
| 221 | |
| 222 | - if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state)) |
| 223 | + mt7915_mac_sta_remove(&dev->mt76, phy->monitor_vif, sta); |
| 224 | + mt76_wcid_mask_clear(dev->mt76.wcid_mask, wcid->idx); |
| 225 | + |
| 226 | + kfree(sta); |
| 227 | + td->tm_wcid[aid] = NULL; |
| 228 | + td->tm_sta_mask &= ~BIT(aid - 1); |
| 229 | +} |
| 230 | + |
| 231 | +static void |
| 232 | +mt7915_tm_sta_remove_all(struct mt7915_phy *phy) |
| 233 | +{ |
| 234 | + int i; |
| 235 | + |
| 236 | + if (!mt76_testmode_has_sta(phy->mt76)) |
| 237 | return; |
| 238 | |
| 239 | - mt7915_mcu_set_sku_en(phy, !en); |
| 240 | + for (i = 1; i < ARRAY_SIZE(phy->mt76->test.tm_wcid); i++) { |
| 241 | + if (phy->mt76->test.tm_wcid[i]) |
| 242 | + mt7915_tm_sta_remove(phy, i); |
| 243 | + } |
| 244 | +} |
| 245 | |
| 246 | - mt7915_tm_mode_ctrl(dev, en); |
| 247 | - mt7915_tm_reg_backup_restore(phy); |
| 248 | - mt7915_tm_set_trx(phy, TM_MAC_TXRX, !en); |
| 249 | +static int |
| 250 | +mt7915_tm_set_sta(struct mt7915_phy *phy) |
| 251 | +{ |
| 252 | + struct mt76_testmode_data *td = &phy->mt76->test; |
| 253 | |
| 254 | - mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en); |
| 255 | - mt7915_mcu_add_sta(dev, phy->monitor_vif, NULL, en); |
| 256 | + if (!td->aid) { |
| 257 | + mt7915_tm_sta_remove_all(phy); |
| 258 | + return 0; |
| 259 | + } |
| 260 | |
| 261 | - phy->mt76->test.flag |= MT_TM_FW_RX_COUNT; |
| 262 | + if (td->tx_count == 0) { |
| 263 | + mt7915_tm_sta_remove(phy, td->aid); |
| 264 | + return 0; |
| 265 | + } |
| 266 | |
| 267 | - if (!en) |
| 268 | - mt7915_tm_set_tam_arb(phy, en, 0); |
| 269 | + return mt7915_tm_sta_add(phy, td->aid, &td->sd); |
| 270 | } |
| 271 | |
| 272 | static void |
| 273 | @@ -563,22 +662,48 @@ mt7915_tm_update_channel(struct mt7915_phy *phy) |
| 274 | mt7915_mcu_set_chan_info(phy, MCU_EXT_CMD(SET_RX_PATH)); |
| 275 | } |
| 276 | |
| 277 | +static bool |
| 278 | +mt7915_tm_check_skb(struct mt7915_phy *phy) |
| 279 | +{ |
| 280 | + struct mt76_testmode_data *td = &phy->mt76->test; |
| 281 | + struct ieee80211_tx_info *info; |
| 282 | + |
| 283 | + if (!mt76_testmode_has_sta(phy->mt76)) { |
| 284 | + if (!td->tx_skb) |
| 285 | + return false; |
| 286 | + |
| 287 | + info = IEEE80211_SKB_CB(td->tx_skb); |
| 288 | + info->control.vif = phy->monitor_vif; |
| 289 | + } else { |
| 290 | + struct mt76_testmode_sta *tm_sta; |
| 291 | + int i; |
| 292 | + |
| 293 | + mt76_testmode_for_each_sta(phy->mt76, i, tm_sta) { |
| 294 | + if (!tm_sta->tx_skb) |
| 295 | + return false; |
| 296 | + |
| 297 | + info = IEEE80211_SKB_CB(tm_sta->tx_skb); |
| 298 | + info->control.vif = phy->monitor_vif; |
| 299 | + } |
| 300 | + } |
| 301 | + |
| 302 | + return true; |
| 303 | +} |
| 304 | + |
| 305 | static void |
| 306 | mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en) |
| 307 | { |
| 308 | static const u8 spe_idx_map[] = {0, 0, 1, 0, 3, 2, 4, 0, |
| 309 | 9, 8, 6, 10, 16, 12, 18, 0}; |
| 310 | struct mt76_testmode_data *td = &phy->mt76->test; |
| 311 | - struct mt7915_dev *dev = phy->dev; |
| 312 | - struct ieee80211_tx_info *info; |
| 313 | - u8 duty_cycle = td->tx_duty_cycle; |
| 314 | - u32 tx_time = td->tx_time; |
| 315 | - u32 ipg = td->tx_ipg; |
| 316 | |
| 317 | mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false); |
| 318 | - mt7915_tm_clean_hwq(phy, dev->mt76.global_wcid.idx); |
| 319 | + mt7915_tm_set_trx(phy, TM_MAC_TX, false); |
| 320 | |
| 321 | if (en) { |
| 322 | + u32 tx_time = td->tx_time, ipg = td->tx_ipg; |
| 323 | + u8 duty_cycle = td->tx_duty_cycle; |
| 324 | + |
| 325 | mt7915_tm_update_channel(phy); |
| 326 | |
| 327 | if (td->tx_spe_idx) { |
| 328 | @@ -586,30 +711,29 @@ mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en) |
| 329 | } else { |
| 330 | phy->test.spe_idx = spe_idx_map[td->tx_antenna_mask]; |
| 331 | } |
| 332 | - } |
| 333 | |
| 334 | - mt7915_tm_set_tam_arb(phy, en, |
| 335 | - td->tx_rate_mode == MT76_TM_TX_MODE_HE_MU); |
| 336 | - |
| 337 | - /* if all three params are set, duty_cycle will be ignored */ |
| 338 | - if (duty_cycle && tx_time && !ipg) { |
| 339 | - ipg = tx_time * 100 / duty_cycle - tx_time; |
| 340 | - } else if (duty_cycle && !tx_time && ipg) { |
| 341 | - if (duty_cycle < 100) |
| 342 | - tx_time = duty_cycle * ipg / (100 - duty_cycle); |
| 343 | - } |
| 344 | + /* if all three params are set, duty_cycle will be ignored */ |
| 345 | + if (duty_cycle && tx_time && !ipg) { |
| 346 | + ipg = tx_time * 100 / duty_cycle - tx_time; |
| 347 | + } else if (duty_cycle && !tx_time && ipg) { |
| 348 | + if (duty_cycle < 100) |
| 349 | + tx_time = duty_cycle * ipg / (100 - duty_cycle); |
| 350 | + } |
| 351 | |
| 352 | - mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode); |
| 353 | - mt7915_tm_set_tx_len(phy, tx_time); |
| 354 | + mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode); |
| 355 | + mt7915_tm_set_tx_len(phy, tx_time); |
| 356 | |
| 357 | - if (ipg) |
| 358 | - td->tx_queued_limit = MT76_TM_TIMEOUT * 1000000 / ipg / 2; |
| 359 | + if (ipg) |
| 360 | + td->tx_queued_limit = MT76_TM_TIMEOUT * 1000000 / ipg / 2; |
| 361 | |
| 362 | - if (!en || !td->tx_skb) |
| 363 | - return; |
| 364 | + if (!mt7915_tm_check_skb(phy)) |
| 365 | + return; |
| 366 | + } else { |
| 367 | + mt7915_tm_clean_hwq(phy); |
| 368 | + } |
| 369 | |
| 370 | - info = IEEE80211_SKB_CB(td->tx_skb); |
| 371 | - info->control.vif = phy->monitor_vif; |
| 372 | + mt7915_tm_set_tam_arb(phy, en, |
| 373 | + td->tx_rate_mode == MT76_TM_TX_MODE_HE_MU); |
| 374 | |
| 375 | mt7915_tm_set_trx(phy, TM_MAC_TX, en); |
| 376 | } |
| 377 | @@ -811,6 +935,31 @@ out: |
| 378 | sizeof(req), true); |
| 379 | } |
| 380 | |
| 381 | +static void |
| 382 | +mt7915_tm_init(struct mt7915_phy *phy, bool en) |
| 383 | +{ |
| 384 | + struct mt7915_dev *dev = phy->dev; |
| 385 | + |
| 386 | + if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state)) |
| 387 | + return; |
| 388 | + |
| 389 | + mt7915_mcu_set_sku_en(phy, !en); |
| 390 | + |
| 391 | + mt7915_tm_mode_ctrl(dev, en); |
| 392 | + mt7915_tm_reg_backup_restore(phy); |
| 393 | + mt7915_tm_set_trx(phy, TM_MAC_TXRX, !en); |
| 394 | + |
| 395 | + mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en); |
| 396 | + mt7915_mcu_add_sta(dev, phy->monitor_vif, NULL, en); |
| 397 | + |
| 398 | + phy->mt76->test.flag |= MT_TM_FW_RX_COUNT; |
| 399 | + |
| 400 | + if (!en) { |
| 401 | + mt7915_tm_set_tam_arb(phy, en, 0); |
| 402 | + mt7915_tm_sta_remove_all(phy); |
| 403 | + } |
| 404 | +} |
| 405 | + |
| 406 | static void |
| 407 | mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed) |
| 408 | { |
| 409 | @@ -825,6 +974,8 @@ mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed) |
| 410 | mt7915_tm_set_cfg(phy); |
| 411 | if (changed & BIT(TM_CHANGED_OFF_CH_SCAN_CH)) |
| 412 | mt7915_tm_set_off_channel_scan(phy); |
| 413 | + if (changed & BIT(TM_CHANGED_AID)) |
| 414 | + mt7915_tm_set_sta(phy); |
| 415 | } |
| 416 | |
| 417 | static int |
| 418 | -- |
| 419 | 2.25.1 |
| 420 | |