1 files changed, 87 insertions, 45 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index 9ec6691adf0d..54ca49ad3472 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_VO:
+                rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+                rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO, 1);
+                rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+                break;
+        case QID_AC_VI:
+                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_VO:
+        case QID_AC_VI:
+        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)
 {
@@ -987,10 +1058,6 @@ static int rt2400pci_set_device_state(struct rt2x00_dev *rt2x00dev,
         case STATE_RADIO_OFF:
                 rt2400pci_disable_radio(rt2x00dev);
                 break;
-        case STATE_RADIO_RX_ON:
-        case STATE_RADIO_RX_OFF:
-                rt2400pci_toggle_rx(rt2x00dev, state);
-                break;
         case STATE_RADIO_IRQ_ON:
         case STATE_RADIO_IRQ_ON_ISR:
         case STATE_RADIO_IRQ_OFF:
@@ -1122,32 +1189,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_write(rt2x00dev, CSR14, 0);
-        } else {
-                rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
-                rt2x00_set_field32(&reg, TXCSR0_ABORT, 1);
-                rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
-        }
-}
-
 /*
  * RX control handlers
  */
@@ -1281,13 +1322,13 @@ static irqreturn_t rt2400pci_interrupt_thread(int irq, void *dev_instance)
          * 4 - Priority ring transmit done interrupt.
          */
         if (rt2x00_get_field32(reg, CSR7_TXDONE_PRIORING))
-                rt2400pci_txdone(rt2x00dev, QID_AC_BE);
+                rt2400pci_txdone(rt2x00dev, QID_AC_VO);
 
         /*
          * 5 - Tx ring transmit done interrupt.
          */
         if (rt2x00_get_field32(reg, CSR7_TXDONE_TXRING))
-                rt2400pci_txdone(rt2x00dev, QID_AC_BK);
+                rt2400pci_txdone(rt2x00dev, QID_AC_VI);
 
         /* Enable interrupts again. */
         rt2x00dev->ops->lib->set_device_state(rt2x00dev,
@@ -1625,10 +1666,11 @@ static const struct rt2x00lib_ops rt2400pci_rt2x00_ops = {
         .link_stats             = rt2400pci_link_stats,
         .reset_tuner            = rt2400pci_reset_tuner,
         .link_tuner             = rt2400pci_link_tuner,
+        .start_queue            = rt2400pci_start_queue,
+        .kick_queue             = rt2400pci_kick_queue,
+        .stop_queue             = rt2400pci_stop_queue,
         .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,
         .fill_rxdone            = rt2400pci_fill_rxdone,
         .config_filter          = rt2400pci_config_filter,
         .config_intf            = rt2400pci_config_intf,