package/kernel/mac80211/patches/300-mac80211-add-an-intermediate-software-queue-implemen.patch

   1 From: Felix Fietkau <nbd@openwrt.org>
   2 Date: Tue, 18 Nov 2014 23:58:51 +0100
   3 Subject: [PATCH] mac80211: add an intermediate software queue implementation
   4
   5 This allows drivers to request per-vif and per-sta-tid queues from which
   6 they can pull frames. This makes it easier to keep the hardware queues
   7 short, and to improve fairness between clients and vifs.
   8
   9 The task of scheduling packet transmission is left up to the driver -
  10 queueing is controlled by mac80211. Drivers can only dequeue packets by
  11 calling ieee80211_tx_dequeue. This makes it possible to add active queue
  12 management later without changing drivers using this code.
  13
  14 This can also be used as a starting point to implement A-MSDU
  15 aggregation in a way that does not add artificially induced latency.
  16
  17 Signed-off-by: Felix Fietkau <nbd@openwrt.org>
  18 ---
  19
  20 --- a/include/net/mac80211.h
  21 +++ b/include/net/mac80211.h
  22 @@ -1257,6 +1257,8 @@ struct ieee80211_vif {
  23         u8 cab_queue;
  24         u8 hw_queue[IEEE80211_NUM_ACS];
  25
  26 +       struct ieee80211_txq *txq;
  27 +
  28         struct ieee80211_chanctx_conf __rcu *chanctx_conf;
  29
  30         u32 driver_flags;
  31 @@ -1519,6 +1521,8 @@ struct ieee80211_sta {
  32         bool tdls_initiator;
  33         bool mfp;
  34
  35 +       struct ieee80211_txq *txq[IEEE80211_NUM_TIDS];
  36 +
  37         /* must be last */
  38         u8 drv_priv[0] __aligned(sizeof(void *));
  39  };
  40 @@ -1547,6 +1551,27 @@ struct ieee80211_tx_control {
  41  };
  42
  43  /**
  44 + * struct ieee80211_txq - Software intermediate tx queue
  45 + *
  46 + * @vif: &struct ieee80211_vif pointer from the add_interface callback.
  47 + * @sta: station table entry, may be NULL for per-vif queue
  48 + * @tid: the TID for this queue (unset for per-vif queue)
  49 + * @ac: the AC for this queue
  50 + *
  51 + * The driver can obtain packets from this queue by calling
  52 + * ieee80211_tx_dequeue().
  53 + */
  54 +struct ieee80211_txq {
  55 +       struct ieee80211_vif *vif;
  56 +       struct ieee80211_sta *sta;
  57 +       u8 tid;
  58 +       u8 ac;
  59 +
  60 +       /* must be last */
  61 +       u8 drv_priv[0] __aligned(sizeof(void *));
  62 +};
  63 +
  64 +/**
  65   * enum ieee80211_hw_flags - hardware flags
  66   *
  67   * These flags are used to indicate hardware capabilities to
  68 @@ -1770,6 +1795,8 @@ enum ieee80211_hw_flags {
  69   *     within &struct ieee80211_sta.
  70   * @chanctx_data_size: size (in bytes) of the drv_priv data area
  71   *     within &struct ieee80211_chanctx_conf.
  72 + * @txq_data_size: size (in bytes) of the drv_priv data area
  73 + *     within @struct ieee80211_txq.
  74   *
  75   * @max_rates: maximum number of alternate rate retry stages the hw
  76   *     can handle.
  77 @@ -1818,6 +1845,9 @@ enum ieee80211_hw_flags {
  78   * @n_cipher_schemes: a size of an array of cipher schemes definitions.
  79   * @cipher_schemes: a pointer to an array of cipher scheme definitions
  80   *     supported by HW.
  81 + *
  82 + * @txq_ac_max_pending: maximum number of frames per AC pending in all txq
  83 + *     entries for a vif.
  84   */
  85  struct ieee80211_hw {
  86         struct ieee80211_conf conf;
  87 @@ -1830,6 +1860,7 @@ struct ieee80211_hw {
  88         int vif_data_size;
  89         int sta_data_size;
  90         int chanctx_data_size;
  91 +       int txq_data_size;
  92         u16 queues;
  93         u16 max_listen_interval;
  94         s8 max_signal;
  95 @@ -1846,6 +1877,7 @@ struct ieee80211_hw {
  96         u8 uapsd_max_sp_len;
  97         u8 n_cipher_schemes;
  98         const struct ieee80211_cipher_scheme *cipher_schemes;
  99 +       int txq_ac_max_pending;
 100  };
 101
 102  /**
 103 @@ -3007,6 +3039,8 @@ enum ieee80211_reconfig_type {
 104   *     response template is provided, together with the location of the
 105   *     switch-timing IE within the template. The skb can only be used within
 106   *     the function call.
 107 + *
 108 + * @wake_tx_queue: Called when new packets have been added to the queue.
 109   */
 110  struct ieee80211_ops {
 111         void (*tx)(struct ieee80211_hw *hw,
 112 @@ -3238,6 +3272,9 @@ struct ieee80211_ops {
 113         void (*tdls_recv_channel_switch)(struct ieee80211_hw *hw,
 114                                          struct ieee80211_vif *vif,
 115                                          struct ieee80211_tdls_ch_sw_params *params);
 116 +
 117 +       void (*wake_tx_queue)(struct ieee80211_hw *hw,
 118 +                             struct ieee80211_txq *txq);
 119  };
 120
 121  /**
 122 @@ -5249,4 +5286,17 @@ void ieee80211_unreserve_tid(struct ieee
 123   */
 124  size_t ieee80211_ie_split(const u8 *ies, size_t ielen,
 125                           const u8 *ids, int n_ids, size_t offset);
 126 +
 127 +/**
 128 + * ieee80211_tx_dequeue - dequeue a packet from a software tx queue
 129 + *
 130 + * @hw: pointer as obtained from ieee80211_alloc_hw()
 131 + * @txq: pointer obtained from .add_tx_queue() call
 132 + *
 133 + * Returns the sjb if successful, ERR_PTR(-EAGAIN) if no frame was available.
 134 + */
 135 +struct sk_buff *ieee80211_tx_dequeue(struct ieee80211_hw *hw,
 136 +                                    struct ieee80211_txq *txq);
 137 +
 138 +
 139  #endif /* MAC80211_H */
 140 --- a/net/mac80211/driver-ops.h
 141 +++ b/net/mac80211/driver-ops.h
 142 @@ -1367,4 +1367,21 @@ drv_tdls_recv_channel_switch(struct ieee
 143         trace_drv_return_void(local);
 144  }
 145
 146 +static inline void drv_wake_tx_queue(struct ieee80211_local *local,
 147 +                                    struct txq_info *txq)
 148 +{
 149 +       struct ieee80211_sub_if_data *sdata = vif_to_sdata(txq->txq.vif);
 150 +
 151 +       if (!check_sdata_in_driver(sdata))
 152 +               return;
 153 +
 154 +       if (txq->txq.sta)
 155 +               trace_drv_wake_sta_tx_queue(local, sdata, txq->txq.sta,
 156 +                                           txq->txq.tid);
 157 +       else
 158 +               trace_drv_wake_vif_tx_queue(local, sdata);
 159 +
 160 +       local->ops->wake_tx_queue(&local->hw, &txq->txq);
 161 +}
 162 +
 163  #endif /* __MAC80211_DRIVER_OPS */
 164 --- a/net/mac80211/ieee80211_i.h
 165 +++ b/net/mac80211/ieee80211_i.h
 166 @@ -809,6 +809,13 @@ struct mac80211_qos_map {
 167         struct rcu_head rcu_head;
 168  };
 169
 170 +struct txq_info {
 171 +       struct sk_buff_head queue;
 172 +
 173 +       /* keep last! */
 174 +       struct ieee80211_txq txq;
 175 +};
 176 +
 177  struct ieee80211_sub_if_data {
 178         struct list_head list;
 179
 180 @@ -853,6 +860,7 @@ struct ieee80211_sub_if_data {
 181         bool control_port_no_encrypt;
 182         int encrypt_headroom;
 183
 184 +       atomic_t txqs_len[IEEE80211_NUM_ACS];
 185         struct ieee80211_tx_queue_params tx_conf[IEEE80211_NUM_ACS];
 186         struct mac80211_qos_map __rcu *qos_map;
 187
 188 @@ -1905,6 +1913,12 @@ static inline bool ieee80211_can_run_wor
 189         return true;
 190  }
 191
 192 +void ieee80211_init_tx_queue(struct ieee80211_sub_if_data *sdata,
 193 +                            struct sta_info *sta,
 194 +                            struct txq_info *txq, int tid);
 195 +void ieee80211_flush_tx_queue(struct ieee80211_local *local,
 196 +                             struct ieee80211_txq *txq);
 197 +
 198  void ieee80211_send_auth(struct ieee80211_sub_if_data *sdata,
 199                          u16 transaction, u16 auth_alg, u16 status,
 200                          const u8 *extra, size_t extra_len, const u8 *bssid,
 201 --- a/net/mac80211/iface.c
 202 +++ b/net/mac80211/iface.c
 203 @@ -969,6 +969,9 @@ static void ieee80211_do_stop(struct iee
 204         }
 205         spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
 206
 207 +       if (sdata->vif.txq)
 208 +               ieee80211_flush_tx_queue(local, sdata->vif.txq);
 209 +
 210         if (local->open_count == 0)
 211                 ieee80211_clear_tx_pending(local);
 212
 213 @@ -1773,6 +1776,15 @@ int ieee80211_if_add(struct ieee80211_lo
 214         ieee80211_setup_sdata(sdata, type);
 215
 216         if (ndev) {
 217 +               struct txq_info *txqi = NULL;
 218 +
 219 +               if (local->ops->wake_tx_queue) {
 220 +                       txqi = kzalloc(sizeof(*txqi) +
 221 +                                     local->hw.txq_data_size, GFP_KERNEL);
 222 +                       if (txqi)
 223 +                               ieee80211_init_tx_queue(sdata, NULL, txqi, 0);
 224 +               }
 225 +
 226                 if (params) {
 227                         ndev->ieee80211_ptr->use_4addr = params->use_4addr;
 228                         if (type == NL80211_IFTYPE_STATION)
 229 @@ -1785,6 +1797,7 @@ int ieee80211_if_add(struct ieee80211_lo
 230
 231                 ret = register_netdevice(ndev);
 232                 if (ret) {
 233 +                       kfree(txqi);
 234                         free_netdev(ndev);
 235                         return ret;
 236                 }
 237 @@ -1802,6 +1815,7 @@ int ieee80211_if_add(struct ieee80211_lo
 238
 239  void ieee80211_if_remove(struct ieee80211_sub_if_data *sdata)
 240  {
 241 +       struct txq_info *txqi;
 242         ASSERT_RTNL();
 243
 244         mutex_lock(&sdata->local->iflist_mtx);
 245 @@ -1810,6 +1824,11 @@ void ieee80211_if_remove(struct ieee8021
 246
 247         synchronize_rcu();
 248
 249 +       if (sdata->vif.txq) {
 250 +               txqi = container_of(sdata->vif.txq, struct txq_info, txq);
 251 +               kfree(txqi);
 252 +       }
 253 +
 254         if (sdata->dev) {
 255                 unregister_netdevice(sdata->dev);
 256         } else {
 257 @@ -1833,6 +1852,7 @@ void ieee80211_sdata_stop(struct ieee802
 258  void ieee80211_remove_interfaces(struct ieee80211_local *local)
 259  {
 260         struct ieee80211_sub_if_data *sdata, *tmp;
 261 +       struct txq_info *txqi;
 262         LIST_HEAD(unreg_list);
 263         LIST_HEAD(wdev_list);
 264
 265 @@ -1851,6 +1871,12 @@ void ieee80211_remove_interfaces(struct
 266         list_for_each_entry_safe(sdata, tmp, &local->interfaces, list) {
 267                 list_del(&sdata->list);
 268
 269 +               if (sdata->vif.txq) {
 270 +                       txqi = container_of(sdata->vif.txq, struct txq_info,
 271 +                                          txq);
 272 +                       kfree(txqi);
 273 +               }
 274 +
 275                 if (sdata->dev)
 276                         unregister_netdevice_queue(sdata->dev, &unreg_list);
 277                 else
 278 --- a/net/mac80211/main.c
 279 +++ b/net/mac80211/main.c
 280 @@ -1019,6 +1019,9 @@ int ieee80211_register_hw(struct ieee802
 281
 282         local->dynamic_ps_forced_timeout = -1;
 283
 284 +       if (!local->hw.txq_ac_max_pending)
 285 +               local->hw.txq_ac_max_pending = 64;
 286 +
 287         result = ieee80211_wep_init(local);
 288         if (result < 0)
 289                 wiphy_debug(local->hw.wiphy, "Failed to initialize wep: %d\n",
 290 --- a/net/mac80211/sta_info.c
 291 +++ b/net/mac80211/sta_info.c
 292 @@ -118,6 +118,11 @@ static void __cleanup_single_sta(struct
 293                 atomic_dec(&ps->num_sta_ps);
 294         }
 295
 296 +       if (sta->txqi) {
 297 +               for (i = 0; i < IEEE80211_NUM_TIDS; i++)
 298 +                       ieee80211_flush_tx_queue(local, sta->sta.txq[i]);
 299 +       }
 300 +
 301         for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
 302                 local->total_ps_buffered -= skb_queue_len(&sta->ps_tx_buf[ac]);
 303                 ieee80211_purge_tx_queue(&local->hw, &sta->ps_tx_buf[ac]);
 304 @@ -234,6 +239,7 @@ void sta_info_free(struct ieee80211_loca
 305
 306         sta_dbg(sta->sdata, "Destroyed STA %pM\n", sta->sta.addr);
 307
 308 +       kfree(sta->txqi);
 309         kfree(rcu_dereference_raw(sta->sta.rates));
 310         kfree(sta);
 311  }
 312 @@ -285,11 +291,12 @@ struct sta_info *sta_info_alloc(struct i
 313                                 const u8 *addr, gfp_t gfp)
 314  {
 315         struct ieee80211_local *local = sdata->local;
 316 +       struct ieee80211_hw *hw = &local->hw;
 317         struct sta_info *sta;
 318         struct timespec uptime;
 319         int i;
 320
 321 -       sta = kzalloc(sizeof(*sta) + local->hw.sta_data_size, gfp);
 322 +       sta = kzalloc(sizeof(*sta) + hw->sta_data_size, gfp);
 323         if (!sta)
 324                 return NULL;
 325
 326 @@ -321,11 +328,25 @@ struct sta_info *sta_info_alloc(struct i
 327         for (i = 0; i < ARRAY_SIZE(sta->chain_signal_avg); i++)
 328                 ewma_init(&sta->chain_signal_avg[i], 1024, 8);
 329
 330 -       if (sta_prepare_rate_control(local, sta, gfp)) {
 331 -               kfree(sta);
 332 -               return NULL;
 333 +       if (local->ops->wake_tx_queue) {
 334 +               void *txq_data;
 335 +               int size = sizeof(struct txq_info) +
 336 +                          ALIGN(hw->txq_data_size, sizeof(void *));
 337 +
 338 +               txq_data = kcalloc(IEEE80211_NUM_TIDS, size, gfp);
 339 +               if (!txq_data)
 340 +                       goto free;
 341 +
 342 +               sta->txqi = txq_data;
 343 +               for (i = 0; i < IEEE80211_NUM_TIDS; i++) {
 344 +                       struct txq_info *txq = txq_data + i * size;
 345 +                       ieee80211_init_tx_queue(sdata, sta, txq, i);
 346 +               }
 347         }
 348
 349 +       if (sta_prepare_rate_control(local, sta, gfp))
 350 +               goto free_txq;
 351 +
 352         for (i = 0; i < IEEE80211_NUM_TIDS; i++) {
 353                 /*
 354                  * timer_to_tid must be initialized with identity mapping
 355 @@ -346,7 +367,7 @@ struct sta_info *sta_info_alloc(struct i
 356         if (sdata->vif.type == NL80211_IFTYPE_AP ||
 357             sdata->vif.type == NL80211_IFTYPE_AP_VLAN) {
 358                 struct ieee80211_supported_band *sband =
 359 -                       local->hw.wiphy->bands[ieee80211_get_sdata_band(sdata)];
 360 +                       hw->wiphy->bands[ieee80211_get_sdata_band(sdata)];
 361                 u8 smps = (sband->ht_cap.cap & IEEE80211_HT_CAP_SM_PS) >>
 362                                 IEEE80211_HT_CAP_SM_PS_SHIFT;
 363                 /*
 364 @@ -371,6 +392,12 @@ struct sta_info *sta_info_alloc(struct i
 365         sta_dbg(sdata, "Allocated STA %pM\n", sta->sta.addr);
 366
 367         return sta;
 368 +
 369 +free_txq:
 370 +       kfree(sta->txqi);
 371 +free:
 372 +       kfree(sta);
 373 +       return NULL;
 374  }
 375
 376  static int sta_info_insert_check(struct sta_info *sta)
 377 @@ -640,6 +667,8 @@ static void __sta_info_recalc_tim(struct
 378
 379                 indicate_tim |=
 380                         sta->driver_buffered_tids & tids;
 381 +               indicate_tim |=
 382 +                       sta->txq_buffered_tids & tids;
 383         }
 384
 385   done:
 386 @@ -1071,7 +1100,7 @@ void ieee80211_sta_ps_deliver_wakeup(str
 387         struct ieee80211_sub_if_data *sdata = sta->sdata;
 388         struct ieee80211_local *local = sdata->local;
 389         struct sk_buff_head pending;
 390 -       int filtered = 0, buffered = 0, ac;
 391 +       int filtered = 0, buffered = 0, ac, i;
 392         unsigned long flags;
 393         struct ps_data *ps;
 394
 395 @@ -1090,10 +1119,25 @@ void ieee80211_sta_ps_deliver_wakeup(str
 396
 397         BUILD_BUG_ON(BITS_TO_LONGS(IEEE80211_NUM_TIDS) > 1);
 398         sta->driver_buffered_tids = 0;
 399 +       sta->txq_buffered_tids = 0;
 400
 401         if (!(local->hw.flags & IEEE80211_HW_AP_LINK_PS))
 402                 drv_sta_notify(local, sdata, STA_NOTIFY_AWAKE, &sta->sta);
 403
 404 +       if (sta->txqi) {
 405 +               for (i = 0; i < ARRAY_SIZE(sta->sta.txq); i++) {
 406 +                       struct txq_info *txqi;
 407 +
 408 +                       txqi = container_of(sta->sta.txq[i], struct txq_info,
 409 +                                           txq);
 410 +
 411 +                       if (!skb_queue_len(&txqi->queue))
 412 +                               continue;
 413 +
 414 +                       drv_wake_tx_queue(local, txqi);
 415 +               }
 416 +       }
 417 +
 418         skb_queue_head_init(&pending);
 419
 420         /* sync with ieee80211_tx_h_unicast_ps_buf */
 421 @@ -1254,7 +1298,7 @@ ieee80211_sta_ps_deliver_response(struct
 422         struct ieee80211_sub_if_data *sdata = sta->sdata;
 423         struct ieee80211_local *local = sdata->local;
 424         bool more_data = false;
 425 -       int ac;
 426 +       int ac, tid;
 427         unsigned long driver_release_tids = 0;
 428         struct sk_buff_head frames;
 429
 430 @@ -1275,8 +1319,10 @@ ieee80211_sta_ps_deliver_response(struct
 431                 /* if we already have frames from software, then we can't also
 432                  * release from hardware queues
 433                  */
 434 -               if (skb_queue_empty(&frames))
 435 +               if (skb_queue_empty(&frames)) {
 436                         driver_release_tids |= sta->driver_buffered_tids & tids;
 437 +                       driver_release_tids |= sta->txq_buffered_tids & tids;
 438 +               }
 439
 440                 if (driver_release_tids) {
 441                         /* If the driver has data on more than one TID then
 442 @@ -1447,6 +1493,8 @@ ieee80211_sta_ps_deliver_response(struct
 443
 444                 sta_info_recalc_tim(sta);
 445         } else {
 446 +               unsigned long tids = sta->txq_buffered_tids & driver_release_tids;
 447 +
 448                 /*
 449                  * We need to release a frame that is buffered somewhere in the
 450                  * driver ... it'll have to handle that.
 451 @@ -1466,8 +1514,25 @@ ieee80211_sta_ps_deliver_response(struct
 452                  * that the TID(s) became empty before returning here from the
 453                  * release function.
 454                  * Either way, however, when the driver tells us that the TID(s)
 455 -                * became empty we'll do the TIM recalculation.
 456 +                * became empty or we find that a txq became empty, we'll do the
 457 +                * TIM recalculation.
 458                  */
 459 +
 460 +               if (!sta->txqi)
 461 +                       return;
 462 +
 463 +               for (tid = 0; tid < IEEE80211_NUM_TIDS; tid++) {
 464 +                       struct txq_info *txqi;
 465 +
 466 +                       txqi = container_of(sta->sta.txq[tid], struct txq_info,
 467 +                                           txq);
 468 +
 469 +                       if (!(tids & BIT(tid)) || skb_queue_len(&txqi->queue))
 470 +                               continue;
 471 +
 472 +                       sta_info_recalc_tim(sta);
 473 +                       break;
 474 +               }
 475         }
 476  }
 477
 478 --- a/net/mac80211/sta_info.h
 479 +++ b/net/mac80211/sta_info.h
 480 @@ -274,6 +274,7 @@ struct sta_ampdu_mlme {
 481   *     entered power saving state, these are also delivered to
 482   *     the station when it leaves powersave or polls for frames
 483   * @driver_buffered_tids: bitmap of TIDs the driver has data buffered on
 484 + * @txq_buffered_tids: bitmap of TIDs that mac80211 has txq data buffered on
 485   * @rx_packets: Number of MSDUs received from this STA
 486   * @rx_bytes: Number of bytes received from this STA
 487   * @last_rx: time (in jiffies) when last frame was received from this STA
 488 @@ -368,6 +369,8 @@ struct sta_info {
 489         struct sk_buff_head ps_tx_buf[IEEE80211_NUM_ACS];
 490         struct sk_buff_head tx_filtered[IEEE80211_NUM_ACS];
 491         unsigned long driver_buffered_tids;
 492 +       unsigned long txq_buffered_tids;
 493 +       struct txq_info *txqi;
 494
 495         /* Updated from RX path only, no locking requirements */
 496         unsigned long rx_packets;
 497 --- a/net/mac80211/trace.h
 498 +++ b/net/mac80211/trace.h
 499 @@ -2312,6 +2312,40 @@ TRACE_EVENT(drv_tdls_recv_channel_switch
 500         )
 501  );
 502
 503 +DEFINE_EVENT(local_sdata_evt, drv_wake_vif_tx_queue,
 504 +       TP_PROTO(struct ieee80211_local *local,
 505 +                struct ieee80211_sub_if_data *sdata),
 506 +       TP_ARGS(local, sdata)
 507 +);
 508 +
 509 +TRACE_EVENT(drv_wake_sta_tx_queue,
 510 +       TP_PROTO(struct ieee80211_local *local,
 511 +                struct ieee80211_sub_if_data *sdata,
 512 +                struct ieee80211_sta *sta,
 513 +                u8 tid),
 514 +
 515 +       TP_ARGS(local, sdata, sta, tid),
 516 +
 517 +       TP_STRUCT__entry(
 518 +               LOCAL_ENTRY
 519 +               VIF_ENTRY
 520 +               STA_ENTRY
 521 +               __field(u8, tid)
 522 +       ),
 523 +
 524 +       TP_fast_assign(
 525 +               LOCAL_ASSIGN;
 526 +               VIF_ASSIGN;
 527 +               STA_ASSIGN;
 528 +               __entry->tid = tid;
 529 +       ),
 530 +
 531 +       TP_printk(
 532 +               LOCAL_PR_FMT  VIF_PR_FMT  STA_PR_FMT " tid: 0x%x",
 533 +               LOCAL_PR_ARG, VIF_PR_ARG, STA_PR_ARG, __entry->tid
 534 +       )
 535 +);
 536 +
 537  #ifdef CPTCFG_MAC80211_MESSAGE_TRACING
 538  #undef TRACE_SYSTEM
 539  #define TRACE_SYSTEM mac80211_msg
 540 --- a/net/mac80211/tx.c
 541 +++ b/net/mac80211/tx.c
 542 @@ -1201,13 +1201,80 @@ ieee80211_tx_prepare(struct ieee80211_su
 543         return TX_CONTINUE;
 544  }
 545
 546 +static void ieee80211_drv_tx(struct ieee80211_local *local,
 547 +                            struct ieee80211_vif *vif,
 548 +                            struct ieee80211_sta *pubsta,
 549 +                            struct sk_buff *skb)
 550 +{
 551 +       struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 552 +       struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
 553 +       struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 554 +       struct ieee80211_tx_control control = {
 555 +               .sta = pubsta
 556 +       };
 557 +       struct ieee80211_txq *txq = NULL;
 558 +       struct txq_info *txqi;
 559 +       u8 ac;
 560 +
 561 +       if (info->control.flags & IEEE80211_TX_CTRL_PS_RESPONSE)
 562 +               goto tx_normal;
 563 +
 564 +       if (ieee80211_is_mgmt(hdr->frame_control) ||
 565 +           ieee80211_is_ctl(hdr->frame_control))
 566 +               goto tx_normal;
 567 +
 568 +       if (pubsta) {
 569 +               u8 tid = skb->priority & IEEE80211_QOS_CTL_TID_MASK;
 570 +               txq = pubsta->txq[tid];
 571 +       } else if (vif) {
 572 +               txq = vif->txq;
 573 +       }
 574 +
 575 +       if (!txq)
 576 +               goto tx_normal;
 577 +
 578 +       ac = txq->ac;
 579 +       txqi = container_of(txq, struct txq_info, txq);
 580 +       atomic_inc(&sdata->txqs_len[ac]);
 581 +       if (atomic_read(&sdata->txqs_len[ac]) >= local->hw.txq_ac_max_pending)
 582 +               netif_stop_subqueue(sdata->dev, ac);
 583 +
 584 +       skb_queue_tail(&txqi->queue, skb);
 585 +       drv_wake_tx_queue(local, txqi);
 586 +
 587 +       return;
 588 +
 589 +tx_normal:
 590 +       drv_tx(local, &control, skb);
 591 +}
 592 +
 593 +struct sk_buff *ieee80211_tx_dequeue(struct ieee80211_hw *hw,
 594 +                                    struct ieee80211_txq *txq)
 595 +{
 596 +       struct ieee80211_local *local = hw_to_local(hw);
 597 +       struct ieee80211_sub_if_data *sdata = vif_to_sdata(txq->vif);
 598 +       struct txq_info *txqi = container_of(txq, struct txq_info, txq);
 599 +       struct sk_buff *skb;
 600 +       u8 ac = txq->ac;
 601 +
 602 +       skb = skb_dequeue(&txqi->queue);
 603 +       if (!skb)
 604 +               return ERR_PTR(-EAGAIN);
 605 +
 606 +       atomic_dec(&sdata->txqs_len[ac]);
 607 +       if (__netif_subqueue_stopped(sdata->dev, ac))
 608 +               ieee80211_propagate_queue_wake(local, sdata->vif.hw_queue[ac]);
 609 +
 610 +       return skb;
 611 +}
 612 +EXPORT_SYMBOL(ieee80211_tx_dequeue);
 613 +
 614  static bool ieee80211_tx_frags(struct ieee80211_local *local,
 615                                struct ieee80211_vif *vif,
 616                                struct ieee80211_sta *sta,
 617                                struct sk_buff_head *skbs,
 618                                bool txpending)
 619  {
 620 -       struct ieee80211_tx_control control;
 621         struct sk_buff *skb, *tmp;
 622         unsigned long flags;
 623
 624 @@ -1265,10 +1332,9 @@ static bool ieee80211_tx_frags(struct ie
 625                 spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
 626
 627                 info->control.vif = vif;
 628 -               control.sta = sta;
 629
 630                 __skb_unlink(skb, skbs);
 631 -               drv_tx(local, &control, skb);
 632 +               ieee80211_drv_tx(local, vif, sta, skb);
 633         }
 634
 635         return true;
 636 --- a/net/mac80211/util.c
 637 +++ b/net/mac80211/util.c
 638 @@ -308,6 +308,11 @@ void ieee80211_propagate_queue_wake(stru
 639                 for (ac = 0; ac < n_acs; ac++) {
 640                         int ac_queue = sdata->vif.hw_queue[ac];
 641
 642 +                       if (local->ops->wake_tx_queue &&
 643 +                           (atomic_read(&sdata->txqs_len[ac]) >
 644 +                            local->hw.txq_ac_max_pending))
 645 +                               continue;
 646 +
 647                         if (ac_queue == queue ||
 648                             (sdata->vif.cab_queue == queue &&
 649                              local->queue_stop_reasons[ac_queue] == 0 &&
 650 @@ -3307,3 +3312,36 @@ u8 *ieee80211_add_wmm_info_ie(u8 *buf, u
 651
 652         return buf;
 653  }
 654 +
 655 +void ieee80211_init_tx_queue(struct ieee80211_sub_if_data *sdata,
 656 +                            struct sta_info *sta,
 657 +                            struct txq_info *txqi, int tid)
 658 +{
 659 +       skb_queue_head_init(&txqi->queue);
 660 +       txqi->txq.vif = &sdata->vif;
 661 +
 662 +       if (sta) {
 663 +               txqi->txq.sta = &sta->sta;
 664 +               sta->sta.txq[tid] = &txqi->txq;
 665 +               txqi->txq.ac = ieee802_1d_to_ac[tid & 7];
 666 +       } else {
 667 +               sdata->vif.txq = &txqi->txq;
 668 +               txqi->txq.ac = IEEE80211_AC_BE;
 669 +       }
 670 +}
 671 +
 672 +void ieee80211_flush_tx_queue(struct ieee80211_local *local,
 673 +                             struct ieee80211_txq *txq)
 674 +{
 675 +       struct txq_info *txqi = container_of(txq, struct txq_info, txq);
 676 +       struct ieee80211_sub_if_data *sdata = vif_to_sdata(txq->vif);
 677 +       struct sk_buff *skb;
 678 +       int n = 0;
 679 +
 680 +       while ((skb = skb_dequeue(&txqi->queue)) != NULL) {
 681 +               n++;
 682 +               ieee80211_free_txskb(&local->hw, skb);
 683 +       }
 684 +
 685 +       atomic_sub(n, &sdata->txqs_len[txq->ac]);
 686 +}
 687 --- a/net/mac80211/rx.c
 688 +++ b/net/mac80211/rx.c
 689 @@ -1176,6 +1176,7 @@ static void sta_ps_start(struct sta_info
 690         struct ieee80211_sub_if_data *sdata = sta->sdata;
 691         struct ieee80211_local *local = sdata->local;
 692         struct ps_data *ps;
 693 +       int tid;
 694
 695         if (sta->sdata->vif.type == NL80211_IFTYPE_AP ||
 696             sta->sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
 697 @@ -1189,6 +1190,20 @@ static void sta_ps_start(struct sta_info
 698                 drv_sta_notify(local, sdata, STA_NOTIFY_SLEEP, &sta->sta);
 699         ps_dbg(sdata, "STA %pM aid %d enters power save mode\n",
 700                sta->sta.addr, sta->sta.aid);
 701 +
 702 +       if (!sta->txqi)
 703 +               return;
 704 +
 705 +       for (tid = 0; tid < IEEE80211_NUM_TIDS; tid++) {
 706 +               struct txq_info *txqi;
 707 +
 708 +               txqi = container_of(sta->sta.txq[tid], struct txq_info, txq);
 709 +
 710 +               if (!skb_queue_len(&txqi->queue))
 711 +                       set_bit(tid, &sta->txq_buffered_tids);
 712 +               else
 713 +                       clear_bit(tid, &sta->txq_buffered_tids);
 714 +       }
 715  }
 716
 717  static void sta_ps_end(struct sta_info *sta)