1 files changed, 67 insertions, 19 deletions

diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index 200138cdb030..75e70bce40f6 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -2440,30 +2440,64 @@ static int b43legacy_rng_init(struct b43legacy_wl *wl)
 	return err;
 }
 
+static void b43legacy_tx_work(struct work_struct *work)
+{
+	struct b43legacy_wl *wl = container_of(work, struct b43legacy_wl,
+				  tx_work);
+	struct b43legacy_wldev *dev;
+	struct sk_buff *skb;
+	int queue_num;
+	int err = 0;
+
+	mutex_lock(&wl->mutex);
+	dev = wl->current_dev;
+	if (unlikely(!dev || b43legacy_status(dev) < B43legacy_STAT_STARTED)) {
+		mutex_unlock(&wl->mutex);
+		return;
+	}
+
+	for (queue_num = 0; queue_num < B43legacy_QOS_QUEUE_NUM; queue_num++) {
+		while (skb_queue_len(&wl->tx_queue[queue_num])) {
+			skb = skb_dequeue(&wl->tx_queue[queue_num]);
+			if (b43legacy_using_pio(dev))
+				err = b43legacy_pio_tx(dev, skb);
+			else
+				err = b43legacy_dma_tx(dev, skb);
+			if (err == -ENOSPC) {
+				wl->tx_queue_stopped[queue_num] = 1;
+				ieee80211_stop_queue(wl->hw, queue_num);
+				skb_queue_head(&wl->tx_queue[queue_num], skb);
+				break;
+			}
+			if (unlikely(err))
+				dev_kfree_skb(skb); /* Drop it */
+			err = 0;
+		}
+
+		if (!err)
+			wl->tx_queue_stopped[queue_num] = 0;
+	}
+
+	mutex_unlock(&wl->mutex);
+}
+
 static void b43legacy_op_tx(struct ieee80211_hw *hw,
 			    struct sk_buff *skb)
 {
 	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
-	struct b43legacy_wldev *dev = wl->current_dev;
-	int err = -ENODEV;
-	unsigned long flags;
 
-	if (unlikely(!dev))
-		goto out;
-	if (unlikely(b43legacy_status(dev) < B43legacy_STAT_STARTED))
-		goto out;
-	/* DMA-TX is done without a global lock. */
-	if (b43legacy_using_pio(dev)) {
-		spin_lock_irqsave(&wl->irq_lock, flags);
-		err = b43legacy_pio_tx(dev, skb);
-		spin_unlock_irqrestore(&wl->irq_lock, flags);
-	} else
-		err = b43legacy_dma_tx(dev, skb);
-out:
-	if (unlikely(err)) {
-		/* Drop the packet. */
+	if (unlikely(skb->len < 2 + 2 + 6)) {
+		/* Too short, this can't be a valid frame. */
 		dev_kfree_skb_any(skb);
+		return;
 	}
+	B43legacy_WARN_ON(skb_shinfo(skb)->nr_frags);
+
+	skb_queue_tail(&wl->tx_queue[skb->queue_mapping], skb);
+	if (!wl->tx_queue_stopped[skb->queue_mapping])
+		ieee80211_queue_work(wl->hw, &wl->tx_work);
+	else
+		ieee80211_stop_queue(wl->hw, skb->queue_mapping);
 }
 
 static int b43legacy_op_conf_tx(struct ieee80211_hw *hw,
@@ -2879,6 +2913,7 @@ static void b43legacy_wireless_core_stop(struct b43legacy_wldev *dev)
 {
 	struct b43legacy_wl *wl = dev->wl;
 	unsigned long flags;
+	int queue_num;
 
 	if (b43legacy_status(dev) < B43legacy_STAT_STARTED)
 		return;
@@ -2898,11 +2933,16 @@ static void b43legacy_wireless_core_stop(struct b43legacy_wldev *dev)
 	/* Must unlock as it would otherwise deadlock. No races here.
 	 * Cancel the possibly running self-rearming periodic work. */
 	cancel_delayed_work_sync(&dev->periodic_work);
+	cancel_work_sync(&wl->tx_work);
 	mutex_lock(&wl->mutex);
 
-	ieee80211_stop_queues(wl->hw); /* FIXME this could cause a deadlock */
+	/* Drain all TX queues. */
+	for (queue_num = 0; queue_num < B43legacy_QOS_QUEUE_NUM; queue_num++) {
+		while (skb_queue_len(&wl->tx_queue[queue_num]))
+			dev_kfree_skb(skb_dequeue(&wl->tx_queue[queue_num]));
+	}
 
-	b43legacy_mac_suspend(dev);
+b43legacy_mac_suspend(dev);
 	free_irq(dev->dev->irq, dev);
 	b43legacydbg(wl, "Wireless interface stopped\n");
 }
@@ -3748,6 +3788,7 @@ static int b43legacy_wireless_init(struct ssb_device *dev)
 	struct ieee80211_hw *hw;
 	struct b43legacy_wl *wl;
 	int err = -ENOMEM;
+	int queue_num;
 
 	b43legacy_sprom_fixup(dev->bus);
 
@@ -3782,6 +3823,13 @@ static int b43legacy_wireless_init(struct ssb_device *dev)
 	mutex_init(&wl->mutex);
 	INIT_LIST_HEAD(&wl->devlist);
 	INIT_WORK(&wl->beacon_update_trigger, b43legacy_beacon_update_trigger_work);
+	INIT_WORK(&wl->tx_work, b43legacy_tx_work);
+
+	/* Initialize queues and flags. */
+	for (queue_num = 0; queue_num < B43legacy_QOS_QUEUE_NUM; queue_num++) {
+		skb_queue_head_init(&wl->tx_queue[queue_num]);
+		wl->tx_queue_stopped[queue_num] = 0;
+	}
 
 	ssb_set_devtypedata(dev, wl);
 	b43legacyinfo(wl, "Broadcom %04X WLAN found (core revision %u)\n",