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 @@ -84,6 +84,39 @@
  23   *
  24   */
  25
  26 +/**
  27 + * DOC: mac80211 software tx queueing
  28 + *
  29 + * mac80211 provides an optional intermediate queueing implementation designed
  30 + * to allow the driver to keep hardware queues short and provide some fairness
  31 + * between different stations/interfaces.
  32 + * In this model, the driver pulls data frames from the mac80211 queue instead
  33 + * of letting mac80211 push them via drv_tx().
  34 + * Other frames (e.g. control or management) are still pushed using drv_tx().
  35 + *
  36 + * Drivers indicate that they use this model by implementing the .wake_tx_queue
  37 + * driver operation.
  38 + *
  39 + * Intermediate queues (struct ieee80211_txq) are kept per-sta per-tid, with a
  40 + * single per-vif queue for multicast data frames.
  41 + *
  42 + * The driver is expected to initialize its private per-queue data for stations
  43 + * and interfaces in the .add_interface and .sta_add ops.
  44 + *
  45 + * The driver can't access the queue directly. To dequeue a frame, it calls
  46 + * ieee80211_tx_dequeue(). Whenever mac80211 adds a new frame to a queue, it
  47 + * calls the .wake_tx_queue driver op.
  48 + *
  49 + * For AP powersave TIM handling, the driver only needs to indicate if it has
  50 + * buffered packets in the driver specific data structures by calling
  51 + * ieee80211_sta_set_buffered(). For frames buffered in the ieee80211_txq
  52 + * struct, mac80211 sets the appropriate TIM PVB bits and calls
  53 + * .release_buffered_frames().
  54 + * In that callback the driver is therefore expected to release its own
  55 + * buffered frames and afterwards also frames from the ieee80211_txq (obtained
  56 + * via the usual ieee80211_tx_dequeue).
  57 + */
  58 +
  59  struct device;
  60
  61  /**
  62 @@ -1246,6 +1279,7 @@ enum ieee80211_vif_flags {
  63   *     monitor interface (if that is requested.)
  64   * @drv_priv: data area for driver use, will always be aligned to
  65   *     sizeof(void *).
  66 + * @txq: the multicast data TX queue (if driver uses the TXQ abstraction)
  67   */
  68  struct ieee80211_vif {
  69         enum nl80211_iftype type;
  70 @@ -1257,6 +1291,8 @@ struct ieee80211_vif {
  71         u8 cab_queue;
  72         u8 hw_queue[IEEE80211_NUM_ACS];
  73
  74 +       struct ieee80211_txq *txq;
  75 +
  76         struct ieee80211_chanctx_conf __rcu *chanctx_conf;
  77
  78         u32 driver_flags;
  79 @@ -1501,6 +1537,7 @@ struct ieee80211_sta_rates {
  80   * @tdls_initiator: indicates the STA is an initiator of the TDLS link. Only
  81   *     valid if the STA is a TDLS peer in the first place.
  82   * @mfp: indicates whether the STA uses management frame protection or not.
  83 + * @txq: per-TID data TX queues (if driver uses the TXQ abstraction)
  84   */
  85  struct ieee80211_sta {
  86         u32 supp_rates[IEEE80211_NUM_BANDS];
  87 @@ -1519,6 +1556,8 @@ struct ieee80211_sta {
  88         bool tdls_initiator;
  89         bool mfp;
  90
  91 +       struct ieee80211_txq *txq[IEEE80211_NUM_TIDS];
  92 +
  93         /* must be last */
  94         u8 drv_priv[0] __aligned(sizeof(void *));
  95  };
  96 @@ -1547,6 +1586,27 @@ struct ieee80211_tx_control {
  97  };
  98
  99  /**
 100 + * struct ieee80211_txq - Software intermediate tx queue
 101 + *
 102 + * @vif: &struct ieee80211_vif pointer from the add_interface callback.
 103 + * @sta: station table entry, %NULL for per-vif queue
 104 + * @tid: the TID for this queue (unused for per-vif queue)
 105 + * @ac: the AC for this queue
 106 + *
 107 + * The driver can obtain packets from this queue by calling
 108 + * ieee80211_tx_dequeue().
 109 + */
 110 +struct ieee80211_txq {
 111 +       struct ieee80211_vif *vif;
 112 +       struct ieee80211_sta *sta;
 113 +       u8 tid;
 114 +       u8 ac;
 115 +
 116 +       /* must be last */
 117 +       u8 drv_priv[0] __aligned(sizeof(void *));
 118 +};
 119 +
 120 +/**
 121   * enum ieee80211_hw_flags - hardware flags
 122   *
 123   * These flags are used to indicate hardware capabilities to
 124 @@ -1770,6 +1830,8 @@ enum ieee80211_hw_flags {
 125   *     within &struct ieee80211_sta.
 126   * @chanctx_data_size: size (in bytes) of the drv_priv data area
 127   *     within &struct ieee80211_chanctx_conf.
 128 + * @txq_data_size: size (in bytes) of the drv_priv data area
 129 + *     within @struct ieee80211_txq.
 130   *
 131   * @max_rates: maximum number of alternate rate retry stages the hw
 132   *     can handle.
 133 @@ -1818,6 +1880,9 @@ enum ieee80211_hw_flags {
 134   * @n_cipher_schemes: a size of an array of cipher schemes definitions.
 135   * @cipher_schemes: a pointer to an array of cipher scheme definitions
 136   *     supported by HW.
 137 + *
 138 + * @txq_ac_max_pending: maximum number of frames per AC pending in all txq
 139 + *     entries for a vif.
 140   */
 141  struct ieee80211_hw {
 142         struct ieee80211_conf conf;
 143 @@ -1830,6 +1895,7 @@ struct ieee80211_hw {
 144         int vif_data_size;
 145         int sta_data_size;
 146         int chanctx_data_size;
 147 +       int txq_data_size;
 148         u16 queues;
 149         u16 max_listen_interval;
 150         s8 max_signal;
 151 @@ -1846,6 +1912,7 @@ struct ieee80211_hw {
 152         u8 uapsd_max_sp_len;
 153         u8 n_cipher_schemes;
 154         const struct ieee80211_cipher_scheme *cipher_schemes;
 155 +       int txq_ac_max_pending;
 156  };
 157
 158  /**
 159 @@ -3007,6 +3074,8 @@ enum ieee80211_reconfig_type {
 160   *     response template is provided, together with the location of the
 161   *     switch-timing IE within the template. The skb can only be used within
 162   *     the function call.
 163 + *
 164 + * @wake_tx_queue: Called when new packets have been added to the queue.
 165   */
 166  struct ieee80211_ops {
 167         void (*tx)(struct ieee80211_hw *hw,
 168 @@ -3238,6 +3307,9 @@ struct ieee80211_ops {
 169         void (*tdls_recv_channel_switch)(struct ieee80211_hw *hw,
 170                                          struct ieee80211_vif *vif,
 171                                          struct ieee80211_tdls_ch_sw_params *params);
 172 +
 173 +       void (*wake_tx_queue)(struct ieee80211_hw *hw,
 174 +                             struct ieee80211_txq *txq);
 175  };
 176
 177  /**
 178 @@ -5249,4 +5321,15 @@ void ieee80211_unreserve_tid(struct ieee
 179   */
 180  size_t ieee80211_ie_split(const u8 *ies, size_t ielen,
 181                           const u8 *ids, int n_ids, size_t offset);
 182 +
 183 +/**
 184 + * ieee80211_tx_dequeue - dequeue a packet from a software tx queue
 185 + *
 186 + * @hw: pointer as obtained from ieee80211_alloc_hw()
 187 + * @txq: pointer obtained from station or virtual interface
 188 + *
 189 + * Returns the skb if successful, %NULL if no frame was available.
 190 + */
 191 +struct sk_buff *ieee80211_tx_dequeue(struct ieee80211_hw *hw,
 192 +                                    struct ieee80211_txq *txq);
 193  #endif /* MAC80211_H */
 194 --- a/net/mac80211/driver-ops.h
 195 +++ b/net/mac80211/driver-ops.h
 196 @@ -1367,4 +1367,16 @@ drv_tdls_recv_channel_switch(struct ieee
 197         trace_drv_return_void(local);
 198  }
 199
 200 +static inline void drv_wake_tx_queue(struct ieee80211_local *local,
 201 +                                    struct txq_info *txq)
 202 +{
 203 +       struct ieee80211_sub_if_data *sdata = vif_to_sdata(txq->txq.vif);
 204 +
 205 +       if (!check_sdata_in_driver(sdata))
 206 +               return;
 207 +
 208 +       trace_drv_wake_tx_queue(local, sdata, txq);
 209 +       local->ops->wake_tx_queue(&local->hw, &txq->txq);
 210 +}
 211 +
 212  #endif /* __MAC80211_DRIVER_OPS */
 213 --- a/net/mac80211/ieee80211_i.h
 214 +++ b/net/mac80211/ieee80211_i.h
 215 @@ -809,6 +809,19 @@ struct mac80211_qos_map {
 216         struct rcu_head rcu_head;
 217  };
 218
 219 +enum txq_info_flags {
 220 +       IEEE80211_TXQ_STOP,
 221 +       IEEE80211_TXQ_AMPDU,
 222 +};
 223 +
 224 +struct txq_info {
 225 +       struct sk_buff_head queue;
 226 +       unsigned long flags;
 227 +
 228 +       /* keep last! */
 229 +       struct ieee80211_txq txq;
 230 +};
 231 +
 232  struct ieee80211_sub_if_data {
 233         struct list_head list;
 234
 235 @@ -853,6 +866,7 @@ struct ieee80211_sub_if_data {
 236         bool control_port_no_encrypt;
 237         int encrypt_headroom;
 238
 239 +       atomic_t txqs_len[IEEE80211_NUM_ACS];
 240         struct ieee80211_tx_queue_params tx_conf[IEEE80211_NUM_ACS];
 241         struct mac80211_qos_map __rcu *qos_map;
 242
 243 @@ -1453,6 +1467,10 @@ static inline struct ieee80211_local *hw
 244         return container_of(hw, struct ieee80211_local, hw);
 245  }
 246
 247 +static inline struct txq_info *to_txq_info(struct ieee80211_txq *txq)
 248 +{
 249 +       return container_of(txq, struct txq_info, txq);
 250 +}
 251
 252  static inline int ieee80211_bssid_match(const u8 *raddr, const u8 *addr)
 253  {
 254 @@ -1905,6 +1923,9 @@ static inline bool ieee80211_can_run_wor
 255         return true;
 256  }
 257
 258 +void ieee80211_init_tx_queue(struct ieee80211_sub_if_data *sdata,
 259 +                            struct sta_info *sta,
 260 +                            struct txq_info *txq, int tid);
 261  void ieee80211_send_auth(struct ieee80211_sub_if_data *sdata,
 262                          u16 transaction, u16 auth_alg, u16 status,
 263                          const u8 *extra, size_t extra_len, const u8 *bssid,
 264 --- a/net/mac80211/iface.c
 265 +++ b/net/mac80211/iface.c
 266 @@ -969,6 +969,13 @@ static void ieee80211_do_stop(struct iee
 267         }
 268         spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
 269
 270 +       if (sdata->vif.txq) {
 271 +               struct txq_info *txqi = to_txq_info(sdata->vif.txq);
 272 +
 273 +               ieee80211_purge_tx_queue(&local->hw, &txqi->queue);
 274 +               atomic_set(&sdata->txqs_len[txqi->txq.ac], 0);
 275 +       }
 276 +
 277         if (local->open_count == 0)
 278                 ieee80211_clear_tx_pending(local);
 279
 280 @@ -1674,6 +1681,7 @@ int ieee80211_if_add(struct ieee80211_lo
 281  {
 282         struct net_device *ndev = NULL;
 283         struct ieee80211_sub_if_data *sdata = NULL;
 284 +       struct txq_info *txqi;
 285         int ret, i;
 286         int txqs = 1;
 287
 288 @@ -1693,10 +1701,18 @@ int ieee80211_if_add(struct ieee80211_lo
 289                 ieee80211_assign_perm_addr(local, wdev->address, type);
 290                 memcpy(sdata->vif.addr, wdev->address, ETH_ALEN);
 291         } else {
 292 +               int size = ALIGN(sizeof(*sdata) + local->hw.vif_data_size,
 293 +                                sizeof(void *));
 294 +               int txq_size = 0;
 295 +
 296 +               if (local->ops->wake_tx_queue)
 297 +                       txq_size += sizeof(struct txq_info) +
 298 +                                   local->hw.txq_data_size;
 299 +
 300                 if (local->hw.queues >= IEEE80211_NUM_ACS)
 301                         txqs = IEEE80211_NUM_ACS;
 302
 303 -               ndev = alloc_netdev_mqs(sizeof(*sdata) + local->hw.vif_data_size,
 304 +               ndev = alloc_netdev_mqs(size + txq_size,
 305                                         name, NET_NAME_UNKNOWN,
 306                                         ieee80211_if_setup, txqs, 1);
 307                 if (!ndev)
 308 @@ -1731,6 +1747,11 @@ int ieee80211_if_add(struct ieee80211_lo
 309                 memcpy(sdata->vif.addr, ndev->dev_addr, ETH_ALEN);
 310                 memcpy(sdata->name, ndev->name, IFNAMSIZ);
 311
 312 +               if (txq_size) {
 313 +                       txqi = netdev_priv(ndev) + size;
 314 +                       ieee80211_init_tx_queue(sdata, NULL, txqi, 0);
 315 +               }
 316 +
 317                 sdata->dev = ndev;
 318         }
 319
 320 --- a/net/mac80211/main.c
 321 +++ b/net/mac80211/main.c
 322 @@ -1019,6 +1019,9 @@ int ieee80211_register_hw(struct ieee802
 323
 324         local->dynamic_ps_forced_timeout = -1;
 325
 326 +       if (!local->hw.txq_ac_max_pending)
 327 +               local->hw.txq_ac_max_pending = 64;
 328 +
 329         result = ieee80211_wep_init(local);
 330         if (result < 0)
 331                 wiphy_debug(local->hw.wiphy, "Failed to initialize wep: %d\n",
 332 --- a/net/mac80211/sta_info.c
 333 +++ b/net/mac80211/sta_info.c
 334 @@ -118,6 +118,16 @@ static void __cleanup_single_sta(struct
 335                 atomic_dec(&ps->num_sta_ps);
 336         }
 337
 338 +       if (sta->sta.txq[0]) {
 339 +               for (i = 0; i < ARRAY_SIZE(sta->sta.txq); i++) {
 340 +                       struct txq_info *txqi = to_txq_info(sta->sta.txq[i]);
 341 +                       int n = skb_queue_len(&txqi->queue);
 342 +
 343 +                       ieee80211_purge_tx_queue(&local->hw, &txqi->queue);
 344 +                       atomic_sub(n, &sdata->txqs_len[txqi->txq.ac]);
 345 +               }
 346 +       }
 347 +
 348         for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
 349                 local->total_ps_buffered -= skb_queue_len(&sta->ps_tx_buf[ac]);
 350                 ieee80211_purge_tx_queue(&local->hw, &sta->ps_tx_buf[ac]);
 351 @@ -234,6 +244,8 @@ void sta_info_free(struct ieee80211_loca
 352
 353         sta_dbg(sta->sdata, "Destroyed STA %pM\n", sta->sta.addr);
 354
 355 +       if (sta->sta.txq[0])
 356 +               kfree(to_txq_info(sta->sta.txq[0]));
 357         kfree(rcu_dereference_raw(sta->sta.rates));
 358         kfree(sta);
 359  }
 360 @@ -285,11 +297,12 @@ struct sta_info *sta_info_alloc(struct i
 361                                 const u8 *addr, gfp_t gfp)
 362  {
 363         struct ieee80211_local *local = sdata->local;
 364 +       struct ieee80211_hw *hw = &local->hw;
 365         struct sta_info *sta;
 366         struct timespec uptime;
 367         int i;
 368
 369 -       sta = kzalloc(sizeof(*sta) + local->hw.sta_data_size, gfp);
 370 +       sta = kzalloc(sizeof(*sta) + hw->sta_data_size, gfp);
 371         if (!sta)
 372                 return NULL;
 373
 374 @@ -321,11 +334,25 @@ struct sta_info *sta_info_alloc(struct i
 375         for (i = 0; i < ARRAY_SIZE(sta->chain_signal_avg); i++)
 376                 ewma_init(&sta->chain_signal_avg[i], 1024, 8);
 377
 378 -       if (sta_prepare_rate_control(local, sta, gfp)) {
 379 -               kfree(sta);
 380 -               return NULL;
 381 +       if (local->ops->wake_tx_queue) {
 382 +               void *txq_data;
 383 +               int size = sizeof(struct txq_info) +
 384 +                          ALIGN(hw->txq_data_size, sizeof(void *));
 385 +
 386 +               txq_data = kcalloc(ARRAY_SIZE(sta->sta.txq), size, gfp);
 387 +               if (!txq_data)
 388 +                       goto free;
 389 +
 390 +               for (i = 0; i < ARRAY_SIZE(sta->sta.txq); i++) {
 391 +                       struct txq_info *txq = txq_data + i * size;
 392 +
 393 +                       ieee80211_init_tx_queue(sdata, sta, txq, i);
 394 +               }
 395         }
 396
 397 +       if (sta_prepare_rate_control(local, sta, gfp))
 398 +               goto free_txq;
 399 +
 400         for (i = 0; i < IEEE80211_NUM_TIDS; i++) {
 401                 /*
 402                  * timer_to_tid must be initialized with identity mapping
 403 @@ -346,7 +373,7 @@ struct sta_info *sta_info_alloc(struct i
 404         if (sdata->vif.type == NL80211_IFTYPE_AP ||
 405             sdata->vif.type == NL80211_IFTYPE_AP_VLAN) {
 406                 struct ieee80211_supported_band *sband =
 407 -                       local->hw.wiphy->bands[ieee80211_get_sdata_band(sdata)];
 408 +                       hw->wiphy->bands[ieee80211_get_sdata_band(sdata)];
 409                 u8 smps = (sband->ht_cap.cap & IEEE80211_HT_CAP_SM_PS) >>
 410                                 IEEE80211_HT_CAP_SM_PS_SHIFT;
 411                 /*
 412 @@ -371,6 +398,13 @@ struct sta_info *sta_info_alloc(struct i
 413         sta_dbg(sdata, "Allocated STA %pM\n", sta->sta.addr);
 414
 415         return sta;
 416 +
 417 +free_txq:
 418 +       if (sta->sta.txq[0])
 419 +               kfree(to_txq_info(sta->sta.txq[0]));
 420 +free:
 421 +       kfree(sta);
 422 +       return NULL;
 423  }
 424
 425  static int sta_info_insert_check(struct sta_info *sta)
 426 @@ -640,6 +674,8 @@ static void __sta_info_recalc_tim(struct
 427
 428                 indicate_tim |=
 429                         sta->driver_buffered_tids & tids;
 430 +               indicate_tim |=
 431 +                       sta->txq_buffered_tids & tids;
 432         }
 433
 434   done:
 435 @@ -1071,7 +1107,7 @@ void ieee80211_sta_ps_deliver_wakeup(str
 436         struct ieee80211_sub_if_data *sdata = sta->sdata;
 437         struct ieee80211_local *local = sdata->local;
 438         struct sk_buff_head pending;
 439 -       int filtered = 0, buffered = 0, ac;
 440 +       int filtered = 0, buffered = 0, ac, i;
 441         unsigned long flags;
 442         struct ps_data *ps;
 443
 444 @@ -1090,10 +1126,22 @@ void ieee80211_sta_ps_deliver_wakeup(str
 445
 446         BUILD_BUG_ON(BITS_TO_LONGS(IEEE80211_NUM_TIDS) > 1);
 447         sta->driver_buffered_tids = 0;
 448 +       sta->txq_buffered_tids = 0;
 449
 450         if (!(local->hw.flags & IEEE80211_HW_AP_LINK_PS))
 451                 drv_sta_notify(local, sdata, STA_NOTIFY_AWAKE, &sta->sta);
 452
 453 +       if (sta->sta.txq[0]) {
 454 +               for (i = 0; i < ARRAY_SIZE(sta->sta.txq); i++) {
 455 +                       struct txq_info *txqi = to_txq_info(sta->sta.txq[i]);
 456 +
 457 +                       if (!skb_queue_len(&txqi->queue))
 458 +                               continue;
 459 +
 460 +                       drv_wake_tx_queue(local, txqi);
 461 +               }
 462 +       }
 463 +
 464         skb_queue_head_init(&pending);
 465
 466         /* sync with ieee80211_tx_h_unicast_ps_buf */
 467 @@ -1275,8 +1323,10 @@ ieee80211_sta_ps_deliver_response(struct
 468                 /* if we already have frames from software, then we can't also
 469                  * release from hardware queues
 470                  */
 471 -               if (skb_queue_empty(&frames))
 472 +               if (skb_queue_empty(&frames)) {
 473                         driver_release_tids |= sta->driver_buffered_tids & tids;
 474 +                       driver_release_tids |= sta->txq_buffered_tids & tids;
 475 +               }
 476
 477                 if (driver_release_tids) {
 478                         /* If the driver has data on more than one TID then
 479 @@ -1447,6 +1497,9 @@ ieee80211_sta_ps_deliver_response(struct
 480
 481                 sta_info_recalc_tim(sta);
 482         } else {
 483 +               unsigned long tids = sta->txq_buffered_tids & driver_release_tids;
 484 +               int tid;
 485 +
 486                 /*
 487                  * We need to release a frame that is buffered somewhere in the
 488                  * driver ... it'll have to handle that.
 489 @@ -1466,8 +1519,22 @@ ieee80211_sta_ps_deliver_response(struct
 490                  * that the TID(s) became empty before returning here from the
 491                  * release function.
 492                  * Either way, however, when the driver tells us that the TID(s)
 493 -                * became empty we'll do the TIM recalculation.
 494 +                * became empty or we find that a txq became empty, we'll do the
 495 +                * TIM recalculation.
 496                  */
 497 +
 498 +               if (!sta->sta.txq[0])
 499 +                       return;
 500 +
 501 +               for (tid = 0; tid < ARRAY_SIZE(sta->sta.txq); tid++) {
 502 +                       struct txq_info *txqi = to_txq_info(sta->sta.txq[tid]);
 503 +
 504 +                       if (!(tids & BIT(tid)) || skb_queue_len(&txqi->queue))
 505 +                               continue;
 506 +
 507 +                       sta_info_recalc_tim(sta);
 508 +                       break;
 509 +               }
 510         }
 511  }
 512
 513 --- a/net/mac80211/sta_info.h
 514 +++ b/net/mac80211/sta_info.h
 515 @@ -274,6 +274,7 @@ struct sta_ampdu_mlme {
 516   *     entered power saving state, these are also delivered to
 517   *     the station when it leaves powersave or polls for frames
 518   * @driver_buffered_tids: bitmap of TIDs the driver has data buffered on
 519 + * @txq_buffered_tids: bitmap of TIDs that mac80211 has txq data buffered on
 520   * @rx_packets: Number of MSDUs received from this STA
 521   * @rx_bytes: Number of bytes received from this STA
 522   * @last_rx: time (in jiffies) when last frame was received from this STA
 523 @@ -368,6 +369,7 @@ struct sta_info {
 524         struct sk_buff_head ps_tx_buf[IEEE80211_NUM_ACS];
 525         struct sk_buff_head tx_filtered[IEEE80211_NUM_ACS];
 526         unsigned long driver_buffered_tids;
 527 +       unsigned long txq_buffered_tids;
 528
 529         /* Updated from RX path only, no locking requirements */
 530         unsigned long rx_packets;
 531 --- a/net/mac80211/trace.h
 532 +++ b/net/mac80211/trace.h
 533 @@ -2312,6 +2312,37 @@ TRACE_EVENT(drv_tdls_recv_channel_switch
 534         )
 535  );
 536
 537 +TRACE_EVENT(drv_wake_tx_queue,
 538 +       TP_PROTO(struct ieee80211_local *local,
 539 +                struct ieee80211_sub_if_data *sdata,
 540 +                struct txq_info *txq),
 541 +
 542 +       TP_ARGS(local, sdata, txq),
 543 +
 544 +       TP_STRUCT__entry(
 545 +               LOCAL_ENTRY
 546 +               VIF_ENTRY
 547 +               STA_ENTRY
 548 +               __field(u8, ac)
 549 +               __field(u8, tid)
 550 +       ),
 551 +
 552 +       TP_fast_assign(
 553 +               struct ieee80211_sta *sta = txq->txq.sta;
 554 +
 555 +               LOCAL_ASSIGN;
 556 +               VIF_ASSIGN;
 557 +               STA_ASSIGN;
 558 +               __entry->ac = txq->txq.ac;
 559 +               __entry->tid = txq->txq.tid;
 560 +       ),
 561 +
 562 +       TP_printk(
 563 +               LOCAL_PR_FMT  VIF_PR_FMT  STA_PR_FMT " ac:%d tid:%d",
 564 +               LOCAL_PR_ARG, VIF_PR_ARG, STA_PR_ARG, __entry->ac, __entry->tid
 565 +       )
 566 +);
 567 +
 568  #ifdef CPTCFG_MAC80211_MESSAGE_TRACING
 569  #undef TRACE_SYSTEM
 570  #define TRACE_SYSTEM mac80211_msg
 571 --- a/net/mac80211/tx.c
 572 +++ b/net/mac80211/tx.c
 573 @@ -776,12 +776,22 @@ ieee80211_tx_h_rate_ctrl(struct ieee8021
 574         return TX_CONTINUE;
 575  }
 576
 577 +static __le16 ieee80211_tx_next_seq(struct sta_info *sta, int tid)
 578 +{
 579 +       u16 *seq = &sta->tid_seq[tid];
 580 +       __le16 ret = cpu_to_le16(*seq);
 581 +
 582 +       /* Increase the sequence number. */
 583 +       *seq = (*seq + 0x10) & IEEE80211_SCTL_SEQ;
 584 +
 585 +       return ret;
 586 +}
 587 +
 588  static ieee80211_tx_result debug_noinline
 589  ieee80211_tx_h_sequence(struct ieee80211_tx_data *tx)
 590  {
 591         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
 592         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx->skb->data;
 593 -       u16 *seq;
 594         u8 *qc;
 595         int tid;
 596
 597 @@ -832,13 +842,10 @@ ieee80211_tx_h_sequence(struct ieee80211
 598
 599         qc = ieee80211_get_qos_ctl(hdr);
 600         tid = *qc & IEEE80211_QOS_CTL_TID_MASK;
 601 -       seq = &tx->sta->tid_seq[tid];
 602         tx->sta->tx_msdu[tid]++;
 603
 604 -       hdr->seq_ctrl = cpu_to_le16(*seq);
 605 -
 606 -       /* Increase the sequence number. */
 607 -       *seq = (*seq + 0x10) & IEEE80211_SCTL_SEQ;
 608 +       if (!tx->sta->sta.txq[0])
 609 +               hdr->seq_ctrl = ieee80211_tx_next_seq(tx->sta, tid);
 610
 611         return TX_CONTINUE;
 612  }
 613 @@ -1067,7 +1074,7 @@ static bool ieee80211_tx_prep_agg(struct
 614                  * nothing -- this aggregation session is being started
 615                  * but that might still fail with the driver
 616                  */
 617 -       } else {
 618 +       } else if (!tx->sta->sta.txq[tid]) {
 619                 spin_lock(&tx->sta->lock);
 620                 /*
 621                  * Need to re-check now, because we may get here
 622 @@ -1201,13 +1208,102 @@ ieee80211_tx_prepare(struct ieee80211_su
 623         return TX_CONTINUE;
 624  }
 625
 626 +static void ieee80211_drv_tx(struct ieee80211_local *local,
 627 +                            struct ieee80211_vif *vif,
 628 +                            struct ieee80211_sta *pubsta,
 629 +                            struct sk_buff *skb)
 630 +{
 631 +       struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 632 +       struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
 633 +       struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 634 +       struct ieee80211_tx_control control = {
 635 +               .sta = pubsta,
 636 +       };
 637 +       struct ieee80211_txq *txq = NULL;
 638 +       struct txq_info *txqi;
 639 +       u8 ac;
 640 +
 641 +       if (info->control.flags & IEEE80211_TX_CTRL_PS_RESPONSE)
 642 +               goto tx_normal;
 643 +
 644 +       if (!ieee80211_is_data(hdr->frame_control))
 645 +               goto tx_normal;
 646 +
 647 +       if (pubsta) {
 648 +               u8 tid = skb->priority & IEEE80211_QOS_CTL_TID_MASK;
 649 +
 650 +               txq = pubsta->txq[tid];
 651 +       } else if (vif) {
 652 +               txq = vif->txq;
 653 +       }
 654 +
 655 +       if (!txq)
 656 +               goto tx_normal;
 657 +
 658 +       ac = txq->ac;
 659 +       txqi = to_txq_info(txq);
 660 +       atomic_inc(&sdata->txqs_len[ac]);
 661 +       if (atomic_read(&sdata->txqs_len[ac]) >= local->hw.txq_ac_max_pending)
 662 +               netif_stop_subqueue(sdata->dev, ac);
 663 +
 664 +       skb_queue_tail(&txqi->queue, skb);
 665 +       drv_wake_tx_queue(local, txqi);
 666 +
 667 +       return;
 668 +
 669 +tx_normal:
 670 +       drv_tx(local, &control, skb);
 671 +}
 672 +
 673 +struct sk_buff *ieee80211_tx_dequeue(struct ieee80211_hw *hw,
 674 +                                    struct ieee80211_txq *txq)
 675 +{
 676 +       struct ieee80211_local *local = hw_to_local(hw);
 677 +       struct ieee80211_sub_if_data *sdata = vif_to_sdata(txq->vif);
 678 +       struct txq_info *txqi = container_of(txq, struct txq_info, txq);
 679 +       struct ieee80211_hdr *hdr;
 680 +       struct sk_buff *skb = NULL;
 681 +       u8 ac = txq->ac;
 682 +
 683 +       spin_lock_bh(&txqi->queue.lock);
 684 +
 685 +       if (test_bit(IEEE80211_TXQ_STOP, &txqi->flags))
 686 +               goto out;
 687 +
 688 +       skb = __skb_dequeue(&txqi->queue);
 689 +       if (!skb)
 690 +               goto out;
 691 +
 692 +       atomic_dec(&sdata->txqs_len[ac]);
 693 +       if (__netif_subqueue_stopped(sdata->dev, ac))
 694 +               ieee80211_propagate_queue_wake(local, sdata->vif.hw_queue[ac]);
 695 +
 696 +       hdr = (struct ieee80211_hdr *)skb->data;
 697 +       if (txq->sta && ieee80211_is_data_qos(hdr->frame_control)) {
 698 +               struct sta_info *sta = container_of(txq->sta, struct sta_info,
 699 +                                                   sta);
 700 +               struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 701 +
 702 +               hdr->seq_ctrl = ieee80211_tx_next_seq(sta, txq->tid);
 703 +               if (test_bit(IEEE80211_TXQ_AMPDU, &txqi->flags))
 704 +                       info->flags |= IEEE80211_TX_CTL_AMPDU;
 705 +               else
 706 +                       info->flags &= ~IEEE80211_TX_CTL_AMPDU;
 707 +       }
 708 +
 709 +out:
 710 +       spin_unlock_bh(&txqi->queue.lock);
 711 +
 712 +       return skb;
 713 +}
 714 +EXPORT_SYMBOL(ieee80211_tx_dequeue);
 715 +
 716  static bool ieee80211_tx_frags(struct ieee80211_local *local,
 717                                struct ieee80211_vif *vif,
 718                                struct ieee80211_sta *sta,
 719                                struct sk_buff_head *skbs,
 720                                bool txpending)
 721  {
 722 -       struct ieee80211_tx_control control;
 723         struct sk_buff *skb, *tmp;
 724         unsigned long flags;
 725
 726 @@ -1265,10 +1361,9 @@ static bool ieee80211_tx_frags(struct ie
 727                 spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
 728
 729                 info->control.vif = vif;
 730 -               control.sta = sta;
 731
 732                 __skb_unlink(skb, skbs);
 733 -               drv_tx(local, &control, skb);
 734 +               ieee80211_drv_tx(local, vif, sta, skb);
 735         }
 736
 737         return true;
 738 --- a/net/mac80211/util.c
 739 +++ b/net/mac80211/util.c
 740 @@ -308,6 +308,11 @@ void ieee80211_propagate_queue_wake(stru
 741                 for (ac = 0; ac < n_acs; ac++) {
 742                         int ac_queue = sdata->vif.hw_queue[ac];
 743
 744 +                       if (local->ops->wake_tx_queue &&
 745 +                           (atomic_read(&sdata->txqs_len[ac]) >
 746 +                            local->hw.txq_ac_max_pending))
 747 +                               continue;
 748 +
 749                         if (ac_queue == queue ||
 750                             (sdata->vif.cab_queue == queue &&
 751                              local->queue_stop_reasons[ac_queue] == 0 &&
 752 @@ -3307,3 +3312,20 @@ u8 *ieee80211_add_wmm_info_ie(u8 *buf, u
 753
 754         return buf;
 755  }
 756 +
 757 +void ieee80211_init_tx_queue(struct ieee80211_sub_if_data *sdata,
 758 +                            struct sta_info *sta,
 759 +                            struct txq_info *txqi, int tid)
 760 +{
 761 +       skb_queue_head_init(&txqi->queue);
 762 +       txqi->txq.vif = &sdata->vif;
 763 +
 764 +       if (sta) {
 765 +               txqi->txq.sta = &sta->sta;
 766 +               sta->sta.txq[tid] = &txqi->txq;
 767 +               txqi->txq.ac = ieee802_1d_to_ac[tid & 7];
 768 +       } else {
 769 +               sdata->vif.txq = &txqi->txq;
 770 +               txqi->txq.ac = IEEE80211_AC_BE;
 771 +       }
 772 +}
 773 --- a/net/mac80211/rx.c
 774 +++ b/net/mac80211/rx.c
 775 @@ -1176,6 +1176,7 @@ static void sta_ps_start(struct sta_info
 776         struct ieee80211_sub_if_data *sdata = sta->sdata;
 777         struct ieee80211_local *local = sdata->local;
 778         struct ps_data *ps;
 779 +       int tid;
 780
 781         if (sta->sdata->vif.type == NL80211_IFTYPE_AP ||
 782             sta->sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
 783 @@ -1189,6 +1190,18 @@ static void sta_ps_start(struct sta_info
 784                 drv_sta_notify(local, sdata, STA_NOTIFY_SLEEP, &sta->sta);
 785         ps_dbg(sdata, "STA %pM aid %d enters power save mode\n",
 786                sta->sta.addr, sta->sta.aid);
 787 +
 788 +       if (!sta->sta.txq[0])
 789 +               return;
 790 +
 791 +       for (tid = 0; tid < ARRAY_SIZE(sta->sta.txq); tid++) {
 792 +               struct txq_info *txqi = to_txq_info(sta->sta.txq[tid]);
 793 +
 794 +               if (!skb_queue_len(&txqi->queue))
 795 +                       set_bit(tid, &sta->txq_buffered_tids);
 796 +               else
 797 +                       clear_bit(tid, &sta->txq_buffered_tids);
 798 +       }
 799  }
 800
 801  static void sta_ps_end(struct sta_info *sta)
 802 --- a/net/mac80211/agg-tx.c
 803 +++ b/net/mac80211/agg-tx.c
 804 @@ -188,6 +188,43 @@ ieee80211_wake_queue_agg(struct ieee8021
 805         __release(agg_queue);
 806  }
 807
 808 +static void
 809 +ieee80211_agg_stop_txq(struct sta_info *sta, int tid)
 810 +{
 811 +       struct ieee80211_txq *txq = sta->sta.txq[tid];
 812 +       struct txq_info *txqi;
 813 +
 814 +       if (!txq)
 815 +               return;
 816 +
 817 +       txqi = to_txq_info(txq);
 818 +
 819 +       /* Lock here to protect against further seqno updates on dequeue */
 820 +       spin_lock_bh(&txqi->queue.lock);
 821 +       set_bit(IEEE80211_TXQ_STOP, &txqi->flags);
 822 +       spin_unlock_bh(&txqi->queue.lock);
 823 +}
 824 +
 825 +static void
 826 +ieee80211_agg_start_txq(struct sta_info *sta, int tid, bool enable)
 827 +{
 828 +       struct ieee80211_txq *txq = sta->sta.txq[tid];
 829 +       struct txq_info *txqi;
 830 +
 831 +       if (!txq)
 832 +               return;
 833 +
 834 +       txqi = to_txq_info(txq);
 835 +
 836 +       if (enable)
 837 +               set_bit(IEEE80211_TXQ_AMPDU, &txqi->flags);
 838 +       else
 839 +               clear_bit(IEEE80211_TXQ_AMPDU, &txqi->flags);
 840 +
 841 +       clear_bit(IEEE80211_TXQ_STOP, &txqi->flags);
 842 +       drv_wake_tx_queue(sta->sdata->local, txqi);
 843 +}
 844 +
 845  /*
 846   * splice packets from the STA's pending to the local pending,
 847   * requires a call to ieee80211_agg_splice_finish later
 848 @@ -247,6 +284,7 @@ static void ieee80211_remove_tid_tx(stru
 849         ieee80211_assign_tid_tx(sta, tid, NULL);
 850
 851         ieee80211_agg_splice_finish(sta->sdata, tid);
 852 +       ieee80211_agg_start_txq(sta, tid, false);
 853
 854         kfree_rcu(tid_tx, rcu_head);
 855  }
 856 @@ -418,6 +456,8 @@ void ieee80211_tx_ba_session_handle_star
 857          */
 858         clear_bit(HT_AGG_STATE_WANT_START, &tid_tx->state);
 859
 860 +       ieee80211_agg_stop_txq(sta, tid);
 861 +
 862         /*
 863          * Make sure no packets are being processed. This ensures that
 864          * we have a valid starting sequence number and that in-flight
 865 @@ -440,6 +480,8 @@ void ieee80211_tx_ba_session_handle_star
 866                 ieee80211_agg_splice_finish(sdata, tid);
 867                 spin_unlock_bh(&sta->lock);
 868
 869 +               ieee80211_agg_start_txq(sta, tid, false);
 870 +
 871                 kfree_rcu(tid_tx, rcu_head);
 872                 return;
 873         }
 874 @@ -666,6 +708,8 @@ static void ieee80211_agg_tx_operational
 875         ieee80211_agg_splice_finish(sta->sdata, tid);
 876
 877         spin_unlock_bh(&sta->lock);
 878 +
 879 +       ieee80211_agg_start_txq(sta, tid, true);
 880  }
 881
 882  void ieee80211_start_tx_ba_cb(struct ieee80211_vif *vif, u8 *ra, u16 tid)