1 files changed, 57 insertions, 38 deletions

diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index 432e75f960b7..2e3076f67535 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -1764,7 +1764,8 @@ static void rt61pci_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;
+        struct queue_entry_priv_pci *entry_priv = skbdesc->entry->priv_data;
+        __le32 *txd = entry_priv->desc;
         u32 word;
 
         /*
@@ -1802,17 +1803,23 @@ static void rt61pci_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, 6, &word);
-        rt2x00_set_field32(&word, TXD_W6_BUFFER_PHYSICAL_ADDRESS,
-                           skbdesc->skb_dma);
-        rt2x00_desc_write(txd, 6, word);
+        if (txdesc->queue != QID_BEACON) {
+                rt2x00_desc_read(txd, 6, &word);
+                rt2x00_set_field32(&word, TXD_W6_BUFFER_PHYSICAL_ADDRESS,
+                                   skbdesc->skb_dma);
+                rt2x00_desc_write(txd, 6, word);
 
-        if (skbdesc->desc_len > TXINFO_SIZE) {
                 rt2x00_desc_read(txd, 11, &word);
-                rt2x00_set_field32(&word, TXD_W11_BUFFER_LENGTH0, skb->len);
+                rt2x00_set_field32(&word, TXD_W11_BUFFER_LENGTH0,
+                                   txdesc->length);
                 rt2x00_desc_write(txd, 11, word);
         }
 
+        /*
+         * Writing TXD word 0 must the last to prevent a race condition with
+         * the device, whereby the device may take hold of the TXD before we
+         * finished updating it.
+         */
         rt2x00_desc_read(txd, 0, &word);
         rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 1);
         rt2x00_set_field32(&word, TXD_W0_VALID, 1);
@@ -1832,20 +1839,28 @@ static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
         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_DATABYTE_COUNT, txdesc->length);
         rt2x00_set_field32(&word, TXD_W0_BURST,
                            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 =
+                (txdesc->queue == QID_BEACON) ?  TXINFO_SIZE : TXD_DESC_SIZE;
 }
 
 /*
  * TX data initialization
  */
-static void rt61pci_write_beacon(struct queue_entry *entry)
+static void rt61pci_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);
+        struct queue_entry_priv_pci *entry_priv = entry->priv_data;
         unsigned int beacon_base;
         u32 reg;
 
@@ -1861,14 +1876,25 @@ static void rt61pci_write_beacon(struct queue_entry *entry)
          * Write entire beacon with descriptor to register.
          */
         beacon_base = HW_BEACON_OFFSET(entry->entry_idx);
-        rt2x00pci_register_multiwrite(rt2x00dev,
-                                      beacon_base,
-                                      skbdesc->desc, skbdesc->desc_len);
-        rt2x00pci_register_multiwrite(rt2x00dev,
-                                      beacon_base + skbdesc->desc_len,
+        rt2x00pci_register_multiwrite(rt2x00dev, beacon_base,
+                                      entry_priv->desc, TXINFO_SIZE);
+        rt2x00pci_register_multiwrite(rt2x00dev, beacon_base + TXINFO_SIZE,
                                       entry->skb->data, entry->skb->len);
 
         /*
+         * Enable beaconing again.
+         *
+         * For Wi-Fi faily generated beacons between participating
+         * stations. Set TBTT phase adaptive adjustment step to 8us.
+         */
+        rt2x00pci_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);
+        rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+
+        /*
          * Clean up beacon skb.
          */
         dev_kfree_skb_any(entry->skb);
@@ -1880,23 +1906,6 @@ static void rt61pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 {
         u32 reg;
 
-        if (queue == QID_BEACON) {
-                /*
-                 * For Wi-Fi faily generated beacons between participating
-                 * stations. Set TBTT phase adaptive adjustment step to 8us.
-                 */
-                rt2x00pci_register_write(rt2x00dev, TXRX_CSR10, 0x00001008);
-
-                rt2x00pci_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);
-                        rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
-                }
-                return;
-        }
-
         rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
         rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC0, (queue == QID_AC_BE));
         rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC1, (queue == QID_AC_BK));
@@ -1968,12 +1977,8 @@ static void rt61pci_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(entry_priv->desc, 2, &rxdesc->iv[0]);
@@ -2118,6 +2123,14 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
         }
 }
 
+static void rt61pci_wakeup(struct rt2x00_dev *rt2x00dev)
+{
+        struct ieee80211_conf conf = { .flags = 0 };
+        struct rt2x00lib_conf libconf = { .conf = &conf };
+
+        rt61pci_config(rt2x00dev, &libconf, IEEE80211_CONF_CHANGE_PS);
+}
+
 static irqreturn_t rt61pci_interrupt(int irq, void *dev_instance)
 {
         struct rt2x00_dev *rt2x00dev = dev_instance;
@@ -2165,6 +2178,12 @@ static irqreturn_t rt61pci_interrupt(int irq, void *dev_instance)
                 rt2x00pci_register_write(rt2x00dev,
                                          M2H_CMD_DONE_CSR, 0xffffffff);
 
+        /*
+         * 4 - MCU Autowakeup interrupt.
+         */
+        if (rt2x00_get_field32(reg_mcu, MCU_INT_SOURCE_CSR_TWAKEUP))
+                rt61pci_wakeup(rt2x00dev);
+
         return IRQ_HANDLED;
 }