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 }, 20*b8cbdb97SShayne Chen [MT76_TM_ATTR_TX_DUTY_CYCLE] = { .type = NLA_U8 }, 21*b8cbdb97SShayne Chen [MT76_TM_ATTR_TX_IPG] = { .type = NLA_U32 }, 22*b8cbdb97SShayne 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; 33f0efa862SFelix Fietkau int qid; 34f0efa862SFelix Fietkau 35f0efa862SFelix Fietkau if (!skb || !td->tx_pending) 36f0efa862SFelix Fietkau return; 37f0efa862SFelix Fietkau 38f0efa862SFelix Fietkau qid = skb_get_queue_mapping(skb); 3991990519SLorenzo Bianconi q = phy->q_tx[qid]; 40f0efa862SFelix Fietkau 41f0efa862SFelix Fietkau spin_lock_bh(&q->lock); 42f0efa862SFelix Fietkau 439729ff4cSFelix Fietkau while (td->tx_pending > 0 && td->tx_queued - td->tx_done < 1000 && 449729ff4cSFelix Fietkau q->queued < q->ndesc / 2) { 45f0efa862SFelix Fietkau int ret; 46f0efa862SFelix Fietkau 4789870594SLorenzo Bianconi ret = dev->queue_ops->tx_queue_skb(dev, q, skb_get(skb), wcid, 4889870594SLorenzo Bianconi NULL); 49f0efa862SFelix Fietkau if (ret < 0) 50f0efa862SFelix Fietkau break; 51f0efa862SFelix Fietkau 52f0efa862SFelix Fietkau td->tx_pending--; 53f0efa862SFelix Fietkau td->tx_queued++; 54f0efa862SFelix Fietkau } 55f0efa862SFelix Fietkau 56f0efa862SFelix Fietkau dev->queue_ops->kick(dev, q); 57f0efa862SFelix Fietkau 58f0efa862SFelix Fietkau spin_unlock_bh(&q->lock); 59f0efa862SFelix Fietkau } 60f0efa862SFelix Fietkau 61f0efa862SFelix Fietkau 62f0efa862SFelix Fietkau static int 63c918c74dSShayne Chen mt76_testmode_tx_init(struct mt76_phy *phy) 64f0efa862SFelix Fietkau { 65c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 66f0efa862SFelix Fietkau struct ieee80211_tx_info *info; 67f0efa862SFelix Fietkau struct ieee80211_hdr *hdr; 68f0efa862SFelix Fietkau struct sk_buff *skb; 69f0efa862SFelix Fietkau u16 fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA | 70f0efa862SFelix Fietkau IEEE80211_FCTL_FROMDS; 71f0efa862SFelix Fietkau struct ieee80211_tx_rate *rate; 7298df2baeSLorenzo Bianconi u8 max_nss = hweight8(phy->antenna_mask); 73c918c74dSShayne Chen bool ext_phy = phy != &phy->dev->phy; 74f0efa862SFelix Fietkau 75f0efa862SFelix Fietkau if (td->tx_antenna_mask) 76f0efa862SFelix Fietkau max_nss = min_t(u8, max_nss, hweight8(td->tx_antenna_mask)); 77f0efa862SFelix Fietkau 78f0efa862SFelix Fietkau skb = alloc_skb(td->tx_msdu_len, GFP_KERNEL); 79f0efa862SFelix Fietkau if (!skb) 80f0efa862SFelix Fietkau return -ENOMEM; 81f0efa862SFelix Fietkau 82f0efa862SFelix Fietkau dev_kfree_skb(td->tx_skb); 83f0efa862SFelix Fietkau td->tx_skb = skb; 84f0efa862SFelix Fietkau hdr = __skb_put_zero(skb, td->tx_msdu_len); 85f0efa862SFelix Fietkau hdr->frame_control = cpu_to_le16(fc); 8698df2baeSLorenzo Bianconi memcpy(hdr->addr1, phy->macaddr, sizeof(phy->macaddr)); 8798df2baeSLorenzo Bianconi memcpy(hdr->addr2, phy->macaddr, sizeof(phy->macaddr)); 8898df2baeSLorenzo Bianconi memcpy(hdr->addr3, phy->macaddr, sizeof(phy->macaddr)); 89f0efa862SFelix Fietkau 90f0efa862SFelix Fietkau info = IEEE80211_SKB_CB(skb); 91f0efa862SFelix Fietkau info->flags = IEEE80211_TX_CTL_INJECTED | 92f0efa862SFelix Fietkau IEEE80211_TX_CTL_NO_ACK | 93f0efa862SFelix Fietkau IEEE80211_TX_CTL_NO_PS_BUFFER; 9461fe7357SShayne Chen 95c918c74dSShayne Chen if (ext_phy) 96c918c74dSShayne Chen info->hw_queue |= MT_TX_HW_QUEUE_EXT_PHY; 97c918c74dSShayne Chen 9861fe7357SShayne Chen if (td->tx_rate_mode > MT76_TM_TX_MODE_VHT) 9961fe7357SShayne Chen goto out; 10061fe7357SShayne Chen 101f0efa862SFelix Fietkau rate = &info->control.rates[0]; 102f0efa862SFelix Fietkau rate->count = 1; 103f0efa862SFelix Fietkau rate->idx = td->tx_rate_idx; 104f0efa862SFelix Fietkau 105f0efa862SFelix Fietkau switch (td->tx_rate_mode) { 106f0efa862SFelix Fietkau case MT76_TM_TX_MODE_CCK: 10798df2baeSLorenzo Bianconi if (phy->chandef.chan->band != NL80211_BAND_2GHZ) 108f0efa862SFelix Fietkau return -EINVAL; 109f0efa862SFelix Fietkau 110f0efa862SFelix Fietkau if (rate->idx > 4) 111f0efa862SFelix Fietkau return -EINVAL; 112f0efa862SFelix Fietkau break; 113f0efa862SFelix Fietkau case MT76_TM_TX_MODE_OFDM: 11498df2baeSLorenzo Bianconi if (phy->chandef.chan->band != NL80211_BAND_2GHZ) 115f0efa862SFelix Fietkau break; 116f0efa862SFelix Fietkau 117f0efa862SFelix Fietkau if (rate->idx > 8) 118f0efa862SFelix Fietkau return -EINVAL; 119f0efa862SFelix Fietkau 120f0efa862SFelix Fietkau rate->idx += 4; 121f0efa862SFelix Fietkau break; 122f0efa862SFelix Fietkau case MT76_TM_TX_MODE_HT: 123f0efa862SFelix Fietkau if (rate->idx > 8 * max_nss && 124f0efa862SFelix Fietkau !(rate->idx == 32 && 12598df2baeSLorenzo Bianconi phy->chandef.width >= NL80211_CHAN_WIDTH_40)) 126f0efa862SFelix Fietkau return -EINVAL; 127f0efa862SFelix Fietkau 128f0efa862SFelix Fietkau rate->flags |= IEEE80211_TX_RC_MCS; 129f0efa862SFelix Fietkau break; 130f0efa862SFelix Fietkau case MT76_TM_TX_MODE_VHT: 131f0efa862SFelix Fietkau if (rate->idx > 9) 132f0efa862SFelix Fietkau return -EINVAL; 133f0efa862SFelix Fietkau 134f0efa862SFelix Fietkau if (td->tx_rate_nss > max_nss) 135f0efa862SFelix Fietkau return -EINVAL; 136f0efa862SFelix Fietkau 137f0efa862SFelix Fietkau ieee80211_rate_set_vht(rate, td->tx_rate_idx, td->tx_rate_nss); 138f0efa862SFelix Fietkau rate->flags |= IEEE80211_TX_RC_VHT_MCS; 139f0efa862SFelix Fietkau break; 140f0efa862SFelix Fietkau default: 141f0efa862SFelix Fietkau break; 142f0efa862SFelix Fietkau } 143f0efa862SFelix Fietkau 144f0efa862SFelix Fietkau if (td->tx_rate_sgi) 145f0efa862SFelix Fietkau rate->flags |= IEEE80211_TX_RC_SHORT_GI; 146f0efa862SFelix Fietkau 147f0efa862SFelix Fietkau if (td->tx_rate_ldpc) 148f0efa862SFelix Fietkau info->flags |= IEEE80211_TX_CTL_LDPC; 149f0efa862SFelix Fietkau 1507f54c742SShayne Chen if (td->tx_rate_stbc) 1517f54c742SShayne Chen info->flags |= IEEE80211_TX_CTL_STBC; 1527f54c742SShayne Chen 153f0efa862SFelix Fietkau if (td->tx_rate_mode >= MT76_TM_TX_MODE_HT) { 15498df2baeSLorenzo Bianconi switch (phy->chandef.width) { 155f0efa862SFelix Fietkau case NL80211_CHAN_WIDTH_40: 156f0efa862SFelix Fietkau rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH; 157f0efa862SFelix Fietkau break; 158f0efa862SFelix Fietkau case NL80211_CHAN_WIDTH_80: 159f0efa862SFelix Fietkau rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH; 160f0efa862SFelix Fietkau break; 161f0efa862SFelix Fietkau case NL80211_CHAN_WIDTH_80P80: 162f0efa862SFelix Fietkau case NL80211_CHAN_WIDTH_160: 163f0efa862SFelix Fietkau rate->flags |= IEEE80211_TX_RC_160_MHZ_WIDTH; 164f0efa862SFelix Fietkau break; 165f0efa862SFelix Fietkau default: 166f0efa862SFelix Fietkau break; 167f0efa862SFelix Fietkau } 168f0efa862SFelix Fietkau } 16961fe7357SShayne Chen out: 170f0efa862SFelix Fietkau skb_set_queue_mapping(skb, IEEE80211_AC_BE); 171f0efa862SFelix Fietkau 172f0efa862SFelix Fietkau return 0; 173f0efa862SFelix Fietkau } 174f0efa862SFelix Fietkau 175f0efa862SFelix Fietkau static void 176c918c74dSShayne Chen mt76_testmode_tx_start(struct mt76_phy *phy) 177f0efa862SFelix Fietkau { 178c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 179c918c74dSShayne Chen struct mt76_dev *dev = phy->dev; 180f0efa862SFelix Fietkau 181f0efa862SFelix Fietkau td->tx_queued = 0; 182f0efa862SFelix Fietkau td->tx_done = 0; 183f0efa862SFelix Fietkau td->tx_pending = td->tx_count; 184781eef5bSFelix Fietkau mt76_worker_schedule(&dev->tx_worker); 185f0efa862SFelix Fietkau } 186f0efa862SFelix Fietkau 187f0efa862SFelix Fietkau static void 188c918c74dSShayne Chen mt76_testmode_tx_stop(struct mt76_phy *phy) 189f0efa862SFelix Fietkau { 190c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 191c918c74dSShayne Chen struct mt76_dev *dev = phy->dev; 192f0efa862SFelix Fietkau 193781eef5bSFelix Fietkau mt76_worker_disable(&dev->tx_worker); 194f0efa862SFelix Fietkau 195f0efa862SFelix Fietkau td->tx_pending = 0; 196f0efa862SFelix Fietkau 197781eef5bSFelix Fietkau mt76_worker_enable(&dev->tx_worker); 198f0efa862SFelix Fietkau 199f0efa862SFelix Fietkau wait_event_timeout(dev->tx_wait, td->tx_done == td->tx_queued, 10 * HZ); 200f0efa862SFelix Fietkau 201f0efa862SFelix Fietkau dev_kfree_skb(td->tx_skb); 202f0efa862SFelix Fietkau td->tx_skb = NULL; 203f0efa862SFelix Fietkau } 204f0efa862SFelix Fietkau 205f0efa862SFelix Fietkau static inline void 206f0efa862SFelix Fietkau mt76_testmode_param_set(struct mt76_testmode_data *td, u16 idx) 207f0efa862SFelix Fietkau { 208f0efa862SFelix Fietkau td->param_set[idx / 32] |= BIT(idx % 32); 209f0efa862SFelix Fietkau } 210f0efa862SFelix Fietkau 211f0efa862SFelix Fietkau static inline bool 212f0efa862SFelix Fietkau mt76_testmode_param_present(struct mt76_testmode_data *td, u16 idx) 213f0efa862SFelix Fietkau { 214f0efa862SFelix Fietkau return td->param_set[idx / 32] & BIT(idx % 32); 215f0efa862SFelix Fietkau } 216f0efa862SFelix Fietkau 217f0efa862SFelix Fietkau static void 218c918c74dSShayne Chen mt76_testmode_init_defaults(struct mt76_phy *phy) 219f0efa862SFelix Fietkau { 220c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 221f0efa862SFelix Fietkau 222f0efa862SFelix Fietkau if (td->tx_msdu_len > 0) 223f0efa862SFelix Fietkau return; 224f0efa862SFelix Fietkau 225f0efa862SFelix Fietkau td->tx_msdu_len = 1024; 226f0efa862SFelix Fietkau td->tx_count = 1; 227f0efa862SFelix Fietkau td->tx_rate_mode = MT76_TM_TX_MODE_OFDM; 228f0efa862SFelix Fietkau td->tx_rate_nss = 1; 229f0efa862SFelix Fietkau } 230f0efa862SFelix Fietkau 231f0efa862SFelix Fietkau static int 232c918c74dSShayne Chen __mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state) 233f0efa862SFelix Fietkau { 234c918c74dSShayne Chen enum mt76_testmode_state prev_state = phy->test.state; 235c918c74dSShayne Chen struct mt76_dev *dev = phy->dev; 236f0efa862SFelix Fietkau int err; 237f0efa862SFelix Fietkau 238f0efa862SFelix Fietkau if (prev_state == MT76_TM_STATE_TX_FRAMES) 239c918c74dSShayne Chen mt76_testmode_tx_stop(phy); 240f0efa862SFelix Fietkau 241f0efa862SFelix Fietkau if (state == MT76_TM_STATE_TX_FRAMES) { 242c918c74dSShayne Chen err = mt76_testmode_tx_init(phy); 243f0efa862SFelix Fietkau if (err) 244f0efa862SFelix Fietkau return err; 245f0efa862SFelix Fietkau } 246f0efa862SFelix Fietkau 247c918c74dSShayne Chen err = dev->test_ops->set_state(phy, state); 248f0efa862SFelix Fietkau if (err) { 249f0efa862SFelix Fietkau if (state == MT76_TM_STATE_TX_FRAMES) 250c918c74dSShayne Chen mt76_testmode_tx_stop(phy); 251f0efa862SFelix Fietkau 252f0efa862SFelix Fietkau return err; 253f0efa862SFelix Fietkau } 254f0efa862SFelix Fietkau 255f0efa862SFelix Fietkau if (state == MT76_TM_STATE_TX_FRAMES) 256c918c74dSShayne Chen mt76_testmode_tx_start(phy); 257f0efa862SFelix Fietkau else if (state == MT76_TM_STATE_RX_FRAMES) { 258c918c74dSShayne Chen memset(&phy->test.rx_stats, 0, sizeof(phy->test.rx_stats)); 259f0efa862SFelix Fietkau } 260f0efa862SFelix Fietkau 261c918c74dSShayne Chen phy->test.state = state; 262f0efa862SFelix Fietkau 263f0efa862SFelix Fietkau return 0; 264f0efa862SFelix Fietkau } 265f0efa862SFelix Fietkau 266c918c74dSShayne Chen int mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state) 267f0efa862SFelix Fietkau { 268c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 269c918c74dSShayne Chen struct ieee80211_hw *hw = phy->hw; 270f0efa862SFelix Fietkau 271f0efa862SFelix Fietkau if (state == td->state && state == MT76_TM_STATE_OFF) 272f0efa862SFelix Fietkau return 0; 273f0efa862SFelix Fietkau 274f0efa862SFelix Fietkau if (state > MT76_TM_STATE_OFF && 275c918c74dSShayne Chen (!test_bit(MT76_STATE_RUNNING, &phy->state) || 276f0efa862SFelix Fietkau !(hw->conf.flags & IEEE80211_CONF_MONITOR))) 277f0efa862SFelix Fietkau return -ENOTCONN; 278f0efa862SFelix Fietkau 279f0efa862SFelix Fietkau if (state != MT76_TM_STATE_IDLE && 280f0efa862SFelix Fietkau td->state != MT76_TM_STATE_IDLE) { 281f0efa862SFelix Fietkau int ret; 282f0efa862SFelix Fietkau 283c918c74dSShayne Chen ret = __mt76_testmode_set_state(phy, MT76_TM_STATE_IDLE); 284f0efa862SFelix Fietkau if (ret) 285f0efa862SFelix Fietkau return ret; 286f0efa862SFelix Fietkau } 287f0efa862SFelix Fietkau 288c918c74dSShayne Chen return __mt76_testmode_set_state(phy, state); 289f0efa862SFelix Fietkau 290f0efa862SFelix Fietkau } 291f0efa862SFelix Fietkau EXPORT_SYMBOL(mt76_testmode_set_state); 292f0efa862SFelix Fietkau 293f0efa862SFelix Fietkau static int 294f0efa862SFelix Fietkau mt76_tm_get_u8(struct nlattr *attr, u8 *dest, u8 min, u8 max) 295f0efa862SFelix Fietkau { 296f0efa862SFelix Fietkau u8 val; 297f0efa862SFelix Fietkau 298f0efa862SFelix Fietkau if (!attr) 299f0efa862SFelix Fietkau return 0; 300f0efa862SFelix Fietkau 301f0efa862SFelix Fietkau val = nla_get_u8(attr); 302f0efa862SFelix Fietkau if (val < min || val > max) 303f0efa862SFelix Fietkau return -EINVAL; 304f0efa862SFelix Fietkau 305f0efa862SFelix Fietkau *dest = val; 306f0efa862SFelix Fietkau return 0; 307f0efa862SFelix Fietkau } 308f0efa862SFelix Fietkau 309f0efa862SFelix Fietkau int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif, 310f0efa862SFelix Fietkau void *data, int len) 311f0efa862SFelix Fietkau { 312f0efa862SFelix Fietkau struct mt76_phy *phy = hw->priv; 313f0efa862SFelix Fietkau struct mt76_dev *dev = phy->dev; 314c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 315f0efa862SFelix Fietkau struct nlattr *tb[NUM_MT76_TM_ATTRS]; 316c918c74dSShayne Chen bool ext_phy = phy != &dev->phy; 317f0efa862SFelix Fietkau u32 state; 318f0efa862SFelix Fietkau int err; 319f0efa862SFelix Fietkau int i; 320f0efa862SFelix Fietkau 321f0efa862SFelix Fietkau if (!dev->test_ops) 322f0efa862SFelix Fietkau return -EOPNOTSUPP; 323f0efa862SFelix Fietkau 324f0efa862SFelix Fietkau err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len, 325f0efa862SFelix Fietkau mt76_tm_policy, NULL); 326f0efa862SFelix Fietkau if (err) 327f0efa862SFelix Fietkau return err; 328f0efa862SFelix Fietkau 329f0efa862SFelix Fietkau err = -EINVAL; 330f0efa862SFelix Fietkau 331f0efa862SFelix Fietkau mutex_lock(&dev->mutex); 332f0efa862SFelix Fietkau 333f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_RESET]) { 334c918c74dSShayne Chen mt76_testmode_set_state(phy, MT76_TM_STATE_OFF); 335f0efa862SFelix Fietkau memset(td, 0, sizeof(*td)); 336f0efa862SFelix Fietkau } 337f0efa862SFelix Fietkau 338c918c74dSShayne Chen mt76_testmode_init_defaults(phy); 339f0efa862SFelix Fietkau 340f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_TX_COUNT]) 341f0efa862SFelix Fietkau td->tx_count = nla_get_u32(tb[MT76_TM_ATTR_TX_COUNT]); 342f0efa862SFelix Fietkau 343f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_TX_LENGTH]) { 344f0efa862SFelix Fietkau u32 val = nla_get_u32(tb[MT76_TM_ATTR_TX_LENGTH]); 345f0efa862SFelix Fietkau 346f0efa862SFelix Fietkau if (val > IEEE80211_MAX_FRAME_LEN || 347f0efa862SFelix Fietkau val < sizeof(struct ieee80211_hdr)) 348f0efa862SFelix Fietkau goto out; 349f0efa862SFelix Fietkau 350f0efa862SFelix Fietkau td->tx_msdu_len = val; 351f0efa862SFelix Fietkau } 352f0efa862SFelix Fietkau 353f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_TX_RATE_IDX]) 354f0efa862SFelix Fietkau td->tx_rate_idx = nla_get_u8(tb[MT76_TM_ATTR_TX_RATE_IDX]); 355f0efa862SFelix Fietkau 356f0efa862SFelix Fietkau if (mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_MODE], &td->tx_rate_mode, 357f0efa862SFelix Fietkau 0, MT76_TM_TX_MODE_MAX) || 358f0efa862SFelix Fietkau mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_NSS], &td->tx_rate_nss, 359f0efa862SFelix Fietkau 1, hweight8(phy->antenna_mask)) || 3601a38c2f5SShayne Chen mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_SGI], &td->tx_rate_sgi, 0, 2) || 361f0efa862SFelix Fietkau mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_LDPC], &td->tx_rate_ldpc, 0, 1) || 3627f54c742SShayne Chen mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_STBC], &td->tx_rate_stbc, 0, 1) || 3631a38c2f5SShayne Chen mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_LTF], &td->tx_ltf, 0, 2) || 364c918c74dSShayne Chen mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_ANTENNA], &td->tx_antenna_mask, 365c918c74dSShayne Chen 1 << (ext_phy * 2), phy->antenna_mask << (ext_phy * 2)) || 366fdc9c18eSShayne Chen mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_SPE_IDX], &td->tx_spe_idx, 0, 27) || 367*b8cbdb97SShayne Chen mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_DUTY_CYCLE], 368*b8cbdb97SShayne Chen &td->tx_duty_cycle, 0, 99) || 369f0efa862SFelix Fietkau mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_POWER_CONTROL], 370f0efa862SFelix Fietkau &td->tx_power_control, 0, 1)) 371f0efa862SFelix Fietkau goto out; 372f0efa862SFelix Fietkau 373*b8cbdb97SShayne Chen if (tb[MT76_TM_ATTR_TX_IPG]) 374*b8cbdb97SShayne Chen td->tx_ipg = nla_get_u32(tb[MT76_TM_ATTR_TX_IPG]); 375*b8cbdb97SShayne Chen 376*b8cbdb97SShayne Chen if (tb[MT76_TM_ATTR_TX_TIME]) 377*b8cbdb97SShayne Chen td->tx_time = nla_get_u32(tb[MT76_TM_ATTR_TX_TIME]); 378*b8cbdb97SShayne Chen 379f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_FREQ_OFFSET]) 380f0efa862SFelix Fietkau td->freq_offset = nla_get_u32(tb[MT76_TM_ATTR_FREQ_OFFSET]); 381f0efa862SFelix Fietkau 382f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_STATE]) { 383f0efa862SFelix Fietkau state = nla_get_u32(tb[MT76_TM_ATTR_STATE]); 384f0efa862SFelix Fietkau if (state > MT76_TM_STATE_MAX) 385f0efa862SFelix Fietkau goto out; 386f0efa862SFelix Fietkau } else { 387f0efa862SFelix Fietkau state = td->state; 388f0efa862SFelix Fietkau } 389f0efa862SFelix Fietkau 390f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_TX_POWER]) { 391f0efa862SFelix Fietkau struct nlattr *cur; 392f0efa862SFelix Fietkau int idx = 0; 393f0efa862SFelix Fietkau int rem; 394f0efa862SFelix Fietkau 395f0efa862SFelix Fietkau nla_for_each_nested(cur, tb[MT76_TM_ATTR_TX_POWER], rem) { 396f0efa862SFelix Fietkau if (nla_len(cur) != 1 || 397f0efa862SFelix Fietkau idx >= ARRAY_SIZE(td->tx_power)) 398f0efa862SFelix Fietkau goto out; 399f0efa862SFelix Fietkau 400f0efa862SFelix Fietkau td->tx_power[idx++] = nla_get_u8(cur); 401f0efa862SFelix Fietkau } 402f0efa862SFelix Fietkau } 403f0efa862SFelix Fietkau 404f0efa862SFelix Fietkau if (dev->test_ops->set_params) { 405c918c74dSShayne Chen err = dev->test_ops->set_params(phy, tb, state); 406f0efa862SFelix Fietkau if (err) 407f0efa862SFelix Fietkau goto out; 408f0efa862SFelix Fietkau } 409f0efa862SFelix Fietkau 410f0efa862SFelix Fietkau for (i = MT76_TM_ATTR_STATE; i < ARRAY_SIZE(tb); i++) 411f0efa862SFelix Fietkau if (tb[i]) 412f0efa862SFelix Fietkau mt76_testmode_param_set(td, i); 413f0efa862SFelix Fietkau 414f0efa862SFelix Fietkau err = 0; 415f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_STATE]) 416c918c74dSShayne Chen err = mt76_testmode_set_state(phy, state); 417f0efa862SFelix Fietkau 418f0efa862SFelix Fietkau out: 419f0efa862SFelix Fietkau mutex_unlock(&dev->mutex); 420f0efa862SFelix Fietkau 421f0efa862SFelix Fietkau return err; 422f0efa862SFelix Fietkau } 423f0efa862SFelix Fietkau EXPORT_SYMBOL(mt76_testmode_cmd); 424f0efa862SFelix Fietkau 425f0efa862SFelix Fietkau static int 426c918c74dSShayne Chen mt76_testmode_dump_stats(struct mt76_phy *phy, struct sk_buff *msg) 427f0efa862SFelix Fietkau { 428c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 429c918c74dSShayne Chen struct mt76_dev *dev = phy->dev; 430f0efa862SFelix Fietkau u64 rx_packets = 0; 431f0efa862SFelix Fietkau u64 rx_fcs_error = 0; 432f0efa862SFelix Fietkau int i; 433f0efa862SFelix Fietkau 434f0efa862SFelix Fietkau for (i = 0; i < ARRAY_SIZE(td->rx_stats.packets); i++) { 435f0efa862SFelix Fietkau rx_packets += td->rx_stats.packets[i]; 436f0efa862SFelix Fietkau rx_fcs_error += td->rx_stats.fcs_error[i]; 437f0efa862SFelix Fietkau } 438f0efa862SFelix Fietkau 439f0efa862SFelix Fietkau if (nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_PENDING, td->tx_pending) || 440f0efa862SFelix Fietkau nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_QUEUED, td->tx_queued) || 441f0efa862SFelix Fietkau nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_DONE, td->tx_done) || 442f0efa862SFelix Fietkau nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_PACKETS, rx_packets, 443f0efa862SFelix Fietkau MT76_TM_STATS_ATTR_PAD) || 444f0efa862SFelix Fietkau nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_FCS_ERROR, rx_fcs_error, 445f0efa862SFelix Fietkau MT76_TM_STATS_ATTR_PAD)) 446f0efa862SFelix Fietkau return -EMSGSIZE; 447f0efa862SFelix Fietkau 448f0efa862SFelix Fietkau if (dev->test_ops->dump_stats) 449c918c74dSShayne Chen return dev->test_ops->dump_stats(phy, msg); 450f0efa862SFelix Fietkau 451f0efa862SFelix Fietkau return 0; 452f0efa862SFelix Fietkau } 453f0efa862SFelix Fietkau 454f0efa862SFelix Fietkau int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg, 455f0efa862SFelix Fietkau struct netlink_callback *cb, void *data, int len) 456f0efa862SFelix Fietkau { 457f0efa862SFelix Fietkau struct mt76_phy *phy = hw->priv; 458f0efa862SFelix Fietkau struct mt76_dev *dev = phy->dev; 459c918c74dSShayne Chen struct mt76_testmode_data *td = &phy->test; 460f0efa862SFelix Fietkau struct nlattr *tb[NUM_MT76_TM_ATTRS] = {}; 461f0efa862SFelix Fietkau int err = 0; 462f0efa862SFelix Fietkau void *a; 463f0efa862SFelix Fietkau int i; 464f0efa862SFelix Fietkau 465f0efa862SFelix Fietkau if (!dev->test_ops) 466f0efa862SFelix Fietkau return -EOPNOTSUPP; 467f0efa862SFelix Fietkau 468f0efa862SFelix Fietkau if (cb->args[2]++ > 0) 469f0efa862SFelix Fietkau return -ENOENT; 470f0efa862SFelix Fietkau 471f0efa862SFelix Fietkau if (data) { 472f0efa862SFelix Fietkau err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len, 473f0efa862SFelix Fietkau mt76_tm_policy, NULL); 474f0efa862SFelix Fietkau if (err) 475f0efa862SFelix Fietkau return err; 476f0efa862SFelix Fietkau } 477f0efa862SFelix Fietkau 478f0efa862SFelix Fietkau mutex_lock(&dev->mutex); 479f0efa862SFelix Fietkau 480f0efa862SFelix Fietkau if (tb[MT76_TM_ATTR_STATS]) { 481ce8463a7SLorenzo Bianconi err = -EINVAL; 482ce8463a7SLorenzo Bianconi 483f0efa862SFelix Fietkau a = nla_nest_start(msg, MT76_TM_ATTR_STATS); 484ce8463a7SLorenzo Bianconi if (a) { 485c918c74dSShayne Chen err = mt76_testmode_dump_stats(phy, msg); 486f0efa862SFelix Fietkau nla_nest_end(msg, a); 487ce8463a7SLorenzo Bianconi } 488f0efa862SFelix Fietkau 489f0efa862SFelix Fietkau goto out; 490f0efa862SFelix Fietkau } 491f0efa862SFelix Fietkau 492c918c74dSShayne Chen mt76_testmode_init_defaults(phy); 493f0efa862SFelix Fietkau 494f0efa862SFelix Fietkau err = -EMSGSIZE; 495f0efa862SFelix Fietkau if (nla_put_u32(msg, MT76_TM_ATTR_STATE, td->state)) 496f0efa862SFelix Fietkau goto out; 497f0efa862SFelix Fietkau 498e7a6a044SShayne Chen if (dev->test_mtd.name && 499e7a6a044SShayne Chen (nla_put_string(msg, MT76_TM_ATTR_MTD_PART, dev->test_mtd.name) || 500e7a6a044SShayne Chen nla_put_u32(msg, MT76_TM_ATTR_MTD_OFFSET, dev->test_mtd.offset))) 501f0efa862SFelix Fietkau goto out; 502f0efa862SFelix Fietkau 503f0efa862SFelix Fietkau if (nla_put_u32(msg, MT76_TM_ATTR_TX_COUNT, td->tx_count) || 504f0efa862SFelix Fietkau nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH, td->tx_msdu_len) || 505f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_MODE, td->tx_rate_mode) || 506f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_NSS, td->tx_rate_nss) || 507f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, td->tx_rate_idx) || 508f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_SGI, td->tx_rate_sgi) || 509f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_LDPC, td->tx_rate_ldpc) || 5107f54c742SShayne Chen nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_STBC, td->tx_rate_stbc) || 5111a38c2f5SShayne Chen (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_LTF) && 5121a38c2f5SShayne Chen nla_put_u8(msg, MT76_TM_ATTR_TX_LTF, td->tx_ltf)) || 513f0efa862SFelix Fietkau (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_ANTENNA) && 514f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, td->tx_antenna_mask)) || 515fdc9c18eSShayne Chen (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_SPE_IDX) && 516fdc9c18eSShayne Chen nla_put_u8(msg, MT76_TM_ATTR_TX_SPE_IDX, td->tx_spe_idx)) || 517*b8cbdb97SShayne Chen (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_DUTY_CYCLE) && 518*b8cbdb97SShayne Chen nla_put_u8(msg, MT76_TM_ATTR_TX_DUTY_CYCLE, td->tx_duty_cycle)) || 519*b8cbdb97SShayne Chen (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_IPG) && 520*b8cbdb97SShayne Chen nla_put_u32(msg, MT76_TM_ATTR_TX_IPG, td->tx_ipg)) || 521*b8cbdb97SShayne Chen (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_TIME) && 522*b8cbdb97SShayne Chen nla_put_u32(msg, MT76_TM_ATTR_TX_TIME, td->tx_time)) || 523f0efa862SFelix Fietkau (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER_CONTROL) && 524f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_TX_POWER_CONTROL, td->tx_power_control)) || 525f0efa862SFelix Fietkau (mt76_testmode_param_present(td, MT76_TM_ATTR_FREQ_OFFSET) && 526f0efa862SFelix Fietkau nla_put_u8(msg, MT76_TM_ATTR_FREQ_OFFSET, td->freq_offset))) 527f0efa862SFelix Fietkau goto out; 528f0efa862SFelix Fietkau 529f0efa862SFelix Fietkau if (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER)) { 530f0efa862SFelix Fietkau a = nla_nest_start(msg, MT76_TM_ATTR_TX_POWER); 531f0efa862SFelix Fietkau if (!a) 532f0efa862SFelix Fietkau goto out; 533f0efa862SFelix Fietkau 534f0efa862SFelix Fietkau for (i = 0; i < ARRAY_SIZE(td->tx_power); i++) 535f0efa862SFelix Fietkau if (nla_put_u8(msg, i, td->tx_power[i])) 536f0efa862SFelix Fietkau goto out; 537f0efa862SFelix Fietkau 538f0efa862SFelix Fietkau nla_nest_end(msg, a); 539f0efa862SFelix Fietkau } 540f0efa862SFelix Fietkau 541f0efa862SFelix Fietkau err = 0; 542f0efa862SFelix Fietkau 543f0efa862SFelix Fietkau out: 544f0efa862SFelix Fietkau mutex_unlock(&dev->mutex); 545f0efa862SFelix Fietkau 546f0efa862SFelix Fietkau return err; 547f0efa862SFelix Fietkau } 548f0efa862SFelix Fietkau EXPORT_SYMBOL(mt76_testmode_dump); 549