11 files changed, 350 insertions, 19 deletions

diff --git a/net/mac80211/agg-tx.c b/net/mac80211/agg-tx.c
index 20522492d8cc..cce9d425c718 100644
--- a/net/mac80211/agg-tx.c
+++ b/net/mac80211/agg-tx.c
@@ -188,6 +188,43 @@ ieee80211_wake_queue_agg(struct ieee80211_sub_if_data *sdata, int tid)
         __release(agg_queue);
 }
 
+static void
+ieee80211_agg_stop_txq(struct sta_info *sta, int tid)
+{
+        struct ieee80211_txq *txq = sta->sta.txq[tid];
+        struct txq_info *txqi;
+
+        if (!txq)
+                return;
+
+        txqi = to_txq_info(txq);
+
+        /* Lock here to protect against further seqno updates on dequeue */
+        spin_lock_bh(&txqi->queue.lock);
+        set_bit(IEEE80211_TXQ_STOP, &txqi->flags);
+        spin_unlock_bh(&txqi->queue.lock);
+}
+
+static void
+ieee80211_agg_start_txq(struct sta_info *sta, int tid, bool enable)
+{
+        struct ieee80211_txq *txq = sta->sta.txq[tid];
+        struct txq_info *txqi;
+
+        if (!txq)
+                return;
+
+        txqi = to_txq_info(txq);
+
+        if (enable)
+                set_bit(IEEE80211_TXQ_AMPDU, &txqi->flags);
+        else
+                clear_bit(IEEE80211_TXQ_AMPDU, &txqi->flags);
+
+        clear_bit(IEEE80211_TXQ_STOP, &txqi->flags);
+        drv_wake_tx_queue(sta->sdata->local, txqi);
+}
+
 /*
  * splice packets from the STA's pending to the local pending,
  * requires a call to ieee80211_agg_splice_finish later
@@ -247,6 +284,7 @@ static void ieee80211_remove_tid_tx(struct sta_info *sta, int tid)
         ieee80211_assign_tid_tx(sta, tid, NULL);
 
         ieee80211_agg_splice_finish(sta->sdata, tid);
+        ieee80211_agg_start_txq(sta, tid, false);
 
         kfree_rcu(tid_tx, rcu_head);
 }
@@ -418,6 +456,8 @@ void ieee80211_tx_ba_session_handle_start(struct sta_info *sta, int tid)
          */
         clear_bit(HT_AGG_STATE_WANT_START, &tid_tx->state);
 
+        ieee80211_agg_stop_txq(sta, tid);
+
         /*
          * Make sure no packets are being processed. This ensures that
          * we have a valid starting sequence number and that in-flight
@@ -440,6 +480,8 @@ void ieee80211_tx_ba_session_handle_start(struct sta_info *sta, int tid)
                 ieee80211_agg_splice_finish(sdata, tid);
                 spin_unlock_bh(&sta->lock);
 
+                ieee80211_agg_start_txq(sta, tid, false);
+
                 kfree_rcu(tid_tx, rcu_head);
                 return;
         }
@@ -669,6 +711,8 @@ static void ieee80211_agg_tx_operational(struct ieee80211_local *local,
         ieee80211_agg_splice_finish(sta->sdata, tid);
 
         spin_unlock_bh(&sta->lock);
+
+        ieee80211_agg_start_txq(sta, tid, true);
 }
 
 void ieee80211_start_tx_ba_cb(struct ieee80211_vif *vif, u8 *ra, u16 tid)
diff --git a/net/mac80211/driver-ops.h b/net/mac80211/driver-ops.h
index 0a39d3db951a..26e1ca8a474a 100644
--- a/net/mac80211/driver-ops.h
+++ b/net/mac80211/driver-ops.h
@@ -1367,4 +1367,16 @@ drv_tdls_recv_channel_switch(struct ieee80211_local *local,
         trace_drv_return_void(local);
 }
 
+static inline void drv_wake_tx_queue(struct ieee80211_local *local,
+                                     struct txq_info *txq)
+{
+        struct ieee80211_sub_if_data *sdata = vif_to_sdata(txq->txq.vif);
+
+        if (!check_sdata_in_driver(sdata))
+                return;
+
+        trace_drv_wake_tx_queue(local, sdata, txq);
+        local->ops->wake_tx_queue(&local->hw, &txq->txq);
+}
+
 #endif /* __MAC80211_DRIVER_OPS */
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index 3c1512b0442c..04b32f3e0395 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -811,6 +811,19 @@ struct mac80211_qos_map {
         struct rcu_head rcu_head;
 };
 
+enum txq_info_flags {
+        IEEE80211_TXQ_STOP,
+        IEEE80211_TXQ_AMPDU,
+};
+
+struct txq_info {
+        struct sk_buff_head queue;
+        unsigned long flags;
+
+        /* keep last! */
+        struct ieee80211_txq txq;
+};
+
 struct ieee80211_sub_if_data {
         struct list_head list;
 
@@ -853,6 +866,7 @@ struct ieee80211_sub_if_data {
         bool control_port_no_encrypt;
         int encrypt_headroom;
 
+        atomic_t txqs_len[IEEE80211_NUM_ACS];
         struct ieee80211_tx_queue_params tx_conf[IEEE80211_NUM_ACS];
         struct mac80211_qos_map __rcu *qos_map;
 
@@ -1450,6 +1464,10 @@ static inline struct ieee80211_local *hw_to_local(
         return container_of(hw, struct ieee80211_local, hw);
 }
 
+static inline struct txq_info *to_txq_info(struct ieee80211_txq *txq)
+{
+        return container_of(txq, struct txq_info, txq);
+}
 
 static inline int ieee80211_bssid_match(const u8 *raddr, const u8 *addr)
 {
@@ -1906,6 +1924,9 @@ static inline bool ieee80211_can_run_worker(struct ieee80211_local *local)
         return true;
 }
 
+void ieee80211_init_tx_queue(struct ieee80211_sub_if_data *sdata,
+                             struct sta_info *sta,
+                             struct txq_info *txq, int tid);
 void ieee80211_send_auth(struct ieee80211_sub_if_data *sdata,
                          u16 transaction, u16 auth_alg, u16 status,
                          const u8 *extra, size_t extra_len, const u8 *bssid,
diff --git a/net/mac80211/iface.c b/net/mac80211/iface.c
index a0cd97fd0c49..b4ac596a7cb7 100644
--- a/net/mac80211/iface.c
+++ b/net/mac80211/iface.c
@@ -969,6 +969,13 @@ static void ieee80211_do_stop(struct ieee80211_sub_if_data *sdata,
         }
         spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
 
+        if (sdata->vif.txq) {
+                struct txq_info *txqi = to_txq_info(sdata->vif.txq);
+
+                ieee80211_purge_tx_queue(&local->hw, &txqi->queue);
+                atomic_set(&sdata->txqs_len[txqi->txq.ac], 0);
+        }
+
         if (local->open_count == 0)
                 ieee80211_clear_tx_pending(local);
 
@@ -1654,6 +1661,7 @@ int ieee80211_if_add(struct ieee80211_local *local, const char *name,
 {
         struct net_device *ndev = NULL;
         struct ieee80211_sub_if_data *sdata = NULL;
+        struct txq_info *txqi;
         int ret, i;
         int txqs = 1;
 
@@ -1673,10 +1681,18 @@ int ieee80211_if_add(struct ieee80211_local *local, const char *name,
                 ieee80211_assign_perm_addr(local, wdev->address, type);
                 memcpy(sdata->vif.addr, wdev->address, ETH_ALEN);
         } else {
+                int size = ALIGN(sizeof(*sdata) + local->hw.vif_data_size,
+                                 sizeof(void *));
+                int txq_size = 0;
+
+                if (local->ops->wake_tx_queue)
+                        txq_size += sizeof(struct txq_info) +
+                                    local->hw.txq_data_size;
+
                 if (local->hw.queues >= IEEE80211_NUM_ACS)
                         txqs = IEEE80211_NUM_ACS;
 
-                ndev = alloc_netdev_mqs(sizeof(*sdata) + local->hw.vif_data_size,
+                ndev = alloc_netdev_mqs(size + txq_size,
                                         name, name_assign_type,
                                         ieee80211_if_setup, txqs, 1);
                 if (!ndev)
@@ -1711,6 +1727,11 @@ int ieee80211_if_add(struct ieee80211_local *local, const char *name,
                 memcpy(sdata->vif.addr, ndev->dev_addr, ETH_ALEN);
                 memcpy(sdata->name, ndev->name, IFNAMSIZ);
 
+                if (txq_size) {
+                        txqi = netdev_priv(ndev) + size;
+                        ieee80211_init_tx_queue(sdata, NULL, txqi, 0);
+                }
+
                 sdata->dev = ndev;
         }
 
diff --git a/net/mac80211/main.c b/net/mac80211/main.c
index 51e0332a4589..df3051d96aff 100644
--- a/net/mac80211/main.c
+++ b/net/mac80211/main.c
@@ -1039,6 +1039,9 @@ int ieee80211_register_hw(struct ieee80211_hw *hw)
 
         local->dynamic_ps_forced_timeout = -1;
 
+        if (!local->hw.txq_ac_max_pending)
+                local->hw.txq_ac_max_pending = 64;
+
         result = ieee80211_wep_init(local);
         if (result < 0)
                 wiphy_debug(local->hw.wiphy, "Failed to initialize wep: %d\n",
diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index 5b60bcf00ec3..bc59c8a20a39 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -1184,6 +1184,7 @@ static void sta_ps_start(struct sta_info *sta)
         struct ieee80211_sub_if_data *sdata = sta->sdata;
         struct ieee80211_local *local = sdata->local;
         struct ps_data *ps;
+        int tid;
 
         if (sta->sdata->vif.type == NL80211_IFTYPE_AP ||
             sta->sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
@@ -1197,6 +1198,18 @@ static void sta_ps_start(struct sta_info *sta)
                 drv_sta_notify(local, sdata, STA_NOTIFY_SLEEP, &sta->sta);
         ps_dbg(sdata, "STA %pM aid %d enters power save mode\n",
                sta->sta.addr, sta->sta.aid);
+
+        if (!sta->sta.txq[0])
+                return;
+
+        for (tid = 0; tid < ARRAY_SIZE(sta->sta.txq); tid++) {
+                struct txq_info *txqi = to_txq_info(sta->sta.txq[tid]);
+
+                if (!skb_queue_len(&txqi->queue))
+                        set_bit(tid, &sta->txq_buffered_tids);
+                else
+                        clear_bit(tid, &sta->txq_buffered_tids);
+        }
 }
 
 static void sta_ps_end(struct sta_info *sta)
diff --git a/net/mac80211/sta_info.c b/net/mac80211/sta_info.c
index 81cc499fa4a9..12971b71d0fa 100644
--- a/net/mac80211/sta_info.c
+++ b/net/mac80211/sta_info.c
@@ -106,6 +106,16 @@ static void __cleanup_single_sta(struct sta_info *sta)
                 atomic_dec(&ps->num_sta_ps);
         }
 
+        if (sta->sta.txq[0]) {
+                for (i = 0; i < ARRAY_SIZE(sta->sta.txq); i++) {
+                        struct txq_info *txqi = to_txq_info(sta->sta.txq[i]);
+                        int n = skb_queue_len(&txqi->queue);
+
+                        ieee80211_purge_tx_queue(&local->hw, &txqi->queue);
+                        atomic_sub(n, &sdata->txqs_len[txqi->txq.ac]);
+                }
+        }
+
         for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
                 local->total_ps_buffered -= skb_queue_len(&sta->ps_tx_buf[ac]);
                 ieee80211_purge_tx_queue(&local->hw, &sta->ps_tx_buf[ac]);
@@ -218,6 +228,8 @@ void sta_info_free(struct ieee80211_local *local, struct sta_info *sta)
 
         sta_dbg(sta->sdata, "Destroyed STA %pM\n", sta->sta.addr);
 
+        if (sta->sta.txq[0])
+                kfree(to_txq_info(sta->sta.txq[0]));
         kfree(rcu_dereference_raw(sta->sta.rates));
         kfree(sta);
 }
@@ -268,11 +280,12 @@ struct sta_info *sta_info_alloc(struct ieee80211_sub_if_data *sdata,
                                 const u8 *addr, gfp_t gfp)
 {
         struct ieee80211_local *local = sdata->local;
+        struct ieee80211_hw *hw = &local->hw;
         struct sta_info *sta;
         struct timespec uptime;
         int i;
 
-        sta = kzalloc(sizeof(*sta) + local->hw.sta_data_size, gfp);
+        sta = kzalloc(sizeof(*sta) + hw->sta_data_size, gfp);
         if (!sta)
                 return NULL;
 
@@ -304,11 +317,25 @@ struct sta_info *sta_info_alloc(struct ieee80211_sub_if_data *sdata,
         for (i = 0; i < ARRAY_SIZE(sta->chain_signal_avg); i++)
                 ewma_init(&sta->chain_signal_avg[i], 1024, 8);
 
-        if (sta_prepare_rate_control(local, sta, gfp)) {
-                kfree(sta);
-                return NULL;
+        if (local->ops->wake_tx_queue) {
+                void *txq_data;
+                int size = sizeof(struct txq_info) +
+                           ALIGN(hw->txq_data_size, sizeof(void *));
+
+                txq_data = kcalloc(ARRAY_SIZE(sta->sta.txq), size, gfp);
+                if (!txq_data)
+                        goto free;
+
+                for (i = 0; i < ARRAY_SIZE(sta->sta.txq); i++) {
+                        struct txq_info *txq = txq_data + i * size;
+
+                        ieee80211_init_tx_queue(sdata, sta, txq, i);
+                }
         }
 
+        if (sta_prepare_rate_control(local, sta, gfp))
+                goto free_txq;
+
         for (i = 0; i < IEEE80211_NUM_TIDS; i++) {
                 /*
                  * timer_to_tid must be initialized with identity mapping
@@ -329,7 +356,7 @@ struct sta_info *sta_info_alloc(struct ieee80211_sub_if_data *sdata,
         if (sdata->vif.type == NL80211_IFTYPE_AP ||
             sdata->vif.type == NL80211_IFTYPE_AP_VLAN) {
                 struct ieee80211_supported_band *sband =
-                        local->hw.wiphy->bands[ieee80211_get_sdata_band(sdata)];
+                        hw->wiphy->bands[ieee80211_get_sdata_band(sdata)];
                 u8 smps = (sband->ht_cap.cap & IEEE80211_HT_CAP_SM_PS) >>
                                 IEEE80211_HT_CAP_SM_PS_SHIFT;
                 /*
@@ -354,6 +381,13 @@ struct sta_info *sta_info_alloc(struct ieee80211_sub_if_data *sdata,
         sta_dbg(sdata, "Allocated STA %pM\n", sta->sta.addr);
 
         return sta;
+
+free_txq:
+        if (sta->sta.txq[0])
+                kfree(to_txq_info(sta->sta.txq[0]));
+free:
+        kfree(sta);
+        return NULL;
 }
 
 static int sta_info_insert_check(struct sta_info *sta)
@@ -623,6 +657,8 @@ static void __sta_info_recalc_tim(struct sta_info *sta, bool ignore_pending)
 
                 indicate_tim |=
                         sta->driver_buffered_tids & tids;
+                indicate_tim |=
+                        sta->txq_buffered_tids & tids;
         }
 
  done:
@@ -1072,7 +1108,7 @@ void ieee80211_sta_ps_deliver_wakeup(struct sta_info *sta)
         struct ieee80211_sub_if_data *sdata = sta->sdata;
         struct ieee80211_local *local = sdata->local;
         struct sk_buff_head pending;
-        int filtered = 0, buffered = 0, ac;
+        int filtered = 0, buffered = 0, ac, i;
         unsigned long flags;
         struct ps_data *ps;
 
@@ -1091,10 +1127,22 @@ void ieee80211_sta_ps_deliver_wakeup(struct sta_info *sta)
 
         BUILD_BUG_ON(BITS_TO_LONGS(IEEE80211_NUM_TIDS) > 1);
         sta->driver_buffered_tids = 0;
+        sta->txq_buffered_tids = 0;
 
         if (!(local->hw.flags & IEEE80211_HW_AP_LINK_PS))
                 drv_sta_notify(local, sdata, STA_NOTIFY_AWAKE, &sta->sta);
 
+        if (sta->sta.txq[0]) {
+                for (i = 0; i < ARRAY_SIZE(sta->sta.txq); i++) {
+                        struct txq_info *txqi = to_txq_info(sta->sta.txq[i]);
+
+                        if (!skb_queue_len(&txqi->queue))
+                                continue;
+
+                        drv_wake_tx_queue(local, txqi);
+                }
+        }
+
         skb_queue_head_init(&pending);
 
         /* sync with ieee80211_tx_h_unicast_ps_buf */
@@ -1276,8 +1324,10 @@ ieee80211_sta_ps_deliver_response(struct sta_info *sta,
                 /* if we already have frames from software, then we can't also
                  * release from hardware queues
                  */
-                if (skb_queue_empty(&frames))
+                if (skb_queue_empty(&frames)) {
                         driver_release_tids |= sta->driver_buffered_tids & tids;
+                        driver_release_tids |= sta->txq_buffered_tids & tids;
+                }
 
                 if (driver_release_tids) {
                         /* If the driver has data on more than one TID then
@@ -1448,6 +1498,9 @@ ieee80211_sta_ps_deliver_response(struct sta_info *sta,
 
                 sta_info_recalc_tim(sta);
         } else {
+                unsigned long tids = sta->txq_buffered_tids & driver_release_tids;
+                int tid;
+
                 /*
                  * We need to release a frame that is buffered somewhere in the
                  * driver ... it'll have to handle that.
@@ -1467,8 +1520,22 @@ ieee80211_sta_ps_deliver_response(struct sta_info *sta,
                  * that the TID(s) became empty before returning here from the
                  * release function.
                  * Either way, however, when the driver tells us that the TID(s)
-                 * became empty we'll do the TIM recalculation.
+                 * became empty or we find that a txq became empty, we'll do the
+                 * TIM recalculation.
                  */
+
+                if (!sta->sta.txq[0])
+                        return;
+
+                for (tid = 0; tid < ARRAY_SIZE(sta->sta.txq); tid++) {
+                        struct txq_info *txqi = to_txq_info(sta->sta.txq[tid]);
+
+                        if (!(tids & BIT(tid)) || skb_queue_len(&txqi->queue))
+                                continue;
+
+                        sta_info_recalc_tim(sta);
+                        break;
+                }
         }
 }
 
diff --git a/net/mac80211/sta_info.h b/net/mac80211/sta_info.h
index 97f25b9e52be..691d8a1f94a5 100644
--- a/net/mac80211/sta_info.h
+++ b/net/mac80211/sta_info.h
@@ -275,6 +275,7 @@ struct sta_ampdu_mlme {
  *      entered power saving state, these are also delivered to
  *      the station when it leaves powersave or polls for frames
  * @driver_buffered_tids: bitmap of TIDs the driver has data buffered on
+ * @txq_buffered_tids: bitmap of TIDs that mac80211 has txq data buffered on
  * @rx_packets: Number of MSDUs received from this STA
  * @rx_bytes: Number of bytes received from this STA
  * @last_rx: time (in jiffies) when last frame was received from this STA
@@ -369,6 +370,7 @@ struct sta_info {
         struct sk_buff_head ps_tx_buf[IEEE80211_NUM_ACS];
         struct sk_buff_head tx_filtered[IEEE80211_NUM_ACS];
         unsigned long driver_buffered_tids;
+        unsigned long txq_buffered_tids;
 
         /* Updated from RX path only, no locking requirements */
         unsigned long rx_packets;
diff --git a/net/mac80211/trace.h b/net/mac80211/trace.h
index e9e462b349e5..790bd45081c4 100644
--- a/net/mac80211/trace.h
+++ b/net/mac80211/trace.h
@@ -2312,6 +2312,37 @@ TRACE_EVENT(drv_tdls_recv_channel_switch,
         )
 );
 
+TRACE_EVENT(drv_wake_tx_queue,
+        TP_PROTO(struct ieee80211_local *local,
+                 struct ieee80211_sub_if_data *sdata,
+                 struct txq_info *txq),
+
+        TP_ARGS(local, sdata, txq),
+
+        TP_STRUCT__entry(
+                LOCAL_ENTRY
+                VIF_ENTRY
+                STA_ENTRY
+                __field(u8, ac)
+                __field(u8, tid)
+        ),
+
+        TP_fast_assign(
+                struct ieee80211_sta *sta = txq->txq.sta;
+
+                LOCAL_ASSIGN;
+                VIF_ASSIGN;
+                STA_ASSIGN;
+                __entry->ac = txq->txq.ac;
+                __entry->tid = txq->txq.tid;
+        ),
+
+        TP_printk(
+                LOCAL_PR_FMT  VIF_PR_FMT  STA_PR_FMT " ac:%d tid:%d",
+                LOCAL_PR_ARG, VIF_PR_ARG, STA_PR_ARG, __entry->ac, __entry->tid
+        )
+);
+
 #ifdef CONFIG_MAC80211_MESSAGE_TRACING
 #undef TRACE_SYSTEM
 #define TRACE_SYSTEM mac80211_msg
diff --git a/net/mac80211/tx.c b/net/mac80211/tx.c
index 9f7fb4eec37b..667111ee6a20 100644
--- a/net/mac80211/tx.c
+++ b/net/mac80211/tx.c
@@ -767,12 +767,22 @@ ieee80211_tx_h_rate_ctrl(struct ieee80211_tx_data *tx)
         return TX_CONTINUE;
 }
 
+static __le16 ieee80211_tx_next_seq(struct sta_info *sta, int tid)
+{
+        u16 *seq = &sta->tid_seq[tid];
+        __le16 ret = cpu_to_le16(*seq);
+
+        /* Increase the sequence number. */
+        *seq = (*seq + 0x10) & IEEE80211_SCTL_SEQ;
+
+        return ret;
+}
+
 static ieee80211_tx_result debug_noinline
 ieee80211_tx_h_sequence(struct ieee80211_tx_data *tx)
 {
         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx->skb->data;
-        u16 *seq;
         u8 *qc;
         int tid;
 
@@ -823,13 +833,10 @@ ieee80211_tx_h_sequence(struct ieee80211_tx_data *tx)
 
         qc = ieee80211_get_qos_ctl(hdr);
         tid = *qc & IEEE80211_QOS_CTL_TID_MASK;
-        seq = &tx->sta->tid_seq[tid];
         tx->sta->tx_msdu[tid]++;
 
-        hdr->seq_ctrl = cpu_to_le16(*seq);
-
-        /* Increase the sequence number. */
-        *seq = (*seq + 0x10) & IEEE80211_SCTL_SEQ;
+        if (!tx->sta->sta.txq[0])
+                hdr->seq_ctrl = ieee80211_tx_next_seq(tx->sta, tid);
 
         return TX_CONTINUE;
 }
@@ -1070,7 +1077,7 @@ static bool ieee80211_tx_prep_agg(struct ieee80211_tx_data *tx,
                  * nothing -- this aggregation session is being started
                  * but that might still fail with the driver
                  */
-        } else {
+        } else if (!tx->sta->sta.txq[tid]) {
                 spin_lock(&tx->sta->lock);
                 /*
                  * Need to re-check now, because we may get here
@@ -1211,13 +1218,102 @@ ieee80211_tx_prepare(struct ieee80211_sub_if_data *sdata,
         return TX_CONTINUE;
 }
 
+static void ieee80211_drv_tx(struct ieee80211_local *local,
+                             struct ieee80211_vif *vif,
+                             struct ieee80211_sta *pubsta,
+                             struct sk_buff *skb)
+{
+        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+        struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+        struct ieee80211_tx_control control = {
+                .sta = pubsta,
+        };
+        struct ieee80211_txq *txq = NULL;
+        struct txq_info *txqi;
+        u8 ac;
+
+        if (info->control.flags & IEEE80211_TX_CTRL_PS_RESPONSE)
+                goto tx_normal;
+
+        if (!ieee80211_is_data(hdr->frame_control))
+                goto tx_normal;
+
+        if (pubsta) {
+                u8 tid = skb->priority & IEEE80211_QOS_CTL_TID_MASK;
+
+                txq = pubsta->txq[tid];
+        } else if (vif) {
+                txq = vif->txq;
+        }
+
+        if (!txq)
+                goto tx_normal;
+
+        ac = txq->ac;
+        txqi = to_txq_info(txq);
+        atomic_inc(&sdata->txqs_len[ac]);
+        if (atomic_read(&sdata->txqs_len[ac]) >= local->hw.txq_ac_max_pending)
+                netif_stop_subqueue(sdata->dev, ac);
+
+        skb_queue_tail(&txqi->queue, skb);
+        drv_wake_tx_queue(local, txqi);
+
+        return;
+
+tx_normal:
+        drv_tx(local, &control, skb);
+}
+
+struct sk_buff *ieee80211_tx_dequeue(struct ieee80211_hw *hw,
+                                     struct ieee80211_txq *txq)
+{
+        struct ieee80211_local *local = hw_to_local(hw);
+        struct ieee80211_sub_if_data *sdata = vif_to_sdata(txq->vif);
+        struct txq_info *txqi = container_of(txq, struct txq_info, txq);
+        struct ieee80211_hdr *hdr;
+        struct sk_buff *skb = NULL;
+        u8 ac = txq->ac;
+
+        spin_lock_bh(&txqi->queue.lock);
+
+        if (test_bit(IEEE80211_TXQ_STOP, &txqi->flags))
+                goto out;
+
+        skb = __skb_dequeue(&txqi->queue);
+        if (!skb)
+                goto out;
+
+        atomic_dec(&sdata->txqs_len[ac]);
+        if (__netif_subqueue_stopped(sdata->dev, ac))
+                ieee80211_propagate_queue_wake(local, sdata->vif.hw_queue[ac]);
+
+        hdr = (struct ieee80211_hdr *)skb->data;
+        if (txq->sta && ieee80211_is_data_qos(hdr->frame_control)) {
+                struct sta_info *sta = container_of(txq->sta, struct sta_info,
+                                                    sta);
+                struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+
+                hdr->seq_ctrl = ieee80211_tx_next_seq(sta, txq->tid);
+                if (test_bit(IEEE80211_TXQ_AMPDU, &txqi->flags))
+                        info->flags |= IEEE80211_TX_CTL_AMPDU;
+                else
+                        info->flags &= ~IEEE80211_TX_CTL_AMPDU;
+        }
+
+out:
+        spin_unlock_bh(&txqi->queue.lock);
+
+        return skb;
+}
+EXPORT_SYMBOL(ieee80211_tx_dequeue);
+
 static bool ieee80211_tx_frags(struct ieee80211_local *local,
                                struct ieee80211_vif *vif,
                                struct ieee80211_sta *sta,
                                struct sk_buff_head *skbs,
                                bool txpending)
 {
-        struct ieee80211_tx_control control;
         struct sk_buff *skb, *tmp;
         unsigned long flags;
 
@@ -1275,10 +1371,9 @@ static bool ieee80211_tx_frags(struct ieee80211_local *local,
                 spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
 
                 info->control.vif = vif;
-                control.sta = sta;
 
                 __skb_unlink(skb, skbs);
-                drv_tx(local, &control, skb);
+                ieee80211_drv_tx(local, vif, sta, skb);
         }
 
         return true;
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index d1742a7d9ea4..482b85c19a36 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -308,6 +308,11 @@ void ieee80211_propagate_queue_wake(struct ieee80211_local *local, int queue)
                 for (ac = 0; ac < n_acs; ac++) {
                         int ac_queue = sdata->vif.hw_queue[ac];
 
+                        if (local->ops->wake_tx_queue &&
+                            (atomic_read(&sdata->txqs_len[ac]) >
+                             local->hw.txq_ac_max_pending))
+                                continue;
+
                         if (ac_queue == queue ||
                             (sdata->vif.cab_queue == queue &&
                              local->queue_stop_reasons[ac_queue] == 0 &&
@@ -3352,3 +3357,20 @@ u8 *ieee80211_add_wmm_info_ie(u8 *buf, u8 qosinfo)
 
         return buf;
 }
+
+void ieee80211_init_tx_queue(struct ieee80211_sub_if_data *sdata,
+                             struct sta_info *sta,
+                             struct txq_info *txqi, int tid)
+{
+        skb_queue_head_init(&txqi->queue);
+        txqi->txq.vif = &sdata->vif;
+
+        if (sta) {
+                txqi->txq.sta = &sta->sta;
+                sta->sta.txq[tid] = &txqi->txq;
+                txqi->txq.ac = ieee802_1d_to_ac[tid & 7];
+        } else {
+                sdata->vif.txq = &txqi->txq;
+                txqi->txq.ac = IEEE80211_AC_BE;
+        }
+}