1 // SPDX-License-Identifier: ISC 2 /* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */ 3 #include "mt76.h" 4 5 static const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS] = { 6 [MT76_TM_ATTR_RESET] = { .type = NLA_FLAG }, 7 [MT76_TM_ATTR_STATE] = { .type = NLA_U8 }, 8 [MT76_TM_ATTR_TX_COUNT] = { .type = NLA_U32 }, 9 [MT76_TM_ATTR_TX_RATE_MODE] = { .type = NLA_U8 }, 10 [MT76_TM_ATTR_TX_RATE_NSS] = { .type = NLA_U8 }, 11 [MT76_TM_ATTR_TX_RATE_IDX] = { .type = NLA_U8 }, 12 [MT76_TM_ATTR_TX_RATE_SGI] = { .type = NLA_U8 }, 13 [MT76_TM_ATTR_TX_RATE_LDPC] = { .type = NLA_U8 }, 14 [MT76_TM_ATTR_TX_ANTENNA] = { .type = NLA_U8 }, 15 [MT76_TM_ATTR_TX_POWER_CONTROL] = { .type = NLA_U8 }, 16 [MT76_TM_ATTR_TX_POWER] = { .type = NLA_NESTED }, 17 [MT76_TM_ATTR_FREQ_OFFSET] = { .type = NLA_U32 }, 18 }; 19 20 void mt76_testmode_tx_pending(struct mt76_dev *dev) 21 { 22 struct mt76_testmode_data *td = &dev->test; 23 struct mt76_wcid *wcid = &dev->global_wcid; 24 struct sk_buff *skb = td->tx_skb; 25 struct mt76_queue *q; 26 int qid; 27 28 if (!skb || !td->tx_pending) 29 return; 30 31 qid = skb_get_queue_mapping(skb); 32 q = dev->q_tx[qid].q; 33 34 spin_lock_bh(&q->lock); 35 36 while (td->tx_pending > 0 && q->queued < q->ndesc / 2) { 37 int ret; 38 39 ret = dev->queue_ops->tx_queue_skb(dev, qid, skb_get(skb), wcid, NULL); 40 if (ret < 0) 41 break; 42 43 td->tx_pending--; 44 td->tx_queued++; 45 } 46 47 dev->queue_ops->kick(dev, q); 48 49 spin_unlock_bh(&q->lock); 50 } 51 52 53 static int 54 mt76_testmode_tx_init(struct mt76_dev *dev) 55 { 56 struct mt76_testmode_data *td = &dev->test; 57 struct ieee80211_tx_info *info; 58 struct ieee80211_hdr *hdr; 59 struct sk_buff *skb; 60 u16 fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA | 61 IEEE80211_FCTL_FROMDS; 62 struct ieee80211_tx_rate *rate; 63 u8 max_nss = hweight8(dev->phy.antenna_mask); 64 65 if (td->tx_antenna_mask) 66 max_nss = min_t(u8, max_nss, hweight8(td->tx_antenna_mask)); 67 68 skb = alloc_skb(td->tx_msdu_len, GFP_KERNEL); 69 if (!skb) 70 return -ENOMEM; 71 72 dev_kfree_skb(td->tx_skb); 73 td->tx_skb = skb; 74 hdr = __skb_put_zero(skb, td->tx_msdu_len); 75 hdr->frame_control = cpu_to_le16(fc); 76 memcpy(hdr->addr1, dev->macaddr, sizeof(dev->macaddr)); 77 memcpy(hdr->addr2, dev->macaddr, sizeof(dev->macaddr)); 78 memcpy(hdr->addr3, dev->macaddr, sizeof(dev->macaddr)); 79 80 info = IEEE80211_SKB_CB(skb); 81 info->flags = IEEE80211_TX_CTL_INJECTED | 82 IEEE80211_TX_CTL_NO_ACK | 83 IEEE80211_TX_CTL_NO_PS_BUFFER; 84 rate = &info->control.rates[0]; 85 rate->count = 1; 86 rate->idx = td->tx_rate_idx; 87 88 switch (td->tx_rate_mode) { 89 case MT76_TM_TX_MODE_CCK: 90 if (dev->phy.chandef.chan->band != NL80211_BAND_2GHZ) 91 return -EINVAL; 92 93 if (rate->idx > 4) 94 return -EINVAL; 95 break; 96 case MT76_TM_TX_MODE_OFDM: 97 if (dev->phy.chandef.chan->band != NL80211_BAND_2GHZ) 98 break; 99 100 if (rate->idx > 8) 101 return -EINVAL; 102 103 rate->idx += 4; 104 break; 105 case MT76_TM_TX_MODE_HT: 106 if (rate->idx > 8 * max_nss && 107 !(rate->idx == 32 && 108 dev->phy.chandef.width >= NL80211_CHAN_WIDTH_40)) 109 return -EINVAL; 110 111 rate->flags |= IEEE80211_TX_RC_MCS; 112 break; 113 case MT76_TM_TX_MODE_VHT: 114 if (rate->idx > 9) 115 return -EINVAL; 116 117 if (td->tx_rate_nss > max_nss) 118 return -EINVAL; 119 120 ieee80211_rate_set_vht(rate, td->tx_rate_idx, td->tx_rate_nss); 121 rate->flags |= IEEE80211_TX_RC_VHT_MCS; 122 break; 123 default: 124 break; 125 } 126 127 if (td->tx_rate_sgi) 128 rate->flags |= IEEE80211_TX_RC_SHORT_GI; 129 130 if (td->tx_rate_ldpc) 131 info->flags |= IEEE80211_TX_CTL_LDPC; 132 133 if (td->tx_rate_mode >= MT76_TM_TX_MODE_HT) { 134 switch (dev->phy.chandef.width) { 135 case NL80211_CHAN_WIDTH_40: 136 rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH; 137 break; 138 case NL80211_CHAN_WIDTH_80: 139 rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH; 140 break; 141 case NL80211_CHAN_WIDTH_80P80: 142 case NL80211_CHAN_WIDTH_160: 143 rate->flags |= IEEE80211_TX_RC_160_MHZ_WIDTH; 144 break; 145 default: 146 break; 147 } 148 } 149 150 skb_set_queue_mapping(skb, IEEE80211_AC_BE); 151 152 return 0; 153 } 154 155 static void 156 mt76_testmode_tx_start(struct mt76_dev *dev) 157 { 158 struct mt76_testmode_data *td = &dev->test; 159 160 td->tx_queued = 0; 161 td->tx_done = 0; 162 td->tx_pending = td->tx_count; 163 tasklet_schedule(&dev->tx_tasklet); 164 } 165 166 static void 167 mt76_testmode_tx_stop(struct mt76_dev *dev) 168 { 169 struct mt76_testmode_data *td = &dev->test; 170 171 tasklet_disable(&dev->tx_tasklet); 172 173 td->tx_pending = 0; 174 175 tasklet_enable(&dev->tx_tasklet); 176 177 wait_event_timeout(dev->tx_wait, td->tx_done == td->tx_queued, 10 * HZ); 178 179 dev_kfree_skb(td->tx_skb); 180 td->tx_skb = NULL; 181 } 182 183 static inline void 184 mt76_testmode_param_set(struct mt76_testmode_data *td, u16 idx) 185 { 186 td->param_set[idx / 32] |= BIT(idx % 32); 187 } 188 189 static inline bool 190 mt76_testmode_param_present(struct mt76_testmode_data *td, u16 idx) 191 { 192 return td->param_set[idx / 32] & BIT(idx % 32); 193 } 194 195 static void 196 mt76_testmode_init_defaults(struct mt76_dev *dev) 197 { 198 struct mt76_testmode_data *td = &dev->test; 199 200 if (td->tx_msdu_len > 0) 201 return; 202 203 td->tx_msdu_len = 1024; 204 td->tx_count = 1; 205 td->tx_rate_mode = MT76_TM_TX_MODE_OFDM; 206 td->tx_rate_nss = 1; 207 } 208 209 static int 210 __mt76_testmode_set_state(struct mt76_dev *dev, enum mt76_testmode_state state) 211 { 212 enum mt76_testmode_state prev_state = dev->test.state; 213 int err; 214 215 if (prev_state == MT76_TM_STATE_TX_FRAMES) 216 mt76_testmode_tx_stop(dev); 217 218 if (state == MT76_TM_STATE_TX_FRAMES) { 219 err = mt76_testmode_tx_init(dev); 220 if (err) 221 return err; 222 } 223 224 err = dev->test_ops->set_state(dev, state); 225 if (err) { 226 if (state == MT76_TM_STATE_TX_FRAMES) 227 mt76_testmode_tx_stop(dev); 228 229 return err; 230 } 231 232 if (state == MT76_TM_STATE_TX_FRAMES) 233 mt76_testmode_tx_start(dev); 234 else if (state == MT76_TM_STATE_RX_FRAMES) { 235 memset(&dev->test.rx_stats, 0, sizeof(dev->test.rx_stats)); 236 } 237 238 dev->test.state = state; 239 240 return 0; 241 } 242 243 int mt76_testmode_set_state(struct mt76_dev *dev, enum mt76_testmode_state state) 244 { 245 struct mt76_testmode_data *td = &dev->test; 246 struct ieee80211_hw *hw = dev->phy.hw; 247 248 if (state == td->state && state == MT76_TM_STATE_OFF) 249 return 0; 250 251 if (state > MT76_TM_STATE_OFF && 252 (!test_bit(MT76_STATE_RUNNING, &dev->phy.state) || 253 !(hw->conf.flags & IEEE80211_CONF_MONITOR))) 254 return -ENOTCONN; 255 256 if (state != MT76_TM_STATE_IDLE && 257 td->state != MT76_TM_STATE_IDLE) { 258 int ret; 259 260 ret = __mt76_testmode_set_state(dev, MT76_TM_STATE_IDLE); 261 if (ret) 262 return ret; 263 } 264 265 return __mt76_testmode_set_state(dev, state); 266 267 } 268 EXPORT_SYMBOL(mt76_testmode_set_state); 269 270 static int 271 mt76_tm_get_u8(struct nlattr *attr, u8 *dest, u8 min, u8 max) 272 { 273 u8 val; 274 275 if (!attr) 276 return 0; 277 278 val = nla_get_u8(attr); 279 if (val < min || val > max) 280 return -EINVAL; 281 282 *dest = val; 283 return 0; 284 } 285 286 int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif, 287 void *data, int len) 288 { 289 struct mt76_phy *phy = hw->priv; 290 struct mt76_dev *dev = phy->dev; 291 struct mt76_testmode_data *td = &dev->test; 292 struct nlattr *tb[NUM_MT76_TM_ATTRS]; 293 u32 state; 294 int err; 295 int i; 296 297 if (!dev->test_ops) 298 return -EOPNOTSUPP; 299 300 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len, 301 mt76_tm_policy, NULL); 302 if (err) 303 return err; 304 305 err = -EINVAL; 306 307 mutex_lock(&dev->mutex); 308 309 if (tb[MT76_TM_ATTR_RESET]) { 310 mt76_testmode_set_state(dev, MT76_TM_STATE_OFF); 311 memset(td, 0, sizeof(*td)); 312 } 313 314 mt76_testmode_init_defaults(dev); 315 316 if (tb[MT76_TM_ATTR_TX_COUNT]) 317 td->tx_count = nla_get_u32(tb[MT76_TM_ATTR_TX_COUNT]); 318 319 if (tb[MT76_TM_ATTR_TX_LENGTH]) { 320 u32 val = nla_get_u32(tb[MT76_TM_ATTR_TX_LENGTH]); 321 322 if (val > IEEE80211_MAX_FRAME_LEN || 323 val < sizeof(struct ieee80211_hdr)) 324 goto out; 325 326 td->tx_msdu_len = val; 327 } 328 329 if (tb[MT76_TM_ATTR_TX_RATE_IDX]) 330 td->tx_rate_idx = nla_get_u8(tb[MT76_TM_ATTR_TX_RATE_IDX]); 331 332 if (mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_MODE], &td->tx_rate_mode, 333 0, MT76_TM_TX_MODE_MAX) || 334 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_NSS], &td->tx_rate_nss, 335 1, hweight8(phy->antenna_mask)) || 336 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_SGI], &td->tx_rate_sgi, 0, 1) || 337 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_LDPC], &td->tx_rate_ldpc, 0, 1) || 338 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_ANTENNA], &td->tx_antenna_mask, 1, 339 phy->antenna_mask) || 340 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_POWER_CONTROL], 341 &td->tx_power_control, 0, 1)) 342 goto out; 343 344 if (tb[MT76_TM_ATTR_FREQ_OFFSET]) 345 td->freq_offset = nla_get_u32(tb[MT76_TM_ATTR_FREQ_OFFSET]); 346 347 if (tb[MT76_TM_ATTR_STATE]) { 348 state = nla_get_u32(tb[MT76_TM_ATTR_STATE]); 349 if (state > MT76_TM_STATE_MAX) 350 goto out; 351 } else { 352 state = td->state; 353 } 354 355 if (tb[MT76_TM_ATTR_TX_POWER]) { 356 struct nlattr *cur; 357 int idx = 0; 358 int rem; 359 360 nla_for_each_nested(cur, tb[MT76_TM_ATTR_TX_POWER], rem) { 361 if (nla_len(cur) != 1 || 362 idx >= ARRAY_SIZE(td->tx_power)) 363 goto out; 364 365 td->tx_power[idx++] = nla_get_u8(cur); 366 } 367 } 368 369 if (dev->test_ops->set_params) { 370 err = dev->test_ops->set_params(dev, tb, state); 371 if (err) 372 goto out; 373 } 374 375 for (i = MT76_TM_ATTR_STATE; i < ARRAY_SIZE(tb); i++) 376 if (tb[i]) 377 mt76_testmode_param_set(td, i); 378 379 err = 0; 380 if (tb[MT76_TM_ATTR_STATE]) 381 err = mt76_testmode_set_state(dev, state); 382 383 out: 384 mutex_unlock(&dev->mutex); 385 386 return err; 387 } 388 EXPORT_SYMBOL(mt76_testmode_cmd); 389 390 static int 391 mt76_testmode_dump_stats(struct mt76_dev *dev, struct sk_buff *msg) 392 { 393 struct mt76_testmode_data *td = &dev->test; 394 u64 rx_packets = 0; 395 u64 rx_fcs_error = 0; 396 int i; 397 398 for (i = 0; i < ARRAY_SIZE(td->rx_stats.packets); i++) { 399 rx_packets += td->rx_stats.packets[i]; 400 rx_fcs_error += td->rx_stats.fcs_error[i]; 401 } 402 403 if (nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_PENDING, td->tx_pending) || 404 nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_QUEUED, td->tx_queued) || 405 nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_DONE, td->tx_done) || 406 nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_PACKETS, rx_packets, 407 MT76_TM_STATS_ATTR_PAD) || 408 nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_FCS_ERROR, rx_fcs_error, 409 MT76_TM_STATS_ATTR_PAD)) 410 return -EMSGSIZE; 411 412 if (dev->test_ops->dump_stats) 413 return dev->test_ops->dump_stats(dev, msg); 414 415 return 0; 416 } 417 418 int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg, 419 struct netlink_callback *cb, void *data, int len) 420 { 421 struct mt76_phy *phy = hw->priv; 422 struct mt76_dev *dev = phy->dev; 423 struct mt76_testmode_data *td = &dev->test; 424 struct nlattr *tb[NUM_MT76_TM_ATTRS] = {}; 425 int err = 0; 426 void *a; 427 int i; 428 429 if (!dev->test_ops) 430 return -EOPNOTSUPP; 431 432 if (cb->args[2]++ > 0) 433 return -ENOENT; 434 435 if (data) { 436 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len, 437 mt76_tm_policy, NULL); 438 if (err) 439 return err; 440 } 441 442 mutex_lock(&dev->mutex); 443 444 if (tb[MT76_TM_ATTR_STATS]) { 445 a = nla_nest_start(msg, MT76_TM_ATTR_STATS); 446 err = mt76_testmode_dump_stats(dev, msg); 447 nla_nest_end(msg, a); 448 449 goto out; 450 } 451 452 mt76_testmode_init_defaults(dev); 453 454 err = -EMSGSIZE; 455 if (nla_put_u32(msg, MT76_TM_ATTR_STATE, td->state)) 456 goto out; 457 458 if (td->mtd_name && 459 (nla_put_string(msg, MT76_TM_ATTR_MTD_PART, td->mtd_name) || 460 nla_put_u32(msg, MT76_TM_ATTR_MTD_OFFSET, td->mtd_offset))) 461 goto out; 462 463 if (nla_put_u32(msg, MT76_TM_ATTR_TX_COUNT, td->tx_count) || 464 nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH, td->tx_msdu_len) || 465 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_MODE, td->tx_rate_mode) || 466 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_NSS, td->tx_rate_nss) || 467 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, td->tx_rate_idx) || 468 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_SGI, td->tx_rate_sgi) || 469 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_LDPC, td->tx_rate_ldpc) || 470 (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_ANTENNA) && 471 nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, td->tx_antenna_mask)) || 472 (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER_CONTROL) && 473 nla_put_u8(msg, MT76_TM_ATTR_TX_POWER_CONTROL, td->tx_power_control)) || 474 (mt76_testmode_param_present(td, MT76_TM_ATTR_FREQ_OFFSET) && 475 nla_put_u8(msg, MT76_TM_ATTR_FREQ_OFFSET, td->freq_offset))) 476 goto out; 477 478 if (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER)) { 479 a = nla_nest_start(msg, MT76_TM_ATTR_TX_POWER); 480 if (!a) 481 goto out; 482 483 for (i = 0; i < ARRAY_SIZE(td->tx_power); i++) 484 if (nla_put_u8(msg, i, td->tx_power[i])) 485 goto out; 486 487 nla_nest_end(msg, a); 488 } 489 490 err = 0; 491 492 out: 493 mutex_unlock(&dev->mutex); 494 495 return err; 496 } 497 EXPORT_SYMBOL(mt76_testmode_dump); 498