1 // SPDX-License-Identifier: BSD-3-Clause-Clear
2 /*
3 * Copyright (C) 2022 MediaTek Inc.
4 */
5
6 #include <linux/etherdevice.h>
7 #include <linux/timekeeping.h>
8 #include "coredump.h"
9 #include "mt7996.h"
10 #include "../dma.h"
11 #include "mac.h"
12 #include "mcu.h"
13
14 #define to_rssi(field, rcpi) ((FIELD_GET(field, rcpi) - 220) / 2)
15
mt7996_rx_get_wcid(struct mt7996_dev * dev,u16 idx,u8 band_idx)16 static struct mt76_wcid *mt7996_rx_get_wcid(struct mt7996_dev *dev,
17 u16 idx, u8 band_idx)
18 {
19 struct mt7996_sta_link *msta_link;
20 struct mt7996_sta *msta;
21 struct mt7996_vif *mvif;
22 struct mt76_wcid *wcid;
23 int i;
24
25 wcid = mt76_wcid_ptr(dev, idx);
26 if (!wcid || !wcid->sta)
27 return NULL;
28
29 if (!mt7996_band_valid(dev, band_idx))
30 return NULL;
31
32 if (wcid->phy_idx == band_idx)
33 return wcid;
34
35 msta_link = container_of(wcid, struct mt7996_sta_link, wcid);
36 msta = msta_link->sta;
37 if (!msta || !msta->vif)
38 return NULL;
39
40 mvif = msta->vif;
41 for (i = 0; i < ARRAY_SIZE(mvif->mt76.link); i++) {
42 struct mt76_vif_link *mlink;
43
44 mlink = rcu_dereference(mvif->mt76.link[i]);
45 if (!mlink)
46 continue;
47
48 if (mlink->band_idx != band_idx)
49 continue;
50
51 msta_link = mt7996_sta_link(msta, i);
52 break;
53 }
54
55 return &msta_link->wcid;
56 }
57
mt7996_mac_wtbl_update(struct mt7996_dev * dev,int idx,u32 mask)58 bool mt7996_mac_wtbl_update(struct mt7996_dev *dev, int idx, u32 mask)
59 {
60 mt76_rmw(dev, MT_WTBL_UPDATE, MT_WTBL_UPDATE_WLAN_IDX,
61 FIELD_PREP(MT_WTBL_UPDATE_WLAN_IDX, idx) | mask);
62
63 return mt76_poll(dev, MT_WTBL_UPDATE, MT_WTBL_UPDATE_BUSY,
64 0, 5000);
65 }
66
mt7996_mac_wtbl_lmac_addr(struct mt7996_dev * dev,u16 wcid,u8 dw)67 u32 mt7996_mac_wtbl_lmac_addr(struct mt7996_dev *dev, u16 wcid, u8 dw)
68 {
69 mt76_wr(dev, MT_WTBLON_TOP_WDUCR,
70 FIELD_PREP(MT_WTBLON_TOP_WDUCR_GROUP, (wcid >> 7)));
71
72 return MT_WTBL_LMAC_OFFS(wcid, dw);
73 }
74
mt7996_mac_sta_poll(struct mt7996_dev * dev)75 static void mt7996_mac_sta_poll(struct mt7996_dev *dev)
76 {
77 static const u8 ac_to_tid[] = {
78 [IEEE80211_AC_BE] = 0,
79 [IEEE80211_AC_BK] = 1,
80 [IEEE80211_AC_VI] = 4,
81 [IEEE80211_AC_VO] = 6
82 };
83 struct mt7996_sta_link *msta_link;
84 struct mt76_vif_link *mlink;
85 struct ieee80211_sta *sta;
86 struct mt7996_sta *msta;
87 u32 tx_time[IEEE80211_NUM_ACS], rx_time[IEEE80211_NUM_ACS];
88 LIST_HEAD(sta_poll_list);
89 struct mt76_wcid *wcid;
90 int i;
91
92 spin_lock_bh(&dev->mt76.sta_poll_lock);
93 list_splice_init(&dev->mt76.sta_poll_list, &sta_poll_list);
94 spin_unlock_bh(&dev->mt76.sta_poll_lock);
95
96 rcu_read_lock();
97
98 while (true) {
99 bool clear = false;
100 u32 addr, val;
101 u16 idx;
102 s8 rssi[4];
103
104 spin_lock_bh(&dev->mt76.sta_poll_lock);
105 if (list_empty(&sta_poll_list)) {
106 spin_unlock_bh(&dev->mt76.sta_poll_lock);
107 break;
108 }
109 msta_link = list_first_entry(&sta_poll_list,
110 struct mt7996_sta_link,
111 wcid.poll_list);
112 msta = msta_link->sta;
113 wcid = &msta_link->wcid;
114 list_del_init(&wcid->poll_list);
115 spin_unlock_bh(&dev->mt76.sta_poll_lock);
116
117 idx = wcid->idx;
118
119 /* refresh peer's airtime reporting */
120 addr = mt7996_mac_wtbl_lmac_addr(dev, idx, 20);
121
122 for (i = 0; i < IEEE80211_NUM_ACS; i++) {
123 u32 tx_last = msta_link->airtime_ac[i];
124 u32 rx_last = msta_link->airtime_ac[i + 4];
125
126 msta_link->airtime_ac[i] = mt76_rr(dev, addr);
127 msta_link->airtime_ac[i + 4] = mt76_rr(dev, addr + 4);
128
129 tx_time[i] = msta_link->airtime_ac[i] - tx_last;
130 rx_time[i] = msta_link->airtime_ac[i + 4] - rx_last;
131
132 if ((tx_last | rx_last) & BIT(30))
133 clear = true;
134
135 addr += 8;
136 }
137
138 if (clear) {
139 mt7996_mac_wtbl_update(dev, idx,
140 MT_WTBL_UPDATE_ADM_COUNT_CLEAR);
141 memset(msta_link->airtime_ac, 0,
142 sizeof(msta_link->airtime_ac));
143 }
144
145 if (!wcid->sta)
146 continue;
147
148 sta = container_of((void *)msta, struct ieee80211_sta,
149 drv_priv);
150 for (i = 0; i < IEEE80211_NUM_ACS; i++) {
151 u8 q = mt76_connac_lmac_mapping(i);
152 u32 tx_cur = tx_time[q];
153 u32 rx_cur = rx_time[q];
154 u8 tid = ac_to_tid[i];
155
156 if (!tx_cur && !rx_cur)
157 continue;
158
159 ieee80211_sta_register_airtime(sta, tid, tx_cur, rx_cur);
160 }
161
162 /* get signal strength of resp frames (CTS/BA/ACK) */
163 addr = mt7996_mac_wtbl_lmac_addr(dev, idx, 34);
164 val = mt76_rr(dev, addr);
165
166 rssi[0] = to_rssi(GENMASK(7, 0), val);
167 rssi[1] = to_rssi(GENMASK(15, 8), val);
168 rssi[2] = to_rssi(GENMASK(23, 16), val);
169 rssi[3] = to_rssi(GENMASK(31, 24), val);
170
171 mlink = rcu_dereference(msta->vif->mt76.link[wcid->link_id]);
172 if (mlink) {
173 struct mt76_phy *mphy = mt76_vif_link_phy(mlink);
174
175 if (mphy)
176 msta_link->ack_signal =
177 mt76_rx_signal(mphy->antenna_mask,
178 rssi);
179 }
180
181 ewma_avg_signal_add(&msta_link->avg_ack_signal,
182 -msta_link->ack_signal);
183 }
184
185 rcu_read_unlock();
186 }
187
188 /* The HW does not translate the mac header to 802.3 for mesh point */
mt7996_reverse_frag0_hdr_trans(struct sk_buff * skb,u16 hdr_gap)189 static int mt7996_reverse_frag0_hdr_trans(struct sk_buff *skb, u16 hdr_gap)
190 {
191 struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb;
192 struct ethhdr *eth_hdr = (struct ethhdr *)(skb->data + hdr_gap);
193 struct mt7996_sta_link *msta_link = (void *)status->wcid;
194 struct mt7996_sta *msta = msta_link->sta;
195 struct ieee80211_bss_conf *link_conf;
196 __le32 *rxd = (__le32 *)skb->data;
197 struct ieee80211_sta *sta;
198 struct ieee80211_vif *vif;
199 struct ieee80211_hdr hdr;
200 u16 frame_control;
201
202 if (le32_get_bits(rxd[3], MT_RXD3_NORMAL_ADDR_TYPE) !=
203 MT_RXD3_NORMAL_U2M)
204 return -EINVAL;
205
206 if (!(le32_to_cpu(rxd[1]) & MT_RXD1_NORMAL_GROUP_4))
207 return -EINVAL;
208
209 if (!msta || !msta->vif)
210 return -EINVAL;
211
212 sta = wcid_to_sta(status->wcid);
213 vif = container_of((void *)msta->vif, struct ieee80211_vif, drv_priv);
214 link_conf = rcu_dereference(vif->link_conf[msta_link->wcid.link_id]);
215 if (!link_conf)
216 return -EINVAL;
217
218 /* store the info from RXD and ethhdr to avoid being overridden */
219 frame_control = le32_get_bits(rxd[8], MT_RXD8_FRAME_CONTROL);
220 hdr.frame_control = cpu_to_le16(frame_control);
221 hdr.seq_ctrl = cpu_to_le16(le32_get_bits(rxd[10], MT_RXD10_SEQ_CTRL));
222 hdr.duration_id = 0;
223
224 ether_addr_copy(hdr.addr1, vif->addr);
225 ether_addr_copy(hdr.addr2, sta->addr);
226 switch (frame_control & (IEEE80211_FCTL_TODS |
227 IEEE80211_FCTL_FROMDS)) {
228 case 0:
229 ether_addr_copy(hdr.addr3, link_conf->bssid);
230 break;
231 case IEEE80211_FCTL_FROMDS:
232 ether_addr_copy(hdr.addr3, eth_hdr->h_source);
233 break;
234 case IEEE80211_FCTL_TODS:
235 ether_addr_copy(hdr.addr3, eth_hdr->h_dest);
236 break;
237 case IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS:
238 ether_addr_copy(hdr.addr3, eth_hdr->h_dest);
239 ether_addr_copy(hdr.addr4, eth_hdr->h_source);
240 break;
241 default:
242 return -EINVAL;
243 }
244
245 skb_pull(skb, hdr_gap + sizeof(struct ethhdr) - 2);
246 if (eth_hdr->h_proto == cpu_to_be16(ETH_P_AARP) ||
247 eth_hdr->h_proto == cpu_to_be16(ETH_P_IPX))
248 ether_addr_copy(skb_push(skb, ETH_ALEN), bridge_tunnel_header);
249 else if (be16_to_cpu(eth_hdr->h_proto) >= ETH_P_802_3_MIN)
250 ether_addr_copy(skb_push(skb, ETH_ALEN), rfc1042_header);
251 else
252 skb_pull(skb, 2);
253
254 if (ieee80211_has_order(hdr.frame_control))
255 memcpy(skb_push(skb, IEEE80211_HT_CTL_LEN), &rxd[11],
256 IEEE80211_HT_CTL_LEN);
257 if (ieee80211_is_data_qos(hdr.frame_control)) {
258 __le16 qos_ctrl;
259
260 qos_ctrl = cpu_to_le16(le32_get_bits(rxd[10], MT_RXD10_QOS_CTL));
261 memcpy(skb_push(skb, IEEE80211_QOS_CTL_LEN), &qos_ctrl,
262 IEEE80211_QOS_CTL_LEN);
263 }
264
265 if (ieee80211_has_a4(hdr.frame_control))
266 memcpy(skb_push(skb, sizeof(hdr)), &hdr, sizeof(hdr));
267 else
268 memcpy(skb_push(skb, sizeof(hdr) - 6), &hdr, sizeof(hdr) - 6);
269
270 return 0;
271 }
272
273 static int
mt7996_mac_fill_rx_rate(struct mt7996_dev * dev,struct mt76_rx_status * status,struct ieee80211_supported_band * sband,__le32 * rxv,u8 * mode)274 mt7996_mac_fill_rx_rate(struct mt7996_dev *dev,
275 struct mt76_rx_status *status,
276 struct ieee80211_supported_band *sband,
277 __le32 *rxv, u8 *mode)
278 {
279 u32 v0, v2;
280 u8 stbc, gi, bw, dcm, nss;
281 int i, idx;
282 bool cck = false;
283
284 v0 = le32_to_cpu(rxv[0]);
285 v2 = le32_to_cpu(rxv[2]);
286
287 idx = FIELD_GET(MT_PRXV_TX_RATE, v0);
288 i = idx;
289 nss = FIELD_GET(MT_PRXV_NSTS, v0) + 1;
290
291 stbc = FIELD_GET(MT_PRXV_HT_STBC, v2);
292 gi = FIELD_GET(MT_PRXV_HT_SHORT_GI, v2);
293 *mode = FIELD_GET(MT_PRXV_TX_MODE, v2);
294 dcm = FIELD_GET(MT_PRXV_DCM, v2);
295 bw = FIELD_GET(MT_PRXV_FRAME_MODE, v2);
296
297 /* the hardware reports NSTS; report the data NSS for STBC frames */
298 if (stbc && nss > 1)
299 nss >>= 1;
300
301 switch (*mode) {
302 case MT_PHY_TYPE_CCK:
303 cck = true;
304 fallthrough;
305 case MT_PHY_TYPE_OFDM:
306 i = mt76_get_rate(&dev->mt76, sband, i, cck);
307 break;
308 case MT_PHY_TYPE_HT_GF:
309 case MT_PHY_TYPE_HT:
310 status->encoding = RX_ENC_HT;
311 if (gi)
312 status->enc_flags |= RX_ENC_FLAG_SHORT_GI;
313 if (i > 31)
314 return -EINVAL;
315 break;
316 case MT_PHY_TYPE_VHT:
317 status->nss = nss;
318 status->encoding = RX_ENC_VHT;
319 if (gi)
320 status->enc_flags |= RX_ENC_FLAG_SHORT_GI;
321 if (i > 11)
322 return -EINVAL;
323 break;
324 case MT_PHY_TYPE_HE_MU:
325 case MT_PHY_TYPE_HE_SU:
326 case MT_PHY_TYPE_HE_EXT_SU:
327 case MT_PHY_TYPE_HE_TB:
328 status->nss = nss;
329 status->encoding = RX_ENC_HE;
330 i &= GENMASK(3, 0);
331
332 if (gi <= NL80211_RATE_INFO_HE_GI_3_2)
333 status->he_gi = gi;
334
335 status->he_dcm = dcm;
336 break;
337 case MT_PHY_TYPE_EHT_SU:
338 case MT_PHY_TYPE_EHT_TRIG:
339 case MT_PHY_TYPE_EHT_MU:
340 status->nss = nss;
341 status->encoding = RX_ENC_EHT;
342 i &= GENMASK(3, 0);
343
344 if (gi <= NL80211_RATE_INFO_EHT_GI_3_2)
345 status->eht.gi = gi;
346 break;
347 default:
348 return -EINVAL;
349 }
350 status->rate_idx = i;
351
352 switch (bw) {
353 case IEEE80211_STA_RX_BW_20:
354 break;
355 case IEEE80211_STA_RX_BW_40:
356 if (*mode == MT_PHY_TYPE_HE_EXT_SU &&
357 (idx & MT_PRXV_TX_ER_SU_106T)) {
358 status->bw = RATE_INFO_BW_HE_RU;
359 status->he_ru =
360 NL80211_RATE_INFO_HE_RU_ALLOC_106;
361 } else {
362 status->bw = RATE_INFO_BW_40;
363 }
364 break;
365 case IEEE80211_STA_RX_BW_80:
366 status->bw = RATE_INFO_BW_80;
367 break;
368 case IEEE80211_STA_RX_BW_160:
369 status->bw = RATE_INFO_BW_160;
370 break;
371 /* rxv reports bw 320-1 and 320-2 separately */
372 case IEEE80211_STA_RX_BW_320:
373 case IEEE80211_STA_RX_BW_320 + 1:
374 status->bw = RATE_INFO_BW_320;
375 break;
376 default:
377 return -EINVAL;
378 }
379
380 status->enc_flags |= RX_ENC_FLAG_STBC_MASK * stbc;
381 if (*mode < MT_PHY_TYPE_HE_SU && gi)
382 status->enc_flags |= RX_ENC_FLAG_SHORT_GI;
383
384 return 0;
385 }
386
387 static void
mt7996_wed_check_ppe(struct mt7996_dev * dev,struct mt76_queue * q,struct mt7996_sta * msta,struct sk_buff * skb,u32 info)388 mt7996_wed_check_ppe(struct mt7996_dev *dev, struct mt76_queue *q,
389 struct mt7996_sta *msta, struct sk_buff *skb,
390 u32 info)
391 {
392 struct ieee80211_vif *vif;
393 struct wireless_dev *wdev;
394
395 if (!msta || !msta->vif)
396 return;
397
398 if (!mt76_queue_is_wed_rx(q))
399 return;
400
401 if (!(info & MT_DMA_INFO_PPE_VLD))
402 return;
403
404 vif = container_of((void *)msta->vif, struct ieee80211_vif,
405 drv_priv);
406 wdev = ieee80211_vif_to_wdev(vif);
407 skb->dev = wdev->netdev;
408
409 mtk_wed_device_ppe_check(&dev->mt76.mmio.wed, skb,
410 FIELD_GET(MT_DMA_PPE_CPU_REASON, info),
411 FIELD_GET(MT_DMA_PPE_ENTRY, info));
412 }
413
414 static int
mt7996_mac_fill_rx(struct mt7996_dev * dev,enum mt76_rxq_id q,struct sk_buff * skb,u32 * info)415 mt7996_mac_fill_rx(struct mt7996_dev *dev, enum mt76_rxq_id q,
416 struct sk_buff *skb, u32 *info)
417 {
418 struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb;
419 struct mt76_phy *mphy = &dev->mt76.phy;
420 struct mt7996_phy *phy = &dev->phy;
421 struct ieee80211_supported_band *sband;
422 __le32 *rxd = (__le32 *)skb->data;
423 __le32 *rxv = NULL;
424 u32 rxd0 = le32_to_cpu(rxd[0]);
425 u32 rxd1 = le32_to_cpu(rxd[1]);
426 u32 rxd2 = le32_to_cpu(rxd[2]);
427 u32 rxd3 = le32_to_cpu(rxd[3]);
428 u32 rxd4 = le32_to_cpu(rxd[4]);
429 u32 csum_mask = MT_RXD3_NORMAL_IP_SUM | MT_RXD3_NORMAL_UDP_TCP_SUM;
430 u32 csum_status = *(u32 *)skb->cb;
431 u32 mesh_mask = MT_RXD0_MESH | MT_RXD0_MHCP;
432 bool is_mesh = (rxd0 & mesh_mask) == mesh_mask;
433 bool unicast, insert_ccmp_hdr = false;
434 u8 remove_pad, amsdu_info, band_idx;
435 u8 mode = 0, qos_ctl = 0;
436 bool hdr_trans;
437 u16 hdr_gap;
438 u16 seq_ctrl = 0;
439 __le16 fc = 0;
440 int idx;
441 u8 hw_aggr = false;
442 struct mt7996_sta *msta = NULL;
443
444 hw_aggr = status->aggr;
445 memset(status, 0, sizeof(*status));
446
447 band_idx = FIELD_GET(MT_RXD1_NORMAL_BAND_IDX, rxd1);
448 if (!mt7996_band_valid(dev, band_idx))
449 return -EINVAL;
450
451 mphy = dev->mt76.phys[band_idx];
452 if (!mphy)
453 return -EINVAL;
454
455 phy = mphy->priv;
456 status->phy_idx = mphy->band_idx;
457
458 if (!test_bit(MT76_STATE_RUNNING, &mphy->state))
459 return -EINVAL;
460
461 if (rxd2 & MT_RXD2_NORMAL_AMSDU_ERR)
462 return -EINVAL;
463
464 hdr_trans = rxd2 & MT_RXD2_NORMAL_HDR_TRANS;
465 if (hdr_trans && (rxd1 & MT_RXD1_NORMAL_CM))
466 return -EINVAL;
467
468 /* ICV error or CCMP/BIP/WPI MIC error */
469 if (rxd1 & MT_RXD1_NORMAL_ICV_ERR)
470 status->flag |= RX_FLAG_ONLY_MONITOR;
471
472 unicast = FIELD_GET(MT_RXD3_NORMAL_ADDR_TYPE, rxd3) == MT_RXD3_NORMAL_U2M;
473 idx = FIELD_GET(MT_RXD1_NORMAL_WLAN_IDX, rxd1);
474 status->wcid = mt7996_rx_get_wcid(dev, idx, band_idx);
475
476 if (status->wcid) {
477 struct mt7996_sta_link *msta_link;
478
479 msta_link = container_of(status->wcid, struct mt7996_sta_link,
480 wcid);
481 msta = msta_link->sta;
482 mt76_wcid_add_poll(&dev->mt76, &msta_link->wcid);
483 }
484
485 status->freq = mphy->chandef.chan->center_freq;
486 status->band = mphy->chandef.chan->band;
487 if (status->band == NL80211_BAND_5GHZ)
488 sband = &mphy->sband_5g.sband;
489 else if (status->band == NL80211_BAND_6GHZ)
490 sband = &mphy->sband_6g.sband;
491 else
492 sband = &mphy->sband_2g.sband;
493
494 if (!sband->channels)
495 return -EINVAL;
496
497 if ((rxd3 & csum_mask) == csum_mask &&
498 !(csum_status & (BIT(0) | BIT(2) | BIT(3))))
499 skb->ip_summed = CHECKSUM_UNNECESSARY;
500
501 if (rxd3 & MT_RXD3_NORMAL_FCS_ERR)
502 status->flag |= RX_FLAG_FAILED_FCS_CRC;
503
504 if (rxd1 & MT_RXD1_NORMAL_TKIP_MIC_ERR)
505 status->flag |= RX_FLAG_MMIC_ERROR;
506
507 if (FIELD_GET(MT_RXD2_NORMAL_SEC_MODE, rxd2) != 0 &&
508 !(rxd1 & (MT_RXD1_NORMAL_CLM | MT_RXD1_NORMAL_CM))) {
509 status->flag |= RX_FLAG_DECRYPTED;
510 status->flag |= RX_FLAG_IV_STRIPPED;
511 status->flag |= RX_FLAG_MMIC_STRIPPED | RX_FLAG_MIC_STRIPPED;
512 }
513
514 remove_pad = FIELD_GET(MT_RXD2_NORMAL_HDR_OFFSET, rxd2);
515
516 if (rxd2 & MT_RXD2_NORMAL_MAX_LEN_ERROR)
517 return -EINVAL;
518
519 rxd += 8;
520 if (rxd1 & MT_RXD1_NORMAL_GROUP_4) {
521 u32 v0 = le32_to_cpu(rxd[0]);
522 u32 v2 = le32_to_cpu(rxd[2]);
523
524 fc = cpu_to_le16(FIELD_GET(MT_RXD8_FRAME_CONTROL, v0));
525 qos_ctl = FIELD_GET(MT_RXD10_QOS_CTL, v2);
526 seq_ctrl = FIELD_GET(MT_RXD10_SEQ_CTRL, v2);
527
528 rxd += 4;
529 if ((u8 *)rxd - skb->data >= skb->len)
530 return -EINVAL;
531 }
532
533 if (rxd1 & MT_RXD1_NORMAL_GROUP_1) {
534 u8 *data = (u8 *)rxd;
535
536 if (status->flag & RX_FLAG_DECRYPTED) {
537 switch (FIELD_GET(MT_RXD2_NORMAL_SEC_MODE, rxd2)) {
538 case MT_CIPHER_AES_CCMP:
539 case MT_CIPHER_CCMP_CCX:
540 case MT_CIPHER_CCMP_256:
541 insert_ccmp_hdr =
542 FIELD_GET(MT_RXD2_NORMAL_FRAG, rxd2);
543 fallthrough;
544 case MT_CIPHER_TKIP:
545 case MT_CIPHER_TKIP_NO_MIC:
546 case MT_CIPHER_GCMP:
547 case MT_CIPHER_GCMP_256:
548 status->iv[0] = data[5];
549 status->iv[1] = data[4];
550 status->iv[2] = data[3];
551 status->iv[3] = data[2];
552 status->iv[4] = data[1];
553 status->iv[5] = data[0];
554 break;
555 default:
556 break;
557 }
558 }
559 rxd += 4;
560 if ((u8 *)rxd - skb->data >= skb->len)
561 return -EINVAL;
562 }
563
564 if (rxd1 & MT_RXD1_NORMAL_GROUP_2) {
565 status->timestamp = le32_to_cpu(rxd[0]);
566 status->flag |= RX_FLAG_MACTIME_START;
567
568 if (!(rxd2 & MT_RXD2_NORMAL_NON_AMPDU)) {
569 status->flag |= RX_FLAG_AMPDU_DETAILS;
570
571 /* all subframes of an A-MPDU have the same timestamp */
572 if (phy->rx_ampdu_ts != status->timestamp) {
573 if (!++phy->ampdu_ref)
574 phy->ampdu_ref++;
575 }
576 phy->rx_ampdu_ts = status->timestamp;
577
578 status->ampdu_ref = phy->ampdu_ref;
579 }
580
581 rxd += 4;
582 if ((u8 *)rxd - skb->data >= skb->len)
583 return -EINVAL;
584 }
585
586 /* RXD Group 3 - P-RXV */
587 if (rxd1 & MT_RXD1_NORMAL_GROUP_3) {
588 u32 v3;
589 int ret;
590
591 rxv = rxd;
592 rxd += 4;
593 if ((u8 *)rxd - skb->data >= skb->len)
594 return -EINVAL;
595
596 v3 = le32_to_cpu(rxv[3]);
597
598 status->chains = mphy->antenna_mask;
599 status->chain_signal[0] = to_rssi(MT_PRXV_RCPI0, v3);
600 status->chain_signal[1] = to_rssi(MT_PRXV_RCPI1, v3);
601 status->chain_signal[2] = to_rssi(MT_PRXV_RCPI2, v3);
602 status->chain_signal[3] = to_rssi(MT_PRXV_RCPI3, v3);
603
604 /* RXD Group 5 - C-RXV */
605 if (rxd1 & MT_RXD1_NORMAL_GROUP_5) {
606 rxd += 24;
607 if ((u8 *)rxd - skb->data >= skb->len)
608 return -EINVAL;
609 }
610
611 ret = mt7996_mac_fill_rx_rate(dev, status, sband, rxv, &mode);
612 if (ret < 0)
613 return ret;
614 }
615
616 amsdu_info = FIELD_GET(MT_RXD4_NORMAL_PAYLOAD_FORMAT, rxd4);
617 status->amsdu = !!amsdu_info;
618 if (status->amsdu) {
619 status->first_amsdu = amsdu_info == MT_RXD4_FIRST_AMSDU_FRAME;
620 status->last_amsdu = amsdu_info == MT_RXD4_LAST_AMSDU_FRAME;
621 }
622
623 /* IEEE 802.11 fragmentation can only be applied to unicast frames.
624 * Hence, drop fragments with multicast/broadcast RA.
625 * This check fixes vulnerabilities, like CVE-2020-26145.
626 */
627 if ((ieee80211_has_morefrags(fc) || seq_ctrl & IEEE80211_SCTL_FRAG) &&
628 FIELD_GET(MT_RXD3_NORMAL_ADDR_TYPE, rxd3) != MT_RXD3_NORMAL_U2M)
629 return -EINVAL;
630
631 hdr_gap = (u8 *)rxd - skb->data + 2 * remove_pad;
632 if (hdr_trans && ieee80211_has_morefrags(fc)) {
633 if (mt7996_reverse_frag0_hdr_trans(skb, hdr_gap))
634 return -EINVAL;
635 hdr_trans = false;
636 } else {
637 int pad_start = 0;
638
639 skb_pull(skb, hdr_gap);
640 if (!hdr_trans && status->amsdu && !(ieee80211_has_a4(fc) && is_mesh)) {
641 pad_start = ieee80211_get_hdrlen_from_skb(skb);
642 } else if (hdr_trans && (rxd2 & MT_RXD2_NORMAL_HDR_TRANS_ERROR)) {
643 /* When header translation failure is indicated,
644 * the hardware will insert an extra 2-byte field
645 * containing the data length after the protocol
646 * type field. This happens either when the LLC-SNAP
647 * pattern did not match, or if a VLAN header was
648 * detected.
649 */
650 pad_start = 12;
651 if (get_unaligned_be16(skb->data + pad_start) == ETH_P_8021Q)
652 pad_start += 4;
653 else
654 pad_start = 0;
655 }
656
657 if (pad_start) {
658 memmove(skb->data + 2, skb->data, pad_start);
659 skb_pull(skb, 2);
660 }
661 }
662
663 if (!hdr_trans) {
664 struct ieee80211_hdr *hdr;
665
666 if (insert_ccmp_hdr) {
667 u8 key_id = FIELD_GET(MT_RXD1_NORMAL_KEY_ID, rxd1);
668
669 mt76_insert_ccmp_hdr(skb, key_id);
670 }
671
672 hdr = mt76_skb_get_hdr(skb);
673 fc = hdr->frame_control;
674 if (ieee80211_is_beacon(fc))
675 mt76_rx_beacon(mphy, skb);
676 if (ieee80211_is_data_qos(fc)) {
677 u8 *qos = ieee80211_get_qos_ctl(hdr);
678
679 seq_ctrl = le16_to_cpu(hdr->seq_ctrl);
680 qos_ctl = *qos;
681
682 /* Mesh DA/SA/Length will be stripped after hardware
683 * de-amsdu, so here needs to clear amsdu present bit
684 * to mark it as a normal mesh frame.
685 */
686 if (ieee80211_has_a4(fc) && is_mesh && status->amsdu)
687 *qos &= ~IEEE80211_QOS_CTL_A_MSDU_PRESENT;
688 }
689 skb_set_mac_header(skb, (unsigned char *)hdr - skb->data);
690 } else {
691 status->flag |= RX_FLAG_8023;
692 mt7996_wed_check_ppe(dev, &dev->mt76.q_rx[q], msta, skb,
693 *info);
694 mt76_npu_check_ppe(&dev->mt76, skb, *info);
695 }
696
697 if (rxv && !(status->flag & RX_FLAG_8023)) {
698 switch (status->encoding) {
699 case RX_ENC_EHT:
700 mt76_connac3_mac_decode_eht_radiotap(skb, rxv, mode);
701 break;
702 case RX_ENC_HE:
703 mt76_connac3_mac_decode_he_radiotap(skb, rxv, mode);
704 break;
705 default:
706 break;
707 }
708 }
709
710 if (!status->wcid || !ieee80211_is_data_qos(fc) || hw_aggr)
711 return 0;
712
713 status->aggr = unicast &&
714 !ieee80211_is_qos_nullfunc(fc);
715 status->qos_ctl = qos_ctl;
716 status->seqno = IEEE80211_SEQ_TO_SN(seq_ctrl);
717
718 return 0;
719 }
720
721 static void
mt7996_mac_write_txwi_8023(struct mt7996_dev * dev,__le32 * txwi,struct sk_buff * skb,struct mt76_wcid * wcid)722 mt7996_mac_write_txwi_8023(struct mt7996_dev *dev, __le32 *txwi,
723 struct sk_buff *skb, struct mt76_wcid *wcid)
724 {
725 u8 tid = skb->priority & IEEE80211_QOS_CTL_TID_MASK;
726 u8 fc_type, fc_stype;
727 u16 ethertype;
728 bool wmm = false;
729 u32 val;
730
731 if (wcid->sta) {
732 struct ieee80211_sta *sta = wcid_to_sta(wcid);
733
734 wmm = sta->wme;
735 }
736
737 val = FIELD_PREP(MT_TXD1_HDR_FORMAT, MT_HDR_FORMAT_802_3) |
738 FIELD_PREP(MT_TXD1_TID, tid);
739
740 ethertype = get_unaligned_be16(&skb->data[12]);
741 if (ethertype >= ETH_P_802_3_MIN)
742 val |= MT_TXD1_ETH_802_3;
743
744 txwi[1] |= cpu_to_le32(val);
745
746 fc_type = IEEE80211_FTYPE_DATA >> 2;
747 fc_stype = wmm ? IEEE80211_STYPE_QOS_DATA >> 4 : 0;
748
749 val = FIELD_PREP(MT_TXD2_FRAME_TYPE, fc_type) |
750 FIELD_PREP(MT_TXD2_SUB_TYPE, fc_stype);
751
752 txwi[2] |= cpu_to_le32(val);
753
754 if (wcid->amsdu)
755 txwi[3] |= cpu_to_le32(MT_TXD3_HW_AMSDU);
756 }
757
758 static void
mt7996_mac_write_txwi_80211(struct mt7996_dev * dev,__le32 * txwi,struct sk_buff * skb,struct ieee80211_key_conf * key,struct mt76_wcid * wcid)759 mt7996_mac_write_txwi_80211(struct mt7996_dev *dev, __le32 *txwi,
760 struct sk_buff *skb,
761 struct ieee80211_key_conf *key,
762 struct mt76_wcid *wcid)
763 {
764 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
765 struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
766 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
767 bool multicast = is_multicast_ether_addr(hdr->addr1);
768 u8 tid = skb->priority & IEEE80211_QOS_CTL_TID_MASK;
769 __le16 fc = hdr->frame_control, sc = hdr->seq_ctrl;
770 struct ieee80211_vif *vif = info->control.vif;
771 u16 seqno = le16_to_cpu(sc);
772 bool hw_bigtk = false;
773 u8 fc_type, fc_stype;
774 u32 val;
775
776 if (ieee80211_is_action(fc) &&
777 skb->len >= IEEE80211_MIN_ACTION_SIZE(action_code) &&
778 mgmt->u.action.category == WLAN_CATEGORY_BACK &&
779 mgmt->u.action.action_code == WLAN_ACTION_ADDBA_REQ) {
780 if (is_mt7990(&dev->mt76))
781 txwi[6] |= cpu_to_le32(FIELD_PREP(MT_TXD6_TID_ADDBA, tid));
782 else
783 txwi[7] |= cpu_to_le32(MT_TXD7_MAC_TXD);
784
785 tid = MT_TX_ADDBA;
786 } else if (ieee80211_is_mgmt(hdr->frame_control)) {
787 tid = MT_TX_NORMAL;
788 }
789
790 val = FIELD_PREP(MT_TXD1_HDR_FORMAT, MT_HDR_FORMAT_802_11) |
791 FIELD_PREP(MT_TXD1_HDR_INFO,
792 ieee80211_get_hdrlen_from_skb(skb) / 2) |
793 FIELD_PREP(MT_TXD1_TID, tid);
794
795 if (!ieee80211_is_data(fc) || multicast ||
796 info->flags & IEEE80211_TX_CTL_USE_MINRATE)
797 val |= MT_TXD1_FIXED_RATE;
798
799 if (is_mt7990(&dev->mt76) && ieee80211_is_beacon(fc) &&
800 (wcid->hw_key_idx2 == 6 || wcid->hw_key_idx2 == 7))
801 hw_bigtk = true;
802
803 if ((key && multicast && ieee80211_is_robust_mgmt_frame(skb)) || hw_bigtk) {
804 val |= MT_TXD1_BIP;
805 txwi[3] &= ~cpu_to_le32(MT_TXD3_PROTECT_FRAME);
806 }
807
808 txwi[1] |= cpu_to_le32(val);
809
810 fc_type = (le16_to_cpu(fc) & IEEE80211_FCTL_FTYPE) >> 2;
811 fc_stype = (le16_to_cpu(fc) & IEEE80211_FCTL_STYPE) >> 4;
812
813 val = FIELD_PREP(MT_TXD2_FRAME_TYPE, fc_type) |
814 FIELD_PREP(MT_TXD2_SUB_TYPE, fc_stype);
815
816 if (ieee80211_has_morefrags(fc) && ieee80211_is_first_frag(sc))
817 val |= FIELD_PREP(MT_TXD2_FRAG, MT_TX_FRAG_FIRST);
818 else if (ieee80211_has_morefrags(fc) && !ieee80211_is_first_frag(sc))
819 val |= FIELD_PREP(MT_TXD2_FRAG, MT_TX_FRAG_MID);
820 else if (!ieee80211_has_morefrags(fc) && !ieee80211_is_first_frag(sc))
821 val |= FIELD_PREP(MT_TXD2_FRAG, MT_TX_FRAG_LAST);
822 else
823 val |= FIELD_PREP(MT_TXD2_FRAG, MT_TX_FRAG_NONE);
824
825 txwi[2] |= cpu_to_le32(val);
826
827 txwi[3] |= cpu_to_le32(FIELD_PREP(MT_TXD3_BCM, multicast));
828 if (ieee80211_is_beacon(fc)) {
829 txwi[3] &= ~cpu_to_le32(MT_TXD3_SW_POWER_MGMT);
830 txwi[3] |= cpu_to_le32(MT_TXD3_REM_TX_COUNT);
831 }
832
833 if (multicast && vif && ieee80211_vif_is_mld(vif)) {
834 val = MT_TXD3_SN_VALID |
835 FIELD_PREP(MT_TXD3_SEQ, IEEE80211_SEQ_TO_SN(seqno));
836 txwi[3] |= cpu_to_le32(val);
837 }
838
839 if (info->flags & IEEE80211_TX_CTL_INJECTED) {
840 if (ieee80211_is_back_req(hdr->frame_control)) {
841 struct ieee80211_bar *bar;
842
843 bar = (struct ieee80211_bar *)skb->data;
844 seqno = le16_to_cpu(bar->start_seq_num);
845 }
846
847 val = MT_TXD3_SN_VALID |
848 FIELD_PREP(MT_TXD3_SEQ, IEEE80211_SEQ_TO_SN(seqno));
849 txwi[3] |= cpu_to_le32(val);
850 txwi[3] &= ~cpu_to_le32(MT_TXD3_HW_AMSDU);
851 }
852
853 if (vif && ieee80211_vif_is_mld(vif) &&
854 (multicast || unlikely(skb->protocol == cpu_to_be16(ETH_P_PAE))))
855 txwi[5] |= cpu_to_le32(MT_TXD5_FL);
856
857 if (ieee80211_is_nullfunc(fc) && ieee80211_has_a4(fc) &&
858 vif && ieee80211_vif_is_mld(vif)) {
859 txwi[5] |= cpu_to_le32(MT_TXD5_FL);
860 txwi[6] |= cpu_to_le32(MT_TXD6_DIS_MAT);
861 }
862
863 if (!wcid->sta && ieee80211_is_mgmt(fc))
864 txwi[6] |= cpu_to_le32(MT_TXD6_DIS_MAT);
865 }
866
867 /* The WLAN_IDX in the TXD and TXP must belong to the primary or secondary
868 * link of an MLD station; any other link id can make the firmware spin when
869 * that link is in powersave. Completion events carry the same index, so the
870 * wcid used for status tracking and accounting must match it
871 */
mt7996_get_tx_wcid(struct mt76_wcid * wcid)872 struct mt76_wcid *mt7996_get_tx_wcid(struct mt76_wcid *wcid)
873 {
874 struct mt7996_sta_link *msta_link;
875 struct mt7996_sta *msta;
876
877 if (!wcid->sta)
878 return wcid;
879
880 msta_link = container_of(wcid, struct mt7996_sta_link, wcid);
881 msta = msta_link->sta;
882
883 if (!msta || wcid->link_id == msta->seclink_id ||
884 wcid->link_id == msta->deflink_id)
885 return wcid;
886
887 msta_link = mt7996_sta_link(msta, msta->deflink_id);
888 if (msta_link)
889 return &msta_link->wcid;
890
891 return wcid;
892 }
893
mt7996_mac_write_txwi(struct mt7996_dev * dev,__le32 * txwi,struct sk_buff * skb,struct mt76_wcid * wcid,struct ieee80211_key_conf * key,int pid,enum mt76_txq_id qid,u32 changed,unsigned int link_id)894 void mt7996_mac_write_txwi(struct mt7996_dev *dev, __le32 *txwi,
895 struct sk_buff *skb, struct mt76_wcid *wcid,
896 struct ieee80211_key_conf *key, int pid,
897 enum mt76_txq_id qid, u32 changed,
898 unsigned int link_id)
899 {
900 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
901 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
902 struct ieee80211_vif *vif = info->control.vif;
903 u8 band_idx = (info->hw_queue & MT_TX_HW_QUEUE_PHY) >> 2;
904 u8 p_fmt, q_idx, omac_idx = 0, wmm_idx = 0;
905 bool is_8023 = info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP;
906 struct mt76_vif_link *mlink = NULL;
907 struct mt7996_vif *mvif;
908 u16 tx_count = 15;
909 u32 val;
910 bool inband_disc = !!(changed & (BSS_CHANGED_UNSOL_BCAST_PROBE_RESP |
911 BSS_CHANGED_FILS_DISCOVERY));
912 bool beacon = !!(changed & (BSS_CHANGED_BEACON |
913 BSS_CHANGED_BEACON_ENABLED)) && (!inband_disc);
914
915 mvif = vif ? (struct mt7996_vif *)vif->drv_priv : NULL;
916 if (mvif) {
917 if (wcid->offchannel)
918 mlink = rcu_dereference(mvif->mt76.offchannel_link);
919 if (!mlink && link_id != IEEE80211_LINK_UNSPECIFIED)
920 mlink = rcu_dereference(mvif->mt76.link[link_id]);
921 }
922
923 if (mlink) {
924 omac_idx = mlink->omac_idx;
925 wmm_idx = mlink->wmm_idx;
926 band_idx = mlink->band_idx;
927 }
928
929 if (inband_disc) {
930 p_fmt = MT_TX_TYPE_FW;
931 q_idx = MT_LMAC_ALTX0;
932 } else if (beacon) {
933 p_fmt = MT_TX_TYPE_FW;
934 q_idx = MT_LMAC_BCN0;
935 } else if (qid >= MT_TXQ_PSD) {
936 p_fmt = MT_TX_TYPE_CT;
937 q_idx = MT_LMAC_ALTX0;
938 } else {
939 p_fmt = MT_TX_TYPE_CT;
940 q_idx = wmm_idx * MT7996_MAX_WMM_SETS +
941 mt76_connac_lmac_mapping(skb_get_queue_mapping(skb));
942 }
943
944 val = FIELD_PREP(MT_TXD0_TX_BYTES, skb->len + MT_TXD_SIZE) |
945 FIELD_PREP(MT_TXD0_PKT_FMT, p_fmt) |
946 FIELD_PREP(MT_TXD0_Q_IDX, q_idx);
947 txwi[0] = cpu_to_le32(val);
948
949 val = FIELD_PREP(MT_TXD1_WLAN_IDX, wcid->idx) |
950 FIELD_PREP(MT_TXD1_OWN_MAC, omac_idx);
951
952 if (band_idx)
953 val |= FIELD_PREP(MT_TXD1_TGID, band_idx);
954
955 txwi[1] = cpu_to_le32(val);
956 txwi[2] = 0;
957
958 val = MT_TXD3_SW_POWER_MGMT |
959 FIELD_PREP(MT_TXD3_REM_TX_COUNT, tx_count);
960 if (key)
961 val |= MT_TXD3_PROTECT_FRAME;
962 if (info->flags & IEEE80211_TX_CTL_NO_ACK)
963 val |= MT_TXD3_NO_ACK;
964
965 txwi[3] = cpu_to_le32(val);
966 txwi[4] = 0;
967
968 val = FIELD_PREP(MT_TXD5_PID, pid);
969 if (pid >= MT_PACKET_ID_FIRST)
970 val |= MT_TXD5_TX_STATUS_HOST;
971 txwi[5] = cpu_to_le32(val);
972
973 val = MT_TXD6_DAS | MT_TXD6_VTA;
974 if ((q_idx >= MT_LMAC_ALTX0 && q_idx <= MT_LMAC_BCN0) ||
975 skb->protocol == cpu_to_be16(ETH_P_PAE))
976 val |= MT_TXD6_DIS_MAT;
977
978 if (is_mt7996(&dev->mt76))
979 val |= FIELD_PREP(MT_TXD6_MSDU_CNT, 1);
980 else if (is_8023 || !ieee80211_is_mgmt(hdr->frame_control))
981 val |= FIELD_PREP(MT_TXD6_MSDU_CNT_V2, 1);
982
983 txwi[6] = cpu_to_le32(val);
984 txwi[7] = 0;
985
986 if (is_8023)
987 mt7996_mac_write_txwi_8023(dev, txwi, skb, wcid);
988 else
989 mt7996_mac_write_txwi_80211(dev, txwi, skb, key, wcid);
990
991 if (txwi[1] & cpu_to_le32(MT_TXD1_FIXED_RATE)) {
992 bool mcast = ieee80211_is_data(hdr->frame_control) &&
993 is_multicast_ether_addr(hdr->addr1);
994 u8 idx = MT7996_BASIC_RATES_TBL;
995
996 if (mlink) {
997 if (mcast && mlink->mcast_rates_idx)
998 idx = mlink->mcast_rates_idx;
999 else if (beacon && mlink->beacon_rates_idx)
1000 idx = mlink->beacon_rates_idx;
1001 else
1002 idx = mlink->basic_rates_idx;
1003 }
1004
1005 val = FIELD_PREP(MT_TXD6_TX_RATE, idx) | MT_TXD6_FIXED_BW;
1006 if (mcast)
1007 val |= MT_TXD6_DIS_MAT;
1008 txwi[6] |= cpu_to_le32(val);
1009 txwi[3] |= cpu_to_le32(MT_TXD3_BA_DISABLE);
1010 }
1011 }
1012
1013 static bool
mt7996_tx_use_mgmt(struct mt7996_dev * dev,struct sk_buff * skb)1014 mt7996_tx_use_mgmt(struct mt7996_dev *dev, struct sk_buff *skb)
1015 {
1016 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
1017
1018 if (ieee80211_is_mgmt(hdr->frame_control))
1019 return true;
1020
1021 /* for SDO to bypass specific data frame */
1022 if (!mt7996_has_wa(dev)) {
1023 if (unlikely(skb->protocol == cpu_to_be16(ETH_P_PAE)))
1024 return true;
1025
1026 if (ieee80211_has_a4(hdr->frame_control) &&
1027 !ieee80211_is_data_present(hdr->frame_control))
1028 return true;
1029 }
1030
1031 return false;
1032 }
1033
mt7996_tx_prepare_skb(struct mt76_dev * mdev,void * txwi_ptr,enum mt76_txq_id qid,struct mt76_wcid * wcid,struct ieee80211_sta * sta,struct mt76_tx_info * tx_info)1034 int mt7996_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
1035 enum mt76_txq_id qid, struct mt76_wcid *wcid,
1036 struct ieee80211_sta *sta,
1037 struct mt76_tx_info *tx_info)
1038 {
1039 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx_info->skb->data;
1040 struct mt7996_dev *dev = container_of(mdev, struct mt7996_dev, mt76);
1041 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_info->skb);
1042 struct ieee80211_key_conf *key = info->control.hw_key;
1043 struct ieee80211_vif *vif = info->control.vif;
1044 struct mt7996_vif *mvif = vif ? (struct mt7996_vif *)vif->drv_priv : NULL;
1045 struct mt7996_sta *msta = sta ? (struct mt7996_sta *)sta->drv_priv : NULL;
1046 struct mt76_vif_link *mlink = NULL;
1047 struct mt76_txwi_cache *t;
1048 int id, i, pid, nbuf = tx_info->nbuf - 1;
1049 bool is_8023 = info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP;
1050 __le32 *ptr = (__le32 *)txwi_ptr;
1051 u8 *txwi = (u8 *)txwi_ptr;
1052 u8 link_id;
1053
1054 if (unlikely(tx_info->skb->len <= ETH_HLEN))
1055 return -EINVAL;
1056
1057 if (!wcid)
1058 wcid = &dev->mt76.global_wcid;
1059
1060 if ((is_8023 || ieee80211_is_data_qos(hdr->frame_control)) && sta->mlo &&
1061 likely(tx_info->skb->protocol != cpu_to_be16(ETH_P_PAE))) {
1062 u8 tid = tx_info->skb->priority & IEEE80211_QOS_CTL_TID_MASK;
1063
1064 link_id = (tid % 2) ? msta->seclink_id : msta->deflink_id;
1065 } else {
1066 link_id = u32_get_bits(info->control.flags,
1067 IEEE80211_TX_CTRL_MLO_LINK);
1068 }
1069
1070 /* non-MLD frames are LINK_UNSPECIFIED; use the wcid's own link */
1071 if (link_id == IEEE80211_LINK_UNSPECIFIED &&
1072 wcid != &dev->mt76.global_wcid)
1073 link_id = wcid->link_id;
1074
1075 if (link_id != wcid->link_id && link_id != IEEE80211_LINK_UNSPECIFIED) {
1076 if (msta) {
1077 struct mt7996_sta_link *msta_link =
1078 mt7996_sta_link(msta, link_id);
1079
1080 if (msta_link)
1081 wcid = &msta_link->wcid;
1082 } else if (mvif) {
1083 mlink = rcu_dereference(mvif->mt76.link[link_id]);
1084 if (mlink && mlink->wcid)
1085 wcid = mlink->wcid;
1086 }
1087 }
1088
1089 t = (struct mt76_txwi_cache *)(txwi + mdev->drv->txwi_size);
1090 t->skb = tx_info->skb;
1091
1092 id = mt76_token_consume(mdev, &t);
1093 if (id < 0)
1094 return id;
1095
1096 /* Since the rules of HW MLD address translation are not fully
1097 * compatible with 802.11 EAPOL frame, we do the translation by
1098 * software
1099 */
1100 if (tx_info->skb->protocol == cpu_to_be16(ETH_P_PAE) && sta->mlo) {
1101 struct ieee80211_hdr *hdr = (void *)tx_info->skb->data;
1102 struct ieee80211_bss_conf *link_conf;
1103 struct ieee80211_link_sta *link_sta;
1104
1105 link_conf = rcu_dereference(vif->link_conf[wcid->link_id]);
1106 if (!link_conf)
1107 goto error_release_token;
1108
1109 link_sta = rcu_dereference(sta->link[wcid->link_id]);
1110 if (!link_sta)
1111 goto error_release_token;
1112
1113 dma_sync_single_for_cpu(mdev->dma_dev, tx_info->buf[1].addr,
1114 tx_info->buf[1].len, DMA_TO_DEVICE);
1115
1116 memcpy(hdr->addr1, link_sta->addr, ETH_ALEN);
1117 memcpy(hdr->addr2, link_conf->addr, ETH_ALEN);
1118 if (ieee80211_has_a4(hdr->frame_control)) {
1119 memcpy(hdr->addr3, sta->addr, ETH_ALEN);
1120 memcpy(hdr->addr4, vif->addr, ETH_ALEN);
1121 } else if (ieee80211_has_tods(hdr->frame_control)) {
1122 memcpy(hdr->addr3, sta->addr, ETH_ALEN);
1123 } else if (ieee80211_has_fromds(hdr->frame_control)) {
1124 memcpy(hdr->addr3, vif->addr, ETH_ALEN);
1125 }
1126
1127 dma_sync_single_for_device(mdev->dma_dev, tx_info->buf[1].addr,
1128 tx_info->buf[1].len, DMA_TO_DEVICE);
1129 }
1130
1131 wcid = mt7996_get_tx_wcid(wcid);
1132
1133 pid = mt76_tx_status_skb_add(mdev, wcid, tx_info->skb);
1134 memset(txwi_ptr, 0, MT_TXD_SIZE);
1135 /* Transmit non qos data by 802.11 header and need to fill txd by host*/
1136 if (!is_8023 || pid >= MT_PACKET_ID_FIRST)
1137 mt7996_mac_write_txwi(dev, txwi_ptr, tx_info->skb, wcid, key,
1138 pid, qid, 0, link_id);
1139
1140 /* MT7996 and MT7992 require driver to provide the MAC TXP for AddBA
1141 * req
1142 */
1143 if (le32_to_cpu(ptr[7]) & MT_TXD7_MAC_TXD) {
1144 u32 val, mac_txp_size = sizeof(struct mt76_connac_hw_txp);
1145
1146 ptr = (__le32 *)(txwi + MT_TXD_SIZE);
1147 memset((void *)ptr, 0, mac_txp_size);
1148
1149 val = FIELD_PREP(MT_TXP0_TOKEN_ID0, id) |
1150 MT_TXP0_TOKEN_ID0_VALID_MASK;
1151 ptr[0] = cpu_to_le32(val);
1152
1153 val = FIELD_PREP(MT_TXP1_TID_ADDBA,
1154 tx_info->skb->priority &
1155 IEEE80211_QOS_CTL_TID_MASK);
1156 ptr[1] = cpu_to_le32(val);
1157 ptr[2] = cpu_to_le32(tx_info->buf[1].addr & 0xFFFFFFFF);
1158
1159 val = FIELD_PREP(MT_TXP_BUF_LEN, tx_info->buf[1].len) |
1160 MT_TXP3_ML0_MASK;
1161 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
1162 val |= FIELD_PREP(MT_TXP3_DMA_ADDR_H,
1163 tx_info->buf[1].addr >> 32);
1164 #endif
1165 ptr[3] = cpu_to_le32(val);
1166
1167 tx_info->buf[0].len = MT_TXD_SIZE + mac_txp_size;
1168 } else {
1169 struct mt76_connac_txp_common *txp;
1170
1171 txp = (struct mt76_connac_txp_common *)(txwi + MT_TXD_SIZE);
1172 for (i = 0; i < nbuf; i++) {
1173 u16 len;
1174
1175 len = FIELD_PREP(MT_TXP_BUF_LEN, tx_info->buf[i + 1].len);
1176 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
1177 len |= FIELD_PREP(MT_TXP_DMA_ADDR_H,
1178 tx_info->buf[i + 1].addr >> 32);
1179 #endif
1180
1181 txp->fw.buf[i] = cpu_to_le32(tx_info->buf[i + 1].addr);
1182 txp->fw.len[i] = cpu_to_le16(len);
1183 }
1184 txp->fw.nbuf = nbuf;
1185
1186 txp->fw.flags = cpu_to_le16(MT_CT_INFO_FROM_HOST);
1187
1188 if (!is_8023 || pid >= MT_PACKET_ID_FIRST)
1189 txp->fw.flags |= cpu_to_le16(MT_CT_INFO_APPLY_TXD);
1190
1191 if (!key)
1192 txp->fw.flags |= cpu_to_le16(MT_CT_INFO_NONE_CIPHER_FRAME);
1193
1194 if (!is_8023 && mt7996_tx_use_mgmt(dev, tx_info->skb))
1195 txp->fw.flags |= cpu_to_le16(MT_CT_INFO_MGMT_FRAME);
1196
1197 if (mvif) {
1198 if (wcid->offchannel)
1199 mlink = rcu_dereference(mvif->mt76.offchannel_link);
1200 if (!mlink)
1201 mlink = rcu_dereference(mvif->mt76.link[wcid->link_id]);
1202
1203 txp->fw.bss_idx = mlink ? mlink->idx : mvif->deflink.mt76.idx;
1204 }
1205
1206 txp->fw.token = cpu_to_le16(id);
1207 txp->fw.rept_wds_wcid = cpu_to_le16(sta ? wcid->idx : 0xfff);
1208 }
1209
1210 tx_info->skb = NULL;
1211
1212 /* pass partial skb header to fw */
1213 tx_info->buf[1].len = MT_CT_PARSE_LEN;
1214 tx_info->buf[1].skip_unmap = true;
1215 tx_info->nbuf = MT_CT_DMA_BUF_NUM;
1216
1217 return 0;
1218
1219 error_release_token:
1220 mt76_token_release(mdev, id, NULL);
1221 return -EINVAL;
1222 }
1223
mt7996_wed_init_buf(void * ptr,dma_addr_t phys,int token_id)1224 u32 mt7996_wed_init_buf(void *ptr, dma_addr_t phys, int token_id)
1225 {
1226 struct mt76_connac_fw_txp *txp = ptr + MT_TXD_SIZE;
1227 __le32 *txwi = ptr;
1228 u32 val;
1229
1230 memset(ptr, 0, MT_TXD_SIZE + sizeof(*txp));
1231
1232 val = FIELD_PREP(MT_TXD0_TX_BYTES, MT_TXD_SIZE) |
1233 FIELD_PREP(MT_TXD0_PKT_FMT, MT_TX_TYPE_CT);
1234 txwi[0] = cpu_to_le32(val);
1235
1236 val = BIT(31) |
1237 FIELD_PREP(MT_TXD1_HDR_FORMAT, MT_HDR_FORMAT_802_3);
1238 txwi[1] = cpu_to_le32(val);
1239
1240 txp->token = cpu_to_le16(token_id);
1241 txp->nbuf = 1;
1242 txp->buf[0] = cpu_to_le32(phys + MT_TXD_SIZE + sizeof(*txp));
1243
1244 return MT_TXD_SIZE + sizeof(*txp);
1245 }
1246
1247 static void
mt7996_tx_check_aggr(struct ieee80211_link_sta * link_sta,struct mt76_wcid * wcid,struct sk_buff * skb)1248 mt7996_tx_check_aggr(struct ieee80211_link_sta *link_sta,
1249 struct mt76_wcid *wcid, struct sk_buff *skb)
1250 {
1251 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1252 bool is_8023 = info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP;
1253 u16 fc, tid;
1254
1255 if (!(link_sta->ht_cap.ht_supported || link_sta->he_cap.has_he))
1256 return;
1257
1258 tid = skb->priority & IEEE80211_QOS_CTL_TID_MASK;
1259
1260 if (is_8023) {
1261 fc = IEEE80211_FTYPE_DATA |
1262 (link_sta->sta->wme ? IEEE80211_STYPE_QOS_DATA
1263 : IEEE80211_STYPE_DATA);
1264 } else {
1265 /* No need to get precise TID for Action/Management Frame,
1266 * since it will not meet the following Frame Control
1267 * condition anyway.
1268 */
1269
1270 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
1271
1272 fc = le16_to_cpu(hdr->frame_control) &
1273 (IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE);
1274 }
1275
1276 if (unlikely(fc != (IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA)))
1277 return;
1278
1279 if (!test_and_set_bit(tid, &wcid->ampdu_state) &&
1280 ieee80211_start_tx_ba_session(link_sta->sta, tid, 0))
1281 clear_bit(tid, &wcid->ampdu_state);
1282 }
1283
1284 static void
mt7996_txp_skb_unmap(struct mt76_dev * mdev,struct mt76_txwi_cache * t)1285 mt7996_txp_skb_unmap(struct mt76_dev *mdev, struct mt76_txwi_cache *t)
1286 {
1287 u8 *txwi_ptr = mt76_get_txwi_ptr(mdev, t);
1288 __le32 *txwi = (__le32 *)txwi_ptr;
1289 __le32 *txp;
1290 dma_addr_t addr;
1291 u32 val;
1292
1293 if (!(le32_to_cpu(txwi[7]) & MT_TXD7_MAC_TXD)) {
1294 mt76_connac_txp_skb_unmap(mdev, t);
1295 return;
1296 }
1297
1298 txp = (__le32 *)(txwi_ptr + MT_TXD_SIZE);
1299 val = le32_to_cpu(txp[3]);
1300 addr = le32_to_cpu(txp[2]);
1301 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
1302 addr |= (dma_addr_t)FIELD_GET(MT_TXP3_DMA_ADDR_H, val) << 32;
1303 #endif
1304 dma_unmap_single(mdev->dma_dev, addr, FIELD_GET(MT_TXP_BUF_LEN, val),
1305 DMA_TO_DEVICE);
1306 }
1307
1308 static void
mt7996_txwi_free(struct mt7996_dev * dev,struct mt76_txwi_cache * t,struct ieee80211_link_sta * link_sta,struct mt76_wcid * wcid,struct list_head * free_list)1309 mt7996_txwi_free(struct mt7996_dev *dev, struct mt76_txwi_cache *t,
1310 struct ieee80211_link_sta *link_sta,
1311 struct mt76_wcid *wcid, struct list_head *free_list)
1312 {
1313 struct mt76_dev *mdev = &dev->mt76;
1314 __le32 *txwi;
1315 u16 wcid_idx;
1316
1317 mt7996_txp_skb_unmap(mdev, t);
1318 if (!t->skb)
1319 goto out;
1320
1321 txwi = (__le32 *)mt76_get_txwi_ptr(mdev, t);
1322 if (link_sta) {
1323 wcid_idx = wcid->idx;
1324 if (likely(t->skb->protocol != cpu_to_be16(ETH_P_PAE))) {
1325 struct mt7996_sta *msta;
1326
1327 /* AMPDU state is stored in the primary link */
1328 msta = (void *)link_sta->sta->drv_priv;
1329 mt7996_tx_check_aggr(link_sta, &msta->deflink.wcid,
1330 t->skb);
1331 }
1332 } else {
1333 wcid_idx = le32_get_bits(txwi[9], MT_TXD9_WLAN_IDX);
1334 }
1335
1336 __mt76_tx_complete_skb(mdev, wcid_idx, t->skb, free_list);
1337
1338 out:
1339 t->skb = NULL;
1340 mt76_put_txwi(mdev, t);
1341 }
1342
1343 static void
mt7996_mac_tx_free(struct mt7996_dev * dev,void * data,int len)1344 mt7996_mac_tx_free(struct mt7996_dev *dev, void *data, int len)
1345 {
1346 __le32 *tx_free = (__le32 *)data, *cur_info;
1347 struct mt76_dev *mdev = &dev->mt76;
1348 struct mt76_phy *phy2 = mdev->phys[MT_BAND1];
1349 struct mt76_phy *phy3 = mdev->phys[MT_BAND2];
1350 struct ieee80211_link_sta *link_sta = NULL;
1351 struct mt76_txwi_cache *txwi;
1352 struct mt76_wcid *wcid = NULL;
1353 LIST_HEAD(free_list);
1354 struct sk_buff *skb, *tmp;
1355 void *end = data + len;
1356 bool wake = false;
1357 u16 total, count = 0;
1358 u8 ver;
1359
1360 /* clean DMA queues and unmap buffers first */
1361 mt76_queue_tx_cleanup(dev, dev->mphy.q_tx[MT_TXQ_PSD], false);
1362 mt76_queue_tx_cleanup(dev, dev->mphy.q_tx[MT_TXQ_BE], false);
1363 if (phy2) {
1364 mt76_queue_tx_cleanup(dev, phy2->q_tx[MT_TXQ_PSD], false);
1365 mt76_queue_tx_cleanup(dev, phy2->q_tx[MT_TXQ_BE], false);
1366 }
1367 if (phy3) {
1368 mt76_queue_tx_cleanup(dev, phy3->q_tx[MT_TXQ_PSD], false);
1369 mt76_queue_tx_cleanup(dev, phy3->q_tx[MT_TXQ_BE], false);
1370 }
1371
1372 ver = le32_get_bits(tx_free[1], MT_TXFREE1_VER);
1373 if (WARN_ON_ONCE(ver < 5))
1374 return;
1375
1376 total = le32_get_bits(tx_free[0], MT_TXFREE0_MSDU_CNT);
1377 for (cur_info = &tx_free[2]; count < total; cur_info++) {
1378 u32 msdu, info;
1379 u8 i;
1380
1381 if (WARN_ON_ONCE((void *)cur_info >= end))
1382 return;
1383 /* 1'b1: new wcid pair.
1384 * 1'b0: msdu_id with the same 'wcid pair' as above.
1385 */
1386 info = le32_to_cpu(*cur_info);
1387 if (info & MT_TXFREE_INFO_PAIR) {
1388 struct ieee80211_sta *sta;
1389 unsigned long valid_links;
1390 struct mt7996_sta *msta;
1391 unsigned int id;
1392 u16 idx;
1393
1394 idx = FIELD_GET(MT_TXFREE_INFO_WLAN_ID, info);
1395 wcid = mt76_wcid_ptr(dev, idx);
1396 sta = wcid_to_sta(wcid);
1397 if (!sta) {
1398 link_sta = NULL;
1399 goto next;
1400 }
1401
1402 link_sta = rcu_dereference(sta->link[wcid->link_id]);
1403 if (!link_sta)
1404 goto next;
1405
1406 msta = (struct mt7996_sta *)sta->drv_priv;
1407 valid_links = sta->valid_links ?: BIT(0);
1408
1409 /* For MLD STA, add all link's wcid to sta_poll_list */
1410 for_each_set_bit(id, &valid_links,
1411 IEEE80211_MLD_MAX_NUM_LINKS) {
1412 struct mt7996_sta_link *msta_link;
1413
1414 msta_link = mt7996_sta_link(msta, id);
1415 if (!msta_link)
1416 continue;
1417
1418 mt76_wcid_add_poll(&dev->mt76,
1419 &msta_link->wcid);
1420 }
1421 next:
1422 /* ver 7 has a new DW with pair = 1, skip it */
1423 if (ver == 7 && ((void *)(cur_info + 1) < end) &&
1424 (le32_to_cpu(*(cur_info + 1)) & MT_TXFREE_INFO_PAIR))
1425 cur_info++;
1426 continue;
1427 } else if (info & MT_TXFREE_INFO_HEADER) {
1428 u32 tx_retries = 0, tx_failed = 0, count;
1429
1430 if (!wcid)
1431 continue;
1432
1433 count = FIELD_GET(MT_TXFREE_INFO_COUNT, info);
1434 tx_retries = count ? count - 1 : 0;
1435 tx_failed = tx_retries +
1436 !!FIELD_GET(MT_TXFREE_INFO_STAT, info);
1437
1438 wcid->stats.tx_retries += tx_retries;
1439 wcid->stats.tx_failed += tx_failed;
1440 continue;
1441 }
1442
1443 for (i = 0; i < 2; i++) {
1444 msdu = (info >> (15 * i)) & MT_TXFREE_INFO_MSDU_ID;
1445 if (msdu == MT_TXFREE_INFO_MSDU_ID)
1446 continue;
1447
1448 count++;
1449 txwi = mt76_token_release(mdev, msdu, &wake);
1450 if (!txwi)
1451 continue;
1452
1453 mt7996_txwi_free(dev, txwi, link_sta, wcid,
1454 &free_list);
1455 }
1456 }
1457
1458 mt7996_mac_sta_poll(dev);
1459
1460 if (wake)
1461 mt76_set_tx_blocked(&dev->mt76, false);
1462
1463 mt76_worker_schedule(&dev->mt76.tx_worker);
1464
1465 list_for_each_entry_safe(skb, tmp, &free_list, list) {
1466 skb_list_del_init(skb);
1467 napi_consume_skb(skb, 1);
1468 }
1469 }
1470
1471 static bool
mt7996_mac_add_txs_skb(struct mt7996_dev * dev,struct mt76_wcid * wcid,int pid,__le32 * txs_data)1472 mt7996_mac_add_txs_skb(struct mt7996_dev *dev, struct mt76_wcid *wcid,
1473 int pid, __le32 *txs_data)
1474 {
1475 struct mt76_sta_stats *stats = &wcid->stats;
1476 struct ieee80211_supported_band *sband;
1477 struct mt76_dev *mdev = &dev->mt76;
1478 struct mt76_phy *mphy;
1479 struct ieee80211_tx_info *info;
1480 struct sk_buff_head list;
1481 struct rate_info rate = {};
1482 struct sk_buff *skb = NULL;
1483 bool cck = false;
1484 u32 txrate, txs, mode, stbc;
1485
1486 txs = le32_to_cpu(txs_data[0]);
1487
1488 mt76_tx_status_lock(mdev, &list);
1489
1490 /* only report MPDU TXS */
1491 if (le32_get_bits(txs_data[0], MT_TXS0_TXS_FORMAT) == 0) {
1492 skb = mt76_tx_status_skb_get(mdev, wcid, pid, &list);
1493 if (skb) {
1494 info = IEEE80211_SKB_CB(skb);
1495 if (!(txs & MT_TXS0_ACK_ERROR_MASK))
1496 info->flags |= IEEE80211_TX_STAT_ACK;
1497
1498 info->status.ampdu_len = 1;
1499 info->status.ampdu_ack_len =
1500 !!(info->flags & IEEE80211_TX_STAT_ACK);
1501
1502 info->status.rates[0].idx = -1;
1503 }
1504 }
1505
1506 if (mtk_wed_device_active(&dev->mt76.mmio.wed) && wcid->sta) {
1507 struct ieee80211_sta *sta;
1508 u8 tid;
1509
1510 sta = wcid_to_sta(wcid);
1511 tid = FIELD_GET(MT_TXS0_TID, txs);
1512 ieee80211_refresh_tx_agg_session_timer(sta, tid);
1513 }
1514
1515 txrate = FIELD_GET(MT_TXS0_TX_RATE, txs);
1516
1517 rate.mcs = FIELD_GET(MT_TX_RATE_IDX, txrate);
1518 rate.nss = FIELD_GET(MT_TX_RATE_NSS, txrate) + 1;
1519 stbc = le32_get_bits(txs_data[3], MT_TXS3_RATE_STBC);
1520
1521 if (stbc && rate.nss > 1)
1522 rate.nss >>= 1;
1523
1524 if (rate.nss - 1 < ARRAY_SIZE(stats->tx_nss))
1525 stats->tx_nss[rate.nss - 1]++;
1526 if (rate.mcs < ARRAY_SIZE(stats->tx_mcs))
1527 stats->tx_mcs[rate.mcs]++;
1528
1529 mode = FIELD_GET(MT_TX_RATE_MODE, txrate);
1530 switch (mode) {
1531 case MT_PHY_TYPE_CCK:
1532 cck = true;
1533 fallthrough;
1534 case MT_PHY_TYPE_OFDM:
1535 mphy = mt76_dev_phy(mdev, wcid->phy_idx);
1536
1537 if (mphy->chandef.chan->band == NL80211_BAND_5GHZ)
1538 sband = &mphy->sband_5g.sband;
1539 else if (mphy->chandef.chan->band == NL80211_BAND_6GHZ)
1540 sband = &mphy->sband_6g.sband;
1541 else
1542 sband = &mphy->sband_2g.sband;
1543
1544 rate.mcs = mt76_get_rate(mphy->dev, sband, rate.mcs, cck);
1545 rate.legacy = sband->bitrates[rate.mcs].bitrate;
1546 break;
1547 case MT_PHY_TYPE_HT:
1548 case MT_PHY_TYPE_HT_GF:
1549 if (rate.mcs > 31)
1550 goto out;
1551
1552 rate.flags = RATE_INFO_FLAGS_MCS;
1553 if (wcid->rate.flags & RATE_INFO_FLAGS_SHORT_GI)
1554 rate.flags |= RATE_INFO_FLAGS_SHORT_GI;
1555 break;
1556 case MT_PHY_TYPE_VHT:
1557 if (rate.mcs > 9)
1558 goto out;
1559
1560 rate.flags = RATE_INFO_FLAGS_VHT_MCS;
1561 if (wcid->rate.flags & RATE_INFO_FLAGS_SHORT_GI)
1562 rate.flags |= RATE_INFO_FLAGS_SHORT_GI;
1563 break;
1564 case MT_PHY_TYPE_HE_SU:
1565 case MT_PHY_TYPE_HE_EXT_SU:
1566 case MT_PHY_TYPE_HE_TB:
1567 case MT_PHY_TYPE_HE_MU:
1568 if (rate.mcs > 11)
1569 goto out;
1570
1571 rate.he_gi = wcid->rate.he_gi;
1572 rate.he_dcm = FIELD_GET(MT_TX_RATE_DCM, txrate);
1573 rate.flags = RATE_INFO_FLAGS_HE_MCS;
1574 break;
1575 case MT_PHY_TYPE_EHT_SU:
1576 case MT_PHY_TYPE_EHT_TRIG:
1577 case MT_PHY_TYPE_EHT_MU:
1578 if (rate.mcs > 13)
1579 goto out;
1580
1581 rate.eht_gi = wcid->rate.eht_gi;
1582 rate.flags = RATE_INFO_FLAGS_EHT_MCS;
1583 break;
1584 default:
1585 goto out;
1586 }
1587
1588 stats->tx_mode[mode]++;
1589
1590 switch (FIELD_GET(MT_TXS0_BW, txs)) {
1591 case IEEE80211_STA_RX_BW_320:
1592 rate.bw = RATE_INFO_BW_320;
1593 stats->tx_bw[4]++;
1594 break;
1595 case IEEE80211_STA_RX_BW_160:
1596 rate.bw = RATE_INFO_BW_160;
1597 stats->tx_bw[3]++;
1598 break;
1599 case IEEE80211_STA_RX_BW_80:
1600 rate.bw = RATE_INFO_BW_80;
1601 stats->tx_bw[2]++;
1602 break;
1603 case IEEE80211_STA_RX_BW_40:
1604 rate.bw = RATE_INFO_BW_40;
1605 stats->tx_bw[1]++;
1606 break;
1607 default:
1608 rate.bw = RATE_INFO_BW_20;
1609 stats->tx_bw[0]++;
1610 break;
1611 }
1612 wcid->rate = rate;
1613
1614 out:
1615 if (skb)
1616 mt76_tx_status_skb_done(mdev, skb, &list);
1617 mt76_tx_status_unlock(mdev, &list);
1618
1619 return !!skb;
1620 }
1621
mt7996_mac_add_txs(struct mt7996_dev * dev,void * data)1622 static void mt7996_mac_add_txs(struct mt7996_dev *dev, void *data)
1623 {
1624 struct mt7996_sta_link *msta_link;
1625 struct mt76_wcid *wcid;
1626 __le32 *txs_data = data;
1627 u16 wcidx;
1628 u8 pid;
1629
1630 wcidx = le32_get_bits(txs_data[2], MT_TXS2_WCID);
1631 pid = le32_get_bits(txs_data[3], MT_TXS3_PID);
1632
1633 if (pid < MT_PACKET_ID_NO_SKB)
1634 return;
1635
1636 rcu_read_lock();
1637
1638 wcid = mt76_wcid_ptr(dev, wcidx);
1639 if (!wcid)
1640 goto out;
1641
1642 mt7996_mac_add_txs_skb(dev, wcid, pid, txs_data);
1643
1644 if (!wcid->sta)
1645 goto out;
1646
1647 msta_link = container_of(wcid, struct mt7996_sta_link, wcid);
1648 mt76_wcid_add_poll(&dev->mt76, &msta_link->wcid);
1649
1650 out:
1651 rcu_read_unlock();
1652 }
1653
mt7996_rx_check(struct mt76_dev * mdev,void * data,int len)1654 bool mt7996_rx_check(struct mt76_dev *mdev, void *data, int len)
1655 {
1656 struct mt7996_dev *dev = container_of(mdev, struct mt7996_dev, mt76);
1657 __le32 *rxd = (__le32 *)data;
1658 __le32 *end = (__le32 *)&rxd[len / 4];
1659 enum rx_pkt_type type;
1660
1661 type = le32_get_bits(rxd[0], MT_RXD0_PKT_TYPE);
1662 if (type != PKT_TYPE_NORMAL) {
1663 u32 sw_type = le32_get_bits(rxd[0], MT_RXD0_SW_PKT_TYPE_MASK);
1664
1665 if (unlikely((sw_type & MT_RXD0_SW_PKT_TYPE_MAP) ==
1666 MT_RXD0_SW_PKT_TYPE_FRAME))
1667 return true;
1668 }
1669
1670 switch (type) {
1671 case PKT_TYPE_TXRX_NOTIFY:
1672 mt7996_mac_tx_free(dev, data, len);
1673 return false;
1674 case PKT_TYPE_TXS:
1675 for (rxd += MT_TXS_HDR_SIZE; rxd + MT_TXS_SIZE <= end; rxd += MT_TXS_SIZE)
1676 mt7996_mac_add_txs(dev, rxd);
1677 return false;
1678 case PKT_TYPE_RX_FW_MONITOR:
1679 mt7996_debugfs_rx_fw_monitor(dev, data, len);
1680 return false;
1681 default:
1682 return true;
1683 }
1684 }
1685
mt7996_queue_rx_skb(struct mt76_dev * mdev,enum mt76_rxq_id q,struct sk_buff * skb,u32 * info)1686 void mt7996_queue_rx_skb(struct mt76_dev *mdev, enum mt76_rxq_id q,
1687 struct sk_buff *skb, u32 *info)
1688 {
1689 struct mt7996_dev *dev = container_of(mdev, struct mt7996_dev, mt76);
1690 __le32 *rxd = (__le32 *)skb->data;
1691 __le32 *end = (__le32 *)&skb->data[skb->len];
1692 enum rx_pkt_type type;
1693
1694 type = le32_get_bits(rxd[0], MT_RXD0_PKT_TYPE);
1695 if (type != PKT_TYPE_NORMAL) {
1696 u32 sw_type = le32_get_bits(rxd[0], MT_RXD0_SW_PKT_TYPE_MASK);
1697
1698 if (unlikely((sw_type & MT_RXD0_SW_PKT_TYPE_MAP) ==
1699 MT_RXD0_SW_PKT_TYPE_FRAME))
1700 type = PKT_TYPE_NORMAL;
1701 }
1702
1703 switch (type) {
1704 case PKT_TYPE_TXRX_NOTIFY:
1705 if (mtk_wed_device_active(&dev->mt76.mmio.wed_hif2) &&
1706 q == MT_RXQ_TXFREE_BAND2) {
1707 dev_kfree_skb(skb);
1708 break;
1709 }
1710
1711 mt7996_mac_tx_free(dev, skb->data, skb->len);
1712 napi_consume_skb(skb, 1);
1713 break;
1714 case PKT_TYPE_RX_EVENT:
1715 mt7996_mcu_rx_event(dev, skb);
1716 break;
1717 case PKT_TYPE_TXS:
1718 for (rxd += MT_TXS_HDR_SIZE; rxd + MT_TXS_SIZE <= end; rxd += MT_TXS_SIZE)
1719 mt7996_mac_add_txs(dev, rxd);
1720 dev_kfree_skb(skb);
1721 break;
1722 case PKT_TYPE_RX_FW_MONITOR:
1723 mt7996_debugfs_rx_fw_monitor(dev, skb->data, skb->len);
1724 dev_kfree_skb(skb);
1725 break;
1726 case PKT_TYPE_NORMAL:
1727 if (!mt7996_mac_fill_rx(dev, q, skb, info)) {
1728 mt76_rx(&dev->mt76, q, skb);
1729 return;
1730 }
1731 fallthrough;
1732 default:
1733 dev_kfree_skb(skb);
1734 break;
1735 }
1736 }
1737
1738 static struct mt7996_msdu_page *
mt7996_msdu_page_get_from_cache(struct mt7996_dev * dev)1739 mt7996_msdu_page_get_from_cache(struct mt7996_dev *dev)
1740 {
1741 struct mt7996_msdu_page *p = NULL;
1742
1743 spin_lock(&dev->wed_rro.lock);
1744
1745 if (!list_empty(&dev->wed_rro.page_cache)) {
1746 p = list_first_entry(&dev->wed_rro.page_cache,
1747 struct mt7996_msdu_page, list);
1748 list_del(&p->list);
1749 }
1750
1751 spin_unlock(&dev->wed_rro.lock);
1752
1753 return p;
1754 }
1755
mt7996_msdu_page_get(struct mt7996_dev * dev)1756 static struct mt7996_msdu_page *mt7996_msdu_page_get(struct mt7996_dev *dev)
1757 {
1758 struct mt7996_msdu_page *p;
1759
1760 p = mt7996_msdu_page_get_from_cache(dev);
1761 if (!p) {
1762 p = kzalloc(L1_CACHE_ALIGN(sizeof(*p)), GFP_ATOMIC);
1763 if (p)
1764 INIT_LIST_HEAD(&p->list);
1765 }
1766
1767 return p;
1768 }
1769
mt7996_msdu_page_put_to_cache(struct mt7996_dev * dev,struct mt7996_msdu_page * p)1770 static void mt7996_msdu_page_put_to_cache(struct mt7996_dev *dev,
1771 struct mt7996_msdu_page *p)
1772 {
1773 if (p->buf) {
1774 mt76_put_page_pool_buf(p->buf, false);
1775 p->buf = NULL;
1776 }
1777
1778 spin_lock(&dev->wed_rro.lock);
1779 list_add(&p->list, &dev->wed_rro.page_cache);
1780 spin_unlock(&dev->wed_rro.lock);
1781 }
1782
mt7996_msdu_page_free_cache(struct mt7996_dev * dev)1783 static void mt7996_msdu_page_free_cache(struct mt7996_dev *dev)
1784 {
1785 while (true) {
1786 struct mt7996_msdu_page *p;
1787
1788 p = mt7996_msdu_page_get_from_cache(dev);
1789 if (!p)
1790 break;
1791
1792 if (p->buf)
1793 mt76_put_page_pool_buf(p->buf, false);
1794
1795 kfree(p);
1796 }
1797 }
1798
mt7996_msdu_page_hash_from_addr(dma_addr_t dma_addr)1799 static u32 mt7996_msdu_page_hash_from_addr(dma_addr_t dma_addr)
1800 {
1801 u32 val = 0;
1802 int i = 0;
1803
1804 while (dma_addr) {
1805 val += (u32)((dma_addr & 0xff) + i) % MT7996_RRO_MSDU_PG_HASH_SIZE;
1806 dma_addr >>= 8;
1807 i += 13;
1808 }
1809
1810 return val % MT7996_RRO_MSDU_PG_HASH_SIZE;
1811 }
1812
1813 static struct mt7996_msdu_page *
mt7996_rro_msdu_page_get(struct mt7996_dev * dev,dma_addr_t dma_addr)1814 mt7996_rro_msdu_page_get(struct mt7996_dev *dev, dma_addr_t dma_addr)
1815 {
1816 u32 hash = mt7996_msdu_page_hash_from_addr(dma_addr);
1817 struct mt7996_msdu_page *p, *tmp, *addr = NULL;
1818
1819 spin_lock(&dev->wed_rro.lock);
1820
1821 list_for_each_entry_safe(p, tmp, &dev->wed_rro.page_map[hash],
1822 list) {
1823 if (p->dma_addr == dma_addr) {
1824 list_del(&p->list);
1825 addr = p;
1826 break;
1827 }
1828 }
1829
1830 spin_unlock(&dev->wed_rro.lock);
1831
1832 return addr;
1833 }
1834
mt7996_rx_token_put(struct mt7996_dev * dev)1835 static void mt7996_rx_token_put(struct mt7996_dev *dev)
1836 {
1837 int i;
1838
1839 for (i = 0; i < dev->mt76.rx_token_size; i++) {
1840 struct mt76_txwi_cache *t;
1841
1842 t = mt76_rx_token_release(&dev->mt76, i);
1843 if (!t || !t->ptr)
1844 continue;
1845
1846 mt76_put_page_pool_buf(t->ptr, false);
1847 t->dma_addr = 0;
1848 t->ptr = NULL;
1849
1850 mt76_put_rxwi(&dev->mt76, t);
1851 }
1852 }
1853
mt7996_rro_msdu_page_map_free(struct mt7996_dev * dev)1854 void mt7996_rro_msdu_page_map_free(struct mt7996_dev *dev)
1855 {
1856 struct mt7996_msdu_page *p, *tmp;
1857 int i;
1858
1859 local_bh_disable();
1860
1861 for (i = 0; i < ARRAY_SIZE(dev->wed_rro.page_map); i++) {
1862 list_for_each_entry_safe(p, tmp, &dev->wed_rro.page_map[i],
1863 list) {
1864 list_del_init(&p->list);
1865 if (p->buf)
1866 mt76_put_page_pool_buf(p->buf, false);
1867 kfree(p);
1868 }
1869 }
1870 mt7996_msdu_page_free_cache(dev);
1871
1872 local_bh_enable();
1873
1874 mt7996_rx_token_put(dev);
1875 }
1876
mt7996_rro_msdu_page_add(struct mt76_dev * mdev,struct mt76_queue * q,dma_addr_t dma_addr,void * data)1877 int mt7996_rro_msdu_page_add(struct mt76_dev *mdev, struct mt76_queue *q,
1878 dma_addr_t dma_addr, void *data)
1879 {
1880 struct mt7996_dev *dev = container_of(mdev, struct mt7996_dev, mt76);
1881 struct mt7996_msdu_page_info *pinfo = data;
1882 struct mt7996_msdu_page *p;
1883 u32 hash;
1884
1885 pinfo->data |= cpu_to_le32(FIELD_PREP(MSDU_PAGE_INFO_OWNER_MASK, 1));
1886 p = mt7996_msdu_page_get(dev);
1887 if (!p)
1888 return -ENOMEM;
1889
1890 p->buf = data;
1891 p->dma_addr = dma_addr;
1892 p->q = q;
1893
1894 hash = mt7996_msdu_page_hash_from_addr(dma_addr);
1895
1896 spin_lock(&dev->wed_rro.lock);
1897 list_add_tail(&p->list, &dev->wed_rro.page_map[hash]);
1898 spin_unlock(&dev->wed_rro.lock);
1899
1900 return 0;
1901 }
1902
1903 static struct mt7996_wed_rro_addr *
mt7996_rro_addr_elem_get(struct mt7996_dev * dev,u16 session_id,u16 seq_num)1904 mt7996_rro_addr_elem_get(struct mt7996_dev *dev, u16 session_id, u16 seq_num)
1905 {
1906 u32 idx = 0;
1907 void *addr;
1908
1909 if (session_id == MT7996_RRO_MAX_SESSION) {
1910 addr = dev->wed_rro.session.ptr;
1911 } else {
1912 idx = session_id / MT7996_RRO_BA_BITMAP_SESSION_SIZE;
1913 addr = dev->wed_rro.addr_elem[idx].ptr;
1914
1915 idx = session_id % MT7996_RRO_BA_BITMAP_SESSION_SIZE;
1916 idx = idx * MT7996_RRO_WINDOW_MAX_LEN;
1917 }
1918 idx += seq_num % MT7996_RRO_WINDOW_MAX_LEN;
1919
1920 return addr + idx * sizeof(struct mt7996_wed_rro_addr);
1921 }
1922
1923 #define MT996_RRO_SN_MASK GENMASK(11, 0)
1924
mt7996_rro_rx_process(struct mt76_dev * mdev,void * data)1925 void mt7996_rro_rx_process(struct mt76_dev *mdev, void *data)
1926 {
1927 struct mt7996_dev *dev = container_of(mdev, struct mt7996_dev, mt76);
1928 struct mt76_wed_rro_ind *cmd = (struct mt76_wed_rro_ind *)data;
1929 u32 cmd_data0 = le32_to_cpu(cmd->data0);
1930 u32 cmd_data1 = le32_to_cpu(cmd->data1);
1931 u8 ind_reason = FIELD_GET(RRO_IND_DATA0_IND_REASON_MASK, cmd_data0);
1932 u16 start_seq = FIELD_GET(RRO_IND_DATA0_START_SEQ_MASK, cmd_data0);
1933 u16 seq_id = FIELD_GET(RRO_IND_DATA0_SEQ_ID_MASK, cmd_data0);
1934 u16 ind_count = FIELD_GET(RRO_IND_DATA1_IND_COUNT_MASK, cmd_data1);
1935 struct mt7996_msdu_page_info *pinfo = NULL;
1936 struct mt7996_msdu_page *p = NULL;
1937 int i, seq_num = 0;
1938
1939 for (i = 0; i < ind_count; i++) {
1940 struct mt7996_wed_rro_addr *e;
1941 struct mt76_rx_status *status;
1942 struct mt7996_rro_hif *rxd;
1943 int j, len, qid, data_len;
1944 struct mt76_txwi_cache *t;
1945 dma_addr_t dma_addr = 0;
1946 u16 rx_token_id, count;
1947 struct mt76_queue *q;
1948 struct sk_buff *skb;
1949 u32 info = 0, data;
1950 u8 signature;
1951 void *buf;
1952 bool ls;
1953
1954 seq_num = FIELD_GET(MT996_RRO_SN_MASK, start_seq + i);
1955 e = mt7996_rro_addr_elem_get(dev, seq_id, seq_num);
1956 data = le32_to_cpu(e->data);
1957 signature = FIELD_GET(WED_RRO_ADDR_SIGNATURE_MASK, data);
1958 if (signature != (seq_num / MT7996_RRO_WINDOW_MAX_LEN)) {
1959 u32 val = FIELD_PREP(WED_RRO_ADDR_SIGNATURE_MASK,
1960 0xff);
1961
1962 e->data |= cpu_to_le32(val);
1963 goto update_ack_seq_num;
1964 }
1965
1966 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
1967 dma_addr = FIELD_GET(WED_RRO_ADDR_HEAD_HIGH_MASK, data);
1968 dma_addr <<= 32;
1969 #endif
1970 dma_addr |= le32_to_cpu(e->head_low);
1971
1972 count = FIELD_GET(WED_RRO_ADDR_COUNT_MASK, data);
1973 for (j = 0; j < count; j++) {
1974 if (!p) {
1975 p = mt7996_rro_msdu_page_get(dev, dma_addr);
1976 if (!p)
1977 continue;
1978
1979 dma_sync_single_for_cpu(mdev->dma_dev, p->dma_addr,
1980 SKB_WITH_OVERHEAD(p->q->buf_size),
1981 page_pool_get_dma_dir(p->q->page_pool));
1982 pinfo = (struct mt7996_msdu_page_info *)p->buf;
1983 }
1984
1985 rxd = &pinfo->rxd[j % MT7996_MAX_HIF_RXD_IN_PG];
1986 len = FIELD_GET(RRO_HIF_DATA1_SDL_MASK,
1987 le32_to_cpu(rxd->data1));
1988
1989 rx_token_id = FIELD_GET(RRO_HIF_DATA4_RX_TOKEN_ID_MASK,
1990 le32_to_cpu(rxd->data4));
1991 t = mt76_rx_token_release(mdev, rx_token_id);
1992 if (!t)
1993 goto next_page;
1994
1995 qid = t->qid;
1996 buf = t->ptr;
1997 q = &mdev->q_rx[qid];
1998 dma_sync_single_for_cpu(mdev->dma_dev, t->dma_addr,
1999 SKB_WITH_OVERHEAD(q->buf_size),
2000 page_pool_get_dma_dir(q->page_pool));
2001
2002 t->dma_addr = 0;
2003 t->ptr = NULL;
2004 mt76_put_rxwi(mdev, t);
2005 if (!buf)
2006 goto next_page;
2007
2008 if (q->rx_head)
2009 data_len = q->buf_size;
2010 else
2011 data_len = SKB_WITH_OVERHEAD(q->buf_size);
2012
2013 if (data_len < len + q->buf_offset) {
2014 dev_kfree_skb(q->rx_head);
2015 mt76_put_page_pool_buf(buf, false);
2016 q->rx_head = NULL;
2017 goto next_page;
2018 }
2019
2020 ls = FIELD_GET(RRO_HIF_DATA1_LS_MASK,
2021 le32_to_cpu(rxd->data1));
2022 if (q->rx_head) {
2023 /* TODO: Take into account non-linear skb. */
2024 mt76_put_page_pool_buf(buf, false);
2025 if (ls) {
2026 dev_kfree_skb(q->rx_head);
2027 q->rx_head = NULL;
2028 }
2029 goto next_page;
2030 }
2031
2032 if (ls && !mt7996_rx_check(mdev, buf, len))
2033 goto next_page;
2034
2035 skb = build_skb(buf, q->buf_size);
2036 if (!skb)
2037 goto next_page;
2038
2039 skb_reserve(skb, q->buf_offset);
2040 skb_mark_for_recycle(skb);
2041 __skb_put(skb, len);
2042
2043 if (ind_reason == 1 || ind_reason == 2) {
2044 dev_kfree_skb(skb);
2045 goto next_page;
2046 }
2047
2048 if (!ls) {
2049 q->rx_head = skb;
2050 goto next_page;
2051 }
2052
2053 status = (struct mt76_rx_status *)skb->cb;
2054 if (seq_id != MT7996_RRO_MAX_SESSION)
2055 status->aggr = true;
2056
2057 mt7996_queue_rx_skb(mdev, qid, skb, &info);
2058 next_page:
2059 if ((j + 1) % MT7996_MAX_HIF_RXD_IN_PG == 0) {
2060 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
2061 dma_addr =
2062 FIELD_GET(MSDU_PAGE_INFO_PG_HIGH_MASK,
2063 le32_to_cpu(pinfo->data));
2064 dma_addr <<= 32;
2065 dma_addr |= le32_to_cpu(pinfo->pg_low);
2066 #else
2067 dma_addr = le32_to_cpu(pinfo->pg_low);
2068 #endif
2069 mt7996_msdu_page_put_to_cache(dev, p);
2070 p = NULL;
2071 }
2072 }
2073
2074 update_ack_seq_num:
2075 if ((i + 1) % 4 == 0)
2076 mt76_wr(dev, MT_RRO_ACK_SN_CTRL,
2077 FIELD_PREP(MT_RRO_ACK_SN_CTRL_SESSION_MASK,
2078 seq_id) |
2079 FIELD_PREP(MT_RRO_ACK_SN_CTRL_SN_MASK,
2080 seq_num));
2081 if (p) {
2082 mt7996_msdu_page_put_to_cache(dev, p);
2083 p = NULL;
2084 }
2085 }
2086
2087 /* Update ack_seq_num for remaining addr_elem */
2088 if (i % 4)
2089 mt76_wr(dev, MT_RRO_ACK_SN_CTRL,
2090 FIELD_PREP(MT_RRO_ACK_SN_CTRL_SESSION_MASK, seq_id) |
2091 FIELD_PREP(MT_RRO_ACK_SN_CTRL_SN_MASK, seq_num));
2092 }
2093
mt7996_mac_cca_stats_reset(struct mt7996_phy * phy)2094 void mt7996_mac_cca_stats_reset(struct mt7996_phy *phy)
2095 {
2096 struct mt7996_dev *dev = phy->dev;
2097 u32 reg = MT_WF_PHYRX_BAND_RX_CTRL1(phy->mt76->band_idx);
2098
2099 mt76_clear(dev, reg, MT_WF_PHYRX_BAND_RX_CTRL1_STSCNT_EN);
2100 mt76_set(dev, reg, BIT(11) | BIT(9));
2101 }
2102
mt7996_mac_reset_counters(struct mt7996_phy * phy)2103 void mt7996_mac_reset_counters(struct mt7996_phy *phy)
2104 {
2105 struct mt7996_dev *dev = phy->dev;
2106 u8 band_idx = phy->mt76->band_idx;
2107 int i;
2108
2109 for (i = 0; i < 16; i++)
2110 mt76_rr(dev, MT_TX_AGG_CNT(band_idx, i));
2111
2112 phy->mt76->survey_time = ktime_get_boottime();
2113
2114 memset(phy->mt76->aggr_stats, 0, sizeof(phy->mt76->aggr_stats));
2115
2116 /* reset airtime counters */
2117 mt76_set(dev, MT_WF_RMAC_MIB_AIRTIME0(band_idx),
2118 MT_WF_RMAC_MIB_RXTIME_CLR);
2119
2120 mt7996_mcu_get_chan_mib_info(phy, true);
2121 }
2122
mt7996_mac_set_coverage_class(struct mt7996_phy * phy)2123 void mt7996_mac_set_coverage_class(struct mt7996_phy *phy)
2124 {
2125 s16 coverage_class = phy->coverage_class;
2126 struct mt7996_dev *dev = phy->dev;
2127 u32 reg_offset;
2128 u32 cck = FIELD_PREP(MT_TIMEOUT_VAL_PLCP, 231) |
2129 FIELD_PREP(MT_TIMEOUT_VAL_CCA, 48);
2130 u32 ofdm = FIELD_PREP(MT_TIMEOUT_VAL_PLCP, 60) |
2131 FIELD_PREP(MT_TIMEOUT_VAL_CCA, 28);
2132 u8 band_idx = phy->mt76->band_idx;
2133 int offset;
2134
2135 if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
2136 return;
2137
2138 offset = 3 * coverage_class;
2139 reg_offset = FIELD_PREP(MT_TIMEOUT_VAL_PLCP, offset) |
2140 FIELD_PREP(MT_TIMEOUT_VAL_CCA, offset);
2141
2142 mt76_wr(dev, MT_TMAC_CDTR(band_idx), cck + reg_offset);
2143 mt76_wr(dev, MT_TMAC_ODTR(band_idx), ofdm + reg_offset);
2144 }
2145
mt7996_mac_enable_nf(struct mt7996_dev * dev,u8 band)2146 void mt7996_mac_enable_nf(struct mt7996_dev *dev, u8 band)
2147 {
2148 mt76_set(dev, MT_WF_PHYRX_CSD_BAND_RXTD12(band),
2149 MT_WF_PHYRX_CSD_BAND_RXTD12_IRPI_SW_CLR_ONLY |
2150 MT_WF_PHYRX_CSD_BAND_RXTD12_IRPI_SW_CLR);
2151
2152 mt76_set(dev, MT_WF_PHYRX_BAND_RX_CTRL1(band),
2153 FIELD_PREP(MT_WF_PHYRX_BAND_RX_CTRL1_IPI_EN, 0x5));
2154 }
2155
2156 static u8
mt7996_phy_get_nf(struct mt7996_phy * phy,u8 band_idx)2157 mt7996_phy_get_nf(struct mt7996_phy *phy, u8 band_idx)
2158 {
2159 static const u8 nf_power[] = { 92, 89, 86, 83, 80, 75, 70, 65, 60, 55, 52 };
2160 struct mt7996_dev *dev = phy->dev;
2161 u32 val, sum = 0, n = 0;
2162 int ant, i;
2163
2164 for (ant = 0; ant < hweight8(phy->mt76->antenna_mask); ant++) {
2165 u32 reg = MT_WF_PHYRX_CSD_IRPI(band_idx, ant);
2166
2167 for (i = 0; i < ARRAY_SIZE(nf_power); i++, reg += 4) {
2168 val = mt76_rr(dev, reg);
2169 sum += val * nf_power[i];
2170 n += val;
2171 }
2172 }
2173
2174 return n ? sum / n : 0;
2175 }
2176
mt7996_update_channel(struct mt76_phy * mphy)2177 void mt7996_update_channel(struct mt76_phy *mphy)
2178 {
2179 struct mt7996_phy *phy = mphy->priv;
2180 struct mt76_channel_state *state = mphy->chan_state;
2181 int nf;
2182
2183 mt7996_mcu_get_chan_mib_info(phy, false);
2184
2185 nf = mt7996_phy_get_nf(phy, mphy->band_idx);
2186 if (!phy->noise)
2187 phy->noise = nf << 4;
2188 else if (nf)
2189 phy->noise += nf - (phy->noise >> 4);
2190
2191 state->noise = -(phy->noise >> 4);
2192 }
2193
2194 static bool
mt7996_wait_reset_state(struct mt7996_dev * dev,u32 state)2195 mt7996_wait_reset_state(struct mt7996_dev *dev, u32 state)
2196 {
2197 bool ret;
2198
2199 ret = wait_event_timeout(dev->reset_wait,
2200 (READ_ONCE(dev->recovery.state) & state),
2201 MT7996_RESET_TIMEOUT);
2202
2203 WARN(!ret, "Timeout waiting for MCU reset state %x\n", state);
2204 return ret;
2205 }
2206
2207 static void
mt7996_update_vif_beacon(void * priv,u8 * mac,struct ieee80211_vif * vif)2208 mt7996_update_vif_beacon(void *priv, u8 *mac, struct ieee80211_vif *vif)
2209 {
2210 struct ieee80211_bss_conf *link_conf;
2211 struct mt7996_phy *phy = priv;
2212 struct mt7996_dev *dev = phy->dev;
2213 unsigned int link_id;
2214
2215
2216 switch (vif->type) {
2217 case NL80211_IFTYPE_MESH_POINT:
2218 case NL80211_IFTYPE_ADHOC:
2219 case NL80211_IFTYPE_AP:
2220 break;
2221 default:
2222 return;
2223 }
2224
2225 for_each_vif_active_link(vif, link_conf, link_id) {
2226 struct mt7996_vif_link *link;
2227 struct mt7996_phy *link_phy;
2228
2229 link = mt7996_vif_link(dev, vif, link_id);
2230 if (!link)
2231 continue;
2232
2233 link_phy = mt7996_vif_link_phy(link);
2234 if (link_phy != phy)
2235 continue;
2236
2237 mt7996_mcu_add_beacon(dev->mt76.hw, vif, link_conf,
2238 link_conf->enable_beacon);
2239 }
2240 }
2241
mt7996_mac_update_beacons(struct mt7996_phy * phy)2242 void mt7996_mac_update_beacons(struct mt7996_phy *phy)
2243 {
2244 ieee80211_iterate_active_interfaces(phy->mt76->hw,
2245 IEEE80211_IFACE_ITER_RESUME_ALL,
2246 mt7996_update_vif_beacon, phy);
2247 }
2248
2249 static void
mt7996_update_beacons(struct mt7996_dev * dev)2250 mt7996_update_beacons(struct mt7996_dev *dev)
2251 {
2252 struct mt76_phy *phy2, *phy3;
2253
2254 mt7996_mac_update_beacons(&dev->phy);
2255
2256 phy2 = dev->mt76.phys[MT_BAND1];
2257 if (phy2)
2258 mt7996_mac_update_beacons(phy2->priv);
2259
2260 phy3 = dev->mt76.phys[MT_BAND2];
2261 if (phy3)
2262 mt7996_mac_update_beacons(phy3->priv);
2263 }
2264
mt7996_tx_token_put(struct mt7996_dev * dev)2265 void mt7996_tx_token_put(struct mt7996_dev *dev)
2266 {
2267 struct mt76_txwi_cache *txwi;
2268 int id;
2269
2270 spin_lock_bh(&dev->mt76.token_lock);
2271 idr_for_each_entry(&dev->mt76.token, txwi, id) {
2272 mt7996_txwi_free(dev, txwi, NULL, NULL, NULL);
2273 dev->mt76.token_count--;
2274 }
2275 spin_unlock_bh(&dev->mt76.token_lock);
2276 idr_destroy(&dev->mt76.token);
2277
2278 for (id = 0; id < __MT_MAX_BAND; id++) {
2279 struct mt76_phy *phy = dev->mt76.phys[id];
2280 if (phy)
2281 atomic_set(&phy->mgmt_tx_pending, 0);
2282 }
2283 }
2284
2285 static int
mt7996_mac_restart(struct mt7996_dev * dev)2286 mt7996_mac_restart(struct mt7996_dev *dev)
2287 {
2288 struct mt76_dev *mdev = &dev->mt76;
2289 struct mt7996_phy *phy;
2290 int i, ret;
2291
2292 if (dev->hif2) {
2293 mt76_wr(dev, MT_INT1_MASK_CSR, 0x0);
2294 mt76_wr(dev, MT_INT1_SOURCE_CSR, ~0);
2295 }
2296
2297 if (dev_is_pci(mdev->dev)) {
2298 mt76_wr(dev, MT_PCIE_MAC_INT_ENABLE, 0x0);
2299 if (dev->hif2)
2300 mt76_wr(dev, MT_PCIE1_MAC_INT_ENABLE, 0x0);
2301 }
2302
2303 set_bit(MT76_MCU_RESET, &dev->mphy.state);
2304 mt7996_for_each_phy(dev, phy)
2305 set_bit(MT76_RESET, &phy->mt76->state);
2306 wake_up(&dev->mt76.mcu.wait);
2307
2308 /* lock/unlock all queues to ensure that no tx is pending */
2309 mt7996_for_each_phy(dev, phy)
2310 mt76_txq_schedule_all(phy->mt76);
2311
2312 /* disable all tx/rx napi */
2313 mt76_worker_disable(&dev->mt76.tx_worker);
2314 mt76_for_each_q_rx(mdev, i) {
2315 if (mtk_wed_device_active(&dev->mt76.mmio.wed) &&
2316 mt76_queue_is_wed_rro(&mdev->q_rx[i]))
2317 continue;
2318
2319 if (mdev->q_rx[i].ndesc)
2320 napi_disable(&dev->mt76.napi[i]);
2321 }
2322 napi_disable(&dev->mt76.tx_napi);
2323
2324 /* token reinit */
2325 mt7996_tx_token_put(dev);
2326 idr_init(&dev->mt76.token);
2327
2328 mt7996_dma_reset(dev, true);
2329
2330 mt76_for_each_q_rx(mdev, i) {
2331 if (mtk_wed_device_active(&dev->mt76.mmio.wed) &&
2332 mt76_queue_is_wed_rro(&mdev->q_rx[i]))
2333 continue;
2334
2335 if (mdev->q_rx[i].ndesc) {
2336 napi_enable(&dev->mt76.napi[i]);
2337 local_bh_disable();
2338 napi_schedule(&dev->mt76.napi[i]);
2339 local_bh_enable();
2340 }
2341 }
2342 clear_bit(MT76_MCU_RESET, &dev->mphy.state);
2343 clear_bit(MT76_STATE_MCU_RUNNING, &dev->mphy.state);
2344
2345 mt76_wr(dev, MT_INT_MASK_CSR, dev->mt76.mmio.irqmask);
2346 mt76_wr(dev, MT_INT_SOURCE_CSR, ~0);
2347 if (dev->hif2) {
2348 mt76_wr(dev, MT_INT1_MASK_CSR, dev->mt76.mmio.irqmask);
2349 mt76_wr(dev, MT_INT1_SOURCE_CSR, ~0);
2350 }
2351 if (dev_is_pci(mdev->dev)) {
2352 mt76_wr(dev, MT_PCIE_MAC_INT_ENABLE, 0xff);
2353 if (dev->hif2)
2354 mt76_wr(dev, MT_PCIE1_MAC_INT_ENABLE, 0xff);
2355 }
2356
2357 /* load firmware */
2358 ret = mt7996_mcu_init_firmware(dev);
2359 if (ret)
2360 goto out;
2361
2362 if (mtk_wed_device_active(&dev->mt76.mmio.wed) &&
2363 mt7996_has_hwrro(dev)) {
2364 u32 wed_irq_mask = dev->mt76.mmio.irqmask |
2365 MT_INT_TX_DONE_BAND2;
2366
2367 mt7996_rro_hw_init(dev);
2368 mt76_for_each_q_rx(&dev->mt76, i) {
2369 if (mt76_queue_is_wed_rro_ind(&dev->mt76.q_rx[i]) ||
2370 mt76_queue_is_wed_rro_msdu_pg(&dev->mt76.q_rx[i]))
2371 mt76_queue_rx_reset(dev, i);
2372 }
2373
2374 mt76_wr(dev, MT_INT_MASK_CSR, wed_irq_mask);
2375 mtk_wed_device_start_hw_rro(&dev->mt76.mmio.wed, wed_irq_mask,
2376 false);
2377 mt7996_irq_enable(dev, wed_irq_mask);
2378 mt7996_irq_disable(dev, 0);
2379 }
2380
2381 if (mtk_wed_device_active(&dev->mt76.mmio.wed_hif2)) {
2382 mt76_wr(dev, MT_INT_PCIE1_MASK_CSR,
2383 MT_INT_TX_RX_DONE_EXT);
2384 mtk_wed_device_start(&dev->mt76.mmio.wed_hif2,
2385 MT_INT_TX_RX_DONE_EXT);
2386 }
2387
2388 /* set the necessary init items */
2389 ret = mt7996_mcu_set_eeprom(dev);
2390 if (ret)
2391 goto out;
2392
2393 mt7996_mac_init(dev);
2394 mt7996_for_each_phy(dev, phy)
2395 mt7996_init_txpower(phy);
2396 ret = mt7996_txbf_init(dev);
2397 if (ret)
2398 goto out;
2399
2400 mt7996_for_each_phy(dev, phy) {
2401 if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
2402 continue;
2403
2404 ret = mt7996_run(phy);
2405 if (ret)
2406 goto out;
2407 }
2408
2409 out:
2410 /* reset done */
2411 mt7996_for_each_phy(dev, phy)
2412 clear_bit(MT76_RESET, &phy->mt76->state);
2413
2414 napi_enable(&dev->mt76.tx_napi);
2415 local_bh_disable();
2416 napi_schedule(&dev->mt76.tx_napi);
2417 local_bh_enable();
2418
2419 mt76_worker_enable(&dev->mt76.tx_worker);
2420 return ret;
2421 }
2422
2423 static void
mt7996_mac_reset_sta_iter(void * data,struct ieee80211_sta * sta)2424 mt7996_mac_reset_sta_iter(void *data, struct ieee80211_sta *sta)
2425 {
2426 struct mt7996_sta *msta = (struct mt7996_sta *)sta->drv_priv;
2427 struct mt7996_dev *dev = data;
2428 int i;
2429
2430 for (i = 0; i < ARRAY_SIZE(msta->link); i++)
2431 mt7996_mac_sta_remove_link(dev, sta, i, true);
2432 }
2433
2434 static void
mt7996_mac_reset_vif_iter(void * data,u8 * mac,struct ieee80211_vif * vif)2435 mt7996_mac_reset_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
2436 {
2437 struct mt76_vif_link *mlink = (struct mt76_vif_link *)vif->drv_priv;
2438 struct mt76_vif_data *mvif = mlink->mvif;
2439 struct mt7996_dev *dev = data;
2440 int i;
2441
2442 rcu_read_lock();
2443 for (i = 0; i < ARRAY_SIZE(mvif->link); i++) {
2444
2445 mlink = mt76_dereference(mvif->link[i], &dev->mt76);
2446 if (!mlink || mlink == (struct mt76_vif_link *)vif->drv_priv)
2447 continue;
2448
2449 rcu_assign_pointer(mvif->link[i], NULL);
2450 kfree_rcu(mlink, rcu_head);
2451 }
2452 mvif->valid_links = 0;
2453 rcu_read_unlock();
2454 }
2455
2456 static void
mt7996_mac_full_reset(struct mt7996_dev * dev)2457 mt7996_mac_full_reset(struct mt7996_dev *dev)
2458 {
2459 struct ieee80211_hw *hw = mt76_hw(dev);
2460 struct mt7996_phy *phy;
2461 LIST_HEAD(list);
2462 int i;
2463
2464 dev->recovery.hw_full_reset = true;
2465
2466 set_bit(MT76_MCU_RESET, &dev->mphy.state);
2467 wake_up(&dev->mt76.mcu.wait);
2468 ieee80211_stop_queues(hw);
2469
2470 cancel_work_sync(&dev->wed_rro.work);
2471 mt7996_for_each_phy(dev, phy)
2472 cancel_delayed_work_sync(&phy->mt76->mac_work);
2473
2474 mt76_abort_scan(&dev->mt76);
2475
2476 mutex_lock(&dev->mt76.mutex);
2477 for (i = 0; i < 10; i++) {
2478 if (!mt7996_mac_restart(dev))
2479 break;
2480 }
2481
2482 if (i == 10)
2483 dev_err(dev->mt76.dev, "chip full reset failed\n");
2484
2485 mt7996_for_each_phy(dev, phy)
2486 phy->omac_mask = 0;
2487 dev->mld_idx_mask = 0;
2488 dev->mld_remap_idx_mask = 0;
2489
2490 ieee80211_iterate_stations_atomic(hw, mt7996_mac_reset_sta_iter, dev);
2491 mt76_reset_device(&dev->mt76);
2492 ieee80211_iterate_active_interfaces_atomic(hw,
2493 IEEE80211_IFACE_SKIP_SDATA_NOT_IN_DRIVER,
2494 mt7996_mac_reset_vif_iter, dev);
2495
2496 INIT_LIST_HEAD(&dev->sta_rc_list);
2497 INIT_LIST_HEAD(&dev->twt_list);
2498
2499 spin_lock_bh(&dev->wed_rro.lock);
2500 list_splice_init(&dev->wed_rro.poll_list, &list);
2501 spin_unlock_bh(&dev->wed_rro.lock);
2502
2503 while (!list_empty(&list)) {
2504 struct mt7996_wed_rro_session_id *e;
2505
2506 e = list_first_entry(&list, struct mt7996_wed_rro_session_id,
2507 list);
2508 list_del_init(&e->list);
2509 kfree(e);
2510 }
2511
2512 i = mt76_wcid_alloc(dev->mt76.wcid_mask, MT7996_WTBL_STA);
2513 dev->mt76.global_wcid.idx = i;
2514 dev->recovery.hw_full_reset = false;
2515
2516 mutex_unlock(&dev->mt76.mutex);
2517
2518 ieee80211_wake_queues(mt76_hw(dev));
2519
2520 ieee80211_restart_hw(mt76_hw(dev));
2521 }
2522
mt7996_mac_reset_work(struct work_struct * work)2523 void mt7996_mac_reset_work(struct work_struct *work)
2524 {
2525 struct ieee80211_hw *hw;
2526 struct mt7996_dev *dev;
2527 struct mt7996_phy *phy;
2528 int i;
2529
2530 dev = container_of(work, struct mt7996_dev, reset_work);
2531 hw = mt76_hw(dev);
2532
2533 /* chip full reset */
2534 if (dev->recovery.restart) {
2535 /* disable WA/WM WDT */
2536 mt76_clear(dev, MT_WFDMA0_MCU_HOST_INT_ENA,
2537 MT_MCU_CMD_WDT_MASK);
2538
2539 if (READ_ONCE(dev->recovery.state) & MT_MCU_CMD_WA_WDT)
2540 dev->recovery.wa_reset_count++;
2541 else
2542 dev->recovery.wm_reset_count++;
2543
2544 mt7996_mac_full_reset(dev);
2545
2546 /* enable mcu irq */
2547 mt7996_irq_enable(dev, MT_INT_MCU_CMD);
2548 mt7996_irq_disable(dev, 0);
2549
2550 /* enable WA/WM WDT */
2551 mt76_set(dev, MT_WFDMA0_MCU_HOST_INT_ENA, MT_MCU_CMD_WDT_MASK);
2552
2553 dev->recovery.state = MT_MCU_CMD_NORMAL_STATE;
2554 dev->recovery.restart = false;
2555 return;
2556 }
2557
2558 if (!(READ_ONCE(dev->recovery.state) & MT_MCU_CMD_STOP_DMA))
2559 return;
2560
2561 dev_info(dev->mt76.dev,"\n%s L1 SER recovery start.",
2562 wiphy_name(hw->wiphy));
2563
2564 if (mtk_wed_device_active(&dev->mt76.mmio.wed_hif2))
2565 mtk_wed_device_stop(&dev->mt76.mmio.wed_hif2);
2566
2567 if (mtk_wed_device_active(&dev->mt76.mmio.wed))
2568 mtk_wed_device_stop(&dev->mt76.mmio.wed);
2569
2570 mt7996_npu_hw_stop(dev);
2571 ieee80211_stop_queues(mt76_hw(dev));
2572
2573 set_bit(MT76_RESET, &dev->mphy.state);
2574 set_bit(MT76_MCU_RESET, &dev->mphy.state);
2575 wake_up(&dev->mt76.mcu.wait);
2576 mt76_abort_scan(&dev->mt76);
2577
2578 cancel_work_sync(&dev->wed_rro.work);
2579 mt7996_for_each_phy(dev, phy) {
2580 mt76_abort_roc(phy->mt76);
2581 set_bit(MT76_RESET, &phy->mt76->state);
2582 cancel_delayed_work_sync(&phy->mt76->mac_work);
2583 }
2584
2585 mutex_lock(&dev->mt76.mutex);
2586
2587 mt76_worker_disable(&dev->mt76.tx_worker);
2588 mt76_for_each_q_rx(&dev->mt76, i) {
2589 if (mtk_wed_device_active(&dev->mt76.mmio.wed) &&
2590 mt76_queue_is_wed_rro(&dev->mt76.q_rx[i]))
2591 continue;
2592
2593 if (mt76_npu_device_active(&dev->mt76) &&
2594 mt76_queue_is_wed_rro(&dev->mt76.q_rx[i]))
2595 continue;
2596
2597 if (mt76_queue_is_npu_txfree(&dev->mt76.q_rx[i]))
2598 continue;
2599
2600 napi_disable(&dev->mt76.napi[i]);
2601 }
2602 napi_disable(&dev->mt76.tx_napi);
2603
2604 mt76_wr(dev, MT_MCU_INT_EVENT, MT_MCU_INT_EVENT_DMA_STOPPED);
2605
2606 if (mt7996_wait_reset_state(dev, MT_MCU_CMD_RESET_DONE)) {
2607 mt7996_dma_reset(dev, false);
2608
2609 mt7996_tx_token_put(dev);
2610 idr_init(&dev->mt76.token);
2611
2612 mt76_wr(dev, MT_MCU_INT_EVENT, MT_MCU_INT_EVENT_DMA_INIT);
2613 mt7996_wait_reset_state(dev, MT_MCU_CMD_RECOVERY_DONE);
2614 }
2615
2616 mt76_wr(dev, MT_MCU_INT_EVENT, MT_MCU_INT_EVENT_RESET_DONE);
2617 mt7996_wait_reset_state(dev, MT_MCU_CMD_NORMAL_STATE);
2618
2619 /* enable DMA Rx/Tx and interrupt */
2620 mt7996_dma_start(dev, false, false);
2621
2622 if (!is_mt7996(&dev->mt76) && dev->mt76.hwrro_mode == MT76_HWRRO_V3)
2623 mt76_set(dev, MT_RRO_3_0_EMU_CONF, MT_RRO_3_0_EMU_CONF_EN_MASK);
2624
2625 if (mtk_wed_device_active(&dev->mt76.mmio.wed)) {
2626 u32 wed_irq_mask = MT_INT_TX_DONE_BAND2 |
2627 dev->mt76.mmio.irqmask;
2628
2629 mt76_wr(dev, MT_INT_MASK_CSR, wed_irq_mask);
2630 mtk_wed_device_start_hw_rro(&dev->mt76.mmio.wed, wed_irq_mask,
2631 true);
2632 mt7996_irq_enable(dev, wed_irq_mask);
2633 mt7996_irq_disable(dev, 0);
2634 }
2635
2636 if (mtk_wed_device_active(&dev->mt76.mmio.wed_hif2)) {
2637 mt76_wr(dev, MT_INT_PCIE1_MASK_CSR, MT_INT_TX_RX_DONE_EXT);
2638 mtk_wed_device_start(&dev->mt76.mmio.wed_hif2,
2639 MT_INT_TX_RX_DONE_EXT);
2640 }
2641
2642 __mt7996_npu_hw_init(dev);
2643
2644 clear_bit(MT76_MCU_RESET, &dev->mphy.state);
2645 mt7996_for_each_phy(dev, phy)
2646 clear_bit(MT76_RESET, &phy->mt76->state);
2647
2648 mt76_for_each_q_rx(&dev->mt76, i) {
2649 if (mtk_wed_device_active(&dev->mt76.mmio.wed) &&
2650 mt76_queue_is_wed_rro(&dev->mt76.q_rx[i]))
2651 continue;
2652
2653 if (mt76_npu_device_active(&dev->mt76) &&
2654 mt76_queue_is_wed_rro(&dev->mt76.q_rx[i]))
2655 continue;
2656
2657 if (mt76_queue_is_npu_txfree(&dev->mt76.q_rx[i]))
2658 continue;
2659
2660 napi_enable(&dev->mt76.napi[i]);
2661 local_bh_disable();
2662 napi_schedule(&dev->mt76.napi[i]);
2663 local_bh_enable();
2664 }
2665
2666 tasklet_schedule(&dev->mt76.irq_tasklet);
2667
2668 mt76_worker_enable(&dev->mt76.tx_worker);
2669
2670 napi_enable(&dev->mt76.tx_napi);
2671 local_bh_disable();
2672 napi_schedule(&dev->mt76.tx_napi);
2673 local_bh_enable();
2674
2675 ieee80211_wake_queues(hw);
2676 mt7996_update_beacons(dev);
2677
2678 mutex_unlock(&dev->mt76.mutex);
2679
2680 mt7996_for_each_phy(dev, phy)
2681 ieee80211_queue_delayed_work(hw, &phy->mt76->mac_work,
2682 MT7996_WATCHDOG_TIME);
2683 dev_info(dev->mt76.dev,"\n%s L1 SER recovery completed.",
2684 wiphy_name(dev->mt76.hw->wiphy));
2685 }
2686
2687 /* firmware coredump */
mt7996_mac_dump_work(struct work_struct * work)2688 void mt7996_mac_dump_work(struct work_struct *work)
2689 {
2690 const struct mt7996_mem_region *mem_region;
2691 struct mt7996_crash_data *crash_data;
2692 struct mt7996_dev *dev;
2693 struct mt7996_mem_hdr *hdr;
2694 size_t buf_len;
2695 int i;
2696 u32 num;
2697 u8 *buf;
2698
2699 dev = container_of(work, struct mt7996_dev, dump_work);
2700
2701 mutex_lock(&dev->dump_mutex);
2702
2703 crash_data = mt7996_coredump_new(dev);
2704 if (!crash_data) {
2705 mutex_unlock(&dev->dump_mutex);
2706 goto skip_coredump;
2707 }
2708
2709 mem_region = mt7996_coredump_get_mem_layout(dev, &num);
2710 if (!mem_region || !crash_data->memdump_buf_len) {
2711 mutex_unlock(&dev->dump_mutex);
2712 goto skip_memdump;
2713 }
2714
2715 buf = crash_data->memdump_buf;
2716 buf_len = crash_data->memdump_buf_len;
2717
2718 /* dumping memory content... */
2719 memset(buf, 0, buf_len);
2720 for (i = 0; i < num; i++) {
2721 if (mem_region->len > buf_len) {
2722 dev_warn(dev->mt76.dev, "%s len %zu is too large\n",
2723 mem_region->name, mem_region->len);
2724 break;
2725 }
2726
2727 /* reserve space for the header */
2728 hdr = (void *)buf;
2729 buf += sizeof(*hdr);
2730 buf_len -= sizeof(*hdr);
2731
2732 mt7996_memcpy_fromio(dev, buf, mem_region->start,
2733 mem_region->len);
2734
2735 hdr->start = mem_region->start;
2736 hdr->len = mem_region->len;
2737
2738 if (!mem_region->len)
2739 /* note: the header remains, just with zero length */
2740 break;
2741
2742 buf += mem_region->len;
2743 buf_len -= mem_region->len;
2744
2745 mem_region++;
2746 }
2747
2748 mutex_unlock(&dev->dump_mutex);
2749
2750 skip_memdump:
2751 mt7996_coredump_submit(dev);
2752 skip_coredump:
2753 queue_work(dev->mt76.wq, &dev->reset_work);
2754 }
2755
mt7996_reset(struct mt7996_dev * dev)2756 void mt7996_reset(struct mt7996_dev *dev)
2757 {
2758 if (!dev->recovery.hw_init_done)
2759 return;
2760
2761 if (dev->recovery.hw_full_reset)
2762 return;
2763
2764 /* wm/wa exception: do full recovery */
2765 if (READ_ONCE(dev->recovery.state) & MT_MCU_CMD_WDT_MASK) {
2766 dev->recovery.restart = true;
2767 dev_info(dev->mt76.dev,
2768 "%s indicated firmware crash, attempting recovery\n",
2769 wiphy_name(dev->mt76.hw->wiphy));
2770
2771 mt7996_irq_disable(dev, MT_INT_MCU_CMD);
2772 queue_work(dev->mt76.wq, &dev->dump_work);
2773 return;
2774 }
2775
2776 if (READ_ONCE(dev->recovery.state) & MT_MCU_CMD_STOP_DMA) {
2777 set_bit(MT76_MCU_RESET, &dev->mphy.state);
2778 wake_up(&dev->mt76.mcu.wait);
2779 }
2780
2781 queue_work(dev->mt76.wq, &dev->reset_work);
2782 wake_up(&dev->reset_wait);
2783 }
2784
mt7996_mac_update_stats(struct mt7996_phy * phy)2785 void mt7996_mac_update_stats(struct mt7996_phy *phy)
2786 {
2787 struct mt76_mib_stats *mib = &phy->mib;
2788 struct mt7996_dev *dev = phy->dev;
2789 u8 band_idx = phy->mt76->band_idx;
2790 u32 cnt;
2791 int i;
2792
2793 cnt = mt76_rr(dev, MT_MIB_RSCR1(band_idx));
2794 mib->fcs_err_cnt += cnt;
2795
2796 cnt = mt76_rr(dev, MT_MIB_RSCR33(band_idx));
2797 mib->rx_fifo_full_cnt += cnt;
2798
2799 cnt = mt76_rr(dev, MT_MIB_RSCR31(band_idx));
2800 mib->rx_mpdu_cnt += cnt;
2801
2802 cnt = mt76_rr(dev, MT_MIB_SDR6(band_idx));
2803 mib->channel_idle_cnt += FIELD_GET(MT_MIB_SDR6_CHANNEL_IDL_CNT_MASK, cnt);
2804
2805 cnt = mt76_rr(dev, MT_MIB_RVSR0(band_idx));
2806 mib->rx_vector_mismatch_cnt += cnt;
2807
2808 cnt = mt76_rr(dev, MT_MIB_RSCR35(band_idx));
2809 mib->rx_delimiter_fail_cnt += cnt;
2810
2811 cnt = mt76_rr(dev, MT_MIB_RSCR36(band_idx));
2812 mib->rx_len_mismatch_cnt += cnt;
2813
2814 cnt = mt76_rr(dev, MT_MIB_TSCR0(band_idx));
2815 mib->tx_ampdu_cnt += cnt;
2816
2817 cnt = mt76_rr(dev, MT_MIB_TSCR2(band_idx));
2818 mib->tx_stop_q_empty_cnt += cnt;
2819
2820 cnt = mt76_rr(dev, MT_MIB_TSCR3(band_idx));
2821 mib->tx_mpdu_attempts_cnt += cnt;
2822
2823 cnt = mt76_rr(dev, MT_MIB_TSCR4(band_idx));
2824 mib->tx_mpdu_success_cnt += cnt;
2825
2826 cnt = mt76_rr(dev, MT_MIB_RSCR27(band_idx));
2827 mib->rx_ampdu_cnt += cnt;
2828
2829 cnt = mt76_rr(dev, MT_MIB_RSCR28(band_idx));
2830 mib->rx_ampdu_bytes_cnt += cnt;
2831
2832 cnt = mt76_rr(dev, MT_MIB_RSCR29(band_idx));
2833 mib->rx_ampdu_valid_subframe_cnt += cnt;
2834
2835 cnt = mt76_rr(dev, MT_MIB_RSCR30(band_idx));
2836 mib->rx_ampdu_valid_subframe_bytes_cnt += cnt;
2837
2838 cnt = mt76_rr(dev, MT_MIB_SDR27(band_idx));
2839 mib->tx_rwp_fail_cnt += FIELD_GET(MT_MIB_SDR27_TX_RWP_FAIL_CNT, cnt);
2840
2841 cnt = mt76_rr(dev, MT_MIB_SDR28(band_idx));
2842 mib->tx_rwp_need_cnt += FIELD_GET(MT_MIB_SDR28_TX_RWP_NEED_CNT, cnt);
2843
2844 cnt = mt76_rr(dev, MT_UMIB_RPDCR(band_idx));
2845 mib->rx_pfdrop_cnt += cnt;
2846
2847 cnt = mt76_rr(dev, MT_MIB_RVSR1(band_idx));
2848 mib->rx_vec_queue_overflow_drop_cnt += cnt;
2849
2850 cnt = mt76_rr(dev, MT_MIB_TSCR1(band_idx));
2851 mib->rx_ba_cnt += cnt;
2852
2853 cnt = mt76_rr(dev, MT_MIB_BSCR0(band_idx));
2854 mib->tx_bf_ebf_ppdu_cnt += cnt;
2855
2856 cnt = mt76_rr(dev, MT_MIB_BSCR1(band_idx));
2857 mib->tx_bf_ibf_ppdu_cnt += cnt;
2858
2859 cnt = mt76_rr(dev, MT_MIB_BSCR2(band_idx));
2860 mib->tx_mu_bf_cnt += cnt;
2861
2862 cnt = mt76_rr(dev, MT_MIB_TSCR5(band_idx));
2863 mib->tx_mu_mpdu_cnt += cnt;
2864
2865 cnt = mt76_rr(dev, MT_MIB_TSCR6(band_idx));
2866 mib->tx_mu_acked_mpdu_cnt += cnt;
2867
2868 cnt = mt76_rr(dev, MT_MIB_TSCR7(band_idx));
2869 mib->tx_su_acked_mpdu_cnt += cnt;
2870
2871 cnt = mt76_rr(dev, MT_MIB_BSCR3(band_idx));
2872 mib->tx_bf_rx_fb_ht_cnt += cnt;
2873 mib->tx_bf_rx_fb_all_cnt += cnt;
2874
2875 cnt = mt76_rr(dev, MT_MIB_BSCR4(band_idx));
2876 mib->tx_bf_rx_fb_vht_cnt += cnt;
2877 mib->tx_bf_rx_fb_all_cnt += cnt;
2878
2879 cnt = mt76_rr(dev, MT_MIB_BSCR5(band_idx));
2880 mib->tx_bf_rx_fb_he_cnt += cnt;
2881 mib->tx_bf_rx_fb_all_cnt += cnt;
2882
2883 cnt = mt76_rr(dev, MT_MIB_BSCR6(band_idx));
2884 mib->tx_bf_rx_fb_eht_cnt += cnt;
2885 mib->tx_bf_rx_fb_all_cnt += cnt;
2886
2887 cnt = mt76_rr(dev, MT_ETBF_RX_FB_CONT(band_idx));
2888 mib->tx_bf_rx_fb_bw = FIELD_GET(MT_ETBF_RX_FB_BW, cnt);
2889 mib->tx_bf_rx_fb_nc_cnt += FIELD_GET(MT_ETBF_RX_FB_NC, cnt);
2890 mib->tx_bf_rx_fb_nr_cnt += FIELD_GET(MT_ETBF_RX_FB_NR, cnt);
2891
2892 cnt = mt76_rr(dev, MT_MIB_BSCR7(band_idx));
2893 mib->tx_bf_fb_trig_cnt += cnt;
2894
2895 cnt = mt76_rr(dev, MT_MIB_BSCR17(band_idx));
2896 mib->tx_bf_fb_cpl_cnt += cnt;
2897
2898 for (i = 0; i < ARRAY_SIZE(mib->tx_amsdu); i++) {
2899 cnt = mt76_rr(dev, MT_PLE_AMSDU_PACK_MSDU_CNT(i));
2900 mib->tx_amsdu[i] += cnt;
2901 mib->tx_amsdu_cnt += cnt;
2902 }
2903
2904 /* rts count */
2905 cnt = mt76_rr(dev, MT_MIB_BTSCR5(band_idx));
2906 mib->rts_cnt += cnt;
2907
2908 /* rts retry count */
2909 cnt = mt76_rr(dev, MT_MIB_BTSCR6(band_idx));
2910 mib->rts_retries_cnt += cnt;
2911
2912 /* ba miss count */
2913 cnt = mt76_rr(dev, MT_MIB_BTSCR0(band_idx));
2914 mib->ba_miss_cnt += cnt;
2915
2916 /* ack fail count */
2917 cnt = mt76_rr(dev, MT_MIB_BFTFCR(band_idx));
2918 mib->ack_fail_cnt += cnt;
2919
2920 for (i = 0; i < 16; i++) {
2921 cnt = mt76_rr(dev, MT_TX_AGG_CNT(band_idx, i));
2922 phy->mt76->aggr_stats[i] += cnt;
2923 }
2924 }
2925
mt7996_mac_sta_rc_work(struct work_struct * work)2926 void mt7996_mac_sta_rc_work(struct work_struct *work)
2927 {
2928 struct mt7996_dev *dev = container_of(work, struct mt7996_dev, rc_work);
2929 struct mt7996_sta_link *msta_link;
2930 struct ieee80211_vif *vif;
2931 struct mt7996_vif *mvif;
2932 LIST_HEAD(list);
2933 u32 changed;
2934
2935 mutex_lock(&dev->mt76.mutex);
2936
2937 spin_lock_bh(&dev->mt76.sta_poll_lock);
2938 list_splice_init(&dev->sta_rc_list, &list);
2939
2940 while (!list_empty(&list)) {
2941 msta_link = list_first_entry(&list, struct mt7996_sta_link,
2942 rc_list);
2943 list_del_init(&msta_link->rc_list);
2944
2945 changed = msta_link->changed;
2946 msta_link->changed = 0;
2947 mvif = msta_link->sta->vif;
2948 vif = container_of((void *)mvif, struct ieee80211_vif,
2949 drv_priv);
2950
2951 spin_unlock_bh(&dev->mt76.sta_poll_lock);
2952
2953 if (changed & (IEEE80211_RC_SUPP_RATES_CHANGED |
2954 IEEE80211_RC_NSS_CHANGED |
2955 IEEE80211_RC_BW_CHANGED))
2956 mt7996_mcu_add_rate_ctrl(dev, msta_link->sta, vif,
2957 msta_link->wcid.link_id,
2958 true);
2959
2960 if (changed & IEEE80211_RC_SMPS_CHANGED)
2961 mt7996_mcu_set_fixed_field(dev, msta_link->sta, NULL,
2962 msta_link->wcid.link_id,
2963 RATE_PARAM_MMPS_UPDATE);
2964
2965 spin_lock_bh(&dev->mt76.sta_poll_lock);
2966 }
2967
2968 spin_unlock_bh(&dev->mt76.sta_poll_lock);
2969
2970 mutex_unlock(&dev->mt76.mutex);
2971 }
2972
mt7996_mac_work(struct work_struct * work)2973 void mt7996_mac_work(struct work_struct *work)
2974 {
2975 struct mt7996_phy *phy;
2976 struct mt76_phy *mphy;
2977
2978 mphy = (struct mt76_phy *)container_of(work, struct mt76_phy,
2979 mac_work.work);
2980 phy = mphy->priv;
2981
2982 mutex_lock(&mphy->dev->mutex);
2983
2984 mt76_update_survey(mphy);
2985 if (++mphy->mac_work_count == 5) {
2986 mphy->mac_work_count = 0;
2987
2988 mt7996_mac_update_stats(phy);
2989
2990 mt7996_mcu_get_all_sta_info(phy, UNI_ALL_STA_TXRX_RATE);
2991 if (mtk_wed_device_active(&phy->dev->mt76.mmio.wed)) {
2992 mt7996_mcu_get_all_sta_info(phy, UNI_ALL_STA_TXRX_ADM_STAT);
2993 mt7996_mcu_get_all_sta_info(phy, UNI_ALL_STA_TXRX_MSDU_COUNT);
2994 }
2995 }
2996
2997 mutex_unlock(&mphy->dev->mutex);
2998
2999 mt76_beacon_mon_check(mphy);
3000 mt76_tx_status_check(mphy->dev, false);
3001
3002 ieee80211_queue_delayed_work(mphy->hw, &mphy->mac_work,
3003 MT7996_WATCHDOG_TIME);
3004 }
3005
mt7996_dfs_stop_radar_detector(struct mt7996_phy * phy)3006 static void mt7996_dfs_stop_radar_detector(struct mt7996_phy *phy)
3007 {
3008 struct mt7996_dev *dev = phy->dev;
3009 int rdd_idx = mt7996_get_rdd_idx(phy, false);
3010
3011 if (rdd_idx < 0)
3012 return;
3013
3014 mt7996_mcu_rdd_cmd(dev, RDD_STOP, rdd_idx, 0);
3015 }
3016
mt7996_dfs_start_rdd(struct mt7996_dev * dev,int rdd_idx)3017 static int mt7996_dfs_start_rdd(struct mt7996_dev *dev, int rdd_idx)
3018 {
3019 int region;
3020
3021 switch (dev->mt76.region) {
3022 case NL80211_DFS_ETSI:
3023 region = 0;
3024 break;
3025 case NL80211_DFS_JP:
3026 region = 2;
3027 break;
3028 case NL80211_DFS_FCC:
3029 default:
3030 region = 1;
3031 break;
3032 }
3033
3034 return mt7996_mcu_rdd_cmd(dev, RDD_START, rdd_idx, region);
3035 }
3036
mt7996_dfs_start_radar_detector(struct mt7996_phy * phy)3037 static int mt7996_dfs_start_radar_detector(struct mt7996_phy *phy)
3038 {
3039 struct mt7996_dev *dev = phy->dev;
3040 int err, rdd_idx;
3041
3042 rdd_idx = mt7996_get_rdd_idx(phy, false);
3043 if (rdd_idx < 0)
3044 return -EINVAL;
3045
3046 /* start CAC */
3047 err = mt7996_mcu_rdd_cmd(dev, RDD_CAC_START, rdd_idx, 0);
3048 if (err < 0)
3049 return err;
3050
3051 err = mt7996_dfs_start_rdd(dev, rdd_idx);
3052
3053 return err;
3054 }
3055
mt7996_dfs_init_radar_detector(struct mt7996_phy * phy)3056 int mt7996_dfs_init_radar_detector(struct mt7996_phy *phy)
3057 {
3058 struct mt7996_dev *dev = phy->dev;
3059 enum mt76_dfs_state dfs_state, prev_state;
3060 int err, rdd_idx = mt7996_get_rdd_idx(phy, false);
3061
3062 prev_state = phy->mt76->dfs_state;
3063 dfs_state = mt76_phy_dfs_state(phy->mt76);
3064
3065 if (prev_state == dfs_state || rdd_idx < 0)
3066 return 0;
3067
3068 if (prev_state == MT_DFS_STATE_UNKNOWN)
3069 mt7996_dfs_stop_radar_detector(phy);
3070
3071 if (dfs_state == MT_DFS_STATE_DISABLED)
3072 goto stop;
3073
3074 if (prev_state <= MT_DFS_STATE_DISABLED) {
3075 err = mt7996_dfs_start_radar_detector(phy);
3076 if (err < 0)
3077 return err;
3078
3079 phy->mt76->dfs_state = MT_DFS_STATE_CAC;
3080 }
3081
3082 if (dfs_state == MT_DFS_STATE_CAC)
3083 return 0;
3084
3085 err = mt7996_mcu_rdd_cmd(dev, RDD_CAC_END, rdd_idx, 0);
3086 if (err < 0) {
3087 phy->mt76->dfs_state = MT_DFS_STATE_UNKNOWN;
3088 return err;
3089 }
3090
3091 phy->mt76->dfs_state = MT_DFS_STATE_ACTIVE;
3092 return 0;
3093
3094 stop:
3095 err = mt7996_mcu_rdd_cmd(dev, RDD_NORMAL_START, rdd_idx, 0);
3096 if (err < 0)
3097 return err;
3098
3099 mt7996_dfs_stop_radar_detector(phy);
3100 phy->mt76->dfs_state = MT_DFS_STATE_DISABLED;
3101
3102 return 0;
3103 }
3104
3105 static int
mt7996_mac_twt_duration_align(int duration)3106 mt7996_mac_twt_duration_align(int duration)
3107 {
3108 return duration << 8;
3109 }
3110
3111 static u64
mt7996_mac_twt_sched_list_add(struct mt7996_dev * dev,struct mt7996_twt_flow * flow)3112 mt7996_mac_twt_sched_list_add(struct mt7996_dev *dev,
3113 struct mt7996_twt_flow *flow)
3114 {
3115 struct mt7996_twt_flow *iter, *iter_next;
3116 u32 duration = flow->duration << 8;
3117 u64 start_tsf;
3118
3119 iter = list_first_entry_or_null(&dev->twt_list,
3120 struct mt7996_twt_flow, list);
3121 if (!iter || !iter->sched || iter->start_tsf > duration) {
3122 /* add flow as first entry in the list */
3123 list_add(&flow->list, &dev->twt_list);
3124 return 0;
3125 }
3126
3127 list_for_each_entry_safe(iter, iter_next, &dev->twt_list, list) {
3128 start_tsf = iter->start_tsf +
3129 mt7996_mac_twt_duration_align(iter->duration);
3130 if (list_is_last(&iter->list, &dev->twt_list))
3131 break;
3132
3133 if (!iter_next->sched ||
3134 iter_next->start_tsf > start_tsf + duration) {
3135 list_add(&flow->list, &iter->list);
3136 goto out;
3137 }
3138 }
3139
3140 /* add flow as last entry in the list */
3141 list_add_tail(&flow->list, &dev->twt_list);
3142 out:
3143 return start_tsf;
3144 }
3145
mt7996_mac_check_twt_req(struct ieee80211_twt_setup * twt)3146 static int mt7996_mac_check_twt_req(struct ieee80211_twt_setup *twt)
3147 {
3148 struct ieee80211_twt_params *twt_agrt;
3149 u64 interval, duration;
3150 u16 mantissa;
3151 u8 exp;
3152
3153 /* only individual agreement supported */
3154 if (twt->control & IEEE80211_TWT_CONTROL_NEG_TYPE_BROADCAST)
3155 return -EOPNOTSUPP;
3156
3157 /* only 256us unit supported */
3158 if (twt->control & IEEE80211_TWT_CONTROL_WAKE_DUR_UNIT)
3159 return -EOPNOTSUPP;
3160
3161 twt_agrt = (struct ieee80211_twt_params *)twt->params;
3162
3163 /* explicit agreement not supported */
3164 if (!(twt_agrt->req_type & cpu_to_le16(IEEE80211_TWT_REQTYPE_IMPLICIT)))
3165 return -EOPNOTSUPP;
3166
3167 exp = FIELD_GET(IEEE80211_TWT_REQTYPE_WAKE_INT_EXP,
3168 le16_to_cpu(twt_agrt->req_type));
3169 mantissa = le16_to_cpu(twt_agrt->mantissa);
3170 duration = twt_agrt->min_twt_dur << 8;
3171
3172 interval = (u64)mantissa << exp;
3173 if (interval < duration)
3174 return -EOPNOTSUPP;
3175
3176 return 0;
3177 }
3178
3179 static bool
mt7996_mac_twt_param_equal(struct mt7996_sta_link * msta_link,struct ieee80211_twt_params * twt_agrt)3180 mt7996_mac_twt_param_equal(struct mt7996_sta_link *msta_link,
3181 struct ieee80211_twt_params *twt_agrt)
3182 {
3183 u16 type = le16_to_cpu(twt_agrt->req_type);
3184 u8 exp;
3185 int i;
3186
3187 exp = FIELD_GET(IEEE80211_TWT_REQTYPE_WAKE_INT_EXP, type);
3188 for (i = 0; i < MT7996_MAX_STA_TWT_AGRT; i++) {
3189 struct mt7996_twt_flow *f;
3190
3191 if (!(msta_link->twt.flowid_mask & BIT(i)))
3192 continue;
3193
3194 f = &msta_link->twt.flow[i];
3195 if (f->duration == twt_agrt->min_twt_dur &&
3196 f->mantissa == twt_agrt->mantissa &&
3197 f->exp == exp &&
3198 f->protection == !!(type & IEEE80211_TWT_REQTYPE_PROTECTION) &&
3199 f->flowtype == !!(type & IEEE80211_TWT_REQTYPE_FLOWTYPE) &&
3200 f->trigger == !!(type & IEEE80211_TWT_REQTYPE_TRIGGER))
3201 return true;
3202 }
3203
3204 return false;
3205 }
3206
mt7996_mac_add_twt_setup(struct ieee80211_hw * hw,struct ieee80211_sta * sta,struct ieee80211_twt_setup * twt)3207 void mt7996_mac_add_twt_setup(struct ieee80211_hw *hw,
3208 struct ieee80211_sta *sta,
3209 struct ieee80211_twt_setup *twt)
3210 {
3211 enum ieee80211_twt_setup_cmd setup_cmd = TWT_SETUP_CMD_REJECT;
3212 struct mt7996_sta *msta = (struct mt7996_sta *)sta->drv_priv;
3213 struct ieee80211_twt_params *twt_agrt = (void *)twt->params;
3214 struct mt7996_sta_link *msta_link = &msta->deflink;
3215 u16 req_type = le16_to_cpu(twt_agrt->req_type);
3216 enum ieee80211_twt_setup_cmd sta_setup_cmd;
3217 struct mt7996_dev *dev = mt7996_hw_dev(hw);
3218 struct mt7996_twt_flow *flow;
3219 u8 flowid, table_id, exp;
3220
3221 /* the firmware does not support iTWT agreements with MLD peers, and
3222 * driver TWT state is only tracked on the default link
3223 */
3224 if (sta->mlo)
3225 goto out;
3226
3227 if (mt7996_mac_check_twt_req(twt))
3228 goto out;
3229
3230 mutex_lock(&dev->mt76.mutex);
3231
3232 if (dev->twt.n_agrt == MT7996_MAX_TWT_AGRT)
3233 goto unlock;
3234
3235 if (hweight8(msta_link->twt.flowid_mask) ==
3236 ARRAY_SIZE(msta_link->twt.flow))
3237 goto unlock;
3238
3239 if (twt_agrt->min_twt_dur < MT7996_MIN_TWT_DUR) {
3240 setup_cmd = TWT_SETUP_CMD_DICTATE;
3241 twt_agrt->min_twt_dur = MT7996_MIN_TWT_DUR;
3242 goto unlock;
3243 }
3244
3245 if (mt7996_mac_twt_param_equal(msta_link, twt_agrt))
3246 goto unlock;
3247
3248 flowid = ffs(~msta_link->twt.flowid_mask) - 1;
3249 twt_agrt->req_type &= ~cpu_to_le16(IEEE80211_TWT_REQTYPE_FLOWID);
3250 twt_agrt->req_type |= le16_encode_bits(flowid,
3251 IEEE80211_TWT_REQTYPE_FLOWID);
3252
3253 table_id = ffs(~dev->twt.table_mask) - 1;
3254 exp = FIELD_GET(IEEE80211_TWT_REQTYPE_WAKE_INT_EXP, req_type);
3255 sta_setup_cmd = FIELD_GET(IEEE80211_TWT_REQTYPE_SETUP_CMD, req_type);
3256
3257 flow = &msta_link->twt.flow[flowid];
3258 memset(flow, 0, sizeof(*flow));
3259 INIT_LIST_HEAD(&flow->list);
3260 flow->wcid = msta_link->wcid.idx;
3261 flow->table_id = table_id;
3262 flow->id = flowid;
3263 flow->duration = twt_agrt->min_twt_dur;
3264 flow->mantissa = twt_agrt->mantissa;
3265 flow->exp = exp;
3266 flow->protection = !!(req_type & IEEE80211_TWT_REQTYPE_PROTECTION);
3267 flow->flowtype = !!(req_type & IEEE80211_TWT_REQTYPE_FLOWTYPE);
3268 flow->trigger = !!(req_type & IEEE80211_TWT_REQTYPE_TRIGGER);
3269
3270 if (sta_setup_cmd == TWT_SETUP_CMD_REQUEST ||
3271 sta_setup_cmd == TWT_SETUP_CMD_SUGGEST) {
3272 u64 interval = (u64)le16_to_cpu(twt_agrt->mantissa) << exp;
3273 u64 flow_tsf, curr_tsf;
3274 u32 rem;
3275
3276 flow->sched = true;
3277 flow->start_tsf = mt7996_mac_twt_sched_list_add(dev, flow);
3278 curr_tsf = __mt7996_get_tsf(hw, &msta->vif->deflink);
3279 div_u64_rem(curr_tsf - flow->start_tsf, interval, &rem);
3280 flow_tsf = curr_tsf + interval - rem;
3281 twt_agrt->twt = cpu_to_le64(flow_tsf);
3282 } else {
3283 list_add_tail(&flow->list, &dev->twt_list);
3284 }
3285 flow->tsf = le64_to_cpu(twt_agrt->twt);
3286
3287 if (mt7996_mcu_twt_agrt_update(dev, &msta->vif->deflink, flow,
3288 MCU_TWT_AGRT_ADD))
3289 goto unlock;
3290
3291 setup_cmd = TWT_SETUP_CMD_ACCEPT;
3292 dev->twt.table_mask |= BIT(table_id);
3293 msta_link->twt.flowid_mask |= BIT(flowid);
3294 dev->twt.n_agrt++;
3295
3296 unlock:
3297 mutex_unlock(&dev->mt76.mutex);
3298 out:
3299 twt_agrt->req_type &= ~cpu_to_le16(IEEE80211_TWT_REQTYPE_SETUP_CMD);
3300 twt_agrt->req_type |=
3301 le16_encode_bits(setup_cmd, IEEE80211_TWT_REQTYPE_SETUP_CMD);
3302 twt->control = twt->control & IEEE80211_TWT_CONTROL_RX_DISABLED;
3303 }
3304
mt7996_mac_twt_teardown_flow(struct mt7996_dev * dev,struct mt7996_vif_link * link,struct mt7996_sta_link * msta_link,u8 flowid)3305 void mt7996_mac_twt_teardown_flow(struct mt7996_dev *dev,
3306 struct mt7996_vif_link *link,
3307 struct mt7996_sta_link *msta_link,
3308 u8 flowid)
3309 {
3310 struct mt7996_twt_flow *flow;
3311
3312 lockdep_assert_held(&dev->mt76.mutex);
3313
3314 if (flowid >= ARRAY_SIZE(msta_link->twt.flow))
3315 return;
3316
3317 if (!(msta_link->twt.flowid_mask & BIT(flowid)))
3318 return;
3319
3320 flow = &msta_link->twt.flow[flowid];
3321 if (mt7996_mcu_twt_agrt_update(dev, link, flow, MCU_TWT_AGRT_DELETE))
3322 return;
3323
3324 list_del_init(&flow->list);
3325 msta_link->twt.flowid_mask &= ~BIT(flowid);
3326 dev->twt.table_mask &= ~BIT(flow->table_id);
3327 dev->twt.n_agrt--;
3328 }
3329