drivers/net/wireless/mediatek/mt76/mt7915/mac.c

   1 // SPDX-License-Identifier: ISC
   2 /* Copyright (C) 2020 MediaTek Inc. */
   3
   4 #include <linux/etherdevice.h>
   5 #include <linux/timekeeping.h>
   6 #include "mt7915.h"
   7 #include "../dma.h"
   8 #include "mac.h"
   9 #include "mcu.h"
  10
  11 #define to_rssi(field, rxv)     ((FIELD_GET(field, rxv) - 220) / 2)
  12
  13 #define HE_BITS(f)              cpu_to_le16(IEEE80211_RADIOTAP_HE_##f)
  14 #define HE_PREP(f, m, v)        le16_encode_bits(le32_get_bits(v, MT_CRXV_HE_##m),\
  15                                                  IEEE80211_RADIOTAP_HE_##f)
  16
  17 static const struct mt7915_dfs_radar_spec etsi_radar_specs = {
  18         .pulse_th = { 110, -10, -80, 40, 5200, 128, 5200 },
  19         .radar_pattern = {
  20                 [5] =  { 1, 0,  6, 32, 28, 0,  990, 5010, 17, 1, 1 },
  21                 [6] =  { 1, 0,  9, 32, 28, 0,  615, 5010, 27, 1, 1 },
  22                 [7] =  { 1, 0, 15, 32, 28, 0,  240,  445, 27, 1, 1 },
  23                 [8] =  { 1, 0, 12, 32, 28, 0,  240,  510, 42, 1, 1 },
  24                 [9] =  { 1, 1,  0,  0,  0, 0, 2490, 3343, 14, 0, 0, 12, 32, 28, { }, 126 },
  25                 [10] = { 1, 1,  0,  0,  0, 0, 2490, 3343, 14, 0, 0, 15, 32, 24, { }, 126 },
  26                 [11] = { 1, 1,  0,  0,  0, 0,  823, 2510, 14, 0, 0, 18, 32, 28, { },  54 },
  27                 [12] = { 1, 1,  0,  0,  0, 0,  823, 2510, 14, 0, 0, 27, 32, 24, { },  54 },
  28         },
  29 };
  30
  31 static const struct mt7915_dfs_radar_spec fcc_radar_specs = {
  32         .pulse_th = { 110, -10, -80, 40, 5200, 128, 5200 },
  33         .radar_pattern = {
  34                 [0] = { 1, 0,  8,  32, 28, 0, 508, 3076, 13, 1,  1 },
  35                 [1] = { 1, 0, 12,  32, 28, 0, 140,  240, 17, 1,  1 },
  36                 [2] = { 1, 0,  8,  32, 28, 0, 190,  510, 22, 1,  1 },
  37                 [3] = { 1, 0,  6,  32, 28, 0, 190,  510, 32, 1,  1 },
  38                 [4] = { 1, 0,  9, 255, 28, 0, 323,  343, 13, 1, 32 },
  39         },
  40 };
  41
  42 static const struct mt7915_dfs_radar_spec jp_radar_specs = {
  43         .pulse_th = { 110, -10, -80, 40, 5200, 128, 5200 },
  44         .radar_pattern = {
  45                 [0] =  { 1, 0,  8,  32, 28, 0,  508, 3076,  13, 1,  1 },
  46                 [1] =  { 1, 0, 12,  32, 28, 0,  140,  240,  17, 1,  1 },
  47                 [2] =  { 1, 0,  8,  32, 28, 0,  190,  510,  22, 1,  1 },
  48                 [3] =  { 1, 0,  6,  32, 28, 0,  190,  510,  32, 1,  1 },
  49                 [4] =  { 1, 0,  9, 255, 28, 0,  323,  343,  13, 1, 32 },
  50                 [13] = { 1, 0,  7,  32, 28, 0, 3836, 3856,  14, 1,  1 },
  51                 [14] = { 1, 0,  6,  32, 28, 0,  615, 5010, 110, 1,  1 },
  52                 [15] = { 1, 1,  0,   0,  0, 0,   15, 5010, 110, 0,  0, 12, 32, 28 },
  53         },
  54 };
  55
  56 static struct mt76_wcid *mt7915_rx_get_wcid(struct mt7915_dev *dev,
  57                                             u16 idx, bool unicast)
  58 {
  59         struct mt7915_sta *sta;
  60         struct mt76_wcid *wcid;
  61
  62         if (idx >= ARRAY_SIZE(dev->mt76.wcid))
  63                 return NULL;
  64
  65         wcid = rcu_dereference(dev->mt76.wcid[idx]);
  66         if (unicast || !wcid)
  67                 return wcid;
  68
  69         if (!wcid->sta)
  70                 return NULL;
  71
  72         sta = container_of(wcid, struct mt7915_sta, wcid);
  73         if (!sta->vif)
  74                 return NULL;
  75
  76         return &sta->vif->sta.wcid;
  77 }
  78
  79 void mt7915_sta_ps(struct mt76_dev *mdev, struct ieee80211_sta *sta, bool ps)
  80 {
  81 }
  82
  83 bool mt7915_mac_wtbl_update(struct mt7915_dev *dev, int idx, u32 mask)
  84 {
  85         mt76_rmw(dev, MT_WTBL_UPDATE, MT_WTBL_UPDATE_WLAN_IDX,
  86                  FIELD_PREP(MT_WTBL_UPDATE_WLAN_IDX, idx) | mask);
  87
  88         return mt76_poll(dev, MT_WTBL_UPDATE, MT_WTBL_UPDATE_BUSY,
  89                          0, 5000);
  90 }
  91
  92 u32 mt7915_mac_wtbl_lmac_addr(struct mt7915_dev *dev, u16 wcid, u8 dw)
  93 {
  94         mt76_wr(dev, MT_WTBLON_TOP_WDUCR,
  95                 FIELD_PREP(MT_WTBLON_TOP_WDUCR_GROUP, (wcid >> 7)));
  96
  97         return MT_WTBL_LMAC_OFFS(wcid, dw);
  98 }
  99
 100 static void mt7915_mac_sta_poll(struct mt7915_dev *dev)
 101 {
 102         static const u8 ac_to_tid[] = {
 103                 [IEEE80211_AC_BE] = 0,
 104                 [IEEE80211_AC_BK] = 1,
 105                 [IEEE80211_AC_VI] = 4,
 106                 [IEEE80211_AC_VO] = 6
 107         };
 108         struct ieee80211_sta *sta;
 109         struct mt7915_sta *msta;
 110         struct rate_info *rate;
 111         u32 tx_time[IEEE80211_NUM_ACS], rx_time[IEEE80211_NUM_ACS];
 112         LIST_HEAD(sta_poll_list);
 113         int i;
 114
 115         spin_lock_bh(&dev->sta_poll_lock);
 116         list_splice_init(&dev->sta_poll_list, &sta_poll_list);
 117         spin_unlock_bh(&dev->sta_poll_lock);
 118
 119         rcu_read_lock();
 120
 121         while (true) {
 122                 bool clear = false;
 123                 u32 addr, val;
 124                 u16 idx;
 125                 u8 bw;
 126
 127                 spin_lock_bh(&dev->sta_poll_lock);
 128                 if (list_empty(&sta_poll_list)) {
 129                         spin_unlock_bh(&dev->sta_poll_lock);
 130                         break;
 131                 }
 132                 msta = list_first_entry(&sta_poll_list,
 133                                         struct mt7915_sta, poll_list);
 134                 list_del_init(&msta->poll_list);
 135                 spin_unlock_bh(&dev->sta_poll_lock);
 136
 137                 idx = msta->wcid.idx;
 138                 addr = mt7915_mac_wtbl_lmac_addr(dev, idx, 20);
 139
 140                 for (i = 0; i < IEEE80211_NUM_ACS; i++) {
 141                         u32 tx_last = msta->airtime_ac[i];
 142                         u32 rx_last = msta->airtime_ac[i + 4];
 143
 144                         msta->airtime_ac[i] = mt76_rr(dev, addr);
 145                         msta->airtime_ac[i + 4] = mt76_rr(dev, addr + 4);
 146
 147                         tx_time[i] = msta->airtime_ac[i] - tx_last;
 148                         rx_time[i] = msta->airtime_ac[i + 4] - rx_last;
 149
 150                         if ((tx_last | rx_last) & BIT(30))
 151                                 clear = true;
 152
 153                         addr += 8;
 154                 }
 155
 156                 if (clear) {
 157                         mt7915_mac_wtbl_update(dev, idx,
 158                                                MT_WTBL_UPDATE_ADM_COUNT_CLEAR);
 159                         memset(msta->airtime_ac, 0, sizeof(msta->airtime_ac));
 160                 }
 161
 162                 if (!msta->wcid.sta)
 163                         continue;
 164
 165                 sta = container_of((void *)msta, struct ieee80211_sta,
 166                                    drv_priv);
 167                 for (i = 0; i < IEEE80211_NUM_ACS; i++) {
 168                         u8 q = mt76_connac_lmac_mapping(i);
 169                         u32 tx_cur = tx_time[q];
 170                         u32 rx_cur = rx_time[q];
 171                         u8 tid = ac_to_tid[i];
 172
 173                         if (!tx_cur && !rx_cur)
 174                                 continue;
 175
 176                         ieee80211_sta_register_airtime(sta, tid, tx_cur,
 177                                                        rx_cur);
 178                 }
 179
 180                 /*
 181                  * We don't support reading GI info from txs packets.
 182                  * For accurate tx status reporting and AQL improvement,
 183                  * we need to make sure that flags match so polling GI
 184                  * from per-sta counters directly.
 185                  */
 186                 rate = &msta->wcid.rate;
 187                 addr = mt7915_mac_wtbl_lmac_addr(dev, idx, 7);
 188                 val = mt76_rr(dev, addr);
 189
 190                 switch (rate->bw) {
 191                 case RATE_INFO_BW_160:
 192                         bw = IEEE80211_STA_RX_BW_160;
 193                         break;
 194                 case RATE_INFO_BW_80:
 195                         bw = IEEE80211_STA_RX_BW_80;
 196                         break;
 197                 case RATE_INFO_BW_40:
 198                         bw = IEEE80211_STA_RX_BW_40;
 199                         break;
 200                 default:
 201                         bw = IEEE80211_STA_RX_BW_20;
 202                         break;
 203                 }
 204
 205                 if (rate->flags & RATE_INFO_FLAGS_HE_MCS) {
 206                         u8 offs = 24 + 2 * bw;
 207
 208                         rate->he_gi = (val & (0x3 << offs)) >> offs;
 209                 } else if (rate->flags &
 210                            (RATE_INFO_FLAGS_VHT_MCS | RATE_INFO_FLAGS_MCS)) {
 211                         if (val & BIT(12 + bw))
 212                                 rate->flags |= RATE_INFO_FLAGS_SHORT_GI;
 213                         else
 214                                 rate->flags &= ~RATE_INFO_FLAGS_SHORT_GI;
 215                 }
 216         }
 217
 218         rcu_read_unlock();
 219 }
 220
 221 static void
 222 mt7915_mac_decode_he_radiotap_ru(struct mt76_rx_status *status,
 223                                  struct ieee80211_radiotap_he *he,
 224                                  __le32 *rxv)
 225 {
 226         u32 ru_h, ru_l;
 227         u8 ru, offs = 0;
 228
 229         ru_l = le32_get_bits(rxv[0], MT_PRXV_HE_RU_ALLOC_L);
 230         ru_h = le32_get_bits(rxv[1], MT_PRXV_HE_RU_ALLOC_H);
 231         ru = (u8)(ru_l | ru_h << 4);
 232
 233         status->bw = RATE_INFO_BW_HE_RU;
 234
 235         switch (ru) {
 236         case 0 ... 36:
 237                 status->he_ru = NL80211_RATE_INFO_HE_RU_ALLOC_26;
 238                 offs = ru;
 239                 break;
 240         case 37 ... 52:
 241                 status->he_ru = NL80211_RATE_INFO_HE_RU_ALLOC_52;
 242                 offs = ru - 37;
 243                 break;
 244         case 53 ... 60:
 245                 status->he_ru = NL80211_RATE_INFO_HE_RU_ALLOC_106;
 246                 offs = ru - 53;
 247                 break;
 248         case 61 ... 64:
 249                 status->he_ru = NL80211_RATE_INFO_HE_RU_ALLOC_242;
 250                 offs = ru - 61;
 251                 break;
 252         case 65 ... 66:
 253                 status->he_ru = NL80211_RATE_INFO_HE_RU_ALLOC_484;
 254                 offs = ru - 65;
 255                 break;
 256         case 67:
 257                 status->he_ru = NL80211_RATE_INFO_HE_RU_ALLOC_996;
 258                 break;
 259         case 68:
 260                 status->he_ru = NL80211_RATE_INFO_HE_RU_ALLOC_2x996;
 261                 break;
 262         }
 263
 264         he->data1 |= HE_BITS(DATA1_BW_RU_ALLOC_KNOWN);
 265         he->data2 |= HE_BITS(DATA2_RU_OFFSET_KNOWN) |
 266                      le16_encode_bits(offs,
 267                                       IEEE80211_RADIOTAP_HE_DATA2_RU_OFFSET);
 268 }
 269
 270 static void
 271 mt7915_mac_decode_he_mu_radiotap(struct sk_buff *skb, __le32 *rxv)
 272 {
 273         struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb;
 274         static const struct ieee80211_radiotap_he_mu mu_known = {
 275                 .flags1 = HE_BITS(MU_FLAGS1_SIG_B_MCS_KNOWN) |
 276                           HE_BITS(MU_FLAGS1_SIG_B_DCM_KNOWN) |
 277                           HE_BITS(MU_FLAGS1_CH1_RU_KNOWN) |
 278                           HE_BITS(MU_FLAGS1_SIG_B_SYMS_USERS_KNOWN),
 279                 .flags2 = HE_BITS(MU_FLAGS2_BW_FROM_SIG_A_BW_KNOWN),
 280         };
 281         struct ieee80211_radiotap_he_mu *he_mu = NULL;
 282
 283         status->flag |= RX_FLAG_RADIOTAP_HE_MU;
 284
 285         he_mu = skb_push(skb, sizeof(mu_known));
 286         memcpy(he_mu, &mu_known, sizeof(mu_known));
 287
 288 #define MU_PREP(f, v)   le16_encode_bits(v, IEEE80211_RADIOTAP_HE_MU_##f)
 289
 290         he_mu->flags1 |= MU_PREP(FLAGS1_SIG_B_MCS, status->rate_idx);
 291         if (status->he_dcm)
 292                 he_mu->flags1 |= MU_PREP(FLAGS1_SIG_B_DCM, status->he_dcm);
 293
 294         he_mu->flags2 |= MU_PREP(FLAGS2_BW_FROM_SIG_A_BW, status->bw) |
 295                          MU_PREP(FLAGS2_SIG_B_SYMS_USERS,
 296                                  le32_get_bits(rxv[2], MT_CRXV_HE_NUM_USER));
 297
 298         he_mu->ru_ch1[0] = le32_get_bits(rxv[3], MT_CRXV_HE_RU0);
 299
 300         if (status->bw >= RATE_INFO_BW_40) {
 301                 he_mu->flags1 |= HE_BITS(MU_FLAGS1_CH2_RU_KNOWN);
 302                 he_mu->ru_ch2[0] = le32_get_bits(rxv[3], MT_CRXV_HE_RU1);
 303         }
 304
 305         if (status->bw >= RATE_INFO_BW_80) {
 306                 he_mu->ru_ch1[1] = le32_get_bits(rxv[3], MT_CRXV_HE_RU2);
 307                 he_mu->ru_ch2[1] = le32_get_bits(rxv[3], MT_CRXV_HE_RU3);
 308         }
 309 }
 310
 311 static void
 312 mt7915_mac_decode_he_radiotap(struct sk_buff *skb, __le32 *rxv, u8 mode)
 313 {
 314         struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb;
 315         static const struct ieee80211_radiotap_he known = {
 316                 .data1 = HE_BITS(DATA1_DATA_MCS_KNOWN) |
 317                          HE_BITS(DATA1_DATA_DCM_KNOWN) |
 318                          HE_BITS(DATA1_STBC_KNOWN) |
 319                          HE_BITS(DATA1_CODING_KNOWN) |
 320                          HE_BITS(DATA1_LDPC_XSYMSEG_KNOWN) |
 321                          HE_BITS(DATA1_DOPPLER_KNOWN) |
 322                          HE_BITS(DATA1_SPTL_REUSE_KNOWN) |
 323                          HE_BITS(DATA1_BSS_COLOR_KNOWN),
 324                 .data2 = HE_BITS(DATA2_GI_KNOWN) |
 325                          HE_BITS(DATA2_TXBF_KNOWN) |
 326                          HE_BITS(DATA2_PE_DISAMBIG_KNOWN) |
 327                          HE_BITS(DATA2_TXOP_KNOWN),
 328         };
 329         struct ieee80211_radiotap_he *he = NULL;
 330         u32 ltf_size = le32_get_bits(rxv[2], MT_CRXV_HE_LTF_SIZE) + 1;
 331
 332         status->flag |= RX_FLAG_RADIOTAP_HE;
 333
 334         he = skb_push(skb, sizeof(known));
 335         memcpy(he, &known, sizeof(known));
 336
 337         he->data3 = HE_PREP(DATA3_BSS_COLOR, BSS_COLOR, rxv[14]) |
 338                     HE_PREP(DATA3_LDPC_XSYMSEG, LDPC_EXT_SYM, rxv[2]);
 339         he->data4 = HE_PREP(DATA4_SU_MU_SPTL_REUSE, SR_MASK, rxv[11]);
 340         he->data5 = HE_PREP(DATA5_PE_DISAMBIG, PE_DISAMBIG, rxv[2]) |
 341                     le16_encode_bits(ltf_size,
 342                                      IEEE80211_RADIOTAP_HE_DATA5_LTF_SIZE);
 343         if (le32_to_cpu(rxv[0]) & MT_PRXV_TXBF)
 344                 he->data5 |= HE_BITS(DATA5_TXBF);
 345         he->data6 = HE_PREP(DATA6_TXOP, TXOP_DUR, rxv[14]) |
 346                     HE_PREP(DATA6_DOPPLER, DOPPLER, rxv[14]);
 347
 348         switch (mode) {
 349         case MT_PHY_TYPE_HE_SU:
 350                 he->data1 |= HE_BITS(DATA1_FORMAT_SU) |
 351                              HE_BITS(DATA1_UL_DL_KNOWN) |
 352                              HE_BITS(DATA1_BEAM_CHANGE_KNOWN) |
 353                              HE_BITS(DATA1_BW_RU_ALLOC_KNOWN);
 354
 355                 he->data3 |= HE_PREP(DATA3_BEAM_CHANGE, BEAM_CHNG, rxv[14]) |
 356                              HE_PREP(DATA3_UL_DL, UPLINK, rxv[2]);
 357                 break;
 358         case MT_PHY_TYPE_HE_EXT_SU:
 359                 he->data1 |= HE_BITS(DATA1_FORMAT_EXT_SU) |
 360                              HE_BITS(DATA1_UL_DL_KNOWN) |
 361                              HE_BITS(DATA1_BW_RU_ALLOC_KNOWN);
 362
 363                 he->data3 |= HE_PREP(DATA3_UL_DL, UPLINK, rxv[2]);
 364                 break;
 365         case MT_PHY_TYPE_HE_MU:
 366                 he->data1 |= HE_BITS(DATA1_FORMAT_MU) |
 367                              HE_BITS(DATA1_UL_DL_KNOWN);
 368
 369                 he->data3 |= HE_PREP(DATA3_UL_DL, UPLINK, rxv[2]);
 370                 he->data4 |= HE_PREP(DATA4_MU_STA_ID, MU_AID, rxv[7]);
 371
 372                 mt7915_mac_decode_he_radiotap_ru(status, he, rxv);
 373                 mt7915_mac_decode_he_mu_radiotap(skb, rxv);
 374                 break;
 375         case MT_PHY_TYPE_HE_TB:
 376                 he->data1 |= HE_BITS(DATA1_FORMAT_TRIG) |
 377                              HE_BITS(DATA1_SPTL_REUSE2_KNOWN) |
 378                              HE_BITS(DATA1_SPTL_REUSE3_KNOWN) |
 379                              HE_BITS(DATA1_SPTL_REUSE4_KNOWN);
 380
 381                 he->data4 |= HE_PREP(DATA4_TB_SPTL_REUSE1, SR_MASK, rxv[11]) |
 382                              HE_PREP(DATA4_TB_SPTL_REUSE2, SR1_MASK, rxv[11]) |
 383                              HE_PREP(DATA4_TB_SPTL_REUSE3, SR2_MASK, rxv[11]) |
 384                              HE_PREP(DATA4_TB_SPTL_REUSE4, SR3_MASK, rxv[11]);
 385
 386                 mt7915_mac_decode_he_radiotap_ru(status, he, rxv);
 387                 break;
 388         default:
 389                 break;
 390         }
 391 }
 392
 393 /* The HW does not translate the mac header to 802.3 for mesh point */
 394 static int mt7915_reverse_frag0_hdr_trans(struct sk_buff *skb, u16 hdr_gap)
 395 {
 396         struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb;
 397         struct ethhdr *eth_hdr = (struct ethhdr *)(skb->data + hdr_gap);
 398         struct mt7915_sta *msta = (struct mt7915_sta *)status->wcid;
 399         __le32 *rxd = (__le32 *)skb->data;
 400         struct ieee80211_sta *sta;
 401         struct ieee80211_vif *vif;
 402         struct ieee80211_hdr hdr;
 403         u16 frame_control;
 404
 405         if (le32_get_bits(rxd[3], MT_RXD3_NORMAL_ADDR_TYPE) !=
 406             MT_RXD3_NORMAL_U2M)
 407                 return -EINVAL;
 408
 409         if (!(le32_to_cpu(rxd[1]) & MT_RXD1_NORMAL_GROUP_4))
 410                 return -EINVAL;
 411
 412         if (!msta || !msta->vif)
 413                 return -EINVAL;
 414
 415         sta = container_of((void *)msta, struct ieee80211_sta, drv_priv);
 416         vif = container_of((void *)msta->vif, struct ieee80211_vif, drv_priv);
 417
 418         /* store the info from RXD and ethhdr to avoid being overridden */
 419         frame_control = le32_get_bits(rxd[6], MT_RXD6_FRAME_CONTROL);
 420         hdr.frame_control = cpu_to_le16(frame_control);
 421         hdr.seq_ctrl = cpu_to_le16(le32_get_bits(rxd[8], MT_RXD8_SEQ_CTRL));
 422         hdr.duration_id = 0;
 423
 424         ether_addr_copy(hdr.addr1, vif->addr);
 425         ether_addr_copy(hdr.addr2, sta->addr);
 426         switch (frame_control & (IEEE80211_FCTL_TODS |
 427                                  IEEE80211_FCTL_FROMDS)) {
 428         case 0:
 429                 ether_addr_copy(hdr.addr3, vif->bss_conf.bssid);
 430                 break;
 431         case IEEE80211_FCTL_FROMDS:
 432                 ether_addr_copy(hdr.addr3, eth_hdr->h_source);
 433                 break;
 434         case IEEE80211_FCTL_TODS:
 435                 ether_addr_copy(hdr.addr3, eth_hdr->h_dest);
 436                 break;
 437         case IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS:
 438                 ether_addr_copy(hdr.addr3, eth_hdr->h_dest);
 439                 ether_addr_copy(hdr.addr4, eth_hdr->h_source);
 440                 break;
 441         default:
 442                 break;
 443         }
 444
 445         skb_pull(skb, hdr_gap + sizeof(struct ethhdr) - 2);
 446         if (eth_hdr->h_proto == cpu_to_be16(ETH_P_AARP) ||
 447             eth_hdr->h_proto == cpu_to_be16(ETH_P_IPX))
 448                 ether_addr_copy(skb_push(skb, ETH_ALEN), bridge_tunnel_header);
 449         else if (be16_to_cpu(eth_hdr->h_proto) >= ETH_P_802_3_MIN)
 450                 ether_addr_copy(skb_push(skb, ETH_ALEN), rfc1042_header);
 451         else
 452                 skb_pull(skb, 2);
 453
 454         if (ieee80211_has_order(hdr.frame_control))
 455                 memcpy(skb_push(skb, IEEE80211_HT_CTL_LEN), &rxd[9],
 456                        IEEE80211_HT_CTL_LEN);
 457         if (ieee80211_is_data_qos(hdr.frame_control)) {
 458                 __le16 qos_ctrl;
 459
 460                 qos_ctrl = cpu_to_le16(le32_get_bits(rxd[8], MT_RXD8_QOS_CTL));
 461                 memcpy(skb_push(skb, IEEE80211_QOS_CTL_LEN), &qos_ctrl,
 462                        IEEE80211_QOS_CTL_LEN);
 463         }
 464
 465         if (ieee80211_has_a4(hdr.frame_control))
 466                 memcpy(skb_push(skb, sizeof(hdr)), &hdr, sizeof(hdr));
 467         else
 468                 memcpy(skb_push(skb, sizeof(hdr) - 6), &hdr, sizeof(hdr) - 6);
 469
 470         return 0;
 471 }
 472
 473 static int
 474 mt7915_mac_fill_rx_rate(struct mt7915_dev *dev,
 475                         struct mt76_rx_status *status,
 476                         struct ieee80211_supported_band *sband,
 477                         __le32 *rxv, u8 *mode)
 478 {
 479         u32 v0, v2;
 480         u8 stbc, gi, bw, dcm, nss;
 481         int i, idx;
 482         bool cck = false;
 483
 484         v0 = le32_to_cpu(rxv[0]);
 485         v2 = le32_to_cpu(rxv[2]);
 486
 487         idx = i = FIELD_GET(MT_PRXV_TX_RATE, v0);
 488         nss = FIELD_GET(MT_PRXV_NSTS, v0) + 1;
 489
 490         if (!is_mt7915(&dev->mt76)) {
 491                 stbc = FIELD_GET(MT_PRXV_HT_STBC, v0);
 492                 gi = FIELD_GET(MT_PRXV_HT_SHORT_GI, v0);
 493                 *mode = FIELD_GET(MT_PRXV_TX_MODE, v0);
 494                 dcm = FIELD_GET(MT_PRXV_DCM, v0);
 495                 bw = FIELD_GET(MT_PRXV_FRAME_MODE, v0);
 496         } else {
 497                 stbc = FIELD_GET(MT_CRXV_HT_STBC, v2);
 498                 gi = FIELD_GET(MT_CRXV_HT_SHORT_GI, v2);
 499                 *mode = FIELD_GET(MT_CRXV_TX_MODE, v2);
 500                 dcm = !!(idx & GENMASK(3, 0) & MT_PRXV_TX_DCM);
 501                 bw = FIELD_GET(MT_CRXV_FRAME_MODE, v2);
 502         }
 503
 504         switch (*mode) {
 505         case MT_PHY_TYPE_CCK:
 506                 cck = true;
 507                 fallthrough;
 508         case MT_PHY_TYPE_OFDM:
 509                 i = mt76_get_rate(&dev->mt76, sband, i, cck);
 510                 break;
 511         case MT_PHY_TYPE_HT_GF:
 512         case MT_PHY_TYPE_HT:
 513                 status->encoding = RX_ENC_HT;
 514                 if (gi)
 515                         status->enc_flags |= RX_ENC_FLAG_SHORT_GI;
 516                 if (i > 31)
 517                         return -EINVAL;
 518                 break;
 519         case MT_PHY_TYPE_VHT:
 520                 status->nss = nss;
 521                 status->encoding = RX_ENC_VHT;
 522                 if (gi)
 523                         status->enc_flags |= RX_ENC_FLAG_SHORT_GI;
 524                 if (i > 11)
 525                         return -EINVAL;
 526                 break;
 527         case MT_PHY_TYPE_HE_MU:
 528         case MT_PHY_TYPE_HE_SU:
 529         case MT_PHY_TYPE_HE_EXT_SU:
 530         case MT_PHY_TYPE_HE_TB:
 531                 status->nss = nss;
 532                 status->encoding = RX_ENC_HE;
 533                 i &= GENMASK(3, 0);
 534
 535                 if (gi <= NL80211_RATE_INFO_HE_GI_3_2)
 536                         status->he_gi = gi;
 537
 538                 status->he_dcm = dcm;
 539                 break;
 540         default:
 541                 return -EINVAL;
 542         }
 543         status->rate_idx = i;
 544
 545         switch (bw) {
 546         case IEEE80211_STA_RX_BW_20:
 547                 break;
 548         case IEEE80211_STA_RX_BW_40:
 549                 if (*mode & MT_PHY_TYPE_HE_EXT_SU &&
 550                     (idx & MT_PRXV_TX_ER_SU_106T)) {
 551                         status->bw = RATE_INFO_BW_HE_RU;
 552                         status->he_ru =
 553                                 NL80211_RATE_INFO_HE_RU_ALLOC_106;
 554                 } else {
 555                         status->bw = RATE_INFO_BW_40;
 556                 }
 557                 break;
 558         case IEEE80211_STA_RX_BW_80:
 559                 status->bw = RATE_INFO_BW_80;
 560                 break;
 561         case IEEE80211_STA_RX_BW_160:
 562                 status->bw = RATE_INFO_BW_160;
 563                 break;
 564         default:
 565                 return -EINVAL;
 566         }
 567
 568         status->enc_flags |= RX_ENC_FLAG_STBC_MASK * stbc;
 569         if (*mode < MT_PHY_TYPE_HE_SU && gi)
 570                 status->enc_flags |= RX_ENC_FLAG_SHORT_GI;
 571
 572         return 0;
 573 }
 574
 575 static int
 576 mt7915_mac_fill_rx(struct mt7915_dev *dev, struct sk_buff *skb)
 577 {
 578         struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb;
 579         struct mt76_phy *mphy = &dev->mt76.phy;
 580         struct mt7915_phy *phy = &dev->phy;
 581         struct ieee80211_supported_band *sband;
 582         __le32 *rxd = (__le32 *)skb->data;
 583         __le32 *rxv = NULL;
 584         u32 rxd0 = le32_to_cpu(rxd[0]);
 585         u32 rxd1 = le32_to_cpu(rxd[1]);
 586         u32 rxd2 = le32_to_cpu(rxd[2]);
 587         u32 rxd3 = le32_to_cpu(rxd[3]);
 588         u32 rxd4 = le32_to_cpu(rxd[4]);
 589         u32 csum_mask = MT_RXD0_NORMAL_IP_SUM | MT_RXD0_NORMAL_UDP_TCP_SUM;
 590         bool unicast, insert_ccmp_hdr = false;
 591         u8 remove_pad, amsdu_info;
 592         u8 mode = 0, qos_ctl = 0;
 593         bool hdr_trans;
 594         u16 hdr_gap;
 595         u16 seq_ctrl = 0;
 596         __le16 fc = 0;
 597         int idx;
 598
 599         memset(status, 0, sizeof(*status));
 600
 601         if ((rxd1 & MT_RXD1_NORMAL_BAND_IDX) && !phy->band_idx) {
 602                 mphy = dev->mt76.phy2;
 603                 if (!mphy)
 604                         return -EINVAL;
 605
 606                 phy = mphy->priv;
 607                 status->ext_phy = true;
 608         }
 609
 610         if (!test_bit(MT76_STATE_RUNNING, &mphy->state))
 611                 return -EINVAL;
 612
 613         if (rxd2 & MT_RXD2_NORMAL_AMSDU_ERR)
 614                 return -EINVAL;
 615
 616         hdr_trans = rxd2 & MT_RXD2_NORMAL_HDR_TRANS;
 617         if (hdr_trans && (rxd1 & MT_RXD1_NORMAL_CM))
 618                 return -EINVAL;
 619
 620         /* ICV error or CCMP/BIP/WPI MIC error */
 621         if (rxd1 & MT_RXD1_NORMAL_ICV_ERR)
 622                 status->flag |= RX_FLAG_ONLY_MONITOR;
 623
 624         unicast = FIELD_GET(MT_RXD3_NORMAL_ADDR_TYPE, rxd3) == MT_RXD3_NORMAL_U2M;
 625         idx = FIELD_GET(MT_RXD1_NORMAL_WLAN_IDX, rxd1);
 626         status->wcid = mt7915_rx_get_wcid(dev, idx, unicast);
 627
 628         if (status->wcid) {
 629                 struct mt7915_sta *msta;
 630
 631                 msta = container_of(status->wcid, struct mt7915_sta, wcid);
 632                 spin_lock_bh(&dev->sta_poll_lock);
 633                 if (list_empty(&msta->poll_list))
 634                         list_add_tail(&msta->poll_list, &dev->sta_poll_list);
 635                 spin_unlock_bh(&dev->sta_poll_lock);
 636         }
 637
 638         status->freq = mphy->chandef.chan->center_freq;
 639         status->band = mphy->chandef.chan->band;
 640         if (status->band == NL80211_BAND_5GHZ)
 641                 sband = &mphy->sband_5g.sband;
 642         else if (status->band == NL80211_BAND_6GHZ)
 643                 sband = &mphy->sband_6g.sband;
 644         else
 645                 sband = &mphy->sband_2g.sband;
 646
 647         if (!sband->channels)
 648                 return -EINVAL;
 649
 650         if ((rxd0 & csum_mask) == csum_mask)
 651                 skb->ip_summed = CHECKSUM_UNNECESSARY;
 652
 653         if (rxd1 & MT_RXD1_NORMAL_FCS_ERR)
 654                 status->flag |= RX_FLAG_FAILED_FCS_CRC;
 655
 656         if (rxd1 & MT_RXD1_NORMAL_TKIP_MIC_ERR)
 657                 status->flag |= RX_FLAG_MMIC_ERROR;
 658
 659         if (FIELD_GET(MT_RXD1_NORMAL_SEC_MODE, rxd1) != 0 &&
 660             !(rxd1 & (MT_RXD1_NORMAL_CLM | MT_RXD1_NORMAL_CM))) {
 661                 status->flag |= RX_FLAG_DECRYPTED;
 662                 status->flag |= RX_FLAG_IV_STRIPPED;
 663                 status->flag |= RX_FLAG_MMIC_STRIPPED | RX_FLAG_MIC_STRIPPED;
 664         }
 665
 666         remove_pad = FIELD_GET(MT_RXD2_NORMAL_HDR_OFFSET, rxd2);
 667
 668         if (rxd2 & MT_RXD2_NORMAL_MAX_LEN_ERROR)
 669                 return -EINVAL;
 670
 671         rxd += 6;
 672         if (rxd1 & MT_RXD1_NORMAL_GROUP_4) {
 673                 u32 v0 = le32_to_cpu(rxd[0]);
 674                 u32 v2 = le32_to_cpu(rxd[2]);
 675
 676                 fc = cpu_to_le16(FIELD_GET(MT_RXD6_FRAME_CONTROL, v0));
 677                 qos_ctl = FIELD_GET(MT_RXD8_QOS_CTL, v2);
 678                 seq_ctrl = FIELD_GET(MT_RXD8_SEQ_CTRL, v2);
 679
 680                 rxd += 4;
 681                 if ((u8 *)rxd - skb->data >= skb->len)
 682                         return -EINVAL;
 683         }
 684
 685         if (rxd1 & MT_RXD1_NORMAL_GROUP_1) {
 686                 u8 *data = (u8 *)rxd;
 687
 688                 if (status->flag & RX_FLAG_DECRYPTED) {
 689                         switch (FIELD_GET(MT_RXD1_NORMAL_SEC_MODE, rxd1)) {
 690                         case MT_CIPHER_AES_CCMP:
 691                         case MT_CIPHER_CCMP_CCX:
 692                         case MT_CIPHER_CCMP_256:
 693                                 insert_ccmp_hdr =
 694                                         FIELD_GET(MT_RXD2_NORMAL_FRAG, rxd2);
 695                                 fallthrough;
 696                         case MT_CIPHER_TKIP:
 697                         case MT_CIPHER_TKIP_NO_MIC:
 698                         case MT_CIPHER_GCMP:
 699                         case MT_CIPHER_GCMP_256:
 700                                 status->iv[0] = data[5];
 701                                 status->iv[1] = data[4];
 702                                 status->iv[2] = data[3];
 703                                 status->iv[3] = data[2];
 704                                 status->iv[4] = data[1];
 705                                 status->iv[5] = data[0];
 706                                 break;
 707                         default:
 708                                 break;
 709                         }
 710                 }
 711                 rxd += 4;
 712                 if ((u8 *)rxd - skb->data >= skb->len)
 713                         return -EINVAL;
 714         }
 715
 716         if (rxd1 & MT_RXD1_NORMAL_GROUP_2) {
 717                 status->timestamp = le32_to_cpu(rxd[0]);
 718                 status->flag |= RX_FLAG_MACTIME_START;
 719
 720                 if (!(rxd2 & MT_RXD2_NORMAL_NON_AMPDU)) {
 721                         status->flag |= RX_FLAG_AMPDU_DETAILS;
 722
 723                         /* all subframes of an A-MPDU have the same timestamp */
 724                         if (phy->rx_ampdu_ts != status->timestamp) {
 725                                 if (!++phy->ampdu_ref)
 726                                         phy->ampdu_ref++;
 727                         }
 728                         phy->rx_ampdu_ts = status->timestamp;
 729
 730                         status->ampdu_ref = phy->ampdu_ref;
 731                 }
 732
 733                 rxd += 2;
 734                 if ((u8 *)rxd - skb->data >= skb->len)
 735                         return -EINVAL;
 736         }
 737
 738         /* RXD Group 3 - P-RXV */
 739         if (rxd1 & MT_RXD1_NORMAL_GROUP_3) {
 740                 u32 v0, v1;
 741                 int ret;
 742
 743                 rxv = rxd;
 744                 rxd += 2;
 745                 if ((u8 *)rxd - skb->data >= skb->len)
 746                         return -EINVAL;
 747
 748                 v0 = le32_to_cpu(rxv[0]);
 749                 v1 = le32_to_cpu(rxv[1]);
 750
 751                 if (v0 & MT_PRXV_HT_AD_CODE)
 752                         status->enc_flags |= RX_ENC_FLAG_LDPC;
 753
 754                 status->chains = mphy->antenna_mask;
 755                 status->chain_signal[0] = to_rssi(MT_PRXV_RCPI0, v1);
 756                 status->chain_signal[1] = to_rssi(MT_PRXV_RCPI1, v1);
 757                 status->chain_signal[2] = to_rssi(MT_PRXV_RCPI2, v1);
 758                 status->chain_signal[3] = to_rssi(MT_PRXV_RCPI3, v1);
 759
 760                 /* RXD Group 5 - C-RXV */
 761                 if (rxd1 & MT_RXD1_NORMAL_GROUP_5) {
 762                         rxd += 18;
 763                         if ((u8 *)rxd - skb->data >= skb->len)
 764                                 return -EINVAL;
 765                 }
 766
 767                 if (!is_mt7915(&dev->mt76) || (rxd1 & MT_RXD1_NORMAL_GROUP_5)) {
 768                         ret = mt7915_mac_fill_rx_rate(dev, status, sband, rxv,
 769                                                       &mode);
 770                         if (ret < 0)
 771                                 return ret;
 772                 }
 773         }
 774
 775         amsdu_info = FIELD_GET(MT_RXD4_NORMAL_PAYLOAD_FORMAT, rxd4);
 776         status->amsdu = !!amsdu_info;
 777         if (status->amsdu) {
 778                 status->first_amsdu = amsdu_info == MT_RXD4_FIRST_AMSDU_FRAME;
 779                 status->last_amsdu = amsdu_info == MT_RXD4_LAST_AMSDU_FRAME;
 780         }
 781
 782         hdr_gap = (u8 *)rxd - skb->data + 2 * remove_pad;
 783         if (hdr_trans && ieee80211_has_morefrags(fc)) {
 784                 if (mt7915_reverse_frag0_hdr_trans(skb, hdr_gap))
 785                         return -EINVAL;
 786                 hdr_trans = false;
 787         } else {
 788                 int pad_start = 0;
 789
 790                 skb_pull(skb, hdr_gap);
 791                 if (!hdr_trans && status->amsdu) {
 792                         pad_start = ieee80211_get_hdrlen_from_skb(skb);
 793                 } else if (hdr_trans && (rxd2 & MT_RXD2_NORMAL_HDR_TRANS_ERROR)) {
 794                         /*
 795                          * When header translation failure is indicated,
 796                          * the hardware will insert an extra 2-byte field
 797                          * containing the data length after the protocol
 798                          * type field.
 799                          */
 800                         pad_start = 12;
 801                         if (get_unaligned_be16(skb->data + pad_start) == ETH_P_8021Q)
 802                                 pad_start += 4;
 803
 804                         if (get_unaligned_be16(skb->data + pad_start) !=
 805                             skb->len - pad_start - 2)
 806                                 pad_start = 0;
 807                 }
 808
 809                 if (pad_start) {
 810                         memmove(skb->data + 2, skb->data, pad_start);
 811                         skb_pull(skb, 2);
 812                 }
 813         }
 814
 815         if (!hdr_trans) {
 816                 struct ieee80211_hdr *hdr;
 817
 818                 if (insert_ccmp_hdr) {
 819                         u8 key_id = FIELD_GET(MT_RXD1_NORMAL_KEY_ID, rxd1);
 820
 821                         mt76_insert_ccmp_hdr(skb, key_id);
 822                 }
 823
 824                 hdr = mt76_skb_get_hdr(skb);
 825                 fc = hdr->frame_control;
 826                 if (ieee80211_is_data_qos(fc)) {
 827                         seq_ctrl = le16_to_cpu(hdr->seq_ctrl);
 828                         qos_ctl = *ieee80211_get_qos_ctl(hdr);
 829                 }
 830         } else {
 831                 status->flag |= RX_FLAG_8023;
 832         }
 833
 834         if (rxv && mode >= MT_PHY_TYPE_HE_SU && !(status->flag & RX_FLAG_8023))
 835                 mt7915_mac_decode_he_radiotap(skb, rxv, mode);
 836
 837         if (!status->wcid || !ieee80211_is_data_qos(fc))
 838                 return 0;
 839
 840         status->aggr = unicast &&
 841                        !ieee80211_is_qos_nullfunc(fc);
 842         status->qos_ctl = qos_ctl;
 843         status->seqno = IEEE80211_SEQ_TO_SN(seq_ctrl);
 844
 845         return 0;
 846 }
 847
 848 static void
 849 mt7915_mac_fill_rx_vector(struct mt7915_dev *dev, struct sk_buff *skb)
 850 {
 851 #ifdef CONFIG_NL80211_TESTMODE
 852         struct mt7915_phy *phy = &dev->phy;
 853         __le32 *rxd = (__le32 *)skb->data;
 854         __le32 *rxv_hdr = rxd + 2;
 855         __le32 *rxv = rxd + 4;
 856         u32 rcpi, ib_rssi, wb_rssi, v20, v21;
 857         u8 band_idx;
 858         s32 foe;
 859         u8 snr;
 860         int i;
 861
 862         band_idx = le32_get_bits(rxv_hdr[1], MT_RXV_HDR_BAND_IDX);
 863         if (band_idx && !phy->band_idx) {
 864                 phy = mt7915_ext_phy(dev);
 865                 if (!phy)
 866                         goto out;
 867         }
 868
 869         rcpi = le32_to_cpu(rxv[6]);
 870         ib_rssi = le32_to_cpu(rxv[7]);
 871         wb_rssi = le32_to_cpu(rxv[8]) >> 5;
 872
 873         for (i = 0; i < 4; i++, rcpi >>= 8, ib_rssi >>= 8, wb_rssi >>= 9) {
 874                 if (i == 3)
 875                         wb_rssi = le32_to_cpu(rxv[9]);
 876
 877                 phy->test.last_rcpi[i] = rcpi & 0xff;
 878                 phy->test.last_ib_rssi[i] = ib_rssi & 0xff;
 879                 phy->test.last_wb_rssi[i] = wb_rssi & 0xff;
 880         }
 881
 882         v20 = le32_to_cpu(rxv[20]);
 883         v21 = le32_to_cpu(rxv[21]);
 884
 885         foe = FIELD_GET(MT_CRXV_FOE_LO, v20) |
 886               (FIELD_GET(MT_CRXV_FOE_HI, v21) << MT_CRXV_FOE_SHIFT);
 887
 888         snr = FIELD_GET(MT_CRXV_SNR, v20) - 16;
 889
 890         phy->test.last_freq_offset = foe;
 891         phy->test.last_snr = snr;
 892 out:
 893 #endif
 894         dev_kfree_skb(skb);
 895 }
 896
 897 static void
 898 mt7915_mac_write_txwi_tm(struct mt7915_phy *phy, __le32 *txwi,
 899                          struct sk_buff *skb)
 900 {
 901 #ifdef CONFIG_NL80211_TESTMODE
 902         struct mt76_testmode_data *td = &phy->mt76->test;
 903         const struct ieee80211_rate *r;
 904         u8 bw, mode, nss = td->tx_rate_nss;
 905         u8 rate_idx = td->tx_rate_idx;
 906         u16 rateval = 0;
 907         u32 val;
 908         bool cck = false;
 909         int band;
 910
 911         if (skb != phy->mt76->test.tx_skb)
 912                 return;
 913
 914         switch (td->tx_rate_mode) {
 915         case MT76_TM_TX_MODE_HT:
 916                 nss = 1 + (rate_idx >> 3);
 917                 mode = MT_PHY_TYPE_HT;
 918                 break;
 919         case MT76_TM_TX_MODE_VHT:
 920                 mode = MT_PHY_TYPE_VHT;
 921                 break;
 922         case MT76_TM_TX_MODE_HE_SU:
 923                 mode = MT_PHY_TYPE_HE_SU;
 924                 break;
 925         case MT76_TM_TX_MODE_HE_EXT_SU:
 926                 mode = MT_PHY_TYPE_HE_EXT_SU;
 927                 break;
 928         case MT76_TM_TX_MODE_HE_TB:
 929                 mode = MT_PHY_TYPE_HE_TB;
 930                 break;
 931         case MT76_TM_TX_MODE_HE_MU:
 932                 mode = MT_PHY_TYPE_HE_MU;
 933                 break;
 934         case MT76_TM_TX_MODE_CCK:
 935                 cck = true;
 936                 fallthrough;
 937         case MT76_TM_TX_MODE_OFDM:
 938                 band = phy->mt76->chandef.chan->band;
 939                 if (band == NL80211_BAND_2GHZ && !cck)
 940                         rate_idx += 4;
 941
 942                 r = &phy->mt76->hw->wiphy->bands[band]->bitrates[rate_idx];
 943                 val = cck ? r->hw_value_short : r->hw_value;
 944
 945                 mode = val >> 8;
 946                 rate_idx = val & 0xff;
 947                 break;
 948         default:
 949                 mode = MT_PHY_TYPE_OFDM;
 950                 break;
 951         }
 952
 953         switch (phy->mt76->chandef.width) {
 954         case NL80211_CHAN_WIDTH_40:
 955                 bw = 1;
 956                 break;
 957         case NL80211_CHAN_WIDTH_80:
 958                 bw = 2;
 959                 break;
 960         case NL80211_CHAN_WIDTH_80P80:
 961         case NL80211_CHAN_WIDTH_160:
 962                 bw = 3;
 963                 break;
 964         default:
 965                 bw = 0;
 966                 break;
 967         }
 968
 969         if (td->tx_rate_stbc && nss == 1) {
 970                 nss++;
 971                 rateval |= MT_TX_RATE_STBC;
 972         }
 973
 974         rateval |= FIELD_PREP(MT_TX_RATE_IDX, rate_idx) |
 975                    FIELD_PREP(MT_TX_RATE_MODE, mode) |
 976                    FIELD_PREP(MT_TX_RATE_NSS, nss - 1);
 977
 978         txwi[2] |= cpu_to_le32(MT_TXD2_FIX_RATE);
 979
 980         le32p_replace_bits(&txwi[3], 1, MT_TXD3_REM_TX_COUNT);
 981         if (td->tx_rate_mode < MT76_TM_TX_MODE_HT)
 982                 txwi[3] |= cpu_to_le32(MT_TXD3_BA_DISABLE);
 983
 984         val = MT_TXD6_FIXED_BW |
 985               FIELD_PREP(MT_TXD6_BW, bw) |
 986               FIELD_PREP(MT_TXD6_TX_RATE, rateval) |
 987               FIELD_PREP(MT_TXD6_SGI, td->tx_rate_sgi);
 988
 989         /* for HE_SU/HE_EXT_SU PPDU
 990          * - 1x, 2x, 4x LTF + 0.8us GI
 991          * - 2x LTF + 1.6us GI, 4x LTF + 3.2us GI
 992          * for HE_MU PPDU
 993          * - 2x, 4x LTF + 0.8us GI
 994          * - 2x LTF + 1.6us GI, 4x LTF + 3.2us GI
 995          * for HE_TB PPDU
 996          * - 1x, 2x LTF + 1.6us GI
 997          * - 4x LTF + 3.2us GI
 998          */
 999         if (mode >= MT_PHY_TYPE_HE_SU)
1000                 val |= FIELD_PREP(MT_TXD6_HELTF, td->tx_ltf);
1001
1002         if (td->tx_rate_ldpc || (bw > 0 && mode >= MT_PHY_TYPE_HE_SU))
1003                 val |= MT_TXD6_LDPC;
1004
1005         txwi[3] &= ~cpu_to_le32(MT_TXD3_SN_VALID);
1006         txwi[6] |= cpu_to_le32(val);
1007         txwi[7] |= cpu_to_le32(FIELD_PREP(MT_TXD7_SPE_IDX,
1008                                           phy->test.spe_idx));
1009 #endif
1010 }
1011
1012 static void
1013 mt7915_mac_write_txwi_8023(struct mt7915_dev *dev, __le32 *txwi,
1014                            struct sk_buff *skb, struct mt76_wcid *wcid)
1015 {
1016
1017         u8 tid = skb->priority & IEEE80211_QOS_CTL_TID_MASK;
1018         u8 fc_type, fc_stype;
1019         u16 ethertype;
1020         bool wmm = false;
1021         u32 val;
1022
1023         if (wcid->sta) {
1024                 struct ieee80211_sta *sta;
1025
1026                 sta = container_of((void *)wcid, struct ieee80211_sta, drv_priv);
1027                 wmm = sta->wme;
1028         }
1029
1030         val = FIELD_PREP(MT_TXD1_HDR_FORMAT, MT_HDR_FORMAT_802_3) |
1031               FIELD_PREP(MT_TXD1_TID, tid);
1032
1033         ethertype = get_unaligned_be16(&skb->data[12]);
1034         if (ethertype >= ETH_P_802_3_MIN)
1035                 val |= MT_TXD1_ETH_802_3;
1036
1037         txwi[1] |= cpu_to_le32(val);
1038
1039         fc_type = IEEE80211_FTYPE_DATA >> 2;
1040         fc_stype = wmm ? IEEE80211_STYPE_QOS_DATA >> 4 : 0;
1041
1042         val = FIELD_PREP(MT_TXD2_FRAME_TYPE, fc_type) |
1043               FIELD_PREP(MT_TXD2_SUB_TYPE, fc_stype);
1044
1045         txwi[2] |= cpu_to_le32(val);
1046
1047         val = FIELD_PREP(MT_TXD7_TYPE, fc_type) |
1048               FIELD_PREP(MT_TXD7_SUB_TYPE, fc_stype);
1049         txwi[7] |= cpu_to_le32(val);
1050 }
1051
1052 static void
1053 mt7915_mac_write_txwi_80211(struct mt7915_dev *dev, __le32 *txwi,
1054                             struct sk_buff *skb, struct ieee80211_key_conf *key,
1055                             bool *mcast)
1056 {
1057         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
1058         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
1059         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1060         u8 tid = skb->priority & IEEE80211_QOS_CTL_TID_MASK;
1061         __le16 fc = hdr->frame_control;
1062         u8 fc_type, fc_stype;
1063         u32 val;
1064
1065         *mcast = is_multicast_ether_addr(hdr->addr1);
1066
1067         if (ieee80211_is_action(fc) &&
1068             mgmt->u.action.category == WLAN_CATEGORY_BACK &&
1069             mgmt->u.action.u.addba_req.action_code == WLAN_ACTION_ADDBA_REQ) {
1070                 u16 capab = le16_to_cpu(mgmt->u.action.u.addba_req.capab);
1071
1072                 txwi[5] |= cpu_to_le32(MT_TXD5_ADD_BA);
1073                 tid = (capab >> 2) & IEEE80211_QOS_CTL_TID_MASK;
1074         } else if (ieee80211_is_back_req(hdr->frame_control)) {
1075                 struct ieee80211_bar *bar = (struct ieee80211_bar *)hdr;
1076                 u16 control = le16_to_cpu(bar->control);
1077
1078                 tid = FIELD_GET(IEEE80211_BAR_CTRL_TID_INFO_MASK, control);
1079         }
1080
1081         val = FIELD_PREP(MT_TXD1_HDR_FORMAT, MT_HDR_FORMAT_802_11) |
1082               FIELD_PREP(MT_TXD1_HDR_INFO,
1083                          ieee80211_get_hdrlen_from_skb(skb) / 2) |
1084               FIELD_PREP(MT_TXD1_TID, tid);
1085         txwi[1] |= cpu_to_le32(val);
1086
1087         fc_type = (le16_to_cpu(fc) & IEEE80211_FCTL_FTYPE) >> 2;
1088         fc_stype = (le16_to_cpu(fc) & IEEE80211_FCTL_STYPE) >> 4;
1089
1090         val = FIELD_PREP(MT_TXD2_FRAME_TYPE, fc_type) |
1091               FIELD_PREP(MT_TXD2_SUB_TYPE, fc_stype) |
1092               FIELD_PREP(MT_TXD2_MULTICAST, *mcast);
1093
1094         if (key && *mcast && ieee80211_is_robust_mgmt_frame(skb) &&
1095             key->cipher == WLAN_CIPHER_SUITE_AES_CMAC) {
1096                 val |= MT_TXD2_BIP;
1097                 txwi[3] &= ~cpu_to_le32(MT_TXD3_PROTECT_FRAME);
1098         }
1099
1100         if (!ieee80211_is_data(fc) || *mcast ||
1101             info->flags & IEEE80211_TX_CTL_USE_MINRATE)
1102                 val |= MT_TXD2_FIX_RATE;
1103
1104         txwi[2] |= cpu_to_le32(val);
1105
1106         if (ieee80211_is_beacon(fc)) {
1107                 txwi[3] &= ~cpu_to_le32(MT_TXD3_SW_POWER_MGMT);
1108                 txwi[3] |= cpu_to_le32(MT_TXD3_REM_TX_COUNT);
1109                 txwi[7] |= cpu_to_le32(FIELD_PREP(MT_TXD7_SPE_IDX, 0x18));
1110         }
1111
1112         if (info->flags & IEEE80211_TX_CTL_INJECTED) {
1113                 u16 seqno = le16_to_cpu(hdr->seq_ctrl);
1114
1115                 if (ieee80211_is_back_req(hdr->frame_control)) {
1116                         struct ieee80211_bar *bar;
1117
1118                         bar = (struct ieee80211_bar *)skb->data;
1119                         seqno = le16_to_cpu(bar->start_seq_num);
1120                 }
1121
1122                 val = MT_TXD3_SN_VALID |
1123                       FIELD_PREP(MT_TXD3_SEQ, IEEE80211_SEQ_TO_SN(seqno));
1124                 txwi[3] |= cpu_to_le32(val);
1125                 txwi[7] &= ~cpu_to_le32(MT_TXD7_HW_AMSDU);
1126         }
1127
1128         val = FIELD_PREP(MT_TXD7_TYPE, fc_type) |
1129               FIELD_PREP(MT_TXD7_SUB_TYPE, fc_stype);
1130         txwi[7] |= cpu_to_le32(val);
1131 }
1132
1133 static u16
1134 mt7915_mac_tx_rate_val(struct mt76_phy *mphy, struct ieee80211_vif *vif,
1135                        bool beacon, bool mcast)
1136 {
1137         u8 mode = 0, band = mphy->chandef.chan->band;
1138         int rateidx = 0, mcast_rate;
1139
1140         if (beacon) {
1141                 struct cfg80211_bitrate_mask *mask;
1142
1143                 mask = &vif->bss_conf.beacon_tx_rate;
1144                 if (hweight16(mask->control[band].he_mcs[0]) == 1) {
1145                         rateidx = ffs(mask->control[band].he_mcs[0]) - 1;
1146                         mode = MT_PHY_TYPE_HE_SU;
1147                         goto out;
1148                 } else if (hweight16(mask->control[band].vht_mcs[0]) == 1) {
1149                         rateidx = ffs(mask->control[band].vht_mcs[0]) - 1;
1150                         mode = MT_PHY_TYPE_VHT;
1151                         goto out;
1152                 } else if (hweight8(mask->control[band].ht_mcs[0]) == 1) {
1153                         rateidx = ffs(mask->control[band].ht_mcs[0]) - 1;
1154                         mode = MT_PHY_TYPE_HT;
1155                         goto out;
1156                 } else if (hweight32(mask->control[band].legacy) == 1) {
1157                         rateidx = ffs(mask->control[band].legacy) - 1;
1158                         goto legacy;
1159                 }
1160         }
1161
1162         mcast_rate = vif->bss_conf.mcast_rate[band];
1163         if (mcast && mcast_rate > 0)
1164                 rateidx = mcast_rate - 1;
1165         else
1166                 rateidx = ffs(vif->bss_conf.basic_rates) - 1;
1167
1168 legacy:
1169         rateidx = mt76_calculate_default_rate(mphy, rateidx);
1170         mode = rateidx >> 8;
1171         rateidx &= GENMASK(7, 0);
1172
1173 out:
1174         return FIELD_PREP(MT_TX_RATE_IDX, rateidx) |
1175                FIELD_PREP(MT_TX_RATE_MODE, mode);
1176 }
1177
1178 void mt7915_mac_write_txwi(struct mt7915_dev *dev, __le32 *txwi,
1179                            struct sk_buff *skb, struct mt76_wcid *wcid, int pid,
1180                            struct ieee80211_key_conf *key, u32 changed)
1181 {
1182         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1183         struct ieee80211_vif *vif = info->control.vif;
1184         struct mt76_phy *mphy = &dev->mphy;
1185         bool ext_phy = info->hw_queue & MT_TX_HW_QUEUE_EXT_PHY;
1186         u8 p_fmt, q_idx, omac_idx = 0, wmm_idx = 0, band_idx = 0;
1187         bool is_8023 = info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP;
1188         bool mcast = false;
1189         u16 tx_count = 15;
1190         u32 val;
1191         bool beacon = !!(changed & (BSS_CHANGED_BEACON |
1192                                     BSS_CHANGED_BEACON_ENABLED));
1193         bool inband_disc = !!(changed & (BSS_CHANGED_UNSOL_BCAST_PROBE_RESP |
1194                                          BSS_CHANGED_FILS_DISCOVERY));
1195
1196         if (vif) {
1197                 struct mt7915_vif *mvif = (struct mt7915_vif *)vif->drv_priv;
1198
1199                 omac_idx = mvif->mt76.omac_idx;
1200                 wmm_idx = mvif->mt76.wmm_idx;
1201                 band_idx = mvif->mt76.band_idx;
1202         }
1203
1204         if (ext_phy && dev->mt76.phy2)
1205                 mphy = dev->mt76.phy2;
1206
1207         if (inband_disc) {
1208                 p_fmt = MT_TX_TYPE_FW;
1209                 q_idx = MT_LMAC_ALTX0;
1210         } else if (beacon) {
1211                 p_fmt = MT_TX_TYPE_FW;
1212                 q_idx = MT_LMAC_BCN0;
1213         } else if (skb_get_queue_mapping(skb) >= MT_TXQ_PSD) {
1214                 p_fmt = MT_TX_TYPE_CT;
1215                 q_idx = MT_LMAC_ALTX0;
1216         } else {
1217                 p_fmt = MT_TX_TYPE_CT;
1218                 q_idx = wmm_idx * MT7915_MAX_WMM_SETS +
1219                         mt76_connac_lmac_mapping(skb_get_queue_mapping(skb));
1220         }
1221
1222         val = FIELD_PREP(MT_TXD0_TX_BYTES, skb->len + MT_TXD_SIZE) |
1223               FIELD_PREP(MT_TXD0_PKT_FMT, p_fmt) |
1224               FIELD_PREP(MT_TXD0_Q_IDX, q_idx);
1225         txwi[0] = cpu_to_le32(val);
1226
1227         val = MT_TXD1_LONG_FORMAT | MT_TXD1_VTA |
1228               FIELD_PREP(MT_TXD1_WLAN_IDX, wcid->idx) |
1229               FIELD_PREP(MT_TXD1_OWN_MAC, omac_idx);
1230
1231         if (ext_phy || band_idx)
1232                 val |= MT_TXD1_TGID;
1233
1234         txwi[1] = cpu_to_le32(val);
1235
1236         txwi[2] = 0;
1237
1238         val = MT_TXD3_SW_POWER_MGMT |
1239               FIELD_PREP(MT_TXD3_REM_TX_COUNT, tx_count);
1240         if (key)
1241                 val |= MT_TXD3_PROTECT_FRAME;
1242         if (info->flags & IEEE80211_TX_CTL_NO_ACK)
1243                 val |= MT_TXD3_NO_ACK;
1244
1245         txwi[3] = cpu_to_le32(val);
1246         txwi[4] = 0;
1247
1248         val = FIELD_PREP(MT_TXD5_PID, pid);
1249         if (pid >= MT_PACKET_ID_FIRST)
1250                 val |= MT_TXD5_TX_STATUS_HOST;
1251         txwi[5] = cpu_to_le32(val);
1252
1253         txwi[6] = 0;
1254         txwi[7] = wcid->amsdu ? cpu_to_le32(MT_TXD7_HW_AMSDU) : 0;
1255
1256         if (is_8023)
1257                 mt7915_mac_write_txwi_8023(dev, txwi, skb, wcid);
1258         else
1259                 mt7915_mac_write_txwi_80211(dev, txwi, skb, key, &mcast);
1260
1261         if (txwi[2] & cpu_to_le32(MT_TXD2_FIX_RATE)) {
1262                 u16 rate = mt7915_mac_tx_rate_val(mphy, vif, beacon, mcast);
1263
1264                 /* hardware won't add HTC for mgmt/ctrl frame */
1265                 txwi[2] |= cpu_to_le32(MT_TXD2_HTC_VLD);
1266
1267                 val = MT_TXD6_FIXED_BW |
1268                       FIELD_PREP(MT_TXD6_TX_RATE, rate);
1269                 txwi[6] |= cpu_to_le32(val);
1270                 txwi[3] |= cpu_to_le32(MT_TXD3_BA_DISABLE);
1271         }
1272
1273         if (mt76_testmode_enabled(mphy))
1274                 mt7915_mac_write_txwi_tm(mphy->priv, txwi, skb);
1275 }
1276
1277 int mt7915_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
1278                           enum mt76_txq_id qid, struct mt76_wcid *wcid,
1279                           struct ieee80211_sta *sta,
1280                           struct mt76_tx_info *tx_info)
1281 {
1282         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx_info->skb->data;
1283         struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
1284         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_info->skb);
1285         struct ieee80211_key_conf *key = info->control.hw_key;
1286         struct ieee80211_vif *vif = info->control.vif;
1287         struct mt76_txwi_cache *t;
1288         struct mt7915_txp *txp;
1289         int id, i, nbuf = tx_info->nbuf - 1;
1290         u8 *txwi = (u8 *)txwi_ptr;
1291         int pid;
1292
1293         if (unlikely(tx_info->skb->len <= ETH_HLEN))
1294                 return -EINVAL;
1295
1296         if (!wcid)
1297                 wcid = &dev->mt76.global_wcid;
1298
1299         if (sta) {
1300                 struct mt7915_sta *msta;
1301
1302                 msta = (struct mt7915_sta *)sta->drv_priv;
1303
1304                 if (time_after(jiffies, msta->jiffies + HZ / 4)) {
1305                         info->flags |= IEEE80211_TX_CTL_REQ_TX_STATUS;
1306                         msta->jiffies = jiffies;
1307                 }
1308         }
1309
1310         t = (struct mt76_txwi_cache *)(txwi + mdev->drv->txwi_size);
1311         t->skb = tx_info->skb;
1312
1313         id = mt76_token_consume(mdev, &t);
1314         if (id < 0)
1315                 return id;
1316
1317         pid = mt76_tx_status_skb_add(mdev, wcid, tx_info->skb);
1318         mt7915_mac_write_txwi(dev, txwi_ptr, tx_info->skb, wcid, pid, key, 0);
1319
1320         txp = (struct mt7915_txp *)(txwi + MT_TXD_SIZE);
1321         for (i = 0; i < nbuf; i++) {
1322                 txp->buf[i] = cpu_to_le32(tx_info->buf[i + 1].addr);
1323                 txp->len[i] = cpu_to_le16(tx_info->buf[i + 1].len);
1324         }
1325         txp->nbuf = nbuf;
1326
1327         txp->flags = cpu_to_le16(MT_CT_INFO_APPLY_TXD | MT_CT_INFO_FROM_HOST);
1328
1329         if (!key)
1330                 txp->flags |= cpu_to_le16(MT_CT_INFO_NONE_CIPHER_FRAME);
1331
1332         if (!(info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP) &&
1333             ieee80211_is_mgmt(hdr->frame_control))
1334                 txp->flags |= cpu_to_le16(MT_CT_INFO_MGMT_FRAME);
1335
1336         if (vif) {
1337                 struct mt7915_vif *mvif = (struct mt7915_vif *)vif->drv_priv;
1338
1339                 txp->bss_idx = mvif->mt76.idx;
1340         }
1341
1342         txp->token = cpu_to_le16(id);
1343         if (test_bit(MT_WCID_FLAG_4ADDR, &wcid->flags))
1344                 txp->rept_wds_wcid = cpu_to_le16(wcid->idx);
1345         else
1346                 txp->rept_wds_wcid = cpu_to_le16(0x3ff);
1347         tx_info->skb = DMA_DUMMY_DATA;
1348
1349         /* pass partial skb header to fw */
1350         tx_info->buf[1].len = MT_CT_PARSE_LEN;
1351         tx_info->buf[1].skip_unmap = true;
1352         tx_info->nbuf = MT_CT_DMA_BUF_NUM;
1353
1354         return 0;
1355 }
1356
1357 u32 mt7915_wed_init_buf(void *ptr, dma_addr_t phys, int token_id)
1358 {
1359         struct mt7915_txp *txp = ptr + MT_TXD_SIZE;
1360         __le32 *txwi = ptr;
1361         u32 val;
1362
1363         memset(ptr, 0, MT_TXD_SIZE + sizeof(*txp));
1364
1365         val = FIELD_PREP(MT_TXD0_TX_BYTES, MT_TXD_SIZE) |
1366               FIELD_PREP(MT_TXD0_PKT_FMT, MT_TX_TYPE_CT);
1367         txwi[0] = cpu_to_le32(val);
1368
1369         val = MT_TXD1_LONG_FORMAT |
1370               FIELD_PREP(MT_TXD1_HDR_FORMAT, MT_HDR_FORMAT_802_3);
1371         txwi[1] = cpu_to_le32(val);
1372
1373         txp->token = cpu_to_le16(token_id);
1374         txp->nbuf = 1;
1375         txp->buf[0] = cpu_to_le32(phys + MT_TXD_SIZE + sizeof(*txp));
1376
1377         return MT_TXD_SIZE + sizeof(*txp);
1378 }
1379
1380 static void
1381 mt7915_tx_check_aggr(struct ieee80211_sta *sta, __le32 *txwi)
1382 {
1383         struct mt7915_sta *msta;
1384         u16 fc, tid;
1385         u32 val;
1386
1387         if (!sta || !(sta->deflink.ht_cap.ht_supported || sta->deflink.he_cap.has_he))
1388                 return;
1389
1390         tid = le32_get_bits(txwi[1], MT_TXD1_TID);
1391         if (tid >= 6) /* skip VO queue */
1392                 return;
1393
1394         val = le32_to_cpu(txwi[2]);
1395         fc = FIELD_GET(MT_TXD2_FRAME_TYPE, val) << 2 |
1396              FIELD_GET(MT_TXD2_SUB_TYPE, val) << 4;
1397         if (unlikely(fc != (IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA)))
1398                 return;
1399
1400         msta = (struct mt7915_sta *)sta->drv_priv;
1401         if (!test_and_set_bit(tid, &msta->ampdu_state))
1402                 ieee80211_start_tx_ba_session(sta, tid, 0);
1403 }
1404
1405 static void
1406 mt7915_txp_skb_unmap(struct mt76_dev *dev, struct mt76_txwi_cache *t)
1407 {
1408         struct mt7915_txp *txp;
1409         int i;
1410
1411         txp = mt7915_txwi_to_txp(dev, t);
1412         for (i = 0; i < txp->nbuf; i++)
1413                 dma_unmap_single(dev->dma_dev, le32_to_cpu(txp->buf[i]),
1414                                  le16_to_cpu(txp->len[i]), DMA_TO_DEVICE);
1415 }
1416
1417 static void
1418 mt7915_txwi_free(struct mt7915_dev *dev, struct mt76_txwi_cache *t,
1419                  struct ieee80211_sta *sta, struct list_head *free_list)
1420 {
1421         struct mt76_dev *mdev = &dev->mt76;
1422         struct mt7915_sta *msta;
1423         struct mt76_wcid *wcid;
1424         __le32 *txwi;
1425         u16 wcid_idx;
1426
1427         mt7915_txp_skb_unmap(mdev, t);
1428         if (!t->skb)
1429                 goto out;
1430
1431         txwi = (__le32 *)mt76_get_txwi_ptr(mdev, t);
1432         if (sta) {
1433                 wcid = (struct mt76_wcid *)sta->drv_priv;
1434                 wcid_idx = wcid->idx;
1435         } else {
1436                 wcid_idx = le32_get_bits(txwi[1], MT_TXD1_WLAN_IDX);
1437                 wcid = rcu_dereference(dev->mt76.wcid[wcid_idx]);
1438
1439                 if (wcid && wcid->sta) {
1440                         msta = container_of(wcid, struct mt7915_sta, wcid);
1441                         sta = container_of((void *)msta, struct ieee80211_sta,
1442                                           drv_priv);
1443                         spin_lock_bh(&dev->sta_poll_lock);
1444                         if (list_empty(&msta->poll_list))
1445                                 list_add_tail(&msta->poll_list, &dev->sta_poll_list);
1446                         spin_unlock_bh(&dev->sta_poll_lock);
1447                 }
1448         }
1449
1450         if (sta && likely(t->skb->protocol != cpu_to_be16(ETH_P_PAE)))
1451                 mt7915_tx_check_aggr(sta, txwi);
1452
1453         __mt76_tx_complete_skb(mdev, wcid_idx, t->skb, free_list);
1454
1455 out:
1456         t->skb = NULL;
1457         mt76_put_txwi(mdev, t);
1458 }
1459
1460 static void
1461 mt7915_mac_tx_free_prepare(struct mt7915_dev *dev)
1462 {
1463         struct mt76_dev *mdev = &dev->mt76;
1464         struct mt76_phy *mphy_ext = mdev->phy2;
1465
1466         /* clean DMA queues and unmap buffers first */
1467         mt76_queue_tx_cleanup(dev, dev->mphy.q_tx[MT_TXQ_PSD], false);
1468         mt76_queue_tx_cleanup(dev, dev->mphy.q_tx[MT_TXQ_BE], false);
1469         if (mphy_ext) {
1470                 mt76_queue_tx_cleanup(dev, mphy_ext->q_tx[MT_TXQ_PSD], false);
1471                 mt76_queue_tx_cleanup(dev, mphy_ext->q_tx[MT_TXQ_BE], false);
1472         }
1473 }
1474
1475 static void
1476 mt7915_mac_tx_free_done(struct mt7915_dev *dev,
1477                         struct list_head *free_list, bool wake)
1478 {
1479         struct sk_buff *skb, *tmp;
1480
1481         mt7915_mac_sta_poll(dev);
1482
1483         if (wake)
1484                 mt76_set_tx_blocked(&dev->mt76, false);
1485
1486         mt76_worker_schedule(&dev->mt76.tx_worker);
1487
1488         list_for_each_entry_safe(skb, tmp, free_list, list) {
1489                 skb_list_del_init(skb);
1490                 napi_consume_skb(skb, 1);
1491         }
1492 }
1493
1494 static void
1495 mt7915_mac_tx_free(struct mt7915_dev *dev, void *data, int len)
1496 {
1497         struct mt7915_tx_free *free = (struct mt7915_tx_free *)data;
1498         struct mt76_dev *mdev = &dev->mt76;
1499         struct mt76_txwi_cache *txwi;
1500         struct ieee80211_sta *sta = NULL;
1501         LIST_HEAD(free_list);
1502         void *end = data + len;
1503         bool v3, wake = false;
1504         u16 total, count = 0;
1505         u32 txd = le32_to_cpu(free->txd);
1506         __le32 *cur_info;
1507
1508         mt7915_mac_tx_free_prepare(dev);
1509
1510         total = le16_get_bits(free->ctrl, MT_TX_FREE_MSDU_CNT);
1511         v3 = (FIELD_GET(MT_TX_FREE_VER, txd) == 0x4);
1512         if (WARN_ON_ONCE((void *)&free->info[total >> v3] > end))
1513                 return;
1514
1515         for (cur_info = &free->info[0]; count < total; cur_info++) {
1516                 u32 msdu, info = le32_to_cpu(*cur_info);
1517                 u8 i;
1518
1519                 /*
1520                  * 1'b1: new wcid pair.
1521                  * 1'b0: msdu_id with the same 'wcid pair' as above.
1522                  */
1523                 if (info & MT_TX_FREE_PAIR) {
1524                         struct mt7915_sta *msta;
1525                         struct mt76_wcid *wcid;
1526                         u16 idx;
1527
1528                         idx = FIELD_GET(MT_TX_FREE_WLAN_ID, info);
1529                         wcid = rcu_dereference(dev->mt76.wcid[idx]);
1530                         sta = wcid_to_sta(wcid);
1531                         if (!sta)
1532                                 continue;
1533
1534                         msta = container_of(wcid, struct mt7915_sta, wcid);
1535                         spin_lock_bh(&dev->sta_poll_lock);
1536                         if (list_empty(&msta->poll_list))
1537                                 list_add_tail(&msta->poll_list, &dev->sta_poll_list);
1538                         spin_unlock_bh(&dev->sta_poll_lock);
1539                         continue;
1540                 }
1541
1542                 if (v3 && (info & MT_TX_FREE_MPDU_HEADER))
1543                         continue;
1544
1545                 for (i = 0; i < 1 + v3; i++) {
1546                         if (v3) {
1547                                 msdu = (info >> (15 * i)) & MT_TX_FREE_MSDU_ID_V3;
1548                                 if (msdu == MT_TX_FREE_MSDU_ID_V3)
1549                                         continue;
1550                         } else {
1551                                 msdu = FIELD_GET(MT_TX_FREE_MSDU_ID, info);
1552                         }
1553                         count++;
1554                         txwi = mt76_token_release(mdev, msdu, &wake);
1555                         if (!txwi)
1556                                 continue;
1557
1558                         mt7915_txwi_free(dev, txwi, sta, &free_list);
1559                 }
1560         }
1561
1562         mt7915_mac_tx_free_done(dev, &free_list, wake);
1563 }
1564
1565 static void
1566 mt7915_mac_tx_free_v0(struct mt7915_dev *dev, void *data, int len)
1567 {
1568         struct mt7915_tx_free *free = (struct mt7915_tx_free *)data;
1569         struct mt76_dev *mdev = &dev->mt76;
1570         __le16 *info = (__le16 *)free->info;
1571         void *end = data + len;
1572         LIST_HEAD(free_list);
1573         bool wake = false;
1574         u8 i, count;
1575
1576         mt7915_mac_tx_free_prepare(dev);
1577
1578         count = FIELD_GET(MT_TX_FREE_MSDU_CNT_V0, le16_to_cpu(free->ctrl));
1579         if (WARN_ON_ONCE((void *)&info[count] > end))
1580                 return;
1581
1582         for (i = 0; i < count; i++) {
1583                 struct mt76_txwi_cache *txwi;
1584                 u16 msdu = le16_to_cpu(info[i]);
1585
1586                 txwi = mt76_token_release(mdev, msdu, &wake);
1587                 if (!txwi)
1588                         continue;
1589
1590                 mt7915_txwi_free(dev, txwi, NULL, &free_list);
1591         }
1592
1593         mt7915_mac_tx_free_done(dev, &free_list, wake);
1594 }
1595
1596 static bool
1597 mt7915_mac_add_txs_skb(struct mt7915_dev *dev, struct mt76_wcid *wcid, int pid,
1598                        __le32 *txs_data, struct mt76_sta_stats *stats)
1599 {
1600         struct ieee80211_supported_band *sband;
1601         struct mt76_dev *mdev = &dev->mt76;
1602         struct mt76_phy *mphy;
1603         struct ieee80211_tx_info *info;
1604         struct sk_buff_head list;
1605         struct rate_info rate = {};
1606         struct sk_buff *skb;
1607         bool cck = false;
1608         u32 txrate, txs, mode;
1609
1610         mt76_tx_status_lock(mdev, &list);
1611         skb = mt76_tx_status_skb_get(mdev, wcid, pid, &list);
1612         if (!skb)
1613                 goto out_no_skb;
1614
1615         txs = le32_to_cpu(txs_data[0]);
1616
1617         info = IEEE80211_SKB_CB(skb);
1618         if (!(txs & MT_TXS0_ACK_ERROR_MASK))
1619                 info->flags |= IEEE80211_TX_STAT_ACK;
1620
1621         info->status.ampdu_len = 1;
1622         info->status.ampdu_ack_len = !!(info->flags &
1623                                         IEEE80211_TX_STAT_ACK);
1624
1625         info->status.rates[0].idx = -1;
1626
1627         txrate = FIELD_GET(MT_TXS0_TX_RATE, txs);
1628
1629         rate.mcs = FIELD_GET(MT_TX_RATE_IDX, txrate);
1630         rate.nss = FIELD_GET(MT_TX_RATE_NSS, txrate) + 1;
1631
1632         if (rate.nss - 1 < ARRAY_SIZE(stats->tx_nss))
1633                 stats->tx_nss[rate.nss - 1]++;
1634         if (rate.mcs < ARRAY_SIZE(stats->tx_mcs))
1635                 stats->tx_mcs[rate.mcs]++;
1636
1637         mode = FIELD_GET(MT_TX_RATE_MODE, txrate);
1638         switch (mode) {
1639         case MT_PHY_TYPE_CCK:
1640                 cck = true;
1641                 fallthrough;
1642         case MT_PHY_TYPE_OFDM:
1643                 mphy = &dev->mphy;
1644                 if (wcid->ext_phy && dev->mt76.phy2)
1645                         mphy = dev->mt76.phy2;
1646
1647                 if (mphy->chandef.chan->band == NL80211_BAND_5GHZ)
1648                         sband = &mphy->sband_5g.sband;
1649                 else if (mphy->chandef.chan->band == NL80211_BAND_6GHZ)
1650                         sband = &mphy->sband_6g.sband;
1651                 else
1652                         sband = &mphy->sband_2g.sband;
1653
1654                 rate.mcs = mt76_get_rate(mphy->dev, sband, rate.mcs, cck);
1655                 rate.legacy = sband->bitrates[rate.mcs].bitrate;
1656                 break;
1657         case MT_PHY_TYPE_HT:
1658         case MT_PHY_TYPE_HT_GF:
1659                 if (rate.mcs > 31)
1660                         goto out;
1661
1662                 rate.flags = RATE_INFO_FLAGS_MCS;
1663                 if (wcid->rate.flags & RATE_INFO_FLAGS_SHORT_GI)
1664                         rate.flags |= RATE_INFO_FLAGS_SHORT_GI;
1665                 break;
1666         case MT_PHY_TYPE_VHT:
1667                 if (rate.mcs > 9)
1668                         goto out;
1669
1670                 rate.flags = RATE_INFO_FLAGS_VHT_MCS;
1671                 break;
1672         case MT_PHY_TYPE_HE_SU:
1673         case MT_PHY_TYPE_HE_EXT_SU:
1674         case MT_PHY_TYPE_HE_TB:
1675         case MT_PHY_TYPE_HE_MU:
1676                 if (rate.mcs > 11)
1677                         goto out;
1678
1679                 rate.he_gi = wcid->rate.he_gi;
1680                 rate.he_dcm = FIELD_GET(MT_TX_RATE_DCM, txrate);
1681                 rate.flags = RATE_INFO_FLAGS_HE_MCS;
1682                 break;
1683         default:
1684                 goto out;
1685         }
1686
1687         stats->tx_mode[mode]++;
1688
1689         switch (FIELD_GET(MT_TXS0_BW, txs)) {
1690         case IEEE80211_STA_RX_BW_160:
1691                 rate.bw = RATE_INFO_BW_160;
1692                 stats->tx_bw[3]++;
1693                 break;
1694         case IEEE80211_STA_RX_BW_80:
1695                 rate.bw = RATE_INFO_BW_80;
1696                 stats->tx_bw[2]++;
1697                 break;
1698         case IEEE80211_STA_RX_BW_40:
1699                 rate.bw = RATE_INFO_BW_40;
1700                 stats->tx_bw[1]++;
1701                 break;
1702         default:
1703                 rate.bw = RATE_INFO_BW_20;
1704                 stats->tx_bw[0]++;
1705                 break;
1706         }
1707         wcid->rate = rate;
1708
1709 out:
1710         mt76_tx_status_skb_done(mdev, skb, &list);
1711
1712 out_no_skb:
1713         mt76_tx_status_unlock(mdev, &list);
1714
1715         return !!skb;
1716 }
1717
1718 static void mt7915_mac_add_txs(struct mt7915_dev *dev, void *data)
1719 {
1720         struct mt7915_sta *msta = NULL;
1721         struct mt76_wcid *wcid;
1722         __le32 *txs_data = data;
1723         u16 wcidx;
1724         u8 pid;
1725
1726         if (le32_get_bits(txs_data[0], MT_TXS0_TXS_FORMAT) > 1)
1727                 return;
1728
1729         wcidx = le32_get_bits(txs_data[2], MT_TXS2_WCID);
1730         pid = le32_get_bits(txs_data[3], MT_TXS3_PID);
1731
1732         if (pid < MT_PACKET_ID_FIRST)
1733                 return;
1734
1735         if (wcidx >= mt7915_wtbl_size(dev))
1736                 return;
1737
1738         rcu_read_lock();
1739
1740         wcid = rcu_dereference(dev->mt76.wcid[wcidx]);
1741         if (!wcid)
1742                 goto out;
1743
1744         msta = container_of(wcid, struct mt7915_sta, wcid);
1745
1746         mt7915_mac_add_txs_skb(dev, wcid, pid, txs_data, &msta->stats);
1747
1748         if (!wcid->sta)
1749                 goto out;
1750
1751         spin_lock_bh(&dev->sta_poll_lock);
1752         if (list_empty(&msta->poll_list))
1753                 list_add_tail(&msta->poll_list, &dev->sta_poll_list);
1754         spin_unlock_bh(&dev->sta_poll_lock);
1755
1756 out:
1757         rcu_read_unlock();
1758 }
1759
1760 bool mt7915_rx_check(struct mt76_dev *mdev, void *data, int len)
1761 {
1762         struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
1763         __le32 *rxd = (__le32 *)data;
1764         __le32 *end = (__le32 *)&rxd[len / 4];
1765         enum rx_pkt_type type;
1766
1767         type = le32_get_bits(rxd[0], MT_RXD0_PKT_TYPE);
1768
1769         switch (type) {
1770         case PKT_TYPE_TXRX_NOTIFY:
1771                 mt7915_mac_tx_free(dev, data, len);
1772                 return false;
1773         case PKT_TYPE_TXRX_NOTIFY_V0:
1774                 mt7915_mac_tx_free_v0(dev, data, len);
1775                 return false;
1776         case PKT_TYPE_TXS:
1777                 for (rxd += 2; rxd + 8 <= end; rxd += 8)
1778                     mt7915_mac_add_txs(dev, rxd);
1779                 return false;
1780         case PKT_TYPE_RX_FW_MONITOR:
1781                 mt7915_debugfs_rx_fw_monitor(dev, data, len);
1782                 return false;
1783         default:
1784                 return true;
1785         }
1786 }
1787
1788 void mt7915_queue_rx_skb(struct mt76_dev *mdev, enum mt76_rxq_id q,
1789                          struct sk_buff *skb)
1790 {
1791         struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
1792         __le32 *rxd = (__le32 *)skb->data;
1793         __le32 *end = (__le32 *)&skb->data[skb->len];
1794         enum rx_pkt_type type;
1795
1796         type = le32_get_bits(rxd[0], MT_RXD0_PKT_TYPE);
1797
1798         switch (type) {
1799         case PKT_TYPE_TXRX_NOTIFY:
1800                 mt7915_mac_tx_free(dev, skb->data, skb->len);
1801                 napi_consume_skb(skb, 1);
1802                 break;
1803         case PKT_TYPE_TXRX_NOTIFY_V0:
1804                 mt7915_mac_tx_free_v0(dev, skb->data, skb->len);
1805                 napi_consume_skb(skb, 1);
1806                 break;
1807         case PKT_TYPE_RX_EVENT:
1808                 mt7915_mcu_rx_event(dev, skb);
1809                 break;
1810         case PKT_TYPE_TXRXV:
1811                 mt7915_mac_fill_rx_vector(dev, skb);
1812                 break;
1813         case PKT_TYPE_TXS:
1814                 for (rxd += 2; rxd + 8 <= end; rxd += 8)
1815                     mt7915_mac_add_txs(dev, rxd);
1816                 dev_kfree_skb(skb);
1817                 break;
1818         case PKT_TYPE_RX_FW_MONITOR:
1819                 mt7915_debugfs_rx_fw_monitor(dev, skb->data, skb->len);
1820                 dev_kfree_skb(skb);
1821                 break;
1822         case PKT_TYPE_NORMAL:
1823                 if (!mt7915_mac_fill_rx(dev, skb)) {
1824                         mt76_rx(&dev->mt76, q, skb);
1825                         return;
1826                 }
1827                 fallthrough;
1828         default:
1829                 dev_kfree_skb(skb);
1830                 break;
1831         }
1832 }
1833
1834 void mt7915_tx_complete_skb(struct mt76_dev *mdev, struct mt76_queue_entry *e)
1835 {
1836         if (!e->txwi) {
1837                 dev_kfree_skb_any(e->skb);
1838                 return;
1839         }
1840
1841         /* error path */
1842         if (e->skb == DMA_DUMMY_DATA) {
1843                 struct mt76_txwi_cache *t;
1844                 struct mt7915_txp *txp;
1845
1846                 txp = mt7915_txwi_to_txp(mdev, e->txwi);
1847                 t = mt76_token_put(mdev, le16_to_cpu(txp->token));
1848                 e->skb = t ? t->skb : NULL;
1849         }
1850
1851         if (e->skb)
1852                 mt76_tx_complete_skb(mdev, e->wcid, e->skb);
1853 }
1854
1855 void mt7915_mac_cca_stats_reset(struct mt7915_phy *phy)
1856 {
1857         struct mt7915_dev *dev = phy->dev;
1858         u32 reg = MT_WF_PHY_RX_CTRL1(phy->band_idx);
1859
1860         mt76_clear(dev, reg, MT_WF_PHY_RX_CTRL1_STSCNT_EN);
1861         mt76_set(dev, reg, BIT(11) | BIT(9));
1862 }
1863
1864 void mt7915_mac_reset_counters(struct mt7915_phy *phy)
1865 {
1866         struct mt7915_dev *dev = phy->dev;
1867         int i;
1868
1869         for (i = 0; i < 4; i++) {
1870                 mt76_rr(dev, MT_TX_AGG_CNT(phy->band_idx, i));
1871                 mt76_rr(dev, MT_TX_AGG_CNT2(phy->band_idx, i));
1872         }
1873
1874         i = 0;
1875         phy->mt76->survey_time = ktime_get_boottime();
1876         if (phy->band_idx)
1877                 i = ARRAY_SIZE(dev->mt76.aggr_stats) / 2;
1878
1879         memset(&dev->mt76.aggr_stats[i], 0, sizeof(dev->mt76.aggr_stats) / 2);
1880
1881         /* reset airtime counters */
1882         mt76_set(dev, MT_WF_RMAC_MIB_AIRTIME0(phy->band_idx),
1883                  MT_WF_RMAC_MIB_RXTIME_CLR);
1884
1885         mt7915_mcu_get_chan_mib_info(phy, true);
1886 }
1887
1888 void mt7915_mac_set_timing(struct mt7915_phy *phy)
1889 {
1890         s16 coverage_class = phy->coverage_class;
1891         struct mt7915_dev *dev = phy->dev;
1892         struct mt7915_phy *ext_phy = mt7915_ext_phy(dev);
1893         u32 val, reg_offset;
1894         u32 cck = FIELD_PREP(MT_TIMEOUT_VAL_PLCP, 231) |
1895                   FIELD_PREP(MT_TIMEOUT_VAL_CCA, 48);
1896         u32 ofdm = FIELD_PREP(MT_TIMEOUT_VAL_PLCP, 60) |
1897                    FIELD_PREP(MT_TIMEOUT_VAL_CCA, 28);
1898         int offset;
1899         bool a_band = !(phy->mt76->chandef.chan->band == NL80211_BAND_2GHZ);
1900
1901         if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
1902                 return;
1903
1904         if (ext_phy)
1905                 coverage_class = max_t(s16, dev->phy.coverage_class,
1906                                        ext_phy->coverage_class);
1907
1908         mt76_set(dev, MT_ARB_SCR(phy->band_idx),
1909                  MT_ARB_SCR_TX_DISABLE | MT_ARB_SCR_RX_DISABLE);
1910         udelay(1);
1911
1912         offset = 3 * coverage_class;
1913         reg_offset = FIELD_PREP(MT_TIMEOUT_VAL_PLCP, offset) |
1914                      FIELD_PREP(MT_TIMEOUT_VAL_CCA, offset);
1915
1916         mt76_wr(dev, MT_TMAC_CDTR(phy->band_idx), cck + reg_offset);
1917         mt76_wr(dev, MT_TMAC_ODTR(phy->band_idx), ofdm + reg_offset);
1918         mt76_wr(dev, MT_TMAC_ICR0(phy->band_idx),
1919                 FIELD_PREP(MT_IFS_EIFS_OFDM, a_band ? 84 : 78) |
1920                 FIELD_PREP(MT_IFS_RIFS, 2) |
1921                 FIELD_PREP(MT_IFS_SIFS, 10) |
1922                 FIELD_PREP(MT_IFS_SLOT, phy->slottime));
1923
1924         mt76_wr(dev, MT_TMAC_ICR1(phy->band_idx),
1925                 FIELD_PREP(MT_IFS_EIFS_CCK, 314));
1926
1927         if (phy->slottime < 20 || a_band)
1928                 val = MT7915_CFEND_RATE_DEFAULT;
1929         else
1930                 val = MT7915_CFEND_RATE_11B;
1931
1932         mt76_rmw_field(dev, MT_AGG_ACR0(phy->band_idx), MT_AGG_ACR_CFEND_RATE, val);
1933         mt76_clear(dev, MT_ARB_SCR(phy->band_idx),
1934                    MT_ARB_SCR_TX_DISABLE | MT_ARB_SCR_RX_DISABLE);
1935 }
1936
1937 void mt7915_mac_enable_nf(struct mt7915_dev *dev, bool ext_phy)
1938 {
1939         u32 reg;
1940
1941         reg = is_mt7915(&dev->mt76) ? MT_WF_PHY_RXTD12(ext_phy) :
1942                 MT_WF_PHY_RXTD12_MT7916(ext_phy);
1943         mt76_set(dev, reg,
1944                  MT_WF_PHY_RXTD12_IRPI_SW_CLR_ONLY |
1945                  MT_WF_PHY_RXTD12_IRPI_SW_CLR);
1946
1947         reg = is_mt7915(&dev->mt76) ? MT_WF_PHY_RX_CTRL1(ext_phy) :
1948                 MT_WF_PHY_RX_CTRL1_MT7916(ext_phy);
1949         mt76_set(dev, reg, FIELD_PREP(MT_WF_PHY_RX_CTRL1_IPI_EN, 0x5));
1950 }
1951
1952 static u8
1953 mt7915_phy_get_nf(struct mt7915_phy *phy, int idx)
1954 {
1955         static const u8 nf_power[] = { 92, 89, 86, 83, 80, 75, 70, 65, 60, 55, 52 };
1956         struct mt7915_dev *dev = phy->dev;
1957         u32 val, sum = 0, n = 0;
1958         int nss, i;
1959
1960         for (nss = 0; nss < hweight8(phy->mt76->chainmask); nss++) {
1961                 u32 reg = is_mt7915(&dev->mt76) ?
1962                         MT_WF_IRPI_NSS(0, nss + (idx << dev->dbdc_support)) :
1963                         MT_WF_IRPI_NSS_MT7916(idx, nss);
1964
1965                 for (i = 0; i < ARRAY_SIZE(nf_power); i++, reg += 4) {
1966                         val = mt76_rr(dev, reg);
1967                         sum += val * nf_power[i];
1968                         n += val;
1969                 }
1970         }
1971
1972         if (!n)
1973                 return 0;
1974
1975         return sum / n;
1976 }
1977
1978 void mt7915_update_channel(struct mt76_phy *mphy)
1979 {
1980         struct mt7915_phy *phy = (struct mt7915_phy *)mphy->priv;
1981         struct mt76_channel_state *state = mphy->chan_state;
1982         int nf;
1983
1984         mt7915_mcu_get_chan_mib_info(phy, false);
1985
1986         nf = mt7915_phy_get_nf(phy, phy->band_idx);
1987         if (!phy->noise)
1988                 phy->noise = nf << 4;
1989         else if (nf)
1990                 phy->noise += nf - (phy->noise >> 4);
1991
1992         state->noise = -(phy->noise >> 4);
1993 }
1994
1995 static bool
1996 mt7915_wait_reset_state(struct mt7915_dev *dev, u32 state)
1997 {
1998         bool ret;
1999
2000         ret = wait_event_timeout(dev->reset_wait,
2001                                  (READ_ONCE(dev->reset_state) & state),
2002                                  MT7915_RESET_TIMEOUT);
2003
2004         WARN(!ret, "Timeout waiting for MCU reset state %x\n", state);
2005         return ret;
2006 }
2007
2008 static void
2009 mt7915_update_vif_beacon(void *priv, u8 *mac, struct ieee80211_vif *vif)
2010 {
2011         struct ieee80211_hw *hw = priv;
2012
2013         switch (vif->type) {
2014         case NL80211_IFTYPE_MESH_POINT:
2015         case NL80211_IFTYPE_ADHOC:
2016         case NL80211_IFTYPE_AP:
2017                 mt7915_mcu_add_beacon(hw, vif, vif->bss_conf.enable_beacon,
2018                                       BSS_CHANGED_BEACON_ENABLED);
2019                 break;
2020         default:
2021                 break;
2022         }
2023 }
2024
2025 static void
2026 mt7915_update_beacons(struct mt7915_dev *dev)
2027 {
2028         ieee80211_iterate_active_interfaces(dev->mt76.hw,
2029                 IEEE80211_IFACE_ITER_RESUME_ALL,
2030                 mt7915_update_vif_beacon, dev->mt76.hw);
2031
2032         if (!dev->mt76.phy2)
2033                 return;
2034
2035         ieee80211_iterate_active_interfaces(dev->mt76.phy2->hw,
2036                 IEEE80211_IFACE_ITER_RESUME_ALL,
2037                 mt7915_update_vif_beacon, dev->mt76.phy2->hw);
2038 }
2039
2040 static void
2041 mt7915_dma_reset(struct mt7915_dev *dev)
2042 {
2043         struct mt76_phy *mphy_ext = dev->mt76.phy2;
2044         u32 hif1_ofs = MT_WFDMA0_PCIE1(0) - MT_WFDMA0(0);
2045         int i;
2046
2047         mt76_clear(dev, MT_WFDMA0_GLO_CFG,
2048                    MT_WFDMA0_GLO_CFG_TX_DMA_EN |
2049                    MT_WFDMA0_GLO_CFG_RX_DMA_EN);
2050
2051         if (is_mt7915(&dev->mt76))
2052                 mt76_clear(dev, MT_WFDMA1_GLO_CFG,
2053                            MT_WFDMA1_GLO_CFG_TX_DMA_EN |
2054                            MT_WFDMA1_GLO_CFG_RX_DMA_EN);
2055         if (dev->hif2) {
2056                 mt76_clear(dev, MT_WFDMA0_GLO_CFG + hif1_ofs,
2057                            MT_WFDMA0_GLO_CFG_TX_DMA_EN |
2058                            MT_WFDMA0_GLO_CFG_RX_DMA_EN);
2059
2060                 if (is_mt7915(&dev->mt76))
2061                         mt76_clear(dev, MT_WFDMA1_GLO_CFG + hif1_ofs,
2062                                    MT_WFDMA1_GLO_CFG_TX_DMA_EN |
2063                                    MT_WFDMA1_GLO_CFG_RX_DMA_EN);
2064         }
2065
2066         usleep_range(1000, 2000);
2067
2068         for (i = 0; i < __MT_TXQ_MAX; i++) {
2069                 mt76_queue_tx_cleanup(dev, dev->mphy.q_tx[i], true);
2070                 if (mphy_ext)
2071                         mt76_queue_tx_cleanup(dev, mphy_ext->q_tx[i], true);
2072         }
2073
2074         for (i = 0; i < __MT_MCUQ_MAX; i++)
2075                 mt76_queue_tx_cleanup(dev, dev->mt76.q_mcu[i], true);
2076
2077         mt76_for_each_q_rx(&dev->mt76, i)
2078                 mt76_queue_rx_reset(dev, i);
2079
2080         mt76_tx_status_check(&dev->mt76, true);
2081
2082         /* re-init prefetch settings after reset */
2083         mt7915_dma_prefetch(dev);
2084
2085         mt76_set(dev, MT_WFDMA0_GLO_CFG,
2086                  MT_WFDMA0_GLO_CFG_TX_DMA_EN | MT_WFDMA0_GLO_CFG_RX_DMA_EN);
2087         if (is_mt7915(&dev->mt76))
2088                 mt76_set(dev, MT_WFDMA1_GLO_CFG,
2089                          MT_WFDMA1_GLO_CFG_TX_DMA_EN |
2090                          MT_WFDMA1_GLO_CFG_RX_DMA_EN |
2091                          MT_WFDMA1_GLO_CFG_OMIT_TX_INFO |
2092                          MT_WFDMA1_GLO_CFG_OMIT_RX_INFO);
2093         if (dev->hif2) {
2094                 mt76_set(dev, MT_WFDMA0_GLO_CFG + hif1_ofs,
2095                          MT_WFDMA0_GLO_CFG_TX_DMA_EN |
2096                          MT_WFDMA0_GLO_CFG_RX_DMA_EN);
2097
2098                 if (is_mt7915(&dev->mt76))
2099                         mt76_set(dev, MT_WFDMA1_GLO_CFG + hif1_ofs,
2100                                  MT_WFDMA1_GLO_CFG_TX_DMA_EN |
2101                                  MT_WFDMA1_GLO_CFG_RX_DMA_EN |
2102                                  MT_WFDMA1_GLO_CFG_OMIT_TX_INFO |
2103                                  MT_WFDMA1_GLO_CFG_OMIT_RX_INFO);
2104         }
2105 }
2106
2107 void mt7915_tx_token_put(struct mt7915_dev *dev)
2108 {
2109         struct mt76_txwi_cache *txwi;
2110         int id;
2111
2112         spin_lock_bh(&dev->mt76.token_lock);
2113         idr_for_each_entry(&dev->mt76.token, txwi, id) {
2114                 mt7915_txwi_free(dev, txwi, NULL, NULL);
2115                 dev->mt76.token_count--;
2116         }
2117         spin_unlock_bh(&dev->mt76.token_lock);
2118         idr_destroy(&dev->mt76.token);
2119 }
2120
2121 /* system error recovery */
2122 void mt7915_mac_reset_work(struct work_struct *work)
2123 {
2124         struct mt7915_phy *phy2;
2125         struct mt76_phy *ext_phy;
2126         struct mt7915_dev *dev;
2127
2128         dev = container_of(work, struct mt7915_dev, reset_work);
2129         ext_phy = dev->mt76.phy2;
2130         phy2 = ext_phy ? ext_phy->priv : NULL;
2131
2132         if (!(READ_ONCE(dev->reset_state) & MT_MCU_CMD_STOP_DMA))
2133                 return;
2134
2135         ieee80211_stop_queues(mt76_hw(dev));
2136         if (ext_phy)
2137                 ieee80211_stop_queues(ext_phy->hw);
2138
2139         set_bit(MT76_RESET, &dev->mphy.state);
2140         set_bit(MT76_MCU_RESET, &dev->mphy.state);
2141         wake_up(&dev->mt76.mcu.wait);
2142         cancel_delayed_work_sync(&dev->mphy.mac_work);
2143         if (phy2) {
2144                 set_bit(MT76_RESET, &phy2->mt76->state);
2145                 cancel_delayed_work_sync(&phy2->mt76->mac_work);
2146         }
2147         mt76_worker_disable(&dev->mt76.tx_worker);
2148         napi_disable(&dev->mt76.napi[0]);
2149         napi_disable(&dev->mt76.napi[1]);
2150         napi_disable(&dev->mt76.napi[2]);
2151         napi_disable(&dev->mt76.tx_napi);
2152
2153         mutex_lock(&dev->mt76.mutex);
2154
2155         mt76_wr(dev, MT_MCU_INT_EVENT, MT_MCU_INT_EVENT_DMA_STOPPED);
2156
2157         if (mt7915_wait_reset_state(dev, MT_MCU_CMD_RESET_DONE)) {
2158                 mt7915_dma_reset(dev);
2159
2160                 mt7915_tx_token_put(dev);
2161                 idr_init(&dev->mt76.token);
2162
2163                 mt76_wr(dev, MT_MCU_INT_EVENT, MT_MCU_INT_EVENT_DMA_INIT);
2164                 mt7915_wait_reset_state(dev, MT_MCU_CMD_RECOVERY_DONE);
2165         }
2166
2167         clear_bit(MT76_MCU_RESET, &dev->mphy.state);
2168         clear_bit(MT76_RESET, &dev->mphy.state);
2169         if (phy2)
2170                 clear_bit(MT76_RESET, &phy2->mt76->state);
2171
2172         local_bh_disable();
2173         napi_enable(&dev->mt76.napi[0]);
2174         napi_schedule(&dev->mt76.napi[0]);
2175
2176         napi_enable(&dev->mt76.napi[1]);
2177         napi_schedule(&dev->mt76.napi[1]);
2178
2179         napi_enable(&dev->mt76.napi[2]);
2180         napi_schedule(&dev->mt76.napi[2]);
2181         local_bh_enable();
2182
2183         tasklet_schedule(&dev->irq_tasklet);
2184
2185         mt76_wr(dev, MT_MCU_INT_EVENT, MT_MCU_INT_EVENT_RESET_DONE);
2186         mt7915_wait_reset_state(dev, MT_MCU_CMD_NORMAL_STATE);
2187
2188         mt76_worker_enable(&dev->mt76.tx_worker);
2189
2190         napi_enable(&dev->mt76.tx_napi);
2191         napi_schedule(&dev->mt76.tx_napi);
2192
2193         ieee80211_wake_queues(mt76_hw(dev));
2194         if (ext_phy)
2195                 ieee80211_wake_queues(ext_phy->hw);
2196
2197         mutex_unlock(&dev->mt76.mutex);
2198
2199         mt7915_update_beacons(dev);
2200
2201         ieee80211_queue_delayed_work(mt76_hw(dev), &dev->mphy.mac_work,
2202                                      MT7915_WATCHDOG_TIME);
2203         if (phy2)
2204                 ieee80211_queue_delayed_work(ext_phy->hw,
2205                                              &phy2->mt76->mac_work,
2206                                              MT7915_WATCHDOG_TIME);
2207 }
2208
2209 void mt7915_mac_update_stats(struct mt7915_phy *phy)
2210 {
2211         struct mt7915_dev *dev = phy->dev;
2212         struct mib_stats *mib = &phy->mib;
2213         int i, aggr0, aggr1, cnt;
2214         u32 val;
2215
2216         cnt = mt76_rr(dev, MT_MIB_SDR3(phy->band_idx));
2217         mib->fcs_err_cnt += is_mt7915(&dev->mt76) ? FIELD_GET(MT_MIB_SDR3_FCS_ERR_MASK, cnt) :
2218                 FIELD_GET(MT_MIB_SDR3_FCS_ERR_MASK_MT7916, cnt);
2219
2220         cnt = mt76_rr(dev, MT_MIB_SDR4(phy->band_idx));
2221         mib->rx_fifo_full_cnt += FIELD_GET(MT_MIB_SDR4_RX_FIFO_FULL_MASK, cnt);
2222
2223         cnt = mt76_rr(dev, MT_MIB_SDR5(phy->band_idx));
2224         mib->rx_mpdu_cnt += cnt;
2225
2226         cnt = mt76_rr(dev, MT_MIB_SDR6(phy->band_idx));
2227         mib->channel_idle_cnt += FIELD_GET(MT_MIB_SDR6_CHANNEL_IDL_CNT_MASK, cnt);
2228
2229         cnt = mt76_rr(dev, MT_MIB_SDR7(phy->band_idx));
2230         mib->rx_vector_mismatch_cnt += FIELD_GET(MT_MIB_SDR7_RX_VECTOR_MISMATCH_CNT_MASK, cnt);
2231
2232         cnt = mt76_rr(dev, MT_MIB_SDR8(phy->band_idx));
2233         mib->rx_delimiter_fail_cnt += FIELD_GET(MT_MIB_SDR8_RX_DELIMITER_FAIL_CNT_MASK, cnt);
2234
2235         cnt = mt76_rr(dev, MT_MIB_SDR11(phy->band_idx));
2236         mib->rx_len_mismatch_cnt += FIELD_GET(MT_MIB_SDR11_RX_LEN_MISMATCH_CNT_MASK, cnt);
2237
2238         cnt = mt76_rr(dev, MT_MIB_SDR12(phy->band_idx));
2239         mib->tx_ampdu_cnt += cnt;
2240
2241         cnt = mt76_rr(dev, MT_MIB_SDR13(phy->band_idx));
2242         mib->tx_stop_q_empty_cnt += FIELD_GET(MT_MIB_SDR13_TX_STOP_Q_EMPTY_CNT_MASK, cnt);
2243
2244         cnt = mt76_rr(dev, MT_MIB_SDR14(phy->band_idx));
2245         mib->tx_mpdu_attempts_cnt += is_mt7915(&dev->mt76) ?
2246                 FIELD_GET(MT_MIB_SDR14_TX_MPDU_ATTEMPTS_CNT_MASK, cnt) :
2247                 FIELD_GET(MT_MIB_SDR14_TX_MPDU_ATTEMPTS_CNT_MASK_MT7916, cnt);
2248
2249         cnt = mt76_rr(dev, MT_MIB_SDR15(phy->band_idx));
2250         mib->tx_mpdu_success_cnt += is_mt7915(&dev->mt76) ?
2251                 FIELD_GET(MT_MIB_SDR15_TX_MPDU_SUCCESS_CNT_MASK, cnt) :
2252                 FIELD_GET(MT_MIB_SDR15_TX_MPDU_SUCCESS_CNT_MASK_MT7916, cnt);
2253
2254         cnt = mt76_rr(dev, MT_MIB_SDR22(phy->band_idx));
2255         mib->rx_ampdu_cnt += cnt;
2256
2257         cnt = mt76_rr(dev, MT_MIB_SDR23(phy->band_idx));
2258         mib->rx_ampdu_bytes_cnt += cnt;
2259
2260         cnt = mt76_rr(dev, MT_MIB_SDR24(phy->band_idx));
2261         mib->rx_ampdu_valid_subframe_cnt += is_mt7915(&dev->mt76) ?
2262                 FIELD_GET(MT_MIB_SDR24_RX_AMPDU_SF_CNT_MASK, cnt) :
2263                 FIELD_GET(MT_MIB_SDR24_RX_AMPDU_SF_CNT_MASK_MT7916, cnt);
2264
2265         cnt = mt76_rr(dev, MT_MIB_SDR25(phy->band_idx));
2266         mib->rx_ampdu_valid_subframe_bytes_cnt += cnt;
2267
2268         cnt = mt76_rr(dev, MT_MIB_SDR27(phy->band_idx));
2269         mib->tx_rwp_fail_cnt += FIELD_GET(MT_MIB_SDR27_TX_RWP_FAIL_CNT_MASK, cnt);
2270
2271         cnt = mt76_rr(dev, MT_MIB_SDR28(phy->band_idx));
2272         mib->tx_rwp_need_cnt += FIELD_GET(MT_MIB_SDR28_TX_RWP_NEED_CNT_MASK, cnt);
2273
2274         cnt = mt76_rr(dev, MT_MIB_SDR29(phy->band_idx));
2275         mib->rx_pfdrop_cnt += is_mt7915(&dev->mt76) ?
2276                 FIELD_GET(MT_MIB_SDR29_RX_PFDROP_CNT_MASK, cnt) :
2277                 FIELD_GET(MT_MIB_SDR29_RX_PFDROP_CNT_MASK_MT7916, cnt);
2278
2279         cnt = mt76_rr(dev, MT_MIB_SDRVEC(phy->band_idx));
2280         mib->rx_vec_queue_overflow_drop_cnt += is_mt7915(&dev->mt76) ?
2281                 FIELD_GET(MT_MIB_SDR30_RX_VEC_QUEUE_OVERFLOW_DROP_CNT_MASK, cnt) :
2282                 FIELD_GET(MT_MIB_SDR30_RX_VEC_QUEUE_OVERFLOW_DROP_CNT_MASK_MT7916, cnt);
2283
2284         cnt = mt76_rr(dev, MT_MIB_SDR31(phy->band_idx));
2285         mib->rx_ba_cnt += cnt;
2286
2287         cnt = mt76_rr(dev, MT_MIB_SDRMUBF(phy->band_idx));
2288         mib->tx_bf_cnt += FIELD_GET(MT_MIB_MU_BF_TX_CNT, cnt);
2289
2290         cnt = mt76_rr(dev, MT_MIB_DR8(phy->band_idx));
2291         mib->tx_mu_mpdu_cnt += cnt;
2292
2293         cnt = mt76_rr(dev, MT_MIB_DR9(phy->band_idx));
2294         mib->tx_mu_acked_mpdu_cnt += cnt;
2295
2296         cnt = mt76_rr(dev, MT_MIB_DR11(phy->band_idx));
2297         mib->tx_su_acked_mpdu_cnt += cnt;
2298
2299         cnt = mt76_rr(dev, MT_ETBF_PAR_RPT0(phy->band_idx));
2300         mib->tx_bf_rx_fb_bw = FIELD_GET(MT_ETBF_PAR_RPT0_FB_BW, cnt);
2301         mib->tx_bf_rx_fb_nc_cnt += FIELD_GET(MT_ETBF_PAR_RPT0_FB_NC, cnt);
2302         mib->tx_bf_rx_fb_nr_cnt += FIELD_GET(MT_ETBF_PAR_RPT0_FB_NR, cnt);
2303
2304         for (i = 0; i < ARRAY_SIZE(mib->tx_amsdu); i++) {
2305                 cnt = mt76_rr(dev, MT_PLE_AMSDU_PACK_MSDU_CNT(i));
2306                 mib->tx_amsdu[i] += cnt;
2307                 mib->tx_amsdu_cnt += cnt;
2308         }
2309
2310         aggr0 = phy->band_idx ? ARRAY_SIZE(dev->mt76.aggr_stats) / 2 : 0;
2311         if (is_mt7915(&dev->mt76)) {
2312                 for (i = 0, aggr1 = aggr0 + 4; i < 4; i++) {
2313                         val = mt76_rr(dev, MT_MIB_MB_SDR1(phy->band_idx, (i << 4)));
2314                         mib->ba_miss_cnt += FIELD_GET(MT_MIB_BA_MISS_COUNT_MASK, val);
2315                         mib->ack_fail_cnt +=
2316                                 FIELD_GET(MT_MIB_ACK_FAIL_COUNT_MASK, val);
2317
2318                         val = mt76_rr(dev, MT_MIB_MB_SDR0(phy->band_idx, (i << 4)));
2319                         mib->rts_cnt += FIELD_GET(MT_MIB_RTS_COUNT_MASK, val);
2320                         mib->rts_retries_cnt +=
2321                                 FIELD_GET(MT_MIB_RTS_RETRIES_COUNT_MASK, val);
2322
2323                         val = mt76_rr(dev, MT_TX_AGG_CNT(phy->band_idx, i));
2324                         dev->mt76.aggr_stats[aggr0++] += val & 0xffff;
2325                         dev->mt76.aggr_stats[aggr0++] += val >> 16;
2326
2327                         val = mt76_rr(dev, MT_TX_AGG_CNT2(phy->band_idx, i));
2328                         dev->mt76.aggr_stats[aggr1++] += val & 0xffff;
2329                         dev->mt76.aggr_stats[aggr1++] += val >> 16;
2330                 }
2331
2332                 cnt = mt76_rr(dev, MT_MIB_SDR32(phy->band_idx));
2333                 mib->tx_pkt_ebf_cnt += FIELD_GET(MT_MIB_SDR32_TX_PKT_EBF_CNT, cnt);
2334
2335                 cnt = mt76_rr(dev, MT_MIB_SDR33(phy->band_idx));
2336                 mib->tx_pkt_ibf_cnt += FIELD_GET(MT_MIB_SDR33_TX_PKT_IBF_CNT, cnt);
2337
2338                 cnt = mt76_rr(dev, MT_ETBF_TX_APP_CNT(phy->band_idx));
2339                 mib->tx_bf_ibf_ppdu_cnt += FIELD_GET(MT_ETBF_TX_IBF_CNT, cnt);
2340                 mib->tx_bf_ebf_ppdu_cnt += FIELD_GET(MT_ETBF_TX_EBF_CNT, cnt);
2341
2342                 cnt = mt76_rr(dev, MT_ETBF_TX_NDP_BFRP(phy->band_idx));
2343                 mib->tx_bf_fb_cpl_cnt += FIELD_GET(MT_ETBF_TX_FB_CPL, cnt);
2344                 mib->tx_bf_fb_trig_cnt += FIELD_GET(MT_ETBF_TX_FB_TRI, cnt);
2345
2346                 cnt = mt76_rr(dev, MT_ETBF_RX_FB_CNT(phy->band_idx));
2347                 mib->tx_bf_rx_fb_all_cnt += FIELD_GET(MT_ETBF_RX_FB_ALL, cnt);
2348                 mib->tx_bf_rx_fb_he_cnt += FIELD_GET(MT_ETBF_RX_FB_HE, cnt);
2349                 mib->tx_bf_rx_fb_vht_cnt += FIELD_GET(MT_ETBF_RX_FB_VHT, cnt);
2350                 mib->tx_bf_rx_fb_ht_cnt += FIELD_GET(MT_ETBF_RX_FB_HT, cnt);
2351         } else {
2352                 for (i = 0; i < 2; i++) {
2353                         /* rts count */
2354                         val = mt76_rr(dev, MT_MIB_MB_SDR0(phy->band_idx, (i << 2)));
2355                         mib->rts_cnt += FIELD_GET(GENMASK(15, 0), val);
2356                         mib->rts_cnt += FIELD_GET(GENMASK(31, 16), val);
2357
2358                         /* rts retry count */
2359                         val = mt76_rr(dev, MT_MIB_MB_SDR1(phy->band_idx, (i << 2)));
2360                         mib->rts_retries_cnt += FIELD_GET(GENMASK(15, 0), val);
2361                         mib->rts_retries_cnt += FIELD_GET(GENMASK(31, 16), val);
2362
2363                         /* ba miss count */
2364                         val = mt76_rr(dev, MT_MIB_MB_SDR2(phy->band_idx, (i << 2)));
2365                         mib->ba_miss_cnt += FIELD_GET(GENMASK(15, 0), val);
2366                         mib->ba_miss_cnt += FIELD_GET(GENMASK(31, 16), val);
2367
2368                         /* ack fail count */
2369                         val = mt76_rr(dev, MT_MIB_MB_BFTF(phy->band_idx, (i << 2)));
2370                         mib->ack_fail_cnt += FIELD_GET(GENMASK(15, 0), val);
2371                         mib->ack_fail_cnt += FIELD_GET(GENMASK(31, 16), val);
2372                 }
2373
2374                 for (i = 0; i < 8; i++) {
2375                         val = mt76_rr(dev, MT_TX_AGG_CNT(phy->band_idx, i));
2376                         dev->mt76.aggr_stats[aggr0++] += FIELD_GET(GENMASK(15, 0), val);
2377                         dev->mt76.aggr_stats[aggr0++] += FIELD_GET(GENMASK(31, 16), val);
2378                 }
2379
2380                 cnt = mt76_rr(dev, MT_MIB_SDR32(phy->band_idx));
2381                 mib->tx_pkt_ibf_cnt += FIELD_GET(MT_MIB_SDR32_TX_PKT_IBF_CNT, cnt);
2382                 mib->tx_bf_ibf_ppdu_cnt += FIELD_GET(MT_MIB_SDR32_TX_PKT_IBF_CNT, cnt);
2383                 mib->tx_pkt_ebf_cnt += FIELD_GET(MT_MIB_SDR32_TX_PKT_EBF_CNT, cnt);
2384                 mib->tx_bf_ebf_ppdu_cnt += FIELD_GET(MT_MIB_SDR32_TX_PKT_EBF_CNT, cnt);
2385
2386                 cnt = mt76_rr(dev, MT_MIB_BFCR7(phy->band_idx));
2387                 mib->tx_bf_fb_cpl_cnt += FIELD_GET(MT_MIB_BFCR7_BFEE_TX_FB_CPL, cnt);
2388
2389                 cnt = mt76_rr(dev, MT_MIB_BFCR2(phy->band_idx));
2390                 mib->tx_bf_fb_trig_cnt += FIELD_GET(MT_MIB_BFCR2_BFEE_TX_FB_TRIG, cnt);
2391
2392                 cnt = mt76_rr(dev, MT_MIB_BFCR0(phy->band_idx));
2393                 mib->tx_bf_rx_fb_vht_cnt += FIELD_GET(MT_MIB_BFCR0_RX_FB_VHT, cnt);
2394                 mib->tx_bf_rx_fb_all_cnt += FIELD_GET(MT_MIB_BFCR0_RX_FB_VHT, cnt);
2395                 mib->tx_bf_rx_fb_ht_cnt += FIELD_GET(MT_MIB_BFCR0_RX_FB_HT, cnt);
2396                 mib->tx_bf_rx_fb_all_cnt += FIELD_GET(MT_MIB_BFCR0_RX_FB_HT, cnt);
2397
2398                 cnt = mt76_rr(dev, MT_MIB_BFCR1(phy->band_idx));
2399                 mib->tx_bf_rx_fb_he_cnt += FIELD_GET(MT_MIB_BFCR1_RX_FB_HE, cnt);
2400                 mib->tx_bf_rx_fb_all_cnt += FIELD_GET(MT_MIB_BFCR1_RX_FB_HE, cnt);
2401         }
2402 }
2403
2404 static void mt7915_mac_severe_check(struct mt7915_phy *phy)
2405 {
2406         struct mt7915_dev *dev = phy->dev;
2407         bool ext_phy = phy != &dev->phy;
2408         u32 trb;
2409
2410         if (!phy->omac_mask)
2411                 return;
2412
2413         /* In rare cases, TRB pointers might be out of sync leads to RMAC
2414          * stopping Rx, so check status periodically to see if TRB hardware
2415          * requires minimal recovery.
2416          */
2417         trb = mt76_rr(dev, MT_TRB_RXPSR0(phy->band_idx));
2418
2419         if ((FIELD_GET(MT_TRB_RXPSR0_RX_RMAC_PTR, trb) !=
2420              FIELD_GET(MT_TRB_RXPSR0_RX_WTBL_PTR, trb)) &&
2421             (FIELD_GET(MT_TRB_RXPSR0_RX_RMAC_PTR, phy->trb_ts) !=
2422              FIELD_GET(MT_TRB_RXPSR0_RX_WTBL_PTR, phy->trb_ts)) &&
2423             trb == phy->trb_ts)
2424                 mt7915_mcu_set_ser(dev, SER_RECOVER, SER_SET_RECOVER_L3_RX_ABORT,
2425                                    ext_phy);
2426
2427         phy->trb_ts = trb;
2428 }
2429
2430 void mt7915_mac_sta_rc_work(struct work_struct *work)
2431 {
2432         struct mt7915_dev *dev = container_of(work, struct mt7915_dev, rc_work);
2433         struct ieee80211_sta *sta;
2434         struct ieee80211_vif *vif;
2435         struct mt7915_sta *msta;
2436         u32 changed;
2437         LIST_HEAD(list);
2438
2439         spin_lock_bh(&dev->sta_poll_lock);
2440         list_splice_init(&dev->sta_rc_list, &list);
2441
2442         while (!list_empty(&list)) {
2443                 msta = list_first_entry(&list, struct mt7915_sta, rc_list);
2444                 list_del_init(&msta->rc_list);
2445                 changed = msta->changed;
2446                 msta->changed = 0;
2447                 spin_unlock_bh(&dev->sta_poll_lock);
2448
2449                 sta = container_of((void *)msta, struct ieee80211_sta, drv_priv);
2450                 vif = container_of((void *)msta->vif, struct ieee80211_vif, drv_priv);
2451
2452                 if (changed & (IEEE80211_RC_SUPP_RATES_CHANGED |
2453                                IEEE80211_RC_NSS_CHANGED |
2454                                IEEE80211_RC_BW_CHANGED))
2455                         mt7915_mcu_add_rate_ctrl(dev, vif, sta, true);
2456
2457                 if (changed & IEEE80211_RC_SMPS_CHANGED)
2458                         mt7915_mcu_add_smps(dev, vif, sta);
2459
2460                 spin_lock_bh(&dev->sta_poll_lock);
2461         }
2462
2463         spin_unlock_bh(&dev->sta_poll_lock);
2464 }
2465
2466 void mt7915_mac_work(struct work_struct *work)
2467 {
2468         struct mt7915_phy *phy;
2469         struct mt76_phy *mphy;
2470
2471         mphy = (struct mt76_phy *)container_of(work, struct mt76_phy,
2472                                                mac_work.work);
2473         phy = mphy->priv;
2474
2475         mutex_lock(&mphy->dev->mutex);
2476
2477         mt76_update_survey(mphy);
2478         if (++mphy->mac_work_count == 5) {
2479                 mphy->mac_work_count = 0;
2480
2481                 mt7915_mac_update_stats(phy);
2482                 mt7915_mac_severe_check(phy);
2483         }
2484
2485         mutex_unlock(&mphy->dev->mutex);
2486
2487         mt76_tx_status_check(mphy->dev, false);
2488
2489         ieee80211_queue_delayed_work(mphy->hw, &mphy->mac_work,
2490                                      MT7915_WATCHDOG_TIME);
2491 }
2492
2493 static void mt7915_dfs_stop_radar_detector(struct mt7915_phy *phy)
2494 {
2495         struct mt7915_dev *dev = phy->dev;
2496
2497         if (phy->rdd_state & BIT(0))
2498                 mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_STOP, 0,
2499                                         MT_RX_SEL0, 0);
2500         if (phy->rdd_state & BIT(1))
2501                 mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_STOP, 1,
2502                                         MT_RX_SEL0, 0);
2503 }
2504
2505 static int mt7915_dfs_start_rdd(struct mt7915_dev *dev, int chain)
2506 {
2507         int err, region;
2508
2509         switch (dev->mt76.region) {
2510         case NL80211_DFS_ETSI:
2511                 region = 0;
2512                 break;
2513         case NL80211_DFS_JP:
2514                 region = 2;
2515                 break;
2516         case NL80211_DFS_FCC:
2517         default:
2518                 region = 1;
2519                 break;
2520         }
2521
2522         err = mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_START, chain,
2523                                       MT_RX_SEL0, region);
2524         if (err < 0)
2525                 return err;
2526
2527         return mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_DET_MODE, chain,
2528                                        MT_RX_SEL0, 1);
2529 }
2530
2531 static int mt7915_dfs_start_radar_detector(struct mt7915_phy *phy)
2532 {
2533         struct cfg80211_chan_def *chandef = &phy->mt76->chandef;
2534         struct mt7915_dev *dev = phy->dev;
2535         int err;
2536
2537         /* start CAC */
2538         err = mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_CAC_START, phy->band_idx,
2539                                       MT_RX_SEL0, 0);
2540         if (err < 0)
2541                 return err;
2542
2543         err = mt7915_dfs_start_rdd(dev, phy->band_idx);
2544         if (err < 0)
2545                 return err;
2546
2547         phy->rdd_state |= BIT(phy->band_idx);
2548
2549         if (!is_mt7915(&dev->mt76))
2550                 return 0;
2551
2552         if (chandef->width == NL80211_CHAN_WIDTH_160 ||
2553             chandef->width == NL80211_CHAN_WIDTH_80P80) {
2554                 err = mt7915_dfs_start_rdd(dev, 1);
2555                 if (err < 0)
2556                         return err;
2557
2558                 phy->rdd_state |= BIT(1);
2559         }
2560
2561         return 0;
2562 }
2563
2564 static int
2565 mt7915_dfs_init_radar_specs(struct mt7915_phy *phy)
2566 {
2567         const struct mt7915_dfs_radar_spec *radar_specs;
2568         struct mt7915_dev *dev = phy->dev;
2569         int err, i;
2570
2571         switch (dev->mt76.region) {
2572         case NL80211_DFS_FCC:
2573                 radar_specs = &fcc_radar_specs;
2574                 err = mt7915_mcu_set_fcc5_lpn(dev, 8);
2575                 if (err < 0)
2576                         return err;
2577                 break;
2578         case NL80211_DFS_ETSI:
2579                 radar_specs = &etsi_radar_specs;
2580                 break;
2581         case NL80211_DFS_JP:
2582                 radar_specs = &jp_radar_specs;
2583                 break;
2584         default:
2585                 return -EINVAL;
2586         }
2587
2588         for (i = 0; i < ARRAY_SIZE(radar_specs->radar_pattern); i++) {
2589                 err = mt7915_mcu_set_radar_th(dev, i,
2590                                               &radar_specs->radar_pattern[i]);
2591                 if (err < 0)
2592                         return err;
2593         }
2594
2595         return mt7915_mcu_set_pulse_th(dev, &radar_specs->pulse_th);
2596 }
2597
2598 int mt7915_dfs_init_radar_detector(struct mt7915_phy *phy)
2599 {
2600         struct mt7915_dev *dev = phy->dev;
2601         enum mt76_dfs_state dfs_state, prev_state;
2602         int err;
2603
2604         prev_state = phy->mt76->dfs_state;
2605         dfs_state = mt76_phy_dfs_state(phy->mt76);
2606
2607         if (prev_state == dfs_state)
2608                 return 0;
2609
2610         if (prev_state == MT_DFS_STATE_UNKNOWN)
2611                 mt7915_dfs_stop_radar_detector(phy);
2612
2613         if (dfs_state == MT_DFS_STATE_DISABLED)
2614                 goto stop;
2615
2616         if (prev_state <= MT_DFS_STATE_DISABLED) {
2617                 err = mt7915_dfs_init_radar_specs(phy);
2618                 if (err < 0)
2619                         return err;
2620
2621                 err = mt7915_dfs_start_radar_detector(phy);
2622                 if (err < 0)
2623                         return err;
2624
2625                 phy->mt76->dfs_state = MT_DFS_STATE_CAC;
2626         }
2627
2628         if (dfs_state == MT_DFS_STATE_CAC)
2629                 return 0;
2630
2631         err = mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_CAC_END,
2632                                       phy->band_idx, MT_RX_SEL0, 0);
2633         if (err < 0) {
2634                 phy->mt76->dfs_state = MT_DFS_STATE_UNKNOWN;
2635                 return err;
2636         }
2637
2638         phy->mt76->dfs_state = MT_DFS_STATE_ACTIVE;
2639         return 0;
2640
2641 stop:
2642         err = mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_NORMAL_START,
2643                                       phy->band_idx, MT_RX_SEL0, 0);
2644         if (err < 0)
2645                 return err;
2646
2647         mt7915_dfs_stop_radar_detector(phy);
2648         phy->mt76->dfs_state = MT_DFS_STATE_DISABLED;
2649
2650         return 0;
2651 }
2652
2653 static int
2654 mt7915_mac_twt_duration_align(int duration)
2655 {
2656         return duration << 8;
2657 }
2658
2659 static u64
2660 mt7915_mac_twt_sched_list_add(struct mt7915_dev *dev,
2661                               struct mt7915_twt_flow *flow)
2662 {
2663         struct mt7915_twt_flow *iter, *iter_next;
2664         u32 duration = flow->duration << 8;
2665         u64 start_tsf;
2666
2667         iter = list_first_entry_or_null(&dev->twt_list,
2668                                         struct mt7915_twt_flow, list);
2669         if (!iter || !iter->sched || iter->start_tsf > duration) {
2670                 /* add flow as first entry in the list */
2671                 list_add(&flow->list, &dev->twt_list);
2672                 return 0;
2673         }
2674
2675         list_for_each_entry_safe(iter, iter_next, &dev->twt_list, list) {
2676                 start_tsf = iter->start_tsf +
2677                             mt7915_mac_twt_duration_align(iter->duration);
2678                 if (list_is_last(&iter->list, &dev->twt_list))
2679                         break;
2680
2681                 if (!iter_next->sched ||
2682                     iter_next->start_tsf > start_tsf + duration) {
2683                         list_add(&flow->list, &iter->list);
2684                         goto out;
2685                 }
2686         }
2687
2688         /* add flow as last entry in the list */
2689         list_add_tail(&flow->list, &dev->twt_list);
2690 out:
2691         return start_tsf;
2692 }
2693
2694 static int mt7915_mac_check_twt_req(struct ieee80211_twt_setup *twt)
2695 {
2696         struct ieee80211_twt_params *twt_agrt;
2697         u64 interval, duration;
2698         u16 mantissa;
2699         u8 exp;
2700
2701         /* only individual agreement supported */
2702         if (twt->control & IEEE80211_TWT_CONTROL_NEG_TYPE_BROADCAST)
2703                 return -EOPNOTSUPP;
2704
2705         /* only 256us unit supported */
2706         if (twt->control & IEEE80211_TWT_CONTROL_WAKE_DUR_UNIT)
2707                 return -EOPNOTSUPP;
2708
2709         twt_agrt = (struct ieee80211_twt_params *)twt->params;
2710
2711         /* explicit agreement not supported */
2712         if (!(twt_agrt->req_type & cpu_to_le16(IEEE80211_TWT_REQTYPE_IMPLICIT)))
2713                 return -EOPNOTSUPP;
2714
2715         exp = FIELD_GET(IEEE80211_TWT_REQTYPE_WAKE_INT_EXP,
2716                         le16_to_cpu(twt_agrt->req_type));
2717         mantissa = le16_to_cpu(twt_agrt->mantissa);
2718         duration = twt_agrt->min_twt_dur << 8;
2719
2720         interval = (u64)mantissa << exp;
2721         if (interval < duration)
2722                 return -EOPNOTSUPP;
2723
2724         return 0;
2725 }
2726
2727 static bool
2728 mt7915_mac_twt_param_equal(struct mt7915_sta *msta,
2729                            struct ieee80211_twt_params *twt_agrt)
2730 {
2731         u16 type = le16_to_cpu(twt_agrt->req_type);
2732         u8 exp;
2733         int i;
2734
2735         exp = FIELD_GET(IEEE80211_TWT_REQTYPE_WAKE_INT_EXP, type);
2736         for (i = 0; i < MT7915_MAX_STA_TWT_AGRT; i++) {
2737                 struct mt7915_twt_flow *f;
2738
2739                 if (!(msta->twt.flowid_mask & BIT(i)))
2740                         continue;
2741
2742                 f = &msta->twt.flow[i];
2743                 if (f->duration == twt_agrt->min_twt_dur &&
2744                     f->mantissa == twt_agrt->mantissa &&
2745                     f->exp == exp &&
2746                     f->protection == !!(type & IEEE80211_TWT_REQTYPE_PROTECTION) &&
2747                     f->flowtype == !!(type & IEEE80211_TWT_REQTYPE_FLOWTYPE) &&
2748                     f->trigger == !!(type & IEEE80211_TWT_REQTYPE_TRIGGER))
2749                         return true;
2750         }
2751
2752         return false;
2753 }
2754
2755 void mt7915_mac_add_twt_setup(struct ieee80211_hw *hw,
2756                               struct ieee80211_sta *sta,
2757                               struct ieee80211_twt_setup *twt)
2758 {
2759         enum ieee80211_twt_setup_cmd setup_cmd = TWT_SETUP_CMD_REJECT;
2760         struct mt7915_sta *msta = (struct mt7915_sta *)sta->drv_priv;
2761         struct ieee80211_twt_params *twt_agrt = (void *)twt->params;
2762         u16 req_type = le16_to_cpu(twt_agrt->req_type);
2763         enum ieee80211_twt_setup_cmd sta_setup_cmd;
2764         struct mt7915_dev *dev = mt7915_hw_dev(hw);
2765         struct mt7915_twt_flow *flow;
2766         int flowid, table_id;
2767         u8 exp;
2768
2769         if (mt7915_mac_check_twt_req(twt))
2770                 goto out;
2771
2772         mutex_lock(&dev->mt76.mutex);
2773
2774         if (dev->twt.n_agrt == MT7915_MAX_TWT_AGRT)
2775                 goto unlock;
2776
2777         if (hweight8(msta->twt.flowid_mask) == ARRAY_SIZE(msta->twt.flow))
2778                 goto unlock;
2779
2780         if (twt_agrt->min_twt_dur < MT7915_MIN_TWT_DUR) {
2781                 setup_cmd = TWT_SETUP_CMD_DICTATE;
2782                 twt_agrt->min_twt_dur = MT7915_MIN_TWT_DUR;
2783                 goto unlock;
2784         }
2785
2786         flowid = ffs(~msta->twt.flowid_mask) - 1;
2787         le16p_replace_bits(&twt_agrt->req_type, flowid,
2788                            IEEE80211_TWT_REQTYPE_FLOWID);
2789
2790         table_id = ffs(~dev->twt.table_mask) - 1;
2791         exp = FIELD_GET(IEEE80211_TWT_REQTYPE_WAKE_INT_EXP, req_type);
2792         sta_setup_cmd = FIELD_GET(IEEE80211_TWT_REQTYPE_SETUP_CMD, req_type);
2793
2794         if (mt7915_mac_twt_param_equal(msta, twt_agrt))
2795                 goto unlock;
2796
2797         flow = &msta->twt.flow[flowid];
2798         memset(flow, 0, sizeof(*flow));
2799         INIT_LIST_HEAD(&flow->list);
2800         flow->wcid = msta->wcid.idx;
2801         flow->table_id = table_id;
2802         flow->id = flowid;
2803         flow->duration = twt_agrt->min_twt_dur;
2804         flow->mantissa = twt_agrt->mantissa;
2805         flow->exp = exp;
2806         flow->protection = !!(req_type & IEEE80211_TWT_REQTYPE_PROTECTION);
2807         flow->flowtype = !!(req_type & IEEE80211_TWT_REQTYPE_FLOWTYPE);
2808         flow->trigger = !!(req_type & IEEE80211_TWT_REQTYPE_TRIGGER);
2809
2810         if (sta_setup_cmd == TWT_SETUP_CMD_REQUEST ||
2811             sta_setup_cmd == TWT_SETUP_CMD_SUGGEST) {
2812                 u64 interval = (u64)le16_to_cpu(twt_agrt->mantissa) << exp;
2813                 u64 flow_tsf, curr_tsf;
2814                 u32 rem;
2815
2816                 flow->sched = true;
2817                 flow->start_tsf = mt7915_mac_twt_sched_list_add(dev, flow);
2818                 curr_tsf = __mt7915_get_tsf(hw, msta->vif);
2819                 div_u64_rem(curr_tsf - flow->start_tsf, interval, &rem);
2820                 flow_tsf = curr_tsf + interval - rem;
2821                 twt_agrt->twt = cpu_to_le64(flow_tsf);
2822         } else {
2823                 list_add_tail(&flow->list, &dev->twt_list);
2824         }
2825         flow->tsf = le64_to_cpu(twt_agrt->twt);
2826
2827         if (mt7915_mcu_twt_agrt_update(dev, msta->vif, flow, MCU_TWT_AGRT_ADD))
2828                 goto unlock;
2829
2830         setup_cmd = TWT_SETUP_CMD_ACCEPT;
2831         dev->twt.table_mask |= BIT(table_id);
2832         msta->twt.flowid_mask |= BIT(flowid);
2833         dev->twt.n_agrt++;
2834
2835 unlock:
2836         mutex_unlock(&dev->mt76.mutex);
2837 out:
2838         le16p_replace_bits(&twt_agrt->req_type, setup_cmd,
2839                            IEEE80211_TWT_REQTYPE_SETUP_CMD);
2840         twt->control = (twt->control & IEEE80211_TWT_CONTROL_WAKE_DUR_UNIT) |
2841                        (twt->control & IEEE80211_TWT_CONTROL_RX_DISABLED);
2842 }
2843
2844 void mt7915_mac_twt_teardown_flow(struct mt7915_dev *dev,
2845                                   struct mt7915_sta *msta,
2846                                   u8 flowid)
2847 {
2848         struct mt7915_twt_flow *flow;
2849
2850         lockdep_assert_held(&dev->mt76.mutex);
2851
2852         if (flowid >= ARRAY_SIZE(msta->twt.flow))
2853                 return;
2854
2855         if (!(msta->twt.flowid_mask & BIT(flowid)))
2856                 return;
2857
2858         flow = &msta->twt.flow[flowid];
2859         if (mt7915_mcu_twt_agrt_update(dev, msta->vif, flow,
2860                                        MCU_TWT_AGRT_DELETE))
2861                 return;
2862
2863         list_del_init(&flow->list);
2864         msta->twt.flowid_mask &= ~BIT(flowid);
2865         dev->twt.table_mask &= ~BIT(flow->table_id);
2866         dev->twt.n_agrt--;
2867 }