1 files changed, 87 insertions, 44 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index 62786608951e..35d9a06a76af 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -633,6 +633,88 @@ static void rt2400pci_link_tuner(struct rt2x00_dev *rt2x00dev,
 }
 
 /*
+ * Queue handlers.
+ */
+static void rt2400pci_start_queue(struct data_queue *queue)
+{
+        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
+        u32 reg;
+
+        switch (queue->qid) {
+        case QID_RX:
+                rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
+                rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX, 0);
+                rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
+                break;
+        case QID_BEACON:
+                rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+                rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
+                rt2x00_set_field32(&reg, CSR14_TBCN, 1);
+                rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 1);
+                rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+                break;
+        default:
+                break;
+        }
+}
+
+static void rt2400pci_kick_queue(struct data_queue *queue)
+{
+        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
+        u32 reg;
+
+        switch (queue->qid) {
+        case QID_AC_BE:
+                rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+                rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO, 1);
+                rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+                break;
+        case QID_AC_BK:
+                rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+                rt2x00_set_field32(&reg, TXCSR0_KICK_TX, 1);
+                rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+                break;
+        case QID_ATIM:
+                rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+                rt2x00_set_field32(&reg, TXCSR0_KICK_ATIM, 1);
+                rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+                break;
+        default:
+                break;
+        }
+}
+
+static void rt2400pci_stop_queue(struct data_queue *queue)
+{
+        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
+        u32 reg;
+
+        switch (queue->qid) {
+        case QID_AC_BE:
+        case QID_AC_BK:
+        case QID_ATIM:
+                rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+                rt2x00_set_field32(&reg, TXCSR0_ABORT, 1);
+                rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+                break;
+        case QID_RX:
+                rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
+                rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX, 1);
+                rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
+                break;
+        case QID_BEACON:
+                rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+                rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
+                rt2x00_set_field32(&reg, CSR14_TBCN, 0);
+                rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
+                rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+                break;
+        default:
+                break;
+        }
+}
+
+/*
  * Initialization functions.
  */
 static bool rt2400pci_get_entry_state(struct queue_entry *entry)
@@ -878,17 +960,6 @@ static int rt2400pci_init_bbp(struct rt2x00_dev *rt2x00dev)
 /*
  * Device state switch handlers.
  */
-static void rt2400pci_toggle_rx(struct rt2x00_dev *rt2x00dev,
-                                enum dev_state state)
-{
-        u32 reg;
-
-        rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
-        rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX,
-                           (state == STATE_RADIO_RX_OFF));
-        rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
-}
-
 static void rt2400pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
                                  enum dev_state state)
 {
@@ -988,8 +1059,10 @@ static int rt2400pci_set_device_state(struct rt2x00_dev *rt2x00dev,
                 rt2400pci_disable_radio(rt2x00dev);
                 break;
         case STATE_RADIO_RX_ON:
+                rt2400pci_start_queue(rt2x00dev->rx);
+                break;
         case STATE_RADIO_RX_OFF:
-                rt2400pci_toggle_rx(rt2x00dev, state);
+                rt2400pci_stop_queue(rt2x00dev->rx);
                 break;
         case STATE_RADIO_IRQ_ON:
         case STATE_RADIO_IRQ_ON_ISR:
@@ -1122,36 +1195,6 @@ static void rt2400pci_write_beacon(struct queue_entry *entry,
         rt2x00pci_register_write(rt2x00dev, CSR14, reg);
 }
 
-static void rt2400pci_kick_tx_queue(struct data_queue *queue)
-{
-        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
-        u32 reg;
-
-        rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
-        rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO, (queue->qid == QID_AC_BE));
-        rt2x00_set_field32(&reg, TXCSR0_KICK_TX, (queue->qid == QID_AC_BK));
-        rt2x00_set_field32(&reg, TXCSR0_KICK_ATIM, (queue->qid == QID_ATIM));
-        rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
-}
-
-static void rt2400pci_kill_tx_queue(struct data_queue *queue)
-{
-        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
-        u32 reg;
-
-        if (queue->qid == QID_BEACON) {
-                rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
-                rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
-                rt2x00_set_field32(&reg, CSR14_TBCN, 0);
-                rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
-                rt2x00pci_register_write(rt2x00dev, CSR14, reg);
-        } else {
-                rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
-                rt2x00_set_field32(&reg, TXCSR0_ABORT, 1);
-                rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
-        }
-}
-
 /*
  * RX control handlers
  */
@@ -1631,8 +1674,8 @@ static const struct rt2x00lib_ops rt2400pci_rt2x00_ops = {
         .link_tuner             = rt2400pci_link_tuner,
         .write_tx_desc          = rt2400pci_write_tx_desc,
         .write_beacon           = rt2400pci_write_beacon,
-        .kick_tx_queue          = rt2400pci_kick_tx_queue,
-        .kill_tx_queue          = rt2400pci_kill_tx_queue,
+        .kick_tx_queue          = rt2400pci_kick_queue,
+        .kill_tx_queue          = rt2400pci_stop_queue,
         .fill_rxdone            = rt2400pci_fill_rxdone,
         .config_filter          = rt2400pci_config_filter,
         .config_intf            = rt2400pci_config_intf,