drivers/net/wireless/mediatek/mt76/mt76x2_mac.c

   1 /*
   2  * Copyright (C) 2016 Felix Fietkau <nbd@nbd.name>
   3  *
   4  * Permission to use, copy, modify, and/or distribute this software for any
   5  * purpose with or without fee is hereby granted, provided that the above
   6  * copyright notice and this permission notice appear in all copies.
   7  *
   8  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
   9  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
  10  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
  11  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  12  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  13  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  14  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  15  */
  16
  17 #include <linux/delay.h>
  18 #include "mt76x2.h"
  19 #include "mt76x2_mcu.h"
  20 #include "mt76x2_eeprom.h"
  21 #include "mt76x2_trace.h"
  22
  23 void mt76x2_mac_set_bssid(struct mt76x2_dev *dev, u8 idx, const u8 *addr)
  24 {
  25         idx &= 7;
  26         mt76_wr(dev, MT_MAC_APC_BSSID_L(idx), get_unaligned_le32(addr));
  27         mt76_rmw_field(dev, MT_MAC_APC_BSSID_H(idx), MT_MAC_APC_BSSID_H_ADDR,
  28                        get_unaligned_le16(addr + 4));
  29 }
  30
  31 static int
  32 mt76x2_mac_process_rate(struct mt76_rx_status *status, u16 rate)
  33 {
  34         u8 idx = FIELD_GET(MT_RXWI_RATE_INDEX, rate);
  35
  36         switch (FIELD_GET(MT_RXWI_RATE_PHY, rate)) {
  37         case MT_PHY_TYPE_OFDM:
  38                 if (idx >= 8)
  39                         idx = 0;
  40
  41                 if (status->band == NL80211_BAND_2GHZ)
  42                         idx += 4;
  43
  44                 status->rate_idx = idx;
  45                 return 0;
  46         case MT_PHY_TYPE_CCK:
  47                 if (idx >= 8) {
  48                         idx -= 8;
  49                         status->enc_flags |= RX_ENC_FLAG_SHORTPRE;
  50                 }
  51
  52                 if (idx >= 4)
  53                         idx = 0;
  54
  55                 status->rate_idx = idx;
  56                 return 0;
  57         case MT_PHY_TYPE_HT_GF:
  58                 status->enc_flags |= RX_ENC_FLAG_HT_GF;
  59                 /* fall through */
  60         case MT_PHY_TYPE_HT:
  61                 status->encoding = RX_ENC_HT;
  62                 status->rate_idx = idx;
  63                 break;
  64         case MT_PHY_TYPE_VHT:
  65                 status->encoding = RX_ENC_VHT;
  66                 status->rate_idx = FIELD_GET(MT_RATE_INDEX_VHT_IDX, idx);
  67                 status->nss = FIELD_GET(MT_RATE_INDEX_VHT_NSS, idx) + 1;
  68                 break;
  69         default:
  70                 return -EINVAL;
  71         }
  72
  73         if (rate & MT_RXWI_RATE_LDPC)
  74                 status->enc_flags |= RX_ENC_FLAG_LDPC;
  75
  76         if (rate & MT_RXWI_RATE_SGI)
  77                 status->enc_flags |= RX_ENC_FLAG_SHORT_GI;
  78
  79         if (rate & MT_RXWI_RATE_STBC)
  80                 status->enc_flags |= 1 << RX_ENC_FLAG_STBC_SHIFT;
  81
  82         switch (FIELD_GET(MT_RXWI_RATE_BW, rate)) {
  83         case MT_PHY_BW_20:
  84                 break;
  85         case MT_PHY_BW_40:
  86                 status->bw = RATE_INFO_BW_40;
  87                 break;
  88         case MT_PHY_BW_80:
  89                 status->bw = RATE_INFO_BW_80;
  90                 break;
  91         default:
  92                 break;
  93         }
  94
  95         return 0;
  96 }
  97
  98 static __le16
  99 mt76x2_mac_tx_rate_val(struct mt76x2_dev *dev,
 100                        const struct ieee80211_tx_rate *rate, u8 *nss_val)
 101 {
 102         u16 rateval;
 103         u8 phy, rate_idx;
 104         u8 nss = 1;
 105         u8 bw = 0;
 106
 107         if (rate->flags & IEEE80211_TX_RC_VHT_MCS) {
 108                 rate_idx = rate->idx;
 109                 nss = 1 + (rate->idx >> 4);
 110                 phy = MT_PHY_TYPE_VHT;
 111                 if (rate->flags & IEEE80211_TX_RC_80_MHZ_WIDTH)
 112                         bw = 2;
 113                 else if (rate->flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
 114                         bw = 1;
 115         } else if (rate->flags & IEEE80211_TX_RC_MCS) {
 116                 rate_idx = rate->idx;
 117                 nss = 1 + (rate->idx >> 3);
 118                 phy = MT_PHY_TYPE_HT;
 119                 if (rate->flags & IEEE80211_TX_RC_GREEN_FIELD)
 120                         phy = MT_PHY_TYPE_HT_GF;
 121                 if (rate->flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
 122                         bw = 1;
 123         } else {
 124                 const struct ieee80211_rate *r;
 125                 int band = dev->mt76.chandef.chan->band;
 126                 u16 val;
 127
 128                 r = &mt76_hw(dev)->wiphy->bands[band]->bitrates[rate->idx];
 129                 if (rate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
 130                         val = r->hw_value_short;
 131                 else
 132                         val = r->hw_value;
 133
 134                 phy = val >> 8;
 135                 rate_idx = val & 0xff;
 136                 bw = 0;
 137         }
 138
 139         rateval = FIELD_PREP(MT_RXWI_RATE_INDEX, rate_idx);
 140         rateval |= FIELD_PREP(MT_RXWI_RATE_PHY, phy);
 141         rateval |= FIELD_PREP(MT_RXWI_RATE_BW, bw);
 142         if (rate->flags & IEEE80211_TX_RC_SHORT_GI)
 143                 rateval |= MT_RXWI_RATE_SGI;
 144
 145         *nss_val = nss;
 146         return cpu_to_le16(rateval);
 147 }
 148
 149 void mt76x2_mac_wcid_set_drop(struct mt76x2_dev *dev, u8 idx, bool drop)
 150 {
 151         u32 val = mt76_rr(dev, MT_WCID_DROP(idx));
 152         u32 bit = MT_WCID_DROP_MASK(idx);
 153
 154         /* prevent unnecessary writes */
 155         if ((val & bit) != (bit * drop))
 156                 mt76_wr(dev, MT_WCID_DROP(idx), (val & ~bit) | (bit * drop));
 157 }
 158
 159 void mt76x2_mac_wcid_set_rate(struct mt76x2_dev *dev, struct mt76_wcid *wcid,
 160                               const struct ieee80211_tx_rate *rate)
 161 {
 162         spin_lock_bh(&dev->mt76.lock);
 163         wcid->tx_rate = mt76x2_mac_tx_rate_val(dev, rate, &wcid->tx_rate_nss);
 164         wcid->tx_rate_set = true;
 165         spin_unlock_bh(&dev->mt76.lock);
 166 }
 167
 168 void mt76x2_mac_write_txwi(struct mt76x2_dev *dev, struct mt76x2_txwi *txwi,
 169                            struct sk_buff *skb, struct mt76_wcid *wcid,
 170                            struct ieee80211_sta *sta)
 171 {
 172         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 173         struct ieee80211_tx_rate *rate = &info->control.rates[0];
 174         struct ieee80211_key_conf *key = info->control.hw_key;
 175         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 176         u16 rate_ht_mask = FIELD_PREP(MT_RXWI_RATE_PHY, BIT(1) | BIT(2));
 177         u16 txwi_flags = 0;
 178         u8 nss;
 179         s8 txpwr_adj, max_txpwr_adj;
 180         u8 ccmp_pn[8];
 181
 182         memset(txwi, 0, sizeof(*txwi));
 183
 184         if (wcid)
 185                 txwi->wcid = wcid->idx;
 186         else
 187                 txwi->wcid = 0xff;
 188
 189         txwi->pktid = 1;
 190
 191         if (wcid && wcid->sw_iv && key) {
 192                 u64 pn = atomic64_inc_return(&key->tx_pn);
 193                 ccmp_pn[0] = pn;
 194                 ccmp_pn[1] = pn >> 8;
 195                 ccmp_pn[2] = 0;
 196                 ccmp_pn[3] = 0x20 | (key->keyidx << 6);
 197                 ccmp_pn[4] = pn >> 16;
 198                 ccmp_pn[5] = pn >> 24;
 199                 ccmp_pn[6] = pn >> 32;
 200                 ccmp_pn[7] = pn >> 40;
 201                 txwi->iv = *((u32 *) &ccmp_pn[0]);
 202                 txwi->eiv = *((u32 *) &ccmp_pn[1]);
 203         }
 204
 205         spin_lock_bh(&dev->mt76.lock);
 206         if (wcid && (rate->idx < 0 || !rate->count)) {
 207                 txwi->rate = wcid->tx_rate;
 208                 max_txpwr_adj = wcid->max_txpwr_adj;
 209                 nss = wcid->tx_rate_nss;
 210         } else {
 211                 txwi->rate = mt76x2_mac_tx_rate_val(dev, rate, &nss);
 212                 max_txpwr_adj = mt76x2_tx_get_max_txpwr_adj(dev, rate);
 213         }
 214         spin_unlock_bh(&dev->mt76.lock);
 215
 216         txpwr_adj = mt76x2_tx_get_txpwr_adj(dev, dev->txpower_conf,
 217                                             max_txpwr_adj);
 218         txwi->ctl2 = FIELD_PREP(MT_TX_PWR_ADJ, txpwr_adj);
 219
 220         if (mt76xx_rev(dev) >= MT76XX_REV_E4)
 221                 txwi->txstream = 0x13;
 222         else if (mt76xx_rev(dev) >= MT76XX_REV_E3 &&
 223                  !(txwi->rate & cpu_to_le16(rate_ht_mask)))
 224                 txwi->txstream = 0x93;
 225
 226         if (info->flags & IEEE80211_TX_CTL_LDPC)
 227                 txwi->rate |= cpu_to_le16(MT_RXWI_RATE_LDPC);
 228         if ((info->flags & IEEE80211_TX_CTL_STBC) && nss == 1)
 229                 txwi->rate |= cpu_to_le16(MT_RXWI_RATE_STBC);
 230         if (nss > 1 && sta && sta->smps_mode == IEEE80211_SMPS_DYNAMIC)
 231                 txwi_flags |= MT_TXWI_FLAGS_MMPS;
 232         if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
 233                 txwi->ack_ctl |= MT_TXWI_ACK_CTL_REQ;
 234         if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ)
 235                 txwi->ack_ctl |= MT_TXWI_ACK_CTL_NSEQ;
 236         if (info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE)
 237                 txwi->pktid |= MT_TXWI_PKTID_PROBE;
 238         if ((info->flags & IEEE80211_TX_CTL_AMPDU) && sta) {
 239                 u8 ba_size = IEEE80211_MIN_AMPDU_BUF;
 240
 241                 ba_size <<= sta->ht_cap.ampdu_factor;
 242                 ba_size = min_t(int, 63, ba_size - 1);
 243                 if (info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE)
 244                         ba_size = 0;
 245                 txwi->ack_ctl |= FIELD_PREP(MT_TXWI_ACK_CTL_BA_WINDOW, ba_size);
 246
 247                 txwi_flags |= MT_TXWI_FLAGS_AMPDU |
 248                          FIELD_PREP(MT_TXWI_FLAGS_MPDU_DENSITY,
 249                                     sta->ht_cap.ampdu_density);
 250         }
 251
 252         if (ieee80211_is_probe_resp(hdr->frame_control) ||
 253             ieee80211_is_beacon(hdr->frame_control))
 254                 txwi_flags |= MT_TXWI_FLAGS_TS;
 255
 256         txwi->flags |= cpu_to_le16(txwi_flags);
 257         txwi->len_ctl = cpu_to_le16(skb->len);
 258 }
 259
 260 static void mt76x2_remove_hdr_pad(struct sk_buff *skb, int len)
 261 {
 262         int hdrlen;
 263
 264         if (!len)
 265                 return;
 266
 267         hdrlen = ieee80211_get_hdrlen_from_skb(skb);
 268         memmove(skb->data + len, skb->data, hdrlen);
 269         skb_pull(skb, len);
 270 }
 271
 272 static struct mt76_wcid *
 273 mt76x2_rx_get_sta_wcid(struct mt76x2_dev *dev, u8 idx, bool unicast)
 274 {
 275         struct mt76x2_sta *sta;
 276         struct mt76_wcid *wcid;
 277
 278         if (idx >= ARRAY_SIZE(dev->wcid))
 279                 return NULL;
 280
 281         wcid = rcu_dereference(dev->wcid[idx]);
 282         if (unicast || !wcid)
 283                 return wcid;
 284
 285         sta = container_of(wcid, struct mt76x2_sta, wcid);
 286         return &sta->vif->group_wcid;
 287 }
 288
 289 int mt76x2_mac_process_rx(struct mt76x2_dev *dev, struct sk_buff *skb,
 290                           void *rxi)
 291 {
 292         struct mt76_rx_status *status = (struct mt76_rx_status *) skb->cb;
 293         struct mt76x2_rxwi *rxwi = rxi;
 294         u32 rxinfo = le32_to_cpu(rxwi->rxinfo);
 295         u32 ctl = le32_to_cpu(rxwi->ctl);
 296         u16 rate = le16_to_cpu(rxwi->rate);
 297         u16 tid_sn = le16_to_cpu(rxwi->tid_sn);
 298         bool unicast = rxwi->rxinfo & cpu_to_le32(MT_RXINFO_UNICAST);
 299         int pad_len = 0;
 300         u8 pn_len;
 301         u8 wcid;
 302         int len;
 303
 304         if (rxinfo & MT_RXINFO_L2PAD)
 305                 pad_len += 2;
 306
 307         if (rxinfo & MT_RXINFO_DECRYPT) {
 308                 status->flag |= RX_FLAG_DECRYPTED;
 309                 status->flag |= RX_FLAG_MMIC_STRIPPED;
 310                 status->flag |= RX_FLAG_MIC_STRIPPED;
 311                 status->flag |= RX_FLAG_IV_STRIPPED;
 312         }
 313
 314         wcid = FIELD_GET(MT_RXWI_CTL_WCID, ctl);
 315         status->wcid = mt76x2_rx_get_sta_wcid(dev, wcid, unicast);
 316
 317         len = FIELD_GET(MT_RXWI_CTL_MPDU_LEN, ctl);
 318         pn_len = FIELD_GET(MT_RXINFO_PN_LEN, rxinfo);
 319         if (pn_len) {
 320                 int offset = ieee80211_get_hdrlen_from_skb(skb) + pad_len;
 321                 u8 *data = skb->data + offset;
 322
 323                 status->iv[0] = data[7];
 324                 status->iv[1] = data[6];
 325                 status->iv[2] = data[5];
 326                 status->iv[3] = data[4];
 327                 status->iv[4] = data[1];
 328                 status->iv[5] = data[0];
 329
 330                 /*
 331                  * Driver CCMP validation can't deal with fragments.
 332                  * Let mac80211 take care of it.
 333                  */
 334                 if (rxinfo & MT_RXINFO_FRAG) {
 335                         status->flag &= ~RX_FLAG_IV_STRIPPED;
 336                 } else {
 337                         pad_len += pn_len << 2;
 338                         len -= pn_len << 2;
 339                 }
 340         }
 341
 342         mt76x2_remove_hdr_pad(skb, pad_len);
 343
 344         if (rxinfo & MT_RXINFO_BA)
 345                 status->aggr = true;
 346
 347         if (WARN_ON_ONCE(len > skb->len))
 348                 return -EINVAL;
 349
 350         pskb_trim(skb, len);
 351         status->chains = BIT(0) | BIT(1);
 352         status->chain_signal[0] = mt76x2_phy_get_rssi(dev, rxwi->rssi[0], 0);
 353         status->chain_signal[1] = mt76x2_phy_get_rssi(dev, rxwi->rssi[1], 1);
 354         status->signal = max(status->chain_signal[0], status->chain_signal[1]);
 355         status->freq = dev->mt76.chandef.chan->center_freq;
 356         status->band = dev->mt76.chandef.chan->band;
 357
 358         status->tid = FIELD_GET(MT_RXWI_TID, tid_sn);
 359         status->seqno = FIELD_GET(MT_RXWI_SN, tid_sn);
 360
 361         return mt76x2_mac_process_rate(status, rate);
 362 }
 363
 364 static int
 365 mt76x2_mac_process_tx_rate(struct ieee80211_tx_rate *txrate, u16 rate,
 366                            enum nl80211_band band)
 367 {
 368         u8 idx = FIELD_GET(MT_RXWI_RATE_INDEX, rate);
 369
 370         txrate->idx = 0;
 371         txrate->flags = 0;
 372         txrate->count = 1;
 373
 374         switch (FIELD_GET(MT_RXWI_RATE_PHY, rate)) {
 375         case MT_PHY_TYPE_OFDM:
 376                 if (band == NL80211_BAND_2GHZ)
 377                         idx += 4;
 378
 379                 txrate->idx = idx;
 380                 return 0;
 381         case MT_PHY_TYPE_CCK:
 382                 if (idx >= 8)
 383                         idx -= 8;
 384
 385                 txrate->idx = idx;
 386                 return 0;
 387         case MT_PHY_TYPE_HT_GF:
 388                 txrate->flags |= IEEE80211_TX_RC_GREEN_FIELD;
 389                 /* fall through */
 390         case MT_PHY_TYPE_HT:
 391                 txrate->flags |= IEEE80211_TX_RC_MCS;
 392                 txrate->idx = idx;
 393                 break;
 394         case MT_PHY_TYPE_VHT:
 395                 txrate->flags |= IEEE80211_TX_RC_VHT_MCS;
 396                 txrate->idx = idx;
 397                 break;
 398         default:
 399                 return -EINVAL;
 400         }
 401
 402         switch (FIELD_GET(MT_RXWI_RATE_BW, rate)) {
 403         case MT_PHY_BW_20:
 404                 break;
 405         case MT_PHY_BW_40:
 406                 txrate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
 407                 break;
 408         case MT_PHY_BW_80:
 409                 txrate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
 410                 break;
 411         default:
 412                 return -EINVAL;
 413                 break;
 414         }
 415
 416         if (rate & MT_RXWI_RATE_SGI)
 417                 txrate->flags |= IEEE80211_TX_RC_SHORT_GI;
 418
 419         return 0;
 420 }
 421
 422 static void
 423 mt76x2_mac_fill_tx_status(struct mt76x2_dev *dev,
 424                           struct ieee80211_tx_info *info,
 425                           struct mt76x2_tx_status *st, int n_frames)
 426 {
 427         struct ieee80211_tx_rate *rate = info->status.rates;
 428         int cur_idx, last_rate;
 429         int i;
 430
 431         if (!n_frames)
 432                 return;
 433
 434         last_rate = min_t(int, st->retry, IEEE80211_TX_MAX_RATES - 1);
 435         mt76x2_mac_process_tx_rate(&rate[last_rate], st->rate,
 436                                  dev->mt76.chandef.chan->band);
 437         if (last_rate < IEEE80211_TX_MAX_RATES - 1)
 438                 rate[last_rate + 1].idx = -1;
 439
 440         cur_idx = rate[last_rate].idx + st->retry;
 441         for (i = 0; i <= last_rate; i++) {
 442                 rate[i].flags = rate[last_rate].flags;
 443                 rate[i].idx = max_t(int, 0, cur_idx - i);
 444                 rate[i].count = 1;
 445         }
 446
 447         if (last_rate > 0)
 448                 rate[last_rate - 1].count = st->retry + 1 - last_rate;
 449
 450         info->status.ampdu_len = n_frames;
 451         info->status.ampdu_ack_len = st->success ? n_frames : 0;
 452
 453         if (st->pktid & MT_TXWI_PKTID_PROBE)
 454                 info->flags |= IEEE80211_TX_CTL_RATE_CTRL_PROBE;
 455
 456         if (st->aggr)
 457                 info->flags |= IEEE80211_TX_CTL_AMPDU |
 458                                IEEE80211_TX_STAT_AMPDU;
 459
 460         if (!st->ack_req)
 461                 info->flags |= IEEE80211_TX_CTL_NO_ACK;
 462         else if (st->success)
 463                 info->flags |= IEEE80211_TX_STAT_ACK;
 464 }
 465
 466 static void
 467 mt76x2_send_tx_status(struct mt76x2_dev *dev, struct mt76x2_tx_status *stat,
 468                       u8 *update)
 469 {
 470         struct ieee80211_tx_info info = {};
 471         struct ieee80211_sta *sta = NULL;
 472         struct mt76_wcid *wcid = NULL;
 473         struct mt76x2_sta *msta = NULL;
 474
 475         rcu_read_lock();
 476         if (stat->wcid < ARRAY_SIZE(dev->wcid))
 477                 wcid = rcu_dereference(dev->wcid[stat->wcid]);
 478
 479         if (wcid) {
 480                 void *priv;
 481
 482                 priv = msta = container_of(wcid, struct mt76x2_sta, wcid);
 483                 sta = container_of(priv, struct ieee80211_sta,
 484                                    drv_priv);
 485         }
 486
 487         if (msta && stat->aggr) {
 488                 u32 stat_val, stat_cache;
 489
 490                 stat_val = stat->rate;
 491                 stat_val |= ((u32) stat->retry) << 16;
 492                 stat_cache = msta->status.rate;
 493                 stat_cache |= ((u32) msta->status.retry) << 16;
 494
 495                 if (*update == 0 && stat_val == stat_cache &&
 496                     stat->wcid == msta->status.wcid && msta->n_frames < 32) {
 497                         msta->n_frames++;
 498                         goto out;
 499                 }
 500
 501                 mt76x2_mac_fill_tx_status(dev, &info, &msta->status,
 502                                           msta->n_frames);
 503
 504                 msta->status = *stat;
 505                 msta->n_frames = 1;
 506                 *update = 0;
 507         } else {
 508                 mt76x2_mac_fill_tx_status(dev, &info, stat, 1);
 509                 *update = 1;
 510         }
 511
 512         ieee80211_tx_status_noskb(mt76_hw(dev), sta, &info);
 513
 514 out:
 515         rcu_read_unlock();
 516 }
 517
 518 void mt76x2_mac_poll_tx_status(struct mt76x2_dev *dev, bool irq)
 519 {
 520         struct mt76x2_tx_status stat = {};
 521         unsigned long flags;
 522         u8 update = 1;
 523
 524         if (!test_bit(MT76_STATE_RUNNING, &dev->mt76.state))
 525                 return;
 526
 527         trace_mac_txstat_poll(dev);
 528
 529         while (!irq || !kfifo_is_full(&dev->txstatus_fifo)) {
 530                 u32 stat1, stat2;
 531
 532                 spin_lock_irqsave(&dev->irq_lock, flags);
 533                 stat2 = mt76_rr(dev, MT_TX_STAT_FIFO_EXT);
 534                 stat1 = mt76_rr(dev, MT_TX_STAT_FIFO);
 535                 if (!(stat1 & MT_TX_STAT_FIFO_VALID)) {
 536                         spin_unlock_irqrestore(&dev->irq_lock, flags);
 537                         break;
 538                 }
 539
 540                 spin_unlock_irqrestore(&dev->irq_lock, flags);
 541
 542                 stat.valid = 1;
 543                 stat.success = !!(stat1 & MT_TX_STAT_FIFO_SUCCESS);
 544                 stat.aggr = !!(stat1 & MT_TX_STAT_FIFO_AGGR);
 545                 stat.ack_req = !!(stat1 & MT_TX_STAT_FIFO_ACKREQ);
 546                 stat.wcid = FIELD_GET(MT_TX_STAT_FIFO_WCID, stat1);
 547                 stat.rate = FIELD_GET(MT_TX_STAT_FIFO_RATE, stat1);
 548                 stat.retry = FIELD_GET(MT_TX_STAT_FIFO_EXT_RETRY, stat2);
 549                 stat.pktid = FIELD_GET(MT_TX_STAT_FIFO_EXT_PKTID, stat2);
 550                 trace_mac_txstat_fetch(dev, &stat);
 551
 552                 if (!irq) {
 553                         mt76x2_send_tx_status(dev, &stat, &update);
 554                         continue;
 555                 }
 556
 557                 kfifo_put(&dev->txstatus_fifo, stat);
 558         }
 559 }
 560
 561 static void
 562 mt76x2_mac_queue_txdone(struct mt76x2_dev *dev, struct sk_buff *skb,
 563                         void *txwi_ptr)
 564 {
 565         struct mt76x2_tx_info *txi = mt76x2_skb_tx_info(skb);
 566         struct mt76x2_txwi *txwi = txwi_ptr;
 567
 568         mt76x2_mac_poll_tx_status(dev, false);
 569
 570         txi->tries = 0;
 571         txi->jiffies = jiffies;
 572         txi->wcid = txwi->wcid;
 573         txi->pktid = txwi->pktid;
 574         trace_mac_txdone_add(dev, txwi->wcid, txwi->pktid);
 575         mt76x2_tx_complete(dev, skb);
 576 }
 577
 578 void mt76x2_mac_process_tx_status_fifo(struct mt76x2_dev *dev)
 579 {
 580         struct mt76x2_tx_status stat;
 581         u8 update = 1;
 582
 583         while (kfifo_get(&dev->txstatus_fifo, &stat))
 584                 mt76x2_send_tx_status(dev, &stat, &update);
 585 }
 586
 587 void mt76x2_tx_complete_skb(struct mt76_dev *mdev, struct mt76_queue *q,
 588                             struct mt76_queue_entry *e, bool flush)
 589 {
 590         struct mt76x2_dev *dev = container_of(mdev, struct mt76x2_dev, mt76);
 591
 592         if (e->txwi)
 593                 mt76x2_mac_queue_txdone(dev, e->skb, &e->txwi->txwi);
 594         else
 595                 dev_kfree_skb_any(e->skb);
 596 }
 597
 598 static enum mt76x2_cipher_type
 599 mt76x2_mac_get_key_info(struct ieee80211_key_conf *key, u8 *key_data)
 600 {
 601         memset(key_data, 0, 32);
 602         if (!key)
 603                 return MT_CIPHER_NONE;
 604
 605         if (key->keylen > 32)
 606                 return MT_CIPHER_NONE;
 607
 608         memcpy(key_data, key->key, key->keylen);
 609
 610         switch (key->cipher) {
 611         case WLAN_CIPHER_SUITE_WEP40:
 612                 return MT_CIPHER_WEP40;
 613         case WLAN_CIPHER_SUITE_WEP104:
 614                 return MT_CIPHER_WEP104;
 615         case WLAN_CIPHER_SUITE_TKIP:
 616                 return MT_CIPHER_TKIP;
 617         case WLAN_CIPHER_SUITE_CCMP:
 618                 return MT_CIPHER_AES_CCMP;
 619         default:
 620                 return MT_CIPHER_NONE;
 621         }
 622 }
 623
 624 void mt76x2_mac_wcid_setup(struct mt76x2_dev *dev, u8 idx, u8 vif_idx, u8 *mac)
 625 {
 626         struct mt76_wcid_addr addr = {};
 627         u32 attr;
 628
 629         attr = FIELD_PREP(MT_WCID_ATTR_BSS_IDX, vif_idx & 7) |
 630                FIELD_PREP(MT_WCID_ATTR_BSS_IDX_EXT, !!(vif_idx & 8));
 631
 632         mt76_wr(dev, MT_WCID_ATTR(idx), attr);
 633
 634         mt76_wr(dev, MT_WCID_TX_RATE(idx), 0);
 635         mt76_wr(dev, MT_WCID_TX_RATE(idx) + 4, 0);
 636
 637         if (idx >= 128)
 638                 return;
 639
 640         if (mac)
 641                 memcpy(addr.macaddr, mac, ETH_ALEN);
 642
 643         mt76_wr_copy(dev, MT_WCID_ADDR(idx), &addr, sizeof(addr));
 644 }
 645
 646 int mt76x2_mac_wcid_set_key(struct mt76x2_dev *dev, u8 idx,
 647                             struct ieee80211_key_conf *key)
 648 {
 649         enum mt76x2_cipher_type cipher;
 650         u8 key_data[32];
 651         u8 iv_data[8];
 652
 653         cipher = mt76x2_mac_get_key_info(key, key_data);
 654         if (cipher == MT_CIPHER_NONE && key)
 655                 return -EOPNOTSUPP;
 656
 657         mt76_rmw_field(dev, MT_WCID_ATTR(idx), MT_WCID_ATTR_PKEY_MODE, cipher);
 658         mt76_wr_copy(dev, MT_WCID_KEY(idx), key_data, sizeof(key_data));
 659
 660         memset(iv_data, 0, sizeof(iv_data));
 661         if (key) {
 662                 mt76_rmw_field(dev, MT_WCID_ATTR(idx), MT_WCID_ATTR_PAIRWISE,
 663                                !!(key->flags & IEEE80211_KEY_FLAG_PAIRWISE));
 664                 iv_data[3] = key->keyidx << 6;
 665                 if (cipher >= MT_CIPHER_TKIP)
 666                         iv_data[3] |= 0x20;
 667         }
 668
 669         mt76_wr_copy(dev, MT_WCID_IV(idx), iv_data, sizeof(iv_data));
 670
 671         return 0;
 672 }
 673
 674 int mt76x2_mac_shared_key_setup(struct mt76x2_dev *dev, u8 vif_idx, u8 key_idx,
 675                               struct ieee80211_key_conf *key)
 676 {
 677         enum mt76x2_cipher_type cipher;
 678         u8 key_data[32];
 679         u32 val;
 680
 681         cipher = mt76x2_mac_get_key_info(key, key_data);
 682         if (cipher == MT_CIPHER_NONE && key)
 683                 return -EOPNOTSUPP;
 684
 685         val = mt76_rr(dev, MT_SKEY_MODE(vif_idx));
 686         val &= ~(MT_SKEY_MODE_MASK << MT_SKEY_MODE_SHIFT(vif_idx, key_idx));
 687         val |= cipher << MT_SKEY_MODE_SHIFT(vif_idx, key_idx);
 688         mt76_wr(dev, MT_SKEY_MODE(vif_idx), val);
 689
 690         mt76_wr_copy(dev, MT_SKEY(vif_idx, key_idx), key_data,
 691                      sizeof(key_data));
 692
 693         return 0;
 694 }
 695
 696 static int
 697 mt76_write_beacon(struct mt76x2_dev *dev, int offset, struct sk_buff *skb)
 698 {
 699         int beacon_len = dev->beacon_offsets[1] - dev->beacon_offsets[0];
 700         struct mt76x2_txwi txwi;
 701
 702         if (WARN_ON_ONCE(beacon_len < skb->len + sizeof(struct mt76x2_txwi)))
 703                 return -ENOSPC;
 704
 705         mt76x2_mac_write_txwi(dev, &txwi, skb, NULL, NULL);
 706
 707         mt76_wr_copy(dev, offset, &txwi, sizeof(txwi));
 708         offset += sizeof(txwi);
 709
 710         mt76_wr_copy(dev, offset, skb->data, skb->len);
 711         return 0;
 712 }
 713
 714 static int
 715 __mt76x2_mac_set_beacon(struct mt76x2_dev *dev, u8 bcn_idx, struct sk_buff *skb)
 716 {
 717         int beacon_len = dev->beacon_offsets[1] - dev->beacon_offsets[0];
 718         int beacon_addr = dev->beacon_offsets[bcn_idx];
 719         int ret = 0;
 720         int i;
 721
 722         /* Prevent corrupt transmissions during update */
 723         mt76_set(dev, MT_BCN_BYPASS_MASK, BIT(bcn_idx));
 724
 725         if (skb) {
 726                 ret = mt76_write_beacon(dev, beacon_addr, skb);
 727                 if (!ret)
 728                         dev->beacon_data_mask |= BIT(bcn_idx) &
 729                                                  dev->beacon_mask;
 730         } else {
 731                 dev->beacon_data_mask &= ~BIT(bcn_idx);
 732                 for (i = 0; i < beacon_len; i += 4)
 733                         mt76_wr(dev, beacon_addr + i, 0);
 734         }
 735
 736         mt76_wr(dev, MT_BCN_BYPASS_MASK, 0xff00 | ~dev->beacon_data_mask);
 737
 738         return ret;
 739 }
 740
 741 int mt76x2_mac_set_beacon(struct mt76x2_dev *dev, u8 vif_idx,
 742                           struct sk_buff *skb)
 743 {
 744         bool force_update = false;
 745         int bcn_idx = 0;
 746         int i;
 747
 748         for (i = 0; i < ARRAY_SIZE(dev->beacons); i++) {
 749                 if (vif_idx == i) {
 750                         force_update = !!dev->beacons[i] ^ !!skb;
 751
 752                         if (dev->beacons[i])
 753                                 dev_kfree_skb(dev->beacons[i]);
 754
 755                         dev->beacons[i] = skb;
 756                         __mt76x2_mac_set_beacon(dev, bcn_idx, skb);
 757                 } else if (force_update && dev->beacons[i]) {
 758                         __mt76x2_mac_set_beacon(dev, bcn_idx, dev->beacons[i]);
 759                 }
 760
 761                 bcn_idx += !!dev->beacons[i];
 762         }
 763
 764         for (i = bcn_idx; i < ARRAY_SIZE(dev->beacons); i++) {
 765                 if (!(dev->beacon_data_mask & BIT(i)))
 766                         break;
 767
 768                 __mt76x2_mac_set_beacon(dev, i, NULL);
 769         }
 770
 771         mt76_rmw_field(dev, MT_MAC_BSSID_DW1, MT_MAC_BSSID_DW1_MBEACON_N,
 772                        bcn_idx - 1);
 773         return 0;
 774 }
 775
 776 void mt76x2_mac_set_beacon_enable(struct mt76x2_dev *dev, u8 vif_idx, bool val)
 777 {
 778         u8 old_mask = dev->beacon_mask;
 779         bool en;
 780         u32 reg;
 781
 782         if (val) {
 783                 dev->beacon_mask |= BIT(vif_idx);
 784         } else {
 785                 dev->beacon_mask &= ~BIT(vif_idx);
 786                 mt76x2_mac_set_beacon(dev, vif_idx, NULL);
 787         }
 788
 789         if (!!old_mask == !!dev->beacon_mask)
 790                 return;
 791
 792         en = dev->beacon_mask;
 793
 794         mt76_rmw_field(dev, MT_INT_TIMER_EN, MT_INT_TIMER_EN_PRE_TBTT_EN, en);
 795         reg = MT_BEACON_TIME_CFG_BEACON_TX |
 796               MT_BEACON_TIME_CFG_TBTT_EN |
 797               MT_BEACON_TIME_CFG_TIMER_EN;
 798         mt76_rmw(dev, MT_BEACON_TIME_CFG, reg, reg * en);
 799
 800         if (en)
 801                 mt76x2_irq_enable(dev, MT_INT_PRE_TBTT | MT_INT_TBTT);
 802         else
 803                 mt76x2_irq_disable(dev, MT_INT_PRE_TBTT | MT_INT_TBTT);
 804 }
 805
 806 void mt76x2_update_channel(struct mt76_dev *mdev)
 807 {
 808         struct mt76x2_dev *dev = container_of(mdev, struct mt76x2_dev, mt76);
 809         struct mt76_channel_state *state;
 810         u32 active, busy;
 811
 812         state = mt76_channel_state(&dev->mt76, dev->mt76.chandef.chan);
 813
 814         busy = mt76_rr(dev, MT_CH_BUSY);
 815         active = busy + mt76_rr(dev, MT_CH_IDLE);
 816
 817         spin_lock_bh(&dev->mt76.cc_lock);
 818         state->cc_busy += busy;
 819         state->cc_active += active;
 820         spin_unlock_bh(&dev->mt76.cc_lock);
 821 }
 822
 823 void mt76x2_mac_work(struct work_struct *work)
 824 {
 825         struct mt76x2_dev *dev = container_of(work, struct mt76x2_dev,
 826                                             mac_work.work);
 827         int i, idx;
 828
 829         mt76x2_update_channel(&dev->mt76);
 830         for (i = 0, idx = 0; i < 16; i++) {
 831                 u32 val = mt76_rr(dev, MT_TX_AGG_CNT(i));
 832
 833                 dev->aggr_stats[idx++] += val & 0xffff;
 834                 dev->aggr_stats[idx++] += val >> 16;
 835         }
 836
 837         ieee80211_queue_delayed_work(mt76_hw(dev), &dev->mac_work,
 838                                      MT_CALIBRATE_INTERVAL);
 839 }