1 files changed, 27 insertions, 39 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index 41d9996c80e6..e88d7033fbc9 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -1083,7 +1083,8 @@ static void rt2500usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
  */
 static void rt2500usb_beacondone(struct urb *urb);
 
-static void rt2500usb_write_beacon(struct queue_entry *entry)
+static void rt2500usb_write_beacon(struct queue_entry *entry,
+                                   struct txentry_desc *txdesc)
 {
         struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
         struct usb_device *usb_dev = to_usb_device_intf(rt2x00dev->dev);
@@ -1091,7 +1092,7 @@ static void rt2500usb_write_beacon(struct queue_entry *entry)
         struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
         int pipe = usb_sndbulkpipe(usb_dev, entry->queue->usb_endpoint);
         int length;
-        u16 reg;
+        u16 reg, reg0;
 
         /*
          * Add the descriptor in front of the skb.
@@ -1133,6 +1134,26 @@ static void rt2500usb_write_beacon(struct queue_entry *entry)
          * Send out the guardian byte.
          */
         usb_submit_urb(bcn_priv->guardian_urb, GFP_ATOMIC);
+
+        /*
+         * Enable beaconing again.
+         */
+        rt2x00_set_field16(&reg, TXRX_CSR19_TSF_COUNT, 1);
+        rt2x00_set_field16(&reg, TXRX_CSR19_TBCN, 1);
+        reg0 = reg;
+        rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 1);
+        /*
+         * Beacon generation will fail initially.
+         * To prevent this we need to change the TXRX_CSR19
+         * register several times (reg0 is the same as reg
+         * except for TXRX_CSR19_BEACON_GEN, which is 0 in reg0
+         * and 1 in reg).
+         */
+        rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
+        rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg0);
+        rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
+        rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg0);
+        rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
 }
 
 static int rt2500usb_get_tx_data_len(struct queue_entry *entry)
@@ -1149,37 +1170,6 @@ static int rt2500usb_get_tx_data_len(struct queue_entry *entry)
         return length;
 }
 
-static void rt2500usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
-                                    const enum data_queue_qid queue)
-{
-        u16 reg, reg0;
-
-        if (queue != QID_BEACON) {
-                rt2x00usb_kick_tx_queue(rt2x00dev, queue);
-                return;
-        }
-
-        rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
-        if (!rt2x00_get_field16(reg, TXRX_CSR19_BEACON_GEN)) {
-                rt2x00_set_field16(&reg, TXRX_CSR19_TSF_COUNT, 1);
-                rt2x00_set_field16(&reg, TXRX_CSR19_TBCN, 1);
-                reg0 = reg;
-                rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 1);
-                /*
-                 * Beacon generation will fail initially.
-                 * To prevent this we need to change the TXRX_CSR19
-                 * register several times (reg0 is the same as reg
-                 * except for TXRX_CSR19_BEACON_GEN, which is 0 in reg0
-                 * and 1 in reg).
-                 */
-                rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
-                rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg0);
-                rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
-                rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg0);
-                rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
-        }
-}
-
 /*
  * RX control handlers
  */
@@ -1214,11 +1204,9 @@ static void rt2500usb_fill_rxdone(struct queue_entry *entry,
         if (rt2x00_get_field32(word0, RXD_W0_PHYSICAL_ERROR))
                 rxdesc->flags |= RX_FLAG_FAILED_PLCP_CRC;
 
-        if (test_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags)) {
-                rxdesc->cipher = rt2x00_get_field32(word0, RXD_W0_CIPHER);
-                if (rt2x00_get_field32(word0, RXD_W0_CIPHER_ERROR))
-                        rxdesc->cipher_status = RX_CRYPTO_FAIL_KEY;
-        }
+        rxdesc->cipher = rt2x00_get_field32(word0, RXD_W0_CIPHER);
+        if (rt2x00_get_field32(word0, RXD_W0_CIPHER_ERROR))
+                rxdesc->cipher_status = RX_CRYPTO_FAIL_KEY;
 
         if (rxdesc->cipher != CIPHER_NONE) {
                 _rt2x00_desc_read(rxd, 2, &rxdesc->iv[0]);
@@ -1780,7 +1768,7 @@ static const struct rt2x00lib_ops rt2500usb_rt2x00_ops = {
         .write_tx_data          = rt2x00usb_write_tx_data,
         .write_beacon           = rt2500usb_write_beacon,
         .get_tx_data_len        = rt2500usb_get_tx_data_len,
-        .kick_tx_queue          = rt2500usb_kick_tx_queue,
+        .kick_tx_queue          = rt2x00usb_kick_tx_queue,
         .kill_tx_queue          = rt2x00usb_kill_tx_queue,
         .fill_rxdone            = rt2500usb_fill_rxdone,
         .config_shared_key      = rt2500usb_config_key,