1 files changed, 55 insertions, 29 deletions

diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index 3934dad709c6..e9b1e3d5f47c 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -1031,6 +1031,57 @@ dynamic_cca_tune:
 }
 
 /*
+ * Queue handlers.
+ */
+static void rt73usb_start_queue(struct data_queue *queue)
+{
+        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
+        u32 reg;
+
+        switch (queue->qid) {
+        case QID_RX:
+                rt2x00usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+                rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 0);
+                rt2x00usb_register_write(rt2x00dev, TXRX_CSR0, reg);
+                break;
+        case QID_BEACON:
+                rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+                rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 1);
+                rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 1);
+                rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 1);
+                rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
+                break;
+        default:
+                break;
+        }
+}
+
+static void rt73usb_stop_queue(struct data_queue *queue)
+{
+        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
+        u32 reg;
+
+        switch (queue->qid) {
+        case QID_RX:
+                rt2x00usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+                rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 1);
+                rt2x00usb_register_write(rt2x00dev, TXRX_CSR0, reg);
+                break;
+        case QID_BEACON:
+                rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+                rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 0);
+                rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 0);
+                rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
+                rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
+                break;
+        default:
+                break;
+        }
+
+        rt2x00usb_stop_queue(queue);
+}
+
+/*
  * Firmware functions
  */
 static char *rt73usb_get_firmware_name(struct rt2x00_dev *rt2x00dev)
@@ -1324,17 +1375,6 @@ static int rt73usb_init_bbp(struct rt2x00_dev *rt2x00dev)
 /*
  * Device state switch handlers.
  */
-static void rt73usb_toggle_rx(struct rt2x00_dev *rt2x00dev,
-                              enum dev_state state)
-{
-        u32 reg;
-
-        rt2x00usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
-        rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX,
-                           (state == STATE_RADIO_RX_OFF));
-        rt2x00usb_register_write(rt2x00dev, TXRX_CSR0, reg);
-}
-
 static int rt73usb_enable_radio(struct rt2x00_dev *rt2x00dev)
 {
         /*
@@ -1402,8 +1442,10 @@ static int rt73usb_set_device_state(struct rt2x00_dev *rt2x00dev,
                 rt73usb_disable_radio(rt2x00dev);
                 break;
         case STATE_RADIO_RX_ON:
+                rt73usb_start_queue(rt2x00dev->rx);
+                break;
         case STATE_RADIO_RX_OFF:
-                rt73usb_toggle_rx(rt2x00dev, state);
+                rt73usb_stop_queue(rt2x00dev->rx);
                 break;
         case STATE_RADIO_IRQ_ON:
         case STATE_RADIO_IRQ_ON_ISR:
@@ -1579,22 +1621,6 @@ static int rt73usb_get_tx_data_len(struct queue_entry *entry)
         return length;
 }
 
-static void rt73usb_kill_tx_queue(struct data_queue *queue)
-{
-        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
-        u32 reg;
-
-        if (queue->qid == QID_BEACON) {
-                rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
-                rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 0);
-                rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 0);
-                rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
-                rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
-        }
-
-        rt2x00usb_kill_tx_queue(queue);
-}
-
 /*
  * RX control handlers
  */
@@ -2290,7 +2316,7 @@ static const struct rt2x00lib_ops rt73usb_rt2x00_ops = {
         .write_beacon           = rt73usb_write_beacon,
         .get_tx_data_len        = rt73usb_get_tx_data_len,
         .kick_tx_queue          = rt2x00usb_kick_tx_queue,
-        .kill_tx_queue          = rt73usb_kill_tx_queue,
+        .kill_tx_queue          = rt73usb_stop_queue,
         .fill_rxdone            = rt73usb_fill_rxdone,
         .config_shared_key      = rt73usb_config_shared_key,
         .config_pairwise_key    = rt73usb_config_pairwise_key,