1 files changed, 58 insertions, 70 deletions

diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index bb58d797fb72..e35bd19c3c5a 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -861,15 +861,15 @@ static void rt73usb_config_ps(struct rt2x00_dev *rt2x00dev,
                 rt2x00usb_vendor_request_sw(rt2x00dev, USB_DEVICE_MODE, 0,
                                             USB_MODE_SLEEP, REGISTER_TIMEOUT);
         } else {
-                rt2x00usb_vendor_request_sw(rt2x00dev, USB_DEVICE_MODE, 0,
-                                            USB_MODE_WAKEUP, REGISTER_TIMEOUT);
-
                 rt2x00usb_register_read(rt2x00dev, MAC_CSR11, &reg);
                 rt2x00_set_field32(&reg, MAC_CSR11_DELAY_AFTER_TBCN, 0);
                 rt2x00_set_field32(&reg, MAC_CSR11_TBCN_BEFORE_WAKEUP, 0);
                 rt2x00_set_field32(&reg, MAC_CSR11_AUTOWAKE, 0);
                 rt2x00_set_field32(&reg, MAC_CSR11_WAKEUP_LATENCY, 0);
                 rt2x00usb_register_write(rt2x00dev, MAC_CSR11, reg);
+
+                rt2x00usb_vendor_request_sw(rt2x00dev, USB_DEVICE_MODE, 0,
+                                            USB_MODE_WAKEUP, REGISTER_TIMEOUT);
         }
 }
 
@@ -1441,12 +1441,38 @@ static void rt73usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
                                   struct txentry_desc *txdesc)
 {
         struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
-        __le32 *txd = skbdesc->desc;
+        __le32 *txd = (__le32 *)(skb->data - TXD_DESC_SIZE);
         u32 word;
 
         /*
          * Start writing the descriptor words.
          */
+        rt2x00_desc_read(txd, 0, &word);
+        rt2x00_set_field32(&word, TXD_W0_BURST,
+                           test_bit(ENTRY_TXD_BURST, &txdesc->flags));
+        rt2x00_set_field32(&word, TXD_W0_VALID, 1);
+        rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
+                           test_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags));
+        rt2x00_set_field32(&word, TXD_W0_ACK,
+                           test_bit(ENTRY_TXD_ACK, &txdesc->flags));
+        rt2x00_set_field32(&word, TXD_W0_TIMESTAMP,
+                           test_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags));
+        rt2x00_set_field32(&word, TXD_W0_OFDM,
+                           (txdesc->rate_mode == RATE_MODE_OFDM));
+        rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
+        rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
+                           test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
+        rt2x00_set_field32(&word, TXD_W0_TKIP_MIC,
+                           test_bit(ENTRY_TXD_ENCRYPT_MMIC, &txdesc->flags));
+        rt2x00_set_field32(&word, TXD_W0_KEY_TABLE,
+                           test_bit(ENTRY_TXD_ENCRYPT_PAIRWISE, &txdesc->flags));
+        rt2x00_set_field32(&word, TXD_W0_KEY_INDEX, txdesc->key_idx);
+        rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, txdesc->length);
+        rt2x00_set_field32(&word, TXD_W0_BURST2,
+                           test_bit(ENTRY_TXD_BURST, &txdesc->flags));
+        rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, txdesc->cipher);
+        rt2x00_desc_write(txd, 0, word);
+
         rt2x00_desc_read(txd, 1, &word);
         rt2x00_set_field32(&word, TXD_W1_HOST_Q_ID, txdesc->queue);
         rt2x00_set_field32(&word, TXD_W1_AIFSN, txdesc->aifs);
@@ -1475,51 +1501,24 @@ static void rt73usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
         rt2x00_set_field32(&word, TXD_W5_WAITING_DMA_DONE_INT, 1);
         rt2x00_desc_write(txd, 5, word);
 
-        rt2x00_desc_read(txd, 0, &word);
-        rt2x00_set_field32(&word, TXD_W0_BURST,
-                           test_bit(ENTRY_TXD_BURST, &txdesc->flags));
-        rt2x00_set_field32(&word, TXD_W0_VALID, 1);
-        rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
-                           test_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags));
-        rt2x00_set_field32(&word, TXD_W0_ACK,
-                           test_bit(ENTRY_TXD_ACK, &txdesc->flags));
-        rt2x00_set_field32(&word, TXD_W0_TIMESTAMP,
-                           test_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags));
-        rt2x00_set_field32(&word, TXD_W0_OFDM,
-                           (txdesc->rate_mode == RATE_MODE_OFDM));
-        rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
-        rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
-                           test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
-        rt2x00_set_field32(&word, TXD_W0_TKIP_MIC,
-                           test_bit(ENTRY_TXD_ENCRYPT_MMIC, &txdesc->flags));
-        rt2x00_set_field32(&word, TXD_W0_KEY_TABLE,
-                           test_bit(ENTRY_TXD_ENCRYPT_PAIRWISE, &txdesc->flags));
-        rt2x00_set_field32(&word, TXD_W0_KEY_INDEX, txdesc->key_idx);
-        rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skb->len);
-        rt2x00_set_field32(&word, TXD_W0_BURST2,
-                           test_bit(ENTRY_TXD_BURST, &txdesc->flags));
-        rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, txdesc->cipher);
-        rt2x00_desc_write(txd, 0, word);
+        /*
+         * Register descriptor details in skb frame descriptor.
+         */
+        skbdesc->desc = txd;
+        skbdesc->desc_len = TXD_DESC_SIZE;
 }
 
 /*
  * TX data initialization
  */
-static void rt73usb_write_beacon(struct queue_entry *entry)
+static void rt73usb_write_beacon(struct queue_entry *entry,
+                                 struct txentry_desc *txdesc)
 {
         struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
-        struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
         unsigned int beacon_base;
         u32 reg;
 
         /*
-         * Add the descriptor in front of the skb.
-         */
-        skb_push(entry->skb, entry->queue->desc_size);
-        memcpy(entry->skb->data, skbdesc->desc, skbdesc->desc_len);
-        skbdesc->desc = entry->skb->data;
-
-        /*
          * Disable beaconing while we are reloading the beacon data,
          * otherwise we might be sending out invalid data.
          */
@@ -1528,6 +1527,11 @@ static void rt73usb_write_beacon(struct queue_entry *entry)
         rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
 
         /*
+         * Take the descriptor in front of the skb into account.
+         */
+        skb_push(entry->skb, TXD_DESC_SIZE);
+
+        /*
          * Write entire beacon with descriptor to register.
          */
         beacon_base = HW_BEACON_OFFSET(entry->entry_idx);
@@ -1537,6 +1541,19 @@ static void rt73usb_write_beacon(struct queue_entry *entry)
                                             REGISTER_TIMEOUT32(entry->skb->len));
 
         /*
+         * Enable beaconing again.
+         *
+         * For Wi-Fi faily generated beacons between participating stations.
+         * Set TBTT phase adaptive adjustment step to 8us (default 16us)
+         */
+        rt2x00usb_register_write(rt2x00dev, TXRX_CSR10, 0x00001008);
+
+        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);
+
+        /*
          * Clean up the beacon skb.
          */
         dev_kfree_skb(entry->skb);
@@ -1557,31 +1574,6 @@ static int rt73usb_get_tx_data_len(struct queue_entry *entry)
         return length;
 }
 
-static void rt73usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
-                                  const enum data_queue_qid queue)
-{
-        u32 reg;
-
-        if (queue != QID_BEACON) {
-                rt2x00usb_kick_tx_queue(rt2x00dev, queue);
-                return;
-        }
-
-        /*
-         * For Wi-Fi faily generated beacons between participating stations.
-         * Set TBTT phase adaptive adjustment step to 8us (default 16us)
-         */
-        rt2x00usb_register_write(rt2x00dev, TXRX_CSR10, 0x00001008);
-
-        rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
-        if (!rt2x00_get_field32(reg, TXRX_CSR9_BEACON_GEN)) {
-                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);
-        }
-}
-
 /*
  * RX control handlers
  */
@@ -1645,12 +1637,8 @@ static void rt73usb_fill_rxdone(struct queue_entry *entry,
         if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
                 rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
 
-        if (test_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags)) {
-                rxdesc->cipher =
-                    rt2x00_get_field32(word0, RXD_W0_CIPHER_ALG);
-                rxdesc->cipher_status =
-                    rt2x00_get_field32(word0, RXD_W0_CIPHER_ERROR);
-        }
+        rxdesc->cipher = rt2x00_get_field32(word0, RXD_W0_CIPHER_ALG);
+        rxdesc->cipher_status = rt2x00_get_field32(word0, RXD_W0_CIPHER_ERROR);
 
         if (rxdesc->cipher != CIPHER_NONE) {
                 _rt2x00_desc_read(rxd, 2, &rxdesc->iv[0]);
@@ -2266,7 +2254,7 @@ static const struct rt2x00lib_ops rt73usb_rt2x00_ops = {
         .write_tx_data          = rt2x00usb_write_tx_data,
         .write_beacon           = rt73usb_write_beacon,
         .get_tx_data_len        = rt73usb_get_tx_data_len,
-        .kick_tx_queue          = rt73usb_kick_tx_queue,
+        .kick_tx_queue          = rt2x00usb_kick_tx_queue,
         .kill_tx_queue          = rt2x00usb_kill_tx_queue,
         .fill_rxdone            = rt73usb_fill_rxdone,
         .config_shared_key      = rt73usb_config_shared_key,