Staging: rt2870: add Antenna Diversity support
[safe/jmp/linux-2.6] / drivers / staging / rt2860 / common / rtmp_init.c
index 19a21ff..e6a43b6 100644 (file)
     Jan Lee  2006-09-15    RT2860. Change for 802.11n , EEPROM, Led, BA, HT.
 */
 #include "../rt_config.h"
-#include       "firmware.h"
+#ifndef RT30xx
+#ifdef RT2860
+#include "firmware.h"
 #include <linux/bitrev.h>
+#endif
+#ifdef RT2870
+#include "../../rt2870/common/firmware.h"
+#endif
+#endif
+#ifdef RT30xx
+#include "../../rt3070/firmware.h"
+#endif
 
 UCHAR    BIT8[] = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};
 ULONG    BIT32[] = {0x00000001, 0x00000002, 0x00000004, 0x00000008,
@@ -90,6 +100,22 @@ const unsigned short ccitt_16Table[] = {
 #define ByteCRC16(v, crc) \
        (unsigned short)((crc << 8) ^  ccitt_16Table[((crc >> 8) ^ (v)) & 255])
 
+#ifdef RT2870
+unsigned char BitReverse(unsigned char x)
+{
+       int i;
+       unsigned char Temp=0;
+       for(i=0; ; i++)
+       {
+               if(x & 0x80)    Temp |= 0x80;
+               if(i==7)                break;
+               x       <<= 1;
+               Temp >>= 1;
+       }
+       return Temp;
+}
+#endif
+
 //
 // BBP register initialization set
 //
@@ -114,6 +140,38 @@ REG_PAIR   BBPRegTable[] = {
 //
 // RF register initialization set
 //
+#ifdef RT2870
+REG_PAIR   RT30xx_RFRegTable[] = {
+        {RF_R04,          0x40},
+        {RF_R05,          0x03},
+        {RF_R06,          0x02},
+        {RF_R07,          0x70},
+        {RF_R09,          0x0F},
+#ifndef RT30xx
+        {RF_R10,          0x71},
+#endif
+#ifdef RT30xx
+        {RF_R10,          0x41},
+#endif
+        {RF_R11,          0x21},
+        {RF_R12,          0x7B},
+        {RF_R14,          0x90},
+        {RF_R15,          0x58},
+        {RF_R16,          0xB3},
+        {RF_R17,          0x92},
+        {RF_R18,          0x2C},
+        {RF_R19,          0x02},
+        {RF_R20,          0xBA},
+        {RF_R21,          0xDB},
+        {RF_R24,          0x16},
+        {RF_R25,          0x01},
+#ifndef RT30xx
+        {RF_R27,          0x03},
+#endif
+        {RF_R29,          0x1F},
+};
+#define        NUM_RF_REG_PARMS        (sizeof(RT30xx_RFRegTable) / sizeof(REG_PAIR))
+#endif // RT2870 //
 
 //
 // ASIC register initialization sets
@@ -146,12 +204,18 @@ RTMP_REG_PAIR     MACRegTable[] = {
        {AUTO_RSP_CFG,                  0x00000013},    // Initial Auto_Responder, because QA will turn off Auto-Responder
        {CCK_PROT_CFG,                  0x05740003 /*0x01740003*/},     // Initial Auto_Responder, because QA will turn off Auto-Responder. And RTS threshold is enabled.
        {OFDM_PROT_CFG,                 0x05740003 /*0x01740003*/},     // Initial Auto_Responder, because QA will turn off Auto-Responder. And RTS threshold is enabled.
+//PS packets use Tx1Q (for HCCA) when dequeue from PS unicast queue (WiFi WPA2 MA9_DT1 for Marvell B STA)
+#ifdef RT2870
+       {PBF_CFG,                               0xf40006},              // Only enable Queue 2
+       {MM40_PROT_CFG,                 0x3F44084},             // Initial Auto_Responder, because QA will turn off Auto-Responder
+       {WPDMA_GLO_CFG,                 0x00000030},
+#endif // RT2870 //
        {GF20_PROT_CFG,                 0x01744004},    // set 19:18 --> Short NAV for MIMO PS
        {GF40_PROT_CFG,                 0x03F44084},
        {MM20_PROT_CFG,                 0x01744004},
 #ifdef RT2860
        {MM40_PROT_CFG,                 0x03F54084},
-#endif // RT2860 //
+#endif
        {TXOP_CTRL_CFG,                 0x0000583f, /*0x0000243f*/ /*0x000024bf*/},     //Extension channel backoff.
        {TX_RTS_CFG,                    0x00092b20},
        {EXP_ACK_TIME,                  0x002400ca},    // default value
@@ -165,20 +229,22 @@ RTMP_REG_PAIR     MACRegTable[] = {
        {PWR_PIN_CFG,                   0x00000003},    // patch for 2880-E
 };
 
-
-#ifdef CONFIG_STA_SUPPORT
 RTMP_REG_PAIR  STAMACRegTable[] =      {
        {WMM_AIFSN_CFG,         0x00002273},
        {WMM_CWMIN_CFG, 0x00002344},
        {WMM_CWMAX_CFG, 0x000034aa},
 };
-#endif // CONFIG_STA_SUPPORT //
 
 #define        NUM_MAC_REG_PARMS               (sizeof(MACRegTable) / sizeof(RTMP_REG_PAIR))
-#ifdef CONFIG_STA_SUPPORT
 #define        NUM_STA_MAC_REG_PARMS   (sizeof(STAMACRegTable) / sizeof(RTMP_REG_PAIR))
-#endif // CONFIG_STA_SUPPORT //
 
+#ifdef RT2870
+//
+// RT2870 Firmware Spec only used 1 oct for version expression
+//
+#define FIRMWARE_MINOR_VERSION 7
+
+#endif // RT2870 //
 
 // New 8k byte firmware size for RT3071/RT3072
 #define FIRMWAREIMAGE_MAX_LENGTH       0x2000
@@ -190,7 +256,7 @@ RTMP_REG_PAIR       STAMACRegTable[] =      {
 
 #ifdef RT2860
 #define FIRMWARE_MINOR_VERSION 2
-#endif // RT2860 //
+#endif
 
 
 /*
@@ -250,7 +316,7 @@ NDIS_STATUS RTMPAllocAdapterBlock(
                NdisAllocateSpinLock(&pAd->MgmtRingLock);
 #ifdef RT2860
                NdisAllocateSpinLock(&pAd->RxRingLock);
-#endif // RT2860 //
+#endif
 
                for (index =0 ; index < NUM_OF_TX_RING; index++)
                {
@@ -980,67 +1046,6 @@ VOID      RTMPReadChannelPwr(
 
        // 4. Print and Debug
        choffset = 14 + 12 + 16 + 7;
-
-
-#if 0
-       // Init the 802.11j channel number for TX channel power
-       // 0. 20MHz
-       for (i = 0; i < 3; i++)
-       {
-               pAd->TxPower11J[i].Channel = 8 + i * 4;
-               pAd->TxPower11J[i].BW = BW_20;
-       }
-
-       for (i = 0; i < 4; i++)
-       {
-               pAd->TxPower11J[i + 3].Channel = 34 + i * 4;
-               pAd->TxPower11J[i + 3].BW = BW_20;
-       }
-
-       for (i = 0; i < 4; i++)
-       {
-               pAd->TxPower11J[i + 7].Channel = 184 + i * 4;
-               pAd->TxPower11J[i + 7].BW = BW_20;
-       }
-
-       // 0. 10MHz
-       for (i = 0; i < 2; i++)
-       {
-               pAd->TxPower11J[i + 11].Channel = 7 + i;
-               pAd->TxPower11J[i + 11].BW = BW_10;
-       }
-       pAd->TxPower11J[13].Channel = 11;
-       pAd->TxPower11J[13].BW = BW_10;
-
-       for (i = 0; i < 3; i++)
-       {
-               pAd->TxPower11J[i + 14].Channel = 183 + i;
-               pAd->TxPower11J[i + 14].BW= BW_10;
-       }
-
-       for (i = 0; i < 3; i++)
-       {
-               pAd->TxPower11J[i + 17].Channel = 187 + i;
-               pAd->TxPower11J[i + 17].BW = BW_10;
-       }
-       for (i = 0; i < 10; i++)
-       {
-               Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_Japan_TX_PWR_OFFSET + i * 2);
-               Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_Japan_TX2_PWR_OFFSET + i * 2);
-
-               if ((Power.field.Byte0 < 36) && (Power.field.Byte0 > -6))
-                       pAd->TxPower11J[i * 2].Power = Power.field.Byte0;
-
-               if ((Power.field.Byte1 < 36) && (Power.field.Byte1 > -6))
-                       pAd->TxPower11J[i * 2 + 1].Power = Power.field.Byte1;
-
-               if ((Power2.field.Byte0 < 36) && (Power2.field.Byte0 > -6))
-                       pAd->TxPower11J[i * 2].Power2 = Power2.field.Byte0;
-
-               if ((Power2.field.Byte1 < 36) && (Power2.field.Byte1 > -6))
-                       pAd->TxPower11J[i * 2 + 1].Power2 = Power2.field.Byte1;
-       }
-#endif
 }
 
 /*
@@ -1077,6 +1082,274 @@ NDIS_STATUS     NICReadRegParameters(
 }
 
 
+#ifdef RT2870
+/*
+       ========================================================================
+
+       Routine Description:
+               For RF filter calibration purpose
+
+       Arguments:
+               pAd                          Pointer to our adapter
+
+       Return Value:
+               None
+
+       IRQL = PASSIVE_LEVEL
+
+       ========================================================================
+*/
+VOID RTMPFilterCalibration(
+       IN PRTMP_ADAPTER pAd)
+{
+       UCHAR   R55x = 0, value, FilterTarget = 0x1E, BBPValue=0;
+       UINT    loop = 0, count = 0, loopcnt = 0, ReTry = 0;
+       UCHAR   RF_R24_Value = 0;
+
+       // Give bbp filter initial value
+#ifndef RT30xx
+       pAd->Mlme.CaliBW20RfR24 = 0x16;
+       pAd->Mlme.CaliBW40RfR24 = 0x36;  //Bit[5] must be 1 for BW 40
+#else
+       pAd->Mlme.CaliBW20RfR24 = 0x1F;
+       pAd->Mlme.CaliBW40RfR24 = 0x2F; //Bit[5] must be 1 for BW 40
+#endif
+       do
+       {
+               if (loop == 1)  //BandWidth = 40 MHz
+               {
+                       // Write 0x27 to RF_R24 to program filter
+                       RF_R24_Value = 0x27;
+                       RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+                       if (IS_RT3090(pAd))
+                               FilterTarget = 0x15;
+                       else
+                               FilterTarget = 0x19;
+
+                       // when calibrate BW40, BBP mask must set to BW40.
+                       RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+                       BBPValue&= (~0x18);
+                       BBPValue|= (0x10);
+                       RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+#ifdef RT30xx
+                       // set to BW40
+                       RT30xxReadRFRegister(pAd, RF_R31, &value);
+                       value |= 0x20;
+                       RT30xxWriteRFRegister(pAd, RF_R31, value);
+#endif
+               }
+               else                    //BandWidth = 20 MHz
+               {
+                       // Write 0x07 to RF_R24 to program filter
+                       RF_R24_Value = 0x07;
+                       RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+                       if (IS_RT3090(pAd))
+                               FilterTarget = 0x13;
+                       else
+                               FilterTarget = 0x16;
+#ifdef RT30xx
+                       // set to BW20
+                       RT30xxReadRFRegister(pAd, RF_R31, &value);
+                       value &= (~0x20);
+                       RT30xxWriteRFRegister(pAd, RF_R31, value);
+#endif
+               }
+
+               // Write 0x01 to RF_R22 to enable baseband loopback mode
+               RT30xxReadRFRegister(pAd, RF_R22, &value);
+               value |= 0x01;
+               RT30xxWriteRFRegister(pAd, RF_R22, value);
+
+               // Write 0x00 to BBP_R24 to set power & frequency of passband test tone
+               RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R24, 0);
+
+               do
+               {
+                       // Write 0x90 to BBP_R25 to transmit test tone
+                       RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R25, 0x90);
+
+                       RTMPusecDelay(1000);
+                       // Read BBP_R55[6:0] for received power, set R55x = BBP_R55[6:0]
+                       RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R55, &value);
+                       R55x = value & 0xFF;
+
+               } while ((ReTry++ < 100) && (R55x == 0));
+
+               // Write 0x06 to BBP_R24 to set power & frequency of stopband test tone
+               RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R24, 0x06);
+
+               while(TRUE)
+               {
+                       // Write 0x90 to BBP_R25 to transmit test tone
+                       RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R25, 0x90);
+
+                       //We need to wait for calibration
+                       RTMPusecDelay(1000);
+                       RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R55, &value);
+                       value &= 0xFF;
+                       if ((R55x - value) < FilterTarget)
+                       {
+                               RF_R24_Value ++;
+                       }
+                       else if ((R55x - value) == FilterTarget)
+                       {
+                               RF_R24_Value ++;
+                               count ++;
+                       }
+                       else
+                       {
+                               break;
+                       }
+
+                       // prevent infinite loop cause driver hang.
+                       if (loopcnt++ > 100)
+                       {
+                               DBGPRINT(RT_DEBUG_ERROR, ("RTMPFilterCalibration - can't find a valid value, loopcnt=%d stop calibrating", loopcnt));
+                               break;
+                       }
+
+                       // Write RF_R24 to program filter
+                       RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+               }
+
+               if (count > 0)
+               {
+                       RF_R24_Value = RF_R24_Value - ((count) ? (1) : (0));
+               }
+
+               // Store for future usage
+               if (loopcnt < 100)
+               {
+                       if (loop++ == 0)
+                       {
+                               //BandWidth = 20 MHz
+                               pAd->Mlme.CaliBW20RfR24 = (UCHAR)RF_R24_Value;
+                       }
+                       else
+                       {
+                               //BandWidth = 40 MHz
+                               pAd->Mlme.CaliBW40RfR24 = (UCHAR)RF_R24_Value;
+                               break;
+                       }
+               }
+               else
+                       break;
+
+               RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+
+               // reset count
+               count = 0;
+       } while(TRUE);
+
+       //
+       // Set back to initial state
+       //
+       RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R24, 0);
+
+       RT30xxReadRFRegister(pAd, RF_R22, &value);
+       value &= ~(0x01);
+       RT30xxWriteRFRegister(pAd, RF_R22, value);
+
+       // set BBP back to BW20
+       RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+       BBPValue&= (~0x18);
+       RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+
+       DBGPRINT(RT_DEBUG_TRACE, ("RTMPFilterCalibration - CaliBW20RfR24=0x%x, CaliBW40RfR24=0x%x\n", pAd->Mlme.CaliBW20RfR24, pAd->Mlme.CaliBW40RfR24));
+}
+
+VOID NICInitRT30xxRFRegisters(IN PRTMP_ADAPTER pAd)
+{
+       INT i;
+       // Driver must read EEPROM to get RfIcType before initial RF registers
+       // Initialize RF register to default value
+       if (IS_RT3070(pAd)
+#ifdef RT30xx
+           || IS_RT3071(pAd)
+#else
+           && (pAd->RfIcType == RFIC_3020 || pAd->RfIcType == RFIC_2020)
+#endif
+          )
+       {
+               // Init RF calibration
+               // Driver should toggle RF R30 bit7 before init RF registers
+               UINT32 RfReg = 0;
+#ifdef RT30xx
+               UINT32 data;
+#endif
+               RT30xxReadRFRegister(pAd, RF_R30, (PUCHAR)&RfReg);
+               RfReg |= 0x80;
+               RT30xxWriteRFRegister(pAd, RF_R30, (UCHAR)RfReg);
+               RTMPusecDelay(1000);
+               RfReg &= 0x7F;
+               RT30xxWriteRFRegister(pAd, RF_R30, (UCHAR)RfReg);
+
+               // Initialize RF register to default value
+               for (i = 0; i < NUM_RF_REG_PARMS; i++)
+               {
+                       RT30xxWriteRFRegister(pAd, RT30xx_RFRegTable[i].Register, RT30xx_RFRegTable[i].Value);
+               }
+
+#ifdef RT30xx
+               if (IS_RT3070(pAd))
+               {
+                       //  Update MAC 0x05D4 from 01xxxxxx to 0Dxxxxxx (voltage 1.2V to 1.35V) for RT3070 to improve yield rate
+                       RTUSBReadMACRegister(pAd, LDO_CFG0, &data);
+                       data = ((data & 0xF0FFFFFF) | 0x0D000000);
+                       RTUSBWriteMACRegister(pAd, LDO_CFG0, data);
+               }
+               else if (IS_RT3071(pAd))
+               {
+                       // Driver should set RF R6 bit6 on before init RF registers
+                       RT30xxReadRFRegister(pAd, RF_R06, (PUCHAR)&RfReg);
+                       RfReg |= 0x40;
+                       RT30xxWriteRFRegister(pAd, RF_R06, (UCHAR)RfReg);
+
+                       // init R31
+                       RT30xxWriteRFRegister(pAd, RF_R31, 0x14);
+
+                       // RT3071 version E has fixed this issue
+                       if ((pAd->NicConfig2.field.DACTestBit == 1) && ((pAd->MACVersion & 0xffff) < 0x0211))
+                       {
+                               // patch tx EVM issue temporarily
+                               RTUSBReadMACRegister(pAd, LDO_CFG0, &data);
+                               data = ((data & 0xE0FFFFFF) | 0x0D000000);
+                               RTUSBWriteMACRegister(pAd, LDO_CFG0, data);
+                       }
+                       else
+                       {
+                               RTMP_IO_READ32(pAd, LDO_CFG0, &data);
+                               data = ((data & 0xE0FFFFFF) | 0x01000000);
+                               RTMP_IO_WRITE32(pAd, LDO_CFG0, data);
+                       }
+
+                       // patch LNA_PE_G1 failed issue
+                       RTUSBReadMACRegister(pAd, GPIO_SWITCH, &data);
+                       data &= ~(0x20);
+                       RTUSBWriteMACRegister(pAd, GPIO_SWITCH, data);
+               }
+#endif
+               //For RF filter Calibration
+               RTMPFilterCalibration(pAd);
+#ifdef RT30xx
+               // Initialize RF R27 register, set RF R27 must be behind RTMPFilterCalibration()
+               if ((pAd->MACVersion & 0xffff) < 0x0211)
+                       RT30xxWriteRFRegister(pAd, RF_R27, 0x3);
+
+               // set led open drain enable
+               RTUSBReadMACRegister(pAd, OPT_14, &data);
+               data |= 0x01;
+               RTUSBWriteMACRegister(pAd, OPT_14, data);
+
+               if (IS_RT3071(pAd))
+               {
+                       // add by johnli, RF power sequence setup, load RF normal operation-mode setup
+                       RT30xxLoadRFNormalModeSetup(pAd);
+               }
+#endif
+       }
+}
+#endif // RT2870 //
 
 
 /*
@@ -1251,11 +1524,21 @@ VOID    NICReadEEPROMParameters(
        Antenna.word = pAd->EEPROMDefaultValue[0];
        if (Antenna.word == 0xFFFF)
        {
+               if(IS_RT3090(pAd))
+               {
+                       Antenna.word = 0;
+                       Antenna.field.RfIcType = RFIC_3020;
+                       Antenna.field.TxPath = 1;
+                       Antenna.field.RxPath = 1;
+               }
+               else
+               {
                Antenna.word = 0;
                Antenna.field.RfIcType = RFIC_2820;
                Antenna.field.TxPath = 1;
                Antenna.field.RxPath = 2;
                DBGPRINT(RT_DEBUG_WARN, ("E2PROM error, hard code as 0x%04x\n", Antenna.word));
+               }
        }
 
        // Choose the desired Tx&Rx stream.
@@ -1283,12 +1566,10 @@ VOID    NICReadEEPROMParameters(
 
        NicConfig2.word = pAd->EEPROMDefaultValue[1];
 
-
-
-#ifdef CONFIG_STA_SUPPORT
-       IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
        {
+#ifndef RT30xx
                NicConfig2.word = 0;
+#endif
                if ((NicConfig2.word & 0x00ff) == 0xff)
                {
                        NicConfig2.word &= 0xff00;
@@ -1299,7 +1580,6 @@ VOID      NICReadEEPROMParameters(
                        NicConfig2.word &= 0x00ff;
                }
        }
-#endif // CONFIG_STA_SUPPORT //
 
        if (NicConfig2.field.DynamicTxAgcControl == 1)
                pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = TRUE;
@@ -1320,13 +1600,11 @@ VOID    NICReadEEPROMParameters(
                if ((pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED) ||
                        (pAd->CommonCfg.PhyMode == PHY_11A))
                        pAd->CommonCfg.PhyMode = PHY_11BG_MIXED;
-#ifdef DOT11_N_SUPPORT
                else if ((pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED)   ||
                                 (pAd->CommonCfg.PhyMode == PHY_11AN_MIXED)     ||
                                 (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED)    ||
                                 (pAd->CommonCfg.PhyMode == PHY_11N_5G))
                        pAd->CommonCfg.PhyMode = PHY_11BGN_MIXED;
-#endif // DOT11_N_SUPPORT //
        }
 
        // Read TSSI reference and TSSI boundary for temperature compensation. This is ugly
@@ -1417,9 +1695,7 @@ VOID      NICReadEEPROMParameters(
                TmpPhy = pAd->CommonCfg.PhyMode;
                pAd->CommonCfg.PhyMode = 0xff;
                RTMPSetPhyMode(pAd, TmpPhy);
-#ifdef DOT11_N_SUPPORT
                SetCommonHT(pAd);
-#endif // DOT11_N_SUPPORT //
        }
 
        //
@@ -1486,11 +1762,6 @@ VOID     NICReadEEPROMParameters(
 
        RTMPReadTxPwrPerRate(pAd);
 
-#ifdef SINGLE_SKU
-       //pAd->CommonCfg.DefineMaxTxPwr = RTMP_EEPROM_READ16(pAd, EEPROM_DEFINE_MAX_TXPWR);
-       RT28xx_EEPROM_READ16(pAd, EEPROM_DEFINE_MAX_TXPWR, pAd->CommonCfg.DefineMaxTxPwr);
-#endif // SINGLE_SKU //
-
        DBGPRINT(RT_DEBUG_TRACE, ("<-- NICReadEEPROMParameters\n"));
 }
 
@@ -1515,10 +1786,8 @@ VOID     NICReadEEPROMParameters(
 VOID   NICInitAsicFromEEPROM(
        IN      PRTMP_ADAPTER   pAd)
 {
-#ifdef CONFIG_STA_SUPPORT
        UINT32                                  data = 0;
        UCHAR   BBPR1 = 0;
-#endif // CONFIG_STA_SUPPORT //
        USHORT                                  i;
        EEPROM_ANTENNA_STRUC    Antenna;
        EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
@@ -1537,16 +1806,49 @@ VOID    NICInitAsicFromEEPROM(
                }
        }
 
+#ifndef RT30xx
        Antenna.word = pAd->Antenna.word;
+#endif
+#ifdef RT30xx
+       Antenna.word = pAd->EEPROMDefaultValue[0];
+       if (Antenna.word == 0xFFFF)
+       {
+               DBGPRINT(RT_DEBUG_ERROR, ("E2PROM error, hard code as 0x%04x\n", Antenna.word));
+               BUG_ON(Antenna.word == 0xFFFF);
+       }
+#endif
        pAd->Mlme.RealRxPath = (UCHAR) Antenna.field.RxPath;
        pAd->RfIcType = (UCHAR) Antenna.field.RfIcType;
 
+#ifdef RT30xx
+       DBGPRINT(RT_DEBUG_WARN, ("pAd->RfIcType = %d, RealRxPath=%d, TxPath = %d\n", pAd->RfIcType, pAd->Mlme.RealRxPath,Antenna.field.TxPath));
+
+       // Save the antenna for future use
+       pAd->Antenna.word = Antenna.word;
+#endif
        NicConfig2.word = pAd->EEPROMDefaultValue[1];
 
+#ifdef RT30xx
+       {
+               if ((NicConfig2.word & 0x00ff) == 0xff)
+               {
+                       NicConfig2.word &= 0xff00;
+               }
 
+               if ((NicConfig2.word >> 8) == 0xff)
+               {
+                       NicConfig2.word &= 0x00ff;
+               }
+       }
+#endif
        // Save the antenna for future use
        pAd->NicConfig2.word = NicConfig2.word;
 
+#ifdef RT30xx
+       // set default antenna as main
+       if (pAd->RfIcType == RFIC_3020)
+               AsicSetRxAnt(pAd, pAd->RxAnt.Pair1PrimaryRxAnt);
+#endif
        //
        // Send LED Setting to MCU.
        //
@@ -1555,10 +1857,13 @@ VOID    NICInitAsicFromEEPROM(
                pAd->LedCntl.word = 0x01;
                pAd->Led1 = 0x5555;
                pAd->Led2 = 0x2221;
-
 #ifdef RT2860
                pAd->Led3 = 0xA9F8;
-#endif // RT2860 //
+#endif
+
+#ifdef RT2870
+               pAd->Led3 = 0x5627;
+#endif // RT2870 //
        }
 
        AsicSendCommandToMcu(pAd, 0x52, 0xff, (UCHAR)pAd->Led1, (UCHAR)(pAd->Led1 >> 8));
@@ -1567,8 +1872,6 @@ VOID      NICInitAsicFromEEPROM(
     pAd->LedIndicatorStregth = 0xFF;
     RTMPSetSignalLED(pAd, -100);       // Force signal strength Led to be turned off, before link up
 
-#ifdef CONFIG_STA_SUPPORT
-       IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
        {
                // Read Hardware controlled Radio state enable bit
                if (NicConfig2.field.HardwareRadioControl == 1)
@@ -1599,10 +1902,9 @@ VOID     NICInitAsicFromEEPROM(
                        AsicSendCommandToMcu(pAd, 0x31, PowerWakeCID, 0x00, 0x00);
                        // 2-1. wait command ok.
                        AsicCheckCommanOk(pAd, PowerWakeCID);
-#endif // RT2860 //
+#endif
                }
        }
-#endif // CONFIG_STA_SUPPORT //
 
        // Turn off patching for cardbus controller
        if (NicConfig2.field.CardbusAcceleration == 1)
@@ -1613,10 +1915,8 @@ VOID     NICInitAsicFromEEPROM(
                pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = TRUE;
        else
                pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = FALSE;
-       //
-       // Since BBP has been progamed, to make sure BBP setting will be
-       // upate inside of AsicAntennaSelect, so reset to UNKNOWN_BAND!!
-       //
+
+       /* BBP has been programmed so reset to UNKNOWN_BAND */
        pAd->CommonCfg.BandState = UNKNOWN_BAND;
 
        RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
@@ -1635,8 +1935,6 @@ VOID      NICInitAsicFromEEPROM(
        }
        RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
 
-#ifdef CONFIG_STA_SUPPORT
-       IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
        {
                // Handle the difference when 1T
                RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BBPR1);
@@ -1648,7 +1946,7 @@ VOID      NICInitAsicFromEEPROM(
 
                DBGPRINT(RT_DEBUG_TRACE, ("Use Hw Radio Control Pin=%d; if used Pin=%d;\n", pAd->CommonCfg.bHardwareRadio, pAd->CommonCfg.bHardwareRadio));
        }
-#endif // CONFIG_STA_SUPPORT //
+
        DBGPRINT(RT_DEBUG_TRACE, ("TxPath = %d, RxPath = %d, RFIC=%d, Polar+LED mode=%x\n", pAd->Antenna.field.TxPath, pAd->Antenna.field.RxPath, pAd->RfIcType, pAd->LedCntl.word));
        DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitAsicFromEEPROM\n"));
 }
@@ -1680,7 +1978,7 @@ NDIS_STATUS       NICInitializeAdapter(
 #ifdef RT2860
        UINT32                  Value;
        DELAY_INT_CFG_STRUC     IntCfg;
-#endif // RT2860 //
+#endif
        ULONG   i =0, j=0;
        AC_TXOP_CSR0_STRUC      csr0;
 
@@ -1723,7 +2021,7 @@ retry:
        RTMP_IO_WRITE32(pAd, WPDMA_RST_IDX, 0x1003f);   // 0x10000 for reset rx, 0x3f resets all 6 tx rings.
        RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0xe1f);
        RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0xe00);
-#endif // RT2860 //
+#endif
 
        // Initialze ASIC for TX & Rx operation
        if (NICInitializeAsic(pAd , bHardReset) != NDIS_STATUS_SUCCESS)
@@ -1810,7 +2108,7 @@ retry:
        // Write RX_RING_CSR register
        Value = RX_RING_SIZE;
        RTMP_IO_WRITE32(pAd, RX_MAX_CNT, Value);
-#endif // RT2860 //
+#endif /* RT2860 */
 
 
        // WMM parameter
@@ -1848,7 +2146,7 @@ retry:
 
        IntCfg.word = 0;
        RTMP_IO_WRITE32(pAd, DELAY_INT_CFG, IntCfg.word);
-#endif // RT2860 //
+#endif
 
 
        // reset action
@@ -1884,6 +2182,12 @@ NDIS_STATUS      NICInitializeAsic(
        ULONG                   Index = 0;
        UCHAR                   R0 = 0xff;
        UINT32                  MacCsr12 = 0, Counter = 0;
+#ifdef RT2870
+       UINT32                  MacCsr0 = 0;
+       NTSTATUS                Status;
+       UCHAR                   Value = 0xff;
+       UINT32                  eFuseCtrl;
+#endif
        USHORT                  KeyIdx;
        INT                             i,apidx;
 
@@ -1896,26 +2200,113 @@ NDIS_STATUS    NICInitializeAsic(
        }
        else
                RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+#endif
+#ifdef RT2870
+       //
+       // Make sure MAC gets ready after NICLoadFirmware().
+       //
+       Index = 0;
+
+       //To avoid hang-on issue when interface up in kernel 2.4,
+       //we use a local variable "MacCsr0" instead of using "pAd->MACVersion" directly.
+       do
+       {
+               RTMP_IO_READ32(pAd, MAC_CSR0, &MacCsr0);
+
+               if ((MacCsr0 != 0x00) && (MacCsr0 != 0xFFFFFFFF))
+                       break;
+
+               RTMPusecDelay(10);
+       } while (Index++ < 100);
+
+       pAd->MACVersion = MacCsr0;
+       DBGPRINT(RT_DEBUG_TRACE, ("MAC_CSR0  [ Ver:Rev=0x%08x]\n", pAd->MACVersion));
+       // turn on bit13 (set to zero) after rt2860D. This is to solve high-current issue.
+       RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &MacCsr12);
+       MacCsr12 &= (~0x2000);
+       RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, MacCsr12);
+
+       RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x3);
+       RTMP_IO_WRITE32(pAd, USB_DMA_CFG, 0x0);
+       Status = RTUSBVenderReset(pAd);
+#endif
 
        RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x0);
+
        // Initialize MAC register to default value
+#ifdef RT2860
        for (Index = 0; Index < NUM_MAC_REG_PARMS; Index++)
        {
                RTMP_IO_WRITE32(pAd, MACRegTable[Index].Register, MACRegTable[Index].Value);
        }
+#endif
+#ifdef RT2870
+       for(Index=0; Index<NUM_MAC_REG_PARMS; Index++)
+       {
+#ifdef RT3070
+               if ((MACRegTable[Index].Register == TX_SW_CFG0) && (IS_RT3070(pAd) || IS_RT3071(pAd)))
+               {
+                       MACRegTable[Index].Value = 0x00000400;
+               }
+#endif // RT3070 //
+               RTMP_IO_WRITE32(pAd, (USHORT)MACRegTable[Index].Register, MACRegTable[Index].Value);
+       }
+
+#ifndef RT30xx
+       if(IS_RT3070(pAd))
+       {
+               // According to Frank Hsu (from Gary Tsao)
+               RTMP_IO_WRITE32(pAd, (USHORT)TX_SW_CFG0, 0x00000400);
+
+               // Initialize RT3070 serial MAC registers which is different from RT2870 serial
+               RTUSBWriteMACRegister(pAd, TX_SW_CFG1, 0);
+               RTUSBWriteMACRegister(pAd, TX_SW_CFG2, 0);
+       }
+#endif
+#endif // RT2870 //
 
 
-#ifdef CONFIG_STA_SUPPORT
-       IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
        {
                for (Index = 0; Index < NUM_STA_MAC_REG_PARMS; Index++)
                {
+#ifdef RT2860
                        RTMP_IO_WRITE32(pAd, STAMACRegTable[Index].Register, STAMACRegTable[Index].Value);
+#endif
+#ifdef RT2870
+                       RTMP_IO_WRITE32(pAd, (USHORT)STAMACRegTable[Index].Register, STAMACRegTable[Index].Value);
+#endif
                }
        }
-#endif // CONFIG_STA_SUPPORT //
-#endif // RT2860 //
 
+       // Initialize RT3070 serial MAc registers which is different from RT2870 serial
+       if (IS_RT3090(pAd))
+       {
+               RTMP_IO_WRITE32(pAd, TX_SW_CFG1, 0);
+
+               // RT3071 version E has fixed this issue
+               if ((pAd->MACVersion & 0xffff) < 0x0211)
+               {
+                       if (pAd->NicConfig2.field.DACTestBit == 1)
+                       {
+                               RTMP_IO_WRITE32(pAd, TX_SW_CFG2, 0x1F); // To fix throughput drop drastically
+                       }
+                       else
+                       {
+                               RTMP_IO_WRITE32(pAd, TX_SW_CFG2, 0x0F); // To fix throughput drop drastically
+                       }
+               }
+               else
+               {
+                       RTMP_IO_WRITE32(pAd, TX_SW_CFG2, 0x0);
+               }
+       }
+#ifdef RT30xx
+       else if (IS_RT3070(pAd))
+       {
+               RTMP_IO_WRITE32(pAd, TX_SW_CFG1, 0);
+               RTMP_IO_WRITE32(pAd, TX_SW_CFG2, 0x1F); // To fix throughput drop drastically
+       }
+#endif // RT30xx //
 
        //
        // Before program BBP, we need to wait BBP/RF get wake up.
@@ -1956,11 +2347,69 @@ NDIS_STATUS     NICInitializeAsic(
                RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBPRegTable[Index].Register, BBPRegTable[Index].Value);
        }
 
+#ifndef RT30xx
        // for rt2860E and after, init BBP_R84 with 0x19. This is for extension channel overlapping IOT.
        if ((pAd->MACVersion&0xffff) != 0x0101)
                RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R84, 0x19);
 
+#ifdef RT2870
+       //write RT3070 BBP wchich different with 2870 after write RT2870 BBP
+       if (IS_RT3070(pAd))
+       {
+               RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0a);
+               RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R84, 0x99);
+               RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R105, 0x05);
+       }
+#endif // RT2870 //
+#endif
+#ifdef RT30xx
+       // for rt2860E and after, init BBP_R84 with 0x19. This is for extension channel overlapping IOT.
+       // RT3090 should not program BBP R84 to 0x19, otherwise TX will block.
+       if (((pAd->MACVersion&0xffff) != 0x0101) && (!IS_RT30xx(pAd)))
+               RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R84, 0x19);
+
+// add by johnli, RF power sequence setup
+       if (IS_RT30xx(pAd))
+       {       //update for RT3070/71/72/90/91/92.
+               RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R79, 0x13);
+               RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R80, 0x05);
+               RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R81, 0x33);
+       }
+
+       if (IS_RT3090(pAd))
+       {
+               UCHAR           bbpreg=0;
+
+               // enable DC filter
+               if ((pAd->MACVersion & 0xffff) >= 0x0211)
+               {
+                       RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R103, 0xc0);
+               }
+
+               // improve power consumption
+               RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R138, &bbpreg);
+               if (pAd->Antenna.field.TxPath == 1)
+               {
+                       // turn off tx DAC_1
+                       bbpreg = (bbpreg | 0x20);
+               }
 
+               if (pAd->Antenna.field.RxPath == 1)
+               {
+                       // turn off tx ADC_1
+                       bbpreg &= (~0x2);
+               }
+               RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R138, bbpreg);
+
+               // improve power consumption in RT3071 Ver.E
+               if ((pAd->MACVersion & 0xffff) >= 0x0211)
+               {
+                       RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R31, &bbpreg);
+                       bbpreg &= (~0x3);
+                       RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R31, bbpreg);
+               }
+       }
+#endif
        if (pAd->MACVersion == 0x28600100)
        {
                RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
@@ -1977,10 +2426,20 @@ NDIS_STATUS     NICInitializeAsic(
                RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, csr);
        }
 
+#ifdef RT2870
+{
+       UCHAR   MAC_Value[]={0xff,0xff,0xff,0xff,0xff,0xff,0xff,0,0};
+
+       //Initialize WCID table
+       Value = 0xff;
+       for(Index =0 ;Index < 254;Index++)
+       {
+               RTUSBMultiWrite(pAd, (USHORT)(MAC_WCID_BASE + Index * 8), MAC_Value, 8);
+       }
+}
+#endif // RT2870 //
 
        // Add radio off control
-#ifdef CONFIG_STA_SUPPORT
-       IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
        {
                if (pAd->StaCfg.bRadio == FALSE)
                {
@@ -1989,7 +2448,6 @@ NDIS_STATUS       NICInitializeAsic(
                        DBGPRINT(RT_DEBUG_TRACE, ("Set Radio Off\n"));
                }
        }
-#endif // CONFIG_STA_SUPPORT //
 
        // Clear raw counters
        RTMP_IO_READ32(pAd, RX_STA_CNT0, &Counter);
@@ -2027,26 +2485,51 @@ NDIS_STATUS     NICInitializeAsic(
                                RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[apidx] + i, 0x00);
                }
        }
+#ifdef RT2870
+       AsicDisableSync(pAd);
+       // Clear raw counters
+       RTMP_IO_READ32(pAd, RX_STA_CNT0, &Counter);
+       RTMP_IO_READ32(pAd, RX_STA_CNT1, &Counter);
+       RTMP_IO_READ32(pAd, RX_STA_CNT2, &Counter);
+       RTMP_IO_READ32(pAd, TX_STA_CNT0, &Counter);
+       RTMP_IO_READ32(pAd, TX_STA_CNT1, &Counter);
+       RTMP_IO_READ32(pAd, TX_STA_CNT2, &Counter);
+       // Default PCI clock cycle per ms is different as default setting, which is based on PCI.
+       RTMP_IO_READ32(pAd, USB_CYC_CFG, &Counter);
+       Counter&=0xffffff00;
+       Counter|=0x000001e;
+       RTMP_IO_WRITE32(pAd, USB_CYC_CFG, Counter);
+
+       pAd->bUseEfuse=FALSE;
+       RTMP_IO_READ32(pAd, EFUSE_CTRL, &eFuseCtrl);
+       pAd->bUseEfuse = ( (eFuseCtrl & 0x80000000) == 0x80000000) ? 1 : 0;
+       if(pAd->bUseEfuse)
+       {
+                       DBGPRINT(RT_DEBUG_TRACE, ("NVM is Efuse\n"));
+       }
+       else
+       {
+                       DBGPRINT(RT_DEBUG_TRACE, ("NVM is EEPROM\n"));
+       }
+#endif
 
-#ifdef CONFIG_STA_SUPPORT
-       IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
        {
                // for rt2860E and after, init TXOP_CTRL_CFG with 0x583f. This is for extension channel overlapping IOT.
                if ((pAd->MACVersion&0xffff) != 0x0101)
                        RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x583f);
        }
-#endif // CONFIG_STA_SUPPORT //
 
        DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitializeAsic\n"));
        return NDIS_STATUS_SUCCESS;
 }
 
 
+#ifdef RT2860
 VOID NICRestoreBBPValue(
        IN PRTMP_ADAPTER pAd)
 {
        UCHAR           index;
-       UCHAR           Value;
+       UCHAR           Value = 0;
        ULONG           Data;
 
        DBGPRINT(RT_DEBUG_TRACE, ("--->  NICRestoreBBPValue !!!!!!!!!!!!!!!!!!!!!!!  \n"));
@@ -2165,6 +2648,7 @@ VOID NICRestoreBBPValue(
 
        DBGPRINT(RT_DEBUG_TRACE, ("<---  NICRestoreBBPValue !!!!!!!!!!!!!!!!!!!!!!!  \n"));
 }
+#endif /* RT2860 */
 
 /*
        ========================================================================
@@ -2235,12 +2719,6 @@ VOID NICUpdateFifoStaCounters(
        CHAR                            reTry;
        UCHAR                           succMCS;
 
-#ifdef RALINK_ATE
-       /* Nothing to do in ATE mode */
-       if (ATE_ON(pAd))
-               return;
-#endif // RALINK_ATE //
-
                do
                {
                        RTMP_IO_READ32(pAd, TX_STA_FIFO, &StaFifo.word);
@@ -2265,10 +2743,8 @@ VOID NICUpdateFifoStaCounters(
 
                        pEntry->DebugFIFOCount++;
 
-#ifdef DOT11_N_SUPPORT
                        if (StaFifo.field.TxBF) // 3*3
                                pEntry->TxBFCount++;
-#endif // DOT11_N_SUPPORT //
 
 #ifdef UAPSD_AP_SUPPORT
                        UAPSD_SP_AUE_Handle(pAd, pEntry, StaFifo.field.TxSuccess);
@@ -2282,23 +2758,15 @@ VOID NICUpdateFifoStaCounters(
                                if (pEntry->FIFOCount >= 1)
                                {
                                        DBGPRINT(RT_DEBUG_TRACE, ("#"));
-#if 0
-                                       SendRefreshBAR(pAd, pEntry);
                                        pEntry->NoBADataCountDown = 64;
-#else
-#ifdef DOT11_N_SUPPORT
-                                       pEntry->NoBADataCountDown = 64;
-#endif // DOT11_N_SUPPORT //
 
                                        if(pEntry->PsMode == PWR_ACTIVE)
                                        {
-#ifdef DOT11_N_SUPPORT
                                                int tid;
                                                for (tid=0; tid<NUM_OF_TID; tid++)
                                                {
                                                        BAOriSessionTearDown(pAd, pEntry->Aid,  tid, FALSE, FALSE);
                                                }
-#endif // DOT11_N_SUPPORT //
 
                                                // Update the continuous transmission counter except PS mode
                                                pEntry->ContinueTxFailCnt++;
@@ -2310,14 +2778,10 @@ VOID NICUpdateFifoStaCounters(
                                                pEntry->FIFOCount = 0;
                                                pEntry->ContinueTxFailCnt = 0;
                                        }
-#endif
-                                       //pEntry->FIFOCount = 0;
                                }
-                               //pEntry->bSendBAR = TRUE;
                        }
                        else
                        {
-#ifdef DOT11_N_SUPPORT
                                if ((pEntry->PsMode != PWR_SAVE) && (pEntry->NoBADataCountDown > 0))
                                {
                                        pEntry->NoBADataCountDown--;
@@ -2326,7 +2790,7 @@ VOID NICUpdateFifoStaCounters(
                                                DBGPRINT(RT_DEBUG_TRACE, ("@\n"));
                                        }
                                }
-#endif // DOT11_N_SUPPORT //
+
                                pEntry->FIFOCount = 0;
                                pEntry->OneSecTxNoRetryOkCount++;
                                // update NoDataIdleCount when sucessful send packet to STA.
@@ -2437,6 +2901,22 @@ VOID NICUpdateRawCounters(
        // Update RX Overflow counter
        pAd->Counters8023.RxNoBuffer += (RxStaCnt2.field.RxFifoOverflowCount);
 
+#ifdef RT2870
+       if (pAd->RalinkCounters.RxCount != pAd->watchDogRxCnt)
+       {
+               pAd->watchDogRxCnt = pAd->RalinkCounters.RxCount;
+               pAd->watchDogRxOverFlowCnt = 0;
+       }
+       else
+       {
+               if (RxStaCnt2.field.RxFifoOverflowCount)
+                       pAd->watchDogRxOverFlowCnt++;
+               else
+                       pAd->watchDogRxOverFlowCnt = 0;
+       }
+#endif // RT2870 //
+
+
        if (!pAd->bUpdateBcnCntDone)
        {
        // Update BEACON sent count
@@ -2511,73 +2991,6 @@ VOID NICUpdateRawCounters(
                pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt7.field.AggSize16Count / 16);
        }
 
-#ifdef DBG_DIAGNOSE
-       {
-               RtmpDiagStruct  *pDiag;
-               COUNTER_RALINK  *pRalinkCounters;
-               UCHAR                   ArrayCurIdx, i;
-
-               pDiag = &pAd->DiagStruct;
-               pRalinkCounters = &pAd->RalinkCounters;
-               ArrayCurIdx = pDiag->ArrayCurIdx;
-
-               if (pDiag->inited == 0)
-               {
-                       NdisZeroMemory(pDiag, sizeof(struct _RtmpDiagStrcut_));
-                       pDiag->ArrayStartIdx = pDiag->ArrayCurIdx = 0;
-                       pDiag->inited = 1;
-               }
-               else
-               {
-                       // Tx
-                       pDiag->TxFailCnt[ArrayCurIdx] = TxStaCnt0.field.TxFailCount;
-                       pDiag->TxAggCnt[ArrayCurIdx] = TxAggCnt.field.AggTxCount;
-                       pDiag->TxNonAggCnt[ArrayCurIdx] = TxAggCnt.field.NonAggTxCount;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][0] = TxAggCnt0.field.AggSize1Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][1] = TxAggCnt0.field.AggSize2Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][2] = TxAggCnt1.field.AggSize3Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][3] = TxAggCnt1.field.AggSize4Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][4] = TxAggCnt2.field.AggSize5Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][5] = TxAggCnt2.field.AggSize6Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][6] = TxAggCnt3.field.AggSize7Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][7] = TxAggCnt3.field.AggSize8Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][8] = TxAggCnt4.field.AggSize9Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][9] = TxAggCnt4.field.AggSize10Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][10] = TxAggCnt5.field.AggSize11Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][11] = TxAggCnt5.field.AggSize12Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][12] = TxAggCnt6.field.AggSize13Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][13] = TxAggCnt6.field.AggSize14Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][14] = TxAggCnt7.field.AggSize15Count;
-                       pDiag->TxAMPDUCnt[ArrayCurIdx][15] = TxAggCnt7.field.AggSize16Count;
-
-                       pDiag->RxCrcErrCnt[ArrayCurIdx] = RxStaCnt0.field.CrcErr;
-
-                       INC_RING_INDEX(pDiag->ArrayCurIdx,  DIAGNOSE_TIME);
-                       ArrayCurIdx = pDiag->ArrayCurIdx;
-                       for (i =0; i < 9; i++)
-                       {
-                               pDiag->TxDescCnt[ArrayCurIdx][i]= 0;
-                               pDiag->TxSWQueCnt[ArrayCurIdx][i] =0;
-                               pDiag->TxMcsCnt[ArrayCurIdx][i] = 0;
-                               pDiag->RxMcsCnt[ArrayCurIdx][i] = 0;
-                       }
-                       pDiag->TxDataCnt[ArrayCurIdx] = 0;
-                       pDiag->TxFailCnt[ArrayCurIdx] = 0;
-                       pDiag->RxDataCnt[ArrayCurIdx] = 0;
-                       pDiag->RxCrcErrCnt[ArrayCurIdx]  = 0;
-                       for (i = 9; i < 24; i++) // 3*3
-                       {
-                               pDiag->TxDescCnt[ArrayCurIdx][i] = 0;
-                               pDiag->TxMcsCnt[ArrayCurIdx][i] = 0;
-                               pDiag->RxMcsCnt[ArrayCurIdx][i] = 0;
-}
-
-                       if (pDiag->ArrayCurIdx == pDiag->ArrayStartIdx)
-                               INC_RING_INDEX(pDiag->ArrayStartIdx,  DIAGNOSE_TIME);
-               }
-
-       }
-#endif // DBG_DIAGNOSE //
 
 
 }
@@ -2664,217 +3077,46 @@ VOID NICEraseFirmware(
 NDIS_STATUS NICLoadFirmware(
        IN PRTMP_ADAPTER pAd)
 {
-#ifdef BIN_IN_FILE
-#define NICLF_DEFAULT_USE()    \
-       flg_default_firm_use = TRUE; \
-       printk("%s - Use default firmware!\n", __func__);
-
-       NDIS_STATUS             Status = NDIS_STATUS_SUCCESS;
-       PUCHAR                  src;
-       struct file             *srcf;
-       INT                     retval, orgfsuid, orgfsgid, i;
-       mm_segment_t    orgfs;
-       PUCHAR                  pFirmwareImage;
-       UINT                    FileLength = 0;
-       UINT32                  MacReg;
-       ULONG                   Index;
-       ULONG                   firm;
-       BOOLEAN                 flg_default_firm_use = FALSE;
-
-
-       DBGPRINT(RT_DEBUG_TRACE, ("===> %s\n", __func__));
-
-       /* init */
-       pFirmwareImage = NULL;
-       src = RTMP_FIRMWARE_FILE_NAME;
-
-       /* save uid and gid used for filesystem access.
-          set user and group to 0 (root) */
-       orgfsuid = current->fsuid;
-       orgfsgid = current->fsgid;
-       current->fsuid = current->fsgid = 0;
-    orgfs = get_fs();
-    set_fs(KERNEL_DS);
-
-       pAd->FirmwareVersion = (FIRMWARE_MAJOR_VERSION << 8) + \
-                                                  FIRMWARE_MINOR_VERSION;
-
-
-       /* allocate firmware buffer */
-    pFirmwareImage = kmalloc(MAX_FIRMWARE_IMAGE_SIZE, MEM_ALLOC_FLAG);
-    if (pFirmwareImage == NULL)
-       {
-               /* allocate fail, use default firmware array in firmware.h */
-               printk("%s - Allocate memory fail!\n", __func__);
-               NICLF_DEFAULT_USE();
-    }
-       else
-       {
-               /* allocate ok! zero the firmware buffer */
-               memset(pFirmwareImage, 0x00, MAX_FIRMWARE_IMAGE_SIZE);
-       } /* End of if */
-
-
-       /* if ok, read firmware file from *.bin file */
-       if (flg_default_firm_use == FALSE)
-       {
-               do
-               {
-                       /* open the bin file */
-                       srcf = filp_open(src, O_RDONLY, 0);
-
-                       if (IS_ERR(srcf))
-                       {
-                               printk("%s - Error %ld opening %s\n",
-                                          __func__, -PTR_ERR(srcf), src);
-                               NICLF_DEFAULT_USE();
-                               break;
-                       } /* End of if */
-
-                       /* the object must have a read method */
-                       if ((srcf->f_op == NULL) || (srcf->f_op->read == NULL))
-                       {
-                               printk("%s - %s does not have a write method\n", __func__, src);
-                               NICLF_DEFAULT_USE();
-                               break;
-                       } /* End of if */
-
-                       /* read the firmware from the file *.bin */
-                       FileLength = srcf->f_op->read(srcf,
-                                                                                 pFirmwareImage,
-                                                                                 MAX_FIRMWARE_IMAGE_SIZE,
-                                                                                 &srcf->f_pos);
-
-                       if (FileLength != MAX_FIRMWARE_IMAGE_SIZE)
-                       {
-                               printk("%s: error file length (=%d) in RT2860AP.BIN\n",
-                                          __func__, FileLength);
-                               NICLF_DEFAULT_USE();
-                               break;
-                       }
-                       else
-                       {
-                               PUCHAR ptr = pFirmwareImage;
-                               USHORT crc = 0xffff;
-
-
-                               /* calculate firmware CRC */
-                               for(i=0; i<(MAX_FIRMWARE_IMAGE_SIZE-2); i++, ptr++)
-                                       crc = ByteCRC16(bitrev8(*ptr), crc);
-                               /* End of for */
-
-                               if ((pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-2] != \
-                                                               (UCHAR)bitrev8((UCHAR)(crc>>8))) ||
-                                       (pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-1] != \
-                                                               (UCHAR)bitrev8((UCHAR)crc)))
-                               {
-                                       /* CRC fail */
-                                       printk("%s: CRC = 0x%02x 0x%02x "
-                                                  "error, should be 0x%02x 0x%02x\n",
-                                                  __func__,
-                                                  pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-2],
-                                                  pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-1],
-                                                  (UCHAR)(crc>>8), (UCHAR)(crc));
-                                       NICLF_DEFAULT_USE();
-                                       break;
-                               }
-                               else
-                               {
-                                       /* firmware is ok */
-                                       pAd->FirmwareVersion = \
-                                               (pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-4] << 8) +
-                                               pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-3];
-
-                                       /* check if firmware version of the file is too old */
-                                       if ((pAd->FirmwareVersion) < \
-                                                                                       ((FIRMWARE_MAJOR_VERSION << 8) +
-                                                                                        FIRMWARE_MINOR_VERSION))
-                                       {
-                                               printk("%s: firmware version too old!\n", __func__);
-                                               NICLF_DEFAULT_USE();
-                                               break;
-                                       } /* End of if */
-                               } /* End of if */
-
-                               DBGPRINT(RT_DEBUG_TRACE,
-                                                ("NICLoadFirmware: CRC ok, ver=%d.%d\n",
-                                                 pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-4],
-                                                 pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-3]));
-                       } /* End of if (FileLength == MAX_FIRMWARE_IMAGE_SIZE) */
-                       break;
-               } while(TRUE);
-
-               /* close firmware file */
-               if (IS_ERR(srcf))
-                       ;
-               else
-               {
-                       retval = filp_close(srcf, NULL);
-                       if (retval)
-                       {
-                               DBGPRINT(RT_DEBUG_ERROR,
-                                                ("--> Error %d closing %s\n", -retval, src));
-                       } /* End of if */
-               } /* End of if */
-       } /* End of if */
-
-
-       /* write firmware to ASIC */
-       if (flg_default_firm_use == TRUE)
-       {
-               /* use default fimeware, free allocated buffer */
-               if (pFirmwareImage != NULL)
-                       kfree(pFirmwareImage);
-               /* End of if */
-
-               /* use default *.bin array */
-               pFirmwareImage = FirmwareImage;
-               FileLength = sizeof(FirmwareImage);
-       } /* End of if */
-
-       /* enable Host program ram write selection */
-       RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x10000);
-
-       for(i=0; i<FileLength; i+=4)
-       {
-               firm = pFirmwareImage[i] +
-                          (pFirmwareImage[i+3] << 24) +
-                          (pFirmwareImage[i+2] << 16) +
-                          (pFirmwareImage[i+1] << 8);
-
-               RTMP_IO_WRITE32(pAd, FIRMWARE_IMAGE_BASE + i, firm);
-       } /* End of for */
-
-       RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x00000);
-       RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x00001);
-
-       /* initialize BBP R/W access agent */
-       RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, 0);
-       RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, 0);
-
-       if (flg_default_firm_use == FALSE)
-       {
-               /* use file firmware, free allocated buffer */
-               if (pFirmwareImage != NULL)
-                       kfree(pFirmwareImage);
-               /* End of if */
-       } /* End of if */
-
-       set_fs(orgfs);
-       current->fsuid = orgfsuid;
-       current->fsgid = orgfsgid;
-#else
-
        NDIS_STATUS             Status = NDIS_STATUS_SUCCESS;
        PUCHAR                  pFirmwareImage;
        ULONG                   FileLength, Index;
        //ULONG                 firm;
        UINT32                  MacReg = 0;
+#ifdef RT2870
+       UINT32                  Version = (pAd->MACVersion >> 16);
+#endif // RT2870 //
 
        pFirmwareImage = FirmwareImage;
        FileLength = sizeof(FirmwareImage);
+#ifdef RT2870
+       // New 8k byte firmware size for RT3071/RT3072
+       //printk("Usb Chip\n");
+       if (FIRMWAREIMAGE_LENGTH == FIRMWAREIMAGE_MAX_LENGTH)
+       //The firmware image consists of two parts. One is the origianl and the other is the new.
+       //Use Second Part
+       {
+               if ((Version != 0x2860) && (Version != 0x2872) && (Version != 0x3070))
+               {       // Use Firmware V2.
+                       //printk("KH:Use New Version,part2\n");
+                       pFirmwareImage = (PUCHAR)&FirmwareImage[FIRMWAREIMAGEV1_LENGTH];
+                       FileLength = FIRMWAREIMAGEV2_LENGTH;
+               }
+               else
+               {
+                       //printk("KH:Use New Version,part1\n");
+                       pFirmwareImage = FirmwareImage;
+                       FileLength = FIRMWAREIMAGEV1_LENGTH;
+               }
+       }
+       else
+       {
+               DBGPRINT(RT_DEBUG_ERROR, ("KH: bin file should be 8KB.\n"));
+               Status = NDIS_STATUS_FAILURE;
+       }
+
+#endif // RT2870 //
+
        RT28XX_WRITE_FIRMWARE(pAd, pFirmwareImage, FileLength);
-#endif
 
        /* check if MCU is ready */
        Index = 0;
@@ -2888,7 +3130,7 @@ NDIS_STATUS NICLoadFirmware(
                RTMPusecDelay(1000);
        } while (Index++ < 1000);
 
-    if (Index >= 1000)
+    if (Index > 1000)
        {
                Status = NDIS_STATUS_FAILURE;
                DBGPRINT(RT_DEBUG_ERROR, ("NICLoadFirmware: MCU is not ready\n\n\n"));
@@ -3139,6 +3381,31 @@ VOID     UserCfgInit(
        //
        //  part I. intialize common configuration
        //
+#ifdef RT2870
+       pAd->BulkOutReq = 0;
+
+       pAd->BulkOutComplete = 0;
+       pAd->BulkOutCompleteOther = 0;
+       pAd->BulkOutCompleteCancel = 0;
+       pAd->BulkInReq = 0;
+       pAd->BulkInComplete = 0;
+       pAd->BulkInCompleteFail = 0;
+
+       //pAd->QuickTimerP = 100;
+       //pAd->TurnAggrBulkInCount = 0;
+       pAd->bUsbTxBulkAggre = 0;
+
+       // init as unsed value to ensure driver will set to MCU once.
+       pAd->LedIndicatorStregth = 0xFF;
+
+       pAd->CommonCfg.MaxPktOneTxBulk = 2;
+       pAd->CommonCfg.TxBulkFactor = 1;
+       pAd->CommonCfg.RxBulkFactor =1;
+
+       pAd->CommonCfg.TxPower = 100; //mW
+
+       NdisZeroMemory(&pAd->CommonCfg.IOTestParm, sizeof(pAd->CommonCfg.IOTestParm));
+#endif // RT2870 //
 
        for(key_index=0; key_index<SHARE_KEY_NUM; key_index++)
        {
@@ -3149,6 +3416,9 @@ VOID      UserCfgInit(
                }
        }
 
+#ifdef RT2870
+       pAd->EepromAccess = FALSE;
+#endif
        pAd->Antenna.word = 0;
        pAd->CommonCfg.BBPCurrentBW = BW_20;
 
@@ -3158,7 +3428,7 @@ VOID      UserCfgInit(
        pAd->RLnkCtrlOffset = 0;
        pAd->HostLnkCtrlOffset = 0;
        pAd->CheckDmaBusyCount = 0;
-#endif // RT2860 //
+#endif
 
        pAd->bAutoTxAgcA = FALSE;                       // Default is OFF
        pAd->bAutoTxAgcG = FALSE;                       // Default is OFF
@@ -3199,23 +3469,11 @@ VOID    UserCfgInit(
 
        NdisZeroMemory(&pAd->BeaconTxWI, sizeof(pAd->BeaconTxWI));
 
-#ifdef DOT11_N_SUPPORT
        NdisZeroMemory(&pAd->CommonCfg.HtCapability, sizeof(pAd->CommonCfg.HtCapability));
        pAd->HTCEnable = FALSE;
        pAd->bBroadComHT = FALSE;
        pAd->CommonCfg.bRdg = FALSE;
 
-#ifdef DOT11N_DRAFT3
-       pAd->CommonCfg.Dot11OBssScanPassiveDwell = dot11OBSSScanPassiveDwell;   // Unit : TU. 5~1000
-       pAd->CommonCfg.Dot11OBssScanActiveDwell = dot11OBSSScanActiveDwell;     // Unit : TU. 10~1000
-       pAd->CommonCfg.Dot11BssWidthTriggerScanInt = dot11BSSWidthTriggerScanInterval;  // Unit : Second
-       pAd->CommonCfg.Dot11OBssScanPassiveTotalPerChannel = dot11OBSSScanPassiveTotalPerChannel;       // Unit : TU. 200~10000
-       pAd->CommonCfg.Dot11OBssScanActiveTotalPerChannel = dot11OBSSScanActiveTotalPerChannel; // Unit : TU. 20~10000
-       pAd->CommonCfg.Dot11BssWidthChanTranDelayFactor = dot11BSSWidthChannelTransactionDelayFactor;
-       pAd->CommonCfg.Dot11OBssScanActivityThre = dot11BSSScanActivityThreshold;       // Unit : percentage
-       pAd->CommonCfg.Dot11BssWidthChanTranDelay = (pAd->CommonCfg.Dot11BssWidthTriggerScanInt * pAd->CommonCfg.Dot11BssWidthChanTranDelayFactor);
-#endif  // DOT11N_DRAFT3 //
-
        NdisZeroMemory(&pAd->CommonCfg.AddHTInfo, sizeof(pAd->CommonCfg.AddHTInfo));
        pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
        pAd->CommonCfg.BACapability.field.MpduDensity = 0;
@@ -3236,7 +3494,6 @@ VOID      UserCfgInit(
        pAd->CommonCfg.TxBASize = 7;
 
        pAd->CommonCfg.REGBACapability.word = pAd->CommonCfg.BACapability.word;
-#endif // DOT11_N_SUPPORT //
 
        //pAd->CommonCfg.HTPhyMode.field.BW = BW_20;
        //pAd->CommonCfg.HTPhyMode.field.MCS = MCS_AUTO;
@@ -3253,8 +3510,6 @@ VOID      UserCfgInit(
        //
        // part II. intialize STA specific configuration
        //
-#ifdef CONFIG_STA_SUPPORT
-       IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
        {
                RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_DIRECT);
                RX_FILTER_CLEAR_FLAG(pAd, fRX_FILTER_ACCEPT_MULTICAST);
@@ -3293,11 +3548,6 @@ VOID     UserCfgInit(
                pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
        }
 
-#ifdef EXT_BUILD_CHANNEL_LIST
-       pAd->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
-#endif // EXT_BUILD_CHANNEL_LIST //
-#endif // CONFIG_STA_SUPPORT //
-
        // global variables mXXXX used in MAC protocol state machines
        OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
        OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
@@ -3307,21 +3557,12 @@ VOID    UserCfgInit(
        pAd->CommonCfg.PhyMode = PHY_11BG_MIXED;                // default PHY mode
        OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);  // CCK use LONG preamble
 
-#ifdef CONFIG_STA_SUPPORT
-       IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
        {
                // user desired power mode
                pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
                pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
                pAd->StaCfg.bWindowsACCAMEnable = FALSE;
 
-#ifdef LEAP_SUPPORT
-               // CCX v1.0 releated init value
-               RTMPInitTimer(pAd, &pAd->StaCfg.LeapAuthTimer, GET_TIMER_FUNCTION(LeapAuthTimeout), pAd, FALSE);
-               pAd->StaCfg.LeapAuthMode = CISCO_AuthModeLEAPNone;
-               pAd->StaCfg.bCkipOn = FALSE;
-#endif // LEAP_SUPPORT //
-
                RTMPInitTimer(pAd, &pAd->StaCfg.StaQuickResponeForRateUpTimer, GET_TIMER_FUNCTION(StaQuickResponeForRateUpExec), pAd, FALSE);
                pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = FALSE;
 
@@ -3349,17 +3590,11 @@ VOID    UserCfgInit(
                NdisZeroMemory(pAd->nickname, IW_ESSID_MAX_SIZE+1);
                sprintf(pAd->nickname, "%s", STA_NIC_DEVICE_NAME);
                RTMPInitTimer(pAd, &pAd->StaCfg.WpaDisassocAndBlockAssocTimer, GET_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc), pAd, FALSE);
-#ifdef WPA_SUPPLICANT_SUPPORT
                pAd->StaCfg.IEEE8021X = FALSE;
                pAd->StaCfg.IEEE8021x_required_keys = FALSE;
                pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
-#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
                pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
-#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
-#endif // WPA_SUPPLICANT_SUPPORT //
-
        }
-#endif // CONFIG_STA_SUPPORT //
 
        // Default for extra information is not valid
        pAd->ExtraInfo = EXTRA_INFO_CLEAR;
@@ -3396,45 +3631,12 @@ VOID    UserCfgInit(
        InitializeQueueHeader(&pAd->MacTab.McastPsQueue);
        NdisAllocateSpinLock(&pAd->MacTabLock);
 
-#ifdef RALINK_ATE
-       NdisZeroMemory(&pAd->ate, sizeof(ATE_INFO));
-       pAd->ate.Mode = ATE_STOP;
-       pAd->ate.TxCount = 200;/* to exceed TX_RING_SIZE ... */
-       pAd->ate.TxLength = 1024;
-       pAd->ate.TxWI.ShortGI = 0;// LONG GI : 800 ns
-       pAd->ate.TxWI.PHYMODE = MODE_CCK;
-       pAd->ate.TxWI.MCS = 3;
-       pAd->ate.TxWI.BW = BW_20;
-       pAd->ate.Channel = 1;
-       pAd->ate.QID = QID_AC_BE;
-       pAd->ate.Addr1[0] = 0x00;
-       pAd->ate.Addr1[1] = 0x11;
-       pAd->ate.Addr1[2] = 0x22;
-       pAd->ate.Addr1[3] = 0xAA;
-       pAd->ate.Addr1[4] = 0xBB;
-       pAd->ate.Addr1[5] = 0xCC;
-       NdisMoveMemory(pAd->ate.Addr2, pAd->ate.Addr1, ETH_LENGTH_OF_ADDRESS);
-       NdisMoveMemory(pAd->ate.Addr3, pAd->ate.Addr1, ETH_LENGTH_OF_ADDRESS);
-       pAd->ate.bRxFer = 0;
-       pAd->ate.bQATxStart = FALSE;
-       pAd->ate.bQARxStart = FALSE;
-#ifdef RT2860
-       pAd->ate.bFWLoading = FALSE;
-#endif // RT2860 //
-#ifdef RALINK_28xx_QA
-       //pAd->ate.Repeat = 0;
-       pAd->ate.TxStatus = 0;
-       pAd->ate.AtePid = THREAD_PID_INIT_VALUE;
-#endif // RALINK_28xx_QA //
-#endif // RALINK_ATE //
-
-
        pAd->CommonCfg.bWiFiTest = FALSE;
 #ifdef RT2860
-    pAd->bPCIclkOff = FALSE;
-#endif // RT2860 //
+       pAd->bPCIclkOff = FALSE;
 
        RTMP_SET_PSFLAG(pAd, fRTMP_PS_CAN_GO_SLEEP);
+#endif
        DBGPRINT(RT_DEBUG_TRACE, ("<-- UserCfgInit\n"));
 }
 
@@ -3536,6 +3738,9 @@ VOID      RTMPInitTimer(
        pTimer->State      = FALSE;
        pTimer->cookie = (ULONG) pData;
 
+#ifdef RT2870
+       pTimer->pAd = pAd;
+#endif // RT2870 //
 
        RTMP_OS_Init_Timer(pAd, &pTimer->TimerObj,      pTimerFunc, (PVOID) pTimer);
 }
@@ -3662,6 +3867,12 @@ VOID     RTMPCancelTimer(
                if (*pCancelled == TRUE)
                        pTimer->State = TRUE;
 
+#ifdef RT2870
+               // We need to go-through the TimerQ to findout this timer handler and remove it if
+               //              it's still waiting for execution.
+
+               RT2870_TimerQ_Remove(pTimer->pAd, pTimer);
+#endif // RT2870 //
        }
        else
        {
@@ -3701,13 +3912,6 @@ VOID RTMPSetLED(
        UCHAR                   HighByte = 0;
        UCHAR                   LowByte;
 
-// In ATE mode of RT2860 AP/STA, we have erased 8051 firmware.
-// So LED mode is not supported when ATE is running.
-#ifdef RALINK_ATE
-       if (ATE_ON(pAd))
-               return;
-#endif // RALINK_ATE //
-
        LowByte = pAd->LedCntl.field.LedMode&0x7f;
        switch (Status)
        {