xref: /freebsd/sys/contrib/dev/mediatek/mt76/mt7915/mac.c (revision e6bfd18d21b225af6a0ed67ceeaf1293b7b9eba5)
1 // SPDX-License-Identifier: ISC
2 /* Copyright (C) 2020 MediaTek Inc. */
3 
4 #include <linux/etherdevice.h>
5 #include <linux/timekeeping.h>
6 #if defined(__FreeBSD__)
7 #include <linux/delay.h>
8 #include <linux/math64.h>
9 #endif
10 #include "mt7915.h"
11 #include "../dma.h"
12 #include "mac.h"
13 #include "mcu.h"
14 
15 #define to_rssi(field, rxv)	((FIELD_GET(field, rxv) - 220) / 2)
16 
17 static const struct mt7915_dfs_radar_spec etsi_radar_specs = {
18 	.pulse_th = { 110, -10, -80, 40, 5200, 128, 5200 },
19 	.radar_pattern = {
20 		[5] =  { 1, 0,  6, 32, 28, 0,  990, 5010, 17, 1, 1 },
21 		[6] =  { 1, 0,  9, 32, 28, 0,  615, 5010, 27, 1, 1 },
22 		[7] =  { 1, 0, 15, 32, 28, 0,  240,  445, 27, 1, 1 },
23 		[8] =  { 1, 0, 12, 32, 28, 0,  240,  510, 42, 1, 1 },
24 		[9] =  { 1, 1,  0,  0,  0, 0, 2490, 3343, 14, 0, 0, 12, 32, 28, { }, 126 },
25 		[10] = { 1, 1,  0,  0,  0, 0, 2490, 3343, 14, 0, 0, 15, 32, 24, { }, 126 },
26 		[11] = { 1, 1,  0,  0,  0, 0,  823, 2510, 14, 0, 0, 18, 32, 28, { },  54 },
27 		[12] = { 1, 1,  0,  0,  0, 0,  823, 2510, 14, 0, 0, 27, 32, 24, { },  54 },
28 	},
29 };
30 
31 static const struct mt7915_dfs_radar_spec fcc_radar_specs = {
32 	.pulse_th = { 110, -10, -80, 40, 5200, 128, 5200 },
33 	.radar_pattern = {
34 		[0] = { 1, 0,  8,  32, 28, 0, 508, 3076, 13, 1,  1 },
35 		[1] = { 1, 0, 12,  32, 28, 0, 140,  240, 17, 1,  1 },
36 		[2] = { 1, 0,  8,  32, 28, 0, 190,  510, 22, 1,  1 },
37 		[3] = { 1, 0,  6,  32, 28, 0, 190,  510, 32, 1,  1 },
38 		[4] = { 1, 0,  9, 255, 28, 0, 323,  343, 13, 1, 32 },
39 	},
40 };
41 
42 static const struct mt7915_dfs_radar_spec jp_radar_specs = {
43 	.pulse_th = { 110, -10, -80, 40, 5200, 128, 5200 },
44 	.radar_pattern = {
45 		[0] =  { 1, 0,  8,  32, 28, 0,  508, 3076,  13, 1,  1 },
46 		[1] =  { 1, 0, 12,  32, 28, 0,  140,  240,  17, 1,  1 },
47 		[2] =  { 1, 0,  8,  32, 28, 0,  190,  510,  22, 1,  1 },
48 		[3] =  { 1, 0,  6,  32, 28, 0,  190,  510,  32, 1,  1 },
49 		[4] =  { 1, 0,  9, 255, 28, 0,  323,  343,  13, 1, 32 },
50 		[13] = { 1, 0,  7,  32, 28, 0, 3836, 3856,  14, 1,  1 },
51 		[14] = { 1, 0,  6,  32, 28, 0,  615, 5010, 110, 1,  1 },
52 		[15] = { 1, 1,  0,   0,  0, 0,   15, 5010, 110, 0,  0, 12, 32, 28 },
53 	},
54 };
55 
56 static struct mt76_wcid *mt7915_rx_get_wcid(struct mt7915_dev *dev,
57 					    u16 idx, bool unicast)
58 {
59 	struct mt7915_sta *sta;
60 	struct mt76_wcid *wcid;
61 
62 	if (idx >= ARRAY_SIZE(dev->mt76.wcid))
63 		return NULL;
64 
65 	wcid = rcu_dereference(dev->mt76.wcid[idx]);
66 	if (unicast || !wcid)
67 		return wcid;
68 
69 	if (!wcid->sta)
70 		return NULL;
71 
72 	sta = container_of(wcid, struct mt7915_sta, wcid);
73 	if (!sta->vif)
74 		return NULL;
75 
76 	return &sta->vif->sta.wcid;
77 }
78 
79 void mt7915_sta_ps(struct mt76_dev *mdev, struct ieee80211_sta *sta, bool ps)
80 {
81 }
82 
83 bool mt7915_mac_wtbl_update(struct mt7915_dev *dev, int idx, u32 mask)
84 {
85 	mt76_rmw(dev, MT_WTBL_UPDATE, MT_WTBL_UPDATE_WLAN_IDX,
86 		 FIELD_PREP(MT_WTBL_UPDATE_WLAN_IDX, idx) | mask);
87 
88 	return mt76_poll(dev, MT_WTBL_UPDATE, MT_WTBL_UPDATE_BUSY,
89 			 0, 5000);
90 }
91 
92 u32 mt7915_mac_wtbl_lmac_addr(struct mt7915_dev *dev, u16 wcid, u8 dw)
93 {
94 	mt76_wr(dev, MT_WTBLON_TOP_WDUCR,
95 		FIELD_PREP(MT_WTBLON_TOP_WDUCR_GROUP, (wcid >> 7)));
96 
97 	return MT_WTBL_LMAC_OFFS(wcid, dw);
98 }
99 
100 static void mt7915_mac_sta_poll(struct mt7915_dev *dev)
101 {
102 	static const u8 ac_to_tid[] = {
103 		[IEEE80211_AC_BE] = 0,
104 		[IEEE80211_AC_BK] = 1,
105 		[IEEE80211_AC_VI] = 4,
106 		[IEEE80211_AC_VO] = 6
107 	};
108 	struct ieee80211_sta *sta;
109 	struct mt7915_sta *msta;
110 	struct rate_info *rate;
111 	u32 tx_time[IEEE80211_NUM_ACS], rx_time[IEEE80211_NUM_ACS];
112 #if defined(__linux__)
113 	LIST_HEAD(sta_poll_list);
114 #elif defined(__FreeBSD__)
115 	LINUX_LIST_HEAD(sta_poll_list);
116 #endif
117 	int i;
118 
119 	spin_lock_bh(&dev->sta_poll_lock);
120 	list_splice_init(&dev->sta_poll_list, &sta_poll_list);
121 	spin_unlock_bh(&dev->sta_poll_lock);
122 
123 	rcu_read_lock();
124 
125 	while (true) {
126 		bool clear = false;
127 		u32 addr, val;
128 		u16 idx;
129 		u8 bw;
130 
131 		spin_lock_bh(&dev->sta_poll_lock);
132 		if (list_empty(&sta_poll_list)) {
133 			spin_unlock_bh(&dev->sta_poll_lock);
134 			break;
135 		}
136 		msta = list_first_entry(&sta_poll_list,
137 					struct mt7915_sta, poll_list);
138 		list_del_init(&msta->poll_list);
139 		spin_unlock_bh(&dev->sta_poll_lock);
140 
141 		idx = msta->wcid.idx;
142 		addr = mt7915_mac_wtbl_lmac_addr(dev, idx, 20);
143 
144 		for (i = 0; i < IEEE80211_NUM_ACS; i++) {
145 			u32 tx_last = msta->airtime_ac[i];
146 			u32 rx_last = msta->airtime_ac[i + 4];
147 
148 			msta->airtime_ac[i] = mt76_rr(dev, addr);
149 			msta->airtime_ac[i + 4] = mt76_rr(dev, addr + 4);
150 
151 			tx_time[i] = msta->airtime_ac[i] - tx_last;
152 			rx_time[i] = msta->airtime_ac[i + 4] - rx_last;
153 
154 			if ((tx_last | rx_last) & BIT(30))
155 				clear = true;
156 
157 			addr += 8;
158 		}
159 
160 		if (clear) {
161 			mt7915_mac_wtbl_update(dev, idx,
162 					       MT_WTBL_UPDATE_ADM_COUNT_CLEAR);
163 			memset(msta->airtime_ac, 0, sizeof(msta->airtime_ac));
164 		}
165 
166 		if (!msta->wcid.sta)
167 			continue;
168 
169 		sta = container_of((void *)msta, struct ieee80211_sta,
170 				   drv_priv);
171 		for (i = 0; i < IEEE80211_NUM_ACS; i++) {
172 			u8 q = mt76_connac_lmac_mapping(i);
173 			u32 tx_cur = tx_time[q];
174 			u32 rx_cur = rx_time[q];
175 			u8 tid = ac_to_tid[i];
176 
177 			if (!tx_cur && !rx_cur)
178 				continue;
179 
180 			ieee80211_sta_register_airtime(sta, tid, tx_cur,
181 						       rx_cur);
182 		}
183 
184 		/*
185 		 * We don't support reading GI info from txs packets.
186 		 * For accurate tx status reporting and AQL improvement,
187 		 * we need to make sure that flags match so polling GI
188 		 * from per-sta counters directly.
189 		 */
190 		rate = &msta->wcid.rate;
191 		addr = mt7915_mac_wtbl_lmac_addr(dev, idx, 7);
192 		val = mt76_rr(dev, addr);
193 
194 		switch (rate->bw) {
195 		case RATE_INFO_BW_160:
196 			bw = IEEE80211_STA_RX_BW_160;
197 			break;
198 		case RATE_INFO_BW_80:
199 			bw = IEEE80211_STA_RX_BW_80;
200 			break;
201 		case RATE_INFO_BW_40:
202 			bw = IEEE80211_STA_RX_BW_40;
203 			break;
204 		default:
205 			bw = IEEE80211_STA_RX_BW_20;
206 			break;
207 		}
208 
209 		if (rate->flags & RATE_INFO_FLAGS_HE_MCS) {
210 			u8 offs = 24 + 2 * bw;
211 
212 			rate->he_gi = (val & (0x3 << offs)) >> offs;
213 		} else if (rate->flags &
214 			   (RATE_INFO_FLAGS_VHT_MCS | RATE_INFO_FLAGS_MCS)) {
215 			if (val & BIT(12 + bw))
216 				rate->flags |= RATE_INFO_FLAGS_SHORT_GI;
217 			else
218 				rate->flags &= ~RATE_INFO_FLAGS_SHORT_GI;
219 		}
220 	}
221 
222 	rcu_read_unlock();
223 }
224 
225 static int
226 mt7915_mac_fill_rx(struct mt7915_dev *dev, struct sk_buff *skb)
227 {
228 	struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb;
229 	struct mt76_phy *mphy = &dev->mt76.phy;
230 	struct mt7915_phy *phy = &dev->phy;
231 	struct ieee80211_supported_band *sband;
232 	__le32 *rxd = (__le32 *)skb->data;
233 	__le32 *rxv = NULL;
234 	u32 rxd0 = le32_to_cpu(rxd[0]);
235 	u32 rxd1 = le32_to_cpu(rxd[1]);
236 	u32 rxd2 = le32_to_cpu(rxd[2]);
237 	u32 rxd3 = le32_to_cpu(rxd[3]);
238 	u32 rxd4 = le32_to_cpu(rxd[4]);
239 	u32 csum_mask = MT_RXD0_NORMAL_IP_SUM | MT_RXD0_NORMAL_UDP_TCP_SUM;
240 	bool unicast, insert_ccmp_hdr = false;
241 	u8 remove_pad, amsdu_info;
242 	u8 mode = 0, qos_ctl = 0;
243 	struct mt7915_sta *msta = NULL;
244 	u32 csum_status = *(u32 *)skb->cb;
245 	bool hdr_trans;
246 	u16 hdr_gap;
247 	u16 seq_ctrl = 0;
248 	__le16 fc = 0;
249 	int idx;
250 
251 	memset(status, 0, sizeof(*status));
252 
253 	if ((rxd1 & MT_RXD1_NORMAL_BAND_IDX) && !phy->band_idx) {
254 		mphy = dev->mt76.phys[MT_BAND1];
255 		if (!mphy)
256 			return -EINVAL;
257 
258 		phy = mphy->priv;
259 		status->phy_idx = 1;
260 	}
261 
262 	if (!test_bit(MT76_STATE_RUNNING, &mphy->state))
263 		return -EINVAL;
264 
265 	if (rxd2 & MT_RXD2_NORMAL_AMSDU_ERR)
266 		return -EINVAL;
267 
268 	hdr_trans = rxd2 & MT_RXD2_NORMAL_HDR_TRANS;
269 	if (hdr_trans && (rxd1 & MT_RXD1_NORMAL_CM))
270 		return -EINVAL;
271 
272 	/* ICV error or CCMP/BIP/WPI MIC error */
273 	if (rxd1 & MT_RXD1_NORMAL_ICV_ERR)
274 		status->flag |= RX_FLAG_ONLY_MONITOR;
275 
276 	unicast = FIELD_GET(MT_RXD3_NORMAL_ADDR_TYPE, rxd3) == MT_RXD3_NORMAL_U2M;
277 	idx = FIELD_GET(MT_RXD1_NORMAL_WLAN_IDX, rxd1);
278 	status->wcid = mt7915_rx_get_wcid(dev, idx, unicast);
279 
280 	if (status->wcid) {
281 		msta = container_of(status->wcid, struct mt7915_sta, wcid);
282 		spin_lock_bh(&dev->sta_poll_lock);
283 		if (list_empty(&msta->poll_list))
284 			list_add_tail(&msta->poll_list, &dev->sta_poll_list);
285 		spin_unlock_bh(&dev->sta_poll_lock);
286 	}
287 
288 	status->freq = mphy->chandef.chan->center_freq;
289 	status->band = mphy->chandef.chan->band;
290 	if (status->band == NL80211_BAND_5GHZ)
291 		sband = &mphy->sband_5g.sband;
292 	else if (status->band == NL80211_BAND_6GHZ)
293 		sband = &mphy->sband_6g.sband;
294 	else
295 		sband = &mphy->sband_2g.sband;
296 
297 	if (!sband->channels)
298 		return -EINVAL;
299 
300 	if ((rxd0 & csum_mask) == csum_mask &&
301 	    !(csum_status & (BIT(0) | BIT(2) | BIT(3))))
302 		skb->ip_summed = CHECKSUM_UNNECESSARY;
303 
304 	if (rxd1 & MT_RXD1_NORMAL_FCS_ERR)
305 		status->flag |= RX_FLAG_FAILED_FCS_CRC;
306 
307 	if (rxd1 & MT_RXD1_NORMAL_TKIP_MIC_ERR)
308 		status->flag |= RX_FLAG_MMIC_ERROR;
309 
310 	if (FIELD_GET(MT_RXD1_NORMAL_SEC_MODE, rxd1) != 0 &&
311 	    !(rxd1 & (MT_RXD1_NORMAL_CLM | MT_RXD1_NORMAL_CM))) {
312 		status->flag |= RX_FLAG_DECRYPTED;
313 		status->flag |= RX_FLAG_IV_STRIPPED;
314 		status->flag |= RX_FLAG_MMIC_STRIPPED | RX_FLAG_MIC_STRIPPED;
315 	}
316 
317 	remove_pad = FIELD_GET(MT_RXD2_NORMAL_HDR_OFFSET, rxd2);
318 
319 	if (rxd2 & MT_RXD2_NORMAL_MAX_LEN_ERROR)
320 		return -EINVAL;
321 
322 	rxd += 6;
323 	if (rxd1 & MT_RXD1_NORMAL_GROUP_4) {
324 		u32 v0 = le32_to_cpu(rxd[0]);
325 		u32 v2 = le32_to_cpu(rxd[2]);
326 
327 		fc = cpu_to_le16(FIELD_GET(MT_RXD6_FRAME_CONTROL, v0));
328 		qos_ctl = FIELD_GET(MT_RXD8_QOS_CTL, v2);
329 		seq_ctrl = FIELD_GET(MT_RXD8_SEQ_CTRL, v2);
330 
331 		rxd += 4;
332 		if ((u8 *)rxd - skb->data >= skb->len)
333 			return -EINVAL;
334 	}
335 
336 	if (rxd1 & MT_RXD1_NORMAL_GROUP_1) {
337 		u8 *data = (u8 *)rxd;
338 
339 		if (status->flag & RX_FLAG_DECRYPTED) {
340 			switch (FIELD_GET(MT_RXD1_NORMAL_SEC_MODE, rxd1)) {
341 			case MT_CIPHER_AES_CCMP:
342 			case MT_CIPHER_CCMP_CCX:
343 			case MT_CIPHER_CCMP_256:
344 				insert_ccmp_hdr =
345 					FIELD_GET(MT_RXD2_NORMAL_FRAG, rxd2);
346 				fallthrough;
347 			case MT_CIPHER_TKIP:
348 			case MT_CIPHER_TKIP_NO_MIC:
349 			case MT_CIPHER_GCMP:
350 			case MT_CIPHER_GCMP_256:
351 				status->iv[0] = data[5];
352 				status->iv[1] = data[4];
353 				status->iv[2] = data[3];
354 				status->iv[3] = data[2];
355 				status->iv[4] = data[1];
356 				status->iv[5] = data[0];
357 				break;
358 			default:
359 				break;
360 			}
361 		}
362 		rxd += 4;
363 		if ((u8 *)rxd - skb->data >= skb->len)
364 			return -EINVAL;
365 	}
366 
367 	if (rxd1 & MT_RXD1_NORMAL_GROUP_2) {
368 		status->timestamp = le32_to_cpu(rxd[0]);
369 		status->flag |= RX_FLAG_MACTIME_START;
370 
371 		if (!(rxd2 & MT_RXD2_NORMAL_NON_AMPDU)) {
372 			status->flag |= RX_FLAG_AMPDU_DETAILS;
373 
374 			/* all subframes of an A-MPDU have the same timestamp */
375 			if (phy->rx_ampdu_ts != status->timestamp) {
376 				if (!++phy->ampdu_ref)
377 					phy->ampdu_ref++;
378 			}
379 			phy->rx_ampdu_ts = status->timestamp;
380 
381 			status->ampdu_ref = phy->ampdu_ref;
382 		}
383 
384 		rxd += 2;
385 		if ((u8 *)rxd - skb->data >= skb->len)
386 			return -EINVAL;
387 	}
388 
389 	/* RXD Group 3 - P-RXV */
390 	if (rxd1 & MT_RXD1_NORMAL_GROUP_3) {
391 		u32 v0, v1;
392 		int ret;
393 
394 		rxv = rxd;
395 		rxd += 2;
396 		if ((u8 *)rxd - skb->data >= skb->len)
397 			return -EINVAL;
398 
399 		v0 = le32_to_cpu(rxv[0]);
400 		v1 = le32_to_cpu(rxv[1]);
401 
402 		if (v0 & MT_PRXV_HT_AD_CODE)
403 			status->enc_flags |= RX_ENC_FLAG_LDPC;
404 
405 		status->chains = mphy->antenna_mask;
406 		status->chain_signal[0] = to_rssi(MT_PRXV_RCPI0, v1);
407 		status->chain_signal[1] = to_rssi(MT_PRXV_RCPI1, v1);
408 		status->chain_signal[2] = to_rssi(MT_PRXV_RCPI2, v1);
409 		status->chain_signal[3] = to_rssi(MT_PRXV_RCPI3, v1);
410 
411 		/* RXD Group 5 - C-RXV */
412 		if (rxd1 & MT_RXD1_NORMAL_GROUP_5) {
413 			rxd += 18;
414 			if ((u8 *)rxd - skb->data >= skb->len)
415 				return -EINVAL;
416 		}
417 
418 		if (!is_mt7915(&dev->mt76) || (rxd1 & MT_RXD1_NORMAL_GROUP_5)) {
419 			ret = mt76_connac2_mac_fill_rx_rate(&dev->mt76, status,
420 							    sband, rxv, &mode);
421 			if (ret < 0)
422 				return ret;
423 		}
424 	}
425 
426 	amsdu_info = FIELD_GET(MT_RXD4_NORMAL_PAYLOAD_FORMAT, rxd4);
427 	status->amsdu = !!amsdu_info;
428 	if (status->amsdu) {
429 		status->first_amsdu = amsdu_info == MT_RXD4_FIRST_AMSDU_FRAME;
430 		status->last_amsdu = amsdu_info == MT_RXD4_LAST_AMSDU_FRAME;
431 	}
432 
433 	hdr_gap = (u8 *)rxd - skb->data + 2 * remove_pad;
434 	if (hdr_trans && ieee80211_has_morefrags(fc)) {
435 		struct ieee80211_vif *vif;
436 		int err;
437 
438 		if (!msta || !msta->vif)
439 			return -EINVAL;
440 
441 		vif = container_of((void *)msta->vif, struct ieee80211_vif,
442 				   drv_priv);
443 		err = mt76_connac2_reverse_frag0_hdr_trans(vif, skb, hdr_gap);
444 		if (err)
445 			return err;
446 
447 		hdr_trans = false;
448 	} else {
449 		int pad_start = 0;
450 
451 		skb_pull(skb, hdr_gap);
452 		if (!hdr_trans && status->amsdu) {
453 			pad_start = ieee80211_get_hdrlen_from_skb(skb);
454 		} else if (hdr_trans && (rxd2 & MT_RXD2_NORMAL_HDR_TRANS_ERROR)) {
455 			/*
456 			 * When header translation failure is indicated,
457 			 * the hardware will insert an extra 2-byte field
458 			 * containing the data length after the protocol
459 			 * type field. This happens either when the LLC-SNAP
460 			 * pattern did not match, or if a VLAN header was
461 			 * detected.
462 			 */
463 			pad_start = 12;
464 			if (get_unaligned_be16(skb->data + pad_start) == ETH_P_8021Q)
465 				pad_start += 4;
466 			else
467 				pad_start = 0;
468 		}
469 
470 		if (pad_start) {
471 			memmove(skb->data + 2, skb->data, pad_start);
472 			skb_pull(skb, 2);
473 		}
474 	}
475 
476 	if (!hdr_trans) {
477 		struct ieee80211_hdr *hdr;
478 
479 		if (insert_ccmp_hdr) {
480 			u8 key_id = FIELD_GET(MT_RXD1_NORMAL_KEY_ID, rxd1);
481 
482 			mt76_insert_ccmp_hdr(skb, key_id);
483 		}
484 
485 		hdr = mt76_skb_get_hdr(skb);
486 		fc = hdr->frame_control;
487 		if (ieee80211_is_data_qos(fc)) {
488 			seq_ctrl = le16_to_cpu(hdr->seq_ctrl);
489 			qos_ctl = *ieee80211_get_qos_ctl(hdr);
490 		}
491 	} else {
492 		status->flag |= RX_FLAG_8023;
493 	}
494 
495 	if (rxv && mode >= MT_PHY_TYPE_HE_SU && !(status->flag & RX_FLAG_8023))
496 		mt76_connac2_mac_decode_he_radiotap(&dev->mt76, skb, rxv, mode);
497 
498 	if (!status->wcid || !ieee80211_is_data_qos(fc))
499 		return 0;
500 
501 	status->aggr = unicast &&
502 		       !ieee80211_is_qos_nullfunc(fc);
503 	status->qos_ctl = qos_ctl;
504 	status->seqno = IEEE80211_SEQ_TO_SN(seq_ctrl);
505 
506 	return 0;
507 }
508 
509 static void
510 mt7915_mac_fill_rx_vector(struct mt7915_dev *dev, struct sk_buff *skb)
511 {
512 #ifdef CONFIG_NL80211_TESTMODE
513 	struct mt7915_phy *phy = &dev->phy;
514 	__le32 *rxd = (__le32 *)skb->data;
515 	__le32 *rxv_hdr = rxd + 2;
516 	__le32 *rxv = rxd + 4;
517 	u32 rcpi, ib_rssi, wb_rssi, v20, v21;
518 	u8 band_idx;
519 	s32 foe;
520 	u8 snr;
521 	int i;
522 
523 	band_idx = le32_get_bits(rxv_hdr[1], MT_RXV_HDR_BAND_IDX);
524 	if (band_idx && !phy->band_idx) {
525 		phy = mt7915_ext_phy(dev);
526 		if (!phy)
527 			goto out;
528 	}
529 
530 	rcpi = le32_to_cpu(rxv[6]);
531 	ib_rssi = le32_to_cpu(rxv[7]);
532 	wb_rssi = le32_to_cpu(rxv[8]) >> 5;
533 
534 	for (i = 0; i < 4; i++, rcpi >>= 8, ib_rssi >>= 8, wb_rssi >>= 9) {
535 		if (i == 3)
536 			wb_rssi = le32_to_cpu(rxv[9]);
537 
538 		phy->test.last_rcpi[i] = rcpi & 0xff;
539 		phy->test.last_ib_rssi[i] = ib_rssi & 0xff;
540 		phy->test.last_wb_rssi[i] = wb_rssi & 0xff;
541 	}
542 
543 	v20 = le32_to_cpu(rxv[20]);
544 	v21 = le32_to_cpu(rxv[21]);
545 
546 	foe = FIELD_GET(MT_CRXV_FOE_LO, v20) |
547 	      (FIELD_GET(MT_CRXV_FOE_HI, v21) << MT_CRXV_FOE_SHIFT);
548 
549 	snr = FIELD_GET(MT_CRXV_SNR, v20) - 16;
550 
551 	phy->test.last_freq_offset = foe;
552 	phy->test.last_snr = snr;
553 out:
554 #endif
555 	dev_kfree_skb(skb);
556 }
557 
558 static void
559 mt7915_mac_write_txwi_tm(struct mt7915_phy *phy, __le32 *txwi,
560 			 struct sk_buff *skb)
561 {
562 #ifdef CONFIG_NL80211_TESTMODE
563 	struct mt76_testmode_data *td = &phy->mt76->test;
564 	const struct ieee80211_rate *r;
565 	u8 bw, mode, nss = td->tx_rate_nss;
566 	u8 rate_idx = td->tx_rate_idx;
567 	u16 rateval = 0;
568 	u32 val;
569 	bool cck = false;
570 	int band;
571 
572 	if (skb != phy->mt76->test.tx_skb)
573 		return;
574 
575 	switch (td->tx_rate_mode) {
576 	case MT76_TM_TX_MODE_HT:
577 		nss = 1 + (rate_idx >> 3);
578 		mode = MT_PHY_TYPE_HT;
579 		break;
580 	case MT76_TM_TX_MODE_VHT:
581 		mode = MT_PHY_TYPE_VHT;
582 		break;
583 	case MT76_TM_TX_MODE_HE_SU:
584 		mode = MT_PHY_TYPE_HE_SU;
585 		break;
586 	case MT76_TM_TX_MODE_HE_EXT_SU:
587 		mode = MT_PHY_TYPE_HE_EXT_SU;
588 		break;
589 	case MT76_TM_TX_MODE_HE_TB:
590 		mode = MT_PHY_TYPE_HE_TB;
591 		break;
592 	case MT76_TM_TX_MODE_HE_MU:
593 		mode = MT_PHY_TYPE_HE_MU;
594 		break;
595 	case MT76_TM_TX_MODE_CCK:
596 		cck = true;
597 		fallthrough;
598 	case MT76_TM_TX_MODE_OFDM:
599 		band = phy->mt76->chandef.chan->band;
600 		if (band == NL80211_BAND_2GHZ && !cck)
601 			rate_idx += 4;
602 
603 		r = &phy->mt76->hw->wiphy->bands[band]->bitrates[rate_idx];
604 		val = cck ? r->hw_value_short : r->hw_value;
605 
606 		mode = val >> 8;
607 		rate_idx = val & 0xff;
608 		break;
609 	default:
610 		mode = MT_PHY_TYPE_OFDM;
611 		break;
612 	}
613 
614 	switch (phy->mt76->chandef.width) {
615 	case NL80211_CHAN_WIDTH_40:
616 		bw = 1;
617 		break;
618 	case NL80211_CHAN_WIDTH_80:
619 		bw = 2;
620 		break;
621 	case NL80211_CHAN_WIDTH_80P80:
622 	case NL80211_CHAN_WIDTH_160:
623 		bw = 3;
624 		break;
625 	default:
626 		bw = 0;
627 		break;
628 	}
629 
630 	if (td->tx_rate_stbc && nss == 1) {
631 		nss++;
632 		rateval |= MT_TX_RATE_STBC;
633 	}
634 
635 	rateval |= FIELD_PREP(MT_TX_RATE_IDX, rate_idx) |
636 		   FIELD_PREP(MT_TX_RATE_MODE, mode) |
637 		   FIELD_PREP(MT_TX_RATE_NSS, nss - 1);
638 
639 	txwi[2] |= cpu_to_le32(MT_TXD2_FIX_RATE);
640 
641 	le32p_replace_bits(&txwi[3], 1, MT_TXD3_REM_TX_COUNT);
642 	if (td->tx_rate_mode < MT76_TM_TX_MODE_HT)
643 		txwi[3] |= cpu_to_le32(MT_TXD3_BA_DISABLE);
644 
645 	val = MT_TXD6_FIXED_BW |
646 	      FIELD_PREP(MT_TXD6_BW, bw) |
647 	      FIELD_PREP(MT_TXD6_TX_RATE, rateval) |
648 	      FIELD_PREP(MT_TXD6_SGI, td->tx_rate_sgi);
649 
650 	/* for HE_SU/HE_EXT_SU PPDU
651 	 * - 1x, 2x, 4x LTF + 0.8us GI
652 	 * - 2x LTF + 1.6us GI, 4x LTF + 3.2us GI
653 	 * for HE_MU PPDU
654 	 * - 2x, 4x LTF + 0.8us GI
655 	 * - 2x LTF + 1.6us GI, 4x LTF + 3.2us GI
656 	 * for HE_TB PPDU
657 	 * - 1x, 2x LTF + 1.6us GI
658 	 * - 4x LTF + 3.2us GI
659 	 */
660 	if (mode >= MT_PHY_TYPE_HE_SU)
661 		val |= FIELD_PREP(MT_TXD6_HELTF, td->tx_ltf);
662 
663 	if (td->tx_rate_ldpc || (bw > 0 && mode >= MT_PHY_TYPE_HE_SU))
664 		val |= MT_TXD6_LDPC;
665 
666 	txwi[3] &= ~cpu_to_le32(MT_TXD3_SN_VALID);
667 	txwi[6] |= cpu_to_le32(val);
668 	txwi[7] |= cpu_to_le32(FIELD_PREP(MT_TXD7_SPE_IDX,
669 					  phy->test.spe_idx));
670 #endif
671 }
672 
673 void mt7915_mac_write_txwi(struct mt76_dev *dev, __le32 *txwi,
674 			   struct sk_buff *skb, struct mt76_wcid *wcid, int pid,
675 			   struct ieee80211_key_conf *key,
676 			   enum mt76_txq_id qid, u32 changed)
677 {
678 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
679 	u8 phy_idx = (info->hw_queue & MT_TX_HW_QUEUE_PHY) >> 2;
680 	struct mt76_phy *mphy = &dev->phy;
681 
682 	if (phy_idx && dev->phys[MT_BAND1])
683 		mphy = dev->phys[MT_BAND1];
684 
685 	mt76_connac2_mac_write_txwi(dev, txwi, skb, wcid, key, pid, qid, changed);
686 
687 	if (mt76_testmode_enabled(mphy))
688 		mt7915_mac_write_txwi_tm(mphy->priv, txwi, skb);
689 }
690 
691 int mt7915_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
692 			  enum mt76_txq_id qid, struct mt76_wcid *wcid,
693 			  struct ieee80211_sta *sta,
694 			  struct mt76_tx_info *tx_info)
695 {
696 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx_info->skb->data;
697 	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
698 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_info->skb);
699 	struct ieee80211_key_conf *key = info->control.hw_key;
700 	struct ieee80211_vif *vif = info->control.vif;
701 	struct mt76_connac_fw_txp *txp;
702 	struct mt76_txwi_cache *t;
703 	int id, i, nbuf = tx_info->nbuf - 1;
704 	u8 *txwi = (u8 *)txwi_ptr;
705 	int pid;
706 
707 	if (unlikely(tx_info->skb->len <= ETH_HLEN))
708 		return -EINVAL;
709 
710 	if (!wcid)
711 		wcid = &dev->mt76.global_wcid;
712 
713 	if (sta) {
714 		struct mt7915_sta *msta;
715 
716 		msta = (struct mt7915_sta *)sta->drv_priv;
717 
718 		if (time_after(jiffies, msta->jiffies + HZ / 4)) {
719 			info->flags |= IEEE80211_TX_CTL_REQ_TX_STATUS;
720 			msta->jiffies = jiffies;
721 		}
722 	}
723 
724 	t = (struct mt76_txwi_cache *)(txwi + mdev->drv->txwi_size);
725 	t->skb = tx_info->skb;
726 
727 	id = mt76_token_consume(mdev, &t);
728 	if (id < 0)
729 		return id;
730 
731 	pid = mt76_tx_status_skb_add(mdev, wcid, tx_info->skb);
732 	mt7915_mac_write_txwi(mdev, txwi_ptr, tx_info->skb, wcid, pid, key,
733 			      qid, 0);
734 
735 	txp = (struct mt76_connac_fw_txp *)(txwi + MT_TXD_SIZE);
736 	for (i = 0; i < nbuf; i++) {
737 		txp->buf[i] = cpu_to_le32(tx_info->buf[i + 1].addr);
738 		txp->len[i] = cpu_to_le16(tx_info->buf[i + 1].len);
739 	}
740 	txp->nbuf = nbuf;
741 
742 	txp->flags = cpu_to_le16(MT_CT_INFO_APPLY_TXD | MT_CT_INFO_FROM_HOST);
743 
744 	if (!key)
745 		txp->flags |= cpu_to_le16(MT_CT_INFO_NONE_CIPHER_FRAME);
746 
747 	if (!(info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP) &&
748 	    ieee80211_is_mgmt(hdr->frame_control))
749 		txp->flags |= cpu_to_le16(MT_CT_INFO_MGMT_FRAME);
750 
751 	if (vif) {
752 		struct mt7915_vif *mvif = (struct mt7915_vif *)vif->drv_priv;
753 
754 		txp->bss_idx = mvif->mt76.idx;
755 	}
756 
757 	txp->token = cpu_to_le16(id);
758 	if (test_bit(MT_WCID_FLAG_4ADDR, &wcid->flags))
759 		txp->rept_wds_wcid = cpu_to_le16(wcid->idx);
760 	else
761 		txp->rept_wds_wcid = cpu_to_le16(0x3ff);
762 	tx_info->skb = DMA_DUMMY_DATA;
763 
764 	/* pass partial skb header to fw */
765 	tx_info->buf[1].len = MT_CT_PARSE_LEN;
766 	tx_info->buf[1].skip_unmap = true;
767 	tx_info->nbuf = MT_CT_DMA_BUF_NUM;
768 
769 	return 0;
770 }
771 
772 u32 mt7915_wed_init_buf(void *ptr, dma_addr_t phys, int token_id)
773 {
774 #if defined(__linux__)
775 	struct mt76_connac_fw_txp *txp = ptr + MT_TXD_SIZE;
776 #elif defined(__FreeBSD__)
777 	struct mt76_connac_fw_txp *txp = (void *)((u8 *)ptr + MT_TXD_SIZE);
778 #endif
779 	__le32 *txwi = ptr;
780 	u32 val;
781 
782 	memset(ptr, 0, MT_TXD_SIZE + sizeof(*txp));
783 
784 	val = FIELD_PREP(MT_TXD0_TX_BYTES, MT_TXD_SIZE) |
785 	      FIELD_PREP(MT_TXD0_PKT_FMT, MT_TX_TYPE_CT);
786 	txwi[0] = cpu_to_le32(val);
787 
788 	val = MT_TXD1_LONG_FORMAT |
789 	      FIELD_PREP(MT_TXD1_HDR_FORMAT, MT_HDR_FORMAT_802_3);
790 	txwi[1] = cpu_to_le32(val);
791 
792 	txp->token = cpu_to_le16(token_id);
793 	txp->nbuf = 1;
794 	txp->buf[0] = cpu_to_le32(phys + MT_TXD_SIZE + sizeof(*txp));
795 
796 	return MT_TXD_SIZE + sizeof(*txp);
797 }
798 
799 static void
800 mt7915_tx_check_aggr(struct ieee80211_sta *sta, __le32 *txwi)
801 {
802 	struct mt7915_sta *msta;
803 	u16 fc, tid;
804 	u32 val;
805 
806 	if (!sta || !(sta->deflink.ht_cap.ht_supported || sta->deflink.he_cap.has_he))
807 		return;
808 
809 	tid = le32_get_bits(txwi[1], MT_TXD1_TID);
810 	if (tid >= 6) /* skip VO queue */
811 		return;
812 
813 	val = le32_to_cpu(txwi[2]);
814 	fc = FIELD_GET(MT_TXD2_FRAME_TYPE, val) << 2 |
815 	     FIELD_GET(MT_TXD2_SUB_TYPE, val) << 4;
816 	if (unlikely(fc != (IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA)))
817 		return;
818 
819 	msta = (struct mt7915_sta *)sta->drv_priv;
820 	if (!test_and_set_bit(tid, &msta->ampdu_state))
821 		ieee80211_start_tx_ba_session(sta, tid, 0);
822 }
823 
824 static void
825 mt7915_txwi_free(struct mt7915_dev *dev, struct mt76_txwi_cache *t,
826 		 struct ieee80211_sta *sta, struct list_head *free_list)
827 {
828 	struct mt76_dev *mdev = &dev->mt76;
829 	struct mt7915_sta *msta;
830 	struct mt76_wcid *wcid;
831 	__le32 *txwi;
832 	u16 wcid_idx;
833 
834 	mt76_connac_txp_skb_unmap(mdev, t);
835 	if (!t->skb)
836 		goto out;
837 
838 	txwi = (__le32 *)mt76_get_txwi_ptr(mdev, t);
839 	if (sta) {
840 		wcid = (struct mt76_wcid *)sta->drv_priv;
841 		wcid_idx = wcid->idx;
842 	} else {
843 		wcid_idx = le32_get_bits(txwi[1], MT_TXD1_WLAN_IDX);
844 		wcid = rcu_dereference(dev->mt76.wcid[wcid_idx]);
845 
846 		if (wcid && wcid->sta) {
847 			msta = container_of(wcid, struct mt7915_sta, wcid);
848 			sta = container_of((void *)msta, struct ieee80211_sta,
849 					  drv_priv);
850 			spin_lock_bh(&dev->sta_poll_lock);
851 			if (list_empty(&msta->poll_list))
852 				list_add_tail(&msta->poll_list, &dev->sta_poll_list);
853 			spin_unlock_bh(&dev->sta_poll_lock);
854 		}
855 	}
856 
857 	if (sta && likely(t->skb->protocol != cpu_to_be16(ETH_P_PAE)))
858 		mt7915_tx_check_aggr(sta, txwi);
859 
860 	__mt76_tx_complete_skb(mdev, wcid_idx, t->skb, free_list);
861 
862 out:
863 	t->skb = NULL;
864 	mt76_put_txwi(mdev, t);
865 }
866 
867 static void
868 mt7915_mac_tx_free_prepare(struct mt7915_dev *dev)
869 {
870 	struct mt76_dev *mdev = &dev->mt76;
871 	struct mt76_phy *mphy_ext = mdev->phys[MT_BAND1];
872 
873 	/* clean DMA queues and unmap buffers first */
874 	mt76_queue_tx_cleanup(dev, dev->mphy.q_tx[MT_TXQ_PSD], false);
875 	mt76_queue_tx_cleanup(dev, dev->mphy.q_tx[MT_TXQ_BE], false);
876 	if (mphy_ext) {
877 		mt76_queue_tx_cleanup(dev, mphy_ext->q_tx[MT_TXQ_PSD], false);
878 		mt76_queue_tx_cleanup(dev, mphy_ext->q_tx[MT_TXQ_BE], false);
879 	}
880 }
881 
882 static void
883 mt7915_mac_tx_free_done(struct mt7915_dev *dev,
884 			struct list_head *free_list, bool wake)
885 {
886 	struct sk_buff *skb, *tmp;
887 
888 	mt7915_mac_sta_poll(dev);
889 
890 	if (wake)
891 		mt76_set_tx_blocked(&dev->mt76, false);
892 
893 	mt76_worker_schedule(&dev->mt76.tx_worker);
894 
895 	list_for_each_entry_safe(skb, tmp, free_list, list) {
896 		skb_list_del_init(skb);
897 		napi_consume_skb(skb, 1);
898 	}
899 }
900 
901 static void
902 mt7915_mac_tx_free(struct mt7915_dev *dev, void *data, int len)
903 {
904 	struct mt76_connac_tx_free *free = data;
905 #if defined(__linux__)
906 	__le32 *tx_info = (__le32 *)(data + sizeof(*free));
907 #elif defined(__FreeBSD__)
908 	__le32 *tx_info = (__le32 *)((u8 *)data + sizeof(*free));
909 #endif
910 	struct mt76_dev *mdev = &dev->mt76;
911 	struct mt76_txwi_cache *txwi;
912 	struct ieee80211_sta *sta = NULL;
913 #if defined(__linux__)
914 	LIST_HEAD(free_list);
915 	void *end = data + len;
916 #elif defined(__FreeBSD__)
917 	LINUX_LIST_HEAD(free_list);
918 	void *end = (u8 *)data + len;
919 #endif
920 	bool v3, wake = false;
921 	u16 total, count = 0;
922 	u32 txd = le32_to_cpu(free->txd);
923 	__le32 *cur_info;
924 
925 	mt7915_mac_tx_free_prepare(dev);
926 
927 	total = le16_get_bits(free->ctrl, MT_TX_FREE_MSDU_CNT);
928 	v3 = (FIELD_GET(MT_TX_FREE_VER, txd) == 0x4);
929 	if (WARN_ON_ONCE((void *)&tx_info[total >> v3] > end))
930 		return;
931 
932 	for (cur_info = tx_info; count < total; cur_info++) {
933 		u32 msdu, info = le32_to_cpu(*cur_info);
934 		u8 i;
935 
936 		/*
937 		 * 1'b1: new wcid pair.
938 		 * 1'b0: msdu_id with the same 'wcid pair' as above.
939 		 */
940 		if (info & MT_TX_FREE_PAIR) {
941 			struct mt7915_sta *msta;
942 			struct mt76_wcid *wcid;
943 			u16 idx;
944 
945 			idx = FIELD_GET(MT_TX_FREE_WLAN_ID, info);
946 			wcid = rcu_dereference(dev->mt76.wcid[idx]);
947 			sta = wcid_to_sta(wcid);
948 			if (!sta)
949 				continue;
950 
951 			msta = container_of(wcid, struct mt7915_sta, wcid);
952 			spin_lock_bh(&dev->sta_poll_lock);
953 			if (list_empty(&msta->poll_list))
954 				list_add_tail(&msta->poll_list, &dev->sta_poll_list);
955 			spin_unlock_bh(&dev->sta_poll_lock);
956 			continue;
957 		}
958 
959 		if (v3 && (info & MT_TX_FREE_MPDU_HEADER))
960 			continue;
961 
962 		for (i = 0; i < 1 + v3; i++) {
963 			if (v3) {
964 				msdu = (info >> (15 * i)) & MT_TX_FREE_MSDU_ID_V3;
965 				if (msdu == MT_TX_FREE_MSDU_ID_V3)
966 					continue;
967 			} else {
968 				msdu = FIELD_GET(MT_TX_FREE_MSDU_ID, info);
969 			}
970 			count++;
971 			txwi = mt76_token_release(mdev, msdu, &wake);
972 			if (!txwi)
973 				continue;
974 
975 			mt7915_txwi_free(dev, txwi, sta, &free_list);
976 		}
977 	}
978 
979 	mt7915_mac_tx_free_done(dev, &free_list, wake);
980 }
981 
982 static void
983 mt7915_mac_tx_free_v0(struct mt7915_dev *dev, void *data, int len)
984 {
985 	struct mt76_connac_tx_free *free = data;
986 #if defined(__linux__)
987 	__le16 *info = (__le16 *)(data + sizeof(*free));
988 #elif defined(__FreeBSD__)
989 	__le16 *info = (__le16 *)((u8 *)data + sizeof(*free));
990 #endif
991 	struct mt76_dev *mdev = &dev->mt76;
992 #if defined(__linux__)
993 	void *end = data + len;
994 	LIST_HEAD(free_list);
995 #elif defined(__FreeBSD__)
996 	void *end = (u8 *)data + len;
997 	LINUX_LIST_HEAD(free_list);
998 #endif
999 	bool wake = false;
1000 	u8 i, count;
1001 
1002 	mt7915_mac_tx_free_prepare(dev);
1003 
1004 	count = FIELD_GET(MT_TX_FREE_MSDU_CNT_V0, le16_to_cpu(free->ctrl));
1005 	if (WARN_ON_ONCE((void *)&info[count] > end))
1006 		return;
1007 
1008 	for (i = 0; i < count; i++) {
1009 		struct mt76_txwi_cache *txwi;
1010 		u16 msdu = le16_to_cpu(info[i]);
1011 
1012 		txwi = mt76_token_release(mdev, msdu, &wake);
1013 		if (!txwi)
1014 			continue;
1015 
1016 		mt7915_txwi_free(dev, txwi, NULL, &free_list);
1017 	}
1018 
1019 	mt7915_mac_tx_free_done(dev, &free_list, wake);
1020 }
1021 
1022 static void mt7915_mac_add_txs(struct mt7915_dev *dev, void *data)
1023 {
1024 	struct mt7915_sta *msta = NULL;
1025 	struct mt76_wcid *wcid;
1026 	__le32 *txs_data = data;
1027 	u16 wcidx;
1028 	u8 pid;
1029 
1030 	if (le32_get_bits(txs_data[0], MT_TXS0_TXS_FORMAT) > 1)
1031 		return;
1032 
1033 	wcidx = le32_get_bits(txs_data[2], MT_TXS2_WCID);
1034 	pid = le32_get_bits(txs_data[3], MT_TXS3_PID);
1035 
1036 	if (pid < MT_PACKET_ID_WED)
1037 		return;
1038 
1039 	if (wcidx >= mt7915_wtbl_size(dev))
1040 		return;
1041 
1042 	rcu_read_lock();
1043 
1044 	wcid = rcu_dereference(dev->mt76.wcid[wcidx]);
1045 	if (!wcid)
1046 		goto out;
1047 
1048 	msta = container_of(wcid, struct mt7915_sta, wcid);
1049 
1050 	if (pid == MT_PACKET_ID_WED)
1051 		mt76_connac2_mac_fill_txs(&dev->mt76, wcid, txs_data);
1052 	else
1053 		mt76_connac2_mac_add_txs_skb(&dev->mt76, wcid, pid, txs_data);
1054 
1055 	if (!wcid->sta)
1056 		goto out;
1057 
1058 	spin_lock_bh(&dev->sta_poll_lock);
1059 	if (list_empty(&msta->poll_list))
1060 		list_add_tail(&msta->poll_list, &dev->sta_poll_list);
1061 	spin_unlock_bh(&dev->sta_poll_lock);
1062 
1063 out:
1064 	rcu_read_unlock();
1065 }
1066 
1067 bool mt7915_rx_check(struct mt76_dev *mdev, void *data, int len)
1068 {
1069 	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
1070 	__le32 *rxd = (__le32 *)data;
1071 	__le32 *end = (__le32 *)&rxd[len / 4];
1072 	enum rx_pkt_type type;
1073 
1074 	type = le32_get_bits(rxd[0], MT_RXD0_PKT_TYPE);
1075 
1076 	switch (type) {
1077 	case PKT_TYPE_TXRX_NOTIFY:
1078 		mt7915_mac_tx_free(dev, data, len);
1079 		return false;
1080 	case PKT_TYPE_TXRX_NOTIFY_V0:
1081 		mt7915_mac_tx_free_v0(dev, data, len);
1082 		return false;
1083 	case PKT_TYPE_TXS:
1084 		for (rxd += 2; rxd + 8 <= end; rxd += 8)
1085 			mt7915_mac_add_txs(dev, rxd);
1086 		return false;
1087 	case PKT_TYPE_RX_FW_MONITOR:
1088 #if !defined(__FreeBSD__) || defined(CONFIG_MT7915_DEBUGFS)
1089 		mt7915_debugfs_rx_fw_monitor(dev, data, len);
1090 #endif
1091 		return false;
1092 	default:
1093 		return true;
1094 	}
1095 }
1096 
1097 void mt7915_queue_rx_skb(struct mt76_dev *mdev, enum mt76_rxq_id q,
1098 			 struct sk_buff *skb)
1099 {
1100 	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
1101 	__le32 *rxd = (__le32 *)skb->data;
1102 	__le32 *end = (__le32 *)&skb->data[skb->len];
1103 	enum rx_pkt_type type;
1104 
1105 	type = le32_get_bits(rxd[0], MT_RXD0_PKT_TYPE);
1106 
1107 	switch (type) {
1108 	case PKT_TYPE_TXRX_NOTIFY:
1109 		mt7915_mac_tx_free(dev, skb->data, skb->len);
1110 		napi_consume_skb(skb, 1);
1111 		break;
1112 	case PKT_TYPE_TXRX_NOTIFY_V0:
1113 		mt7915_mac_tx_free_v0(dev, skb->data, skb->len);
1114 		napi_consume_skb(skb, 1);
1115 		break;
1116 	case PKT_TYPE_RX_EVENT:
1117 		mt7915_mcu_rx_event(dev, skb);
1118 		break;
1119 	case PKT_TYPE_TXRXV:
1120 		mt7915_mac_fill_rx_vector(dev, skb);
1121 		break;
1122 	case PKT_TYPE_TXS:
1123 		for (rxd += 2; rxd + 8 <= end; rxd += 8)
1124 			mt7915_mac_add_txs(dev, rxd);
1125 		dev_kfree_skb(skb);
1126 		break;
1127 	case PKT_TYPE_RX_FW_MONITOR:
1128 #if !defined(__FreeBSD__) || defined(CONFIG_MT7915_DEBUGFS)
1129 		mt7915_debugfs_rx_fw_monitor(dev, skb->data, skb->len);
1130 #endif
1131 		dev_kfree_skb(skb);
1132 		break;
1133 	case PKT_TYPE_NORMAL:
1134 		if (!mt7915_mac_fill_rx(dev, skb)) {
1135 			mt76_rx(&dev->mt76, q, skb);
1136 			return;
1137 		}
1138 		fallthrough;
1139 	default:
1140 		dev_kfree_skb(skb);
1141 		break;
1142 	}
1143 }
1144 
1145 void mt7915_mac_cca_stats_reset(struct mt7915_phy *phy)
1146 {
1147 	struct mt7915_dev *dev = phy->dev;
1148 	u32 reg = MT_WF_PHY_RX_CTRL1(phy->band_idx);
1149 
1150 	mt76_clear(dev, reg, MT_WF_PHY_RX_CTRL1_STSCNT_EN);
1151 	mt76_set(dev, reg, BIT(11) | BIT(9));
1152 }
1153 
1154 void mt7915_mac_reset_counters(struct mt7915_phy *phy)
1155 {
1156 	struct mt7915_dev *dev = phy->dev;
1157 	int i;
1158 
1159 	for (i = 0; i < 4; i++) {
1160 		mt76_rr(dev, MT_TX_AGG_CNT(phy->band_idx, i));
1161 		mt76_rr(dev, MT_TX_AGG_CNT2(phy->band_idx, i));
1162 	}
1163 
1164 	i = 0;
1165 	phy->mt76->survey_time = ktime_get_boottime();
1166 	if (phy->band_idx)
1167 		i = ARRAY_SIZE(dev->mt76.aggr_stats) / 2;
1168 
1169 	memset(&dev->mt76.aggr_stats[i], 0, sizeof(dev->mt76.aggr_stats) / 2);
1170 
1171 	/* reset airtime counters */
1172 	mt76_set(dev, MT_WF_RMAC_MIB_AIRTIME0(phy->band_idx),
1173 		 MT_WF_RMAC_MIB_RXTIME_CLR);
1174 
1175 	mt7915_mcu_get_chan_mib_info(phy, true);
1176 }
1177 
1178 void mt7915_mac_set_timing(struct mt7915_phy *phy)
1179 {
1180 	s16 coverage_class = phy->coverage_class;
1181 	struct mt7915_dev *dev = phy->dev;
1182 	struct mt7915_phy *ext_phy = mt7915_ext_phy(dev);
1183 	u32 val, reg_offset;
1184 	u32 cck = FIELD_PREP(MT_TIMEOUT_VAL_PLCP, 231) |
1185 		  FIELD_PREP(MT_TIMEOUT_VAL_CCA, 48);
1186 	u32 ofdm = FIELD_PREP(MT_TIMEOUT_VAL_PLCP, 60) |
1187 		   FIELD_PREP(MT_TIMEOUT_VAL_CCA, 28);
1188 	int offset;
1189 	bool a_band = !(phy->mt76->chandef.chan->band == NL80211_BAND_2GHZ);
1190 
1191 	if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
1192 		return;
1193 
1194 	if (ext_phy)
1195 		coverage_class = max_t(s16, dev->phy.coverage_class,
1196 				       ext_phy->coverage_class);
1197 
1198 	mt76_set(dev, MT_ARB_SCR(phy->band_idx),
1199 		 MT_ARB_SCR_TX_DISABLE | MT_ARB_SCR_RX_DISABLE);
1200 	udelay(1);
1201 
1202 	offset = 3 * coverage_class;
1203 	reg_offset = FIELD_PREP(MT_TIMEOUT_VAL_PLCP, offset) |
1204 		     FIELD_PREP(MT_TIMEOUT_VAL_CCA, offset);
1205 
1206 	mt76_wr(dev, MT_TMAC_CDTR(phy->band_idx), cck + reg_offset);
1207 	mt76_wr(dev, MT_TMAC_ODTR(phy->band_idx), ofdm + reg_offset);
1208 	mt76_wr(dev, MT_TMAC_ICR0(phy->band_idx),
1209 		FIELD_PREP(MT_IFS_EIFS_OFDM, a_band ? 84 : 78) |
1210 		FIELD_PREP(MT_IFS_RIFS, 2) |
1211 		FIELD_PREP(MT_IFS_SIFS, 10) |
1212 		FIELD_PREP(MT_IFS_SLOT, phy->slottime));
1213 
1214 	mt76_wr(dev, MT_TMAC_ICR1(phy->band_idx),
1215 		FIELD_PREP(MT_IFS_EIFS_CCK, 314));
1216 
1217 	if (phy->slottime < 20 || a_band)
1218 		val = MT7915_CFEND_RATE_DEFAULT;
1219 	else
1220 		val = MT7915_CFEND_RATE_11B;
1221 
1222 	mt76_rmw_field(dev, MT_AGG_ACR0(phy->band_idx), MT_AGG_ACR_CFEND_RATE, val);
1223 	mt76_clear(dev, MT_ARB_SCR(phy->band_idx),
1224 		   MT_ARB_SCR_TX_DISABLE | MT_ARB_SCR_RX_DISABLE);
1225 }
1226 
1227 void mt7915_mac_enable_nf(struct mt7915_dev *dev, bool ext_phy)
1228 {
1229 	u32 reg;
1230 
1231 	reg = is_mt7915(&dev->mt76) ? MT_WF_PHY_RXTD12(ext_phy) :
1232 		MT_WF_PHY_RXTD12_MT7916(ext_phy);
1233 	mt76_set(dev, reg,
1234 		 MT_WF_PHY_RXTD12_IRPI_SW_CLR_ONLY |
1235 		 MT_WF_PHY_RXTD12_IRPI_SW_CLR);
1236 
1237 	reg = is_mt7915(&dev->mt76) ? MT_WF_PHY_RX_CTRL1(ext_phy) :
1238 		MT_WF_PHY_RX_CTRL1_MT7916(ext_phy);
1239 	mt76_set(dev, reg, FIELD_PREP(MT_WF_PHY_RX_CTRL1_IPI_EN, 0x5));
1240 }
1241 
1242 static u8
1243 mt7915_phy_get_nf(struct mt7915_phy *phy, int idx)
1244 {
1245 	static const u8 nf_power[] = { 92, 89, 86, 83, 80, 75, 70, 65, 60, 55, 52 };
1246 	struct mt7915_dev *dev = phy->dev;
1247 	u32 val, sum = 0, n = 0;
1248 	int nss, i;
1249 
1250 	for (nss = 0; nss < hweight8(phy->mt76->chainmask); nss++) {
1251 		u32 reg = is_mt7915(&dev->mt76) ?
1252 			MT_WF_IRPI_NSS(0, nss + (idx << dev->dbdc_support)) :
1253 			MT_WF_IRPI_NSS_MT7916(idx, nss);
1254 
1255 		for (i = 0; i < ARRAY_SIZE(nf_power); i++, reg += 4) {
1256 			val = mt76_rr(dev, reg);
1257 			sum += val * nf_power[i];
1258 			n += val;
1259 		}
1260 	}
1261 
1262 	if (!n)
1263 		return 0;
1264 
1265 	return sum / n;
1266 }
1267 
1268 void mt7915_update_channel(struct mt76_phy *mphy)
1269 {
1270 	struct mt7915_phy *phy = (struct mt7915_phy *)mphy->priv;
1271 	struct mt76_channel_state *state = mphy->chan_state;
1272 	int nf;
1273 
1274 	mt7915_mcu_get_chan_mib_info(phy, false);
1275 
1276 	nf = mt7915_phy_get_nf(phy, phy->band_idx);
1277 	if (!phy->noise)
1278 		phy->noise = nf << 4;
1279 	else if (nf)
1280 		phy->noise += nf - (phy->noise >> 4);
1281 
1282 	state->noise = -(phy->noise >> 4);
1283 }
1284 
1285 static bool
1286 mt7915_wait_reset_state(struct mt7915_dev *dev, u32 state)
1287 {
1288 	bool ret;
1289 
1290 	ret = wait_event_timeout(dev->reset_wait,
1291 				 (READ_ONCE(dev->reset_state) & state),
1292 				 MT7915_RESET_TIMEOUT);
1293 
1294 	WARN(!ret, "Timeout waiting for MCU reset state %x\n", state);
1295 	return ret;
1296 }
1297 
1298 static void
1299 mt7915_update_vif_beacon(void *priv, u8 *mac, struct ieee80211_vif *vif)
1300 {
1301 	struct ieee80211_hw *hw = priv;
1302 
1303 	switch (vif->type) {
1304 	case NL80211_IFTYPE_MESH_POINT:
1305 	case NL80211_IFTYPE_ADHOC:
1306 	case NL80211_IFTYPE_AP:
1307 		mt7915_mcu_add_beacon(hw, vif, vif->bss_conf.enable_beacon,
1308 				      BSS_CHANGED_BEACON_ENABLED);
1309 		break;
1310 	default:
1311 		break;
1312 	}
1313 }
1314 
1315 static void
1316 mt7915_update_beacons(struct mt7915_dev *dev)
1317 {
1318 	struct mt76_phy *mphy_ext = dev->mt76.phys[MT_BAND1];
1319 
1320 	ieee80211_iterate_active_interfaces(dev->mt76.hw,
1321 		IEEE80211_IFACE_ITER_RESUME_ALL,
1322 		mt7915_update_vif_beacon, dev->mt76.hw);
1323 
1324 	if (!mphy_ext)
1325 		return;
1326 
1327 	ieee80211_iterate_active_interfaces(mphy_ext->hw,
1328 		IEEE80211_IFACE_ITER_RESUME_ALL,
1329 		mt7915_update_vif_beacon, mphy_ext->hw);
1330 }
1331 
1332 static void
1333 mt7915_dma_reset(struct mt7915_dev *dev)
1334 {
1335 	struct mt76_phy *mphy_ext = dev->mt76.phys[MT_BAND1];
1336 	u32 hif1_ofs = MT_WFDMA0_PCIE1(0) - MT_WFDMA0(0);
1337 	int i;
1338 
1339 	mt76_clear(dev, MT_WFDMA0_GLO_CFG,
1340 		   MT_WFDMA0_GLO_CFG_TX_DMA_EN |
1341 		   MT_WFDMA0_GLO_CFG_RX_DMA_EN);
1342 
1343 	if (is_mt7915(&dev->mt76))
1344 		mt76_clear(dev, MT_WFDMA1_GLO_CFG,
1345 			   MT_WFDMA1_GLO_CFG_TX_DMA_EN |
1346 			   MT_WFDMA1_GLO_CFG_RX_DMA_EN);
1347 	if (dev->hif2) {
1348 		mt76_clear(dev, MT_WFDMA0_GLO_CFG + hif1_ofs,
1349 			   MT_WFDMA0_GLO_CFG_TX_DMA_EN |
1350 			   MT_WFDMA0_GLO_CFG_RX_DMA_EN);
1351 
1352 		if (is_mt7915(&dev->mt76))
1353 			mt76_clear(dev, MT_WFDMA1_GLO_CFG + hif1_ofs,
1354 				   MT_WFDMA1_GLO_CFG_TX_DMA_EN |
1355 				   MT_WFDMA1_GLO_CFG_RX_DMA_EN);
1356 	}
1357 
1358 	usleep_range(1000, 2000);
1359 
1360 	for (i = 0; i < __MT_TXQ_MAX; i++) {
1361 		mt76_queue_tx_cleanup(dev, dev->mphy.q_tx[i], true);
1362 		if (mphy_ext)
1363 			mt76_queue_tx_cleanup(dev, mphy_ext->q_tx[i], true);
1364 	}
1365 
1366 	for (i = 0; i < __MT_MCUQ_MAX; i++)
1367 		mt76_queue_tx_cleanup(dev, dev->mt76.q_mcu[i], true);
1368 
1369 	mt76_for_each_q_rx(&dev->mt76, i)
1370 		mt76_queue_rx_reset(dev, i);
1371 
1372 	mt76_tx_status_check(&dev->mt76, true);
1373 
1374 	/* re-init prefetch settings after reset */
1375 	mt7915_dma_prefetch(dev);
1376 
1377 	mt76_set(dev, MT_WFDMA0_GLO_CFG,
1378 		 MT_WFDMA0_GLO_CFG_TX_DMA_EN | MT_WFDMA0_GLO_CFG_RX_DMA_EN);
1379 	if (is_mt7915(&dev->mt76))
1380 		mt76_set(dev, MT_WFDMA1_GLO_CFG,
1381 			 MT_WFDMA1_GLO_CFG_TX_DMA_EN |
1382 			 MT_WFDMA1_GLO_CFG_RX_DMA_EN |
1383 			 MT_WFDMA1_GLO_CFG_OMIT_TX_INFO |
1384 			 MT_WFDMA1_GLO_CFG_OMIT_RX_INFO);
1385 	if (dev->hif2) {
1386 		mt76_set(dev, MT_WFDMA0_GLO_CFG + hif1_ofs,
1387 			 MT_WFDMA0_GLO_CFG_TX_DMA_EN |
1388 			 MT_WFDMA0_GLO_CFG_RX_DMA_EN);
1389 
1390 		if (is_mt7915(&dev->mt76))
1391 			mt76_set(dev, MT_WFDMA1_GLO_CFG + hif1_ofs,
1392 				 MT_WFDMA1_GLO_CFG_TX_DMA_EN |
1393 				 MT_WFDMA1_GLO_CFG_RX_DMA_EN |
1394 				 MT_WFDMA1_GLO_CFG_OMIT_TX_INFO |
1395 				 MT_WFDMA1_GLO_CFG_OMIT_RX_INFO);
1396 	}
1397 }
1398 
1399 void mt7915_tx_token_put(struct mt7915_dev *dev)
1400 {
1401 	struct mt76_txwi_cache *txwi;
1402 	int id;
1403 
1404 	spin_lock_bh(&dev->mt76.token_lock);
1405 	idr_for_each_entry(&dev->mt76.token, txwi, id) {
1406 		mt7915_txwi_free(dev, txwi, NULL, NULL);
1407 		dev->mt76.token_count--;
1408 	}
1409 	spin_unlock_bh(&dev->mt76.token_lock);
1410 	idr_destroy(&dev->mt76.token);
1411 }
1412 
1413 /* system error recovery */
1414 void mt7915_mac_reset_work(struct work_struct *work)
1415 {
1416 	struct mt7915_phy *phy2;
1417 	struct mt76_phy *ext_phy;
1418 	struct mt7915_dev *dev;
1419 	int i;
1420 
1421 	dev = container_of(work, struct mt7915_dev, reset_work);
1422 	ext_phy = dev->mt76.phys[MT_BAND1];
1423 	phy2 = ext_phy ? ext_phy->priv : NULL;
1424 
1425 	if (!(READ_ONCE(dev->reset_state) & MT_MCU_CMD_STOP_DMA))
1426 		return;
1427 
1428 	ieee80211_stop_queues(mt76_hw(dev));
1429 	if (ext_phy)
1430 		ieee80211_stop_queues(ext_phy->hw);
1431 
1432 	set_bit(MT76_RESET, &dev->mphy.state);
1433 	set_bit(MT76_MCU_RESET, &dev->mphy.state);
1434 	wake_up(&dev->mt76.mcu.wait);
1435 	cancel_delayed_work_sync(&dev->mphy.mac_work);
1436 	if (phy2) {
1437 		set_bit(MT76_RESET, &phy2->mt76->state);
1438 		cancel_delayed_work_sync(&phy2->mt76->mac_work);
1439 	}
1440 	mt76_worker_disable(&dev->mt76.tx_worker);
1441 	mt76_for_each_q_rx(&dev->mt76, i)
1442 		napi_disable(&dev->mt76.napi[i]);
1443 	napi_disable(&dev->mt76.tx_napi);
1444 
1445 	mutex_lock(&dev->mt76.mutex);
1446 
1447 	mt76_wr(dev, MT_MCU_INT_EVENT, MT_MCU_INT_EVENT_DMA_STOPPED);
1448 
1449 	if (mt7915_wait_reset_state(dev, MT_MCU_CMD_RESET_DONE)) {
1450 		mt7915_dma_reset(dev);
1451 
1452 		mt7915_tx_token_put(dev);
1453 		idr_init(&dev->mt76.token);
1454 
1455 		mt76_wr(dev, MT_MCU_INT_EVENT, MT_MCU_INT_EVENT_DMA_INIT);
1456 		mt7915_wait_reset_state(dev, MT_MCU_CMD_RECOVERY_DONE);
1457 	}
1458 
1459 	clear_bit(MT76_MCU_RESET, &dev->mphy.state);
1460 	clear_bit(MT76_RESET, &dev->mphy.state);
1461 	if (phy2)
1462 		clear_bit(MT76_RESET, &phy2->mt76->state);
1463 
1464 	local_bh_disable();
1465 	mt76_for_each_q_rx(&dev->mt76, i) {
1466 		napi_enable(&dev->mt76.napi[i]);
1467 		napi_schedule(&dev->mt76.napi[i]);
1468 	}
1469 	local_bh_enable();
1470 
1471 	tasklet_schedule(&dev->irq_tasklet);
1472 
1473 	mt76_wr(dev, MT_MCU_INT_EVENT, MT_MCU_INT_EVENT_RESET_DONE);
1474 	mt7915_wait_reset_state(dev, MT_MCU_CMD_NORMAL_STATE);
1475 
1476 	mt76_worker_enable(&dev->mt76.tx_worker);
1477 
1478 	local_bh_disable();
1479 	napi_enable(&dev->mt76.tx_napi);
1480 	napi_schedule(&dev->mt76.tx_napi);
1481 	local_bh_enable();
1482 
1483 	ieee80211_wake_queues(mt76_hw(dev));
1484 	if (ext_phy)
1485 		ieee80211_wake_queues(ext_phy->hw);
1486 
1487 	mutex_unlock(&dev->mt76.mutex);
1488 
1489 	mt7915_update_beacons(dev);
1490 
1491 	ieee80211_queue_delayed_work(mt76_hw(dev), &dev->mphy.mac_work,
1492 				     MT7915_WATCHDOG_TIME);
1493 	if (phy2)
1494 		ieee80211_queue_delayed_work(ext_phy->hw,
1495 					     &phy2->mt76->mac_work,
1496 					     MT7915_WATCHDOG_TIME);
1497 }
1498 
1499 void mt7915_mac_update_stats(struct mt7915_phy *phy)
1500 {
1501 	struct mt7915_dev *dev = phy->dev;
1502 	struct mib_stats *mib = &phy->mib;
1503 	int i, aggr0, aggr1, cnt;
1504 	u32 val;
1505 
1506 	cnt = mt76_rr(dev, MT_MIB_SDR3(phy->band_idx));
1507 	mib->fcs_err_cnt += is_mt7915(&dev->mt76) ?
1508 		FIELD_GET(MT_MIB_SDR3_FCS_ERR_MASK, cnt) :
1509 		FIELD_GET(MT_MIB_SDR3_FCS_ERR_MASK_MT7916, cnt);
1510 
1511 	cnt = mt76_rr(dev, MT_MIB_SDR4(phy->band_idx));
1512 	mib->rx_fifo_full_cnt += FIELD_GET(MT_MIB_SDR4_RX_FIFO_FULL_MASK, cnt);
1513 
1514 	cnt = mt76_rr(dev, MT_MIB_SDR5(phy->band_idx));
1515 	mib->rx_mpdu_cnt += cnt;
1516 
1517 	cnt = mt76_rr(dev, MT_MIB_SDR6(phy->band_idx));
1518 	mib->channel_idle_cnt += FIELD_GET(MT_MIB_SDR6_CHANNEL_IDL_CNT_MASK, cnt);
1519 
1520 	cnt = mt76_rr(dev, MT_MIB_SDR7(phy->band_idx));
1521 	mib->rx_vector_mismatch_cnt +=
1522 		FIELD_GET(MT_MIB_SDR7_RX_VECTOR_MISMATCH_CNT_MASK, cnt);
1523 
1524 	cnt = mt76_rr(dev, MT_MIB_SDR8(phy->band_idx));
1525 	mib->rx_delimiter_fail_cnt +=
1526 		FIELD_GET(MT_MIB_SDR8_RX_DELIMITER_FAIL_CNT_MASK, cnt);
1527 
1528 	cnt = mt76_rr(dev, MT_MIB_SDR10(phy->band_idx));
1529 	mib->rx_mrdy_cnt += is_mt7915(&dev->mt76) ?
1530 		FIELD_GET(MT_MIB_SDR10_MRDY_COUNT_MASK, cnt) :
1531 		FIELD_GET(MT_MIB_SDR10_MRDY_COUNT_MASK_MT7916, cnt);
1532 
1533 	cnt = mt76_rr(dev, MT_MIB_SDR11(phy->band_idx));
1534 	mib->rx_len_mismatch_cnt +=
1535 		FIELD_GET(MT_MIB_SDR11_RX_LEN_MISMATCH_CNT_MASK, cnt);
1536 
1537 	cnt = mt76_rr(dev, MT_MIB_SDR12(phy->band_idx));
1538 	mib->tx_ampdu_cnt += cnt;
1539 
1540 	cnt = mt76_rr(dev, MT_MIB_SDR13(phy->band_idx));
1541 	mib->tx_stop_q_empty_cnt +=
1542 		FIELD_GET(MT_MIB_SDR13_TX_STOP_Q_EMPTY_CNT_MASK, cnt);
1543 
1544 	cnt = mt76_rr(dev, MT_MIB_SDR14(phy->band_idx));
1545 	mib->tx_mpdu_attempts_cnt += is_mt7915(&dev->mt76) ?
1546 		FIELD_GET(MT_MIB_SDR14_TX_MPDU_ATTEMPTS_CNT_MASK, cnt) :
1547 		FIELD_GET(MT_MIB_SDR14_TX_MPDU_ATTEMPTS_CNT_MASK_MT7916, cnt);
1548 
1549 	cnt = mt76_rr(dev, MT_MIB_SDR15(phy->band_idx));
1550 	mib->tx_mpdu_success_cnt += is_mt7915(&dev->mt76) ?
1551 		FIELD_GET(MT_MIB_SDR15_TX_MPDU_SUCCESS_CNT_MASK, cnt) :
1552 		FIELD_GET(MT_MIB_SDR15_TX_MPDU_SUCCESS_CNT_MASK_MT7916, cnt);
1553 
1554 	cnt = mt76_rr(dev, MT_MIB_SDR16(phy->band_idx));
1555 	mib->primary_cca_busy_time +=
1556 		FIELD_GET(MT_MIB_SDR16_PRIMARY_CCA_BUSY_TIME_MASK, cnt);
1557 
1558 	cnt = mt76_rr(dev, MT_MIB_SDR17(phy->band_idx));
1559 	mib->secondary_cca_busy_time +=
1560 		FIELD_GET(MT_MIB_SDR17_SECONDARY_CCA_BUSY_TIME_MASK, cnt);
1561 
1562 	cnt = mt76_rr(dev, MT_MIB_SDR18(phy->band_idx));
1563 	mib->primary_energy_detect_time +=
1564 		FIELD_GET(MT_MIB_SDR18_PRIMARY_ENERGY_DETECT_TIME_MASK, cnt);
1565 
1566 	cnt = mt76_rr(dev, MT_MIB_SDR19(phy->band_idx));
1567 	mib->cck_mdrdy_time += FIELD_GET(MT_MIB_SDR19_CCK_MDRDY_TIME_MASK, cnt);
1568 
1569 	cnt = mt76_rr(dev, MT_MIB_SDR20(phy->band_idx));
1570 	mib->ofdm_mdrdy_time +=
1571 		FIELD_GET(MT_MIB_SDR20_OFDM_VHT_MDRDY_TIME_MASK, cnt);
1572 
1573 	cnt = mt76_rr(dev, MT_MIB_SDR21(phy->band_idx));
1574 	mib->green_mdrdy_time +=
1575 		FIELD_GET(MT_MIB_SDR21_GREEN_MDRDY_TIME_MASK, cnt);
1576 
1577 	cnt = mt76_rr(dev, MT_MIB_SDR22(phy->band_idx));
1578 	mib->rx_ampdu_cnt += cnt;
1579 
1580 	cnt = mt76_rr(dev, MT_MIB_SDR23(phy->band_idx));
1581 	mib->rx_ampdu_bytes_cnt += cnt;
1582 
1583 	cnt = mt76_rr(dev, MT_MIB_SDR24(phy->band_idx));
1584 	mib->rx_ampdu_valid_subframe_cnt += is_mt7915(&dev->mt76) ?
1585 		FIELD_GET(MT_MIB_SDR24_RX_AMPDU_SF_CNT_MASK, cnt) :
1586 		FIELD_GET(MT_MIB_SDR24_RX_AMPDU_SF_CNT_MASK_MT7916, cnt);
1587 
1588 	cnt = mt76_rr(dev, MT_MIB_SDR25(phy->band_idx));
1589 	mib->rx_ampdu_valid_subframe_bytes_cnt += cnt;
1590 
1591 	cnt = mt76_rr(dev, MT_MIB_SDR27(phy->band_idx));
1592 	mib->tx_rwp_fail_cnt +=
1593 		FIELD_GET(MT_MIB_SDR27_TX_RWP_FAIL_CNT_MASK, cnt);
1594 
1595 	cnt = mt76_rr(dev, MT_MIB_SDR28(phy->band_idx));
1596 	mib->tx_rwp_need_cnt +=
1597 		FIELD_GET(MT_MIB_SDR28_TX_RWP_NEED_CNT_MASK, cnt);
1598 
1599 	cnt = mt76_rr(dev, MT_MIB_SDR29(phy->band_idx));
1600 	mib->rx_pfdrop_cnt += is_mt7915(&dev->mt76) ?
1601 		FIELD_GET(MT_MIB_SDR29_RX_PFDROP_CNT_MASK, cnt) :
1602 		FIELD_GET(MT_MIB_SDR29_RX_PFDROP_CNT_MASK_MT7916, cnt);
1603 
1604 	cnt = mt76_rr(dev, MT_MIB_SDRVEC(phy->band_idx));
1605 	mib->rx_vec_queue_overflow_drop_cnt += is_mt7915(&dev->mt76) ?
1606 		FIELD_GET(MT_MIB_SDR30_RX_VEC_QUEUE_OVERFLOW_DROP_CNT_MASK, cnt) :
1607 		FIELD_GET(MT_MIB_SDR30_RX_VEC_QUEUE_OVERFLOW_DROP_CNT_MASK_MT7916, cnt);
1608 
1609 	cnt = mt76_rr(dev, MT_MIB_SDR31(phy->band_idx));
1610 	mib->rx_ba_cnt += cnt;
1611 
1612 	cnt = mt76_rr(dev, MT_MIB_SDRMUBF(phy->band_idx));
1613 	mib->tx_bf_cnt += FIELD_GET(MT_MIB_MU_BF_TX_CNT, cnt);
1614 
1615 	cnt = mt76_rr(dev, MT_MIB_DR8(phy->band_idx));
1616 	mib->tx_mu_mpdu_cnt += cnt;
1617 
1618 	cnt = mt76_rr(dev, MT_MIB_DR9(phy->band_idx));
1619 	mib->tx_mu_acked_mpdu_cnt += cnt;
1620 
1621 	cnt = mt76_rr(dev, MT_MIB_DR11(phy->band_idx));
1622 	mib->tx_su_acked_mpdu_cnt += cnt;
1623 
1624 	cnt = mt76_rr(dev, MT_ETBF_PAR_RPT0(phy->band_idx));
1625 	mib->tx_bf_rx_fb_bw = FIELD_GET(MT_ETBF_PAR_RPT0_FB_BW, cnt);
1626 	mib->tx_bf_rx_fb_nc_cnt += FIELD_GET(MT_ETBF_PAR_RPT0_FB_NC, cnt);
1627 	mib->tx_bf_rx_fb_nr_cnt += FIELD_GET(MT_ETBF_PAR_RPT0_FB_NR, cnt);
1628 
1629 	for (i = 0; i < ARRAY_SIZE(mib->tx_amsdu); i++) {
1630 		cnt = mt76_rr(dev, MT_PLE_AMSDU_PACK_MSDU_CNT(i));
1631 		mib->tx_amsdu[i] += cnt;
1632 		mib->tx_amsdu_cnt += cnt;
1633 	}
1634 
1635 	aggr0 = phy->band_idx ? ARRAY_SIZE(dev->mt76.aggr_stats) / 2 : 0;
1636 	if (is_mt7915(&dev->mt76)) {
1637 		for (i = 0, aggr1 = aggr0 + 4; i < 4; i++) {
1638 			val = mt76_rr(dev, MT_MIB_MB_SDR1(phy->band_idx, (i << 4)));
1639 			mib->ba_miss_cnt +=
1640 				FIELD_GET(MT_MIB_BA_MISS_COUNT_MASK, val);
1641 			mib->ack_fail_cnt +=
1642 				FIELD_GET(MT_MIB_ACK_FAIL_COUNT_MASK, val);
1643 
1644 			val = mt76_rr(dev, MT_MIB_MB_SDR0(phy->band_idx, (i << 4)));
1645 			mib->rts_cnt += FIELD_GET(MT_MIB_RTS_COUNT_MASK, val);
1646 			mib->rts_retries_cnt +=
1647 				FIELD_GET(MT_MIB_RTS_RETRIES_COUNT_MASK, val);
1648 
1649 			val = mt76_rr(dev, MT_TX_AGG_CNT(phy->band_idx, i));
1650 			dev->mt76.aggr_stats[aggr0++] += val & 0xffff;
1651 			dev->mt76.aggr_stats[aggr0++] += val >> 16;
1652 
1653 			val = mt76_rr(dev, MT_TX_AGG_CNT2(phy->band_idx, i));
1654 			dev->mt76.aggr_stats[aggr1++] += val & 0xffff;
1655 			dev->mt76.aggr_stats[aggr1++] += val >> 16;
1656 		}
1657 
1658 		cnt = mt76_rr(dev, MT_MIB_SDR32(phy->band_idx));
1659 		mib->tx_pkt_ebf_cnt += FIELD_GET(MT_MIB_SDR32_TX_PKT_EBF_CNT, cnt);
1660 
1661 		cnt = mt76_rr(dev, MT_MIB_SDR33(phy->band_idx));
1662 		mib->tx_pkt_ibf_cnt += FIELD_GET(MT_MIB_SDR33_TX_PKT_IBF_CNT, cnt);
1663 
1664 		cnt = mt76_rr(dev, MT_ETBF_TX_APP_CNT(phy->band_idx));
1665 		mib->tx_bf_ibf_ppdu_cnt += FIELD_GET(MT_ETBF_TX_IBF_CNT, cnt);
1666 		mib->tx_bf_ebf_ppdu_cnt += FIELD_GET(MT_ETBF_TX_EBF_CNT, cnt);
1667 
1668 		cnt = mt76_rr(dev, MT_ETBF_TX_NDP_BFRP(phy->band_idx));
1669 		mib->tx_bf_fb_cpl_cnt += FIELD_GET(MT_ETBF_TX_FB_CPL, cnt);
1670 		mib->tx_bf_fb_trig_cnt += FIELD_GET(MT_ETBF_TX_FB_TRI, cnt);
1671 
1672 		cnt = mt76_rr(dev, MT_ETBF_RX_FB_CNT(phy->band_idx));
1673 		mib->tx_bf_rx_fb_all_cnt += FIELD_GET(MT_ETBF_RX_FB_ALL, cnt);
1674 		mib->tx_bf_rx_fb_he_cnt += FIELD_GET(MT_ETBF_RX_FB_HE, cnt);
1675 		mib->tx_bf_rx_fb_vht_cnt += FIELD_GET(MT_ETBF_RX_FB_VHT, cnt);
1676 		mib->tx_bf_rx_fb_ht_cnt += FIELD_GET(MT_ETBF_RX_FB_HT, cnt);
1677 	} else {
1678 		for (i = 0; i < 2; i++) {
1679 			/* rts count */
1680 			val = mt76_rr(dev, MT_MIB_MB_SDR0(phy->band_idx, (i << 2)));
1681 			mib->rts_cnt += FIELD_GET(GENMASK(15, 0), val);
1682 			mib->rts_cnt += FIELD_GET(GENMASK(31, 16), val);
1683 
1684 			/* rts retry count */
1685 			val = mt76_rr(dev, MT_MIB_MB_SDR1(phy->band_idx, (i << 2)));
1686 			mib->rts_retries_cnt += FIELD_GET(GENMASK(15, 0), val);
1687 			mib->rts_retries_cnt += FIELD_GET(GENMASK(31, 16), val);
1688 
1689 			/* ba miss count */
1690 			val = mt76_rr(dev, MT_MIB_MB_SDR2(phy->band_idx, (i << 2)));
1691 			mib->ba_miss_cnt += FIELD_GET(GENMASK(15, 0), val);
1692 			mib->ba_miss_cnt += FIELD_GET(GENMASK(31, 16), val);
1693 
1694 			/* ack fail count */
1695 			val = mt76_rr(dev, MT_MIB_MB_BFTF(phy->band_idx, (i << 2)));
1696 			mib->ack_fail_cnt += FIELD_GET(GENMASK(15, 0), val);
1697 			mib->ack_fail_cnt += FIELD_GET(GENMASK(31, 16), val);
1698 		}
1699 
1700 		for (i = 0; i < 8; i++) {
1701 			val = mt76_rr(dev, MT_TX_AGG_CNT(phy->band_idx, i));
1702 			dev->mt76.aggr_stats[aggr0++] += FIELD_GET(GENMASK(15, 0), val);
1703 			dev->mt76.aggr_stats[aggr0++] += FIELD_GET(GENMASK(31, 16), val);
1704 		}
1705 
1706 		cnt = mt76_rr(dev, MT_MIB_SDR32(phy->band_idx));
1707 		mib->tx_pkt_ibf_cnt += FIELD_GET(MT_MIB_SDR32_TX_PKT_IBF_CNT, cnt);
1708 		mib->tx_bf_ibf_ppdu_cnt += FIELD_GET(MT_MIB_SDR32_TX_PKT_IBF_CNT, cnt);
1709 		mib->tx_pkt_ebf_cnt += FIELD_GET(MT_MIB_SDR32_TX_PKT_EBF_CNT, cnt);
1710 		mib->tx_bf_ebf_ppdu_cnt += FIELD_GET(MT_MIB_SDR32_TX_PKT_EBF_CNT, cnt);
1711 
1712 		cnt = mt76_rr(dev, MT_MIB_BFCR7(phy->band_idx));
1713 		mib->tx_bf_fb_cpl_cnt += FIELD_GET(MT_MIB_BFCR7_BFEE_TX_FB_CPL, cnt);
1714 
1715 		cnt = mt76_rr(dev, MT_MIB_BFCR2(phy->band_idx));
1716 		mib->tx_bf_fb_trig_cnt += FIELD_GET(MT_MIB_BFCR2_BFEE_TX_FB_TRIG, cnt);
1717 
1718 		cnt = mt76_rr(dev, MT_MIB_BFCR0(phy->band_idx));
1719 		mib->tx_bf_rx_fb_vht_cnt += FIELD_GET(MT_MIB_BFCR0_RX_FB_VHT, cnt);
1720 		mib->tx_bf_rx_fb_all_cnt += FIELD_GET(MT_MIB_BFCR0_RX_FB_VHT, cnt);
1721 		mib->tx_bf_rx_fb_ht_cnt += FIELD_GET(MT_MIB_BFCR0_RX_FB_HT, cnt);
1722 		mib->tx_bf_rx_fb_all_cnt += FIELD_GET(MT_MIB_BFCR0_RX_FB_HT, cnt);
1723 
1724 		cnt = mt76_rr(dev, MT_MIB_BFCR1(phy->band_idx));
1725 		mib->tx_bf_rx_fb_he_cnt += FIELD_GET(MT_MIB_BFCR1_RX_FB_HE, cnt);
1726 		mib->tx_bf_rx_fb_all_cnt += FIELD_GET(MT_MIB_BFCR1_RX_FB_HE, cnt);
1727 	}
1728 }
1729 
1730 static void mt7915_mac_severe_check(struct mt7915_phy *phy)
1731 {
1732 	struct mt7915_dev *dev = phy->dev;
1733 	bool ext_phy = phy != &dev->phy;
1734 	u32 trb;
1735 
1736 	if (!phy->omac_mask)
1737 		return;
1738 
1739 	/* In rare cases, TRB pointers might be out of sync leads to RMAC
1740 	 * stopping Rx, so check status periodically to see if TRB hardware
1741 	 * requires minimal recovery.
1742 	 */
1743 	trb = mt76_rr(dev, MT_TRB_RXPSR0(phy->band_idx));
1744 
1745 	if ((FIELD_GET(MT_TRB_RXPSR0_RX_RMAC_PTR, trb) !=
1746 	     FIELD_GET(MT_TRB_RXPSR0_RX_WTBL_PTR, trb)) &&
1747 	    (FIELD_GET(MT_TRB_RXPSR0_RX_RMAC_PTR, phy->trb_ts) !=
1748 	     FIELD_GET(MT_TRB_RXPSR0_RX_WTBL_PTR, phy->trb_ts)) &&
1749 	    trb == phy->trb_ts)
1750 		mt7915_mcu_set_ser(dev, SER_RECOVER, SER_SET_RECOVER_L3_RX_ABORT,
1751 				   ext_phy);
1752 
1753 	phy->trb_ts = trb;
1754 }
1755 
1756 void mt7915_mac_sta_rc_work(struct work_struct *work)
1757 {
1758 	struct mt7915_dev *dev = container_of(work, struct mt7915_dev, rc_work);
1759 	struct ieee80211_sta *sta;
1760 	struct ieee80211_vif *vif;
1761 	struct mt7915_sta *msta;
1762 	u32 changed;
1763 #if defined(__linux__)
1764 	LIST_HEAD(list);
1765 #elif defined(__FreeBSD__)
1766 	LINUX_LIST_HEAD(list);
1767 #endif
1768 
1769 	spin_lock_bh(&dev->sta_poll_lock);
1770 	list_splice_init(&dev->sta_rc_list, &list);
1771 
1772 	while (!list_empty(&list)) {
1773 		msta = list_first_entry(&list, struct mt7915_sta, rc_list);
1774 		list_del_init(&msta->rc_list);
1775 		changed = msta->changed;
1776 		msta->changed = 0;
1777 		spin_unlock_bh(&dev->sta_poll_lock);
1778 
1779 		sta = container_of((void *)msta, struct ieee80211_sta, drv_priv);
1780 		vif = container_of((void *)msta->vif, struct ieee80211_vif, drv_priv);
1781 
1782 		if (changed & (IEEE80211_RC_SUPP_RATES_CHANGED |
1783 			       IEEE80211_RC_NSS_CHANGED |
1784 			       IEEE80211_RC_BW_CHANGED))
1785 			mt7915_mcu_add_rate_ctrl(dev, vif, sta, true);
1786 
1787 		if (changed & IEEE80211_RC_SMPS_CHANGED)
1788 			mt7915_mcu_add_smps(dev, vif, sta);
1789 
1790 		spin_lock_bh(&dev->sta_poll_lock);
1791 	}
1792 
1793 	spin_unlock_bh(&dev->sta_poll_lock);
1794 }
1795 
1796 void mt7915_mac_work(struct work_struct *work)
1797 {
1798 	struct mt7915_phy *phy;
1799 	struct mt76_phy *mphy;
1800 
1801 	mphy = (struct mt76_phy *)container_of(work, struct mt76_phy,
1802 					       mac_work.work);
1803 	phy = mphy->priv;
1804 
1805 	mutex_lock(&mphy->dev->mutex);
1806 
1807 	mt76_update_survey(mphy);
1808 	if (++mphy->mac_work_count == 5) {
1809 		mphy->mac_work_count = 0;
1810 
1811 		mt7915_mac_update_stats(phy);
1812 		mt7915_mac_severe_check(phy);
1813 	}
1814 
1815 	mutex_unlock(&mphy->dev->mutex);
1816 
1817 	mt76_tx_status_check(mphy->dev, false);
1818 
1819 	ieee80211_queue_delayed_work(mphy->hw, &mphy->mac_work,
1820 				     MT7915_WATCHDOG_TIME);
1821 }
1822 
1823 static void mt7915_dfs_stop_radar_detector(struct mt7915_phy *phy)
1824 {
1825 	struct mt7915_dev *dev = phy->dev;
1826 
1827 	if (phy->rdd_state & BIT(0))
1828 		mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_STOP, 0,
1829 					MT_RX_SEL0, 0);
1830 	if (phy->rdd_state & BIT(1))
1831 		mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_STOP, 1,
1832 					MT_RX_SEL0, 0);
1833 }
1834 
1835 static int mt7915_dfs_start_rdd(struct mt7915_dev *dev, int chain)
1836 {
1837 	int err, region;
1838 
1839 	switch (dev->mt76.region) {
1840 	case NL80211_DFS_ETSI:
1841 		region = 0;
1842 		break;
1843 	case NL80211_DFS_JP:
1844 		region = 2;
1845 		break;
1846 	case NL80211_DFS_FCC:
1847 	default:
1848 		region = 1;
1849 		break;
1850 	}
1851 
1852 	err = mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_START, chain,
1853 				      MT_RX_SEL0, region);
1854 	if (err < 0)
1855 		return err;
1856 
1857 	return mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_DET_MODE, chain,
1858 				       MT_RX_SEL0, 1);
1859 }
1860 
1861 static int mt7915_dfs_start_radar_detector(struct mt7915_phy *phy)
1862 {
1863 	struct cfg80211_chan_def *chandef = &phy->mt76->chandef;
1864 	struct mt7915_dev *dev = phy->dev;
1865 	int err;
1866 
1867 	/* start CAC */
1868 	err = mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_CAC_START, phy->band_idx,
1869 				      MT_RX_SEL0, 0);
1870 	if (err < 0)
1871 		return err;
1872 
1873 	err = mt7915_dfs_start_rdd(dev, phy->band_idx);
1874 	if (err < 0)
1875 		return err;
1876 
1877 	phy->rdd_state |= BIT(phy->band_idx);
1878 
1879 	if (!is_mt7915(&dev->mt76))
1880 		return 0;
1881 
1882 	if (chandef->width == NL80211_CHAN_WIDTH_160 ||
1883 	    chandef->width == NL80211_CHAN_WIDTH_80P80) {
1884 		err = mt7915_dfs_start_rdd(dev, 1);
1885 		if (err < 0)
1886 			return err;
1887 
1888 		phy->rdd_state |= BIT(1);
1889 	}
1890 
1891 	return 0;
1892 }
1893 
1894 static int
1895 mt7915_dfs_init_radar_specs(struct mt7915_phy *phy)
1896 {
1897 	const struct mt7915_dfs_radar_spec *radar_specs;
1898 	struct mt7915_dev *dev = phy->dev;
1899 	int err, i;
1900 
1901 	switch (dev->mt76.region) {
1902 	case NL80211_DFS_FCC:
1903 		radar_specs = &fcc_radar_specs;
1904 		err = mt7915_mcu_set_fcc5_lpn(dev, 8);
1905 		if (err < 0)
1906 			return err;
1907 		break;
1908 	case NL80211_DFS_ETSI:
1909 		radar_specs = &etsi_radar_specs;
1910 		break;
1911 	case NL80211_DFS_JP:
1912 		radar_specs = &jp_radar_specs;
1913 		break;
1914 	default:
1915 		return -EINVAL;
1916 	}
1917 
1918 	for (i = 0; i < ARRAY_SIZE(radar_specs->radar_pattern); i++) {
1919 		err = mt7915_mcu_set_radar_th(dev, i,
1920 					      &radar_specs->radar_pattern[i]);
1921 		if (err < 0)
1922 			return err;
1923 	}
1924 
1925 	return mt7915_mcu_set_pulse_th(dev, &radar_specs->pulse_th);
1926 }
1927 
1928 int mt7915_dfs_init_radar_detector(struct mt7915_phy *phy)
1929 {
1930 	struct mt7915_dev *dev = phy->dev;
1931 	enum mt76_dfs_state dfs_state, prev_state;
1932 	int err;
1933 
1934 	prev_state = phy->mt76->dfs_state;
1935 	dfs_state = mt76_phy_dfs_state(phy->mt76);
1936 
1937 	if (prev_state == dfs_state)
1938 		return 0;
1939 
1940 	if (prev_state == MT_DFS_STATE_UNKNOWN)
1941 		mt7915_dfs_stop_radar_detector(phy);
1942 
1943 	if (dfs_state == MT_DFS_STATE_DISABLED)
1944 		goto stop;
1945 
1946 	if (prev_state <= MT_DFS_STATE_DISABLED) {
1947 		err = mt7915_dfs_init_radar_specs(phy);
1948 		if (err < 0)
1949 			return err;
1950 
1951 		err = mt7915_dfs_start_radar_detector(phy);
1952 		if (err < 0)
1953 			return err;
1954 
1955 		phy->mt76->dfs_state = MT_DFS_STATE_CAC;
1956 	}
1957 
1958 	if (dfs_state == MT_DFS_STATE_CAC)
1959 		return 0;
1960 
1961 	err = mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_CAC_END,
1962 				      phy->band_idx, MT_RX_SEL0, 0);
1963 	if (err < 0) {
1964 		phy->mt76->dfs_state = MT_DFS_STATE_UNKNOWN;
1965 		return err;
1966 	}
1967 
1968 	phy->mt76->dfs_state = MT_DFS_STATE_ACTIVE;
1969 	return 0;
1970 
1971 stop:
1972 	err = mt76_connac_mcu_rdd_cmd(&dev->mt76, RDD_NORMAL_START,
1973 				      phy->band_idx, MT_RX_SEL0, 0);
1974 	if (err < 0)
1975 		return err;
1976 
1977 	mt7915_dfs_stop_radar_detector(phy);
1978 	phy->mt76->dfs_state = MT_DFS_STATE_DISABLED;
1979 
1980 	return 0;
1981 }
1982 
1983 static int
1984 mt7915_mac_twt_duration_align(int duration)
1985 {
1986 	return duration << 8;
1987 }
1988 
1989 static u64
1990 mt7915_mac_twt_sched_list_add(struct mt7915_dev *dev,
1991 			      struct mt7915_twt_flow *flow)
1992 {
1993 	struct mt7915_twt_flow *iter, *iter_next;
1994 	u32 duration = flow->duration << 8;
1995 	u64 start_tsf;
1996 
1997 	iter = list_first_entry_or_null(&dev->twt_list,
1998 					struct mt7915_twt_flow, list);
1999 	if (!iter || !iter->sched || iter->start_tsf > duration) {
2000 		/* add flow as first entry in the list */
2001 		list_add(&flow->list, &dev->twt_list);
2002 		return 0;
2003 	}
2004 
2005 	list_for_each_entry_safe(iter, iter_next, &dev->twt_list, list) {
2006 		start_tsf = iter->start_tsf +
2007 			    mt7915_mac_twt_duration_align(iter->duration);
2008 		if (list_is_last(&iter->list, &dev->twt_list))
2009 			break;
2010 
2011 		if (!iter_next->sched ||
2012 		    iter_next->start_tsf > start_tsf + duration) {
2013 			list_add(&flow->list, &iter->list);
2014 			goto out;
2015 		}
2016 	}
2017 
2018 	/* add flow as last entry in the list */
2019 	list_add_tail(&flow->list, &dev->twt_list);
2020 out:
2021 	return start_tsf;
2022 }
2023 
2024 static int mt7915_mac_check_twt_req(struct ieee80211_twt_setup *twt)
2025 {
2026 	struct ieee80211_twt_params *twt_agrt;
2027 	u64 interval, duration;
2028 	u16 mantissa;
2029 	u8 exp;
2030 
2031 	/* only individual agreement supported */
2032 	if (twt->control & IEEE80211_TWT_CONTROL_NEG_TYPE_BROADCAST)
2033 		return -EOPNOTSUPP;
2034 
2035 	/* only 256us unit supported */
2036 	if (twt->control & IEEE80211_TWT_CONTROL_WAKE_DUR_UNIT)
2037 		return -EOPNOTSUPP;
2038 
2039 	twt_agrt = (struct ieee80211_twt_params *)twt->params;
2040 
2041 	/* explicit agreement not supported */
2042 	if (!(twt_agrt->req_type & cpu_to_le16(IEEE80211_TWT_REQTYPE_IMPLICIT)))
2043 		return -EOPNOTSUPP;
2044 
2045 	exp = FIELD_GET(IEEE80211_TWT_REQTYPE_WAKE_INT_EXP,
2046 			le16_to_cpu(twt_agrt->req_type));
2047 	mantissa = le16_to_cpu(twt_agrt->mantissa);
2048 	duration = twt_agrt->min_twt_dur << 8;
2049 
2050 	interval = (u64)mantissa << exp;
2051 	if (interval < duration)
2052 		return -EOPNOTSUPP;
2053 
2054 	return 0;
2055 }
2056 
2057 static bool
2058 mt7915_mac_twt_param_equal(struct mt7915_sta *msta,
2059 			   struct ieee80211_twt_params *twt_agrt)
2060 {
2061 	u16 type = le16_to_cpu(twt_agrt->req_type);
2062 	u8 exp;
2063 	int i;
2064 
2065 	exp = FIELD_GET(IEEE80211_TWT_REQTYPE_WAKE_INT_EXP, type);
2066 	for (i = 0; i < MT7915_MAX_STA_TWT_AGRT; i++) {
2067 		struct mt7915_twt_flow *f;
2068 
2069 		if (!(msta->twt.flowid_mask & BIT(i)))
2070 			continue;
2071 
2072 		f = &msta->twt.flow[i];
2073 		if (f->duration == twt_agrt->min_twt_dur &&
2074 		    f->mantissa == twt_agrt->mantissa &&
2075 		    f->exp == exp &&
2076 		    f->protection == !!(type & IEEE80211_TWT_REQTYPE_PROTECTION) &&
2077 		    f->flowtype == !!(type & IEEE80211_TWT_REQTYPE_FLOWTYPE) &&
2078 		    f->trigger == !!(type & IEEE80211_TWT_REQTYPE_TRIGGER))
2079 			return true;
2080 	}
2081 
2082 	return false;
2083 }
2084 
2085 void mt7915_mac_add_twt_setup(struct ieee80211_hw *hw,
2086 			      struct ieee80211_sta *sta,
2087 			      struct ieee80211_twt_setup *twt)
2088 {
2089 	enum ieee80211_twt_setup_cmd setup_cmd = TWT_SETUP_CMD_REJECT;
2090 	struct mt7915_sta *msta = (struct mt7915_sta *)sta->drv_priv;
2091 	struct ieee80211_twt_params *twt_agrt = (void *)twt->params;
2092 	u16 req_type = le16_to_cpu(twt_agrt->req_type);
2093 	enum ieee80211_twt_setup_cmd sta_setup_cmd;
2094 	struct mt7915_dev *dev = mt7915_hw_dev(hw);
2095 	struct mt7915_twt_flow *flow;
2096 	int flowid, table_id;
2097 	u8 exp;
2098 
2099 	if (mt7915_mac_check_twt_req(twt))
2100 		goto out;
2101 
2102 	mutex_lock(&dev->mt76.mutex);
2103 
2104 	if (dev->twt.n_agrt == MT7915_MAX_TWT_AGRT)
2105 		goto unlock;
2106 
2107 	if (hweight8(msta->twt.flowid_mask) == ARRAY_SIZE(msta->twt.flow))
2108 		goto unlock;
2109 
2110 	if (twt_agrt->min_twt_dur < MT7915_MIN_TWT_DUR) {
2111 		setup_cmd = TWT_SETUP_CMD_DICTATE;
2112 		twt_agrt->min_twt_dur = MT7915_MIN_TWT_DUR;
2113 		goto unlock;
2114 	}
2115 
2116 	flowid = ffs(~msta->twt.flowid_mask) - 1;
2117 	twt_agrt->req_type &= ~cpu_to_le16(IEEE80211_TWT_REQTYPE_FLOWID);
2118 	twt_agrt->req_type |= le16_encode_bits(flowid,
2119 					       IEEE80211_TWT_REQTYPE_FLOWID);
2120 
2121 	table_id = ffs(~dev->twt.table_mask) - 1;
2122 	exp = FIELD_GET(IEEE80211_TWT_REQTYPE_WAKE_INT_EXP, req_type);
2123 	sta_setup_cmd = FIELD_GET(IEEE80211_TWT_REQTYPE_SETUP_CMD, req_type);
2124 
2125 	if (mt7915_mac_twt_param_equal(msta, twt_agrt))
2126 		goto unlock;
2127 
2128 	flow = &msta->twt.flow[flowid];
2129 	memset(flow, 0, sizeof(*flow));
2130 	INIT_LIST_HEAD(&flow->list);
2131 	flow->wcid = msta->wcid.idx;
2132 	flow->table_id = table_id;
2133 	flow->id = flowid;
2134 	flow->duration = twt_agrt->min_twt_dur;
2135 	flow->mantissa = twt_agrt->mantissa;
2136 	flow->exp = exp;
2137 	flow->protection = !!(req_type & IEEE80211_TWT_REQTYPE_PROTECTION);
2138 	flow->flowtype = !!(req_type & IEEE80211_TWT_REQTYPE_FLOWTYPE);
2139 	flow->trigger = !!(req_type & IEEE80211_TWT_REQTYPE_TRIGGER);
2140 
2141 	if (sta_setup_cmd == TWT_SETUP_CMD_REQUEST ||
2142 	    sta_setup_cmd == TWT_SETUP_CMD_SUGGEST) {
2143 		u64 interval = (u64)le16_to_cpu(twt_agrt->mantissa) << exp;
2144 		u64 flow_tsf, curr_tsf;
2145 		u32 rem;
2146 
2147 		flow->sched = true;
2148 		flow->start_tsf = mt7915_mac_twt_sched_list_add(dev, flow);
2149 		curr_tsf = __mt7915_get_tsf(hw, msta->vif);
2150 		div_u64_rem(curr_tsf - flow->start_tsf, interval, &rem);
2151 		flow_tsf = curr_tsf + interval - rem;
2152 		twt_agrt->twt = cpu_to_le64(flow_tsf);
2153 	} else {
2154 		list_add_tail(&flow->list, &dev->twt_list);
2155 	}
2156 	flow->tsf = le64_to_cpu(twt_agrt->twt);
2157 
2158 	if (mt7915_mcu_twt_agrt_update(dev, msta->vif, flow, MCU_TWT_AGRT_ADD))
2159 		goto unlock;
2160 
2161 	setup_cmd = TWT_SETUP_CMD_ACCEPT;
2162 	dev->twt.table_mask |= BIT(table_id);
2163 	msta->twt.flowid_mask |= BIT(flowid);
2164 	dev->twt.n_agrt++;
2165 
2166 unlock:
2167 	mutex_unlock(&dev->mt76.mutex);
2168 out:
2169 	twt_agrt->req_type &= ~cpu_to_le16(IEEE80211_TWT_REQTYPE_SETUP_CMD);
2170 	twt_agrt->req_type |=
2171 		le16_encode_bits(setup_cmd, IEEE80211_TWT_REQTYPE_SETUP_CMD);
2172 	twt->control = (twt->control & IEEE80211_TWT_CONTROL_WAKE_DUR_UNIT) |
2173 		       (twt->control & IEEE80211_TWT_CONTROL_RX_DISABLED);
2174 }
2175 
2176 void mt7915_mac_twt_teardown_flow(struct mt7915_dev *dev,
2177 				  struct mt7915_sta *msta,
2178 				  u8 flowid)
2179 {
2180 	struct mt7915_twt_flow *flow;
2181 
2182 	lockdep_assert_held(&dev->mt76.mutex);
2183 
2184 	if (flowid >= ARRAY_SIZE(msta->twt.flow))
2185 		return;
2186 
2187 	if (!(msta->twt.flowid_mask & BIT(flowid)))
2188 		return;
2189 
2190 	flow = &msta->twt.flow[flowid];
2191 	if (mt7915_mcu_twt_agrt_update(dev, msta->vif, flow,
2192 				       MCU_TWT_AGRT_DELETE))
2193 		return;
2194 
2195 	list_del_init(&flow->list);
2196 	msta->twt.flowid_mask &= ~BIT(flowid);
2197 	dev->twt.table_mask &= ~BIT(flow->table_id);
2198 	dev->twt.n_agrt--;
2199 }
2200