1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
2 /* Copyright(c) 2018-2019  Realtek Corporation
3  */
4 
5 #include "main.h"
6 #include "tx.h"
7 #include "fw.h"
8 #include "ps.h"
9 #include "debug.h"
10 
11 static
12 void rtw_tx_stats(struct rtw_dev *rtwdev, struct ieee80211_vif *vif,
13 		  struct sk_buff *skb)
14 {
15 	struct ieee80211_hdr *hdr;
16 	struct rtw_vif *rtwvif;
17 
18 	hdr = (struct ieee80211_hdr *)skb->data;
19 
20 	if (!ieee80211_is_data(hdr->frame_control))
21 		return;
22 
23 	if (!is_broadcast_ether_addr(hdr->addr1) &&
24 	    !is_multicast_ether_addr(hdr->addr1)) {
25 		rtwdev->stats.tx_unicast += skb->len;
26 		rtwdev->stats.tx_cnt++;
27 		if (vif) {
28 			rtwvif = (struct rtw_vif *)vif->drv_priv;
29 			rtwvif->stats.tx_unicast += skb->len;
30 			rtwvif->stats.tx_cnt++;
31 		}
32 	}
33 }
34 
35 void rtw_tx_fill_tx_desc(struct rtw_tx_pkt_info *pkt_info, struct sk_buff *skb)
36 {
37 	__le32 *txdesc = (__le32 *)skb->data;
38 
39 	SET_TX_DESC_TXPKTSIZE(txdesc,  pkt_info->tx_pkt_size);
40 	SET_TX_DESC_OFFSET(txdesc, pkt_info->offset);
41 	SET_TX_DESC_PKT_OFFSET(txdesc, pkt_info->pkt_offset);
42 	SET_TX_DESC_QSEL(txdesc, pkt_info->qsel);
43 	SET_TX_DESC_BMC(txdesc, pkt_info->bmc);
44 	SET_TX_DESC_RATE_ID(txdesc, pkt_info->rate_id);
45 	SET_TX_DESC_DATARATE(txdesc, pkt_info->rate);
46 	SET_TX_DESC_DISDATAFB(txdesc, pkt_info->dis_rate_fallback);
47 	SET_TX_DESC_USE_RATE(txdesc, pkt_info->use_rate);
48 	SET_TX_DESC_SEC_TYPE(txdesc, pkt_info->sec_type);
49 	SET_TX_DESC_DATA_BW(txdesc, pkt_info->bw);
50 	SET_TX_DESC_SW_SEQ(txdesc, pkt_info->seq);
51 	SET_TX_DESC_MAX_AGG_NUM(txdesc, pkt_info->ampdu_factor);
52 	SET_TX_DESC_AMPDU_DENSITY(txdesc, pkt_info->ampdu_density);
53 	SET_TX_DESC_DATA_STBC(txdesc, pkt_info->stbc);
54 	SET_TX_DESC_DATA_LDPC(txdesc, pkt_info->ldpc);
55 	SET_TX_DESC_AGG_EN(txdesc, pkt_info->ampdu_en);
56 	SET_TX_DESC_LS(txdesc, pkt_info->ls);
57 	SET_TX_DESC_DATA_SHORT(txdesc, pkt_info->short_gi);
58 	SET_TX_DESC_SPE_RPT(txdesc, pkt_info->report);
59 	SET_TX_DESC_SW_DEFINE(txdesc, pkt_info->sn);
60 	SET_TX_DESC_USE_RTS(txdesc, pkt_info->rts);
61 	if (pkt_info->rts) {
62 		SET_TX_DESC_RTSRATE(txdesc, DESC_RATE24M);
63 		SET_TX_DESC_DATA_RTS_SHORT(txdesc, 1);
64 	}
65 	SET_TX_DESC_DISQSELSEQ(txdesc, pkt_info->dis_qselseq);
66 	SET_TX_DESC_EN_HWSEQ(txdesc, pkt_info->en_hwseq);
67 	SET_TX_DESC_HW_SSN_SEL(txdesc, pkt_info->hw_ssn_sel);
68 	SET_TX_DESC_NAVUSEHDR(txdesc, pkt_info->nav_use_hdr);
69 	SET_TX_DESC_BT_NULL(txdesc, pkt_info->bt_null);
70 }
71 EXPORT_SYMBOL(rtw_tx_fill_tx_desc);
72 
73 static u8 get_tx_ampdu_factor(struct ieee80211_sta *sta)
74 {
75 	u8 exp = sta->ht_cap.ampdu_factor;
76 
77 	/* the least ampdu factor is 8K, and the value in the tx desc is the
78 	 * max aggregation num, which represents val * 2 packets can be
79 	 * aggregated in an AMPDU, so here we should use 8/2=4 as the base
80 	 */
81 	return (BIT(2) << exp) - 1;
82 }
83 
84 static u8 get_tx_ampdu_density(struct ieee80211_sta *sta)
85 {
86 	return sta->ht_cap.ampdu_density;
87 }
88 
89 static u8 get_highest_ht_tx_rate(struct rtw_dev *rtwdev,
90 				 struct ieee80211_sta *sta)
91 {
92 	u8 rate;
93 
94 	if (rtwdev->hal.rf_type == RF_2T2R && sta->ht_cap.mcs.rx_mask[1] != 0)
95 		rate = DESC_RATEMCS15;
96 	else
97 		rate = DESC_RATEMCS7;
98 
99 	return rate;
100 }
101 
102 static u8 get_highest_vht_tx_rate(struct rtw_dev *rtwdev,
103 				  struct ieee80211_sta *sta)
104 {
105 	struct rtw_efuse *efuse = &rtwdev->efuse;
106 	u8 rate;
107 	u16 tx_mcs_map;
108 
109 	tx_mcs_map = le16_to_cpu(sta->vht_cap.vht_mcs.tx_mcs_map);
110 	if (efuse->hw_cap.nss == 1) {
111 		switch (tx_mcs_map & 0x3) {
112 		case IEEE80211_VHT_MCS_SUPPORT_0_7:
113 			rate = DESC_RATEVHT1SS_MCS7;
114 			break;
115 		case IEEE80211_VHT_MCS_SUPPORT_0_8:
116 			rate = DESC_RATEVHT1SS_MCS8;
117 			break;
118 		default:
119 		case IEEE80211_VHT_MCS_SUPPORT_0_9:
120 			rate = DESC_RATEVHT1SS_MCS9;
121 			break;
122 		}
123 	} else if (efuse->hw_cap.nss >= 2) {
124 		switch ((tx_mcs_map & 0xc) >> 2) {
125 		case IEEE80211_VHT_MCS_SUPPORT_0_7:
126 			rate = DESC_RATEVHT2SS_MCS7;
127 			break;
128 		case IEEE80211_VHT_MCS_SUPPORT_0_8:
129 			rate = DESC_RATEVHT2SS_MCS8;
130 			break;
131 		default:
132 		case IEEE80211_VHT_MCS_SUPPORT_0_9:
133 			rate = DESC_RATEVHT2SS_MCS9;
134 			break;
135 		}
136 	} else {
137 		rate = DESC_RATEVHT1SS_MCS9;
138 	}
139 
140 	return rate;
141 }
142 
143 static void rtw_tx_report_enable(struct rtw_dev *rtwdev,
144 				 struct rtw_tx_pkt_info *pkt_info)
145 {
146 	struct rtw_tx_report *tx_report = &rtwdev->tx_report;
147 
148 	/* [11:8], reserved, fills with zero
149 	 * [7:2],  tx report sequence number
150 	 * [1:0],  firmware use, fills with zero
151 	 */
152 	pkt_info->sn = (atomic_inc_return(&tx_report->sn) << 2) & 0xfc;
153 	pkt_info->report = true;
154 }
155 
156 void rtw_tx_report_purge_timer(struct timer_list *t)
157 {
158 	struct rtw_dev *rtwdev = from_timer(rtwdev, t, tx_report.purge_timer);
159 	struct rtw_tx_report *tx_report = &rtwdev->tx_report;
160 	unsigned long flags;
161 
162 	if (skb_queue_len(&tx_report->queue) == 0)
163 		return;
164 
165 	rtw_warn(rtwdev, "failed to get tx report from firmware\n");
166 
167 	spin_lock_irqsave(&tx_report->q_lock, flags);
168 	skb_queue_purge(&tx_report->queue);
169 	spin_unlock_irqrestore(&tx_report->q_lock, flags);
170 }
171 
172 void rtw_tx_report_enqueue(struct rtw_dev *rtwdev, struct sk_buff *skb, u8 sn)
173 {
174 	struct rtw_tx_report *tx_report = &rtwdev->tx_report;
175 	unsigned long flags;
176 	u8 *drv_data;
177 
178 	/* pass sn to tx report handler through driver data */
179 	drv_data = (u8 *)IEEE80211_SKB_CB(skb)->status.status_driver_data;
180 	*drv_data = sn;
181 
182 	spin_lock_irqsave(&tx_report->q_lock, flags);
183 	__skb_queue_tail(&tx_report->queue, skb);
184 	spin_unlock_irqrestore(&tx_report->q_lock, flags);
185 
186 	mod_timer(&tx_report->purge_timer, jiffies + RTW_TX_PROBE_TIMEOUT);
187 }
188 EXPORT_SYMBOL(rtw_tx_report_enqueue);
189 
190 static void rtw_tx_report_tx_status(struct rtw_dev *rtwdev,
191 				    struct sk_buff *skb, bool acked)
192 {
193 	struct ieee80211_tx_info *info;
194 
195 	info = IEEE80211_SKB_CB(skb);
196 	ieee80211_tx_info_clear_status(info);
197 	if (acked)
198 		info->flags |= IEEE80211_TX_STAT_ACK;
199 	else
200 		info->flags &= ~IEEE80211_TX_STAT_ACK;
201 
202 	ieee80211_tx_status_irqsafe(rtwdev->hw, skb);
203 }
204 
205 void rtw_tx_report_handle(struct rtw_dev *rtwdev, struct sk_buff *skb, int src)
206 {
207 	struct rtw_tx_report *tx_report = &rtwdev->tx_report;
208 	struct rtw_c2h_cmd *c2h;
209 	struct sk_buff *cur, *tmp;
210 	unsigned long flags;
211 	u8 sn, st;
212 	u8 *n;
213 
214 	c2h = get_c2h_from_skb(skb);
215 
216 	if (src == C2H_CCX_TX_RPT) {
217 		sn = GET_CCX_REPORT_SEQNUM_V0(c2h->payload);
218 		st = GET_CCX_REPORT_STATUS_V0(c2h->payload);
219 	} else {
220 		sn = GET_CCX_REPORT_SEQNUM_V1(c2h->payload);
221 		st = GET_CCX_REPORT_STATUS_V1(c2h->payload);
222 	}
223 
224 	spin_lock_irqsave(&tx_report->q_lock, flags);
225 	skb_queue_walk_safe(&tx_report->queue, cur, tmp) {
226 		n = (u8 *)IEEE80211_SKB_CB(cur)->status.status_driver_data;
227 		if (*n == sn) {
228 			__skb_unlink(cur, &tx_report->queue);
229 			rtw_tx_report_tx_status(rtwdev, cur, st == 0);
230 			break;
231 		}
232 	}
233 	spin_unlock_irqrestore(&tx_report->q_lock, flags);
234 }
235 
236 static u8 rtw_get_mgmt_rate(struct rtw_dev *rtwdev, struct sk_buff *skb,
237 			    u8 lowest_rate, bool ignore_rate)
238 {
239 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
240 	struct ieee80211_vif *vif = tx_info->control.vif;
241 	bool force_lowest = test_bit(RTW_FLAG_FORCE_LOWEST_RATE, rtwdev->flags);
242 
243 	if (!vif || !vif->bss_conf.basic_rates || ignore_rate || force_lowest)
244 		return lowest_rate;
245 
246 	return __ffs(vif->bss_conf.basic_rates) + lowest_rate;
247 }
248 
249 static void rtw_tx_pkt_info_update_rate(struct rtw_dev *rtwdev,
250 					struct rtw_tx_pkt_info *pkt_info,
251 					struct sk_buff *skb,
252 					bool ignore_rate)
253 {
254 	if (rtwdev->hal.current_band_type == RTW_BAND_2G) {
255 		pkt_info->rate_id = RTW_RATEID_B_20M;
256 		pkt_info->rate = rtw_get_mgmt_rate(rtwdev, skb, DESC_RATE1M,
257 						   ignore_rate);
258 	} else {
259 		pkt_info->rate_id = RTW_RATEID_G;
260 		pkt_info->rate = rtw_get_mgmt_rate(rtwdev, skb, DESC_RATE6M,
261 						   ignore_rate);
262 	}
263 
264 	pkt_info->use_rate = true;
265 	pkt_info->dis_rate_fallback = true;
266 }
267 
268 static void rtw_tx_pkt_info_update_sec(struct rtw_dev *rtwdev,
269 				       struct rtw_tx_pkt_info *pkt_info,
270 				       struct sk_buff *skb)
271 {
272 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
273 	u8 sec_type = 0;
274 
275 	if (info && info->control.hw_key) {
276 		struct ieee80211_key_conf *key = info->control.hw_key;
277 
278 		switch (key->cipher) {
279 		case WLAN_CIPHER_SUITE_WEP40:
280 		case WLAN_CIPHER_SUITE_WEP104:
281 		case WLAN_CIPHER_SUITE_TKIP:
282 			sec_type = 0x01;
283 			break;
284 		case WLAN_CIPHER_SUITE_CCMP:
285 			sec_type = 0x03;
286 			break;
287 		default:
288 			break;
289 		}
290 	}
291 
292 	pkt_info->sec_type = sec_type;
293 }
294 
295 static void rtw_tx_mgmt_pkt_info_update(struct rtw_dev *rtwdev,
296 					struct rtw_tx_pkt_info *pkt_info,
297 					struct ieee80211_sta *sta,
298 					struct sk_buff *skb)
299 {
300 	rtw_tx_pkt_info_update_rate(rtwdev, pkt_info, skb, false);
301 	pkt_info->dis_qselseq = true;
302 	pkt_info->en_hwseq = true;
303 	pkt_info->hw_ssn_sel = 0;
304 	/* TODO: need to change hw port and hw ssn sel for multiple vifs */
305 }
306 
307 static void rtw_tx_data_pkt_info_update(struct rtw_dev *rtwdev,
308 					struct rtw_tx_pkt_info *pkt_info,
309 					struct ieee80211_sta *sta,
310 					struct sk_buff *skb)
311 {
312 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
313 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
314 	struct ieee80211_hw *hw = rtwdev->hw;
315 	struct rtw_dm_info *dm_info = &rtwdev->dm_info;
316 	struct rtw_sta_info *si;
317 	u8 fix_rate;
318 	u16 seq;
319 	u8 ampdu_factor = 0;
320 	u8 ampdu_density = 0;
321 	bool ampdu_en = false;
322 	u8 rate = DESC_RATE6M;
323 	u8 rate_id = 6;
324 	u8 bw = RTW_CHANNEL_WIDTH_20;
325 	bool stbc = false;
326 	bool ldpc = false;
327 
328 	seq = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
329 
330 	/* for broadcast/multicast, use default values */
331 	if (!sta)
332 		goto out;
333 
334 	if (info->flags & IEEE80211_TX_CTL_AMPDU) {
335 		ampdu_en = true;
336 		ampdu_factor = get_tx_ampdu_factor(sta);
337 		ampdu_density = get_tx_ampdu_density(sta);
338 	}
339 
340 	if (info->control.use_rts || skb->len > hw->wiphy->rts_threshold)
341 		pkt_info->rts = true;
342 
343 	if (sta->vht_cap.vht_supported)
344 		rate = get_highest_vht_tx_rate(rtwdev, sta);
345 	else if (sta->ht_cap.ht_supported)
346 		rate = get_highest_ht_tx_rate(rtwdev, sta);
347 	else if (sta->supp_rates[0] <= 0xf)
348 		rate = DESC_RATE11M;
349 	else
350 		rate = DESC_RATE54M;
351 
352 	si = (struct rtw_sta_info *)sta->drv_priv;
353 
354 	bw = si->bw_mode;
355 	rate_id = si->rate_id;
356 	stbc = si->stbc_en;
357 	ldpc = si->ldpc_en;
358 
359 out:
360 	pkt_info->seq = seq;
361 	pkt_info->ampdu_factor = ampdu_factor;
362 	pkt_info->ampdu_density = ampdu_density;
363 	pkt_info->ampdu_en = ampdu_en;
364 	pkt_info->rate = rate;
365 	pkt_info->rate_id = rate_id;
366 	pkt_info->bw = bw;
367 	pkt_info->stbc = stbc;
368 	pkt_info->ldpc = ldpc;
369 
370 	fix_rate = dm_info->fix_rate;
371 	if (fix_rate < DESC_RATE_MAX) {
372 		pkt_info->rate = fix_rate;
373 		pkt_info->dis_rate_fallback = true;
374 		pkt_info->use_rate = true;
375 	}
376 }
377 
378 void rtw_tx_pkt_info_update(struct rtw_dev *rtwdev,
379 			    struct rtw_tx_pkt_info *pkt_info,
380 			    struct ieee80211_sta *sta,
381 			    struct sk_buff *skb)
382 {
383 	struct rtw_chip_info *chip = rtwdev->chip;
384 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
385 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
386 	struct rtw_sta_info *si;
387 	struct ieee80211_vif *vif = NULL;
388 	__le16 fc = hdr->frame_control;
389 	bool bmc;
390 
391 	if (sta) {
392 		si = (struct rtw_sta_info *)sta->drv_priv;
393 		vif = si->vif;
394 	}
395 
396 	if (ieee80211_is_mgmt(fc) || ieee80211_is_nullfunc(fc))
397 		rtw_tx_mgmt_pkt_info_update(rtwdev, pkt_info, sta, skb);
398 	else if (ieee80211_is_data(fc))
399 		rtw_tx_data_pkt_info_update(rtwdev, pkt_info, sta, skb);
400 
401 	bmc = is_broadcast_ether_addr(hdr->addr1) ||
402 	      is_multicast_ether_addr(hdr->addr1);
403 
404 	if (info->flags & IEEE80211_TX_CTL_REQ_TX_STATUS)
405 		rtw_tx_report_enable(rtwdev, pkt_info);
406 
407 	pkt_info->bmc = bmc;
408 	rtw_tx_pkt_info_update_sec(rtwdev, pkt_info, skb);
409 	pkt_info->tx_pkt_size = skb->len;
410 	pkt_info->offset = chip->tx_pkt_desc_sz;
411 	pkt_info->qsel = skb->priority;
412 	pkt_info->ls = true;
413 
414 	/* maybe merge with tx status ? */
415 	rtw_tx_stats(rtwdev, vif, skb);
416 }
417 
418 void rtw_tx_rsvd_page_pkt_info_update(struct rtw_dev *rtwdev,
419 				      struct rtw_tx_pkt_info *pkt_info,
420 				      struct sk_buff *skb,
421 				      enum rtw_rsvd_packet_type type)
422 {
423 	struct rtw_chip_info *chip = rtwdev->chip;
424 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
425 	bool bmc;
426 
427 	/* A beacon or dummy reserved page packet indicates that it is the first
428 	 * reserved page, and the qsel of it will be set in each hci.
429 	 */
430 	if (type != RSVD_BEACON && type != RSVD_DUMMY)
431 		pkt_info->qsel = TX_DESC_QSEL_MGMT;
432 
433 	rtw_tx_pkt_info_update_rate(rtwdev, pkt_info, skb, true);
434 
435 	bmc = is_broadcast_ether_addr(hdr->addr1) ||
436 	      is_multicast_ether_addr(hdr->addr1);
437 	pkt_info->bmc = bmc;
438 	pkt_info->tx_pkt_size = skb->len;
439 	pkt_info->offset = chip->tx_pkt_desc_sz;
440 	pkt_info->ls = true;
441 	if (type == RSVD_PS_POLL) {
442 		pkt_info->nav_use_hdr = true;
443 	} else {
444 		pkt_info->dis_qselseq = true;
445 		pkt_info->en_hwseq = true;
446 		pkt_info->hw_ssn_sel = 0;
447 	}
448 	if (type == RSVD_QOS_NULL)
449 		pkt_info->bt_null = true;
450 
451 	rtw_tx_pkt_info_update_sec(rtwdev, pkt_info, skb);
452 
453 	/* TODO: need to change hw port and hw ssn sel for multiple vifs */
454 }
455 
456 struct sk_buff *
457 rtw_tx_write_data_rsvd_page_get(struct rtw_dev *rtwdev,
458 				struct rtw_tx_pkt_info *pkt_info,
459 				u8 *buf, u32 size)
460 {
461 	struct rtw_chip_info *chip = rtwdev->chip;
462 	struct sk_buff *skb;
463 	u32 tx_pkt_desc_sz;
464 	u32 length;
465 
466 	tx_pkt_desc_sz = chip->tx_pkt_desc_sz;
467 	length = size + tx_pkt_desc_sz;
468 	skb = dev_alloc_skb(length);
469 	if (!skb) {
470 		rtw_err(rtwdev, "failed to alloc write data rsvd page skb\n");
471 		return NULL;
472 	}
473 
474 	skb_reserve(skb, tx_pkt_desc_sz);
475 	skb_put_data(skb, buf, size);
476 	rtw_tx_rsvd_page_pkt_info_update(rtwdev, pkt_info, skb, RSVD_BEACON);
477 
478 	return skb;
479 }
480 EXPORT_SYMBOL(rtw_tx_write_data_rsvd_page_get);
481 
482 struct sk_buff *
483 rtw_tx_write_data_h2c_get(struct rtw_dev *rtwdev,
484 			  struct rtw_tx_pkt_info *pkt_info,
485 			  u8 *buf, u32 size)
486 {
487 	struct rtw_chip_info *chip = rtwdev->chip;
488 	struct sk_buff *skb;
489 	u32 tx_pkt_desc_sz;
490 	u32 length;
491 
492 	tx_pkt_desc_sz = chip->tx_pkt_desc_sz;
493 	length = size + tx_pkt_desc_sz;
494 	skb = dev_alloc_skb(length);
495 	if (!skb) {
496 		rtw_err(rtwdev, "failed to alloc write data h2c skb\n");
497 		return NULL;
498 	}
499 
500 	skb_reserve(skb, tx_pkt_desc_sz);
501 	skb_put_data(skb, buf, size);
502 	pkt_info->tx_pkt_size = size;
503 
504 	return skb;
505 }
506 EXPORT_SYMBOL(rtw_tx_write_data_h2c_get);
507 
508 void rtw_tx(struct rtw_dev *rtwdev,
509 	    struct ieee80211_tx_control *control,
510 	    struct sk_buff *skb)
511 {
512 	struct rtw_tx_pkt_info pkt_info = {0};
513 	int ret;
514 
515 	rtw_tx_pkt_info_update(rtwdev, &pkt_info, control->sta, skb);
516 	ret = rtw_hci_tx_write(rtwdev, &pkt_info, skb);
517 	if (ret) {
518 		rtw_err(rtwdev, "failed to write TX skb to HCI\n");
519 		goto out;
520 	}
521 
522 	rtw_hci_tx_kick_off(rtwdev);
523 
524 	return;
525 
526 out:
527 	ieee80211_free_txskb(rtwdev->hw, skb);
528 }
529 
530 static void rtw_txq_check_agg(struct rtw_dev *rtwdev,
531 			      struct rtw_txq *rtwtxq,
532 			      struct sk_buff *skb)
533 {
534 	struct ieee80211_txq *txq = rtwtxq_to_txq(rtwtxq);
535 	struct ieee80211_tx_info *info;
536 	struct rtw_sta_info *si;
537 
538 	if (test_bit(RTW_TXQ_AMPDU, &rtwtxq->flags)) {
539 		info = IEEE80211_SKB_CB(skb);
540 		info->flags |= IEEE80211_TX_CTL_AMPDU;
541 		return;
542 	}
543 
544 	if (skb_get_queue_mapping(skb) == IEEE80211_AC_VO)
545 		return;
546 
547 	if (test_bit(RTW_TXQ_BLOCK_BA, &rtwtxq->flags))
548 		return;
549 
550 	if (unlikely(skb->protocol == cpu_to_be16(ETH_P_PAE)))
551 		return;
552 
553 	if (!txq->sta)
554 		return;
555 
556 	si = (struct rtw_sta_info *)txq->sta->drv_priv;
557 	set_bit(txq->tid, si->tid_ba);
558 
559 	ieee80211_queue_work(rtwdev->hw, &rtwdev->ba_work);
560 }
561 
562 static int rtw_txq_push_skb(struct rtw_dev *rtwdev,
563 			    struct rtw_txq *rtwtxq,
564 			    struct sk_buff *skb)
565 {
566 	struct ieee80211_txq *txq = rtwtxq_to_txq(rtwtxq);
567 	struct rtw_tx_pkt_info pkt_info = {0};
568 	int ret;
569 
570 	rtw_txq_check_agg(rtwdev, rtwtxq, skb);
571 
572 	rtw_tx_pkt_info_update(rtwdev, &pkt_info, txq->sta, skb);
573 	ret = rtw_hci_tx_write(rtwdev, &pkt_info, skb);
574 	if (ret) {
575 		rtw_err(rtwdev, "failed to write TX skb to HCI\n");
576 		return ret;
577 	}
578 	rtwtxq->last_push = jiffies;
579 
580 	return 0;
581 }
582 
583 static struct sk_buff *rtw_txq_dequeue(struct rtw_dev *rtwdev,
584 				       struct rtw_txq *rtwtxq)
585 {
586 	struct ieee80211_txq *txq = rtwtxq_to_txq(rtwtxq);
587 	struct sk_buff *skb;
588 
589 	skb = ieee80211_tx_dequeue(rtwdev->hw, txq);
590 	if (!skb)
591 		return NULL;
592 
593 	return skb;
594 }
595 
596 static void rtw_txq_push(struct rtw_dev *rtwdev,
597 			 struct rtw_txq *rtwtxq,
598 			 unsigned long frames)
599 {
600 	struct sk_buff *skb;
601 	int ret;
602 	int i;
603 
604 	rcu_read_lock();
605 
606 	for (i = 0; i < frames; i++) {
607 		skb = rtw_txq_dequeue(rtwdev, rtwtxq);
608 		if (!skb)
609 			break;
610 
611 		ret = rtw_txq_push_skb(rtwdev, rtwtxq, skb);
612 		if (ret) {
613 			rtw_err(rtwdev, "failed to pusk skb, ret %d\n", ret);
614 			break;
615 		}
616 	}
617 
618 	rcu_read_unlock();
619 }
620 
621 void rtw_tx_work(struct work_struct *w)
622 {
623 	struct rtw_dev *rtwdev = container_of(w, struct rtw_dev, tx_work);
624 	struct rtw_txq *rtwtxq, *tmp;
625 
626 	spin_lock_bh(&rtwdev->txq_lock);
627 
628 	list_for_each_entry_safe(rtwtxq, tmp, &rtwdev->txqs, list) {
629 		struct ieee80211_txq *txq = rtwtxq_to_txq(rtwtxq);
630 		unsigned long frame_cnt;
631 		unsigned long byte_cnt;
632 
633 		ieee80211_txq_get_depth(txq, &frame_cnt, &byte_cnt);
634 		rtw_txq_push(rtwdev, rtwtxq, frame_cnt);
635 
636 		list_del_init(&rtwtxq->list);
637 	}
638 
639 	rtw_hci_tx_kick_off(rtwdev);
640 
641 	spin_unlock_bh(&rtwdev->txq_lock);
642 }
643 
644 void rtw_txq_init(struct rtw_dev *rtwdev, struct ieee80211_txq *txq)
645 {
646 	struct rtw_txq *rtwtxq;
647 
648 	if (!txq)
649 		return;
650 
651 	rtwtxq = (struct rtw_txq *)txq->drv_priv;
652 	INIT_LIST_HEAD(&rtwtxq->list);
653 }
654 
655 void rtw_txq_cleanup(struct rtw_dev *rtwdev, struct ieee80211_txq *txq)
656 {
657 	struct rtw_txq *rtwtxq;
658 
659 	if (!txq)
660 		return;
661 
662 	rtwtxq = (struct rtw_txq *)txq->drv_priv;
663 	spin_lock_bh(&rtwdev->txq_lock);
664 	if (!list_empty(&rtwtxq->list))
665 		list_del_init(&rtwtxq->list);
666 	spin_unlock_bh(&rtwdev->txq_lock);
667 }
668