rt2x00 : fix rt3290 resuming failed.

This patch is going to fix the resuming failed from S3/S4
for rt3290 chip.

Signed-off-by: Woody Hung <Woody.Hung@mediatek.com>
Cc: Kevin Chou <kevin.chou@mediatek.com>
Signed-off-by: Chen, Chien-Chia <machen@suse.com>
Reviewed-by: Stanislaw Gruszka <sgruszka@redhat.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

2 files changed, 68 insertions, 71 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index 88455b1b9fe0..cb8c2aca54e4 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -221,6 +221,67 @@ static void rt2800_rf_write(struct rt2x00_dev *rt2x00dev,
         mutex_unlock(&rt2x00dev->csr_mutex);
 }
 
+static int rt2800_enable_wlan_rt3290(struct rt2x00_dev *rt2x00dev)
+{
+        u32 reg;
+        int i, count;
+
+        rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+        if (rt2x00_get_field32(reg, WLAN_EN))
+                return 0;
+
+        rt2x00_set_field32(&reg, WLAN_GPIO_OUT_OE_BIT_ALL, 0xff);
+        rt2x00_set_field32(&reg, FRC_WL_ANT_SET, 1);
+        rt2x00_set_field32(&reg, WLAN_CLK_EN, 0);
+        rt2x00_set_field32(&reg, WLAN_EN, 1);
+        rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
+
+        udelay(REGISTER_BUSY_DELAY);
+
+        count = 0;
+        do {
+                /*
+                 * Check PLL_LD & XTAL_RDY.
+                 */
+                for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+                        rt2800_register_read(rt2x00dev, CMB_CTRL, &reg);
+                        if (rt2x00_get_field32(reg, PLL_LD) &&
+                            rt2x00_get_field32(reg, XTAL_RDY))
+                                break;
+                        udelay(REGISTER_BUSY_DELAY);
+                }
+
+                if (i >= REGISTER_BUSY_COUNT) {
+
+                        if (count >= 10)
+                                return -EIO;
+
+                        rt2800_register_write(rt2x00dev, 0x58, 0x018);
+                        udelay(REGISTER_BUSY_DELAY);
+                        rt2800_register_write(rt2x00dev, 0x58, 0x418);
+                        udelay(REGISTER_BUSY_DELAY);
+                        rt2800_register_write(rt2x00dev, 0x58, 0x618);
+                        udelay(REGISTER_BUSY_DELAY);
+                        count++;
+                } else {
+                        count = 0;
+                }
+
+                rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+                rt2x00_set_field32(&reg, PCIE_APP0_CLK_REQ, 0);
+                rt2x00_set_field32(&reg, WLAN_CLK_EN, 1);
+                rt2x00_set_field32(&reg, WLAN_RESET, 1);
+                rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
+                udelay(10);
+                rt2x00_set_field32(&reg, WLAN_RESET, 0);
+                rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
+                udelay(10);
+                rt2800_register_write(rt2x00dev, INT_SOURCE_CSR, 0x7fffffff);
+        } while (count != 0);
+
+        return 0;
+}
+
 void rt2800_mcu_request(struct rt2x00_dev *rt2x00dev,
                         const u8 command, const u8 token,
                         const u8 arg0, const u8 arg1)
@@ -400,6 +461,13 @@ int rt2800_load_firmware(struct rt2x00_dev *rt2x00dev,
 {
         unsigned int i;
         u32 reg;
+        int retval;
+
+        if (rt2x00_rt(rt2x00dev, RT3290)) {
+                retval = rt2800_enable_wlan_rt3290(rt2x00dev);
+                if (retval)
+                        return -EBUSY;
+        }
 
         /*
          * If driver doesn't wake up firmware here,
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index 235376e9cb04..98aa426a3564 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -980,66 +980,6 @@ static int rt2800pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
         return rt2800_validate_eeprom(rt2x00dev);
 }
 
-static int rt2800_enable_wlan_rt3290(struct rt2x00_dev *rt2x00dev)
-{
-        u32 reg;
-        int i, count;
-
-        rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
-        if (rt2x00_get_field32(reg, WLAN_EN))
-                return 0;
-
-        rt2x00_set_field32(&reg, WLAN_GPIO_OUT_OE_BIT_ALL, 0xff);
-        rt2x00_set_field32(&reg, FRC_WL_ANT_SET, 1);
-        rt2x00_set_field32(&reg, WLAN_CLK_EN, 0);
-        rt2x00_set_field32(&reg, WLAN_EN, 1);
-        rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
-
-        udelay(REGISTER_BUSY_DELAY);
-
-        count = 0;
-        do {
-                /*
-                 * Check PLL_LD & XTAL_RDY.
-                 */
-                for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-                        rt2800_register_read(rt2x00dev, CMB_CTRL, &reg);
-                        if (rt2x00_get_field32(reg, PLL_LD) &&
-                            rt2x00_get_field32(reg, XTAL_RDY))
-                                break;
-                        udelay(REGISTER_BUSY_DELAY);
-                }
-
-                if (i >= REGISTER_BUSY_COUNT) {
-
-                        if (count >= 10)
-                                return -EIO;
-
-                        rt2800_register_write(rt2x00dev, 0x58, 0x018);
-                        udelay(REGISTER_BUSY_DELAY);
-                        rt2800_register_write(rt2x00dev, 0x58, 0x418);
-                        udelay(REGISTER_BUSY_DELAY);
-                        rt2800_register_write(rt2x00dev, 0x58, 0x618);
-                        udelay(REGISTER_BUSY_DELAY);
-                        count++;
-                } else {
-                        count = 0;
-                }
-
-                rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
-                rt2x00_set_field32(&reg, PCIE_APP0_CLK_REQ, 0);
-                rt2x00_set_field32(&reg, WLAN_CLK_EN, 1);
-                rt2x00_set_field32(&reg, WLAN_RESET, 1);
-                rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
-                udelay(10);
-                rt2x00_set_field32(&reg, WLAN_RESET, 0);
-                rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
-                udelay(10);
-                rt2800_register_write(rt2x00dev, INT_SOURCE_CSR, 0x7fffffff);
-        } while (count != 0);
-
-        return 0;
-}
 static int rt2800pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 {
         int retval;
@@ -1063,17 +1003,6 @@ static int rt2800pci_probe_hw(struct rt2x00_dev *rt2x00dev)
                 return retval;
 
         /*
-         * In probe phase call rt2800_enable_wlan_rt3290 to enable wlan
-         * clk for rt3290. That avoid the MCU fail in start phase.
-         */
-        if (rt2x00_rt(rt2x00dev, RT3290)) {
-                retval = rt2800_enable_wlan_rt3290(rt2x00dev);
-
-                if (retval)
-                        return retval;
-        }
-
-        /*
          * This device has multiple filters for control frames
          * and has a separate filter for PS Poll frames.
          */