2 files changed, 46 insertions, 12 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index 93ebf245876..cf4a97f32ab 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -984,9 +984,9 @@ static void rt2800usb_config_ps(struct rt2x00_dev *rt2x00dev,
                 rt2x00_set_field32(&reg, AUTOWAKEUP_CFG_AUTOWAKE, 1);
                 rt2x00usb_register_write(rt2x00dev, AUTOWAKEUP_CFG, reg);
 
-                rt2800usb_mcu_request(rt2x00dev, MCU_SLEEP, 0xff, 0, 0);
+                rt2x00dev->ops->lib->set_device_state(rt2x00dev, state);
         } else {
-                rt2800usb_mcu_request(rt2x00dev, MCU_WAKEUP, 0xff, 0, 0);
+                rt2x00dev->ops->lib->set_device_state(rt2x00dev, state);
 
                 rt2x00usb_register_read(rt2x00dev, AUTOWAKEUP_CFG, &reg);
                 rt2x00_set_field32(&reg, AUTOWAKEUP_CFG_AUTO_LEAD_TIME, 0);
@@ -1171,7 +1171,9 @@ static int rt2800usb_load_firmware(struct rt2x00_dev *rt2x00dev,
         /*
          * Check which section of the firmware we need.
          */
-        if ((chipset == 0x2860) || (chipset == 0x2872) || (chipset == 0x3070)) {
+        if ((chipset == 0x2860) ||
+            (chipset == 0x2872) ||
+            (chipset == 0x3070)) {
                 offset = 0;
                 length = 4096;
         } else {
@@ -1218,6 +1220,22 @@ static int rt2800usb_load_firmware(struct rt2x00_dev *rt2x00dev,
                 return status;
         }
 
+        msleep(10);
+        rt2x00usb_register_write(rt2x00dev, H2M_MAILBOX_CSR, 0);
+
+        /*
+         * Send signal to firmware during boot time.
+         */
+        rt2800usb_mcu_request(rt2x00dev, MCU_BOOT_SIGNAL, 0xff, 0, 0);
+
+        if ((chipset == 0x3070) ||
+            (chipset == 0x3071) ||
+            (chipset == 0x3572)) {
+                udelay(200);
+                rt2800usb_mcu_request(rt2x00dev, MCU_CURRENT, 0, 0, 0);
+                udelay(10);
+        }
+
         /*
          * Wait for device to stabilize.
          */
@@ -1566,6 +1584,14 @@ static int rt2800usb_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
         unsigned int i;
         u8 value;
 
+        /*
+         * BBP was enabled after firmware was loaded,
+         * but we need to reactivate it now.
+         */
+        rt2x00usb_register_write(rt2x00dev, H2M_BBP_AGENT, 0);
+        rt2x00usb_register_write(rt2x00dev, H2M_MAILBOX_CSR, 0);
+        msleep(1);
+
         for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
                 rt2800usb_bbp_read(rt2x00dev, 0, &value);
                 if ((value != 0xff) && (value != 0x00))
@@ -1823,8 +1849,12 @@ static int rt2800usb_enable_radio(struct rt2x00_dev *rt2x00dev)
         rt2x00_set_field32(&reg, USB_DMA_CFG_RX_BULK_AGG_EN,
                            (rt2x00dev->rx->usb_maxpacket == 512));
         rt2x00_set_field32(&reg, USB_DMA_CFG_RX_BULK_AGG_TIMEOUT, 128);
-        /* FIXME: Calculate this value based on Aggregation defines */
-        rt2x00_set_field32(&reg, USB_DMA_CFG_RX_BULK_AGG_LIMIT, 21);
+        /*
+         * Total room for RX frames in kilobytes, PBF might still exceed
+         * this limit so reduce the number to prevent errors.
+         */
+        rt2x00_set_field32(&reg, USB_DMA_CFG_RX_BULK_AGG_LIMIT,
+                           ((RX_ENTRIES * DATA_FRAME_SIZE) / 1024) - 3);
         rt2x00_set_field32(&reg, USB_DMA_CFG_RX_BULK_EN, 1);
         rt2x00_set_field32(&reg, USB_DMA_CFG_TX_BULK_EN, 1);
         rt2x00usb_register_write(rt2x00dev, USB_DMA_CFG, reg);
@@ -1835,11 +1865,6 @@ static int rt2800usb_enable_radio(struct rt2x00_dev *rt2x00dev)
         rt2x00usb_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
 
         /*
-         * Send signal to firmware during boot time.
-         */
-        rt2800usb_mcu_request(rt2x00dev, MCU_BOOT_SIGNAL, 0xff, 0, 0);
-
-        /*
          * Initialize LED control
          */
         rt2x00_eeprom_read(rt2x00dev, EEPROM_LED1, &word);
@@ -1879,8 +1904,6 @@ static void rt2800usb_disable_radio(struct rt2x00_dev *rt2x00dev)
 static int rt2800usb_set_state(struct rt2x00_dev *rt2x00dev,
                                enum dev_state state)
 {
-        rt2x00usb_register_write(rt2x00dev, AUTOWAKEUP_CFG, 0);
-
         if (state == STATE_AWAKE)
                 rt2800usb_mcu_request(rt2x00dev, MCU_WAKEUP, 0xff, 0, 0);
         else
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.h b/drivers/net/wireless/rt2x00/rt2800usb.h
index 8e4291d280b..61a8be61d3f 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.h
+++ b/drivers/net/wireless/rt2x00/rt2800usb.h
@@ -1375,6 +1375,10 @@ struct mac_iveiv_entry {
  * H2M_MAILBOX_CID:
  */
 #define H2M_MAILBOX_CID                 0x7014
+#define H2M_MAILBOX_CID_CMD0            FIELD32(0x000000ff)
+#define H2M_MAILBOX_CID_CMD1            FIELD32(0x0000ff00)
+#define H2M_MAILBOX_CID_CMD2            FIELD32(0x00ff0000)
+#define H2M_MAILBOX_CID_CMD3            FIELD32(0xff000000)
 
 /*
  * H2M_MAILBOX_STATUS:
@@ -1715,6 +1719,7 @@ struct mac_iveiv_entry {
 #define MCU_SLEEP                       0x30
 #define MCU_WAKEUP                      0x31
 #define MCU_RADIO_OFF                   0x35
+#define MCU_CURRENT                     0x36
 #define MCU_LED                         0x50
 #define MCU_LED_STRENGTH                0x51
 #define MCU_LED_1                       0x52
@@ -1723,6 +1728,12 @@ struct mac_iveiv_entry {
 #define MCU_RADAR                       0x60
 #define MCU_BOOT_SIGNAL                 0x72
 #define MCU_BBP_SIGNAL                  0x80
+#define MCU_POWER_SAVE                  0x83
+
+/*
+ * MCU mailbox tokens
+ */
+#define TOKEN_WAKUP                     3
 
 /*
  * DMA descriptor defines.