drivers/net/wireless/realtek/rtw88/tx.c

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