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 = *((__le32 *)&ccmp_pn[0]);
 202                 txwi->eiv = *((__le32 *)&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) && !(rxinfo & MT_RXINFO_NULL))
 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         }
 414
 415         if (rate & MT_RXWI_RATE_SGI)
 416                 txrate->flags |= IEEE80211_TX_RC_SHORT_GI;
 417
 418         return 0;
 419 }
 420
 421 static void
 422 mt76x2_mac_fill_tx_status(struct mt76x2_dev *dev,
 423                           struct ieee80211_tx_info *info,
 424                           struct mt76x2_tx_status *st, int n_frames)
 425 {
 426         struct ieee80211_tx_rate *rate = info->status.rates;
 427         int cur_idx, last_rate;
 428         int i;
 429
 430         if (!n_frames)
 431                 return;
 432
 433         last_rate = min_t(int, st->retry, IEEE80211_TX_MAX_RATES - 1);
 434         mt76x2_mac_process_tx_rate(&rate[last_rate], st->rate,
 435                                  dev->mt76.chandef.chan->band);
 436         if (last_rate < IEEE80211_TX_MAX_RATES - 1)
 437                 rate[last_rate + 1].idx = -1;
 438
 439         cur_idx = rate[last_rate].idx + st->retry;
 440         for (i = 0; i <= last_rate; i++) {
 441                 rate[i].flags = rate[last_rate].flags;
 442                 rate[i].idx = max_t(int, 0, cur_idx - i);
 443                 rate[i].count = 1;
 444         }
 445
 446         if (last_rate > 0)
 447                 rate[last_rate - 1].count = st->retry + 1 - last_rate;
 448
 449         info->status.ampdu_len = n_frames;
 450         info->status.ampdu_ack_len = st->success ? n_frames : 0;
 451
 452         if (st->pktid & MT_TXWI_PKTID_PROBE)
 453                 info->flags |= IEEE80211_TX_CTL_RATE_CTRL_PROBE;
 454
 455         if (st->aggr)
 456                 info->flags |= IEEE80211_TX_CTL_AMPDU |
 457                                IEEE80211_TX_STAT_AMPDU;
 458
 459         if (!st->ack_req)
 460                 info->flags |= IEEE80211_TX_CTL_NO_ACK;
 461         else if (st->success)
 462                 info->flags |= IEEE80211_TX_STAT_ACK;
 463 }
 464
 465 static void
 466 mt76x2_send_tx_status(struct mt76x2_dev *dev, struct mt76x2_tx_status *stat,
 467                       u8 *update)
 468 {
 469         struct ieee80211_tx_info info = {};
 470         struct ieee80211_sta *sta = NULL;
 471         struct mt76_wcid *wcid = NULL;
 472         struct mt76x2_sta *msta = NULL;
 473
 474         rcu_read_lock();
 475         if (stat->wcid < ARRAY_SIZE(dev->wcid))
 476                 wcid = rcu_dereference(dev->wcid[stat->wcid]);
 477
 478         if (wcid) {
 479                 void *priv;
 480
 481                 priv = msta = container_of(wcid, struct mt76x2_sta, wcid);
 482                 sta = container_of(priv, struct ieee80211_sta,
 483                                    drv_priv);
 484         }
 485
 486         if (msta && stat->aggr) {
 487                 u32 stat_val, stat_cache;
 488
 489                 stat_val = stat->rate;
 490                 stat_val |= ((u32) stat->retry) << 16;
 491                 stat_cache = msta->status.rate;
 492                 stat_cache |= ((u32) msta->status.retry) << 16;
 493
 494                 if (*update == 0 && stat_val == stat_cache &&
 495                     stat->wcid == msta->status.wcid && msta->n_frames < 32) {
 496                         msta->n_frames++;
 497                         goto out;
 498                 }
 499
 500                 mt76x2_mac_fill_tx_status(dev, &info, &msta->status,
 501                                           msta->n_frames);
 502
 503                 msta->status = *stat;
 504                 msta->n_frames = 1;
 505                 *update = 0;
 506         } else {
 507                 mt76x2_mac_fill_tx_status(dev, &info, stat, 1);
 508                 *update = 1;
 509         }
 510
 511         ieee80211_tx_status_noskb(mt76_hw(dev), sta, &info);
 512
 513 out:
 514         rcu_read_unlock();
 515 }
 516
 517 void mt76x2_mac_poll_tx_status(struct mt76x2_dev *dev, bool irq)
 518 {
 519         struct mt76x2_tx_status stat = {};
 520         unsigned long flags;
 521         u8 update = 1;
 522
 523         if (!test_bit(MT76_STATE_RUNNING, &dev->mt76.state))
 524                 return;
 525
 526         trace_mac_txstat_poll(dev);
 527
 528         while (!irq || !kfifo_is_full(&dev->txstatus_fifo)) {
 529                 u32 stat1, stat2;
 530
 531                 spin_lock_irqsave(&dev->irq_lock, flags);
 532                 stat2 = mt76_rr(dev, MT_TX_STAT_FIFO_EXT);
 533                 stat1 = mt76_rr(dev, MT_TX_STAT_FIFO);
 534                 if (!(stat1 & MT_TX_STAT_FIFO_VALID)) {
 535                         spin_unlock_irqrestore(&dev->irq_lock, flags);
 536                         break;
 537                 }
 538
 539                 spin_unlock_irqrestore(&dev->irq_lock, flags);
 540
 541                 stat.valid = 1;
 542                 stat.success = !!(stat1 & MT_TX_STAT_FIFO_SUCCESS);
 543                 stat.aggr = !!(stat1 & MT_TX_STAT_FIFO_AGGR);
 544                 stat.ack_req = !!(stat1 & MT_TX_STAT_FIFO_ACKREQ);
 545                 stat.wcid = FIELD_GET(MT_TX_STAT_FIFO_WCID, stat1);
 546                 stat.rate = FIELD_GET(MT_TX_STAT_FIFO_RATE, stat1);
 547                 stat.retry = FIELD_GET(MT_TX_STAT_FIFO_EXT_RETRY, stat2);
 548                 stat.pktid = FIELD_GET(MT_TX_STAT_FIFO_EXT_PKTID, stat2);
 549                 trace_mac_txstat_fetch(dev, &stat);
 550
 551                 if (!irq) {
 552                         mt76x2_send_tx_status(dev, &stat, &update);
 553                         continue;
 554                 }
 555
 556                 kfifo_put(&dev->txstatus_fifo, stat);
 557         }
 558 }
 559
 560 static void
 561 mt76x2_mac_queue_txdone(struct mt76x2_dev *dev, struct sk_buff *skb,
 562                         void *txwi_ptr)
 563 {
 564         struct mt76x2_tx_info *txi = mt76x2_skb_tx_info(skb);
 565         struct mt76x2_txwi *txwi = txwi_ptr;
 566
 567         mt76x2_mac_poll_tx_status(dev, false);
 568
 569         txi->tries = 0;
 570         txi->jiffies = jiffies;
 571         txi->wcid = txwi->wcid;
 572         txi->pktid = txwi->pktid;
 573         trace_mac_txdone_add(dev, txwi->wcid, txwi->pktid);
 574         mt76x2_tx_complete(dev, skb);
 575 }
 576
 577 void mt76x2_mac_process_tx_status_fifo(struct mt76x2_dev *dev)
 578 {
 579         struct mt76x2_tx_status stat;
 580         u8 update = 1;
 581
 582         while (kfifo_get(&dev->txstatus_fifo, &stat))
 583                 mt76x2_send_tx_status(dev, &stat, &update);
 584 }
 585
 586 void mt76x2_tx_complete_skb(struct mt76_dev *mdev, struct mt76_queue *q,
 587                             struct mt76_queue_entry *e, bool flush)
 588 {
 589         struct mt76x2_dev *dev = container_of(mdev, struct mt76x2_dev, mt76);
 590
 591         if (e->txwi)
 592                 mt76x2_mac_queue_txdone(dev, e->skb, &e->txwi->txwi);
 593         else
 594                 dev_kfree_skb_any(e->skb);
 595 }
 596
 597 static enum mt76x2_cipher_type
 598 mt76x2_mac_get_key_info(struct ieee80211_key_conf *key, u8 *key_data)
 599 {
 600         memset(key_data, 0, 32);
 601         if (!key)
 602                 return MT_CIPHER_NONE;
 603
 604         if (key->keylen > 32)
 605                 return MT_CIPHER_NONE;
 606
 607         memcpy(key_data, key->key, key->keylen);
 608
 609         switch (key->cipher) {
 610         case WLAN_CIPHER_SUITE_WEP40:
 611                 return MT_CIPHER_WEP40;
 612         case WLAN_CIPHER_SUITE_WEP104:
 613                 return MT_CIPHER_WEP104;
 614         case WLAN_CIPHER_SUITE_TKIP:
 615                 return MT_CIPHER_TKIP;
 616         case WLAN_CIPHER_SUITE_CCMP:
 617                 return MT_CIPHER_AES_CCMP;
 618         default:
 619                 return MT_CIPHER_NONE;
 620         }
 621 }
 622
 623 void mt76x2_mac_wcid_setup(struct mt76x2_dev *dev, u8 idx, u8 vif_idx, u8 *mac)
 624 {
 625         struct mt76_wcid_addr addr = {};
 626         u32 attr;
 627
 628         attr = FIELD_PREP(MT_WCID_ATTR_BSS_IDX, vif_idx & 7) |
 629                FIELD_PREP(MT_WCID_ATTR_BSS_IDX_EXT, !!(vif_idx & 8));
 630
 631         mt76_wr(dev, MT_WCID_ATTR(idx), attr);
 632
 633         mt76_wr(dev, MT_WCID_TX_RATE(idx), 0);
 634         mt76_wr(dev, MT_WCID_TX_RATE(idx) + 4, 0);
 635
 636         if (idx >= 128)
 637                 return;
 638
 639         if (mac)
 640                 memcpy(addr.macaddr, mac, ETH_ALEN);
 641
 642         mt76_wr_copy(dev, MT_WCID_ADDR(idx), &addr, sizeof(addr));
 643 }
 644
 645 int mt76x2_mac_wcid_set_key(struct mt76x2_dev *dev, u8 idx,
 646                             struct ieee80211_key_conf *key)
 647 {
 648         enum mt76x2_cipher_type cipher;
 649         u8 key_data[32];
 650         u8 iv_data[8];
 651
 652         cipher = mt76x2_mac_get_key_info(key, key_data);
 653         if (cipher == MT_CIPHER_NONE && key)
 654                 return -EOPNOTSUPP;
 655
 656         mt76_rmw_field(dev, MT_WCID_ATTR(idx), MT_WCID_ATTR_PKEY_MODE, cipher);
 657         mt76_wr_copy(dev, MT_WCID_KEY(idx), key_data, sizeof(key_data));
 658
 659         memset(iv_data, 0, sizeof(iv_data));
 660         if (key) {
 661                 mt76_rmw_field(dev, MT_WCID_ATTR(idx), MT_WCID_ATTR_PAIRWISE,
 662                                !!(key->flags & IEEE80211_KEY_FLAG_PAIRWISE));
 663                 iv_data[3] = key->keyidx << 6;
 664                 if (cipher >= MT_CIPHER_TKIP)
 665                         iv_data[3] |= 0x20;
 666         }
 667
 668         mt76_wr_copy(dev, MT_WCID_IV(idx), iv_data, sizeof(iv_data));
 669
 670         return 0;
 671 }
 672
 673 int mt76x2_mac_shared_key_setup(struct mt76x2_dev *dev, u8 vif_idx, u8 key_idx,
 674                               struct ieee80211_key_conf *key)
 675 {
 676         enum mt76x2_cipher_type cipher;
 677         u8 key_data[32];
 678         u32 val;
 679
 680         cipher = mt76x2_mac_get_key_info(key, key_data);
 681         if (cipher == MT_CIPHER_NONE && key)
 682                 return -EOPNOTSUPP;
 683
 684         val = mt76_rr(dev, MT_SKEY_MODE(vif_idx));
 685         val &= ~(MT_SKEY_MODE_MASK << MT_SKEY_MODE_SHIFT(vif_idx, key_idx));
 686         val |= cipher << MT_SKEY_MODE_SHIFT(vif_idx, key_idx);
 687         mt76_wr(dev, MT_SKEY_MODE(vif_idx), val);
 688
 689         mt76_wr_copy(dev, MT_SKEY(vif_idx, key_idx), key_data,
 690                      sizeof(key_data));
 691
 692         return 0;
 693 }
 694
 695 static int
 696 mt76_write_beacon(struct mt76x2_dev *dev, int offset, struct sk_buff *skb)
 697 {
 698         int beacon_len = dev->beacon_offsets[1] - dev->beacon_offsets[0];
 699         struct mt76x2_txwi txwi;
 700
 701         if (WARN_ON_ONCE(beacon_len < skb->len + sizeof(struct mt76x2_txwi)))
 702                 return -ENOSPC;
 703
 704         mt76x2_mac_write_txwi(dev, &txwi, skb, NULL, NULL);
 705
 706         mt76_wr_copy(dev, offset, &txwi, sizeof(txwi));
 707         offset += sizeof(txwi);
 708
 709         mt76_wr_copy(dev, offset, skb->data, skb->len);
 710         return 0;
 711 }
 712
 713 static int
 714 __mt76x2_mac_set_beacon(struct mt76x2_dev *dev, u8 bcn_idx, struct sk_buff *skb)
 715 {
 716         int beacon_len = dev->beacon_offsets[1] - dev->beacon_offsets[0];
 717         int beacon_addr = dev->beacon_offsets[bcn_idx];
 718         int ret = 0;
 719         int i;
 720
 721         /* Prevent corrupt transmissions during update */
 722         mt76_set(dev, MT_BCN_BYPASS_MASK, BIT(bcn_idx));
 723
 724         if (skb) {
 725                 ret = mt76_write_beacon(dev, beacon_addr, skb);
 726                 if (!ret)
 727                         dev->beacon_data_mask |= BIT(bcn_idx) &
 728                                                  dev->beacon_mask;
 729         } else {
 730                 dev->beacon_data_mask &= ~BIT(bcn_idx);
 731                 for (i = 0; i < beacon_len; i += 4)
 732                         mt76_wr(dev, beacon_addr + i, 0);
 733         }
 734
 735         mt76_wr(dev, MT_BCN_BYPASS_MASK, 0xff00 | ~dev->beacon_data_mask);
 736
 737         return ret;
 738 }
 739
 740 int mt76x2_mac_set_beacon(struct mt76x2_dev *dev, u8 vif_idx,
 741                           struct sk_buff *skb)
 742 {
 743         bool force_update = false;
 744         int bcn_idx = 0;
 745         int i;
 746
 747         for (i = 0; i < ARRAY_SIZE(dev->beacons); i++) {
 748                 if (vif_idx == i) {
 749                         force_update = !!dev->beacons[i] ^ !!skb;
 750
 751                         if (dev->beacons[i])
 752                                 dev_kfree_skb(dev->beacons[i]);
 753
 754                         dev->beacons[i] = skb;
 755                         __mt76x2_mac_set_beacon(dev, bcn_idx, skb);
 756                 } else if (force_update && dev->beacons[i]) {
 757                         __mt76x2_mac_set_beacon(dev, bcn_idx, dev->beacons[i]);
 758                 }
 759
 760                 bcn_idx += !!dev->beacons[i];
 761         }
 762
 763         for (i = bcn_idx; i < ARRAY_SIZE(dev->beacons); i++) {
 764                 if (!(dev->beacon_data_mask & BIT(i)))
 765                         break;
 766
 767                 __mt76x2_mac_set_beacon(dev, i, NULL);
 768         }
 769
 770         mt76_rmw_field(dev, MT_MAC_BSSID_DW1, MT_MAC_BSSID_DW1_MBEACON_N,
 771                        bcn_idx - 1);
 772         return 0;
 773 }
 774
 775 void mt76x2_mac_set_beacon_enable(struct mt76x2_dev *dev, u8 vif_idx, bool val)
 776 {
 777         u8 old_mask = dev->beacon_mask;
 778         bool en;
 779         u32 reg;
 780
 781         if (val) {
 782                 dev->beacon_mask |= BIT(vif_idx);
 783         } else {
 784                 dev->beacon_mask &= ~BIT(vif_idx);
 785                 mt76x2_mac_set_beacon(dev, vif_idx, NULL);
 786         }
 787
 788         if (!!old_mask == !!dev->beacon_mask)
 789                 return;
 790
 791         en = dev->beacon_mask;
 792
 793         mt76_rmw_field(dev, MT_INT_TIMER_EN, MT_INT_TIMER_EN_PRE_TBTT_EN, en);
 794         reg = MT_BEACON_TIME_CFG_BEACON_TX |
 795               MT_BEACON_TIME_CFG_TBTT_EN |
 796               MT_BEACON_TIME_CFG_TIMER_EN;
 797         mt76_rmw(dev, MT_BEACON_TIME_CFG, reg, reg * en);
 798
 799         if (en)
 800                 mt76x2_irq_enable(dev, MT_INT_PRE_TBTT | MT_INT_TBTT);
 801         else
 802                 mt76x2_irq_disable(dev, MT_INT_PRE_TBTT | MT_INT_TBTT);
 803 }
 804
 805 void mt76x2_update_channel(struct mt76_dev *mdev)
 806 {
 807         struct mt76x2_dev *dev = container_of(mdev, struct mt76x2_dev, mt76);
 808         struct mt76_channel_state *state;
 809         u32 active, busy;
 810
 811         state = mt76_channel_state(&dev->mt76, dev->mt76.chandef.chan);
 812
 813         busy = mt76_rr(dev, MT_CH_BUSY);
 814         active = busy + mt76_rr(dev, MT_CH_IDLE);
 815
 816         spin_lock_bh(&dev->mt76.cc_lock);
 817         state->cc_busy += busy;
 818         state->cc_active += active;
 819         spin_unlock_bh(&dev->mt76.cc_lock);
 820 }
 821
 822 void mt76x2_mac_work(struct work_struct *work)
 823 {
 824         struct mt76x2_dev *dev = container_of(work, struct mt76x2_dev,
 825                                             mac_work.work);
 826         int i, idx;
 827
 828         mt76x2_update_channel(&dev->mt76);
 829         for (i = 0, idx = 0; i < 16; i++) {
 830                 u32 val = mt76_rr(dev, MT_TX_AGG_CNT(i));
 831
 832                 dev->aggr_stats[idx++] += val & 0xffff;
 833                 dev->aggr_stats[idx++] += val >> 16;
 834         }
 835
 836         ieee80211_queue_delayed_work(mt76_hw(dev), &dev->mac_work,
 837                                      MT_CALIBRATE_INTERVAL);
 838 }