1 files changed, 49 insertions, 11 deletions

diff --git a/drivers/net/wireless/wl12xx/tx.c b/drivers/net/wireless/wl12xx/tx.c
index d332b3f6d0fa..b44c75cd8c1e 100644
--- a/drivers/net/wireless/wl12xx/tx.c
+++ b/drivers/net/wireless/wl12xx/tx.c
@@ -125,7 +125,6 @@ static void wl1271_tx_fill_hdr(struct wl1271 *wl, struct sk_buff *skb,
         /* queue (we use same identifiers for tid's and ac's */
         ac = wl1271_tx_get_queue(skb_get_queue_mapping(skb));
         desc->tid = ac;
-
         desc->aid = TX_HW_DEFAULT_AID;
         desc->reserved = 0;
 
@@ -228,7 +227,7 @@ static void handle_tx_low_watermark(struct wl1271 *wl)
         unsigned long flags;
 
         if (test_bit(WL1271_FLAG_TX_QUEUE_STOPPED, &wl->flags) &&
-            skb_queue_len(&wl->tx_queue) <= WL1271_TX_QUEUE_LOW_WATERMARK) {
+            wl->tx_queue_count <= WL1271_TX_QUEUE_LOW_WATERMARK) {
                 /* firmware buffer has space, restart queues */
                 spin_lock_irqsave(&wl->wl_lock, flags);
                 ieee80211_wake_queues(wl->hw);
@@ -237,6 +236,43 @@ static void handle_tx_low_watermark(struct wl1271 *wl)
         }
 }
 
+static struct sk_buff *wl1271_skb_dequeue(struct wl1271 *wl)
+{
+        struct sk_buff *skb = NULL;
+        unsigned long flags;
+
+        skb = skb_dequeue(&wl->tx_queue[CONF_TX_AC_VO]);
+        if (skb)
+                goto out;
+        skb = skb_dequeue(&wl->tx_queue[CONF_TX_AC_VI]);
+        if (skb)
+                goto out;
+        skb = skb_dequeue(&wl->tx_queue[CONF_TX_AC_BE]);
+        if (skb)
+                goto out;
+        skb = skb_dequeue(&wl->tx_queue[CONF_TX_AC_BK]);
+
+out:
+        if (skb) {
+                spin_lock_irqsave(&wl->wl_lock, flags);
+                wl->tx_queue_count--;
+                spin_unlock_irqrestore(&wl->wl_lock, flags);
+        }
+
+        return skb;
+}
+
+static void wl1271_skb_queue_head(struct wl1271 *wl, struct sk_buff *skb)
+{
+        unsigned long flags;
+        int q = wl1271_tx_get_queue(skb_get_queue_mapping(skb));
+
+        skb_queue_head(&wl->tx_queue[q], skb);
+        spin_lock_irqsave(&wl->wl_lock, flags);
+        wl->tx_queue_count++;
+        spin_unlock_irqrestore(&wl->wl_lock, flags);
+}
+
 void wl1271_tx_work_locked(struct wl1271 *wl)
 {
         struct sk_buff *skb;
@@ -270,7 +306,7 @@ void wl1271_tx_work_locked(struct wl1271 *wl)
                 wl1271_acx_rate_policies(wl);
         }
 
-        while ((skb = skb_dequeue(&wl->tx_queue))) {
+        while ((skb = wl1271_skb_dequeue(wl))) {
                 if (!woken_up) {
                         ret = wl1271_ps_elp_wakeup(wl, false);
                         if (ret < 0)
@@ -284,9 +320,9 @@ void wl1271_tx_work_locked(struct wl1271 *wl)
                          * Aggregation buffer is full.
                          * Flush buffer and try again.
                          */
-                        skb_queue_head(&wl->tx_queue, skb);
+                        wl1271_skb_queue_head(wl, skb);
                         wl1271_write(wl, WL1271_SLV_MEM_DATA, wl->aggr_buf,
-                                buf_offset, true);
+                                     buf_offset, true);
                         sent_packets = true;
                         buf_offset = 0;
                         continue;
@@ -295,7 +331,7 @@ void wl1271_tx_work_locked(struct wl1271 *wl)
                          * Firmware buffer is full.
                          * Queue back last skb, and stop aggregating.
                          */
-                        skb_queue_head(&wl->tx_queue, skb);
+                        wl1271_skb_queue_head(wl, skb);
                         /* No work left, avoid scheduling redundant tx work */
                         set_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags);
                         goto out_ack;
@@ -440,10 +476,13 @@ void wl1271_tx_reset(struct wl1271 *wl)
         struct sk_buff *skb;
 
         /* TX failure */
-        while ((skb = skb_dequeue(&wl->tx_queue))) {
-                wl1271_debug(DEBUG_TX, "freeing skb 0x%p", skb);
-                ieee80211_tx_status(wl->hw, skb);
+        for (i = 0; i < NUM_TX_QUEUES; i++) {
+                while ((skb = skb_dequeue(&wl->tx_queue[i]))) {
+                        wl1271_debug(DEBUG_TX, "freeing skb 0x%p", skb);
+                        ieee80211_tx_status(wl->hw, skb);
+                }
         }
+        wl->tx_queue_count = 0;
 
         /*
          * Make sure the driver is at a consistent state, in case this
@@ -472,8 +511,7 @@ void wl1271_tx_flush(struct wl1271 *wl)
                 mutex_lock(&wl->mutex);
                 wl1271_debug(DEBUG_TX, "flushing tx buffer: %d",
                              wl->tx_frames_cnt);
-                if ((wl->tx_frames_cnt == 0) &&
-                    skb_queue_empty(&wl->tx_queue)) {
+                if ((wl->tx_frames_cnt == 0) && (wl->tx_queue_count == 0)) {
                         mutex_unlock(&wl->mutex);
                         return;
                 }