Staging: rt2860: Ported v1.7.1.1 changes into v1.8.0.0, becoming v1.8.1.1

Staging: rt2860: Ported v1.7.1.1 changes into v1.8.0.0, becoming v1.8.1.1

When RaLink released rt2860 v1.7.0.0, it lacked proper support for both WEP
and WPA/WPA2 encryption. Either was possible, but the module had to be
compiled to support only one or the other, never both.

Since the EeePC was the most common device with this hardware (and these
users were complaining to RaLink that WPA/WPA2 encryption didn't work)
RaLink released a fix as an "eeepc-specific" version of this driver, v1.7.1.1

Unfortunately, when v1.8.0.0 was released, this WPA/WPA2 fix was never
included.

What complicates things further is that RaLink has no interest in
continuing work on this Linux driver for their hardware.

This commit ports the changes introduced in v1.7.1.1 into the v1.8.0.0
release, upgrading the kernel's module to v1.8.1.1

Signed-off-by: Adam McDaniel <adam@array.org>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>

6 files changed, 492 insertions, 64 deletions

diff --git a/drivers/staging/rt2860/common/cmm_data.c b/drivers/staging/rt2860/common/cmm_data.c
index b67b9eba722..14a99b3e321 100644
--- a/drivers/staging/rt2860/common/cmm_data.c
+++ b/drivers/staging/rt2860/common/cmm_data.c
@@ -366,7 +366,7 @@ NDIS_STATUS MlmeHardTransmitTxRing(
 	{
 		// outgoing frame always wakeup PHY to prevent frame lost
 		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
-			AsicForceWakeup(pAd, TRUE);
+			AsicForceWakeup(pAd, FROM_TX);
 	}
 #endif // CONFIG_STA_SUPPORT //
 	pFirstTxWI	=(PTXWI_STRUC)pSrcBufVA;
@@ -541,7 +541,7 @@ NDIS_STATUS MlmeHardTransmitMgmtRing(
 	{
 		// outgoing frame always wakeup PHY to prevent frame lost
 		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
-			AsicForceWakeup(pAd, TRUE);
+			AsicForceWakeup(pAd, FROM_TX);
 	}
 #endif // CONFIG_STA_SUPPORT //
 
diff --git a/drivers/staging/rt2860/common/cmm_data_2860.c b/drivers/staging/rt2860/common/cmm_data_2860.c
index 419e50c3fc4..fae741e4bee 100644
--- a/drivers/staging/rt2860/common/cmm_data_2860.c
+++ b/drivers/staging/rt2860/common/cmm_data_2860.c
@@ -634,7 +634,7 @@ VOID RT28xxPciAsicRadioOff(
 	}
 
     // Once go into this function, disable tx because don't want too many packets in queue to prevent HW stops.
-	pAd->bPCIclkOffDisableTx = TRUE;
+	RTMP_SET_PSFLAG(pAd, fRTMP_PS_DISABLE_TX);
 
 	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
 	{
@@ -651,7 +651,7 @@ VOID RT28xxPciAsicRadioOff(
 			{
 				DBGPRINT(RT_DEBUG_TRACE, ("TbTTTime = 0x%x , give up this sleep. \n", TbTTTime));
 	            OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
-	            pAd->bPCIclkOffDisableTx = FALSE;
+				RTMP_CLEAR_PSFLAG(pAd, fRTMP_PS_DISABLE_TX);
 				return;
 			}
 			else
@@ -688,18 +688,25 @@ VOID RT28xxPciAsicRadioOff(
 	if (i >= 50)
 	{
 		DBGPRINT(RT_DEBUG_TRACE, ("DMA keeps busy.  return on RT28xxPciAsicRadioOff ()\n"));
-		pAd->bPCIclkOffDisableTx = FALSE;
 		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
 		DmaCfg.field.EnableTxDMA = 1;
 		RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, DmaCfg.word);
+		pAd->CheckDmaBusyCount++;
 		return;
 	}
+	else
+	{
+		pAd->CheckDmaBusyCount = 0;
+	}
 
     RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_IDLE_RADIO_OFF);
 
     // Set to 1R.
-    tempBBP_R3 = (pAd->StaCfg.BBPR3 & 0xE7);
-	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, tempBBP_R3);
+	if (pAd->Antenna.field.RxPath > 1)
+	{
+		tempBBP_R3 = (pAd->StaCfg.BBPR3 & 0xE7);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, tempBBP_R3);
+	}
 
 	// In Radio Off, we turn off RF clk, So now need to call ASICSwitchChannel again.
 	if (INFRA_ON(pAd) && (pAd->CommonCfg.CentralChannel != pAd->CommonCfg.Channel)
@@ -714,8 +721,15 @@ VOID RT28xxPciAsicRadioOff(
 		AsicTurnOffRFClk(pAd, pAd->CommonCfg.Channel);
 	}
 
-    // When PCI clock is off, don't want to service interrupt.
-	RTMP_IO_WRITE32(pAd, INT_MASK_CSR, AutoWakeupInt);
+	if (Level != RTMP_HALT)
+	{
+		// Change Interrupt bitmask.
+		RTMP_IO_WRITE32(pAd, INT_MASK_CSR, AutoWakeupInt);
+	}
+	else
+	{
+		NICDisableInterrupt(pAd);
+	}
 
     RTMP_IO_WRITE32(pAd, RX_CRX_IDX, pAd->RxRing.RxCpuIdx);
 	// Disable MAC Rx
@@ -726,7 +740,8 @@ VOID RT28xxPciAsicRadioOff(
 	//  2. Send Sleep command
 	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_STATUS, 0xffffffff);
 	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CID, 0xffffffff);
-	AsicSendCommandToMcu(pAd, 0x30, PowerSafeCID, 0xff, 0x00);   // send POWER-SAVE command to MCU. Timeout unit:40us.
+	// send POWER-SAVE command to MCU. high-byte = 1 save power as much as possible. high byte = 0 save less power
+	AsicSendCommandToMcu(pAd, 0x30, PowerSafeCID, 0xff, 0x1);
 	//  2-1. Wait command success
 	// Status = 1 : success, Status = 2, already sleep, Status = 3, Maybe MAC is busy so can't finish this task.
 	brc = AsicCheckCommanOk(pAd, PowerSafeCID);
@@ -734,7 +749,7 @@ VOID RT28xxPciAsicRadioOff(
     if (brc == FALSE)
     {
         // try again
-    	AsicSendCommandToMcu(pAd, 0x30, PowerSafeCID, 0xff, 0x00);   // send POWER-SAVE command to MCU. Timeout unit:40us.
+    	AsicSendCommandToMcu(pAd, 0x30, PowerSafeCID, 0xff, 0x01);   // send POWER-SAVE command to MCU. Timeout unit:40us.
     	//RTMPusecDelay(200);
     	brc = AsicCheckCommanOk(pAd, PowerSafeCID);
     }
@@ -759,7 +774,7 @@ VOID RT28xxPciAsicRadioOff(
 	do
 	{
 		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
-		if (DmaCfg.field.RxDMABusy == 0)
+		if ((DmaCfg.field.RxDMABusy == 0) && (DmaCfg.field.TxDMABusy == 0))
 			break;
 		RTMPusecDelay(20);
 		i++;
@@ -767,13 +782,12 @@ VOID RT28xxPciAsicRadioOff(
 
 	if (i >= 50)
 	{
+		pAd->CheckDmaBusyCount++;
 		DBGPRINT(RT_DEBUG_TRACE, ("DMA Rx keeps busy.  on RT28xxPciAsicRadioOff ()\n"));
 	}
-	// disable DMA Rx.
+	else
 	{
-		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
-		DmaCfg.field.EnableRxDMA = 0;
-		RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, DmaCfg.word);
+		pAd->CheckDmaBusyCount = 0;
 	}
 
 	if (Level == DOT11POWERSAVE)
@@ -799,7 +813,7 @@ VOID RT28xxPciAsicRadioOff(
 	if (Level == RTMP_HALT)
 	{
 		if ((brc == TRUE) && (i < 50))
-			RTMPPCIeLinkCtrlSetting(pAd, 1);
+			RTMPPCIeLinkCtrlSetting(pAd, 0);
 	}
 	//  4. Set PCI configuration Space Link Comtrol fields.  Only Radio Off needs to call this function
 	else
@@ -808,7 +822,7 @@ VOID RT28xxPciAsicRadioOff(
 			RTMPPCIeLinkCtrlSetting(pAd, 3);
 	}
 
-    pAd->bPCIclkOffDisableTx = FALSE;
+	RTMP_CLEAR_PSFLAG(pAd, fRTMP_PS_DISABLE_TX);
 }
 
 
@@ -835,7 +849,8 @@ BOOLEAN RT28xxPciAsicRadioOn(
 	{
 	    pAd->Mlme.bPsPollTimerRunning = FALSE;
 		RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
-		if ((Level == GUIRADIO_OFF) || (Level == GUI_IDLE_POWER_SAVE))
+		if ((Level == GUIRADIO_OFF) || (Level == GUI_IDLE_POWER_SAVE)
+		|| (RTMP_TEST_PSFLAG(pAd, fRTMP_PS_SET_PCI_CLK_OFF_COMMAND)))
 		{
 			DBGPRINT(RT_DEBUG_TRACE, ("RT28xxPciAsicRadioOn ()\n"));
 			// 1. Set PCI Link Control in Configuration Space.
@@ -845,15 +860,14 @@ BOOLEAN RT28xxPciAsicRadioOn(
 	}
 
     pAd->bPCIclkOff = FALSE;
-
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x3a80);
 	// 2. Send wake up command.
-	AsicSendCommandToMcu(pAd, 0x31, PowerWakeCID, 0x00, 0x00);
+	AsicSendCommandToMcu(pAd, 0x31, PowerWakeCID, 0x00, 0x02);
 
 	// 2-1. wait command ok.
 	brv = AsicCheckCommanOk(pAd, PowerWakeCID);
     if (brv)
     {
-    	//RTMP_IO_WRITE32(pAd, INT_MASK_CSR, (DELAYINTMASK|RxINT));
     	NICEnableInterrupt(pAd);
 
     	// 3. Enable Tx DMA.
@@ -893,13 +907,10 @@ BOOLEAN RT28xxPciAsicRadioOn(
 
 VOID RT28xxPciStaAsicForceWakeup(
 	IN PRTMP_ADAPTER pAd,
-	IN BOOLEAN       bFromTx)
+	IN UCHAR    	 Level)
 {
     AUTO_WAKEUP_STRUC	AutoWakeupCfg;
 
-    if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
-        return;
-
     if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WAKEUP_NOW))
     {
         DBGPRINT(RT_DEBUG_TRACE, ("waking up now!\n"));
@@ -907,38 +918,48 @@ VOID RT28xxPciStaAsicForceWakeup(
     }
 
     OPSTATUS_SET_FLAG(pAd, fOP_STATUS_WAKEUP_NOW);
+	RTMP_CLEAR_PSFLAG(pAd, fRTMP_PS_GO_TO_SLEEP_NOW);
 
     if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
     {
         // Support PCIe Advance Power Save
-    	if (bFromTx == TRUE)
+    	if (((Level == FROM_TX) && (pAd->Mlme.bPsPollTimerRunning == TRUE)) ||
+			(Level == RTMP_HALT))
     	{
             pAd->Mlme.bPsPollTimerRunning = FALSE;
     		RTMPPCIeLinkCtrlValueRestore(pAd, RESTORE_WAKEUP);
-    		RTMPusecDelay(3000);
+    		RTMPusecDelay(5000);
             DBGPRINT(RT_DEBUG_TRACE, ("=======AsicForceWakeup===bFromTx\n"));
     	}
 
 		AutoWakeupCfg.word = 0;
 		RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
 
-        if (RT28xxPciAsicRadioOn(pAd, DOT11POWERSAVE))
-        {
-            // In Radio Off, we turn off RF clk, So now need to call ASICSwitchChannel again.
-        	if (INFRA_ON(pAd) && (pAd->CommonCfg.CentralChannel != pAd->CommonCfg.Channel)
-        		&& (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
-        	{
-        		// Must using 40MHz.
-        		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
-        		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
-        	}
-        	else
-        	{
-        		// Must using 20MHz.
-        		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
-        		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
-        	}
-        }
+		// If this is called from Halt. ALWAYS force wakeup!!!
+		if (Level == RTMP_HALT)
+		{
+			RT28xxPciAsicRadioOn(pAd, RTMP_HALT);
+		}
+		else
+		{
+			if (RT28xxPciAsicRadioOn(pAd, DOT11POWERSAVE))
+			{
+				// In Radio Off, we turn off RF clk, So now need to call ASICSwitchChannel again.
+				if (INFRA_ON(pAd) && (pAd->CommonCfg.CentralChannel != pAd->CommonCfg.Channel)
+					&& (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+				{
+					// Must using 40MHz.
+					AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+				}
+				else
+				{
+					// Must using 20MHz.
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+				}
+			}
+		}
     }
     else
     {
@@ -1122,6 +1143,7 @@ VOID RT28xxPciMlmeRadioOn(
     {
     	NICResetFromError(pAd);
 
+    	/*
     	RTMPRingCleanUp(pAd, QID_AC_BK);
     	RTMPRingCleanUp(pAd, QID_AC_BE);
     	RTMPRingCleanUp(pAd, QID_AC_VI);
@@ -1129,6 +1151,7 @@ VOID RT28xxPciMlmeRadioOn(
     	RTMPRingCleanUp(pAd, QID_HCCA);
     	RTMPRingCleanUp(pAd, QID_MGMT);
     	RTMPRingCleanUp(pAd, QID_RX);
+		*/
 
     	// Enable Tx/Rx
     	RTMPEnableRxTx(pAd);
@@ -1162,6 +1185,12 @@ VOID RT28xxPciMlmeRadioOFF(
     WPDMA_GLO_CFG_STRUC	GloCfg;
 	UINT32	i;
 
+	if (pAd->StaCfg.bRadio == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("-->MlmeRadioOff() return on bRadio == TRUE; \n"));
+		return;
+	}
+
     if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
     	return;
 
@@ -1169,13 +1198,12 @@ VOID RT28xxPciMlmeRadioOFF(
 
 	// Set LED
 	RTMPSetLED(pAd, LED_RADIO_OFF);
-	// Set Radio off flag
-	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
 
 #ifdef CONFIG_STA_SUPPORT
 	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
     {
     	BOOLEAN		Cancelled;
+
     	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
     	{
 			RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &Cancelled);
@@ -1185,6 +1213,15 @@ VOID RT28xxPciMlmeRadioOFF(
 		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
         {
             BOOLEAN Cancelled;
+
+			// Always radio on since the NIC needs to set the MCU command (LED_RADIO_OFF).

+			if ((pAd->OpMode == OPMODE_STA) && 

+			     (IDLE_ON(pAd)) && 

+			     (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_IDLE_RADIO_OFF)))

+			{

+				RT28xxPciAsicRadioOn(pAd, GUI_IDLE_POWER_SAVE);

+			}
+
             pAd->Mlme.bPsPollTimerRunning = FALSE;
             RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
 	        RTMPCancelTimer(&pAd->Mlme.RadioOnOffTimer,	&Cancelled);
@@ -1197,9 +1234,26 @@ VOID RT28xxPciMlmeRadioOFF(
         //==========================================
         // Clean up old bss table
         BssTableInit(&pAd->ScanTab);
+
+		RTMPRingCleanUp(pAd, QID_AC_BK);
+    	RTMPRingCleanUp(pAd, QID_AC_BE);
+    	RTMPRingCleanUp(pAd, QID_AC_VI);
+    	RTMPRingCleanUp(pAd, QID_AC_VO);
+    	RTMPRingCleanUp(pAd, QID_HCCA);
+    	RTMPRingCleanUp(pAd, QID_MGMT);
+    	RTMPRingCleanUp(pAd, QID_RX);
+
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+		{
+			RTMPSetTimer(&pAd->Mlme.RadioOnOffTimer, 500);
+			return;
+		}
     }
 #endif // CONFIG_STA_SUPPORT //
 
+	// Set Radio off flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
 	// Disable Tx/Rx DMA
 	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);	   // disable DMA
 	GloCfg.field.EnableTxDMA = 0;
diff --git a/drivers/staging/rt2860/common/cmm_sync.c b/drivers/staging/rt2860/common/cmm_sync.c
index 40e4109118e..d29e0b630e2 100644
--- a/drivers/staging/rt2860/common/cmm_sync.c
+++ b/drivers/staging/rt2860/common/cmm_sync.c
@@ -470,7 +470,7 @@ VOID ScanNextChannel(
 		{
 		// BBP and RF are not accessible in PS mode, we has to wake them up first
 		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
-			AsicForceWakeup(pAd, TRUE);
+				AsicForceWakeup(pAd, FROM_TX);
 
 			// leave PSM during scanning. otherwise we may lost ProbeRsp & BEACON
 			if (pAd->StaCfg.Psm == PWR_SAVE)
diff --git a/drivers/staging/rt2860/common/cmm_wpa.c b/drivers/staging/rt2860/common/cmm_wpa.c
index 81c332ac252..69baf522fa0 100644
--- a/drivers/staging/rt2860/common/cmm_wpa.c
+++ b/drivers/staging/rt2860/common/cmm_wpa.c
@@ -39,8 +39,10 @@
 // WPA OUI
 UCHAR		OUI_WPA_NONE_AKM[4]		= {0x00, 0x50, 0xF2, 0x00};
 UCHAR       OUI_WPA_VERSION[4]      = {0x00, 0x50, 0xF2, 0x01};
+UCHAR       OUI_WPA_WEP40[4]      = {0x00, 0x50, 0xF2, 0x01};
 UCHAR       OUI_WPA_TKIP[4]     = {0x00, 0x50, 0xF2, 0x02};
 UCHAR       OUI_WPA_CCMP[4]     = {0x00, 0x50, 0xF2, 0x04};
+UCHAR       OUI_WPA_WEP104[4]      = {0x00, 0x50, 0xF2, 0x05};
 UCHAR       OUI_WPA_8021X_AKM[4]	= {0x00, 0x50, 0xF2, 0x01};
 UCHAR       OUI_WPA_PSK_AKM[4]      = {0x00, 0x50, 0xF2, 0x02};
 // WPA2 OUI
@@ -49,6 +51,7 @@ UCHAR       OUI_WPA2_TKIP[4]        = {0x00, 0x0F, 0xAC, 0x02};
 UCHAR       OUI_WPA2_CCMP[4]        = {0x00, 0x0F, 0xAC, 0x04};
 UCHAR       OUI_WPA2_8021X_AKM[4]   = {0x00, 0x0F, 0xAC, 0x01};
 UCHAR       OUI_WPA2_PSK_AKM[4]   	= {0x00, 0x0F, 0xAC, 0x02};
+UCHAR       OUI_WPA2_WEP104[4]   = {0x00, 0x0F, 0xAC, 0x05};
 // MSA OUI
 UCHAR   	OUI_MSA_8021X_AKM[4]    = {0x00, 0x0F, 0xAC, 0x05};		// Not yet final - IEEE 802.11s-D1.06
 UCHAR   	OUI_MSA_PSK_AKM[4]   	= {0x00, 0x0F, 0xAC, 0x06};		// Not yet final - IEEE 802.11s-D1.06
@@ -367,6 +370,24 @@ static VOID RTMPInsertRsnIeCipher(
                 break;
         }
 
+#ifdef CONFIG_STA_SUPPORT
+		if ((pAd->OpMode == OPMODE_STA) &&
+			(pAd->StaCfg.GroupCipher != Ndis802_11Encryption2Enabled) &&
+			(pAd->StaCfg.GroupCipher != Ndis802_11Encryption3Enabled))
+		{
+			UINT GroupCipher = pAd->StaCfg.GroupCipher;
+			switch(GroupCipher)
+			{
+				case Ndis802_11GroupWEP40Enabled:
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_WEP40, 4);
+					break;
+				case Ndis802_11GroupWEP104Enabled:
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_WEP104, 4);
+					break;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
 		// swap for big-endian platform
 		pRsnie_cipher->version = cpu2le16(pRsnie_cipher->version);
 	    pRsnie_cipher->ucount = cpu2le16(pRsnie_cipher->ucount);
@@ -427,11 +448,28 @@ static VOID RTMPInsertRsnIeCipher(
                 break;
         }
 
+#ifdef CONFIG_STA_SUPPORT
+		if ((pAd->OpMode == OPMODE_STA) &&
+			(pAd->StaCfg.GroupCipher != Ndis802_11Encryption2Enabled) &&
+			(pAd->StaCfg.GroupCipher != Ndis802_11Encryption3Enabled))
+		{
+			UINT GroupCipher = pAd->StaCfg.GroupCipher;
+			switch(GroupCipher)
+			{
+				case Ndis802_11GroupWEP40Enabled:
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_WEP40, 4);
+					break;
+				case Ndis802_11GroupWEP104Enabled:
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_WEP104, 4);
+					break;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
 		// swap for big-endian platform
 		pRsnie_cipher->version = cpu2le16(pRsnie_cipher->version);
 	    pRsnie_cipher->ucount = cpu2le16(pRsnie_cipher->ucount);
 	}
-
 }
 
 /*
diff --git a/drivers/staging/rt2860/common/mlme.c b/drivers/staging/rt2860/common/mlme.c
index 229747095ae..75f810ca9ae 100644
--- a/drivers/staging/rt2860/common/mlme.c
+++ b/drivers/staging/rt2860/common/mlme.c
@@ -808,21 +808,35 @@ VOID MlmePeriodicExec(
 	ULONG			TxTotalCnt;
 	PRTMP_ADAPTER	pAd = (RTMP_ADAPTER *)FunctionContext;
 
+	//Baron 2008/07/10
+	//printk("Baron_Test:\t%s", RTMPGetRalinkEncryModeStr(pAd->StaCfg.WepStatus));
+	//If the STA security setting is OPEN or WEP, pAd->StaCfg.WpaSupplicantUP = 0.
+	//If the STA security setting is WPAPSK or WPA2PSK, pAd->StaCfg.WpaSupplicantUP = 1.
+	if(pAd->StaCfg.WepStatus<2)
+	{
+		pAd->StaCfg.WpaSupplicantUP = 0;
+	}
+	else
+	{
+		pAd->StaCfg.WpaSupplicantUP = 1;
+	}
+
 #ifdef CONFIG_STA_SUPPORT
 #ifdef RT2860
 	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
 	{
 	    // If Hardware controlled Radio enabled, we have to check GPIO pin2 every 2 second.
 		// Move code to here, because following code will return when radio is off
-		if ((pAd->Mlme.PeriodicRound % (MLME_TASK_EXEC_MULTIPLE * 2) == 0) && (pAd->StaCfg.bHardwareRadio == TRUE) &&
+		if ((pAd->Mlme.PeriodicRound % (MLME_TASK_EXEC_MULTIPLE * 2) == 0) &&
+			(pAd->StaCfg.bHardwareRadio == TRUE) &&
+			(RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_START_UP)) &&
 			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
-			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
-			(pAd->bPCIclkOff == FALSE))
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)))
 		{
 			UINT32				data = 0;
 
 			// Read GPIO pin2 as Hardware controlled radio state
-			RTMP_IO_READ32(pAd, GPIO_CTRL_CFG, &data);
+			RTMP_IO_FORCE_READ32(pAd, GPIO_CTRL_CFG, &data);
 			if (data & 0x04)
 			{
 				pAd->StaCfg.bHwRadio = TRUE;
@@ -860,6 +874,45 @@ VOID MlmePeriodicExec(
 								fRTMP_ADAPTER_RESET_IN_PROGRESS))))
 		return;
 
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if ((pAd->RalinkCounters.LastReceivedByteCount == pAd->RalinkCounters.ReceivedByteCount) && (pAd->StaCfg.bRadio == TRUE))
+		{
+			// If ReceiveByteCount doesn't change,  increase SameRxByteCount by 1.
+			pAd->SameRxByteCount++;
+		}
+		else
+			pAd->SameRxByteCount = 0;
+
+		// If after BBP, still not work...need to check to reset PBF&MAC.
+		if (pAd->SameRxByteCount == 702)
+		{
+			pAd->SameRxByteCount = 0;
+			AsicResetPBF(pAd);
+			AsicResetMAC(pAd);
+		}
+
+		// If SameRxByteCount keeps happens for 2 second in infra mode, or for 60 seconds in idle mode.
+		if (((INFRA_ON(pAd)) && (pAd->SameRxByteCount > 20)) || ((IDLE_ON(pAd)) && (pAd->SameRxByteCount > 600)))
+		{
+			if ((pAd->StaCfg.bRadio == TRUE) && (pAd->SameRxByteCount < 700))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("--->  SameRxByteCount = %d !!!!!!!!!!!!!!! \n", pAd->SameRxByteCount));
+				pAd->SameRxByteCount = 700;
+				AsicResetBBP(pAd);
+			}
+		}
+
+		// Update lastReceiveByteCount.
+		pAd->RalinkCounters.LastReceivedByteCount = pAd->RalinkCounters.ReceivedByteCount;
+
+		if ((pAd->CheckDmaBusyCount > 3) && (IDLE_ON(pAd)))
+		{
+			pAd->CheckDmaBusyCount = 0;
+			AsicResetFromDMABusy(pAd);
+		}
+	}
+
 	RT28XX_MLME_PRE_SANITY_CHECK(pAd);
 
 #ifdef RALINK_ATE
@@ -1081,6 +1134,19 @@ VOID STAMlmePeriodicExec(
     		pAd->StaCfg.bBlockAssoc = FALSE;
     }
 
+	//Baron 2008/07/10
+	//printk("Baron_Test:\t%s", RTMPGetRalinkEncryModeStr(pAd->StaCfg.WepStatus));
+	//If the STA security setting is OPEN or WEP, pAd->StaCfg.WpaSupplicantUP = 0.
+	//If the STA security setting is WPAPSK or WPA2PSK, pAd->StaCfg.WpaSupplicantUP = 1.
+	if(pAd->StaCfg.WepStatus<2)
+	{
+		pAd->StaCfg.WpaSupplicantUP = 0;
+	}
+	else
+	{
+		pAd->StaCfg.WpaSupplicantUP = 1;
+	}
+
     if ((pAd->PreMediaState != pAd->IndicateMediaState) && (pAd->CommonCfg.bWirelessEvent))
 	{
 		if (pAd->IndicateMediaState == NdisMediaStateConnected)
@@ -1090,6 +1156,15 @@ VOID STAMlmePeriodicExec(
 		pAd->PreMediaState = pAd->IndicateMediaState;
 	}
 
+	if ((pAd->OpMode == OPMODE_STA) && (IDLE_ON(pAd)) &&
+        (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE)) &&
+		(pAd->Mlme.SyncMachine.CurrState == SYNC_IDLE) &&
+		(pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE) &&
+		(RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_START_UP)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_IDLE_RADIO_OFF)))
+	{
+		RT28xxPciAsicRadioOff(pAd, GUI_IDLE_POWER_SAVE, 0);
+	}
 
 
 
@@ -2781,7 +2856,7 @@ VOID MlmeCheckPsmChange(
 	if (INFRA_ON(pAd) &&
 		(PowerMode != Ndis802_11PowerModeCAM) &&
 		(pAd->StaCfg.Psm == PWR_ACTIVE) &&
-		(pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE))
+		RTMP_TEST_PSFLAG(pAd, fRTMP_PS_CAN_GO_SLEEP))
 	{
 		NdisGetSystemUpTime(&pAd->Mlme.LastSendNULLpsmTime);
 		pAd->RalinkCounters.RxCountSinceLastNULL = 0;
@@ -4065,7 +4140,9 @@ VOID BssTableSsidSort(
 							continue;
 
 					// check group cipher
-					if (pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher)
+					if ((pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher) &&
+						(pInBss->WPA.GroupCipher != Ndis802_11GroupWEP40Enabled) &&
+						(pInBss->WPA.GroupCipher != Ndis802_11GroupWEP104Enabled))
 						continue;
 
 					// check pairwise cipher, skip if none matched
@@ -4084,7 +4161,9 @@ VOID BssTableSsidSort(
 							continue;
 
 					// check group cipher
-					if (pAd->StaCfg.WepStatus < pInBss->WPA2.GroupCipher)
+					if ((pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher) &&
+						(pInBss->WPA2.GroupCipher != Ndis802_11GroupWEP40Enabled) &&
+						(pInBss->WPA2.GroupCipher != Ndis802_11GroupWEP104Enabled))
 						continue;
 
 					// check pairwise cipher, skip if none matched
@@ -4371,8 +4450,10 @@ VOID BssCipherParse(
 				switch (*pTmp)
 				{
 					case 1:
-					case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
-						pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+						pBss->WPA.GroupCipher = Ndis802_11GroupWEP40Enabled;
+						break;
+					case 5:
+						pBss->WPA.GroupCipher = Ndis802_11GroupWEP104Enabled;
 						break;
 					case 2:
 						pBss->WPA.GroupCipher = Ndis802_11Encryption2Enabled;
@@ -4489,8 +4570,10 @@ VOID BssCipherParse(
 				switch (pCipher->Type)
 				{
 					case 1:
-					case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
-						pBss->WPA2.GroupCipher = Ndis802_11Encryption1Enabled;
+						pBss->WPA2.GroupCipher = Ndis802_11GroupWEP40Enabled;
+						break;
+					case 5:
+						pBss->WPA2.GroupCipher = Ndis802_11GroupWEP104Enabled;
 						break;
 					case 2:
 						pBss->WPA2.GroupCipher = Ndis802_11Encryption2Enabled;
@@ -6149,6 +6232,12 @@ VOID AsicAdjustTxPower(
 	ULONG		TxPwr[5];
 	CHAR		Value;
 
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE)
+		|| (pAd->bPCIclkOff == TRUE)
+		|| RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_IDLE_RADIO_OFF)
+		|| RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		return;
+
 	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
 	{
 		if (pAd->CommonCfg.CentralChannel > 14)
@@ -6493,10 +6582,10 @@ VOID AsicForceSleep(
  */
 VOID AsicForceWakeup(
 	IN PRTMP_ADAPTER pAd,
-	IN BOOLEAN    bFromTx)
+	IN UCHAR    	 Level)
 {
     DBGPRINT(RT_DEBUG_TRACE, ("--> AsicForceWakeup \n"));
-    RT28XX_STA_FORCE_WAKEUP(pAd, bFromTx);
+    RT28XX_STA_FORCE_WAKEUP(pAd, Level);
 }
 #endif // CONFIG_STA_SUPPORT //
 /*
@@ -7585,9 +7674,30 @@ BOOLEAN AsicSendCommandToMcu(
 #endif // RALINK_ATE //
 #endif // RT2860 //
 		{
+			UINT32 Data;
+
+			// Reset DMA
+			RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &Data);
+			Data |= 0x2;
+			RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, Data);
+
+			// After Reset DMA, DMA index will become Zero. So Driver need to reset all ring indexs too.
+			// Reset DMA/CPU ring index
+			RTMPRingCleanUp(pAd, QID_AC_BK);
+			RTMPRingCleanUp(pAd, QID_AC_BE);
+			RTMPRingCleanUp(pAd, QID_AC_VI);
+			RTMPRingCleanUp(pAd, QID_AC_VO);
+			RTMPRingCleanUp(pAd, QID_HCCA);
+			RTMPRingCleanUp(pAd, QID_MGMT);
+			RTMPRingCleanUp(pAd, QID_RX);
+
+			// Clear Reset
+			RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &Data);
+			Data &= 0xfffffffd;
+			RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, Data);
 		DBGPRINT_ERR(("H2M_MAILBOX still hold by MCU. command fail\n"));
 		}
-		return FALSE;
+		//return FALSE;
 	}
 
 #ifdef RT2860
@@ -8518,6 +8628,106 @@ VOID AsicStaBbpTuning(
 
 	}
 }
+
+VOID AsicResetFromDMABusy(
+	IN PRTMP_ADAPTER pAd)
+{
+	UINT32		Data;
+	BOOLEAN		bCtrl = FALSE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->  AsicResetFromDMABusy  !!!!!!!!!!!!!!!!!!!!!!! \n"));
+
+	// Be sure restore link control value so we can write register.
+	RTMP_CLEAR_PSFLAG(pAd, fRTMP_PS_CAN_GO_SLEEP);
+	if (RTMP_TEST_PSFLAG(pAd, fRTMP_PS_SET_PCI_CLK_OFF_COMMAND))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("AsicResetFromDMABusy==>\n"));
+		RTMPPCIeLinkCtrlValueRestore(pAd, RESTORE_HALT);
+		RTMPusecDelay(6000);
+		pAd->bPCIclkOff = FALSE;
+		bCtrl = TRUE;
+	}
+	// Reset DMA
+	RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &Data);
+	Data |= 0x2;
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, Data);
+
+	// After Reset DMA, DMA index will become Zero. So Driver need to reset all ring indexs too.
+	// Reset DMA/CPU ring index
+	RTMPRingCleanUp(pAd, QID_AC_BK);
+	RTMPRingCleanUp(pAd, QID_AC_BE);
+	RTMPRingCleanUp(pAd, QID_AC_VI);
+	RTMPRingCleanUp(pAd, QID_AC_VO);
+	RTMPRingCleanUp(pAd, QID_HCCA);
+	RTMPRingCleanUp(pAd, QID_MGMT);
+	RTMPRingCleanUp(pAd, QID_RX);
+
+	// Clear Reset
+	RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &Data);
+	Data &= 0xfffffffd;
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, Data);
+
+	// If in Radio off, should call RTMPPCIePowerLinkCtrl again.
+	if ((bCtrl == TRUE) && (pAd->StaCfg.bRadio == FALSE))
+		RTMPPCIeLinkCtrlSetting(pAd, 3);
+
+	RTMP_SET_PSFLAG(pAd, fRTMP_PS_CAN_GO_SLEEP);
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_HALT_IN_PROGRESS);
+	DBGPRINT(RT_DEBUG_TRACE, ("<---  AsicResetFromDMABusy !!!!!!!!!!!!!!!!!!!!!!!  \n"));
+}
+
+VOID AsicResetBBP(
+	IN PRTMP_ADAPTER pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("--->  Asic HardReset BBP  !!!!!!!!!!!!!!!!!!!!!!! \n"));
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x0);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x2);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0xc);
+
+	// After hard-reset BBP, initialize all BBP values.
+	NICRestoreBBPValue(pAd);
+	DBGPRINT(RT_DEBUG_TRACE, ("<---  Asic HardReset BBP !!!!!!!!!!!!!!!!!!!!!!!  \n"));
+}
+
+VOID AsicResetMAC(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG		Data;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->  AsicResetMAC   !!!! \n"));
+	RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &Data);
+	Data |= 0x4;
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, Data);
+	Data &= 0xfffffffb;
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, Data);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<---  AsicResetMAC   !!!! \n"));
+}
+
+VOID AsicResetPBF(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG		Value1, Value2;
+	ULONG		Data;
+
+	RTMP_IO_READ32(pAd, TXRXQ_PCNT, &Value1);
+	RTMP_IO_READ32(pAd, PBF_DBG, &Value2);
+
+	Value2 &= 0xff;
+	// sum should be equals to 0xff, which is the total buffer size.
+	if ((Value1 + Value2) < 0xff)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("--->  Asic HardReset PBF !!!! \n"));
+		RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &Data);
+		Data |= 0x8;
+		RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, Data);
+		Data &= 0xfffffff7;
+		RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, Data);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("<---  Asic HardReset PBF !!!! \n"));
+	}
+}
 #endif // CONFIG_STA_SUPPORT //
 
 VOID RTMPSetAGCInitValue(
diff --git a/drivers/staging/rt2860/common/rtmp_init.c b/drivers/staging/rt2860/common/rtmp_init.c
index 563f2c5bb85..19a21ff489a 100644
--- a/drivers/staging/rt2860/common/rtmp_init.c
+++ b/drivers/staging/rt2860/common/rtmp_init.c
@@ -2041,6 +2041,131 @@ NDIS_STATUS	NICInitializeAsic(
 	return NDIS_STATUS_SUCCESS;
 }
 
+
+VOID NICRestoreBBPValue(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR		index;
+	UCHAR		Value;
+	ULONG		Data;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->  NICRestoreBBPValue !!!!!!!!!!!!!!!!!!!!!!!  \n"));
+	// Initialize BBP register to default value (rtmp_init.c)
+	for (index = 0; index < NUM_BBP_REG_PARMS; index++)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBPRegTable[index].Register, BBPRegTable[index].Value);
+	}
+	// copy from (rtmp_init.c)
+	if (pAd->MACVersion == 0x28600100)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x12);
+	}
+
+	// copy from (connect.c LinkUp function)
+	if (INFRA_ON(pAd))
+	{
+		// Change to AP channel
+		if ((pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel) && (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+		{
+			// Must using 40MHz.
+			pAd->CommonCfg.BBPCurrentBW = BW_40;
+			AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+			Value &= (~0x18);
+			Value |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+			//  RX : control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+			Value &= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+			// Record BBPR3 setting, But don't keep R Antenna # information.
+			pAd->StaCfg.BBPR3 = Value;
+
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+			Data &= 0xfffffffe;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+			if (pAd->MACVersion == 0x28600100)
+			{
+				RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x1A);
+				RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0A);
+				RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x16);
+				DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Lower LINK UP !!! Control Channel at Below. Central = %d \n", pAd->CommonCfg.CentralChannel ));
+		}
+		else if ((pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel) && (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+		{
+			// Must using 40MHz.
+			pAd->CommonCfg.BBPCurrentBW = BW_40;
+			AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+			Value &= (~0x18);
+			Value |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+			Data |= 0x1;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+			Value |= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+			// Record BBPR3 setting, But don't keep R Antenna # information.
+			pAd->StaCfg.BBPR3 = Value;
+
+			if (pAd->MACVersion == 0x28600100)
+			{
+				RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x1A);
+				RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0A);
+				RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x16);
+				DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Upper LINK UP !!! Control Channel at UpperCentral = %d \n", pAd->CommonCfg.CentralChannel ));
+		}
+		else
+		{
+			pAd->CommonCfg.BBPCurrentBW = BW_20;
+			AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+			Value &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+			Data &= 0xfffffffe;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+			Value &= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+			// Record BBPR3 setting, But don't keep R Antenna # information.
+			pAd->StaCfg.BBPR3 = Value;
+
+			if (pAd->MACVersion == 0x28600100)
+			{
+				RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+				RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x08);
+				RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x11);
+				DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!20MHz LINK UP !!! \n" ));
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<---  NICRestoreBBPValue !!!!!!!!!!!!!!!!!!!!!!!  \n"));
+}
+
 /*
 	========================================================================
 
@@ -3032,6 +3157,7 @@ VOID	UserCfgInit(
 	pAd->LedIndicatorStregth = 0;
 	pAd->RLnkCtrlOffset = 0;
 	pAd->HostLnkCtrlOffset = 0;
+	pAd->CheckDmaBusyCount = 0;
 #endif // RT2860 //
 
 	pAd->bAutoTxAgcA = FALSE;			// Default is OFF
@@ -3308,7 +3434,7 @@ VOID	UserCfgInit(
     pAd->bPCIclkOff = FALSE;
 #endif // RT2860 //
 
-
+	RTMP_SET_PSFLAG(pAd, fRTMP_PS_CAN_GO_SLEEP);
 	DBGPRINT(RT_DEBUG_TRACE, ("<-- UserCfgInit\n"));
 }