1f0efa862SFelix Fietkau // SPDX-License-Identifier: ISC 2f0efa862SFelix Fietkau /* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */ 3f0efa862SFelix Fietkau #include "mt76.h" 4f0efa862SFelix Fietkau 5f0efa862SFelix Fietkau static const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS] = { 6f0efa862SFelix Fietkau [MT76_TM_ATTR_RESET] = { .type = NLA_FLAG }, 7f0efa862SFelix Fietkau [MT76_TM_ATTR_STATE] = { .type = NLA_U8 }, 8f0efa862SFelix Fietkau [MT76_TM_ATTR_TX_COUNT] = { .type = NLA_U32 }, 9f0efa862SFelix Fietkau [MT76_TM_ATTR_TX_RATE_MODE] = { .type = NLA_U8 }, 10f0efa862SFelix Fietkau [MT76_TM_ATTR_TX_RATE_NSS] = { .type = NLA_U8 }, 11f0efa862SFelix Fietkau [MT76_TM_ATTR_TX_RATE_IDX] = { .type = NLA_U8 }, 12f0efa862SFelix Fietkau [MT76_TM_ATTR_TX_RATE_SGI] = { .type = NLA_U8 }, 13f0efa862SFelix Fietkau [MT76_TM_ATTR_TX_RATE_LDPC] = { .type = NLA_U8 }, 147f54c742SShayne Chen [MT76_TM_ATTR_TX_RATE_STBC] = { .type = NLA_U8 }, 151a38c2f5SShayne Chen [MT76_TM_ATTR_TX_LTF] = { .type = NLA_U8 }, 16f0efa862SFelix Fietkau [MT76_TM_ATTR_TX_ANTENNA] = { .type = NLA_U8 }, 17fdc9c18eSShayne Chen [MT76_TM_ATTR_TX_SPE_IDX] = { .type = NLA_U8 }, 18f0efa862SFelix Fietkau [MT76_TM_ATTR_TX_POWER_CONTROL] = { .type = NLA_U8 }, 19f0efa862SFelix Fietkau [MT76_TM_ATTR_TX_POWER] = { .type = NLA_NESTED }, 20b8cbdb97SShayne Chen [MT76_TM_ATTR_TX_DUTY_CYCLE] = { .type = NLA_U8 }, 21b8cbdb97SShayne Chen [MT76_TM_ATTR_TX_IPG] = { .type = NLA_U32 }, 22b8cbdb97SShayne Chen [MT76_TM_ATTR_TX_TIME] = { .type = NLA_U32 }, 23f0efa862SFelix Fietkau [MT76_TM_ATTR_FREQ_OFFSET] = { .type = NLA_U32 }, 24f0efa862SFelix Fietkau }; 25f0efa862SFelix Fietkau 26c918c74dSShayne Chen void mt76_testmode_tx_pending(struct mt76_phy *phy) 27f0efa862SFelix Fietkau { 28c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 29c918c74dSShayne Chen struct mt76_dev *dev = phy->dev; 30f0efa862SFelix Fietkau struct mt76_wcid *wcid = &dev->global_wcid; 31f0efa862SFelix Fietkau struct sk_buff *skb = td->tx_skb; 32f0efa862SFelix Fietkau struct mt76_queue *q; 33ba459094SShayne Chen u16 tx_queued_limit; 34f0efa862SFelix Fietkau int qid; 35f0efa862SFelix Fietkau 36f0efa862SFelix Fietkau if (!skb || !td->tx_pending) 37f0efa862SFelix Fietkau return; 38f0efa862SFelix Fietkau 39f0efa862SFelix Fietkau qid = skb_get_queue_mapping(skb); 4091990519SLorenzo Bianconi q = phy->q_tx[qid]; 41f0efa862SFelix Fietkau 42ba459094SShayne Chen tx_queued_limit = td->tx_queued_limit ? td->tx_queued_limit : 1000; 43ba459094SShayne Chen 44f0efa862SFelix Fietkau spin_lock_bh(&q->lock); 45f0efa862SFelix Fietkau 46ba459094SShayne Chen while (td->tx_pending > 0 && 47ba459094SShayne Chen td->tx_queued - td->tx_done < tx_queued_limit && 489729ff4cSFelix Fietkau q->queued < q->ndesc / 2) { 49f0efa862SFelix Fietkau int ret; 50f0efa862SFelix Fietkau 5189870594SLorenzo Bianconi ret = dev->queue_ops->tx_queue_skb(dev, q, skb_get(skb), wcid, 5289870594SLorenzo Bianconi NULL); 53f0efa862SFelix Fietkau if (ret < 0) 54f0efa862SFelix Fietkau break; 55f0efa862SFelix Fietkau 56f0efa862SFelix Fietkau td->tx_pending--; 57f0efa862SFelix Fietkau td->tx_queued++; 58f0efa862SFelix Fietkau } 59f0efa862SFelix Fietkau 60f0efa862SFelix Fietkau dev->queue_ops->kick(dev, q); 61f0efa862SFelix Fietkau 62f0efa862SFelix Fietkau spin_unlock_bh(&q->lock); 63f0efa862SFelix Fietkau } 64f0efa862SFelix Fietkau 652601dda8SShayne Chen static u32 662601dda8SShayne Chen mt76_testmode_max_mpdu_len(struct mt76_phy *phy, u8 tx_rate_mode) 672601dda8SShayne Chen { 682601dda8SShayne Chen switch (tx_rate_mode) { 692601dda8SShayne Chen case MT76_TM_TX_MODE_HT: 702601dda8SShayne Chen return IEEE80211_MAX_MPDU_LEN_HT_7935; 712601dda8SShayne Chen case MT76_TM_TX_MODE_VHT: 722601dda8SShayne Chen case MT76_TM_TX_MODE_HE_SU: 732601dda8SShayne Chen case MT76_TM_TX_MODE_HE_EXT_SU: 742601dda8SShayne Chen case MT76_TM_TX_MODE_HE_TB: 752601dda8SShayne Chen case MT76_TM_TX_MODE_HE_MU: 762601dda8SShayne Chen if (phy->sband_5g.sband.vht_cap.cap & 772601dda8SShayne Chen IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991) 782601dda8SShayne Chen return IEEE80211_MAX_MPDU_LEN_VHT_7991; 792601dda8SShayne Chen return IEEE80211_MAX_MPDU_LEN_VHT_11454; 802601dda8SShayne Chen case MT76_TM_TX_MODE_CCK: 812601dda8SShayne Chen case MT76_TM_TX_MODE_OFDM: 822601dda8SShayne Chen default: 832601dda8SShayne Chen return IEEE80211_MAX_FRAME_LEN; 842601dda8SShayne Chen } 852601dda8SShayne Chen } 86f0efa862SFelix Fietkau 872601dda8SShayne Chen static void 882601dda8SShayne Chen mt76_testmode_free_skb(struct mt76_phy *phy) 89f0efa862SFelix Fietkau { 90c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 912601dda8SShayne Chen struct sk_buff *skb = td->tx_skb; 922601dda8SShayne Chen 932601dda8SShayne Chen if (!skb) 942601dda8SShayne Chen return; 952601dda8SShayne Chen 962601dda8SShayne Chen if (skb_has_frag_list(skb)) { 972601dda8SShayne Chen kfree_skb_list(skb_shinfo(skb)->frag_list); 982601dda8SShayne Chen skb_shinfo(skb)->frag_list = NULL; 992601dda8SShayne Chen } 1002601dda8SShayne Chen 1012601dda8SShayne Chen dev_kfree_skb(skb); 1022601dda8SShayne Chen td->tx_skb = NULL; 1032601dda8SShayne Chen } 1042601dda8SShayne Chen 1052601dda8SShayne Chen int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len) 1062601dda8SShayne Chen { 1072601dda8SShayne Chen #define MT_TXP_MAX_LEN 4095 108f0efa862SFelix Fietkau u16 fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA | 109f0efa862SFelix Fietkau IEEE80211_FCTL_FROMDS; 1102601dda8SShayne Chen struct mt76_testmode_data *td = &phy->test; 111c918c74dSShayne Chen bool ext_phy = phy != &phy->dev->phy; 1122601dda8SShayne Chen struct sk_buff **frag_tail, *head; 1132601dda8SShayne Chen struct ieee80211_tx_info *info; 1142601dda8SShayne Chen struct ieee80211_hdr *hdr; 1152601dda8SShayne Chen u32 max_len, head_len; 1162601dda8SShayne Chen int nfrags, i; 117f0efa862SFelix Fietkau 1182601dda8SShayne Chen max_len = mt76_testmode_max_mpdu_len(phy, td->tx_rate_mode); 1192601dda8SShayne Chen if (len > max_len) 1202601dda8SShayne Chen len = max_len; 1212601dda8SShayne Chen else if (len < sizeof(struct ieee80211_hdr)) 1222601dda8SShayne Chen len = sizeof(struct ieee80211_hdr); 123f0efa862SFelix Fietkau 1242601dda8SShayne Chen nfrags = len / MT_TXP_MAX_LEN; 1252601dda8SShayne Chen head_len = nfrags ? MT_TXP_MAX_LEN : len; 1262601dda8SShayne Chen 1272601dda8SShayne Chen if (len > IEEE80211_MAX_FRAME_LEN) 1282601dda8SShayne Chen fc |= IEEE80211_STYPE_QOS_DATA; 1292601dda8SShayne Chen 1302601dda8SShayne Chen head = alloc_skb(head_len, GFP_KERNEL); 1312601dda8SShayne Chen if (!head) 132f0efa862SFelix Fietkau return -ENOMEM; 133f0efa862SFelix Fietkau 1342601dda8SShayne Chen hdr = __skb_put_zero(head, head_len); 135f0efa862SFelix Fietkau hdr->frame_control = cpu_to_le16(fc); 13698df2baeSLorenzo Bianconi memcpy(hdr->addr1, phy->macaddr, sizeof(phy->macaddr)); 13798df2baeSLorenzo Bianconi memcpy(hdr->addr2, phy->macaddr, sizeof(phy->macaddr)); 13898df2baeSLorenzo Bianconi memcpy(hdr->addr3, phy->macaddr, sizeof(phy->macaddr)); 1392601dda8SShayne Chen skb_set_queue_mapping(head, IEEE80211_AC_BE); 140f0efa862SFelix Fietkau 1412601dda8SShayne Chen info = IEEE80211_SKB_CB(head); 142f0efa862SFelix Fietkau info->flags = IEEE80211_TX_CTL_INJECTED | 143f0efa862SFelix Fietkau IEEE80211_TX_CTL_NO_ACK | 144f0efa862SFelix Fietkau IEEE80211_TX_CTL_NO_PS_BUFFER; 14561fe7357SShayne Chen 146c918c74dSShayne Chen if (ext_phy) 147c918c74dSShayne Chen info->hw_queue |= MT_TX_HW_QUEUE_EXT_PHY; 148c918c74dSShayne Chen 1492601dda8SShayne Chen frag_tail = &skb_shinfo(head)->frag_list; 1502601dda8SShayne Chen 1512601dda8SShayne Chen for (i = 0; i < nfrags; i++) { 1522601dda8SShayne Chen struct sk_buff *frag; 1532601dda8SShayne Chen u16 frag_len; 1542601dda8SShayne Chen 1552601dda8SShayne Chen if (i == nfrags - 1) 1562601dda8SShayne Chen frag_len = len % MT_TXP_MAX_LEN; 1572601dda8SShayne Chen else 1582601dda8SShayne Chen frag_len = MT_TXP_MAX_LEN; 1592601dda8SShayne Chen 1602601dda8SShayne Chen frag = alloc_skb(frag_len, GFP_KERNEL); 161*fe2c3b1fSLorenzo Bianconi if (!frag) { 162*fe2c3b1fSLorenzo Bianconi mt76_testmode_free_skb(phy); 163*fe2c3b1fSLorenzo Bianconi dev_kfree_skb(head); 1642601dda8SShayne Chen return -ENOMEM; 165*fe2c3b1fSLorenzo Bianconi } 1662601dda8SShayne Chen 1672601dda8SShayne Chen __skb_put_zero(frag, frag_len); 1682601dda8SShayne Chen head->len += frag->len; 1692601dda8SShayne Chen head->data_len += frag->len; 1702601dda8SShayne Chen 1712601dda8SShayne Chen if (*frag_tail) { 1722601dda8SShayne Chen (*frag_tail)->next = frag; 1732601dda8SShayne Chen frag_tail = &frag; 1742601dda8SShayne Chen } else { 1752601dda8SShayne Chen *frag_tail = frag; 1762601dda8SShayne Chen } 1772601dda8SShayne Chen } 1782601dda8SShayne Chen 1792601dda8SShayne Chen mt76_testmode_free_skb(phy); 1802601dda8SShayne Chen td->tx_skb = head; 1812601dda8SShayne Chen 1822601dda8SShayne Chen return 0; 1832601dda8SShayne Chen } 1842601dda8SShayne Chen EXPORT_SYMBOL(mt76_testmode_alloc_skb); 1852601dda8SShayne Chen 1862601dda8SShayne Chen static int 1872601dda8SShayne Chen mt76_testmode_tx_init(struct mt76_phy *phy) 1882601dda8SShayne Chen { 1892601dda8SShayne Chen struct mt76_testmode_data *td = &phy->test; 1902601dda8SShayne Chen struct ieee80211_tx_info *info; 1912601dda8SShayne Chen struct ieee80211_tx_rate *rate; 1922601dda8SShayne Chen u8 max_nss = hweight8(phy->antenna_mask); 1932601dda8SShayne Chen int ret; 1942601dda8SShayne Chen 1952601dda8SShayne Chen ret = mt76_testmode_alloc_skb(phy, td->tx_mpdu_len); 1962601dda8SShayne Chen if (ret) 1972601dda8SShayne Chen return ret; 1982601dda8SShayne Chen 19961fe7357SShayne Chen if (td->tx_rate_mode > MT76_TM_TX_MODE_VHT) 20061fe7357SShayne Chen goto out; 20161fe7357SShayne Chen 2022601dda8SShayne Chen if (td->tx_antenna_mask) 2032601dda8SShayne Chen max_nss = min_t(u8, max_nss, hweight8(td->tx_antenna_mask)); 2042601dda8SShayne Chen 2052601dda8SShayne Chen info = IEEE80211_SKB_CB(td->tx_skb); 206f0efa862SFelix Fietkau rate = &info->control.rates[0]; 207f0efa862SFelix Fietkau rate->count = 1; 208f0efa862SFelix Fietkau rate->idx = td->tx_rate_idx; 209f0efa862SFelix Fietkau 210f0efa862SFelix Fietkau switch (td->tx_rate_mode) { 211f0efa862SFelix Fietkau case MT76_TM_TX_MODE_CCK: 21298df2baeSLorenzo Bianconi if (phy->chandef.chan->band != NL80211_BAND_2GHZ) 213f0efa862SFelix Fietkau return -EINVAL; 214f0efa862SFelix Fietkau 215f0efa862SFelix Fietkau if (rate->idx > 4) 216f0efa862SFelix Fietkau return -EINVAL; 217f0efa862SFelix Fietkau break; 218f0efa862SFelix Fietkau case MT76_TM_TX_MODE_OFDM: 21998df2baeSLorenzo Bianconi if (phy->chandef.chan->band != NL80211_BAND_2GHZ) 220f0efa862SFelix Fietkau break; 221f0efa862SFelix Fietkau 222f0efa862SFelix Fietkau if (rate->idx > 8) 223f0efa862SFelix Fietkau return -EINVAL; 224f0efa862SFelix Fietkau 225f0efa862SFelix Fietkau rate->idx += 4; 226f0efa862SFelix Fietkau break; 227f0efa862SFelix Fietkau case MT76_TM_TX_MODE_HT: 228f0efa862SFelix Fietkau if (rate->idx > 8 * max_nss && 229f0efa862SFelix Fietkau !(rate->idx == 32 && 23098df2baeSLorenzo Bianconi phy->chandef.width >= NL80211_CHAN_WIDTH_40)) 231f0efa862SFelix Fietkau return -EINVAL; 232f0efa862SFelix Fietkau 233f0efa862SFelix Fietkau rate->flags |= IEEE80211_TX_RC_MCS; 234f0efa862SFelix Fietkau break; 235f0efa862SFelix Fietkau case MT76_TM_TX_MODE_VHT: 236f0efa862SFelix Fietkau if (rate->idx > 9) 237f0efa862SFelix Fietkau return -EINVAL; 238f0efa862SFelix Fietkau 239f0efa862SFelix Fietkau if (td->tx_rate_nss > max_nss) 240f0efa862SFelix Fietkau return -EINVAL; 241f0efa862SFelix Fietkau 242f0efa862SFelix Fietkau ieee80211_rate_set_vht(rate, td->tx_rate_idx, td->tx_rate_nss); 243f0efa862SFelix Fietkau rate->flags |= IEEE80211_TX_RC_VHT_MCS; 244f0efa862SFelix Fietkau break; 245f0efa862SFelix Fietkau default: 246f0efa862SFelix Fietkau break; 247f0efa862SFelix Fietkau } 248f0efa862SFelix Fietkau 249f0efa862SFelix Fietkau if (td->tx_rate_sgi) 250f0efa862SFelix Fietkau rate->flags |= IEEE80211_TX_RC_SHORT_GI; 251f0efa862SFelix Fietkau 252f0efa862SFelix Fietkau if (td->tx_rate_ldpc) 253f0efa862SFelix Fietkau info->flags |= IEEE80211_TX_CTL_LDPC; 254f0efa862SFelix Fietkau 2557f54c742SShayne Chen if (td->tx_rate_stbc) 2567f54c742SShayne Chen info->flags |= IEEE80211_TX_CTL_STBC; 2577f54c742SShayne Chen 258f0efa862SFelix Fietkau if (td->tx_rate_mode >= MT76_TM_TX_MODE_HT) { 25998df2baeSLorenzo Bianconi switch (phy->chandef.width) { 260f0efa862SFelix Fietkau case NL80211_CHAN_WIDTH_40: 261f0efa862SFelix Fietkau rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH; 262f0efa862SFelix Fietkau break; 263f0efa862SFelix Fietkau case NL80211_CHAN_WIDTH_80: 264f0efa862SFelix Fietkau rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH; 265f0efa862SFelix Fietkau break; 266f0efa862SFelix Fietkau case NL80211_CHAN_WIDTH_80P80: 267f0efa862SFelix Fietkau case NL80211_CHAN_WIDTH_160: 268f0efa862SFelix Fietkau rate->flags |= IEEE80211_TX_RC_160_MHZ_WIDTH; 269f0efa862SFelix Fietkau break; 270f0efa862SFelix Fietkau default: 271f0efa862SFelix Fietkau break; 272f0efa862SFelix Fietkau } 273f0efa862SFelix Fietkau } 27461fe7357SShayne Chen out: 275f0efa862SFelix Fietkau return 0; 276f0efa862SFelix Fietkau } 277f0efa862SFelix Fietkau 278f0efa862SFelix Fietkau static void 279c918c74dSShayne Chen mt76_testmode_tx_start(struct mt76_phy *phy) 280f0efa862SFelix Fietkau { 281c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 282c918c74dSShayne Chen struct mt76_dev *dev = phy->dev; 283f0efa862SFelix Fietkau 284f0efa862SFelix Fietkau td->tx_queued = 0; 285f0efa862SFelix Fietkau td->tx_done = 0; 286f0efa862SFelix Fietkau td->tx_pending = td->tx_count; 287781eef5bSFelix Fietkau mt76_worker_schedule(&dev->tx_worker); 288f0efa862SFelix Fietkau } 289f0efa862SFelix Fietkau 290f0efa862SFelix Fietkau static void 291c918c74dSShayne Chen mt76_testmode_tx_stop(struct mt76_phy *phy) 292f0efa862SFelix Fietkau { 293c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 294c918c74dSShayne Chen struct mt76_dev *dev = phy->dev; 295f0efa862SFelix Fietkau 296781eef5bSFelix Fietkau mt76_worker_disable(&dev->tx_worker); 297f0efa862SFelix Fietkau 298f0efa862SFelix Fietkau td->tx_pending = 0; 299f0efa862SFelix Fietkau 300781eef5bSFelix Fietkau mt76_worker_enable(&dev->tx_worker); 301f0efa862SFelix Fietkau 302ba459094SShayne Chen wait_event_timeout(dev->tx_wait, td->tx_done == td->tx_queued, 303ba459094SShayne Chen MT76_TM_TIMEOUT * HZ); 304f0efa862SFelix Fietkau 3052601dda8SShayne Chen mt76_testmode_free_skb(phy); 306f0efa862SFelix Fietkau } 307f0efa862SFelix Fietkau 308f0efa862SFelix Fietkau static inline void 309f0efa862SFelix Fietkau mt76_testmode_param_set(struct mt76_testmode_data *td, u16 idx) 310f0efa862SFelix Fietkau { 311f0efa862SFelix Fietkau td->param_set[idx / 32] |= BIT(idx % 32); 312f0efa862SFelix Fietkau } 313f0efa862SFelix Fietkau 314f0efa862SFelix Fietkau static inline bool 315f0efa862SFelix Fietkau mt76_testmode_param_present(struct mt76_testmode_data *td, u16 idx) 316f0efa862SFelix Fietkau { 317f0efa862SFelix Fietkau return td->param_set[idx / 32] & BIT(idx % 32); 318f0efa862SFelix Fietkau } 319f0efa862SFelix Fietkau 320f0efa862SFelix Fietkau static void 321c918c74dSShayne Chen mt76_testmode_init_defaults(struct mt76_phy *phy) 322f0efa862SFelix Fietkau { 323c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 324f0efa862SFelix Fietkau 3252601dda8SShayne Chen if (td->tx_mpdu_len > 0) 326f0efa862SFelix Fietkau return; 327f0efa862SFelix Fietkau 3282601dda8SShayne Chen td->tx_mpdu_len = 1024; 329f0efa862SFelix Fietkau td->tx_count = 1; 330f0efa862SFelix Fietkau td->tx_rate_mode = MT76_TM_TX_MODE_OFDM; 331f0efa862SFelix Fietkau td->tx_rate_nss = 1; 332f0efa862SFelix Fietkau } 333f0efa862SFelix Fietkau 334f0efa862SFelix Fietkau static int 335c918c74dSShayne Chen __mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state) 336f0efa862SFelix Fietkau { 337c918c74dSShayne Chen enum mt76_testmode_state prev_state = phy->test.state; 338c918c74dSShayne Chen struct mt76_dev *dev = phy->dev; 339f0efa862SFelix Fietkau int err; 340f0efa862SFelix Fietkau 341f0efa862SFelix Fietkau if (prev_state == MT76_TM_STATE_TX_FRAMES) 342c918c74dSShayne Chen mt76_testmode_tx_stop(phy); 343f0efa862SFelix Fietkau 344f0efa862SFelix Fietkau if (state == MT76_TM_STATE_TX_FRAMES) { 345c918c74dSShayne Chen err = mt76_testmode_tx_init(phy); 346f0efa862SFelix Fietkau if (err) 347f0efa862SFelix Fietkau return err; 348f0efa862SFelix Fietkau } 349f0efa862SFelix Fietkau 350c918c74dSShayne Chen err = dev->test_ops->set_state(phy, state); 351f0efa862SFelix Fietkau if (err) { 352f0efa862SFelix Fietkau if (state == MT76_TM_STATE_TX_FRAMES) 353c918c74dSShayne Chen mt76_testmode_tx_stop(phy); 354f0efa862SFelix Fietkau 355f0efa862SFelix Fietkau return err; 356f0efa862SFelix Fietkau } 357f0efa862SFelix Fietkau 358f0efa862SFelix Fietkau if (state == MT76_TM_STATE_TX_FRAMES) 359c918c74dSShayne Chen mt76_testmode_tx_start(phy); 360f0efa862SFelix Fietkau else if (state == MT76_TM_STATE_RX_FRAMES) { 361c918c74dSShayne Chen memset(&phy->test.rx_stats, 0, sizeof(phy->test.rx_stats)); 362f0efa862SFelix Fietkau } 363f0efa862SFelix Fietkau 364c918c74dSShayne Chen phy->test.state = state; 365f0efa862SFelix Fietkau 366f0efa862SFelix Fietkau return 0; 367f0efa862SFelix Fietkau } 368f0efa862SFelix Fietkau 369c918c74dSShayne Chen int mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state) 370f0efa862SFelix Fietkau { 371c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 372c918c74dSShayne Chen struct ieee80211_hw *hw = phy->hw; 373f0efa862SFelix Fietkau 374f0efa862SFelix Fietkau if (state == td->state && state == MT76_TM_STATE_OFF) 375f0efa862SFelix Fietkau return 0; 376f0efa862SFelix Fietkau 377f0efa862SFelix Fietkau if (state > MT76_TM_STATE_OFF && 378c918c74dSShayne Chen (!test_bit(MT76_STATE_RUNNING, &phy->state) || 379f0efa862SFelix Fietkau !(hw->conf.flags & IEEE80211_CONF_MONITOR))) 380f0efa862SFelix Fietkau return -ENOTCONN; 381f0efa862SFelix Fietkau 382f0efa862SFelix Fietkau if (state != MT76_TM_STATE_IDLE && 383f0efa862SFelix Fietkau td->state != MT76_TM_STATE_IDLE) { 384f0efa862SFelix Fietkau int ret; 385f0efa862SFelix Fietkau 386c918c74dSShayne Chen ret = __mt76_testmode_set_state(phy, MT76_TM_STATE_IDLE); 387f0efa862SFelix Fietkau if (ret) 388f0efa862SFelix Fietkau return ret; 389f0efa862SFelix Fietkau } 390f0efa862SFelix Fietkau 391c918c74dSShayne Chen return __mt76_testmode_set_state(phy, state); 392f0efa862SFelix Fietkau 393f0efa862SFelix Fietkau } 394f0efa862SFelix Fietkau EXPORT_SYMBOL(mt76_testmode_set_state); 395f0efa862SFelix Fietkau 396f0efa862SFelix Fietkau static int 397f0efa862SFelix Fietkau mt76_tm_get_u8(struct nlattr *attr, u8 *dest, u8 min, u8 max) 398f0efa862SFelix Fietkau { 399f0efa862SFelix Fietkau u8 val; 400f0efa862SFelix Fietkau 401f0efa862SFelix Fietkau if (!attr) 402f0efa862SFelix Fietkau return 0; 403f0efa862SFelix Fietkau 404f0efa862SFelix Fietkau val = nla_get_u8(attr); 405f0efa862SFelix Fietkau if (val < min || val > max) 406f0efa862SFelix Fietkau return -EINVAL; 407f0efa862SFelix Fietkau 408f0efa862SFelix Fietkau *dest = val; 409f0efa862SFelix Fietkau return 0; 410f0efa862SFelix Fietkau } 411f0efa862SFelix Fietkau 412f0efa862SFelix Fietkau int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif, 413f0efa862SFelix Fietkau void *data, int len) 414f0efa862SFelix Fietkau { 415f0efa862SFelix Fietkau struct mt76_phy *phy = hw->priv; 416f0efa862SFelix Fietkau struct mt76_dev *dev = phy->dev; 417c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 418f0efa862SFelix Fietkau struct nlattr *tb[NUM_MT76_TM_ATTRS]; 419c918c74dSShayne Chen bool ext_phy = phy != &dev->phy; 420f0efa862SFelix Fietkau u32 state; 421f0efa862SFelix Fietkau int err; 422f0efa862SFelix Fietkau int i; 423f0efa862SFelix Fietkau 424f0efa862SFelix Fietkau if (!dev->test_ops) 425f0efa862SFelix Fietkau return -EOPNOTSUPP; 426f0efa862SFelix Fietkau 427f0efa862SFelix Fietkau err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len, 428f0efa862SFelix Fietkau mt76_tm_policy, NULL); 429f0efa862SFelix Fietkau if (err) 430f0efa862SFelix Fietkau return err; 431f0efa862SFelix Fietkau 432f0efa862SFelix Fietkau err = -EINVAL; 433f0efa862SFelix Fietkau 434f0efa862SFelix Fietkau mutex_lock(&dev->mutex); 435f0efa862SFelix Fietkau 436f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_RESET]) { 437c918c74dSShayne Chen mt76_testmode_set_state(phy, MT76_TM_STATE_OFF); 438f0efa862SFelix Fietkau memset(td, 0, sizeof(*td)); 439f0efa862SFelix Fietkau } 440f0efa862SFelix Fietkau 441c918c74dSShayne Chen mt76_testmode_init_defaults(phy); 442f0efa862SFelix Fietkau 443f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_TX_COUNT]) 444f0efa862SFelix Fietkau td->tx_count = nla_get_u32(tb[MT76_TM_ATTR_TX_COUNT]); 445f0efa862SFelix Fietkau 446f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_TX_RATE_IDX]) 447f0efa862SFelix Fietkau td->tx_rate_idx = nla_get_u8(tb[MT76_TM_ATTR_TX_RATE_IDX]); 448f0efa862SFelix Fietkau 449f0efa862SFelix Fietkau if (mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_MODE], &td->tx_rate_mode, 450f0efa862SFelix Fietkau 0, MT76_TM_TX_MODE_MAX) || 451f0efa862SFelix Fietkau mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_NSS], &td->tx_rate_nss, 452f0efa862SFelix Fietkau 1, hweight8(phy->antenna_mask)) || 4531a38c2f5SShayne Chen mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_SGI], &td->tx_rate_sgi, 0, 2) || 454f0efa862SFelix Fietkau mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_LDPC], &td->tx_rate_ldpc, 0, 1) || 4557f54c742SShayne Chen mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_STBC], &td->tx_rate_stbc, 0, 1) || 4561a38c2f5SShayne Chen mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_LTF], &td->tx_ltf, 0, 2) || 457c918c74dSShayne Chen mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_ANTENNA], &td->tx_antenna_mask, 458c918c74dSShayne Chen 1 << (ext_phy * 2), phy->antenna_mask << (ext_phy * 2)) || 459fdc9c18eSShayne Chen mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_SPE_IDX], &td->tx_spe_idx, 0, 27) || 460b8cbdb97SShayne Chen mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_DUTY_CYCLE], 461b8cbdb97SShayne Chen &td->tx_duty_cycle, 0, 99) || 462f0efa862SFelix Fietkau mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_POWER_CONTROL], 463f0efa862SFelix Fietkau &td->tx_power_control, 0, 1)) 464f0efa862SFelix Fietkau goto out; 465f0efa862SFelix Fietkau 4662601dda8SShayne Chen if (tb[MT76_TM_ATTR_TX_LENGTH]) { 4672601dda8SShayne Chen u32 val = nla_get_u32(tb[MT76_TM_ATTR_TX_LENGTH]); 4682601dda8SShayne Chen 4692601dda8SShayne Chen if (val > mt76_testmode_max_mpdu_len(phy, td->tx_rate_mode) || 4702601dda8SShayne Chen val < sizeof(struct ieee80211_hdr)) 4712601dda8SShayne Chen goto out; 4722601dda8SShayne Chen 4732601dda8SShayne Chen td->tx_mpdu_len = val; 4742601dda8SShayne Chen } 4752601dda8SShayne Chen 476b8cbdb97SShayne Chen if (tb[MT76_TM_ATTR_TX_IPG]) 477b8cbdb97SShayne Chen td->tx_ipg = nla_get_u32(tb[MT76_TM_ATTR_TX_IPG]); 478b8cbdb97SShayne Chen 479b8cbdb97SShayne Chen if (tb[MT76_TM_ATTR_TX_TIME]) 480b8cbdb97SShayne Chen td->tx_time = nla_get_u32(tb[MT76_TM_ATTR_TX_TIME]); 481b8cbdb97SShayne Chen 482f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_FREQ_OFFSET]) 483f0efa862SFelix Fietkau td->freq_offset = nla_get_u32(tb[MT76_TM_ATTR_FREQ_OFFSET]); 484f0efa862SFelix Fietkau 485f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_STATE]) { 486f0efa862SFelix Fietkau state = nla_get_u32(tb[MT76_TM_ATTR_STATE]); 487f0efa862SFelix Fietkau if (state > MT76_TM_STATE_MAX) 488f0efa862SFelix Fietkau goto out; 489f0efa862SFelix Fietkau } else { 490f0efa862SFelix Fietkau state = td->state; 491f0efa862SFelix Fietkau } 492f0efa862SFelix Fietkau 493f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_TX_POWER]) { 494f0efa862SFelix Fietkau struct nlattr *cur; 495f0efa862SFelix Fietkau int idx = 0; 496f0efa862SFelix Fietkau int rem; 497f0efa862SFelix Fietkau 498f0efa862SFelix Fietkau nla_for_each_nested(cur, tb[MT76_TM_ATTR_TX_POWER], rem) { 499f0efa862SFelix Fietkau if (nla_len(cur) != 1 || 500f0efa862SFelix Fietkau idx >= ARRAY_SIZE(td->tx_power)) 501f0efa862SFelix Fietkau goto out; 502f0efa862SFelix Fietkau 503f0efa862SFelix Fietkau td->tx_power[idx++] = nla_get_u8(cur); 504f0efa862SFelix Fietkau } 505f0efa862SFelix Fietkau } 506f0efa862SFelix Fietkau 507f0efa862SFelix Fietkau if (dev->test_ops->set_params) { 508c918c74dSShayne Chen err = dev->test_ops->set_params(phy, tb, state); 509f0efa862SFelix Fietkau if (err) 510f0efa862SFelix Fietkau goto out; 511f0efa862SFelix Fietkau } 512f0efa862SFelix Fietkau 513f0efa862SFelix Fietkau for (i = MT76_TM_ATTR_STATE; i < ARRAY_SIZE(tb); i++) 514f0efa862SFelix Fietkau if (tb[i]) 515f0efa862SFelix Fietkau mt76_testmode_param_set(td, i); 516f0efa862SFelix Fietkau 517f0efa862SFelix Fietkau err = 0; 518f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_STATE]) 519c918c74dSShayne Chen err = mt76_testmode_set_state(phy, state); 520f0efa862SFelix Fietkau 521f0efa862SFelix Fietkau out: 522f0efa862SFelix Fietkau mutex_unlock(&dev->mutex); 523f0efa862SFelix Fietkau 524f0efa862SFelix Fietkau return err; 525f0efa862SFelix Fietkau } 526f0efa862SFelix Fietkau EXPORT_SYMBOL(mt76_testmode_cmd); 527f0efa862SFelix Fietkau 528f0efa862SFelix Fietkau static int 529c918c74dSShayne Chen mt76_testmode_dump_stats(struct mt76_phy *phy, struct sk_buff *msg) 530f0efa862SFelix Fietkau { 531c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 532c918c74dSShayne Chen struct mt76_dev *dev = phy->dev; 533f0efa862SFelix Fietkau u64 rx_packets = 0; 534f0efa862SFelix Fietkau u64 rx_fcs_error = 0; 535f0efa862SFelix Fietkau int i; 536f0efa862SFelix Fietkau 537f0efa862SFelix Fietkau for (i = 0; i < ARRAY_SIZE(td->rx_stats.packets); i++) { 538f0efa862SFelix Fietkau rx_packets += td->rx_stats.packets[i]; 539f0efa862SFelix Fietkau rx_fcs_error += td->rx_stats.fcs_error[i]; 540f0efa862SFelix Fietkau } 541f0efa862SFelix Fietkau 542f0efa862SFelix Fietkau if (nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_PENDING, td->tx_pending) || 543f0efa862SFelix Fietkau nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_QUEUED, td->tx_queued) || 544f0efa862SFelix Fietkau nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_DONE, td->tx_done) || 545f0efa862SFelix Fietkau nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_PACKETS, rx_packets, 546f0efa862SFelix Fietkau MT76_TM_STATS_ATTR_PAD) || 547f0efa862SFelix Fietkau nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_FCS_ERROR, rx_fcs_error, 548f0efa862SFelix Fietkau MT76_TM_STATS_ATTR_PAD)) 549f0efa862SFelix Fietkau return -EMSGSIZE; 550f0efa862SFelix Fietkau 551f0efa862SFelix Fietkau if (dev->test_ops->dump_stats) 552c918c74dSShayne Chen return dev->test_ops->dump_stats(phy, msg); 553f0efa862SFelix Fietkau 554f0efa862SFelix Fietkau return 0; 555f0efa862SFelix Fietkau } 556f0efa862SFelix Fietkau 557f0efa862SFelix Fietkau int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg, 558f0efa862SFelix Fietkau struct netlink_callback *cb, void *data, int len) 559f0efa862SFelix Fietkau { 560f0efa862SFelix Fietkau struct mt76_phy *phy = hw->priv; 561f0efa862SFelix Fietkau struct mt76_dev *dev = phy->dev; 562c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 563f0efa862SFelix Fietkau struct nlattr *tb[NUM_MT76_TM_ATTRS] = {}; 564f0efa862SFelix Fietkau int err = 0; 565f0efa862SFelix Fietkau void *a; 566f0efa862SFelix Fietkau int i; 567f0efa862SFelix Fietkau 568f0efa862SFelix Fietkau if (!dev->test_ops) 569f0efa862SFelix Fietkau return -EOPNOTSUPP; 570f0efa862SFelix Fietkau 571f0efa862SFelix Fietkau if (cb->args[2]++ > 0) 572f0efa862SFelix Fietkau return -ENOENT; 573f0efa862SFelix Fietkau 574f0efa862SFelix Fietkau if (data) { 575f0efa862SFelix Fietkau err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len, 576f0efa862SFelix Fietkau mt76_tm_policy, NULL); 577f0efa862SFelix Fietkau if (err) 578f0efa862SFelix Fietkau return err; 579f0efa862SFelix Fietkau } 580f0efa862SFelix Fietkau 581f0efa862SFelix Fietkau mutex_lock(&dev->mutex); 582f0efa862SFelix Fietkau 583f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_STATS]) { 584ce8463a7SLorenzo Bianconi err = -EINVAL; 585ce8463a7SLorenzo Bianconi 586f0efa862SFelix Fietkau a = nla_nest_start(msg, MT76_TM_ATTR_STATS); 587ce8463a7SLorenzo Bianconi if (a) { 588c918c74dSShayne Chen err = mt76_testmode_dump_stats(phy, msg); 589f0efa862SFelix Fietkau nla_nest_end(msg, a); 590ce8463a7SLorenzo Bianconi } 591f0efa862SFelix Fietkau 592f0efa862SFelix Fietkau goto out; 593f0efa862SFelix Fietkau } 594f0efa862SFelix Fietkau 595c918c74dSShayne Chen mt76_testmode_init_defaults(phy); 596f0efa862SFelix Fietkau 597f0efa862SFelix Fietkau err = -EMSGSIZE; 598f0efa862SFelix Fietkau if (nla_put_u32(msg, MT76_TM_ATTR_STATE, td->state)) 599f0efa862SFelix Fietkau goto out; 600f0efa862SFelix Fietkau 601e7a6a044SShayne Chen if (dev->test_mtd.name && 602e7a6a044SShayne Chen (nla_put_string(msg, MT76_TM_ATTR_MTD_PART, dev->test_mtd.name) || 603e7a6a044SShayne Chen nla_put_u32(msg, MT76_TM_ATTR_MTD_OFFSET, dev->test_mtd.offset))) 604f0efa862SFelix Fietkau goto out; 605f0efa862SFelix Fietkau 606f0efa862SFelix Fietkau if (nla_put_u32(msg, MT76_TM_ATTR_TX_COUNT, td->tx_count) || 6072601dda8SShayne Chen nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH, td->tx_mpdu_len) || 608f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_MODE, td->tx_rate_mode) || 609f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_NSS, td->tx_rate_nss) || 610f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, td->tx_rate_idx) || 611f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_SGI, td->tx_rate_sgi) || 612f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_LDPC, td->tx_rate_ldpc) || 6137f54c742SShayne Chen nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_STBC, td->tx_rate_stbc) || 6141a38c2f5SShayne Chen (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_LTF) && 6151a38c2f5SShayne Chen nla_put_u8(msg, MT76_TM_ATTR_TX_LTF, td->tx_ltf)) || 616f0efa862SFelix Fietkau (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_ANTENNA) && 617f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, td->tx_antenna_mask)) || 618fdc9c18eSShayne Chen (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_SPE_IDX) && 619fdc9c18eSShayne Chen nla_put_u8(msg, MT76_TM_ATTR_TX_SPE_IDX, td->tx_spe_idx)) || 620b8cbdb97SShayne Chen (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_DUTY_CYCLE) && 621b8cbdb97SShayne Chen nla_put_u8(msg, MT76_TM_ATTR_TX_DUTY_CYCLE, td->tx_duty_cycle)) || 622b8cbdb97SShayne Chen (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_IPG) && 623b8cbdb97SShayne Chen nla_put_u32(msg, MT76_TM_ATTR_TX_IPG, td->tx_ipg)) || 624b8cbdb97SShayne Chen (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_TIME) && 625b8cbdb97SShayne Chen nla_put_u32(msg, MT76_TM_ATTR_TX_TIME, td->tx_time)) || 626f0efa862SFelix Fietkau (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER_CONTROL) && 627f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_POWER_CONTROL, td->tx_power_control)) || 628f0efa862SFelix Fietkau (mt76_testmode_param_present(td, MT76_TM_ATTR_FREQ_OFFSET) && 629f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_FREQ_OFFSET, td->freq_offset))) 630f0efa862SFelix Fietkau goto out; 631f0efa862SFelix Fietkau 632f0efa862SFelix Fietkau if (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER)) { 633f0efa862SFelix Fietkau a = nla_nest_start(msg, MT76_TM_ATTR_TX_POWER); 634f0efa862SFelix Fietkau if (!a) 635f0efa862SFelix Fietkau goto out; 636f0efa862SFelix Fietkau 637f0efa862SFelix Fietkau for (i = 0; i < ARRAY_SIZE(td->tx_power); i++) 638f0efa862SFelix Fietkau if (nla_put_u8(msg, i, td->tx_power[i])) 639f0efa862SFelix Fietkau goto out; 640f0efa862SFelix Fietkau 641f0efa862SFelix Fietkau nla_nest_end(msg, a); 642f0efa862SFelix Fietkau } 643f0efa862SFelix Fietkau 644f0efa862SFelix Fietkau err = 0; 645f0efa862SFelix Fietkau 646f0efa862SFelix Fietkau out: 647f0efa862SFelix Fietkau mutex_unlock(&dev->mutex); 648f0efa862SFelix Fietkau 649f0efa862SFelix Fietkau return err; 650f0efa862SFelix Fietkau } 651f0efa862SFelix Fietkau EXPORT_SYMBOL(mt76_testmode_dump); 652