1 files changed, 92 insertions, 98 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index d4361dc0773e..fca29ae57e71 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -261,6 +261,89 @@ static void rt2x00usb_kick_tx_entry(struct queue_entry *entry)
         }
 }
 
+/*
+ * RX data handlers.
+ */
+static void rt2x00usb_work_rxdone(struct work_struct *work)
+{
+        struct rt2x00_dev *rt2x00dev =
+            container_of(work, struct rt2x00_dev, rxdone_work);
+        struct queue_entry *entry;
+        struct skb_frame_desc *skbdesc;
+        u8 rxd[32];
+
+        while (!rt2x00queue_empty(rt2x00dev->rx)) {
+                entry = rt2x00queue_get_entry(rt2x00dev->rx, Q_INDEX_DONE);
+
+                if (test_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
+                        break;
+
+                /*
+                 * Fill in desc fields of the skb descriptor
+                 */
+                skbdesc = get_skb_frame_desc(entry->skb);
+                skbdesc->desc = rxd;
+                skbdesc->desc_len = entry->queue->desc_size;
+
+                /*
+                 * Send the frame to rt2x00lib for further processing.
+                 */
+                rt2x00lib_rxdone(entry);
+        }
+}
+
+static void rt2x00usb_interrupt_rxdone(struct urb *urb)
+{
+        struct queue_entry *entry = (struct queue_entry *)urb->context;
+        struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+
+        if (!test_and_clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
+                return;
+
+        /*
+         * Report the frame as DMA done
+         */
+        rt2x00lib_dmadone(entry);
+
+        /*
+         * Check if the received data is simply too small
+         * to be actually valid, or if the urb is signaling
+         * a problem.
+         */
+        if (urb->actual_length < entry->queue->desc_size || urb->status)
+                set_bit(ENTRY_DATA_IO_FAILED, &entry->flags);
+
+        /*
+         * Schedule the delayed work for reading the RX status
+         * from the device.
+         */
+        ieee80211_queue_work(rt2x00dev->hw, &rt2x00dev->rxdone_work);
+}
+
+static void rt2x00usb_kick_rx_entry(struct queue_entry *entry)
+{
+        struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+        struct usb_device *usb_dev = to_usb_device_intf(rt2x00dev->dev);
+        struct queue_entry_priv_usb *entry_priv = entry->priv_data;
+        int status;
+
+        if (test_and_set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
+                return;
+
+        usb_fill_bulk_urb(entry_priv->urb, usb_dev,
+                          usb_rcvbulkpipe(usb_dev, entry->queue->usb_endpoint),
+                          entry->skb->data, entry->skb->len,
+                          rt2x00usb_interrupt_rxdone, entry);
+
+        status = usb_submit_urb(entry_priv->urb, GFP_ATOMIC);
+        if (status) {
+                if (status == -ENODEV)
+                        clear_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags);
+                set_bit(ENTRY_DATA_IO_FAILED, &entry->flags);
+                rt2x00lib_dmadone(entry);
+        }
+}
+
 void rt2x00usb_kick_queue(struct data_queue *queue)
 {
         switch (queue->qid) {
@@ -272,6 +355,11 @@ void rt2x00usb_kick_queue(struct data_queue *queue)
                         rt2x00queue_for_each_entry(queue, Q_INDEX_DONE, Q_INDEX,
                                                    rt2x00usb_kick_tx_entry);
                 break;
+        case QID_RX:
+                if (!rt2x00queue_full(queue))
+                        rt2x00queue_for_each_entry(queue, Q_INDEX_DONE, Q_INDEX,
+                                                   rt2x00usb_kick_rx_entry);
+                break;
         default:
                 break;
         }
@@ -307,7 +395,6 @@ EXPORT_SYMBOL_GPL(rt2x00usb_stop_queue);
 static void rt2x00usb_watchdog_tx_dma(struct data_queue *queue)
 {
         struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
-        unsigned short threshold = queue->threshold;
 
         WARNING(queue->rt2x00dev, "TX queue %d DMA timed out,"
                 " invoke forced forced reset\n", queue->qid);
@@ -315,18 +402,8 @@ static void rt2x00usb_watchdog_tx_dma(struct data_queue *queue)
         /*
          * Temporarily disable the TX queue, this will force mac80211
          * to use the other queues until this queue has been restored.
-         *
-         * Set the queue threshold to the queue limit. This prevents the
-         * queue from being enabled during the txdone handler.
          */
-        queue->threshold = queue->limit;
-        ieee80211_stop_queue(rt2x00dev->hw, queue->qid);
-
-        /*
-         * Kill all entries in the queue, afterwards we need to
-         * wait a bit for all URBs to be cancelled.
-         */
-        rt2x00usb_stop_queue(queue);
+        rt2x00queue_stop_queue(queue);
 
         /*
          * In case that a driver has overriden the txdone_work
@@ -338,8 +415,7 @@ static void rt2x00usb_watchdog_tx_dma(struct data_queue *queue)
          * The queue has been reset, and mac80211 is allowed to use the
          * queue again.
          */
-        queue->threshold = threshold;
-        ieee80211_wake_queue(rt2x00dev->hw, queue->qid);
+        rt2x00queue_start_queue(queue);
 }
 
 static void rt2x00usb_watchdog_tx_status(struct data_queue *queue)
@@ -366,73 +442,12 @@ void rt2x00usb_watchdog(struct rt2x00_dev *rt2x00dev)
 EXPORT_SYMBOL_GPL(rt2x00usb_watchdog);
 
 /*
- * RX data handlers.
- */
-static void rt2x00usb_work_rxdone(struct work_struct *work)
-{
-        struct rt2x00_dev *rt2x00dev =
-            container_of(work, struct rt2x00_dev, rxdone_work);
-        struct queue_entry *entry;
-        struct skb_frame_desc *skbdesc;
-        u8 rxd[32];
-
-        while (!rt2x00queue_empty(rt2x00dev->rx)) {
-                entry = rt2x00queue_get_entry(rt2x00dev->rx, Q_INDEX_DONE);
-
-                if (test_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
-                        break;
-
-                /*
-                 * Fill in desc fields of the skb descriptor
-                 */
-                skbdesc = get_skb_frame_desc(entry->skb);
-                skbdesc->desc = rxd;
-                skbdesc->desc_len = entry->queue->desc_size;
-
-                /*
-                 * Send the frame to rt2x00lib for further processing.
-                 */
-                rt2x00lib_rxdone(entry);
-        }
-}
-
-static void rt2x00usb_interrupt_rxdone(struct urb *urb)
-{
-        struct queue_entry *entry = (struct queue_entry *)urb->context;
-        struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
-
-        if (!test_and_clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
-                return;
-
-        /*
-         * Report the frame as DMA done
-         */
-        rt2x00lib_dmadone(entry);
-
-        /*
-         * Check if the received data is simply too small
-         * to be actually valid, or if the urb is signaling
-         * a problem.
-         */
-        if (urb->actual_length < entry->queue->desc_size || urb->status)
-                set_bit(ENTRY_DATA_IO_FAILED, &entry->flags);
-
-        /*
-         * Schedule the delayed work for reading the RX status
-         * from the device.
-         */
-        ieee80211_queue_work(rt2x00dev->hw, &rt2x00dev->rxdone_work);
-}
-
-/*
  * Radio handlers
  */
 void rt2x00usb_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
         rt2x00usb_vendor_request_sw(rt2x00dev, USB_RX_CONTROL, 0, 0,
                                     REGISTER_TIMEOUT);
-
-        rt2x00dev->ops->lib->stop_queue(rt2x00dev->rx);
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_disable_radio);
 
@@ -441,31 +456,10 @@ EXPORT_SYMBOL_GPL(rt2x00usb_disable_radio);
  */
 void rt2x00usb_clear_entry(struct queue_entry *entry)
 {
-        struct usb_device *usb_dev =
-            to_usb_device_intf(entry->queue->rt2x00dev->dev);
-        struct queue_entry_priv_usb *entry_priv = entry->priv_data;
-        int pipe;
-        int status;
-
         entry->flags = 0;
 
-        if (entry->queue->qid == QID_RX) {
-                pipe = usb_rcvbulkpipe(usb_dev, entry->queue->usb_endpoint);
-                usb_fill_bulk_urb(entry_priv->urb, usb_dev, pipe,
-                                entry->skb->data, entry->skb->len,
-                                rt2x00usb_interrupt_rxdone, entry);
-
-                set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
-
-                status = usb_submit_urb(entry_priv->urb, GFP_ATOMIC);
-                if (status) {
-                        if (status == -ENODEV)
-                                clear_bit(DEVICE_STATE_PRESENT,
-                                          &entry->queue->rt2x00dev->flags);
-                        set_bit(ENTRY_DATA_IO_FAILED, &entry->flags);
-                        rt2x00lib_dmadone(entry);
-                }
-        }
+        if (entry->queue->qid == QID_RX)
+                rt2x00usb_kick_rx_entry(entry);
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_clear_entry);