linux/drivers/net/wireless/realtek/rtw88/tx.c
<<
>>
Prefs
   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
  11static
  12void 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
  35void 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}
  71EXPORT_SYMBOL(rtw_tx_fill_tx_desc);
  72
  73static 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
  84static u8 get_tx_ampdu_density(struct ieee80211_sta *sta)
  85{
  86        return sta->ht_cap.ampdu_density;
  87}
  88
  89static 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
 102static 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
 143static 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
 156void 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
 172void 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}
 188EXPORT_SYMBOL(rtw_tx_report_enqueue);
 189
 190static 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
 205void 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
 236static void rtw_tx_pkt_info_update_rate(struct rtw_dev *rtwdev,
 237                                        struct rtw_tx_pkt_info *pkt_info,
 238                                        struct sk_buff *skb)
 239{
 240        if (rtwdev->hal.current_band_type == RTW_BAND_2G) {
 241                pkt_info->rate_id = RTW_RATEID_B_20M;
 242                pkt_info->rate = DESC_RATE1M;
 243        } else {
 244                pkt_info->rate_id = RTW_RATEID_G;
 245                pkt_info->rate = DESC_RATE6M;
 246        }
 247        pkt_info->use_rate = true;
 248        pkt_info->dis_rate_fallback = true;
 249}
 250
 251static void rtw_tx_pkt_info_update_sec(struct rtw_dev *rtwdev,
 252                                       struct rtw_tx_pkt_info *pkt_info,
 253                                       struct sk_buff *skb)
 254{
 255        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 256        u8 sec_type = 0;
 257
 258        if (info && info->control.hw_key) {
 259                struct ieee80211_key_conf *key = info->control.hw_key;
 260
 261                switch (key->cipher) {
 262                case WLAN_CIPHER_SUITE_WEP40:
 263                case WLAN_CIPHER_SUITE_WEP104:
 264                case WLAN_CIPHER_SUITE_TKIP:
 265                        sec_type = 0x01;
 266                        break;
 267                case WLAN_CIPHER_SUITE_CCMP:
 268                        sec_type = 0x03;
 269                        break;
 270                default:
 271                        break;
 272                }
 273        }
 274
 275        pkt_info->sec_type = sec_type;
 276}
 277
 278static void rtw_tx_mgmt_pkt_info_update(struct rtw_dev *rtwdev,
 279                                        struct rtw_tx_pkt_info *pkt_info,
 280                                        struct ieee80211_sta *sta,
 281                                        struct sk_buff *skb)
 282{
 283        rtw_tx_pkt_info_update_rate(rtwdev, pkt_info, skb);
 284        pkt_info->dis_qselseq = true;
 285        pkt_info->en_hwseq = true;
 286        pkt_info->hw_ssn_sel = 0;
 287        /* TODO: need to change hw port and hw ssn sel for multiple vifs */
 288}
 289
 290static void rtw_tx_data_pkt_info_update(struct rtw_dev *rtwdev,
 291                                        struct rtw_tx_pkt_info *pkt_info,
 292                                        struct ieee80211_sta *sta,
 293                                        struct sk_buff *skb)
 294{
 295        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 296        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 297        struct ieee80211_hw *hw = rtwdev->hw;
 298        struct rtw_sta_info *si;
 299        u16 seq;
 300        u8 ampdu_factor = 0;
 301        u8 ampdu_density = 0;
 302        bool ampdu_en = false;
 303        u8 rate = DESC_RATE6M;
 304        u8 rate_id = 6;
 305        u8 bw = RTW_CHANNEL_WIDTH_20;
 306        bool stbc = false;
 307        bool ldpc = false;
 308
 309        seq = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
 310
 311        /* for broadcast/multicast, use default values */
 312        if (!sta)
 313                goto out;
 314
 315        if (info->flags & IEEE80211_TX_CTL_AMPDU) {
 316                ampdu_en = true;
 317                ampdu_factor = get_tx_ampdu_factor(sta);
 318                ampdu_density = get_tx_ampdu_density(sta);
 319        }
 320
 321        if (info->control.use_rts || skb->len > hw->wiphy->rts_threshold)
 322                pkt_info->rts = true;
 323
 324        if (sta->vht_cap.vht_supported)
 325                rate = get_highest_vht_tx_rate(rtwdev, sta);
 326        else if (sta->ht_cap.ht_supported)
 327                rate = get_highest_ht_tx_rate(rtwdev, sta);
 328        else if (sta->supp_rates[0] <= 0xf)
 329                rate = DESC_RATE11M;
 330        else
 331                rate = DESC_RATE54M;
 332
 333        si = (struct rtw_sta_info *)sta->drv_priv;
 334
 335        bw = si->bw_mode;
 336        rate_id = si->rate_id;
 337        stbc = si->stbc_en;
 338        ldpc = si->ldpc_en;
 339
 340out:
 341        pkt_info->seq = seq;
 342        pkt_info->ampdu_factor = ampdu_factor;
 343        pkt_info->ampdu_density = ampdu_density;
 344        pkt_info->ampdu_en = ampdu_en;
 345        pkt_info->rate = rate;
 346        pkt_info->rate_id = rate_id;
 347        pkt_info->bw = bw;
 348        pkt_info->stbc = stbc;
 349        pkt_info->ldpc = ldpc;
 350}
 351
 352void rtw_tx_pkt_info_update(struct rtw_dev *rtwdev,
 353                            struct rtw_tx_pkt_info *pkt_info,
 354                            struct ieee80211_sta *sta,
 355                            struct sk_buff *skb)
 356{
 357        struct rtw_chip_info *chip = rtwdev->chip;
 358        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 359        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 360        struct rtw_sta_info *si;
 361        struct ieee80211_vif *vif = NULL;
 362        __le16 fc = hdr->frame_control;
 363        bool bmc;
 364
 365        if (sta) {
 366                si = (struct rtw_sta_info *)sta->drv_priv;
 367                vif = si->vif;
 368        }
 369
 370        if (ieee80211_is_mgmt(fc) || ieee80211_is_nullfunc(fc))
 371                rtw_tx_mgmt_pkt_info_update(rtwdev, pkt_info, sta, skb);
 372        else if (ieee80211_is_data(fc))
 373                rtw_tx_data_pkt_info_update(rtwdev, pkt_info, sta, skb);
 374
 375        bmc = is_broadcast_ether_addr(hdr->addr1) ||
 376              is_multicast_ether_addr(hdr->addr1);
 377
 378        if (info->flags & IEEE80211_TX_CTL_REQ_TX_STATUS)
 379                rtw_tx_report_enable(rtwdev, pkt_info);
 380
 381        pkt_info->bmc = bmc;
 382        rtw_tx_pkt_info_update_sec(rtwdev, pkt_info, skb);
 383        pkt_info->tx_pkt_size = skb->len;
 384        pkt_info->offset = chip->tx_pkt_desc_sz;
 385        pkt_info->qsel = skb->priority;
 386        pkt_info->ls = true;
 387
 388        /* maybe merge with tx status ? */
 389        rtw_tx_stats(rtwdev, vif, skb);
 390}
 391
 392void rtw_tx_rsvd_page_pkt_info_update(struct rtw_dev *rtwdev,
 393                                      struct rtw_tx_pkt_info *pkt_info,
 394                                      struct sk_buff *skb,
 395                                      enum rtw_rsvd_packet_type type)
 396{
 397        struct rtw_chip_info *chip = rtwdev->chip;
 398        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 399        bool bmc;
 400
 401        /* A beacon or dummy reserved page packet indicates that it is the first
 402         * reserved page, and the qsel of it will be set in each hci.
 403         */
 404        if (type != RSVD_BEACON && type != RSVD_DUMMY)
 405                pkt_info->qsel = TX_DESC_QSEL_MGMT;
 406
 407        rtw_tx_pkt_info_update_rate(rtwdev, pkt_info, skb);
 408
 409        bmc = is_broadcast_ether_addr(hdr->addr1) ||
 410              is_multicast_ether_addr(hdr->addr1);
 411        pkt_info->bmc = bmc;
 412        pkt_info->tx_pkt_size = skb->len;
 413        pkt_info->offset = chip->tx_pkt_desc_sz;
 414        pkt_info->ls = true;
 415        if (type == RSVD_PS_POLL) {
 416                pkt_info->nav_use_hdr = true;
 417        } else {
 418                pkt_info->dis_qselseq = true;
 419                pkt_info->en_hwseq = true;
 420                pkt_info->hw_ssn_sel = 0;
 421        }
 422        if (type == RSVD_QOS_NULL)
 423                pkt_info->bt_null = true;
 424
 425        rtw_tx_pkt_info_update_sec(rtwdev, pkt_info, skb);
 426
 427        /* TODO: need to change hw port and hw ssn sel for multiple vifs */
 428}
 429
 430struct sk_buff *
 431rtw_tx_write_data_rsvd_page_get(struct rtw_dev *rtwdev,
 432                                struct rtw_tx_pkt_info *pkt_info,
 433                                u8 *buf, u32 size)
 434{
 435        struct rtw_chip_info *chip = rtwdev->chip;
 436        struct sk_buff *skb;
 437        u32 tx_pkt_desc_sz;
 438        u32 length;
 439
 440        tx_pkt_desc_sz = chip->tx_pkt_desc_sz;
 441        length = size + tx_pkt_desc_sz;
 442        skb = dev_alloc_skb(length);
 443        if (!skb) {
 444                rtw_err(rtwdev, "failed to alloc write data rsvd page skb\n");
 445                return NULL;
 446        }
 447
 448        skb_reserve(skb, tx_pkt_desc_sz);
 449        skb_put_data(skb, buf, size);
 450        rtw_tx_rsvd_page_pkt_info_update(rtwdev, pkt_info, skb, RSVD_BEACON);
 451
 452        return skb;
 453}
 454EXPORT_SYMBOL(rtw_tx_write_data_rsvd_page_get);
 455
 456struct sk_buff *
 457rtw_tx_write_data_h2c_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 h2c skb\n");
 471                return NULL;
 472        }
 473
 474        skb_reserve(skb, tx_pkt_desc_sz);
 475        skb_put_data(skb, buf, size);
 476        pkt_info->tx_pkt_size = size;
 477
 478        return skb;
 479}
 480EXPORT_SYMBOL(rtw_tx_write_data_h2c_get);
 481
 482void rtw_tx(struct rtw_dev *rtwdev,
 483            struct ieee80211_tx_control *control,
 484            struct sk_buff *skb)
 485{
 486        struct rtw_tx_pkt_info pkt_info = {0};
 487        int ret;
 488
 489        rtw_tx_pkt_info_update(rtwdev, &pkt_info, control->sta, skb);
 490        ret = rtw_hci_tx_write(rtwdev, &pkt_info, skb);
 491        if (ret) {
 492                rtw_err(rtwdev, "failed to write TX skb to HCI\n");
 493                goto out;
 494        }
 495
 496        rtw_hci_tx_kick_off(rtwdev);
 497
 498        return;
 499
 500out:
 501        ieee80211_free_txskb(rtwdev->hw, skb);
 502}
 503
 504static void rtw_txq_check_agg(struct rtw_dev *rtwdev,
 505                              struct rtw_txq *rtwtxq,
 506                              struct sk_buff *skb)
 507{
 508        struct ieee80211_txq *txq = rtwtxq_to_txq(rtwtxq);
 509        struct ieee80211_tx_info *info;
 510        struct rtw_sta_info *si;
 511
 512        if (test_bit(RTW_TXQ_AMPDU, &rtwtxq->flags)) {
 513                info = IEEE80211_SKB_CB(skb);
 514                info->flags |= IEEE80211_TX_CTL_AMPDU;
 515                return;
 516        }
 517
 518        if (skb_get_queue_mapping(skb) == IEEE80211_AC_VO)
 519                return;
 520
 521        if (test_bit(RTW_TXQ_BLOCK_BA, &rtwtxq->flags))
 522                return;
 523
 524        if (unlikely(skb->protocol == cpu_to_be16(ETH_P_PAE)))
 525                return;
 526
 527        if (!txq->sta)
 528                return;
 529
 530        si = (struct rtw_sta_info *)txq->sta->drv_priv;
 531        set_bit(txq->tid, si->tid_ba);
 532
 533        ieee80211_queue_work(rtwdev->hw, &rtwdev->ba_work);
 534}
 535
 536static int rtw_txq_push_skb(struct rtw_dev *rtwdev,
 537                            struct rtw_txq *rtwtxq,
 538                            struct sk_buff *skb)
 539{
 540        struct ieee80211_txq *txq = rtwtxq_to_txq(rtwtxq);
 541        struct rtw_tx_pkt_info pkt_info = {0};
 542        int ret;
 543
 544        rtw_txq_check_agg(rtwdev, rtwtxq, skb);
 545
 546        rtw_tx_pkt_info_update(rtwdev, &pkt_info, txq->sta, skb);
 547        ret = rtw_hci_tx_write(rtwdev, &pkt_info, skb);
 548        if (ret) {
 549                rtw_err(rtwdev, "failed to write TX skb to HCI\n");
 550                return ret;
 551        }
 552        rtwtxq->last_push = jiffies;
 553
 554        return 0;
 555}
 556
 557static struct sk_buff *rtw_txq_dequeue(struct rtw_dev *rtwdev,
 558                                       struct rtw_txq *rtwtxq)
 559{
 560        struct ieee80211_txq *txq = rtwtxq_to_txq(rtwtxq);
 561        struct sk_buff *skb;
 562
 563        skb = ieee80211_tx_dequeue(rtwdev->hw, txq);
 564        if (!skb)
 565                return NULL;
 566
 567        return skb;
 568}
 569
 570static void rtw_txq_push(struct rtw_dev *rtwdev,
 571                         struct rtw_txq *rtwtxq,
 572                         unsigned long frames)
 573{
 574        struct sk_buff *skb;
 575        int ret;
 576        int i;
 577
 578        rcu_read_lock();
 579
 580        for (i = 0; i < frames; i++) {
 581                skb = rtw_txq_dequeue(rtwdev, rtwtxq);
 582                if (!skb)
 583                        break;
 584
 585                ret = rtw_txq_push_skb(rtwdev, rtwtxq, skb);
 586                if (ret) {
 587                        rtw_err(rtwdev, "failed to pusk skb, ret %d\n", ret);
 588                        break;
 589                }
 590        }
 591
 592        rcu_read_unlock();
 593}
 594
 595void rtw_tx_work(struct work_struct *w)
 596{
 597        struct rtw_dev *rtwdev = container_of(w, struct rtw_dev, tx_work);
 598        struct rtw_txq *rtwtxq, *tmp;
 599
 600        spin_lock_bh(&rtwdev->txq_lock);
 601
 602        list_for_each_entry_safe(rtwtxq, tmp, &rtwdev->txqs, list) {
 603                struct ieee80211_txq *txq = rtwtxq_to_txq(rtwtxq);
 604                unsigned long frame_cnt;
 605                unsigned long byte_cnt;
 606
 607                ieee80211_txq_get_depth(txq, &frame_cnt, &byte_cnt);
 608                rtw_txq_push(rtwdev, rtwtxq, frame_cnt);
 609
 610                list_del_init(&rtwtxq->list);
 611        }
 612
 613        rtw_hci_tx_kick_off(rtwdev);
 614
 615        spin_unlock_bh(&rtwdev->txq_lock);
 616}
 617
 618void rtw_txq_init(struct rtw_dev *rtwdev, struct ieee80211_txq *txq)
 619{
 620        struct rtw_txq *rtwtxq;
 621
 622        if (!txq)
 623                return;
 624
 625        rtwtxq = (struct rtw_txq *)txq->drv_priv;
 626        INIT_LIST_HEAD(&rtwtxq->list);
 627}
 628
 629void rtw_txq_cleanup(struct rtw_dev *rtwdev, struct ieee80211_txq *txq)
 630{
 631        struct rtw_txq *rtwtxq;
 632
 633        if (!txq)
 634                return;
 635
 636        rtwtxq = (struct rtw_txq *)txq->drv_priv;
 637        spin_lock_bh(&rtwdev->txq_lock);
 638        if (!list_empty(&rtwtxq->list))
 639                list_del_init(&rtwtxq->list);
 640        spin_unlock_bh(&rtwdev->txq_lock);
 641}
 642