Staging: add rt2870 wireless driver

This is the Ralink RT2870 driver from the company that does horrible
things like reading a config file from /etc.  However, the driver that
is currently under development from the wireless development community
is not working at all yet, so distros and users are using this version
instead (quite common hardware on a lot of netbook machines).

So here is this driver, for now, until the wireless developers get a
"clean" version into the main tree, or until this version is cleaned up
sufficiently to move out of the staging tree.

Ported to the Linux build system and cleaned up a bit already by me.

Cc: Linux wireless <linux-wireless@vger.kernel.org>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>

23 files changed, 39917 insertions, 0 deletions

diff --git a/drivers/staging/rt2870/common/2870_rtmp_init.c b/drivers/staging/rt2870/common/2870_rtmp_init.c
new file mode 100644
index 00000000000..359c1820e6d
--- /dev/null
+++ b/drivers/staging/rt2870/common/2870_rtmp_init.c
@@ -0,0 +1,1778 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	2870_rtmp_init.c
+
+	Abstract:
+	Miniport generic portion header file
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    2002-08-01    created
+    John Chang  2004-08-20    RT2561/2661 use scatter-gather scheme
+    Jan Lee  	2006-09-15    RT2860. Change for 802.11n , EEPROM, Led, BA, HT.
+	Sample Lin	2007-05-31    Merge RT2860 and RT2870 drivers.
+*/
+
+#include "../rt_config.h"
+
+
+static void rx_done_tasklet(unsigned long data);
+static void rt2870_hcca_dma_done_tasklet(unsigned long data);
+static void rt2870_ac3_dma_done_tasklet(unsigned long data);
+static void rt2870_ac2_dma_done_tasklet(unsigned long data);
+static void rt2870_ac1_dma_done_tasklet(unsigned long data);
+static void rt2870_ac0_dma_done_tasklet(unsigned long data);
+static void rt2870_mgmt_dma_done_tasklet(unsigned long data);
+static void rt2870_null_frame_complete_tasklet(unsigned long data);
+static void rt2870_rts_frame_complete_tasklet(unsigned long data);
+static void rt2870_pspoll_frame_complete_tasklet(unsigned long data);
+static void rt2870_dataout_complete_tasklet(unsigned long data);
+
+
+/*
+========================================================================
+Routine Description:
+    Initialize receive data structures.
+
+Arguments:
+    pAd					Pointer to our adapter
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_RESOURCES
+
+Note:
+	Initialize all receive releated private buffer, include those define
+	in RTMP_ADAPTER structure and all private data structures. The mahor
+	work is to allocate buffer for each packet and chain buffer to
+	NDIS packet descriptor.
+========================================================================
+*/
+NDIS_STATUS	NICInitRecv(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR				i;
+	NDIS_STATUS			Status = NDIS_STATUS_SUCCESS;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitRecv\n"));
+	pObj = pObj;
+
+	//InterlockedExchange(&pAd->PendingRx, 0);
+	pAd->PendingRx = 0;
+	pAd->NextRxBulkInReadIndex 	= 0;	// Next Rx Read index
+	pAd->NextRxBulkInIndex		= 0 ; //RX_RING_SIZE -1; // Rx Bulk pointer
+	pAd->NextRxBulkInPosition 	= 0;
+
+	for (i = 0; i < (RX_RING_SIZE); i++)
+	{
+		PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+		//Allocate URB
+		pRxContext->pUrb = RTUSB_ALLOC_URB(0);
+		if (pRxContext->pUrb == NULL)
+		{
+			Status = NDIS_STATUS_RESOURCES;
+			goto out1;
+		}
+
+		// Allocate transfer buffer
+		pRxContext->TransferBuffer = RTUSB_URB_ALLOC_BUFFER(pObj->pUsb_Dev, MAX_RXBULK_SIZE, &pRxContext->data_dma);
+		if (pRxContext->TransferBuffer == NULL)
+		{
+			Status = NDIS_STATUS_RESOURCES;
+			goto out1;
+		}
+
+		NdisZeroMemory(pRxContext->TransferBuffer, MAX_RXBULK_SIZE);
+
+		pRxContext->pAd	= pAd;
+		pRxContext->pIrp = NULL;
+		pRxContext->InUse		= FALSE;
+		pRxContext->IRPPending	= FALSE;
+		pRxContext->Readable	= FALSE;
+		//pRxContext->ReorderInUse = FALSE;
+		pRxContext->bRxHandling = FALSE;
+		pRxContext->BulkInOffset = 0;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitRecv\n"));
+	return Status;
+
+out1:
+	for (i = 0; i < (RX_RING_SIZE); i++)
+	{
+		PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+		if (NULL != pRxContext->TransferBuffer)
+		{
+			RTUSB_URB_FREE_BUFFER(pObj->pUsb_Dev, MAX_RXBULK_SIZE,
+								pRxContext->TransferBuffer, pRxContext->data_dma);
+			pRxContext->TransferBuffer = NULL;
+		}
+
+		if (NULL != pRxContext->pUrb)
+		{
+			RTUSB_UNLINK_URB(pRxContext->pUrb);
+			RTUSB_FREE_URB(pRxContext->pUrb);
+			pRxContext->pUrb = NULL;
+		}
+	}
+
+	return Status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Initialize transmit data structures.
+
+Arguments:
+    pAd					Pointer to our adapter
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_RESOURCES
+
+Note:
+========================================================================
+*/
+NDIS_STATUS	NICInitTransmit(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#define LM_USB_ALLOC(pObj, Context, TB_Type, BufferSize, Status, msg1, err1, msg2, err2)	\
+	Context->pUrb = RTUSB_ALLOC_URB(0);		\
+	if (Context->pUrb == NULL) {			\
+		DBGPRINT(RT_DEBUG_ERROR, msg1);		\
+		Status = NDIS_STATUS_RESOURCES;		\
+		goto err1; }						\
+											\
+	Context->TransferBuffer = 				\
+		(TB_Type)RTUSB_URB_ALLOC_BUFFER(pObj->pUsb_Dev, BufferSize, &Context->data_dma);	\
+	if (Context->TransferBuffer == NULL) {	\
+		DBGPRINT(RT_DEBUG_ERROR, msg2);		\
+		Status = NDIS_STATUS_RESOURCES;		\
+		goto err2; }
+
+#define LM_URB_FREE(pObj, Context, BufferSize)				\
+	if (NULL != Context->pUrb) {							\
+		RTUSB_UNLINK_URB(Context->pUrb);					\
+		RTUSB_FREE_URB(Context->pUrb);						\
+		Context->pUrb = NULL; }								\
+	if (NULL != Context->TransferBuffer) {				\
+		RTUSB_URB_FREE_BUFFER(pObj->pUsb_Dev, BufferSize,	\
+								Context->TransferBuffer,	\
+								Context->data_dma);			\
+		Context->TransferBuffer = NULL; }
+
+	UCHAR			i, acidx;
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	PTX_CONTEXT		pNullContext   = &(pAd->NullContext);
+	PTX_CONTEXT		pPsPollContext = &(pAd->PsPollContext);
+	PTX_CONTEXT		pRTSContext    = &(pAd->RTSContext);
+	PTX_CONTEXT		pMLMEContext = NULL;
+//	PHT_TX_CONTEXT	pHTTXContext = NULL;
+	POS_COOKIE		pObj = (POS_COOKIE) pAd->OS_Cookie;
+	PVOID			RingBaseVa;
+//	RTMP_TX_RING	*pTxRing;
+	RTMP_MGMT_RING  *pMgmtRing;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitTransmit\n"));
+	pObj = pObj;
+
+	// Init 4 set of Tx parameters
+	for(acidx = 0; acidx < NUM_OF_TX_RING; acidx++)
+	{
+		// Initialize all Transmit releated queues
+		InitializeQueueHeader(&pAd->TxSwQueue[acidx]);
+
+		// Next Local tx ring pointer waiting for buck out
+		pAd->NextBulkOutIndex[acidx] = acidx;
+		pAd->BulkOutPending[acidx] = FALSE; // Buck Out control flag
+		//pAd->DataBulkDoneIdx[acidx] = 0;
+	}
+
+	//pAd->NextMLMEIndex	= 0;
+	//pAd->PushMgmtIndex	= 0;
+	//pAd->PopMgmtIndex	= 0;
+	//InterlockedExchange(&pAd->MgmtQueueSize, 0);
+	//InterlockedExchange(&pAd->TxCount, 0);
+
+	//pAd->PrioRingFirstIndex	= 0;
+	//pAd->PrioRingTxCnt		= 0;
+
+	do
+	{
+		//
+		// TX_RING_SIZE, 4 ACs
+		//
+		for(acidx=0; acidx<4; acidx++)
+		{
+			PHT_TX_CONTEXT	pHTTXContext = &(pAd->TxContext[acidx]);
+
+			NdisZeroMemory(pHTTXContext, sizeof(HT_TX_CONTEXT));
+			//Allocate URB
+			LM_USB_ALLOC(pObj, pHTTXContext, PHTTX_BUFFER, sizeof(HTTX_BUFFER), Status,
+							("<-- ERROR in Alloc TX TxContext[%d] urb!! \n", acidx),
+							done,
+							("<-- ERROR in Alloc TX TxContext[%d] HTTX_BUFFER !! \n", acidx),
+							out1);
+
+			NdisZeroMemory(pHTTXContext->TransferBuffer->Aggregation, 4);
+			pHTTXContext->pAd = pAd;
+			pHTTXContext->pIrp = NULL;
+			pHTTXContext->IRPPending = FALSE;
+			pHTTXContext->NextBulkOutPosition = 0;
+			pHTTXContext->ENextBulkOutPosition = 0;
+			pHTTXContext->CurWritePosition = 0;
+			pHTTXContext->CurWriteRealPos = 0;
+			pHTTXContext->BulkOutSize = 0;
+			pHTTXContext->BulkOutPipeId = acidx;
+			pHTTXContext->bRingEmpty = TRUE;
+			pHTTXContext->bCopySavePad = FALSE;
+
+			pAd->BulkOutPending[acidx] = FALSE;
+		}
+
+
+		//
+		// MGMT_RING_SIZE
+		//
+#if 0
+		for(i=0; i<MGMT_RING_SIZE; i++) // 8
+		{
+			PTX_CONTEXT	pMLMEContext = &(pAd->MLMEContext[i]);
+
+
+			NdisZeroMemory(pMLMEContext, sizeof(TX_CONTEXT));
+
+			//Allocate URB
+			LM_USB_ALLOC(pObj, pMLMEContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+							("<-- ERROR in Alloc TX MLMEContext[%d] urb!! \n", i),
+							out2,
+							("<-- ERROR in Alloc TX MLMEContext[%d] TX_BUFFER !! \n", i),
+							out2);
+
+			pMLMEContext->pAd = pAd;
+			pMLMEContext->pIrp = NULL;
+			pMLMEContext->InUse = FALSE;
+			pMLMEContext->IRPPending = FALSE;
+		}
+#else
+		// Allocate MGMT ring descriptor's memory
+		pAd->MgmtDescRing.AllocSize = MGMT_RING_SIZE * sizeof(TX_CONTEXT);
+		RTMPAllocateMemory(&pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize);
+		if (pAd->MgmtDescRing.AllocVa == NULL)
+		{
+			DBGPRINT_ERR(("Failed to allocate a big buffer for MgmtDescRing!\n"));
+			Status = NDIS_STATUS_RESOURCES;
+			goto out1;
+		}
+		NdisZeroMemory(pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize);
+		RingBaseVa     = pAd->MgmtDescRing.AllocVa;
+
+		// Initialize MGMT Ring and associated buffer memory
+		pMgmtRing = &pAd->MgmtRing;
+		for (i = 0; i < MGMT_RING_SIZE; i++)
+		{
+			// link the pre-allocated Mgmt buffer to MgmtRing.Cell
+			pMgmtRing->Cell[i].AllocSize = sizeof(TX_CONTEXT);
+			pMgmtRing->Cell[i].AllocVa = RingBaseVa;
+			pMgmtRing->Cell[i].pNdisPacket = NULL;
+			pMgmtRing->Cell[i].pNextNdisPacket = NULL;
+
+			//Allocate URB for MLMEContext
+			pMLMEContext = (PTX_CONTEXT) pAd->MgmtRing.Cell[i].AllocVa;
+			pMLMEContext->pUrb = RTUSB_ALLOC_URB(0);
+			if (pMLMEContext->pUrb == NULL)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("<-- ERROR in Alloc TX MLMEContext[%d] urb!! \n", i));
+				Status = NDIS_STATUS_RESOURCES;
+				goto out2;
+			}
+			pMLMEContext->pAd = pAd;
+			pMLMEContext->pIrp = NULL;
+			pMLMEContext->TransferBuffer = NULL;
+			pMLMEContext->InUse = FALSE;
+			pMLMEContext->IRPPending = FALSE;
+			pMLMEContext->bWaitingBulkOut = FALSE;
+			pMLMEContext->BulkOutSize = 0;
+			pMLMEContext->SelfIdx = i;
+
+			// Offset to next ring descriptor address
+			RingBaseVa = (PUCHAR) RingBaseVa + sizeof(TX_CONTEXT);
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("MGMT Ring: total %d entry allocated\n", i));
+
+		//pAd->MgmtRing.TxSwFreeIdx = (MGMT_RING_SIZE - 1);
+		pAd->MgmtRing.TxSwFreeIdx = MGMT_RING_SIZE;
+		pAd->MgmtRing.TxCpuIdx = 0;
+		pAd->MgmtRing.TxDmaIdx = 0;
+#endif
+
+		//
+		// BEACON_RING_SIZE
+		//
+		for(i=0; i<BEACON_RING_SIZE; i++) // 2
+		{
+			PTX_CONTEXT	pBeaconContext = &(pAd->BeaconContext[i]);
+
+
+			NdisZeroMemory(pBeaconContext, sizeof(TX_CONTEXT));
+
+			//Allocate URB
+			LM_USB_ALLOC(pObj, pBeaconContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+							("<-- ERROR in Alloc TX BeaconContext[%d] urb!! \n", i),
+							out2,
+							("<-- ERROR in Alloc TX BeaconContext[%d] TX_BUFFER !! \n", i),
+							out3);
+
+			pBeaconContext->pAd = pAd;
+			pBeaconContext->pIrp = NULL;
+			pBeaconContext->InUse = FALSE;
+			pBeaconContext->IRPPending = FALSE;
+		}
+
+		//
+		// NullContext
+		//
+		NdisZeroMemory(pNullContext, sizeof(TX_CONTEXT));
+
+		//Allocate URB
+		LM_USB_ALLOC(pObj, pNullContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+						("<-- ERROR in Alloc TX NullContext urb!! \n"),
+						out3,
+						("<-- ERROR in Alloc TX NullContext TX_BUFFER !! \n"),
+						out4);
+
+		pNullContext->pAd = pAd;
+		pNullContext->pIrp = NULL;
+		pNullContext->InUse = FALSE;
+		pNullContext->IRPPending = FALSE;
+
+		//
+		// RTSContext
+		//
+		NdisZeroMemory(pRTSContext, sizeof(TX_CONTEXT));
+
+		//Allocate URB
+		LM_USB_ALLOC(pObj, pRTSContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+						("<-- ERROR in Alloc TX RTSContext urb!! \n"),
+						out4,
+						("<-- ERROR in Alloc TX RTSContext TX_BUFFER !! \n"),
+						out5);
+
+		pRTSContext->pAd = pAd;
+		pRTSContext->pIrp = NULL;
+		pRTSContext->InUse = FALSE;
+		pRTSContext->IRPPending = FALSE;
+
+		//
+		// PsPollContext
+		//
+		//NdisZeroMemory(pPsPollContext, sizeof(TX_CONTEXT));
+		//Allocate URB
+		LM_USB_ALLOC(pObj, pPsPollContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+						("<-- ERROR in Alloc TX PsPollContext urb!! \n"),
+						out5,
+						("<-- ERROR in Alloc TX PsPollContext TX_BUFFER !! \n"),
+						out6);
+
+		pPsPollContext->pAd = pAd;
+		pPsPollContext->pIrp = NULL;
+		pPsPollContext->InUse = FALSE;
+		pPsPollContext->IRPPending = FALSE;
+		pPsPollContext->bAggregatible = FALSE;
+		pPsPollContext->LastOne = TRUE;
+
+	}   while (FALSE);
+
+
+done:
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitTransmit\n"));
+
+	return Status;
+
+	/* --------------------------- ERROR HANDLE --------------------------- */
+out6:
+	LM_URB_FREE(pObj, pPsPollContext, sizeof(TX_BUFFER));
+
+out5:
+	LM_URB_FREE(pObj, pRTSContext, sizeof(TX_BUFFER));
+
+out4:
+	LM_URB_FREE(pObj, pNullContext, sizeof(TX_BUFFER));
+
+out3:
+	for(i=0; i<BEACON_RING_SIZE; i++)
+	{
+		PTX_CONTEXT	pBeaconContext = &(pAd->BeaconContext[i]);
+		if (pBeaconContext)
+			LM_URB_FREE(pObj, pBeaconContext, sizeof(TX_BUFFER));
+	}
+
+out2:
+	if (pAd->MgmtDescRing.AllocVa)
+	{
+		pMgmtRing = &pAd->MgmtRing;
+	for(i=0; i<MGMT_RING_SIZE; i++)
+	{
+		pMLMEContext = (PTX_CONTEXT) pAd->MgmtRing.Cell[i].AllocVa;
+		if (pMLMEContext)
+			LM_URB_FREE(pObj, pMLMEContext, sizeof(TX_BUFFER));
+	}
+		NdisFreeMemory(pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize, 0);
+		pAd->MgmtDescRing.AllocVa = NULL;
+	}
+
+out1:
+	for (acidx = 0; acidx < 4; acidx++)
+	{
+		PHT_TX_CONTEXT pTxContext = &(pAd->TxContext[acidx]);
+		if (pTxContext)
+			LM_URB_FREE(pObj, pTxContext, sizeof(HTTX_BUFFER));
+	}
+
+	// Here we didn't have any pre-allocated memory need to free.
+
+	return Status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Allocate DMA memory blocks for send, receive.
+
+Arguments:
+    pAd					Pointer to our adapter
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_FAILURE
+	NDIS_STATUS_RESOURCES
+
+Note:
+========================================================================
+*/
+NDIS_STATUS	RTMPAllocTxRxRingMemory(
+	IN	PRTMP_ADAPTER	pAd)
+{
+//	COUNTER_802_11	pCounter = &pAd->WlanCounters;
+	NDIS_STATUS		Status;
+	INT				num;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RTMPAllocTxRxRingMemory\n"));
+
+
+	do
+	{
+		// Init the CmdQ and CmdQLock
+		NdisAllocateSpinLock(&pAd->CmdQLock);
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		RTUSBInitializeCmdQ(&pAd->CmdQ);
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+
+
+		NdisAllocateSpinLock(&pAd->MLMEBulkOutLock);
+		//NdisAllocateSpinLock(&pAd->MLMEWaitQueueLock);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[0]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[1]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[2]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[3]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[4]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[5]);
+		NdisAllocateSpinLock(&pAd->BulkInLock);
+
+		for (num = 0; num < NUM_OF_TX_RING; num++)
+		{
+			NdisAllocateSpinLock(&pAd->TxContextQueueLock[num]);
+		}
+
+#ifdef RALINK_ATE
+		NdisAllocateSpinLock(&pAd->GenericLock);
+#endif // RALINK_ATE //
+
+//		NdisAllocateSpinLock(&pAd->MemLock);	// Not used in RT28XX
+
+//		NdisAllocateSpinLock(&pAd->MacTabLock); // init it in UserCfgInit()
+//		NdisAllocateSpinLock(&pAd->BATabLock); // init it in BATableInit()
+
+//		for(num=0; num<MAX_LEN_OF_BA_REC_TABLE; num++)
+//		{
+//			NdisAllocateSpinLock(&pAd->BATable.BARecEntry[num].RxReRingLock);
+//		}
+
+		//
+		// Init Mac Table
+		//
+//		MacTableInitialize(pAd);
+
+		//
+		// Init send data structures and related parameters
+		//
+		Status = NICInitTransmit(pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		//
+		// Init receive data structures and related parameters
+		//
+		Status = NICInitRecv(pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		pAd->PendingIoCount = 1;
+
+	} while (FALSE);
+
+	NdisZeroMemory(&pAd->FragFrame, sizeof(FRAGMENT_FRAME));
+	pAd->FragFrame.pFragPacket =  RTMP_AllocateFragPacketBuffer(pAd, RX_BUFFER_NORMSIZE);
+
+	if (pAd->FragFrame.pFragPacket == NULL)
+	{
+		Status = NDIS_STATUS_RESOURCES;
+	}
+
+	DBGPRINT_S(Status, ("<-- RTMPAllocTxRxRingMemory, Status=%x\n", Status));
+	return Status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+	Calls USB_InterfaceStop and frees memory allocated for the URBs
+    calls NdisMDeregisterDevice and frees the memory
+    allocated in VNetInitialize for the Adapter Object
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID	RTMPFreeTxRxRingMemory(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#define LM_URB_FREE(pObj, Context, BufferSize)				\
+	if (NULL != Context->pUrb) {							\
+		RTUSB_UNLINK_URB(Context->pUrb);					\
+		RTUSB_FREE_URB(Context->pUrb);						\
+		Context->pUrb = NULL; }								\
+	if (NULL != Context->TransferBuffer) {					\
+		RTUSB_URB_FREE_BUFFER(pObj->pUsb_Dev, BufferSize,	\
+								Context->TransferBuffer,	\
+								Context->data_dma);			\
+		Context->TransferBuffer = NULL; }
+
+
+	UINT                i, acidx;
+	PTX_CONTEXT			pNullContext   = &pAd->NullContext;
+	PTX_CONTEXT			pPsPollContext = &pAd->PsPollContext;
+	PTX_CONTEXT			pRTSContext    = &pAd->RTSContext;
+//	PHT_TX_CONTEXT 		pHTTXContext;
+	//PRTMP_REORDERBUF	pReorderBuf;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+//	RTMP_TX_RING		*pTxRing;
+
+	DBGPRINT(RT_DEBUG_ERROR, ("---> RTMPFreeTxRxRingMemory\n"));
+	pObj = pObj;
+
+	// Free all resources for the RECEIVE buffer queue.
+	for(i=0; i<(RX_RING_SIZE); i++)
+	{
+		PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+		if (pRxContext)
+			LM_URB_FREE(pObj, pRxContext, MAX_RXBULK_SIZE);
+	}
+
+	// Free PsPoll frame resource
+	LM_URB_FREE(pObj, pPsPollContext, sizeof(TX_BUFFER));
+
+	// Free NULL frame resource
+	LM_URB_FREE(pObj, pNullContext, sizeof(TX_BUFFER));
+
+	// Free RTS frame resource
+	LM_URB_FREE(pObj, pRTSContext, sizeof(TX_BUFFER));
+
+
+	// Free beacon frame resource
+	for(i=0; i<BEACON_RING_SIZE; i++)
+	{
+		PTX_CONTEXT	pBeaconContext = &(pAd->BeaconContext[i]);
+		if (pBeaconContext)
+			LM_URB_FREE(pObj, pBeaconContext, sizeof(TX_BUFFER));
+	}
+
+
+	// Free mgmt frame resource
+	for(i = 0; i < MGMT_RING_SIZE; i++)
+	{
+		PTX_CONTEXT pMLMEContext = (PTX_CONTEXT)pAd->MgmtRing.Cell[i].AllocVa;
+		//LM_URB_FREE(pObj, pMLMEContext, sizeof(TX_BUFFER));
+		if (NULL != pAd->MgmtRing.Cell[i].pNdisPacket)
+		{
+			RTMPFreeNdisPacket(pAd, pAd->MgmtRing.Cell[i].pNdisPacket);
+			pAd->MgmtRing.Cell[i].pNdisPacket = NULL;
+			pMLMEContext->TransferBuffer = NULL;
+		}
+
+		if (pMLMEContext)
+		{
+			if (NULL != pMLMEContext->pUrb)
+			{
+				RTUSB_UNLINK_URB(pMLMEContext->pUrb);
+				RTUSB_FREE_URB(pMLMEContext->pUrb);
+				pMLMEContext->pUrb = NULL;
+			}
+		}
+	}
+	if (pAd->MgmtDescRing.AllocVa)
+		NdisFreeMemory(pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize, 0);
+
+
+	// Free Tx frame resource
+	for (acidx = 0; acidx < 4; acidx++)
+		{
+		PHT_TX_CONTEXT pHTTXContext = &(pAd->TxContext[acidx]);
+			if (pHTTXContext)
+				LM_URB_FREE(pObj, pHTTXContext, sizeof(HTTX_BUFFER));
+		}
+
+	if (pAd->FragFrame.pFragPacket)
+		RELEASE_NDIS_PACKET(pAd, pAd->FragFrame.pFragPacket, NDIS_STATUS_SUCCESS);
+
+	for(i=0; i<6; i++)
+	{
+		NdisFreeSpinLock(&pAd->BulkOutLock[i]);
+	}
+
+	NdisFreeSpinLock(&pAd->BulkInLock);
+	NdisFreeSpinLock(&pAd->MLMEBulkOutLock);
+
+	NdisFreeSpinLock(&pAd->CmdQLock);
+#ifdef RALINK_ATE
+	NdisFreeSpinLock(&pAd->GenericLock);
+#endif // RALINK_ATE //
+	// Clear all pending bulk-out request flags.
+	RTUSB_CLEAR_BULK_FLAG(pAd, 0xffffffff);
+
+//	NdisFreeSpinLock(&pAd->MacTabLock);
+
+//	for(i=0; i<MAX_LEN_OF_BA_REC_TABLE; i++)
+//	{
+//		NdisFreeSpinLock(&pAd->BATable.BARecEntry[i].RxReRingLock);
+//	}
+
+	DBGPRINT(RT_DEBUG_ERROR, ("<--- ReleaseAdapter\n"));
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Allocate memory for adapter control block.
+
+Arguments:
+    pAd					Pointer to our adapter
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_FAILURE
+	NDIS_STATUS_RESOURCES
+
+Note:
+========================================================================
+*/
+NDIS_STATUS AdapterBlockAllocateMemory(
+	IN PVOID	handle,
+	OUT	PVOID	*ppAd)
+{
+	PUSB_DEV	usb_dev;
+	POS_COOKIE	pObj = (POS_COOKIE) handle;
+
+
+	usb_dev = pObj->pUsb_Dev;
+
+	pObj->MLMEThr_pid		= THREAD_PID_INIT_VALUE;
+	pObj->RTUSBCmdThr_pid	= THREAD_PID_INIT_VALUE;
+
+	*ppAd = (PVOID)vmalloc(sizeof(RTMP_ADAPTER));
+
+	if (*ppAd)
+	{
+		NdisZeroMemory(*ppAd, sizeof(RTMP_ADAPTER));
+		((PRTMP_ADAPTER)*ppAd)->OS_Cookie = handle;
+		return (NDIS_STATUS_SUCCESS);
+	}
+	else
+	{
+		return (NDIS_STATUS_FAILURE);
+	}
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Create kernel threads & tasklets.
+
+Arguments:
+    *net_dev			Pointer to wireless net device interface
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_FAILURE
+
+Note:
+========================================================================
+*/
+NDIS_STATUS	 CreateThreads(
+	IN	struct net_device *net_dev)
+{
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) net_dev->priv;
+	POS_COOKIE pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pid_t pid_number = -1;
+
+	//init_MUTEX(&(pAd->usbdev_semaphore));
+
+	init_MUTEX_LOCKED(&(pAd->mlme_semaphore));
+	init_completion (&pAd->mlmeComplete);
+
+	init_MUTEX_LOCKED(&(pAd->RTUSBCmd_semaphore));
+	init_completion (&pAd->CmdQComplete);
+
+	init_MUTEX_LOCKED(&(pAd->RTUSBTimer_semaphore));
+	init_completion (&pAd->TimerQComplete);
+
+	// Creat MLME Thread
+	pObj->MLMEThr_pid= THREAD_PID_INIT_VALUE;
+	pid_number = kernel_thread(MlmeThread, pAd, CLONE_VM);
+	if (pid_number < 0)
+	{
+		printk (KERN_WARNING "%s: unable to start Mlme thread\n",pAd->net_dev->name);
+		return NDIS_STATUS_FAILURE;
+	}
+	pObj->MLMEThr_pid = GET_PID(pid_number);
+	// Wait for the thread to start
+	wait_for_completion(&(pAd->mlmeComplete));
+
+	// Creat Command Thread
+	pObj->RTUSBCmdThr_pid= THREAD_PID_INIT_VALUE;
+	pid_number = kernel_thread(RTUSBCmdThread, pAd, CLONE_VM);
+	if (pid_number < 0)
+	{
+		printk (KERN_WARNING "%s: unable to start RTUSBCmd thread\n",pAd->net_dev->name);
+		return NDIS_STATUS_FAILURE;
+	}
+	pObj->RTUSBCmdThr_pid = GET_PID(pid_number);
+	wait_for_completion(&(pAd->CmdQComplete));
+
+	pObj->TimerQThr_pid= THREAD_PID_INIT_VALUE;
+	pid_number = kernel_thread(TimerQThread, pAd, CLONE_VM);
+	if (pid_number < 0)
+	{
+		printk (KERN_WARNING "%s: unable to start TimerQThread\n",pAd->net_dev->name);
+		return NDIS_STATUS_FAILURE;
+	}
+	pObj->TimerQThr_pid = GET_PID(pid_number);
+	// Wait for the thread to start
+	wait_for_completion(&(pAd->TimerQComplete));
+
+	// Create receive tasklet
+	tasklet_init(&pObj->rx_done_task, rx_done_tasklet, (ULONG)pAd);
+	tasklet_init(&pObj->mgmt_dma_done_task, rt2870_mgmt_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac0_dma_done_task, rt2870_ac0_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac1_dma_done_task, rt2870_ac1_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac2_dma_done_task, rt2870_ac2_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac3_dma_done_task, rt2870_ac3_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->hcca_dma_done_task, rt2870_hcca_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->tbtt_task, tbtt_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->null_frame_complete_task, rt2870_null_frame_complete_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->rts_frame_complete_task, rt2870_rts_frame_complete_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->pspoll_frame_complete_task, rt2870_pspoll_frame_complete_tasklet, (unsigned long)pAd);
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+========================================================================
+Routine Description:
+ 	As STA's BSSID is a WC too, it uses shared key table.
+ 	This function write correct unicast TX key to ASIC WCID.
+ 	And we still make a copy in our MacTab.Content[BSSID_WCID].PairwiseKey.
+	Caller guarantee TKIP/AES always has keyidx = 0. (pairwise key)
+	Caller guarantee WEP calls this function when set Txkey,  default key index=0~3.
+
+Arguments:
+	pAd 					Pointer to our adapter
+	pKey					Pointer to the where the key stored
+
+Return Value:
+	NDIS_SUCCESS			Add key successfully
+
+Note:
+========================================================================
+*/
+VOID	RTMPAddBSSIDCipher(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	UCHAR				Aid,
+	IN	PNDIS_802_11_KEY	pKey,
+	IN  UCHAR   			CipherAlg)
+{
+	PUCHAR		pTxMic, pRxMic;
+	BOOLEAN 	bKeyRSC, bAuthenticator; // indicate the receive SC set by KeyRSC value
+//	UCHAR		CipherAlg;
+	UCHAR		i;
+	ULONG		WCIDAttri;
+	USHORT	 	offset;
+	UCHAR		KeyIdx, IVEIV[8];
+	UINT32		Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddBSSIDCipher==> Aid = %d\n",Aid));
+
+	// Bit 29 of Add-key KeyRSC
+	bKeyRSC 	   = (pKey->KeyIndex & 0x20000000) ? TRUE : FALSE;
+
+	// Bit 28 of Add-key Authenticator
+	bAuthenticator = (pKey->KeyIndex & 0x10000000) ? TRUE : FALSE;
+	KeyIdx = (UCHAR)pKey->KeyIndex&0xff;
+
+	if (KeyIdx > 4)
+		return;
+
+
+	if (pAd->MacTab.Content[Aid].PairwiseKey.CipherAlg == CIPHER_TKIP)
+	{	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		{
+			// for WPA-None Tx, Rx MIC is the same
+			pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+			pRxMic = pTxMic;
+		}
+		else if (bAuthenticator == TRUE)
+		{
+			pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+			pRxMic = (PUCHAR) (&pKey->KeyMaterial) + 24;
+		}
+		else
+		{
+			pRxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+			pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 24;
+		}
+
+		offset = PAIRWISE_KEY_TABLE_BASE + (Aid * HW_KEY_ENTRY_SIZE) + 0x10;
+		for (i=0; i<8; )
+		{
+			Value = *(pTxMic+i);
+			Value += (*(pTxMic+i+1)<<8);
+			Value += (*(pTxMic+i+2)<<16);
+			Value += (*(pTxMic+i+3)<<24);
+			RTUSBWriteMACRegister(pAd, offset+i, Value);
+			i+=4;
+		}
+
+		offset = PAIRWISE_KEY_TABLE_BASE + (Aid * HW_KEY_ENTRY_SIZE) + 0x18;
+		for (i=0; i<8; )
+		{
+			Value = *(pRxMic+i);
+			Value += (*(pRxMic+i+1)<<8);
+			Value += (*(pRxMic+i+2)<<16);
+			Value += (*(pRxMic+i+3)<<24);
+			RTUSBWriteMACRegister(pAd, offset+i, Value);
+			i+=4;
+		}
+
+		// Only Key lenth equal to TKIP key have these
+		NdisMoveMemory(pAd->MacTab.Content[Aid].PairwiseKey.RxMic, pRxMic, 8);
+		NdisMoveMemory(pAd->MacTab.Content[Aid].PairwiseKey.TxMic, pTxMic, 8);
+
+		DBGPRINT(RT_DEBUG_TRACE,
+				("	TxMIC  = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x \n",
+				pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],
+				pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+		DBGPRINT(RT_DEBUG_TRACE,
+				("	RxMIC  = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x \n",
+				pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],
+				pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+
+	// 2. Record Security Key.
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.KeyLen= (UCHAR)pKey->KeyLength;
+	NdisMoveMemory(pAd->MacTab.Content[Aid].PairwiseKey.Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+	// 3. Check RxTsc. And used to init to ASIC IV.
+	if (bKeyRSC == TRUE)
+		NdisMoveMemory(pAd->MacTab.Content[Aid].PairwiseKey.RxTsc, &pKey->KeyRSC, 6);
+	else
+		NdisZeroMemory(pAd->MacTab.Content[Aid].PairwiseKey.RxTsc, 6);
+
+	// 4. Init TxTsc to one based on WiFi WPA specs
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[0] = 1;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[1] = 0;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[2] = 0;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[3] = 0;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[4] = 0;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[5] = 0;
+
+	CipherAlg = pAd->MacTab.Content[Aid].PairwiseKey.CipherAlg;
+
+	offset = PAIRWISE_KEY_TABLE_BASE + (Aid * HW_KEY_ENTRY_SIZE);
+	RTUSBMultiWrite(pAd, (USHORT) offset, pKey->KeyMaterial,
+				((pKey->KeyLength == LEN_TKIP_KEY) ? 16 : (USHORT)pKey->KeyLength));
+
+	offset = SHARED_KEY_TABLE_BASE + (KeyIdx * HW_KEY_ENTRY_SIZE);
+	RTUSBMultiWrite(pAd, (USHORT) offset, pKey->KeyMaterial, (USHORT)pKey->KeyLength);
+
+	offset = PAIRWISE_IVEIV_TABLE_BASE + (Aid * HW_IVEIV_ENTRY_SIZE);
+	NdisZeroMemory(IVEIV, 8);
+
+	// IV/EIV
+	if ((CipherAlg == CIPHER_TKIP) ||
+		(CipherAlg == CIPHER_TKIP_NO_MIC) ||
+		(CipherAlg == CIPHER_AES))
+	{
+		IVEIV[3] = 0x20; // Eiv bit on. keyid always 0 for pairwise key
+	}
+	// default key idx needs to set.
+	// in TKIP/AES KeyIdx = 0 , WEP KeyIdx is default tx key.
+	else
+	{
+		IVEIV[3] |= (KeyIdx<< 6);
+	}
+	RTUSBMultiWrite(pAd, (USHORT) offset, IVEIV, 8);
+
+	// WCID Attribute UDF:3, BSSIdx:3, Alg:3, Keytable:1=PAIRWISE KEY, BSSIdx is 0
+	if ((CipherAlg == CIPHER_TKIP) ||
+		(CipherAlg == CIPHER_TKIP_NO_MIC) ||
+		(CipherAlg == CIPHER_AES))
+	{
+		WCIDAttri = (CipherAlg<<1)|SHAREDKEYTABLE;
+	}
+	else
+		WCIDAttri = (CipherAlg<<1)|SHAREDKEYTABLE;
+
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Aid* HW_WCID_ATTRI_SIZE);
+	RTUSBWriteMACRegister(pAd, offset, WCIDAttri);
+	RTUSBReadMACRegister(pAd, offset, &Value);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BSSID_WCID : offset = %x, WCIDAttri = %lx\n",
+			offset, WCIDAttri));
+
+	// pAddr
+	// Add Bssid mac address at linkup. not here.  check!
+	/*offset = MAC_WCID_BASE + (BSSID_WCID * HW_WCID_ENTRY_SIZE);
+	*for (i=0; i<MAC_ADDR_LEN; i++)
+	{
+		RTMP_IO_WRITE8(pAd, offset+i, pKey->BSSID[i]);
+	}
+	*/
+
+	DBGPRINT(RT_DEBUG_ERROR, ("AddBSSIDasWCIDEntry: Alg=%s, KeyLength = %d\n",
+			CipherName[CipherAlg], pKey->KeyLength));
+	DBGPRINT(RT_DEBUG_TRACE, ("Key [idx=%x] [KeyLen = %d]\n",
+			pKey->KeyIndex, pKey->KeyLength));
+	for(i=0; i<pKey->KeyLength; i++)
+		DBGPRINT_RAW(RT_DEBUG_TRACE,(" %x:", pKey->KeyMaterial[i]));
+	DBGPRINT(RT_DEBUG_TRACE,("	 \n"));
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+========================================================================
+Routine Description:
+    Get a received packet.
+
+Arguments:
+	pAd					device control block
+	pSaveRxD			receive descriptor information
+	*pbReschedule		need reschedule flag
+	*pRxPending			pending received packet flag
+
+Return Value:
+    the recieved packet
+
+Note:
+========================================================================
+*/
+#define RT2870_RXDMALEN_FIELD_SIZE			4
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER		pAd,
+	OUT		PRT28XX_RXD_STRUC	pSaveRxD,
+	OUT		BOOLEAN				*pbReschedule,
+	IN OUT	UINT32				*pRxPending)
+{
+	PRX_CONTEXT		pRxContext;
+	PNDIS_PACKET	pSkb;
+	PUCHAR			pData;
+	ULONG			ThisFrameLen;
+	ULONG			RxBufferLength;
+	PRXWI_STRUC		pRxWI;
+
+	pRxContext = &pAd->RxContext[pAd->NextRxBulkInReadIndex];
+	if ((pRxContext->Readable == FALSE) || (pRxContext->InUse == TRUE))
+		return NULL;
+
+	RxBufferLength = pRxContext->BulkInOffset - pAd->ReadPosition;
+	if (RxBufferLength < (RT2870_RXDMALEN_FIELD_SIZE + sizeof(RXWI_STRUC) + sizeof(RXINFO_STRUC)))
+	{
+		goto label_null;
+	}
+
+	pData = &pRxContext->TransferBuffer[pAd->ReadPosition]; /* 4KB */
+	// The RXDMA field is 4 bytes, now just use the first 2 bytes. The Length including the (RXWI + MSDU + Padding)
+	ThisFrameLen = *pData + (*(pData+1)<<8);
+    if (ThisFrameLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("BIRIdx(%d): RXDMALen is zero.[%ld], BulkInBufLen = %ld)\n",
+								pAd->NextRxBulkInReadIndex, ThisFrameLen, pRxContext->BulkInOffset));
+		goto label_null;
+	}
+	if ((ThisFrameLen&0x3) != 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("BIRIdx(%d): RXDMALen not multiple of 4.[%ld], BulkInBufLen = %ld)\n",
+								pAd->NextRxBulkInReadIndex, ThisFrameLen, pRxContext->BulkInOffset));
+		goto label_null;
+	}
+
+	if ((ThisFrameLen + 8)> RxBufferLength)	// 8 for (RT2870_RXDMALEN_FIELD_SIZE + sizeof(RXINFO_STRUC))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("BIRIdx(%d):FrameLen(0x%lx) outranges. BulkInLen=0x%lx, remaining RxBufLen=0x%lx, ReadPos=0x%lx\n",
+						pAd->NextRxBulkInReadIndex, ThisFrameLen, pRxContext->BulkInOffset, RxBufferLength, pAd->ReadPosition));
+
+		// error frame. finish this loop
+		goto label_null;
+	}
+
+	// skip USB frame length field
+	pData += RT2870_RXDMALEN_FIELD_SIZE;
+	pRxWI = (PRXWI_STRUC)pData;
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange(pData, TYPE_RXWI);
+#endif // RT_BIG_ENDIAN //
+	if (pRxWI->MPDUtotalByteCount > ThisFrameLen)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s():pRxWIMPDUtotalByteCount(%d) large than RxDMALen(%ld)\n",
+									__FUNCTION__, pRxWI->MPDUtotalByteCount, ThisFrameLen));
+		goto label_null;
+	}
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange(pData, TYPE_RXWI);
+#endif // RT_BIG_ENDIAN //
+
+	// allocate a rx packet
+	pSkb = dev_alloc_skb(ThisFrameLen);
+	if (pSkb == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("%s():Cannot Allocate sk buffer for this Bulk-In buffer!\n", __FUNCTION__));
+		goto label_null;
+	}
+
+	// copy the rx packet
+	memcpy(skb_put(pSkb, ThisFrameLen), pData, ThisFrameLen);
+	RTPKT_TO_OSPKT(pSkb)->dev = get_netdev_from_bssid(pAd, BSS0);
+	RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pSkb), PKTSRC_NDIS);
+
+	// copy RxD
+	*pSaveRxD = *(PRXINFO_STRUC)(pData + ThisFrameLen);
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pSaveRxD, TYPE_RXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// update next packet read position.
+	pAd->ReadPosition += (ThisFrameLen + RT2870_RXDMALEN_FIELD_SIZE + RXINFO_SIZE);	// 8 for (RT2870_RXDMALEN_FIELD_SIZE + sizeof(RXINFO_STRUC))
+
+	return pSkb;
+
+label_null:
+
+	return NULL;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Handle received packets.
+
+Arguments:
+	data				- URB information pointer
+
+Return Value:
+    None
+
+Note:
+========================================================================
+*/
+static void rx_done_tasklet(unsigned long data)
+{
+	purbb_t 			pUrb;
+	PRX_CONTEXT			pRxContext;
+	PRTMP_ADAPTER		pAd;
+	NTSTATUS			Status;
+	unsigned int		IrqFlags;
+
+	pUrb		= (purbb_t)data;
+	pRxContext	= (PRX_CONTEXT)pUrb->context;
+	pAd 		= pRxContext->pAd;
+	Status = pUrb->status;
+
+
+	RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+	pRxContext->InUse = FALSE;
+	pRxContext->IRPPending = FALSE;
+	pRxContext->BulkInOffset += pUrb->actual_length;
+	//NdisInterlockedDecrement(&pAd->PendingRx);
+	pAd->PendingRx--;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		pAd->BulkInComplete++;
+		pAd->NextRxBulkInPosition = 0;
+		if (pRxContext->BulkInOffset)	// As jan's comment, it may bulk-in success but size is zero.
+		{
+			pRxContext->Readable = TRUE;
+			INC_RING_INDEX(pAd->NextRxBulkInIndex, RX_RING_SIZE);
+		}
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+	}
+	else	 // STATUS_OTHER
+	{
+		pAd->BulkInCompleteFail++;
+		// Still read this packet although it may comtain wrong bytes.
+		pRxContext->Readable = FALSE;
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+		// Parsing all packets. because after reset, the index will reset to all zero.
+		if ((!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+									fRTMP_ADAPTER_BULKIN_RESET |
+									fRTMP_ADAPTER_HALT_IN_PROGRESS |
+									fRTMP_ADAPTER_NIC_NOT_EXIST))))
+		{
+
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk In Failed. Status=%d, BIIdx=0x%x, BIRIdx=0x%x, actual_length= 0x%x\n",
+							Status, pAd->NextRxBulkInIndex, pAd->NextRxBulkInReadIndex, pRxContext->pUrb->actual_length));
+
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_IN, NULL, 0);
+		}
+	}
+
+	ASSERT((pRxContext->InUse == pRxContext->IRPPending));
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+    {
+		// If the driver is in ATE mode and Rx frame is set into here.
+		if (pAd->ContinBulkIn == TRUE)
+		{
+			RTUSBBulkReceive(pAd);
+		}
+	}
+	else
+#endif // RALINK_ATE //
+	RTUSBBulkReceive(pAd);
+
+	return;
+
+}
+
+
+static void rt2870_mgmt_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER 	pAd;
+	PTX_CONTEXT		pMLMEContext;
+	int				index;
+	PNDIS_PACKET	pPacket;
+	purbb_t			pUrb;
+	NTSTATUS		Status;
+	unsigned long	IrqFlags;
+
+
+	pUrb			= (purbb_t)data;
+	pMLMEContext	= (PTX_CONTEXT)pUrb->context;
+	pAd 			= pMLMEContext->pAd;
+	Status			= pUrb->status;
+	index 			= pMLMEContext->SelfIdx;
+
+	ASSERT((pAd->MgmtRing.TxDmaIdx == index));
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+
+	if (Status != USB_ST_NOERROR)
+	{
+		//Bulk-Out fail status handle
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out MLME Failed, Status=%d!\n", Status));
+			// TODO: How to handle about the MLMEBulkOut failed issue. Need to resend the mgmt pkt?
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+		}
+	}
+
+	pAd->BulkOutPending[MGMTPIPEIDX] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+	RTMP_IRQ_LOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+	// Reset MLME context flags
+	pMLMEContext->IRPPending = FALSE;
+	pMLMEContext->InUse = FALSE;
+	pMLMEContext->bWaitingBulkOut = FALSE;
+	pMLMEContext->BulkOutSize = 0;
+
+	pPacket = pAd->MgmtRing.Cell[index].pNdisPacket;
+	pAd->MgmtRing.Cell[index].pNdisPacket = NULL;
+
+	// Increase MgmtRing Index
+	INC_RING_INDEX(pAd->MgmtRing.TxDmaIdx, MGMT_RING_SIZE);
+	pAd->MgmtRing.TxSwFreeIdx++;
+	RTMP_IRQ_UNLOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+
+	// No-matter success or fail, we free the mgmt packet.
+	if (pPacket)
+		RTMPFreeNdisPacket(pAd, pPacket);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET) &&
+			((pAd->bulkResetPipeid & BULKOUT_MGMT_RESET_FLAG) == BULKOUT_MGMT_RESET_FLAG))
+		{	// For Mgmt Bulk-Out failed, ignore it now.
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{
+
+			// Always call Bulk routine, even reset bulk.
+			// The protectioon of rest bulk should be in BulkOut routine
+			if (pAd->MgmtRing.TxSwFreeIdx < MGMT_RING_SIZE /* pMLMEContext->bWaitingBulkOut == TRUE */)
+			{
+				RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+			}
+				RTUSBKickBulkOut(pAd);
+			}
+		}
+
+}
+
+
+static void rt2870_hcca_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 4;
+	purbb_t				pUrb;
+
+
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("--->hcca_dma_done_tasklet\n"));
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("<---hcca_dma_done_tasklet\n"));
+
+		return;
+}
+
+
+static void rt2870_ac3_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 3;
+	purbb_t				pUrb;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL<<3);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+
+		return;
+}
+
+
+static void rt2870_ac2_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 2;
+	purbb_t				pUrb;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL<<2);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+		return;
+}
+
+
+static void rt2870_ac1_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 1;
+	purbb_t				pUrb;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL<<1);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+
+	return;
+}
+
+
+static void rt2870_ac0_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 0;
+	purbb_t				pUrb;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*  ((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+
+	return;
+
+}
+
+
+static void rt2870_null_frame_complete_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER	pAd;
+	PTX_CONTEXT		pNullContext;
+	purbb_t			pUrb;
+	NTSTATUS		Status;
+	unsigned long	irqFlag;
+
+
+	pUrb			= (purbb_t)data;
+	pNullContext	= (PTX_CONTEXT)pUrb->context;
+	pAd 			= pNullContext->pAd;
+	Status 			= pUrb->status;
+
+	// Reset Null frame context flags
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], irqFlag);
+	pNullContext->IRPPending 	= FALSE;
+	pNullContext->InUse 		= FALSE;
+	pAd->BulkOutPending[0] = FALSE;
+	pAd->watchDogTxPendingCnt[0] = 0;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else	// STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out Null Frame Failed, ReasonCode=%d!\n", Status));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+		}
+	}
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+
+}
+
+
+static void rt2870_rts_frame_complete_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER	pAd;
+	PTX_CONTEXT		pRTSContext;
+	purbb_t			pUrb;
+	NTSTATUS		Status;
+	unsigned long	irqFlag;
+
+
+	pUrb		= (purbb_t)data;
+	pRTSContext	= (PTX_CONTEXT)pUrb->context;
+	pAd			= pRTSContext->pAd;
+	Status		= pUrb->status;
+
+	// Reset RTS frame context flags
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], irqFlag);
+	pRTSContext->IRPPending = FALSE;
+	pRTSContext->InUse		= FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else	// STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out RTS Frame Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+		}
+	}
+
+	RTMP_SEM_LOCK(&pAd->BulkOutLock[pRTSContext->BulkOutPipeId]);
+	pAd->BulkOutPending[pRTSContext->BulkOutPipeId] = FALSE;
+	RTMP_SEM_UNLOCK(&pAd->BulkOutLock[pRTSContext->BulkOutPipeId]);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+
+}
+
+
+static void rt2870_pspoll_frame_complete_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER	pAd;
+	PTX_CONTEXT		pPsPollContext;
+	purbb_t			pUrb;
+	NTSTATUS		Status;
+
+
+	pUrb			= (purbb_t)data;
+	pPsPollContext	= (PTX_CONTEXT)pUrb->context;
+	pAd				= pPsPollContext->pAd;
+	Status			= pUrb->status;
+
+	// Reset PsPoll context flags
+	pPsPollContext->IRPPending	= FALSE;
+	pPsPollContext->InUse		= FALSE;
+	pAd->watchDogTxPendingCnt[0] = 0;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else // STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out PSPoll Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+	}
+
+	RTMP_SEM_LOCK(&pAd->BulkOutLock[0]);
+	pAd->BulkOutPending[0] = FALSE;
+	RTMP_SEM_UNLOCK(&pAd->BulkOutLock[0]);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+
+}
+
+
+static void rt2870_dataout_complete_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	purbb_t				pUrb;
+	POS_COOKIE			pObj;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId;
+	NTSTATUS			Status;
+	unsigned long		IrqFlags;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+	pObj 			= (POS_COOKIE) pAd->OS_Cookie;
+	Status			= pUrb->status;
+
+	// Store BulkOut PipeId
+	BulkOutPipeId = pHTTXContext->BulkOutPipeId;
+	pAd->BulkOutDataOneSecCount++;
+
+	//DBGPRINT(RT_DEBUG_LOUD, ("Done-B(%d):I=0x%lx, CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", BulkOutPipeId, in_interrupt(), pHTTXContext->CurWritePosition,
+	//		pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+	pHTTXContext->IRPPending = FALSE;
+	pAd->watchDogTxPendingCnt[BulkOutPipeId] = 0;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		pAd->BulkOutComplete++;
+
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+		pAd->Counters8023.GoodTransmits++;
+		//RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+		FREE_HTTX_RING(pAd, BulkOutPipeId, pHTTXContext);
+		//RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+
+	}
+	else	// STATUS_OTHER
+	{
+		PUCHAR	pBuf;
+
+		pAd->BulkOutCompleteOther++;
+
+		pBuf = &pHTTXContext->TransferBuffer->field.WirelessPacket[pHTTXContext->NextBulkOutPosition];
+
+		if (!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+									fRTMP_ADAPTER_HALT_IN_PROGRESS |
+									fRTMP_ADAPTER_NIC_NOT_EXIST |
+									fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = BulkOutPipeId;
+			pAd->bulkResetReq[BulkOutPipeId] = pAd->BulkOutReq;
+		}
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("BulkOutDataPacket failed: ReasonCode=%d!\n", Status));
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("\t>>BulkOut Req=0x%lx, Complete=0x%lx, Other=0x%lx\n", pAd->BulkOutReq, pAd->BulkOutComplete, pAd->BulkOutCompleteOther));
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("\t>>BulkOut Header:%x %x %x %x %x %x %x %x\n", pBuf[0], pBuf[1], pBuf[2], pBuf[3], pBuf[4], pBuf[5], pBuf[6], pBuf[7]));
+		//DBGPRINT_RAW(RT_DEBUG_ERROR, (">>BulkOutCompleteCancel=0x%x, BulkOutCompleteOther=0x%x\n", pAd->BulkOutCompleteCancel, pAd->BulkOutCompleteOther));
+
+	}
+
+	//
+	// bInUse = TRUE, means some process are filling TX data, after that must turn on bWaitingBulkOut
+	// bWaitingBulkOut = TRUE, means the TX data are waiting for bulk out.
+	//
+	//RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	if ((pHTTXContext->ENextBulkOutPosition != pHTTXContext->CurWritePosition) &&
+		(pHTTXContext->ENextBulkOutPosition != (pHTTXContext->CurWritePosition+8)) &&
+		!RTUSB_TEST_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_FRAG << BulkOutPipeId)))
+	{
+		// Indicate There is data avaliable
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	//RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protection of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+}
+
+/* End of 2870_rtmp_init.c */
diff --git a/drivers/staging/rt2870/common/action.c b/drivers/staging/rt2870/common/action.c
new file mode 100644
index 00000000000..3a48a7f5e2b
--- /dev/null
+++ b/drivers/staging/rt2870/common/action.c
@@ -0,0 +1,1046 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+	action.c
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+	Jan Lee		2006	  	created for rt2860
+ */
+
+#include "../rt_config.h"
+#include "action.h"
+
+
+static VOID ReservedAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+/*
+    ==========================================================================
+    Description:
+        association state machine init, including state transition and timer init
+    Parameters:
+        S - pointer to the association state machine
+    Note:
+        The state machine looks like the following
+
+                                    ASSOC_IDLE
+        MT2_MLME_DISASSOC_REQ    mlme_disassoc_req_action
+        MT2_PEER_DISASSOC_REQ    peer_disassoc_action
+        MT2_PEER_ASSOC_REQ       drop
+        MT2_PEER_REASSOC_REQ     drop
+        MT2_CLS3ERR              cls3err_action
+    ==========================================================================
+ */
+VOID ActionStateMachineInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S, (STATE_MACHINE_FUNC *)Trans, MAX_ACT_STATE, MAX_ACT_MSG, (STATE_MACHINE_FUNC)Drop, ACT_IDLE, ACT_MACHINE_BASE);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_SPECTRUM_CATE, (STATE_MACHINE_FUNC)PeerSpectrumAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_QOS_CATE, (STATE_MACHINE_FUNC)PeerQOSAction);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_DLS_CATE, (STATE_MACHINE_FUNC)ReservedAction);
+#ifdef QOS_DLS_SUPPORT
+		StateMachineSetAction(S, ACT_IDLE, MT2_PEER_DLS_CATE, (STATE_MACHINE_FUNC)PeerDLSAction);
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_BA_CATE, (STATE_MACHINE_FUNC)PeerBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_HT_CATE, (STATE_MACHINE_FUNC)PeerHTAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_ADD_BA_CATE, (STATE_MACHINE_FUNC)MlmeADDBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_ORI_DELBA_CATE, (STATE_MACHINE_FUNC)MlmeDELBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_REC_DELBA_CATE, (STATE_MACHINE_FUNC)MlmeDELBAAction);
+#endif // DOT11_N_SUPPORT //
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_PUBLIC_CATE, (STATE_MACHINE_FUNC)PeerPublicAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_RM_CATE, (STATE_MACHINE_FUNC)PeerRMAction);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_QOS_CATE, (STATE_MACHINE_FUNC)MlmeQOSAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_DLS_CATE, (STATE_MACHINE_FUNC)MlmeDLSAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_ACT_INVALID, (STATE_MACHINE_FUNC)MlmeInvalidAction);
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID MlmeADDBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+
+{
+	MLME_ADDBA_REQ_STRUCT *pInfo;
+	UCHAR           Addr[6];
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG		Idx;
+	FRAME_ADDBA_REQ  Frame;
+	ULONG		FrameLen;
+	BA_ORI_ENTRY			*pBAEntry = NULL;
+
+	pInfo = (MLME_ADDBA_REQ_STRUCT *)Elem->Msg;
+	NdisZeroMemory(&Frame, sizeof(FRAME_ADDBA_REQ));
+
+	if(MlmeAddBAReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr))
+	{
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - MlmeADDBAAction() allocate memory failed \n"));
+			return;
+		}
+		// 1. find entry
+		Idx = pAd->MacTab.Content[pInfo->Wcid].BAOriWcidArray[pInfo->TID];
+		if (Idx == 0)
+		{
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() can't find BAOriEntry \n"));
+			return;
+		}
+		else
+		{
+			pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (ADHOC_ON(pAd))
+				ActHeaderInit(pAd, &Frame.Hdr, pInfo->pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#ifdef QOS_DLS_SUPPORT
+			if (pAd->MacTab.Content[pInfo->Wcid].ValidAsDls)
+				ActHeaderInit(pAd, &Frame.Hdr, pInfo->pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#endif // QOS_DLS_SUPPORT //
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pInfo->pAddr);
+
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		Frame.Category = CATEGORY_BA;
+		Frame.Action = ADDBA_REQ;
+		Frame.BaParm.AMSDUSupported = 0;
+		Frame.BaParm.BAPolicy = IMMED_BA;
+		Frame.BaParm.TID = pInfo->TID;
+		Frame.BaParm.BufSize = pInfo->BaBufSize;
+		Frame.Token = pInfo->Token;
+		Frame.TimeOutValue = pInfo->TimeOutValue;
+		Frame.BaStartSeq.field.FragNum = 0;
+		Frame.BaStartSeq.field.StartSeq = pAd->MacTab.Content[pInfo->Wcid].TxSeq[pInfo->TID];
+
+		*(USHORT *)(&Frame.BaParm) = cpu2le16(*(USHORT *)(&Frame.BaParm));
+		Frame.TimeOutValue = cpu2le16(Frame.TimeOutValue);
+		Frame.BaStartSeq.word = cpu2le16(Frame.BaStartSeq.word);
+
+		MakeOutgoingFrame(pOutBuffer,		   &FrameLen,
+		              sizeof(FRAME_ADDBA_REQ), &Frame,
+		              END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("BA - Send ADDBA request. StartSeq = %x,  FrameLen = %ld. BufSize = %d\n", Frame.BaStartSeq.field.StartSeq, FrameLen, Frame.BaParm.BufSize));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        send DELBA and delete BaEntry if any
+    Parametrs:
+        Elem - MLME message MLME_DELBA_REQ_STRUCT
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDELBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_DELBA_REQ_STRUCT *pInfo;
+	PUCHAR         pOutBuffer = NULL;
+	PUCHAR		   pOutBuffer2 = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG		Idx;
+	FRAME_DELBA_REQ  Frame;
+	ULONG		FrameLen;
+	FRAME_BAR	FrameBar;
+
+	pInfo = (MLME_DELBA_REQ_STRUCT *)Elem->Msg;
+	// must send back DELBA
+	NdisZeroMemory(&Frame, sizeof(FRAME_DELBA_REQ));
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeDELBAAction(), Initiator(%d) \n", pInfo->Initiator));
+
+	if(MlmeDelBAReqSanity(pAd, Elem->Msg, Elem->MsgLen))
+	{
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeDELBAAction() allocate memory failed 1. \n"));
+			return;
+		}
+
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer2);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_ERROR, ("BA - MlmeDELBAAction() allocate memory failed 2. \n"));
+			return;
+		}
+
+		// SEND BAR (Send BAR to refresh peer reordering buffer.)
+		Idx = pAd->MacTab.Content[pInfo->Wcid].BAOriWcidArray[pInfo->TID];
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			BarHeaderInit(pAd, &FrameBar, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+		FrameBar.StartingSeq.field.FragNum = 0; // make sure sequence not clear in DEL funciton.
+		FrameBar.StartingSeq.field.StartSeq = pAd->MacTab.Content[pInfo->Wcid].TxSeq[pInfo->TID]; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.TID = pInfo->TID; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.ACKPolicy = IMMED_BA; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.Compressed = 1; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.MTID = 0; // make sure sequence not clear in DEL funciton.
+
+		MakeOutgoingFrame(pOutBuffer2,				&FrameLen,
+					  sizeof(FRAME_BAR),	  &FrameBar,
+					  END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer2, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer2);
+		DBGPRINT(RT_DEBUG_TRACE,("BA - MlmeDELBAAction() . Send BAR to refresh peer reordering buffer \n"));
+
+		// SEND DELBA FRAME
+		FrameLen = 0;
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (ADHOC_ON(pAd))
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#ifdef QOS_DLS_SUPPORT
+			if (pAd->MacTab.Content[pInfo->Wcid].ValidAsDls)
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#endif // QOS_DLS_SUPPORT //
+				ActHeaderInit(pAd, &Frame.Hdr,  pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->MacTab.Content[pInfo->Wcid].Addr);
+		}
+#endif // CONFIG_STA_SUPPORT //
+		Frame.Category = CATEGORY_BA;
+		Frame.Action = DELBA;
+		Frame.DelbaParm.Initiator = pInfo->Initiator;
+		Frame.DelbaParm.TID = pInfo->TID;
+		Frame.ReasonCode = 39; // Time Out
+		*(USHORT *)(&Frame.DelbaParm) = cpu2le16(*(USHORT *)(&Frame.DelbaParm));
+		Frame.ReasonCode = cpu2le16(Frame.ReasonCode);
+
+		MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+		              sizeof(FRAME_DELBA_REQ),    &Frame,
+		              END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("BA - MlmeDELBAAction() . 3 DELBA sent. Initiator(%d)\n", pInfo->Initiator));
+    	}
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID MlmeQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+VOID MlmeDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+VOID MlmeInvalidAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	//PUCHAR		   pOutBuffer = NULL;
+	//Return the receiving frame except the MSB of category filed set to 1.  7.3.1.11
+}
+
+VOID PeerQOSAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+#ifdef QOS_DLS_SUPPORT
+VOID PeerDLSAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	switch(Action)
+	{
+		case ACTION_DLS_REQUEST:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsReqAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+
+		case ACTION_DLS_RESPONSE:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsRspAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+
+		case ACTION_DLS_TEARDOWN:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsTearDownAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+	}
+}
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	switch(Action)
+	{
+		case ADDBA_REQ:
+			PeerAddBAReqAction(pAd,Elem);
+			break;
+		case ADDBA_RESP:
+			PeerAddBARspAction(pAd,Elem);
+			break;
+		case DELBA:
+			PeerDelBAAction(pAd,Elem);
+			break;
+	}
+}
+
+
+#ifdef DOT11N_DRAFT3
+
+#ifdef CONFIG_STA_SUPPORT
+VOID StaPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Bss2040Coexist)
+{
+	BSS_2040_COEXIST_IE		BssCoexist;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+
+	BssCoexist.word = Bss2040Coexist;
+	// AP asks Station to return a 20/40 BSS Coexistence mgmt frame.  So we first starts a scan, then send back 20/40 BSS Coexistence mgmt frame
+	if ((BssCoexist.field.InfoReq == 1) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SCAN_2040)))
+	{
+		// Clear record first.  After scan , will update those bit and send back to transmiter.
+		pAd->CommonCfg.BSSCoexist2040.field.InfoReq = 1;
+		pAd->CommonCfg.BSSCoexist2040.field.Intolerant40 = 0;
+		pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq = 0;
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_2040_BSS_COEXIST);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+	}
+}
+
+
+/*
+Description : Build Intolerant Channel Rerpot from Trigger event table.
+return : how many bytes copied.
+*/
+ULONG BuildIntolerantChannelRep(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    PUCHAR  pDest)
+{
+	ULONG			FrameLen = 0;
+	ULONG			ReadOffset = 0;
+	UCHAR			i;
+	UCHAR			LastRegClass = 0xff;
+	PUCHAR			pLen;
+
+	for ( i = 0;i < MAX_TRIGGER_EVENT;i++)
+	{
+		if (pAd->CommonCfg.TriggerEventTab.EventA[i].bValid == TRUE)
+		{
+			if (pAd->CommonCfg.TriggerEventTab.EventA[i].RegClass == LastRegClass)
+			{
+				*(pDest + ReadOffset) = (UCHAR)pAd->CommonCfg.TriggerEventTab.EventA[i].Channel;
+				*pLen++;
+				ReadOffset++;
+				FrameLen++;
+			}
+			else
+			{
+				*(pDest + ReadOffset) = IE_2040_BSS_INTOLERANT_REPORT;  // IE
+				*(pDest + ReadOffset + 1) = 2;	// Len = RegClass byte + channel byte.
+				pLen = pDest + ReadOffset + 1;
+				LastRegClass = pAd->CommonCfg.TriggerEventTab.EventA[i].RegClass;
+				*(pDest + ReadOffset + 2) = LastRegClass;	// Len = RegClass byte + channel byte.
+				*(pDest + ReadOffset + 3) = (UCHAR)pAd->CommonCfg.TriggerEventTab.EventA[i].Channel;
+				FrameLen += 4;
+				ReadOffset += 4;
+			}
+
+		}
+	}
+	return FrameLen;
+}
+
+
+/*
+Description : Send 20/40 BSS Coexistence Action frame If one trigger event is triggered.
+*/
+VOID Send2040CoexistAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS 	NStatus;
+	FRAME_ACTION_HDR	Frame;
+	ULONG			FrameLen;
+	ULONG			IntolerantChaRepLen;
+
+	IntolerantChaRepLen = 0;
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ACT - Send2040CoexistAction() allocate memory failed \n"));
+		return;
+	}
+	ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[Wcid].Addr, pAd->CommonCfg.Bssid);
+	Frame.Category = CATEGORY_PUBLIC;
+	Frame.Action = ACTION_BSS_2040_COEXIST;
+
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+				  sizeof(FRAME_ACTION_HDR),	  &Frame,
+				  END_OF_ARGS);
+
+	*(pOutBuffer + FrameLen) = pAd->CommonCfg.BSSCoexist2040.word;
+	FrameLen++;
+
+	if (bAddIntolerantCha == TRUE)
+		IntolerantChaRepLen = BuildIntolerantChannelRep(pAd, pOutBuffer + FrameLen);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen + IntolerantChaRepLen);
+	DBGPRINT(RT_DEBUG_ERROR,("ACT - Send2040CoexistAction( BSSCoexist2040 = 0x%x )  \n", pAd->CommonCfg.BSSCoexist2040.word));
+
+}
+
+
+/*
+	==========================================================================
+	Description:
+	After scan, Update 20/40 BSS Coexistence IE and send out.
+	According to 802.11n D3.03 11.14.10
+
+	Parameters:
+	==========================================================================
+ */
+VOID Update2040CoexistFrameAndNotify(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha)
+{
+	BSS_2040_COEXIST_IE	OldValue;
+
+	OldValue.word = pAd->CommonCfg.BSSCoexist2040.word;
+	if ((pAd->CommonCfg.TriggerEventTab.EventANo > 0) || (pAd->CommonCfg.TriggerEventTab.EventBCountDown > 0))
+		pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq = 1;
+
+	// Need to check !!!!
+	// How STA will set Intolerant40 if implementation dependent. Now we don't set this bit first.!!!!!
+	// So Only check BSS20WidthReq change.
+	if (OldValue.field.BSS20WidthReq != pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq)
+	{
+		Send2040CoexistAction(pAd, Wcid, bAddIntolerantCha);
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+BOOLEAN ChannelSwitchSanityCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary)
+{
+	UCHAR		i;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	if ((NewChannel > 7) && (Secondary == 1))
+		return FALSE;
+
+	if ((NewChannel < 5) && (Secondary == 3))
+		return FALSE;
+
+	// 0. Check if new channel is in the channellist.
+	for (i = 0;i < pAd->ChannelListNum;i++)
+	{
+		if (pAd->ChannelList[i].Channel == NewChannel)
+		{
+			break;
+		}
+	}
+
+	if (i == pAd->ChannelListNum)
+		return FALSE;
+
+	return TRUE;
+}
+
+
+VOID ChannelSwitchAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary)
+{
+	UCHAR		BBPValue = 0;
+	ULONG		MACValue;
+
+	DBGPRINT(RT_DEBUG_TRACE,("SPECTRUM - ChannelSwitchAction(NewChannel = %d , Secondary = %d)  \n", NewChannel, Secondary));
+
+	if (ChannelSwitchSanityCheck(pAd, Wcid, NewChannel, Secondary) == FALSE)
+		return;
+
+	// 1.  Switches to BW = 20.
+	if (Secondary == 0)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue&= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		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" ));
+		}
+		pAd->CommonCfg.BBPCurrentBW = BW_20;
+		pAd->CommonCfg.Channel = NewChannel;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel,FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->MacTab.Content[Wcid].HTPhyMode.field.BW = 0;
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!20MHz   !!! \n" ));
+	}
+	// 1.  Switches to BW = 40 And Station supports BW = 40.
+	else if (((Secondary == 1) || (Secondary == 3)) && (pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth == 1))
+	{
+		pAd->CommonCfg.Channel = NewChannel;
+
+		if (Secondary == 1)
+		{
+			// Secondary above.
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel + 2;
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &MACValue);
+			MACValue &= 0xfe;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, MACValue);
+			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);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPValue);
+			BBPValue&= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Lower LINK UP !!! Control Channel at Below. Central = %d \n", pAd->CommonCfg.CentralChannel ));
+		}
+		else
+		{
+			// Secondary below.
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 2;
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &MACValue);
+			MACValue &= 0xfe;
+			MACValue |= 0x1;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, MACValue);
+			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);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPValue);
+			BBPValue&= (~0x20);
+			BBPValue|= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Upper LINK UP !!! Control Channel at UpperCentral = %d \n", pAd->CommonCfg.CentralChannel ));
+		}
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+		pAd->MacTab.Content[Wcid].HTPhyMode.field.BW = 1;
+	}
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+VOID PeerPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+#ifdef DOT11N_DRAFT3
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+#endif // DOT11N_DRAFT3 //
+
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+#ifdef DOT11N_DRAFT3
+	switch(Action)
+	{
+		case ACTION_BSS_2040_COEXIST:	// Format defined in IEEE 7.4.7a.1 in 11n Draf3.03
+			{
+				//UCHAR	BssCoexist;
+				BSS_2040_COEXIST_ELEMENT		*pCoexistInfo;
+				BSS_2040_COEXIST_IE 			*pBssCoexistIe;
+				BSS_2040_INTOLERANT_CH_REPORT	*pIntolerantReport = NULL;
+
+				if (Elem->MsgLen <= (LENGTH_802_11 + sizeof(BSS_2040_COEXIST_ELEMENT)) )
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("ACTION - 20/40 BSS Coexistence Management Frame length too short! len = %ld!\n", Elem->MsgLen));
+					break;
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("ACTION - 20/40 BSS Coexistence Management action----> \n"));
+				hex_dump("CoexistenceMgmtFrame", Elem->Msg, Elem->MsgLen);
+
+
+				pCoexistInfo = (BSS_2040_COEXIST_ELEMENT *) &Elem->Msg[LENGTH_802_11+2];
+				//hex_dump("CoexistInfo", (PUCHAR)pCoexistInfo, sizeof(BSS_2040_COEXIST_ELEMENT));
+				if (Elem->MsgLen >= (LENGTH_802_11 + sizeof(BSS_2040_COEXIST_ELEMENT) + sizeof(BSS_2040_INTOLERANT_CH_REPORT)))
+				{
+					pIntolerantReport = (BSS_2040_INTOLERANT_CH_REPORT *)((PUCHAR)pCoexistInfo + sizeof(BSS_2040_COEXIST_ELEMENT));
+				}
+				//hex_dump("IntolerantReport ", (PUCHAR)pIntolerantReport, sizeof(BSS_2040_INTOLERANT_CH_REPORT));
+
+				pBssCoexistIe = (BSS_2040_COEXIST_IE *)(&pCoexistInfo->BssCoexistIe);
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					if (INFRA_ON(pAd))
+					{
+						StaPublicAction(pAd, pCoexistInfo);
+					}
+				}
+#endif // CONFIG_STA_SUPPORT //
+
+			}
+			break;
+	}
+
+#endif // DOT11N_DRAFT3 //
+
+}
+
+
+static VOID ReservedAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR Category;
+
+	if (Elem->MsgLen <= LENGTH_802_11)
+	{
+		return;
+	}
+
+	Category = Elem->Msg[LENGTH_802_11];
+	DBGPRINT(RT_DEBUG_TRACE,("Rcv reserved category(%d) Action Frame\n", Category));
+	hex_dump("Reserved Action Frame", &Elem->Msg[0], Elem->MsgLen);
+}
+
+VOID PeerRMAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	return;
+}
+
+#ifdef DOT11_N_SUPPORT
+static VOID respond_ht_information_exchange_action(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen;
+	FRAME_HT_INFO	HTINFOframe, *pFrame;
+	UCHAR   		*pAddr;
+
+
+	// 2. Always send back ADDBA Response
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ACTION - respond_ht_information_exchange_action() allocate memory failed \n"));
+		return;
+	}
+
+	// get RA
+	pFrame = (FRAME_HT_INFO *) &Elem->Msg[0];
+	pAddr = pFrame->Hdr.Addr2;
+
+	NdisZeroMemory(&HTINFOframe, sizeof(FRAME_HT_INFO));
+	// 2-1. Prepare ADDBA Response frame.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (ADHOC_ON(pAd))
+			ActHeaderInit(pAd, &HTINFOframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+			ActHeaderInit(pAd, &HTINFOframe.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAddr);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	HTINFOframe.Category = CATEGORY_HT;
+	HTINFOframe.Action = HT_INFO_EXCHANGE;
+	HTINFOframe.HT_Info.Request = 0;
+	HTINFOframe.HT_Info.Forty_MHz_Intolerant = pAd->CommonCfg.HtCapability.HtCapInfo.Forty_Mhz_Intolerant;
+	HTINFOframe.HT_Info.STA_Channel_Width	 = pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth;
+
+	MakeOutgoingFrame(pOutBuffer,					&FrameLen,
+					  sizeof(FRAME_HT_INFO),	&HTINFOframe,
+					  END_OF_ARGS);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+
+#ifdef DOT11N_DRAFT3
+VOID SendNotifyBWActionFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR  Wcid,
+	IN UCHAR apidx)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS 	NStatus;
+	FRAME_ACTION_HDR	Frame;
+	ULONG			FrameLen;
+	PUCHAR			pAddr1;
+
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ACT - SendNotifyBWAction() allocate memory failed \n"));
+		return;
+	}
+
+	if (Wcid == MCAST_WCID)
+		pAddr1 = &BROADCAST_ADDR[0];
+	else
+		pAddr1 = pAd->MacTab.Content[Wcid].Addr;
+	ActHeaderInit(pAd, &Frame.Hdr, pAddr1, pAd->ApCfg.MBSSID[apidx].Bssid, pAd->ApCfg.MBSSID[apidx].Bssid);
+
+	Frame.Category = CATEGORY_HT;
+	Frame.Action = NOTIFY_BW_ACTION;
+
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+				  sizeof(FRAME_ACTION_HDR),	  &Frame,
+				  END_OF_ARGS);
+
+	*(pOutBuffer + FrameLen) = pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth;
+	FrameLen++;
+
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	DBGPRINT(RT_DEBUG_TRACE,("ACT - SendNotifyBWAction(NotifyBW= %d)!\n", pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth));
+
+}
+#endif // DOT11N_DRAFT3 //
+
+
+VOID PeerHTAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	switch(Action)
+	{
+		case NOTIFY_BW_ACTION:
+			DBGPRINT(RT_DEBUG_TRACE,("ACTION - HT Notify Channel bandwidth action----> \n"));
+#ifdef CONFIG_STA_SUPPORT
+			if(pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+			{
+				// Note, this is to patch DIR-1353 AP. When the AP set to Wep, it will use legacy mode. But AP still keeps
+				// sending BW_Notify Action frame, and cause us to linkup and linkdown.
+				// In legacy mode, don't need to parse HT action frame.
+				DBGPRINT(RT_DEBUG_TRACE,("ACTION -Ignore HT Notify Channel BW when link as legacy mode. BW = %d---> \n",
+								Elem->Msg[LENGTH_802_11+2] ));
+				break;
+			}
+#endif // CONFIG_STA_SUPPORT //
+
+			if (Elem->Msg[LENGTH_802_11+2] == 0)	// 7.4.8.2. if value is 1, keep the same as supported channel bandwidth.
+				pAd->MacTab.Content[Elem->Wcid].HTPhyMode.field.BW = 0;
+
+			break;
+
+		case SMPS_ACTION:
+			// 7.3.1.25
+ 			DBGPRINT(RT_DEBUG_TRACE,("ACTION - SMPS action----> \n"));
+			if (((Elem->Msg[LENGTH_802_11+2]&0x1) == 0))
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_ENABLE;
+			}
+			else if (((Elem->Msg[LENGTH_802_11+2]&0x2) == 0))
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_STATIC;
+			}
+			else
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_DYNAMIC;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,("Aid(%d) MIMO PS = %d\n", Elem->Wcid, pAd->MacTab.Content[Elem->Wcid].MmpsMode));
+			// rt2860c : add something for smps change.
+			break;
+
+		case SETPCO_ACTION:
+			break;
+
+		case MIMO_CHA_MEASURE_ACTION:
+			break;
+
+		case HT_INFO_EXCHANGE:
+			{
+				HT_INFORMATION_OCTET	*pHT_info;
+
+				pHT_info = (HT_INFORMATION_OCTET *) &Elem->Msg[LENGTH_802_11+2];
+    				// 7.4.8.10
+    				DBGPRINT(RT_DEBUG_TRACE,("ACTION - HT Information Exchange action----> \n"));
+    				if (pHT_info->Request)
+    				{
+    					respond_ht_information_exchange_action(pAd, Elem);
+    				}
+			}
+    			break;
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID ORIBATimerTimeout(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	MAC_TABLE_ENTRY	*pEntry;
+	INT			i, total;
+//	FRAME_BAR			FrameBar;
+//	ULONG			FrameLen;
+//	NDIS_STATUS 	NStatus;
+//	PUCHAR			pOutBuffer = NULL;
+//	USHORT			Sequence;
+	UCHAR			TID;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	total = pAd->MacTab.Size * NUM_OF_TID;
+
+	for (i = 1; ((i <MAX_LEN_OF_BA_ORI_TABLE) && (total > 0)) ; i++)
+	{
+		if  (pAd->BATable.BAOriEntry[i].ORI_BA_Status == Originator_Done)
+		{
+			pEntry = &pAd->MacTab.Content[pAd->BATable.BAOriEntry[i].Wcid];
+			TID = pAd->BATable.BAOriEntry[i].TID;
+
+			ASSERT(pAd->BATable.BAOriEntry[i].Wcid < MAX_LEN_OF_MAC_TABLE);
+		}
+		total --;
+	}
+}
+
+
+VOID SendRefreshBAR(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry)
+{
+	FRAME_BAR		FrameBar;
+	ULONG			FrameLen;
+	NDIS_STATUS 	NStatus;
+	PUCHAR			pOutBuffer = NULL;
+	USHORT			Sequence;
+	UCHAR			i, TID;
+	USHORT			idx;
+	BA_ORI_ENTRY	*pBAEntry;
+
+	for (i = 0; i <NUM_OF_TID; i++)
+	{
+		idx = pEntry->BAOriWcidArray[i];
+		if (idx == 0)
+		{
+			continue;
+		}
+		pBAEntry = &pAd->BATable.BAOriEntry[idx];
+
+		if  (pBAEntry->ORI_BA_Status == Originator_Done)
+		{
+			TID = pBAEntry->TID;
+
+			ASSERT(pBAEntry->Wcid < MAX_LEN_OF_MAC_TABLE);
+
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if(NStatus != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() allocate memory failed \n"));
+				return;
+			}
+
+			Sequence = pEntry->TxSeq[TID];
+
+
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				BarHeaderInit(pAd, &FrameBar, pEntry->Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+			FrameBar.StartingSeq.field.FragNum = 0; // make sure sequence not clear in DEL function.
+			FrameBar.StartingSeq.field.StartSeq = Sequence; // make sure sequence not clear in DEL funciton.
+			FrameBar.BarControl.TID = TID; // make sure sequence not clear in DEL funciton.
+
+			MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+							  sizeof(FRAME_BAR),	  &FrameBar,
+							  END_OF_ARGS);
+			//if (!(CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_RALINK_CHIPSET)))
+			if (1)	// Now we always send BAR.
+			{
+				//MiniportMMRequestUnlock(pAd, 0, pOutBuffer, FrameLen);
+				MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			}
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID ActHeaderInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN OUT PHEADER_802_11 pHdr80211,
+    IN PUCHAR Addr1,
+    IN PUCHAR Addr2,
+    IN PUCHAR Addr3)
+{
+    NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+    pHdr80211->FC.Type = BTYPE_MGMT;
+    pHdr80211->FC.SubType = SUBTYPE_ACTION;
+
+    COPY_MAC_ADDR(pHdr80211->Addr1, Addr1);
+	COPY_MAC_ADDR(pHdr80211->Addr2, Addr2);
+    COPY_MAC_ADDR(pHdr80211->Addr3, Addr3);
+}
+
+VOID BarHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PFRAME_BAR pCntlBar,
+	IN PUCHAR pDA,
+	IN PUCHAR pSA)
+{
+//	USHORT	Duration;
+
+	NdisZeroMemory(pCntlBar, sizeof(FRAME_BAR));
+	pCntlBar->FC.Type = BTYPE_CNTL;
+	pCntlBar->FC.SubType = SUBTYPE_BLOCK_ACK_REQ;
+   	pCntlBar->BarControl.MTID = 0;
+	pCntlBar->BarControl.Compressed = 1;
+	pCntlBar->BarControl.ACKPolicy = 0;
+
+
+	pCntlBar->Duration = 16 + RTMPCalcDuration(pAd, RATE_1, sizeof(FRAME_BA));
+
+	COPY_MAC_ADDR(pCntlBar->Addr1, pDA);
+	COPY_MAC_ADDR(pCntlBar->Addr2, pSA);
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Insert Category and action code into the action frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. category code of the frame.
+		4. action code of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID InsertActField(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 Category,
+	IN UINT8 ActCode)
+{
+	ULONG TempLen;
+
+	MakeOutgoingFrame(	pFrameBuf,		&TempLen,
+						1,				&Category,
+						1,				&ActCode,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
diff --git a/drivers/staging/rt2870/common/action.h b/drivers/staging/rt2870/common/action.h
new file mode 100644
index 00000000000..ce3877dce81
--- /dev/null
+++ b/drivers/staging/rt2870/common/action.h
@@ -0,0 +1,68 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	Paul Lin	04-06-15		Initial
+*/
+
+#ifndef	__ACTION_H__
+#define	__ACTION_H__
+
+typedef struct PACKED __HT_INFO_OCTET
+{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	Reserved:5;
+	UCHAR 	STA_Channel_Width:1;
+	UCHAR	Forty_MHz_Intolerant:1;
+	UCHAR	Request:1;
+#else
+	UCHAR	Request:1;
+	UCHAR	Forty_MHz_Intolerant:1;
+	UCHAR 	STA_Channel_Width:1;
+	UCHAR	Reserved:5;
+#endif
+} HT_INFORMATION_OCTET;
+
+
+typedef struct PACKED __FRAME_HT_INFO
+{
+	HEADER_802_11   		Hdr;
+	UCHAR					Category;
+	UCHAR					Action;
+	HT_INFORMATION_OCTET	HT_Info;
+}   FRAME_HT_INFO, *PFRAME_HT_INFO;
+
+#endif /* __ACTION_H__ */
+
+
diff --git a/drivers/staging/rt2870/common/ba_action.c b/drivers/staging/rt2870/common/ba_action.c
new file mode 100644
index 00000000000..95addb20bce
--- /dev/null
+++ b/drivers/staging/rt2870/common/ba_action.c
@@ -0,0 +1,1798 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+
+#ifdef DOT11_N_SUPPORT
+
+#include "../rt_config.h"
+
+
+
+#define BA_ORI_INIT_SEQ		(pEntry->TxSeq[TID]) //1			// inital sequence number of BA session
+
+#define ORI_SESSION_MAX_RETRY	8
+#define ORI_BA_SESSION_TIMEOUT	(2000)	// ms
+#define REC_BA_SESSION_IDLE_TIMEOUT	(1000)	// ms
+
+#define REORDERING_PACKET_TIMEOUT		((100 * HZ)/1000)	// system ticks -- 100 ms
+#define MAX_REORDERING_PACKET_TIMEOUT	((3000 * HZ)/1000)	// system ticks -- 100 ms
+
+#define RESET_RCV_SEQ		(0xFFFF)
+
+static void ba_mpdu_blk_free(PRTMP_ADAPTER pAd, struct reordering_mpdu *mpdu_blk);
+
+
+BA_ORI_ENTRY *BATableAllocOriEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx);
+
+BA_REC_ENTRY *BATableAllocRecEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx);
+
+VOID BAOriSessionSetupTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID BARecSessionIdleTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+
+BUILD_TIMER_FUNCTION(BAOriSessionSetupTimeout);
+BUILD_TIMER_FUNCTION(BARecSessionIdleTimeout);
+
+#define ANNOUNCE_REORDERING_PACKET(_pAd, _mpdu_blk)	\
+			Announce_Reordering_Packet(_pAd, _mpdu_blk);
+
+VOID BA_MaxWinSizeReasign(
+	IN PRTMP_ADAPTER	pAd,
+	IN MAC_TABLE_ENTRY  *pEntryPeer,
+	OUT UCHAR			*pWinSize)
+{
+	UCHAR MaxSize;
+
+
+	if (pAd->MACVersion >= RALINK_2883_VERSION) // 3*3
+	{
+		if (pAd->MACVersion >= RALINK_3070_VERSION)
+		{
+			if (pEntryPeer->WepStatus != Ndis802_11EncryptionDisabled)
+				MaxSize = 7; // for non-open mode
+			else
+				MaxSize = 13;
+		}
+		else
+			MaxSize = 31;
+	}
+	else if (pAd->MACVersion >= RALINK_2880E_VERSION) // 2880 e
+	{
+		if (pEntryPeer->WepStatus != Ndis802_11EncryptionDisabled)
+			MaxSize = 7; // for non-open mode
+		else
+			MaxSize = 13;
+	}
+	else
+		MaxSize = 7;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ba> Win Size = %d, Max Size = %d\n",
+			*pWinSize, MaxSize));
+
+	if ((*pWinSize) > MaxSize)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ba> reassign max win size from %d to %d\n",
+				*pWinSize, MaxSize));
+
+		*pWinSize = MaxSize;
+	}
+}
+
+void Announce_Reordering_Packet(IN PRTMP_ADAPTER			pAd,
+								IN struct reordering_mpdu	*mpdu)
+{
+	PNDIS_PACKET    pPacket;
+
+	pPacket = mpdu->pPacket;
+
+	if (mpdu->bAMSDU)
+	{
+		ASSERT(0);
+		BA_Reorder_AMSDU_Annnounce(pAd, pPacket);
+	}
+	else
+	{
+		//
+		// pass this 802.3 packet to upper layer or forward this packet to WM directly
+		//
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pPacket, RTMP_GET_PACKET_IF(pPacket));
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+/*
+ * Insert a reordering mpdu into sorted linked list by sequence no.
+ */
+BOOLEAN ba_reordering_mpdu_insertsorted(struct reordering_list *list, struct reordering_mpdu *mpdu)
+{
+
+	struct reordering_mpdu **ppScan = &list->next;
+
+	while (*ppScan != NULL)
+	{
+		if (SEQ_SMALLER((*ppScan)->Sequence, mpdu->Sequence, MAXSEQ))
+		{
+			ppScan = &(*ppScan)->next;
+		}
+		else if ((*ppScan)->Sequence == mpdu->Sequence)
+		{
+			/* give up this duplicated frame */
+			return(FALSE);
+		}
+		else
+		{
+			/* find position */
+			break;
+		}
+	}
+
+	mpdu->next = *ppScan;
+	*ppScan = mpdu;
+	list->qlen++;
+	return TRUE;
+}
+
+
+/*
+ * caller lock critical section if necessary
+ */
+static inline void ba_enqueue(struct reordering_list *list, struct reordering_mpdu *mpdu_blk)
+{
+	list->qlen++;
+	mpdu_blk->next = list->next;
+	list->next = mpdu_blk;
+}
+
+/*
+ * caller lock critical section if necessary
+ */
+static inline struct reordering_mpdu * ba_dequeue(struct reordering_list *list)
+{
+	struct reordering_mpdu *mpdu_blk = NULL;
+
+	ASSERT(list);
+
+		if (list->qlen)
+		{
+			list->qlen--;
+			mpdu_blk = list->next;
+			if (mpdu_blk)
+			{
+				list->next = mpdu_blk->next;
+				mpdu_blk->next = NULL;
+			}
+		}
+	return mpdu_blk;
+}
+
+
+static inline struct reordering_mpdu  *ba_reordering_mpdu_dequeue(struct reordering_list *list)
+{
+	return(ba_dequeue(list));
+}
+
+
+static inline struct reordering_mpdu  *ba_reordering_mpdu_probe(struct reordering_list *list)
+	{
+	ASSERT(list);
+
+		return(list->next);
+	}
+
+
+/*
+ * free all resource for reordering mechanism
+ */
+void ba_reordering_resource_release(PRTMP_ADAPTER pAd)
+{
+	BA_TABLE        *Tab;
+	PBA_REC_ENTRY   pBAEntry;
+	struct reordering_mpdu *mpdu_blk;
+	int i;
+
+	Tab = &pAd->BATable;
+
+	/* I.  release all pending reordering packet */
+	NdisAcquireSpinLock(&pAd->BATabLock);
+	for (i = 0; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		pBAEntry = &Tab->BARecEntry[i];
+		if (pBAEntry->REC_BA_Status != Recipient_NONE)
+		{
+			while ((mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list)))
+			{
+				ASSERT(mpdu_blk->pPacket);
+				RELEASE_NDIS_PACKET(pAd, mpdu_blk->pPacket, NDIS_STATUS_FAILURE);
+				ba_mpdu_blk_free(pAd, mpdu_blk);
+			}
+		}
+	}
+	NdisReleaseSpinLock(&pAd->BATabLock);
+
+	ASSERT(pBAEntry->list.qlen == 0);
+	/* II. free memory of reordering mpdu table */
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+	os_free_mem(pAd, pAd->mpdu_blk_pool.mem);
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+}
+
+
+
+/*
+ * Allocate all resource for reordering mechanism
+ */
+BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num)
+{
+	int     i;
+	PUCHAR  mem;
+	struct reordering_mpdu *mpdu_blk;
+	struct reordering_list *freelist;
+
+	/* allocate spinlock */
+	NdisAllocateSpinLock(&pAd->mpdu_blk_pool.lock);
+
+	/* initialize freelist */
+	freelist = &pAd->mpdu_blk_pool.freelist;
+	freelist->next = NULL;
+	freelist->qlen = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Allocate %d memory for BA reordering\n", (UINT32)(num*sizeof(struct reordering_mpdu))));
+
+	/* allocate number of mpdu_blk memory */
+	os_alloc_mem(pAd, (PUCHAR *)&mem, (num*sizeof(struct reordering_mpdu)));
+
+	pAd->mpdu_blk_pool.mem = mem;
+
+	if (mem == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Can't Allocate Memory for BA Reordering\n"));
+		return(FALSE);
+	}
+
+	/* build mpdu_blk free list */
+	for (i=0; i<num; i++)
+	{
+		/* get mpdu_blk */
+		mpdu_blk = (struct reordering_mpdu *) mem;
+		/* initial mpdu_blk */
+		NdisZeroMemory(mpdu_blk, sizeof(struct reordering_mpdu));
+		/* next mpdu_blk */
+		mem += sizeof(struct reordering_mpdu);
+		/* insert mpdu_blk into freelist */
+		ba_enqueue(freelist, mpdu_blk);
+	}
+
+	return(TRUE);
+}
+
+//static int blk_count=0; // sample take off, no use
+
+static struct reordering_mpdu *ba_mpdu_blk_alloc(PRTMP_ADAPTER pAd)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+	mpdu_blk = ba_dequeue(&pAd->mpdu_blk_pool.freelist);
+	if (mpdu_blk)
+	{
+//		blk_count++;
+		/* reset mpdu_blk */
+		NdisZeroMemory(mpdu_blk, sizeof(struct reordering_mpdu));
+	}
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+	return mpdu_blk;
+}
+
+static void ba_mpdu_blk_free(PRTMP_ADAPTER pAd, struct reordering_mpdu *mpdu_blk)
+{
+	ASSERT(mpdu_blk);
+
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+//	blk_count--;
+	ba_enqueue(&pAd->mpdu_blk_pool.freelist, mpdu_blk);
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+}
+
+
+static USHORT ba_indicate_reordering_mpdus_in_order(
+												   IN PRTMP_ADAPTER    pAd,
+												   IN PBA_REC_ENTRY    pBAEntry,
+												   IN USHORT           StartSeq)
+{
+	struct reordering_mpdu *mpdu_blk;
+	USHORT  LastIndSeq = RESET_RCV_SEQ;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+	while ((mpdu_blk = ba_reordering_mpdu_probe(&pBAEntry->list)))
+		{
+			/* find in-order frame */
+		if (!SEQ_STEPONE(mpdu_blk->Sequence, StartSeq, MAXSEQ))
+			{
+				break;
+			}
+			/* dequeue in-order frame from reodering list */
+			mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list);
+			/* pass this frame up */
+		ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+		/* move to next sequence */
+			StartSeq = mpdu_blk->Sequence;
+		LastIndSeq = StartSeq;
+		/* free mpdu_blk */
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+	}
+
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+
+	/* update last indicated sequence */
+	return LastIndSeq;
+}
+
+static void ba_indicate_reordering_mpdus_le_seq(
+											   IN PRTMP_ADAPTER    pAd,
+											   IN PBA_REC_ENTRY    pBAEntry,
+											   IN USHORT           Sequence)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+	while ((mpdu_blk = ba_reordering_mpdu_probe(&pBAEntry->list)))
+		{
+			/* find in-order frame */
+		if ((mpdu_blk->Sequence == Sequence) || SEQ_SMALLER(mpdu_blk->Sequence, Sequence, MAXSEQ))
+		{
+			/* dequeue in-order frame from reodering list */
+			mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list);
+			/* pass this frame up */
+			ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+			/* free mpdu_blk */
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+		}
+		else
+			{
+				break;
+			}
+	}
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+}
+
+
+static void ba_refresh_reordering_mpdus(
+									   IN PRTMP_ADAPTER    pAd,
+									   PBA_REC_ENTRY       pBAEntry)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+			/* dequeue in-order frame from reodering list */
+	while ((mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list)))
+	{
+			/* pass this frame up */
+		ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+
+		pBAEntry->LastIndSeq = mpdu_blk->Sequence;
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+
+		/* update last indicated sequence */
+	}
+	ASSERT(pBAEntry->list.qlen == 0);
+	pBAEntry->LastIndSeq = RESET_RCV_SEQ;
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+}
+
+
+//static
+void ba_flush_reordering_timeout_mpdus(
+									IN PRTMP_ADAPTER    pAd,
+									IN PBA_REC_ENTRY    pBAEntry,
+									IN ULONG            Now32)
+
+{
+	USHORT Sequence;
+
+//	if ((RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+REORDERING_PACKET_TIMEOUT)) &&
+//		 (pBAEntry->list.qlen > ((pBAEntry->BAWinSize*7)/8))) //||
+//		(RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(10*REORDERING_PACKET_TIMEOUT))) &&
+//		 (pBAEntry->list.qlen > (pBAEntry->BAWinSize/8)))
+	if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(MAX_REORDERING_PACKET_TIMEOUT/6)))
+		 &&(pBAEntry->list.qlen > 1)
+		)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("timeout[%d] (%08lx-%08lx = %d > %d): %x, flush all!\n ", pBAEntry->list.qlen, Now32, (pBAEntry->LastIndSeqAtTimer),
+			   (int)((long) Now32 - (long)(pBAEntry->LastIndSeqAtTimer)), MAX_REORDERING_PACKET_TIMEOUT,
+			   pBAEntry->LastIndSeq));
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		pBAEntry->LastIndSeqAtTimer = Now32;
+	}
+	else
+	if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(REORDERING_PACKET_TIMEOUT)))
+		&& (pBAEntry->list.qlen > 0)
+	   )
+		{
+    		//
+		// force LastIndSeq to shift to LastIndSeq+1
+    		//
+    		Sequence = (pBAEntry->LastIndSeq+1) & MAXSEQ;
+    		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, Sequence);
+    		pBAEntry->LastIndSeqAtTimer = Now32;
+			pBAEntry->LastIndSeq = Sequence;
+    		//
+    		// indicate in-order mpdus
+    		//
+    		Sequence = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, Sequence);
+    		if (Sequence != RESET_RCV_SEQ)
+    		{
+    			pBAEntry->LastIndSeq = Sequence;
+    		}
+
+	}
+#if 0
+	else if (
+			 (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(MAX_REORDERING_PACKET_TIMEOUT))) &&
+			  (pBAEntry->list.qlen > 1))
+			)
+		{
+		DBGPRINT(RT_DEBUG_TRACE,("timeout[%d] (%lx-%lx = %d > %d): %x\n ", pBAEntry->list.qlen, Now32, (pBAEntry->LastIndSeqAtTimer),
+			   (int)((long) Now32 - (long)(pBAEntry->LastIndSeqAtTimer)), MAX_REORDERING_PACKET_TIMEOUT,
+			   pBAEntry->LastIndSeq));
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+			pBAEntry->LastIndSeqAtTimer = Now32;
+				}
+#endif
+}
+
+
+/*
+ * generate ADDBA request to
+ * set up BA agreement
+ */
+VOID BAOriSessionSetUp(
+					  IN PRTMP_ADAPTER    pAd,
+					  IN MAC_TABLE_ENTRY  *pEntry,
+					  IN UCHAR            TID,
+					  IN USHORT           TimeOut,
+					  IN ULONG            DelayTime,
+					  IN BOOLEAN          isForced)
+
+{
+	//MLME_ADDBA_REQ_STRUCT	AddbaReq;
+	BA_ORI_ENTRY            *pBAEntry = NULL;
+	USHORT                  Idx;
+	BOOLEAN                 Cancelled;
+
+	if ((pAd->CommonCfg.BACapability.field.AutoBA != TRUE)  &&  (isForced == FALSE))
+		return;
+
+	// if this entry is limited to use legacy tx mode, it doesn't generate BA.
+	if (RTMPStaFixedTxMode(pAd, pEntry) != FIXED_TXMODE_HT)
+		return;
+
+	if ((pEntry->BADeclineBitmap & (1<<TID)) && (isForced == FALSE))
+	{
+		// try again after 3 secs
+		DelayTime = 3000;
+//		printk("DeCline BA from Peer\n");
+//		return;
+	}
+
+
+	Idx = pEntry->BAOriWcidArray[TID];
+	if (Idx == 0)
+	{
+		// allocate a BA session
+		pBAEntry = BATableAllocOriEntry(pAd, &Idx);
+		if (pBAEntry == NULL)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ADDBA - MlmeADDBAAction() allocate BA session failed \n"));
+			return;
+		}
+	}
+	else
+	{
+		pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+	}
+
+	if (pBAEntry->ORI_BA_Status >= Originator_WaitRes)
+	{
+		return;
+	}
+
+	pEntry->BAOriWcidArray[TID] = Idx;
+
+	// Initialize BA session
+	pBAEntry->ORI_BA_Status = Originator_WaitRes;
+	pBAEntry->Wcid = pEntry->Aid;
+	pBAEntry->BAWinSize = pAd->CommonCfg.BACapability.field.RxBAWinLimit;
+	pBAEntry->Sequence = BA_ORI_INIT_SEQ;
+	pBAEntry->Token = 1;	// (2008-01-21) Jan Lee recommends it - this token can't be 0
+	pBAEntry->TID = TID;
+	pBAEntry->TimeOutValue = TimeOut;
+	pBAEntry->pAdapter = pAd;
+
+	if (!(pEntry->TXBAbitmap & (1<<TID)))
+	{
+		RTMPInitTimer(pAd, &pBAEntry->ORIBATimer, GET_TIMER_FUNCTION(BAOriSessionSetupTimeout), pBAEntry, FALSE);
+	}
+	else
+		RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+
+	// set timer to send ADDBA request
+	RTMPSetTimer(&pBAEntry->ORIBATimer, DelayTime);
+}
+
+VOID BAOriSessionAdd(
+			IN PRTMP_ADAPTER    pAd,
+					IN MAC_TABLE_ENTRY  *pEntry,
+			IN PFRAME_ADDBA_RSP pFrame)
+{
+	BA_ORI_ENTRY  *pBAEntry = NULL;
+	BOOLEAN       Cancelled;
+	UCHAR         TID;
+	USHORT        Idx;
+	PUCHAR          pOutBuffer2 = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG           FrameLen;
+	FRAME_BAR       FrameBar;
+
+	TID = pFrame->BaParm.TID;
+	Idx = pEntry->BAOriWcidArray[TID];
+	pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+
+	// Start fill in parameters.
+	if ((Idx !=0) && (pBAEntry->TID == TID) && (pBAEntry->ORI_BA_Status == Originator_WaitRes))
+	{
+		pBAEntry->BAWinSize = min(pBAEntry->BAWinSize, ((UCHAR)pFrame->BaParm.BufSize));
+		BA_MaxWinSizeReasign(pAd, pEntry, &pBAEntry->BAWinSize);
+
+		pBAEntry->TimeOutValue = pFrame->TimeOutValue;
+		pBAEntry->ORI_BA_Status = Originator_Done;
+		// reset sequence number
+		pBAEntry->Sequence = BA_ORI_INIT_SEQ;
+		// Set Bitmap flag.
+		pEntry->TXBAbitmap |= (1<<TID);
+				RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+
+		pBAEntry->ORIBATimer.TimerValue = 0;	//pFrame->TimeOutValue;
+
+		DBGPRINT(RT_DEBUG_TRACE,("%s : TXBAbitmap = %x, BAWinSize = %d, TimeOut = %ld\n", __FUNCTION__, pEntry->TXBAbitmap,
+								 pBAEntry->BAWinSize, pBAEntry->ORIBATimer.TimerValue));
+
+		// SEND BAR ;
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer2);  //Get an unused nonpaged memory
+		if (NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - BAOriSessionAdd() allocate memory failed \n"));
+			return;
+		}
+
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			BarHeaderInit(pAd, &FrameBar, pAd->MacTab.Content[pBAEntry->Wcid].Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+		FrameBar.StartingSeq.field.FragNum = 0;	// make sure sequence not clear in DEL function.
+		FrameBar.StartingSeq.field.StartSeq = pBAEntry->Sequence; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.TID = pBAEntry->TID; // make sure sequence not clear in DEL funciton.
+		MakeOutgoingFrame(pOutBuffer2,              &FrameLen,
+						  sizeof(FRAME_BAR),      &FrameBar,
+					  END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer2, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer2);
+
+
+		if (pBAEntry->ORIBATimer.TimerValue)
+			RTMPSetTimer(&pBAEntry->ORIBATimer, pBAEntry->ORIBATimer.TimerValue); // in mSec
+	}
+}
+
+BOOLEAN BARecSessionAdd(
+					   IN PRTMP_ADAPTER    pAd,
+					   IN MAC_TABLE_ENTRY  *pEntry,
+					   IN PFRAME_ADDBA_REQ pFrame)
+{
+	BA_REC_ENTRY            *pBAEntry = NULL;
+	BOOLEAN                 Status = TRUE;
+	BOOLEAN                 Cancelled;
+	USHORT                  Idx;
+	UCHAR                   TID;
+	UCHAR                   BAWinSize;
+	//UINT32                  Value;
+	//UINT                    offset;
+
+
+	ASSERT(pEntry);
+
+	// find TID
+	TID = pFrame->BaParm.TID;
+
+	BAWinSize = min(((UCHAR)pFrame->BaParm.BufSize), (UCHAR)pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+
+	// Intel patch
+	if (BAWinSize == 0)
+	{
+		BAWinSize = 64;
+	}
+
+	Idx = pEntry->BARecWcidArray[TID];
+
+
+	if (Idx == 0)
+	{
+		pBAEntry = BATableAllocRecEntry(pAd, &Idx);
+	}
+	else
+	{
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+		// flush all pending reordering mpdus
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s(%ld): Idx = %d, BAWinSize(req %d) = %d\n", __FUNCTION__, pAd->BATable.numAsRecipient, Idx,
+							 pFrame->BaParm.BufSize, BAWinSize));
+
+	// Start fill in parameters.
+	if (pBAEntry != NULL)
+	{
+		ASSERT(pBAEntry->list.qlen == 0);
+
+		pBAEntry->REC_BA_Status = Recipient_HandleRes;
+		pBAEntry->BAWinSize = BAWinSize;
+		pBAEntry->Wcid = pEntry->Aid;
+		pBAEntry->TID = TID;
+		pBAEntry->TimeOutValue = pFrame->TimeOutValue;
+		pBAEntry->REC_BA_Status = Recipient_Accept;
+		// initial sequence number
+		pBAEntry->LastIndSeq = RESET_RCV_SEQ; //pFrame->BaStartSeq.field.StartSeq;
+
+		printk("Start Seq = %08x\n",  pFrame->BaStartSeq.field.StartSeq);
+
+		if (pEntry->RXBAbitmap & (1<<TID))
+		{
+			RTMPCancelTimer(&pBAEntry->RECBATimer, &Cancelled);
+		}
+		else
+		{
+			RTMPInitTimer(pAd, &pBAEntry->RECBATimer, GET_TIMER_FUNCTION(BARecSessionIdleTimeout), pBAEntry, TRUE);
+		}
+
+#if 0	// for debugging
+		RTMPSetTimer(&pBAEntry->RECBATimer, REC_BA_SESSION_IDLE_TIMEOUT);
+#endif
+
+		// Set Bitmap flag.
+		pEntry->RXBAbitmap |= (1<<TID);
+		pEntry->BARecWcidArray[TID] = Idx;
+
+		pEntry->BADeclineBitmap &= ~(1<<TID);
+
+		// Set BA session mask in WCID table.
+		RT28XX_ADD_BA_SESSION_TO_ASIC(pAd, pEntry->Aid, TID);
+
+		DBGPRINT(RT_DEBUG_TRACE,("MACEntry[%d]RXBAbitmap = 0x%x. BARecWcidArray=%d\n",
+				pEntry->Aid, pEntry->RXBAbitmap, pEntry->BARecWcidArray[TID]));
+	}
+	else
+	{
+		Status = FALSE;
+		DBGPRINT(RT_DEBUG_TRACE,("Can't Accept ADDBA for %02x:%02x:%02x:%02x:%02x:%02x TID = %d\n",
+				PRINT_MAC(pEntry->Addr), TID));
+	}
+	return(Status);
+}
+
+
+BA_REC_ENTRY *BATableAllocRecEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx)
+{
+	int             i;
+	BA_REC_ENTRY    *pBAEntry = NULL;
+
+
+	NdisAcquireSpinLock(&pAd->BATabLock);
+
+	if (pAd->BATable.numAsRecipient >= MAX_BARECI_SESSION)
+	{
+		printk("BA Recipeint Session (%ld) > %d\n", pAd->BATable.numAsRecipient,
+			MAX_BARECI_SESSION);
+		goto done;
+	}
+
+	// reserve idx 0 to identify BAWcidArray[TID] as empty
+	for (i=1; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		pBAEntry =&pAd->BATable.BARecEntry[i];
+		if ((pBAEntry->REC_BA_Status == Recipient_NONE))
+		{
+			// get one
+			pAd->BATable.numAsRecipient++;
+			pBAEntry->REC_BA_Status = Recipient_USED;
+			*Idx = i;
+			break;
+		}
+	}
+
+done:
+	NdisReleaseSpinLock(&pAd->BATabLock);
+	return pBAEntry;
+}
+
+BA_ORI_ENTRY *BATableAllocOriEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx)
+{
+	int             i;
+	BA_ORI_ENTRY    *pBAEntry = NULL;
+
+	NdisAcquireSpinLock(&pAd->BATabLock);
+
+	if (pAd->BATable.numAsOriginator >= (MAX_LEN_OF_BA_ORI_TABLE))
+	{
+		goto done;
+	}
+
+	// reserve idx 0 to identify BAWcidArray[TID] as empty
+	for (i=1; i<MAX_LEN_OF_BA_ORI_TABLE; i++)
+	{
+		pBAEntry =&pAd->BATable.BAOriEntry[i];
+		if ((pBAEntry->ORI_BA_Status == Originator_NONE))
+		{
+			// get one
+			pAd->BATable.numAsOriginator++;
+			pBAEntry->ORI_BA_Status = Originator_USED;
+			pBAEntry->pAdapter = pAd;
+			*Idx = i;
+			break;
+		}
+	}
+
+done:
+	NdisReleaseSpinLock(&pAd->BATabLock);
+	return pBAEntry;
+}
+
+
+VOID BATableFreeOriEntry(
+						IN  PRTMP_ADAPTER   pAd,
+						IN  ULONG           Idx)
+{
+	BA_ORI_ENTRY    *pBAEntry = NULL;
+	MAC_TABLE_ENTRY *pEntry;
+
+
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_ORI_TABLE))
+		return;
+
+	pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+
+	if (pBAEntry->ORI_BA_Status != Originator_NONE)
+	{
+		pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+		pEntry->BAOriWcidArray[pBAEntry->TID] = 0;
+
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+		if (pBAEntry->ORI_BA_Status == Originator_Done)
+		{
+		 	pEntry->TXBAbitmap &= (~(1<<(pBAEntry->TID) ));
+			DBGPRINT(RT_DEBUG_TRACE, ("BATableFreeOriEntry numAsOriginator= %ld\n", pAd->BATable.numAsRecipient));
+			// Erase Bitmap flag.
+		}
+
+		ASSERT(pAd->BATable.numAsOriginator != 0);
+
+		pAd->BATable.numAsOriginator -= 1;
+
+		pBAEntry->ORI_BA_Status = Originator_NONE;
+		pBAEntry->Token = 0;
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+
+
+VOID BATableFreeRecEntry(
+						IN  PRTMP_ADAPTER   pAd,
+						IN  ULONG           Idx)
+{
+	BA_REC_ENTRY    *pBAEntry = NULL;
+	MAC_TABLE_ENTRY *pEntry;
+
+
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_REC_TABLE))
+		return;
+
+	pBAEntry =&pAd->BATable.BARecEntry[Idx];
+
+	if (pBAEntry->REC_BA_Status != Recipient_NONE)
+	{
+		pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+		pEntry->BARecWcidArray[pBAEntry->TID] = 0;
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+
+		ASSERT(pAd->BATable.numAsRecipient != 0);
+
+		pAd->BATable.numAsRecipient -= 1;
+
+		pBAEntry->REC_BA_Status = Recipient_NONE;
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+
+
+VOID BAOriSessionTearDown(
+						 IN OUT  PRTMP_ADAPTER   pAd,
+						 IN      UCHAR           Wcid,
+						 IN      UCHAR           TID,
+						 IN      BOOLEAN         bPassive,
+						 IN      BOOLEAN         bForceSend)
+{
+	ULONG           Idx = 0;
+	BA_ORI_ENTRY    *pBAEntry;
+	BOOLEAN         Cancelled;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+	{
+		return;
+	}
+
+	//
+	// Locate corresponding BA Originator Entry in BA Table with the (pAddr,TID).
+	//
+	Idx = pAd->MacTab.Content[Wcid].BAOriWcidArray[TID];
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_ORI_TABLE))
+	{
+		if (bForceSend == TRUE)
+		{
+			// force send specified TID DelBA
+			MLME_DELBA_REQ_STRUCT   DelbaReq;
+			MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+			NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+			NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+			COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+			DelbaReq.Wcid = Wcid;
+			DelbaReq.TID = TID;
+			DelbaReq.Initiator = ORIGINATOR;
+#if 1
+			Elem->MsgLen  = sizeof(DelbaReq);
+			NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+			MlmeDELBAAction(pAd, Elem);
+			kfree(Elem);
+#else
+			MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+			RT28XX_MLME_HANDLER(pAd);
+#endif
+		}
+
+		return;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s===>Wcid=%d.TID=%d \n", __FUNCTION__, Wcid, TID));
+
+	pBAEntry = &pAd->BATable.BAOriEntry[Idx];
+	DBGPRINT(RT_DEBUG_TRACE,("\t===>Idx = %ld, Wcid=%d.TID=%d, ORI_BA_Status = %d \n", Idx, Wcid, TID, pBAEntry->ORI_BA_Status));
+	//
+	// Prepare DelBA action frame and send to the peer.
+	//
+	if ((bPassive == FALSE) && (TID == pBAEntry->TID) && (pBAEntry->ORI_BA_Status == Originator_Done))
+	{
+		MLME_DELBA_REQ_STRUCT   DelbaReq;
+		MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+		NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+		NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+		COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+		DelbaReq.Wcid = Wcid;
+		DelbaReq.TID = pBAEntry->TID;
+		DelbaReq.Initiator = ORIGINATOR;
+#if 1
+		Elem->MsgLen  = sizeof(DelbaReq);
+		NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+		MlmeDELBAAction(pAd, Elem);
+		kfree(Elem);
+#else
+		MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+		RT28XX_MLME_HANDLER(pAd);
+#endif
+	}
+	RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+	BATableFreeOriEntry(pAd, Idx);
+
+	if (bPassive)
+	{
+		//BAOriSessionSetUp(pAd, &pAd->MacTab.Content[Wcid], TID, 0, 10000, TRUE);
+	}
+}
+
+VOID BARecSessionTearDown(
+						 IN OUT  PRTMP_ADAPTER   pAd,
+						 IN      UCHAR           Wcid,
+						 IN      UCHAR           TID,
+						 IN      BOOLEAN         bPassive)
+{
+	ULONG           Idx = 0;
+	BA_REC_ENTRY    *pBAEntry;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+	{
+		return;
+	}
+
+	//
+	//  Locate corresponding BA Originator Entry in BA Table with the (pAddr,TID).
+	//
+	Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+	if (Idx == 0)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s===>Wcid=%d.TID=%d \n", __FUNCTION__, Wcid, TID));
+
+
+	pBAEntry = &pAd->BATable.BARecEntry[Idx];
+	DBGPRINT(RT_DEBUG_TRACE,("\t===>Idx = %ld, Wcid=%d.TID=%d, REC_BA_Status = %d \n", Idx, Wcid, TID, pBAEntry->REC_BA_Status));
+	//
+	// Prepare DelBA action frame and send to the peer.
+	//
+	if ((TID == pBAEntry->TID) && (pBAEntry->REC_BA_Status == Recipient_Accept))
+	{
+		MLME_DELBA_REQ_STRUCT   DelbaReq;
+		BOOLEAN 				Cancelled;
+		MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+		//ULONG   offset;
+		//UINT32  VALUE;
+
+		RTMPCancelTimer(&pBAEntry->RECBATimer, &Cancelled);
+
+		//
+		// 1. Send DELBA Action Frame
+		//
+		if (bPassive == FALSE)
+		{
+			NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+			NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+			COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+			DelbaReq.Wcid = Wcid;
+			DelbaReq.TID = TID;
+			DelbaReq.Initiator = RECIPIENT;
+#if 1
+			Elem->MsgLen  = sizeof(DelbaReq);
+			NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+			MlmeDELBAAction(pAd, Elem);
+			kfree(Elem);
+#else
+			MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+			RT28XX_MLME_HANDLER(pAd);
+#endif
+		}
+
+
+		//
+		// 2. Free resource of BA session
+		//
+		// flush all pending reordering mpdus
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+
+		// Erase Bitmap flag.
+		pBAEntry->LastIndSeq = RESET_RCV_SEQ;
+		pBAEntry->BAWinSize = 0;
+		// Erase Bitmap flag at software mactable
+		pAd->MacTab.Content[Wcid].RXBAbitmap &= (~(1<<(pBAEntry->TID)));
+		pAd->MacTab.Content[Wcid].BARecWcidArray[TID] = 0;
+
+		RT28XX_DEL_BA_SESSION_FROM_ASIC(pAd, Wcid, TID);
+
+		NdisReleaseSpinLock(&pAd->BATabLock);
+
+	}
+
+	BATableFreeRecEntry(pAd, Idx);
+}
+
+VOID BASessionTearDownALL(
+						 IN OUT  PRTMP_ADAPTER pAd,
+						 IN      UCHAR Wcid)
+{
+	int i;
+
+	for (i=0; i<NUM_OF_TID; i++)
+	{
+		BAOriSessionTearDown(pAd, Wcid, i, FALSE, FALSE);
+		BARecSessionTearDown(pAd, Wcid, i, FALSE);
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID BAOriSessionSetupTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+	BA_ORI_ENTRY    *pBAEntry = (BA_ORI_ENTRY *)FunctionContext;
+	MAC_TABLE_ENTRY *pEntry;
+	PRTMP_ADAPTER   pAd;
+
+	if (pBAEntry == NULL)
+		return;
+
+	pAd = pBAEntry->pAdapter;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Do nothing if monitor mode is on
+		if (MONITOR_ON(pAd))
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+
+	if ((pBAEntry->ORI_BA_Status == Originator_WaitRes) && (pBAEntry->Token < ORI_SESSION_MAX_RETRY))
+	{
+		MLME_ADDBA_REQ_STRUCT    AddbaReq;
+
+		NdisZeroMemory(&AddbaReq, sizeof(AddbaReq));
+		COPY_MAC_ADDR(AddbaReq.pAddr, pEntry->Addr);
+		AddbaReq.Wcid = (UCHAR)(pEntry->Aid);
+		AddbaReq.TID = pBAEntry->TID;
+		AddbaReq.BaBufSize = pAd->CommonCfg.BACapability.field.RxBAWinLimit;
+		AddbaReq.TimeOutValue = 0;
+		AddbaReq.Token = pBAEntry->Token;
+		MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ADD_BA_CATE, sizeof(MLME_ADDBA_REQ_STRUCT), (PVOID)&AddbaReq);
+		RT28XX_MLME_HANDLER(pAd);
+		DBGPRINT(RT_DEBUG_TRACE,("BA Ori Session Timeout(%d) : Send ADD BA again\n", pBAEntry->Token));
+
+		pBAEntry->Token++;
+		RTMPSetTimer(&pBAEntry->ORIBATimer, ORI_BA_SESSION_TIMEOUT);
+	}
+	else
+	{
+		BATableFreeOriEntry(pAd, pEntry->BAOriWcidArray[pBAEntry->TID]);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID BARecSessionIdleTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+
+	BA_REC_ENTRY    *pBAEntry = (BA_REC_ENTRY *)FunctionContext;
+	PRTMP_ADAPTER   pAd;
+	ULONG           Now32;
+
+	if (pBAEntry == NULL)
+		return;
+
+	if ((pBAEntry->REC_BA_Status == Recipient_Accept))
+	{
+		NdisGetSystemUpTime(&Now32);
+
+		if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer + REC_BA_SESSION_IDLE_TIMEOUT)))
+		{
+			pAd = pBAEntry->pAdapter;
+			// flush all pending reordering mpdus
+			ba_refresh_reordering_mpdus(pAd, pBAEntry);
+			printk("%ld: REC BA session Timeout\n", Now32);
+		}
+	}
+}
+
+
+VOID PeerAddBAReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//	7.4.4.1
+	//ULONG	Idx;
+	UCHAR   Status = 1;
+	UCHAR   pAddr[6];
+	FRAME_ADDBA_RSP ADDframe;
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	PFRAME_ADDBA_REQ  pAddreqFrame = NULL;
+	//UCHAR		BufSize;
+	ULONG       FrameLen;
+	PULONG      ptemp;
+	PMAC_TABLE_ENTRY	pMacEntry;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s ==> (Wcid = %d)\n", __FUNCTION__, Elem->Wcid));
+
+	//hex_dump("AddBAReq", Elem->Msg, Elem->MsgLen);
+
+	//ADDBA Request from unknown peer, ignore this.
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	pMacEntry = &pAd->MacTab.Content[Elem->Wcid];
+	DBGPRINT(RT_DEBUG_TRACE,("BA - PeerAddBAReqAction----> \n"));
+	ptemp = (PULONG)Elem->Msg;
+	//DBGPRINT_RAW(RT_DEBUG_EMU, ("%08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x\n", *(ptemp), *(ptemp+1), *(ptemp+2), *(ptemp+3), *(ptemp+4), *(ptemp+5), *(ptemp+6), *(ptemp+7), *(ptemp+8)));
+
+	if (PeerAddBAReqActionSanity(pAd, Elem->Msg, Elem->MsgLen, pAddr))
+	{
+
+		if ((pAd->CommonCfg.bBADecline == FALSE) && IS_HT_STA(pMacEntry))
+		{
+			pAddreqFrame = (PFRAME_ADDBA_REQ)(&Elem->Msg[0]);
+			printk("Rcv Wcid(%d) AddBAReq\n", Elem->Wcid);
+			if (BARecSessionAdd(pAd, &pAd->MacTab.Content[Elem->Wcid], pAddreqFrame))
+				Status = 0;
+			else
+				Status = 38; // more parameters have invalid values
+		}
+		else
+		{
+			Status = 37; // the request has been declined.
+		}
+	}
+
+	if (pAd->MacTab.Content[Elem->Wcid].ValidAsCLI)
+		ASSERT(pAd->MacTab.Content[Elem->Wcid].Sst == SST_ASSOC);
+
+	pAddreqFrame = (PFRAME_ADDBA_REQ)(&Elem->Msg[0]);
+	// 2. Always send back ADDBA Response
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ACTION - PeerBAAction() allocate memory failed \n"));
+		return;
+	}
+
+	NdisZeroMemory(&ADDframe, sizeof(FRAME_ADDBA_RSP));
+	// 2-1. Prepare ADDBA Response frame.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (ADHOC_ON(pAd))
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+#ifdef QOS_DLS_SUPPORT
+		if (pAd->MacTab.Content[Elem->Wcid].ValidAsDls)
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+#endif // QOS_DLS_SUPPORT //
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAddr);
+	}
+#endif // CONFIG_STA_SUPPORT //
+	ADDframe.Category = CATEGORY_BA;
+	ADDframe.Action = ADDBA_RESP;
+	ADDframe.Token = pAddreqFrame->Token;
+	// What is the Status code??  need to check.
+	ADDframe.StatusCode = Status;
+	ADDframe.BaParm.BAPolicy = IMMED_BA;
+	ADDframe.BaParm.AMSDUSupported = 0;
+	ADDframe.BaParm.TID = pAddreqFrame->BaParm.TID;
+	ADDframe.BaParm.BufSize = min(((UCHAR)pAddreqFrame->BaParm.BufSize), (UCHAR)pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+	if (ADDframe.BaParm.BufSize == 0)
+	{
+		ADDframe.BaParm.BufSize = 64;
+	}
+	ADDframe.TimeOutValue = 0; //pAddreqFrame->TimeOutValue;
+
+	*(USHORT *)(&ADDframe.BaParm) = cpu2le16(*(USHORT *)(&ADDframe.BaParm));
+	ADDframe.StatusCode = cpu2le16(ADDframe.StatusCode);
+	ADDframe.TimeOutValue = cpu2le16(ADDframe.TimeOutValue);
+
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+					  sizeof(FRAME_ADDBA_RSP),  &ADDframe,
+			  END_OF_ARGS);
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s(%d): TID(%d), BufSize(%d) <== \n", __FUNCTION__, Elem->Wcid, ADDframe.BaParm.TID,
+							  ADDframe.BaParm.BufSize));
+}
+
+
+VOID PeerAddBARspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//UCHAR		Idx, i;
+	//PUCHAR		   pOutBuffer = NULL;
+	PFRAME_ADDBA_RSP    pFrame = NULL;
+	//PBA_ORI_ENTRY		pBAEntry;
+
+	//ADDBA Response from unknown peer, ignore this.
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s ==> Wcid(%d)\n", __FUNCTION__, Elem->Wcid));
+
+	//hex_dump("PeerAddBARspAction()", Elem->Msg, Elem->MsgLen);
+
+	if (PeerAddBARspActionSanity(pAd, Elem->Msg, Elem->MsgLen))
+	{
+		pFrame = (PFRAME_ADDBA_RSP)(&Elem->Msg[0]);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("\t\t StatusCode = %d\n", pFrame->StatusCode));
+		switch (pFrame->StatusCode)
+		{
+			case 0:
+				// I want a BAsession with this peer as an originator.
+				BAOriSessionAdd(pAd, &pAd->MacTab.Content[Elem->Wcid], pFrame);
+				break;
+			default:
+				// check status == USED ???
+				BAOriSessionTearDown(pAd, Elem->Wcid, pFrame->BaParm.TID, TRUE, FALSE);
+				break;
+		}
+		// Rcv Decline StatusCode
+		if ((pFrame->StatusCode == 37)
+#ifdef CONFIG_STA_SUPPORT
+            || ((pAd->OpMode == OPMODE_STA) && STA_TGN_WIFI_ON(pAd) && (pFrame->StatusCode != 0))
+#endif // CONFIG_STA_SUPPORT //
+            )
+		{
+			pAd->MacTab.Content[Elem->Wcid].BADeclineBitmap |= 1<<pFrame->BaParm.TID;
+		}
+	}
+}
+
+VOID PeerDelBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//UCHAR				Idx;
+	//PUCHAR				pOutBuffer = NULL;
+	PFRAME_DELBA_REQ    pDelFrame = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s ==>\n", __FUNCTION__));
+	//DELBA Request from unknown peer, ignore this.
+	if (PeerDelBAActionSanity(pAd, Elem->Wcid, Elem->Msg, Elem->MsgLen))
+	{
+		pDelFrame = (PFRAME_DELBA_REQ)(&Elem->Msg[0]);
+		if (pDelFrame->DelbaParm.Initiator == ORIGINATOR)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - PeerDelBAAction----> ORIGINATOR\n"));
+			BARecSessionTearDown(pAd, Elem->Wcid, pDelFrame->DelbaParm.TID, TRUE);
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - PeerDelBAAction----> RECIPIENT, Reason = %d\n",  pDelFrame->ReasonCode));
+			//hex_dump("DelBA Frame", pDelFrame, Elem->MsgLen);
+			BAOriSessionTearDown(pAd, Elem->Wcid, pDelFrame->DelbaParm.TID, TRUE, FALSE);
+		}
+	}
+}
+
+
+BOOLEAN CntlEnqueueForRecv(
+						  IN PRTMP_ADAPTER		pAd,
+						  IN ULONG				Wcid,
+						  IN ULONG				MsgLen,
+						  IN PFRAME_BA_REQ		pMsg)
+{
+	PFRAME_BA_REQ   pFrame = pMsg;
+	//PRTMP_REORDERBUF	pBuffer;
+	//PRTMP_REORDERBUF	pDmaBuf;
+	PBA_REC_ENTRY pBAEntry;
+	//BOOLEAN 	Result;
+	ULONG   Idx;
+	//UCHAR	NumRxPkt;
+	UCHAR	TID;//, i;
+
+	TID = (UCHAR)pFrame->BARControl.TID;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s(): BAR-Wcid(%ld), Tid (%d)\n", __FUNCTION__, Wcid, TID));
+	//hex_dump("BAR", (PCHAR) pFrame, MsgLen);
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return FALSE;
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: frame too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+	else if (MsgLen != sizeof(FRAME_BA_REQ))
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: BlockAck Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	else if (MsgLen != sizeof(FRAME_BA_REQ))
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: BlockAck Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+
+	if ((Wcid < MAX_LEN_OF_MAC_TABLE) && (TID < 8))
+		{
+		// if this receiving packet is from SA that is in our OriEntry. Since WCID <9 has direct mapping. no need search.
+		Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+		}
+		else
+		{
+		return FALSE;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BAR(%ld) : Tid (%d) - %04x:%04x\n", Wcid, TID, pFrame->BAStartingSeq.field.StartSeq, pBAEntry->LastIndSeq ));
+
+	if (SEQ_SMALLER(pBAEntry->LastIndSeq, pFrame->BAStartingSeq.field.StartSeq, MAXSEQ))
+	{
+		//printk("BAR Seq = %x, LastIndSeq = %x\n", pFrame->BAStartingSeq.field.StartSeq, pBAEntry->LastIndSeq);
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, pFrame->BAStartingSeq.field.StartSeq);
+		pBAEntry->LastIndSeq = (pFrame->BAStartingSeq.field.StartSeq == 0) ? MAXSEQ :(pFrame->BAStartingSeq.field.StartSeq -1);
+	}
+	//ba_refresh_reordering_mpdus(pAd, pBAEntry);
+	return TRUE;
+}
+
+/*
+Description : Send PSMP Action frame If PSMP mode switches.
+*/
+VOID SendPSMPAction(
+				   IN PRTMP_ADAPTER		pAd,
+				   IN UCHAR				Wcid,
+				   IN UCHAR				Psmp)
+{
+	PUCHAR          pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	//ULONG           Idx;
+	FRAME_PSMP_ACTION   Frame;
+	ULONG           FrameLen;
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() allocate memory failed \n"));
+		return;
+	}
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ActHeaderInit(pAd, &Frame.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->MacTab.Content[Wcid].Addr);
+#endif // CONFIG_STA_SUPPORT //
+
+	Frame.Category = CATEGORY_HT;
+	Frame.Action = SMPS_ACTION;
+	switch (Psmp)
+	{
+		case MMPS_ENABLE:
+			Frame.Psmp = 0;
+			break;
+		case MMPS_DYNAMIC:
+			Frame.Psmp = 3;
+			break;
+		case MMPS_STATIC:
+			Frame.Psmp = 1;
+			break;
+	}
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+					  sizeof(FRAME_PSMP_ACTION),      &Frame,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+	DBGPRINT(RT_DEBUG_ERROR,("HT - SendPSMPAction( %d )  \n", Frame.Psmp));
+}
+
+
+#define RADIO_MEASUREMENT_REQUEST_ACTION	0
+
+typedef struct PACKED
+{
+	UCHAR	RegulatoryClass;
+	UCHAR	ChannelNumber;
+	USHORT	RandomInterval;
+	USHORT	MeasurementDuration;
+	UCHAR	MeasurementMode;
+	UCHAR   BSSID[MAC_ADDR_LEN];
+	UCHAR	ReportingCondition;
+	UCHAR	Threshold;
+	UCHAR   SSIDIE[2];			// 2 byte
+} BEACON_REQUEST;
+
+typedef struct PACKED
+{
+	UCHAR	ID;
+	UCHAR	Length;
+	UCHAR	Token;
+	UCHAR	RequestMode;
+	UCHAR	Type;
+} MEASUREMENT_REQ;
+
+
+
+
+void convert_reordering_packet_to_preAMSDU_or_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN  UCHAR			FromWhichBSSID)
+{
+	PNDIS_PACKET	pRxPkt;
+	UCHAR			Header802_3[LENGTH_802_3];
+
+	// 1. get 802.3 Header
+	// 2. remove LLC
+	// 		a. pointer pRxBlk->pData to payload
+	//      b. modify pRxBlk->DataSize
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+	ASSERT(pRxBlk->pRxPacket);
+	pRxPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	RTPKT_TO_OSPKT(pRxPkt)->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	RTPKT_TO_OSPKT(pRxPkt)->data = pRxBlk->pData;
+	RTPKT_TO_OSPKT(pRxPkt)->len = pRxBlk->DataSize;
+	RTPKT_TO_OSPKT(pRxPkt)->tail = RTPKT_TO_OSPKT(pRxPkt)->data + RTPKT_TO_OSPKT(pRxPkt)->len;
+
+	//
+	// copy 802.3 header, if necessary
+	//
+	if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef LINUX
+			NdisMoveMemory(skb_push(pRxPkt, LENGTH_802_3), Header802_3, LENGTH_802_3);
+#endif
+#ifdef UCOS
+			NdisMoveMemory(net_pkt_push(pRxPkt, LENGTH_802_3), Header802_3, LENGTH_802_3);
+#endif
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+
+#define INDICATE_LEGACY_OR_AMSDU(_pAd, _pRxBlk, _fromWhichBSSID)		\
+	do																	\
+	{																	\
+    	if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_AMSDU))						\
+    	{																\
+    		Indicate_AMSDU_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+    	}																\
+		else if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_EAP))					\
+		{																\
+			Indicate_EAPOL_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+		}																\
+    	else															\
+    	{																\
+    		Indicate_Legacy_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+    	}																\
+	} while (0);
+
+
+
+static VOID ba_enqueue_reordering_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PBA_REC_ENTRY	pBAEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct reordering_mpdu *mpdu_blk;
+	UINT16	Sequence = (UINT16) pRxBlk->pHeader->Sequence;
+
+	mpdu_blk = ba_mpdu_blk_alloc(pAd);
+	if (mpdu_blk != NULL)
+	{
+		// Write RxD buffer address & allocated buffer length
+		NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+		mpdu_blk->Sequence = Sequence;
+
+		mpdu_blk->bAMSDU = RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU);
+
+		convert_reordering_packet_to_preAMSDU_or_802_3_packet(pAd, pRxBlk, FromWhichBSSID);
+
+		STATS_INC_RX_PACKETS(pAd, FromWhichBSSID);
+
+        //
+		// it is necessary for reordering packet to record
+		// which BSS it come from
+		//
+		RTMP_SET_PACKET_IF(pRxBlk->pRxPacket, FromWhichBSSID);
+
+		mpdu_blk->pPacket = pRxBlk->pRxPacket;
+
+		if (ba_reordering_mpdu_insertsorted(&pBAEntry->list, mpdu_blk) == FALSE)
+		{
+			// had been already within reordering list
+			// don't indicate
+			RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_SUCCESS);
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+		}
+
+		ASSERT((0<= pBAEntry->list.qlen)  && (pBAEntry->list.qlen <= pBAEntry->BAWinSize));
+		NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+	}
+	else
+	{
+#if 0
+		DBGPRINT(RT_DEBUG_ERROR,  ("!!! (%d:%d) Can't allocate reordering mpdu blk\n",
+								   blk_count, pBAEntry->list.qlen));
+#else
+		DBGPRINT(RT_DEBUG_ERROR,  ("!!! (%d) Can't allocate reordering mpdu blk\n",
+								   pBAEntry->list.qlen));
+#endif
+		/*
+		 * flush all pending reordering mpdus
+		 * and receving mpdu to upper layer
+		 * make tcp/ip to take care reordering mechanism
+		 */
+		//ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, Sequence);
+
+		pBAEntry->LastIndSeq = Sequence;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Indicate this packet to upper layer or put it into reordering buffer
+
+	Parametrs:
+		pRxBlk         : carry necessary packet info 802.11 format
+		FromWhichBSSID : the packet received from which BSS
+
+	Return	:
+			  none
+
+	Note    :
+	          the packet queued into reordering buffer need to cover to 802.3 format
+			  or pre_AMSDU format
+	==========================================================================
+ */
+
+VOID Indicate_AMPDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	USHORT				Idx;
+	PBA_REC_ENTRY		pBAEntry = NULL;
+	UINT16				Sequence = pRxBlk->pHeader->Sequence;
+	ULONG				Now32;
+	UCHAR				Wcid = pRxBlk->pRxWI->WirelessCliID;
+	UCHAR				TID = pRxBlk->pRxWI->TID;
+
+
+	if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU) &&  (pRxBlk->DataSize > MAX_RX_PKT_LEN))
+	{
+#if 0 // sample take off, no use
+		static int err_size;
+
+		err_size++;
+		if (err_size > 20) {
+			 printk("AMPDU DataSize = %d\n", pRxBlk->DataSize);
+			 hex_dump("802.11 Header", (UCHAR *)pRxBlk->pHeader, 24);
+			 hex_dump("Payload", pRxBlk->pData, 64);
+			 err_size = 0;
+		}
+#endif
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+
+#if 0 // test
+	/* Rec BA Session had been torn down */
+	INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+	return;
+#endif
+
+	if (Wcid < MAX_LEN_OF_MAC_TABLE)
+	{
+		Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+		if (Idx == 0)
+		{
+			/* Rec BA Session had been torn down */
+			INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+			return;
+		}
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+	}
+	else
+	{
+		// impossible !!!
+		ASSERT(0);
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	ASSERT(pBAEntry);
+
+	// update last rx time
+	NdisGetSystemUpTime(&Now32);
+
+	pBAEntry->rcvSeq = Sequence;
+
+
+	ba_flush_reordering_timeout_mpdus(pAd, pBAEntry, Now32);
+	pBAEntry->LastIndSeqAtTimer = Now32;
+
+	//
+	// Reset Last Indicate Sequence
+	//
+	if (pBAEntry->LastIndSeq == RESET_RCV_SEQ)
+	{
+		ASSERT((pBAEntry->list.qlen == 0) && (pBAEntry->list.next == NULL));
+
+		// reset rcv sequence of BA session
+		pBAEntry->LastIndSeq = Sequence;
+		pBAEntry->LastIndSeqAtTimer = Now32;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+		return;
+	}
+
+
+	//
+	// I. Check if in order.
+	//
+	if (SEQ_STEPONE(Sequence, pBAEntry->LastIndSeq, MAXSEQ))
+	{
+		USHORT  LastIndSeq;
+
+		pBAEntry->LastIndSeq = Sequence;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+ 		LastIndSeq = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, pBAEntry->LastIndSeq);
+		if (LastIndSeq != RESET_RCV_SEQ)
+		{
+			pBAEntry->LastIndSeq = LastIndSeq;
+		}
+		pBAEntry->LastIndSeqAtTimer = Now32;
+	}
+	//
+	// II. Drop Duplicated Packet
+	//
+	else if (Sequence == pBAEntry->LastIndSeq)
+	{
+
+		// drop and release packet
+		pBAEntry->nDropPacket++;
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	}
+	//
+	// III. Drop Old Received Packet
+	//
+	else if (SEQ_SMALLER(Sequence, pBAEntry->LastIndSeq, MAXSEQ))
+	{
+
+		// drop and release packet
+		pBAEntry->nDropPacket++;
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	}
+	//
+	// IV. Receive Sequence within Window Size
+	//
+	else if (SEQ_SMALLER(Sequence, (((pBAEntry->LastIndSeq+pBAEntry->BAWinSize+1)) & MAXSEQ), MAXSEQ))
+	{
+		ba_enqueue_reordering_packet(pAd, pBAEntry, pRxBlk, FromWhichBSSID);
+	}
+	//
+	// V. Receive seq surpasses Win(lastseq + nMSDU). So refresh all reorder buffer
+	//
+	else
+	{
+#if 0
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+#else
+		LONG WinStartSeq, TmpSeq;
+
+
+		TmpSeq = Sequence - (pBAEntry->BAWinSize) -1;
+		if (TmpSeq < 0)
+		{
+			TmpSeq = (MAXSEQ+1) + TmpSeq;
+		}
+		WinStartSeq = (TmpSeq+1) & MAXSEQ;
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, WinStartSeq);
+		pBAEntry->LastIndSeq = WinStartSeq; //TmpSeq;
+
+		pBAEntry->LastIndSeqAtTimer = Now32;
+
+		ba_enqueue_reordering_packet(pAd, pBAEntry, pRxBlk, FromWhichBSSID);
+
+		TmpSeq = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, pBAEntry->LastIndSeq);
+		if (TmpSeq != RESET_RCV_SEQ)
+		{
+			pBAEntry->LastIndSeq = TmpSeq;
+		}
+#endif
+	}
+}
+
+#endif // DOT11_N_SUPPORT //
+
diff --git a/drivers/staging/rt2870/common/cmm_data.c b/drivers/staging/rt2870/common/cmm_data.c
new file mode 100644
index 00000000000..4477a8e64a2
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_data.c
@@ -0,0 +1,2734 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+#include "../rt_config.h"
+
+#define MAX_TX_IN_TBTT		(16)
+
+
+UCHAR	SNAP_802_1H[] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00};
+UCHAR	SNAP_BRIDGE_TUNNEL[] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8};
+// Add Cisco Aironet SNAP heade for CCX2 support
+UCHAR	SNAP_AIRONET[] = {0xaa, 0xaa, 0x03, 0x00, 0x40, 0x96, 0x00, 0x00};
+UCHAR	CKIP_LLC_SNAP[] = {0xaa, 0xaa, 0x03, 0x00, 0x40, 0x96, 0x00, 0x02};
+UCHAR	EAPOL_LLC_SNAP[]= {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00, 0x88, 0x8e};
+UCHAR	EAPOL[] = {0x88, 0x8e};
+UCHAR   TPID[] = {0x81, 0x00}; /* VLAN related */
+
+UCHAR	IPX[] = {0x81, 0x37};
+UCHAR	APPLE_TALK[] = {0x80, 0xf3};
+UCHAR	RateIdToPlcpSignal[12] = {
+	 0, /* RATE_1 */	1, /* RATE_2 */ 	2, /* RATE_5_5 */	3, /* RATE_11 */	// see BBP spec
+	11, /* RATE_6 */   15, /* RATE_9 */    10, /* RATE_12 */   14, /* RATE_18 */	// see IEEE802.11a-1999 p.14
+	 9, /* RATE_24 */  13, /* RATE_36 */	8, /* RATE_48 */   12  /* RATE_54 */ }; // see IEEE802.11a-1999 p.14
+
+UCHAR	 OfdmSignalToRateId[16] = {
+	RATE_54,  RATE_54,	RATE_54,  RATE_54,	// OFDM PLCP Signal = 0,  1,  2,  3 respectively
+	RATE_54,  RATE_54,	RATE_54,  RATE_54,	// OFDM PLCP Signal = 4,  5,  6,  7 respectively
+	RATE_48,  RATE_24,	RATE_12,  RATE_6,	// OFDM PLCP Signal = 8,  9,  10, 11 respectively
+	RATE_54,  RATE_36,	RATE_18,  RATE_9,	// OFDM PLCP Signal = 12, 13, 14, 15 respectively
+};
+
+UCHAR	 OfdmRateToRxwiMCS[12] = {
+	0,  0,	0,  0,
+	0,  1,	2,  3,	// OFDM rate 6,9,12,18 = rxwi mcs 0,1,2,3
+	4,  5,	6,  7,	// OFDM rate 24,36,48,54 = rxwi mcs 4,5,6,7
+};
+UCHAR	 RxwiMCSToOfdmRate[12] = {
+	RATE_6,  RATE_9,	RATE_12,  RATE_18,
+	RATE_24,  RATE_36,	RATE_48,  RATE_54,	// OFDM rate 6,9,12,18 = rxwi mcs 0,1,2,3
+	4,  5,	6,  7,	// OFDM rate 24,36,48,54 = rxwi mcs 4,5,6,7
+};
+
+char*   MCSToMbps[] = {"1Mbps","2Mbps","5.5Mbps","11Mbps","06Mbps","09Mbps","12Mbps","18Mbps","24Mbps","36Mbps","48Mbps","54Mbps","MM-0","MM-1","MM-2","MM-3","MM-4","MM-5","MM-6","MM-7","MM-8","MM-9","MM-10","MM-11","MM-12","MM-13","MM-14","MM-15","MM-32","ee1","ee2","ee3"};
+
+UCHAR default_cwmin[]={CW_MIN_IN_BITS, CW_MIN_IN_BITS, CW_MIN_IN_BITS-1, CW_MIN_IN_BITS-2};
+//UCHAR default_cwmax[]={CW_MAX_IN_BITS, CW_MAX_IN_BITS, CW_MIN_IN_BITS, CW_MIN_IN_BITS-1};
+UCHAR default_sta_aifsn[]={3,7,2,2};
+
+UCHAR MapUserPriorityToAccessCategory[8] = {QID_AC_BE, QID_AC_BK, QID_AC_BK, QID_AC_BE, QID_AC_VI, QID_AC_VI, QID_AC_VO, QID_AC_VO};
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		API for MLME to transmit management frame to AP (BSS Mode)
+	or station (IBSS Mode)
+
+	Arguments:
+		pAd Pointer to our adapter
+		pData		Pointer to the outgoing 802.11 frame
+		Length		Size of outgoing management frame
+
+	Return Value:
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_PENDING
+		NDIS_STATUS_SUCCESS
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS MiniportMMRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	PUCHAR			pData,
+	IN	UINT			Length)
+{
+	PNDIS_PACKET	pPacket;
+	NDIS_STATUS  	Status = NDIS_STATUS_SUCCESS;
+	ULONG	 		FreeNum;
+	UCHAR			IrqState;
+	UCHAR			rtmpHwHdr[TXINFO_SIZE + TXWI_SIZE]; //RTMP_HW_HDR_LEN];
+
+	ASSERT(Length <= MGMT_DMA_BUFFER_SIZE);
+
+	QueIdx=3;
+
+	// 2860C use Tx Ring
+
+	IrqState = pAd->irq_disabled;
+
+	do
+	{
+		// Reset is in progress, stop immediately
+		if ( RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+			 RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)||
+			 !RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+		{
+			Status = NDIS_STATUS_FAILURE;
+			break;
+		}
+
+		// Check Free priority queue
+		// Since we use PBF Queue2 for management frame.  Its corresponding DMA ring should be using TxRing.
+
+		// 2860C use Tx Ring
+		if (pAd->MACVersion == 0x28600100)
+		{
+			FreeNum = GET_TXRING_FREENO(pAd, QueIdx);
+		}
+		else
+		{
+			FreeNum = GET_MGMTRING_FREENO(pAd);
+		}
+
+		if ((FreeNum > 0))
+		{
+			// We need to reserve space for rtmp hardware header. i.e., TxWI for RT2860 and TxInfo+TxWI for RT2870
+			NdisZeroMemory(&rtmpHwHdr, (TXINFO_SIZE + TXWI_SIZE));
+			Status = RTMPAllocateNdisPacket(pAd, &pPacket, (PUCHAR)&rtmpHwHdr, (TXINFO_SIZE + TXWI_SIZE), pData, Length);
+			if (Status != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("MiniportMMRequest (error:: can't allocate NDIS PACKET)\n"));
+				break;
+			}
+
+			//pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+			//pAd->CommonCfg.MlmeRate = RATE_2;
+
+
+			Status = MlmeHardTransmit(pAd, QueIdx, pPacket);
+			if (Status != NDIS_STATUS_SUCCESS)
+				RTMPFreeNdisPacket(pAd, pPacket);
+		}
+		else
+		{
+			pAd->RalinkCounters.MgmtRingFullCount++;
+			DBGPRINT(RT_DEBUG_ERROR, ("Qidx(%d), not enough space in MgmtRing, MgmtRingFullCount=%ld!\n",
+										QueIdx, pAd->RalinkCounters.MgmtRingFullCount));
+		}
+
+	} while (FALSE);
+
+
+	return Status;
+}
+
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware transmit function
+
+	Arguments:
+		pAd Pointer to our adapter
+		pBuffer 	Pointer to	memory of outgoing frame
+		Length		Size of outgoing management frame
+
+	Return Value:
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_PENDING
+		NDIS_STATUS_SUCCESS
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS MlmeHardTransmit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	PNDIS_PACKET	pPacket)
+{
+	if ((pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE)
+#ifdef CARRIER_DETECTION_SUPPORT
+#endif // CARRIER_DETECTION_SUPPORT //
+		)
+	{
+		return NDIS_STATUS_FAILURE;
+	}
+
+		return MlmeHardTransmitMgmtRing(pAd,QueIdx,pPacket);
+
+}
+
+
+
+NDIS_STATUS MlmeHardTransmitMgmtRing(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	QueIdx,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	PHEADER_802_11	pHeader_802_11;
+	BOOLEAN 		bAckRequired, bInsertTimestamp;
+	UCHAR			MlmeRate;
+	PTXWI_STRUC 	pFirstTxWI;
+	MAC_TABLE_ENTRY	*pMacEntry = NULL;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	// Make sure MGMT ring resource won't be used by other threads
+// sample, for IRQ LOCK -> SEM LOCK
+//	IrqState = pAd->irq_disabled;
+//	if (!IrqState)
+		RTMP_SEM_LOCK(&pAd->MgmtRingLock);
+
+
+	if (pSrcBufVA == NULL)
+	{
+		// The buffer shouldn't be NULL
+//		if (!IrqState)
+			RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+		return NDIS_STATUS_FAILURE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// outgoing frame always wakeup PHY to prevent frame lost
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+			AsicForceWakeup(pAd, TRUE);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pFirstTxWI = (PTXWI_STRUC)(pSrcBufVA +  TXINFO_SIZE);
+	pHeader_802_11 = (PHEADER_802_11) (pSrcBufVA + TXINFO_SIZE + TXWI_SIZE); //TXWI_SIZE);
+
+	if (pHeader_802_11->Addr1[0] & 0x01)
+	{
+		MlmeRate = pAd->CommonCfg.BasicMlmeRate;
+	}
+	else
+	{
+		MlmeRate = pAd->CommonCfg.MlmeRate;
+	}
+
+	// Verify Mlme rate for a / g bands.
+	if ((pAd->LatchRfRegs.Channel > 14) && (MlmeRate < RATE_6)) // 11A band
+		MlmeRate = RATE_6;
+
+	if ((pHeader_802_11->FC.Type == BTYPE_DATA) &&
+		(pHeader_802_11->FC.SubType == SUBTYPE_QOS_NULL))
+	{
+		pMacEntry = MacTableLookup(pAd, pHeader_802_11->Addr1);
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Fixed W52 with Activity scan issue in ABG_MIXED and ABGN_MIXED mode.
+		if (pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED
+#ifdef DOT11_N_SUPPORT
+			|| pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED
+#endif // DOT11_N_SUPPORT //
+		)
+		{
+			if (pAd->LatchRfRegs.Channel > 14)
+				pAd->CommonCfg.MlmeTransmit.field.MODE = 1;
+			else
+				pAd->CommonCfg.MlmeTransmit.field.MODE = 0;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	//
+	// Should not be hard code to set PwrMgmt to 0 (PWR_ACTIVE)
+	// Snice it's been set to 0 while on MgtMacHeaderInit
+	// By the way this will cause frame to be send on PWR_SAVE failed.
+	//
+	// pHeader_802_11->FC.PwrMgmt = 0; // (pAd->StaCfg.Psm == PWR_SAVE);
+	//
+	// In WMM-UAPSD, mlme frame should be set psm as power saving but probe request frame
+#ifdef CONFIG_STA_SUPPORT
+    // Data-Null packets alse pass through MMRequest in RT2860, however, we hope control the psm bit to pass APSD
+	if ((pHeader_802_11->FC.Type != BTYPE_DATA) && (pHeader_802_11->FC.Type != BTYPE_CNTL))
+	{
+		if ((pAd->StaCfg.Psm == PWR_SAVE) &&
+			(pHeader_802_11->FC.SubType == SUBTYPE_ACTION))
+			pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+		else
+			pHeader_802_11->FC.PwrMgmt = PWR_ACTIVE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	bInsertTimestamp = FALSE;
+	if (pHeader_802_11->FC.Type == BTYPE_CNTL) // must be PS-POLL
+	{
+#ifdef CONFIG_STA_SUPPORT
+		//Set PM bit in ps-poll, to fix WLK 1.2  PowerSaveMode_ext failure issue.
+		if ((pAd->OpMode == OPMODE_STA) && (pHeader_802_11->FC.SubType == SUBTYPE_PS_POLL))
+		{
+			pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+		}
+#endif // CONFIG_STA_SUPPORT //
+		bAckRequired = FALSE;
+	}
+	else // BTYPE_MGMT or BTYPE_DATA(must be NULL frame)
+	{
+		//pAd->Sequence++;
+		//pHeader_802_11->Sequence = pAd->Sequence;
+
+		if (pHeader_802_11->Addr1[0] & 0x01) // MULTICAST, BROADCAST
+		{
+			bAckRequired = FALSE;
+			pHeader_802_11->Duration = 0;
+		}
+		else
+		{
+			bAckRequired = TRUE;
+			pHeader_802_11->Duration = RTMPCalcDuration(pAd, MlmeRate, 14);
+			if (pHeader_802_11->FC.SubType == SUBTYPE_PROBE_RSP)
+			{
+				bInsertTimestamp = TRUE;
+			}
+		}
+	}
+
+	pHeader_802_11->Sequence = pAd->Sequence++;
+	if (pAd->Sequence >0xfff)
+		pAd->Sequence = 0;
+
+	// Before radar detection done, mgmt frame can not be sent but probe req
+	// Because we need to use probe req to trigger driver to send probe req in passive scan
+	if ((pHeader_802_11->FC.SubType != SUBTYPE_PROBE_REQ)
+		&& (pAd->CommonCfg.bIEEE80211H == 1)
+		&& (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("MlmeHardTransmit --> radar detect not in normal mode !!!\n"));
+//		if (!IrqState)
+			RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+		return (NDIS_STATUS_FAILURE);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pHeader_802_11, DIR_WRITE, FALSE);
+#endif
+
+	//
+	// fill scatter-and-gather buffer list into TXD. Internally created NDIS PACKET
+	// should always has only one ohysical buffer, and the whole frame size equals
+	// to the first scatter buffer size
+	//
+
+	// Initialize TX Descriptor
+	// For inter-frame gap, the number is for this frame and next frame
+	// For MLME rate, we will fix as 2Mb to match other vendor's implement
+//	pAd->CommonCfg.MlmeTransmit.field.MODE = 1;
+
+// management frame doesn't need encryption. so use RESERVED_WCID no matter u are sending to specific wcid or not.
+	if (pMacEntry == NULL)
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE, bInsertTimestamp, FALSE, bAckRequired, FALSE,
+		0, RESERVED_WCID, (SrcBufLen - TXINFO_SIZE - TXWI_SIZE), PID_MGMT, 0,  (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_BACKOFF, FALSE, &pAd->CommonCfg.MlmeTransmit);
+	}
+	else
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE,
+					bInsertTimestamp, FALSE, bAckRequired, FALSE,
+					0, pMacEntry->Aid, (SrcBufLen - TXINFO_SIZE - TXWI_SIZE),
+					pMacEntry->MaxHTPhyMode.field.MCS, 0,
+					(UCHAR)pMacEntry->MaxHTPhyMode.field.MCS,
+					IFS_BACKOFF, FALSE, &pMacEntry->MaxHTPhyMode);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pFirstTxWI, TYPE_TXWI);
+#endif
+
+	// Now do hardware-depened kick out.
+	HAL_KickOutMgmtTx(pAd, QueIdx, pPacket, pSrcBufVA, SrcBufLen);
+
+	// Make sure to release MGMT ring resource
+//	if (!IrqState)
+		RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+/********************************************************************************
+
+	New DeQueue Procedures.
+
+ ********************************************************************************/
+
+#define DEQUEUE_LOCK(lock, bIntContext, IrqFlags) 				\
+			do{													\
+				if (bIntContext == FALSE)						\
+				RTMP_IRQ_LOCK((lock), IrqFlags);		\
+			}while(0)
+
+#define DEQUEUE_UNLOCK(lock, bIntContext, IrqFlags)				\
+			do{													\
+				if (bIntContext == FALSE)						\
+					RTMP_IRQ_UNLOCK((lock), IrqFlags);	\
+			}while(0)
+
+
+#if 0
+static VOID dumpTxBlk(TX_BLK *pTxBlk)
+{
+	NDIS_PACKET *pPacket;
+	int i, frameNum;
+	PQUEUE_ENTRY	pQEntry;
+
+	printk("Dump TX_BLK Structure:\n");
+	printk("\tTxFrameType=%d!\n", pTxBlk->TxFrameType);
+	printk("\tTotalFrameLen=%d\n", pTxBlk->TotalFrameLen);
+	printk("\tTotalFrameNum=%ld!\n", pTxBlk->TxPacketList.Number);
+	printk("\tTotalFragNum=%d!\n", pTxBlk->TotalFragNum);
+	printk("\tpPacketList=\n");
+
+	frameNum = pTxBlk->TxPacketList.Number;
+
+	for(i=0; i < frameNum; i++)
+	{	int j;
+		UCHAR	*pBuf;
+
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if (pPacket)
+		{
+			pBuf = GET_OS_PKT_DATAPTR(pPacket);
+			printk("\t\t[%d]:ptr=0x%x, Len=%d!\n", i, (UINT32)(GET_OS_PKT_DATAPTR(pPacket)), GET_OS_PKT_LEN(pPacket));
+			printk("\t\t");
+			for (j =0 ; j < GET_OS_PKT_LEN(pPacket); j++)
+			{
+				printk("%02x ", (pBuf[j] & 0xff));
+				if (j == 16)
+					break;
+			}
+			InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+		}
+	}
+	printk("\tWcid=%d!\n", pTxBlk->Wcid);
+	printk("\tapidx=%d!\n", pTxBlk->apidx);
+	printk("----EndOfDump\n");
+
+}
+#endif
+
+
+/*
+	========================================================================
+	Tx Path design algorithm:
+		Basically, we divide the packets into four types, Broadcast/Multicast, 11N Rate(AMPDU, AMSDU, Normal), B/G Rate(ARALINK, Normal),
+		Specific Packet Type. Following show the classification rule and policy for each kinds of packets.
+				Classification Rule=>
+					Multicast: (*addr1 & 0x01) == 0x01
+					Specific : bDHCPFrame, bARPFrame, bEAPOLFrame, etc.
+					11N Rate : If peer support HT
+								(1).AMPDU  -- If TXBA is negotiated.
+								(2).AMSDU  -- If AMSDU is capable for both peer and ourself.
+											*). AMSDU can embedded in a AMPDU, but now we didn't support it.
+								(3).Normal -- Other packets which send as 11n rate.
+
+					B/G Rate : If peer is b/g only.
+								(1).ARALINK-- If both of peer/us supprot Ralink proprietary Aggregation and the TxRate is large than RATE_6
+								(2).Normal -- Other packets which send as b/g rate.
+					Fragment:
+								The packet must be unicast, NOT A-RALINK, NOT A-MSDU, NOT 11n, then can consider about fragment.
+
+				Classified Packet Handle Rule=>
+					Multicast:
+								No ACK, 		//pTxBlk->bAckRequired = FALSE;
+								No WMM, 		//pTxBlk->bWMM = FALSE;
+								No piggyback,   //pTxBlk->bPiggyBack = FALSE;
+								Force LowRate,  //pTxBlk->bForceLowRate = TRUE;
+					Specific :	Basically, for specific packet, we should handle it specifically, but now all specific packets are use
+									the same policy to handle it.
+								Force LowRate,  //pTxBlk->bForceLowRate = TRUE;
+
+					11N Rate :
+								No piggyback,	//pTxBlk->bPiggyBack = FALSE;
+
+								(1).AMSDU
+									pTxBlk->bWMM = TRUE;
+								(2).AMPDU
+									pTxBlk->bWMM = TRUE;
+								(3).Normal
+
+					B/G Rate :
+								(1).ARALINK
+
+								(2).Normal
+	========================================================================
+*/
+static UCHAR TxPktClassification(
+	IN RTMP_ADAPTER *pAd,
+	IN PNDIS_PACKET  pPacket)
+{
+	UCHAR			TxFrameType = TX_UNKOWN_FRAME;
+	UCHAR			Wcid;
+	MAC_TABLE_ENTRY	*pMacEntry = NULL;
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN			bHTRate = FALSE;
+#endif // DOT11_N_SUPPORT //
+
+	Wcid = RTMP_GET_PACKET_WCID(pPacket);
+	if (Wcid == MCAST_WCID)
+	{	// Handle for RA is Broadcast/Multicast Address.
+		return TX_MCAST_FRAME;
+	}
+
+	// Handle for unicast packets
+	pMacEntry = &pAd->MacTab.Content[Wcid];
+	if (RTMP_GET_PACKET_LOWRATE(pPacket))
+	{	// It's a specific packet need to force low rate, i.e., bDHCPFrame, bEAPOLFrame, bWAIFrame
+		TxFrameType = TX_LEGACY_FRAME;
+	}
+#ifdef DOT11_N_SUPPORT
+	else if (IS_HT_RATE(pMacEntry))
+	{	// it's a 11n capable packet
+
+		// Depends on HTPhyMode to check if the peer support the HTRate transmission.
+		// 	Currently didn't support A-MSDU embedded in A-MPDU
+		bHTRate = TRUE;
+		if (RTMP_GET_PACKET_MOREDATA(pPacket) || (pMacEntry->PsMode == PWR_SAVE))
+			TxFrameType = TX_LEGACY_FRAME;
+#ifdef UAPSD_AP_SUPPORT
+		else if (RTMP_GET_PACKET_EOSP(pPacket))
+			TxFrameType = TX_LEGACY_FRAME;
+#endif // UAPSD_AP_SUPPORT //
+		else if((pMacEntry->TXBAbitmap & (1<<(RTMP_GET_PACKET_UP(pPacket)))) != 0)
+			return TX_AMPDU_FRAME;
+		else if(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_AMSDU_INUSED))
+			return TX_AMSDU_FRAME;
+		else
+			TxFrameType = TX_LEGACY_FRAME;
+	}
+#endif // DOT11_N_SUPPORT //
+	else
+	{	// it's a legacy b/g packet.
+		if ((CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_AGGREGATION_CAPABLE) && pAd->CommonCfg.bAggregationCapable) &&
+			(RTMP_GET_PACKET_TXRATE(pPacket) >= RATE_6) &&
+			(!(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))))
+		{	// if peer support Ralink Aggregation, we use it.
+			TxFrameType = TX_RALINK_FRAME;
+		}
+		else
+		{
+			TxFrameType = TX_LEGACY_FRAME;
+		}
+	}
+
+	// Currently, our fragment only support when a unicast packet send as NOT-ARALINK, NOT-AMSDU and NOT-AMPDU.
+	if ((RTMP_GET_PACKET_FRAGMENTS(pPacket) > 1) && (TxFrameType == TX_LEGACY_FRAME))
+		TxFrameType = TX_FRAG_FRAME;
+
+	return TxFrameType;
+}
+
+
+BOOLEAN RTMP_FillTxBlkInfo(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK *pTxBlk)
+{
+	PACKET_INFO			PacketInfo;
+	PNDIS_PACKET		pPacket;
+	PMAC_TABLE_ENTRY	pMacEntry = NULL;
+
+	pPacket = pTxBlk->pPacket;
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pTxBlk->pSrcBufHeader, &pTxBlk->SrcBufLen);
+
+	pTxBlk->Wcid	 	 		= RTMP_GET_PACKET_WCID(pPacket);
+	pTxBlk->apidx		 		= RTMP_GET_PACKET_IF(pPacket);
+	pTxBlk->UserPriority 		= RTMP_GET_PACKET_UP(pPacket);
+	pTxBlk->FrameGap = IFS_HTTXOP;		// ASIC determine Frame Gap
+
+	if (RTMP_GET_PACKET_CLEAR_EAP_FRAME(pTxBlk->pPacket))
+		TX_BLK_SET_FLAG(pTxBlk, fTX_bClearEAPFrame);
+	else
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bClearEAPFrame);
+
+	// Default to clear this flag
+	TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bForceNonQoS);
+
+
+	if (pTxBlk->Wcid == MCAST_WCID)
+	{
+		pTxBlk->pMacEntry = NULL;
+		{
+#ifdef MCAST_RATE_SPECIFIC
+			PUCHAR pDA = GET_OS_PKT_DATAPTR(pPacket);
+			if (((*pDA & 0x01) == 0x01) && (*pDA != 0xff))
+				pTxBlk->pTransmit = &pAd->CommonCfg.MCastPhyMode;
+			else
+#endif // MCAST_RATE_SPECIFIC //
+				pTxBlk->pTransmit = &pAd->MacTab.Content[MCAST_WCID].HTPhyMode;
+		}
+
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAckRequired);	// AckRequired = FALSE, when broadcast packet in Adhoc mode.
+		//TX_BLK_SET_FLAG(pTxBlk, fTX_bForceLowRate);
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAllowFrag);
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bWMM);
+		if (RTMP_GET_PACKET_MOREDATA(pPacket))
+		{
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bMoreData);
+		}
+
+	}
+	else
+	{
+		pTxBlk->pMacEntry = &pAd->MacTab.Content[pTxBlk->Wcid];
+		pTxBlk->pTransmit = &pTxBlk->pMacEntry->HTPhyMode;
+
+		pMacEntry = pTxBlk->pMacEntry;
+
+
+		// For all unicast packets, need Ack unless the Ack Policy is not set as NORMAL_ACK.
+		if (pAd->CommonCfg.AckPolicy[pTxBlk->QueIdx] != NORMAL_ACK)
+			TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAckRequired);
+		else
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bAckRequired);
+
+		{
+
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			{
+
+				// If support WMM, enable it.
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) &&
+					CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))
+					TX_BLK_SET_FLAG(pTxBlk, fTX_bWMM);
+			}
+#endif // CONFIG_STA_SUPPORT //
+		}
+
+		if (pTxBlk->TxFrameType == TX_LEGACY_FRAME)
+		{
+			if ( (RTMP_GET_PACKET_LOWRATE(pPacket)) ||
+                ((pAd->OpMode == OPMODE_AP) && (pMacEntry->MaxHTPhyMode.field.MODE == MODE_CCK) && (pMacEntry->MaxHTPhyMode.field.MCS == RATE_1)))
+			{	// Specific packet, i.e., bDHCPFrame, bEAPOLFrame, bWAIFrame, need force low rate.
+				pTxBlk->pTransmit = &pAd->MacTab.Content[MCAST_WCID].HTPhyMode;
+#ifdef DOT11_N_SUPPORT
+				// Modify the WMM bit for ICV issue. If we have a packet with EOSP field need to set as 1, how to handle it???
+				if (IS_HT_STA(pTxBlk->pMacEntry) &&
+					(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_RALINK_CHIPSET)) &&
+					((pAd->CommonCfg.bRdg == TRUE) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_RDG_CAPABLE)))
+				{
+					TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bWMM);
+					TX_BLK_SET_FLAG(pTxBlk, fTX_bForceNonQoS);
+				}
+#endif // DOT11_N_SUPPORT //
+			}
+
+#ifdef DOT11_N_SUPPORT
+			if ( (IS_HT_RATE(pMacEntry) == FALSE) &&
+				(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_PIGGYBACK_CAPABLE)))
+			{	// Currently piggy-back only support when peer is operate in b/g mode.
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bPiggyBack);
+			}
+#endif // DOT11_N_SUPPORT //
+
+			if (RTMP_GET_PACKET_MOREDATA(pPacket))
+			{
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bMoreData);
+			}
+#ifdef UAPSD_AP_SUPPORT
+			if (RTMP_GET_PACKET_EOSP(pPacket))
+			{
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bWMM_UAPSD_EOSP);
+			}
+#endif // UAPSD_AP_SUPPORT //
+		}
+		else if (pTxBlk->TxFrameType == TX_FRAG_FRAME)
+		{
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bAllowFrag);
+		}
+
+		pMacEntry->DebugTxCount++;
+	}
+
+	return TRUE;
+
+FillTxBlkErr:
+	return FALSE;
+}
+
+
+BOOLEAN CanDoAggregateTransmit(
+	IN RTMP_ADAPTER *pAd,
+	IN NDIS_PACKET *pPacket,
+	IN TX_BLK		*pTxBlk)
+{
+
+	//printk("Check if can do aggregation! TxFrameType=%d!\n", pTxBlk->TxFrameType);
+
+	if (RTMP_GET_PACKET_WCID(pPacket) == MCAST_WCID)
+		return FALSE;
+
+	if (RTMP_GET_PACKET_DHCP(pPacket) ||
+		RTMP_GET_PACKET_EAPOL(pPacket) ||
+		RTMP_GET_PACKET_WAI(pPacket))
+		return FALSE;
+
+	if ((pTxBlk->TxFrameType == TX_AMSDU_FRAME) &&
+		((pTxBlk->TotalFrameLen + GET_OS_PKT_LEN(pPacket))> (RX_BUFFER_AGGRESIZE - 100)))
+	{	// For AMSDU, allow the packets with total length < max-amsdu size
+		return FALSE;
+	}
+
+	if ((pTxBlk->TxFrameType == TX_RALINK_FRAME) &&
+		(pTxBlk->TxPacketList.Number == 2))
+	{	// For RALINK-Aggregation, allow two frames in one batch.
+		return FALSE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA)) // must be unicast to AP
+		return TRUE;
+	else
+#endif // CONFIG_STA_SUPPORT //
+		return FALSE;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		To do the enqueue operation and extract the first item of waiting
+		list. If a number of available shared memory segments could meet
+		the request of extracted item, the extracted item will be fragmented
+		into shared memory segments.
+
+	Arguments:
+		pAd Pointer to our adapter
+		pQueue		Pointer to Waiting Queue
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPDeQueuePacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN         bIntContext,
+	IN  UCHAR			QIdx, /* BulkOutPipeId */
+	IN  UCHAR           Max_Tx_Packets)
+{
+	PQUEUE_ENTRY    pEntry = NULL;
+	PNDIS_PACKET 	pPacket;
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	UCHAR           Count=0;
+	PQUEUE_HEADER   pQueue;
+	ULONG           FreeNumber[NUM_OF_TX_RING];
+	UCHAR			QueIdx, sQIdx, eQIdx;
+	unsigned long	IrqFlags = 0;
+	BOOLEAN			hasTxDesc = FALSE;
+	TX_BLK			TxBlk;
+	TX_BLK			*pTxBlk;
+
+#ifdef DBG_DIAGNOSE
+	BOOLEAN			firstRound;
+	RtmpDiagStruct	*pDiagStruct = &pAd->DiagStruct;
+#endif
+
+
+	if (QIdx == NUM_OF_TX_RING)
+	{
+		sQIdx = 0;
+		eQIdx = 3;	// 4 ACs, start from 0.
+	}
+	else
+	{
+		sQIdx = eQIdx = QIdx;
+	}
+
+	for (QueIdx=sQIdx; QueIdx <= eQIdx; QueIdx++)
+	{
+		Count=0;
+
+		RT28XX_START_DEQUEUE(pAd, QueIdx, IrqFlags);
+
+#ifdef DBG_DIAGNOSE
+		firstRound = ((QueIdx == 0) ? TRUE : FALSE);
+#endif // DBG_DIAGNOSE //
+
+		while (1)
+		{
+			if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS |
+										fRTMP_ADAPTER_RADIO_OFF |
+										fRTMP_ADAPTER_RESET_IN_PROGRESS |
+										fRTMP_ADAPTER_HALT_IN_PROGRESS |
+										fRTMP_ADAPTER_NIC_NOT_EXIST))))
+			{
+				RT28XX_STOP_DEQUEUE(pAd, QueIdx, IrqFlags);
+				return;
+			}
+
+			if (Count >= Max_Tx_Packets)
+				break;
+
+			DEQUEUE_LOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+			if (&pAd->TxSwQueue[QueIdx] == NULL)
+			{
+#ifdef DBG_DIAGNOSE
+				if (firstRound == TRUE)
+					pDiagStruct->TxSWQueCnt[pDiagStruct->ArrayCurIdx][0]++;
+#endif // DBG_DIAGNOSE //
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+				break;
+			}
+
+
+			// probe the Queue Head
+			pQueue = &pAd->TxSwQueue[QueIdx];
+			if ((pEntry = pQueue->Head) == NULL)
+			{
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+				break;
+			}
+
+			pTxBlk = &TxBlk;
+			NdisZeroMemory((PUCHAR)pTxBlk, sizeof(TX_BLK));
+			//InitializeQueueHeader(&pTxBlk->TxPacketList);		// Didn't need it because we already memzero it.
+			pTxBlk->QueIdx = QueIdx;
+
+			pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+
+			// Early check to make sure we have enoguh Tx Resource.
+			hasTxDesc = RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, FreeNumber[QueIdx], pPacket);
+			if (!hasTxDesc)
+			{
+				pAd->PrivateInfo.TxRingFullCnt++;
+
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+
+				break;
+			}
+
+			pTxBlk->TxFrameType = TxPktClassification(pAd, pPacket);
+			pEntry = RemoveHeadQueue(pQueue);
+			pTxBlk->TotalFrameNum++;
+			pTxBlk->TotalFragNum += RTMP_GET_PACKET_FRAGMENTS(pPacket);	// The real fragment number maybe vary
+			pTxBlk->TotalFrameLen += GET_OS_PKT_LEN(pPacket);
+			pTxBlk->pPacket = pPacket;
+			InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+
+			if (pTxBlk->TxFrameType == TX_RALINK_FRAME || pTxBlk->TxFrameType == TX_AMSDU_FRAME)
+			{
+				// Enhance SW Aggregation Mechanism
+				if (NEED_QUEUE_BACK_FOR_AGG(pAd, QueIdx, FreeNumber[QueIdx], pTxBlk->TxFrameType))
+				{
+					InsertHeadQueue(pQueue, PACKET_TO_QUEUE_ENTRY(pPacket));
+					DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+					break;
+				}
+
+				do{
+					if((pEntry = pQueue->Head) == NULL)
+						break;
+
+					// For TX_AMSDU_FRAME/TX_RALINK_FRAME, Need to check if next pakcet can do aggregation.
+					pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+					FreeNumber[QueIdx] = GET_TXRING_FREENO(pAd, QueIdx);
+					hasTxDesc = RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, FreeNumber[QueIdx], pPacket);
+					if ((hasTxDesc == FALSE) || (CanDoAggregateTransmit(pAd, pPacket, pTxBlk) == FALSE))
+						break;
+
+					//Remove the packet from the TxSwQueue and insert into pTxBlk
+					pEntry = RemoveHeadQueue(pQueue);
+					ASSERT(pEntry);
+					pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+					pTxBlk->TotalFrameNum++;
+					pTxBlk->TotalFragNum += RTMP_GET_PACKET_FRAGMENTS(pPacket);	// The real fragment number maybe vary
+					pTxBlk->TotalFrameLen += GET_OS_PKT_LEN(pPacket);
+					InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+				}while(1);
+
+				if (pTxBlk->TxPacketList.Number == 1)
+					pTxBlk->TxFrameType = TX_LEGACY_FRAME;
+			}
+
+#ifdef RT2870
+			DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+#endif // RT2870 //
+
+			Count += pTxBlk->TxPacketList.Number;
+
+				// Do HardTransmit now.
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				Status = STAHardTransmit(pAd, pTxBlk, QueIdx);
+#endif // CONFIG_STA_SUPPORT //
+
+
+#if 0	// We should not break if HardTransmit failed. Well, at least now we should not!
+			if (Status != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_TRACE /*RT_DEBUG_INFO*/,("RTMPHardTransmit return failed!!!\n"));
+				break;
+			}
+#endif
+		}
+
+		RT28XX_STOP_DEQUEUE(pAd, QueIdx, IrqFlags);
+
+#ifdef RT2870
+		if (!hasTxDesc)
+			RTUSBKickBulkOut(pAd);
+#endif // RT2870 //
+
+#ifdef BLOCK_NET_IF
+		if ((pAd->blockQueueTab[QueIdx].SwTxQueueBlockFlag == TRUE)
+			&& (pAd->TxSwQueue[QueIdx].Number < 1))
+		{
+			releaseNetIf(&pAd->blockQueueTab[QueIdx]);
+		}
+#endif // BLOCK_NET_IF //
+
+	}
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		Rate			Transmit rate
+		Size			Frame size in units of byte
+
+	Return Value:
+		Duration number in units of usec
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+USHORT	RTMPCalcDuration(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Rate,
+	IN	ULONG			Size)
+{
+	ULONG	Duration = 0;
+
+	if (Rate < RATE_FIRST_OFDM_RATE) // CCK
+	{
+		if ((Rate > RATE_1) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED))
+			Duration = 96;	// 72+24 preamble+plcp
+		else
+			Duration = 192; // 144+48 preamble+plcp
+
+		Duration += (USHORT)((Size << 4) / RateIdTo500Kbps[Rate]);
+		if ((Size << 4) % RateIdTo500Kbps[Rate])
+			Duration ++;
+	}
+	else if (Rate <= RATE_LAST_OFDM_RATE)// OFDM rates
+	{
+		Duration = 20 + 6;		// 16+4 preamble+plcp + Signal Extension
+		Duration += 4 * (USHORT)((11 + Size * 4) / RateIdTo500Kbps[Rate]);
+		if ((11 + Size * 4) % RateIdTo500Kbps[Rate])
+			Duration += 4;
+	}
+	else	//mimo rate
+	{
+		Duration = 20 + 6;		// 16+4 preamble+plcp + Signal Extension
+	}
+
+	return (USHORT)Duration;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pTxWI		Pointer to head of each MPDU to HW.
+		Ack 		Setting for Ack requirement bit
+		Fragment	Setting for Fragment bit
+		RetryMode	Setting for retry mode
+		Ifs 		Setting for IFS gap
+		Rate		Setting for transmit rate
+		Service 	Setting for service
+		Length		Frame length
+		TxPreamble	Short or Long preamble when using CCK rates
+		QueIdx - 0-3, according to 802.11e/d4.4 June/2003
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+    See also : BASmartHardTransmit()    !!!
+
+	========================================================================
+*/
+VOID RTMPWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pOutTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			CFACK,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit)
+{
+	PMAC_TABLE_ENTRY	pMac = NULL;
+	TXWI_STRUC 		TxWI;
+	PTXWI_STRUC 	pTxWI;
+
+	if (WCID < MAX_LEN_OF_MAC_TABLE)
+		pMac = &pAd->MacTab.Content[WCID];
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	NdisZeroMemory(&TxWI, TXWI_SIZE);
+	pTxWI = &TxWI;
+
+	pTxWI->FRAG= FRAG;
+
+	pTxWI->CFACK = CFACK;
+	pTxWI->TS= InsTimestamp;
+	pTxWI->AMPDU = AMPDU;
+	pTxWI->ACK = Ack;
+	pTxWI->txop= Txopmode;
+
+	pTxWI->NSEQ = NSeq;
+	// John tune the performace with Intel Client in 20 MHz performance
+#ifdef DOT11_N_SUPPORT
+	BASize = pAd->CommonCfg.TxBASize;
+
+	if( BASize >7 )
+		BASize =7;
+	pTxWI->BAWinSize = BASize;
+	pTxWI->ShortGI = pTransmit->field.ShortGI;
+	pTxWI->STBC = pTransmit->field.STBC;
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->WirelessCliID = WCID;
+	pTxWI->MPDUtotalByteCount = Length;
+	pTxWI->PacketId = PID;
+
+	// If CCK or OFDM, BW must be 20
+	pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+
+	pTxWI->MCS = pTransmit->field.MCS;
+	pTxWI->PHYMODE = pTransmit->field.MODE;
+	pTxWI->CFACK = CfAck;
+
+#ifdef DOT11_N_SUPPORT
+	if (pMac)
+	{
+		if (pAd->CommonCfg.bMIMOPSEnable)
+		{
+			if ((pMac->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+			{
+				// Dynamic MIMO Power Save Mode
+				pTxWI->MIMOps = 1;
+			}
+			else if (pMac->MmpsMode == MMPS_STATIC)
+			{
+				// Static MIMO Power Save Mode
+				if (pTransmit->field.MODE >= MODE_HTMIX && pTransmit->field.MCS > 7)
+				{
+					pTxWI->MCS = 7;
+					pTxWI->MIMOps = 0;
+				}
+			}
+		}
+		//pTxWI->MIMOps = (pMac->PsMode == PWR_MMPS)? 1:0;
+		if (pMac->bIAmBadAtheros && (pMac->WepStatus != Ndis802_11WEPDisabled))
+		{
+			pTxWI->MpduDensity = 7;
+		}
+		else
+		{
+			pTxWI->MpduDensity = pMac->MpduDensity;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->PacketId = pTxWI->MCS;
+	NdisMoveMemory(pOutTxWI, &TxWI, sizeof(TXWI_STRUC));
+}
+
+
+VOID RTMPWriteTxWI_Data(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk)
+{
+	HTTRANSMIT_SETTING	*pTransmit;
+	PMAC_TABLE_ENTRY	pMacEntry;
+#ifdef DOT11_N_SUPPORT
+	UCHAR				BASize;
+#endif // DOT11_N_SUPPORT //
+
+
+	ASSERT(pTxWI);
+
+	pTransmit = pTxBlk->pTransmit;
+	pMacEntry = pTxBlk->pMacEntry;
+
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	NdisZeroMemory(pTxWI, TXWI_SIZE);
+
+	pTxWI->FRAG		= TX_BLK_TEST_FLAG(pTxBlk, fTX_bAllowFrag);
+	pTxWI->ACK		= TX_BLK_TEST_FLAG(pTxBlk, fTX_bAckRequired);
+	pTxWI->txop		= pTxBlk->FrameGap;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+	if (pMacEntry &&
+		(pAd->StaCfg.BssType == BSS_INFRA) &&
+		(pMacEntry->ValidAsDls == TRUE))
+		pTxWI->WirelessCliID = BSSID_WCID;
+	else
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+		pTxWI->WirelessCliID		= pTxBlk->Wcid;
+
+	pTxWI->MPDUtotalByteCount	= pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+	pTxWI->CFACK				= TX_BLK_TEST_FLAG(pTxBlk, fTX_bPiggyBack);
+
+	// If CCK or OFDM, BW must be 20
+	pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+	pTxWI->AMPDU	= ((pTxBlk->TxFrameType == TX_AMPDU_FRAME) ? TRUE : FALSE);
+
+	// John tune the performace with Intel Client in 20 MHz performance
+	BASize = pAd->CommonCfg.TxBASize;
+	if((pTxBlk->TxFrameType == TX_AMPDU_FRAME) && (pMacEntry))
+	{
+		UCHAR		RABAOriIdx = 0;	//The RA's BA Originator table index.
+
+		RABAOriIdx = pTxBlk->pMacEntry->BAOriWcidArray[pTxBlk->UserPriority];
+		BASize = pAd->BATable.BAOriEntry[RABAOriIdx].BAWinSize;
+	}
+
+#if 0 // 3*3
+	if (BASize > 7)
+		BASize = 7;
+#endif
+
+	pTxWI->TxBF = pTransmit->field.TxBF;
+	pTxWI->BAWinSize = BASize;
+	pTxWI->ShortGI = pTransmit->field.ShortGI;
+	pTxWI->STBC = pTransmit->field.STBC;
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->MCS = pTransmit->field.MCS;
+	pTxWI->PHYMODE = pTransmit->field.MODE;
+
+#ifdef DOT11_N_SUPPORT
+	if (pMacEntry)
+	{
+		if ((pMacEntry->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+		{
+			// Dynamic MIMO Power Save Mode
+			pTxWI->MIMOps = 1;
+		}
+		else if (pMacEntry->MmpsMode == MMPS_STATIC)
+		{
+			// Static MIMO Power Save Mode
+			if (pTransmit->field.MODE >= MODE_HTMIX && pTransmit->field.MCS > 7)
+			{
+				pTxWI->MCS = 7;
+				pTxWI->MIMOps = 0;
+			}
+		}
+
+		if (pMacEntry->bIAmBadAtheros && (pMacEntry->WepStatus != Ndis802_11WEPDisabled))
+		{
+			pTxWI->MpduDensity = 7;
+		}
+		else
+		{
+			pTxWI->MpduDensity = pMacEntry->MpduDensity;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DBG_DIAGNOSE
+		if (pTxBlk->QueIdx== 0)
+		{
+			pAd->DiagStruct.TxDataCnt[pAd->DiagStruct.ArrayCurIdx]++;
+			pAd->DiagStruct.TxMcsCnt[pAd->DiagStruct.ArrayCurIdx][pTxWI->MCS]++;
+		}
+#endif // DBG_DIAGNOSE //
+
+	// for rate adapation
+	pTxWI->PacketId = pTxWI->MCS;
+}
+
+
+VOID RTMPWriteTxWI_Cache(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk)
+{
+	PHTTRANSMIT_SETTING	/*pTxHTPhyMode,*/ pTransmit;
+	PMAC_TABLE_ENTRY	pMacEntry;
+
+	//
+	// update TXWI
+	//
+	pMacEntry = pTxBlk->pMacEntry;
+	pTransmit = pTxBlk->pTransmit;
+
+	if (pMacEntry->bAutoTxRateSwitch)
+	{
+		pTxWI->txop = IFS_HTTXOP;
+
+		// If CCK or OFDM, BW must be 20
+		pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+		pTxWI->ShortGI = pTransmit->field.ShortGI;
+		pTxWI->STBC = pTransmit->field.STBC;
+
+		pTxWI->MCS = pTransmit->field.MCS;
+		pTxWI->PHYMODE = pTransmit->field.MODE;
+
+		// set PID for TxRateSwitching
+		pTxWI->PacketId = pTransmit->field.MCS;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	pTxWI->AMPDU = ((pMacEntry->NoBADataCountDown == 0) ? TRUE: FALSE);
+	pTxWI->MIMOps = 0;
+
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+
+	if (pAd->CommonCfg.bMIMOPSEnable)
+	{
+		// MIMO Power Save Mode
+		if ((pMacEntry->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+		{
+			// Dynamic MIMO Power Save Mode
+			pTxWI->MIMOps = 1;
+		}
+		else if (pMacEntry->MmpsMode == MMPS_STATIC)
+		{
+			// Static MIMO Power Save Mode
+			if ((pTransmit->field.MODE >= MODE_HTMIX) && (pTransmit->field.MCS > 7))
+			{
+				pTxWI->MCS = 7;
+				pTxWI->MIMOps = 0;
+			}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DBG_DIAGNOSE
+	if (pTxBlk->QueIdx== 0)
+	{
+		pAd->DiagStruct.TxDataCnt[pAd->DiagStruct.ArrayCurIdx]++;
+		pAd->DiagStruct.TxMcsCnt[pAd->DiagStruct.ArrayCurIdx][pTxWI->MCS]++;
+	}
+#endif // DBG_DIAGNOSE //
+
+	pTxWI->MPDUtotalByteCount = pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pTxD		Pointer to transmit descriptor
+		Ack 		Setting for Ack requirement bit
+		Fragment	Setting for Fragment bit
+		RetryMode	Setting for retry mode
+		Ifs 		Setting for IFS gap
+		Rate		Setting for transmit rate
+		Service 	Setting for service
+		Length		Frame length
+		TxPreamble	Short or Long preamble when using CCK rates
+		QueIdx - 0-3, according to 802.11e/d4.4 June/2003
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID RTMPWriteTxDescriptor(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXD_STRUC		pTxD,
+	IN	BOOLEAN 		bWIV,
+	IN	UCHAR			QueueSEL)
+{
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+
+	pTxD->WIV	= (bWIV) ? 1: 0;
+	pTxD->QSEL= (QueueSEL);
+	//RT2860c??  fixed using EDCA queue for test...  We doubt Queue1 has problem.  2006-09-26 Jan
+	//pTxD->QSEL= FIFO_EDCA;
+	if (pAd->bGenOneHCCA == TRUE)
+		pTxD->QSEL= FIFO_HCCA;
+	pTxD->DMADONE = 0;
+}
+
+
+// should be called only when -
+// 1. MEADIA_CONNECTED
+// 2. AGGREGATION_IN_USED
+// 3. Fragmentation not in used
+// 4. either no previous frame (pPrevAddr1=NULL) .OR. previoud frame is aggregatible
+BOOLEAN TxFrameIsAggregatible(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pPrevAddr1,
+	IN	PUCHAR			p8023hdr)
+{
+
+	// can't aggregate EAPOL (802.1x) frame
+	if ((p8023hdr[12] == 0x88) && (p8023hdr[13] == 0x8e))
+		return FALSE;
+
+	// can't aggregate multicast/broadcast frame
+	if (p8023hdr[0] & 0x01)
+		return FALSE;
+
+	if (INFRA_ON(pAd)) // must be unicast to AP
+		return TRUE;
+	else if ((pPrevAddr1 == NULL) || MAC_ADDR_EQUAL(pPrevAddr1, p8023hdr)) // unicast to same STA
+		return TRUE;
+	else
+		return FALSE;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+	   Check the MSDU Aggregation policy
+	1.HT aggregation is A-MSDU
+	2.legaacy rate aggregation is software aggregation by Ralink.
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN PeerIsAggreOn(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG		   TxRate,
+	IN	PMAC_TABLE_ENTRY pMacEntry)
+{
+	ULONG	AFlags = (fCLIENT_STATUS_AMSDU_INUSED | fCLIENT_STATUS_AGGREGATION_CAPABLE);
+
+	if (pMacEntry != NULL && CLIENT_STATUS_TEST_FLAG(pMacEntry, AFlags))
+	{
+#ifdef DOT11_N_SUPPORT
+		if (pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+		{
+			return TRUE;
+		}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+		if (TxRate >= RATE_6 && pAd->CommonCfg.bAggregationCapable && (!(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))))
+		{	// legacy  Ralink Aggregation support
+			return TRUE;
+		}
+#endif // AGGREGATION_SUPPORT //
+	}
+
+	return FALSE;
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check and fine the packet waiting in SW queue with highest priority
+
+	Arguments:
+		pAd Pointer to our adapter
+
+	Return Value:
+		pQueue		Pointer to Waiting Queue
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+PQUEUE_HEADER	RTMPCheckTxSwQueue(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT PUCHAR			pQueIdx)
+{
+
+	ULONG	Number;
+	// 2004-11-15 to be removed. test aggregation only
+//	if ((OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED)) && (*pNumber < 2))
+//		 return NULL;
+
+	Number = pAd->TxSwQueue[QID_AC_BK].Number
+			 + pAd->TxSwQueue[QID_AC_BE].Number
+			 + pAd->TxSwQueue[QID_AC_VI].Number
+			 + pAd->TxSwQueue[QID_AC_VO].Number
+			 + pAd->TxSwQueue[QID_HCCA].Number;
+
+	if (pAd->TxSwQueue[QID_AC_VO].Head != NULL)
+	{
+		*pQueIdx = QID_AC_VO;
+		return (&pAd->TxSwQueue[QID_AC_VO]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_VI].Head != NULL)
+	{
+		*pQueIdx = QID_AC_VI;
+		return (&pAd->TxSwQueue[QID_AC_VI]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_BE].Head != NULL)
+	{
+		*pQueIdx = QID_AC_BE;
+		return (&pAd->TxSwQueue[QID_AC_BE]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_BK].Head != NULL)
+	{
+		*pQueIdx = QID_AC_BK;
+		return (&pAd->TxSwQueue[QID_AC_BK]);
+	}
+	else if (pAd->TxSwQueue[QID_HCCA].Head != NULL)
+	{
+		*pQueIdx = QID_HCCA;
+		return (&pAd->TxSwQueue[QID_HCCA]);
+	}
+
+	// No packet pending in Tx Sw queue
+	*pQueIdx = QID_AC_BK;
+
+	return (NULL);
+}
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Suspend MSDU transmission
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPSuspendMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE,("SCANNING, suspend MSDU transmission ...\n"));
+
+
+	//
+	// Before BSS_SCAN_IN_PROGRESS, we need to keep Current R66 value and
+	// use Lowbound as R66 value on ScanNextChannel(...)
+	//
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &pAd->BbpTuning.R66CurrentValue);
+
+	// set BBP_R66 to 0x30/0x40 when scanning (AsicSwitchChannel will set R66 according to channel when scanning)
+	//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, (0x26 + GET_LNA_GAIN(pAd)));
+	RTMPSetAGCInitValue(pAd, BW_20);
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+	//RTMP_IO_WRITE32(pAd, TX_CNTL_CSR, 0x000f0000);		// abort all TX rings
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Resume MSDU transmission
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPResumeMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd)
+{
+//    UCHAR			IrqState;
+
+	DBGPRINT(RT_DEBUG_TRACE,("SCAN done, resume MSDU transmission ...\n"));
+
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, pAd->BbpTuning.R66CurrentValue);
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+// sample, for IRQ LOCK to SEM LOCK
+//    IrqState = pAd->irq_disabled;
+//	if (IrqState)
+//		RTMPDeQueuePacket(pAd, TRUE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+//    else
+	RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+}
+
+
+UINT deaggregate_AMSDU_announce(
+	IN	PRTMP_ADAPTER	pAd,
+	PNDIS_PACKET		pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize)
+{
+	USHORT 			PayloadSize;
+	USHORT 			SubFrameSize;
+	PHEADER_802_3 	pAMSDUsubheader;
+	UINT			nMSDU;
+    UCHAR			Header802_3[14];
+
+	PUCHAR			pPayload, pDA, pSA, pRemovedLLCSNAP;
+	PNDIS_PACKET	pClonePacket;
+
+
+
+	nMSDU = 0;
+
+	while (DataSize > LENGTH_802_3)
+	{
+
+		nMSDU++;
+
+		//hex_dump("subheader", pData, 64);
+		pAMSDUsubheader = (PHEADER_802_3)pData;
+		//pData += LENGTH_802_3;
+		PayloadSize = pAMSDUsubheader->Octet[1] + (pAMSDUsubheader->Octet[0]<<8);
+		SubFrameSize = PayloadSize + LENGTH_802_3;
+
+
+		if ((DataSize < SubFrameSize) || (PayloadSize > 1518 ))
+		{
+			break;
+		}
+
+		//printk("%d subframe: Size = %d\n",  nMSDU, PayloadSize);
+
+		pPayload = pData + LENGTH_802_3;
+		pDA = pData;
+		pSA = pData + MAC_ADDR_LEN;
+
+		// convert to 802.3 header
+        CONVERT_TO_802_3(Header802_3, pDA, pSA, pPayload, PayloadSize, pRemovedLLCSNAP);
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((Header802_3[12] == 0x88) && (Header802_3[13] == 0x8E) )
+		{
+		    // avoid local heap overflow, use dyanamic allocation
+		   MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+		   memmove(Elem->Msg+(LENGTH_802_11 + LENGTH_802_1_H), pPayload, PayloadSize);
+		   Elem->MsgLen = LENGTH_802_11 + LENGTH_802_1_H + PayloadSize;
+		   WpaEAPOLKeyAction(pAd, Elem);
+		   kfree(Elem);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+	        	if (pRemovedLLCSNAP)
+	        	{
+	    			pPayload -= LENGTH_802_3;
+	    			PayloadSize += LENGTH_802_3;
+	    			NdisMoveMemory(pPayload, &Header802_3[0], LENGTH_802_3);
+	        	}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		pClonePacket = ClonePacket(pAd, pPacket, pPayload, PayloadSize);
+		if (pClonePacket)
+		{
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pClonePacket, RTMP_GET_PACKET_IF(pPacket));
+#endif // CONFIG_STA_SUPPORT //
+		}
+
+
+		// A-MSDU has padding to multiple of 4 including subframe header.
+		// align SubFrameSize up to multiple of 4
+		SubFrameSize = (SubFrameSize+3)&(~0x3);
+
+
+		if (SubFrameSize > 1528 || SubFrameSize < 32)
+		{
+			break;
+		}
+
+		if (DataSize > SubFrameSize)
+		{
+			pData += SubFrameSize;
+			DataSize -= SubFrameSize;
+		}
+		else
+		{
+			// end of A-MSDU
+			DataSize = 0;
+		}
+	}
+
+	// finally release original rx packet
+	RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+
+	return nMSDU;
+}
+
+
+UINT BA_Reorder_AMSDU_Annnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PUCHAR			pData;
+	USHORT			DataSize;
+	UINT			nMSDU = 0;
+
+	pData = (PUCHAR) GET_OS_PKT_DATAPTR(pPacket);
+	DataSize = (USHORT) GET_OS_PKT_LEN(pPacket);
+
+	nMSDU = deaggregate_AMSDU_announce(pAd, pPacket, pData, DataSize);
+
+	return nMSDU;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Look up the MAC address in the MAC table. Return NULL if not found.
+	Return:
+		pEntry - pointer to the MAC entry; NULL is not found
+	==========================================================================
+*/
+MAC_TABLE_ENTRY *MacTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	PUCHAR pAddr)
+{
+	ULONG HashIdx;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	pEntry = pAd->MacTab.Hash[HashIdx];
+
+	while (pEntry && (pEntry->ValidAsCLI || pEntry->ValidAsWDS || pEntry->ValidAsApCli || pEntry->ValidAsMesh))
+	{
+		if (MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+			break;
+		}
+		else
+			pEntry = pEntry->pNext;
+	}
+
+	return pEntry;
+}
+
+MAC_TABLE_ENTRY *MacTableInsertEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR			pAddr,
+	IN	UCHAR			apidx,
+	IN BOOLEAN	CleanAll)
+{
+	UCHAR HashIdx;
+	int i, FirstWcid;
+	MAC_TABLE_ENTRY *pEntry = NULL, *pCurrEntry;
+
+	// if FULL, return
+	if (pAd->MacTab.Size >= MAX_LEN_OF_MAC_TABLE)
+		return NULL;
+
+	FirstWcid = 1;
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	if (pAd->StaCfg.BssType == BSS_INFRA)
+		FirstWcid = 2;
+#endif // CONFIG_STA_SUPPORT //
+
+	// allocate one MAC entry
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	for (i = FirstWcid; i< MAX_LEN_OF_MAC_TABLE; i++)   // skip entry#0 so that "entry index == AID" for fast lookup
+	{
+		// pick up the first available vacancy
+		if ((pAd->MacTab.Content[i].ValidAsCLI == FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsWDS == FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsApCli== FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsMesh == FALSE)
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			&& (pAd->MacTab.Content[i].ValidAsDls == FALSE)
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			)
+		{
+			pEntry = &pAd->MacTab.Content[i];
+			if (CleanAll == TRUE)
+			{
+				pEntry->MaxSupportedRate = RATE_11;
+				pEntry->CurrTxRate = RATE_11;
+				NdisZeroMemory(pEntry, sizeof(MAC_TABLE_ENTRY));
+				pEntry->PairwiseKey.KeyLen = 0;
+				pEntry->PairwiseKey.CipherAlg = CIPHER_NONE;
+			}
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			if (apidx >= MIN_NET_DEVICE_FOR_DLS)
+			{
+				pEntry->ValidAsCLI = FALSE;
+				pEntry->ValidAsWDS = FALSE;
+				pEntry->ValidAsApCli = FALSE;
+				pEntry->ValidAsMesh = FALSE;
+				pEntry->ValidAsDls = TRUE;
+				pEntry->isCached = FALSE;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			{
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pEntry->ValidAsCLI = TRUE;
+					pEntry->ValidAsWDS = FALSE;
+					pEntry->ValidAsApCli = FALSE;
+					pEntry->ValidAsMesh = FALSE;
+					pEntry->ValidAsDls = FALSE;
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+
+			pEntry->bIAmBadAtheros = FALSE;
+			pEntry->pAd = pAd;
+			pEntry->CMTimerRunning = FALSE;
+			pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+			pEntry->RSNIE_Len = 0;
+			NdisZeroMemory(pEntry->R_Counter, sizeof(pEntry->R_Counter));
+			pEntry->ReTryCounter = PEER_MSG1_RETRY_TIMER_CTR;
+
+			if (pEntry->ValidAsMesh)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_MESH);
+			else if (pEntry->ValidAsApCli)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_APCLI);
+			else if (pEntry->ValidAsWDS)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_WDS);
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			else if (pEntry->ValidAsDls)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_DLS);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			else
+				pEntry->apidx = apidx;
+
+			{
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pEntry->AuthMode = pAd->StaCfg.AuthMode;
+					pEntry->WepStatus = pAd->StaCfg.WepStatus;
+					pEntry->PrivacyFilter = Ndis802_11PrivFilterAcceptAll;
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+
+			pEntry->GTKState = REKEY_NEGOTIATING;
+			pEntry->PairwiseKey.KeyLen = 0;
+			pEntry->PairwiseKey.CipherAlg = CIPHER_NONE;
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			if (pEntry->ValidAsDls == TRUE)
+				pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+#endif //QOS_DLS_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+			pEntry->PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			pEntry->PMKID_CacheIdx = ENTRY_NOT_FOUND;
+			COPY_MAC_ADDR(pEntry->Addr, pAddr);
+			pEntry->Sst = SST_NOT_AUTH;
+			pEntry->AuthState = AS_NOT_AUTH;
+			pEntry->Aid = (USHORT)i;  //0;
+			pEntry->CapabilityInfo = 0;
+			pEntry->PsMode = PWR_ACTIVE;
+			pEntry->PsQIdleCount = 0;
+			pEntry->NoDataIdleCount = 0;
+			pEntry->ContinueTxFailCnt = 0;
+			InitializeQueueHeader(&pEntry->PsQueue);
+
+
+			pAd->MacTab.Size ++;
+			// Add this entry into ASIC RX WCID search table
+			RT28XX_STA_ENTRY_ADD(pAd, pEntry);
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableInsertEntry - allocate entry #%d, Total= %d\n",i, pAd->MacTab.Size));
+			break;
+		}
+	}
+
+	// add this MAC entry into HASH table
+	if (pEntry)
+	{
+		HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+		if (pAd->MacTab.Hash[HashIdx] == NULL)
+		{
+			pAd->MacTab.Hash[HashIdx] = pEntry;
+		}
+		else
+		{
+			pCurrEntry = pAd->MacTab.Hash[HashIdx];
+			while (pCurrEntry->pNext != NULL)
+				pCurrEntry = pCurrEntry->pNext;
+			pCurrEntry->pNext = pEntry;
+		}
+	}
+
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+	return pEntry;
+}
+
+/*
+	==========================================================================
+	Description:
+		Delete a specified client from MAC table
+	==========================================================================
+ */
+BOOLEAN MacTableDeleteEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr)
+{
+	USHORT HashIdx;
+	MAC_TABLE_ENTRY *pEntry, *pPrevEntry, *pProbeEntry;
+	BOOLEAN Cancelled;
+	//USHORT	offset;	// unused variable
+	//UCHAR	j;			// unused variable
+
+	if (wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	//pEntry = pAd->MacTab.Hash[HashIdx];
+	pEntry = &pAd->MacTab.Content[wcid];
+
+	if (pEntry && (pEntry->ValidAsCLI || pEntry->ValidAsApCli || pEntry->ValidAsWDS || pEntry->ValidAsMesh
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+ 		|| pEntry->ValidAsDls
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+		))
+	{
+		if (MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+
+			// Delete this entry from ASIC on-chip WCID Table
+			RT28XX_STA_ENTRY_MAC_RESET(pAd, wcid);
+
+#ifdef DOT11_N_SUPPORT
+			// free resources of BA
+			BASessionTearDownALL(pAd, pEntry->Aid);
+#endif // DOT11_N_SUPPORT //
+
+
+			pPrevEntry = NULL;
+			pProbeEntry = pAd->MacTab.Hash[HashIdx];
+			ASSERT(pProbeEntry);
+
+			// update Hash list
+			do
+			{
+				if (pProbeEntry == pEntry)
+				{
+					if (pPrevEntry == NULL)
+					{
+						pAd->MacTab.Hash[HashIdx] = pEntry->pNext;
+					}
+					else
+					{
+						pPrevEntry->pNext = pEntry->pNext;
+					}
+					break;
+				}
+
+				pPrevEntry = pProbeEntry;
+				pProbeEntry = pProbeEntry->pNext;
+			} while (pProbeEntry);
+
+			// not found !!!
+			ASSERT(pProbeEntry != NULL);
+
+			RT28XX_STA_ENTRY_KEY_DEL(pAd, BSS0, wcid);
+
+
+		if (pEntry->EnqueueEapolStartTimerRunning != EAPOL_START_DISABLE)
+		{
+			RTMPCancelTimer(&pEntry->EnqueueStartForPSKTimer, &Cancelled);
+			pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+		}
+
+
+   			NdisZeroMemory(pEntry, sizeof(MAC_TABLE_ENTRY));
+			pAd->MacTab.Size --;
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableDeleteEntry1 - Total= %d\n", pAd->MacTab.Size));
+		}
+		else
+		{
+			printk("\n%s: Impossible Wcid = %d !!!!!\n", __FUNCTION__, wcid);
+		}
+	}
+
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	//Reset operating mode when no Sta.
+	if (pAd->MacTab.Size == 0)
+	{
+#ifdef DOT11_N_SUPPORT
+		pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode = 0;
+#endif // DOT11_N_SUPPORT //
+		AsicUpdateProtect(pAd, 0 /*pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode*/, (ALLN_SETPROTECT), TRUE, 0 /*pAd->MacTab.fAnyStationNonGF*/);
+	}
+
+	return TRUE;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		This routine reset the entire MAC table. All packets pending in
+		the power-saving queues are freed here.
+	==========================================================================
+ */
+VOID MacTableReset(
+	IN  PRTMP_ADAPTER  pAd)
+{
+	int         i;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MacTableReset\n"));
+	//NdisAcquireSpinLock(&pAd->MacTabLock);
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		if (pAd->MacTab.Content[i].ValidAsCLI == TRUE)
+	   {
+
+#ifdef DOT11_N_SUPPORT
+			// free resources of BA
+			BASessionTearDownALL(pAd, i);
+#endif // DOT11_N_SUPPORT //
+
+			pAd->MacTab.Content[i].ValidAsCLI = FALSE;
+
+
+
+#ifdef RT2870
+			NdisZeroMemory(pAd->MacTab.Content[i].Addr, 6);
+			RT28XX_STA_ENTRY_MAC_RESET(pAd, i);
+#endif // RT2870 //
+
+			//AsicDelWcidTab(pAd, i);
+		}
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID AssocParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_ASSOC_REQ_STRUCT *AssocReq,
+	IN PUCHAR                     pAddr,
+	IN USHORT                     CapabilityInfo,
+	IN ULONG                      Timeout,
+	IN USHORT                     ListenIntv)
+{
+	COPY_MAC_ADDR(AssocReq->Addr, pAddr);
+	// Add mask to support 802.11b mode only
+	AssocReq->CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO; // not cf-pollable, not cf-poll-request
+	AssocReq->Timeout = Timeout;
+	AssocReq->ListenIntv = ListenIntv;
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID DisassocParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DISASSOC_REQ_STRUCT *DisassocReq,
+	IN PUCHAR pAddr,
+	IN USHORT Reason)
+{
+	COPY_MAC_ADDR(DisassocReq->Addr, pAddr);
+	DisassocReq->Reason = Reason;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check the out going frame, if this is an DHCP or ARP datagram
+	will be duplicate another frame at low data rate transmit.
+
+	Arguments:
+		pAd 		Pointer to our adapter
+		pPacket 	Pointer to outgoing Ndis frame
+
+	Return Value:
+		TRUE		To be duplicate at Low data rate transmit. (1mb)
+		FALSE		Do nothing.
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+		MAC header + IP Header + UDP Header
+		  14 Bytes	  20 Bytes
+
+		UDP Header
+		00|01|02|03|04|05|06|07|08|09|10|11|12|13|14|15|
+						Source Port
+		16|17|18|19|20|21|22|23|24|25|26|27|28|29|30|31|
+					Destination Port
+
+		port 0x43 means Bootstrap Protocol, server.
+		Port 0x44 means Bootstrap Protocol, client.
+
+	========================================================================
+*/
+
+BOOLEAN RTMPCheckDHCPFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	ULONG			NumberOfBytesRead = 0;
+	ULONG			CurrentOffset = 0;
+	PVOID			pVirtualAddress = NULL;
+	UINT			NdisBufferLength;
+	PUCHAR			pSrc;
+	USHORT			Protocol;
+	UCHAR			ByteOffset36 = 0;
+	UCHAR			ByteOffset38 = 0;
+	BOOLEAN 		ReadFirstParm = TRUE;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, (PUCHAR *)&pVirtualAddress, &NdisBufferLength);
+
+	NumberOfBytesRead += NdisBufferLength;
+	pSrc = (PUCHAR) pVirtualAddress;
+	Protocol = *(pSrc + 12) * 256 + *(pSrc + 13);
+
+	//
+	// Check DHCP & BOOTP protocol
+	//
+	while (NumberOfBytesRead <= PacketInfo.TotalPacketLength)
+	{
+		if ((NumberOfBytesRead >= 35) && (ReadFirstParm == TRUE))
+		{
+			CurrentOffset = 35 - (NumberOfBytesRead - NdisBufferLength);
+			ByteOffset36 = *(pSrc + CurrentOffset);
+			ReadFirstParm = FALSE;
+		}
+
+		if (NumberOfBytesRead >= 37)
+		{
+			CurrentOffset = 37 - (NumberOfBytesRead - NdisBufferLength);
+			ByteOffset38 = *(pSrc + CurrentOffset);
+			//End of Read
+			break;
+		}
+		return FALSE;
+	}
+
+	// Check for DHCP & BOOTP protocol
+	if ((ByteOffset36 != 0x44) || (ByteOffset38 != 0x43))
+		{
+		//
+		// 2054 (hex 0806) for ARP datagrams
+		// if this packet is not ARP datagrams, then do nothing
+		// ARP datagrams will also be duplicate at 1mb broadcast frames
+		//
+		if (Protocol != 0x0806 )
+			return FALSE;
+		}
+
+	return TRUE;
+}
+
+
+BOOLEAN RTMPCheckEtherType(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	USHORT	TypeLen;
+	UCHAR	Byte0, Byte1;
+	PUCHAR	pSrcBuf;
+	UINT32	pktLen;
+	UINT16 	srcPort, dstPort;
+	BOOLEAN	status = TRUE;
+
+
+	pSrcBuf = GET_OS_PKT_DATAPTR(pPacket);
+	pktLen = GET_OS_PKT_LEN(pPacket);
+
+	ASSERT(pSrcBuf);
+
+	RTMP_SET_PACKET_SPECIFIC(pPacket, 0);
+
+	// get Ethernet protocol field
+	TypeLen = (pSrcBuf[12] << 8) + pSrcBuf[13];
+
+	pSrcBuf += LENGTH_802_3;	// Skip the Ethernet Header.
+
+	if (TypeLen <= 1500)
+	{	// 802.3, 802.3 LLC
+		/*
+			DestMAC(6) + SrcMAC(6) + Lenght(2) +
+			DSAP(1) + SSAP(1) + Control(1) +
+			if the DSAP = 0xAA, SSAP=0xAA, Contorl = 0x03, it has a 5-bytes SNAP header.
+				=> + SNAP (5, OriginationID(3) + etherType(2))
+		*/
+		if (pSrcBuf[0] == 0xAA && pSrcBuf[1] == 0xAA && pSrcBuf[2] == 0x03)
+		{
+			Sniff2BytesFromNdisBuffer(pSrcBuf, 6, &Byte0, &Byte1);
+			RTMP_SET_PACKET_LLCSNAP(pPacket, 1);
+			TypeLen = (USHORT)((Byte0 << 8) + Byte1);
+			pSrcBuf += 8; // Skip this LLC/SNAP header
+		}
+		else
+		{
+			//It just has 3-byte LLC header, maybe a legacy ether type frame. we didn't handle it.
+		}
+	}
+
+	// If it's a VLAN packet, get the real Type/Length field.
+	if (TypeLen == 0x8100)
+	{
+		/* 0x8100 means VLAN packets */
+
+		/* Dest. MAC Address (6-bytes) +
+		   Source MAC Address (6-bytes) +
+		   Length/Type = 802.1Q Tag Type (2-byte) +
+		   Tag Control Information (2-bytes) +
+		   Length / Type (2-bytes) +
+		   data payload (0-n bytes) +
+		   Pad (0-p bytes) +
+		   Frame Check Sequence (4-bytes) */
+
+		RTMP_SET_PACKET_VLAN(pPacket, 1);
+		Sniff2BytesFromNdisBuffer(pSrcBuf, 2, &Byte0, &Byte1);
+		TypeLen = (USHORT)((Byte0 << 8) + Byte1);
+
+		pSrcBuf += 4; // Skip the VLAN Header.
+	}
+
+	switch (TypeLen)
+	{
+		case 0x0800:
+			{
+				ASSERT((pktLen > 34));
+				if (*(pSrcBuf + 9) == 0x11)
+				{	// udp packet
+					ASSERT((pktLen > 34));	// 14 for ethernet header, 20 for IP header
+
+					pSrcBuf += 20;	// Skip the IP header
+					srcPort = OS_NTOHS(*((UINT16 *)pSrcBuf));
+					dstPort = OS_NTOHS(*((UINT16 *)(pSrcBuf +2)));
+
+					if ((srcPort==0x44 && dstPort==0x43) || (srcPort==0x43 && dstPort==0x44))
+					{	//It's a BOOTP/DHCP packet
+						RTMP_SET_PACKET_DHCP(pPacket, 1);
+					}
+				}
+			}
+			break;
+		case 0x0806:
+			{
+				//ARP Packet.
+				RTMP_SET_PACKET_DHCP(pPacket, 1);
+			}
+			break;
+		case 0x888e:
+			{
+				// EAPOL Packet.
+				RTMP_SET_PACKET_EAPOL(pPacket, 1);
+			}
+			break;
+		default:
+			status = FALSE;
+			break;
+	}
+
+	return status;
+
+}
+
+
+
+VOID Update_Rssi_Sample(
+	IN PRTMP_ADAPTER	pAd,
+	IN RSSI_SAMPLE		*pRssi,
+	IN PRXWI_STRUC		pRxWI)
+		{
+	CHAR	rssi0 = pRxWI->RSSI0;
+	CHAR	rssi1 = pRxWI->RSSI1;
+	CHAR	rssi2 = pRxWI->RSSI2;
+
+	if (rssi0 != 0)
+	{
+		pRssi->LastRssi0	= ConvertToRssi(pAd, (CHAR)rssi0, RSSI_0);
+		pRssi->AvgRssi0X8	= (pRssi->AvgRssi0X8 - pRssi->AvgRssi0) + pRssi->LastRssi0;
+		pRssi->AvgRssi0	= pRssi->AvgRssi0X8 >> 3;
+	}
+
+	if (rssi1 != 0)
+	{
+		pRssi->LastRssi1	= ConvertToRssi(pAd, (CHAR)rssi1, RSSI_1);
+		pRssi->AvgRssi1X8	= (pRssi->AvgRssi1X8 - pRssi->AvgRssi1) + pRssi->LastRssi1;
+		pRssi->AvgRssi1	= pRssi->AvgRssi1X8 >> 3;
+	}
+
+	if (rssi2 != 0)
+	{
+		pRssi->LastRssi2	= ConvertToRssi(pAd, (CHAR)rssi2, RSSI_2);
+		pRssi->AvgRssi2X8  = (pRssi->AvgRssi2X8 - pRssi->AvgRssi2) + pRssi->LastRssi2;
+		pRssi->AvgRssi2 = pRssi->AvgRssi2X8 >> 3;
+	}
+}
+
+
+
+// Normal legacy Rx packet indication
+VOID Indicate_Legacy_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+	UCHAR			Header802_3[LENGTH_802_3];
+
+	// 1. get 802.3 Header
+	// 2. remove LLC
+	// 		a. pointer pRxBlk->pData to payload
+	//      b. modify pRxBlk->DataSize
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pRxBlk->DataSize > MAX_RX_PKT_LEN)
+	{
+#if 0 // sample take off, for multiple card design
+		static int err_size;
+
+		err_size++;
+		if (err_size > 20)
+		{
+			 printk("Legacy DataSize = %d\n", pRxBlk->DataSize);
+			 hex_dump("802.3 Header", Header802_3, LENGTH_802_3);
+			 hex_dump("Payload", pRxBlk->pData, 64);
+			 err_size = 0;
+		}
+#endif
+
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+
+	STATS_INC_RX_PACKETS(pAd, FromWhichBSSID);
+
+#ifdef RT2870
+#ifdef DOT11_N_SUPPORT
+	if (pAd->CommonCfg.bDisableReordering == 0)
+	{
+		PBA_REC_ENTRY		pBAEntry;
+		ULONG				Now32;
+		UCHAR				Wcid = pRxBlk->pRxWI->WirelessCliID;
+		UCHAR				TID = pRxBlk->pRxWI->TID;
+		USHORT				Idx;
+
+#define REORDERING_PACKET_TIMEOUT		((100 * HZ)/1000)	// system ticks -- 100 ms
+
+		if (Wcid < MAX_LEN_OF_MAC_TABLE)
+		{
+			Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+			if (Idx != 0)
+			{
+				pBAEntry = &pAd->BATable.BARecEntry[Idx];
+				// update last rx time
+				NdisGetSystemUpTime(&Now32);
+				if ((pBAEntry->list.qlen > 0) &&
+					 RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(REORDERING_PACKET_TIMEOUT)))
+	   				)
+				{
+					printk("Indicate_Legacy_Packet():flush reordering_timeout_mpdus! RxWI->Flags=%d, pRxWI.TID=%d, RxD->AMPDU=%d!\n", pRxBlk->Flags, pRxBlk->pRxWI->TID, pRxBlk->RxD.AMPDU);
+					hex_dump("Dump the legacy Packet:", GET_OS_PKT_DATAPTR(pRxBlk->pRxPacket), 64);
+					ba_flush_reordering_timeout_mpdus(pAd, pBAEntry, Now32);
+				}
+			}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+#endif // RT2870 //
+
+	wlan_802_11_to_802_3_packet(pAd, pRxBlk, Header802_3, FromWhichBSSID);
+
+	//
+	// pass this 802.3 packet to upper layer or forward this packet to WM directly
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pRxPacket, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+
+// Normal, AMPDU or AMSDU
+VOID CmmRxnonRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+#ifdef DOT11_N_SUPPORT
+	if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMPDU) && (pAd->CommonCfg.bDisableReordering == 0))
+	{
+		Indicate_AMPDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+	}
+	else
+#endif // DOT11_N_SUPPORT //
+	{
+#ifdef DOT11_N_SUPPORT
+		if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU))
+		{
+			// handle A-MSDU
+			Indicate_AMSDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+	}
+}
+
+
+VOID CmmRxRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	UCHAR			Header802_3[LENGTH_802_3];
+	UINT16			Msdu2Size;
+	UINT16 			Payload1Size, Payload2Size;
+	PUCHAR 			pData2;
+	PNDIS_PACKET	pPacket2 = NULL;
+
+
+
+	Msdu2Size = *(pRxBlk->pData) + (*(pRxBlk->pData+1) << 8);
+
+	if ((Msdu2Size <= 1536) && (Msdu2Size < pRxBlk->DataSize))
+	{
+		/* skip two byte MSDU2 len */
+		pRxBlk->pData += 2;
+		pRxBlk->DataSize -= 2;
+	}
+	else
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	// get 802.3 Header and  remove LLC
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+
+	ASSERT(pRxBlk->pRxPacket);
+
+	// Ralink Aggregation frame
+	pAd->RalinkCounters.OneSecRxAggregationCount ++;
+	Payload1Size = pRxBlk->DataSize - Msdu2Size;
+	Payload2Size = Msdu2Size - LENGTH_802_3;
+
+	pData2 = pRxBlk->pData + Payload1Size + LENGTH_802_3;
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		pPacket2 = duplicate_pkt(pAd, (pData2-LENGTH_802_3), LENGTH_802_3, pData2, Payload2Size, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (!pPacket2)
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	// update payload size of 1st packet
+	pRxBlk->DataSize = Payload1Size;
+	wlan_802_11_to_802_3_packet(pAd, pRxBlk, Header802_3, FromWhichBSSID);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pRxBlk->pRxPacket, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pPacket2)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pPacket2, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+
+#define RESET_FRAGFRAME(_fragFrame) \
+	{								\
+		_fragFrame.RxSize = 0;		\
+		_fragFrame.Sequence = 0;	\
+		_fragFrame.LastFrag = 0;	\
+		_fragFrame.Flags = 0;		\
+	}
+
+
+PNDIS_PACKET RTMPDeFragmentDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+	UCHAR			*pData = pRxBlk->pData;
+	USHORT			DataSize = pRxBlk->DataSize;
+	PNDIS_PACKET	pRetPacket = NULL;
+	UCHAR			*pFragBuffer = NULL;
+	BOOLEAN 		bReassDone = FALSE;
+	UCHAR			HeaderRoom = 0;
+
+
+	ASSERT(pHeader);
+
+	HeaderRoom = pData - (UCHAR *)pHeader;
+
+	// Re-assemble the fragmented packets
+	if (pHeader->Frag == 0)		// Frag. Number is 0 : First frag or only one pkt
+	{
+		// the first pkt of fragment, record it.
+		if (pHeader->FC.MoreFrag)
+		{
+			ASSERT(pAd->FragFrame.pFragPacket);
+			pFragBuffer = GET_OS_PKT_DATAPTR(pAd->FragFrame.pFragPacket);
+			pAd->FragFrame.RxSize   = DataSize + HeaderRoom;
+			NdisMoveMemory(pFragBuffer,	 pHeader, pAd->FragFrame.RxSize);
+			pAd->FragFrame.Sequence = pHeader->Sequence;
+			pAd->FragFrame.LastFrag = pHeader->Frag;	   // Should be 0
+			ASSERT(pAd->FragFrame.LastFrag == 0);
+			goto done;	// end of processing this frame
+		}
+	}
+	else	//Middle & End of fragment
+	{
+		if ((pHeader->Sequence != pAd->FragFrame.Sequence) ||
+			(pHeader->Frag != (pAd->FragFrame.LastFrag + 1)))
+		{
+			// Fragment is not the same sequence or out of fragment number order
+			// Reset Fragment control blk
+			RESET_FRAGFRAME(pAd->FragFrame);
+			DBGPRINT(RT_DEBUG_ERROR, ("Fragment is not the same sequence or out of fragment number order.\n"));
+			goto done; // give up this frame
+		}
+		else if ((pAd->FragFrame.RxSize + DataSize) > MAX_FRAME_SIZE)
+		{
+			// Fragment frame is too large, it exeeds the maximum frame size.
+			// Reset Fragment control blk
+			RESET_FRAGFRAME(pAd->FragFrame);
+			DBGPRINT(RT_DEBUG_ERROR, ("Fragment frame is too large, it exeeds the maximum frame size.\n"));
+			goto done; // give up this frame
+		}
+
+        //
+		// Broadcom AP(BCM94704AGR) will send out LLC in fragment's packet, LLC only can accpet at first fragment.
+		// In this case, we will dropt it.
+		//
+		if (NdisEqualMemory(pData, SNAP_802_1H, sizeof(SNAP_802_1H)))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Find another LLC at Middle or End fragment(SN=%d, Frag=%d)\n", pHeader->Sequence, pHeader->Frag));
+			goto done; // give up this frame
+		}
+
+		pFragBuffer = GET_OS_PKT_DATAPTR(pAd->FragFrame.pFragPacket);
+
+		// concatenate this fragment into the re-assembly buffer
+		NdisMoveMemory((pFragBuffer + pAd->FragFrame.RxSize), pData, DataSize);
+		pAd->FragFrame.RxSize  += DataSize;
+		pAd->FragFrame.LastFrag = pHeader->Frag;	   // Update fragment number
+
+		// Last fragment
+		if (pHeader->FC.MoreFrag == FALSE)
+		{
+			bReassDone = TRUE;
+		}
+	}
+
+done:
+	// always release rx fragmented packet
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+
+	// return defragmented packet if packet is reassembled completely
+	// otherwise return NULL
+	if (bReassDone)
+	{
+		PNDIS_PACKET pNewFragPacket;
+
+		// allocate a new packet buffer for fragment
+		pNewFragPacket = RTMP_AllocateFragPacketBuffer(pAd, RX_BUFFER_NORMSIZE);
+		if (pNewFragPacket)
+		{
+			// update RxBlk
+			pRetPacket = pAd->FragFrame.pFragPacket;
+			pAd->FragFrame.pFragPacket = pNewFragPacket;
+			pRxBlk->pHeader = (PHEADER_802_11) GET_OS_PKT_DATAPTR(pRetPacket);
+			pRxBlk->pData = (UCHAR *)pRxBlk->pHeader + HeaderRoom;
+			pRxBlk->DataSize = pAd->FragFrame.RxSize - HeaderRoom;
+			pRxBlk->pRxPacket = pRetPacket;
+		}
+		else
+		{
+			RESET_FRAGFRAME(pAd->FragFrame);
+		}
+	}
+
+	return pRetPacket;
+}
+
+
+VOID Indicate_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	UINT			nMSDU;
+
+	update_os_packet_info(pAd, pRxBlk, FromWhichBSSID);
+	RTMP_SET_PACKET_IF(pRxBlk->pRxPacket, FromWhichBSSID);
+	nMSDU = deaggregate_AMSDU_announce(pAd, pRxBlk->pRxPacket, pRxBlk->pData, pRxBlk->DataSize);
+}
+
+VOID Indicate_EAPOL_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pEntry = &pAd->MacTab.Content[BSSID_WCID];
+		STARxEAPOLFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pEntry == NULL)
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("Indicate_EAPOL_Packet: drop and release the invalid packet.\n"));
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+}
+
+#define BCN_TBTT_OFFSET		64	//defer 64 us
+VOID ReSyncBeaconTime(
+	IN  PRTMP_ADAPTER   pAd)
+{
+
+	UINT32  Offset;
+
+
+	Offset = (pAd->TbttTickCount) % (BCN_TBTT_OFFSET);
+
+	pAd->TbttTickCount++;
+
+	//
+	// The updated BeaconInterval Value will affect Beacon Interval after two TBTT
+	// beacasue the original BeaconInterval had been loaded into next TBTT_TIMER
+	//
+	if (Offset == (BCN_TBTT_OFFSET-2))
+	{
+		BCN_TIME_CFG_STRUC csr;
+		RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+		csr.field.BeaconInterval = (pAd->CommonCfg.BeaconPeriod << 4) - 1 ;	// ASIC register in units of 1/16 TU = 64us
+		RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+	}
+	else
+	{
+		if (Offset == (BCN_TBTT_OFFSET-1))
+		{
+			BCN_TIME_CFG_STRUC csr;
+
+			RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+			csr.field.BeaconInterval = (pAd->CommonCfg.BeaconPeriod) << 4; // ASIC register in units of 1/16 TU
+			RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+		}
+	}
+}
+
diff --git a/drivers/staging/rt2870/common/cmm_data_2870.c b/drivers/staging/rt2870/common/cmm_data_2870.c
new file mode 100644
index 00000000000..f77000f336a
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_data_2870.c
@@ -0,0 +1,963 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+/*
+   All functions in this file must be USB-depended, or you should out your function
+	in other files.
+
+*/
+#include "../rt_config.h"
+
+
+/*
+	We can do copy the frame into pTxContext when match following conditions.
+		=>
+		=>
+		=>
+*/
+static inline NDIS_STATUS RtmpUSBCanDoWrite(
+	IN RTMP_ADAPTER		*pAd,
+	IN UCHAR			QueIdx,
+	IN HT_TX_CONTEXT 	*pHTTXContext)
+{
+	NDIS_STATUS	canWrite = NDIS_STATUS_RESOURCES;
+
+	if (((pHTTXContext->CurWritePosition) < pHTTXContext->NextBulkOutPosition) && (pHTTXContext->CurWritePosition + LOCAL_TXBUF_SIZE) > pHTTXContext->NextBulkOutPosition)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("RtmpUSBCanDoWrite c1!\n"));
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << QueIdx));
+	}
+	else if ((pHTTXContext->CurWritePosition == 8) && (pHTTXContext->NextBulkOutPosition < LOCAL_TXBUF_SIZE))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("RtmpUSBCanDoWrite c2!\n"));
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << QueIdx));
+	}
+	else if (pHTTXContext->bCurWriting == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("RtmpUSBCanDoWrite c3!\n"));
+	}
+	else
+	{
+		canWrite = NDIS_STATUS_SUCCESS;
+	}
+
+
+	return canWrite;
+}
+
+
+USHORT RtmpUSB_WriteSubTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber)
+{
+
+	// Dummy function. Should be removed in the future.
+	return 0;
+
+}
+
+USHORT	RtmpUSB_WriteFragTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			fragNum,
+	OUT	USHORT			*FreeNumber)
+{
+	HT_TX_CONTEXT	*pHTTXContext;
+	USHORT			hwHdrLen;	// The hwHdrLen consist of 802.11 header length plus the header padding length.
+	UINT32			fillOffset;
+	TXINFO_STRUC	*pTxInfo;
+	TXWI_STRUC		*pTxWI;
+	PUCHAR			pWirelessPacket = NULL;
+	UCHAR			QueIdx;
+	NDIS_STATUS		Status;
+	unsigned long	IrqFlags;
+	UINT32			USBDMApktLen = 0, DMAHdrLen, padding;
+	BOOLEAN			TxQLastRound = FALSE;
+
+	//
+	// get Tx Ring Resource & Dma Buffer address
+	//
+	QueIdx = pTxBlk->QueIdx;
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+	fillOffset = pHTTXContext->CurWritePosition;
+
+	if(fragNum == 0)
+	{
+		// Check if we have enough space for this bulk-out batch.
+		Status = RtmpUSBCanDoWrite(pAd, QueIdx, pHTTXContext);
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			pHTTXContext->bCurWriting = TRUE;
+
+			// Reserve space for 8 bytes padding.
+			if ((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition))
+			{
+				pHTTXContext->ENextBulkOutPosition += 8;
+				pHTTXContext->CurWritePosition += 8;
+				fillOffset += 8;
+			}
+			pTxBlk->Priv = 0;
+			pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			return(Status);
+		}
+	}
+	else
+	{
+		// For sub-sequent frames of this bulk-out batch. Just copy it to our bulk-out buffer.
+		Status = ((pHTTXContext->bCurWriting == TRUE) ? NDIS_STATUS_SUCCESS : NDIS_STATUS_FAILURE);
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			fillOffset += pTxBlk->Priv;
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			return(Status);
+		}
+	}
+
+	NdisZeroMemory((PUCHAR)(&pTxBlk->HeaderBuf[0]), TXINFO_SIZE);
+	pTxInfo = (PTXINFO_STRUC)(&pTxBlk->HeaderBuf[0]);
+	pTxWI= (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]);
+
+	pWirelessPacket = &pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset];
+
+	// copy TXWI + WLAN Header + LLC into DMA Header Buffer
+	//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen, 4);
+	hwHdrLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+	// Build our URB for USBD
+	DMAHdrLen = TXWI_SIZE + hwHdrLen;
+	USBDMApktLen = DMAHdrLen + pTxBlk->SrcBufLen;
+	padding = (4 - (USBDMApktLen % 4)) & 0x03;	// round up to 4 byte alignment
+	USBDMApktLen += padding;
+
+	pTxBlk->Priv += (TXINFO_SIZE + USBDMApktLen);
+
+	// For TxInfo, the length of USBDMApktLen = TXWI_SIZE + 802.11 header + payload
+	RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(USBDMApktLen), FALSE, FIFO_EDCA, FALSE /*NextValid*/,  FALSE);
+
+	if (fragNum == pTxBlk->TotalFragNum)
+	{
+		pTxInfo->USBDMATxburst = 0;
+		if ((pHTTXContext->CurWritePosition + pTxBlk->Priv + 3906)> MAX_TXBULK_LIMIT)
+		{
+			pTxInfo->SwUseLastRound = 1;
+			TxQLastRound = TRUE;
+		}
+	}
+	else
+	{
+		pTxInfo->USBDMATxburst = 1;
+	}
+
+	NdisMoveMemory(pWirelessPacket, pTxBlk->HeaderBuf, TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)(pWirelessPacket + TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+#endif // RT_BIG_ENDIAN //
+	pWirelessPacket += (TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+	pHTTXContext->CurWriteRealPos += (TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	NdisMoveMemory(pWirelessPacket, pTxBlk->pSrcBufData, pTxBlk->SrcBufLen);
+
+	//	Zero the last padding.
+	pWirelessPacket += pTxBlk->SrcBufLen;
+	NdisZeroMemory(pWirelessPacket, padding + 8);
+
+	if (fragNum == pTxBlk->TotalFragNum)
+	{
+		RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+		// Update the pHTTXContext->CurWritePosition. 3906 used to prevent the NextBulkOut is a A-RALINK/A-MSDU Frame.
+		pHTTXContext->CurWritePosition += pTxBlk->Priv;
+		if (TxQLastRound == TRUE)
+			pHTTXContext->CurWritePosition = 8;
+		pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+
+		// Finally, set bCurWriting as FALSE
+	pHTTXContext->bCurWriting = FALSE;
+
+		RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+		// succeed and release the skb buffer
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_SUCCESS);
+	}
+
+
+	return(Status);
+
+}
+
+
+USHORT RtmpUSB_WriteSingleTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber)
+{
+	HT_TX_CONTEXT	*pHTTXContext;
+	USHORT			hwHdrLen;
+	UINT32			fillOffset;
+	TXINFO_STRUC	*pTxInfo;
+	TXWI_STRUC		*pTxWI;
+	PUCHAR			pWirelessPacket;
+	UCHAR			QueIdx;
+	unsigned long	IrqFlags;
+	NDIS_STATUS		Status;
+	UINT32			USBDMApktLen = 0, DMAHdrLen, padding;
+	BOOLEAN			bTxQLastRound = FALSE;
+
+	// For USB, didn't need PCI_MAP_SINGLE()
+	//SrcBufPA = PCI_MAP_SINGLE(pAd, (char *) pTxBlk->pSrcBufData, pTxBlk->SrcBufLen, PCI_DMA_TODEVICE);
+
+
+	//
+	// get Tx Ring Resource & Dma Buffer address
+	//
+	QueIdx = pTxBlk->QueIdx;
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+	fillOffset = pHTTXContext->CurWritePosition;
+
+
+
+	// Check ring full.
+	Status = RtmpUSBCanDoWrite(pAd, QueIdx, pHTTXContext);
+	if(Status == NDIS_STATUS_SUCCESS)
+	{
+		pHTTXContext->bCurWriting = TRUE;
+
+		pTxInfo = (PTXINFO_STRUC)(&pTxBlk->HeaderBuf[0]);
+		pTxWI= (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]);
+
+		// Reserve space for 8 bytes padding.
+		if ((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition))
+		{
+			pHTTXContext->ENextBulkOutPosition += 8;
+			pHTTXContext->CurWritePosition += 8;
+			fillOffset += 8;
+		}
+		pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+		pWirelessPacket = &pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset];
+
+		// copy TXWI + WLAN Header + LLC into DMA Header Buffer
+		//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen, 4);
+		hwHdrLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+		// Build our URB for USBD
+		DMAHdrLen = TXWI_SIZE + hwHdrLen;
+		USBDMApktLen = DMAHdrLen + pTxBlk->SrcBufLen;
+		padding = (4 - (USBDMApktLen % 4)) & 0x03;	// round up to 4 byte alignment
+		USBDMApktLen += padding;
+
+		pTxBlk->Priv = (TXINFO_SIZE + USBDMApktLen);
+
+		// For TxInfo, the length of USBDMApktLen = TXWI_SIZE + 802.11 header + payload
+		RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(USBDMApktLen), FALSE, FIFO_EDCA, FALSE /*NextValid*/,  FALSE);
+
+		if ((pHTTXContext->CurWritePosition + 3906 + pTxBlk->Priv) > MAX_TXBULK_LIMIT)
+		{
+			pTxInfo->SwUseLastRound = 1;
+			bTxQLastRound = TRUE;
+		}
+		NdisMoveMemory(pWirelessPacket, pTxBlk->HeaderBuf, TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+#ifdef RT_BIG_ENDIAN
+		RTMPFrameEndianChange(pAd, (PUCHAR)(pWirelessPacket + TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+#endif // RT_BIG_ENDIAN //
+		pWirelessPacket += (TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+
+		// We unlock it here to prevent the first 8 bytes maybe over-writed issue.
+		//	1. First we got CurWritePosition but the first 8 bytes still not write to the pTxcontext.
+		//	2. An interrupt break our routine and handle bulk-out complete.
+		//	3. In the bulk-out compllete, it need to do another bulk-out,
+		//			if the ENextBulkOutPosition is just the same as CurWritePosition, it will save the first 8 bytes from CurWritePosition,
+		//			but the payload still not copyed. the pTxContext->SavedPad[] will save as allzero. and set the bCopyPad = TRUE.
+		//	4. Interrupt complete.
+		//  5. Our interrupted routine go back and fill the first 8 bytes to pTxContext.
+		//	6. Next time when do bulk-out, it found the bCopyPad==TRUE and will copy the SavedPad[] to pTxContext->NextBulkOutPosition.
+		//		and the packet will wrong.
+		pHTTXContext->CurWriteRealPos += (TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+		RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+		NdisMoveMemory(pWirelessPacket, pTxBlk->pSrcBufData, pTxBlk->SrcBufLen);
+		pWirelessPacket += pTxBlk->SrcBufLen;
+		NdisZeroMemory(pWirelessPacket, padding + 8);
+
+		RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+		pHTTXContext->CurWritePosition += pTxBlk->Priv;
+		if (bTxQLastRound)
+			pHTTXContext->CurWritePosition = 8;
+		pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+	pHTTXContext->bCurWriting = FALSE;
+	}
+
+
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+
+	// succeed and release the skb buffer
+	RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_SUCCESS);
+
+	return(Status);
+
+}
+
+
+USHORT RtmpUSB_WriteMultiTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			frameNum,
+	OUT	USHORT			*FreeNumber)
+{
+	HT_TX_CONTEXT	*pHTTXContext;
+	USHORT			hwHdrLen;	// The hwHdrLen consist of 802.11 header length plus the header padding length.
+	UINT32			fillOffset;
+	TXINFO_STRUC	*pTxInfo;
+	TXWI_STRUC		*pTxWI;
+	PUCHAR			pWirelessPacket = NULL;
+	UCHAR			QueIdx;
+	NDIS_STATUS		Status;
+	unsigned long	IrqFlags;
+	//UINT32			USBDMApktLen = 0, DMAHdrLen, padding;
+
+	//
+	// get Tx Ring Resource & Dma Buffer address
+	//
+	QueIdx = pTxBlk->QueIdx;
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	if(frameNum == 0)
+	{
+		// Check if we have enough space for this bulk-out batch.
+		Status = RtmpUSBCanDoWrite(pAd, QueIdx, pHTTXContext);
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			pHTTXContext->bCurWriting = TRUE;
+
+			pTxInfo = (PTXINFO_STRUC)(&pTxBlk->HeaderBuf[0]);
+			pTxWI= (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]);
+
+
+			// Reserve space for 8 bytes padding.
+			if ((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition))
+			{
+
+				pHTTXContext->CurWritePosition += 8;
+				pHTTXContext->ENextBulkOutPosition += 8;
+			}
+			fillOffset = pHTTXContext->CurWritePosition;
+			pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+			pWirelessPacket = &pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset];
+
+			//
+			// Copy TXINFO + TXWI + WLAN Header + LLC into DMA Header Buffer
+			//
+			if (pTxBlk->TxFrameType == TX_AMSDU_FRAME)
+				//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen-LENGTH_AMSDU_SUBFRAMEHEAD, 4)+LENGTH_AMSDU_SUBFRAMEHEAD;
+				hwHdrLen = pTxBlk->MpduHeaderLen-LENGTH_AMSDU_SUBFRAMEHEAD + pTxBlk->HdrPadLen + LENGTH_AMSDU_SUBFRAMEHEAD;
+			else if (pTxBlk->TxFrameType == TX_RALINK_FRAME)
+				//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen-LENGTH_ARALINK_HEADER_FIELD, 4)+LENGTH_ARALINK_HEADER_FIELD;
+				hwHdrLen = pTxBlk->MpduHeaderLen-LENGTH_ARALINK_HEADER_FIELD + pTxBlk->HdrPadLen + LENGTH_ARALINK_HEADER_FIELD;
+			else
+				//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen, 4);
+				hwHdrLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+			// Update the pTxBlk->Priv.
+			pTxBlk->Priv = TXINFO_SIZE + TXWI_SIZE + hwHdrLen;
+
+			//	pTxInfo->USBDMApktLen now just a temp value and will to correct latter.
+			RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(pTxBlk->Priv), FALSE, FIFO_EDCA, FALSE /*NextValid*/,  FALSE);
+
+			// Copy it.
+			NdisMoveMemory(pWirelessPacket, pTxBlk->HeaderBuf, pTxBlk->Priv);
+#ifdef RT_BIG_ENDIAN
+			RTMPFrameEndianChange(pAd, (PUCHAR)(pWirelessPacket+ TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+#endif // RT_BIG_ENDIAN //
+			pHTTXContext->CurWriteRealPos += pTxBlk->Priv;
+			pWirelessPacket += pTxBlk->Priv;
+		}
+	}
+	else
+	{	// For sub-sequent frames of this bulk-out batch. Just copy it to our bulk-out buffer.
+
+		Status = ((pHTTXContext->bCurWriting == TRUE) ? NDIS_STATUS_SUCCESS : NDIS_STATUS_FAILURE);
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			fillOffset =  (pHTTXContext->CurWritePosition + pTxBlk->Priv);
+			pWirelessPacket = &pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset];
+
+			//hwHdrLen = pTxBlk->MpduHeaderLen;
+			NdisMoveMemory(pWirelessPacket, pTxBlk->HeaderBuf, pTxBlk->MpduHeaderLen);
+			pWirelessPacket += (pTxBlk->MpduHeaderLen);
+			pTxBlk->Priv += pTxBlk->MpduHeaderLen;
+		}
+		else
+		{	// It should not happened now unless we are going to shutdown.
+			DBGPRINT(RT_DEBUG_ERROR, ("WriteMultiTxResource():bCurWriting is FALSE when handle sub-sequent frames.\n"));
+			Status = NDIS_STATUS_FAILURE;
+		}
+	}
+
+
+	// We unlock it here to prevent the first 8 bytes maybe over-write issue.
+	//	1. First we got CurWritePosition but the first 8 bytes still not write to the pTxContext.
+	//	2. An interrupt break our routine and handle bulk-out complete.
+	//	3. In the bulk-out compllete, it need to do another bulk-out,
+	//			if the ENextBulkOutPosition is just the same as CurWritePosition, it will save the first 8 bytes from CurWritePosition,
+	//			but the payload still not copyed. the pTxContext->SavedPad[] will save as allzero. and set the bCopyPad = TRUE.
+	//	4. Interrupt complete.
+	//  5. Our interrupted routine go back and fill the first 8 bytes to pTxContext.
+	//	6. Next time when do bulk-out, it found the bCopyPad==TRUE and will copy the SavedPad[] to pTxContext->NextBulkOutPosition.
+	//		and the packet will wrong.
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("WriteMultiTxResource: CWPos = %ld, NBOutPos = %ld.\n", pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition));
+		goto done;
+	}
+
+	// Copy the frame content into DMA buffer and update the pTxBlk->Priv
+	NdisMoveMemory(pWirelessPacket, pTxBlk->pSrcBufData, pTxBlk->SrcBufLen);
+	pWirelessPacket += pTxBlk->SrcBufLen;
+	pTxBlk->Priv += pTxBlk->SrcBufLen;
+
+done:
+	// Release the skb buffer here
+	RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_SUCCESS);
+
+	return(Status);
+
+}
+
+
+VOID RtmpUSB_FinalWriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	USHORT			totalMPDUSize,
+	IN	USHORT			TxIdx)
+{
+	UCHAR			QueIdx;
+	HT_TX_CONTEXT	*pHTTXContext;
+	UINT32			fillOffset;
+	TXINFO_STRUC	*pTxInfo;
+	TXWI_STRUC		*pTxWI;
+	UINT32			USBDMApktLen, padding;
+	unsigned long	IrqFlags;
+	PUCHAR			pWirelessPacket;
+
+	QueIdx = pTxBlk->QueIdx;
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	if (pHTTXContext->bCurWriting == TRUE)
+	{
+		fillOffset = pHTTXContext->CurWritePosition;
+		if (((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition) || ((pHTTXContext->ENextBulkOutPosition-8) == pHTTXContext->CurWritePosition))
+			&& (pHTTXContext->bCopySavePad == TRUE))
+			pWirelessPacket = (PUCHAR)(&pHTTXContext->SavedPad[0]);
+		else
+			pWirelessPacket = (PUCHAR)(&pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset]);
+
+		//
+		// Update TxInfo->USBDMApktLen ,
+		//		the length = TXWI_SIZE + 802.11_hdr + 802.11_hdr_pad + payload_of_all_batch_frames + Bulk-Out-padding
+		//
+		pTxInfo = (PTXINFO_STRUC)(pWirelessPacket);
+
+		// Calculate the bulk-out padding
+		USBDMApktLen = pTxBlk->Priv - TXINFO_SIZE;
+		padding = (4 - (USBDMApktLen % 4)) & 0x03;	// round up to 4 byte alignment
+		USBDMApktLen += padding;
+
+		pTxInfo->USBDMATxPktLen = USBDMApktLen;
+
+		//
+		// Update TXWI->MPDUtotalByteCount ,
+		//		the length = 802.11 header + payload_of_all_batch_frames
+		pTxWI= (PTXWI_STRUC)(pWirelessPacket + TXINFO_SIZE);
+		pTxWI->MPDUtotalByteCount = totalMPDUSize;
+
+		//
+		// Update the pHTTXContext->CurWritePosition
+		//
+		pHTTXContext->CurWritePosition += (TXINFO_SIZE + USBDMApktLen);
+		if ((pHTTXContext->CurWritePosition + 3906)> MAX_TXBULK_LIMIT)
+		{	// Add 3906 for prevent the NextBulkOut packet size is a A-RALINK/A-MSDU Frame.
+			pHTTXContext->CurWritePosition = 8;
+			pTxInfo->SwUseLastRound = 1;
+		}
+		pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+
+		//
+		//	Zero the last padding.
+		//
+		pWirelessPacket = (&pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset + pTxBlk->Priv]);
+		NdisZeroMemory(pWirelessPacket, padding + 8);
+
+		// Finally, set bCurWriting as FALSE
+		pHTTXContext->bCurWriting = FALSE;
+
+	}
+	else
+	{	// It should not happened now unless we are going to shutdown.
+		DBGPRINT(RT_DEBUG_ERROR, ("FinalWriteTxResource():bCurWriting is FALSE when handle last frames.\n"));
+	}
+
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+}
+
+
+VOID RtmpUSBDataLastTxIdx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	USHORT			TxIdx)
+{
+	// DO nothing for USB.
+}
+
+
+/*
+	When can do bulk-out:
+		1. TxSwFreeIdx < TX_RING_SIZE;
+			It means has at least one Ring entity is ready for bulk-out, kick it out.
+		2. If TxSwFreeIdx == TX_RING_SIZE
+			Check if the CurWriting flag is FALSE, if it's FALSE, we can do kick out.
+
+*/
+VOID RtmpUSBDataKickOut(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			QueIdx)
+{
+	RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << QueIdx));
+	RTUSBKickBulkOut(pAd);
+
+}
+
+
+/*
+	Must be run in Interrupt context
+	This function handle RT2870 specific TxDesc and cpu index update and kick the packet out.
+ */
+int RtmpUSBMgmtKickOut(
+	IN RTMP_ADAPTER 	*pAd,
+	IN UCHAR 			QueIdx,
+	IN PNDIS_PACKET		pPacket,
+	IN PUCHAR			pSrcBufVA,
+	IN UINT 			SrcBufLen)
+{
+	PTXINFO_STRUC	pTxInfo;
+	ULONG			BulkOutSize;
+	UCHAR			padLen;
+	PUCHAR			pDest;
+	ULONG			SwIdx = pAd->MgmtRing.TxCpuIdx;
+	PTX_CONTEXT		pMLMEContext = (PTX_CONTEXT)pAd->MgmtRing.Cell[SwIdx].AllocVa;
+	unsigned long	IrqFlags;
+
+
+	pTxInfo = (PTXINFO_STRUC)(pSrcBufVA);
+
+	// Build our URB for USBD
+	BulkOutSize = SrcBufLen;
+	BulkOutSize = (BulkOutSize + 3) & (~3);
+	RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(BulkOutSize - TXINFO_SIZE), TRUE, EpToQueue[MGMTPIPEIDX], FALSE,  FALSE);
+
+	BulkOutSize += 4; // Always add 4 extra bytes at every packet.
+
+	// If BulkOutSize is multiple of BulkOutMaxPacketSize, add extra 4 bytes again.
+	if ((BulkOutSize % pAd->BulkOutMaxPacketSize) == 0)
+		BulkOutSize += 4;
+
+	padLen = BulkOutSize - SrcBufLen;
+	ASSERT((padLen <= RTMP_PKT_TAIL_PADDING));
+
+	// Now memzero all extra padding bytes.
+	pDest = (PUCHAR)(pSrcBufVA + SrcBufLen);
+	skb_put(GET_OS_PKT_TYPE(pPacket), padLen);
+	NdisZeroMemory(pDest, padLen);
+
+	RTMP_IRQ_LOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+
+	pAd->MgmtRing.Cell[pAd->MgmtRing.TxCpuIdx].pNdisPacket = pPacket;
+	pMLMEContext->TransferBuffer = (PTX_BUFFER)(GET_OS_PKT_DATAPTR(pPacket));
+
+	// Length in TxInfo should be 8 less than bulkout size.
+	pMLMEContext->BulkOutSize = BulkOutSize;
+	pMLMEContext->InUse = TRUE;
+	pMLMEContext->bWaitingBulkOut = TRUE;
+
+
+	//for debug
+	//hex_dump("RtmpUSBMgmtKickOut", &pMLMEContext->TransferBuffer->field.WirelessPacket[0], (pMLMEContext->BulkOutSize > 16 ? 16 : pMLMEContext->BulkOutSize));
+
+	//pAd->RalinkCounters.KickTxCount++;
+	//pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	//if (pAd->MgmtRing.TxSwFreeIdx == MGMT_RING_SIZE)
+	//	needKickOut = TRUE;
+
+	// Decrease the TxSwFreeIdx and Increase the TX_CTX_IDX
+	pAd->MgmtRing.TxSwFreeIdx--;
+	INC_RING_INDEX(pAd->MgmtRing.TxCpuIdx, MGMT_RING_SIZE);
+
+	RTMP_IRQ_UNLOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+
+	RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+	//if (needKickOut)
+	RTUSBKickBulkOut(pAd);
+
+	return 0;
+}
+
+
+VOID RtmpUSBNullFrameKickOut(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		QueIdx,
+	IN UCHAR		*pNullFrame,
+	IN UINT32		frameLen)
+{
+	if (pAd->NullContext.InUse == FALSE)
+	{
+		PTX_CONTEXT		pNullContext;
+		PTXINFO_STRUC	pTxInfo;
+		PTXWI_STRUC		pTxWI;
+		PUCHAR			pWirelessPkt;
+
+		pNullContext = &(pAd->NullContext);
+
+		// Set the in use bit
+		pNullContext->InUse = TRUE;
+		pWirelessPkt = (PUCHAR)&pNullContext->TransferBuffer->field.WirelessPacket[0];
+
+		RTMPZeroMemory(&pWirelessPkt[0], 100);
+		pTxInfo = (PTXINFO_STRUC)&pWirelessPkt[0];
+		RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(sizeof(HEADER_802_11)+TXWI_SIZE), TRUE, EpToQueue[MGMTPIPEIDX], FALSE,  FALSE);
+		pTxInfo->QSEL = FIFO_EDCA;
+		pTxWI = (PTXWI_STRUC)&pWirelessPkt[TXINFO_SIZE];
+		RTMPWriteTxWI(pAd, pTxWI,  FALSE, FALSE, FALSE, FALSE, TRUE, FALSE, 0, BSSID_WCID, (sizeof(HEADER_802_11)),
+			0, 0, (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_HTTXOP, FALSE, &pAd->CommonCfg.MlmeTransmit);
+#ifdef RT_BIG_ENDIAN
+		RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+#endif // RT_BIG_ENDIAN //
+
+		RTMPMoveMemory(&pWirelessPkt[TXWI_SIZE+TXINFO_SIZE], &pAd->NullFrame, sizeof(HEADER_802_11));
+#ifdef RT_BIG_ENDIAN
+		RTMPFrameEndianChange(pAd, (PUCHAR)&pWirelessPkt[TXINFO_SIZE + TXWI_SIZE], DIR_WRITE, FALSE);
+#endif // RT_BIG_ENDIAN //
+		pAd->NullContext.BulkOutSize =  TXINFO_SIZE + TXWI_SIZE + sizeof(pAd->NullFrame) + 4;
+
+		// Fill out frame length information for global Bulk out arbitor
+		//pNullContext->BulkOutSize = TransferBufferLength;
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - send NULL Frame @%d Mbps...\n", RateIdToMbps[pAd->CommonCfg.TxRate]));
+		RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NULL);
+
+		// Kick bulk out
+		RTUSBKickBulkOut(pAd);
+	}
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	========================================================================
+
+	Routine	Description:
+		Check Rx descriptor, return NDIS_STATUS_FAILURE if any error dound
+
+	Arguments:
+		pRxD		Pointer	to the Rx descriptor
+
+	Return Value:
+		NDIS_STATUS_SUCCESS		No err
+		NDIS_STATUS_FAILURE		Error
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPCheckRxError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHEADER_802_11	pHeader,
+	IN	PRXWI_STRUC	pRxWI,
+	IN	PRT28XX_RXD_STRUC	pRxINFO)
+{
+	PCIPHER_KEY pWpaKey;
+	INT	dBm;
+
+	if (pAd->bPromiscuous == TRUE)
+		return(NDIS_STATUS_SUCCESS);
+	if(pRxINFO == NULL)
+		return(NDIS_STATUS_FAILURE);
+
+	// Phy errors & CRC errors
+	if (pRxINFO->Crc)
+	{
+		// Check RSSI for Noise Hist statistic collection.
+		dBm = (INT) (pRxWI->RSSI0) - pAd->BbpRssiToDbmDelta;
+		if (dBm <= -87)
+			pAd->StaCfg.RPIDensity[0] += 1;
+		else if (dBm <= -82)
+			pAd->StaCfg.RPIDensity[1] += 1;
+		else if (dBm <= -77)
+			pAd->StaCfg.RPIDensity[2] += 1;
+		else if (dBm <= -72)
+			pAd->StaCfg.RPIDensity[3] += 1;
+		else if (dBm <= -67)
+			pAd->StaCfg.RPIDensity[4] += 1;
+		else if (dBm <= -62)
+			pAd->StaCfg.RPIDensity[5] += 1;
+		else if (dBm <= -57)
+			pAd->StaCfg.RPIDensity[6] += 1;
+		else if (dBm > -57)
+			pAd->StaCfg.RPIDensity[7] += 1;
+
+		return(NDIS_STATUS_FAILURE);
+	}
+
+	// Add Rx size to channel load counter, we should ignore error counts
+	pAd->StaCfg.CLBusyBytes += (pRxWI->MPDUtotalByteCount+ 14);
+
+	// Drop ToDs promiscous frame, it is opened due to CCX 2 channel load statistics
+	if (pHeader->FC.ToDs)
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Err;FC.ToDs\n"));
+		return NDIS_STATUS_FAILURE;
+	}
+
+	// Paul 04-03 for OFDM Rx length issue
+	if (pRxWI->MPDUtotalByteCount > MAX_AGGREGATION_SIZE)
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("received packet too long\n"));
+		return NDIS_STATUS_FAILURE;
+	}
+
+	// Drop not U2M frames, cant's drop here because we will drop beacon in this case
+	// I am kind of doubting the U2M bit operation
+	// if (pRxD->U2M == 0)
+	//	return(NDIS_STATUS_FAILURE);
+
+	// drop decyption fail frame
+	if (pRxINFO->Decrypted && pRxINFO->CipherErr)
+	{
+
+		//
+		// MIC Error
+		//
+		if ((pRxINFO->CipherErr == 2) && pRxINFO->MyBss)
+		{
+			pWpaKey = &pAd->SharedKey[BSS0][pRxWI->KeyIndex];
+			RTMPReportMicError(pAd, pWpaKey);
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("Rx MIC Value error\n"));
+		}
+
+		if (pRxINFO->Decrypted &&
+			(pAd->SharedKey[BSS0][pRxWI->KeyIndex].CipherAlg == CIPHER_AES) &&
+			(pHeader->Sequence == pAd->FragFrame.Sequence))
+		{
+			//
+			// Acceptable since the First FragFrame no CipherErr problem.
+			//
+			return(NDIS_STATUS_SUCCESS);
+		}
+
+		return(NDIS_STATUS_FAILURE);
+	}
+
+	return(NDIS_STATUS_SUCCESS);
+}
+
+VOID RT28xxUsbStaAsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN       bFromTx)
+{
+    AUTO_WAKEUP_STRUC	AutoWakeupCfg;
+
+	AutoWakeupCfg.word = 0;
+	RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+	AsicSendCommandToMcu(pAd, 0x31, 0xff, 0x00, 0x00);
+
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+}
+
+VOID RT28xxUsbStaAsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp)
+{
+	AUTO_WAKEUP_STRUC	AutoWakeupCfg;
+
+	// we have decided to SLEEP, so at least do it for a BEACON period.
+	if (TbttNumToNextWakeUp == 0)
+		TbttNumToNextWakeUp = 1;
+
+	AutoWakeupCfg.word = 0;
+	RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+	AutoWakeupCfg.field.NumofSleepingTbtt = TbttNumToNextWakeUp - 1;
+	AutoWakeupCfg.field.EnableAutoWakeup = 1;
+	AutoWakeupCfg.field.AutoLeadTime = 5;
+	RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+	AsicSendCommandToMcu(pAd, 0x30, 0xff, 0xff, 0x02);   // send POWER-SAVE command to MCU. Timeout 40us.
+
+	OPSTATUS_SET_FLAG(pAd, fOP_STATUS_DOZE);
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+VOID RT28xxUsbMlmeRadioOn(
+	IN PRTMP_ADAPTER pAd)
+{
+    DBGPRINT(RT_DEBUG_TRACE,("RT28xxUsbMlmeRadioOn()\n"));
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+		return;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+    	AsicSendCommandToMcu(pAd, 0x31, 0xff, 0x00, 0x00);
+		RTMPusecDelay(10000);
+	}
+#endif // CONFIG_STA_SUPPORT //
+	NICResetFromError(pAd);
+
+	// Enable Tx/Rx
+	RTMPEnableRxTx(pAd);
+
+	// Clear Radio off flag
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTUSBBulkReceive(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+	// Set LED
+	RTMPSetLED(pAd, LED_RADIO_ON);
+}
+
+VOID RT28xxUsbMlmeRadioOFF(
+	IN PRTMP_ADAPTER pAd)
+{
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+	UINT32	Value, i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RT28xxUsbMlmeRadioOFF()\n"));
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+		return;
+
+	// 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)
+	{
+		// Link down first if any association exists
+		if (INFRA_ON(pAd) || ADHOC_ON(pAd))
+			LinkDown(pAd, FALSE);
+		RTMPusecDelay(10000);
+
+		//==========================================
+		// Clean up old bss table
+		BssTableInit(&pAd->ScanTab);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+	// Disable MAC Tx/Rx
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value &= (0xfffffff3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+	// MAC_SYS_CTRL => value = 0x0 => 40mA
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0);
+
+	// PWR_PIN_CFG => value = 0x0 => 40mA
+	RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0);
+
+	// TX_PIN_CFG => value = 0x0 => 20mA
+	RTMP_IO_WRITE32(pAd, TX_PIN_CFG, 0);
+
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		// Must using 40MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.CentralChannel);
+	}
+	else
+	{
+		// Must using 20MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.Channel);
+	}
+
+	// Waiting for DMA idle
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0) && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		RTMPusecDelay(1000);
+	}while (i++ < 100);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		AsicSendCommandToMcu(pAd, 0x30, 0xff, 0xff, 0x02);
+#endif // CONFIG_STA_SUPPORT //
+}
+
diff --git a/drivers/staging/rt2870/common/cmm_info.c b/drivers/staging/rt2870/common/cmm_info.c
new file mode 100644
index 00000000000..40792e16237
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_info.c
@@ -0,0 +1,3712 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+#include "../rt_config.h"
+
+INT	Show_SSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef DOT11_N_SUPPORT
+INT	Show_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // DOT11_N_SUPPORT //
+
+INT	Show_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_CountryCode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef AGGREGATION_SUPPORT
+INT	Show_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // AGGREGATION_SUPPORT //
+
+#ifdef WMM_SUPPORT
+INT	Show_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // WMM_SUPPORT //
+
+INT	Show_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef CONFIG_STA_SUPPORT
+INT	Show_NetworkType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Show_AuthMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_EncrypType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_DefaultKeyID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_WPAPSK_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+static struct {
+	CHAR *name;
+	INT (*show_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC, RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC[] = {
+	{"SSID",					Show_SSID_Proc},
+	{"WirelessMode",			Show_WirelessMode_Proc},
+	{"TxBurst",					Show_TxBurst_Proc},
+	{"TxPreamble",				Show_TxPreamble_Proc},
+	{"TxPower",					Show_TxPower_Proc},
+	{"Channel",					Show_Channel_Proc},
+	{"BGProtection",			Show_BGProtection_Proc},
+	{"RTSThreshold",			Show_RTSThreshold_Proc},
+	{"FragThreshold",			Show_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",					Show_HtBw_Proc},
+	{"HtMcs",					Show_HtMcs_Proc},
+	{"HtGi",					Show_HtGi_Proc},
+	{"HtOpMode",				Show_HtOpMode_Proc},
+	{"HtExtcha",				Show_HtExtcha_Proc},
+	{"HtMpduDensity",			Show_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        Show_HtBaWinSize_Proc},
+	{"HtRdg",		        	Show_HtRdg_Proc},
+	{"HtAmsdu",		        	Show_HtAmsdu_Proc},
+	{"HtAutoBa",		        Show_HtAutoBa_Proc},
+#endif // DOT11_N_SUPPORT //
+	{"CountryRegion",			Show_CountryRegion_Proc},
+	{"CountryRegionABand",		Show_CountryRegionABand_Proc},
+	{"CountryCode",				Show_CountryCode_Proc},
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",			Show_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",				Show_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",				Show_IEEE80211H_Proc},
+#ifdef CONFIG_STA_SUPPORT
+    {"NetworkType",				Show_NetworkType_Proc},
+#endif // CONFIG_STA_SUPPORT //
+	{"AuthMode",				Show_AuthMode_Proc},
+	{"EncrypType",				Show_EncrypType_Proc},
+	{"DefaultKeyID",			Show_DefaultKeyID_Proc},
+	{"Key1",					Show_Key1_Proc},
+	{"Key2",					Show_Key2_Proc},
+	{"Key3",					Show_Key3_Proc},
+	{"Key4",					Show_Key4_Proc},
+	{"WPAPSK",					Show_WPAPSK_Proc},
+	{NULL, NULL}
+};
+
+/*
+    ==========================================================================
+    Description:
+        Get Driver version.
+
+    Return:
+    ==========================================================================
+*/
+INT Set_DriverVersion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		DBGPRINT(RT_DEBUG_TRACE, ("Driver version-%s\n", STA_DRIVER_VERSION));
+#endif // CONFIG_STA_SUPPORT //
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Country Region.
+        This command will not work, if the field of CountryRegion in eeprom is programmed.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG region;
+
+	region = simple_strtol(arg, 0, 10);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	return -EOPNOTSUPP;
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Country can be set only when EEPROM not programmed
+	if (pAd->CommonCfg.CountryRegion & 0x80)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegion_Proc::parameter of CountryRegion in eeprom is programmed \n"));
+		return FALSE;
+	}
+
+	if((region >= 0) && (region <= REGION_MAXIMUM_BG_BAND))
+	{
+		pAd->CommonCfg.CountryRegion = (UCHAR) region;
+	}
+	else if (region == REGION_31_BG_BAND)
+	{
+		pAd->CommonCfg.CountryRegion = (UCHAR) region;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegion_Proc::parameters out of range\n"));
+		return FALSE;
+	}
+
+	// if set country region, driver needs to be reset
+	BuildChannelList(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_CountryRegion_Proc::(CountryRegion=%d)\n", pAd->CommonCfg.CountryRegion));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Country Region for A band.
+        This command will not work, if the field of CountryRegion in eeprom is programmed.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG region;
+
+	region = simple_strtol(arg, 0, 10);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	return -EOPNOTSUPP;
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Country can be set only when EEPROM not programmed
+	if (pAd->CommonCfg.CountryRegionForABand & 0x80)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegionABand_Proc::parameter of CountryRegion in eeprom is programmed \n"));
+		return FALSE;
+	}
+
+	if((region >= 0) && (region <= REGION_MAXIMUM_A_BAND))
+	{
+		pAd->CommonCfg.CountryRegionForABand = (UCHAR) region;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegionABand_Proc::parameters out of range\n"));
+		return FALSE;
+	}
+
+	// if set country region, driver needs to be reset
+	BuildChannelList(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_CountryRegionABand_Proc::(CountryRegion=%d)\n", pAd->CommonCfg.CountryRegionForABand));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Wireless Mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG	WirelessMode;
+	INT		success = TRUE;
+
+	WirelessMode = simple_strtol(arg, 0, 10);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		INT MaxPhyMode = PHY_11G;
+
+#ifdef DOT11_N_SUPPORT
+		MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+		if (WirelessMode <= MaxPhyMode)
+		{
+			RTMPSetPhyMode(pAd, WirelessMode);
+#ifdef DOT11_N_SUPPORT
+			if (WirelessMode >= PHY_11ABGN_MIXED)
+			{
+				pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+				pAd->CommonCfg.REGBACapability.field.AutoBA = TRUE;
+			}
+			else
+			{
+				pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+				pAd->CommonCfg.REGBACapability.field.AutoBA = FALSE;
+			}
+#endif // DOT11_N_SUPPORT //
+			// Set AdhocMode rates
+			if (pAd->StaCfg.BssType == BSS_ADHOC)
+			{
+				MlmeUpdateTxRates(pAd, FALSE, 0);
+				MakeIbssBeacon(pAd);           // re-build BEACON frame
+				AsicEnableIbssSync(pAd);       // copy to on-chip memory
+			}
+		}
+		else
+		{
+			success = FALSE;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// it is needed to set SSID to take effect
+	if (success == TRUE)
+	{
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_WirelessMode_Proc::(=%ld)\n", WirelessMode));
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_WirelessMode_Proc::parameters out of range\n"));
+	}
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Channel
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+ 	INT		success = TRUE;
+	UCHAR	Channel;
+
+	Channel = (UCHAR) simple_strtol(arg, 0, 10);
+
+	// check if this channel is valid
+	if (ChannelSanity(pAd, Channel) == TRUE)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pAd->CommonCfg.Channel = Channel;
+
+			if (MONITOR_ON(pAd))
+			{
+#ifdef DOT11_N_SUPPORT
+				N_ChannelCheck(pAd);
+				if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+					pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+				{
+					N_SetCenCh(pAd);
+					AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+					DBGPRINT(RT_DEBUG_TRACE, ("BW_40, control_channel(%d), CentralChannel(%d) \n",
+								pAd->CommonCfg.Channel, pAd->CommonCfg.CentralChannel));
+				}
+				else
+#endif // DOT11_N_SUPPORT //
+				{
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAd->CommonCfg.Channel));
+				}
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+		success = TRUE;
+	}
+	else
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			success = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	if (success == TRUE)
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_Channel_Proc::(Channel=%d)\n", pAd->CommonCfg.Channel));
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Short Slot Time Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ShortSlot_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG ShortSlot;
+
+	ShortSlot = simple_strtol(arg, 0, 10);
+
+	if (ShortSlot == 1)
+		pAd->CommonCfg.bUseShortSlotTime = TRUE;
+	else if (ShortSlot == 0)
+		pAd->CommonCfg.bUseShortSlotTime = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ShortSlot_Proc::(ShortSlot=%d)\n", pAd->CommonCfg.bUseShortSlotTime));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Tx power
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG TxPower;
+	INT   success = FALSE;
+
+	TxPower = (ULONG) simple_strtol(arg, 0, 10);
+	if (TxPower <= 100)
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pAd->CommonCfg.TxPowerDefault = TxPower;
+			pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+		}
+#endif // CONFIG_STA_SUPPORT //
+		success = TRUE;
+	}
+	else
+		success = FALSE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxPower_Proc::(TxPowerPercentage=%ld)\n", pAd->CommonCfg.TxPowerPercentage));
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set 11B/11G Protection
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	switch (simple_strtol(arg, 0, 10))
+	{
+		case 0: //AUTO
+			pAd->CommonCfg.UseBGProtection = 0;
+			break;
+		case 1: //Always On
+			pAd->CommonCfg.UseBGProtection = 1;
+			break;
+		case 2: //Always OFF
+			pAd->CommonCfg.UseBGProtection = 2;
+			break;
+		default:  //Invalid argument
+			return FALSE;
+	}
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_BGProtection_Proc::(BGProtection=%ld)\n", pAd->CommonCfg.UseBGProtection));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set TxPreamble
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	RT_802_11_PREAMBLE	Preamble;
+
+	Preamble = simple_strtol(arg, 0, 10);
+
+
+	switch (Preamble)
+	{
+		case Rt802_11PreambleShort:
+			pAd->CommonCfg.TxPreamble = Preamble;
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				MlmeSetTxPreamble(pAd, Rt802_11PreambleShort);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+		case Rt802_11PreambleLong:
+#ifdef CONFIG_STA_SUPPORT
+		case Rt802_11PreambleAuto:
+			// if user wants AUTO, initialize to LONG here, then change according to AP's
+			// capability upon association.
+#endif // CONFIG_STA_SUPPORT //
+			pAd->CommonCfg.TxPreamble = Preamble;
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				MlmeSetTxPreamble(pAd, Rt802_11PreambleLong);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+		default: //Invalid argument
+			return FALSE;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxPreamble_Proc::(TxPreamble=%ld)\n", pAd->CommonCfg.TxPreamble));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set RTS Threshold
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	 NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+
+	RtsThresh = simple_strtol(arg, 0, 10);
+
+	if((RtsThresh > 0) && (RtsThresh <= MAX_RTS_THRESHOLD))
+		pAd->CommonCfg.RtsThreshold  = (USHORT)RtsThresh;
+#ifdef CONFIG_STA_SUPPORT
+	else if (RtsThresh == 0)
+		pAd->CommonCfg.RtsThreshold = MAX_RTS_THRESHOLD;
+#endif // CONFIG_STA_SUPPORT //
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_RTSThreshold_Proc::(RTSThreshold=%d)\n", pAd->CommonCfg.RtsThreshold));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Fragment Threshold
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	 NDIS_802_11_FRAGMENTATION_THRESHOLD     FragThresh;
+
+	FragThresh = simple_strtol(arg, 0, 10);
+
+	if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+	{
+		//Illegal FragThresh so we set it to default
+		pAd->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+	}
+	else if (FragThresh % 2 == 1)
+	{
+		// The length of each fragment shall always be an even number of octets, except for the last fragment
+		// of an MSDU or MMPDU, which may be either an even or an odd number of octets.
+		pAd->CommonCfg.FragmentThreshold = (USHORT)(FragThresh - 1);
+	}
+	else
+	{
+		pAd->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->CommonCfg.FragmentThreshold == MAX_FRAG_THRESHOLD)
+			pAd->CommonCfg.bUseZeroToDisableFragment = TRUE;
+		else
+			pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_FragThreshold_Proc::(FragThreshold=%d)\n", pAd->CommonCfg.FragmentThreshold));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set TxBurst
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG TxBurst;
+
+	TxBurst = simple_strtol(arg, 0, 10);
+	if (TxBurst == 1)
+		pAd->CommonCfg.bEnableTxBurst = TRUE;
+	else if (TxBurst == 0)
+		pAd->CommonCfg.bEnableTxBurst = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxBurst_Proc::(TxBurst=%d)\n", pAd->CommonCfg.bEnableTxBurst));
+
+	return TRUE;
+}
+
+#ifdef AGGREGATION_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set TxBurst
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG aggre;
+
+	aggre = simple_strtol(arg, 0, 10);
+
+	if (aggre == 1)
+		pAd->CommonCfg.bAggregationCapable = TRUE;
+	else if (aggre == 0)
+		pAd->CommonCfg.bAggregationCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_PktAggregate_Proc::(AGGRE=%d)\n", pAd->CommonCfg.bAggregationCapable));
+
+	return TRUE;
+}
+#endif
+
+/*
+    ==========================================================================
+    Description:
+        Set IEEE80211H.
+        This parameter is 1 when needs radar detection, otherwise 0
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    ULONG ieee80211h;
+
+	ieee80211h = simple_strtol(arg, 0, 10);
+
+	if (ieee80211h == 1)
+		pAd->CommonCfg.bIEEE80211H = TRUE;
+	else if (ieee80211h == 0)
+		pAd->CommonCfg.bIEEE80211H = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_IEEE80211H_Proc::(IEEE80211H=%d)\n", pAd->CommonCfg.bIEEE80211H));
+
+	return TRUE;
+}
+
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        For Debug information
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_Debug_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("==> Set_Debug_Proc *******************\n"));
+
+    if(simple_strtol(arg, 0, 10) <= RT_DEBUG_LOUD)
+        RTDebugLevel = simple_strtol(arg, 0, 10);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== Set_Debug_Proc(RTDebugLevel = %ld)\n", RTDebugLevel));
+
+	return TRUE;
+}
+#endif
+
+INT	Show_DescInfo_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Reset statistics counter
+
+    Arguments:
+        pAdapter            Pointer to our adapter
+        arg
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ResetStatCounter_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	//UCHAR           i;
+	//MAC_TABLE_ENTRY *pEntry;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==>Set_ResetStatCounter_Proc\n"));
+
+	// add the most up-to-date h/w raw counters into software counters
+	NICUpdateRawCounters(pAd);
+
+	NdisZeroMemory(&pAd->WlanCounters, sizeof(COUNTER_802_11));
+	NdisZeroMemory(&pAd->Counters8023, sizeof(COUNTER_802_3));
+	NdisZeroMemory(&pAd->RalinkCounters, sizeof(COUNTER_RALINK));
+
+	// Reset HotSpot counter
+#if 0 // ToDo.
+	for (i = 0; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		if ((pEntry->Valid == FALSE) || (pEntry->Sst != SST_ASSOC))
+			continue;
+
+		pEntry->HSCounter.LastDataPacketTime = 0;
+		pEntry->HSCounter.TotalRxByteCount= 0;
+		pEntry->HSCounter.TotalTxByteCount= 0;
+	}
+#endif
+
+
+	return TRUE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Add WPA key process.
+		In Adhoc WPANONE, bPairwise = 0;  KeyIdx = 0;
+
+	Arguments:
+		pAd 					Pointer to our adapter
+		pBuf							Pointer to the where the key stored
+
+	Return Value:
+		NDIS_SUCCESS					Add key successfully
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+#if 0 // remove by AlbertY
+NDIS_STATUS RTMPWPAAddKeyProc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PVOID			pBuf)
+{
+	PNDIS_802_11_KEY	pKey;
+	ULONG				KeyIdx;
+//	NDIS_STATUS 		Status;
+//	ULONG 	offset;	// unused variable, snowpin 2006.07.13
+
+	PUCHAR		pTxMic, pRxMic;
+	BOOLEAN 	bTxKey; 		// Set the key as transmit key
+	BOOLEAN 	bPairwise;		// Indicate the key is pairwise key
+	BOOLEAN 	bKeyRSC;		// indicate the receive  SC set by KeyRSC value.
+								// Otherwise, it will set by the NIC.
+	BOOLEAN 	bAuthenticator; // indicate key is set by authenticator.
+	UCHAR		apidx = BSS0;
+
+	pKey = (PNDIS_802_11_KEY) pBuf;
+	KeyIdx = pKey->KeyIndex & 0xff;
+	// Bit 31 of Add-key, Tx Key
+	bTxKey		   = (pKey->KeyIndex & 0x80000000) ? TRUE : FALSE;
+	// Bit 30 of Add-key PairwiseKey
+	bPairwise	   = (pKey->KeyIndex & 0x40000000) ? TRUE : FALSE;
+	// Bit 29 of Add-key KeyRSC
+	bKeyRSC 	   = (pKey->KeyIndex & 0x20000000) ? TRUE : FALSE;
+	// Bit 28 of Add-key Authenticator
+	bAuthenticator = (pKey->KeyIndex & 0x10000000) ? TRUE : FALSE;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPWPAAddKeyProc==>pKey->KeyIndex = %x. bPairwise= %d\n", pKey->KeyIndex, bPairwise));
+	// 1. Check Group / Pairwise Key
+	if (bPairwise)	// Pairwise Key
+	{
+		// 1. KeyIdx must be 0, otherwise, return NDIS_STATUS_INVALID_DATA
+		if (KeyIdx != 0)
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 2. Check bTx, it must be true, otherwise, return NDIS_STATUS_INVALID_DATA
+		if (bTxKey == FALSE)
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 3. If BSSID is all 0xff, return NDIS_STATUS_INVALID_DATA
+		if (MAC_ADDR_EQUAL(pKey->BSSID, BROADCAST_ADDR))
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 3.1 Check Pairwise key length for TKIP key. For AES, it's always 128 bits
+		//if ((pAdapter->PortCfg.WepStatus == Ndis802_11Encryption2Enabled) && (pKey->KeyLength != LEN_TKIP_KEY))
+		if ((pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled) && (pKey->KeyLength != LEN_TKIP_KEY))
+			return(NDIS_STATUS_INVALID_DATA);
+
+		pAd->SharedKey[apidx][KeyIdx].Type = PAIRWISE_KEY;
+
+		if (pAd->ApCfg.MBSSID[apidx].AuthMode == Ndis802_11AuthModeWPA2)
+		{
+			// Send media specific event to start PMKID caching
+			RTMPIndicateWPA2Status(pAd);
+		}
+	}
+	else
+	{
+		// 1. Check BSSID, if not current BSSID or Bcast, return NDIS_STATUS_INVALID_DATA
+		if ((! MAC_ADDR_EQUAL(pKey->BSSID, BROADCAST_ADDR)) &&
+			(! MAC_ADDR_EQUAL(pKey->BSSID, pAd->ApCfg.MBSSID[apidx].Bssid)))
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 2. Check Key index for supported Group Key
+		if (KeyIdx >= GROUP_KEY_NUM)
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 3. Set as default Tx Key if bTxKey is TRUE
+		if (bTxKey == TRUE)
+			pAd->ApCfg.MBSSID[apidx].DefaultKeyId = (UCHAR) KeyIdx;
+
+		pAd->SharedKey[apidx][KeyIdx].Type = GROUP_KEY;
+	}
+
+	// 4. Select RxMic / TxMic based on Supp / Authenticator
+	if (pAd->ApCfg.MBSSID[apidx].AuthMode == Ndis802_11AuthModeWPANone)
+	{
+		// for WPA-None Tx, Rx MIC is the same
+		pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+		pRxMic = pTxMic;
+	}
+	else if (bAuthenticator == TRUE)
+	{
+		pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+		pRxMic = (PUCHAR) (&pKey->KeyMaterial) + 24;
+	}
+	else
+	{
+		pRxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+		pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 24;
+	}
+
+	// 6. Check RxTsc
+	if (bKeyRSC == TRUE)
+	{
+		NdisMoveMemory(pAd->SharedKey[apidx][KeyIdx].RxTsc, &pKey->KeyRSC, 6);
+		NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].PairwiseKey.RxTsc, &pKey->KeyRSC, 6);
+	}
+	else
+	{
+		NdisZeroMemory(pAd->SharedKey[apidx][KeyIdx].RxTsc, 6);
+	}
+
+	// 7. Copy information into Pairwise Key structure.
+	// pKey->KeyLength will include TxMic and RxMic, therefore, we use 16 bytes hardcoded.
+	pAd->SharedKey[apidx][KeyIdx].KeyLen = (UCHAR) pKey->KeyLength;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.KeyLen = (UCHAR)pKey->KeyLength;
+	NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pKey->KeyMaterial, 16);
+	NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].PairwiseKey.Key, &pKey->KeyMaterial, 16);
+	if (pKey->KeyLength == LEN_TKIP_KEY)
+	{
+		// Only Key lenth equal to TKIP key have these
+		NdisMoveMemory(pAd->SharedKey[apidx][KeyIdx].RxMic, pRxMic, 8);
+		NdisMoveMemory(pAd->SharedKey[apidx][KeyIdx].TxMic, pTxMic, 8);
+		NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].PairwiseKey.RxMic, pRxMic, 8);
+		NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxMic, pTxMic, 8);
+	}
+
+	COPY_MAC_ADDR(pAd->SharedKey[BSS0][KeyIdx].BssId, pKey->BSSID);
+
+	// Init TxTsc to one based on WiFi WPA specs
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[0] = 1;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[1] = 0;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[2] = 0;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[3] = 0;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[4] = 0;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[5] = 0;
+	// 4. Init TxTsc to one based on WiFi WPA specs
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[0] = 1;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[1] = 0;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[2] = 0;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[3] = 0;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[4] = 0;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[5] = 0;
+
+	if (pAd->ApCfg.MBSSID[apidx].WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_AES;
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_AES;
+	}
+	else if (pAd->ApCfg.MBSSID[apidx].WepStatus == Ndis802_11Encryption2Enabled)
+	{
+		pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_TKIP;
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_TKIP;
+	}
+	else if (pAd->ApCfg.MBSSID[apidx].WepStatus == Ndis802_11Encryption1Enabled)
+	{
+		if (pAd->SharedKey[apidx][KeyIdx].KeyLen == 5)
+		{
+			pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_WEP64;
+			pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_WEP64;
+		}
+		else if (pAd->SharedKey[apidx][KeyIdx].KeyLen == 13)
+		{
+			pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_WEP128;
+			pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_WEP128;
+		}
+		else
+		{
+			pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_NONE;
+			pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_NONE;
+		}
+	}
+	else
+	{
+		pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_NONE;
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_NONE;
+	}
+
+	if ((pAd->OpMode == OPMODE_STA))  // Pairwise Key. Add BSSID to WCTable
+	{
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.KeyLen = pAd->SharedKey[BSS0][KeyIdx].KeyLen;
+	}
+
+	if ((pAd->ApCfg.MBSSID[apidx].AuthMode == Ndis802_11AuthModeWPA2) ||
+		(pAd->ApCfg.MBSSID[apidx].AuthMode == Ndis802_11AuthModeWPA2PSK))
+	{
+		//
+		// On WPA2, Update Group Key Cipher.
+		//
+		if (!bPairwise)
+		{
+			if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+				pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_AES;
+			else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+				pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_TKIP;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("pAd->SharedKey[%d][%d].CipherAlg = %d\n", apidx, KeyIdx, pAd->SharedKey[apidx][KeyIdx].CipherAlg));
+
+#if 0
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("%s Key #%d", CipherName[pAd->SharedKey[apidx][KeyIdx].CipherAlg],KeyIdx));
+	for (i = 0; i < 16; i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("%02x:", pAd->SharedKey[apidx][KeyIdx].Key[i]));
+	}
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("\n	  Rx MIC Key = "));
+	for (i = 0; i < 8; i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("%02x:", pAd->SharedKey[apidx][KeyIdx].RxMic[i]));
+	}
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("\n	  Tx MIC Key = "));
+	for (i = 0; i < 8; i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("%02x:", pAd->SharedKey[apidx][KeyIdx].TxMic[i]));
+	}
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("\n	  RxTSC = "));
+	for (i = 0; i < 6; i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("%02x:", pAd->SharedKey[apidx][KeyIdx].RxTsc[i]));
+	}
+#endif
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("\n pKey-> BSSID:%02x:%02x:%02x:%02x:%02x:%02x \n",
+		pKey->BSSID[0],pKey->BSSID[1],pKey->BSSID[2],pKey->BSSID[3],pKey->BSSID[4],pKey->BSSID[5]));
+
+	if ((bTxKey) && (pAd->OpMode == OPMODE_STA))  // Pairwise Key. Add BSSID to WCTable
+		RTMPAddBSSIDCipher(pAd, BSSID_WCID, pKey, pAd->SharedKey[BSS0][KeyIdx].CipherAlg);
+
+
+	// No matter pairwise key or what leyidx is, always has a copy at on-chip SharedKeytable.
+	AsicAddSharedKeyEntry(pAd,
+						  apidx,
+						  (UCHAR)KeyIdx,
+						  pAd->SharedKey[apidx][KeyIdx].CipherAlg,
+						  pAd->SharedKey[apidx][KeyIdx].Key,
+						  pAd->SharedKey[apidx][KeyIdx].TxMic,
+						  pAd->SharedKey[apidx][KeyIdx].RxMic);
+
+	// The WCID key specified in used at Tx. For STA, always use pairwise key.
+
+	// ad-hoc mode need to specify WAP Group key with WCID index=BSS0Mcast_WCID. Let's always set this key here.
+/*	if (bPairwise == FALSE)
+	{
+		offset = MAC_IVEIV_TABLE_BASE + (BSS0Mcast_WCID * HW_IVEIV_ENTRY_SIZE);
+		NdisZeroMemory(IVEIV, 8);
+		// 1. IV/EIV
+		// Specify key index to find shared key.
+		if ((pAd->SharedKey[BSS0][KeyIdx].CipherAlg==CIPHER_TKIP) ||
+			(pAd->SharedKey[BSS0][KeyIdx].CipherAlg==CIPHER_AES))
+		IVEIV[3] = 0x20;		// Eiv bit on. keyid always 0 for pairwise key
+		IVEIV[3] |= (KeyIdx<< 6);	// groupkey index is not 0
+		for (i=0; i<8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset+i, IVEIV[i]);
+		}
+
+		// 2. WCID Attribute UDF:3, BSSIdx:3, Alg:3, Keytable:use share key, BSSIdx is 0
+		WCIDAttri = (pAd->SharedKey[BSS0][KeyIdx].CipherAlg<<1)|PAIRWISEKEYTABLE;
+		offset = MAC_WCID_ATTRIBUTE_BASE + (BSS0Mcast_WCID* HW_WCID_ATTRI_SIZE);
+		RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+
+	}
+
+*/
+
+   if (pAd->SharedKey[apidx][KeyIdx].Type == GROUP_KEY)
+	{
+		// 802.1x port control
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		DBGPRINT(RT_DEBUG_TRACE,("!!WPA_802_1X_PORT_SECURED!!\n"));
+
+	}
+
+	return (NDIS_STATUS_SUCCESS);
+}
+#endif
+
+BOOLEAN RTMPCheckStrPrintAble(
+    IN  CHAR *pInPutStr,
+    IN  UCHAR strLen)
+{
+    UCHAR i=0;
+
+    for (i=0; i<strLen; i++)
+    {
+        if ((pInPutStr[i] < 0x21) ||
+            (pInPutStr[i] > 0x7E))
+            return FALSE;
+    }
+
+    return TRUE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Remove WPA Key process
+
+	Arguments:
+		pAd 					Pointer to our adapter
+		pBuf							Pointer to the where the key stored
+
+	Return Value:
+		NDIS_SUCCESS					Add key successfully
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+#ifdef CONFIG_STA_SUPPORT
+VOID    RTMPSetDesiredRates(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  LONG            Rates)
+{
+    NDIS_802_11_RATES aryRates;
+
+    memset(&aryRates, 0x00, sizeof(NDIS_802_11_RATES));
+    switch (pAdapter->CommonCfg.PhyMode)
+    {
+        case PHY_11A: // A only
+            switch (Rates)
+            {
+                case 6000000: //6M
+                    aryRates[0] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 9000000: //9M
+                    aryRates[0] = 0x12; // 9M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 12000000: //12M
+                    aryRates[0] = 0x18; // 12M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 18000000: //18M
+                    aryRates[0] = 0x24; // 18M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 24000000: //24M
+                    aryRates[0] = 0x30; // 24M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_4;
+                    break;
+                case 36000000: //36M
+                    aryRates[0] = 0x48; // 36M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_5;
+                    break;
+                case 48000000: //48M
+                    aryRates[0] = 0x60; // 48M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_6;
+                    break;
+                case 54000000: //54M
+                    aryRates[0] = 0x6c; // 54M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_7;
+                    break;
+                case -1: //Auto
+                default:
+                    aryRates[0] = 0x6c; // 54Mbps
+                    aryRates[1] = 0x60; // 48Mbps
+                    aryRates[2] = 0x48; // 36Mbps
+                    aryRates[3] = 0x30; // 24Mbps
+                    aryRates[4] = 0x24; // 18M
+                    aryRates[5] = 0x18; // 12M
+                    aryRates[6] = 0x12; // 9M
+                    aryRates[7] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+                    break;
+            }
+            break;
+        case PHY_11BG_MIXED: // B/G Mixed
+        case PHY_11B: // B only
+        case PHY_11ABG_MIXED: // A/B/G Mixed
+        default:
+            switch (Rates)
+            {
+                case 1000000: //1M
+                    aryRates[0] = 0x02;
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 2000000: //2M
+                    aryRates[0] = 0x04;
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 5000000: //5.5M
+                    aryRates[0] = 0x0b; // 5.5M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 11000000: //11M
+                    aryRates[0] = 0x16; // 11M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 6000000: //6M
+                    aryRates[0] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 9000000: //9M
+                    aryRates[0] = 0x12; // 9M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 12000000: //12M
+                    aryRates[0] = 0x18; // 12M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 18000000: //18M
+                    aryRates[0] = 0x24; // 18M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 24000000: //24M
+                    aryRates[0] = 0x30; // 24M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_4;
+                    break;
+                case 36000000: //36M
+                    aryRates[0] = 0x48; // 36M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_5;
+                    break;
+                case 48000000: //48M
+                    aryRates[0] = 0x60; // 48M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_6;
+                    break;
+                case 54000000: //54M
+                    aryRates[0] = 0x6c; // 54M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_7;
+                    break;
+                case -1: //Auto
+                default:
+                    if (pAdapter->CommonCfg.PhyMode == PHY_11B)
+                    { //B Only
+                        aryRates[0] = 0x16; // 11Mbps
+                        aryRates[1] = 0x0b; // 5.5Mbps
+                        aryRates[2] = 0x04; // 2Mbps
+                        aryRates[3] = 0x02; // 1Mbps
+                    }
+                    else
+                    { //(B/G) Mixed or (A/B/G) Mixed
+                        aryRates[0] = 0x6c; // 54Mbps
+                        aryRates[1] = 0x60; // 48Mbps
+                        aryRates[2] = 0x48; // 36Mbps
+                        aryRates[3] = 0x30; // 24Mbps
+                        aryRates[4] = 0x16; // 11Mbps
+                        aryRates[5] = 0x0b; // 5.5Mbps
+                        aryRates[6] = 0x04; // 2Mbps
+                        aryRates[7] = 0x02; // 1Mbps
+                    }
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+                    break;
+            }
+            break;
+    }
+
+    NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+    NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+    DBGPRINT(RT_DEBUG_TRACE, (" RTMPSetDesiredRates (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+        pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+        pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+        pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+        pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+    // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+    MlmeUpdateTxRates(pAdapter, FALSE, 0);
+}
+
+NDIS_STATUS RTMPWPARemoveKeyProc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PVOID			pBuf)
+{
+	PNDIS_802_11_REMOVE_KEY pKey;
+	ULONG					KeyIdx;
+	NDIS_STATUS 			Status = NDIS_STATUS_FAILURE;
+	BOOLEAN 	bTxKey; 		// Set the key as transmit key
+	BOOLEAN 	bPairwise;		// Indicate the key is pairwise key
+	BOOLEAN 	bKeyRSC;		// indicate the receive  SC set by KeyRSC value.
+								// Otherwise, it will set by the NIC.
+	BOOLEAN 	bAuthenticator; // indicate key is set by authenticator.
+	INT 		i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("---> RTMPWPARemoveKeyProc\n"));
+
+	pKey = (PNDIS_802_11_REMOVE_KEY) pBuf;
+	KeyIdx = pKey->KeyIndex & 0xff;
+	// Bit 31 of Add-key, Tx Key
+	bTxKey		   = (pKey->KeyIndex & 0x80000000) ? TRUE : FALSE;
+	// Bit 30 of Add-key PairwiseKey
+	bPairwise	   = (pKey->KeyIndex & 0x40000000) ? TRUE : FALSE;
+	// Bit 29 of Add-key KeyRSC
+	bKeyRSC 	   = (pKey->KeyIndex & 0x20000000) ? TRUE : FALSE;
+	// Bit 28 of Add-key Authenticator
+	bAuthenticator = (pKey->KeyIndex & 0x10000000) ? TRUE : FALSE;
+
+	// 1. If bTx is TRUE, return failure information
+	if (bTxKey == TRUE)
+		return(NDIS_STATUS_INVALID_DATA);
+
+	// 2. Check Pairwise Key
+	if (bPairwise)
+	{
+		// a. If BSSID is broadcast, remove all pairwise keys.
+		// b. If not broadcast, remove the pairwise specified by BSSID
+		for (i = 0; i < SHARE_KEY_NUM; i++)
+		{
+			if (MAC_ADDR_EQUAL(pAd->SharedKey[BSS0][i].BssId, pKey->BSSID))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveKeyProc(KeyIdx=%d)\n", i));
+				pAd->SharedKey[BSS0][i].KeyLen = 0;
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_NONE;
+				AsicRemoveSharedKeyEntry(pAd, BSS0, (UCHAR)i);
+				Status = NDIS_STATUS_SUCCESS;
+				break;
+			}
+		}
+	}
+	// 3. Group Key
+	else
+	{
+		// a. If BSSID is broadcast, remove all group keys indexed
+		// b. If BSSID matched, delete the group key indexed.
+		DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveKeyProc(KeyIdx=%ld)\n", KeyIdx));
+		pAd->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+		pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAd, BSS0, (UCHAR)KeyIdx);
+		Status = NDIS_STATUS_SUCCESS;
+	}
+
+	return (Status);
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	========================================================================
+
+	Routine Description:
+		Remove All WPA Keys
+
+	Arguments:
+		pAd 					Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPWPARemoveAllKeys(
+	IN	PRTMP_ADAPTER	pAd)
+{
+
+	UCHAR 	i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveAllKeys(AuthMode=%d, WepStatus=%d)\n", pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus));
+
+	// For WEP/CKIP, there is no need to remove it, since WinXP won't set it again after
+	// Link up. And it will be replaced if user changed it.
+	if (pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+		return;
+
+	// For WPA-None, there is no need to remove it, since WinXP won't set it again after
+	// Link up. And it will be replaced if user changed it.
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		return;
+
+	// set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	AsicRemovePairwiseKeyEntry(pAd, BSS0, BSSID_WCID);
+
+	// set all shared key mode as no-security.
+	for (i = 0; i < SHARE_KEY_NUM; i++)
+    {
+		DBGPRINT(RT_DEBUG_TRACE,("remove %s key #%d\n", CipherName[pAd->SharedKey[BSS0][i].CipherAlg], i));
+		NdisZeroMemory(&pAd->SharedKey[BSS0][i], sizeof(CIPHER_KEY));
+
+		AsicRemoveSharedKeyEntry(pAd, BSS0, i);
+	}
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	========================================================================
+	Routine Description:
+		Change NIC PHY mode. Re-association may be necessary. possible settings
+		include - PHY_11B, PHY_11BG_MIXED, PHY_11A, and PHY_11ABG_MIXED
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPSetPhyMode(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG phymode)
+{
+	INT i;
+	// the selected phymode must be supported by the RF IC encoded in E2PROM
+
+	// if no change, do nothing
+	/* bug fix
+	if (pAd->CommonCfg.PhyMode == phymode)
+		return;
+    */
+	pAd->CommonCfg.PhyMode = (UCHAR)phymode;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPSetPhyMode : PhyMode=%d, channel=%d \n", pAd->CommonCfg.PhyMode, pAd->CommonCfg.Channel));
+#ifdef EXT_BUILD_CHANNEL_LIST
+	BuildChannelListEx(pAd);
+#else
+	BuildChannelList(pAd);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// sanity check user setting
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->CommonCfg.Channel == pAd->ChannelList[i].Channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			pAd->CommonCfg.Channel = FirstChannel(pAd);
+#endif // CONFIG_STA_SUPPORT //
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSetPhyMode: channel is out of range, use first channel=%d \n", pAd->CommonCfg.Channel));
+	}
+
+	NdisZeroMemory(pAd->CommonCfg.SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+	NdisZeroMemory(pAd->CommonCfg.ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+	NdisZeroMemory(pAd->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+	switch (phymode) {
+		case PHY_11B:
+			pAd->CommonCfg.SupRate[0]  = 0x82;	  // 1 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x84;	  // 2 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[2]  = 0x8B;	  // 5.5 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x96;	  // 11 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRateLen  = 4;
+			pAd->CommonCfg.ExtRateLen  = 0;
+			pAd->CommonCfg.DesireRate[0]  = 2;	   // 1 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 4;	   // 2 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 11;    // 5.5 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 22;    // 11 mbps, in units of 0.5 Mbps
+			//pAd->CommonCfg.HTPhyMode.field.MODE = MODE_CCK; // This MODE is only FYI. not use
+			break;
+
+		case PHY_11G:
+		case PHY_11BG_MIXED:
+		case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11N_2_4G:
+		case PHY_11ABGN_MIXED:
+		case PHY_11BGN_MIXED:
+		case PHY_11GN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			pAd->CommonCfg.SupRate[0]  = 0x82;	  // 1 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x84;	  // 2 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[2]  = 0x8B;	  // 5.5 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x96;	  // 11 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[4]  = 0x12;	  // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[5]  = 0x24;	  // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[6]  = 0x48;	  // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[7]  = 0x6c;	  // 54 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRateLen  = 8;
+			pAd->CommonCfg.ExtRate[0]  = 0x0C;	  // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[1]  = 0x18;	  // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[2]  = 0x30;	  // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[3]  = 0x60;	  // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRateLen  = 4;
+			pAd->CommonCfg.DesireRate[0]  = 2;	   // 1 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 4;	   // 2 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 11;    // 5.5 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 22;    // 11 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[4]  = 12;    // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[5]  = 18;    // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[6]  = 24;    // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[7]  = 36;    // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[8]  = 48;    // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[9]  = 72;    // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[10] = 96;    // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[11] = 108;   // 54 mbps, in units of 0.5 Mbps
+			break;
+
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AN_MIXED:
+		case PHY_11AGN_MIXED:
+		case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+			pAd->CommonCfg.SupRate[0]  = 0x8C;	  // 6 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x12;	  // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[2]  = 0x98;	  // 12 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x24;	  // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[4]  = 0xb0;	  // 24 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[5]  = 0x48;	  // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[6]  = 0x60;	  // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[7]  = 0x6c;	  // 54 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRateLen  = 8;
+			pAd->CommonCfg.ExtRateLen  = 0;
+			pAd->CommonCfg.DesireRate[0]  = 12;    // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 18;    // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 24;    // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 36;    // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[4]  = 48;    // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[5]  = 72;    // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[6]  = 96;    // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[7]  = 108;   // 54 mbps, in units of 0.5 Mbps
+			//pAd->CommonCfg.HTPhyMode.field.MODE = MODE_OFDM; // This MODE is only FYI. not use
+			break;
+
+		default:
+			break;
+	}
+
+
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+}
+
+
+#ifdef DOT11_N_SUPPORT
+/*
+	========================================================================
+	Routine Description:
+		Caller ensures we has 802.11n support.
+		Calls at setting HT from AP/STASetinformation
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	========================================================================
+*/
+VOID	RTMPSetHT(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	OID_SET_HT_PHYMODE *pHTPhyMode)
+{
+	//ULONG	*pmcs;
+	UINT32	Value = 0;
+	UCHAR	BBPValue = 0;
+	UCHAR	BBP3Value = 0;
+	UCHAR	RxStream = pAd->CommonCfg.RxStream;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : HT_mode(%d), ExtOffset(%d), MCS(%d), BW(%d), STBC(%d), SHORTGI(%d)\n",
+										pHTPhyMode->HtMode, pHTPhyMode->ExtOffset,
+										pHTPhyMode->MCS, pHTPhyMode->BW,
+										pHTPhyMode->STBC, pHTPhyMode->SHORTGI));
+
+	// Don't zero supportedHyPhy structure.
+	RTMPZeroMemory(&pAd->CommonCfg.HtCapability, sizeof(pAd->CommonCfg.HtCapability));
+	RTMPZeroMemory(&pAd->CommonCfg.AddHTInfo, sizeof(pAd->CommonCfg.AddHTInfo));
+	RTMPZeroMemory(&pAd->CommonCfg.NewExtChanOffset, sizeof(pAd->CommonCfg.NewExtChanOffset));
+	RTMPZeroMemory(&pAd->CommonCfg.DesiredHtPhy, sizeof(pAd->CommonCfg.DesiredHtPhy));
+
+   	if (pAd->CommonCfg.bRdg)
+	{
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.PlusHTC = 1;
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.RDGSupport = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.PlusHTC = 0;
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.RDGSupport = 0;
+	}
+
+	pAd->CommonCfg.HtCapability.HtCapParm.MaxRAmpduFactor = 3;
+	pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor = 3;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : RxBAWinLimit = %d\n", pAd->CommonCfg.BACapability.field.RxBAWinLimit));
+
+	// Mimo power save, A-MSDU size,
+	pAd->CommonCfg.DesiredHtPhy.AmsduEnable = (USHORT)pAd->CommonCfg.BACapability.field.AmsduEnable;
+	pAd->CommonCfg.DesiredHtPhy.AmsduSize = (UCHAR)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.DesiredHtPhy.MimoPs = (UCHAR)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.DesiredHtPhy.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+
+	pAd->CommonCfg.HtCapability.HtCapInfo.AMsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.HtCapability.HtCapInfo.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : AMsduSize = %d, MimoPs = %d, MpduDensity = %d, MaxRAmpduFactor = %d\n",
+													pAd->CommonCfg.DesiredHtPhy.AmsduSize,
+													pAd->CommonCfg.DesiredHtPhy.MimoPs,
+													pAd->CommonCfg.DesiredHtPhy.MpduDensity,
+													pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor));
+
+	if(pHTPhyMode->HtMode == HTMODE_GF)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.GF = 1;
+		pAd->CommonCfg.DesiredHtPhy.GF = 1;
+	}
+	else
+		pAd->CommonCfg.DesiredHtPhy.GF = 0;
+
+	// Decide Rx MCSSet
+	switch (RxStream)
+	{
+		case 1:
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0x00;
+			break;
+
+		case 2:
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0xff;
+			break;
+
+		case 3: // 3*3
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[2] =  0xff;
+			break;
+	}
+
+	if (pAd->CommonCfg.bForty_Mhz_Intolerant && (pAd->CommonCfg.Channel <= 14) && (pHTPhyMode->BW == BW_40) )
+	{
+		pHTPhyMode->BW = BW_20;
+		pAd->CommonCfg.HtCapability.HtCapInfo.Forty_Mhz_Intolerant = 1;
+	}
+
+	if(pHTPhyMode->BW == BW_40)
+	{
+		pAd->CommonCfg.HtCapability.MCSSet[4] = 0x1; // MCS 32
+		pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth = 1;
+		if (pAd->CommonCfg.Channel <= 14)
+			pAd->CommonCfg.HtCapability.HtCapInfo.CCKmodein40 = 1;
+
+		pAd->CommonCfg.DesiredHtPhy.ChannelWidth = 1;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth = 1;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset = (pHTPhyMode->ExtOffset == EXTCHA_BELOW)? (EXTCHA_BELOW): EXTCHA_ABOVE;
+		// Set Regsiter for extension channel position.
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBP3Value);
+		if ((pHTPhyMode->ExtOffset == EXTCHA_BELOW))
+		{
+			Value |= 0x1;
+			BBP3Value |= (0x20);
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+		}
+		else if ((pHTPhyMode->ExtOffset == EXTCHA_ABOVE))
+		{
+			Value &= 0xfe;
+			BBP3Value &= (~0x20);
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+		}
+
+		// Turn on BBP 40MHz mode now only as AP .
+		// Sta can turn on BBP 40MHz after connection with 40MHz AP. Sta only broadcast 40MHz capability before connection.
+		if ((pAd->OpMode == OPMODE_AP) || INFRA_ON(pAd) || ADHOC_ON(pAd)
+			)
+		{
+			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);
+
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBP3Value);
+			pAd->CommonCfg.BBPCurrentBW = BW_40;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth = 0;
+		pAd->CommonCfg.DesiredHtPhy.ChannelWidth = 0;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth = 0;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset = EXTCHA_NONE;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		// Turn on BBP 20MHz mode by request here.
+		{
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			pAd->CommonCfg.BBPCurrentBW = BW_20;
+		}
+	}
+
+	if(pHTPhyMode->STBC == STBC_USE)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.TxSTBC = 1;
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 1;
+		pAd->CommonCfg.HtCapability.HtCapInfo.RxSTBC = 1;
+		pAd->CommonCfg.DesiredHtPhy.RxSTBC = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 0;
+		pAd->CommonCfg.DesiredHtPhy.RxSTBC = 0;
+	}
+
+#ifdef RT2870
+	/* Frank recommend ,If not, Tx maybe block in high power. Rx has no problem*/
+	if(IS_RT3070(pAd) && ((pAd->RfIcType == RFIC_3020) || (pAd->RfIcType == RFIC_2020)))
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.TxSTBC = 0;
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 0;
+	}
+#endif // RT2870 //
+
+	if(pHTPhyMode->SHORTGI == GI_400)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor20 = 1;
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor40 = 1;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 = 1;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor20 = 0;
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor40 = 0;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 = 0;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 = 0;
+	}
+
+	// We support link adaptation for unsolicit MCS feedback, set to 2.
+	pAd->CommonCfg.HtCapability.ExtHtCapInfo.MCSFeedback = MCSFBK_NONE; //MCSFBK_UNSOLICIT;
+	pAd->CommonCfg.AddHTInfo.ControlChan = pAd->CommonCfg.Channel;
+	// 1, the extension channel above the control channel.
+
+	// EDCA parameters used for AP's own transmission
+	if (pAd->CommonCfg.APEdcaParm.bValid == FALSE)
+	{
+		pAd->CommonCfg.APEdcaParm.bValid = TRUE;
+		pAd->CommonCfg.APEdcaParm.Aifsn[0] = 3;
+		pAd->CommonCfg.APEdcaParm.Aifsn[1] = 7;
+		pAd->CommonCfg.APEdcaParm.Aifsn[2] = 1;
+		pAd->CommonCfg.APEdcaParm.Aifsn[3] = 1;
+
+		pAd->CommonCfg.APEdcaParm.Cwmin[0] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[1] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[2] = 3;
+		pAd->CommonCfg.APEdcaParm.Cwmin[3] = 2;
+
+		pAd->CommonCfg.APEdcaParm.Cwmax[0] = 6;
+		pAd->CommonCfg.APEdcaParm.Cwmax[1] = 10;
+		pAd->CommonCfg.APEdcaParm.Cwmax[2] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmax[3] = 3;
+
+		pAd->CommonCfg.APEdcaParm.Txop[0]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[1]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[2]  = 94;
+		pAd->CommonCfg.APEdcaParm.Txop[3]  = 47;
+	}
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		RTMPSetIndividualHT(pAd, 0);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Caller ensures we has 802.11n support.
+		Calls at setting HT from AP/STASetinformation
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	========================================================================
+*/
+VOID	RTMPSetIndividualHT(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	UCHAR				apidx)
+{
+	PRT_HT_PHY_INFO		pDesired_ht_phy = NULL;
+	UCHAR	TxStream = pAd->CommonCfg.TxStream;
+	UCHAR	DesiredMcs	= MCS_AUTO;
+
+	do
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pDesired_ht_phy = &pAd->StaCfg.DesiredHtPhyInfo;
+			DesiredMcs = pAd->StaCfg.DesiredTransmitSetting.field.MCS;
+			//pAd->StaCfg.bAutoTxRateSwitch = (DesiredMcs == MCS_AUTO) ? TRUE : FALSE;
+				break;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	} while (FALSE);
+
+	if (pDesired_ht_phy == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSetIndividualHT: invalid apidx(%d)\n", apidx));
+		return;
+	}
+	RTMPZeroMemory(pDesired_ht_phy, sizeof(RT_HT_PHY_INFO));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetIndividualHT : Desired MCS = %d\n", DesiredMcs));
+	// Check the validity of MCS
+	if ((TxStream == 1) && ((DesiredMcs >= MCS_8) && (DesiredMcs <= MCS_15)))
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("RTMPSetIndividualHT: MCS(%d) is invalid in 1S, reset it as MCS_7\n", DesiredMcs));
+		DesiredMcs = MCS_7;
+	}
+
+	if ((pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BW_20) && (DesiredMcs == MCS_32))
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("RTMPSetIndividualHT: MCS_32 is only supported in 40-MHz, reset it as MCS_0\n"));
+		DesiredMcs = MCS_0;
+	}
+
+	pDesired_ht_phy->bHtEnable = TRUE;
+
+	// Decide desired Tx MCS
+	switch (TxStream)
+	{
+		case 1:
+			if (DesiredMcs == MCS_AUTO)
+			{
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0x00;
+			}
+			else if (DesiredMcs <= MCS_7)
+			{
+				pDesired_ht_phy->MCSSet[0]= 1<<DesiredMcs;
+				pDesired_ht_phy->MCSSet[1]= 0x00;
+			}
+			break;
+
+		case 2:
+			if (DesiredMcs == MCS_AUTO)
+			{
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0xff;
+			}
+			else if (DesiredMcs <= MCS_15)
+			{
+				ULONG mode;
+
+				mode = DesiredMcs / 8;
+				if (mode < 2)
+					pDesired_ht_phy->MCSSet[mode] = (1 << (DesiredMcs - mode * 8));
+			}
+			break;
+
+		case 3: // 3*3
+			if (DesiredMcs == MCS_AUTO)
+			{
+				/* MCS0 ~ MCS23, 3 bytes */
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0xff;
+				pDesired_ht_phy->MCSSet[2]= 0xff;
+			}
+			else if (DesiredMcs <= MCS_23)
+			{
+				ULONG mode;
+
+				mode = DesiredMcs / 8;
+				if (mode < 3)
+					pDesired_ht_phy->MCSSet[mode] = (1 << (DesiredMcs - mode * 8));
+			}
+			break;
+	}
+
+	if(pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BW_40)
+	{
+		if (DesiredMcs == MCS_AUTO || DesiredMcs == MCS_32)
+			pDesired_ht_phy->MCSSet[4] = 0x1;
+	}
+
+	// update HT Rate setting
+    if (pAd->OpMode == OPMODE_STA)
+        MlmeUpdateHtTxRates(pAd, BSS0);
+    else
+	    MlmeUpdateHtTxRates(pAd, apidx);
+}
+
+
+/*
+	========================================================================
+	Routine Description:
+		Update HT IE from our capability.
+
+	Arguments:
+		Send all HT IE in beacon/probe rsp/assoc rsp/action frame.
+
+
+	========================================================================
+*/
+VOID	RTMPUpdateHTIE(
+	IN	RT_HT_CAPABILITY	*pRtHt,
+	IN		UCHAR				*pMcsSet,
+	OUT		HT_CAPABILITY_IE *pHtCapability,
+	OUT		ADD_HT_INFO_IE		*pAddHtInfo)
+{
+	RTMPZeroMemory(pHtCapability, sizeof(HT_CAPABILITY_IE));
+	RTMPZeroMemory(pAddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+		pHtCapability->HtCapInfo.ChannelWidth = pRtHt->ChannelWidth;
+		pHtCapability->HtCapInfo.MimoPs = pRtHt->MimoPs;
+		pHtCapability->HtCapInfo.GF = pRtHt->GF;
+		pHtCapability->HtCapInfo.ShortGIfor20 = pRtHt->ShortGIfor20;
+		pHtCapability->HtCapInfo.ShortGIfor40 = pRtHt->ShortGIfor40;
+		pHtCapability->HtCapInfo.TxSTBC = pRtHt->TxSTBC;
+		pHtCapability->HtCapInfo.RxSTBC = pRtHt->RxSTBC;
+		pHtCapability->HtCapInfo.AMsduSize = pRtHt->AmsduSize;
+		pHtCapability->HtCapParm.MaxRAmpduFactor = pRtHt->MaxRAmpduFactor;
+		pHtCapability->HtCapParm.MpduDensity = pRtHt->MpduDensity;
+
+		pAddHtInfo->AddHtInfo.ExtChanOffset = pRtHt->ExtChanOffset ;
+		pAddHtInfo->AddHtInfo.RecomWidth = pRtHt->RecomWidth;
+		pAddHtInfo->AddHtInfo2.OperaionMode = pRtHt->OperaionMode;
+		pAddHtInfo->AddHtInfo2.NonGfPresent = pRtHt->NonGfPresent;
+		RTMPMoveMemory(pAddHtInfo->MCSSet, /*pRtHt->MCSSet*/pMcsSet, 4); // rt2860 only support MCS max=32, no need to copy all 16 uchar.
+
+        DBGPRINT(RT_DEBUG_TRACE,("RTMPUpdateHTIE <== \n"));
+}
+#endif // DOT11_N_SUPPORT //
+
+/*
+	========================================================================
+	Description:
+		Add Client security information into ASIC WCID table and IVEIV table.
+    Return:
+	========================================================================
+*/
+VOID	RTMPAddWcidAttributeEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BssIdx,
+	IN 	UCHAR		 	KeyIdx,
+	IN 	UCHAR		 	CipherAlg,
+	IN 	MAC_TABLE_ENTRY *pEntry)
+{
+	UINT32		WCIDAttri = 0;
+	USHORT		offset;
+	UCHAR		IVEIV = 0;
+	USHORT		Wcid = 0;
+
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (BssIdx > BSS0)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("RTMPAddWcidAttributeEntry: The BSS-index(%d) is out of range for Infra link. \n", BssIdx));
+				return;
+			}
+
+			// 1.	In ADHOC mode, the AID is wcid number. And NO mesh link exists.
+			// 2.	In Infra mode, the AID:1 MUST be wcid of infra STA.
+			//					   the AID:2~ assign to mesh link entry.
+			if (pEntry && ADHOC_ON(pAd))
+				Wcid = pEntry->Aid;
+			else if (pEntry && INFRA_ON(pAd))
+			{
+#ifdef QOS_DLS_SUPPORT
+				if (pEntry->ValidAsDls == TRUE)
+					Wcid = pEntry->Aid;
+				else
+#endif // QOS_DLS_SUPPORT //
+				Wcid = BSSID_WCID;
+			}
+			else
+				Wcid = MCAST_WCID;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// Update WCID attribute table
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Wcid * HW_WCID_ATTRI_SIZE);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pEntry && pEntry->ValidAsMesh)
+			WCIDAttri = (CipherAlg<<1) | PAIRWISEKEYTABLE;
+#ifdef QOS_DLS_SUPPORT
+		else if ((pEntry) && (pEntry->ValidAsDls) &&
+					((CipherAlg == CIPHER_TKIP) ||
+				 	(CipherAlg == CIPHER_TKIP_NO_MIC) ||
+					(CipherAlg == CIPHER_AES) ||
+				 	(CipherAlg == CIPHER_NONE)))
+			WCIDAttri = (CipherAlg<<1) | PAIRWISEKEYTABLE;
+#endif // QOS_DLS_SUPPORT //
+		else
+			WCIDAttri = (CipherAlg<<1) | SHAREDKEYTABLE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+
+
+	// Update IV/EIV table
+	offset = MAC_IVEIV_TABLE_BASE + (Wcid * HW_IVEIV_ENTRY_SIZE);
+
+	// WPA mode
+	if ((CipherAlg == CIPHER_TKIP) || (CipherAlg == CIPHER_TKIP_NO_MIC) || (CipherAlg == CIPHER_AES))
+	{
+		// Eiv bit on. keyid always is 0 for pairwise key
+		IVEIV = (KeyIdx <<6) | 0x20;
+	}
+	else
+	{
+		// WEP KeyIdx is default tx key.
+		IVEIV = (KeyIdx << 6);
+	}
+
+	// For key index and ext IV bit, so only need to update the position(offset+3).
+#ifdef RT2870
+	RTUSBMultiWrite_OneByte(pAd, offset+3, &IVEIV);
+#endif // RT2870 //
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPAddWcidAttributeEntry: WCID #%d, KeyIndex #%d, Alg=%s\n",Wcid, KeyIdx, CipherName[CipherAlg]));
+	DBGPRINT(RT_DEBUG_TRACE,("	WCIDAttri = 0x%x \n",  WCIDAttri));
+
+}
+
+/*
+    ==========================================================================
+    Description:
+        Parse encryption type
+Arguments:
+    pAdapter                    Pointer to our adapter
+    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+    ==========================================================================
+*/
+CHAR *GetEncryptType(CHAR enc)
+{
+    if(enc == Ndis802_11WEPDisabled)
+        return "NONE";
+    if(enc == Ndis802_11WEPEnabled)
+    	return "WEP";
+    if(enc == Ndis802_11Encryption2Enabled)
+    	return "TKIP";
+    if(enc == Ndis802_11Encryption3Enabled)
+    	return "AES";
+	if(enc == Ndis802_11Encryption4Enabled)
+    	return "TKIPAES";
+    else
+    	return "UNKNOW";
+}
+
+CHAR *GetAuthMode(CHAR auth)
+{
+    if(auth == Ndis802_11AuthModeOpen)
+    	return "OPEN";
+    if(auth == Ndis802_11AuthModeShared)
+    	return "SHARED";
+	if(auth == Ndis802_11AuthModeAutoSwitch)
+    	return "AUTOWEP";
+    if(auth == Ndis802_11AuthModeWPA)
+    	return "WPA";
+    if(auth == Ndis802_11AuthModeWPAPSK)
+    	return "WPAPSK";
+    if(auth == Ndis802_11AuthModeWPANone)
+    	return "WPANONE";
+    if(auth == Ndis802_11AuthModeWPA2)
+    	return "WPA2";
+    if(auth == Ndis802_11AuthModeWPA2PSK)
+    	return "WPA2PSK";
+	if(auth == Ndis802_11AuthModeWPA1WPA2)
+    	return "WPA1WPA2";
+	if(auth == Ndis802_11AuthModeWPA1PSKWPA2PSK)
+    	return "WPA1PSKWPA2PSK";
+
+    	return "UNKNOW";
+}
+
+#if 1 //#ifndef UCOS
+/*
+    ==========================================================================
+    Description:
+        Get site survey results
+	Arguments:
+	    pAdapter                    Pointer to our adapter
+	    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+        		1.) UI needs to wait 4 seconds after issue a site survey command
+        		2.) iwpriv ra0 get_site_survey
+        		3.) UI needs to prepare at least 4096bytes to get the results
+    ==========================================================================
+*/
+#define	LINE_LEN	(4+33+20+8+10+9+7+3)	// Channel+SSID+Bssid+WepStatus+AuthMode+Signal+WiressMode+NetworkType
+VOID RTMPIoctlGetSiteSurvey(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR		*msg;
+	INT 		i=0;
+	INT			WaitCnt;
+	INT 		Status=0;
+	CHAR		Ssid[MAX_LEN_OF_SSID +1];
+    INT         Rssi = 0, max_len = LINE_LEN;
+	UINT        Rssi_Quality = 0;
+	NDIS_802_11_NETWORK_TYPE    wireless_mode;
+
+	os_alloc_mem(NULL, (PUCHAR *)&msg, sizeof(CHAR)*((MAX_LEN_OF_BSS_TABLE)*max_len));
+
+	if (msg == NULL)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPIoctlGetSiteSurvey - msg memory alloc fail.\n"));
+		return;
+	}
+
+	memset(msg, 0 ,(MAX_LEN_OF_BSS_TABLE)*max_len );
+	memset(Ssid, 0 ,(MAX_LEN_OF_SSID +1));
+	sprintf(msg,"%s","\n");
+	sprintf(msg+strlen(msg),"%-4s%-33s%-20s%-8s%-10s%-9s%-7s%-3s\n",
+	    "Ch", "SSID", "BSSID", "Enc", "Auth", "Siganl(%)", "W-Mode", " NT");
+
+
+	WaitCnt = 0;
+#ifdef CONFIG_STA_SUPPORT
+	pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	while ((ScanRunning(pAdapter) == TRUE) && (WaitCnt++ < 200))
+		OS_WAIT(500);
+#endif // CONFIG_STA_SUPPORT //
+
+	for(i=0; i<pAdapter->ScanTab.BssNr ;i++)
+	{
+		if( pAdapter->ScanTab.BssEntry[i].Channel==0)
+			break;
+
+		if((strlen(msg)+max_len ) >= IW_SCAN_MAX_DATA)
+			break;
+
+		//Channel
+		sprintf(msg+strlen(msg),"%-4d", pAdapter->ScanTab.BssEntry[i].Channel);
+		//SSID
+		memcpy(Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+		Ssid[pAdapter->ScanTab.BssEntry[i].SsidLen] = '\0';
+		sprintf(msg+strlen(msg),"%-33s", Ssid);
+		//BSSID
+		sprintf(msg+strlen(msg),"%02x:%02x:%02x:%02x:%02x:%02x   ",
+			pAdapter->ScanTab.BssEntry[i].Bssid[0],
+			pAdapter->ScanTab.BssEntry[i].Bssid[1],
+			pAdapter->ScanTab.BssEntry[i].Bssid[2],
+			pAdapter->ScanTab.BssEntry[i].Bssid[3],
+			pAdapter->ScanTab.BssEntry[i].Bssid[4],
+			pAdapter->ScanTab.BssEntry[i].Bssid[5]);
+		//Encryption Type
+		sprintf(msg+strlen(msg),"%-8s",GetEncryptType(pAdapter->ScanTab.BssEntry[i].WepStatus));
+		//Authentication Mode
+		if (pAdapter->ScanTab.BssEntry[i].WepStatus == Ndis802_11WEPEnabled)
+			sprintf(msg+strlen(msg),"%-10s", "UNKNOW");
+		else
+			sprintf(msg+strlen(msg),"%-10s",GetAuthMode(pAdapter->ScanTab.BssEntry[i].AuthMode));
+		// Rssi
+		Rssi = (INT)pAdapter->ScanTab.BssEntry[i].Rssi;
+		if (Rssi >= -50)
+			Rssi_Quality = 100;
+		else if (Rssi >= -80)    // between -50 ~ -80dbm
+			Rssi_Quality = (UINT)(24 + ((Rssi + 80) * 26)/10);
+		else if (Rssi >= -90)   // between -80 ~ -90dbm
+			Rssi_Quality = (UINT)(((Rssi + 90) * 26)/10);
+		else    // < -84 dbm
+			Rssi_Quality = 0;
+		sprintf(msg+strlen(msg),"%-9d", Rssi_Quality);
+		// Wireless Mode
+		wireless_mode = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+		if (wireless_mode == Ndis802_11FH ||
+			wireless_mode == Ndis802_11DS)
+			sprintf(msg+strlen(msg),"%-7s", "11b");
+		else if (wireless_mode == Ndis802_11OFDM5)
+			sprintf(msg+strlen(msg),"%-7s", "11a");
+		else if (wireless_mode == Ndis802_11OFDM5_N)
+			sprintf(msg+strlen(msg),"%-7s", "11a/n");
+		else if (wireless_mode == Ndis802_11OFDM24)
+			sprintf(msg+strlen(msg),"%-7s", "11b/g");
+		else if (wireless_mode == Ndis802_11OFDM24_N)
+			sprintf(msg+strlen(msg),"%-7s", "11b/g/n");
+		else
+			sprintf(msg+strlen(msg),"%-7s", "unknow");
+		//Network Type
+		if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_ADHOC)
+			sprintf(msg+strlen(msg),"%-3s", " Ad");
+		else
+			sprintf(msg+strlen(msg),"%-3s", " In");
+
+        sprintf(msg+strlen(msg),"\n");
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPIoctlGetSiteSurvey - wrq->u.data.length = %d\n", wrq->u.data.length));
+	os_free_mem(NULL, (PUCHAR)msg);
+}
+
+
+#define	MAC_LINE_LEN	(14+4+4+10+10+10+6+6)	// Addr+aid+psm+datatime+rxbyte+txbyte+current tx rate+last tx rate
+VOID RTMPIoctlGetMacTable(
+	IN PRTMP_ADAPTER pAd,
+	IN struct iwreq *wrq)
+{
+	INT i;
+	RT_802_11_MAC_TABLE MacTab;
+	char *msg;
+
+	MacTab.Num = 0;
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		if (pAd->MacTab.Content[i].ValidAsCLI && (pAd->MacTab.Content[i].Sst == SST_ASSOC))
+		{
+			COPY_MAC_ADDR(MacTab.Entry[MacTab.Num].Addr, &pAd->MacTab.Content[i].Addr);
+			MacTab.Entry[MacTab.Num].Aid = (UCHAR)pAd->MacTab.Content[i].Aid;
+			MacTab.Entry[MacTab.Num].Psm = pAd->MacTab.Content[i].PsMode;
+#ifdef DOT11_N_SUPPORT
+			MacTab.Entry[MacTab.Num].MimoPs = pAd->MacTab.Content[i].MmpsMode;
+#endif // DOT11_N_SUPPORT //
+
+			// Fill in RSSI per entry
+			MacTab.Entry[MacTab.Num].AvgRssi0 = pAd->MacTab.Content[i].RssiSample.AvgRssi0;
+			MacTab.Entry[MacTab.Num].AvgRssi1 = pAd->MacTab.Content[i].RssiSample.AvgRssi1;
+			MacTab.Entry[MacTab.Num].AvgRssi2 = pAd->MacTab.Content[i].RssiSample.AvgRssi2;
+
+			// the connected time per entry
+			MacTab.Entry[MacTab.Num].ConnectedTime = pAd->MacTab.Content[i].StaConnectTime;
+#if 0 // ToDo
+			MacTab.Entry[MacTab.Num].HSCounter.LastDataPacketTime = pAd->MacTab.Content[i].HSCounter.LastDataPacketTime;
+			MacTab.Entry[MacTab.Num].HSCounter.TotalRxByteCount = pAd->MacTab.Content[i].HSCounter.TotalRxByteCount;
+			MacTab.Entry[MacTab.Num].HSCounter.TotalTxByteCount = pAd->MacTab.Content[i].HSCounter.TotalTxByteCount;
+#endif
+			MacTab.Entry[MacTab.Num].TxRate.field.MCS = pAd->MacTab.Content[i].HTPhyMode.field.MCS;
+			MacTab.Entry[MacTab.Num].TxRate.field.BW = pAd->MacTab.Content[i].HTPhyMode.field.BW;
+			MacTab.Entry[MacTab.Num].TxRate.field.ShortGI = pAd->MacTab.Content[i].HTPhyMode.field.ShortGI;
+			MacTab.Entry[MacTab.Num].TxRate.field.STBC = pAd->MacTab.Content[i].HTPhyMode.field.STBC;
+			MacTab.Entry[MacTab.Num].TxRate.field.rsv = pAd->MacTab.Content[i].HTPhyMode.field.rsv;
+			MacTab.Entry[MacTab.Num].TxRate.field.MODE = pAd->MacTab.Content[i].HTPhyMode.field.MODE;
+			MacTab.Entry[MacTab.Num].TxRate.word = pAd->MacTab.Content[i].HTPhyMode.word;
+
+			MacTab.Num += 1;
+		}
+	}
+	wrq->u.data.length = sizeof(RT_802_11_MAC_TABLE);
+	if (copy_to_user(wrq->u.data.pointer, &MacTab, wrq->u.data.length))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s: copy_to_user() fail\n", __FUNCTION__));
+	}
+
+	msg = (CHAR *) kmalloc(sizeof(CHAR)*(MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN), MEM_ALLOC_FLAG);
+	memset(msg, 0 ,MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN );
+	sprintf(msg,"%s","\n");
+	sprintf(msg+strlen(msg),"%-14s%-4s%-4s%-10s%-10s%-10s%-6s%-6s\n",
+		"MAC", "AID", "PSM", "LDT", "RxB", "TxB","CTxR", "LTxR");
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (pEntry->ValidAsCLI && (pEntry->Sst == SST_ASSOC))
+		{
+			if((strlen(msg)+MAC_LINE_LEN ) >= (MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN) )
+				break;
+			sprintf(msg+strlen(msg),"%02x%02x%02x%02x%02x%02x  ",
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			sprintf(msg+strlen(msg),"%-4d", (int)pEntry->Aid);
+			sprintf(msg+strlen(msg),"%-4d", (int)pEntry->PsMode);
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.LastDataPacketTime*/); // ToDo
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.TotalRxByteCount*/); // ToDo
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.TotalTxByteCount*/); // ToDo
+			sprintf(msg+strlen(msg),"%-6d",RateIdToMbps[pAd->MacTab.Content[i].CurrTxRate]);
+			sprintf(msg+strlen(msg),"%-6d\n",0/*RateIdToMbps[pAd->MacTab.Content[i].LastTxRate]*/); // ToDo
+		}
+	}
+	// for compatible with old API just do the printk to console
+	//wrq->u.data.length = strlen(msg);
+	//if (copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s", msg));
+	}
+
+	kfree(msg);
+}
+#endif // UCOS //
+
+#ifdef DOT11_N_SUPPORT
+INT	Set_BASetup_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+/*
+	The BASetup inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+	//printk("\n%s\n", arg);
+
+	if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > 15)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x\n", mac[0], mac[1],
+				mac[2], mac[3], mac[4], mac[5], tid);
+
+	    pEntry = MacTableLookup(pAd, mac);
+
+    	if (pEntry) {
+        	printk("\nSetup BA Session: Tid = %d\n", tid);
+	        BAOriSessionSetUp(pAd, pEntry, tid, 0, 100, TRUE);
+    	}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_BADecline_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG bBADecline;
+
+	bBADecline = simple_strtol(arg, 0, 10);
+
+	if (bBADecline == 0)
+	{
+		pAd->CommonCfg.bBADecline = FALSE;
+	}
+	else if (bBADecline == 1)
+	{
+		pAd->CommonCfg.bBADecline = TRUE;
+	}
+	else
+	{
+		return FALSE; //Invalid argument
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_BADecline_Proc::(BADecline=%d)\n", pAd->CommonCfg.bBADecline));
+
+	return TRUE;
+}
+
+INT	Set_BAOriTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BAOriTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > NUM_OF_TID)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+	    printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+	           mac[2], mac[3], mac[4], mac[5], tid);
+
+	    pEntry = MacTableLookup(pAd, mac);
+
+	    if (pEntry) {
+	        printk("\nTear down Ori BA Session: Tid = %d\n", tid);
+        BAOriSessionTearDown(pAd, pEntry->Aid, tid, FALSE, TRUE);
+	    }
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_BARecTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BARecTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > NUM_OF_TID)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+		       mac[2], mac[3], mac[4], mac[5], tid);
+
+		pEntry = MacTableLookup(pAd, mac);
+
+		if (pEntry) {
+		    printk("\nTear down Rec BA Session: Tid = %d\n", tid);
+		    BARecSessionTearDown(pAd, pEntry->Aid, tid, FALSE);
+		}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtBw;
+
+	HtBw = simple_strtol(arg, 0, 10);
+	if (HtBw == BW_40)
+		pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_40;
+	else if (HtBw == BW_20)
+		pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+	else
+		return FALSE;  //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtBw_Proc::(HtBw=%d)\n", pAd->CommonCfg.RegTransmitSetting.field.BW));
+
+	return TRUE;
+}
+
+INT	Set_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtMcs, Mcs_tmp;
+#ifdef CONFIG_STA_SUPPORT
+    BOOLEAN bAutoRate = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+
+	Mcs_tmp = simple_strtol(arg, 0, 10);
+
+	if (Mcs_tmp <= 15 || Mcs_tmp == 32)
+		HtMcs = Mcs_tmp;
+	else
+		HtMcs = MCS_AUTO;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = HtMcs;
+		pAd->StaCfg.bAutoTxRateSwitch = (HtMcs == MCS_AUTO) ? TRUE:FALSE;
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMcs_Proc::(HtMcs=%d, bAutoTxRateSwitch = %d)\n",
+						pAd->StaCfg.DesiredTransmitSetting.field.MCS, pAd->StaCfg.bAutoTxRateSwitch));
+
+		if ((pAd->CommonCfg.PhyMode < PHY_11ABGN_MIXED) ||
+			(pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE < MODE_HTMIX))
+		{
+	        if ((pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO) &&
+				(HtMcs >= 0 && HtMcs <= 3) &&
+	            (pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode == FIXED_TXMODE_CCK))
+			{
+				RTMPSetDesiredRates(pAd, (LONG) (RateIdToMbps[HtMcs] * 1000000));
+			}
+	        else if ((pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO) &&
+					(HtMcs >= 0 && HtMcs <= 7) &&
+	            	(pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode == FIXED_TXMODE_OFDM))
+			{
+				RTMPSetDesiredRates(pAd, (LONG) (RateIdToMbps[HtMcs+4] * 1000000));
+			}
+			else
+				bAutoRate = TRUE;
+
+			if (bAutoRate)
+			{
+	            pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+				RTMPSetDesiredRates(pAd, -1);
+			}
+	        DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMcs_Proc::(FixedTxMode=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode));
+		}
+        if (ADHOC_ON(pAd))
+            return TRUE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	SetCommonHT(pAd);
+
+	return TRUE;
+}
+
+INT	Set_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtGi;
+
+	HtGi = simple_strtol(arg, 0, 10);
+
+	if ( HtGi == GI_400)
+		pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_400;
+	else if ( HtGi == GI_800 )
+		pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_800;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtGi_Proc::(ShortGI=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.ShortGI));
+
+	return TRUE;
+}
+
+
+INT	Set_HtTxBASize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR Size;
+
+	Size = simple_strtol(arg, 0, 10);
+
+	if (Size <=0 || Size >=64)
+	{
+		Size = 8;
+	}
+	pAd->CommonCfg.TxBASize = Size-1;
+	DBGPRINT(RT_DEBUG_ERROR, ("Set_HtTxBASize ::(TxBASize= %d)\n", Size));
+
+	return TRUE;
+}
+
+
+INT	Set_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == HTMODE_GF)
+		pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_GF;
+	else if ( Value == HTMODE_MM )
+		pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_MM;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtOpMode_Proc::(HtOpMode=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.HTMODE));
+
+	return TRUE;
+
+}
+
+INT	Set_HtStbc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == STBC_USE)
+		pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_USE;
+	else if ( Value == STBC_NONE )
+		pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_NONE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_Stbc_Proc::(HtStbc=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.STBC));
+
+	return TRUE;
+}
+
+INT	Set_HtHtc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->HTCEnable = FALSE;
+	else if ( Value ==1 )
+        	pAd->HTCEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtHtc_Proc::(HtHtc=%d)\n",pAd->HTCEnable));
+
+	return TRUE;
+}
+
+INT	Set_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == 0)
+		pAd->CommonCfg.RegTransmitSetting.field.EXTCHA  = EXTCHA_BELOW;
+	else if ( Value ==1 )
+        pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtExtcha_Proc::(HtExtcha=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.EXTCHA));
+
+	return TRUE;
+}
+
+INT	Set_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value <=7 && Value >= 0)
+		pAd->CommonCfg.BACapability.field.MpduDensity = Value;
+	else
+		pAd->CommonCfg.BACapability.field.MpduDensity = 4;
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMpduDensity_Proc::(HtMpduDensity=%d)\n",pAd->CommonCfg.BACapability.field.MpduDensity));
+
+	return TRUE;
+}
+
+INT	Set_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+
+	if (Value >=1 && Value <= 64)
+	{
+		pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = Value;
+		pAd->CommonCfg.BACapability.field.RxBAWinLimit = Value;
+	}
+	else
+	{
+        pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = 64;
+		pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64;
+	}
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtBaWinSize_Proc::(HtBaWinSize=%d)\n",pAd->CommonCfg.BACapability.field.RxBAWinLimit));
+
+	return TRUE;
+}
+
+INT	Set_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == 0)
+		pAd->CommonCfg.bRdg = FALSE;
+	else if ( Value ==1 )
+	{
+		pAd->HTCEnable = TRUE;
+        	pAd->CommonCfg.bRdg = TRUE;
+	}
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtRdg_Proc::(HtRdg=%d)\n",pAd->CommonCfg.bRdg));
+
+	return TRUE;
+}
+
+INT	Set_HtLinkAdapt_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->bLinkAdapt = FALSE;
+	else if ( Value ==1 )
+	{
+			pAd->HTCEnable = TRUE;
+			pAd->bLinkAdapt = TRUE;
+	}
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtLinkAdapt_Proc::(HtLinkAdapt=%d)\n",pAd->bLinkAdapt));
+
+	return TRUE;
+}
+
+INT	Set_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.BACapability.field.AmsduEnable = FALSE;
+	else if ( Value == 1 )
+        pAd->CommonCfg.BACapability.field.AmsduEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtAmsdu_Proc::(HtAmsdu=%d)\n",pAd->CommonCfg.BACapability.field.AmsduEnable));
+
+	return TRUE;
+}
+
+INT	Set_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+    else if (Value == 1)
+		pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+    pAd->CommonCfg.REGBACapability.field.AutoBA = pAd->CommonCfg.BACapability.field.AutoBA;
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtAutoBa_Proc::(HtAutoBa=%d)\n",pAd->CommonCfg.BACapability.field.AutoBA));
+
+	return TRUE;
+
+}
+
+INT	Set_HtProtect_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.bHTProtect = FALSE;
+    else if (Value == 1)
+		pAd->CommonCfg.bHTProtect = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtProtect_Proc::(HtProtect=%d)\n",pAd->CommonCfg.bHTProtect));
+
+	return TRUE;
+}
+
+INT	Set_SendPSMPAction_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], mode;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BARecTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the mode value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and mode value in decimal format.
+		return FALSE;
+
+   	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		mode = simple_strtol((token+1), 0, 10);
+		if (mode > MMPS_ENABLE)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+		       mac[2], mac[3], mac[4], mac[5], mode);
+
+		pEntry = MacTableLookup(pAd, mac);
+
+		if (pEntry) {
+		    printk("\nSendPSMPAction MIPS mode = %d\n", mode);
+		    SendPSMPAction(pAd, pEntry->Aid, mode);
+		}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+
+}
+
+INT	Set_HtMIMOPSmode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value <=3 && Value >= 0)
+		pAd->CommonCfg.BACapability.field.MMPSmode = Value;
+	else
+		pAd->CommonCfg.BACapability.field.MMPSmode = 3;
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMIMOPSmode_Proc::(MIMOPS mode=%d)\n",pAd->CommonCfg.BACapability.field.MMPSmode));
+
+	return TRUE;
+}
+
+
+INT	Set_ForceShortGI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->WIFItestbed.bShortGI = FALSE;
+	else if (Value == 1)
+		pAd->WIFItestbed.bShortGI = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ForceShortGI_Proc::(ForceShortGI=%d)\n", pAd->WIFItestbed.bShortGI));
+
+	return TRUE;
+}
+
+
+
+INT	Set_ForceGF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->WIFItestbed.bGreenField = FALSE;
+	else if (Value == 1)
+		pAd->WIFItestbed.bGreenField = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ForceGF_Proc::(ForceGF=%d)\n", pAd->WIFItestbed.bGreenField));
+
+	return TRUE;
+}
+
+INT	Set_HtMimoPs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.bMIMOPSEnable = FALSE;
+	else if (Value == 1)
+		pAd->CommonCfg.bMIMOPSEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMimoPs_Proc::(HtMimoPs=%d)\n",pAd->CommonCfg.bMIMOPSEnable));
+
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef DOT11_N_SUPPORT
+INT	SetCommonHT(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	OID_SET_HT_PHYMODE		SetHT;
+
+	if (pAd->CommonCfg.PhyMode < PHY_11ABGN_MIXED)
+		return FALSE;
+
+	SetHT.PhyMode = pAd->CommonCfg.PhyMode;
+	SetHT.TransmitNo = ((UCHAR)pAd->Antenna.field.TxPath);
+	SetHT.HtMode = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.HTMODE;
+	SetHT.ExtOffset = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.EXTCHA;
+	SetHT.MCS = MCS_AUTO;
+	SetHT.BW = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.BW;
+	SetHT.STBC = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.STBC;
+	SetHT.SHORTGI = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.ShortGI;
+
+	RTMPSetHT(pAd, &SetHT);
+
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+
+INT	Set_FixedTxMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR	fix_tx_mode = FIXED_TXMODE_HT;
+
+	if (strcmp(arg, "OFDM") == 0 || strcmp(arg, "ofdm") == 0)
+	{
+		fix_tx_mode = FIXED_TXMODE_OFDM;
+	}
+	else if (strcmp(arg, "CCK") == 0 || strcmp(arg, "cck") == 0)
+	{
+        fix_tx_mode = FIXED_TXMODE_CCK;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode = fix_tx_mode;
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_FixedTxMode_Proc::(FixedTxMode=%d)\n", fix_tx_mode));
+
+	return TRUE;
+}
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+INT	Set_OpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+#ifdef RT2870
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+#endif // RT2870 //
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Can not switch operate mode on interface up !! \n"));
+		return FALSE;
+	}
+
+	if (Value == 0)
+		pAd->OpMode = OPMODE_STA;
+	else if (Value == 1)
+		pAd->OpMode = OPMODE_AP;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_OpMode_Proc::(OpMode=%s)\n", pAd->OpMode == 1 ? "AP Mode" : "STA Mode"));
+
+	return TRUE;
+}
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+
+/////////////////////////////////////////////////////////////////////////
+PCHAR   RTMPGetRalinkAuthModeStr(
+    IN  NDIS_802_11_AUTHENTICATION_MODE authMode)
+{
+	switch(authMode)
+	{
+		case Ndis802_11AuthModeOpen:
+			return "OPEN";
+		case Ndis802_11AuthModeWPAPSK:
+			return "WPAPSK";
+		case Ndis802_11AuthModeShared:
+			return "SHARED";
+		case Ndis802_11AuthModeWPA:
+			return "WPA";
+		case Ndis802_11AuthModeWPA2:
+			return "WPA2";
+		case Ndis802_11AuthModeWPA2PSK:
+			return "WPA2PSK";
+        case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+			return "WPAPSKWPA2PSK";
+        case Ndis802_11AuthModeWPA1WPA2:
+			return "WPA1WPA2";
+		case Ndis802_11AuthModeWPANone:
+			return "WPANONE";
+		default:
+			return "UNKNOW";
+	}
+}
+
+PCHAR   RTMPGetRalinkEncryModeStr(
+    IN  USHORT encryMode)
+{
+	switch(encryMode)
+	{
+		case Ndis802_11WEPDisabled:
+			return "NONE";
+		case Ndis802_11WEPEnabled:
+			return "WEP";
+		case Ndis802_11Encryption2Enabled:
+			return "TKIP";
+		case Ndis802_11Encryption3Enabled:
+			return "AES";
+        case Ndis802_11Encryption4Enabled:
+			return "TKIPAES";
+		default:
+			return "UNKNOW";
+	}
+}
+
+INT RTMPShowCfgValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pName,
+	IN	PUCHAR			pBuf)
+{
+	INT	Status = 0;
+
+	for (PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC = RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC++)
+	{
+		if (!strcmp(pName, PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name))
+		{
+			if(PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->show_proc(pAd, pBuf))
+				Status = -EINVAL;
+			break;  //Exit for loop.
+		}
+	}
+
+	if(PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name == NULL)
+	{
+		sprintf(pBuf, "\n");
+		for (PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC = RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC++)
+			sprintf(pBuf, "%s%s\n", pBuf, PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name);
+	}
+
+	return Status;
+}
+
+INT	Show_SSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%s", pAd->CommonCfg.Ssid);
+#endif // CONFIG_STA_SUPPORT //
+	return 0;
+}
+
+INT	Show_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.PhyMode)
+	{
+		case PHY_11BG_MIXED:
+			sprintf(pBuf, "\t11B/G");
+			break;
+		case PHY_11B:
+			sprintf(pBuf, "\t11B");
+			break;
+		case PHY_11A:
+			sprintf(pBuf, "\t11A");
+			break;
+		case PHY_11ABG_MIXED:
+			sprintf(pBuf, "\t11A/B/G");
+			break;
+		case PHY_11G:
+			sprintf(pBuf, "\t11G");
+			break;
+#ifdef DOT11_N_SUPPORT
+		case PHY_11ABGN_MIXED:
+			sprintf(pBuf, "\t11A/B/G/N");
+			break;
+		case PHY_11N_2_4G:
+			sprintf(pBuf, "\t11N only with 2.4G");
+			break;
+		case PHY_11GN_MIXED:
+			sprintf(pBuf, "\t11G/N");
+			break;
+		case PHY_11AN_MIXED:
+			sprintf(pBuf, "\t11A/N");
+			break;
+		case PHY_11BGN_MIXED:
+			sprintf(pBuf, "\t11B/G/N");
+			break;
+		case PHY_11AGN_MIXED:
+			sprintf(pBuf, "\t11A/G/N");
+			break;
+		case PHY_11N_5G:
+			sprintf(pBuf, "\t11N only with 5G");
+			break;
+#endif // DOT11_N_SUPPORT //
+		default:
+			sprintf(pBuf, "\tUnknow Value(%d)", pAd->CommonCfg.PhyMode);
+			break;
+	}
+	return 0;
+}
+
+
+INT	Show_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bEnableTxBurst ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.TxPreamble)
+	{
+		case Rt802_11PreambleShort:
+			sprintf(pBuf, "\tShort");
+			break;
+		case Rt802_11PreambleLong:
+			sprintf(pBuf, "\tLong");
+			break;
+		case Rt802_11PreambleAuto:
+			sprintf(pBuf, "\tAuto");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%lu)", pAd->CommonCfg.TxPreamble);
+			break;
+	}
+
+	return 0;
+}
+
+INT	Show_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%lu", pAd->CommonCfg.TxPowerPercentage);
+	return 0;
+}
+
+INT	Show_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.Channel);
+	return 0;
+}
+
+INT	Show_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.UseBGProtection)
+	{
+		case 1: //Always On
+			sprintf(pBuf, "\tON");
+			break;
+		case 2: //Always OFF
+			sprintf(pBuf, "\tOFF");
+			break;
+		case 0: //AUTO
+			sprintf(pBuf, "\tAuto");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%lu)", pAd->CommonCfg.UseBGProtection);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.RtsThreshold);
+	return 0;
+}
+
+INT	Show_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.FragmentThreshold);
+	return 0;
+}
+
+#ifdef DOT11_N_SUPPORT
+INT	Show_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	if (pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+	{
+		sprintf(pBuf, "\t40 MHz");
+	}
+	else
+	{
+        sprintf(pBuf, "\t20 MHz");
+	}
+	return 0;
+}
+
+INT	Show_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%u", pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+#endif // CONFIG_STA_SUPPORT //
+	return 0;
+}
+
+INT	Show_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.ShortGI)
+	{
+		case GI_400:
+			sprintf(pBuf, "\tGI_400");
+			break;
+		case GI_800:
+			sprintf(pBuf, "\tGI_800");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.ShortGI);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.HTMODE)
+	{
+		case HTMODE_GF:
+			sprintf(pBuf, "\tGF");
+			break;
+		case HTMODE_MM:
+			sprintf(pBuf, "\tMM");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.HTMODE);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.EXTCHA)
+	{
+		case EXTCHA_BELOW:
+			sprintf(pBuf, "\tBelow");
+			break;
+		case EXTCHA_ABOVE:
+			sprintf(pBuf, "\tAbove");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.EXTCHA);
+			break;
+	}
+	return 0;
+}
+
+
+INT	Show_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.BACapability.field.MpduDensity);
+	return 0;
+}
+
+INT	Show_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+	return 0;
+}
+
+INT	Show_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bRdg ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.BACapability.field.AmsduEnable ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.BACapability.field.AutoBA ? "TRUE":"FALSE");
+	return 0;
+}
+#endif // DOT11_N_SUPPORT //
+
+INT	Show_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.CountryRegion);
+	return 0;
+}
+
+INT	Show_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.CountryRegionForABand);
+	return 0;
+}
+
+INT	Show_CountryCode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.CountryCode);
+	return 0;
+}
+
+#ifdef AGGREGATION_SUPPORT
+INT	Show_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bAggregationCapable ? "TRUE":"FALSE");
+	return 0;
+}
+#endif // AGGREGATION_SUPPORT //
+
+#ifdef WMM_SUPPORT
+INT	Show_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%s", pAd->CommonCfg.bWmmCapable ? "TRUE":"FALSE");
+#endif // CONFIG_STA_SUPPORT //
+
+	return 0;
+}
+#endif // WMM_SUPPORT //
+
+INT	Show_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bIEEE80211H ? "TRUE":"FALSE");
+	return 0;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+INT	Show_NetworkType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->StaCfg.BssType)
+	{
+		case BSS_ADHOC:
+			sprintf(pBuf, "\tAdhoc");
+			break;
+		case BSS_INFRA:
+			sprintf(pBuf, "\tInfra");
+			break;
+		case BSS_ANY:
+			sprintf(pBuf, "\tAny");
+			break;
+		case BSS_MONITOR:
+			sprintf(pBuf, "\tMonitor");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%d)", pAd->StaCfg.BssType);
+			break;
+	}
+	return 0;
+}
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Show_AuthMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	NDIS_802_11_AUTHENTICATION_MODE	AuthMode = Ndis802_11AuthModeOpen;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		AuthMode = pAd->StaCfg.AuthMode;
+#endif // CONFIG_STA_SUPPORT //
+
+	if ((AuthMode >= Ndis802_11AuthModeOpen) &&
+		(AuthMode <= Ndis802_11AuthModeWPA1PSKWPA2PSK))
+		sprintf(pBuf, "\t%s", RTMPGetRalinkAuthModeStr(AuthMode));
+	else
+		sprintf(pBuf, "\tUnknow Value(%d)", AuthMode);
+
+	return 0;
+}
+
+INT	Show_EncrypType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	NDIS_802_11_WEP_STATUS	WepStatus = Ndis802_11WEPDisabled;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		WepStatus = pAd->StaCfg.WepStatus;
+#endif // CONFIG_STA_SUPPORT //
+
+	if ((WepStatus >= Ndis802_11WEPEnabled) &&
+		(WepStatus <= Ndis802_11Encryption4KeyAbsent))
+		sprintf(pBuf, "\t%s", RTMPGetRalinkEncryModeStr(WepStatus));
+	else
+		sprintf(pBuf, "\tUnknow Value(%d)", WepStatus);
+
+	return 0;
+}
+
+INT	Show_DefaultKeyID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	UCHAR DefaultKeyId = 0;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		DefaultKeyId = pAd->StaCfg.DefaultKeyId;
+#endif // CONFIG_STA_SUPPORT //
+
+	sprintf(pBuf, "\t%d", DefaultKeyId);
+
+	return 0;
+}
+
+INT	Show_WepKey_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  INT				KeyIdx,
+	OUT	PUCHAR			pBuf)
+{
+	UCHAR   Key[16] = {0}, KeyLength = 0;
+	INT		index = BSS0;
+
+	KeyLength = pAd->SharedKey[index][KeyIdx].KeyLen;
+	NdisMoveMemory(Key, pAd->SharedKey[index][KeyIdx].Key, KeyLength);
+
+	//check key string is ASCII or not
+    if (RTMPCheckStrPrintAble(Key, KeyLength))
+        sprintf(pBuf, "\t%s", Key);
+    else
+    {
+        int idx;
+        sprintf(pBuf, "\t");
+        for (idx = 0; idx < KeyLength; idx++)
+            sprintf(pBuf+strlen(pBuf), "%02X", Key[idx]);
+    }
+	return 0;
+}
+
+INT	Show_Key1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 0, pBuf);
+	return 0;
+}
+
+INT	Show_Key2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 1, pBuf);
+	return 0;
+}
+
+INT	Show_Key3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 2, pBuf);
+	return 0;
+}
+
+INT	Show_Key4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 3, pBuf);
+	return 0;
+}
+
+INT	Show_WPAPSK_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	INT 	idx;
+	UCHAR	PMK[32] = {0};
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisMoveMemory(PMK, pAd->StaCfg.PMK, 32);
+#endif // CONFIG_STA_SUPPORT //
+
+    sprintf(pBuf, "\tPMK = ");
+    for (idx = 0; idx < 32; idx++)
+        sprintf(pBuf+strlen(pBuf), "%02X", PMK[idx]);
+
+	return 0;
+}
+
diff --git a/drivers/staging/rt2870/common/cmm_sanity.c b/drivers/staging/rt2870/common/cmm_sanity.c
new file mode 100644
index 00000000000..1e24320177b
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_sanity.c
@@ -0,0 +1,1663 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sanity.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang  2004-09-01      add WMM support
+*/
+#include "../rt_config.h"
+
+
+extern UCHAR	CISCO_OUI[];
+
+extern UCHAR	WPA_OUI[];
+extern UCHAR	RSN_OUI[];
+extern UCHAR	WME_INFO_ELEM[];
+extern UCHAR	WME_PARM_ELEM[];
+extern UCHAR	Ccx2QosInfo[];
+extern UCHAR	RALINK_OUI[];
+extern UCHAR	BROADCOM_OUI[];
+extern UCHAR    WPS_OUI[];
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeAddBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2)
+{
+    PMLME_ADDBA_REQ_STRUCT   pInfo;
+
+    pInfo = (MLME_ADDBA_REQ_STRUCT *)Msg;
+
+    if ((MsgLen != sizeof(MLME_ADDBA_REQ_STRUCT)))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - message lenght not correct.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->Wcid >= MAX_LEN_OF_MAC_TABLE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - The peer Mac is not associated yet.\n"));
+        return FALSE;
+    }
+
+	/*
+    if ((pInfo->BaBufSize > MAX_RX_REORDERBUF) || (pInfo->BaBufSize < 2))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - Rx Reordering buffer too big or too small\n"));
+        return FALSE;
+    }
+	*/
+
+    if ((pInfo->pAddr[0]&0x01) == 0x01)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - broadcast address not support BA\n"));
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeDelBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen)
+{
+	MLME_DELBA_REQ_STRUCT *pInfo;
+	pInfo = (MLME_DELBA_REQ_STRUCT *)Msg;
+
+    if ((MsgLen != sizeof(MLME_DELBA_REQ_STRUCT)))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - message lenght not correct.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->Wcid >= MAX_LEN_OF_MAC_TABLE))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - The peer Mac is not associated yet.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->TID & 0xf0))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - The peer TID is incorrect.\n"));
+        return FALSE;
+    }
+
+	if (NdisEqualMemory(pAd->MacTab.Content[pInfo->Wcid].Addr, pInfo->Addr, MAC_ADDR_LEN) == 0)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - the peer addr dosen't exist.\n"));
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+BOOLEAN PeerAddBAReqActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr2)
+{
+	PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_ADDBA_REQ pAddFrame;
+	pAddFrame = (PFRAME_ADDBA_REQ)(pMsg);
+	if (MsgLen < (sizeof(FRAME_ADDBA_REQ)))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	// we support immediate BA.
+	*(USHORT *)(&pAddFrame->BaParm) = cpu2le16(*(USHORT *)(&pAddFrame->BaParm));
+	pAddFrame->TimeOutValue = cpu2le16(pAddFrame->TimeOutValue);
+	pAddFrame->BaStartSeq.word = cpu2le16(pAddFrame->BaStartSeq.word);
+
+	if (pAddFrame->BaParm.BAPolicy != IMMED_BA)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request Ba Policy[%d] not support\n", pAddFrame->BaParm.BAPolicy));
+		DBGPRINT(RT_DEBUG_ERROR,("ADDBA Request. tid=%x, Bufsize=%x, AMSDUSupported=%x \n", pAddFrame->BaParm.TID, pAddFrame->BaParm.BufSize, pAddFrame->BaParm.AMSDUSupported));
+		return FALSE;
+	}
+
+	// we support immediate BA.
+	if (pAddFrame->BaParm.TID &0xfff0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request incorrect TID = %d\n", pAddFrame->BaParm.TID));
+		return FALSE;
+	}
+	COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+	return TRUE;
+}
+
+BOOLEAN PeerAddBARspActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen)
+{
+	//PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_ADDBA_RSP pAddFrame;
+
+	pAddFrame = (PFRAME_ADDBA_RSP)(pMsg);
+	if (MsgLen < (sizeof(FRAME_ADDBA_RSP)))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBARspActionSanity: ADDBA Response frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	// we support immediate BA.
+	*(USHORT *)(&pAddFrame->BaParm) = cpu2le16(*(USHORT *)(&pAddFrame->BaParm));
+	pAddFrame->StatusCode = cpu2le16(pAddFrame->StatusCode);
+	pAddFrame->TimeOutValue = cpu2le16(pAddFrame->TimeOutValue);
+
+	if (pAddFrame->BaParm.BAPolicy != IMMED_BA)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Response Ba Policy[%d] not support\n", pAddFrame->BaParm.BAPolicy));
+		return FALSE;
+	}
+
+	// we support immediate BA.
+	if (pAddFrame->BaParm.TID &0xfff0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBARspActionSanity: ADDBA Response incorrect TID = %d\n", pAddFrame->BaParm.TID));
+		return FALSE;
+	}
+	return TRUE;
+
+}
+
+BOOLEAN PeerDelBAActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR Wcid,
+    IN VOID *pMsg,
+    IN ULONG MsgLen )
+{
+	//PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_DELBA_REQ  pDelFrame;
+	if (MsgLen != (sizeof(FRAME_DELBA_REQ)))
+		return FALSE;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	pDelFrame = (PFRAME_DELBA_REQ)(pMsg);
+
+	*(USHORT *)(&pDelFrame->DelbaParm) = cpu2le16(*(USHORT *)(&pDelFrame->DelbaParm));
+	pDelFrame->ReasonCode = cpu2le16(pDelFrame->ReasonCode);
+
+	if (pDelFrame->DelbaParm.TID &0xfff0)
+		return FALSE;
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerBeaconAndProbeRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    IN UCHAR  MsgChannel,
+    OUT PUCHAR pAddr2,
+    OUT PUCHAR pBssid,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen,
+    OUT UCHAR *pBssType,
+    OUT USHORT *pBeaconPeriod,
+    OUT UCHAR *pChannel,
+    OUT UCHAR *pNewChannel,
+    OUT LARGE_INTEGER *pTimestamp,
+    OUT CF_PARM *pCfParm,
+    OUT USHORT *pAtimWin,
+    OUT USHORT *pCapabilityInfo,
+    OUT UCHAR *pErp,
+    OUT UCHAR *pDtimCount,
+    OUT UCHAR *pDtimPeriod,
+    OUT UCHAR *pBcastFlag,
+    OUT UCHAR *pMessageToMe,
+    OUT UCHAR SupRate[],
+    OUT UCHAR *pSupRateLen,
+    OUT UCHAR ExtRate[],
+    OUT UCHAR *pExtRateLen,
+    OUT	UCHAR *pCkipFlag,
+    OUT	UCHAR *pAironetCellPowerLimit,
+    OUT PEDCA_PARM       pEdcaParm,
+    OUT PQBSS_LOAD_PARM  pQbssLoad,
+    OUT PQOS_CAPABILITY_PARM pQosCapability,
+    OUT ULONG *pRalinkIe,
+    OUT UCHAR		 *pHtCapabilityLen,
+#ifdef CONFIG_STA_SUPPORT
+    OUT UCHAR		 *pPreNHtCapabilityLen,
+#endif // CONFIG_STA_SUPPORT //
+    OUT HT_CAPABILITY_IE *pHtCapability,
+	OUT UCHAR		 *AddHtInfoLen,
+	OUT ADD_HT_INFO_IE *AddHtInfo,
+	OUT UCHAR *NewExtChannelOffset,		// Ht extension channel offset(above or below)
+    OUT USHORT *LengthVIE,
+    OUT	PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+    CHAR				*Ptr;
+#ifdef CONFIG_STA_SUPPORT
+	CHAR 				TimLen;
+#endif // CONFIG_STA_SUPPORT //
+    PFRAME_802_11		pFrame;
+    PEID_STRUCT         pEid;
+    UCHAR				SubType;
+    UCHAR				Sanity;
+    //UCHAR				ECWMin, ECWMax;
+    //MAC_CSR9_STRUC		Csr9;
+    ULONG				Length = 0;
+
+	// For some 11a AP which didn't have DS_IE, we use two conditions to decide the channel
+	//	1. If the AP is 11n enabled, then check the control channel.
+	//	2. If the AP didn't have any info about channel, use the channel we received this frame as the channel. (May inaccuracy!!)
+	UCHAR			CtrlChannel = 0;
+
+    // Add for 3 necessary EID field check
+    Sanity = 0;
+
+    *pAtimWin = 0;
+    *pErp = 0;
+    *pDtimCount = 0;
+    *pDtimPeriod = 0;
+    *pBcastFlag = 0;
+    *pMessageToMe = 0;
+    *pExtRateLen = 0;
+    *pCkipFlag = 0;			        // Default of CkipFlag is 0
+    *pAironetCellPowerLimit = 0xFF;  // Default of AironetCellPowerLimit is 0xFF
+    *LengthVIE = 0;					// Set the length of VIE to init value 0
+    *pHtCapabilityLen = 0;					// Set the length of VIE to init value 0
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+		*pPreNHtCapabilityLen = 0;					// Set the length of VIE to init value 0
+#endif // CONFIG_STA_SUPPORT //
+    *AddHtInfoLen = 0;					// Set the length of VIE to init value 0
+    *pRalinkIe = 0;
+    *pNewChannel = 0;
+    *NewExtChannelOffset = 0xff;	//Default 0xff means no such IE
+    pCfParm->bValid = FALSE;        // default: no IE_CF found
+    pQbssLoad->bValid = FALSE;      // default: no IE_QBSS_LOAD found
+    pEdcaParm->bValid = FALSE;      // default: no IE_EDCA_PARAMETER found
+    pQosCapability->bValid = FALSE; // default: no IE_QOS_CAPABILITY found
+
+    pFrame = (PFRAME_802_11)Msg;
+
+    // get subtype from header
+    SubType = (UCHAR)pFrame->Hdr.FC.SubType;
+
+    // get Addr2 and BSSID from header
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    COPY_MAC_ADDR(pBssid, pFrame->Hdr.Addr3);
+
+//	hex_dump("Beacon", Msg, MsgLen);
+
+    Ptr = pFrame->Octet;
+    Length += LENGTH_802_11;
+
+    // get timestamp from payload and advance the pointer
+    NdisMoveMemory(pTimestamp, Ptr, TIMESTAMP_LEN);
+
+	pTimestamp->u.LowPart = cpu2le32(pTimestamp->u.LowPart);
+	pTimestamp->u.HighPart = cpu2le32(pTimestamp->u.HighPart);
+
+    Ptr += TIMESTAMP_LEN;
+    Length += TIMESTAMP_LEN;
+
+    // get beacon interval from payload and advance the pointer
+    NdisMoveMemory(pBeaconPeriod, Ptr, 2);
+    Ptr += 2;
+    Length += 2;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+    Ptr += 2;
+    Length += 2;
+
+    if (CAP_IS_ESS_ON(*pCapabilityInfo))
+        *pBssType = BSS_INFRA;
+    else
+        *pBssType = BSS_ADHOC;
+
+    pEid = (PEID_STRUCT) Ptr;
+
+    // get variable fields from payload and advance the pointer
+    while ((Length + 2 + pEid->Len) <= MsgLen)
+    {
+        //
+        // Secure copy VIE to VarIE[MAX_VIE_LEN] didn't overflow.
+        //
+        if ((*LengthVIE + pEid->Len + 2) >= MAX_VIE_LEN)
+        {
+            DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - Variable IEs out of resource [len(=%d) > MAX_VIE_LEN(=%d)]\n",
+                    (*LengthVIE + pEid->Len + 2), MAX_VIE_LEN));
+            break;
+        }
+
+        switch(pEid->Eid)
+        {
+            case IE_SSID:
+                // Already has one SSID EID in this beacon, ignore the second one
+                if (Sanity & 0x1)
+                    break;
+                if(pEid->Len <= MAX_LEN_OF_SSID)
+                {
+                    NdisMoveMemory(Ssid, pEid->Octet, pEid->Len);
+                    *pSsidLen = pEid->Len;
+                    Sanity |= 0x1;
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SSID (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_SUPP_RATES:
+                if(pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    Sanity |= 0x2;
+                    NdisMoveMemory(SupRate, pEid->Octet, pEid->Len);
+                    *pSupRateLen = pEid->Len;
+
+                    // TODO: 2004-09-14 not a good design here, cause it exclude extra rates
+                    // from ScanTab. We should report as is. And filter out unsupported
+                    // rates in MlmeAux.
+                    // Check against the supported rates
+                    // RTMPCheckRates(pAd, SupRate, pSupRateLen);
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SUPP_RATES (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_HT_CAP:
+			if (pEid->Len >= SIZE_HT_CAP_IE)  //Note: allow extension.!!
+			{
+				NdisMoveMemory(pHtCapability, pEid->Octet, sizeof(HT_CAPABILITY_IE));
+				*pHtCapabilityLen = SIZE_HT_CAP_IE;	// Nnow we only support 26 bytes.
+
+				*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+				*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					*pPreNHtCapabilityLen = 0;	// Nnow we only support 26 bytes.
+
+					Ptr = (PUCHAR) pVIE;
+					NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+					*LengthVIE += (pEid->Len + 2);
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_HT_CAP. pEid->Len = %d\n", pEid->Len));
+			}
+
+		break;
+            case IE_ADD_HT:
+			if (pEid->Len >= sizeof(ADD_HT_INFO_IE))
+			{
+				// This IE allows extension, but we can ignore extra bytes beyond our knowledge , so only
+				// copy first sizeof(ADD_HT_INFO_IE)
+				NdisMoveMemory(AddHtInfo, pEid->Octet, sizeof(ADD_HT_INFO_IE));
+				*AddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+
+				CtrlChannel = AddHtInfo->ControlChan;
+
+				*(USHORT *)(&AddHtInfo->AddHtInfo2) = cpu2le16(*(USHORT *)(&AddHtInfo->AddHtInfo2));
+				*(USHORT *)(&AddHtInfo->AddHtInfo3) = cpu2le16(*(USHORT *)(&AddHtInfo->AddHtInfo3));
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+			                Ptr = (PUCHAR) pVIE;
+			                NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+			                *LengthVIE += (pEid->Len + 2);
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_ADD_HT. \n"));
+			}
+
+		break;
+            case IE_SECONDARY_CH_OFFSET:
+			if (pEid->Len == 1)
+			{
+				*NewExtChannelOffset = pEid->Octet[0];
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_SECONDARY_CH_OFFSET. \n"));
+			}
+
+		break;
+            case IE_FH_PARM:
+                DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity(IE_FH_PARM) \n"));
+                break;
+
+            case IE_DS_PARM:
+                if(pEid->Len == 1)
+                {
+                    *pChannel = *pEid->Octet;
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						if (ChannelSanity(pAd, *pChannel) == 0)
+						{
+
+							return FALSE;
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+                    Sanity |= 0x4;
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_DS_PARM (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_CF_PARM:
+                if(pEid->Len == 6)
+                {
+                    pCfParm->bValid = TRUE;
+                    pCfParm->CfpCount = pEid->Octet[0];
+                    pCfParm->CfpPeriod = pEid->Octet[1];
+                    pCfParm->CfpMaxDuration = pEid->Octet[2] + 256 * pEid->Octet[3];
+                    pCfParm->CfpDurRemaining = pEid->Octet[4] + 256 * pEid->Octet[5];
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_CF_PARM\n"));
+                    return FALSE;
+                }
+                break;
+
+            case IE_IBSS_PARM:
+                if(pEid->Len == 2)
+                {
+                    NdisMoveMemory(pAtimWin, pEid->Octet, pEid->Len);
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_IBSS_PARM\n"));
+                    return FALSE;
+                }
+                break;
+
+#ifdef CONFIG_STA_SUPPORT
+            case IE_TIM:
+                if(INFRA_ON(pAd) && SubType == SUBTYPE_BEACON)
+                {
+                    GetTimBit((PUCHAR)pEid, pAd->StaActive.Aid, &TimLen, pBcastFlag, pDtimCount, pDtimPeriod, pMessageToMe);
+                }
+                break;
+#endif // CONFIG_STA_SUPPORT //
+            case IE_CHANNEL_SWITCH_ANNOUNCEMENT:
+                if(pEid->Len == 3)
+                {
+                	*pNewChannel = pEid->Octet[1];	//extract new channel number
+                }
+                break;
+
+            // New for WPA
+            // CCX v2 has the same IE, we need to parse that too
+            // Wifi WMM use the same IE vale, need to parse that too
+            // case IE_WPA:
+            case IE_VENDOR_SPECIFIC:
+                // Check Broadcom/Atheros 802.11n OUI version, for HT Capability IE.
+                // This HT IE is before IEEE draft set HT IE value.2006-09-28 by Jan.
+                /*if (NdisEqualMemory(pEid->Octet, BROADCOM_OUI, 3) && (pEid->Len >= 4))
+                {
+                	if ((pEid->Octet[3] == OUI_BROADCOM_HT) && (pEid->Len >= 30))
+            		{
+				{
+					NdisMoveMemory(pHtCapability, &pEid->Octet[4], sizeof(HT_CAPABILITY_IE));
+					*pHtCapabilityLen = SIZE_HT_CAP_IE;	// Nnow we only support 26 bytes.
+				}
+         		}
+                	if ((pEid->Octet[3] == OUI_BROADCOM_HT) && (pEid->Len >= 26))
+            		{
+				{
+					NdisMoveMemory(AddHtInfo, &pEid->Octet[4], sizeof(ADD_HT_INFO_IE));
+					*AddHtInfoLen = SIZE_ADD_HT_INFO_IE;	// Nnow we only support 26 bytes.
+				}
+         		}
+                }
+				*/
+                // Check the OUI version, filter out non-standard usage
+                if (NdisEqualMemory(pEid->Octet, RALINK_OUI, 3) && (pEid->Len == 7))
+                {
+                    //*pRalinkIe = pEid->Octet[3];
+                    if (pEid->Octet[3] != 0)
+        				*pRalinkIe = pEid->Octet[3];
+        			else
+        				*pRalinkIe = 0xf0000000; // Set to non-zero value (can't set bit0-2) to represent this is Ralink Chip. So at linkup, we will set ralinkchip flag.
+                }
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+		// This HT IE is before IEEE draft set HT IE value.2006-09-28 by Jan.
+
+                // Other vendors had production before IE_HT_CAP value is assigned. To backward support those old-firmware AP,
+                // Check broadcom-defiend pre-802.11nD1.0 OUI for HT related IE, including HT Capatilities IE and HT Information IE
+                else if ((*pHtCapabilityLen == 0) && NdisEqualMemory(pEid->Octet, PRE_N_HT_OUI, 3) && (pEid->Len >= 4) && (pAd->OpMode == OPMODE_STA))
+                {
+                    if ((pEid->Octet[3] == OUI_PREN_HT_CAP) && (pEid->Len >= 30) && (*pHtCapabilityLen == 0))
+                    {
+                        NdisMoveMemory(pHtCapability, &pEid->Octet[4], sizeof(HT_CAPABILITY_IE));
+                        *pPreNHtCapabilityLen = SIZE_HT_CAP_IE;
+                    }
+
+                    if ((pEid->Octet[3] == OUI_PREN_ADD_HT) && (pEid->Len >= 26))
+                    {
+                        NdisMoveMemory(AddHtInfo, &pEid->Octet[4], sizeof(ADD_HT_INFO_IE));
+                        *AddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+                    }
+                }
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+                else if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+                {
+                    // Copy to pVIE which will report to microsoft bssid list.
+                    Ptr = (PUCHAR) pVIE;
+                    NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                    *LengthVIE += (pEid->Len + 2);
+                }
+                else if (NdisEqualMemory(pEid->Octet, WME_PARM_ELEM, 6) && (pEid->Len == 24))
+                {
+                    PUCHAR ptr;
+                    int i;
+
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+                    ptr = &pEid->Octet[8];
+                    for (i=0; i<4; i++)
+                    {
+                        UCHAR aci = (*ptr & 0x60) >> 5; // b5~6 is AC INDEX
+                        pEdcaParm->bACM[aci]  = (((*ptr) & 0x10) == 0x10);   // b5 is ACM
+                        pEdcaParm->Aifsn[aci] = (*ptr) & 0x0f;               // b0~3 is AIFSN
+                        pEdcaParm->Cwmin[aci] = *(ptr+1) & 0x0f;             // b0~4 is Cwmin
+                        pEdcaParm->Cwmax[aci] = *(ptr+1) >> 4;               // b5~8 is Cwmax
+                        pEdcaParm->Txop[aci]  = *(ptr+2) + 256 * (*(ptr+3)); // in unit of 32-us
+                        ptr += 4; // point to next AC
+                    }
+                }
+                else if (NdisEqualMemory(pEid->Octet, WME_INFO_ELEM, 6) && (pEid->Len == 7))
+                {
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+
+                    // use default EDCA parameter
+                    pEdcaParm->bACM[QID_AC_BE]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_BE] = 3;
+                    pEdcaParm->Cwmin[QID_AC_BE] = CW_MIN_IN_BITS;
+                    pEdcaParm->Cwmax[QID_AC_BE] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_BE]  = 0;
+
+                    pEdcaParm->bACM[QID_AC_BK]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_BK] = 7;
+                    pEdcaParm->Cwmin[QID_AC_BK] = CW_MIN_IN_BITS;
+                    pEdcaParm->Cwmax[QID_AC_BK] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_BK]  = 0;
+
+                    pEdcaParm->bACM[QID_AC_VI]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_VI] = 2;
+                    pEdcaParm->Cwmin[QID_AC_VI] = CW_MIN_IN_BITS-1;
+                    pEdcaParm->Cwmax[QID_AC_VI] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_VI]  = 96;   // AC_VI: 96*32us ~= 3ms
+
+                    pEdcaParm->bACM[QID_AC_VO]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_VO] = 2;
+                    pEdcaParm->Cwmin[QID_AC_VO] = CW_MIN_IN_BITS-2;
+                    pEdcaParm->Cwmax[QID_AC_VO] = CW_MAX_IN_BITS-1;
+                    pEdcaParm->Txop[QID_AC_VO]  = 48;   // AC_VO: 48*32us ~= 1.5ms
+                }
+                break;
+
+            case IE_EXT_SUPP_RATES:
+                if (pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(ExtRate, pEid->Octet, pEid->Len);
+                    *pExtRateLen = pEid->Len;
+
+                    // TODO: 2004-09-14 not a good design here, cause it exclude extra rates
+                    // from ScanTab. We should report as is. And filter out unsupported
+                    // rates in MlmeAux.
+                    // Check against the supported rates
+                    // RTMPCheckRates(pAd, ExtRate, pExtRateLen);
+                }
+                break;
+
+            case IE_ERP:
+                if (pEid->Len == 1)
+                {
+                    *pErp = (UCHAR)pEid->Octet[0];
+                }
+                break;
+
+            case IE_AIRONET_CKIP:
+                // 0. Check Aironet IE length, it must be larger or equal to 28
+                // Cisco AP350 used length as 28
+                // Cisco AP12XX used length as 30
+                if (pEid->Len < (CKIP_NEGOTIATION_LENGTH - 2))
+                    break;
+
+                // 1. Copy CKIP flag byte to buffer for process
+                *pCkipFlag = *(pEid->Octet + 8);
+                break;
+
+            case IE_AP_TX_POWER:
+                // AP Control of Client Transmit Power
+                //0. Check Aironet IE length, it must be 6
+                if (pEid->Len != 0x06)
+                    break;
+
+                // Get cell power limit in dBm
+                if (NdisEqualMemory(pEid->Octet, CISCO_OUI, 3) == 1)
+                    *pAironetCellPowerLimit = *(pEid->Octet + 4);
+                break;
+
+            // WPA2 & 802.11i RSN
+            case IE_RSN:
+                // There is no OUI for version anymore, check the group cipher OUI before copying
+                if (RTMPEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+                {
+                    // Copy to pVIE which will report to microsoft bssid list.
+                    Ptr = (PUCHAR) pVIE;
+                    NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                    *LengthVIE += (pEid->Len + 2);
+                }
+                break;
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+			case IE_COUNTRY:
+				Ptr = (PUCHAR) pVIE;
+                NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                *LengthVIE += (pEid->Len + 2);
+				break;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+            default:
+                break;
+        }
+
+        Length = Length + 2 + pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+        pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+    }
+
+    // For some 11a AP. it did not have the channel EID, patch here
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		UCHAR LatchRfChannel = MsgChannel;
+		if ((pAd->LatchRfRegs.Channel > 14) && ((Sanity & 0x4) == 0))
+		{
+			if (CtrlChannel != 0)
+				*pChannel = CtrlChannel;
+			else
+				*pChannel = LatchRfChannel;
+			Sanity |= 0x4;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (Sanity != 0x7)
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - missing field, Sanity=0x%02x\n", Sanity));
+		return FALSE;
+	}
+	else
+	{
+		return TRUE;
+	}
+
+}
+
+#ifdef DOT11N_DRAFT3
+/*
+	==========================================================================
+	Description:
+		MLME message sanity check for some IE addressed  in 802.11n d3.03.
+	Return:
+		TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+BOOLEAN PeerBeaconAndProbeRspSanity2(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *Msg,
+	IN ULONG MsgLen,
+	OUT UCHAR 	*RegClass)
+{
+	CHAR				*Ptr;
+	PFRAME_802_11		pFrame;
+	PEID_STRUCT			pEid;
+	ULONG				Length = 0;
+
+	pFrame = (PFRAME_802_11)Msg;
+
+	*RegClass = 0;
+	Ptr = pFrame->Octet;
+	Length += LENGTH_802_11;
+
+	// get timestamp from payload and advance the pointer
+	Ptr += TIMESTAMP_LEN;
+	Length += TIMESTAMP_LEN;
+
+	// get beacon interval from payload and advance the pointer
+	Ptr += 2;
+	Length += 2;
+
+	// get capability info from payload and advance the pointer
+	Ptr += 2;
+	Length += 2;
+
+	pEid = (PEID_STRUCT) Ptr;
+
+	// get variable fields from payload and advance the pointer
+	while ((Length + 2 + pEid->Len) <= MsgLen)
+	{
+		switch(pEid->Eid)
+		{
+			case IE_SUPP_REG_CLASS:
+				if(pEid->Len > 0)
+				{
+					*RegClass = *pEid->Octet;
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SSID (len=%d)\n",pEid->Len));
+					return FALSE;
+				}
+				break;
+		}
+
+		Length = Length + 2 + pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+		pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+	}
+
+	return TRUE;
+
+}
+#endif // DOT11N_DRAFT3 //
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeScanReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *Msg,
+	IN ULONG MsgLen,
+	OUT UCHAR *pBssType,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen,
+	OUT UCHAR *pScanType)
+{
+	MLME_SCAN_REQ_STRUCT *Info;
+
+	Info = (MLME_SCAN_REQ_STRUCT *)(Msg);
+	*pBssType = Info->BssType;
+	*pSsidLen = Info->SsidLen;
+	NdisMoveMemory(Ssid, Info->Ssid, *pSsidLen);
+	*pScanType = Info->ScanType;
+
+	if ((*pBssType == BSS_INFRA || *pBssType == BSS_ADHOC || *pBssType == BSS_ANY)
+		&& (*pScanType == SCAN_ACTIVE || *pScanType == SCAN_PASSIVE
+#ifdef CONFIG_STA_SUPPORT
+		|| *pScanType == SCAN_CISCO_PASSIVE || *pScanType == SCAN_CISCO_ACTIVE
+		|| *pScanType == SCAN_CISCO_CHANNEL_LOAD || *pScanType == SCAN_CISCO_NOISE
+#endif // CONFIG_STA_SUPPORT //
+		))
+	{
+		return TRUE;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqSanity fail - wrong BssType or ScanType\n"));
+		return FALSE;
+	}
+}
+
+// IRQL = DISPATCH_LEVEL
+UCHAR ChannelSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR channel)
+{
+    int i;
+
+    for (i = 0; i < pAd->ChannelListNum; i ++)
+    {
+        if (channel == pAd->ChannelList[i].Channel)
+            return 1;
+    }
+    return 0;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerDeauthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pReason)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    NdisMoveMemory(pReason, &pFrame->Octet[0], 2);
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerAuthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr,
+    OUT USHORT *pAlg,
+    OUT USHORT *pSeq,
+    OUT USHORT *pStatus,
+    CHAR *pChlgText)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr,   pFrame->Hdr.Addr2);
+    NdisMoveMemory(pAlg,    &pFrame->Octet[0], 2);
+    NdisMoveMemory(pSeq,    &pFrame->Octet[2], 2);
+    NdisMoveMemory(pStatus, &pFrame->Octet[4], 2);
+
+    if ((*pAlg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+      || (*pAlg == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+      )
+    {
+        if (*pSeq == 1 || *pSeq == 2)
+        {
+            return TRUE;
+        }
+        else
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong Seg#\n"));
+            return FALSE;
+        }
+    }
+    else if (*pAlg == Ndis802_11AuthModeShared)
+    {
+        if (*pSeq == 1 || *pSeq == 4)
+        {
+            return TRUE;
+        }
+        else if (*pSeq == 2 || *pSeq == 3)
+        {
+            NdisMoveMemory(pChlgText, &pFrame->Octet[8], CIPHER_TEXT_LEN);
+            return TRUE;
+        }
+        else
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong Seg#\n"));
+            return FALSE;
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong algorithm\n"));
+        return FALSE;
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeAuthReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr,
+    OUT ULONG *pTimeout,
+    OUT USHORT *pAlg)
+{
+    MLME_AUTH_REQ_STRUCT *pInfo;
+
+    pInfo  = (MLME_AUTH_REQ_STRUCT *)Msg;
+    COPY_MAC_ADDR(pAddr, pInfo->Addr);
+    *pTimeout = pInfo->Timeout;
+    *pAlg = pInfo->Alg;
+
+    if (((*pAlg == Ndis802_11AuthModeShared) ||(*pAlg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+     || (*pAlg == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+     	) &&
+        ((*pAddr & 0x01) == 0))
+    {
+        return TRUE;
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAuthReqSanity fail - wrong algorithm\n"));
+        return FALSE;
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeAssocReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pApAddr,
+    OUT USHORT *pCapabilityInfo,
+    OUT ULONG *pTimeout,
+    OUT USHORT *pListenIntv)
+{
+    MLME_ASSOC_REQ_STRUCT *pInfo;
+
+    pInfo = (MLME_ASSOC_REQ_STRUCT *)Msg;
+    *pTimeout = pInfo->Timeout;                             // timeout
+    COPY_MAC_ADDR(pApAddr, pInfo->Addr);                   // AP address
+    *pCapabilityInfo = pInfo->CapabilityInfo;               // capability info
+    *pListenIntv = pInfo->ListenIntv;
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerDisassocSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pReason)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    NdisMoveMemory(pReason, &pFrame->Octet[0], 2);
+
+    return TRUE;
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Sanity check NetworkType (11b, 11g or 11a)
+
+	Arguments:
+		pBss - Pointer to BSS table.
+
+	Return Value:
+        Ndis802_11DS .......(11b)
+        Ndis802_11OFDM24....(11g)
+        Ndis802_11OFDM5.....(11a)
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+NDIS_802_11_NETWORK_TYPE NetworkTypeInUseSanity(
+    IN PBSS_ENTRY pBss)
+{
+	NDIS_802_11_NETWORK_TYPE	NetWorkType;
+	UCHAR						rate, i;
+
+	NetWorkType = Ndis802_11DS;
+
+	if (pBss->Channel <= 14)
+	{
+		//
+		// First check support Rate.
+		//
+		for (i = 0; i < pBss->SupRateLen; i++)
+		{
+			rate = pBss->SupRate[i] & 0x7f; // Mask out basic rate set bit
+			if ((rate == 2) || (rate == 4) || (rate == 11) || (rate == 22))
+			{
+				continue;
+			}
+			else
+			{
+				//
+				// Otherwise (even rate > 108) means Ndis802_11OFDM24
+				//
+				NetWorkType = Ndis802_11OFDM24;
+				break;
+			}
+		}
+
+		//
+		// Second check Extend Rate.
+		//
+		if (NetWorkType != Ndis802_11OFDM24)
+		{
+			for (i = 0; i < pBss->ExtRateLen; i++)
+			{
+				rate = pBss->SupRate[i] & 0x7f; // Mask out basic rate set bit
+				if ((rate == 2) || (rate == 4) || (rate == 11) || (rate == 22))
+				{
+					continue;
+				}
+				else
+				{
+					//
+					// Otherwise (even rate > 108) means Ndis802_11OFDM24
+					//
+					NetWorkType = Ndis802_11OFDM24;
+					break;
+				}
+			}
+		}
+	}
+	else
+	{
+		NetWorkType = Ndis802_11OFDM5;
+	}
+
+    if (pBss->HtCapabilityLen != 0)
+    {
+        if (NetWorkType == Ndis802_11OFDM5)
+            NetWorkType = Ndis802_11OFDM5_N;
+        else
+            NetWorkType = Ndis802_11OFDM24_N;
+    }
+
+	return NetWorkType;
+}
+
+/*
+    ==========================================================================
+    Description:
+        WPA message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN PeerWpaMessageSanity(
+    IN 	PRTMP_ADAPTER 		pAd,
+    IN 	PEAPOL_PACKET 		pMsg,
+    IN 	ULONG 				MsgLen,
+    IN 	UCHAR				MsgType,
+    IN 	MAC_TABLE_ENTRY  	*pEntry)
+{
+	UCHAR			mic[LEN_KEY_DESC_MIC], digest[80], KEYDATA[MAX_LEN_OF_RSNIE];
+	BOOLEAN			bReplayDiff = FALSE;
+	BOOLEAN			bWPA2 = FALSE;
+	KEY_INFO		EapolKeyInfo;
+	UCHAR			GroupKeyIndex = 0;
+
+
+	NdisZeroMemory(mic, sizeof(mic));
+	NdisZeroMemory(digest, sizeof(digest));
+	NdisZeroMemory(KEYDATA, sizeof(KEYDATA));
+	NdisZeroMemory((PUCHAR)&EapolKeyInfo, sizeof(EapolKeyInfo));
+
+	NdisMoveMemory((PUCHAR)&EapolKeyInfo, (PUCHAR)&pMsg->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT *)&EapolKeyInfo) = cpu2le16(*((USHORT *)&EapolKeyInfo));
+
+	// Choose WPA2 or not
+	if ((pEntry->AuthMode == Ndis802_11AuthModeWPA2) || (pEntry->AuthMode == Ndis802_11AuthModeWPA2PSK))
+		bWPA2 = TRUE;
+
+	// 0. Check MsgType
+	if ((MsgType > EAPOL_GROUP_MSG_2) || (MsgType < EAPOL_PAIR_MSG_1))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("The message type is invalid(%d)! \n", MsgType));
+		return FALSE;
+	}
+
+	// 1. Replay counter check
+ 	if (MsgType == EAPOL_PAIR_MSG_1 || MsgType == EAPOL_PAIR_MSG_3 || MsgType == EAPOL_GROUP_MSG_1)	// For supplicant
+    {
+    	// First validate replay counter, only accept message with larger replay counter.
+		// Let equal pass, some AP start with all zero replay counter
+		UCHAR	ZeroReplay[LEN_KEY_DESC_REPLAY];
+
+        NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+		if ((RTMPCompareMemory(pMsg->KeyDesc.ReplayCounter, pEntry->R_Counter, LEN_KEY_DESC_REPLAY) != 1) &&
+			(RTMPCompareMemory(pMsg->KeyDesc.ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+    	{
+			bReplayDiff = TRUE;
+    	}
+ 	}
+	else if (MsgType == EAPOL_PAIR_MSG_2 || MsgType == EAPOL_PAIR_MSG_4 || MsgType == EAPOL_GROUP_MSG_2)	// For authenticator
+	{
+		// check Replay Counter coresponds to MSG from authenticator, otherwise discard
+    	if (!NdisEqualMemory(pMsg->KeyDesc.ReplayCounter, pEntry->R_Counter, LEN_KEY_DESC_REPLAY))
+    	{
+			bReplayDiff = TRUE;
+    	}
+	}
+
+	// Replay Counter different condition
+	if (bReplayDiff)
+	{
+		// send wireless event - for replay counter different
+		if (pAd->CommonCfg.bWirelessEvent)
+			RTMPSendWirelessEvent(pAd, IW_REPLAY_COUNTER_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+		if (MsgType < EAPOL_GROUP_MSG_1)
+		{
+           	DBGPRINT(RT_DEBUG_ERROR, ("Replay Counter Different in pairwise msg %d of 4-way handshake!\n", MsgType));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Replay Counter Different in group msg %d of 2-way handshake!\n", (MsgType - EAPOL_PAIR_MSG_4)));
+		}
+
+		hex_dump("Receive replay counter ", pMsg->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+		hex_dump("Current replay counter ", pEntry->R_Counter, LEN_KEY_DESC_REPLAY);
+        return FALSE;
+	}
+
+	// 2. Verify MIC except Pairwise Msg1
+	if (MsgType != EAPOL_PAIR_MSG_1)
+	{
+		UCHAR			rcvd_mic[LEN_KEY_DESC_MIC];
+
+		// Record the received MIC for check later
+		NdisMoveMemory(rcvd_mic, pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+		NdisZeroMemory(pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+        if (pEntry->WepStatus == Ndis802_11Encryption2Enabled)	// TKIP
+        {
+            hmac_md5(pEntry->PTK, LEN_EAP_MICK, (PUCHAR)pMsg, MsgLen, mic);
+        }
+        else if (pEntry->WepStatus == Ndis802_11Encryption3Enabled)	// AES
+        {
+            HMAC_SHA1((PUCHAR)pMsg, MsgLen, pEntry->PTK, LEN_EAP_MICK, digest);
+            NdisMoveMemory(mic, digest, LEN_KEY_DESC_MIC);
+        }
+
+        if (!NdisEqualMemory(rcvd_mic, mic, LEN_KEY_DESC_MIC))
+        {
+			// send wireless event - for MIC different
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_MIC_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+			if (MsgType < EAPOL_GROUP_MSG_1)
+			{
+            	DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in pairwise msg %d of 4-way handshake!\n", MsgType));
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in group msg %d of 2-way handshake!\n", (MsgType - EAPOL_PAIR_MSG_4)));
+			}
+
+			hex_dump("Received MIC", rcvd_mic, LEN_KEY_DESC_MIC);
+			hex_dump("Desired  MIC", mic, LEN_KEY_DESC_MIC);
+
+			return FALSE;
+        }
+	}
+
+	// Extract the context of the Key Data field if it exist
+	// The field in pairwise_msg_2_WPA1(WPA2) & pairwise_msg_3_WPA1 is un-encrypted.
+	// The field in group_msg_1_WPA1(WPA2) & pairwise_msg_3_WPA2 is encrypted.
+	if (pMsg->KeyDesc.KeyDataLen[1] > 0)
+	{
+		// Decrypt this field
+		if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2) || (MsgType == EAPOL_GROUP_MSG_1))
+		{
+			if(pEntry->WepStatus == Ndis802_11Encryption3Enabled)
+			{
+				// AES
+				AES_GTK_KEY_UNWRAP(&pEntry->PTK[16], KEYDATA, pMsg->KeyDesc.KeyDataLen[1],pMsg->KeyDesc.KeyData);
+			}
+			else
+			{
+				INT 	i;
+				UCHAR   Key[32];
+				// Decrypt TKIP GTK
+				// Construct 32 bytes RC4 Key
+				NdisMoveMemory(Key, pMsg->KeyDesc.KeyIv, 16);
+				NdisMoveMemory(&Key[16], &pEntry->PTK[16], 16);
+				ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+				//discard first 256 bytes
+				for(i = 0; i < 256; i++)
+					ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+				// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+				ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pMsg->KeyDesc.KeyData, pMsg->KeyDesc.KeyDataLen[1]);
+			}
+
+			if (!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))
+				GroupKeyIndex = EapolKeyInfo.KeyIndex;
+
+		}
+		else if ((MsgType == EAPOL_PAIR_MSG_2) || (MsgType == EAPOL_PAIR_MSG_3 && !bWPA2))
+		{
+			NdisMoveMemory(KEYDATA, pMsg->KeyDesc.KeyData, pMsg->KeyDesc.KeyDataLen[1]);
+		}
+		else
+		{
+
+			return TRUE;
+		}
+
+		// Parse Key Data field to
+		// 1. verify RSN IE for pairwise_msg_2_WPA1(WPA2) ,pairwise_msg_3_WPA1(WPA2)
+		// 2. verify KDE format for pairwise_msg_3_WPA2, group_msg_1_WPA2
+		// 3. update shared key for pairwise_msg_3_WPA2, group_msg_1_WPA1(WPA2)
+		if (!RTMPParseEapolKeyData(pAd, KEYDATA, pMsg->KeyDesc.KeyDataLen[1], GroupKeyIndex, MsgType, bWPA2, pEntry))
+		{
+			return FALSE;
+		}
+	}
+
+	return TRUE;
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN MlmeDlsReqSanity(
+	IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PRT_802_11_DLS *pDLS,
+    OUT PUSHORT pReason)
+{
+	MLME_DLS_REQ_STRUCT *pInfo;
+
+    pInfo = (MLME_DLS_REQ_STRUCT *)Msg;
+
+	*pDLS = pInfo->pDLS;
+	*pReason = pInfo->Reason;
+
+	return TRUE;
+}
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN PeerDlsReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pDlsTimeout,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+	OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability)
+{
+	CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+	PEID_STRUCT  eid_ptr;
+
+    // to prevent caller from using garbage output value
+    *pCapabilityInfo	= 0;
+    *pDlsTimeout	= 0;
+	*pHtCapabilityLen = 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+    Ptr += 2;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pDlsTimeout, Ptr, 2);
+    Ptr += 2;
+
+	// Category and Action field + DA + SA + capability + Timeout
+	eid_ptr = (PEID_STRUCT) &Fr->Octet[18];
+
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((UCHAR*)Fr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_SUPP_RATES:
+                if ((eid_ptr->Len <= MAX_LEN_OF_SUPPORTED_RATES) && (eid_ptr->Len > 0))
+                {
+                    NdisMoveMemory(Rates, eid_ptr->Octet, eid_ptr->Len);
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - IE_SUPP_RATES., Len=%d. Rates[0]=%x\n",eid_ptr->Len, Rates[0]));
+                    DBGPRINT(RT_DEBUG_TRACE, ("Rates[1]=%x %x %x %x %x %x %x\n", Rates[1], Rates[2], Rates[3], Rates[4], Rates[5], Rates[6], Rates[7]));
+                    *pRatesLen = eid_ptr->Len;
+                }
+                else
+                {
+                    *pRatesLen = 8;
+					Rates[0] = 0x82;
+					Rates[1] = 0x84;
+					Rates[2] = 0x8b;
+					Rates[3] = 0x96;
+					Rates[4] = 0x12;
+					Rates[5] = 0x24;
+					Rates[6] = 0x48;
+					Rates[7] = 0x6c;
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - wrong IE_SUPP_RATES., Len=%d\n",eid_ptr->Len));
+                }
+				break;
+
+			case IE_EXT_SUPP_RATES:
+                if (eid_ptr->Len + *pRatesLen <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, eid_ptr->Len);
+                    *pRatesLen = (*pRatesLen) + eid_ptr->Len;
+                }
+                else
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, MAX_LEN_OF_SUPPORTED_RATES - (*pRatesLen));
+                    *pRatesLen = MAX_LEN_OF_SUPPORTED_RATES;
+                }
+				break;
+
+			case IE_HT_CAP:
+				if (eid_ptr->Len >= sizeof(HT_CAPABILITY_IE))
+				{
+					NdisMoveMemory(pHtCapability, eid_ptr->Octet, sizeof(HT_CAPABILITY_IE));
+
+					*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+					*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+					*pHtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - IE_HT_CAP\n"));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - wrong IE_HT_CAP.eid_ptr->Len = %d\n", eid_ptr->Len));
+				}
+				break;
+
+			default:
+				break;
+		}
+
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+    return TRUE;
+}
+
+BOOLEAN PeerDlsRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability)
+{
+    CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+	PEID_STRUCT  eid_ptr;
+
+    // to prevent caller from using garbage output value
+    *pStatus		= 0;
+    *pCapabilityInfo	= 0;
+	*pHtCapabilityLen = 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+	// get status code from payload and advance the pointer
+    NdisMoveMemory(pStatus, Ptr, 2);
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+	if (pStatus == 0)
+	{
+	    // get capability info from payload and advance the pointer
+	    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+	    Ptr += 2;
+	}
+
+	// Category and Action field + status code + DA + SA + capability
+	eid_ptr = (PEID_STRUCT) &Fr->Octet[18];
+
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((UCHAR*)Fr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_SUPP_RATES:
+                if ((eid_ptr->Len <= MAX_LEN_OF_SUPPORTED_RATES) && (eid_ptr->Len > 0))
+                {
+                    NdisMoveMemory(Rates, eid_ptr->Octet, eid_ptr->Len);
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - IE_SUPP_RATES., Len=%d. Rates[0]=%x\n",eid_ptr->Len, Rates[0]));
+                    DBGPRINT(RT_DEBUG_TRACE, ("Rates[1]=%x %x %x %x %x %x %x\n", Rates[1], Rates[2], Rates[3], Rates[4], Rates[5], Rates[6], Rates[7]));
+                    *pRatesLen = eid_ptr->Len;
+                }
+                else
+                {
+                    *pRatesLen = 8;
+					Rates[0] = 0x82;
+					Rates[1] = 0x84;
+					Rates[2] = 0x8b;
+					Rates[3] = 0x96;
+					Rates[4] = 0x12;
+					Rates[5] = 0x24;
+					Rates[6] = 0x48;
+					Rates[7] = 0x6c;
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - wrong IE_SUPP_RATES., Len=%d\n",eid_ptr->Len));
+                }
+				break;
+
+			case IE_EXT_SUPP_RATES:
+                if (eid_ptr->Len + *pRatesLen <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, eid_ptr->Len);
+                    *pRatesLen = (*pRatesLen) + eid_ptr->Len;
+                }
+                else
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, MAX_LEN_OF_SUPPORTED_RATES - (*pRatesLen));
+                    *pRatesLen = MAX_LEN_OF_SUPPORTED_RATES;
+                }
+				break;
+
+			case IE_HT_CAP:
+				if (eid_ptr->Len >= sizeof(HT_CAPABILITY_IE))
+				{
+					NdisMoveMemory(pHtCapability, eid_ptr->Octet, sizeof(HT_CAPABILITY_IE));
+
+					*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+					*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+					*pHtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - IE_HT_CAP\n"));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - wrong IE_HT_CAP.eid_ptr->Len = %d\n", eid_ptr->Len));
+				}
+				break;
+
+			default:
+				break;
+		}
+
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+    return TRUE;
+}
+
+BOOLEAN PeerDlsTearDownSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pReason)
+{
+    CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+
+    // to prevent caller from using garbage output value
+    *pReason	= 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+	// get reason code from payload and advance the pointer
+    NdisMoveMemory(pReason, Ptr, 2);
+    Ptr += 2;
+
+    return TRUE;
+}
+#endif // QOS_DLS_SUPPORT //
+
diff --git a/drivers/staging/rt2870/common/cmm_sync.c b/drivers/staging/rt2870/common/cmm_sync.c
new file mode 100644
index 00000000000..2be7c77a384
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_sync.c
@@ -0,0 +1,711 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sync.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-09-01      modified for rt2561/2661
+*/
+#include "../rt_config.h"
+
+// 2.4 Ghz channel plan index in the TxPower arrays.
+#define	BG_BAND_REGION_0_START	0			// 1,2,3,4,5,6,7,8,9,10,11
+#define	BG_BAND_REGION_0_SIZE	11
+#define	BG_BAND_REGION_1_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13
+#define	BG_BAND_REGION_1_SIZE	13
+#define	BG_BAND_REGION_2_START	9			// 10,11
+#define	BG_BAND_REGION_2_SIZE	2
+#define	BG_BAND_REGION_3_START	9			// 10,11,12,13
+#define	BG_BAND_REGION_3_SIZE	4
+#define	BG_BAND_REGION_4_START	13			// 14
+#define	BG_BAND_REGION_4_SIZE	1
+#define	BG_BAND_REGION_5_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13,14
+#define	BG_BAND_REGION_5_SIZE	14
+#define	BG_BAND_REGION_6_START	2			// 3,4,5,6,7,8,9
+#define	BG_BAND_REGION_6_SIZE	7
+#define	BG_BAND_REGION_7_START	4			// 5,6,7,8,9,10,11,12,13
+#define	BG_BAND_REGION_7_SIZE	9
+#define	BG_BAND_REGION_31_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13,14
+#define	BG_BAND_REGION_31_SIZE	14
+
+// 5 Ghz channel plan index in the TxPower arrays.
+UCHAR A_BAND_REGION_0_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_1_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+UCHAR A_BAND_REGION_2_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64};
+UCHAR A_BAND_REGION_3_CHANNEL_LIST[]={52, 56, 60, 64, 149, 153, 157, 161};
+UCHAR A_BAND_REGION_4_CHANNEL_LIST[]={149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_5_CHANNEL_LIST[]={149, 153, 157, 161};
+UCHAR A_BAND_REGION_6_CHANNEL_LIST[]={36, 40, 44, 48};
+UCHAR A_BAND_REGION_7_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_8_CHANNEL_LIST[]={52, 56, 60, 64};
+UCHAR A_BAND_REGION_9_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_10_CHANNEL_LIST[]={36, 40, 44, 48, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_11_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 149, 153, 157, 161};
+
+//BaSizeArray follows the 802.11n definition as MaxRxFactor.  2^(13+factor) bytes. When factor =0, it's about Ba buffer size =8.
+UCHAR BaSizeArray[4] = {8,16,32,64};
+
+/*
+	==========================================================================
+	Description:
+		Update StaCfg->ChannelList[] according to 1) Country Region 2) RF IC type,
+		and 3) PHY-mode user selected.
+		The outcome is used by driver when doing site survey.
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID BuildChannelList(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR i, j, index=0, num=0;
+	PUCHAR	pChannelList = NULL;
+
+	NdisZeroMemory(pAd->ChannelList, MAX_NUM_OF_CHANNELS * sizeof(CHANNEL_TX_POWER));
+
+	// if not 11a-only mode, channel list starts from 2.4Ghz band
+	if ((pAd->CommonCfg.PhyMode != PHY_11A)
+#ifdef DOT11_N_SUPPORT
+		&& (pAd->CommonCfg.PhyMode != PHY_11AN_MIXED) && (pAd->CommonCfg.PhyMode != PHY_11N_5G)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		switch (pAd->CommonCfg.CountryRegion  & 0x7f)
+		{
+			case REGION_0_BG_BAND:	// 1 -11
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_0_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_0_SIZE);
+				index += BG_BAND_REGION_0_SIZE;
+				break;
+			case REGION_1_BG_BAND:	// 1 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_1_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_1_SIZE);
+				index += BG_BAND_REGION_1_SIZE;
+				break;
+			case REGION_2_BG_BAND:	// 10 - 11
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_2_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_2_SIZE);
+				index += BG_BAND_REGION_2_SIZE;
+				break;
+			case REGION_3_BG_BAND:	// 10 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_3_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_3_SIZE);
+				index += BG_BAND_REGION_3_SIZE;
+				break;
+			case REGION_4_BG_BAND:	// 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_4_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_4_SIZE);
+				index += BG_BAND_REGION_4_SIZE;
+				break;
+			case REGION_5_BG_BAND:	// 1 - 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_5_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_5_SIZE);
+				index += BG_BAND_REGION_5_SIZE;
+				break;
+			case REGION_6_BG_BAND:	// 3 - 9
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_6_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_6_SIZE);
+				index += BG_BAND_REGION_6_SIZE;
+				break;
+			case REGION_7_BG_BAND:  // 5 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_7_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_7_SIZE);
+				index += BG_BAND_REGION_7_SIZE;
+				break;
+			case REGION_31_BG_BAND:	// 1 - 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_31_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_31_SIZE);
+				index += BG_BAND_REGION_31_SIZE;
+				break;
+			default:            // Error. should never happen
+				break;
+		}
+		for (i=0; i<index; i++)
+			pAd->ChannelList[i].MaxTxPwr = 20;
+	}
+
+	if ((pAd->CommonCfg.PhyMode == PHY_11A) || (pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED)
+#ifdef DOT11_N_SUPPORT
+		|| (pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11AN_MIXED)
+		|| (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		switch (pAd->CommonCfg.CountryRegionForABand & 0x7f)
+		{
+			case REGION_0_A_BAND:
+				num = sizeof(A_BAND_REGION_0_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_0_CHANNEL_LIST;
+				break;
+			case REGION_1_A_BAND:
+				num = sizeof(A_BAND_REGION_1_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_1_CHANNEL_LIST;
+				break;
+			case REGION_2_A_BAND:
+				num = sizeof(A_BAND_REGION_2_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_2_CHANNEL_LIST;
+				break;
+			case REGION_3_A_BAND:
+				num = sizeof(A_BAND_REGION_3_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_3_CHANNEL_LIST;
+				break;
+			case REGION_4_A_BAND:
+				num = sizeof(A_BAND_REGION_4_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_4_CHANNEL_LIST;
+				break;
+			case REGION_5_A_BAND:
+				num = sizeof(A_BAND_REGION_5_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_5_CHANNEL_LIST;
+				break;
+			case REGION_6_A_BAND:
+				num = sizeof(A_BAND_REGION_6_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_6_CHANNEL_LIST;
+				break;
+			case REGION_7_A_BAND:
+				num = sizeof(A_BAND_REGION_7_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_7_CHANNEL_LIST;
+				break;
+			case REGION_8_A_BAND:
+				num = sizeof(A_BAND_REGION_8_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_8_CHANNEL_LIST;
+				break;
+			case REGION_9_A_BAND:
+				num = sizeof(A_BAND_REGION_9_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_9_CHANNEL_LIST;
+				break;
+
+			case REGION_10_A_BAND:
+				num = sizeof(A_BAND_REGION_10_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_10_CHANNEL_LIST;
+				break;
+
+			case REGION_11_A_BAND:
+				num = sizeof(A_BAND_REGION_11_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_11_CHANNEL_LIST;
+				break;
+
+			default:            // Error. should never happen
+				DBGPRINT(RT_DEBUG_WARN,("countryregion=%d not support", pAd->CommonCfg.CountryRegionForABand));
+				break;
+		}
+
+		if (num != 0)
+		{
+			UCHAR RadarCh[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+			for (i=0; i<num; i++)
+			{
+				for (j=0; j<MAX_NUM_OF_CHANNELS; j++)
+				{
+					if (pChannelList[i] == pAd->TxPower[j].Channel)
+						NdisMoveMemory(&pAd->ChannelList[index+i], &pAd->TxPower[j], sizeof(CHANNEL_TX_POWER));
+					}
+				for (j=0; j<15; j++)
+				{
+					if (pChannelList[i] == RadarCh[j])
+						pAd->ChannelList[index+i].DfsReq = TRUE;
+				}
+				pAd->ChannelList[index+i].MaxTxPwr = 20;
+			}
+			index += num;
+		}
+	}
+
+	pAd->ChannelListNum = index;
+	DBGPRINT(RT_DEBUG_TRACE,("country code=%d/%d, RFIC=%d, PHY mode=%d, support %d channels\n",
+		pAd->CommonCfg.CountryRegion, pAd->CommonCfg.CountryRegionForABand, pAd->RfIcType, pAd->CommonCfg.PhyMode, pAd->ChannelListNum));
+#ifdef DBG
+	for (i=0;i<pAd->ChannelListNum;i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("BuildChannel # %d :: Pwr0 = %d, Pwr1 =%d, \n ", pAd->ChannelList[i].Channel, pAd->ChannelList[i].Power, pAd->ChannelList[i].Power2));
+	}
+#endif
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine return the first channel number according to the country
+		code selection and RF IC selection (signal band or dual band). It is called
+		whenever driver need to start a site survey of all supported channels.
+	Return:
+		ch - the first channel number of current country code setting
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+UCHAR FirstChannel(
+	IN PRTMP_ADAPTER pAd)
+{
+	return pAd->ChannelList[0].Channel;
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine returns the next channel number. This routine is called
+		during driver need to start a site survey of all supported channels.
+	Return:
+		next_channel - the next channel number valid in current country code setting.
+	Note:
+		return 0 if no more next channel
+	==========================================================================
+ */
+UCHAR NextChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel)
+{
+	int i;
+	UCHAR next_channel = 0;
+
+	for (i = 0; i < (pAd->ChannelListNum - 1); i++)
+		if (channel == pAd->ChannelList[i].Channel)
+		{
+			next_channel = pAd->ChannelList[i+1].Channel;
+			break;
+	}
+	return next_channel;
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine is for Cisco Compatible Extensions 2.X
+		Spec31. AP Control of Client Transmit Power
+	Return:
+		None
+	Note:
+	   Required by Aironet dBm(mW)
+		   0dBm(1mW),   1dBm(5mW), 13dBm(20mW), 15dBm(30mW),
+		  17dBm(50mw), 20dBm(100mW)
+
+	   We supported
+		   3dBm(Lowest), 6dBm(10%), 9dBm(25%), 12dBm(50%),
+		  14dBm(75%),   15dBm(100%)
+
+		The client station's actual transmit power shall be within +/- 5dB of
+		the minimum value or next lower value.
+	==========================================================================
+ */
+VOID ChangeToCellPowerLimit(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         AironetCellPowerLimit)
+{
+	//valud 0xFF means that hasn't found power limit information
+	//from the AP's Beacon/Probe response.
+	if (AironetCellPowerLimit == 0xFF)
+		return;
+
+	if (AironetCellPowerLimit < 6) //Used Lowest Power Percentage.
+		pAd->CommonCfg.TxPowerPercentage = 6;
+	else if (AironetCellPowerLimit < 9)
+		pAd->CommonCfg.TxPowerPercentage = 10;
+	else if (AironetCellPowerLimit < 12)
+		pAd->CommonCfg.TxPowerPercentage = 25;
+	else if (AironetCellPowerLimit < 14)
+		pAd->CommonCfg.TxPowerPercentage = 50;
+	else if (AironetCellPowerLimit < 15)
+		pAd->CommonCfg.TxPowerPercentage = 75;
+	else
+		pAd->CommonCfg.TxPowerPercentage = 100; //else used maximum
+
+	if (pAd->CommonCfg.TxPowerPercentage > pAd->CommonCfg.TxPowerDefault)
+		pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+
+}
+
+CHAR	ConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR			Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+	LNAGain = GET_LNA_GAIN(pAd);
+    if (pAd->LatchRfRegs.Channel > 14)
+    {
+        if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+    }
+    else
+    {
+        if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+    }
+
+    return (-12 - RssiOffset - LNAGain - Rssi);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan next channel
+	==========================================================================
+ */
+VOID ScanNextChannel(
+	IN PRTMP_ADAPTER pAd)
+{
+	HEADER_802_11   Hdr80211;
+	PUCHAR          pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG           FrameLen = 0;
+	UCHAR           SsidLen = 0, ScanType = pAd->MlmeAux.ScanType, BBPValue = 0;
+#ifdef CONFIG_STA_SUPPORT
+	USHORT          Status;
+	PHEADER_802_11  pHdr80211;
+#endif // CONFIG_STA_SUPPORT //
+	UINT			ScanTimeIn5gChannel = SHORT_CHANNEL_TIME;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (MONITOR_ON(pAd))
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	if (pAd->MlmeAux.Channel == 0)
+	{
+		if ((pAd->CommonCfg.BBPCurrentBW == BW_40)
+#ifdef CONFIG_STA_SUPPORT
+			&& (INFRA_ON(pAd)
+				|| (pAd->OpMode == OPMODE_AP))
+#endif // CONFIG_STA_SUPPORT //
+			)
+		{
+			AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+			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);
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to 40MHz channel %d, Total BSS[%02d]\n",pAd->CommonCfg.CentralChannel, pAd->ScanTab.BssNr));
+		}
+		else
+		{
+			AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to channel %d, Total BSS[%02d]\n",pAd->CommonCfg.Channel, pAd->ScanTab.BssNr));
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			//
+			// To prevent data lost.
+			// Send an NULL data with turned PSM bit on to current associated AP before SCAN progress.
+			// Now, we need to send an NULL data with turned PSM bit off to AP, when scan progress done
+			//
+			if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) && (INFRA_ON(pAd)))
+			{
+				NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);
+				if (NStatus	== NDIS_STATUS_SUCCESS)
+				{
+					pHdr80211 = (PHEADER_802_11) pOutBuffer;
+					MgtMacHeaderInit(pAd, pHdr80211, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+					pHdr80211->Duration = 0;
+					pHdr80211->FC.Type = BTYPE_DATA;
+					pHdr80211->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+
+					// Send using priority queue
+					MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+					DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqAction -- Send PSM Data frame\n"));
+					MlmeFreeMemory(pAd, pOutBuffer);
+					RTMPusecDelay(5000);
+				}
+			}
+
+			pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+			Status = MLME_SUCCESS;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+	}
+#ifdef RT2870
+#ifdef CONFIG_STA_SUPPORT
+	else if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST) && (pAd->OpMode == OPMODE_STA))
+	{
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		MlmeCntlConfirm(pAd, MT2_SCAN_CONF, MLME_FAIL_NO_RESOURCE);
+	}
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2870 //
+	else
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+		// 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);
+
+			// leave PSM during scanning. otherwise we may lost ProbeRsp & BEACON
+			if (pAd->StaCfg.Psm == PWR_SAVE)
+				MlmeSetPsmBit(pAd, PWR_ACTIVE);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, TRUE);
+		AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (pAd->MlmeAux.Channel > 14)
+			{
+				if ((pAd->CommonCfg.bIEEE80211H == 1) && RadarChannelCheck(pAd, pAd->MlmeAux.Channel))
+				{
+					ScanType = SCAN_PASSIVE;
+					ScanTimeIn5gChannel = MIN_CHANNEL_TIME;
+				}
+			}
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+			// carrier detection
+			if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+			{
+				ScanType = SCAN_PASSIVE;
+				ScanTimeIn5gChannel = MIN_CHANNEL_TIME;
+			}
+#endif // CARRIER_DETECTION_SUPPORT //
+		}
+
+#endif // CONFIG_STA_SUPPORT //
+
+		//Global country domain(ch1-11:active scan, ch12-14 passive scan)
+		if ((pAd->MlmeAux.Channel <= 14) && (pAd->MlmeAux.Channel >= 12) && ((pAd->CommonCfg.CountryRegion & 0x7f) == REGION_31_BG_BAND))
+		{
+			ScanType = SCAN_PASSIVE;
+		}
+
+		// We need to shorten active scan time in order for WZC connect issue
+		// Chnage the channel scan time for CISCO stuff based on its IAPP announcement
+		if (ScanType == FAST_SCAN_ACTIVE)
+			RTMPSetTimer(&pAd->MlmeAux.ScanTimer, FAST_ACTIVE_SCAN_TIME);
+#ifdef CONFIG_STA_SUPPORT
+		else if (((ScanType == SCAN_CISCO_ACTIVE) ||
+				(ScanType == SCAN_CISCO_PASSIVE) ||
+				(ScanType == SCAN_CISCO_CHANNEL_LOAD) ||
+				(ScanType == SCAN_CISCO_NOISE)) && (pAd->OpMode == OPMODE_STA))
+		{
+			if (pAd->StaCfg.CCXScanTime < 25)
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, pAd->StaCfg.CCXScanTime * 2);
+			else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, pAd->StaCfg.CCXScanTime);
+		}
+#endif // CONFIG_STA_SUPPORT //
+		else // must be SCAN_PASSIVE or SCAN_ACTIVE
+		{
+			if ((pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED)
+#ifdef DOT11_N_SUPPORT
+				|| (pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED)
+#endif // DOT11_N_SUPPORT //
+			)
+			{
+				if (pAd->MlmeAux.Channel > 14)
+					RTMPSetTimer(&pAd->MlmeAux.ScanTimer, ScanTimeIn5gChannel);
+				else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, MIN_CHANNEL_TIME);
+			}
+			else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, MAX_CHANNEL_TIME);
+		}
+
+		if ((ScanType == SCAN_ACTIVE) || (ScanType == FAST_SCAN_ACTIVE) ||
+			(ScanType == SCAN_CISCO_ACTIVE))
+		{
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("SYNC - ScanNextChannel() allocate memory fail\n"));
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+					Status = MLME_FAIL_NO_RESOURCE;
+					MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+				}
+#endif // CONFIG_STA_SUPPORT //
+
+				return;
+			}
+
+			// There is no need to send broadcast probe request if active scan is in effect.
+			if ((ScanType == SCAN_ACTIVE) || (ScanType == FAST_SCAN_ACTIVE)
+				)
+				SsidLen = pAd->MlmeAux.SsidLen;
+			else
+				SsidLen = 0;
+
+			MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+			MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+							  sizeof(HEADER_802_11),    &Hdr80211,
+							  1,                        &SsidIe,
+							  1,                        &SsidLen,
+							  SsidLen,			        pAd->MlmeAux.Ssid,
+							  1,                        &SupRateIe,
+							  1,                        &pAd->CommonCfg.SupRateLen,
+							  pAd->CommonCfg.SupRateLen,  pAd->CommonCfg.SupRate,
+							  END_OF_ARGS);
+
+			if (pAd->CommonCfg.ExtRateLen)
+			{
+				ULONG Tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+								  1,                                &ExtRateIe,
+								  1,                                &pAd->CommonCfg.ExtRateLen,
+								  pAd->CommonCfg.ExtRateLen,          pAd->CommonCfg.ExtRate,
+								  END_OF_ARGS);
+				FrameLen += Tmp;
+			}
+
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				ULONG	Tmp;
+				UCHAR	HtLen;
+				UCHAR	BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+#ifdef RT_BIG_ENDIAN
+				HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+				if (pAd->bBroadComHT == TRUE)
+				{
+					HtLen = pAd->MlmeAux.HtCapabilityLen + 4;
+#ifdef RT_BIG_ENDIAN
+					NdisMoveMemory(&HtCapabilityTmp, &pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+					*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+					*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &WpaIe,
+									1,                                &HtLen,
+									4,                                &BROADCOM[0],
+									pAd->MlmeAux.HtCapabilityLen,     &HtCapabilityTmp,
+									END_OF_ARGS);
+#else
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &WpaIe,
+									1,                                &HtLen,
+									4,                                &BROADCOM[0],
+									pAd->MlmeAux.HtCapabilityLen,     &pAd->MlmeAux.HtCapability,
+									END_OF_ARGS);
+#endif // RT_BIG_ENDIAN //
+				}
+				else
+				{
+					HtLen = pAd->MlmeAux.HtCapabilityLen;
+#ifdef RT_BIG_ENDIAN
+					NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, SIZE_HT_CAP_IE);
+					*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+					*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &HtCapIe,
+									1,                                &HtLen,
+									HtLen,                            &HtCapabilityTmp,
+									END_OF_ARGS);
+#else
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &HtCapIe,
+									1,                                &HtLen,
+									HtLen,                            &pAd->CommonCfg.HtCapability,
+									END_OF_ARGS);
+#endif // RT_BIG_ENDIAN //
+				}
+				FrameLen += Tmp;
+
+#ifdef DOT11N_DRAFT3
+				if (pAd->CommonCfg.BACapability.field.b2040CoexistScanSup == 1)
+				{
+					ULONG		Tmp;
+					HtLen = 1;
+					MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+									  1,					&ExtHtCapIe,
+									  1,					&HtLen,
+									  1,          			&pAd->CommonCfg.BSSCoexist2040.word,
+									  END_OF_ARGS);
+
+					FrameLen += Tmp;
+				}
+#endif // DOT11N_DRAFT3 //
+			}
+#endif // DOT11_N_SUPPORT //
+
+
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+
+		// For SCAN_CISCO_PASSIVE, do nothing and silently wait for beacon or other probe reponse
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			pAd->Mlme.SyncMachine.CurrState = SCAN_LISTEN;
+#endif // CONFIG_STA_SUPPORT //
+
+	}
+}
+
+VOID MgtProbReqMacHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+
+	pHdr80211->FC.Type = BTYPE_MGMT;
+	pHdr80211->FC.SubType = SubType;
+	if (SubType == SUBTYPE_ACK)
+		pHdr80211->FC.Type = BTYPE_CNTL;
+	pHdr80211->FC.ToDs = ToDs;
+	COPY_MAC_ADDR(pHdr80211->Addr1, pDA);
+	COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHdr80211->Addr3, pBssid);
+}
+
+
diff --git a/drivers/staging/rt2870/common/cmm_wpa.c b/drivers/staging/rt2870/common/cmm_wpa.c
new file mode 100644
index 00000000000..d2c24bd49e7
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_wpa.c
@@ -0,0 +1,1654 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Jan	Lee		03-07-22		Initial
+	Paul Lin	03-11-28		Modify for supplicant
+*/
+#include "../rt_config.h"
+// 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
+UCHAR       OUI_WPA2_WEP40[4]   = {0x00, 0x0F, 0xAC, 0x01};
+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
+
+/*
+	========================================================================
+
+	Routine Description:
+		The pseudo-random function(PRF) that hashes various inputs to
+		derive a pseudo-random value. To add liveness to the pseudo-random
+		value, a nonce should be one of the inputs.
+
+		It is used to generate PTK, GTK or some specific random value.
+
+	Arguments:
+		UCHAR	*key,		-	the key material for HMAC_SHA1 use
+		INT		key_len		-	the length of key
+		UCHAR	*prefix		-	a prefix label
+		INT		prefix_len	-	the length of the label
+		UCHAR	*data		-	a specific data with variable length
+		INT		data_len	-	the length of a specific data
+		INT		len			-	the output lenght
+
+	Return Value:
+		UCHAR	*output		-	the calculated result
+
+	Note:
+		802.11i-2004	Annex H.3
+
+	========================================================================
+*/
+VOID	PRF(
+	IN	UCHAR	*key,
+	IN	INT		key_len,
+	IN	UCHAR	*prefix,
+	IN	INT		prefix_len,
+	IN	UCHAR	*data,
+	IN	INT		data_len,
+	OUT	UCHAR	*output,
+	IN	INT		len)
+{
+	INT		i;
+    UCHAR   *input;
+	INT		currentindex = 0;
+	INT		total_len;
+
+	// Allocate memory for input
+	os_alloc_mem(NULL, (PUCHAR *)&input, 1024);
+
+    if (input == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("!!!PRF: no memory!!!\n"));
+        return;
+    }
+
+	// Generate concatenation input
+	NdisMoveMemory(input, prefix, prefix_len);
+
+	// Concatenate a single octet containing 0
+	input[prefix_len] =	0;
+
+	// Concatenate specific data
+	NdisMoveMemory(&input[prefix_len + 1], data, data_len);
+	total_len =	prefix_len + 1 + data_len;
+
+	// Concatenate a single octet containing 0
+	// This octet shall be update later
+	input[total_len] = 0;
+	total_len++;
+
+	// Iterate to calculate the result by hmac-sha-1
+	// Then concatenate to last result
+	for	(i = 0;	i <	(len + 19) / 20; i++)
+	{
+		HMAC_SHA1(input, total_len,	key, key_len, &output[currentindex]);
+		currentindex +=	20;
+
+		// update the last octet
+		input[total_len - 1]++;
+	}
+    os_free_mem(NULL, input);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		It utilizes PRF-384 or PRF-512 to derive session-specific keys from a PMK.
+		It shall be called by 4-way handshake processing.
+
+	Arguments:
+		pAd 	-	pointer to our pAdapter context
+		PMK		-	pointer to PMK
+		ANonce	-	pointer to ANonce
+		AA		-	pointer to Authenticator Address
+		SNonce	-	pointer to SNonce
+		SA		-	pointer to Supplicant Address
+		len		-	indicate the length of PTK (octet)
+
+	Return Value:
+		Output		pointer to the PTK
+
+	Note:
+		Refer to IEEE 802.11i-2004 8.5.1.2
+
+	========================================================================
+*/
+VOID WpaCountPTK(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	*PMK,
+	IN	UCHAR	*ANonce,
+	IN	UCHAR	*AA,
+	IN	UCHAR	*SNonce,
+	IN	UCHAR	*SA,
+	OUT	UCHAR	*output,
+	IN	UINT	len)
+{
+	UCHAR	concatenation[76];
+	UINT	CurrPos = 0;
+	UCHAR	temp[32];
+	UCHAR	Prefix[] = {'P', 'a', 'i', 'r', 'w', 'i', 's', 'e', ' ', 'k', 'e', 'y', ' ',
+						'e', 'x', 'p', 'a', 'n', 's', 'i', 'o', 'n'};
+
+	// initiate the concatenation input
+	NdisZeroMemory(temp, sizeof(temp));
+	NdisZeroMemory(concatenation, 76);
+
+	// Get smaller address
+	if (RTMPCompareMemory(SA, AA, 6) == 1)
+		NdisMoveMemory(concatenation, AA, 6);
+	else
+		NdisMoveMemory(concatenation, SA, 6);
+	CurrPos += 6;
+
+	// Get larger address
+	if (RTMPCompareMemory(SA, AA, 6) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], SA, 6);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], AA, 6);
+
+	// store the larger mac address for backward compatible of
+	// ralink proprietary STA-key issue
+	NdisMoveMemory(temp, &concatenation[CurrPos], MAC_ADDR_LEN);
+	CurrPos += 6;
+
+	// Get smaller Nonce
+	if (RTMPCompareMemory(ANonce, SNonce, 32) == 0)
+		NdisMoveMemory(&concatenation[CurrPos], temp, 32);	// patch for ralink proprietary STA-key issue
+	else if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], SNonce, 32);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], ANonce, 32);
+	CurrPos += 32;
+
+	// Get larger Nonce
+	if (RTMPCompareMemory(ANonce, SNonce, 32) == 0)
+		NdisMoveMemory(&concatenation[CurrPos], temp, 32);	// patch for ralink proprietary STA-key issue
+	else if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], ANonce, 32);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], SNonce, 32);
+	CurrPos += 32;
+
+	hex_dump("concatenation=", concatenation, 76);
+
+	// Use PRF to generate PTK
+	PRF(PMK, LEN_MASTER_KEY, Prefix, 22, concatenation, 76, output, len);
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Generate random number by software.
+
+	Arguments:
+		pAd		-	pointer to our pAdapter context
+		macAddr	-	pointer to local MAC address
+
+	Return Value:
+
+	Note:
+		802.1ii-2004  Annex H.5
+
+	========================================================================
+*/
+VOID	GenRandom(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			*macAddr,
+	OUT	UCHAR			*random)
+{
+	INT		i, curr;
+	UCHAR	local[80], KeyCounter[32];
+	UCHAR	result[80];
+	ULONG	CurrentTime;
+	UCHAR	prefix[] = {'I', 'n', 'i', 't', ' ', 'C', 'o', 'u', 'n', 't', 'e', 'r'};
+
+	// Zero the related information
+	NdisZeroMemory(result, 80);
+	NdisZeroMemory(local, 80);
+	NdisZeroMemory(KeyCounter, 32);
+
+	for	(i = 0;	i <	32;	i++)
+	{
+		// copy the local MAC address
+		COPY_MAC_ADDR(local, macAddr);
+		curr =	MAC_ADDR_LEN;
+
+		// concatenate the current time
+		NdisGetSystemUpTime(&CurrentTime);
+		NdisMoveMemory(&local[curr],  &CurrentTime,	sizeof(CurrentTime));
+		curr +=	sizeof(CurrentTime);
+
+		// concatenate the last result
+		NdisMoveMemory(&local[curr],  result, 32);
+		curr +=	32;
+
+		// concatenate a variable
+		NdisMoveMemory(&local[curr],  &i,  2);
+		curr +=	2;
+
+		// calculate the result
+		PRF(KeyCounter, 32, prefix,12, local, curr, result, 32);
+	}
+
+	NdisMoveMemory(random, result,	32);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build cipher suite in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+    	WepStatus	-	indicate the encryption type
+		bMixCipher	-	a boolean to indicate the pairwise cipher and group
+						cipher are the same or not
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeCipher(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UINT			WepStatus,
+	IN	BOOLEAN			bMixCipher,
+	IN	UCHAR			FlexibleCipher,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	UCHAR	PairwiseCnt;
+
+	*rsn_len = 0;
+
+	// decide WPA2 or WPA1
+	if (ElementID == Wpa2Ie)
+	{
+		RSNIE2	*pRsnie_cipher = (RSNIE2*)pRsnIe;
+
+		// Assign the verson as 1
+		pRsnie_cipher->version = 1;
+
+        switch (WepStatus)
+        {
+        	// TKIP mode
+            case Ndis802_11Encryption2Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_TKIP, 4);
+                *rsn_len = sizeof(RSNIE2);
+                break;
+
+			// AES mode
+            case Ndis802_11Encryption3Enabled:
+				if (bMixCipher)
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+				else
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_CCMP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_CCMP, 4);
+                *rsn_len = sizeof(RSNIE2);
+                break;
+
+			// TKIP-AES mix mode
+            case Ndis802_11Encryption4Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+
+				PairwiseCnt = 1;
+				// Insert WPA2 TKIP as the first pairwise cipher
+				if (MIX_CIPHER_WPA2_TKIP_ON(FlexibleCipher))
+				{
+                	NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_TKIP, 4);
+					// Insert WPA2 AES as the secondary pairwise cipher
+					if (MIX_CIPHER_WPA2_AES_ON(FlexibleCipher))
+					{
+                		NdisMoveMemory(pRsnie_cipher->ucast[0].oui + 4, OUI_WPA2_CCMP, 4);
+						PairwiseCnt = 2;
+					}
+				}
+				else
+				{
+					// Insert WPA2 AES as the first pairwise cipher
+					NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_CCMP, 4);
+				}
+
+                pRsnie_cipher->ucount = PairwiseCnt;
+                *rsn_len = sizeof(RSNIE2) + (4 * (PairwiseCnt - 1));
+                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);
+	}
+	else
+	{
+		RSNIE	*pRsnie_cipher = (RSNIE*)pRsnIe;
+
+		// Assign OUI and version
+		NdisMoveMemory(pRsnie_cipher->oui, OUI_WPA_VERSION, 4);
+        pRsnie_cipher->version = 1;
+
+		switch (WepStatus)
+		{
+			// TKIP mode
+            case Ndis802_11Encryption2Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_TKIP, 4);
+                *rsn_len = sizeof(RSNIE);
+                break;
+
+			// AES mode
+            case Ndis802_11Encryption3Enabled:
+				if (bMixCipher)
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+				else
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_CCMP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_CCMP, 4);
+                *rsn_len = sizeof(RSNIE);
+                break;
+
+			// TKIP-AES mix mode
+            case Ndis802_11Encryption4Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+
+				PairwiseCnt = 1;
+				// Insert WPA TKIP as the first pairwise cipher
+				if (MIX_CIPHER_WPA_TKIP_ON(FlexibleCipher))
+				{
+                	NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_TKIP, 4);
+					// Insert WPA AES as the secondary pairwise cipher
+					if (MIX_CIPHER_WPA_AES_ON(FlexibleCipher))
+					{
+                		NdisMoveMemory(pRsnie_cipher->ucast[0].oui + 4, OUI_WPA_CCMP, 4);
+						PairwiseCnt = 2;
+					}
+				}
+				else
+				{
+					// Insert WPA AES as the first pairwise cipher
+					NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_CCMP, 4);
+				}
+
+                pRsnie_cipher->ucount = PairwiseCnt;
+                *rsn_len = sizeof(RSNIE) + (4 * (PairwiseCnt - 1));
+                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);
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build AKM suite in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+    	AuthMode	-	indicate the authentication mode
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeAKM(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UINT			AuthMode,
+	IN	UCHAR			apidx,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	RSNIE_AUTH		*pRsnie_auth;
+
+	pRsnie_auth = (RSNIE_AUTH*)(pRsnIe + (*rsn_len));
+
+	// decide WPA2 or WPA1
+	if (ElementID == Wpa2Ie)
+	{
+		switch (AuthMode)
+        {
+            case Ndis802_11AuthModeWPA2:
+            case Ndis802_11AuthModeWPA1WPA2:
+                pRsnie_auth->acount = 1;
+                	NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA2_8021X_AKM, 4);
+                break;
+
+            case Ndis802_11AuthModeWPA2PSK:
+            case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+                pRsnie_auth->acount = 1;
+                	NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA2_PSK_AKM, 4);
+                break;
+        }
+	}
+	else
+	{
+		switch (AuthMode)
+        {
+            case Ndis802_11AuthModeWPA:
+            case Ndis802_11AuthModeWPA1WPA2:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_8021X_AKM, 4);
+                break;
+
+            case Ndis802_11AuthModeWPAPSK:
+            case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_PSK_AKM, 4);
+                break;
+
+			case Ndis802_11AuthModeWPANone:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_NONE_AKM, 4);
+                break;
+        }
+	}
+
+	pRsnie_auth->acount = cpu2le16(pRsnie_auth->acount);
+
+	(*rsn_len) += sizeof(RSNIE_AUTH);	// update current RSNIE length
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build capability in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeCap(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UCHAR			apidx,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	RSN_CAPABILITIES    *pRSN_Cap;
+
+	// it could be ignored in WPA1 mode
+	if (ElementID == WpaIe)
+		return;
+
+	pRSN_Cap = (RSN_CAPABILITIES*)(pRsnIe + (*rsn_len));
+
+
+	pRSN_Cap->word = cpu2le16(pRSN_Cap->word);
+
+	(*rsn_len) += sizeof(RSN_CAPABILITIES);	// update current RSNIE length
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build RSN IE context. It is not included element-ID and length.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	AuthMode	-	indicate the authentication mode
+    	WepStatus	-	indicate the encryption type
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPMakeRSNIE(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  UINT            AuthMode,
+    IN  UINT            WepStatus,
+	IN	UCHAR			apidx)
+{
+	PUCHAR		pRsnIe = NULL;			// primary RSNIE
+	UCHAR 		*rsnielen_cur_p = 0;	// the length of the primary RSNIE
+	UCHAR		*rsnielen_ex_cur_p = 0;	// the length of the secondary RSNIE
+	UCHAR		PrimaryRsnie;
+	BOOLEAN		bMixCipher = FALSE;	// indicate the pairwise and group cipher are different
+	UCHAR		p_offset;
+	WPA_MIX_PAIR_CIPHER		FlexibleCipher = MIX_CIPHER_NOTUSE;	// it provide the more flexible cipher combination in WPA-WPA2 and TKIPAES mode
+
+	rsnielen_cur_p = NULL;
+	rsnielen_ex_cur_p = NULL;
+
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef WPA_SUPPLICANT_SUPPORT
+			if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+			{
+				if (AuthMode < Ndis802_11AuthModeWPA)
+					return;
+			}
+			else
+#endif // WPA_SUPPLICANT_SUPPORT //
+			{
+				// Support WPAPSK or WPA2PSK in STA-Infra mode
+				// Support WPANone in STA-Adhoc mode
+				if ((AuthMode != Ndis802_11AuthModeWPAPSK) &&
+					(AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+					(AuthMode != Ndis802_11AuthModeWPANone)
+					)
+					return;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,("==> RTMPMakeRSNIE(STA)\n"));
+
+			// Zero RSNIE context
+			pAd->StaCfg.RSNIE_Len = 0;
+			NdisZeroMemory(pAd->StaCfg.RSN_IE, MAX_LEN_OF_RSNIE);
+
+			// Pointer to RSNIE
+			rsnielen_cur_p = &pAd->StaCfg.RSNIE_Len;
+			pRsnIe = pAd->StaCfg.RSN_IE;
+
+			bMixCipher = pAd->StaCfg.bMixCipher;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// indicate primary RSNIE as WPA or WPA2
+	if ((AuthMode == Ndis802_11AuthModeWPA) ||
+		(AuthMode == Ndis802_11AuthModeWPAPSK) ||
+		(AuthMode == Ndis802_11AuthModeWPANone) ||
+		(AuthMode == Ndis802_11AuthModeWPA1WPA2) ||
+		(AuthMode == Ndis802_11AuthModeWPA1PSKWPA2PSK))
+		PrimaryRsnie = WpaIe;
+	else
+		PrimaryRsnie = Wpa2Ie;
+
+	{
+		// Build the primary RSNIE
+		// 1. insert cipher suite
+		RTMPInsertRsnIeCipher(pAd, PrimaryRsnie, WepStatus, bMixCipher, FlexibleCipher, pRsnIe, &p_offset);
+
+		// 2. insert AKM
+		RTMPInsertRsnIeAKM(pAd, PrimaryRsnie, AuthMode, apidx, pRsnIe, &p_offset);
+
+		// 3. insert capability
+		RTMPInsertRsnIeCap(pAd, PrimaryRsnie, apidx, pRsnIe, &p_offset);
+	}
+
+	// 4. update the RSNIE length
+	*rsnielen_cur_p = p_offset;
+
+	hex_dump("The primary RSNIE", pRsnIe, (*rsnielen_cur_p));
+
+
+}
+
+/*
+    ==========================================================================
+    Description:
+		Check whether the received frame is EAP frame.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+		pEntry			-	pointer to active entry
+		pData			-	the received frame
+		DataByteCount 	-	the received frame's length
+		FromWhichBSSID	-	indicate the interface index
+
+    Return:
+         TRUE 			-	This frame is EAP frame
+         FALSE 			-	otherwise
+    ==========================================================================
+*/
+BOOLEAN RTMPCheckWPAframe(
+    IN PRTMP_ADAPTER    pAd,
+    IN PMAC_TABLE_ENTRY	pEntry,
+    IN PUCHAR           pData,
+    IN ULONG            DataByteCount,
+	IN UCHAR			FromWhichBSSID)
+{
+	ULONG	Body_len;
+	BOOLEAN Cancelled;
+
+
+    if(DataByteCount < (LENGTH_802_1_H + LENGTH_EAPOL_H))
+        return FALSE;
+
+
+	// Skip LLC header
+    if (NdisEqualMemory(SNAP_802_1H, pData, 6) ||
+        // Cisco 1200 AP may send packet with SNAP_BRIDGE_TUNNEL
+        NdisEqualMemory(SNAP_BRIDGE_TUNNEL, pData, 6))
+    {
+        pData += 6;
+    }
+	// Skip 2-bytes EAPoL type
+    if (NdisEqualMemory(EAPOL, pData, 2))
+    {
+        pData += 2;
+    }
+    else
+        return FALSE;
+
+    switch (*(pData+1))
+    {
+        case EAPPacket:
+			Body_len = (*(pData+2)<<8) | (*(pData+3));
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAP-Packet frame, TYPE = 0, Length = %ld\n", Body_len));
+            break;
+        case EAPOLStart:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL-Start frame, TYPE = 1 \n"));
+			if (pEntry->EnqueueEapolStartTimerRunning != EAPOL_START_DISABLE)
+            {
+            	DBGPRINT(RT_DEBUG_TRACE, ("Cancel the EnqueueEapolStartTimerRunning \n"));
+                RTMPCancelTimer(&pEntry->EnqueueStartForPSKTimer, &Cancelled);
+                pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+            }
+            break;
+        case EAPOLLogoff:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOLLogoff frame, TYPE = 2 \n"));
+            break;
+        case EAPOLKey:
+			Body_len = (*(pData+2)<<8) | (*(pData+3));
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL-Key frame, TYPE = 3, Length = %ld\n", Body_len));
+            break;
+        case EAPOLASFAlert:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOLASFAlert frame, TYPE = 4 \n"));
+            break;
+        default:
+            return FALSE;
+
+    }
+    return TRUE;
+}
+
+
+/*
+    ==========================================================================
+    Description:
+        ENCRYPT AES GTK before sending in EAPOL frame.
+        AES GTK length = 128 bit,  so fix blocks for aes-key-wrap as 2 in this function.
+        This function references to RFC 3394 for aes key wrap algorithm.
+    Return:
+    ==========================================================================
+*/
+VOID AES_GTK_KEY_WRAP(
+    IN UCHAR    *key,
+    IN UCHAR    *plaintext,
+    IN UCHAR    p_len,
+    OUT UCHAR   *ciphertext)
+{
+    UCHAR       A[8], BIN[16], BOUT[16];
+    UCHAR       R[512];
+    INT         num_blocks = p_len/8;   // unit:64bits
+    INT         i, j;
+    aes_context aesctx;
+    UCHAR       xor;
+
+    rtmp_aes_set_key(&aesctx, key, 128);
+
+    // Init IA
+    for (i = 0; i < 8; i++)
+        A[i] = 0xa6;
+
+    //Input plaintext
+    for (i = 0; i < num_blocks; i++)
+    {
+        for (j = 0 ; j < 8; j++)
+            R[8 * (i + 1) + j] = plaintext[8 * i + j];
+    }
+
+    // Key Mix
+    for (j = 0; j < 6; j++)
+    {
+        for(i = 1; i <= num_blocks; i++)
+        {
+            //phase 1
+            NdisMoveMemory(BIN, A, 8);
+            NdisMoveMemory(&BIN[8], &R[8 * i], 8);
+            rtmp_aes_encrypt(&aesctx, BIN, BOUT);
+
+            NdisMoveMemory(A, &BOUT[0], 8);
+            xor = num_blocks * j + i;
+            A[7] = BOUT[7] ^ xor;
+            NdisMoveMemory(&R[8 * i], &BOUT[8], 8);
+        }
+    }
+
+    // Output ciphertext
+    NdisMoveMemory(ciphertext, A, 8);
+
+    for (i = 1; i <= num_blocks; i++)
+    {
+        for (j = 0 ; j < 8; j++)
+            ciphertext[8 * i + j] = R[8 * i + j];
+    }
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Misc function to decrypt AES body
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+		This function references to	RFC	3394 for aes key unwrap algorithm.
+
+	========================================================================
+*/
+VOID	AES_GTK_KEY_UNWRAP(
+	IN	UCHAR	*key,
+	OUT	UCHAR	*plaintext,
+	IN	UCHAR    c_len,
+	IN	UCHAR	*ciphertext)
+
+{
+	UCHAR       A[8], BIN[16], BOUT[16];
+	UCHAR       xor;
+	INT         i, j;
+	aes_context aesctx;
+	UCHAR       *R;
+	INT         num_blocks = c_len/8;	// unit:64bits
+
+
+	os_alloc_mem(NULL, (PUCHAR *)&R, 512);
+
+	if (R == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("!!!AES_GTK_KEY_UNWRAP: no memory!!!\n"));
+        return;
+    } /* End of if */
+
+	// Initialize
+	NdisMoveMemory(A, ciphertext, 8);
+	//Input plaintext
+	for(i = 0; i < (c_len-8); i++)
+	{
+		R[ i] = ciphertext[i + 8];
+	}
+
+	rtmp_aes_set_key(&aesctx, key, 128);
+
+	for(j = 5; j >= 0; j--)
+	{
+		for(i = (num_blocks-1); i > 0; i--)
+		{
+			xor = (num_blocks -1 )* j + i;
+			NdisMoveMemory(BIN, A, 8);
+			BIN[7] = A[7] ^ xor;
+			NdisMoveMemory(&BIN[8], &R[(i-1)*8], 8);
+			rtmp_aes_decrypt(&aesctx, BIN, BOUT);
+			NdisMoveMemory(A, &BOUT[0], 8);
+			NdisMoveMemory(&R[(i-1)*8], &BOUT[8], 8);
+		}
+	}
+
+	// OUTPUT
+	for(i = 0; i < c_len; i++)
+	{
+		plaintext[i] = R[i];
+	}
+
+
+	os_free_mem(NULL, R);
+}
+
+/*
+    ==========================================================================
+    Description:
+		Report the EAP message type
+
+	Arguments:
+		msg		-	EAPOL_PAIR_MSG_1
+					EAPOL_PAIR_MSG_2
+					EAPOL_PAIR_MSG_3
+					EAPOL_PAIR_MSG_4
+					EAPOL_GROUP_MSG_1
+					EAPOL_GROUP_MSG_2
+
+    Return:
+         message type string
+
+    ==========================================================================
+*/
+CHAR *GetEapolMsgType(CHAR msg)
+{
+    if(msg == EAPOL_PAIR_MSG_1)
+        return "Pairwise Message 1";
+    else if(msg == EAPOL_PAIR_MSG_2)
+        return "Pairwise Message 2";
+	else if(msg == EAPOL_PAIR_MSG_3)
+        return "Pairwise Message 3";
+	else if(msg == EAPOL_PAIR_MSG_4)
+        return "Pairwise Message 4";
+	else if(msg == EAPOL_GROUP_MSG_1)
+        return "Group Message 1";
+	else if(msg == EAPOL_GROUP_MSG_2)
+        return "Group Message 2";
+    else
+    	return "Invalid Message";
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Check Sanity RSN IE of EAPoL message
+
+    Arguments:
+
+    Return Value:
+
+
+    ========================================================================
+*/
+BOOLEAN RTMPCheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	OUT	UCHAR			*Offset)
+{
+	PUCHAR              pVIE;
+	UCHAR               len;
+	PEID_STRUCT         pEid;
+	BOOLEAN				result = FALSE;
+
+	pVIE = pData;
+	len	 = DataLen;
+	*Offset = 0;
+
+	while (len > sizeof(RSNIE2))
+	{
+		pEid = (PEID_STRUCT) pVIE;
+		// WPA RSN IE
+		if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4)))
+		{
+			if ((pEntry->AuthMode == Ndis802_11AuthModeWPA || pEntry->AuthMode == Ndis802_11AuthModeWPAPSK) &&
+				(NdisEqualMemory(pVIE, pEntry->RSN_IE, pEntry->RSNIE_Len)) &&
+				(pEntry->RSNIE_Len == (pEid->Len + 2)))
+			{
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		// WPA2 RSN IE
+		else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3)))
+		{
+			if ((pEntry->AuthMode == Ndis802_11AuthModeWPA2 || pEntry->AuthMode == Ndis802_11AuthModeWPA2PSK) &&
+				(NdisEqualMemory(pVIE, pEntry->RSN_IE, pEntry->RSNIE_Len)) &&
+				(pEntry->RSNIE_Len == (pEid->Len + 2))/* ToDo-AlbertY for mesh*/)
+			{
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		else
+		{
+			break;
+		}
+
+		pVIE += (pEid->Len + 2);
+		len  -= (pEid->Len + 2);
+	}
+
+
+	return result;
+
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Parse KEYDATA field.  KEYDATA[] May contain 2 RSN IE and optionally GTK.
+    GTK  is encaptulated in KDE format at  p.83 802.11i D10
+
+    Arguments:
+
+    Return Value:
+
+    Note:
+        802.11i D10
+
+    ========================================================================
+*/
+BOOLEAN RTMPParseEapolKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			GroupKeyIndex,
+	IN	UCHAR			MsgType,
+	IN	BOOLEAN			bWPA2,
+	IN  MAC_TABLE_ENTRY *pEntry)
+{
+    PKDE_ENCAP          pKDE = NULL;
+    PUCHAR              pMyKeyData = pKeyData;
+    UCHAR               KeyDataLength = KeyDataLen;
+    UCHAR               GTKLEN = 0;
+	UCHAR				DefaultIdx = 0;
+	UCHAR				skip_offset;
+
+	// Verify The RSN IE contained in pairewise_msg_2 && pairewise_msg_3 and skip it
+	if (MsgType == EAPOL_PAIR_MSG_2 || MsgType == EAPOL_PAIR_MSG_3)
+    {
+		// Check RSN IE whether it is WPA2/WPA2PSK
+		if (!RTMPCheckRSNIE(pAd, pKeyData, KeyDataLen, pEntry, &skip_offset))
+		{
+			// send wireless event - for RSN IE different
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_RSNIE_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+        	DBGPRINT(RT_DEBUG_ERROR, ("RSN_IE Different in msg %d of 4-way handshake!\n", MsgType));
+			hex_dump("Receive RSN_IE ", pKeyData, KeyDataLen);
+			hex_dump("Desired RSN_IE ", pEntry->RSN_IE, pEntry->RSNIE_Len);
+
+			return FALSE;
+    	}
+    	else
+		{
+			if (bWPA2 && MsgType == EAPOL_PAIR_MSG_3)
+			{
+				// skip RSN IE
+				pMyKeyData += skip_offset;
+				KeyDataLength -= skip_offset;
+				DBGPRINT(RT_DEBUG_TRACE, ("RTMPParseEapolKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
+			}
+			else
+				return TRUE;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPParseEapolKeyData ==> KeyDataLength %d without RSN_IE \n", KeyDataLength));
+
+	// Parse EKD format in pairwise_msg_3_WPA2 && group_msg_1_WPA2
+	if (bWPA2 && (MsgType == EAPOL_PAIR_MSG_3 || MsgType == EAPOL_GROUP_MSG_1))
+	{
+		if (KeyDataLength >= 8)	// KDE format exclude GTK length
+    	{
+        	pKDE = (PKDE_ENCAP) pMyKeyData;
+
+
+			DefaultIdx = pKDE->GTKEncap.Kid;
+
+			// Sanity check - KED length
+			if (KeyDataLength < (pKDE->Len + 2))
+    		{
+        		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: The len from KDE is too short \n"));
+        		return FALSE;
+    		}
+
+			// Get GTK length - refer to IEEE 802.11i-2004 p.82
+			GTKLEN = pKDE->Len -6;
+			if (GTKLEN < LEN_AES_KEY)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key length is too short (%d) \n", GTKLEN));
+        		return FALSE;
+			}
+
+    	}
+		else
+    	{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR: KDE format length is too short \n"));
+	        return FALSE;
+    	}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK in KDE format ,DefaultKeyID=%d, KeyLen=%d \n", DefaultIdx, GTKLEN));
+		// skip it
+		pMyKeyData += 8;
+		KeyDataLength -= 8;
+
+	}
+	else if (!bWPA2 && MsgType == EAPOL_GROUP_MSG_1)
+	{
+		DefaultIdx = GroupKeyIndex;
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK DefaultKeyID=%d \n", DefaultIdx));
+	}
+
+	// Sanity check - shared key index must be 1 ~ 3
+	if (DefaultIdx < 1 || DefaultIdx > 3)
+    {
+     	DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key index(%d) is invalid in %s %s \n", DefaultIdx, ((bWPA2) ? "WPA2" : "WPA"), GetEapolMsgType(MsgType)));
+        return FALSE;
+    }
+
+
+#ifdef CONFIG_STA_SUPPORT
+	// Todo
+#endif // CONFIG_STA_SUPPORT //
+
+	return TRUE;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Construct EAPoL message for WPA handshaking
+		Its format is below,
+
+		+--------------------+
+		| Protocol Version	 |  1 octet
+		+--------------------+
+		| Protocol Type		 |	1 octet
+		+--------------------+
+		| Body Length		 |  2 octets
+		+--------------------+
+		| Descriptor Type	 |	1 octet
+		+--------------------+
+		| Key Information    |	2 octets
+		+--------------------+
+		| Key Length	     |  1 octet
+		+--------------------+
+		| Key Repaly Counter |	8 octets
+		+--------------------+
+		| Key Nonce		     |  32 octets
+		+--------------------+
+		| Key IV			 |  16 octets
+		+--------------------+
+		| Key RSC			 |  8 octets
+		+--------------------+
+		| Key ID or Reserved |	8 octets
+		+--------------------+
+		| Key MIC			 |	16 octets
+		+--------------------+
+		| Key Data Length	 |	2 octets
+		+--------------------+
+		| Key Data			 |	n octets
+		+--------------------+
+
+
+	Arguments:
+		pAd			Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ConstructEapolMsg(
+	IN 	PRTMP_ADAPTER    	pAd,
+    IN 	UCHAR				AuthMode,
+    IN 	UCHAR				WepStatus,
+    IN 	UCHAR				GroupKeyWepStatus,
+    IN 	UCHAR				MsgType,
+    IN	UCHAR				DefaultKeyIdx,
+    IN 	UCHAR				*ReplayCounter,
+	IN 	UCHAR				*KeyNonce,
+	IN	UCHAR				*TxRSC,
+	IN	UCHAR				*PTK,
+	IN	UCHAR				*GTK,
+	IN	UCHAR				*RSNIE,
+	IN	UCHAR				RSNIE_Len,
+    OUT PEAPOL_PACKET       pMsg)
+{
+	BOOLEAN	bWPA2 = FALSE;
+
+	// Choose WPA2 or not
+	if ((AuthMode == Ndis802_11AuthModeWPA2) || (AuthMode == Ndis802_11AuthModeWPA2PSK))
+		bWPA2 = TRUE;
+
+    // Init Packet and Fill header
+    pMsg->ProVer = EAPOL_VER;
+    pMsg->ProType = EAPOLKey;
+
+	// Default 95 bytes, the EAPoL-Key descriptor exclude Key-data field
+	pMsg->Body_Len[1] = LEN_EAPOL_KEY_MSG;
+
+	// Fill in EAPoL descriptor
+	if (bWPA2)
+		pMsg->KeyDesc.Type = WPA2_KEY_DESC;
+	else
+		pMsg->KeyDesc.Type = WPA1_KEY_DESC;
+
+	// Fill in Key information, refer to IEEE Std 802.11i-2004 page 78
+	// When either the pairwise or the group cipher is AES, the DESC_TYPE_AES(2) shall be used.
+	pMsg->KeyDesc.KeyInfo.KeyDescVer =
+        	(((WepStatus == Ndis802_11Encryption3Enabled) || (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	// Specify Key Type as Group(0) or Pairwise(1)
+	if (MsgType >= EAPOL_GROUP_MSG_1)
+		pMsg->KeyDesc.KeyInfo.KeyType = GROUPKEY;
+	else
+		pMsg->KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// Specify Key Index, only group_msg1_WPA1
+	if (!bWPA2 && (MsgType >= EAPOL_GROUP_MSG_1))
+		pMsg->KeyDesc.KeyInfo.KeyIndex = DefaultKeyIdx;
+
+	if (MsgType == EAPOL_PAIR_MSG_3)
+		pMsg->KeyDesc.KeyInfo.Install = 1;
+
+	if ((MsgType == EAPOL_PAIR_MSG_1) || (MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1))
+		pMsg->KeyDesc.KeyInfo.KeyAck = 1;
+
+	if (MsgType != EAPOL_PAIR_MSG_1)
+		pMsg->KeyDesc.KeyInfo.KeyMic = 1;
+
+	if ((bWPA2 && (MsgType >= EAPOL_PAIR_MSG_3)) || (!bWPA2 && (MsgType >= EAPOL_GROUP_MSG_1)))
+    {
+       	pMsg->KeyDesc.KeyInfo.Secure = 1;
+    }
+
+	if (bWPA2 && ((MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1)))
+    {
+        pMsg->KeyDesc.KeyInfo.EKD_DL = 1;
+    }
+
+	// key Information element has done.
+	*(USHORT *)(&pMsg->KeyDesc.KeyInfo) = cpu2le16(*(USHORT *)(&pMsg->KeyDesc.KeyInfo));
+
+	// Fill in Key Length
+#if 0
+	if (bWPA2)
+	{
+		// In WPA2 mode, the field indicates the length of pairwise key cipher,
+		// so only pairwise_msg_1 and pairwise_msg_3 need to fill.
+		if ((MsgType == EAPOL_PAIR_MSG_1) || (MsgType == EAPOL_PAIR_MSG_3))
+			pMsg->KeyDesc.KeyLength[1] = ((WepStatus == Ndis802_11Encryption2Enabled) ? LEN_TKIP_KEY : LEN_AES_KEY);
+	}
+	else if (!bWPA2)
+#endif
+	{
+		if (MsgType >= EAPOL_GROUP_MSG_1)
+		{
+			// the length of group key cipher
+			pMsg->KeyDesc.KeyLength[1] = ((GroupKeyWepStatus == Ndis802_11Encryption2Enabled) ? TKIP_GTK_LENGTH : LEN_AES_KEY);
+		}
+		else
+		{
+			// the length of pairwise key cipher
+			pMsg->KeyDesc.KeyLength[1] = ((WepStatus == Ndis802_11Encryption2Enabled) ? LEN_TKIP_KEY : LEN_AES_KEY);
+		}
+	}
+
+ 	// Fill in replay counter
+    NdisMoveMemory(pMsg->KeyDesc.ReplayCounter, ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Fill Key Nonce field
+	// ANonce : pairwise_msg1 & pairwise_msg3
+	// SNonce : pairwise_msg2
+	// GNonce : group_msg1_wpa1
+	if ((MsgType <= EAPOL_PAIR_MSG_3) || ((!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))))
+    	NdisMoveMemory(pMsg->KeyDesc.KeyNonce, KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Fill key IV - WPA2 as 0, WPA1 as random
+	if (!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))
+	{
+		// Suggest IV be random number plus some number,
+		NdisMoveMemory(pMsg->KeyDesc.KeyIv, &KeyNonce[16], LEN_KEY_DESC_IV);
+        pMsg->KeyDesc.KeyIv[15] += 2;
+	}
+
+    // Fill Key RSC field
+    // It contains the RSC for the GTK being installed.
+	if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2) || (MsgType == EAPOL_GROUP_MSG_1))
+	{
+        NdisMoveMemory(pMsg->KeyDesc.KeyRsc, TxRSC, 6);
+	}
+
+	// Clear Key MIC field for MIC calculation later
+    NdisZeroMemory(pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+	ConstructEapolKeyData(pAd,
+						  AuthMode,
+						  WepStatus,
+						  GroupKeyWepStatus,
+						  MsgType,
+						  DefaultKeyIdx,
+						  bWPA2,
+						  PTK,
+						  GTK,
+						  RSNIE,
+						  RSNIE_Len,
+						  pMsg);
+
+	// Calculate MIC and fill in KeyMic Field except Pairwise Msg 1.
+	if (MsgType != EAPOL_PAIR_MSG_1)
+	{
+		CalculateMIC(pAd, WepStatus, PTK, pMsg);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> ConstructEapolMsg for %s %s\n", ((bWPA2) ? "WPA2" : "WPA"), GetEapolMsgType(MsgType)));
+	DBGPRINT(RT_DEBUG_TRACE, ("	     Body length = %d \n", pMsg->Body_Len[1]));
+	DBGPRINT(RT_DEBUG_TRACE, ("	     Key length  = %d \n", pMsg->KeyDesc.KeyLength[1]));
+
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Construct the Key Data field of EAPoL message
+
+	Arguments:
+		pAd			Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ConstructEapolKeyData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			AuthMode,
+	IN	UCHAR			WepStatus,
+	IN	UCHAR			GroupKeyWepStatus,
+	IN 	UCHAR			MsgType,
+	IN	UCHAR			DefaultKeyIdx,
+	IN	BOOLEAN			bWPA2Capable,
+	IN	UCHAR			*PTK,
+	IN	UCHAR			*GTK,
+	IN	UCHAR			*RSNIE,
+	IN	UCHAR			RSNIE_LEN,
+	OUT PEAPOL_PACKET   pMsg)
+{
+	UCHAR		*mpool, *Key_Data, *Rc4GTK;
+	UCHAR       ekey[(LEN_KEY_DESC_IV+LEN_EAP_EK)];
+	UCHAR		data_offset;
+
+
+	if (MsgType == EAPOL_PAIR_MSG_1 || MsgType == EAPOL_PAIR_MSG_4 || MsgType == EAPOL_GROUP_MSG_2)
+		return;
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1500);
+
+    if (mpool == NULL)
+		return;
+
+	/* Rc4GTK Len = 512 */
+	Rc4GTK = (UCHAR *) ROUND_UP(mpool, 4);
+	/* Key_Data Len = 512 */
+	Key_Data = (UCHAR *) ROUND_UP(Rc4GTK + 512, 4);
+
+	NdisZeroMemory(Key_Data, 512);
+	pMsg->KeyDesc.KeyDataLen[1] = 0;
+	data_offset = 0;
+
+	// Encapsulate RSNIE in pairwise_msg2 & pairwise_msg3
+	if (RSNIE_LEN && ((MsgType == EAPOL_PAIR_MSG_2) || (MsgType == EAPOL_PAIR_MSG_3)))
+	{
+		if (bWPA2Capable)
+			Key_Data[data_offset + 0] = IE_WPA2;
+		else
+			Key_Data[data_offset + 0] = IE_WPA;
+
+        Key_Data[data_offset + 1] = RSNIE_LEN;
+		NdisMoveMemory(&Key_Data[data_offset + 2], RSNIE, RSNIE_LEN);
+		data_offset += (2 + RSNIE_LEN);
+	}
+
+	// Encapsulate KDE format in pairwise_msg3_WPA2 & group_msg1_WPA2
+	if (bWPA2Capable && ((MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1)))
+	{
+		// Key Data Encapsulation (KDE) format - 802.11i-2004  Figure-43w and Table-20h
+        Key_Data[data_offset + 0] = 0xDD;
+
+		if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+		{
+			Key_Data[data_offset + 1] = 0x16;// 4+2+16(OUI+DataType+DataField)
+		}
+		else
+		{
+			Key_Data[data_offset + 1] = 0x26;// 4+2+32(OUI+DataType+DataField)
+		}
+
+        Key_Data[data_offset + 2] = 0x00;
+        Key_Data[data_offset + 3] = 0x0F;
+        Key_Data[data_offset + 4] = 0xAC;
+        Key_Data[data_offset + 5] = 0x01;
+
+		// GTK KDE format - 802.11i-2004  Figure-43x
+        Key_Data[data_offset + 6] = (DefaultKeyIdx & 0x03);
+        Key_Data[data_offset + 7] = 0x00;	// Reserved Byte
+
+		data_offset += 8;
+	}
+
+
+	// Encapsulate GTK and encrypt the key-data field with KEK.
+	// Only for pairwise_msg3_WPA2 and group_msg1
+	if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2Capable) || (MsgType == EAPOL_GROUP_MSG_1))
+	{
+		// Fill in GTK
+		if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+		{
+			NdisMoveMemory(&Key_Data[data_offset], GTK, LEN_AES_KEY);
+			data_offset += LEN_AES_KEY;
+		}
+		else
+		{
+			NdisMoveMemory(&Key_Data[data_offset], GTK, TKIP_GTK_LENGTH);
+			data_offset += TKIP_GTK_LENGTH;
+		}
+
+		// Still dont know why, but if not append will occur "GTK not include in MSG3"
+		// Patch for compatibility between zero config and funk
+		if (MsgType == EAPOL_PAIR_MSG_3 && bWPA2Capable)
+		{
+			if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+			{
+				Key_Data[data_offset + 0] = 0xDD;
+				Key_Data[data_offset + 1] = 0;
+				data_offset += 2;
+			}
+			else
+			{
+				Key_Data[data_offset + 0] = 0xDD;
+				Key_Data[data_offset + 1] = 0;
+				Key_Data[data_offset + 2] = 0;
+				Key_Data[data_offset + 3] = 0;
+				Key_Data[data_offset + 4] = 0;
+				Key_Data[data_offset + 5] = 0;
+				data_offset += 6;
+			}
+		}
+
+		// Encrypt the data material in key data field
+		if (WepStatus == Ndis802_11Encryption3Enabled)
+		{
+			AES_GTK_KEY_WRAP(&PTK[16], Key_Data, data_offset, Rc4GTK);
+            // AES wrap function will grow 8 bytes in length
+            data_offset += 8;
+		}
+		else
+		{
+			// PREPARE Encrypted  "Key DATA" field.  (Encrypt GTK with RC4, usinf PTK[16]->[31] as Key, IV-field as IV)
+			// put TxTsc in Key RSC field
+			pAd->PrivateInfo.FCSCRC32 = PPPINITFCS32;   //Init crc32.
+
+			// ekey is the contanetion of IV-field, and PTK[16]->PTK[31]
+			NdisMoveMemory(ekey, pMsg->KeyDesc.KeyIv, LEN_KEY_DESC_IV);
+			NdisMoveMemory(&ekey[LEN_KEY_DESC_IV], &PTK[16], LEN_EAP_EK);
+			ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, ekey, sizeof(ekey));  //INIT SBOX, KEYLEN+3(IV)
+			pAd->PrivateInfo.FCSCRC32 = RTMP_CALC_FCS32(pAd->PrivateInfo.FCSCRC32, Key_Data, data_offset);
+			WPAARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, Rc4GTK, Key_Data, data_offset);
+		}
+
+		NdisMoveMemory(pMsg->KeyDesc.KeyData, Rc4GTK, data_offset);
+	}
+	else
+	{
+		NdisMoveMemory(pMsg->KeyDesc.KeyData, Key_Data, data_offset);
+	}
+
+	// set key data length field and total length
+	pMsg->KeyDesc.KeyDataLen[1] = data_offset;
+    pMsg->Body_Len[1] += data_offset;
+
+	os_free_mem(pAd, mpool);
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calcaulate MIC. It is used during 4-ways handsharking.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+    	PeerWepStatus	-	indicate the encryption type
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	CalculateMIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			*PTK,
+	OUT PEAPOL_PACKET   pMsg)
+{
+    UCHAR   *OutBuffer;
+	ULONG	FrameLen = 0;
+	UCHAR	mic[LEN_KEY_DESC_MIC];
+	UCHAR	digest[80];
+
+	// allocate memory for MIC calculation
+	os_alloc_mem(pAd, (PUCHAR *)&OutBuffer, 512);
+
+    if (OutBuffer == NULL)
+    {
+		DBGPRINT(RT_DEBUG_ERROR, ("!!!CalculateMIC: no memory!!!\n"));
+		return;
+    }
+
+	// make a frame for calculating MIC.
+    MakeOutgoingFrame(OutBuffer,            	&FrameLen,
+                      pMsg->Body_Len[1] + 4,  	pMsg,
+                      END_OF_ARGS);
+
+	NdisZeroMemory(mic, sizeof(mic));
+
+	// Calculate MIC
+    if (PeerWepStatus == Ndis802_11Encryption3Enabled)
+ 	{
+		HMAC_SHA1(OutBuffer,  FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(PTK,  LEN_EAP_MICK, OutBuffer, FrameLen, mic);
+	}
+
+	// store the calculated MIC
+	NdisMoveMemory(pMsg->KeyDesc.KeyMic, mic, LEN_KEY_DESC_MIC);
+
+	os_free_mem(pAd, OutBuffer);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Some received frames can't decrypt by Asic, so decrypt them by software.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+    	PeerWepStatus	-	indicate the encryption type
+
+	Return Value:
+		NDIS_STATUS_SUCCESS		-	decryption successful
+		NDIS_STATUS_FAILURE		-	decryption failure
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPSoftDecryptBroadCastData(
+	IN	PRTMP_ADAPTER					pAd,
+	IN	RX_BLK							*pRxBlk,
+	IN  NDIS_802_11_ENCRYPTION_STATUS 	GroupCipher,
+	IN  PCIPHER_KEY						pShard_key)
+{
+	PRXWI_STRUC			pRxWI = pRxBlk->pRxWI;
+
+
+
+	// handle WEP decryption
+	if (GroupCipher == Ndis802_11Encryption1Enabled)
+    {
+		if (RTMPSoftDecryptWEP(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount, pShard_key))
+		{
+
+			//Minus IV[4] & ICV[4]
+			pRxWI->MPDUtotalByteCount -= 8;
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : Software decrypt WEP data fails.\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+		}
+	}
+	// handle TKIP decryption
+	else if (GroupCipher == Ndis802_11Encryption2Enabled)
+	{
+		if (RTMPSoftDecryptTKIP(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount, 0, pShard_key))
+		{
+
+			//Minus 8 bytes MIC, 8 bytes IV/EIV, 4 bytes ICV
+			pRxWI->MPDUtotalByteCount -= 20;
+		}
+        else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : RTMPSoftDecryptTKIP Failed\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+        }
+	}
+	// handle AES decryption
+	else if (GroupCipher == Ndis802_11Encryption3Enabled)
+	{
+		if (RTMPSoftDecryptAES(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount , pShard_key))
+		{
+
+			//8 bytes MIC, 8 bytes IV/EIV (CCMP Header)
+			pRxWI->MPDUtotalByteCount -= 16;
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : RTMPSoftDecryptAES Failed\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+		}
+	}
+	else
+	{
+		// give up this frame
+		return NDIS_STATUS_FAILURE;
+	}
+
+	return NDIS_STATUS_SUCCESS;
+
+}
+
diff --git a/drivers/staging/rt2870/common/dfs.c b/drivers/staging/rt2870/common/dfs.c
new file mode 100644
index 00000000000..23cf1510e2d
--- /dev/null
+++ b/drivers/staging/rt2870/common/dfs.c
@@ -0,0 +1,453 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    ap_dfs.c
+
+    Abstract:
+    Support DFS function.
+
+    Revision History:
+    Who       When            What
+    --------  ----------      ----------------------------------------------
+    Fonchi    03-12-2007      created
+*/
+
+#include "../rt_config.h"
+
+typedef struct _RADAR_DURATION_TABLE
+{
+	ULONG RDDurRegion;
+	ULONG RadarSignalDuration;
+	ULONG Tolerance;
+} RADAR_DURATION_TABLE, *PRADAR_DURATION_TABLE;
+
+
+static UCHAR RdIdleTimeTable[MAX_RD_REGION][4] =
+{
+	{9, 250, 250, 250},		// CE
+	{4, 250, 250, 250},		// FCC
+	{4, 250, 250, 250},		// JAP
+	{15, 250, 250, 250},	// JAP_W53
+	{4, 250, 250, 250}		// JAP_W56
+};
+
+/*
+	========================================================================
+
+	Routine Description:
+		Bbp Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID BbpRadarDetectionStart(
+	IN PRTMP_ADAPTER pAd)
+{
+	UINT8 RadarPeriod;
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 114, 0x02);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 121, 0x20);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 122, 0x00);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 123, 0x08/*0x80*/);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 124, 0x28);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 125, 0xff);
+
+#if 0
+	// toggle Rx enable bit for radar detection.
+	// it's Andy's recommand.
+	{
+		UINT32 Value;
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value |= (0x1 << 3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	Value &= ~(0x1 << 3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+#endif
+	RadarPeriod = ((UINT)RdIdleTimeTable[pAd->CommonCfg.RadarDetect.RDDurRegion][0] + (UINT)pAd->CommonCfg.RadarDetect.DfsSessionTime) < 250 ?
+			(RdIdleTimeTable[pAd->CommonCfg.RadarDetect.RDDurRegion][0] + pAd->CommonCfg.RadarDetect.DfsSessionTime) : 250;
+
+	RTMP_IO_WRITE8(pAd, 0x7020, 0x1d);
+	RTMP_IO_WRITE8(pAd, 0x7021, 0x40);
+
+	RadarDetectionStart(pAd, 0, RadarPeriod);
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Bbp Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID BbpRadarDetectionStop(
+	IN PRTMP_ADAPTER pAd)
+{
+	RTMP_IO_WRITE8(pAd, 0x7020, 0x1d);
+	RTMP_IO_WRITE8(pAd, 0x7021, 0x60);
+
+	RadarDetectionStop(pAd);
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID RadarDetectionStart(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN CTSProtect,
+	IN UINT8 CTSPeriod)
+{
+	UINT8 DfsActiveTime = (pAd->CommonCfg.RadarDetect.DfsSessionTime & 0x1f);
+	UINT8 CtsProtect = (CTSProtect == 1) ? 0x02 : 0x01; // CTS protect.
+
+	if (CTSProtect != 0)
+	{
+		switch(pAd->CommonCfg.RadarDetect.RDDurRegion)
+		{
+		case FCC:
+		case JAP_W56:
+			CtsProtect = 0x03;
+			break;
+
+		case CE:
+		case JAP_W53:
+		default:
+			CtsProtect = 0x02;
+			break;
+		}
+	}
+	else
+		CtsProtect = 0x01;
+
+
+	// send start-RD with CTS protection command to MCU
+	// highbyte [7]		reserve
+	// highbyte [6:5]	0x: stop Carrier/Radar detection
+	// highbyte [10]:	Start Carrier/Radar detection without CTS protection, 11: Start Carrier/Radar detection with CTS protection
+	// highbyte [4:0]	Radar/carrier detection duration. In 1ms.
+
+	// lowbyte [7:0]	Radar/carrier detection period, in 1ms.
+	AsicSendCommandToMcu(pAd, 0x60, 0xff, CTSPeriod, DfsActiveTime | (CtsProtect << 5));
+	//AsicSendCommandToMcu(pAd, 0x63, 0xff, 10, 0);
+
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		TRUE	Found radar signal
+		FALSE	Not found radar signal
+
+	========================================================================
+*/
+VOID RadarDetectionStop(
+	IN PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE,("RadarDetectionStop.\n"));
+	AsicSendCommandToMcu(pAd, 0x60, 0xff, 0x00, 0x00);	// send start-RD with CTS protection command to MCU
+
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar channel check routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		TRUE	need to do radar detect
+		FALSE	need not to do radar detect
+
+	========================================================================
+*/
+BOOLEAN RadarChannelCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Ch)
+{
+#if 1
+	INT		i;
+	BOOLEAN result = FALSE;
+
+	for (i=0; i<pAd->ChannelListNum; i++)
+	{
+		if (Ch == pAd->ChannelList[i].Channel)
+		{
+			result = pAd->ChannelList[i].DfsReq;
+			break;
+		}
+	}
+
+	return result;
+#else
+	INT		i;
+	UCHAR	Channel[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+
+	for (i=0; i<15; i++)
+	{
+		if (Ch == Channel[i])
+		{
+			break;
+		}
+	}
+
+	if (i != 15)
+		return TRUE;
+	else
+		return FALSE;
+#endif
+}
+
+ULONG JapRadarType(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG		i;
+	const UCHAR	Channel[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+
+	if (pAd->CommonCfg.RadarDetect.RDDurRegion != JAP)
+	{
+		return pAd->CommonCfg.RadarDetect.RDDurRegion;
+	}
+
+	for (i=0; i<15; i++)
+	{
+		if (pAd->CommonCfg.Channel == Channel[i])
+		{
+			break;
+		}
+	}
+
+	if (i < 4)
+		return JAP_W53;
+	else if (i < 15)
+		return JAP_W56;
+	else
+		return JAP; // W52
+
+}
+
+ULONG RTMPBbpReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UINT8 byteValue = 0;
+	ULONG result;
+
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R115, &byteValue);
+
+	result = 0;
+	switch (byteValue)
+	{
+	case 1: // radar signal detected by pulse mode.
+	case 2: // radar signal detected by width mode.
+		result = RTMPReadRadarDuration(pAd);
+		break;
+
+	case 0: // No radar signal.
+	default:
+
+		result = 0;
+		break;
+	}
+
+	return result;
+}
+
+ULONG RTMPReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	ULONG result = 0;
+
+#ifdef DFS_SUPPORT
+	UINT8 duration1 = 0, duration2 = 0, duration3 = 0;
+
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R116, &duration1);
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R117, &duration2);
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R118, &duration3);
+	result = (duration1 << 16) + (duration2 << 8) + duration3;
+#endif // DFS_SUPPORT //
+
+	return result;
+
+}
+
+VOID RTMPCleanRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	return;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Radar wave detection. The API should be invoke each second.
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID ApRadarDetectPeriodic(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT	i;
+
+	pAd->CommonCfg.RadarDetect.InServiceMonitorCount++;
+
+	for (i=0; i<pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].RemainingTimeForUse > 0)
+		{
+			pAd->ChannelList[i].RemainingTimeForUse --;
+			if ((pAd->Mlme.PeriodicRound%5) == 0)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("RadarDetectPeriodic - ch=%d, RemainingTimeForUse=%d\n", pAd->ChannelList[i].Channel, pAd->ChannelList[i].RemainingTimeForUse));
+			}
+		}
+	}
+
+	//radar detect
+	if ((pAd->CommonCfg.Channel > 14)
+		&& (pAd->CommonCfg.bIEEE80211H == 1)
+		&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+	{
+		RadarDetectPeriodic(pAd);
+	}
+
+	return;
+}
+
+// Periodic Radar detection, switch channel will occur in RTMPHandleTBTTInterrupt()
+// Before switch channel, driver needs doing channel switch announcement.
+VOID RadarDetectPeriodic(
+	IN PRTMP_ADAPTER	pAd)
+{
+	// need to check channel availability, after switch channel
+	if (pAd->CommonCfg.RadarDetect.RDMode != RD_SILENCE_MODE)
+			return;
+
+	// channel availability check time is 60sec, use 65 for assurance
+	if (pAd->CommonCfg.RadarDetect.RDCount++ > pAd->CommonCfg.RadarDetect.ChMovingTime)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Not found radar signal, start send beacon and radar detection in service monitor\n\n"));
+			BbpRadarDetectionStop(pAd);
+		AsicEnableBssSync(pAd);
+		pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+
+
+		return;
+	}
+
+	return;
+}
+
+
+/*
+    ==========================================================================
+    Description:
+		change channel moving time for DFS testing.
+
+	Arguments:
+	    pAdapter                    Pointer to our adapter
+	    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 set ChMovTime=[value]
+    ==========================================================================
+*/
+INT Set_ChMovingTime_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	UINT8 Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	pAd->CommonCfg.RadarDetect.ChMovingTime = Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s:: %d\n", __FUNCTION__,
+		pAd->CommonCfg.RadarDetect.ChMovingTime));
+
+	return TRUE;
+}
+
+INT Set_LongPulseRadarTh_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	UINT8 Value;
+
+	Value = simple_strtol(arg, 0, 10) > 10 ? 10 : simple_strtol(arg, 0, 10);
+
+	pAd->CommonCfg.RadarDetect.LongPulseRadarTh = Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s:: %d\n", __FUNCTION__,
+		pAd->CommonCfg.RadarDetect.LongPulseRadarTh));
+
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2870/common/eeprom.c b/drivers/staging/rt2870/common/eeprom.c
new file mode 100644
index 00000000000..33f16ed9c49
--- /dev/null
+++ b/drivers/staging/rt2870/common/eeprom.c
@@ -0,0 +1,254 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	eeprom.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#include "../rt_config.h"
+
+#if 0
+#define EEPROM_SIZE								0x200
+#define NVRAM_OFFSET			0x30000
+#define RF_OFFSET				0x40000
+
+static UCHAR init_flag = 0;
+static PUCHAR nv_ee_start = 0;
+
+static UCHAR EeBuffer[EEPROM_SIZE];
+#endif
+// IRQL = PASSIVE_LEVEL
+VOID RaiseClock(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  UINT32 *x)
+{
+    *x = *x | EESK;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, *x);
+    RTMPusecDelay(1);				// Max frequency = 1MHz in Spec. definition
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID LowerClock(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  UINT32 *x)
+{
+    *x = *x & ~EESK;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, *x);
+    RTMPusecDelay(1);
+}
+
+// IRQL = PASSIVE_LEVEL
+USHORT ShiftInBits(
+    IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32		x,i;
+	USHORT      data=0;
+
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~( EEDO | EEDI);
+
+    for(i=0; i<16; i++)
+    {
+        data = data << 1;
+        RaiseClock(pAd, &x);
+
+        RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+        x &= ~(EEDI);
+        if(x & EEDO)
+            data |= 1;
+
+        LowerClock(pAd, &x);
+    }
+
+    return data;
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID ShiftOutBits(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT data,
+    IN  USHORT count)
+{
+    UINT32       x,mask;
+
+    mask = 0x01 << (count - 1);
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~(EEDO | EEDI);
+
+    do
+    {
+        x &= ~EEDI;
+        if(data & mask)		x |= EEDI;
+
+        RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+        RaiseClock(pAd, &x);
+        LowerClock(pAd, &x);
+
+        mask = mask >> 1;
+    } while(mask);
+
+    x &= ~EEDI;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID EEpromCleanup(
+    IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32 x;
+
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~(EECS | EEDI);
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+    RaiseClock(pAd, &x);
+    LowerClock(pAd, &x);
+}
+
+VOID EWEN(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32	x;
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and six pulse in that order
+    ShiftOutBits(pAd, EEPROM_EWEN_OPCODE, 5);
+    ShiftOutBits(pAd, 0, 6);
+
+    EEpromCleanup(pAd);
+}
+
+VOID EWDS(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32	x;
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and six pulse in that order
+    ShiftOutBits(pAd, EEPROM_EWDS_OPCODE, 5);
+    ShiftOutBits(pAd, 0, 6);
+
+    EEpromCleanup(pAd);
+}
+
+// IRQL = PASSIVE_LEVEL
+USHORT RTMP_EEPROM_READ16(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT Offset)
+{
+    UINT32		x;
+    USHORT		data;
+
+    Offset /= 2;
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and register number in that order
+    ShiftOutBits(pAd, EEPROM_READ_OPCODE, 3);
+    ShiftOutBits(pAd, Offset, pAd->EEPROMAddressNum);
+
+    // Now read the data (16 bits) in from the selected EEPROM word
+    data = ShiftInBits(pAd);
+
+    EEpromCleanup(pAd);
+
+    return data;
+}	//ReadEEprom
+
+VOID RTMP_EEPROM_WRITE16(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT Offset,
+    IN  USHORT Data)
+{
+    UINT32 x;
+
+	Offset /= 2;
+
+	EWEN(pAd);
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode ,register number and data in that order
+    ShiftOutBits(pAd, EEPROM_WRITE_OPCODE, 3);
+    ShiftOutBits(pAd, Offset, pAd->EEPROMAddressNum);
+	ShiftOutBits(pAd, Data, 16);		// 16-bit access
+
+    // read DO status
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+	EEpromCleanup(pAd);
+
+	RTMPusecDelay(10000);	//delay for twp(MAX)=10ms
+
+	EWDS(pAd);
+
+    EEpromCleanup(pAd);
+}
+
diff --git a/drivers/staging/rt2870/common/firmware.h b/drivers/staging/rt2870/common/firmware.h
new file mode 100644
index 00000000000..a40669895ef
--- /dev/null
+++ b/drivers/staging/rt2870/common/firmware.h
@@ -0,0 +1,558 @@
+/*
+ Copyright (c) 2007, Ralink Technology Corporation
+ All rights reserved.
+
+ Redistribution.  Redistribution and use in binary form, without
+ modification, are permitted provided that the following conditions are
+ met:
+
+ 	* Redistributions must reproduce the above copyright notice and the
+ 	following disclaimer in the documentation and/or other materials
+ 	provided with the distribution.
+ 	* Neither the name of Ralink Technology Corporation nor the names of its
+ 	suppliers may be used to endorse or promote products derived from this
+ 	software without specific prior written permission.
+ 	* No reverse engineering, decompilation, or disassembly of this software
+ 	is permitted.
+
+ Limited patent license. Ralink Technology Corporation grants a world-wide,
+ royalty-free, non-exclusive license under patents it now or hereafter
+ owns or controls to make, have made, use, import, offer to sell and
+ sell ("Utilize") this software, but solely to the extent that any
+ such patent is necessary to Utilize the software alone, or in
+ combination with an operating system licensed under an approved Open
+ Source license as listed by the Open Source Initiative at
+ http://opensource.org/licenses.  The patent license shall not apply to
+ any other combinations which include this software.  No hardware per
+ se is licensed hereunder.
+
+ DISCLAIMER.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
+ BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
+ USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+ DAMAGE.
+*/
+/* AUTO GEN PLEASE DO NOT MODIFY IT */
+/* AUTO GEN PLEASE DO NOT MODIFY IT */
+
+
+UCHAR FirmwareImage [] = {
+0xff, 0xff, 0xff, 0x02, 0x10, 0x28, 0x02, 0x10, 0x32, 0x02, 0x10, 0x78, 0x02, 0x12, 0x79, 0x02,
+0x12, 0x7a, 0x02, 0x12, 0x99, 0x02, 0x12, 0x9e, 0x12, 0x12, 0x9a, 0x22, 0x02, 0x16, 0x36, 0x02,
+0x17, 0x0c, 0x02, 0x13, 0x89, 0x02, 0x12, 0x9f, 0x30, 0x05, 0x06, 0x20, 0x0d, 0x03, 0x12, 0x17,
+0xae, 0x22, 0x90, 0x01, 0x8c, 0xe0, 0x30, 0xe3, 0x1b, 0xe5, 0x4c, 0x30, 0xe0, 0x04, 0x7f, 0x40,
+0x80, 0x02, 0x7f, 0x00, 0x90, 0x10, 0x2f, 0xef, 0xf0, 0x90, 0x01, 0x8c, 0x74, 0x08, 0xf0, 0xe4,
+0x90, 0x01, 0xa7, 0xf0, 0x90, 0x01, 0x8c, 0xe0, 0x30, 0xe0, 0x1c, 0x90, 0x01, 0x80, 0xe0, 0xb4,
+0x02, 0x15, 0xa3, 0xe0, 0xb4, 0x01, 0x10, 0x90, 0x01, 0x84, 0xe0, 0xb4, 0x81, 0x09, 0x90, 0x01,
+0x8c, 0x74, 0x01, 0xf0, 0x12, 0x0d, 0xc8, 0x22, 0x90, 0x04, 0x14, 0xe0, 0x20, 0xe7, 0x03, 0x02,
+0x12, 0x6e, 0x90, 0x70, 0x12, 0xe0, 0xf5, 0x56, 0x90, 0x04, 0x04, 0xe0, 0x12, 0x0a, 0x9d, 0x10,
+0xb7, 0x31, 0x10, 0xe2, 0x50, 0x11, 0x08, 0x51, 0x11, 0x13, 0x52, 0x11, 0x13, 0x53, 0x11, 0x13,
+0x54, 0x11, 0x54, 0x55, 0x11, 0x79, 0x70, 0x11, 0xa4, 0x71, 0x11, 0xd2, 0x72, 0x12, 0x25, 0x73,
+0x12, 0x46, 0x80, 0x00, 0x00, 0x12, 0x6e, 0x20, 0x02, 0x03, 0x30, 0x03, 0x1d, 0x7d, 0x02, 0xaf,
+0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5,
+0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0x6e, 0x02, 0x12, 0x67, 0x85, 0x56, 0x41, 0xd2, 0x02, 0x02,
+0x12, 0x6e, 0x90, 0x70, 0x10, 0xe0, 0x54, 0x7f, 0x64, 0x02, 0x60, 0x03, 0x02, 0x12, 0x6e, 0x90,
+0x70, 0x11, 0xe0, 0x64, 0x08, 0x60, 0x08, 0xe0, 0x64, 0x20, 0x60, 0x03, 0x02, 0x12, 0x6e, 0x75,
+0x4e, 0x03, 0x75, 0x4f, 0x20, 0x02, 0x12, 0x6e, 0x90, 0x70, 0x11, 0xe0, 0x24, 0xff, 0x92, 0x47,
+0x02, 0x12, 0x6e, 0x90, 0x04, 0x04, 0xe0, 0x25, 0xe0, 0x24, 0x5d, 0xf5, 0x57, 0x90, 0x70, 0x10,
+0xe0, 0xff, 0x74, 0x47, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0x90, 0x70, 0x11, 0xe0, 0xff, 0x74,
+0x48, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04,
+0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12,
+0x6e, 0x02, 0x12, 0x67, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x0b, 0xe5, 0x47, 0x64, 0x08, 0x60, 0x05,
+0xe5, 0x47, 0xb4, 0x09, 0x08, 0x90, 0x70, 0x11, 0xe0, 0x54, 0x0f, 0xf5, 0x3a, 0xe4, 0xfd, 0xaf,
+0x56, 0x12, 0x0b, 0x91, 0xd2, 0x04, 0x02, 0x12, 0x6e, 0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90, 0x70,
+0x11, 0xe0, 0xfd, 0xed, 0xf8, 0xe6, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04,
+0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12,
+0x6e, 0x02, 0x12, 0x67, 0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90, 0x70, 0x11, 0xe0, 0xfd, 0xed, 0xf5,
+0x82, 0x8e, 0x83, 0xe0, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74,
+0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0x6e, 0x02,
+0x12, 0x67, 0x90, 0x10, 0x02, 0xe0, 0xb4, 0x70, 0x1e, 0xa3, 0xe0, 0xb4, 0x30, 0x19, 0x90, 0x05,
+0x08, 0xe0, 0x44, 0x01, 0xf0, 0xfd, 0x90, 0x05, 0x05, 0xe0, 0x54, 0xfb, 0xf0, 0x44, 0x04, 0xf0,
+0xed, 0x54, 0xfe, 0x90, 0x05, 0x08, 0xf0, 0xe4, 0xf5, 0x4e, 0xf5, 0x4f, 0x75, 0x3a, 0xff, 0xad,
+0x57, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13,
+0xf0, 0xe5, 0x56, 0xf4, 0x60, 0x07, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x70, 0x40,
+0xe5, 0x3a, 0xf0, 0x80, 0x49, 0x90, 0x70, 0x10, 0xe0, 0x24, 0xff, 0x92, 0x93, 0xe4, 0xfd, 0xaf,
+0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5,
+0x56, 0xf4, 0x60, 0x2a, 0x80, 0x21, 0x90, 0x70, 0x10, 0xe0, 0x24, 0xff, 0x92, 0x4a, 0xd2, 0x05,
+0xad, 0x57, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70,
+0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x60, 0x07, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x70,
+0x42, 0xe5, 0x3a, 0xf0, 0xa3, 0x74, 0xab, 0xf0, 0x22, 0x22, 0xe5, 0x53, 0x70, 0x1a, 0x30, 0x60,
+0x09, 0xb2, 0x4d, 0x30, 0x4d, 0x04, 0x05, 0x46, 0xc2, 0x04, 0xe5, 0x4f, 0x45, 0x4e, 0x60, 0x08,
+0xe5, 0x4f, 0x15, 0x4f, 0x70, 0x02, 0x15, 0x4e, 0x22, 0x22, 0xc2, 0x42, 0xd3, 0x22, 0x22, 0xc2,
+0x4b, 0xc2, 0x4c, 0xe5, 0x44, 0x12, 0x0a, 0x9d, 0x12, 0xc1, 0x00, 0x13, 0x54, 0x04, 0x13, 0x50,
+0x08, 0x13, 0x2b, 0x10, 0x12, 0xd5, 0x20, 0x12, 0xf5, 0x60, 0x13, 0x06, 0xa0, 0x00, 0x00, 0x13,
+0x56, 0x85, 0x48, 0x43, 0x85, 0x4a, 0x42, 0x85, 0x4c, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x60, 0x03,
+0x02, 0x13, 0x56, 0x80, 0x1b, 0xe5, 0x48, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4a, 0xc4, 0x54,
+0x0f, 0xf5, 0x42, 0xe5, 0x4c, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x66,
+0x53, 0x43, 0x0f, 0x80, 0x61, 0x85, 0x49, 0x43, 0x85, 0x4b, 0x42, 0x85, 0x4d, 0x5e, 0xe5, 0x47,
+0x64, 0x06, 0x70, 0x52, 0x80, 0x1b, 0xe5, 0x49, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4b, 0xc4,
+0x54, 0x0f, 0xf5, 0x42, 0xe5, 0x4d, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70,
+0x35, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x10, 0xf5, 0x43, 0x80, 0x2b, 0xe5, 0x47, 0xb4, 0x04, 0x06,
+0x53, 0x5e, 0xfb, 0x75, 0x42, 0x09, 0xe5, 0x47, 0xb4, 0x05, 0x06, 0x43, 0x5e, 0x04, 0x75, 0x42,
+0x09, 0xe5, 0x47, 0xb4, 0x06, 0x10, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x30, 0xf5, 0x43, 0x80, 0x06,
+0xd2, 0x4b, 0x80, 0x02, 0xd2, 0x4c, 0xe4, 0xf5, 0x25, 0xe5, 0x42, 0xc4, 0x54, 0xf0, 0xff, 0xe5,
+0x43, 0x54, 0x0f, 0x4f, 0xf5, 0x5f, 0x90, 0x70, 0x44, 0xf0, 0xa3, 0xe5, 0x5e, 0xf0, 0xa3, 0xe5,
+0x4a, 0xf0, 0xa3, 0xe5, 0x48, 0xf0, 0xa3, 0xe5, 0x4c, 0xf0, 0xa3, 0xe5, 0x44, 0xf0, 0xa3, 0xe5,
+0x42, 0xf0, 0xa3, 0xe5, 0x43, 0xf0, 0xd2, 0x60, 0x22, 0xe5, 0x47, 0x60, 0x10, 0x24, 0xc0, 0x70,
+0x03, 0x12, 0x16, 0x16, 0x12, 0x13, 0x9e, 0xc2, 0xaf, 0xc2, 0x04, 0xd2, 0xaf, 0x22, 0xc2, 0xaf,
+0x90, 0x04, 0x14, 0xe0, 0x54, 0x0e, 0x60, 0x04, 0xd2, 0x18, 0x80, 0x08, 0xe5, 0x4e, 0x45, 0x4f,
+0x24, 0xff, 0x92, 0x18, 0xd2, 0xaf, 0x90, 0x04, 0x14, 0xe0, 0xa2, 0xe4, 0x92, 0x19, 0x74, 0x1e,
+0xf0, 0xe5, 0x5f, 0x54, 0x0f, 0xf5, 0x2d, 0xe5, 0x25, 0x70, 0x13, 0x30, 0x18, 0x05, 0xe5, 0x5f,
+0x20, 0xe5, 0x0b, 0x30, 0x19, 0x19, 0xe5, 0x5f, 0x54, 0x30, 0xff, 0xbf, 0x30, 0x11, 0xe5, 0x25,
+0x70, 0x05, 0x75, 0x25, 0x0c, 0x80, 0x02, 0x15, 0x25, 0xd2, 0x6c, 0xd2, 0x6d, 0x80, 0x0f, 0xe5,
+0x5f, 0x30, 0xe6, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x04, 0xd2, 0x6c, 0xc2, 0x6d, 0xe5, 0x47,
+0x64, 0x03, 0x70, 0x21, 0x30, 0x4b, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x18, 0xe5, 0x25, 0x70,
+0x03, 0x30, 0x4c, 0x11, 0xc2, 0x4c, 0xe5, 0x25, 0x70, 0x05, 0x75, 0x25, 0x07, 0x80, 0x02, 0x15,
+0x25, 0xd2, 0x6c, 0xd2, 0x6d, 0xe5, 0x47, 0xb4, 0x09, 0x14, 0xe5, 0x44, 0x20, 0xe3, 0x0b, 0xe5,
+0x3a, 0x64, 0x02, 0x60, 0x05, 0xe5, 0x3a, 0xb4, 0x03, 0x04, 0xc2, 0x6c, 0xd2, 0x6d, 0x90, 0x70,
+0x46, 0xe5, 0x2d, 0xf0, 0x20, 0x69, 0x07, 0xe5, 0x5e, 0x20, 0xe0, 0x02, 0xb2, 0x68, 0x20, 0x6b,
+0x07, 0xe5, 0x5e, 0x20, 0xe1, 0x02, 0xb2, 0x6a, 0x20, 0x6d, 0x07, 0xe5, 0x5e, 0x20, 0xe2, 0x02,
+0xb2, 0x6c, 0x90, 0x70, 0x47, 0xe5, 0x2d, 0xf0, 0x75, 0x2e, 0x40, 0x20, 0x69, 0x04, 0xa2, 0x68,
+0x80, 0x26, 0x30, 0x68, 0x06, 0xe5, 0x46, 0xa2, 0xe2, 0x80, 0x1d, 0xe5, 0x5e, 0x20, 0xe2, 0x04,
+0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0xe5, 0x46, 0x54, 0xf0, 0xfe, 0xbe, 0xf0, 0x04, 0x7e, 0x01,
+0x80, 0x02, 0x7e, 0x00, 0xee, 0x6f, 0x24, 0xff, 0x92, 0x73, 0x92, 0x72, 0x20, 0x6b, 0x04, 0xa2,
+0x6a, 0x80, 0x26, 0x30, 0x6a, 0x06, 0xe5, 0x46, 0xa2, 0xe2, 0x80, 0x1d, 0xe5, 0x5e, 0x20, 0xe0,
+0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0xe5, 0x46, 0x54, 0xf0, 0xfe, 0xbe, 0xf0, 0x04, 0x7e,
+0x01, 0x80, 0x02, 0x7e, 0x00, 0xee, 0x6f, 0x24, 0xff, 0x92, 0x75, 0x92, 0x74, 0x20, 0x6d, 0x04,
+0xa2, 0x6c, 0x80, 0x26, 0x30, 0x6c, 0x06, 0xe5, 0x46, 0xa2, 0xe2, 0x80, 0x1d, 0xe5, 0x5e, 0x20,
+0xe1, 0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0xe5, 0x46, 0x54, 0xf0, 0xfe, 0xbe, 0xf0, 0x04,
+0x7e, 0x01, 0x80, 0x02, 0x7e, 0x00, 0xee, 0x6f, 0x24, 0xff, 0x92, 0x71, 0x92, 0x70, 0x90, 0x10,
+0x2f, 0xe5, 0x2e, 0xf0, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x4c, 0x90, 0x02, 0x29, 0xe0, 0x54, 0xfe,
+0xf0, 0xe5, 0x43, 0xc4, 0x54, 0x0f, 0x14, 0x60, 0x14, 0x24, 0xfe, 0x60, 0x23, 0x24, 0x03, 0x60,
+0x03, 0x02, 0x16, 0x05, 0x90, 0x02, 0x28, 0xe0, 0x30, 0x47, 0x0f, 0x80, 0x07, 0x90, 0x02, 0x28,
+0xe0, 0x20, 0x47, 0x06, 0x54, 0xfe, 0xf0, 0x02, 0x16, 0x05, 0x44, 0x01, 0xf0, 0x02, 0x16, 0x05,
+0xe5, 0x46, 0x30, 0xe2, 0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0x90, 0x02, 0x28, 0xe0, 0x54,
+0xfe, 0x4f, 0xf0, 0x02, 0x16, 0x05, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x0f, 0xe5, 0x47, 0x64, 0x08,
+0x60, 0x09, 0xe5, 0x47, 0x64, 0x09, 0x60, 0x03, 0x02, 0x16, 0x05, 0xe4, 0xf5, 0x27, 0x90, 0x02,
+0x29, 0xe0, 0x54, 0xfc, 0xf0, 0xe5, 0x3a, 0x14, 0x60, 0x2d, 0x14, 0x60, 0x2e, 0x14, 0x60, 0x36,
+0x24, 0xfc, 0x60, 0x5f, 0x24, 0xf9, 0x60, 0x1f, 0x24, 0x0e, 0x70, 0x69, 0xe5, 0x46, 0x13, 0x13,
+0x54, 0x3f, 0x75, 0xf0, 0x01, 0x84, 0xaf, 0xf0, 0x20, 0x47, 0x04, 0x7e, 0x01, 0x80, 0x02, 0x7e,
+0x00, 0xef, 0x6e, 0x24, 0xff, 0x80, 0x45, 0xa2, 0x47, 0x80, 0x41, 0xe5, 0x46, 0x30, 0xe2, 0x03,
+0xd3, 0x80, 0x27, 0xc3, 0x80, 0x24, 0xe5, 0x46, 0x30, 0xe2, 0x0d, 0x54, 0x38, 0xc3, 0x94, 0x30,
+0x50, 0x06, 0x7e, 0x00, 0x7f, 0x01, 0x80, 0x04, 0x7e, 0x00, 0x7f, 0x00, 0x20, 0x47, 0x04, 0x7d,
+0x01, 0x80, 0x02, 0x7d, 0x00, 0xef, 0x6d, 0x4e, 0x24, 0xff, 0x92, 0x38, 0xa2, 0x47, 0xb3, 0x92,
+0x39, 0x80, 0x19, 0xe5, 0x46, 0x30, 0xe2, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x39, 0xa2, 0x47,
+0xb3, 0x92, 0x38, 0x80, 0x07, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x92, 0x39, 0x90, 0x02, 0x28, 0xe0,
+0x54, 0xfc, 0x45, 0x27, 0xf0, 0x90, 0x70, 0x9c, 0xe5, 0x3a, 0xf0, 0xa3, 0xe5, 0x47, 0xf0, 0x90,
+0x70, 0x41, 0xe5, 0x3a, 0xf0, 0x22, 0xe4, 0x90, 0x02, 0x29, 0xf0, 0x30, 0x47, 0x04, 0xaf, 0x45,
+0x80, 0x04, 0xe5, 0x45, 0xf4, 0xff, 0x90, 0x02, 0x28, 0xef, 0xf0, 0x22, 0x8f, 0x50, 0xd2, 0x59,
+0x22, 0x8f, 0x54, 0xd2, 0x58, 0x22, 0xe4, 0xf5, 0x62, 0xc2, 0xaf, 0xe5, 0x51, 0x14, 0x60, 0x46,
+0x14, 0x60, 0x62, 0x24, 0x02, 0x60, 0x03, 0x02, 0x16, 0xf0, 0xd2, 0x59, 0x75, 0x55, 0x01, 0x90,
+0x02, 0xa2, 0xe0, 0x54, 0x7f, 0xf0, 0xa3, 0xe0, 0x20, 0xe7, 0x22, 0x90, 0x04, 0x34, 0xe0, 0xb4,
+0x02, 0x1b, 0xa3, 0xe0, 0xb4, 0x02, 0x16, 0xa3, 0xe0, 0xb4, 0x02, 0x11, 0x7f, 0x20, 0x12, 0x16,
+0x2c, 0x90, 0x10, 0x04, 0xe0, 0x54, 0xf3, 0xf0, 0x75, 0x51, 0x01, 0x80, 0x73, 0xe5, 0x50, 0x70,
+0x05, 0x75, 0x62, 0x03, 0x80, 0x6a, 0x90, 0x12, 0x00, 0xe0, 0x54, 0x03, 0x70, 0x11, 0x7f, 0x20,
+0x12, 0x16, 0x2c, 0x90, 0x02, 0xa2, 0xe0, 0x54, 0xbf, 0xf0, 0x75, 0x51, 0x02, 0x80, 0x51, 0xe5,
+0x50, 0x70, 0x02, 0x80, 0x46, 0x90, 0x02, 0xa3, 0xe0, 0x20, 0xe6, 0x3b, 0x90, 0x04, 0x37, 0xe0,
+0x64, 0x22, 0x70, 0x33, 0x90, 0x01, 0x8a, 0x74, 0x7e, 0xf0, 0x90, 0x01, 0x96, 0xf0, 0x90, 0x12,
+0x04, 0x74, 0x0a, 0xf0, 0x90, 0x13, 0x28, 0xe0, 0x54, 0xf0, 0xf0, 0xa3, 0xe0, 0x54, 0xf0, 0xf0,
+0xa3, 0xe0, 0x54, 0xfa, 0xf0, 0x90, 0x04, 0x01, 0xe0, 0x54, 0xf9, 0xf0, 0x75, 0x62, 0x01, 0x75,
+0x55, 0x02, 0xe4, 0xf5, 0x51, 0x80, 0x09, 0xe5, 0x50, 0x70, 0x05, 0x75, 0x62, 0x03, 0xf5, 0x51,
+0xe5, 0x62, 0x60, 0x15, 0xc2, 0x01, 0xe4, 0xf5, 0x51, 0xc2, 0x59, 0xad, 0x62, 0xaf, 0x40, 0x12,
+0x17, 0x7a, 0xe5, 0x62, 0xb4, 0x03, 0x02, 0xd2, 0x03, 0xd2, 0xaf, 0x22, 0xc2, 0xaf, 0x30, 0x01,
+0x12, 0xe4, 0x90, 0x01, 0x96, 0xf0, 0xf5, 0x51, 0xc2, 0x59, 0xc2, 0x01, 0x7d, 0x02, 0xaf, 0x40,
+0x12, 0x17, 0x7a, 0xe5, 0x52, 0x14, 0x60, 0x09, 0x04, 0x70, 0x4c, 0x75, 0x52, 0x01, 0x75, 0x55,
+0x03, 0x90, 0x04, 0x01, 0xe0, 0x44, 0x0e, 0xf0, 0x90, 0x13, 0x28, 0xe0, 0x44, 0x0f, 0xf0, 0xa3,
+0xe0, 0x44, 0x0f, 0xf0, 0xa3, 0xe0, 0x44, 0x05, 0xf0, 0x90, 0x12, 0x04, 0x74, 0x03, 0xf0, 0x90,
+0x02, 0xa2, 0xe0, 0x44, 0xc0, 0xf0, 0x90, 0x10, 0x04, 0xe0, 0x44, 0x0c, 0xf0, 0xe4, 0xf5, 0x52,
+0xf5, 0x55, 0x30, 0x02, 0x0b, 0xc2, 0x02, 0x7d, 0x01, 0xaf, 0x41, 0x12, 0x17, 0x7a, 0x80, 0x02,
+0xc2, 0x03, 0xe4, 0x90, 0x01, 0x96, 0xf0, 0xd2, 0xaf, 0x22, 0xef, 0xf4, 0x60, 0x2d, 0xe4, 0xfe,
+0x74, 0x14, 0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xe0, 0xb4, 0xff, 0x19, 0x74, 0x14,
+0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xef, 0xf0, 0x74, 0x1c, 0x2e, 0xf5, 0x82, 0xe4,
+0x34, 0x70, 0xf5, 0x83, 0xed, 0xf0, 0x22, 0x0e, 0xbe, 0x04, 0xd5, 0x22, 0x22, 0x22, 0x90, 0x70,
+0x2a, 0xe0, 0x30, 0xe1, 0x4d, 0xc2, 0xaf, 0x90, 0x70, 0x28, 0xe0, 0x90, 0x10, 0x1c, 0xf0, 0x90,
+0x70, 0x29, 0xe0, 0x90, 0x10, 0x1d, 0xf0, 0x90, 0x70, 0x2a, 0xe0, 0x90, 0x10, 0x1e, 0xf0, 0x90,
+0x10, 0x1c, 0xe0, 0xf5, 0x62, 0x90, 0x10, 0x1e, 0xe0, 0x20, 0xe1, 0xf3, 0x90, 0x10, 0x1c, 0xe0,
+0x90, 0x70, 0x28, 0xf0, 0x90, 0x10, 0x1d, 0xe0, 0x90, 0x70, 0x29, 0xf0, 0x90, 0x10, 0x1e, 0xe0,
+0x90, 0x70, 0x2a, 0xf0, 0x30, 0x4a, 0x07, 0x90, 0x70, 0x24, 0xe0, 0x44, 0x01, 0xf0, 0xc2, 0x05,
+0xd2, 0xaf, 0x22, 0x22, 0x22, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a, 0x69, 0x77,
+0xff, 0xff, 0xff, 0x02, 0x10, 0x28, 0x02, 0x10, 0x32, 0x02, 0x10, 0x78, 0x02, 0x12, 0x25, 0x02,
+0x12, 0x26, 0x02, 0x12, 0x39, 0x02, 0x12, 0x3e, 0x12, 0x12, 0x3a, 0x22, 0x02, 0x15, 0x72, 0x02,
+0x16, 0x48, 0x02, 0x13, 0x29, 0x02, 0x12, 0x3f, 0x30, 0x05, 0x06, 0x20, 0x0d, 0x03, 0x12, 0x16,
+0xea, 0x22, 0x90, 0x01, 0x8c, 0xe0, 0x30, 0xe3, 0x1b, 0xe5, 0x4c, 0x30, 0xe0, 0x04, 0x7f, 0x40,
+0x80, 0x02, 0x7f, 0x00, 0x90, 0x10, 0x2f, 0xef, 0xf0, 0x90, 0x01, 0x8c, 0x74, 0x08, 0xf0, 0xe4,
+0x90, 0x01, 0xa7, 0xf0, 0x90, 0x01, 0x8c, 0xe0, 0x30, 0xe0, 0x1c, 0x90, 0x01, 0x80, 0xe0, 0xb4,
+0x02, 0x15, 0xa3, 0xe0, 0xb4, 0x01, 0x10, 0x90, 0x01, 0x84, 0xe0, 0xb4, 0x81, 0x09, 0x90, 0x01,
+0x8c, 0x74, 0x01, 0xf0, 0x12, 0x0d, 0xdd, 0x22, 0x90, 0x04, 0x14, 0xe0, 0x20, 0xe7, 0x03, 0x02,
+0x12, 0x1a, 0x90, 0x70, 0x12, 0xe0, 0xf5, 0x56, 0x90, 0x04, 0x04, 0xe0, 0x12, 0x0a, 0xb6, 0x10,
+0xb4, 0x31, 0x10, 0xdf, 0x50, 0x11, 0x05, 0x51, 0x11, 0x10, 0x52, 0x11, 0x10, 0x53, 0x11, 0x10,
+0x54, 0x11, 0x51, 0x55, 0x11, 0x70, 0x70, 0x11, 0x9a, 0x71, 0x11, 0xc4, 0x72, 0x11, 0xf2, 0x80,
+0x00, 0x00, 0x12, 0x1a, 0x20, 0x02, 0x03, 0x30, 0x03, 0x1d, 0x7d, 0x02, 0xaf, 0x56, 0x12, 0x0b,
+0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70,
+0x03, 0x02, 0x12, 0x1a, 0x02, 0x12, 0x13, 0x85, 0x56, 0x41, 0xd2, 0x02, 0x02, 0x12, 0x1a, 0x90,
+0x70, 0x10, 0xe0, 0x54, 0x7f, 0x64, 0x02, 0x60, 0x03, 0x02, 0x12, 0x1a, 0x90, 0x70, 0x11, 0xe0,
+0x64, 0x08, 0x60, 0x08, 0xe0, 0x64, 0x20, 0x60, 0x03, 0x02, 0x12, 0x1a, 0x75, 0x4e, 0x03, 0x75,
+0x4f, 0x20, 0x02, 0x12, 0x1a, 0x90, 0x70, 0x11, 0xe0, 0x24, 0xff, 0x92, 0x47, 0x02, 0x12, 0x1a,
+0x90, 0x04, 0x04, 0xe0, 0x25, 0xe0, 0x24, 0x5d, 0xf5, 0x57, 0x90, 0x70, 0x10, 0xe0, 0xff, 0x74,
+0x47, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0x90, 0x70, 0x11, 0xe0, 0xff, 0x74, 0x48, 0x25, 0x57,
+0xf8, 0xc6, 0xef, 0xc6, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80,
+0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0x1a, 0x02, 0x12,
+0x13, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x05, 0xe5, 0x47, 0xb4, 0x08, 0x08, 0x90, 0x70, 0x11, 0xe0,
+0x54, 0x07, 0xf5, 0x3a, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0xaa, 0xd2, 0x04, 0x02, 0x12, 0x1a,
+0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90, 0x70, 0x11, 0xe0, 0xfd, 0xed, 0xf8, 0xe6, 0xf5, 0x57, 0xfd,
+0xaf, 0x56, 0x12, 0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0,
+0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0x1a, 0x80, 0x79, 0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90,
+0x70, 0x11, 0xe0, 0xfd, 0xed, 0xf5, 0x82, 0x8e, 0x83, 0xe0, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12,
+0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4,
+0x60, 0x58, 0x80, 0x4f, 0xe4, 0xf5, 0x4e, 0xf5, 0x4f, 0x75, 0x3a, 0xff, 0xad, 0x57, 0xaf, 0x56,
+0x12, 0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56,
+0xf4, 0x60, 0x07, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x70, 0x40, 0xe5, 0x3a, 0xf0,
+0x80, 0x28, 0x90, 0x70, 0x10, 0xe0, 0x24, 0xff, 0x92, 0x4a, 0xd2, 0x05, 0xad, 0x57, 0xaf, 0x56,
+0x12, 0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56,
+0xf4, 0x60, 0x07, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x70, 0x42, 0xe5, 0x3a, 0xf0,
+0xa3, 0x74, 0xab, 0xf0, 0x22, 0x22, 0xe5, 0x53, 0x70, 0x0e, 0xe5, 0x4f, 0x45, 0x4e, 0x60, 0x08,
+0xe5, 0x4f, 0x15, 0x4f, 0x70, 0x02, 0x15, 0x4e, 0x22, 0x22, 0xc2, 0x42, 0xd3, 0x22, 0x22, 0xc2,
+0x4b, 0xc2, 0x4c, 0xe5, 0x44, 0x12, 0x0a, 0xb6, 0x12, 0x61, 0x00, 0x12, 0xf4, 0x04, 0x12, 0xf0,
+0x08, 0x12, 0xcb, 0x10, 0x12, 0x75, 0x20, 0x12, 0x95, 0x60, 0x12, 0xa6, 0xa0, 0x00, 0x00, 0x12,
+0xf6, 0x85, 0x48, 0x43, 0x85, 0x4a, 0x42, 0x85, 0x4c, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x60, 0x03,
+0x02, 0x12, 0xf6, 0x80, 0x1b, 0xe5, 0x48, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4a, 0xc4, 0x54,
+0x0f, 0xf5, 0x42, 0xe5, 0x4c, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x66,
+0x53, 0x43, 0x0f, 0x80, 0x61, 0x85, 0x49, 0x43, 0x85, 0x4b, 0x42, 0x85, 0x4d, 0x5e, 0xe5, 0x47,
+0x64, 0x06, 0x70, 0x52, 0x80, 0x1b, 0xe5, 0x49, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4b, 0xc4,
+0x54, 0x0f, 0xf5, 0x42, 0xe5, 0x4d, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70,
+0x35, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x10, 0xf5, 0x43, 0x80, 0x2b, 0xe5, 0x47, 0xb4, 0x04, 0x06,
+0x53, 0x5e, 0xfb, 0x75, 0x42, 0x09, 0xe5, 0x47, 0xb4, 0x05, 0x06, 0x43, 0x5e, 0x04, 0x75, 0x42,
+0x09, 0xe5, 0x47, 0xb4, 0x06, 0x10, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x30, 0xf5, 0x43, 0x80, 0x06,
+0xd2, 0x4b, 0x80, 0x02, 0xd2, 0x4c, 0xe4, 0xf5, 0x25, 0xe5, 0x42, 0xc4, 0x54, 0xf0, 0xff, 0xe5,
+0x43, 0x54, 0x0f, 0x4f, 0xf5, 0x5f, 0x90, 0x70, 0x44, 0xf0, 0xa3, 0xe5, 0x5e, 0xf0, 0xa3, 0xe5,
+0x4a, 0xf0, 0xa3, 0xe5, 0x48, 0xf0, 0xa3, 0xe5, 0x4c, 0xf0, 0xa3, 0xe5, 0x44, 0xf0, 0xa3, 0xe5,
+0x42, 0xf0, 0xa3, 0xe5, 0x43, 0xf0, 0xd2, 0x60, 0x22, 0xe5, 0x47, 0x60, 0x10, 0x24, 0xc0, 0x70,
+0x03, 0x12, 0x15, 0x52, 0x12, 0x13, 0x3e, 0xc2, 0xaf, 0xc2, 0x04, 0xd2, 0xaf, 0x22, 0xc2, 0xaf,
+0x90, 0x04, 0x14, 0xe0, 0x54, 0x0e, 0x60, 0x04, 0xd2, 0x18, 0x80, 0x08, 0xe5, 0x4e, 0x45, 0x4f,
+0x24, 0xff, 0x92, 0x18, 0xd2, 0xaf, 0x90, 0x04, 0x14, 0xe0, 0xa2, 0xe4, 0x92, 0x19, 0x74, 0x1e,
+0xf0, 0xe5, 0x5f, 0x54, 0x0f, 0xf5, 0x2d, 0xe5, 0x25, 0x70, 0x13, 0x30, 0x18, 0x05, 0xe5, 0x5f,
+0x20, 0xe5, 0x0b, 0x30, 0x19, 0x19, 0xe5, 0x5f, 0x54, 0x30, 0xff, 0xbf, 0x30, 0x11, 0xe5, 0x25,
+0x70, 0x05, 0x75, 0x25, 0x0c, 0x80, 0x02, 0x15, 0x25, 0xd2, 0x6c, 0xd2, 0x6d, 0x80, 0x0f, 0xe5,
+0x5f, 0x30, 0xe6, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x04, 0xd2, 0x6c, 0xc2, 0x6d, 0xe5, 0x47,
+0x64, 0x03, 0x70, 0x21, 0x30, 0x4b, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x18, 0xe5, 0x25, 0x70,
+0x03, 0x30, 0x4c, 0x11, 0xc2, 0x4c, 0xe5, 0x25, 0x70, 0x05, 0x75, 0x25, 0x07, 0x80, 0x02, 0x15,
+0x25, 0xd2, 0x6c, 0xd2, 0x6d, 0x90, 0x70, 0x46, 0xe5, 0x2d, 0xf0, 0x20, 0x69, 0x07, 0xe5, 0x5e,
+0x20, 0xe0, 0x02, 0xb2, 0x68, 0x20, 0x6b, 0x07, 0xe5, 0x5e, 0x20, 0xe1, 0x02, 0xb2, 0x6a, 0x20,
+0x6d, 0x07, 0xe5, 0x5e, 0x20, 0xe2, 0x02, 0xb2, 0x6c, 0x90, 0x70, 0x47, 0xe5, 0x2d, 0xf0, 0x75,
+0x2e, 0x40, 0x20, 0x69, 0x04, 0xa2, 0x68, 0x80, 0x15, 0x30, 0x68, 0x06, 0xe5, 0x46, 0xa2, 0xe3,
+0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x73,
+0x92, 0x72, 0x20, 0x6b, 0x04, 0xa2, 0x6a, 0x80, 0x15, 0x30, 0x6a, 0x06, 0xe5, 0x46, 0xa2, 0xe3,
+0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x75,
+0x92, 0x74, 0x20, 0x6d, 0x04, 0xa2, 0x6c, 0x80, 0x15, 0x30, 0x6c, 0x06, 0xe5, 0x46, 0xa2, 0xe3,
+0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x71,
+0x92, 0x70, 0x90, 0x10, 0x2f, 0xe5, 0x2e, 0xf0, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x4c, 0x90, 0x02,
+0x29, 0xe0, 0x54, 0xfe, 0xf0, 0xe5, 0x43, 0xc4, 0x54, 0x0f, 0x14, 0x60, 0x14, 0x24, 0xfe, 0x60,
+0x23, 0x24, 0x03, 0x60, 0x03, 0x02, 0x15, 0x41, 0x90, 0x02, 0x28, 0xe0, 0x30, 0x47, 0x0f, 0x80,
+0x07, 0x90, 0x02, 0x28, 0xe0, 0x20, 0x47, 0x06, 0x54, 0xfe, 0xf0, 0x02, 0x15, 0x41, 0x44, 0x01,
+0xf0, 0x02, 0x15, 0x41, 0xe5, 0x46, 0x30, 0xe3, 0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0x90,
+0x02, 0x28, 0xe0, 0x54, 0xfe, 0x4f, 0xf0, 0x02, 0x15, 0x41, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x09,
+0xe5, 0x47, 0x64, 0x08, 0x60, 0x03, 0x02, 0x15, 0x41, 0xe4, 0xf5, 0x27, 0x90, 0x02, 0x29, 0xe0,
+0x54, 0xfc, 0xf0, 0xe5, 0x3a, 0x14, 0x60, 0x26, 0x14, 0x60, 0x2e, 0x14, 0x60, 0x36, 0x24, 0x03,
+0x70, 0x5f, 0xe5, 0x46, 0x13, 0x13, 0x13, 0x54, 0x1f, 0x75, 0xf0, 0x03, 0x84, 0xaf, 0xf0, 0x20,
+0x47, 0x04, 0x7e, 0x01, 0x80, 0x02, 0x7e, 0x00, 0xef, 0x6e, 0x24, 0xff, 0x80, 0x02, 0xa2, 0x47,
+0x92, 0x39, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x80, 0x3f, 0xe5, 0x46, 0x30, 0xe3, 0x03, 0xd3, 0x80,
+0x27, 0xc3, 0x80, 0x24, 0xe5, 0x46, 0x30, 0xe3, 0x0d, 0x54, 0x70, 0xc3, 0x94, 0x60, 0x50, 0x06,
+0x7e, 0x00, 0x7f, 0x01, 0x80, 0x04, 0x7e, 0x00, 0x7f, 0x00, 0x20, 0x47, 0x04, 0x7d, 0x01, 0x80,
+0x02, 0x7d, 0x00, 0xef, 0x6d, 0x4e, 0x24, 0xff, 0x92, 0x38, 0xa2, 0x47, 0xb3, 0x92, 0x39, 0x80,
+0x07, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x92, 0x39, 0x90, 0x02, 0x28, 0xe0, 0x54, 0xfc, 0x45, 0x27,
+0xf0, 0x90, 0x70, 0x9c, 0xe5, 0x3a, 0xf0, 0xa3, 0xe5, 0x47, 0xf0, 0x90, 0x70, 0x41, 0xe5, 0x3a,
+0xf0, 0x22, 0xe4, 0x90, 0x02, 0x29, 0xf0, 0x30, 0x47, 0x04, 0xaf, 0x45, 0x80, 0x04, 0xe5, 0x45,
+0xf4, 0xff, 0x90, 0x02, 0x28, 0xef, 0xf0, 0x22, 0x8f, 0x50, 0xd2, 0x59, 0x22, 0x8f, 0x54, 0xd2,
+0x58, 0x22, 0xe4, 0xf5, 0x62, 0xc2, 0xaf, 0xe5, 0x51, 0x14, 0x60, 0x46, 0x14, 0x60, 0x62, 0x24,
+0x02, 0x60, 0x03, 0x02, 0x16, 0x2c, 0xd2, 0x59, 0x75, 0x55, 0x01, 0x90, 0x02, 0xa2, 0xe0, 0x54,
+0x7f, 0xf0, 0xa3, 0xe0, 0x20, 0xe7, 0x22, 0x90, 0x04, 0x34, 0xe0, 0xb4, 0x02, 0x1b, 0xa3, 0xe0,
+0xb4, 0x02, 0x16, 0xa3, 0xe0, 0xb4, 0x02, 0x11, 0x7f, 0x20, 0x12, 0x15, 0x68, 0x90, 0x10, 0x04,
+0xe0, 0x54, 0xf3, 0xf0, 0x75, 0x51, 0x01, 0x80, 0x73, 0xe5, 0x50, 0x70, 0x05, 0x75, 0x62, 0x03,
+0x80, 0x6a, 0x90, 0x12, 0x00, 0xe0, 0x54, 0x03, 0x70, 0x11, 0x7f, 0x20, 0x12, 0x15, 0x68, 0x90,
+0x02, 0xa2, 0xe0, 0x54, 0xbf, 0xf0, 0x75, 0x51, 0x02, 0x80, 0x51, 0xe5, 0x50, 0x70, 0x02, 0x80,
+0x46, 0x90, 0x02, 0xa3, 0xe0, 0x20, 0xe6, 0x3b, 0x90, 0x04, 0x37, 0xe0, 0x64, 0x22, 0x70, 0x33,
+0x90, 0x01, 0x8a, 0x74, 0x7e, 0xf0, 0x90, 0x01, 0x96, 0xf0, 0x90, 0x12, 0x04, 0x74, 0x0a, 0xf0,
+0x90, 0x13, 0x28, 0xe0, 0x54, 0xf0, 0xf0, 0xa3, 0xe0, 0x54, 0xf0, 0xf0, 0xa3, 0xe0, 0x54, 0xfa,
+0xf0, 0x90, 0x04, 0x01, 0xe0, 0x54, 0xf9, 0xf0, 0x75, 0x62, 0x01, 0x75, 0x55, 0x02, 0xe4, 0xf5,
+0x51, 0x80, 0x09, 0xe5, 0x50, 0x70, 0x05, 0x75, 0x62, 0x03, 0xf5, 0x51, 0xe5, 0x62, 0x60, 0x15,
+0xc2, 0x01, 0xe4, 0xf5, 0x51, 0xc2, 0x59, 0xad, 0x62, 0xaf, 0x40, 0x12, 0x16, 0xb6, 0xe5, 0x62,
+0xb4, 0x03, 0x02, 0xd2, 0x03, 0xd2, 0xaf, 0x22, 0xc2, 0xaf, 0x30, 0x01, 0x12, 0xe4, 0x90, 0x01,
+0x96, 0xf0, 0xf5, 0x51, 0xc2, 0x59, 0xc2, 0x01, 0x7d, 0x02, 0xaf, 0x40, 0x12, 0x16, 0xb6, 0xe5,
+0x52, 0x14, 0x60, 0x09, 0x04, 0x70, 0x4c, 0x75, 0x52, 0x01, 0x75, 0x55, 0x03, 0x90, 0x04, 0x01,
+0xe0, 0x44, 0x0e, 0xf0, 0x90, 0x13, 0x28, 0xe0, 0x44, 0x0f, 0xf0, 0xa3, 0xe0, 0x44, 0x0f, 0xf0,
+0xa3, 0xe0, 0x44, 0x05, 0xf0, 0x90, 0x12, 0x04, 0x74, 0x03, 0xf0, 0x90, 0x02, 0xa2, 0xe0, 0x44,
+0xc0, 0xf0, 0x90, 0x10, 0x04, 0xe0, 0x44, 0x0c, 0xf0, 0xe4, 0xf5, 0x52, 0xf5, 0x55, 0x30, 0x02,
+0x0b, 0xc2, 0x02, 0x7d, 0x01, 0xaf, 0x41, 0x12, 0x16, 0xb6, 0x80, 0x02, 0xc2, 0x03, 0xe4, 0x90,
+0x01, 0x96, 0xf0, 0xd2, 0xaf, 0x22, 0xef, 0xf4, 0x60, 0x2d, 0xe4, 0xfe, 0x74, 0x14, 0x2e, 0xf5,
+0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xe0, 0xb4, 0xff, 0x19, 0x74, 0x14, 0x2e, 0xf5, 0x82, 0xe4,
+0x34, 0x70, 0xf5, 0x83, 0xef, 0xf0, 0x74, 0x1c, 0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83,
+0xed, 0xf0, 0x22, 0x0e, 0xbe, 0x04, 0xd5, 0x22, 0x22, 0x22, 0x90, 0x70, 0x2a, 0xe0, 0x30, 0xe1,
+0x4d, 0xc2, 0xaf, 0x90, 0x70, 0x28, 0xe0, 0x90, 0x10, 0x1c, 0xf0, 0x90, 0x70, 0x29, 0xe0, 0x90,
+0x10, 0x1d, 0xf0, 0x90, 0x70, 0x2a, 0xe0, 0x90, 0x10, 0x1e, 0xf0, 0x90, 0x10, 0x1c, 0xe0, 0xf5,
+0x62, 0x90, 0x10, 0x1e, 0xe0, 0x20, 0xe1, 0xf3, 0x90, 0x10, 0x1c, 0xe0, 0x90, 0x70, 0x28, 0xf0,
+0x90, 0x10, 0x1d, 0xe0, 0x90, 0x70, 0x29, 0xf0, 0x90, 0x10, 0x1e, 0xe0, 0x90, 0x70, 0x2a, 0xf0,
+0x30, 0x4a, 0x07, 0x90, 0x70, 0x24, 0xe0, 0x44, 0x01, 0xf0, 0xc2, 0x05, 0xd2, 0xaf, 0x22, 0x22,
+0x22, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xe9, 0x00, } ;
diff --git a/drivers/staging/rt2870/common/md5.c b/drivers/staging/rt2870/common/md5.c
new file mode 100644
index 00000000000..774776b4b8c
--- /dev/null
+++ b/drivers/staging/rt2870/common/md5.c
@@ -0,0 +1,1427 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    md5.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	jan			10-28-03		Initial
+	Rita    	11-23-04		Modify MD5 and SHA-1
+	Rita		10-14-05		Modify SHA-1 in big-endian platform
+ */
+#include "../rt_config.h"
+
+/**
+ * md5_mac:
+ * @key: pointer to	the	key	used for MAC generation
+ * @key_len: length	of the key in bytes
+ * @data: pointer to the data area for which the MAC is	generated
+ * @data_len: length of	the	data in	bytes
+ * @mac: pointer to	the	buffer holding space for the MAC; the buffer should
+ * have	space for 128-bit (16 bytes) MD5 hash value
+ *
+ * md5_mac() determines	the	message	authentication code	by using secure	hash
+ * MD5(key | data |	key).
+ */
+void md5_mac(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac)
+{
+	MD5_CTX	context;
+
+	MD5Init(&context);
+	MD5Update(&context,	key, key_len);
+	MD5Update(&context,	data, data_len);
+	MD5Update(&context,	key, key_len);
+	MD5Final(mac, &context);
+}
+
+/**
+ * hmac_md5:
+ * @key: pointer to	the	key	used for MAC generation
+ * @key_len: length	of the key in bytes
+ * @data: pointer to the data area for which the MAC is	generated
+ * @data_len: length of	the	data in	bytes
+ * @mac: pointer to	the	buffer holding space for the MAC; the buffer should
+ * have	space for 128-bit (16 bytes) MD5 hash value
+ *
+ * hmac_md5() determines the message authentication	code using HMAC-MD5.
+ * This	implementation is based	on the sample code presented in	RFC	2104.
+ */
+void hmac_md5(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac)
+{
+	MD5_CTX	context;
+    u8 k_ipad[65]; /* inner padding - key XORd with ipad */
+    u8 k_opad[65]; /* outer padding - key XORd with opad */
+    u8 tk[16];
+	int	i;
+
+	//assert(key != NULL && data != NULL && mac != NULL);
+
+	/* if key is longer	than 64	bytes reset	it to key =	MD5(key) */
+	if (key_len	> 64) {
+		MD5_CTX	ttcontext;
+
+		MD5Init(&ttcontext);
+		MD5Update(&ttcontext, key, key_len);
+		MD5Final(tk, &ttcontext);
+		//key=(PUCHAR)ttcontext.buf;
+		key	= tk;
+		key_len	= 16;
+	}
+
+	/* the HMAC_MD5	transform looks	like:
+	 *
+	 * MD5(K XOR opad, MD5(K XOR ipad, text))
+	 *
+	 * where K is an n byte	key
+	 * ipad	is the byte	0x36 repeated 64 times
+	 * opad	is the byte	0x5c repeated 64 times
+	 * and text	is the data	being protected	*/
+
+	/* start out by	storing	key	in pads	*/
+	NdisZeroMemory(k_ipad, sizeof(k_ipad));
+	NdisZeroMemory(k_opad,	sizeof(k_opad));
+	//assert(key_len < sizeof(k_ipad));
+	NdisMoveMemory(k_ipad, key,	key_len);
+	NdisMoveMemory(k_opad, key,	key_len);
+
+	/* XOR key with	ipad and opad values */
+	for	(i = 0;	i <	64;	i++) {
+		k_ipad[i] ^= 0x36;
+		k_opad[i] ^= 0x5c;
+	}
+
+	/* perform inner MD5 */
+	MD5Init(&context);					 /*	init context for 1st pass */
+	MD5Update(&context,	k_ipad,	64);	 /*	start with inner pad */
+	MD5Update(&context,	data, data_len); /*	then text of datagram */
+	MD5Final(mac, &context);			 /*	finish up 1st pass */
+
+	/* perform outer MD5 */
+	MD5Init(&context);					 /*	init context for 2nd pass */
+	MD5Update(&context,	k_opad,	64);	 /*	start with outer pad */
+	MD5Update(&context,	mac, 16);		 /*	then results of	1st	hash */
+	MD5Final(mac, &context);			 /*	finish up 2nd pass */
+}
+
+#ifndef RT_BIG_ENDIAN
+#define byteReverse(buf, len)   /* Nothing */
+#else
+void byteReverse(unsigned char *buf, unsigned longs);
+void byteReverse(unsigned char *buf, unsigned longs)
+{
+    do {
+        *(UINT32 *)buf = SWAP32(*(UINT32 *)buf);
+        buf += 4;
+    } while (--longs);
+}
+#endif
+
+
+/* ==========================  MD5 implementation =========================== */
+// four base functions for MD5
+#define MD5_F1(x, y, z) (((x) & (y)) | ((~x) & (z)))
+#define MD5_F2(x, y, z) (((x) & (z)) | ((y) & (~z)))
+#define MD5_F3(x, y, z) ((x) ^ (y) ^ (z))
+#define MD5_F4(x, y, z) ((y) ^ ((x) | (~z)))
+#define CYCLIC_LEFT_SHIFT(w, s) (((w) << (s)) | ((w) >> (32-(s))))
+
+#define	MD5Step(f, w, x, y,	z, data, t, s)	\
+	( w	+= f(x,	y, z) +	data + t,  w = (CYCLIC_LEFT_SHIFT(w, s)) & 0xffffffff, w +=	x )
+
+
+/*
+ *  Function Description:
+ *      Initiate MD5 Context satisfied in RFC 1321
+ *
+ *  Arguments:
+ *      pCtx        Pointer	to MD5 context
+ *
+ *  Return Value:
+ *      None
+ */
+VOID MD5Init(MD5_CTX *pCtx)
+{
+    pCtx->Buf[0]=0x67452301;
+    pCtx->Buf[1]=0xefcdab89;
+    pCtx->Buf[2]=0x98badcfe;
+    pCtx->Buf[3]=0x10325476;
+
+    pCtx->LenInBitCount[0]=0;
+    pCtx->LenInBitCount[1]=0;
+}
+
+
+/*
+ *  Function Description:
+ *      Update MD5 Context, allow of an arrary of octets as the next portion
+ *      of the message
+ *
+ *  Arguments:
+ *      pCtx		Pointer	to MD5 context
+ * 	    pData       Pointer to input data
+ *      LenInBytes  The length of input data (unit: byte)
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called after MD5Init or MD5Update(itself)
+ */
+VOID MD5Update(MD5_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes)
+{
+
+    UINT32 TfTimes;
+    UINT32 temp;
+	unsigned int i;
+
+    temp = pCtx->LenInBitCount[0];
+
+    pCtx->LenInBitCount[0] = (UINT32) (pCtx->LenInBitCount[0] + (LenInBytes << 3));
+
+    if (pCtx->LenInBitCount[0] < temp)
+        pCtx->LenInBitCount[1]++;   //carry in
+
+    pCtx->LenInBitCount[1] += LenInBytes >> 29;
+
+    // mod 64 bytes
+    temp = (temp >> 3) & 0x3f;
+
+    // process lacks of 64-byte data
+    if (temp)
+    {
+        UCHAR *pAds = (UCHAR *) pCtx->Input + temp;
+
+        if ((temp+LenInBytes) < 64)
+        {
+            NdisMoveMemory(pAds, (UCHAR *)pData, LenInBytes);
+            return;
+        }
+
+        NdisMoveMemory(pAds, (UCHAR *)pData, 64-temp);
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+        pData += 64-temp;
+        LenInBytes -= 64-temp;
+    } // end of if (temp)
+
+
+    TfTimes = (LenInBytes >> 6);
+
+    for (i=TfTimes; i>0; i--)
+    {
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, 64);
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+        pData += 64;
+        LenInBytes -= 64;
+    } // end of for
+
+    // buffering lacks of 64-byte data
+    if(LenInBytes)
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, LenInBytes);
+
+}
+
+
+/*
+ *  Function Description:
+ *      Append padding bits and length of original message in the tail
+ *      The message digest has to be completed in the end
+ *
+ *  Arguments:
+ *      Digest		Output of Digest-Message for MD5
+ *  	pCtx        Pointer	to MD5 context
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called after MD5Update
+ */
+VOID MD5Final(UCHAR Digest[16], MD5_CTX *pCtx)
+{
+    UCHAR Remainder;
+    UCHAR PadLenInBytes;
+    UCHAR *pAppend=0;
+    unsigned int i;
+
+    Remainder = (UCHAR)((pCtx->LenInBitCount[0] >> 3) & 0x3f);
+
+    PadLenInBytes = (Remainder < 56) ? (56-Remainder) : (120-Remainder);
+
+    pAppend = (UCHAR *)pCtx->Input + Remainder;
+
+    // padding bits without crossing block(64-byte based) boundary
+    if (Remainder < 56)
+    {
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, PadLenInBytes);
+
+		// add data-length field, from low to high
+       	for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[0] >> (i << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[1] >> (i << 3)) & 0xff);
+      	}
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of if
+
+    // padding bits with crossing block(64-byte based) boundary
+    else
+    {
+        // the first block ===
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, (64-Remainder-1));
+        PadLenInBytes -= (64 - Remainder - 1);
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+
+        // the second block ===
+        NdisZeroMemory((UCHAR *)pCtx->Input, PadLenInBytes);
+
+        // add data-length field
+        for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[0] >> (i << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[1] >> (i << 3)) & 0xff);
+      	}
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of else
+
+
+    NdisMoveMemory((UCHAR *)Digest, (UINT32 *)pCtx->Buf, 16); // output
+    byteReverse((UCHAR *)Digest, 4);
+    NdisZeroMemory(pCtx, sizeof(pCtx)); // memory free
+}
+
+
+/*
+ *  Function Description:
+ *      The central algorithm of MD5, consists of four rounds and sixteen
+ *  	steps per round
+ *
+ *  Arguments:
+ *      Buf     Buffers of four states (output: 16 bytes)
+ * 	    Mes     Input data (input: 64 bytes)
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called by MD5Update or MD5Final
+ */
+VOID MD5Transform(UINT32 Buf[4], UINT32 Mes[16])
+{
+    UINT32 Reg[4], Temp;
+	unsigned int i;
+
+    static UCHAR LShiftVal[16] =
+    {
+        7, 12, 17, 22,
+		5, 9 , 14, 20,
+		4, 11, 16, 23,
+ 		6, 10, 15, 21,
+ 	};
+
+
+	// [equal to 4294967296*abs(sin(index))]
+    static UINT32 MD5Table[64] =
+	{
+		0xd76aa478,	0xe8c7b756,	0x242070db,	0xc1bdceee,
+		0xf57c0faf,	0x4787c62a,	0xa8304613, 0xfd469501,
+		0x698098d8,	0x8b44f7af,	0xffff5bb1,	0x895cd7be,
+    	0x6b901122,	0xfd987193,	0xa679438e,	0x49b40821,
+
+    	0xf61e2562,	0xc040b340,	0x265e5a51,	0xe9b6c7aa,
+    	0xd62f105d,	0x02441453,	0xd8a1e681,	0xe7d3fbc8,
+    	0x21e1cde6,	0xc33707d6,	0xf4d50d87,	0x455a14ed,
+    	0xa9e3e905,	0xfcefa3f8,	0x676f02d9,	0x8d2a4c8a,
+
+    	0xfffa3942,	0x8771f681,	0x6d9d6122,	0xfde5380c,
+    	0xa4beea44,	0x4bdecfa9,	0xf6bb4b60,	0xbebfbc70,
+    	0x289b7ec6,	0xeaa127fa,	0xd4ef3085,	0x04881d05,
+    	0xd9d4d039,	0xe6db99e5,	0x1fa27cf8,	0xc4ac5665,
+
+    	0xf4292244,	0x432aff97,	0xab9423a7,	0xfc93a039,
+   		0x655b59c3,	0x8f0ccc92,	0xffeff47d,	0x85845dd1,
+    	0x6fa87e4f,	0xfe2ce6e0,	0xa3014314,	0x4e0811a1,
+    	0xf7537e82,	0xbd3af235,	0x2ad7d2bb,	0xeb86d391
+	};
+
+
+    for (i=0; i<4; i++)
+        Reg[i]=Buf[i];
+
+
+    // 64 steps in MD5 algorithm
+    for (i=0; i<16; i++)
+    {
+        MD5Step(MD5_F1, Reg[0], Reg[1], Reg[2], Reg[3], Mes[i],
+                MD5Table[i], LShiftVal[i & 0x3]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=16; i<32; i++)
+    {
+        MD5Step(MD5_F2, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(5*(i & 0xf)+1) & 0xf],
+                MD5Table[i], LShiftVal[(0x1 << 2)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=32; i<48; i++)
+    {
+        MD5Step(MD5_F3, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(3*(i & 0xf)+5) & 0xf],
+                MD5Table[i], LShiftVal[(0x1 << 3)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=48; i<64; i++)
+    {
+        MD5Step(MD5_F4, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(7*(i & 0xf)) & 0xf],
+                MD5Table[i], LShiftVal[(0x3 << 2)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+
+
+    // (temporary)output
+    for (i=0; i<4; i++)
+        Buf[i] += Reg[i];
+
+}
+
+
+
+/* =========================  SHA-1 implementation ========================== */
+// four base functions for SHA-1
+#define SHA1_F1(b, c, d)    (((b) & (c)) | ((~b) & (d)))
+#define SHA1_F2(b, c, d)    ((b) ^ (c) ^ (d))
+#define SHA1_F3(b, c, d)    (((b) & (c)) | ((b) & (d)) | ((c) & (d)))
+
+
+#define SHA1Step(f, a, b, c, d, e, w, k)    \
+    ( e	+= ( f(b, c, d) + w + k + CYCLIC_LEFT_SHIFT(a, 5)) & 0xffffffff, \
+      b = CYCLIC_LEFT_SHIFT(b, 30) )
+
+//Initiate SHA-1 Context satisfied in RFC 3174
+VOID SHAInit(SHA_CTX *pCtx)
+{
+    pCtx->Buf[0]=0x67452301;
+    pCtx->Buf[1]=0xefcdab89;
+    pCtx->Buf[2]=0x98badcfe;
+    pCtx->Buf[3]=0x10325476;
+    pCtx->Buf[4]=0xc3d2e1f0;
+
+    pCtx->LenInBitCount[0]=0;
+    pCtx->LenInBitCount[1]=0;
+}
+
+/*
+ *  Function Description:
+ *      Update SHA-1 Context, allow of an arrary of octets as the next
+ *      portion of the message
+ *
+ *  Arguments:
+ *      pCtx		Pointer	to SHA-1 context
+ * 	    pData       Pointer to input data
+ *      LenInBytes  The length of input data (unit: byte)
+ *
+ *  Return Value:
+ *      error       indicate more than pow(2,64) bits of data
+ *
+ *  Note:
+ *      Called after SHAInit or SHAUpdate(itself)
+ */
+UCHAR SHAUpdate(SHA_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes)
+{
+    UINT32 TfTimes;
+    UINT32 temp1,temp2;
+	unsigned int i;
+	UCHAR err=1;
+
+    temp1 = pCtx->LenInBitCount[0];
+    temp2 = pCtx->LenInBitCount[1];
+
+    pCtx->LenInBitCount[0] = (UINT32) (pCtx->LenInBitCount[0] + (LenInBytes << 3));
+    if (pCtx->LenInBitCount[0] < temp1)
+        pCtx->LenInBitCount[1]++;   //carry in
+
+
+    pCtx->LenInBitCount[1] = (UINT32) (pCtx->LenInBitCount[1] +(LenInBytes >> 29));
+    if (pCtx->LenInBitCount[1] < temp2)
+        return (err);   //check total length of original data
+
+
+    // mod 64 bytes
+    temp1 = (temp1 >> 3) & 0x3f;
+
+    // process lacks of 64-byte data
+    if (temp1)
+    {
+        UCHAR *pAds = (UCHAR *) pCtx->Input + temp1;
+
+        if ((temp1+LenInBytes) < 64)
+        {
+            NdisMoveMemory(pAds, (UCHAR *)pData, LenInBytes);
+            return (0);
+        }
+
+        NdisMoveMemory(pAds, (UCHAR *)pData, 64-temp1);
+        byteReverse((UCHAR *)pCtx->Input, 16);
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+        pData += 64-temp1;
+        LenInBytes -= 64-temp1;
+    } // end of if (temp1)
+
+
+    TfTimes = (LenInBytes >> 6);
+
+    for (i=TfTimes; i>0; i--)
+    {
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, 64);
+        byteReverse((UCHAR *)pCtx->Input, 16);
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+        pData += 64;
+        LenInBytes -= 64;
+    } // end of for
+
+    // buffering lacks of 64-byte data
+    if(LenInBytes)
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, LenInBytes);
+
+	return (0);
+
+}
+
+// Append padding bits and length of original message in the tail
+// The message digest has to be completed in the end
+VOID SHAFinal(SHA_CTX *pCtx, UCHAR Digest[20])
+{
+    UCHAR Remainder;
+    UCHAR PadLenInBytes;
+    UCHAR *pAppend=0;
+    unsigned int i;
+
+    Remainder = (UCHAR)((pCtx->LenInBitCount[0] >> 3) & 0x3f);
+
+    pAppend = (UCHAR *)pCtx->Input + Remainder;
+
+    PadLenInBytes = (Remainder < 56) ? (56-Remainder) : (120-Remainder);
+
+    // padding bits without crossing block(64-byte based) boundary
+    if (Remainder < 56)
+    {
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, PadLenInBytes);
+
+		// add data-length field, from high to low
+        for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[1] >> ((3-i) << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[0] >> ((3-i) << 3)) & 0xff);
+      	}
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 14);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of if
+
+    // padding bits with crossing block(64-byte based) boundary
+    else
+    {
+        // the first block ===
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, (64-Remainder-1));
+        PadLenInBytes -= (64 - Remainder - 1);
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+
+        // the second block ===
+        NdisZeroMemory((UCHAR *)pCtx->Input, PadLenInBytes);
+
+		// add data-length field
+		for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[1] >> ((3-i) << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[0] >> ((3-i) << 3)) & 0xff);
+      	}
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of else
+
+
+    //Output, bytereverse
+    for (i=0; i<20; i++)
+    {
+        Digest [i] = (UCHAR)(pCtx->Buf[i>>2] >> 8*(3-(i & 0x3)));
+    }
+
+    NdisZeroMemory(pCtx, sizeof(pCtx)); // memory free
+}
+
+
+// The central algorithm of SHA-1, consists of four rounds and
+// twenty steps per round
+VOID SHATransform(UINT32 Buf[5], UINT32 Mes[20])
+{
+    UINT32 Reg[5],Temp;
+	unsigned int i;
+    UINT32 W[80];
+
+    static UINT32 SHA1Table[4] = { 0x5a827999, 0x6ed9eba1,
+                                  0x8f1bbcdc, 0xca62c1d6 };
+
+    Reg[0]=Buf[0];
+	Reg[1]=Buf[1];
+	Reg[2]=Buf[2];
+	Reg[3]=Buf[3];
+	Reg[4]=Buf[4];
+
+    //the first octet of a word is stored in the 0th element, bytereverse
+	for(i = 0; i < 16; i++)
+    {
+    	W[i]  = (Mes[i] >> 24) & 0xff;
+        W[i] |= (Mes[i] >> 8 ) & 0xff00;
+        W[i] |= (Mes[i] << 8 ) & 0xff0000;
+        W[i] |= (Mes[i] << 24) & 0xff000000;
+    }
+
+
+    for	(i = 0; i < 64; i++)
+	    W[16+i] = CYCLIC_LEFT_SHIFT(W[i] ^ W[2+i] ^ W[8+i] ^ W[13+i], 1);
+
+
+    // 80 steps in SHA-1 algorithm
+    for (i=0; i<80; i++)
+    {
+        if (i<20)
+            SHA1Step(SHA1_F1, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[0]);
+
+        else if (i>=20 && i<40)
+            SHA1Step(SHA1_F2, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[1]);
+
+		else if (i>=40 && i<60)
+            SHA1Step(SHA1_F3, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                      W[i], SHA1Table[2]);
+
+        else
+            SHA1Step(SHA1_F2, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[3]);
+
+
+       // one-word right shift
+		Temp   = Reg[4];
+        Reg[4] = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+
+    } // end of for-loop
+
+
+    // (temporary)output
+    for (i=0; i<5; i++)
+        Buf[i] += Reg[i];
+
+}
+
+
+/* =========================  AES En/Decryption ========================== */
+
+/* forward S-box */
+static uint32 FSb[256] =
+{
+	0x63, 0x7C,	0x77, 0x7B,	0xF2, 0x6B,	0x6F, 0xC5,
+	0x30, 0x01,	0x67, 0x2B,	0xFE, 0xD7,	0xAB, 0x76,
+	0xCA, 0x82,	0xC9, 0x7D,	0xFA, 0x59,	0x47, 0xF0,
+	0xAD, 0xD4,	0xA2, 0xAF,	0x9C, 0xA4,	0x72, 0xC0,
+	0xB7, 0xFD,	0x93, 0x26,	0x36, 0x3F,	0xF7, 0xCC,
+	0x34, 0xA5,	0xE5, 0xF1,	0x71, 0xD8,	0x31, 0x15,
+	0x04, 0xC7,	0x23, 0xC3,	0x18, 0x96,	0x05, 0x9A,
+	0x07, 0x12,	0x80, 0xE2,	0xEB, 0x27,	0xB2, 0x75,
+	0x09, 0x83,	0x2C, 0x1A,	0x1B, 0x6E,	0x5A, 0xA0,
+	0x52, 0x3B,	0xD6, 0xB3,	0x29, 0xE3,	0x2F, 0x84,
+	0x53, 0xD1,	0x00, 0xED,	0x20, 0xFC,	0xB1, 0x5B,
+	0x6A, 0xCB,	0xBE, 0x39,	0x4A, 0x4C,	0x58, 0xCF,
+	0xD0, 0xEF,	0xAA, 0xFB,	0x43, 0x4D,	0x33, 0x85,
+	0x45, 0xF9,	0x02, 0x7F,	0x50, 0x3C,	0x9F, 0xA8,
+	0x51, 0xA3,	0x40, 0x8F,	0x92, 0x9D,	0x38, 0xF5,
+	0xBC, 0xB6,	0xDA, 0x21,	0x10, 0xFF,	0xF3, 0xD2,
+	0xCD, 0x0C,	0x13, 0xEC,	0x5F, 0x97,	0x44, 0x17,
+	0xC4, 0xA7,	0x7E, 0x3D,	0x64, 0x5D,	0x19, 0x73,
+	0x60, 0x81,	0x4F, 0xDC,	0x22, 0x2A,	0x90, 0x88,
+	0x46, 0xEE,	0xB8, 0x14,	0xDE, 0x5E,	0x0B, 0xDB,
+	0xE0, 0x32,	0x3A, 0x0A,	0x49, 0x06,	0x24, 0x5C,
+	0xC2, 0xD3,	0xAC, 0x62,	0x91, 0x95,	0xE4, 0x79,
+	0xE7, 0xC8,	0x37, 0x6D,	0x8D, 0xD5,	0x4E, 0xA9,
+	0x6C, 0x56,	0xF4, 0xEA,	0x65, 0x7A,	0xAE, 0x08,
+	0xBA, 0x78,	0x25, 0x2E,	0x1C, 0xA6,	0xB4, 0xC6,
+	0xE8, 0xDD,	0x74, 0x1F,	0x4B, 0xBD,	0x8B, 0x8A,
+	0x70, 0x3E,	0xB5, 0x66,	0x48, 0x03,	0xF6, 0x0E,
+	0x61, 0x35,	0x57, 0xB9,	0x86, 0xC1,	0x1D, 0x9E,
+	0xE1, 0xF8,	0x98, 0x11,	0x69, 0xD9,	0x8E, 0x94,
+	0x9B, 0x1E,	0x87, 0xE9,	0xCE, 0x55,	0x28, 0xDF,
+	0x8C, 0xA1,	0x89, 0x0D,	0xBF, 0xE6,	0x42, 0x68,
+	0x41, 0x99,	0x2D, 0x0F,	0xB0, 0x54,	0xBB, 0x16
+};
+
+/* forward table */
+#define	FT \
+\
+	V(C6,63,63,A5),	V(F8,7C,7C,84),	V(EE,77,77,99),	V(F6,7B,7B,8D),	\
+	V(FF,F2,F2,0D),	V(D6,6B,6B,BD),	V(DE,6F,6F,B1),	V(91,C5,C5,54),	\
+	V(60,30,30,50),	V(02,01,01,03),	V(CE,67,67,A9),	V(56,2B,2B,7D),	\
+	V(E7,FE,FE,19),	V(B5,D7,D7,62),	V(4D,AB,AB,E6),	V(EC,76,76,9A),	\
+	V(8F,CA,CA,45),	V(1F,82,82,9D),	V(89,C9,C9,40),	V(FA,7D,7D,87),	\
+	V(EF,FA,FA,15),	V(B2,59,59,EB),	V(8E,47,47,C9),	V(FB,F0,F0,0B),	\
+	V(41,AD,AD,EC),	V(B3,D4,D4,67),	V(5F,A2,A2,FD),	V(45,AF,AF,EA),	\
+	V(23,9C,9C,BF),	V(53,A4,A4,F7),	V(E4,72,72,96),	V(9B,C0,C0,5B),	\
+	V(75,B7,B7,C2),	V(E1,FD,FD,1C),	V(3D,93,93,AE),	V(4C,26,26,6A),	\
+	V(6C,36,36,5A),	V(7E,3F,3F,41),	V(F5,F7,F7,02),	V(83,CC,CC,4F),	\
+	V(68,34,34,5C),	V(51,A5,A5,F4),	V(D1,E5,E5,34),	V(F9,F1,F1,08),	\
+	V(E2,71,71,93),	V(AB,D8,D8,73),	V(62,31,31,53),	V(2A,15,15,3F),	\
+	V(08,04,04,0C),	V(95,C7,C7,52),	V(46,23,23,65),	V(9D,C3,C3,5E),	\
+	V(30,18,18,28),	V(37,96,96,A1),	V(0A,05,05,0F),	V(2F,9A,9A,B5),	\
+	V(0E,07,07,09),	V(24,12,12,36),	V(1B,80,80,9B),	V(DF,E2,E2,3D),	\
+	V(CD,EB,EB,26),	V(4E,27,27,69),	V(7F,B2,B2,CD),	V(EA,75,75,9F),	\
+	V(12,09,09,1B),	V(1D,83,83,9E),	V(58,2C,2C,74),	V(34,1A,1A,2E),	\
+	V(36,1B,1B,2D),	V(DC,6E,6E,B2),	V(B4,5A,5A,EE),	V(5B,A0,A0,FB),	\
+	V(A4,52,52,F6),	V(76,3B,3B,4D),	V(B7,D6,D6,61),	V(7D,B3,B3,CE),	\
+	V(52,29,29,7B),	V(DD,E3,E3,3E),	V(5E,2F,2F,71),	V(13,84,84,97),	\
+	V(A6,53,53,F5),	V(B9,D1,D1,68),	V(00,00,00,00),	V(C1,ED,ED,2C),	\
+	V(40,20,20,60),	V(E3,FC,FC,1F),	V(79,B1,B1,C8),	V(B6,5B,5B,ED),	\
+	V(D4,6A,6A,BE),	V(8D,CB,CB,46),	V(67,BE,BE,D9),	V(72,39,39,4B),	\
+	V(94,4A,4A,DE),	V(98,4C,4C,D4),	V(B0,58,58,E8),	V(85,CF,CF,4A),	\
+	V(BB,D0,D0,6B),	V(C5,EF,EF,2A),	V(4F,AA,AA,E5),	V(ED,FB,FB,16),	\
+	V(86,43,43,C5),	V(9A,4D,4D,D7),	V(66,33,33,55),	V(11,85,85,94),	\
+	V(8A,45,45,CF),	V(E9,F9,F9,10),	V(04,02,02,06),	V(FE,7F,7F,81),	\
+	V(A0,50,50,F0),	V(78,3C,3C,44),	V(25,9F,9F,BA),	V(4B,A8,A8,E3),	\
+	V(A2,51,51,F3),	V(5D,A3,A3,FE),	V(80,40,40,C0),	V(05,8F,8F,8A),	\
+	V(3F,92,92,AD),	V(21,9D,9D,BC),	V(70,38,38,48),	V(F1,F5,F5,04),	\
+	V(63,BC,BC,DF),	V(77,B6,B6,C1),	V(AF,DA,DA,75),	V(42,21,21,63),	\
+	V(20,10,10,30),	V(E5,FF,FF,1A),	V(FD,F3,F3,0E),	V(BF,D2,D2,6D),	\
+	V(81,CD,CD,4C),	V(18,0C,0C,14),	V(26,13,13,35),	V(C3,EC,EC,2F),	\
+	V(BE,5F,5F,E1),	V(35,97,97,A2),	V(88,44,44,CC),	V(2E,17,17,39),	\
+	V(93,C4,C4,57),	V(55,A7,A7,F2),	V(FC,7E,7E,82),	V(7A,3D,3D,47),	\
+	V(C8,64,64,AC),	V(BA,5D,5D,E7),	V(32,19,19,2B),	V(E6,73,73,95),	\
+	V(C0,60,60,A0),	V(19,81,81,98),	V(9E,4F,4F,D1),	V(A3,DC,DC,7F),	\
+	V(44,22,22,66),	V(54,2A,2A,7E),	V(3B,90,90,AB),	V(0B,88,88,83),	\
+	V(8C,46,46,CA),	V(C7,EE,EE,29),	V(6B,B8,B8,D3),	V(28,14,14,3C),	\
+	V(A7,DE,DE,79),	V(BC,5E,5E,E2),	V(16,0B,0B,1D),	V(AD,DB,DB,76),	\
+	V(DB,E0,E0,3B),	V(64,32,32,56),	V(74,3A,3A,4E),	V(14,0A,0A,1E),	\
+	V(92,49,49,DB),	V(0C,06,06,0A),	V(48,24,24,6C),	V(B8,5C,5C,E4),	\
+	V(9F,C2,C2,5D),	V(BD,D3,D3,6E),	V(43,AC,AC,EF),	V(C4,62,62,A6),	\
+	V(39,91,91,A8),	V(31,95,95,A4),	V(D3,E4,E4,37),	V(F2,79,79,8B),	\
+	V(D5,E7,E7,32),	V(8B,C8,C8,43),	V(6E,37,37,59),	V(DA,6D,6D,B7),	\
+	V(01,8D,8D,8C),	V(B1,D5,D5,64),	V(9C,4E,4E,D2),	V(49,A9,A9,E0),	\
+	V(D8,6C,6C,B4),	V(AC,56,56,FA),	V(F3,F4,F4,07),	V(CF,EA,EA,25),	\
+	V(CA,65,65,AF),	V(F4,7A,7A,8E),	V(47,AE,AE,E9),	V(10,08,08,18),	\
+	V(6F,BA,BA,D5),	V(F0,78,78,88),	V(4A,25,25,6F),	V(5C,2E,2E,72),	\
+	V(38,1C,1C,24),	V(57,A6,A6,F1),	V(73,B4,B4,C7),	V(97,C6,C6,51),	\
+	V(CB,E8,E8,23),	V(A1,DD,DD,7C),	V(E8,74,74,9C),	V(3E,1F,1F,21),	\
+	V(96,4B,4B,DD),	V(61,BD,BD,DC),	V(0D,8B,8B,86),	V(0F,8A,8A,85),	\
+	V(E0,70,70,90),	V(7C,3E,3E,42),	V(71,B5,B5,C4),	V(CC,66,66,AA),	\
+	V(90,48,48,D8),	V(06,03,03,05),	V(F7,F6,F6,01),	V(1C,0E,0E,12),	\
+	V(C2,61,61,A3),	V(6A,35,35,5F),	V(AE,57,57,F9),	V(69,B9,B9,D0),	\
+	V(17,86,86,91),	V(99,C1,C1,58),	V(3A,1D,1D,27),	V(27,9E,9E,B9),	\
+	V(D9,E1,E1,38),	V(EB,F8,F8,13),	V(2B,98,98,B3),	V(22,11,11,33),	\
+	V(D2,69,69,BB),	V(A9,D9,D9,70),	V(07,8E,8E,89),	V(33,94,94,A7),	\
+	V(2D,9B,9B,B6),	V(3C,1E,1E,22),	V(15,87,87,92),	V(C9,E9,E9,20),	\
+	V(87,CE,CE,49),	V(AA,55,55,FF),	V(50,28,28,78),	V(A5,DF,DF,7A),	\
+	V(03,8C,8C,8F),	V(59,A1,A1,F8),	V(09,89,89,80),	V(1A,0D,0D,17),	\
+	V(65,BF,BF,DA),	V(D7,E6,E6,31),	V(84,42,42,C6),	V(D0,68,68,B8),	\
+	V(82,41,41,C3),	V(29,99,99,B0),	V(5A,2D,2D,77),	V(1E,0F,0F,11),	\
+	V(7B,B0,B0,CB),	V(A8,54,54,FC),	V(6D,BB,BB,D6),	V(2C,16,16,3A)
+
+#define	V(a,b,c,d) 0x##a##b##c##d
+static uint32 FT0[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##d##a##b##c
+static uint32 FT1[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##c##d##a##b
+static uint32 FT2[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##b##c##d##a
+static uint32 FT3[256] = { FT };
+#undef V
+
+#undef FT
+
+/* reverse S-box */
+
+static uint32 RSb[256] =
+{
+	0x52, 0x09,	0x6A, 0xD5,	0x30, 0x36,	0xA5, 0x38,
+	0xBF, 0x40,	0xA3, 0x9E,	0x81, 0xF3,	0xD7, 0xFB,
+	0x7C, 0xE3,	0x39, 0x82,	0x9B, 0x2F,	0xFF, 0x87,
+	0x34, 0x8E,	0x43, 0x44,	0xC4, 0xDE,	0xE9, 0xCB,
+	0x54, 0x7B,	0x94, 0x32,	0xA6, 0xC2,	0x23, 0x3D,
+	0xEE, 0x4C,	0x95, 0x0B,	0x42, 0xFA,	0xC3, 0x4E,
+	0x08, 0x2E,	0xA1, 0x66,	0x28, 0xD9,	0x24, 0xB2,
+	0x76, 0x5B,	0xA2, 0x49,	0x6D, 0x8B,	0xD1, 0x25,
+	0x72, 0xF8,	0xF6, 0x64,	0x86, 0x68,	0x98, 0x16,
+	0xD4, 0xA4,	0x5C, 0xCC,	0x5D, 0x65,	0xB6, 0x92,
+	0x6C, 0x70,	0x48, 0x50,	0xFD, 0xED,	0xB9, 0xDA,
+	0x5E, 0x15,	0x46, 0x57,	0xA7, 0x8D,	0x9D, 0x84,
+	0x90, 0xD8,	0xAB, 0x00,	0x8C, 0xBC,	0xD3, 0x0A,
+	0xF7, 0xE4,	0x58, 0x05,	0xB8, 0xB3,	0x45, 0x06,
+	0xD0, 0x2C,	0x1E, 0x8F,	0xCA, 0x3F,	0x0F, 0x02,
+	0xC1, 0xAF,	0xBD, 0x03,	0x01, 0x13,	0x8A, 0x6B,
+	0x3A, 0x91,	0x11, 0x41,	0x4F, 0x67,	0xDC, 0xEA,
+	0x97, 0xF2,	0xCF, 0xCE,	0xF0, 0xB4,	0xE6, 0x73,
+	0x96, 0xAC,	0x74, 0x22,	0xE7, 0xAD,	0x35, 0x85,
+	0xE2, 0xF9,	0x37, 0xE8,	0x1C, 0x75,	0xDF, 0x6E,
+	0x47, 0xF1,	0x1A, 0x71,	0x1D, 0x29,	0xC5, 0x89,
+	0x6F, 0xB7,	0x62, 0x0E,	0xAA, 0x18,	0xBE, 0x1B,
+	0xFC, 0x56,	0x3E, 0x4B,	0xC6, 0xD2,	0x79, 0x20,
+	0x9A, 0xDB,	0xC0, 0xFE,	0x78, 0xCD,	0x5A, 0xF4,
+	0x1F, 0xDD,	0xA8, 0x33,	0x88, 0x07,	0xC7, 0x31,
+	0xB1, 0x12,	0x10, 0x59,	0x27, 0x80,	0xEC, 0x5F,
+	0x60, 0x51,	0x7F, 0xA9,	0x19, 0xB5,	0x4A, 0x0D,
+	0x2D, 0xE5,	0x7A, 0x9F,	0x93, 0xC9,	0x9C, 0xEF,
+	0xA0, 0xE0,	0x3B, 0x4D,	0xAE, 0x2A,	0xF5, 0xB0,
+	0xC8, 0xEB,	0xBB, 0x3C,	0x83, 0x53,	0x99, 0x61,
+	0x17, 0x2B,	0x04, 0x7E,	0xBA, 0x77,	0xD6, 0x26,
+	0xE1, 0x69,	0x14, 0x63,	0x55, 0x21,	0x0C, 0x7D
+};
+
+/* reverse table */
+
+#define	RT \
+\
+	V(51,F4,A7,50),	V(7E,41,65,53),	V(1A,17,A4,C3),	V(3A,27,5E,96),	\
+	V(3B,AB,6B,CB),	V(1F,9D,45,F1),	V(AC,FA,58,AB),	V(4B,E3,03,93),	\
+	V(20,30,FA,55),	V(AD,76,6D,F6),	V(88,CC,76,91),	V(F5,02,4C,25),	\
+	V(4F,E5,D7,FC),	V(C5,2A,CB,D7),	V(26,35,44,80),	V(B5,62,A3,8F),	\
+	V(DE,B1,5A,49),	V(25,BA,1B,67),	V(45,EA,0E,98),	V(5D,FE,C0,E1),	\
+	V(C3,2F,75,02),	V(81,4C,F0,12),	V(8D,46,97,A3),	V(6B,D3,F9,C6),	\
+	V(03,8F,5F,E7),	V(15,92,9C,95),	V(BF,6D,7A,EB),	V(95,52,59,DA),	\
+	V(D4,BE,83,2D),	V(58,74,21,D3),	V(49,E0,69,29),	V(8E,C9,C8,44),	\
+	V(75,C2,89,6A),	V(F4,8E,79,78),	V(99,58,3E,6B),	V(27,B9,71,DD),	\
+	V(BE,E1,4F,B6),	V(F0,88,AD,17),	V(C9,20,AC,66),	V(7D,CE,3A,B4),	\
+	V(63,DF,4A,18),	V(E5,1A,31,82),	V(97,51,33,60),	V(62,53,7F,45),	\
+	V(B1,64,77,E0),	V(BB,6B,AE,84),	V(FE,81,A0,1C),	V(F9,08,2B,94),	\
+	V(70,48,68,58),	V(8F,45,FD,19),	V(94,DE,6C,87),	V(52,7B,F8,B7),	\
+	V(AB,73,D3,23),	V(72,4B,02,E2),	V(E3,1F,8F,57),	V(66,55,AB,2A),	\
+	V(B2,EB,28,07),	V(2F,B5,C2,03),	V(86,C5,7B,9A),	V(D3,37,08,A5),	\
+	V(30,28,87,F2),	V(23,BF,A5,B2),	V(02,03,6A,BA),	V(ED,16,82,5C),	\
+	V(8A,CF,1C,2B),	V(A7,79,B4,92),	V(F3,07,F2,F0),	V(4E,69,E2,A1),	\
+	V(65,DA,F4,CD),	V(06,05,BE,D5),	V(D1,34,62,1F),	V(C4,A6,FE,8A),	\
+	V(34,2E,53,9D),	V(A2,F3,55,A0),	V(05,8A,E1,32),	V(A4,F6,EB,75),	\
+	V(0B,83,EC,39),	V(40,60,EF,AA),	V(5E,71,9F,06),	V(BD,6E,10,51),	\
+	V(3E,21,8A,F9),	V(96,DD,06,3D),	V(DD,3E,05,AE),	V(4D,E6,BD,46),	\
+	V(91,54,8D,B5),	V(71,C4,5D,05),	V(04,06,D4,6F),	V(60,50,15,FF),	\
+	V(19,98,FB,24),	V(D6,BD,E9,97),	V(89,40,43,CC),	V(67,D9,9E,77),	\
+	V(B0,E8,42,BD),	V(07,89,8B,88),	V(E7,19,5B,38),	V(79,C8,EE,DB),	\
+	V(A1,7C,0A,47),	V(7C,42,0F,E9),	V(F8,84,1E,C9),	V(00,00,00,00),	\
+	V(09,80,86,83),	V(32,2B,ED,48),	V(1E,11,70,AC),	V(6C,5A,72,4E),	\
+	V(FD,0E,FF,FB),	V(0F,85,38,56),	V(3D,AE,D5,1E),	V(36,2D,39,27),	\
+	V(0A,0F,D9,64),	V(68,5C,A6,21),	V(9B,5B,54,D1),	V(24,36,2E,3A),	\
+	V(0C,0A,67,B1),	V(93,57,E7,0F),	V(B4,EE,96,D2),	V(1B,9B,91,9E),	\
+	V(80,C0,C5,4F),	V(61,DC,20,A2),	V(5A,77,4B,69),	V(1C,12,1A,16),	\
+	V(E2,93,BA,0A),	V(C0,A0,2A,E5),	V(3C,22,E0,43),	V(12,1B,17,1D),	\
+	V(0E,09,0D,0B),	V(F2,8B,C7,AD),	V(2D,B6,A8,B9),	V(14,1E,A9,C8),	\
+	V(57,F1,19,85),	V(AF,75,07,4C),	V(EE,99,DD,BB),	V(A3,7F,60,FD),	\
+	V(F7,01,26,9F),	V(5C,72,F5,BC),	V(44,66,3B,C5),	V(5B,FB,7E,34),	\
+	V(8B,43,29,76),	V(CB,23,C6,DC),	V(B6,ED,FC,68),	V(B8,E4,F1,63),	\
+	V(D7,31,DC,CA),	V(42,63,85,10),	V(13,97,22,40),	V(84,C6,11,20),	\
+	V(85,4A,24,7D),	V(D2,BB,3D,F8),	V(AE,F9,32,11),	V(C7,29,A1,6D),	\
+	V(1D,9E,2F,4B),	V(DC,B2,30,F3),	V(0D,86,52,EC),	V(77,C1,E3,D0),	\
+	V(2B,B3,16,6C),	V(A9,70,B9,99),	V(11,94,48,FA),	V(47,E9,64,22),	\
+	V(A8,FC,8C,C4),	V(A0,F0,3F,1A),	V(56,7D,2C,D8),	V(22,33,90,EF),	\
+	V(87,49,4E,C7),	V(D9,38,D1,C1),	V(8C,CA,A2,FE),	V(98,D4,0B,36),	\
+	V(A6,F5,81,CF),	V(A5,7A,DE,28),	V(DA,B7,8E,26),	V(3F,AD,BF,A4),	\
+	V(2C,3A,9D,E4),	V(50,78,92,0D),	V(6A,5F,CC,9B),	V(54,7E,46,62),	\
+	V(F6,8D,13,C2),	V(90,D8,B8,E8),	V(2E,39,F7,5E),	V(82,C3,AF,F5),	\
+	V(9F,5D,80,BE),	V(69,D0,93,7C),	V(6F,D5,2D,A9),	V(CF,25,12,B3),	\
+	V(C8,AC,99,3B),	V(10,18,7D,A7),	V(E8,9C,63,6E),	V(DB,3B,BB,7B),	\
+	V(CD,26,78,09),	V(6E,59,18,F4),	V(EC,9A,B7,01),	V(83,4F,9A,A8),	\
+	V(E6,95,6E,65),	V(AA,FF,E6,7E),	V(21,BC,CF,08),	V(EF,15,E8,E6),	\
+	V(BA,E7,9B,D9),	V(4A,6F,36,CE),	V(EA,9F,09,D4),	V(29,B0,7C,D6),	\
+	V(31,A4,B2,AF),	V(2A,3F,23,31),	V(C6,A5,94,30),	V(35,A2,66,C0),	\
+	V(74,4E,BC,37),	V(FC,82,CA,A6),	V(E0,90,D0,B0),	V(33,A7,D8,15),	\
+	V(F1,04,98,4A),	V(41,EC,DA,F7),	V(7F,CD,50,0E),	V(17,91,F6,2F),	\
+	V(76,4D,D6,8D),	V(43,EF,B0,4D),	V(CC,AA,4D,54),	V(E4,96,04,DF),	\
+	V(9E,D1,B5,E3),	V(4C,6A,88,1B),	V(C1,2C,1F,B8),	V(46,65,51,7F),	\
+	V(9D,5E,EA,04),	V(01,8C,35,5D),	V(FA,87,74,73),	V(FB,0B,41,2E),	\
+	V(B3,67,1D,5A),	V(92,DB,D2,52),	V(E9,10,56,33),	V(6D,D6,47,13),	\
+	V(9A,D7,61,8C),	V(37,A1,0C,7A),	V(59,F8,14,8E),	V(EB,13,3C,89),	\
+	V(CE,A9,27,EE),	V(B7,61,C9,35),	V(E1,1C,E5,ED),	V(7A,47,B1,3C),	\
+	V(9C,D2,DF,59),	V(55,F2,73,3F),	V(18,14,CE,79),	V(73,C7,37,BF),	\
+	V(53,F7,CD,EA),	V(5F,FD,AA,5B),	V(DF,3D,6F,14),	V(78,44,DB,86),	\
+	V(CA,AF,F3,81),	V(B9,68,C4,3E),	V(38,24,34,2C),	V(C2,A3,40,5F),	\
+	V(16,1D,C3,72),	V(BC,E2,25,0C),	V(28,3C,49,8B),	V(FF,0D,95,41),	\
+	V(39,A8,01,71),	V(08,0C,B3,DE),	V(D8,B4,E4,9C),	V(64,56,C1,90),	\
+	V(7B,CB,84,61),	V(D5,32,B6,70),	V(48,6C,5C,74),	V(D0,B8,57,42)
+
+#define	V(a,b,c,d) 0x##a##b##c##d
+static uint32 RT0[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##d##a##b##c
+static uint32 RT1[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##c##d##a##b
+static uint32 RT2[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##b##c##d##a
+static uint32 RT3[256] = { RT };
+#undef V
+
+#undef RT
+
+/* round constants */
+
+static uint32 RCON[10] =
+{
+	0x01000000,	0x02000000,	0x04000000,	0x08000000,
+	0x10000000,	0x20000000,	0x40000000,	0x80000000,
+	0x1B000000,	0x36000000
+};
+
+/* key schedule	tables */
+
+static int KT_init = 1;
+
+static uint32 KT0[256];
+static uint32 KT1[256];
+static uint32 KT2[256];
+static uint32 KT3[256];
+
+/* platform-independant	32-bit integer manipulation	macros */
+
+#define	GET_UINT32(n,b,i)						\
+{												\
+	(n)	= (	(uint32) (b)[(i)	] << 24	)		\
+		| (	(uint32) (b)[(i) + 1] << 16	)		\
+		| (	(uint32) (b)[(i) + 2] <<  8	)		\
+		| (	(uint32) (b)[(i) + 3]		);		\
+}
+
+#define	PUT_UINT32(n,b,i)						\
+{												\
+	(b)[(i)	   ] = (uint8) ( (n) >>	24 );		\
+	(b)[(i)	+ 1] = (uint8) ( (n) >>	16 );		\
+	(b)[(i)	+ 2] = (uint8) ( (n) >>	 8 );		\
+	(b)[(i)	+ 3] = (uint8) ( (n)	   );		\
+}
+
+/* AES key scheduling routine */
+
+int	rtmp_aes_set_key( aes_context *ctx, uint8 *key, int nbits )
+{
+	int	i;
+	uint32 *RK,	*SK;
+
+	switch(	nbits )
+	{
+		case 128: ctx->nr =	10;	break;
+		case 192: ctx->nr =	12;	break;
+		case 256: ctx->nr =	14;	break;
+		default	: return( 1	);
+	}
+
+	RK = ctx->erk;
+
+	for( i = 0;	i <	(nbits >> 5); i++ )
+	{
+		GET_UINT32(	RK[i], key,	i *	4 );
+	}
+
+	/* setup encryption	round keys */
+
+	switch(	nbits )
+	{
+	case 128:
+
+		for( i = 0;	i <	10;	i++, RK	+= 4 )
+		{
+			RK[4]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[3] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[3] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[3]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[3] >> 24 ) ]	   );
+
+			RK[5]  = RK[1] ^ RK[4];
+			RK[6]  = RK[2] ^ RK[5];
+			RK[7]  = RK[3] ^ RK[6];
+		}
+		break;
+
+	case 192:
+
+		for( i = 0;	i <	8; i++,	RK += 6	)
+		{
+			RK[6]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[5] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[5] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[5]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[5] >> 24 ) ]	   );
+
+			RK[7]  = RK[1] ^ RK[6];
+			RK[8]  = RK[2] ^ RK[7];
+			RK[9]  = RK[3] ^ RK[8];
+			RK[10] = RK[4] ^ RK[9];
+			RK[11] = RK[5] ^ RK[10];
+		}
+		break;
+
+	case 256:
+
+		for( i = 0;	i <	7; i++,	RK += 8	)
+		{
+			RK[8]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[7] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[7] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[7]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[7] >> 24 ) ]	   );
+
+			RK[9]  = RK[1] ^ RK[8];
+			RK[10] = RK[2] ^ RK[9];
+			RK[11] = RK[3] ^ RK[10];
+
+			RK[12] = RK[4] ^
+						( FSb[ (uint8) ( RK[11]	>> 24 )	] << 24	) ^
+						( FSb[ (uint8) ( RK[11]	>> 16 )	] << 16	) ^
+						( FSb[ (uint8) ( RK[11]	>>	8 )	] <<  8	) ^
+						( FSb[ (uint8) ( RK[11]		  )	]		);
+
+			RK[13] = RK[5] ^ RK[12];
+			RK[14] = RK[6] ^ RK[13];
+			RK[15] = RK[7] ^ RK[14];
+		}
+		break;
+	}
+
+	/* setup decryption	round keys */
+
+	if(	KT_init	)
+	{
+		for( i = 0;	i <	256; i++ )
+		{
+			KT0[i] = RT0[ FSb[i] ];
+			KT1[i] = RT1[ FSb[i] ];
+			KT2[i] = RT2[ FSb[i] ];
+			KT3[i] = RT3[ FSb[i] ];
+		}
+
+		KT_init	= 0;
+	}
+
+	SK = ctx->drk;
+
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+
+	for( i = 1;	i <	ctx->nr; i++ )
+	{
+		RK -= 8;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+	}
+
+	RK -= 8;
+
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+
+	return(	0 );
+}
+
+/* AES 128-bit block encryption	routine	*/
+
+void rtmp_aes_encrypt(aes_context *ctx, uint8 input[16],	uint8 output[16] )
+{
+	uint32 *RK,	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3;
+
+	RK = ctx->erk;
+	GET_UINT32(	X0,	input,	0 ); X0	^= RK[0];
+	GET_UINT32(	X1,	input,	4 ); X1	^= RK[1];
+	GET_UINT32(	X2,	input,	8 ); X2	^= RK[2];
+	GET_UINT32(	X3,	input, 12 ); X3	^= RK[3];
+
+#define	AES_FROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)		\
+{												\
+	RK += 4;									\
+												\
+	X0 = RK[0] ^ FT0[ (uint8) (	Y0 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y1 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y2 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y3		 ) ];	\
+												\
+	X1 = RK[1] ^ FT0[ (uint8) (	Y1 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y2 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y3 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y0		 ) ];	\
+												\
+	X2 = RK[2] ^ FT0[ (uint8) (	Y2 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y3 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y0 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y1		 ) ];	\
+												\
+	X3 = RK[3] ^ FT0[ (uint8) (	Y3 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y0 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y1 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y2		 ) ];	\
+}
+
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 1 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 2 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 3 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 4 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 5 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 6 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 7 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 8 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 9 */
+
+	if(	ctx->nr	> 10 )
+	{
+		AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 10	*/
+		AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 11	*/
+	}
+
+	if(	ctx->nr	> 12 )
+	{
+		AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 12	*/
+		AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 13	*/
+	}
+
+	/* last	round */
+
+	RK += 4;
+
+	X0 = RK[0] ^ ( FSb[	(uint8)	( Y0 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y1 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y2 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y3	   ) ]		 );
+
+	X1 = RK[1] ^ ( FSb[	(uint8)	( Y1 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y2 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y3 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y0	   ) ]		 );
+
+	X2 = RK[2] ^ ( FSb[	(uint8)	( Y2 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y3 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y0 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y1	   ) ]		 );
+
+	X3 = RK[3] ^ ( FSb[	(uint8)	( Y3 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y0 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y1 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y2	   ) ]		 );
+
+	PUT_UINT32(	X0,	output,	 0 );
+	PUT_UINT32(	X1,	output,	 4 );
+	PUT_UINT32(	X2,	output,	 8 );
+	PUT_UINT32(	X3,	output,	12 );
+}
+
+/* AES 128-bit block decryption	routine	*/
+
+void rtmp_aes_decrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] )
+{
+	uint32 *RK,	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3;
+
+	RK = ctx->drk;
+
+	GET_UINT32(	X0,	input,	0 ); X0	^= RK[0];
+	GET_UINT32(	X1,	input,	4 ); X1	^= RK[1];
+	GET_UINT32(	X2,	input,	8 ); X2	^= RK[2];
+	GET_UINT32(	X3,	input, 12 ); X3	^= RK[3];
+
+#define	AES_RROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)		\
+{												\
+	RK += 4;									\
+												\
+	X0 = RK[0] ^ RT0[ (uint8) (	Y0 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y3 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y2 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y1		 ) ];	\
+												\
+	X1 = RK[1] ^ RT0[ (uint8) (	Y1 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y0 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y3 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y2		 ) ];	\
+												\
+	X2 = RK[2] ^ RT0[ (uint8) (	Y2 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y1 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y0 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y3		 ) ];	\
+												\
+	X3 = RK[3] ^ RT0[ (uint8) (	Y3 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y2 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y1 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y0		 ) ];	\
+}
+
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 1 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 2 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 3 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 4 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 5 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 6 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 7 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 8 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 9 */
+
+	if(	ctx->nr	> 10 )
+	{
+		AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 10	*/
+		AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 11	*/
+	}
+
+	if(	ctx->nr	> 12 )
+	{
+		AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 12	*/
+		AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 13	*/
+	}
+
+	/* last	round */
+
+	RK += 4;
+
+	X0 = RK[0] ^ ( RSb[	(uint8)	( Y0 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y3 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y2 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y1	   ) ]		 );
+
+	X1 = RK[1] ^ ( RSb[	(uint8)	( Y1 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y0 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y3 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y2	   ) ]		 );
+
+	X2 = RK[2] ^ ( RSb[	(uint8)	( Y2 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y1 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y0 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y3	   ) ]		 );
+
+	X3 = RK[3] ^ ( RSb[	(uint8)	( Y3 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y2 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y1 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y0	   ) ]		 );
+
+	PUT_UINT32(	X0,	output,	 0 );
+	PUT_UINT32(	X1,	output,	 4 );
+	PUT_UINT32(	X2,	output,	 8 );
+	PUT_UINT32(	X3,	output,	12 );
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		SHA1 function
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	HMAC_SHA1(
+	IN	UCHAR	*text,
+	IN	UINT	text_len,
+	IN	UCHAR	*key,
+	IN	UINT	key_len,
+	IN	UCHAR	*digest)
+{
+	SHA_CTX	context;
+	UCHAR	k_ipad[65]; /* inner padding - key XORd with ipad	*/
+	UCHAR	k_opad[65]; /* outer padding - key XORd with opad	*/
+	INT		i;
+
+	// if key is longer	than 64	bytes reset	it to key=SHA1(key)
+	if (key_len	> 64)
+	{
+		SHA_CTX		 tctx;
+		SHAInit(&tctx);
+		SHAUpdate(&tctx, key, key_len);
+		SHAFinal(&tctx,	key);
+		key_len	= 20;
+	}
+	NdisZeroMemory(k_ipad, sizeof(k_ipad));
+	NdisZeroMemory(k_opad, sizeof(k_opad));
+	NdisMoveMemory(k_ipad, key,	key_len);
+	NdisMoveMemory(k_opad, key,	key_len);
+
+	// XOR key with	ipad and opad values
+	for	(i = 0;	i <	64;	i++)
+	{
+		k_ipad[i] ^= 0x36;
+		k_opad[i] ^= 0x5c;
+	}
+
+	// perform inner SHA1
+	SHAInit(&context); 						/* init context for 1st pass */
+	SHAUpdate(&context,	k_ipad,	64);		/*	start with inner pad */
+	SHAUpdate(&context,	text, text_len);	/*	then text of datagram */
+	SHAFinal(&context, digest);				/* finish up 1st pass */
+
+	//perform outer	SHA1
+	SHAInit(&context);					/* init context for 2nd pass */
+	SHAUpdate(&context,	k_opad,	64);	/*	start with outer pad */
+	SHAUpdate(&context,	digest,	20);	/*	then results of	1st	hash */
+	SHAFinal(&context, digest);			/* finish up 2nd pass */
+
+}
+
+/*
+* F(P, S, c, i) = U1 xor U2 xor ... Uc
+* U1 = PRF(P, S || Int(i))
+* U2 = PRF(P, U1)
+* Uc = PRF(P, Uc-1)
+*/
+
+void F(char *password, unsigned char *ssid, int ssidlength, int iterations, int count, unsigned char *output)
+{
+    unsigned char digest[36], digest1[SHA_DIGEST_LEN];
+    int i, j;
+
+    /* U1 = PRF(P, S || int(i)) */
+    memcpy(digest, ssid, ssidlength);
+    digest[ssidlength] = (unsigned char)((count>>24) & 0xff);
+    digest[ssidlength+1] = (unsigned char)((count>>16) & 0xff);
+    digest[ssidlength+2] = (unsigned char)((count>>8) & 0xff);
+    digest[ssidlength+3] = (unsigned char)(count & 0xff);
+    HMAC_SHA1(digest, ssidlength+4, (unsigned char*) password, (int) strlen(password), digest1); // for WPA update
+
+    /* output = U1 */
+    memcpy(output, digest1, SHA_DIGEST_LEN);
+
+    for (i = 1; i < iterations; i++)
+    {
+        /* Un = PRF(P, Un-1) */
+        HMAC_SHA1(digest1, SHA_DIGEST_LEN, (unsigned char*) password, (int) strlen(password), digest); // for WPA update
+        memcpy(digest1, digest, SHA_DIGEST_LEN);
+
+        /* output = output xor Un */
+        for (j = 0; j < SHA_DIGEST_LEN; j++)
+        {
+            output[j] ^= digest[j];
+        }
+    }
+}
+/*
+* password - ascii string up to 63 characters in length
+* ssid - octet string up to 32 octets
+* ssidlength - length of ssid in octets
+* output must be 40 octets in length and outputs 256 bits of key
+*/
+int PasswordHash(char *password, unsigned char *ssid, int ssidlength, unsigned char *output)
+{
+    if ((strlen(password) > 63) || (ssidlength > 32))
+        return 0;
+
+    F(password, ssid, ssidlength, 4096, 1, output);
+    F(password, ssid, ssidlength, 4096, 2, &output[SHA_DIGEST_LEN]);
+    return 1;
+}
+
+
diff --git a/drivers/staging/rt2870/common/mlme.c b/drivers/staging/rt2870/common/mlme.c
new file mode 100644
index 00000000000..8a82cee8bf2
--- /dev/null
+++ b/drivers/staging/rt2870/common/mlme.c
@@ -0,0 +1,8609 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	mlme.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-08-25		Modify from RT2500 code base
+	John Chang	2004-09-06		modified for RT2600
+*/
+
+#include "../rt_config.h"
+#include <stdarg.h>
+
+UCHAR	CISCO_OUI[] = {0x00, 0x40, 0x96};
+
+UCHAR	WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
+UCHAR	RSN_OUI[] = {0x00, 0x0f, 0xac};
+UCHAR	WAPI_OUI[] = {0x00, 0x14, 0x72};
+UCHAR   WME_INFO_ELEM[]  = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+UCHAR   WME_PARM_ELEM[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+UCHAR	Ccx2QosInfo[] = {0x00, 0x40, 0x96, 0x04};
+UCHAR   RALINK_OUI[]  = {0x00, 0x0c, 0x43};
+UCHAR   BROADCOM_OUI[]  = {0x00, 0x90, 0x4c};
+UCHAR   WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+UCHAR	PRE_N_HT_OUI[]	= {0x00, 0x90, 0x4c};
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+UCHAR RateSwitchTable[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x11, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x21,  0, 30, 50,
+    0x05, 0x21,  1, 20, 50,
+    0x06, 0x21,  2, 20, 50,
+    0x07, 0x21,  3, 15, 50,
+    0x08, 0x21,  4, 15, 30,
+    0x09, 0x21,  5, 10, 25,
+    0x0a, 0x21,  6,  8, 25,
+    0x0b, 0x21,  7,  8, 25,
+    0x0c, 0x20, 12,  15, 30,
+    0x0d, 0x20, 13,  8, 20,
+    0x0e, 0x20, 14,  8, 20,
+    0x0f, 0x20, 15,  8, 25,
+    0x10, 0x22, 15,  8, 25,
+    0x11, 0x00,  0,  0,  0,
+    0x12, 0x00,  0,  0,  0,
+    0x13, 0x00,  0,  0,  0,
+    0x14, 0x00,  0,  0,  0,
+    0x15, 0x00,  0,  0,  0,
+    0x16, 0x00,  0,  0,  0,
+    0x17, 0x00,  0,  0,  0,
+    0x18, 0x00,  0,  0,  0,
+    0x19, 0x00,  0,  0,  0,
+    0x1a, 0x00,  0,  0,  0,
+    0x1b, 0x00,  0,  0,  0,
+    0x1c, 0x00,  0,  0,  0,
+    0x1d, 0x00,  0,  0,  0,
+    0x1e, 0x00,  0,  0,  0,
+    0x1f, 0x00,  0,  0,  0,
+};
+
+UCHAR RateSwitchTable11B[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x04, 0x03,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+};
+
+UCHAR RateSwitchTable11BG[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x10,  2, 20, 35,
+    0x05, 0x10,  3, 16, 35,
+    0x06, 0x10,  4, 10, 25,
+    0x07, 0x10,  5, 16, 25,
+    0x08, 0x10,  6, 10, 25,
+    0x09, 0x10,  7, 10, 13,
+};
+
+UCHAR RateSwitchTable11G[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x08, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x10,  0, 20, 101,
+    0x01, 0x10,  1, 20, 35,
+    0x02, 0x10,  2, 20, 35,
+    0x03, 0x10,  3, 16, 35,
+    0x04, 0x10,  4, 10, 25,
+    0x05, 0x10,  5, 16, 25,
+    0x06, 0x10,  6, 10, 25,
+    0x07, 0x10,  7, 10, 13,
+};
+
+#ifdef DOT11_N_SUPPORT
+UCHAR RateSwitchTable11N1S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x09, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 10, 25,
+    0x06, 0x21,  6,  8, 14,
+    0x07, 0x21,  7,  8, 14,
+    0x08, 0x23,  7,  8, 14,
+};
+
+UCHAR RateSwitchTable11N2S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,      // Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12,  15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N3S[] = {
+// Item No.	Mode	Curr-MCS	TrainUp	TrainDown	// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,      // Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12,  15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N2SForABand[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12,  15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N3SForABand[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12,  15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN1S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0d, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x21,  0, 30,101,	//50
+    0x05, 0x21,  1, 20, 50,
+    0x06, 0x21,  2, 20, 50,
+    0x07, 0x21,  3, 15, 50,
+    0x08, 0x21,  4, 15, 30,
+    0x09, 0x21,  5, 10, 25,
+    0x0a, 0x21,  6,  8, 14,
+    0x0b, 0x21,  7,  8, 14,
+	0x0c, 0x23,  7,  8, 14,
+};
+
+UCHAR RateSwitchTable11BGN2S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12, 15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN3S[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 20, 50,
+    0x04, 0x21,  4, 15, 50,
+#if 1
+    0x05, 0x20, 20, 15, 30,
+    0x06, 0x20, 21,  8, 20,
+    0x07, 0x20, 22,  8, 20,
+    0x08, 0x20, 23,  8, 25,
+    0x09, 0x22, 23,  8, 25,
+#else // for RT2860 2*3 test
+    0x05, 0x20, 12, 15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+#endif
+};
+
+UCHAR RateSwitchTable11BGN2SForABand[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12, 15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN3SForABand[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0c, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x21, 12, 15, 30,
+    0x07, 0x20, 20, 15, 30,
+    0x08, 0x20, 21,  8, 20,
+    0x09, 0x20, 22,  8, 20,
+    0x0a, 0x20, 23,  8, 25,
+    0x0b, 0x22, 23,  8, 25,
+};
+#endif // DOT11_N_SUPPORT //
+
+PUCHAR ReasonString[] = {
+	/* 0  */	 "Reserved",
+	/* 1  */	 "Unspecified Reason",
+	/* 2  */	 "Previous Auth no longer valid",
+	/* 3  */	 "STA is leaving / has left",
+	/* 4  */	 "DIS-ASSOC due to inactivity",
+	/* 5  */	 "AP unable to hanle all associations",
+	/* 6  */	 "class 2 error",
+	/* 7  */	 "class 3 error",
+	/* 8  */	 "STA is leaving / has left",
+	/* 9  */	 "require auth before assoc/re-assoc",
+	/* 10 */	 "Reserved",
+	/* 11 */	 "Reserved",
+	/* 12 */	 "Reserved",
+	/* 13 */	 "invalid IE",
+	/* 14 */	 "MIC error",
+	/* 15 */	 "4-way handshake timeout",
+	/* 16 */	 "2-way (group key) handshake timeout",
+	/* 17 */	 "4-way handshake IE diff among AssosReq/Rsp/Beacon",
+	/* 18 */
+};
+
+extern UCHAR	 OfdmRateToRxwiMCS[];
+// since RT61 has better RX sensibility, we have to limit TX ACK rate not to exceed our normal data TX rate.
+// otherwise the WLAN peer may not be able to receive the ACK thus downgrade its data TX rate
+ULONG BasicRateMask[12]				= {0xfffff001 /* 1-Mbps */, 0xfffff003 /* 2 Mbps */, 0xfffff007 /* 5.5 */, 0xfffff00f /* 11 */,
+									  0xfffff01f /* 6 */	 , 0xfffff03f /* 9 */	  , 0xfffff07f /* 12 */ , 0xfffff0ff /* 18 */,
+									  0xfffff1ff /* 24 */	 , 0xfffff3ff /* 36 */	  , 0xfffff7ff /* 48 */ , 0xffffffff /* 54 */};
+
+UCHAR MULTICAST_ADDR[MAC_ADDR_LEN] = {0x1,  0x00, 0x00, 0x00, 0x00, 0x00};
+UCHAR BROADCAST_ADDR[MAC_ADDR_LEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+UCHAR ZERO_MAC_ADDR[MAC_ADDR_LEN]  = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+// e.g. RssiSafeLevelForTxRate[RATE_36]" means if the current RSSI is greater than
+//		this value, then it's quaranteed capable of operating in 36 mbps TX rate in
+//		clean environment.
+//								  TxRate: 1   2   5.5	11	 6	  9    12	18	 24   36   48	54	 72  100
+CHAR RssiSafeLevelForTxRate[] ={  -92, -91, -90, -87, -88, -86, -85, -83, -81, -78, -72, -71, -40, -40 };
+
+UCHAR  RateIdToMbps[]	 = { 1, 2, 5, 11, 6, 9, 12, 18, 24, 36, 48, 54, 72, 100};
+USHORT RateIdTo500Kbps[] = { 2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 144, 200};
+
+UCHAR  SsidIe	 = IE_SSID;
+UCHAR  SupRateIe = IE_SUPP_RATES;
+UCHAR  ExtRateIe = IE_EXT_SUPP_RATES;
+#ifdef DOT11_N_SUPPORT
+UCHAR  HtCapIe = IE_HT_CAP;
+UCHAR  AddHtInfoIe = IE_ADD_HT;
+UCHAR  NewExtChanIe = IE_SECONDARY_CH_OFFSET;
+#ifdef DOT11N_DRAFT3
+UCHAR  ExtHtCapIe = IE_EXT_CAPABILITY;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+UCHAR  ErpIe	 = IE_ERP;
+UCHAR  DsIe 	 = IE_DS_PARM;
+UCHAR  TimIe	 = IE_TIM;
+UCHAR  WpaIe	 = IE_WPA;
+UCHAR  Wpa2Ie	 = IE_WPA2;
+UCHAR  IbssIe	 = IE_IBSS_PARM;
+UCHAR  Ccx2Ie	 = IE_CCX_V2;
+UCHAR  WapiIe	 = IE_WAPI;
+
+extern UCHAR	WPA_OUI[];
+
+UCHAR	SES_OUI[] = {0x00, 0x90, 0x4c};
+
+UCHAR	ZeroSsid[32] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+
+// Reset the RFIC setting to new series
+RTMP_RF_REGS RF2850RegTable[] = {
+//		ch	 R1 		 R2 		 R3(TX0~4=0) R4
+		{1,  0x98402ecc, 0x984c0786, 0x9816b455, 0x9800510b},
+		{2,  0x98402ecc, 0x984c0786, 0x98168a55, 0x9800519f},
+		{3,  0x98402ecc, 0x984c078a, 0x98168a55, 0x9800518b},
+		{4,  0x98402ecc, 0x984c078a, 0x98168a55, 0x9800519f},
+		{5,  0x98402ecc, 0x984c078e, 0x98168a55, 0x9800518b},
+		{6,  0x98402ecc, 0x984c078e, 0x98168a55, 0x9800519f},
+		{7,  0x98402ecc, 0x984c0792, 0x98168a55, 0x9800518b},
+		{8,  0x98402ecc, 0x984c0792, 0x98168a55, 0x9800519f},
+		{9,  0x98402ecc, 0x984c0796, 0x98168a55, 0x9800518b},
+		{10, 0x98402ecc, 0x984c0796, 0x98168a55, 0x9800519f},
+		{11, 0x98402ecc, 0x984c079a, 0x98168a55, 0x9800518b},
+		{12, 0x98402ecc, 0x984c079a, 0x98168a55, 0x9800519f},
+		{13, 0x98402ecc, 0x984c079e, 0x98168a55, 0x9800518b},
+		{14, 0x98402ecc, 0x984c07a2, 0x98168a55, 0x98005193},
+
+		// 802.11 UNI / HyperLan 2
+		{36, 0x98402ecc, 0x984c099a, 0x98158a55, 0x980ed1a3},
+		{38, 0x98402ecc, 0x984c099e, 0x98158a55, 0x980ed193},
+		{40, 0x98402ec8, 0x984c0682, 0x98158a55, 0x980ed183},
+		{44, 0x98402ec8, 0x984c0682, 0x98158a55, 0x980ed1a3},
+		{46, 0x98402ec8, 0x984c0686, 0x98158a55, 0x980ed18b},
+		{48, 0x98402ec8, 0x984c0686, 0x98158a55, 0x980ed19b},
+		{52, 0x98402ec8, 0x984c068a, 0x98158a55, 0x980ed193},
+		{54, 0x98402ec8, 0x984c068a, 0x98158a55, 0x980ed1a3},
+		{56, 0x98402ec8, 0x984c068e, 0x98158a55, 0x980ed18b},
+		{60, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed183},
+		{62, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed193},
+		{64, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed1a3}, // Plugfest#4, Day4, change RFR3 left4th 9->5.
+
+		// 802.11 HyperLan 2
+		{100, 0x98402ec8, 0x984c06b2, 0x98178a55, 0x980ed783},
+
+		// 2008.04.30 modified
+		// The system team has AN to improve the EVM value
+		// for channel 102 to 108 for the RT2850/RT2750 dual band solution.
+		{102, 0x98402ec8, 0x985c06b2, 0x98578a55, 0x980ed793},
+		{104, 0x98402ec8, 0x985c06b2, 0x98578a55, 0x980ed1a3},
+		{108, 0x98402ecc, 0x985c0a32, 0x98578a55, 0x980ed193},
+
+		{110, 0x98402ecc, 0x984c0a36, 0x98178a55, 0x980ed183},
+		{112, 0x98402ecc, 0x984c0a36, 0x98178a55, 0x980ed19b},
+		{116, 0x98402ecc, 0x984c0a3a, 0x98178a55, 0x980ed1a3},
+		{118, 0x98402ecc, 0x984c0a3e, 0x98178a55, 0x980ed193},
+		{120, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed183},
+		{124, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed193},
+		{126, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed15b}, // 0x980ed1bb->0x980ed15b required by Rory 20070927
+		{128, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed1a3},
+		{132, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed18b},
+		{134, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed193},
+		{136, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed19b},
+		{140, 0x98402ec4, 0x984c038a, 0x98178a55, 0x980ed183},
+
+		// 802.11 UNII
+		{149, 0x98402ec4, 0x984c038a, 0x98178a55, 0x980ed1a7},
+		{151, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed187},
+		{153, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed18f},
+		{157, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed19f},
+		{159, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed1a7},
+		{161, 0x98402ec4, 0x984c0392, 0x98178a55, 0x980ed187},
+		{165, 0x98402ec4, 0x984c0392, 0x98178a55, 0x980ed197},
+
+		// Japan
+		{184, 0x95002ccc, 0x9500491e, 0x9509be55, 0x950c0a0b},
+		{188, 0x95002ccc, 0x95004922, 0x9509be55, 0x950c0a13},
+		{192, 0x95002ccc, 0x95004926, 0x9509be55, 0x950c0a1b},
+		{196, 0x95002ccc, 0x9500492a, 0x9509be55, 0x950c0a23},
+		{208, 0x95002ccc, 0x9500493a, 0x9509be55, 0x950c0a13},
+		{212, 0x95002ccc, 0x9500493e, 0x9509be55, 0x950c0a1b},
+		{216, 0x95002ccc, 0x95004982, 0x9509be55, 0x950c0a23},
+
+		// still lack of MMAC(Japan) ch 34,38,42,46
+};
+UCHAR	NUM_OF_2850_CHNL = (sizeof(RF2850RegTable) / sizeof(RTMP_RF_REGS));
+
+FREQUENCY_ITEM FreqItems3020[] =
+{
+	/**************************************************/
+	// ISM : 2.4 to 2.483 GHz                         //
+	/**************************************************/
+	// 11g
+	/**************************************************/
+	//-CH---N-------R---K-----------
+	{1,    241,  2,  2},
+	{2,    241,	 2,  7},
+	{3,    242,	 2,  2},
+	{4,    242,	 2,  7},
+	{5,    243,	 2,  2},
+	{6,    243,	 2,  7},
+	{7,    244,	 2,  2},
+	{8,    244,	 2,  7},
+	{9,    245,	 2,  2},
+	{10,   245,	 2,  7},
+	{11,   246,	 2,  2},
+	{12,   246,	 2,  7},
+	{13,   247,	 2,  2},
+	{14,   248,	 2,  4},
+};
+#define	NUM_OF_3020_CHNL	(sizeof(FreqItems3020) / sizeof(FREQUENCY_ITEM))
+
+/*
+	==========================================================================
+	Description:
+		initialize the MLME task and its data structure (queue, spinlock,
+		timer, state machines).
+
+	IRQL = PASSIVE_LEVEL
+
+	Return:
+		always return NDIS_STATUS_SUCCESS
+
+	==========================================================================
+*/
+NDIS_STATUS MlmeInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS Status = NDIS_STATUS_SUCCESS;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> MLME Initialize\n"));
+
+	do
+	{
+		Status = MlmeQueueInit(&pAd->Mlme.Queue);
+		if(Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		pAd->Mlme.bRunning = FALSE;
+		NdisAllocateSpinLock(&pAd->Mlme.TaskLock);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			BssTableInit(&pAd->ScanTab);
+
+			// init STA state machines
+			AssocStateMachineInit(pAd, &pAd->Mlme.AssocMachine, pAd->Mlme.AssocFunc);
+			AuthStateMachineInit(pAd, &pAd->Mlme.AuthMachine, pAd->Mlme.AuthFunc);
+			AuthRspStateMachineInit(pAd, &pAd->Mlme.AuthRspMachine, pAd->Mlme.AuthRspFunc);
+			SyncStateMachineInit(pAd, &pAd->Mlme.SyncMachine, pAd->Mlme.SyncFunc);
+			WpaPskStateMachineInit(pAd, &pAd->Mlme.WpaPskMachine, pAd->Mlme.WpaPskFunc);
+			AironetStateMachineInit(pAd, &pAd->Mlme.AironetMachine, pAd->Mlme.AironetFunc);
+
+#ifdef QOS_DLS_SUPPORT
+			DlsStateMachineInit(pAd, &pAd->Mlme.DlsMachine, pAd->Mlme.DlsFunc);
+#endif // QOS_DLS_SUPPORT //
+
+
+			// Since we are using switch/case to implement it, the init is different from the above
+			// state machine init
+			MlmeCntlInit(pAd, &pAd->Mlme.CntlMachine, NULL);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+		ActionStateMachineInit(pAd, &pAd->Mlme.ActMachine, pAd->Mlme.ActFunc);
+
+		// Init mlme periodic timer
+		RTMPInitTimer(pAd, &pAd->Mlme.PeriodicTimer, GET_TIMER_FUNCTION(MlmePeriodicExec), pAd, TRUE);
+
+		// Set mlme periodic timer
+		RTMPSetTimer(&pAd->Mlme.PeriodicTimer, MLME_TASK_EXEC_INTV);
+
+		// software-based RX Antenna diversity
+		RTMPInitTimer(pAd, &pAd->Mlme.RxAntEvalTimer, GET_TIMER_FUNCTION(AsicRxAntEvalTimeout), pAd, FALSE);
+
+	} while (FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- MLME Initialize\n"));
+
+	return Status;
+}
+
+/*
+	==========================================================================
+	Description:
+		main loop of the MLME
+	Pre:
+		Mlme has to be initialized, and there are something inside the queue
+	Note:
+		This function is invoked from MPSetInformation and MPReceive;
+		This task guarantee only one MlmeHandler will run.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeHandler(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_QUEUE_ELEM 	   *Elem = NULL;
+#ifdef APCLI_SUPPORT
+	SHORT apcliIfIndex;
+#endif
+
+	// Only accept MLME and Frame from peer side, no other (control/data) frame should
+	// get into this state machine
+
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	if(pAd->Mlme.bRunning)
+	{
+		NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+		return;
+	}
+	else
+	{
+		pAd->Mlme.bRunning = TRUE;
+	}
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+
+	while (!MlmeQueueEmpty(&pAd->Mlme.Queue))
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS) ||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS) ||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("Device Halted or Removed or MlmeRest, exit MlmeHandler! (queue num = %ld)\n", pAd->Mlme.Queue.Num));
+			break;
+		}
+
+#ifdef RALINK_ATE
+		if(ATE_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now in MlmeHandler\n"));
+			break;
+		}
+#endif // RALINK_ATE //
+
+		//From message type, determine which state machine I should drive
+		if (MlmeDequeue(&pAd->Mlme.Queue, &Elem))
+		{
+#ifdef RT2870
+			if (Elem->MsgType == MT2_RESET_CONF)
+			{
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("!!! reset MLME state machine !!!\n"));
+				MlmeRestartStateMachine(pAd);
+				Elem->Occupied = FALSE;
+				Elem->MsgLen = 0;
+				continue;
+			}
+#endif // RT2870 //
+
+			// if dequeue success
+			switch (Elem->Machine)
+			{
+				// STA state machines
+#ifdef	CONFIG_STA_SUPPORT
+				case ASSOC_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AssocMachine, Elem);
+					break;
+				case AUTH_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AuthMachine, Elem);
+					break;
+				case AUTH_RSP_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AuthRspMachine, Elem);
+					break;
+				case SYNC_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.SyncMachine, Elem);
+					break;
+				case MLME_CNTL_STATE_MACHINE:
+					MlmeCntlMachinePerformAction(pAd, &pAd->Mlme.CntlMachine, Elem);
+					break;
+				case WPA_PSK_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.WpaPskMachine, Elem);
+					break;
+#ifdef LEAP_SUPPORT
+				case LEAP_STATE_MACHINE:
+					LeapMachinePerformAction(pAd, &pAd->Mlme.LeapMachine, Elem);
+					break;
+#endif
+				case AIRONET_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AironetMachine, Elem);
+					break;
+
+#ifdef QOS_DLS_SUPPORT
+				case DLS_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.DlsMachine, Elem);
+					break;
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+				case ACTION_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.ActMachine, Elem);
+					break;
+
+
+
+
+				default:
+					DBGPRINT(RT_DEBUG_TRACE, ("ERROR: Illegal machine %ld in MlmeHandler()\n", Elem->Machine));
+					break;
+			} // end of switch
+
+			// free MLME element
+			Elem->Occupied = FALSE;
+			Elem->MsgLen = 0;
+
+		}
+		else {
+			DBGPRINT_ERR(("MlmeHandler: MlmeQueue empty\n"));
+		}
+	}
+
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	pAd->Mlme.bRunning = FALSE;
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+}
+
+/*
+	==========================================================================
+	Description:
+		Destructor of MLME (Destroy queue, state machine, spin lock and timer)
+	Parameters:
+		Adapter - NIC Adapter pointer
+	Post:
+		The MLME task will no longer work properly
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeHalt(
+	IN PRTMP_ADAPTER pAd)
+{
+	BOOLEAN 	  Cancelled;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeHalt\n"));
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		// disable BEACON generation and other BEACON related hardware timers
+		AsicDisableSync(pAd);
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef QOS_DLS_SUPPORT
+		UCHAR		i;
+#endif // QOS_DLS_SUPPORT //
+		// Cancel pending timers
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,	&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,		&Cancelled);
+
+#ifdef QOS_DLS_SUPPORT
+		for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &Cancelled);
+		}
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMPCancelTimer(&pAd->Mlme.PeriodicTimer,		&Cancelled);
+	RTMPCancelTimer(&pAd->Mlme.RxAntEvalTimer,		&Cancelled);
+
+
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		// Set LED
+		RTMPSetLED(pAd, LED_HALT);
+        RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, firmware is not done it.
+#ifdef RT2870
+        {
+            LED_CFG_STRUC LedCfg;
+            RTMP_IO_READ32(pAd, LED_CFG, &LedCfg.word);
+            LedCfg.field.LedPolar = 0;
+            LedCfg.field.RLedMode = 0;
+            LedCfg.field.GLedMode = 0;
+            LedCfg.field.YLedMode = 0;
+            RTMP_IO_WRITE32(pAd, LED_CFG, LedCfg.word);
+        }
+#endif // RT2870 //
+	}
+
+	RTMPusecDelay(5000);    //  5 msec to gurantee Ant Diversity timer canceled
+
+	MlmeQueueDestroy(&pAd->Mlme.Queue);
+	NdisFreeSpinLock(&pAd->Mlme.TaskLock);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeHalt\n"));
+}
+
+VOID MlmeResetRalinkCounters(
+	IN  PRTMP_ADAPTER   pAd)
+{
+	pAd->RalinkCounters.LastOneSecRxOkDataCnt = pAd->RalinkCounters.OneSecRxOkDataCnt;
+	// clear all OneSecxxx counters.
+	pAd->RalinkCounters.OneSecBeaconSentCnt = 0;
+	pAd->RalinkCounters.OneSecFalseCCACnt = 0;
+	pAd->RalinkCounters.OneSecRxFcsErrCnt = 0;
+	pAd->RalinkCounters.OneSecRxOkCnt = 0;
+	pAd->RalinkCounters.OneSecTxFailCount = 0;
+	pAd->RalinkCounters.OneSecTxNoRetryOkCount = 0;
+	pAd->RalinkCounters.OneSecTxRetryOkCount = 0;
+	pAd->RalinkCounters.OneSecRxOkDataCnt = 0;
+
+	// TODO: for debug only. to be removed
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_BE] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_BK] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_VI] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_VO] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_BE] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_BK] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_VI] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_VO] = 0;
+	pAd->RalinkCounters.OneSecTxDoneCount = 0;
+	pAd->RalinkCounters.OneSecRxCount = 0;
+	pAd->RalinkCounters.OneSecTxAggregationCount = 0;
+	pAd->RalinkCounters.OneSecRxAggregationCount = 0;
+
+	return;
+}
+
+unsigned long rx_AMSDU;
+unsigned long rx_Total;
+
+/*
+	==========================================================================
+	Description:
+		This routine is executed periodically to -
+		1. Decide if it's a right time to turn on PwrMgmt bit of all
+		   outgoiing frames
+		2. Calculate ChannelQuality based on statistics of the last
+		   period, so that TX rate won't toggling very frequently between a
+		   successful TX and a failed TX.
+		3. If the calculated ChannelQuality indicated current connection not
+		   healthy, then a ROAMing attempt is tried here.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+#define ADHOC_BEACON_LOST_TIME		(8*OS_HZ)  // 8 sec
+VOID MlmePeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	ULONG			TxTotalCnt;
+	PRTMP_ADAPTER	pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_RADIO_OFF |
+								fRTMP_ADAPTER_RADIO_MEASUREMENT |
+								fRTMP_ADAPTER_RESET_IN_PROGRESS))))
+		return;
+
+	RT28XX_MLME_PRE_SANITY_CHECK(pAd);
+
+#ifdef RALINK_ATE
+	/* Do not show RSSI until "Normal 1 second Mlme PeriodicExec". */
+	if (ATE_ON(pAd))
+	{
+		if (pAd->Mlme.PeriodicRound % MLME_TASK_EXEC_MULTIPLE != (MLME_TASK_EXEC_MULTIPLE - 1))
+	{
+			pAd->Mlme.PeriodicRound ++;
+			return;
+		}
+	}
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Do nothing if monitor mode is on
+		if (MONITOR_ON(pAd))
+			return;
+
+		if (pAd->Mlme.PeriodicRound & 0x1)
+		{
+			// This is the fix for wifi 11n extension channel overlapping test case.  for 2860D
+			if (((pAd->MACVersion & 0xffff) == 0x0101) &&
+				(STA_TGN_WIFI_ON(pAd)) &&
+				(pAd->CommonCfg.IOTestParm.bToggle == FALSE))
+
+				{
+					RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x24Bf);
+					pAd->CommonCfg.IOTestParm.bToggle = TRUE;
+				}
+				else if ((STA_TGN_WIFI_ON(pAd)) &&
+						((pAd->MACVersion & 0xffff) == 0x0101))
+				{
+					RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x243f);
+					pAd->CommonCfg.IOTestParm.bToggle = FALSE;
+				}
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->bUpdateBcnCntDone = FALSE;
+
+//	RECBATimerTimeout(SystemSpecific1,FunctionContext,SystemSpecific2,SystemSpecific3);
+	pAd->Mlme.PeriodicRound ++;
+
+	// execute every 500ms
+	if ((pAd->Mlme.PeriodicRound % 5 == 0) && RTMPAutoRateSwitchCheck(pAd)/*(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED))*/)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		// perform dynamic tx rate switching based on past TX history
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if ((OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+					)
+				&& (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE)))
+				MlmeDynamicTxRateSwitching(pAd);
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// Normal 1 second Mlme PeriodicExec.
+	if (pAd->Mlme.PeriodicRound %MLME_TASK_EXEC_MULTIPLE == 0)
+	{
+                pAd->Mlme.OneSecPeriodicRound ++;
+
+#ifdef RALINK_ATE
+    	if (ATE_ON(pAd))
+    	{
+			/* request from Baron : move this routine from later to here */
+			/* for showing Rx error count in ATE RXFRAME */
+            NICUpdateRawCounters(pAd);
+			if (pAd->ate.bRxFer == 1)
+			{
+				pAd->ate.RxTotalCnt += pAd->ate.RxCntPerSec;
+			    ate_print(KERN_EMERG "MlmePeriodicExec: Rx packet cnt = %d/%d\n", pAd->ate.RxCntPerSec, pAd->ate.RxTotalCnt);
+				pAd->ate.RxCntPerSec = 0;
+
+				if (pAd->ate.RxAntennaSel == 0)
+					ate_print(KERN_EMERG "MlmePeriodicExec: Rx AvgRssi0=%d, AvgRssi1=%d, AvgRssi2=%d\n\n",
+						pAd->ate.AvgRssi0, pAd->ate.AvgRssi1, pAd->ate.AvgRssi2);
+				else
+					ate_print(KERN_EMERG "MlmePeriodicExec: Rx AvgRssi=%d\n\n", pAd->ate.AvgRssi0);
+			}
+			MlmeResetRalinkCounters(pAd);
+			return;
+    	}
+#endif // RALINK_ATE //
+
+
+		if (rx_Total)
+		{
+
+			// reset counters
+			rx_AMSDU = 0;
+			rx_Total = 0;
+		}
+
+		//ORIBATimerTimeout(pAd);
+
+		// Media status changed, report to NDIS
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE))
+		{
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE);
+			if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+			{
+				pAd->IndicateMediaState = NdisMediaStateConnected;
+				RTMP_IndicateMediaState(pAd);
+
+			}
+			else
+			{
+				pAd->IndicateMediaState = NdisMediaStateDisconnected;
+				RTMP_IndicateMediaState(pAd);
+			}
+		}
+
+		NdisGetSystemUpTime(&pAd->Mlme.Now32);
+
+		// add the most up-to-date h/w raw counters into software variable, so that
+		// the dynamic tuning mechanism below are based on most up-to-date information
+		NICUpdateRawCounters(pAd);
+
+#ifdef RT2870
+		RT2870_WatchDog(pAd);
+#endif // RT2870 //
+
+#ifdef DOT11_N_SUPPORT
+   		// Need statistics after read counter. So put after NICUpdateRawCounters
+		ORIBATimerTimeout(pAd);
+#endif // DOT11_N_SUPPORT //
+
+		// if MGMT RING is full more than twice within 1 second, we consider there's
+		// a hardware problem stucking the TX path. In this case, try a hardware reset
+		// to recover the system
+	//	if (pAd->RalinkCounters.MgmtRingFullCount >= 2)
+	//		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HARDWARE_ERROR);
+	//	else
+	//		pAd->RalinkCounters.MgmtRingFullCount = 0;
+
+		// The time period for checking antenna is according to traffic
+		if (pAd->Mlme.bEnableAutoAntennaCheck)
+		{
+			TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+							 pAd->RalinkCounters.OneSecTxRetryOkCount +
+							 pAd->RalinkCounters.OneSecTxFailCount;
+
+			if (TxTotalCnt > 50)
+			{
+				if (pAd->Mlme.OneSecPeriodicRound % 10 == 0)
+				{
+					AsicEvaluateRxAnt(pAd);
+				}
+			}
+			else
+			{
+				if (pAd->Mlme.OneSecPeriodicRound % 3 == 0)
+				{
+					AsicEvaluateRxAnt(pAd);
+				}
+			}
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			STAMlmePeriodicExec(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+		MlmeResetRalinkCounters(pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			{
+				// When Adhoc beacon is enabled and RTS/CTS is enabled, there is a chance that hardware MAC FSM will run into a deadlock
+				// and sending CTS-to-self over and over.
+				// Software Patch Solution:
+				// 1. Polling debug state register 0x10F4 every one second.
+				// 2. If in 0x10F4 the ((bit29==1) && (bit7==1)) OR ((bit29==1) && (bit5==1)), it means the deadlock has occurred.
+				// 3. If the deadlock occurred, reset MAC/BBP by setting 0x1004 to 0x0001 for a while then setting it back to 0x000C again.
+
+				UINT32	MacReg = 0;
+
+				RTMP_IO_READ32(pAd, 0x10F4, &MacReg);
+				if (((MacReg & 0x20000000) && (MacReg & 0x80)) || ((MacReg & 0x20000000) && (MacReg & 0x20)))
+				{
+					RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+					RTMPusecDelay(1);
+					RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0xC);
+
+					DBGPRINT(RT_DEBUG_WARN,("Warning, MAC specific condition occurs \n"));
+				}
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+
+	pAd->bUpdateBcnCntDone = FALSE;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+VOID STAMlmePeriodicExec(
+	PRTMP_ADAPTER pAd)
+{
+	ULONG	TxTotalCnt;
+	int 	i;
+
+//
+// We return here in ATE mode, because the statistics
+// that ATE needs are not collected via this routine.
+//
+#ifdef RALINK_ATE
+	// It is supposed that we will never reach here in ATE mode.
+	ASSERT(!(ATE_ON(pAd)));
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_DISABLE)
+#endif // WPA_SUPPLICANT_SUPPORT //
+    {
+    	// WPA MIC error should block association attempt for 60 seconds
+    	if (pAd->StaCfg.bBlockAssoc && (pAd->StaCfg.LastMicErrorTime + (60 * OS_HZ) < pAd->Mlme.Now32))
+    		pAd->StaCfg.bBlockAssoc = FALSE;
+    }
+
+    if ((pAd->PreMediaState != pAd->IndicateMediaState) && (pAd->CommonCfg.bWirelessEvent))
+	{
+		if (pAd->IndicateMediaState == NdisMediaStateConnected)
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKUP_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+		pAd->PreMediaState = pAd->IndicateMediaState;
+	}
+
+
+
+
+   	AsicStaBbpTuning(pAd);
+
+	TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+					 pAd->RalinkCounters.OneSecTxRetryOkCount +
+					 pAd->RalinkCounters.OneSecTxFailCount;
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		// update channel quality for Roaming and UI LinkQuality display
+		MlmeCalculateChannelQuality(pAd, pAd->Mlme.Now32);
+	}
+
+	// must be AFTER MlmeDynamicTxRateSwitching() because it needs to know if
+	// Radio is currently in noisy environment
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		AsicAdjustTxPower(pAd);
+
+	if (INFRA_ON(pAd))
+	{
+#ifdef QOS_DLS_SUPPORT
+		// Check DLS time out, then tear down those session
+		RTMPCheckDLSTimeOut(pAd);
+#endif // QOS_DLS_SUPPORT //
+
+		// Is PSM bit consistent with user power management policy?
+		// This is the only place that will set PSM bit ON.
+		if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+		MlmeCheckPsmChange(pAd, pAd->Mlme.Now32);
+
+		pAd->RalinkCounters.LastOneSecTotalTxCount = TxTotalCnt;
+
+		if ((pAd->StaCfg.LastBeaconRxTime + 1*OS_HZ < pAd->Mlme.Now32) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) &&
+			((TxTotalCnt + pAd->RalinkCounters.OneSecRxOkCnt < 600)))
+		{
+			RTMPSetAGCInitValue(pAd, BW_20);
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - No BEACON. restore R66 to the low bound(%d) \n", (0x2E + GET_LNA_GAIN(pAd))));
+		}
+
+        //if ((pAd->RalinkCounters.OneSecTxNoRetryOkCount == 0) &&
+        //    (pAd->RalinkCounters.OneSecTxRetryOkCount == 0))
+        {
+    		if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable)
+    		{
+    		    // When APSD is enabled, the period changes as 20 sec
+    			if ((pAd->Mlme.OneSecPeriodicRound % 20) == 8)
+    				RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+    		}
+    		else
+    		{
+    		    // Send out a NULL frame every 10 sec to inform AP that STA is still alive (Avoid being age out)
+    			if ((pAd->Mlme.OneSecPeriodicRound % 10) == 8)
+                {
+                    if (pAd->CommonCfg.bWmmCapable)
+    					RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+                    else
+						RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, FALSE);
+                }
+    		}
+        }
+
+		if (CQI_IS_DEAD(pAd->Mlme.ChannelQuality))
+			{
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - No BEACON. Dead CQI. Auto Recovery attempt #%ld\n", pAd->RalinkCounters.BadCQIAutoRecoveryCount));
+			pAd->StaCfg.CCXAdjacentAPReportFlag = TRUE;
+			pAd->StaCfg.CCXAdjacentAPLinkDownTime = pAd->StaCfg.LastBeaconRxTime;
+
+			// Lost AP, send disconnect & link down event
+			LinkDown(pAd, FALSE);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP)
+			{
+                union iwreq_data    wrqu;
+                //send disassociate event to wpa_supplicant
+                memset(&wrqu, 0, sizeof(wrqu));
+                wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+                wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+			// RTMPPatchMacBbpBug(pAd);
+			MlmeAutoReconnectLastSSID(pAd);
+		}
+		else if (CQI_IS_BAD(pAd->Mlme.ChannelQuality))
+		{
+			pAd->RalinkCounters.BadCQIAutoRecoveryCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Bad CQI. Auto Recovery attempt #%ld\n", pAd->RalinkCounters.BadCQIAutoRecoveryCount));
+			MlmeAutoReconnectLastSSID(pAd);
+		}
+
+		// Add auto seamless roaming
+		if (pAd->StaCfg.bFastRoaming)
+		{
+			SHORT	dBmToRoam = (SHORT)pAd->StaCfg.dBmToRoam;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("Rssi=%d, dBmToRoam=%d\n", RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2), (CHAR)dBmToRoam));
+
+			if (RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2) <= (CHAR)dBmToRoam)
+			{
+				MlmeCheckForFastRoaming(pAd, pAd->Mlme.Now32);
+			}
+		}
+	}
+	else if (ADHOC_ON(pAd))
+	{
+		//radar detect
+		if ((pAd->CommonCfg.Channel > 14)
+			&& (pAd->CommonCfg.bIEEE80211H == 1)
+			&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+		{
+			RadarDetectPeriodic(pAd);
+		}
+
+		// If all peers leave, and this STA becomes the last one in this IBSS, then change MediaState
+		// to DISCONNECTED. But still holding this IBSS (i.e. sending BEACON) so that other STAs can
+		// join later.
+		if ((pAd->StaCfg.LastBeaconRxTime + ADHOC_BEACON_LOST_TIME < pAd->Mlme.Now32) &&
+			OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		{
+			MLME_START_REQ_STRUCT     StartReq;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - excessive BEACON lost, last STA in this IBSS, MediaState=Disconnected\n"));
+			LinkDown(pAd, FALSE);
+
+			StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+		}
+
+		for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+		{
+			MAC_TABLE_ENTRY *pEntry = &pAd->MacTab.Content[i];
+
+			if (pEntry->ValidAsCLI == FALSE)
+				continue;
+
+			if (pEntry->LastBeaconRxTime + ADHOC_BEACON_LOST_TIME < pAd->Mlme.Now32)
+				MacTableDeleteEntry(pAd, pEntry->Aid, pEntry->Addr);
+		}
+	}
+	else // no INFRA nor ADHOC connection
+	{
+
+		if (pAd->StaCfg.bScanReqIsFromWebUI &&
+            ((pAd->StaCfg.LastScanTime + 30 * OS_HZ) > pAd->Mlme.Now32))
+			goto SKIP_AUTO_SCAN_CONN;
+        else
+            pAd->StaCfg.bScanReqIsFromWebUI = FALSE;
+
+		if ((pAd->StaCfg.bAutoReconnect == TRUE)
+			&& RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP)
+			&& (MlmeValidateSSID(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen) == TRUE))
+		{
+			if ((pAd->ScanTab.BssNr==0) && (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE))
+			{
+				MLME_SCAN_REQ_STRUCT	   ScanReq;
+
+				if ((pAd->StaCfg.LastScanTime + 10 * OS_HZ) < pAd->Mlme.Now32)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("STAMlmePeriodicExec():CNTL - ScanTab.BssNr==0, start a new ACTIVE scan SSID[%s]\n", pAd->MlmeAux.AutoReconnectSsid));
+					ScanParmFill(pAd, &ScanReq, pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen, BSS_ANY, SCAN_ACTIVE);
+					MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+					pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+					// Reset Missed scan number
+					pAd->StaCfg.LastScanTime = pAd->Mlme.Now32;
+				}
+				else if (pAd->StaCfg.BssType == BSS_ADHOC)	// Quit the forever scan when in a very clean room
+					MlmeAutoReconnectLastSSID(pAd);
+			}
+			else if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+			{
+				if ((pAd->Mlme.OneSecPeriodicRound % 7) == 0)
+				{
+					MlmeAutoScan(pAd);
+					pAd->StaCfg.LastScanTime = pAd->Mlme.Now32;
+				}
+				else
+				{
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+					if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+					{
+						if ((pAd->Mlme.OneSecPeriodicRound % 5) == 1)
+							MlmeAutoReconnectLastSSID(pAd);
+					}
+					else
+#endif // CARRIER_DETECTION_SUPPORT //
+						MlmeAutoReconnectLastSSID(pAd);
+				}
+			}
+		}
+	}
+
+SKIP_AUTO_SCAN_CONN:
+
+#ifdef DOT11_N_SUPPORT
+    if ((pAd->MacTab.Content[BSSID_WCID].TXBAbitmap !=0) && (pAd->MacTab.fAnyBASession == FALSE))
+	{
+		pAd->MacTab.fAnyBASession = TRUE;
+		AsicUpdateProtect(pAd, HT_FORCERTSCTS,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+	else if ((pAd->MacTab.Content[BSSID_WCID].TXBAbitmap ==0) && (pAd->MacTab.fAnyBASession == TRUE))
+	{
+		pAd->MacTab.fAnyBASession = FALSE;
+		AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SCAN_2040))
+		TriEventCounterMaintenance(pAd);
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	return;
+}
+
+// Link down report
+VOID LinkDownExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	pAd->IndicateMediaState = NdisMediaStateDisconnected;
+	RTMP_IndicateMediaState(pAd);
+    pAd->ExtraInfo = GENERAL_LINK_DOWN;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeAutoScan(
+	IN PRTMP_ADAPTER pAd)
+{
+	// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+	if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Driver auto scan\n"));
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_BSSID_LIST_SCAN,
+					0,
+					NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeAutoReconnectLastSSID(
+	IN PRTMP_ADAPTER pAd)
+{
+
+
+	// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+	if ((pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE) &&
+		(MlmeValidateSSID(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen) == TRUE))
+	{
+		NDIS_802_11_SSID OidSsid;
+		OidSsid.SsidLength = pAd->MlmeAux.AutoReconnectSsidLen;
+		NdisMoveMemory(OidSsid.Ssid, pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Driver auto reconnect to last OID_802_11_SSID setting - %s, len - %d\n", pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen));
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_SSID,
+					sizeof(NDIS_802_11_SSID),
+					&OidSsid);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	==========================================================================
+	Validate SSID for connection try and rescan purpose
+	Valid SSID will have visible chars only.
+	The valid length is from 0 to 32.
+	IRQL = DISPATCH_LEVEL
+	==========================================================================
+ */
+BOOLEAN MlmeValidateSSID(
+	IN PUCHAR	pSsid,
+	IN UCHAR	SsidLen)
+{
+	int	index;
+
+	if (SsidLen > MAX_LEN_OF_SSID)
+		return (FALSE);
+
+	// Check each character value
+	for (index = 0; index < SsidLen; index++)
+	{
+		if (pSsid[index] < 0x20)
+			return (FALSE);
+	}
+
+	// All checked
+	return (TRUE);
+}
+
+VOID MlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx)
+{
+	do
+	{
+		// decide the rate table for tuning
+		if (pAd->CommonCfg.TxRateTableSize > 0)
+		{
+			*ppTable = RateSwitchTable;
+			*pTableSize = RateSwitchTable[0];
+			*pInitTxRateIdx = RateSwitchTable[1];
+
+			break;
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((pAd->OpMode == OPMODE_STA) && ADHOC_ON(pAd))
+		{
+#ifdef DOT11_N_SUPPORT
+			if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) &&
+				(pEntry->HTCapability.MCSSet[0] == 0xff) &&
+				((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->Antenna.field.TxPath == 1)))
+			{// 11N 1S Adhoc
+				*ppTable = RateSwitchTable11N1S;
+				*pTableSize = RateSwitchTable11N1S[0];
+				*pInitTxRateIdx = RateSwitchTable11N1S[1];
+
+			}
+			else if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) &&
+					(pEntry->HTCapability.MCSSet[0] == 0xff) &&
+					(pEntry->HTCapability.MCSSet[1] == 0xff) &&
+					(pAd->Antenna.field.TxPath == 2))
+			{// 11N 2S Adhoc
+				if (pAd->LatchRfRegs.Channel <= 14)
+				{
+					*ppTable = RateSwitchTable11N2S;
+					*pTableSize = RateSwitchTable11N2S[0];
+					*pInitTxRateIdx = RateSwitchTable11N2S[1];
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2SForABand;
+					*pTableSize = RateSwitchTable11N2SForABand[0];
+					*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+				}
+
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+				if ((pEntry->RateLen == 4)
+#ifdef DOT11_N_SUPPORT
+					&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+					)
+			{
+				*ppTable = RateSwitchTable11B;
+				*pTableSize = RateSwitchTable11B[0];
+				*pInitTxRateIdx = RateSwitchTable11B[1];
+
+			}
+			else if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				*ppTable = RateSwitchTable11BG;
+				*pTableSize = RateSwitchTable11BG[0];
+				*pInitTxRateIdx = RateSwitchTable11BG[1];
+
+			}
+			else
+			{
+				*ppTable = RateSwitchTable11G;
+				*pTableSize = RateSwitchTable11G[0];
+				*pInitTxRateIdx = RateSwitchTable11G[1];
+
+			}
+			break;
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+		//if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen == 12) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) &&
+		//	((pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0x00) || (pAd->Antenna.field.TxPath == 1)))
+		if ((pEntry->RateLen == 12) && (pEntry->HTCapability.MCSSet[0] == 0xff) &&
+			((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->CommonCfg.TxStream == 1)))
+		{// 11BGN 1S AP
+			*ppTable = RateSwitchTable11BGN1S;
+			*pTableSize = RateSwitchTable11BGN1S[0];
+			*pInitTxRateIdx = RateSwitchTable11BGN1S[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen == 12) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) &&
+		//	(pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0xff) && (pAd->Antenna.field.TxPath == 2))
+		if ((pEntry->RateLen == 12) && (pEntry->HTCapability.MCSSet[0] == 0xff) &&
+			(pEntry->HTCapability.MCSSet[1] == 0xff) && (pAd->CommonCfg.TxStream == 2))
+		{// 11BGN 2S AP
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				*ppTable = RateSwitchTable11BGN2S;
+				*pTableSize = RateSwitchTable11BGN2S[0];
+				*pInitTxRateIdx = RateSwitchTable11BGN2S[1];
+
+			}
+			else
+			{
+				*ppTable = RateSwitchTable11BGN2SForABand;
+				*pTableSize = RateSwitchTable11BGN2SForABand[0];
+				*pInitTxRateIdx = RateSwitchTable11BGN2SForABand[1];
+
+			}
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) && ((pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0x00) || (pAd->Antenna.field.TxPath == 1)))
+		if ((pEntry->HTCapability.MCSSet[0] == 0xff) && ((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->CommonCfg.TxStream == 1)))
+		{// 11N 1S AP
+			*ppTable = RateSwitchTable11N1S;
+			*pTableSize = RateSwitchTable11N1S[0];
+			*pInitTxRateIdx = RateSwitchTable11N1S[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0xff) && (pAd->Antenna.field.TxPath == 2))
+		if ((pEntry->HTCapability.MCSSet[0] == 0xff) && (pEntry->HTCapability.MCSSet[1] == 0xff) && (pAd->CommonCfg.TxStream == 2))
+		{// 11N 2S AP
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+			*ppTable = RateSwitchTable11N2S;
+			*pTableSize = RateSwitchTable11N2S[0];
+			*pInitTxRateIdx = RateSwitchTable11N2S[1];
+            }
+			else
+			{
+				*ppTable = RateSwitchTable11N2SForABand;
+				*pTableSize = RateSwitchTable11N2SForABand[0];
+				*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+			}
+
+			break;
+		}
+#endif // DOT11_N_SUPPORT //
+		//else if ((pAd->StaActive.SupRateLen == 4) && (pAd->StaActive.ExtRateLen == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen == 4)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// B only AP
+			*ppTable = RateSwitchTable11B;
+			*pTableSize = RateSwitchTable11B[0];
+			*pInitTxRateIdx = RateSwitchTable11B[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen > 8) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen > 8)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// B/G  mixed AP
+			*ppTable = RateSwitchTable11BG;
+			*pTableSize = RateSwitchTable11BG[0];
+			*pInitTxRateIdx = RateSwitchTable11BG[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen == 8) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen == 8)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// G only AP
+			*ppTable = RateSwitchTable11G;
+			*pTableSize = RateSwitchTable11G[0];
+			*pInitTxRateIdx = RateSwitchTable11G[1];
+
+			break;
+		}
+#ifdef DOT11_N_SUPPORT
+#endif // DOT11_N_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef DOT11_N_SUPPORT
+			//else if ((pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+			if ((pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0))
+#endif // DOT11_N_SUPPORT //
+			{	// Legacy mode
+				if (pAd->CommonCfg.MaxTxRate <= RATE_11)
+				{
+					*ppTable = RateSwitchTable11B;
+					*pTableSize = RateSwitchTable11B[0];
+					*pInitTxRateIdx = RateSwitchTable11B[1];
+				}
+				else if ((pAd->CommonCfg.MaxTxRate > RATE_11) && (pAd->CommonCfg.MinTxRate > RATE_11))
+				{
+					*ppTable = RateSwitchTable11G;
+					*pTableSize = RateSwitchTable11G[0];
+					*pInitTxRateIdx = RateSwitchTable11G[1];
+
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11BG;
+					*pTableSize = RateSwitchTable11BG[0];
+					*pInitTxRateIdx = RateSwitchTable11BG[1];
+				}
+				break;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				if (pAd->CommonCfg.TxStream == 1)
+				{
+					*ppTable = RateSwitchTable11N1S;
+					*pTableSize = RateSwitchTable11N1S[0];
+					*pInitTxRateIdx = RateSwitchTable11N1S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 1S AP \n"));
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2S;
+					*pTableSize = RateSwitchTable11N2S[0];
+					*pInitTxRateIdx = RateSwitchTable11N2S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 2S AP \n"));
+				}
+			}
+			else
+			{
+				if (pAd->CommonCfg.TxStream == 1)
+				{
+					*ppTable = RateSwitchTable11N1S;
+					*pTableSize = RateSwitchTable11N1S[0];
+					*pInitTxRateIdx = RateSwitchTable11N1S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 1S AP \n"));
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2SForABand;
+					*pTableSize = RateSwitchTable11N2SForABand[0];
+					*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 2S AP \n"));
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode (SupRateLen=%d, ExtRateLen=%d, MCSSet[0]=0x%x, MCSSet[1]=0x%x)\n",
+				pAd->StaActive.SupRateLen, pAd->StaActive.ExtRateLen, pAd->StaActive.SupportedPhyInfo.MCSSet[0], pAd->StaActive.SupportedPhyInfo.MCSSet[1]));
+		}
+#endif // CONFIG_STA_SUPPORT //
+	} while(FALSE);
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		This routine checks if there're other APs out there capable for
+		roaming. Caller should call this routine only when Link up in INFRA mode
+		and channel quality is below CQI_GOOD_THRESHOLD.
+
+	IRQL = DISPATCH_LEVEL
+
+	Output:
+	==========================================================================
+ */
+VOID MlmeCheckForRoaming(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG	Now32)
+{
+	USHORT	   i;
+	BSS_TABLE  *pRoamTab = &pAd->MlmeAux.RoamTab;
+	BSS_ENTRY  *pBss;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeCheckForRoaming\n"));
+	// put all roaming candidates into RoamTab, and sort in RSSI order
+	BssTableInit(pRoamTab);
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		pBss = &pAd->ScanTab.BssEntry[i];
+
+		if ((pBss->LastBeaconRxTime + BEACON_LOST_TIME) < Now32)
+			continue;	 // AP disappear
+		if (pBss->Rssi <= RSSI_THRESHOLD_FOR_ROAMING)
+			continue;	 // RSSI too weak. forget it.
+		if (MAC_ADDR_EQUAL(pBss->Bssid, pAd->CommonCfg.Bssid))
+			continue;	 // skip current AP
+		if (pBss->Rssi < (pAd->StaCfg.RssiSample.LastRssi0 + RSSI_DELTA))
+			continue;	 // only AP with stronger RSSI is eligible for roaming
+
+		// AP passing all above rules is put into roaming candidate table
+		NdisMoveMemory(&pRoamTab->BssEntry[pRoamTab->BssNr], pBss, sizeof(BSS_ENTRY));
+		pRoamTab->BssNr += 1;
+	}
+
+	if (pRoamTab->BssNr > 0)
+	{
+		// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+		{
+			pAd->RalinkCounters.PoorCQIRoamingCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming attempt #%ld\n", pAd->RalinkCounters.PoorCQIRoamingCount));
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_MLME_ROAMING_REQ, 0, NULL);
+			RT28XX_MLME_HANDLER(pAd);
+		}
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeCheckForRoaming(# of candidate= %d)\n",pRoamTab->BssNr));
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine checks if there're other APs out there capable for
+		roaming. Caller should call this routine only when link up in INFRA mode
+		and channel quality is below CQI_GOOD_THRESHOLD.
+
+	IRQL = DISPATCH_LEVEL
+
+	Output:
+	==========================================================================
+ */
+VOID MlmeCheckForFastRoaming(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG			Now)
+{
+	USHORT		i;
+	BSS_TABLE	*pRoamTab = &pAd->MlmeAux.RoamTab;
+	BSS_ENTRY	*pBss;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeCheckForFastRoaming\n"));
+	// put all roaming candidates into RoamTab, and sort in RSSI order
+	BssTableInit(pRoamTab);
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		pBss = &pAd->ScanTab.BssEntry[i];
+
+        if ((pBss->Rssi <= -50) && (pBss->Channel == pAd->CommonCfg.Channel))
+			continue;	 // RSSI too weak. forget it.
+		if (MAC_ADDR_EQUAL(pBss->Bssid, pAd->CommonCfg.Bssid))
+			continue;	 // skip current AP
+		if (!SSID_EQUAL(pBss->Ssid, pBss->SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen))
+			continue;	 // skip different SSID
+        if (pBss->Rssi < (RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2) + RSSI_DELTA))
+			continue;	 // skip AP without better RSSI
+
+        DBGPRINT(RT_DEBUG_TRACE, ("LastRssi0 = %d, pBss->Rssi = %d\n", RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2), pBss->Rssi));
+		// AP passing all above rules is put into roaming candidate table
+		NdisMoveMemory(&pRoamTab->BssEntry[pRoamTab->BssNr], pBss, sizeof(BSS_ENTRY));
+		pRoamTab->BssNr += 1;
+	}
+
+	if (pRoamTab->BssNr > 0)
+	{
+		// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+		{
+			pAd->RalinkCounters.PoorCQIRoamingCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming attempt #%ld\n", pAd->RalinkCounters.PoorCQIRoamingCount));
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_MLME_ROAMING_REQ, 0, NULL);
+			RT28XX_MLME_HANDLER(pAd);
+		}
+	}
+	// Maybe site survey required
+	else
+	{
+		if ((pAd->StaCfg.LastScanTime + 10 * 1000) < Now)
+		{
+			// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming, No eligable entry, try new scan!\n"));
+			pAd->StaCfg.ScanCnt = 2;
+			pAd->StaCfg.LastScanTime = Now;
+			MlmeAutoScan(pAd);
+		}
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeCheckForFastRoaming (BssNr=%d)\n", pRoamTab->BssNr));
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine calculates TxPER, RxPER of the past N-sec period. And
+		according to the calculation result, ChannelQuality is calculated here
+		to decide if current AP is still doing the job.
+
+		If ChannelQuality is not good, a ROAMing attempt may be tried later.
+	Output:
+		StaCfg.ChannelQuality - 0..100
+
+	IRQL = DISPATCH_LEVEL
+
+	NOTE: This routine decide channle quality based on RX CRC error ratio.
+		Caller should make sure a function call to NICUpdateRawCounters(pAd)
+		is performed right before this routine, so that this routine can decide
+		channel quality based on the most up-to-date information
+	==========================================================================
+ */
+VOID MlmeCalculateChannelQuality(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Now32)
+{
+	ULONG TxOkCnt, TxCnt, TxPER, TxPRR;
+	ULONG RxCnt, RxPER;
+	UCHAR NorRssi;
+	CHAR  MaxRssi;
+	ULONG BeaconLostTime = BEACON_LOST_TIME;
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+	// longer beacon lost time when carrier detection enabled
+	if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+	{
+		BeaconLostTime = BEACON_LOST_TIME + BEACON_LOST_TIME/2;
+	}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	MaxRssi = RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2);
+
+	//
+	// calculate TX packet error ratio and TX retry ratio - if too few TX samples, skip TX related statistics
+	//
+	TxOkCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount + pAd->RalinkCounters.OneSecTxRetryOkCount;
+	TxCnt = TxOkCnt + pAd->RalinkCounters.OneSecTxFailCount;
+	if (TxCnt < 5)
+	{
+		TxPER = 0;
+		TxPRR = 0;
+	}
+	else
+	{
+		TxPER = (pAd->RalinkCounters.OneSecTxFailCount * 100) / TxCnt;
+		TxPRR = ((TxCnt - pAd->RalinkCounters.OneSecTxNoRetryOkCount) * 100) / TxCnt;
+	}
+
+	//
+	// calculate RX PER - don't take RxPER into consideration if too few sample
+	//
+	RxCnt = pAd->RalinkCounters.OneSecRxOkCnt + pAd->RalinkCounters.OneSecRxFcsErrCnt;
+	if (RxCnt < 5)
+		RxPER = 0;
+	else
+		RxPER = (pAd->RalinkCounters.OneSecRxFcsErrCnt * 100) / RxCnt;
+
+	//
+	// decide ChannelQuality based on: 1)last BEACON received time, 2)last RSSI, 3)TxPER, and 4)RxPER
+	//
+	if (INFRA_ON(pAd) &&
+		(pAd->RalinkCounters.OneSecTxNoRetryOkCount < 2) && // no heavy traffic
+		(pAd->StaCfg.LastBeaconRxTime + BeaconLostTime < Now32))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("BEACON lost > %ld msec with TxOkCnt=%ld -> CQI=0\n", BeaconLostTime, TxOkCnt));
+		pAd->Mlme.ChannelQuality = 0;
+	}
+	else
+	{
+		// Normalize Rssi
+		if (MaxRssi > -40)
+			NorRssi = 100;
+		else if (MaxRssi < -90)
+			NorRssi = 0;
+		else
+			NorRssi = (MaxRssi + 90) * 2;
+
+		// ChannelQuality = W1*RSSI + W2*TxPRR + W3*RxPER	 (RSSI 0..100), (TxPER 100..0), (RxPER 100..0)
+		pAd->Mlme.ChannelQuality = (RSSI_WEIGHTING * NorRssi +
+								   TX_WEIGHTING * (100 - TxPRR) +
+								   RX_WEIGHTING* (100 - RxPER)) / 100;
+		if (pAd->Mlme.ChannelQuality >= 100)
+			pAd->Mlme.ChannelQuality = 100;
+	}
+
+}
+
+VOID MlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate)
+{
+	UCHAR	MaxMode = MODE_OFDM;
+
+#ifdef DOT11_N_SUPPORT
+	MaxMode = MODE_HTGREENFIELD;
+
+	if (pTxRate->STBC && (pAd->StaCfg.MaxHTPhyMode.field.STBC) && (pAd->Antenna.field.TxPath == 2))
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_USE;
+	else
+#endif // DOT11_N_SUPPORT //
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_NONE;
+
+	if (pTxRate->CurrMCS < MCS_AUTO)
+		pAd->StaCfg.HTPhyMode.field.MCS = pTxRate->CurrMCS;
+
+	if (pAd->StaCfg.HTPhyMode.field.MCS > 7)
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_NONE;
+
+   	if (ADHOC_ON(pAd))
+	{
+		// If peer adhoc is b-only mode, we can't send 11g rate.
+		pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+		pEntry->HTPhyMode.field.STBC	= STBC_NONE;
+
+		//
+		// For Adhoc MODE_CCK, driver will use AdhocBOnlyJoined flag to roll back to B only if necessary
+		//
+		pEntry->HTPhyMode.field.MODE	= pTxRate->Mode;
+		pEntry->HTPhyMode.field.ShortGI	= pAd->StaCfg.HTPhyMode.field.ShortGI;
+		pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+
+		// Patch speed error in status page
+		pAd->StaCfg.HTPhyMode.field.MODE = pEntry->HTPhyMode.field.MODE;
+	}
+	else
+	{
+		if (pTxRate->Mode <= MaxMode)
+			pAd->StaCfg.HTPhyMode.field.MODE = pTxRate->Mode;
+
+#ifdef DOT11_N_SUPPORT
+		if (pTxRate->ShortGI && (pAd->StaCfg.MaxHTPhyMode.field.ShortGI))
+			pAd->StaCfg.HTPhyMode.field.ShortGI = GI_400;
+		else
+#endif // DOT11_N_SUPPORT //
+			pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+
+#ifdef DOT11_N_SUPPORT
+		// Reexam each bandwidth's SGI support.
+		if (pAd->StaCfg.HTPhyMode.field.ShortGI == GI_400)
+		{
+			if ((pEntry->HTPhyMode.field.BW == BW_20) && (!CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE)))
+				pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+			if ((pEntry->HTPhyMode.field.BW == BW_40) && (!CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE)))
+				pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+		}
+
+		// Turn RTS/CTS rate to 6Mbps.
+		if ((pEntry->HTPhyMode.field.MCS == 0) && (pAd->StaCfg.HTPhyMode.field.MCS != 0))
+		{
+			pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+			if (pAd->MacTab.fAnyBASession)
+			{
+				AsicUpdateProtect(pAd, HT_FORCERTSCTS, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+			else
+			{
+				AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+		}
+		else if ((pEntry->HTPhyMode.field.MCS == 8) && (pAd->StaCfg.HTPhyMode.field.MCS != 8))
+		{
+			pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+			if (pAd->MacTab.fAnyBASession)
+			{
+				AsicUpdateProtect(pAd, HT_FORCERTSCTS, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+			else
+			{
+				AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+		}
+		else if ((pEntry->HTPhyMode.field.MCS != 0) && (pAd->StaCfg.HTPhyMode.field.MCS == 0))
+		{
+			AsicUpdateProtect(pAd, HT_RTSCTS_6M, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+
+		}
+		else if ((pEntry->HTPhyMode.field.MCS != 8) && (pAd->StaCfg.HTPhyMode.field.MCS == 8))
+		{
+			AsicUpdateProtect(pAd, HT_RTSCTS_6M, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+		}
+#endif // DOT11_N_SUPPORT //
+
+		pEntry->HTPhyMode.field.STBC	= pAd->StaCfg.HTPhyMode.field.STBC;
+		pEntry->HTPhyMode.field.ShortGI	= pAd->StaCfg.HTPhyMode.field.ShortGI;
+		pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+		pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+#ifdef DOT11_N_SUPPORT
+		if ((pAd->StaCfg.MaxHTPhyMode.field.MODE == MODE_HTGREENFIELD) &&
+		    pAd->WIFItestbed.bGreenField)
+		    pEntry->HTPhyMode.field.MODE = MODE_HTGREENFIELD;
+#endif // DOT11_N_SUPPORT //
+	}
+
+	pAd->LastTxRate = (USHORT)(pEntry->HTPhyMode.word);
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine calculates the acumulated TxPER of eaxh TxRate. And
+		according to the calculation result, change CommonCfg.TxRate which
+		is the stable TX Rate we expect the Radio situation could sustained.
+
+		CommonCfg.TxRate will change dynamically within {RATE_1/RATE_6, MaxTxRate}
+	Output:
+		CommonCfg.TxRate -
+
+	IRQL = DISPATCH_LEVEL
+
+	NOTE:
+		call this routine every second
+	==========================================================================
+ */
+VOID MlmeDynamicTxRateSwitching(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR					UpRateIdx = 0, DownRateIdx = 0, CurrRateIdx;
+	ULONG					i, AccuTxTotalCnt = 0, TxTotalCnt;
+	ULONG					TxErrorRatio = 0;
+	BOOLEAN					bTxRateChanged, bUpgradeQuality = FALSE;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate = NULL;
+	PUCHAR					pTable;
+	UCHAR					TableSize = 0;
+	UCHAR					InitTxRateIdx = 0, TrainUp, TrainDown;
+	CHAR					Rssi, RssiOffset = 0;
+	TX_STA_CNT1_STRUC		StaTx1;
+	TX_STA_CNT0_STRUC		TxStaCnt0;
+	ULONG					TxRetransmit = 0, TxSuccess = 0, TxFailCount = 0;
+	MAC_TABLE_ENTRY			*pEntry;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		return;
+	}
+#endif // RALINK_ATE //
+
+	//
+	// walk through MAC table, see if need to change AP's TX rate toward each entry
+	//
+   	for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		// check if this entry need to switch rate automatically
+		if (RTMPCheckEntryEnableAutoRateSwitch(pAd, pEntry) == FALSE)
+			continue;
+
+		if ((pAd->MacTab.Size == 1) || (pEntry->ValidAsDls))
+		{
+			Rssi = RTMPMaxRssi(pAd,
+							   pAd->StaCfg.RssiSample.AvgRssi0,
+							   pAd->StaCfg.RssiSample.AvgRssi1,
+							   pAd->StaCfg.RssiSample.AvgRssi2);
+
+			// Update statistic counter
+			RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+			RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+			pAd->bUpdateBcnCntDone = TRUE;
+			TxRetransmit = StaTx1.field.TxRetransmit;
+			TxSuccess = StaTx1.field.TxSuccess;
+			TxFailCount = TxStaCnt0.field.TxFailCount;
+			TxTotalCnt = TxRetransmit + TxSuccess + TxFailCount;
+
+			pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+			pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+			pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+			pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+			pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+			pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+
+			// if no traffic in the past 1-sec period, don't change TX rate,
+			// but clear all bad history. because the bad history may affect the next
+			// Chariot throughput test
+			AccuTxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((TxRetransmit + TxFailCount) * 100) / TxTotalCnt;
+		}
+		else
+		{
+			if (INFRA_ON(pAd) && (i == 1))
+				Rssi = RTMPMaxRssi(pAd,
+								   pAd->StaCfg.RssiSample.AvgRssi0,
+								   pAd->StaCfg.RssiSample.AvgRssi1,
+								   pAd->StaCfg.RssiSample.AvgRssi2);
+			else
+				Rssi = RTMPMaxRssi(pAd,
+								   pEntry->RssiSample.AvgRssi0,
+								   pEntry->RssiSample.AvgRssi1,
+								   pEntry->RssiSample.AvgRssi2);
+
+			TxTotalCnt = pEntry->OneSecTxNoRetryOkCount +
+				 pEntry->OneSecTxRetryOkCount +
+				 pEntry->OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((pEntry->OneSecTxRetryOkCount + pEntry->OneSecTxFailCount) * 100) / TxTotalCnt;
+		}
+
+		CurrRateIdx = pEntry->CurrTxRateIndex;
+
+		MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &InitTxRateIdx);
+
+		if (CurrRateIdx >= TableSize)
+		{
+			CurrRateIdx = TableSize - 1;
+		}
+
+		// When switch from Fixed rate -> auto rate, the REAL TX rate might be different from pAd->CommonCfg.TxRateIndex.
+		// So need to sync here.
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+		if ((pEntry->HTPhyMode.field.MCS != pCurrTxRate->CurrMCS)
+			//&& (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+			)
+		{
+
+			// Need to sync Real Tx rate and our record.
+			// Then return for next DRS.
+			pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(InitTxRateIdx+1)*5];
+			pEntry->CurrTxRateIndex = InitTxRateIdx;
+			MlmeSetTxRate(pAd, pEntry, pCurrTxRate);
+
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+			continue;
+		}
+
+		// decide the next upgrade rate and downgrade rate, if any
+		if ((CurrRateIdx > 0) && (CurrRateIdx < (TableSize - 1)))
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx -1;
+		}
+		else if (CurrRateIdx == 0)
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx;
+		}
+		else if (CurrRateIdx == (TableSize - 1))
+		{
+			UpRateIdx = CurrRateIdx;
+			DownRateIdx = CurrRateIdx - 1;
+		}
+
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+
+#ifdef DOT11_N_SUPPORT
+		if ((Rssi > -65) && (pCurrTxRate->Mode >= MODE_HTMIX))
+		{
+			TrainUp		= (pCurrTxRate->TrainUp + (pCurrTxRate->TrainUp >> 1));
+			TrainDown	= (pCurrTxRate->TrainDown + (pCurrTxRate->TrainDown >> 1));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			TrainUp		= pCurrTxRate->TrainUp;
+			TrainDown	= pCurrTxRate->TrainDown;
+		}
+
+		//pAd->DrsCounters.LastTimeTxRateChangeAction = pAd->DrsCounters.LastSecTxRateChangeAction;
+
+		//
+		// Keep the last time TxRateChangeAction status.
+		//
+		pEntry->LastTimeTxRateChangeAction = pEntry->LastSecTxRateChangeAction;
+
+
+
+		//
+		// CASE 1. when TX samples are fewer than 15, then decide TX rate solely on RSSI
+		//         (criteria copied from RT2500 for Netopia case)
+		//
+		if (TxTotalCnt <= 15)
+		{
+			CHAR	idx = 0;
+			UCHAR	TxRateIdx;
+			//UCHAR	MCS0 = 0, MCS1 = 0, MCS2 = 0, MCS3 = 0, MCS4 = 0, MCS7 = 0, MCS12 = 0, MCS13 = 0, MCS14 = 0, MCS15 = 0;
+			UCHAR	MCS0 = 0, MCS1 = 0, MCS2 = 0, MCS3 = 0, MCS4 = 0,  MCS5 =0, MCS6 = 0, MCS7 = 0;
+	        UCHAR	MCS12 = 0, MCS13 = 0, MCS14 = 0, MCS15 = 0;
+			UCHAR	MCS20 = 0, MCS21 = 0, MCS22 = 0, MCS23 = 0; // 3*3
+
+			// check the existence and index of each needed MCS
+			while (idx < pTable[0])
+			{
+				pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(idx+1)*5];
+
+				if (pCurrTxRate->CurrMCS == MCS_0)
+				{
+					MCS0 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_1)
+				{
+					MCS1 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_2)
+				{
+					MCS2 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_3)
+				{
+					MCS3 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_4)
+				{
+					MCS4 = idx;
+				}
+	            else if (pCurrTxRate->CurrMCS == MCS_5)
+	            {
+	                MCS5 = idx;
+	            }
+	            else if (pCurrTxRate->CurrMCS == MCS_6)
+	            {
+	                MCS6 = idx;
+	            }
+				//else if (pCurrTxRate->CurrMCS == MCS_7)
+				else if ((pCurrTxRate->CurrMCS == MCS_7) && (pCurrTxRate->ShortGI == GI_800))	// prevent the highest MCS using short GI when 1T and low throughput
+				{
+					MCS7 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_12)
+				{
+					MCS12 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_13)
+				{
+					MCS13 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_14)
+				{
+					MCS14 = idx;
+				}
+				//else if ((pCurrTxRate->CurrMCS == MCS_15)/* && (pCurrTxRate->ShortGI == GI_800)*/)	//we hope to use ShortGI as initial rate
+				else if ((pCurrTxRate->CurrMCS == MCS_15) && (pCurrTxRate->ShortGI == GI_800))	//we hope to use ShortGI as initial rate, however Atheros's chip has bugs when short GI
+				{
+					MCS15 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_20) // 3*3
+				{
+					MCS20 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_21)
+				{
+					MCS21 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_22)
+				{
+					MCS22 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_23)
+				{
+					MCS23 = idx;
+				}
+				idx ++;
+			}
+
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				if (pAd->NicConfig2.field.ExternalLNAForG)
+				{
+					RssiOffset = 2;
+				}
+				else
+				{
+					RssiOffset = 5;
+				}
+			}
+			else
+			{
+				if (pAd->NicConfig2.field.ExternalLNAForA)
+				{
+					RssiOffset = 5;
+				}
+				else
+				{
+					RssiOffset = 8;
+				}
+			}
+#ifdef DOT11_N_SUPPORT
+			/*if (MCS15)*/
+			if ((pTable == RateSwitchTable11BGN3S) ||
+				(pTable == RateSwitchTable11N3S) ||
+				(pTable == RateSwitchTable))
+			{// N mode with 3 stream // 3*3
+				if (MCS23 && (Rssi >= -70))
+					TxRateIdx = MCS15;
+				else if (MCS22 && (Rssi >= -72))
+					TxRateIdx = MCS14;
+        	    else if (MCS21 && (Rssi >= -76))
+					TxRateIdx = MCS13;
+				else if (MCS20 && (Rssi >= -78))
+					TxRateIdx = MCS12;
+			else if (MCS4 && (Rssi >= -82))
+				TxRateIdx = MCS4;
+			else if (MCS3 && (Rssi >= -84))
+				TxRateIdx = MCS3;
+			else if (MCS2 && (Rssi >= -86))
+				TxRateIdx = MCS2;
+			else if (MCS1 && (Rssi >= -88))
+				TxRateIdx = MCS1;
+			else
+				TxRateIdx = MCS0;
+		}
+//		else if ((pTable == RateSwitchTable11BGN2S) || (pTable == RateSwitchTable11BGN2SForABand) ||(pTable == RateSwitchTable11N2S) ||(pTable == RateSwitchTable11N2SForABand) || (pTable == RateSwitchTable))
+		else if ((pTable == RateSwitchTable11BGN2S) || (pTable == RateSwitchTable11BGN2SForABand) ||(pTable == RateSwitchTable11N2S) ||(pTable == RateSwitchTable11N2SForABand)) // 3*3
+			{// N mode with 2 stream
+				if (MCS15 && (Rssi >= (-70+RssiOffset)))
+					TxRateIdx = MCS15;
+				else if (MCS14 && (Rssi >= (-72+RssiOffset)))
+					TxRateIdx = MCS14;
+				else if (MCS13 && (Rssi >= (-76+RssiOffset)))
+					TxRateIdx = MCS13;
+				else if (MCS12 && (Rssi >= (-78+RssiOffset)))
+					TxRateIdx = MCS12;
+				else if (MCS4 && (Rssi >= (-82+RssiOffset)))
+					TxRateIdx = MCS4;
+				else if (MCS3 && (Rssi >= (-84+RssiOffset)))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi >= (-86+RssiOffset)))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi >= (-88+RssiOffset)))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+			else if ((pTable == RateSwitchTable11BGN1S) || (pTable == RateSwitchTable11N1S))
+			{// N mode with 1 stream
+				if (MCS7 && (Rssi > (-72+RssiOffset)))
+					TxRateIdx = MCS7;
+				else if (MCS6 && (Rssi > (-74+RssiOffset)))
+					TxRateIdx = MCS6;
+				else if (MCS5 && (Rssi > (-77+RssiOffset)))
+					TxRateIdx = MCS5;
+				else if (MCS4 && (Rssi > (-79+RssiOffset)))
+					TxRateIdx = MCS4;
+				else if (MCS3 && (Rssi > (-81+RssiOffset)))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi > (-83+RssiOffset)))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi > (-86+RssiOffset)))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{// Legacy mode
+				if (MCS7 && (Rssi > -70))
+					TxRateIdx = MCS7;
+				else if (MCS6 && (Rssi > -74))
+					TxRateIdx = MCS6;
+				else if (MCS5 && (Rssi > -78))
+					TxRateIdx = MCS5;
+				else if (MCS4 && (Rssi > -82))
+					TxRateIdx = MCS4;
+				else if (MCS4 == 0)	// for B-only mode
+					TxRateIdx = MCS3;
+				else if (MCS3 && (Rssi > -85))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi > -87))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi > -90))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+
+	//		if (TxRateIdx != pAd->CommonCfg.TxRateIndex)
+			{
+				pEntry->CurrTxRateIndex = TxRateIdx;
+				pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pEntry->CurrTxRateIndex+1)*5];
+				MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+			}
+
+			NdisZeroMemory(pEntry->TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pEntry->PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+			pEntry->fLastSecAccordingRSSI = TRUE;
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+
+			continue;
+		}
+
+		if (pEntry->fLastSecAccordingRSSI == TRUE)
+		{
+			pEntry->fLastSecAccordingRSSI = FALSE;
+			pEntry->LastSecTxRateChangeAction = 0;
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+
+			continue;
+		}
+
+		do
+		{
+			BOOLEAN	bTrainUpDown = FALSE;
+
+			pEntry->CurrTxRateStableTime ++;
+
+			// downgrade TX quality if PER >= Rate-Down threshold
+			if (TxErrorRatio >= TrainDown)
+			{
+				bTrainUpDown = TRUE;
+				pEntry->TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+			// upgrade TX quality if PER <= Rate-Up threshold
+			else if (TxErrorRatio <= TrainUp)
+			{
+				bTrainUpDown = TRUE;
+				bUpgradeQuality = TRUE;
+				if (pEntry->TxQuality[CurrRateIdx])
+					pEntry->TxQuality[CurrRateIdx] --;  // quality very good in CurrRate
+
+				if (pEntry->TxRateUpPenalty)
+					pEntry->TxRateUpPenalty --;
+				else if (pEntry->TxQuality[UpRateIdx])
+					pEntry->TxQuality[UpRateIdx] --;    // may improve next UP rate's quality
+			}
+
+			pEntry->PER[CurrRateIdx] = (UCHAR)TxErrorRatio;
+
+			if (bTrainUpDown)
+			{
+				// perform DRS - consider TxRate Down first, then rate up.
+				if ((CurrRateIdx != DownRateIdx) && (pEntry->TxQuality[CurrRateIdx] >= DRS_TX_QUALITY_WORST_BOUND))
+				{
+					pEntry->CurrTxRateIndex = DownRateIdx;
+				}
+				else if ((CurrRateIdx != UpRateIdx) && (pEntry->TxQuality[UpRateIdx] <= 0))
+				{
+					pEntry->CurrTxRateIndex = UpRateIdx;
+				}
+			}
+		} while (FALSE);
+
+		// if rate-up happen, clear all bad history of all TX rates
+		if (pEntry->CurrTxRateIndex > CurrRateIdx)
+		{
+			pEntry->CurrTxRateStableTime = 0;
+			pEntry->TxRateUpPenalty = 0;
+			pEntry->LastSecTxRateChangeAction = 1; // rate UP
+			NdisZeroMemory(pEntry->TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pEntry->PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+
+			//
+			// For TxRate fast train up
+			//
+			if (!pAd->StaCfg.StaQuickResponeForRateUpTimerRunning)
+			{
+				RTMPSetTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, 100);
+
+				pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = TRUE;
+			}
+			bTxRateChanged = TRUE;
+		}
+		// if rate-down happen, only clear DownRate's bad history
+		else if (pEntry->CurrTxRateIndex < CurrRateIdx)
+		{
+			pEntry->CurrTxRateStableTime = 0;
+			pEntry->TxRateUpPenalty = 0;           // no penalty
+			pEntry->LastSecTxRateChangeAction = 2; // rate DOWN
+			pEntry->TxQuality[pEntry->CurrTxRateIndex] = 0;
+			pEntry->PER[pEntry->CurrTxRateIndex] = 0;
+
+			//
+			// For TxRate fast train down
+			//
+			if (!pAd->StaCfg.StaQuickResponeForRateUpTimerRunning)
+			{
+				RTMPSetTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, 100);
+
+				pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = TRUE;
+			}
+			bTxRateChanged = TRUE;
+		}
+		else
+		{
+			pEntry->LastSecTxRateChangeAction = 0; // rate no change
+			bTxRateChanged = FALSE;
+		}
+
+		pEntry->LastTxOkCount = TxSuccess;
+
+		// reset all OneSecTx counters
+		RESET_ONE_SEC_TX_CNT(pEntry);
+
+		pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pEntry->CurrTxRateIndex+1)*5];
+		if (bTxRateChanged && pNextTxRate)
+		{
+			MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+		}
+	}
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Station side, Auto TxRate faster train up timer call back function.
+
+	Arguments:
+		SystemSpecific1			- Not used.
+		FunctionContext			- Pointer to our Adapter context.
+		SystemSpecific2			- Not used.
+		SystemSpecific3			- Not used.
+
+	Return Value:
+		None
+
+	========================================================================
+*/
+VOID StaQuickResponeForRateUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	PRTMP_ADAPTER			pAd = (PRTMP_ADAPTER)FunctionContext;
+	UCHAR					UpRateIdx = 0, DownRateIdx = 0, CurrRateIdx = 0;
+	ULONG					TxTotalCnt;
+	ULONG					TxErrorRatio = 0;
+	BOOLEAN					bTxRateChanged; //, bUpgradeQuality = FALSE;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate = NULL;
+	PUCHAR					pTable;
+	UCHAR					TableSize = 0;
+	UCHAR					InitTxRateIdx = 0, TrainUp, TrainDown;
+	TX_STA_CNT1_STRUC		StaTx1;
+	TX_STA_CNT0_STRUC		TxStaCnt0;
+	CHAR					Rssi, ratio;
+	ULONG					TxRetransmit = 0, TxSuccess = 0, TxFailCount = 0;
+	MAC_TABLE_ENTRY			*pEntry;
+	ULONG					i;
+
+	pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = FALSE;
+
+    //
+    // walk through MAC table, see if need to change AP's TX rate toward each entry
+    //
+	for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		// check if this entry need to switch rate automatically
+		if (RTMPCheckEntryEnableAutoRateSwitch(pAd, pEntry) == FALSE)
+			continue;
+
+		if (INFRA_ON(pAd) && (i == 1))
+			Rssi = RTMPMaxRssi(pAd,
+							   pAd->StaCfg.RssiSample.AvgRssi0,
+							   pAd->StaCfg.RssiSample.AvgRssi1,
+							   pAd->StaCfg.RssiSample.AvgRssi2);
+		else
+			Rssi = RTMPMaxRssi(pAd,
+							   pEntry->RssiSample.AvgRssi0,
+							   pEntry->RssiSample.AvgRssi1,
+							   pEntry->RssiSample.AvgRssi2);
+
+		CurrRateIdx = pAd->CommonCfg.TxRateIndex;
+
+			MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &InitTxRateIdx);
+
+		// decide the next upgrade rate and downgrade rate, if any
+		if ((CurrRateIdx > 0) && (CurrRateIdx < (TableSize - 1)))
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx -1;
+		}
+		else if (CurrRateIdx == 0)
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx;
+		}
+		else if (CurrRateIdx == (TableSize - 1))
+		{
+			UpRateIdx = CurrRateIdx;
+			DownRateIdx = CurrRateIdx - 1;
+		}
+
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+
+#ifdef DOT11_N_SUPPORT
+		if ((Rssi > -65) && (pCurrTxRate->Mode >= MODE_HTMIX))
+		{
+			TrainUp		= (pCurrTxRate->TrainUp + (pCurrTxRate->TrainUp >> 1));
+			TrainDown	= (pCurrTxRate->TrainDown + (pCurrTxRate->TrainDown >> 1));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			TrainUp		= pCurrTxRate->TrainUp;
+			TrainDown	= pCurrTxRate->TrainDown;
+		}
+
+		if (pAd->MacTab.Size == 1)
+		{
+			// Update statistic counter
+			RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+			RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+
+			TxRetransmit = StaTx1.field.TxRetransmit;
+			TxSuccess = StaTx1.field.TxSuccess;
+			TxFailCount = TxStaCnt0.field.TxFailCount;
+			TxTotalCnt = TxRetransmit + TxSuccess + TxFailCount;
+
+			pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+			pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+			pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+			pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+			pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+			pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+
+#if 0 // test by Gary.
+			// if no traffic in the past 1-sec period, don't change TX rate,
+			// but clear all bad history. because the bad history may affect the next
+			// Chariot throughput test
+			TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxFailCount;
+#endif
+			if (TxTotalCnt)
+				TxErrorRatio = ((TxRetransmit + TxFailCount) * 100) / TxTotalCnt;
+		}
+		else
+		{
+			TxTotalCnt = pEntry->OneSecTxNoRetryOkCount +
+				 pEntry->OneSecTxRetryOkCount +
+				 pEntry->OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((pEntry->OneSecTxRetryOkCount + pEntry->OneSecTxFailCount) * 100) / TxTotalCnt;
+		}
+
+
+		//
+		// CASE 1. when TX samples are fewer than 15, then decide TX rate solely on RSSI
+		//         (criteria copied from RT2500 for Netopia case)
+		//
+		if (TxTotalCnt <= 12)
+		{
+			NdisZeroMemory(pAd->DrsCounters.TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pAd->DrsCounters.PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+
+			if ((pAd->DrsCounters.LastSecTxRateChangeAction == 1) && (CurrRateIdx != DownRateIdx))
+			{
+				pAd->CommonCfg.TxRateIndex = DownRateIdx;
+				pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+			else if ((pAd->DrsCounters.LastSecTxRateChangeAction == 2) && (CurrRateIdx != UpRateIdx))
+			{
+				pAd->CommonCfg.TxRateIndex = UpRateIdx;
+			}
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("QuickDRS: TxTotalCnt <= 15, train back to original rate \n"));
+			return;
+		}
+
+		do
+		{
+			ULONG OneSecTxNoRetryOKRationCount;
+
+			if (pAd->DrsCounters.LastTimeTxRateChangeAction == 0)
+				ratio = 5;
+			else
+				ratio = 4;
+
+			// downgrade TX quality if PER >= Rate-Down threshold
+			if (TxErrorRatio >= TrainDown)
+			{
+				pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+
+			pAd->DrsCounters.PER[CurrRateIdx] = (UCHAR)TxErrorRatio;
+
+			OneSecTxNoRetryOKRationCount = (TxSuccess * ratio);
+
+			// perform DRS - consider TxRate Down first, then rate up.
+			if ((pAd->DrsCounters.LastSecTxRateChangeAction == 1) && (CurrRateIdx != DownRateIdx))
+			{
+				if ((pAd->DrsCounters.LastTxOkCount + 2) >= OneSecTxNoRetryOKRationCount)
+				{
+					pAd->CommonCfg.TxRateIndex = DownRateIdx;
+					pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+
+				}
+
+			}
+			else if ((pAd->DrsCounters.LastSecTxRateChangeAction == 2) && (CurrRateIdx != UpRateIdx))
+			{
+				if ((TxErrorRatio >= 50) || (TxErrorRatio >= TrainDown))
+				{
+
+				}
+				else if ((pAd->DrsCounters.LastTxOkCount + 2) >= OneSecTxNoRetryOKRationCount)
+				{
+					pAd->CommonCfg.TxRateIndex = UpRateIdx;
+				}
+			}
+		}while (FALSE);
+
+		// if rate-up happen, clear all bad history of all TX rates
+		if (pAd->CommonCfg.TxRateIndex > CurrRateIdx)
+		{
+			pAd->DrsCounters.TxRateUpPenalty = 0;
+			NdisZeroMemory(pAd->DrsCounters.TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pAd->DrsCounters.PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+			bTxRateChanged = TRUE;
+		}
+		// if rate-down happen, only clear DownRate's bad history
+		else if (pAd->CommonCfg.TxRateIndex < CurrRateIdx)
+		{
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("QuickDRS: --TX rate from %d to %d \n", CurrRateIdx, pAd->CommonCfg.TxRateIndex));
+
+			pAd->DrsCounters.TxRateUpPenalty = 0;           // no penalty
+			pAd->DrsCounters.TxQuality[pAd->CommonCfg.TxRateIndex] = 0;
+			pAd->DrsCounters.PER[pAd->CommonCfg.TxRateIndex] = 0;
+			bTxRateChanged = TRUE;
+		}
+		else
+		{
+			bTxRateChanged = FALSE;
+		}
+
+		pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pAd->CommonCfg.TxRateIndex+1)*5];
+		if (bTxRateChanged && pNextTxRate)
+		{
+			MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine is executed periodically inside MlmePeriodicExec() after
+		association with an AP.
+		It checks if StaCfg.Psm is consistent with user policy (recorded in
+		StaCfg.WindowsPowerMode). If not, enforce user policy. However,
+		there're some conditions to consider:
+		1. we don't support power-saving in ADHOC mode, so Psm=PWR_ACTIVE all
+		   the time when Mibss==TRUE
+		2. When link up in INFRA mode, Psm should not be switch to PWR_SAVE
+		   if outgoing traffic available in TxRing or MgmtRing.
+	Output:
+		1. change pAd->StaCfg.Psm to PWR_SAVE or leave it untouched
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeCheckPsmChange(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG	Now32)
+{
+	ULONG	PowerMode;
+
+	// condition -
+	// 1. Psm maybe ON only happen in INFRASTRUCTURE mode
+	// 2. user wants either MAX_PSP or FAST_PSP
+	// 3. but current psm is not in PWR_SAVE
+	// 4. CNTL state machine is not doing SCANning
+	// 5. no TX SUCCESS event for the past 1-sec period
+#ifdef NDIS51_MINIPORT
+	if (pAd->StaCfg.WindowsPowerProfile == NdisPowerProfileBattery)
+		PowerMode = pAd->StaCfg.WindowsBatteryPowerMode;
+	else
+#endif
+		PowerMode = pAd->StaCfg.WindowsPowerMode;
+
+	if (INFRA_ON(pAd) &&
+		(PowerMode != Ndis802_11PowerModeCAM) &&
+		(pAd->StaCfg.Psm == PWR_ACTIVE) &&
+//		(! RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		(pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE) /*&&
+		(pAd->RalinkCounters.OneSecTxNoRetryOkCount == 0) &&
+		(pAd->RalinkCounters.OneSecTxRetryOkCount == 0)*/)
+	{
+		NdisGetSystemUpTime(&pAd->Mlme.LastSendNULLpsmTime);
+		pAd->RalinkCounters.RxCountSinceLastNULL = 0;
+		MlmeSetPsmBit(pAd, PWR_SAVE);
+		if (!(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable))
+		{
+			RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, FALSE);
+		}
+		else
+		{
+			RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+		}
+	}
+}
+
+// IRQL = PASSIVE_LEVEL
+// IRQL = DISPATCH_LEVEL
+VOID MlmeSetPsmBit(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT psm)
+{
+	AUTO_RSP_CFG_STRUC csr4;
+
+	pAd->StaCfg.Psm = psm;
+	RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &csr4.word);
+	csr4.field.AckCtsPsmBit = (psm == PWR_SAVE)? 1:0;
+	RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, csr4.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetPsmBit = %d\n", psm));
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeSetTxPreamble(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TxPreamble)
+{
+	AUTO_RSP_CFG_STRUC csr4;
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	//TxPreamble = Rt802_11PreambleLong;
+
+	RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &csr4.word);
+	if (TxPreamble == Rt802_11PreambleLong)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetTxPreamble (= LONG PREAMBLE)\n"));
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+		csr4.field.AutoResponderPreamble = 0;
+	}
+	else
+	{
+		// NOTE: 1Mbps should always use long preamble
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetTxPreamble (= SHORT PREAMBLE)\n"));
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+		csr4.field.AutoResponderPreamble = 1;
+	}
+
+	RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, csr4.word);
+}
+
+/*
+    ==========================================================================
+    Description:
+        Update basic rate bitmap
+    ==========================================================================
+ */
+
+VOID UpdateBasicRateBitmap(
+    IN  PRTMP_ADAPTER   pAdapter)
+{
+    INT  i, j;
+                  /* 1  2  5.5, 11,  6,  9, 12, 18, 24, 36, 48,  54 */
+    UCHAR rate[] = { 2, 4,  11, 22, 12, 18, 24, 36, 48, 72, 96, 108 };
+    UCHAR *sup_p = pAdapter->CommonCfg.SupRate;
+    UCHAR *ext_p = pAdapter->CommonCfg.ExtRate;
+    ULONG bitmap = pAdapter->CommonCfg.BasicRateBitmap;
+
+
+    /* if A mode, always use fix BasicRateBitMap */
+    //if (pAdapter->CommonCfg.Channel == PHY_11A)
+	if (pAdapter->CommonCfg.Channel > 14)
+        pAdapter->CommonCfg.BasicRateBitmap = 0x150; /* 6, 12, 24M */
+    /* End of if */
+
+    if (pAdapter->CommonCfg.BasicRateBitmap > 4095)
+    {
+        /* (2 ^ MAX_LEN_OF_SUPPORTED_RATES) -1 */
+        return;
+    } /* End of if */
+
+    for(i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+    {
+        sup_p[i] &= 0x7f;
+        ext_p[i] &= 0x7f;
+    } /* End of for */
+
+    for(i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+    {
+        if (bitmap & (1 << i))
+        {
+            for(j=0; j<MAX_LEN_OF_SUPPORTED_RATES; j++)
+            {
+                if (sup_p[j] == rate[i])
+                    sup_p[j] |= 0x80;
+                /* End of if */
+            } /* End of for */
+
+            for(j=0; j<MAX_LEN_OF_SUPPORTED_RATES; j++)
+            {
+                if (ext_p[j] == rate[i])
+                    ext_p[j] |= 0x80;
+                /* End of if */
+            } /* End of for */
+        } /* End of if */
+    } /* End of for */
+} /* End of UpdateBasicRateBitmap */
+
+// IRQL = PASSIVE_LEVEL
+// IRQL = DISPATCH_LEVEL
+// bLinkUp is to identify the inital link speed.
+// TRUE indicates the rate update at linkup, we should not try to set the rate at 54Mbps.
+VOID MlmeUpdateTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN 	BOOLEAN		 		bLinkUp,
+	IN	UCHAR				apidx)
+{
+	int i, num;
+	UCHAR Rate = RATE_6, MaxDesire = RATE_1, MaxSupport = RATE_1;
+	UCHAR MinSupport = RATE_54;
+	ULONG BasicRateBitmap = 0;
+	UCHAR CurrBasicRate = RATE_1;
+	UCHAR *pSupRate, SupRateLen, *pExtRate, ExtRateLen;
+	PHTTRANSMIT_SETTING		pHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMaxHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMinHtPhy = NULL;
+	BOOLEAN 				*auto_rate_cur_p;
+	UCHAR					HtMcs = MCS_AUTO;
+
+	// find max desired rate
+	UpdateBasicRateBitmap(pAd);
+
+	num = 0;
+	auto_rate_cur_p = NULL;
+	for (i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+	{
+		switch (pAd->CommonCfg.DesireRate[i] & 0x7f)
+		{
+			case 2:  Rate = RATE_1;   num++;   break;
+			case 4:  Rate = RATE_2;   num++;   break;
+			case 11: Rate = RATE_5_5; num++;   break;
+			case 22: Rate = RATE_11;  num++;   break;
+			case 12: Rate = RATE_6;   num++;   break;
+			case 18: Rate = RATE_9;   num++;   break;
+			case 24: Rate = RATE_12;  num++;   break;
+			case 36: Rate = RATE_18;  num++;   break;
+			case 48: Rate = RATE_24;  num++;   break;
+			case 72: Rate = RATE_36;  num++;   break;
+			case 96: Rate = RATE_48;  num++;   break;
+			case 108: Rate = RATE_54; num++;   break;
+			//default: Rate = RATE_1;   break;
+		}
+		if (MaxDesire < Rate)  MaxDesire = Rate;
+	}
+
+//===========================================================================
+//===========================================================================
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pHtPhy 		= &pAd->StaCfg.HTPhyMode;
+		pMaxHtPhy	= &pAd->StaCfg.MaxHTPhyMode;
+		pMinHtPhy	= &pAd->StaCfg.MinHTPhyMode;
+
+		auto_rate_cur_p = &pAd->StaCfg.bAutoTxRateSwitch;
+		HtMcs 		= pAd->StaCfg.DesiredTransmitSetting.field.MCS;
+
+		if ((pAd->StaCfg.BssType == BSS_ADHOC) &&
+			(pAd->CommonCfg.PhyMode == PHY_11B) &&
+			(MaxDesire > RATE_11))
+		{
+			MaxDesire = RATE_11;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->CommonCfg.MaxDesiredRate = MaxDesire;
+	pMinHtPhy->word = 0;
+	pMaxHtPhy->word = 0;
+	pHtPhy->word = 0;
+
+	// Auto rate switching is enabled only if more than one DESIRED RATES are
+	// specified; otherwise disabled
+	if (num <= 1)
+	{
+		//OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED);
+		//pAd->CommonCfg.bAutoTxRateSwitch	= FALSE;
+		*auto_rate_cur_p = FALSE;
+	}
+	else
+	{
+		//OPSTATUS_SET_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED);
+		//pAd->CommonCfg.bAutoTxRateSwitch	= TRUE;
+		*auto_rate_cur_p = TRUE;
+	}
+
+#if 1
+	if (HtMcs != MCS_AUTO)
+	{
+		//OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED);
+		//pAd->CommonCfg.bAutoTxRateSwitch	= FALSE;
+		*auto_rate_cur_p = FALSE;
+	}
+	else
+	{
+		//OPSTATUS_SET_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED);
+		//pAd->CommonCfg.bAutoTxRateSwitch	= TRUE;
+		*auto_rate_cur_p = TRUE;
+	}
+#endif
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((ADHOC_ON(pAd) || INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA))
+	{
+		pSupRate = &pAd->StaActive.SupRate[0];
+		pExtRate = &pAd->StaActive.ExtRate[0];
+		SupRateLen = pAd->StaActive.SupRateLen;
+		ExtRateLen = pAd->StaActive.ExtRateLen;
+	}
+	else
+#endif // CONFIG_STA_SUPPORT //
+	{
+		pSupRate = &pAd->CommonCfg.SupRate[0];
+		pExtRate = &pAd->CommonCfg.ExtRate[0];
+		SupRateLen = pAd->CommonCfg.SupRateLen;
+		ExtRateLen = pAd->CommonCfg.ExtRateLen;
+	}
+
+	// find max supported rate
+	for (i=0; i<SupRateLen; i++)
+	{
+		switch (pSupRate[i] & 0x7f)
+		{
+			case 2:   Rate = RATE_1;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0001;	 break;
+			case 4:   Rate = RATE_2;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0002;	 break;
+			case 11:  Rate = RATE_5_5;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0004;	 break;
+			case 22:  Rate = RATE_11;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0008;	 break;
+			case 12:  Rate = RATE_6;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0010;  break;
+			case 18:  Rate = RATE_9;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0020;	 break;
+			case 24:  Rate = RATE_12;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0040;  break;
+			case 36:  Rate = RATE_18;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0080;	 break;
+			case 48:  Rate = RATE_24;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0100;  break;
+			case 72:  Rate = RATE_36;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0200;	 break;
+			case 96:  Rate = RATE_48;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0400;	 break;
+			case 108: Rate = RATE_54;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0800;	 break;
+			default:  Rate = RATE_1;	break;
+		}
+		if (MaxSupport < Rate)	MaxSupport = Rate;
+
+		if (MinSupport > Rate) MinSupport = Rate;
+	}
+
+	for (i=0; i<ExtRateLen; i++)
+	{
+		switch (pExtRate[i] & 0x7f)
+		{
+			case 2:   Rate = RATE_1;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0001;	 break;
+			case 4:   Rate = RATE_2;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0002;	 break;
+			case 11:  Rate = RATE_5_5;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0004;	 break;
+			case 22:  Rate = RATE_11;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0008;	 break;
+			case 12:  Rate = RATE_6;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0010;  break;
+			case 18:  Rate = RATE_9;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0020;	 break;
+			case 24:  Rate = RATE_12;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0040;  break;
+			case 36:  Rate = RATE_18;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0080;	 break;
+			case 48:  Rate = RATE_24;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0100;  break;
+			case 72:  Rate = RATE_36;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0200;	 break;
+			case 96:  Rate = RATE_48;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0400;	 break;
+			case 108: Rate = RATE_54;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0800;	 break;
+			default:  Rate = RATE_1;	break;
+		}
+		if (MaxSupport < Rate)	MaxSupport = Rate;
+
+		if (MinSupport > Rate) MinSupport = Rate;
+	}
+
+	RTMP_IO_WRITE32(pAd, LEGACY_BASIC_RATE, BasicRateBitmap);
+
+	// bug fix
+	// pAd->CommonCfg.BasicRateBitmap = BasicRateBitmap;
+
+	// calculate the exptected ACK rate for each TX rate. This info is used to caculate
+	// the DURATION field of outgoing uniicast DATA/MGMT frame
+	for (i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+	{
+		if (BasicRateBitmap & (0x01 << i))
+			CurrBasicRate = (UCHAR)i;
+		pAd->CommonCfg.ExpectedACKRate[i] = CurrBasicRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateTxRates[MaxSupport = %d] = MaxDesire %d Mbps\n", RateIdToMbps[MaxSupport], RateIdToMbps[MaxDesire]));
+	// max tx rate = min {max desire rate, max supported rate}
+	if (MaxSupport < MaxDesire)
+		pAd->CommonCfg.MaxTxRate = MaxSupport;
+	else
+		pAd->CommonCfg.MaxTxRate = MaxDesire;
+
+	pAd->CommonCfg.MinTxRate = MinSupport;
+	// 2003-07-31 john - 2500 doesn't have good sensitivity at high OFDM rates. to increase the success
+	// ratio of initial DHCP packet exchange, TX rate starts from a lower rate depending
+	// on average RSSI
+	//	 1. RSSI >= -70db, start at 54 Mbps (short distance)
+	//	 2. -70 > RSSI >= -75, start at 24 Mbps (mid distance)
+	//	 3. -75 > RSSI, start at 11 Mbps (long distance)
+	//if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED)/* &&
+	//	OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)*/)
+	if (*auto_rate_cur_p)
+	{
+		short dbm = 0;
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			dbm = pAd->StaCfg.RssiSample.AvgRssi0 - pAd->BbpRssiToDbmDelta;
+#endif // CONFIG_STA_SUPPORT //
+		if (bLinkUp == TRUE)
+			pAd->CommonCfg.TxRate = RATE_24;
+		else
+			pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+
+		if (dbm < -75)
+			pAd->CommonCfg.TxRate = RATE_11;
+		else if (dbm < -70)
+			pAd->CommonCfg.TxRate = RATE_24;
+
+		// should never exceed MaxTxRate (consider 11B-only mode)
+		if (pAd->CommonCfg.TxRate > pAd->CommonCfg.MaxTxRate)
+			pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+
+		pAd->CommonCfg.TxRateIndex = 0;
+	}
+	else
+	{
+		pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+		pHtPhy->field.MCS	= (pAd->CommonCfg.MaxTxRate > 3) ? (pAd->CommonCfg.MaxTxRate - 4) : pAd->CommonCfg.MaxTxRate;
+		pHtPhy->field.MODE	= (pAd->CommonCfg.MaxTxRate > 3) ? MODE_OFDM : MODE_CCK;
+
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC	= pHtPhy->field.STBC;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI	= pHtPhy->field.ShortGI;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS		= pHtPhy->field.MCS;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE	= pHtPhy->field.MODE;
+	}
+
+	if (pAd->CommonCfg.TxRate <= RATE_11)
+	{
+		pMaxHtPhy->field.MODE = MODE_CCK;
+		pMaxHtPhy->field.MCS = pAd->CommonCfg.TxRate;
+		pMinHtPhy->field.MCS = pAd->CommonCfg.MinTxRate;
+	}
+	else
+	{
+		pMaxHtPhy->field.MODE = MODE_OFDM;
+		pMaxHtPhy->field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.TxRate];
+		if (pAd->CommonCfg.MinTxRate >= RATE_6 && (pAd->CommonCfg.MinTxRate <= RATE_54))
+			{pMinHtPhy->field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MinTxRate];}
+		else
+			{pMinHtPhy->field.MCS = pAd->CommonCfg.MinTxRate;}
+	}
+
+	pHtPhy->word = (pMaxHtPhy->word);
+	if (bLinkUp && (pAd->OpMode == OPMODE_STA))
+	{
+			pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word = pHtPhy->word;
+			pAd->MacTab.Content[BSSID_WCID].MaxHTPhyMode.word = pMaxHtPhy->word;
+			pAd->MacTab.Content[BSSID_WCID].MinHTPhyMode.word = pMinHtPhy->word;
+	}
+	else
+	{
+		switch (pAd->CommonCfg.PhyMode)
+		{
+			case PHY_11BG_MIXED:
+			case PHY_11B:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11BGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+				pAd->CommonCfg.MlmeRate = RATE_1;
+				pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = RATE_1;
+
+//#ifdef	WIFI_TEST
+				pAd->CommonCfg.RtsRate = RATE_11;
+//#else
+//				pAd->CommonCfg.RtsRate = RATE_1;
+//#endif
+				break;
+			case PHY_11G:
+			case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11AGN_MIXED:
+			case PHY_11GN_MIXED:
+			case PHY_11N_2_4G:
+			case PHY_11AN_MIXED:
+			case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+				pAd->CommonCfg.MlmeRate = RATE_6;
+				pAd->CommonCfg.RtsRate = RATE_6;
+				pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				break;
+			case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11ABGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+				if (pAd->CommonCfg.Channel <= 14)
+				{
+					pAd->CommonCfg.MlmeRate = RATE_1;
+					pAd->CommonCfg.RtsRate = RATE_1;
+					pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+					pAd->CommonCfg.MlmeTransmit.field.MCS = RATE_1;
+				}
+				else
+				{
+					pAd->CommonCfg.MlmeRate = RATE_6;
+					pAd->CommonCfg.RtsRate = RATE_6;
+					pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+					pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				}
+				break;
+			default: // error
+				pAd->CommonCfg.MlmeRate = RATE_6;
+                        	pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				pAd->CommonCfg.RtsRate = RATE_1;
+				break;
+		}
+		//
+		// Keep Basic Mlme Rate.
+		//
+		pAd->MacTab.Content[MCAST_WCID].HTPhyMode.word = pAd->CommonCfg.MlmeTransmit.word;
+		if (pAd->CommonCfg.MlmeTransmit.field.MODE == MODE_OFDM)
+			pAd->MacTab.Content[MCAST_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[RATE_24];
+		else
+			pAd->MacTab.Content[MCAST_WCID].HTPhyMode.field.MCS = RATE_1;
+		pAd->CommonCfg.BasicMlmeRate = pAd->CommonCfg.MlmeRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (MaxDesire=%d, MaxSupport=%d, MaxTxRate=%d, MinRate=%d, Rate Switching =%d)\n",
+			 RateIdToMbps[MaxDesire], RateIdToMbps[MaxSupport], RateIdToMbps[pAd->CommonCfg.MaxTxRate], RateIdToMbps[pAd->CommonCfg.MinTxRate],
+			 /*OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED)*/*auto_rate_cur_p));
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (TxRate=%d, RtsRate=%d, BasicRateBitmap=0x%04lx)\n",
+			 RateIdToMbps[pAd->CommonCfg.TxRate], RateIdToMbps[pAd->CommonCfg.RtsRate], BasicRateBitmap));
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeUpdateTxRates (MlmeTransmit=0x%x, MinHTPhyMode=%x, MaxHTPhyMode=0x%x, HTPhyMode=0x%x)\n",
+			 pAd->CommonCfg.MlmeTransmit.word, pAd->MacTab.Content[BSSID_WCID].MinHTPhyMode.word ,pAd->MacTab.Content[BSSID_WCID].MaxHTPhyMode.word ,pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word ));
+}
+
+#ifdef DOT11_N_SUPPORT
+/*
+	==========================================================================
+	Description:
+		This function update HT Rate setting.
+		Input Wcid value is valid for 2 case :
+		1. it's used for Station in infra mode that copy AP rate to Mactable.
+		2. OR Station 	in adhoc mode to copy peer's HT rate to Mactable.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeUpdateHtTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN	UCHAR				apidx)
+{
+	UCHAR	StbcMcs; //j, StbcMcs, bitmask;
+	CHAR 	i; // 3*3
+	RT_HT_CAPABILITY 	*pRtHtCap = NULL;
+	RT_HT_PHY_INFO		*pActiveHtPhy = NULL;
+	ULONG		BasicMCS;
+	UCHAR j, bitmask;
+	PRT_HT_PHY_INFO			pDesireHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMaxHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMinHtPhy = NULL;
+	BOOLEAN 				*auto_rate_cur_p;
+
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates===> \n"));
+
+	auto_rate_cur_p = NULL;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pDesireHtPhy	= &pAd->StaCfg.DesiredHtPhyInfo;
+		pActiveHtPhy	= &pAd->StaCfg.DesiredHtPhyInfo;
+		pHtPhy 		= &pAd->StaCfg.HTPhyMode;
+		pMaxHtPhy	= &pAd->StaCfg.MaxHTPhyMode;
+		pMinHtPhy	= &pAd->StaCfg.MinHTPhyMode;
+
+		auto_rate_cur_p = &pAd->StaCfg.bAutoTxRateSwitch;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((ADHOC_ON(pAd) || INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA))
+	{
+		if (pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+			return;
+
+		pRtHtCap = &pAd->StaActive.SupportedHtPhy;
+		pActiveHtPhy = &pAd->StaActive.SupportedPhyInfo;
+		StbcMcs = (UCHAR)pAd->MlmeAux.AddHtInfo.AddHtInfo3.StbcMcs;
+		BasicMCS =pAd->MlmeAux.AddHtInfo.MCSSet[0]+(pAd->MlmeAux.AddHtInfo.MCSSet[1]<<8)+(StbcMcs<<16);
+		if ((pAd->CommonCfg.DesiredHtPhy.TxSTBC) && (pRtHtCap->RxSTBC) && (pAd->Antenna.field.TxPath == 2))
+			pMaxHtPhy->field.STBC = STBC_USE;
+		else
+			pMaxHtPhy->field.STBC = STBC_NONE;
+	}
+	else
+#endif // CONFIG_STA_SUPPORT //
+	{
+		if (pDesireHtPhy->bHtEnable == FALSE)
+			return;
+
+		pRtHtCap = &pAd->CommonCfg.DesiredHtPhy;
+		StbcMcs = (UCHAR)pAd->CommonCfg.AddHTInfo.AddHtInfo3.StbcMcs;
+		BasicMCS = pAd->CommonCfg.AddHTInfo.MCSSet[0]+(pAd->CommonCfg.AddHTInfo.MCSSet[1]<<8)+(StbcMcs<<16);
+		if ((pAd->CommonCfg.DesiredHtPhy.TxSTBC) && (pRtHtCap->RxSTBC) && (pAd->Antenna.field.TxPath == 2))
+			pMaxHtPhy->field.STBC = STBC_USE;
+		else
+			pMaxHtPhy->field.STBC = STBC_NONE;
+	}
+
+	// Decide MAX ht rate.
+	if ((pRtHtCap->GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+		pMaxHtPhy->field.MODE = MODE_HTGREENFIELD;
+	else
+		pMaxHtPhy->field.MODE = MODE_HTMIX;
+
+    if ((pAd->CommonCfg.DesiredHtPhy.ChannelWidth) && (pRtHtCap->ChannelWidth))
+		pMaxHtPhy->field.BW = BW_40;
+	else
+		pMaxHtPhy->field.BW = BW_20;
+
+    if (pMaxHtPhy->field.BW == BW_20)
+		pMaxHtPhy->field.ShortGI = (pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 & pRtHtCap->ShortGIfor20);
+	else
+		pMaxHtPhy->field.ShortGI = (pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 & pRtHtCap->ShortGIfor40);
+
+	for (i=23; i>=0; i--) // 3*3
+	{
+		j = i/8;
+		bitmask = (1<<(i-(j*8)));
+
+		if ((pActiveHtPhy->MCSSet[j] & bitmask) && (pDesireHtPhy->MCSSet[j] & bitmask))
+		{
+			pMaxHtPhy->field.MCS = i;
+			break;
+		}
+
+		if (i==0)
+			break;
+	}
+
+	// Copy MIN ht rate.  rt2860???
+	pMinHtPhy->field.BW = BW_20;
+	pMinHtPhy->field.MCS = 0;
+	pMinHtPhy->field.STBC = 0;
+	pMinHtPhy->field.ShortGI = 0;
+	//If STA assigns fixed rate. update to fixed here.
+#ifdef CONFIG_STA_SUPPORT
+	if ( (pAd->OpMode == OPMODE_STA) && (pDesireHtPhy->MCSSet[0] != 0xff))
+	{
+		if (pDesireHtPhy->MCSSet[4] != 0)
+		{
+			pMaxHtPhy->field.MCS = 32;
+			pMinHtPhy->field.MCS = 32;
+			DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates<=== Use Fixed MCS = %d\n",pMinHtPhy->field.MCS));
+		}
+
+		for (i=23; (CHAR)i >= 0; i--) // 3*3
+		{
+			j = i/8;
+			bitmask = (1<<(i-(j*8)));
+			if ( (pDesireHtPhy->MCSSet[j] & bitmask) && (pActiveHtPhy->MCSSet[j] & bitmask))
+			{
+				pMaxHtPhy->field.MCS = i;
+				pMinHtPhy->field.MCS = i;
+				break;
+			}
+			if (i==0)
+				break;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+	// Decide ht rate
+	pHtPhy->field.STBC = pMaxHtPhy->field.STBC;
+	pHtPhy->field.BW = pMaxHtPhy->field.BW;
+	pHtPhy->field.MODE = pMaxHtPhy->field.MODE;
+	pHtPhy->field.MCS = pMaxHtPhy->field.MCS;
+	pHtPhy->field.ShortGI = pMaxHtPhy->field.ShortGI;
+
+	// use default now. rt2860
+	if (pDesireHtPhy->MCSSet[0] != 0xff)
+		*auto_rate_cur_p = FALSE;
+	else
+		*auto_rate_cur_p = TRUE;
+
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateHtTxRates<---.AMsduSize = %d  \n", pAd->CommonCfg.DesiredHtPhy.AmsduSize ));
+	DBGPRINT(RT_DEBUG_TRACE,("TX: MCS[0] = %x (choose %d), BW = %d, ShortGI = %d, MODE = %d,  \n", pActiveHtPhy->MCSSet[0],pHtPhy->field.MCS,
+		pHtPhy->field.BW, pHtPhy->field.ShortGI, pHtPhy->field.MODE));
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates<=== \n"));
+}
+#endif // DOT11_N_SUPPORT //
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeRadioOff(
+	IN PRTMP_ADAPTER pAd)
+{
+	RT28XX_MLME_RADIO_OFF(pAd);
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeRadioOn(
+	IN PRTMP_ADAPTER pAd)
+{
+	RT28XX_MLME_RADIO_ON(pAd);
+}
+
+// ===========================================================================================
+// bss_table.c
+// ===========================================================================================
+
+
+/*! \brief initialize BSS table
+ *	\param p_tab pointer to the table
+ *	\return none
+ *	\pre
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID BssTableInit(
+	IN BSS_TABLE *Tab)
+{
+	int i;
+
+	Tab->BssNr = 0;
+    Tab->BssOverlapNr = 0;
+	for (i = 0; i < MAX_LEN_OF_BSS_TABLE; i++)
+	{
+		NdisZeroMemory(&Tab->BssEntry[i], sizeof(BSS_ENTRY));
+		Tab->BssEntry[i].Rssi = -127;	// initial the rssi as a minimum value
+	}
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInit(
+	IN PRTMP_ADAPTER pAd,
+    IN BA_TABLE *Tab)
+{
+	int i;
+
+	Tab->numAsOriginator = 0;
+	Tab->numAsRecipient = 0;
+	NdisAllocateSpinLock(&pAd->BATabLock);
+	for (i = 0; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		Tab->BARecEntry[i].REC_BA_Status = Recipient_NONE;
+		NdisAllocateSpinLock(&(Tab->BARecEntry[i].RxReRingLock));
+	}
+	for (i = 0; i < MAX_LEN_OF_BA_ORI_TABLE; i++)
+	{
+		Tab->BAOriEntry[i].ORI_BA_Status = Originator_NONE;
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+/*! \brief search the BSS table by SSID
+ *	\param p_tab pointer to the bss table
+ *	\param ssid SSID string
+ *	\return index of the table, BSS_NOT_FOUND if not in the table
+ *	\pre
+ *	\post
+ *	\note search by sequential search
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+ULONG BssTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 pBssid,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		//
+		// Some AP that support A/B/G mode that may used the same BSSID on 11A and 11B/G.
+		// We should distinguish this case.
+		//
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			 ((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+ULONG BssSsidTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 pBssid,
+	IN PUCHAR	 pSsid,
+	IN UCHAR	 SsidLen,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		//
+		// Some AP that support A/B/G mode that may used the same BSSID on 11A and 11B/G.
+		// We should distinguish this case.
+		//
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			 ((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid) &&
+			SSID_EQUAL(pSsid, SsidLen, Tab->BssEntry[i].Ssid, Tab->BssEntry[i].SsidLen))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+ULONG BssTableSearchWithSSID(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 Bssid,
+	IN PUCHAR	 pSsid,
+	IN UCHAR	 SsidLen,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(&(Tab->BssEntry[i].Bssid), Bssid) &&
+			(SSID_EQUAL(pSsid, SsidLen, Tab->BssEntry[i].Ssid, Tab->BssEntry[i].SsidLen) ||
+			(NdisEqualMemory(pSsid, ZeroSsid, SsidLen)) ||
+			(NdisEqualMemory(Tab->BssEntry[i].Ssid, ZeroSsid, Tab->BssEntry[i].SsidLen))))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableDeleteEntry(
+	IN OUT	BSS_TABLE *Tab,
+	IN		PUCHAR	  pBssid,
+	IN		UCHAR	  Channel)
+{
+	UCHAR i, j;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		if ((Tab->BssEntry[i].Channel == Channel) &&
+			(MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid)))
+		{
+			for (j = i; j < Tab->BssNr - 1; j++)
+			{
+				NdisMoveMemory(&(Tab->BssEntry[j]), &(Tab->BssEntry[j + 1]), sizeof(BSS_ENTRY));
+			}
+			NdisZeroMemory(&(Tab->BssEntry[Tab->BssNr - 1]), sizeof(BSS_ENTRY));
+			Tab->BssNr -= 1;
+			return;
+		}
+	}
+}
+
+#ifdef DOT11_N_SUPPORT
+/*
+	========================================================================
+	Routine Description:
+		Delete the Originator Entry in BAtable. Or decrease numAs Originator by 1 if needed.
+
+	Arguments:
+	// IRQL = DISPATCH_LEVEL
+	========================================================================
+*/
+VOID BATableDeleteORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_ORI_ENTRY	*pBAORIEntry)
+{
+
+	if (pBAORIEntry->ORI_BA_Status != Originator_NONE)
+	{
+		NdisAcquireSpinLock(&pAd->BATabLock);
+		if (pBAORIEntry->ORI_BA_Status == Originator_Done)
+		{
+			pAd->BATable.numAsOriginator -= 1;
+			DBGPRINT(RT_DEBUG_TRACE, ("BATableDeleteORIEntry numAsOriginator= %ld\n", pAd->BATable.numAsRecipient));
+			// Erase Bitmap flag.
+		}
+		pAd->MacTab.Content[pBAORIEntry->Wcid].TXBAbitmap &= (~(1<<(pBAORIEntry->TID) ));	// If STA mode,  erase flag here
+		pAd->MacTab.Content[pBAORIEntry->Wcid].BAOriWcidArray[pBAORIEntry->TID] = 0;	// If STA mode,  erase flag here
+		pBAORIEntry->ORI_BA_Status = Originator_NONE;
+		pBAORIEntry->Token = 1;
+		// Not clear Sequence here.
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+/*! \brief
+ *	\param
+ *	\return
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID BssEntrySet(
+	IN PRTMP_ADAPTER	pAd,
+	OUT BSS_ENTRY *pBss,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN PCF_PARM pCfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+	COPY_MAC_ADDR(pBss->Bssid, pBssid);
+	// Default Hidden SSID to be TRUE, it will be turned to FALSE after coping SSID
+	pBss->Hidden = 1;
+	if (SsidLen > 0)
+	{
+		// For hidden SSID AP, it might send beacon with SSID len equal to 0
+		// Or send beacon /probe response with SSID len matching real SSID length,
+		// but SSID is all zero. such as "00-00-00-00" with length 4.
+		// We have to prevent this case overwrite correct table
+		if (NdisEqualMemory(Ssid, ZeroSsid, SsidLen) == 0)
+		{
+		    NdisZeroMemory(pBss->Ssid, MAX_LEN_OF_SSID);
+			NdisMoveMemory(pBss->Ssid, Ssid, SsidLen);
+			pBss->SsidLen = SsidLen;
+			pBss->Hidden = 0;
+		}
+	}
+	else
+		pBss->SsidLen = 0;
+	pBss->BssType = BssType;
+	pBss->BeaconPeriod = BeaconPeriod;
+	if (BssType == BSS_INFRA)
+	{
+		if (pCfParm->bValid)
+		{
+			pBss->CfpCount = pCfParm->CfpCount;
+			pBss->CfpPeriod = pCfParm->CfpPeriod;
+			pBss->CfpMaxDuration = pCfParm->CfpMaxDuration;
+			pBss->CfpDurRemaining = pCfParm->CfpDurRemaining;
+		}
+	}
+	else
+	{
+		pBss->AtimWin = AtimWin;
+	}
+
+	pBss->CapabilityInfo = CapabilityInfo;
+	// The privacy bit indicate security is ON, it maight be WEP, TKIP or AES
+	// Combine with AuthMode, they will decide the connection methods.
+	pBss->Privacy = CAP_IS_PRIVACY_ON(pBss->CapabilityInfo);
+	ASSERT(SupRateLen <= MAX_LEN_OF_SUPPORTED_RATES);
+	if (SupRateLen <= MAX_LEN_OF_SUPPORTED_RATES)
+		NdisMoveMemory(pBss->SupRate, SupRate, SupRateLen);
+	else
+		NdisMoveMemory(pBss->SupRate, SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+	pBss->SupRateLen = SupRateLen;
+	ASSERT(ExtRateLen <= MAX_LEN_OF_SUPPORTED_RATES);
+	NdisMoveMemory(pBss->ExtRate, ExtRate, ExtRateLen);
+	NdisMoveMemory(&pBss->HtCapability, pHtCapability, HtCapabilityLen);
+	NdisMoveMemory(&pBss->AddHtInfo, pAddHtInfo, AddHtInfoLen);
+	pBss->NewExtChanOffset = NewExtChanOffset;
+	pBss->ExtRateLen = ExtRateLen;
+	pBss->Channel = Channel;
+	pBss->CentralChannel = Channel;
+	pBss->Rssi = Rssi;
+	// Update CkipFlag. if not exists, the value is 0x0
+	pBss->CkipFlag = CkipFlag;
+
+	// New for microsoft Fixed IEs
+	NdisMoveMemory(pBss->FixIEs.Timestamp, &TimeStamp, 8);
+	pBss->FixIEs.BeaconInterval = BeaconPeriod;
+	pBss->FixIEs.Capabilities = CapabilityInfo;
+
+	// New for microsoft Variable IEs
+	if (LengthVIE != 0)
+	{
+		pBss->VarIELen = LengthVIE;
+		NdisMoveMemory(pBss->VarIEs, pVIE, pBss->VarIELen);
+	}
+	else
+	{
+		pBss->VarIELen = 0;
+	}
+
+	pBss->AddHtInfoLen = 0;
+	pBss->HtCapabilityLen = 0;
+#ifdef DOT11_N_SUPPORT
+	if (HtCapabilityLen> 0)
+	{
+		pBss->HtCapabilityLen = HtCapabilityLen;
+		NdisMoveMemory(&pBss->HtCapability, pHtCapability, HtCapabilityLen);
+		if (AddHtInfoLen > 0)
+		{
+			pBss->AddHtInfoLen = AddHtInfoLen;
+			NdisMoveMemory(&pBss->AddHtInfo, pAddHtInfo, AddHtInfoLen);
+
+	 			if ((pAddHtInfo->ControlChan > 2)&& (pAddHtInfo->AddHtInfo.ExtChanOffset == EXTCHA_BELOW) && (pHtCapability->HtCapInfo.ChannelWidth == BW_40))
+	 			{
+	 				pBss->CentralChannel = pAddHtInfo->ControlChan - 2;
+	 			}
+	 			else if ((pAddHtInfo->AddHtInfo.ExtChanOffset == EXTCHA_ABOVE) && (pHtCapability->HtCapInfo.ChannelWidth == BW_40))
+				{
+		 				pBss->CentralChannel = pAddHtInfo->ControlChan + 2;
+				}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	BssCipherParse(pBss);
+
+	// new for QOS
+	if (pEdcaParm)
+		NdisMoveMemory(&pBss->EdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+	else
+		pBss->EdcaParm.bValid = FALSE;
+	if (pQosCapability)
+		NdisMoveMemory(&pBss->QosCapability, pQosCapability, sizeof(QOS_CAPABILITY_PARM));
+	else
+		pBss->QosCapability.bValid = FALSE;
+	if (pQbssLoad)
+		NdisMoveMemory(&pBss->QbssLoad, pQbssLoad, sizeof(QBSS_LOAD_PARM));
+	else
+		pBss->QbssLoad.bValid = FALSE;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		PEID_STRUCT     pEid;
+		USHORT          Length = 0;
+
+
+		NdisZeroMemory(&pBss->WpaIE.IE[0], MAX_CUSTOM_LEN);
+		NdisZeroMemory(&pBss->RsnIE.IE[0], MAX_CUSTOM_LEN);
+#ifdef EXT_BUILD_CHANNEL_LIST
+		NdisZeroMemory(&pBss->CountryString[0], 3);
+		pBss->bHasCountryIE = FALSE;
+#endif // EXT_BUILD_CHANNEL_LIST //
+		pEid = (PEID_STRUCT) pVIE;
+		while ((Length + 2 + (USHORT)pEid->Len) <= LengthVIE)
+		{
+			switch(pEid->Eid)
+			{
+				case IE_WPA:
+					if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+					{
+						if ((pEid->Len + 2) > MAX_CUSTOM_LEN)
+						{
+							pBss->WpaIE.IELen = 0;
+							break;
+						}
+						pBss->WpaIE.IELen = pEid->Len + 2;
+						NdisMoveMemory(pBss->WpaIE.IE, pEid, pBss->WpaIE.IELen);
+					}
+					break;
+                case IE_RSN:
+                    if (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+					{
+						if ((pEid->Len + 2) > MAX_CUSTOM_LEN)
+						{
+							pBss->RsnIE.IELen = 0;
+							break;
+						}
+						pBss->RsnIE.IELen = pEid->Len + 2;
+						NdisMoveMemory(pBss->RsnIE.IE, pEid, pBss->RsnIE.IELen);
+			}
+				break;
+#ifdef EXT_BUILD_CHANNEL_LIST
+				case IE_COUNTRY:
+					NdisMoveMemory(&pBss->CountryString[0], pEid->Octet, 3);
+					pBss->bHasCountryIE = TRUE;
+					break;
+#endif // EXT_BUILD_CHANNEL_LIST //
+            }
+			Length = Length + 2 + (USHORT)pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+			pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+}
+
+/*!
+ *	\brief insert an entry into the bss table
+ *	\param p_tab The BSS table
+ *	\param Bssid BSSID
+ *	\param ssid SSID
+ *	\param ssid_len Length of SSID
+ *	\param bss_type
+ *	\param beacon_period
+ *	\param timestamp
+ *	\param p_cf
+ *	\param atim_win
+ *	\param cap
+ *	\param rates
+ *	\param rates_len
+ *	\param channel_idx
+ *	\return none
+ *	\pre
+ *	\post
+ *	\note If SSID is identical, the old entry will be replaced by the new one
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+ULONG BssTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT BSS_TABLE *Tab,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN CF_PARM *CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR ChannelNo,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+	ULONG	Idx;
+
+	Idx = BssTableSearchWithSSID(Tab, pBssid,  Ssid, SsidLen, ChannelNo);
+	if (Idx == BSS_NOT_FOUND)
+	{
+		if (Tab->BssNr >= MAX_LEN_OF_BSS_TABLE)
+	    {
+			//
+			// It may happen when BSS Table was full.
+			// The desired AP will not be added into BSS Table
+			// In this case, if we found the desired AP then overwrite BSS Table.
+			//
+			if(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+			{
+				if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, pBssid) ||
+					SSID_EQUAL(pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, Ssid, SsidLen))
+				{
+					Idx = Tab->BssOverlapNr;
+					BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod, CfParm, AtimWin,
+						CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+						NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+                    Tab->BssOverlapNr = (Tab->BssOverlapNr++) % MAX_LEN_OF_BSS_TABLE;
+				}
+				return Idx;
+			}
+			else
+			{
+			return BSS_NOT_FOUND;
+			}
+		}
+		Idx = Tab->BssNr;
+		BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod, CfParm, AtimWin,
+					CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+					NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+		Tab->BssNr++;
+	}
+	else
+	{
+		BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod,CfParm, AtimWin,
+					CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+					NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+	}
+
+	return Idx;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+VOID  TriEventInit(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR		i;
+
+	for (i = 0;i < MAX_TRIGGER_EVENT;i++)
+		pAd->CommonCfg.TriggerEventTab.EventA[i].bValid = FALSE;
+
+	pAd->CommonCfg.TriggerEventTab.EventANo = 0;
+	pAd->CommonCfg.TriggerEventTab.EventBCountDown = 0;
+}
+
+ULONG TriEventTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT TRIGGER_EVENT_TAB *Tab,
+	IN PUCHAR pBssid,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			RegClass,
+	IN UCHAR ChannelNo)
+{
+	// Event A
+	if (HtCapabilityLen == 0)
+	{
+		if (Tab->EventANo < MAX_TRIGGER_EVENT)
+		{
+			RTMPMoveMemory(Tab->EventA[Tab->EventANo].BSSID, pBssid, 6);
+			Tab->EventA[Tab->EventANo].bValid = TRUE;
+			Tab->EventA[Tab->EventANo].Channel = ChannelNo;
+			Tab->EventA[Tab->EventANo].CDCounter = pAd->CommonCfg.Dot11BssWidthChanTranDelay;
+			if (RegClass != 0)
+			{
+				// Beacon has Regulatory class IE. So use beacon's
+				Tab->EventA[Tab->EventANo].RegClass = RegClass;
+			}
+			else
+			{
+				// Use Station's Regulatory class instead.
+				if (pAd->StaActive.SupportedHtPhy.bHtEnable == TRUE)
+				{
+					if (pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel)
+					{
+						Tab->EventA[Tab->EventANo].RegClass = 32;
+					}
+					else if (pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel)
+						Tab->EventA[Tab->EventANo].RegClass = 33;
+				}
+				else
+					Tab->EventA[Tab->EventANo].RegClass = ??;
+
+			}
+
+			Tab->EventANo ++;
+		}
+	}
+	else if (pHtCapability->HtCapInfo.Intolerant40)
+	{
+		Tab->EventBCountDown = pAd->CommonCfg.Dot11BssWidthChanTranDelay;
+	}
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Trigger Event table Maintainence called once every second.
+
+	Arguments:
+	// IRQL = DISPATCH_LEVEL
+	========================================================================
+*/
+VOID TriEventCounterMaintenance(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR		i;
+	BOOLEAN			bNotify = FALSE;
+	for (i = 0;i < MAX_TRIGGER_EVENT;i++)
+	{
+		if (pAd->CommonCfg.TriggerEventTab.EventA[i].bValid && (pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter > 0))
+		{
+			pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter--;
+			if (pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter == 0)
+			{
+				pAd->CommonCfg.TriggerEventTab.EventA[i].bValid = FALSE;
+				pAd->CommonCfg.TriggerEventTab.EventANo --;
+				// Need to send 20/40 Coexistence Notify frame if has status change.
+				bNotify = TRUE;
+			}
+		}
+	}
+	if (pAd->CommonCfg.TriggerEventTab.EventBCountDown > 0)
+	{
+		pAd->CommonCfg.TriggerEventTab.EventBCountDown--;
+		if (pAd->CommonCfg.TriggerEventTab.EventBCountDown == 0)
+			bNotify = TRUE;
+	}
+
+	if (bNotify == TRUE)
+		Update2040CoexistFrameAndNotify(pAd, BSSID_WCID, TRUE);
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableSsidSort(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT BSS_TABLE *OutTab,
+	IN	CHAR Ssid[],
+	IN	UCHAR SsidLen)
+{
+	INT i;
+	BssTableInit(OutTab);
+
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		BSS_ENTRY *pInBss = &pAd->ScanTab.BssEntry[i];
+		BOOLEAN	bIsHiddenApIncluded = FALSE;
+
+		if (((pAd->CommonCfg.bIEEE80211H == 1) &&
+            (pAd->MlmeAux.Channel > 14) &&
+             RadarChannelCheck(pAd, pInBss->Channel))
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+             || (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+#endif // CARRIER_DETECTION_SUPPORT //
+            )
+		{
+			if (pInBss->Hidden)
+				bIsHiddenApIncluded = TRUE;
+		}
+
+		if ((pInBss->BssType == pAd->StaCfg.BssType) &&
+			(SSID_EQUAL(Ssid, SsidLen, pInBss->Ssid, pInBss->SsidLen) || bIsHiddenApIncluded))
+		{
+			BSS_ENTRY *pOutBss = &OutTab->BssEntry[OutTab->BssNr];
+
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+			// If no Country IE exists no Connection will be established when IEEE80211dClientMode is strict.
+			if ((pAd->StaCfg.IEEE80211dClientMode == Rt802_11_D_Strict) &&
+				(pInBss->bHasCountryIE == FALSE))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("StaCfg.IEEE80211dClientMode == Rt802_11_D_Strict, but this AP doesn't have country IE.\n"));
+				continue;
+			}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef DOT11_N_SUPPORT
+			// 2.4G/5G N only mode
+			if ((pInBss->HtCapabilityLen == 0) &&
+				((pAd->CommonCfg.PhyMode == PHY_11N_2_4G) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("STA is in N-only Mode, this AP don't have Ht capability in Beacon.\n"));
+				continue;
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// New for WPA2
+			// Check the Authmode first
+			if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+			{
+				// Check AuthMode and AuthModeAux for matching, in case AP support dual-mode
+				if ((pAd->StaCfg.AuthMode != pInBss->AuthMode) && (pAd->StaCfg.AuthMode != pInBss->AuthModeAux))
+					// None matched
+					continue;
+
+				// Check cipher suite, AP must have more secured cipher than station setting
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA.GroupCipher)
+							continue;
+
+					// check group cipher
+					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
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipherAux))
+						continue;
+				}
+				else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA2.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA2.GroupCipher)
+							continue;
+
+					// check group cipher
+					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
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipherAux))
+						continue;
+				}
+			}
+			// Bss Type matched, SSID matched.
+			// We will check wepstatus for qualification Bss
+			else if (pAd->StaCfg.WepStatus != pInBss->WepStatus)
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("StaCfg.WepStatus=%d, while pInBss->WepStatus=%d\n", pAd->StaCfg.WepStatus, pInBss->WepStatus));
+				//
+				// For the SESv2 case, we will not qualify WepStatus.
+				//
+				if (!pInBss->bSES)
+					continue;
+			}
+
+			// Since the AP is using hidden SSID, and we are trying to connect to ANY
+			// It definitely will fail. So, skip it.
+			// CCX also require not even try to connect it!!
+			if (SsidLen == 0)
+				continue;
+
+#ifdef DOT11_N_SUPPORT
+			// If both station and AP use 40MHz, still need to check if the 40MHZ band's legality in my country region
+			// If this 40MHz wideband is not allowed in my country list, use bandwidth 20MHZ instead,
+			if ((pInBss->CentralChannel != pInBss->Channel) &&
+				(pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40))
+			{
+				if (RTMPCheckChannel(pAd, pInBss->CentralChannel, pInBss->Channel) == FALSE)
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_20;
+					SetCommonHT(pAd);
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_40;
+				}
+				else
+				{
+					if (pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BAND_WIDTH_20)
+					{
+						SetCommonHT(pAd);
+					}
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// copy matching BSS from InTab to OutTab
+			NdisMoveMemory(pOutBss, pInBss, sizeof(BSS_ENTRY));
+
+			OutTab->BssNr++;
+		}
+		else if ((pInBss->BssType == pAd->StaCfg.BssType) && (SsidLen == 0))
+		{
+			BSS_ENTRY *pOutBss = &OutTab->BssEntry[OutTab->BssNr];
+
+
+#ifdef DOT11_N_SUPPORT
+			// 2.4G/5G N only mode
+			if ((pInBss->HtCapabilityLen == 0) &&
+				((pAd->CommonCfg.PhyMode == PHY_11N_2_4G) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("STA is in N-only Mode, this AP don't have Ht capability in Beacon.\n"));
+				continue;
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// New for WPA2
+			// Check the Authmode first
+			if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+			{
+				// Check AuthMode and AuthModeAux for matching, in case AP support dual-mode
+				if ((pAd->StaCfg.AuthMode != pInBss->AuthMode) && (pAd->StaCfg.AuthMode != pInBss->AuthModeAux))
+					// None matched
+					continue;
+
+				// Check cipher suite, AP must have more secured cipher than station setting
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipherAux))
+						continue;
+				}
+				else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA2.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA2.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA2.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipherAux))
+						continue;
+				}
+			}
+			// Bss Type matched, SSID matched.
+			// We will check wepstatus for qualification Bss
+			else if (pAd->StaCfg.WepStatus != pInBss->WepStatus)
+					continue;
+
+#ifdef DOT11_N_SUPPORT
+			// If both station and AP use 40MHz, still need to check if the 40MHZ band's legality in my country region
+			// If this 40MHz wideband is not allowed in my country list, use bandwidth 20MHZ instead,
+			if ((pInBss->CentralChannel != pInBss->Channel) &&
+				(pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40))
+			{
+				if (RTMPCheckChannel(pAd, pInBss->CentralChannel, pInBss->Channel) == FALSE)
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_20;
+					SetCommonHT(pAd);
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_40;
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// copy matching BSS from InTab to OutTab
+			NdisMoveMemory(pOutBss, pInBss, sizeof(BSS_ENTRY));
+
+			OutTab->BssNr++;
+		}
+
+		if (OutTab->BssNr >= MAX_LEN_OF_BSS_TABLE)
+			break;
+	}
+
+	BssTableSortByRssi(OutTab);
+}
+
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableSortByRssi(
+	IN OUT BSS_TABLE *OutTab)
+{
+	INT 	  i, j;
+	BSS_ENTRY TmpBss;
+
+	for (i = 0; i < OutTab->BssNr - 1; i++)
+	{
+		for (j = i+1; j < OutTab->BssNr; j++)
+		{
+			if (OutTab->BssEntry[j].Rssi > OutTab->BssEntry[i].Rssi)
+			{
+				NdisMoveMemory(&TmpBss, &OutTab->BssEntry[j], sizeof(BSS_ENTRY));
+				NdisMoveMemory(&OutTab->BssEntry[j], &OutTab->BssEntry[i], sizeof(BSS_ENTRY));
+				NdisMoveMemory(&OutTab->BssEntry[i], &TmpBss, sizeof(BSS_ENTRY));
+			}
+		}
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+VOID BssCipherParse(
+	IN OUT	PBSS_ENTRY	pBss)
+{
+	PEID_STRUCT 		 pEid;
+	PUCHAR				pTmp;
+	PRSN_IE_HEADER_STRUCT			pRsnHeader;
+	PCIPHER_SUITE_STRUCT			pCipher;
+	PAKM_SUITE_STRUCT				pAKM;
+	USHORT							Count;
+	INT								Length;
+	NDIS_802_11_ENCRYPTION_STATUS	TmpCipher;
+
+	//
+	// WepStatus will be reset later, if AP announce TKIP or AES on the beacon frame.
+	//
+	if (pBss->Privacy)
+	{
+		pBss->WepStatus 	= Ndis802_11WEPEnabled;
+	}
+	else
+	{
+		pBss->WepStatus 	= Ndis802_11WEPDisabled;
+	}
+	// Set default to disable & open authentication before parsing variable IE
+	pBss->AuthMode		= Ndis802_11AuthModeOpen;
+	pBss->AuthModeAux	= Ndis802_11AuthModeOpen;
+
+	// Init WPA setting
+	pBss->WPA.PairCipher	= Ndis802_11WEPDisabled;
+	pBss->WPA.PairCipherAux = Ndis802_11WEPDisabled;
+	pBss->WPA.GroupCipher	= Ndis802_11WEPDisabled;
+	pBss->WPA.RsnCapability = 0;
+	pBss->WPA.bMixMode		= FALSE;
+
+	// Init WPA2 setting
+	pBss->WPA2.PairCipher	 = Ndis802_11WEPDisabled;
+	pBss->WPA2.PairCipherAux = Ndis802_11WEPDisabled;
+	pBss->WPA2.GroupCipher	 = Ndis802_11WEPDisabled;
+	pBss->WPA2.RsnCapability = 0;
+	pBss->WPA2.bMixMode 	 = FALSE;
+
+
+	Length = (INT) pBss->VarIELen;
+
+	while (Length > 0)
+	{
+		// Parse cipher suite base on WPA1 & WPA2, they should be parsed differently
+		pTmp = ((PUCHAR) pBss->VarIEs) + pBss->VarIELen - Length;
+		pEid = (PEID_STRUCT) pTmp;
+		switch (pEid->Eid)
+		{
+			case IE_WPA:
+				//Parse Cisco IE_WPA (LEAP, CCKM, etc.)
+				if ( NdisEqualMemory((pTmp+8), CISCO_OUI, 3))
+				{
+					pTmp   += 11;
+					switch (*pTmp)
+					{
+						case 1:
+						case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							pBss->WepStatus = Ndis802_11Encryption1Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							pBss->WepStatus = Ndis802_11Encryption2Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 4:
+							pBss->WepStatus = Ndis802_11Encryption3Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						default:
+							break;
+					}
+
+					// if Cisco IE_WPA, break
+					break;
+				}
+				else if (NdisEqualMemory(pEid->Octet, SES_OUI, 3) && (pEid->Len == 7))
+				{
+					pBss->bSES = TRUE;
+					break;
+				}
+				else if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4) != 1)
+				{
+					// if unsupported vendor specific IE
+					break;
+				}
+				// Skip OUI, version, and multicast suite
+				// This part should be improved in the future when AP supported multiple cipher suite.
+				// For now, it's OK since almost all APs have fixed cipher suite supported.
+				// pTmp = (PUCHAR) pEid->Octet;
+				pTmp   += 11;
+
+				// Cipher Suite Selectors from Spec P802.11i/D3.2 P26.
+				//	Value	   Meaning
+				//	0			None
+				//	1			WEP-40
+				//	2			Tkip
+				//	3			WRAP
+				//	4			AES
+				//	5			WEP-104
+				// Parse group cipher
+				switch (*pTmp)
+				{
+					case 1:
+						pBss->WPA.GroupCipher = Ndis802_11GroupWEP40Enabled;
+						break;
+					case 5:
+						pBss->WPA.GroupCipher = Ndis802_11GroupWEP104Enabled;
+						break;
+					case 2:
+						pBss->WPA.GroupCipher = Ndis802_11Encryption2Enabled;
+						break;
+					case 4:
+						pBss->WPA.GroupCipher = Ndis802_11Encryption3Enabled;
+						break;
+					default:
+						break;
+				}
+				// number of unicast suite
+				pTmp   += 1;
+
+				// skip all unicast cipher suites
+				//Count = *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// Parsing all unicast cipher suite
+				while (Count > 0)
+				{
+					// Skip OUI
+					pTmp += 3;
+					TmpCipher = Ndis802_11WEPDisabled;
+					switch (*pTmp)
+					{
+						case 1:
+						case 5: // Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							TmpCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							TmpCipher = Ndis802_11Encryption2Enabled;
+							break;
+						case 4:
+							TmpCipher = Ndis802_11Encryption3Enabled;
+							break;
+						default:
+							break;
+					}
+					if (TmpCipher > pBss->WPA.PairCipher)
+					{
+						// Move the lower cipher suite to PairCipherAux
+						pBss->WPA.PairCipherAux = pBss->WPA.PairCipher;
+						pBss->WPA.PairCipher	= TmpCipher;
+					}
+					else
+					{
+						pBss->WPA.PairCipherAux = TmpCipher;
+					}
+					pTmp++;
+					Count--;
+				}
+
+				// 4. get AKM suite counts
+				//Count	= *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+				pTmp   += 3;
+
+				switch (*pTmp)
+				{
+					case 1:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA;
+						break;
+					case 2:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPAPSK;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPAPSK;
+						break;
+					default:
+						break;
+				}
+				pTmp   += 1;
+
+				// Fixed for WPA-None
+				if (pBss->BssType == BSS_ADHOC)
+				{
+					pBss->AuthMode	  = Ndis802_11AuthModeWPANone;
+					pBss->AuthModeAux = Ndis802_11AuthModeWPANone;
+					pBss->WepStatus   = pBss->WPA.GroupCipher;
+					// Patched bugs for old driver
+					if (pBss->WPA.PairCipherAux == Ndis802_11WEPDisabled)
+						pBss->WPA.PairCipherAux = pBss->WPA.GroupCipher;
+				}
+				else
+					pBss->WepStatus   = pBss->WPA.PairCipher;
+
+				// Check the Pair & Group, if different, turn on mixed mode flag
+				if (pBss->WPA.GroupCipher != pBss->WPA.PairCipher)
+					pBss->WPA.bMixMode = TRUE;
+
+				break;
+
+			case IE_RSN:
+				pRsnHeader = (PRSN_IE_HEADER_STRUCT) pTmp;
+
+				// 0. Version must be 1
+				if (le2cpu16(pRsnHeader->Version) != 1)
+					break;
+				pTmp   += sizeof(RSN_IE_HEADER_STRUCT);
+
+				// 1. Check group cipher
+				pCipher = (PCIPHER_SUITE_STRUCT) pTmp;
+				if (!RTMPEqualMemory(pTmp, RSN_OUI, 3))
+					break;
+
+				// Parse group cipher
+				switch (pCipher->Type)
+				{
+					case 1:
+						pBss->WPA2.GroupCipher = Ndis802_11GroupWEP40Enabled;
+						break;
+					case 5:
+						pBss->WPA2.GroupCipher = Ndis802_11GroupWEP104Enabled;
+						break;
+					case 2:
+						pBss->WPA2.GroupCipher = Ndis802_11Encryption2Enabled;
+						break;
+					case 4:
+						pBss->WPA2.GroupCipher = Ndis802_11Encryption3Enabled;
+						break;
+					default:
+						break;
+				}
+				// set to correct offset for next parsing
+				pTmp   += sizeof(CIPHER_SUITE_STRUCT);
+
+				// 2. Get pairwise cipher counts
+				//Count = *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// 3. Get pairwise cipher
+				// Parsing all unicast cipher suite
+				while (Count > 0)
+				{
+					// Skip OUI
+					pCipher = (PCIPHER_SUITE_STRUCT) pTmp;
+					TmpCipher = Ndis802_11WEPDisabled;
+					switch (pCipher->Type)
+					{
+						case 1:
+						case 5: // Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							TmpCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							TmpCipher = Ndis802_11Encryption2Enabled;
+							break;
+						case 4:
+							TmpCipher = Ndis802_11Encryption3Enabled;
+							break;
+						default:
+							break;
+					}
+					if (TmpCipher > pBss->WPA2.PairCipher)
+					{
+						// Move the lower cipher suite to PairCipherAux
+						pBss->WPA2.PairCipherAux = pBss->WPA2.PairCipher;
+						pBss->WPA2.PairCipher	 = TmpCipher;
+					}
+					else
+					{
+						pBss->WPA2.PairCipherAux = TmpCipher;
+					}
+					pTmp += sizeof(CIPHER_SUITE_STRUCT);
+					Count--;
+				}
+
+				// 4. get AKM suite counts
+				//Count	= *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// 5. Get AKM ciphers
+				pAKM = (PAKM_SUITE_STRUCT) pTmp;
+				if (!RTMPEqualMemory(pTmp, RSN_OUI, 3))
+					break;
+
+				switch (pAKM->Type)
+				{
+					case 1:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA2;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA2;
+						break;
+					case 2:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA2PSK;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA2PSK;
+						break;
+					default:
+						break;
+				}
+				pTmp   += (Count * sizeof(AKM_SUITE_STRUCT));
+
+				// Fixed for WPA-None
+				if (pBss->BssType == BSS_ADHOC)
+				{
+					pBss->AuthMode = Ndis802_11AuthModeWPANone;
+					pBss->AuthModeAux = Ndis802_11AuthModeWPANone;
+					pBss->WPA.PairCipherAux = pBss->WPA2.PairCipherAux;
+					pBss->WPA.GroupCipher	= pBss->WPA2.GroupCipher;
+					pBss->WepStatus 		= pBss->WPA.GroupCipher;
+					// Patched bugs for old driver
+					if (pBss->WPA.PairCipherAux == Ndis802_11WEPDisabled)
+						pBss->WPA.PairCipherAux = pBss->WPA.GroupCipher;
+				}
+				pBss->WepStatus   = pBss->WPA2.PairCipher;
+
+				// 6. Get RSN capability
+				//pBss->WPA2.RsnCapability = *(PUSHORT) pTmp;
+				pBss->WPA2.RsnCapability = (pTmp[1]<<8) + pTmp[0];
+				pTmp += sizeof(USHORT);
+
+				// Check the Pair & Group, if different, turn on mixed mode flag
+				if (pBss->WPA2.GroupCipher != pBss->WPA2.PairCipher)
+					pBss->WPA2.bMixMode = TRUE;
+
+				break;
+			default:
+				break;
+		}
+		Length -= (pEid->Len + 2);
+	}
+}
+
+// ===========================================================================================
+// mac_table.c
+// ===========================================================================================
+
+/*! \brief generates a random mac address value for IBSS BSSID
+ *	\param Addr the bssid location
+ *	\return none
+ *	\pre
+ *	\post
+ */
+VOID MacAddrRandomBssid(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pAddr)
+{
+	INT i;
+
+	for (i = 0; i < MAC_ADDR_LEN; i++)
+	{
+		pAddr[i] = RandomByte(pAd);
+	}
+
+	pAddr[0] = (pAddr[0] & 0xfe) | 0x02;  // the first 2 bits must be 01xxxxxxxx
+}
+
+/*! \brief init the management mac frame header
+ *	\param p_hdr mac header
+ *	\param subtype subtype of the frame
+ *	\param p_ds destination address, don't care if it is a broadcast address
+ *	\return none
+ *	\pre the station has the following information in the pAd->StaCfg
+ *	 - bssid
+ *	 - station address
+ *	\post
+ *	\note this function initializes the following field
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID MgtMacHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+
+	pHdr80211->FC.Type = BTYPE_MGMT;
+	pHdr80211->FC.SubType = SubType;
+//	if (SubType == SUBTYPE_ACK)	// sample, no use, it will conflict with ACTION frame sub type
+//		pHdr80211->FC.Type = BTYPE_CNTL;
+	pHdr80211->FC.ToDs = ToDs;
+	COPY_MAC_ADDR(pHdr80211->Addr1, pDA);
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+	COPY_MAC_ADDR(pHdr80211->Addr3, pBssid);
+}
+
+// ===========================================================================================
+// mem_mgmt.c
+// ===========================================================================================
+
+/*!***************************************************************************
+ * This routine build an outgoing frame, and fill all information specified
+ * in argument list to the frame body. The actual frame size is the summation
+ * of all arguments.
+ * input params:
+ *		Buffer - pointer to a pre-allocated memory segment
+ *		args - a list of <int arg_size, arg> pairs.
+ *		NOTE NOTE NOTE!!!! the last argument must be NULL, otherwise this
+ *						   function will FAIL!!!
+ * return:
+ *		Size of the buffer
+ * usage:
+ *		MakeOutgoingFrame(Buffer, output_length, 2, &fc, 2, &dur, 6, p_addr1, 6,p_addr2, END_OF_ARGS);
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ ****************************************************************************/
+ULONG MakeOutgoingFrame(
+	OUT CHAR *Buffer,
+	OUT ULONG *FrameLen, ...)
+{
+	CHAR   *p;
+	int 	leng;
+	ULONG	TotLeng;
+	va_list Args;
+
+	// calculates the total length
+	TotLeng = 0;
+	va_start(Args, FrameLen);
+	do
+	{
+		leng = va_arg(Args, int);
+		if (leng == END_OF_ARGS)
+		{
+			break;
+		}
+		p = va_arg(Args, PVOID);
+		NdisMoveMemory(&Buffer[TotLeng], p, leng);
+		TotLeng = TotLeng + leng;
+	} while(TRUE);
+
+	va_end(Args); /* clean up */
+	*FrameLen = TotLeng;
+	return TotLeng;
+}
+
+// ===========================================================================================
+// mlme_queue.c
+// ===========================================================================================
+
+/*! \brief	Initialize The MLME Queue, used by MLME Functions
+ *	\param	*Queue	   The MLME Queue
+ *	\return Always	   Return NDIS_STATE_SUCCESS in this implementation
+ *	\pre
+ *	\post
+ *	\note	Because this is done only once (at the init stage), no need to be locked
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+NDIS_STATUS MlmeQueueInit(
+	IN MLME_QUEUE *Queue)
+{
+	INT i;
+
+	NdisAllocateSpinLock(&Queue->Lock);
+
+	Queue->Num	= 0;
+	Queue->Head = 0;
+	Queue->Tail = 0;
+
+	for (i = 0; i < MAX_LEN_OF_MLME_QUEUE; i++)
+	{
+		Queue->Entry[i].Occupied = FALSE;
+		Queue->Entry[i].MsgLen = 0;
+		NdisZeroMemory(Queue->Entry[i].Msg, MGMT_DMA_BUFFER_SIZE);
+	}
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*! \brief	 Enqueue a message for other threads, if they want to send messages to MLME thread
+ *	\param	*Queue	  The MLME Queue
+ *	\param	 Machine  The State Machine Id
+ *	\param	 MsgType  The Message Type
+ *	\param	 MsgLen   The Message length
+ *	\param	*Msg	  The message pointer
+ *	\return  TRUE if enqueue is successful, FALSE if the queue is full
+ *	\pre
+ *	\post
+ *	\note	 The message has to be initialized
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeEnqueue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Machine,
+	IN ULONG MsgType,
+	IN ULONG MsgLen,
+	IN VOID *Msg)
+{
+	INT Tail;
+	MLME_QUEUE	*Queue = (MLME_QUEUE *)&pAd->Mlme.Queue;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return FALSE;
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("MlmeEnqueue: msg too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+
+	if (MlmeQueueFull(Queue))
+	{
+		return FALSE;
+	}
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Tail = Queue->Tail;
+	Queue->Tail++;
+	Queue->Num++;
+	if (Queue->Tail == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Tail = 0;
+	}
+
+	Queue->Entry[Tail].Wcid = RESERVED_WCID;
+	Queue->Entry[Tail].Occupied = TRUE;
+	Queue->Entry[Tail].Machine = Machine;
+	Queue->Entry[Tail].MsgType = MsgType;
+	Queue->Entry[Tail].MsgLen  = MsgLen;
+
+	if (Msg != NULL)
+	{
+		NdisMoveMemory(Queue->Entry[Tail].Msg, Msg, MsgLen);
+	}
+
+	NdisReleaseSpinLock(&(Queue->Lock));
+	return TRUE;
+}
+
+/*! \brief	 This function is used when Recv gets a MLME message
+ *	\param	*Queue			 The MLME Queue
+ *	\param	 TimeStampHigh	 The upper 32 bit of timestamp
+ *	\param	 TimeStampLow	 The lower 32 bit of timestamp
+ *	\param	 Rssi			 The receiving RSSI strength
+ *	\param	 MsgLen 		 The length of the message
+ *	\param	*Msg			 The message pointer
+ *	\return  TRUE if everything ok, FALSE otherwise (like Queue Full)
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeEnqueueForRecv(
+	IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Wcid,
+	IN ULONG TimeStampHigh,
+	IN ULONG TimeStampLow,
+	IN UCHAR Rssi0,
+	IN UCHAR Rssi1,
+	IN UCHAR Rssi2,
+	IN ULONG MsgLen,
+	IN VOID *Msg,
+	IN UCHAR Signal)
+{
+	INT 		 Tail, Machine;
+	PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+	INT		 MsgType;
+	MLME_QUEUE	*Queue = (MLME_QUEUE *)&pAd->Mlme.Queue;
+
+#ifdef RALINK_ATE
+	/* Nothing to do in ATE mode */
+	if(ATE_ON(pAd))
+		return FALSE;
+#endif // RALINK_ATE //
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		DBGPRINT_ERR(("MlmeEnqueueForRecv: fRTMP_ADAPTER_HALT_IN_PROGRESS\n"));
+		return FALSE;
+	}
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("MlmeEnqueueForRecv: frame too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+
+	if (MlmeQueueFull(Queue))
+	{
+		return FALSE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (!MsgTypeSubst(pAd, pFrame, &Machine, &MsgType))
+		{
+			DBGPRINT_ERR(("MlmeEnqueueForRecv: un-recongnized mgmt->subtype=%d\n",pFrame->Hdr.FC.SubType));
+			return FALSE;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// OK, we got all the informations, it is time to put things into queue
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Tail = Queue->Tail;
+	Queue->Tail++;
+	Queue->Num++;
+	if (Queue->Tail == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Tail = 0;
+	}
+	Queue->Entry[Tail].Occupied = TRUE;
+	Queue->Entry[Tail].Machine = Machine;
+	Queue->Entry[Tail].MsgType = MsgType;
+	Queue->Entry[Tail].MsgLen  = MsgLen;
+	Queue->Entry[Tail].TimeStamp.u.LowPart = TimeStampLow;
+	Queue->Entry[Tail].TimeStamp.u.HighPart = TimeStampHigh;
+	Queue->Entry[Tail].Rssi0 = Rssi0;
+	Queue->Entry[Tail].Rssi1 = Rssi1;
+	Queue->Entry[Tail].Rssi2 = Rssi2;
+	Queue->Entry[Tail].Signal = Signal;
+	Queue->Entry[Tail].Wcid = (UCHAR)Wcid;
+
+	Queue->Entry[Tail].Channel = pAd->LatchRfRegs.Channel;
+
+	if (Msg != NULL)
+	{
+		NdisMoveMemory(Queue->Entry[Tail].Msg, Msg, MsgLen);
+	}
+
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	RT28XX_MLME_HANDLER(pAd);
+
+	return TRUE;
+}
+
+
+/*! \brief	 Dequeue a message from the MLME Queue
+ *	\param	*Queue	  The MLME Queue
+ *	\param	*Elem	  The message dequeued from MLME Queue
+ *	\return  TRUE if the Elem contains something, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeDequeue(
+	IN MLME_QUEUE *Queue,
+	OUT MLME_QUEUE_ELEM **Elem)
+{
+	NdisAcquireSpinLock(&(Queue->Lock));
+	*Elem = &(Queue->Entry[Queue->Head]);
+	Queue->Num--;
+	Queue->Head++;
+	if (Queue->Head == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Head = 0;
+	}
+	NdisReleaseSpinLock(&(Queue->Lock));
+	return TRUE;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID	MlmeRestartStateMachine(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN				Cancelled;
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeRestartStateMachine \n"));
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef QOS_DLS_SUPPORT
+		UCHAR i;
+#endif // QOS_DLS_SUPPORT //
+		// Cancel all timer events
+		// Be careful to cancel new added timer
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,	  &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,  &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,	   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,	   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,	   &Cancelled);
+
+#ifdef QOS_DLS_SUPPORT
+		for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &Cancelled);
+		}
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Change back to original channel in case of doing scan
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// Resume MSDU which is turned off durning scan
+	RTMPResumeMsduTransmission(pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Set all state machines back IDLE
+		pAd->Mlme.CntlMachine.CurrState    = CNTL_IDLE;
+		pAd->Mlme.AssocMachine.CurrState   = ASSOC_IDLE;
+		pAd->Mlme.AuthMachine.CurrState    = AUTH_REQ_IDLE;
+		pAd->Mlme.AuthRspMachine.CurrState = AUTH_RSP_IDLE;
+		pAd->Mlme.SyncMachine.CurrState    = SYNC_IDLE;
+		pAd->Mlme.ActMachine.CurrState    = ACT_IDLE;
+#ifdef QOS_DLS_SUPPORT
+		pAd->Mlme.DlsMachine.CurrState    = DLS_IDLE;
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+/*! \brief	test if the MLME Queue is empty
+ *	\param	*Queue	  The MLME Queue
+ *	\return TRUE if the Queue is empty, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeQueueEmpty(
+	IN MLME_QUEUE *Queue)
+{
+	BOOLEAN Ans;
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Ans = (Queue->Num == 0);
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	return Ans;
+}
+
+/*! \brief	 test if the MLME Queue is full
+ *	\param	 *Queue 	 The MLME Queue
+ *	\return  TRUE if the Queue is empty, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeQueueFull(
+	IN MLME_QUEUE *Queue)
+{
+	BOOLEAN Ans;
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Ans = (Queue->Num == MAX_LEN_OF_MLME_QUEUE || Queue->Entry[Queue->Tail].Occupied);
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	return Ans;
+}
+
+/*! \brief	 The destructor of MLME Queue
+ *	\param
+ *	\return
+ *	\pre
+ *	\post
+ *	\note	Clear Mlme Queue, Set Queue->Num to Zero.
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID MlmeQueueDestroy(
+	IN MLME_QUEUE *pQueue)
+{
+	NdisAcquireSpinLock(&(pQueue->Lock));
+	pQueue->Num  = 0;
+	pQueue->Head = 0;
+	pQueue->Tail = 0;
+	NdisReleaseSpinLock(&(pQueue->Lock));
+	NdisFreeSpinLock(&(pQueue->Lock));
+}
+
+/*! \brief	 To substitute the message type if the message is coming from external
+ *	\param	pFrame		   The frame received
+ *	\param	*Machine	   The state machine
+ *	\param	*MsgType	   the message type for the state machine
+ *	\return TRUE if the substitution is successful, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+#ifdef CONFIG_STA_SUPPORT
+BOOLEAN MsgTypeSubst(
+	IN PRTMP_ADAPTER  pAd,
+	IN PFRAME_802_11 pFrame,
+	OUT INT *Machine,
+	OUT INT *MsgType)
+{
+	USHORT	Seq;
+	UCHAR	EAPType;
+	PUCHAR	pData;
+
+	// Pointer to start of data frames including SNAP header
+	pData = (PUCHAR) pFrame + LENGTH_802_11;
+
+	// The only data type will pass to this function is EAPOL frame
+	if (pFrame->Hdr.FC.Type == BTYPE_DATA)
+	{
+		if (NdisEqualMemory(SNAP_AIRONET, pData, LENGTH_802_1_H))
+		{
+			// Cisco Aironet SNAP header
+			*Machine = AIRONET_STATE_MACHINE;
+			*MsgType = MT2_AIRONET_MSG;
+			return (TRUE);
+		}
+#ifdef LEAP_SUPPORT
+		if ( pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP ) //LEAP
+		{
+			// LEAP frames
+			*Machine = LEAP_STATE_MACHINE;
+			EAPType = *((UCHAR*)pFrame + LENGTH_802_11 + LENGTH_802_1_H + 1);
+			return (LeapMsgTypeSubst(EAPType, MsgType));
+		}
+		else
+#endif // LEAP_SUPPORT //
+		{
+			*Machine = WPA_PSK_STATE_MACHINE;
+			EAPType = *((UCHAR*)pFrame + LENGTH_802_11 + LENGTH_802_1_H + 1);
+			return(WpaMsgTypeSubst(EAPType, MsgType));
+		}
+	}
+
+	switch (pFrame->Hdr.FC.SubType)
+	{
+		case SUBTYPE_ASSOC_REQ:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ASSOC_REQ;
+			break;
+		case SUBTYPE_ASSOC_RSP:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ASSOC_RSP;
+			break;
+		case SUBTYPE_REASSOC_REQ:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_REASSOC_REQ;
+			break;
+		case SUBTYPE_REASSOC_RSP:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_REASSOC_RSP;
+			break;
+		case SUBTYPE_PROBE_REQ:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_PROBE_REQ;
+			break;
+		case SUBTYPE_PROBE_RSP:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_PROBE_RSP;
+			break;
+		case SUBTYPE_BEACON:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_BEACON;
+			break;
+		case SUBTYPE_ATIM:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ATIM;
+			break;
+		case SUBTYPE_DISASSOC:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_DISASSOC_REQ;
+			break;
+		case SUBTYPE_AUTH:
+			// get the sequence number from payload 24 Mac Header + 2 bytes algorithm
+			NdisMoveMemory(&Seq, &pFrame->Octet[2], sizeof(USHORT));
+			if (Seq == 1 || Seq == 3)
+			{
+				*Machine = AUTH_RSP_STATE_MACHINE;
+				*MsgType = MT2_PEER_AUTH_ODD;
+			}
+			else if (Seq == 2 || Seq == 4)
+			{
+				*Machine = AUTH_STATE_MACHINE;
+				*MsgType = MT2_PEER_AUTH_EVEN;
+			}
+			else
+			{
+				return FALSE;
+			}
+			break;
+		case SUBTYPE_DEAUTH:
+			*Machine = AUTH_RSP_STATE_MACHINE;
+			*MsgType = MT2_PEER_DEAUTH;
+			break;
+		case SUBTYPE_ACTION:
+			*Machine = ACTION_STATE_MACHINE;
+			//  Sometimes Sta will return with category bytes with MSB = 1, if they receive catogory out of their support
+			if ((pFrame->Octet[0]&0x7F) > MAX_PEER_CATE_MSG)
+			{
+				*MsgType = MT2_ACT_INVALID;
+			}
+			else
+			{
+				*MsgType = (pFrame->Octet[0]&0x7F);
+			}
+			break;
+		default:
+			return FALSE;
+			break;
+	}
+
+	return TRUE;
+}
+#endif // CONFIG_STA_SUPPORT //
+
+// ===========================================================================================
+// state_machine.c
+// ===========================================================================================
+
+/*! \brief Initialize the state machine.
+ *	\param *S			pointer to the state machine
+ *	\param	Trans		State machine transition function
+ *	\param	StNr		number of states
+ *	\param	MsgNr		number of messages
+ *	\param	DefFunc 	default function, when there is invalid state/message combination
+ *	\param	InitState	initial state of the state machine
+ *	\param	Base		StateMachine base, internal use only
+ *	\pre p_sm should be a legal pointer
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID StateMachineInit(
+	IN STATE_MACHINE *S,
+	IN STATE_MACHINE_FUNC Trans[],
+	IN ULONG StNr,
+	IN ULONG MsgNr,
+	IN STATE_MACHINE_FUNC DefFunc,
+	IN ULONG InitState,
+	IN ULONG Base)
+{
+	ULONG i, j;
+
+	// set number of states and messages
+	S->NrState = StNr;
+	S->NrMsg   = MsgNr;
+	S->Base    = Base;
+
+	S->TransFunc  = Trans;
+
+	// init all state transition to default function
+	for (i = 0; i < StNr; i++)
+	{
+		for (j = 0; j < MsgNr; j++)
+		{
+			S->TransFunc[i * MsgNr + j] = DefFunc;
+		}
+	}
+
+	// set the starting state
+	S->CurrState = InitState;
+}
+
+/*! \brief This function fills in the function pointer into the cell in the state machine
+ *	\param *S	pointer to the state machine
+ *	\param St	state
+ *	\param Msg	incoming message
+ *	\param f	the function to be executed when (state, message) combination occurs at the state machine
+ *	\pre *S should be a legal pointer to the state machine, st, msg, should be all within the range, Base should be set in the initial state
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID StateMachineSetAction(
+	IN STATE_MACHINE *S,
+	IN ULONG St,
+	IN ULONG Msg,
+	IN STATE_MACHINE_FUNC Func)
+{
+	ULONG MsgIdx;
+
+	MsgIdx = Msg - S->Base;
+
+	if (St < S->NrState && MsgIdx < S->NrMsg)
+	{
+		// boundary checking before setting the action
+		S->TransFunc[St * S->NrMsg + MsgIdx] = Func;
+	}
+}
+
+/*! \brief	 This function does the state transition
+ *	\param	 *Adapter the NIC adapter pointer
+ *	\param	 *S 	  the state machine
+ *	\param	 *Elem	  the message to be executed
+ *	\return   None
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID StateMachinePerformAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	(*(S->TransFunc[S->CurrState * S->NrMsg + Elem->MsgType - S->Base]))(pAd, Elem);
+}
+
+/*
+	==========================================================================
+	Description:
+		The drop function, when machine executes this, the message is simply
+		ignored. This function does nothing, the message is freed in
+		StateMachinePerformAction()
+	==========================================================================
+ */
+VOID Drop(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+// ===========================================================================================
+// lfsr.c
+// ===========================================================================================
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID LfsrInit(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Seed)
+{
+	if (Seed == 0)
+		pAd->Mlme.ShiftReg = 1;
+	else
+		pAd->Mlme.ShiftReg = Seed;
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+UCHAR RandomByte(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG i;
+	UCHAR R, Result;
+
+	R = 0;
+
+	if (pAd->Mlme.ShiftReg == 0)
+	NdisGetSystemUpTime((ULONG *)&pAd->Mlme.ShiftReg);
+
+	for (i = 0; i < 8; i++)
+	{
+		if (pAd->Mlme.ShiftReg & 0x00000001)
+		{
+			pAd->Mlme.ShiftReg = ((pAd->Mlme.ShiftReg ^ LFSR_MASK) >> 1) | 0x80000000;
+			Result = 1;
+		}
+		else
+		{
+			pAd->Mlme.ShiftReg = pAd->Mlme.ShiftReg >> 1;
+			Result = 0;
+		}
+		R = (R << 1) | Result;
+	}
+
+	return R;
+}
+
+VOID AsicUpdateAutoFallBackTable(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pRateTable)
+{
+	UCHAR					i;
+	HT_FBK_CFG0_STRUC		HtCfg0;
+	HT_FBK_CFG1_STRUC		HtCfg1;
+	LG_FBK_CFG0_STRUC		LgCfg0;
+	LG_FBK_CFG1_STRUC		LgCfg1;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate;
+
+	// set to initial value
+	HtCfg0.word = 0x65432100;
+	HtCfg1.word = 0xedcba988;
+	LgCfg0.word = 0xedcba988;
+	LgCfg1.word = 0x00002100;
+
+	pNextTxRate = (PRTMP_TX_RATE_SWITCH)pRateTable+1;
+	for (i = 1; i < *((PUCHAR) pRateTable); i++)
+	{
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH)pRateTable+1+i;
+		switch (pCurrTxRate->Mode)
+		{
+			case 0:		//CCK
+				break;
+			case 1:		//OFDM
+				{
+					switch(pCurrTxRate->CurrMCS)
+					{
+						case 0:
+							LgCfg0.field.OFDMMCS0FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 1:
+							LgCfg0.field.OFDMMCS1FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 2:
+							LgCfg0.field.OFDMMCS2FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 3:
+							LgCfg0.field.OFDMMCS3FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 4:
+							LgCfg0.field.OFDMMCS4FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 5:
+							LgCfg0.field.OFDMMCS5FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 6:
+							LgCfg0.field.OFDMMCS6FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 7:
+							LgCfg0.field.OFDMMCS7FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+					}
+				}
+				break;
+#ifdef DOT11_N_SUPPORT
+			case 2:		//HT-MIX
+			case 3:		//HT-GF
+				{
+					if ((pNextTxRate->Mode >= MODE_HTMIX) && (pCurrTxRate->CurrMCS != pNextTxRate->CurrMCS))
+					{
+						switch(pCurrTxRate->CurrMCS)
+						{
+							case 0:
+								HtCfg0.field.HTMCS0FBK = pNextTxRate->CurrMCS;
+								break;
+							case 1:
+								HtCfg0.field.HTMCS1FBK = pNextTxRate->CurrMCS;
+								break;
+							case 2:
+								HtCfg0.field.HTMCS2FBK = pNextTxRate->CurrMCS;
+								break;
+							case 3:
+								HtCfg0.field.HTMCS3FBK = pNextTxRate->CurrMCS;
+								break;
+							case 4:
+								HtCfg0.field.HTMCS4FBK = pNextTxRate->CurrMCS;
+								break;
+							case 5:
+								HtCfg0.field.HTMCS5FBK = pNextTxRate->CurrMCS;
+								break;
+							case 6:
+								HtCfg0.field.HTMCS6FBK = pNextTxRate->CurrMCS;
+								break;
+							case 7:
+								HtCfg0.field.HTMCS7FBK = pNextTxRate->CurrMCS;
+								break;
+							case 8:
+								HtCfg1.field.HTMCS8FBK = pNextTxRate->CurrMCS;
+								break;
+							case 9:
+								HtCfg1.field.HTMCS9FBK = pNextTxRate->CurrMCS;
+								break;
+							case 10:
+								HtCfg1.field.HTMCS10FBK = pNextTxRate->CurrMCS;
+								break;
+							case 11:
+								HtCfg1.field.HTMCS11FBK = pNextTxRate->CurrMCS;
+								break;
+							case 12:
+								HtCfg1.field.HTMCS12FBK = pNextTxRate->CurrMCS;
+								break;
+							case 13:
+								HtCfg1.field.HTMCS13FBK = pNextTxRate->CurrMCS;
+								break;
+							case 14:
+								HtCfg1.field.HTMCS14FBK = pNextTxRate->CurrMCS;
+								break;
+							case 15:
+								HtCfg1.field.HTMCS15FBK = pNextTxRate->CurrMCS;
+								break;
+							default:
+								DBGPRINT(RT_DEBUG_ERROR, ("AsicUpdateAutoFallBackTable: not support CurrMCS=%d\n", pCurrTxRate->CurrMCS));
+						}
+					}
+				}
+				break;
+#endif // DOT11_N_SUPPORT //
+		}
+
+		pNextTxRate = pCurrTxRate;
+	}
+
+	RTMP_IO_WRITE32(pAd, HT_FBK_CFG0, HtCfg0.word);
+	RTMP_IO_WRITE32(pAd, HT_FBK_CFG1, HtCfg1.word);
+	RTMP_IO_WRITE32(pAd, LG_FBK_CFG0, LgCfg0.word);
+	RTMP_IO_WRITE32(pAd, LG_FBK_CFG1, LgCfg1.word);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set MAC register value according operation mode.
+		OperationMode AND bNonGFExist are for MM and GF Proteciton.
+		If MM or GF mask is not set, those passing argument doesn't not take effect.
+
+		Operation mode meaning:
+		= 0 : Pure HT, no preotection.
+		= 0x01; there may be non-HT devices in both the control and extension channel, protection is optional in BSS.
+		= 0x10: No Transmission in 40M is protected.
+		= 0x11: Transmission in both 40M and 20M shall be protected
+		if (bNonGFExist)
+			we should choose not to use GF. But still set correct ASIC registers.
+	========================================================================
+*/
+VOID 	AsicUpdateProtect(
+	IN		PRTMP_ADAPTER	pAd,
+	IN 		USHORT			OperationMode,
+	IN 		UCHAR			SetMask,
+	IN		BOOLEAN			bDisableBGProtect,
+	IN		BOOLEAN			bNonGFExist)
+{
+	PROT_CFG_STRUC	ProtCfg, ProtCfg4;
+	UINT32 Protect[6];
+	USHORT			offset;
+	UCHAR			i;
+	UINT32 MacReg = 0;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+	if (!(pAd->CommonCfg.bHTProtect) && (OperationMode != 8))
+	{
+		return;
+	}
+
+	if (pAd->BATable.numAsOriginator)
+	{
+		//
+		// enable the RTS/CTS to avoid channel collision
+		//
+		SetMask = ALLN_SETPROTECT;
+		OperationMode = 8;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	// Config ASIC RTS threshold register
+	RTMP_IO_READ32(pAd, TX_RTS_CFG, &MacReg);
+	MacReg &= 0xFF0000FF;
+#if 0
+	MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+#else
+	// If the user want disable RtsThreshold and enbale Amsdu/Ralink-Aggregation, set the RtsThreshold as 4096
+        if ((
+#ifdef DOT11_N_SUPPORT
+			(pAd->CommonCfg.BACapability.field.AmsduEnable) ||
+#endif // DOT11_N_SUPPORT //
+			(pAd->CommonCfg.bAggregationCapable == TRUE))
+            && pAd->CommonCfg.RtsThreshold == MAX_RTS_THRESHOLD)
+        {
+			MacReg |= (0x1000 << 8);
+        }
+        else
+        {
+			MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+        }
+#endif
+
+	RTMP_IO_WRITE32(pAd, TX_RTS_CFG, MacReg);
+
+	// Initial common protection settings
+	RTMPZeroMemory(Protect, sizeof(Protect));
+	ProtCfg4.word = 0;
+	ProtCfg.word = 0;
+	ProtCfg.field.TxopAllowGF40 = 1;
+	ProtCfg.field.TxopAllowGF20 = 1;
+	ProtCfg.field.TxopAllowMM40 = 1;
+	ProtCfg.field.TxopAllowMM20 = 1;
+	ProtCfg.field.TxopAllowOfdm = 1;
+	ProtCfg.field.TxopAllowCck = 1;
+	ProtCfg.field.RTSThEn = 1;
+	ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+
+	// update PHY mode and rate
+	if (pAd->CommonCfg.Channel > 14)
+		ProtCfg.field.ProtectRate = 0x4000;
+	ProtCfg.field.ProtectRate |= pAd->CommonCfg.RtsRate;
+
+	// Handle legacy(B/G) protection
+	if (bDisableBGProtect)
+	{
+		//ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+		ProtCfg.field.ProtectCtrl = 0;
+		Protect[0] = ProtCfg.word;
+		Protect[1] = ProtCfg.word;
+	}
+	else
+	{
+		//ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+		ProtCfg.field.ProtectCtrl = 0;			// CCK do not need to be protected
+		Protect[0] = ProtCfg.word;
+		ProtCfg.field.ProtectCtrl = ASIC_CTS;	// OFDM needs using CCK to protect
+		Protect[1] = ProtCfg.word;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// Decide HT frame protection.
+	if ((SetMask & ALLN_SETPROTECT) != 0)
+	{
+		switch(OperationMode)
+		{
+			case 0x0:
+				// NO PROTECT
+				// 1.All STAs in the BSS are 20/40 MHz HT
+				// 2. in ai 20/40MHz BSS
+				// 3. all STAs are 20MHz in a 20MHz BSS
+				// Pure HT. no protection.
+
+				// MM20_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 010111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+				Protect[2] = 0x01744004;
+
+				// MM40_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 111111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+				Protect[3] = 0x03f44084;
+
+				// CF20_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 010111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+				Protect[4] = 0x01744004;
+
+				// CF40_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 111111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+				Protect[5] = 0x03f44084;
+
+				if (bNonGFExist)
+				{
+					// PROT_NAV(19:18)  -- 01 (Short NAV protectiion)
+					// PROT_CTRL(17:16) -- 01 (RTS/CTS)
+					Protect[4] = 0x01754004;
+					Protect[5] = 0x03f54084;
+				}
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+				break;
+
+ 			case 1:
+				// This is "HT non-member protection mode."
+				// If there may be non-HT STAs my BSS
+				ProtCfg.word = 0x01744004;	// PROT_CTRL(17:16) : 0 (None)
+				ProtCfg4.word = 0x03f44084; // duplicaet legacy 24M. BW set 1.
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					ProtCfg.word = 0x01740003;	//ERP use Protection bit is set, use protection rate at Clause 18..
+					ProtCfg4.word = 0x03f40003; // Don't duplicate RTS/CTS in CCK mode. 0x03f40083;
+				}
+				//Assign Protection method for 20&40 MHz packets
+				ProtCfg.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+
+			case 2:
+				// If only HT STAs are in BSS. at least one is 20MHz. Only protect 40MHz packets
+				ProtCfg.word = 0x01744004;  // PROT_CTRL(17:16) : 0 (None)
+				ProtCfg4.word = 0x03f44084; // duplicaet legacy 24M. BW set 1.
+
+				//Assign Protection method for 40MHz packets
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				if (bNonGFExist)
+				{
+					ProtCfg.field.ProtectCtrl = ASIC_RTS;
+					ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				}
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+				break;
+
+			case 3:
+				// HT mixed mode.	 PROTECT ALL!
+				// Assign Rate
+				ProtCfg.word = 0x01744004;	//duplicaet legacy 24M. BW set 1.
+				ProtCfg4.word = 0x03f44084;
+				// both 20MHz and 40MHz are protected. Whether use RTS or CTS-to-self depends on the
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					ProtCfg.word = 0x01740003;	//ERP use Protection bit is set, use protection rate at Clause 18..
+					ProtCfg4.word = 0x03f40003; // Don't duplicate RTS/CTS in CCK mode. 0x03f40083
+				}
+				//Assign Protection method for 20&40 MHz packets
+				ProtCfg.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+
+			case 8:
+				// Special on for Atheros problem n chip.
+				Protect[2] = 0x01754004;
+				Protect[3] = 0x03f54084;
+				Protect[4] = 0x01754004;
+				Protect[5] = 0x03f54084;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	offset = CCK_PROT_CFG;
+	for (i = 0;i < 6;i++)
+	{
+		if ((SetMask & (1<< i)))
+		{
+			RTMP_IO_WRITE32(pAd, offset + i*4, Protect[i]);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSwitchChannel(
+					  IN PRTMP_ADAPTER pAd,
+	IN	UCHAR			Channel,
+	IN	BOOLEAN			bScan)
+{
+	ULONG			R2 = 0, R3 = DEFAULT_RF_TX_POWER, R4 = 0;
+	CHAR    TxPwer = 0, TxPwer2 = DEFAULT_RF_TX_POWER; //Bbp94 = BBPR94_DEFAULT, TxPwer2 = DEFAULT_RF_TX_POWER;
+	UCHAR	index;
+	UINT32 	Value = 0; //BbpReg, Value;
+	RTMP_RF_REGS *RFRegTable;
+
+	// Search Tx power value
+	for (index = 0; index < pAd->ChannelListNum; index++)
+	{
+		if (Channel == pAd->ChannelList[index].Channel)
+		{
+			TxPwer = pAd->ChannelList[index].Power;
+			TxPwer2 = pAd->ChannelList[index].Power2;
+			break;
+		}
+	}
+
+	if (index == MAX_NUM_OF_CHANNELS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: Cant find the Channel#%d \n", Channel));
+	}
+
+#ifdef RT2870
+	// The RF programming sequence is difference between 3xxx and 2xxx
+	if (IS_RT3070(pAd) && ((pAd->RfIcType == RFIC_3020) || (pAd->RfIcType == RFIC_2020)))
+	{
+		/* modify by WY for Read RF Reg. error */
+		UCHAR RFValue;
+
+		for (index = 0; index < NUM_OF_3020_CHNL; index++)
+		{
+			if (Channel == FreqItems3020[index].Channel)
+			{
+				// Programming channel parameters
+				RT30xxWriteRFRegister(pAd, RF_R02, FreqItems3020[index].N);
+				RT30xxWriteRFRegister(pAd, RF_R03, FreqItems3020[index].K);
+
+				RT30xxReadRFRegister(pAd, RF_R06, (PUCHAR)&RFValue);
+				RFValue = (RFValue & 0xFC) | FreqItems3020[index].R;
+				RT30xxWriteRFRegister(pAd, RF_R06, (UCHAR)RFValue);
+
+				// Set Tx Power
+				RT30xxReadRFRegister(pAd, RF_R12, (PUCHAR)&RFValue);
+				RFValue = (RFValue & 0xE0) | TxPwer;
+				RT30xxWriteRFRegister(pAd, RF_R12, (UCHAR)RFValue);
+
+				// Set RF offset
+				RT30xxReadRFRegister(pAd, RF_R23, (PUCHAR)&RFValue);
+				RFValue = (RFValue & 0x80) | pAd->RfFreqOffset;
+				RT30xxWriteRFRegister(pAd, RF_R23, (UCHAR)RFValue);
+
+				// Set BW
+				if (!bScan && (pAd->CommonCfg.BBPCurrentBW == BW_40))
+				{
+					RFValue = pAd->Mlme.CaliBW40RfR24;
+					//DISABLE_11N_CHECK(pAd);
+				}
+				else
+				{
+					RFValue = pAd->Mlme.CaliBW20RfR24;
+				}
+				RT30xxWriteRFRegister(pAd, RF_R24, (UCHAR)RFValue);
+
+				// Enable RF tuning
+				RT30xxReadRFRegister(pAd, RF_R07, (PUCHAR)&RFValue);
+				RFValue = RFValue | 0x1;
+				RT30xxWriteRFRegister(pAd, RF_R07, (UCHAR)RFValue);
+
+				// latch channel for future usage.
+				pAd->LatchRfRegs.Channel = Channel;
+
+				break;
+			}
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, Pwr0=%d, Pwr1=%d, %dT), N=0x%02X, K=0x%02X, R=0x%02X\n",
+			Channel,
+			pAd->RfIcType,
+			TxPwer,
+			TxPwer2,
+			pAd->Antenna.field.TxPath,
+			FreqItems3020[index].N,
+			FreqItems3020[index].K,
+			FreqItems3020[index].R));
+	}
+	else
+#endif // RT2870 //
+	{
+		RFRegTable = RF2850RegTable;
+
+		switch (pAd->RfIcType)
+		{
+			case RFIC_2820:
+			case RFIC_2850:
+			case RFIC_2720:
+			case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						R2 |= 0x40;	// write 1 to off Rxpath.
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+
+					if (Channel > 14)
+					{
+						// initialize R3, R4
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff);
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->RfFreqOffset << 15);
+
+						// 5G band power range: 0xF9~0X0F, TX0 Reg3 bit9/TX1 Reg4 bit6="0" means the TX power reduce 7dB
+						// R3
+						if ((TxPwer >= -7) && (TxPwer < 0))
+						{
+							TxPwer = (7+TxPwer);
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10);
+							DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: TxPwer=%d \n", TxPwer));
+						}
+						else
+						{
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10) | (1 << 9);
+						}
+
+						// R4
+						if ((TxPwer2 >= -7) && (TxPwer2 < 0))
+						{
+							TxPwer2 = (7+TxPwer2);
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7);
+							DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: TxPwer2=%d \n", TxPwer2));
+						}
+						else
+						{
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7) | (1 << 6);
+						}
+					}
+					else
+					{
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff) | (TxPwer << 9); // set TX power0
+					R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->RfFreqOffset << 15) | (TxPwer2 <<6);// Set freq Offset & TxPwr1
+					}
+
+					// Based on BBP current mode before changing RF channel.
+					if (!bScan && (pAd->CommonCfg.BBPCurrentBW == BW_40))
+					{
+						R4 |=0x200000;
+					}
+
+					// Update variables
+					pAd->LatchRfRegs.Channel = Channel;
+					pAd->LatchRfRegs.R1 = RFRegTable[index].R1;
+					pAd->LatchRfRegs.R2 = R2;
+					pAd->LatchRfRegs.R3 = R3;
+					pAd->LatchRfRegs.R4 = R4;
+
+					// Set RF value 1's set R3[bit2] = [0]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					RTMPusecDelay(200);
+
+					// Set RF value 2's set R3[bit2] = [1]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 | 0x04));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					RTMPusecDelay(200);
+
+					// Set RF value 3's set R3[bit2] = [0]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					break;
+				}
+			}
+			break;
+
+			default:
+			break;
+		}
+	}
+
+	// Change BBP setting during siwtch from a->g, g->a
+	if (Channel <= 14)
+	{
+	    ULONG	TxPinCfg = 0x00050F0A;//Gary 2007/08/09 0x050A0A
+
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0);//(0x44 - GET_LNA_GAIN(pAd)));	// According the Rory's suggestion to solve the middle range issue.
+		//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+
+		// Rx High power VGA offset for LNA select
+	    if (pAd->NicConfig2.field.ExternalLNAForG)
+	    {
+	        RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x46);
+	    }
+	    else
+	    {
+	        RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x84);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x50);
+	    }
+
+		// 5G band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x04);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+		}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+	else
+	{
+	    ULONG	TxPinCfg = 0x00050F05;//Gary 2007/8/9 0x050505
+
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0);//(0x44 - GET_LNA_GAIN(pAd)));   // According the Rory's suggestion to solve the middle range issue.
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0xF2);
+
+		// Rx High power VGA offset for LNA select
+		if (pAd->NicConfig2.field.ExternalLNAForA)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x46);
+		}
+		else
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x50);
+		}
+
+		// 5G band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x02);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+	}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+	}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+
+    // R66 should be set according to Channel and use 20MHz when scanning
+	//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, (0x2E + GET_LNA_GAIN(pAd)));
+	if (bScan)
+		RTMPSetAGCInitValue(pAd, BW_20);
+	else
+		RTMPSetAGCInitValue(pAd, pAd->CommonCfg.BBPCurrentBW);
+
+	//
+	// On 11A, We should delay and wait RF/BBP to be stable
+	// and the appropriate time should be 1000 micro seconds
+	// 2005/06/05 - On 11G, We also need this delay time. Otherwise it's difficult to pass the WHQL.
+	//
+	RTMPusecDelay(1000);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, Pwr0=%lu, Pwr1=%lu, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+							  Channel,
+							  pAd->RfIcType,
+							  (R3 & 0x00003e00) >> 9,
+							  (R4 & 0x000007c0) >> 6,
+							  pAd->Antenna.field.TxPath,
+							  pAd->LatchRfRegs.R1,
+							  pAd->LatchRfRegs.R2,
+							  pAd->LatchRfRegs.R3,
+							  pAd->LatchRfRegs.R4));
+}
+
+/*
+	==========================================================================
+	Description:
+		This function is required for 2421 only, and should not be used during
+		site survey. It's only required after NIC decided to stay at a channel
+		for a longer period.
+		When this function is called, it's always after AsicSwitchChannel().
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicLockChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel)
+{
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID	AsicAntennaSelect(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Channel)
+{
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Antenna miscellaneous setting.
+
+	Arguments:
+		pAd						Pointer to our adapter
+		BandState				Indicate current Band State.
+
+	Return Value:
+		None
+
+	IRQL <= DISPATCH_LEVEL
+
+	Note:
+		1.) Frame End type control
+			only valid for G only (RF_2527 & RF_2529)
+			0: means DPDT, set BBP R4 bit 5 to 1
+			1: means SPDT, set BBP R4 bit 5 to 0
+
+
+	========================================================================
+*/
+VOID	AsicAntennaSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ABGBAND_STATE	BandState)
+{
+}
+
+VOID AsicRfTuningExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+}
+
+/*
+	==========================================================================
+	Description:
+		Gives CCK TX rate 2 more dB TX power.
+		This routine works only in LINK UP in INFRASTRUCTURE mode.
+
+		calculate desired Tx power in RF R3.Tx0~5,	should consider -
+		0. if current radio is a noisy environment (pAd->DrsCounters.fNoisyEnvironment)
+		1. TxPowerPercentage
+		2. auto calibration based on TSSI feedback
+		3. extra 2 db for CCK
+		4. -10 db upon very-short distance (AvgRSSI >= -40db) to AP
+
+	NOTE: Since this routine requires the value of (pAd->DrsCounters.fNoisyEnvironment),
+		it should be called AFTER MlmeDynamicTxRatSwitching()
+	==========================================================================
+ */
+VOID AsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT			i, j;
+	CHAR		DeltaPwr = 0;
+	BOOLEAN		bAutoTxAgc = FALSE;
+	UCHAR		TssiRef, *pTssiMinusBoundary, *pTssiPlusBoundary, TxAgcStep;
+	UCHAR		BbpR1 = 0, BbpR49 = 0, idx;
+	PCHAR		pTxAgcCompensate;
+	ULONG		TxPwr[5];
+	CHAR		Value;
+
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		if (pAd->CommonCfg.CentralChannel > 14)
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgGBand[4];
+		}
+	}
+	else
+	{
+		if (pAd->CommonCfg.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgGBand[4];
+		}
+	}
+
+	// TX power compensation for temperature variation based on TSSI. try every 4 second
+	if (pAd->Mlme.OneSecPeriodicRound % 4 == 0)
+	{
+		if (pAd->CommonCfg.Channel <= 14)
+		{
+			/* bg channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			TssiRef            = pAd->TssiRefG;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryG[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryG[0];
+			TxAgcStep          = pAd->TxAgcStepG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			/* a channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			TssiRef            = pAd->TssiRefA;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryA[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryA[0];
+			TxAgcStep          = pAd->TxAgcStepA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+		{
+			/* BbpR1 is unsigned char */
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R49, &BbpR49);
+
+			/* (p) TssiPlusBoundaryG[0] = 0 = (m) TssiMinusBoundaryG[0] */
+			/* compensate: +4     +3   +2   +1    0   -1   -2   -3   -4 * steps */
+			/* step value is defined in pAd->TxAgcStepG for tx power value */
+
+			/* [4]+1+[4]   p4     p3   p2   p1   o1   m1   m2   m3   m4 */
+			/* ex:         0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+			   above value are examined in mass factory production */
+			/*             [4]    [3]  [2]  [1]  [0]  [1]  [2]  [3]  [4] */
+
+			/* plus (+) is 0x00 ~ 0x45, minus (-) is 0xa0 ~ 0xf0 */
+			/* if value is between p1 ~ o1 or o1 ~ s1, no need to adjust tx power */
+			/* if value is 0xa5, tx power will be -= TxAgcStep*(2-1) */
+
+			if (BbpR49 > pTssiMinusBoundary[1])
+			{
+				// Reading is larger than the reference value
+				// check for how large we need to decrease the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 <= pTssiMinusBoundary[idx])  // Found the range
+						break;
+				}
+				// The index is the step we should decrease, idx = 0 means there is nothing to compensate
+//				if (R3 > (ULONG) (TxAgcStep * (idx-1)))
+					*pTxAgcCompensate = -(TxAgcStep * (idx-1));
+//				else
+//					*pTxAgcCompensate = -((UCHAR)R3);
+
+				DeltaPwr += (*pTxAgcCompensate);
+				DBGPRINT(RT_DEBUG_TRACE, ("-- Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = -%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else if (BbpR49 < pTssiPlusBoundary[1])
+			{
+				// Reading is smaller than the reference value
+				// check for how large we need to increase the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 >= pTssiPlusBoundary[idx])   // Found the range
+						break;
+				}
+				// The index is the step we should increase, idx = 0 means there is nothing to compensate
+				*pTxAgcCompensate = TxAgcStep * (idx-1);
+				DeltaPwr += (*pTxAgcCompensate);
+				DBGPRINT(RT_DEBUG_TRACE, ("++ Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else
+			{
+				*pTxAgcCompensate = 0;
+				DBGPRINT(RT_DEBUG_TRACE, ("   Tx Power, BBP R49=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, 0));
+			}
+		}
+	}
+	else
+	{
+		if (pAd->CommonCfg.Channel <= 14)
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+			DeltaPwr += (*pTxAgcCompensate);
+	}
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpR1);
+	BbpR1 &= 0xFC;
+
+#ifdef SINGLE_SKU
+	// Handle regulatory max tx power constrain
+	do
+	{
+		UCHAR    TxPwrInEEPROM = 0xFF, CountryTxPwr = 0xFF, criterion;
+		UCHAR    AdjustMaxTxPwr[40];
+
+		if (pAd->CommonCfg.Channel > 14) // 5G band
+			TxPwrInEEPROM = ((pAd->CommonCfg.DefineMaxTxPwr & 0xFF00) >> 8);
+		else // 2.4G band
+			TxPwrInEEPROM = (pAd->CommonCfg.DefineMaxTxPwr & 0x00FF);
+		CountryTxPwr = GetCuntryMaxTxPwr(pAd, pAd->CommonCfg.Channel);
+
+		// error handling, range check
+		if ((TxPwrInEEPROM > 0x50) || (CountryTxPwr > 0x50))
+		{
+			DBGPRINT(RT_DEBUG_ERROR,("AsicAdjustTxPower - Invalid max tx power (=0x%02x), CountryTxPwr=%d\n", TxPwrInEEPROM, CountryTxPwr));
+			break;
+		}
+
+		criterion = *((PUCHAR)TxPwr + 2) & 0xF;        // FAE use OFDM 6M as criterion
+
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (criterion=%d, TxPwrInEEPROM=%d, CountryTxPwr=%d)\n", criterion, TxPwrInEEPROM, CountryTxPwr));
+
+		// Adjust max tx power according to the relationship of tx power in E2PROM
+		for (i=0; i<5; i++)
+		{
+			// CCK will have 4dBm larger than OFDM
+			// Therefore, we should separate to parse the tx power field
+			if (i == 0)
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					if (j < 4)
+					{
+						// CCK will have 4dBm larger than OFDM
+						AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion) + 4;
+					}
+					else
+					{
+						AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion);
+					}
+					DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+				}
+			}
+			else
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion);
+					DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+				}
+			}
+		}
+
+		// Adjust tx power according to the relationship
+		for (i=0; i<5; i++)
+		{
+			if (TxPwr[i] != 0xffffffff)
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					// The system tx power is larger than the regulatory, the power should be restrain
+					if (AdjustMaxTxPwr[i*8+j] > CountryTxPwr)
+					{
+						// decrease to zero and don't need to take care BBPR1
+						if ((Value - (AdjustMaxTxPwr[i*8+j] - CountryTxPwr)) > 0)
+							Value -= (AdjustMaxTxPwr[i*8+j] - CountryTxPwr);
+						else
+							Value = 0;
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+					}
+					else
+						DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d, no change)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+
+						TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+				}
+			}
+		}
+	} while (FALSE);
+#endif // SINGLE_SKU //
+
+	/* calculate delta power based on the percentage specified from UI */
+	// E2PROM setting is calibrated for maximum TX power (i.e. 100%)
+	// We lower TX power here according to the percentage specified from UI
+	if (pAd->CommonCfg.TxPowerPercentage == 0xffffffff)       // AUTO TX POWER control
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 90)  // 91 ~ 100% & AUTO, treat as 100% in terms of mW
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 60)  // 61 ~ 90%, treat as 75% in terms of mW		// DeltaPwr -= 1;
+	{
+		DeltaPwr -= 1;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 30)  // 31 ~ 60%, treat as 50% in terms of mW		// DeltaPwr -= 3;
+	{
+		DeltaPwr -= 3;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 15)  // 16 ~ 30%, treat as 25% in terms of mW		// DeltaPwr -= 6;
+	{
+		BbpR1 |= 0x01;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 9)   // 10 ~ 15%, treat as 12.5% in terms of mW		// DeltaPwr -= 9;
+	{
+		BbpR1 |= 0x01;
+		DeltaPwr -= 3;
+	}
+	else                                           // 0 ~ 9 %, treat as MIN(~3%) in terms of mW		// DeltaPwr -= 12;
+	{
+		BbpR1 |= 0x02;
+	}
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpR1);
+
+	/* reset different new tx power for different TX rate */
+	for(i=0; i<5; i++)
+	{
+		if (TxPwr[i] != 0xffffffff)
+		{
+			for (j=0; j<8; j++)
+			{
+				Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F); /* 0 ~ 15 */
+
+				if ((Value + DeltaPwr) < 0)
+				{
+					Value = 0; /* min */
+				}
+				else if ((Value + DeltaPwr) > 0xF)
+				{
+					Value = 0xF; /* max */
+				}
+				else
+				{
+					Value += DeltaPwr; /* temperature compensation */
+				}
+
+				/* fill new value to CSR offset */
+				TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+			}
+
+			/* write tx power value to CSR */
+			/* TX_PWR_CFG_0 (8 tx rate) for	TX power for OFDM 12M/18M
+											TX power for OFDM 6M/9M
+											TX power for CCK5.5M/11M
+											TX power for CCK1M/2M */
+			/* TX_PWR_CFG_1 ~ TX_PWR_CFG_4 */
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, TxPwr[i]);
+		}
+	}
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		put PHY to sleep here, and set next wakeup timer. PHY doesn't not wakeup
+		automatically. Instead, MCU will issue a TwakeUpInterrupt to host after
+		the wakeup timer timeout. Driver has to issue a separate command to wake
+		PHY up.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp)
+{
+    RT28XX_STA_SLEEP_THEN_AUTO_WAKEUP(pAd, TbttNumToNextWakeUp);
+}
+
+/*
+	==========================================================================
+	Description:
+		AsicForceWakeup() is used whenever manual wakeup is required
+		AsicForceSleep() should only be used when not in INFRA BSS. When
+		in INFRA BSS, we should use AsicSleepThenAutoWakeup() instead.
+	==========================================================================
+ */
+VOID AsicForceSleep(
+	IN PRTMP_ADAPTER pAd)
+{
+
+}
+
+/*
+	==========================================================================
+	Description:
+		AsicForceWakeup() is used whenever Twakeup timer (set via AsicSleepThenAutoWakeup)
+		expired.
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+	==========================================================================
+ */
+VOID AsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN    bFromTx)
+{
+    DBGPRINT(RT_DEBUG_TRACE, ("--> AsicForceWakeup \n"));
+    RT28XX_STA_FORCE_WAKEUP(pAd, bFromTx);
+}
+#endif // CONFIG_STA_SUPPORT //
+/*
+	==========================================================================
+	Description:
+		Set My BSSID
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSetBssid(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pBssid)
+{
+	ULONG		  Addr4;
+	DBGPRINT(RT_DEBUG_TRACE, ("==============> AsicSetBssid %x:%x:%x:%x:%x:%x\n",
+		pBssid[0],pBssid[1],pBssid[2],pBssid[3], pBssid[4],pBssid[5]));
+
+	Addr4 = (ULONG)(pBssid[0])		 |
+			(ULONG)(pBssid[1] << 8)  |
+			(ULONG)(pBssid[2] << 16) |
+			(ULONG)(pBssid[3] << 24);
+	RTMP_IO_WRITE32(pAd, MAC_BSSID_DW0, Addr4);
+
+	Addr4 = 0;
+	// always one BSSID in STA mode
+	Addr4 = (ULONG)(pBssid[4]) | (ULONG)(pBssid[5] << 8);
+
+	RTMP_IO_WRITE32(pAd, MAC_BSSID_DW1, Addr4);
+}
+
+VOID AsicSetMcastWC(
+	IN PRTMP_ADAPTER pAd)
+{
+	MAC_TABLE_ENTRY *pEntry = &pAd->MacTab.Content[MCAST_WCID];
+	USHORT		offset;
+
+	pEntry->Sst        = SST_ASSOC;
+	pEntry->Aid        = MCAST_WCID;	// Softap supports 1 BSSID and use WCID=0 as multicast Wcid index
+	pEntry->PsMode     = PWR_ACTIVE;
+	pEntry->CurrTxRate = pAd->CommonCfg.MlmeRate;
+	offset = MAC_WCID_BASE + BSS0Mcast_WCID * HW_WCID_ENTRY_SIZE;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDelWcidTab(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	Wcid)
+{
+	ULONG		  Addr0 = 0x0, Addr1 = 0x0;
+	ULONG 		offset;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicDelWcidTab==>Wcid = 0x%x\n",Wcid));
+	offset = MAC_WCID_BASE + Wcid * HW_WCID_ENTRY_SIZE;
+	RTMP_IO_WRITE32(pAd, offset, Addr0);
+	offset += 4;
+	RTMP_IO_WRITE32(pAd, offset, Addr1);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableRDG(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_LINK_CFG_STRUC	TxLinkCfg;
+	UINT32				Data = 0;
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+	TxLinkCfg.field.TxRDGEn = 1;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+
+	RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+	Data  &= 0xFFFFFF00;
+	Data  |= 0x80;
+	RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+	//OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDisableRDG(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_LINK_CFG_STRUC	TxLinkCfg;
+	UINT32				Data = 0;
+
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+	TxLinkCfg.field.TxRDGEn = 0;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+
+	RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+
+	Data  &= 0xFFFFFF00;
+	//Data  |= 0x20;
+#ifndef WIFI_TEST
+	//if ( pAd->CommonCfg.bEnableTxBurst )
+	//	Data |= 0x60; // for performance issue not set the TXOP to 0
+#endif
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_DYNAMIC_BE_TXOP_ACTIVE)
+#ifdef DOT11_N_SUPPORT
+		&& (pAd->MacTab.fAnyStationMIMOPSDynamic == FALSE)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		// For CWC test, change txop from 0x30 to 0x20 in TxBurst mode
+		if (pAd->CommonCfg.bEnableTxBurst)
+			Data |= 0x20;
+	}
+	RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDisableSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->Disable TSF synchronization\n"));
+
+	// 2003-12-20 disable TSF and TBTT while NIC in power-saving have side effect
+	//			  that NIC will never wakes up because TSF stops and no more
+	//			  TBTT interrupts
+	pAd->TbttTickCount = 0;
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+	csr.field.bBeaconGen = 0;
+	csr.field.bTBTTEnable = 0;
+	csr.field.TsfSyncMode = 0;
+	csr.field.bTsfTicking = 0;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableBssSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->AsicEnableBssSync(INFRA mode)\n"));
+
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+//	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, 0x00000000);
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		csr.field.BeaconInterval = pAd->CommonCfg.BeaconPeriod << 4; // ASIC register in units of 1/16 TU
+		csr.field.bTsfTicking = 1;
+		csr.field.TsfSyncMode = 1; // sync TSF in INFRASTRUCTURE mode
+		csr.field.bBeaconGen  = 0; // do NOT generate BEACON
+		csr.field.bTBTTEnable = 1;
+	}
+#endif // CONFIG_STA_SUPPORT //
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+}
+
+/*
+	==========================================================================
+	Description:
+	Note:
+		BEACON frame in shared memory should be built ok before this routine
+		can be called. Otherwise, a garbage frame maybe transmitted out every
+		Beacon period.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableIbssSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr9;
+	PUCHAR			ptr;
+	UINT i;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->AsicEnableIbssSync(ADHOC mode. MPDUtotalByteCount = %d)\n", pAd->BeaconTxWI.MPDUtotalByteCount));
+
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr9.word);
+	csr9.field.bBeaconGen = 0;
+	csr9.field.bTBTTEnable = 0;
+	csr9.field.bTsfTicking = 0;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr9.word);
+
+
+#ifdef RT2870
+	// move BEACON TXD and frame content to on-chip memory
+	ptr = (PUCHAR)&pAd->BeaconTxWI;
+	for (i=0; i<TXWI_SIZE; i+=2)  // 16-byte TXWI field
+	{
+		//UINT32 longptr =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+		//RTMP_IO_WRITE32(pAd, HW_BEACON_BASE0 + i, longptr);
+		RTUSBMultiWrite(pAd, HW_BEACON_BASE0 + i, ptr, 2);
+		ptr += 2;
+	}
+
+	// start right after the 16-byte TXWI field
+	ptr = pAd->BeaconBuf;
+	for (i=0; i< pAd->BeaconTxWI.MPDUtotalByteCount; i+=2)
+	{
+		//UINT32 longptr =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+		//RTMP_IO_WRITE32(pAd, HW_BEACON_BASE0 + TXWI_SIZE + i, longptr);
+		RTUSBMultiWrite(pAd, HW_BEACON_BASE0 + TXWI_SIZE + i, ptr, 2);
+		ptr +=2;
+	}
+#endif // RT2870 //
+
+	//
+	// For Wi-Fi faily generated beacons between participating stations.
+	// Set TBTT phase adaptive adjustment step to 8us (default 16us)
+	// don't change settings 2006-5- by Jerry
+	//RTMP_IO_WRITE32(pAd, TBTT_SYNC_CFG, 0x00001010);
+
+	// start sending BEACON
+	csr9.field.BeaconInterval = pAd->CommonCfg.BeaconPeriod << 4; // ASIC register in units of 1/16 TU
+	csr9.field.bTsfTicking = 1;
+	csr9.field.TsfSyncMode = 2; // sync TSF in IBSS mode
+	csr9.field.bTBTTEnable = 1;
+	csr9.field.bBeaconGen = 1;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr9.word);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSetEdcaParm(
+	IN PRTMP_ADAPTER pAd,
+	IN PEDCA_PARM	 pEdcaParm)
+{
+	EDCA_AC_CFG_STRUC   Ac0Cfg, Ac1Cfg, Ac2Cfg, Ac3Cfg;
+	AC_TXOP_CSR0_STRUC csr0;
+	AC_TXOP_CSR1_STRUC csr1;
+	AIFSN_CSR_STRUC    AifsnCsr;
+	CWMIN_CSR_STRUC    CwminCsr;
+	CWMAX_CSR_STRUC    CwmaxCsr;
+	int i;
+
+	Ac0Cfg.word = 0;
+	Ac1Cfg.word = 0;
+	Ac2Cfg.word = 0;
+	Ac3Cfg.word = 0;
+	if ((pEdcaParm == NULL) || (pEdcaParm->bValid == FALSE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("AsicSetEdcaParm\n"));
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_WMM_INUSED);
+		for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+		{
+			if (pAd->MacTab.Content[i].ValidAsCLI || pAd->MacTab.Content[i].ValidAsApCli)
+				CLIENT_STATUS_CLEAR_FLAG(&pAd->MacTab.Content[i], fCLIENT_STATUS_WMM_CAPABLE);
+		}
+
+		//========================================================
+		//      MAC Register has a copy .
+		//========================================================
+//#ifndef WIFI_TEST
+		if( pAd->CommonCfg.bEnableTxBurst )
+		{
+			// For CWC test, change txop from 0x30 to 0x20 in TxBurst mode
+			Ac0Cfg.field.AcTxop = 0x20; // Suggest by John for TxBurst in HT Mode
+		}
+		else
+			Ac0Cfg.field.AcTxop = 0;	// QID_AC_BE
+//#else
+//		Ac0Cfg.field.AcTxop = 0;	// QID_AC_BE
+//#endif
+		Ac0Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac0Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac0Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Ac0Cfg.word);
+
+		Ac1Cfg.field.AcTxop = 0;	// QID_AC_BK
+		Ac1Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac1Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac1Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC1_CFG, Ac1Cfg.word);
+
+		if (pAd->CommonCfg.PhyMode == PHY_11B)
+		{
+			Ac2Cfg.field.AcTxop = 192;	// AC_VI: 192*32us ~= 6ms
+			Ac3Cfg.field.AcTxop = 96;	// AC_VO: 96*32us  ~= 3ms
+		}
+		else
+		{
+			Ac2Cfg.field.AcTxop = 96;	// AC_VI: 96*32us ~= 3ms
+			Ac3Cfg.field.AcTxop = 48;	// AC_VO: 48*32us ~= 1.5ms
+		}
+		Ac2Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac2Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac2Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC2_CFG, Ac2Cfg.word);
+		Ac3Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac3Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac3Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC3_CFG, Ac3Cfg.word);
+
+		//========================================================
+		//      DMA Register has a copy too.
+		//========================================================
+		csr0.field.Ac0Txop = 0;		// QID_AC_BE
+		csr0.field.Ac1Txop = 0;		// QID_AC_BK
+		RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+		if (pAd->CommonCfg.PhyMode == PHY_11B)
+		{
+			csr1.field.Ac2Txop = 192;		// AC_VI: 192*32us ~= 6ms
+			csr1.field.Ac3Txop = 96;		// AC_VO: 96*32us  ~= 3ms
+		}
+		else
+		{
+			csr1.field.Ac2Txop = 96;		// AC_VI: 96*32us ~= 3ms
+			csr1.field.Ac3Txop = 48;		// AC_VO: 48*32us ~= 1.5ms
+		}
+		RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr1.word);
+
+		CwminCsr.word = 0;
+		CwminCsr.field.Cwmin0 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin1 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin2 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin3 = CW_MIN_IN_BITS;
+		RTMP_IO_WRITE32(pAd, WMM_CWMIN_CFG, CwminCsr.word);
+
+		CwmaxCsr.word = 0;
+		CwmaxCsr.field.Cwmax0 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax1 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax2 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax3 = CW_MAX_IN_BITS;
+		RTMP_IO_WRITE32(pAd, WMM_CWMAX_CFG, CwmaxCsr.word);
+
+		RTMP_IO_WRITE32(pAd, WMM_AIFSN_CFG, 0x00002222);
+
+		NdisZeroMemory(&pAd->CommonCfg.APEdcaParm, sizeof(EDCA_PARM));
+	}
+	else
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_WMM_INUSED);
+		//========================================================
+		//      MAC Register has a copy.
+		//========================================================
+		//
+		// Modify Cwmin/Cwmax/Txop on queue[QID_AC_VI], Recommend by Jerry 2005/07/27
+		// To degrade our VIDO Queue's throughput for WiFi WMM S3T07 Issue.
+		//
+		//pEdcaParm->Txop[QID_AC_VI] = pEdcaParm->Txop[QID_AC_VI] * 7 / 10; // rt2860c need this
+
+		Ac0Cfg.field.AcTxop =  pEdcaParm->Txop[QID_AC_BE];
+		Ac0Cfg.field.Cwmin= pEdcaParm->Cwmin[QID_AC_BE];
+		Ac0Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_BE];
+		Ac0Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_BE]; //+1;
+
+		Ac1Cfg.field.AcTxop =  pEdcaParm->Txop[QID_AC_BK];
+		Ac1Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_BK]; //+2;
+		Ac1Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_BK];
+		Ac1Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_BK]; //+1;
+
+		Ac2Cfg.field.AcTxop = (pEdcaParm->Txop[QID_AC_VI] * 6) / 10;
+		Ac2Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_VI];
+		Ac2Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_VI];
+		Ac2Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			// Tuning for Wi-Fi WMM S06
+			if (pAd->CommonCfg.bWiFiTest &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+				Ac2Cfg.field.Aifsn -= 1;
+
+			// Tuning for TGn Wi-Fi 5.2.32
+			// STA TestBed changes in this item: conexant legacy sta ==> broadcom 11n sta
+			if (STA_TGN_WIFI_ON(pAd) &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+			{
+				Ac0Cfg.field.Aifsn = 3;
+				Ac2Cfg.field.AcTxop = 5;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		Ac3Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_VO];
+		Ac3Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_VO];
+		Ac3Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_VO];
+		Ac3Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_VO];
+
+//#ifdef WIFI_TEST
+		if (pAd->CommonCfg.bWiFiTest)
+		{
+			if (Ac3Cfg.field.AcTxop == 102)
+			{
+			Ac0Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_BE] ? pEdcaParm->Txop[QID_AC_BE] : 10;
+				Ac0Cfg.field.Aifsn  = pEdcaParm->Aifsn[QID_AC_BE]-1; /* AIFSN must >= 1 */
+			Ac1Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_BK];
+				Ac1Cfg.field.Aifsn  = pEdcaParm->Aifsn[QID_AC_BK];
+			Ac2Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_VI];
+			} /* End of if */
+		}
+//#endif // WIFI_TEST //
+
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Ac0Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC1_CFG, Ac1Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC2_CFG, Ac2Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC3_CFG, Ac3Cfg.word);
+
+
+		//========================================================
+		//      DMA Register has a copy too.
+		//========================================================
+		csr0.field.Ac0Txop = Ac0Cfg.field.AcTxop;
+		csr0.field.Ac1Txop = Ac1Cfg.field.AcTxop;
+		RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+
+		csr1.field.Ac2Txop = Ac2Cfg.field.AcTxop;
+		csr1.field.Ac3Txop = Ac3Cfg.field.AcTxop;
+		RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr1.word);
+
+		CwminCsr.word = 0;
+		CwminCsr.field.Cwmin0 = pEdcaParm->Cwmin[QID_AC_BE];
+		CwminCsr.field.Cwmin1 = pEdcaParm->Cwmin[QID_AC_BK];
+		CwminCsr.field.Cwmin2 = pEdcaParm->Cwmin[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			CwminCsr.field.Cwmin3 = pEdcaParm->Cwmin[QID_AC_VO] - 1; //for TGn wifi test
+#endif // CONFIG_STA_SUPPORT //
+		RTMP_IO_WRITE32(pAd, WMM_CWMIN_CFG, CwminCsr.word);
+
+		CwmaxCsr.word = 0;
+		CwmaxCsr.field.Cwmax0 = pEdcaParm->Cwmax[QID_AC_BE];
+		CwmaxCsr.field.Cwmax1 = pEdcaParm->Cwmax[QID_AC_BK];
+		CwmaxCsr.field.Cwmax2 = pEdcaParm->Cwmax[QID_AC_VI];
+		CwmaxCsr.field.Cwmax3 = pEdcaParm->Cwmax[QID_AC_VO];
+		RTMP_IO_WRITE32(pAd, WMM_CWMAX_CFG, CwmaxCsr.word);
+
+		AifsnCsr.word = 0;
+		AifsnCsr.field.Aifsn0 = Ac0Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_BE];
+		AifsnCsr.field.Aifsn1 = Ac1Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_BK];
+		AifsnCsr.field.Aifsn2 = Ac2Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			// Tuning for Wi-Fi WMM S06
+			if (pAd->CommonCfg.bWiFiTest &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+				AifsnCsr.field.Aifsn2 = Ac2Cfg.field.Aifsn - 4;
+
+			// Tuning for TGn Wi-Fi 5.2.32
+			// STA TestBed changes in this item: conexant legacy sta ==> broadcom 11n sta
+			if (STA_TGN_WIFI_ON(pAd) &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+			{
+				AifsnCsr.field.Aifsn0 = 3;
+				AifsnCsr.field.Aifsn2 = 7;
+			}
+
+			if (INFRA_ON(pAd))
+				CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_WMM_CAPABLE);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			AifsnCsr.field.Aifsn3 = Ac3Cfg.field.Aifsn - 1; //pEdcaParm->Aifsn[QID_AC_VO]; //for TGn wifi test
+#endif // CONFIG_STA_SUPPORT //
+		RTMP_IO_WRITE32(pAd, WMM_AIFSN_CFG, AifsnCsr.word);
+
+		NdisMoveMemory(&pAd->CommonCfg.APEdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+		if (!ADHOC_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("EDCA [#%d]: AIFSN CWmin CWmax  TXOP(us)  ACM\n", pEdcaParm->EdcaUpdateCount));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_BE      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[0],
+									 pEdcaParm->Cwmin[0],
+									 pEdcaParm->Cwmax[0],
+									 pEdcaParm->Txop[0]<<5,
+									 pEdcaParm->bACM[0]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_BK      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[1],
+									 pEdcaParm->Cwmin[1],
+									 pEdcaParm->Cwmax[1],
+									 pEdcaParm->Txop[1]<<5,
+									 pEdcaParm->bACM[1]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_VI      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[2],
+									 pEdcaParm->Cwmin[2],
+									 pEdcaParm->Cwmax[2],
+									 pEdcaParm->Txop[2]<<5,
+									 pEdcaParm->bACM[2]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_VO      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[3],
+									 pEdcaParm->Cwmin[3],
+									 pEdcaParm->Cwmax[3],
+									 pEdcaParm->Txop[3]<<5,
+									 pEdcaParm->bACM[3]));
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID 	AsicSetSlotTime(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN bUseShortSlotTime)
+{
+	ULONG	SlotTime;
+	UINT32	RegValue = 0;
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->CommonCfg.Channel > 14)
+		bUseShortSlotTime = TRUE;
+#endif // CONFIG_STA_SUPPORT //
+
+	if (bUseShortSlotTime)
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED);
+	else
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED);
+
+	SlotTime = (bUseShortSlotTime)? 9 : 20;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// force using short SLOT time for FAE to demo performance when TxBurst is ON
+		if (((pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)))
+#ifdef DOT11_N_SUPPORT
+			|| ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE) && (pAd->CommonCfg.BACapability.field.Policy == BA_NOTUSE))
+#endif // DOT11_N_SUPPORT //
+			)
+		{
+			// In this case, we will think it is doing Wi-Fi test
+			// And we will not set to short slot when bEnableTxBurst is TRUE.
+		}
+		else if (pAd->CommonCfg.bEnableTxBurst)
+			SlotTime = 9;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	//
+	// For some reasons, always set it to short slot time.
+	//
+	// ToDo: Should consider capability with 11B
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.BssType == BSS_ADHOC)
+			SlotTime = 20;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_IO_READ32(pAd, BKOFF_SLOT_CFG, &RegValue);
+	RegValue = RegValue & 0xFFFFFF00;
+
+	RegValue |= SlotTime;
+
+	RTMP_IO_WRITE32(pAd, BKOFF_SLOT_CFG, RegValue);
+}
+
+/*
+	========================================================================
+	Description:
+		Add Shared key information into ASIC.
+		Update shared key, TxMic and RxMic to Asic Shared key table
+		Update its cipherAlg to Asic Shared key Mode.
+
+    Return:
+	========================================================================
+*/
+VOID AsicAddSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIndex,
+	IN UCHAR		 KeyIdx,
+	IN UCHAR		 CipherAlg,
+	IN PUCHAR		 pKey,
+	IN PUCHAR		 pTxMic,
+	IN PUCHAR		 pRxMic)
+{
+	ULONG offset; //, csr0;
+	SHAREDKEY_MODE_STRUC csr1;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicAddSharedKeyEntry BssIndex=%d, KeyIdx=%d\n", BssIndex,KeyIdx));
+//============================================================================================
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicAddSharedKeyEntry: %s key #%d\n", CipherName[CipherAlg], BssIndex*4 + KeyIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pKey[0],pKey[1],pKey[2],pKey[3],pKey[4],pKey[5],pKey[6],pKey[7],pKey[8],pKey[9],pKey[10],pKey[11],pKey[12],pKey[13],pKey[14],pKey[15]));
+	if (pRxMic)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Rx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+	if (pTxMic)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Tx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+	}
+//============================================================================================
+	//
+	// fill key material - key + TX MIC + RX MIC
+	//
+
+#ifdef RT2870
+{
+	offset = SHARED_KEY_TABLE_BASE + (4*BssIndex + KeyIdx)*HW_KEY_ENTRY_SIZE;
+	RTUSBMultiWrite(pAd, offset, pKey, MAX_LEN_OF_SHARE_KEY);
+
+	offset += MAX_LEN_OF_SHARE_KEY;
+	if (pTxMic)
+	{
+		RTUSBMultiWrite(pAd, offset, pTxMic, 8);
+	}
+
+	offset += 8;
+	if (pRxMic)
+	{
+		RTUSBMultiWrite(pAd, offset, pRxMic, 8);
+	}
+}
+#endif // RT2870 //
+
+	//
+	// Update cipher algorithm. WSTA always use BSS0
+	//
+	RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), &csr1.word);
+	DBGPRINT(RT_DEBUG_TRACE,("Read: SHARED_KEY_MODE_BASE at this Bss[%d] KeyIdx[%d]= 0x%x \n", BssIndex,KeyIdx, csr1.word));
+	if ((BssIndex%2) == 0)
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss0Key0CipherAlg = CipherAlg;
+		else if (KeyIdx == 1)
+			csr1.field.Bss0Key1CipherAlg = CipherAlg;
+		else if (KeyIdx == 2)
+			csr1.field.Bss0Key2CipherAlg = CipherAlg;
+		else
+			csr1.field.Bss0Key3CipherAlg = CipherAlg;
+	}
+	else
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss1Key0CipherAlg = CipherAlg;
+		else if (KeyIdx == 1)
+			csr1.field.Bss1Key1CipherAlg = CipherAlg;
+		else if (KeyIdx == 2)
+			csr1.field.Bss1Key2CipherAlg = CipherAlg;
+		else
+			csr1.field.Bss1Key3CipherAlg = CipherAlg;
+	}
+	DBGPRINT(RT_DEBUG_TRACE,("Write: SHARED_KEY_MODE_BASE at this Bss[%d] = 0x%x \n", BssIndex, csr1.word));
+	RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), csr1.word);
+
+}
+
+//	IRQL = DISPATCH_LEVEL
+VOID AsicRemoveSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIndex,
+	IN UCHAR		 KeyIdx)
+{
+	//ULONG SecCsr0;
+	SHAREDKEY_MODE_STRUC csr1;
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicRemoveSharedKeyEntry: #%d \n", BssIndex*4 + KeyIdx));
+
+	RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), &csr1.word);
+	if ((BssIndex%2) == 0)
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss0Key0CipherAlg = 0;
+		else if (KeyIdx == 1)
+			csr1.field.Bss0Key1CipherAlg = 0;
+		else if (KeyIdx == 2)
+			csr1.field.Bss0Key2CipherAlg = 0;
+		else
+			csr1.field.Bss0Key3CipherAlg = 0;
+	}
+	else
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss1Key0CipherAlg = 0;
+		else if (KeyIdx == 1)
+			csr1.field.Bss1Key1CipherAlg = 0;
+		else if (KeyIdx == 2)
+			csr1.field.Bss1Key2CipherAlg = 0;
+		else
+			csr1.field.Bss1Key3CipherAlg = 0;
+	}
+	DBGPRINT(RT_DEBUG_TRACE,("Write: SHARED_KEY_MODE_BASE at this Bss[%d] = 0x%x \n", BssIndex, csr1.word));
+	RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), csr1.word);
+	ASSERT(BssIndex < 4);
+	ASSERT(KeyIdx < 4);
+
+}
+
+
+VOID AsicUpdateWCIDAttribute(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR        CipherAlg,
+	IN BOOLEAN		bUsePairewiseKeyTable)
+{
+	ULONG   WCIDAttri = 0, offset;
+
+	//
+	// Update WCID attribute.
+	// Only TxKey could update WCID attribute.
+	//
+	offset = MAC_WCID_ATTRIBUTE_BASE + (WCID * HW_WCID_ATTRI_SIZE);
+	WCIDAttri = (BssIndex << 4) | (CipherAlg << 1) | (bUsePairewiseKeyTable);
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+}
+
+VOID AsicUpdateWCIDIVEIV(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN ULONG        uIV,
+	IN ULONG        uEIV)
+{
+	ULONG	offset;
+
+	offset = MAC_IVEIV_TABLE_BASE + (WCID * HW_IVEIV_ENTRY_SIZE);
+
+	RTMP_IO_WRITE32(pAd, offset, uIV);
+	RTMP_IO_WRITE32(pAd, offset + 4, uEIV);
+}
+
+VOID AsicUpdateRxWCIDTable(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN PUCHAR        pAddr)
+{
+	ULONG offset;
+	ULONG Addr;
+
+	offset = MAC_WCID_BASE + (WCID * HW_WCID_ENTRY_SIZE);
+	Addr = pAddr[0] + (pAddr[1] << 8) +(pAddr[2] << 16) +(pAddr[3] << 24);
+	RTMP_IO_WRITE32(pAd, offset, Addr);
+	Addr = pAddr[4] + (pAddr[5] << 8);
+	RTMP_IO_WRITE32(pAd, offset + 4, Addr);
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Set Cipher Key, Cipher algorithm, IV/EIV to Asic
+
+    Arguments:
+        pAd                     Pointer to our adapter
+        WCID                    WCID Entry number.
+        BssIndex                BSSID index, station or none multiple BSSID support
+                                this value should be 0.
+        KeyIdx                  This KeyIdx will set to IV's KeyID if bTxKey enabled
+        pCipherKey              Pointer to Cipher Key.
+        bUsePairewiseKeyTable   TRUE means saved the key in SharedKey table,
+                                otherwise PairewiseKey table
+        bTxKey                  This is the transmit key if enabled.
+
+    Return Value:
+        None
+
+    Note:
+        This routine will set the relative key stuff to Asic including WCID attribute,
+        Cipher Key, Cipher algorithm and IV/EIV.
+
+        IV/EIV will be update if this CipherKey is the transmission key because
+        ASIC will base on IV's KeyID value to select Cipher Key.
+
+        If bTxKey sets to FALSE, this is not the TX key, but it could be
+        RX key
+
+    	For AP mode bTxKey must be always set to TRUE.
+    ========================================================================
+*/
+VOID AsicAddKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR		KeyIdx,
+	IN PCIPHER_KEY	pCipherKey,
+	IN BOOLEAN		bUsePairewiseKeyTable,
+	IN BOOLEAN		bTxKey)
+{
+	ULONG	offset;
+//	ULONG   WCIDAttri = 0;
+	UCHAR	IV4 = 0;
+	PUCHAR		pKey = pCipherKey->Key;
+//	ULONG		KeyLen = pCipherKey->KeyLen;
+	PUCHAR		pTxMic = pCipherKey->TxMic;
+	PUCHAR		pRxMic = pCipherKey->RxMic;
+	PUCHAR		pTxtsc = pCipherKey->TxTsc;
+	UCHAR		CipherAlg = pCipherKey->CipherAlg;
+	SHAREDKEY_MODE_STRUC csr1;
+
+//	ASSERT(KeyLen <= MAX_LEN_OF_PEER_KEY);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> AsicAddKeyEntry\n"));
+	//
+	// 1.) decide key table offset
+	//
+	if (bUsePairewiseKeyTable)
+		offset = PAIRWISE_KEY_TABLE_BASE + (WCID * HW_KEY_ENTRY_SIZE);
+	else
+		offset = SHARED_KEY_TABLE_BASE + (4 * BssIndex + KeyIdx) * HW_KEY_ENTRY_SIZE;
+
+	//
+	// 2.) Set Key to Asic
+	//
+	//for (i = 0; i < KeyLen; i++)
+
+#ifdef RT2870
+	RTUSBMultiWrite(pAd, offset, pKey, MAX_LEN_OF_PEER_KEY);
+	offset += MAX_LEN_OF_PEER_KEY;
+
+	//
+	// 3.) Set MIC key if available
+	//
+	if (pTxMic)
+	{
+		RTUSBMultiWrite(pAd, offset, pTxMic, 8);
+	}
+	offset += LEN_TKIP_TXMICK;
+
+	if (pRxMic)
+	{
+		RTUSBMultiWrite(pAd, offset, pRxMic, 8);
+	}
+#endif // RT2870 //
+
+	//
+	// 4.) Modify IV/EIV if needs
+	//     This will force Asic to use this key ID by setting IV.
+	//
+	if (bTxKey)
+	{
+
+#ifdef RT2870
+		UINT32 tmpVal;
+
+		//
+		// Write IV
+		//
+		IV4 = (KeyIdx << 6);
+		if ((CipherAlg == CIPHER_TKIP) || (CipherAlg == CIPHER_TKIP_NO_MIC) ||(CipherAlg == CIPHER_AES))
+			IV4 |= 0x20;  // turn on extension bit means EIV existence
+
+		tmpVal = pTxtsc[1] + (((pTxtsc[1] | 0x20) & 0x7f) << 8) + (pTxtsc[0] << 16) + (IV4 << 24);
+		RTMP_IO_WRITE32(pAd, offset, tmpVal);
+
+		//
+		// Write EIV
+		//
+		offset += 4;
+		RTMP_IO_WRITE32(pAd, offset, *(PUINT32)&pCipherKey->TxTsc[2]);
+#endif // RT2870 //
+		AsicUpdateWCIDAttribute(pAd, WCID, BssIndex, CipherAlg, bUsePairewiseKeyTable);
+	}
+
+	if (!bUsePairewiseKeyTable)
+	{
+		//
+		// Only update the shared key security mode
+		//
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE + 4 * (BssIndex / 2), &csr1.word);
+		if ((BssIndex % 2) == 0)
+		{
+			if (KeyIdx == 0)
+				csr1.field.Bss0Key0CipherAlg = CipherAlg;
+			else if (KeyIdx == 1)
+				csr1.field.Bss0Key1CipherAlg = CipherAlg;
+			else if (KeyIdx == 2)
+				csr1.field.Bss0Key2CipherAlg = CipherAlg;
+			else
+				csr1.field.Bss0Key3CipherAlg = CipherAlg;
+		}
+		else
+		{
+			if (KeyIdx == 0)
+				csr1.field.Bss1Key0CipherAlg = CipherAlg;
+			else if (KeyIdx == 1)
+				csr1.field.Bss1Key1CipherAlg = CipherAlg;
+			else if (KeyIdx == 2)
+				csr1.field.Bss1Key2CipherAlg = CipherAlg;
+			else
+				csr1.field.Bss1Key3CipherAlg = CipherAlg;
+		}
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE + 4 * (BssIndex / 2), csr1.word);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== AsicAddKeyEntry\n"));
+}
+
+
+/*
+	========================================================================
+	Description:
+		Add Pair-wise key material into ASIC.
+		Update pairwise key, TxMic and RxMic to Asic Pair-wise key table
+
+    Return:
+	========================================================================
+*/
+VOID AsicAddPairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR		WCID,
+	IN CIPHER_KEY		 *pCipherKey)
+{
+	INT i;
+	ULONG 		offset;
+	PUCHAR		 pKey = pCipherKey->Key;
+	PUCHAR		 pTxMic = pCipherKey->TxMic;
+	PUCHAR		 pRxMic = pCipherKey->RxMic;
+#ifdef DBG
+	UCHAR		CipherAlg = pCipherKey->CipherAlg;
+#endif // DBG //
+
+	// EKEY
+	offset = PAIRWISE_KEY_TABLE_BASE + (WCID * HW_KEY_ENTRY_SIZE);
+#ifdef RT2870
+	RTUSBMultiWrite(pAd, offset, &pCipherKey->Key[0], MAX_LEN_OF_PEER_KEY);
+#endif // RT2870 //
+	for (i=0; i<MAX_LEN_OF_PEER_KEY; i+=4)
+	{
+		UINT32 Value;
+		RTMP_IO_READ32(pAd, offset + i, &Value);
+	}
+
+	offset += MAX_LEN_OF_PEER_KEY;
+
+	//  MIC KEY
+	if (pTxMic)
+	{
+#ifdef RT2870
+		RTUSBMultiWrite(pAd, offset, &pCipherKey->TxMic[0], 8);
+#endif // RT2870 //
+	}
+	offset += 8;
+	if (pRxMic)
+	{
+#ifdef RT2870
+		RTUSBMultiWrite(pAd, offset, &pCipherKey->RxMic[0], 8);
+#endif // RT2870 //
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicAddPairwiseKeyEntry: WCID #%d Alg=%s\n",WCID, CipherName[CipherAlg]));
+	DBGPRINT(RT_DEBUG_TRACE,("	Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pKey[0],pKey[1],pKey[2],pKey[3],pKey[4],pKey[5],pKey[6],pKey[7],pKey[8],pKey[9],pKey[10],pKey[11],pKey[12],pKey[13],pKey[14],pKey[15]));
+	if (pRxMic)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("	Rx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+	if (pTxMic)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("	Tx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+	}
+}
+/*
+	========================================================================
+	Description:
+		Remove Pair-wise key material from ASIC.
+
+    Return:
+	========================================================================
+*/
+VOID AsicRemovePairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIdx,
+	IN UCHAR		 Wcid)
+{
+	ULONG		WCIDAttri;
+	USHORT		offset;
+
+	// re-set the entry's WCID attribute as OPEN-NONE.
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Wcid * HW_WCID_ATTRI_SIZE);
+	WCIDAttri = (BssIdx<<4) | PAIRWISEKEYTABLE;
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+}
+
+BOOLEAN AsicSendCommandToMcu(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 Command,
+	IN UCHAR		 Token,
+	IN UCHAR		 Arg0,
+	IN UCHAR		 Arg1)
+{
+	HOST_CMD_CSR_STRUC	H2MCmd;
+	H2M_MAILBOX_STRUC	H2MMailbox;
+	ULONG				i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, H2M_MAILBOX_CSR, &H2MMailbox.word);
+		if (H2MMailbox.field.Owner == 0)
+			break;
+
+		RTMPusecDelay(2);
+	} while(i++ < 100);
+
+	if (i >= 100)
+	{
+		{
+		DBGPRINT_ERR(("H2M_MAILBOX still hold by MCU. command fail\n"));
+		}
+		return FALSE;
+	}
+
+
+	H2MMailbox.field.Owner	  = 1;	   // pass ownership to MCU
+	H2MMailbox.field.CmdToken = Token;
+	H2MMailbox.field.HighByte = Arg1;
+	H2MMailbox.field.LowByte  = Arg0;
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, H2MMailbox.word);
+
+	H2MCmd.word 			  = 0;
+	H2MCmd.field.HostCommand  = Command;
+	RTMP_IO_WRITE32(pAd, HOST_CMD_CSR, H2MCmd.word);
+
+	if (Command != 0x80)
+	{
+	}
+
+	return TRUE;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for different PHY type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPCheckRates(
+	IN		PRTMP_ADAPTER	pAd,
+	IN OUT	UCHAR			SupRate[],
+	IN OUT	UCHAR			*SupRateLen)
+{
+	UCHAR	RateIdx, i, j;
+	UCHAR	NewRate[12], NewRateLen;
+
+	NewRateLen = 0;
+
+	if (pAd->CommonCfg.PhyMode == PHY_11B)
+		RateIdx = 4;
+	else
+		RateIdx = 12;
+
+	// Check for support rates exclude basic rate bit
+	for (i = 0; i < *SupRateLen; i++)
+		for (j = 0; j < RateIdx; j++)
+			if ((SupRate[i] & 0x7f) == RateIdTo500Kbps[j])
+				NewRate[NewRateLen++] = SupRate[i];
+
+	*SupRateLen = NewRateLen;
+	NdisMoveMemory(SupRate, NewRate, NewRateLen);
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+BOOLEAN RTMPCheckChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		CentralChannel,
+	IN UCHAR		Channel)
+{
+	UCHAR		k;
+	UCHAR		UpperChannel = 0, LowerChannel = 0;
+	UCHAR		NoEffectChannelinList = 0;
+
+	// Find upper and lower channel according to 40MHz current operation.
+	if (CentralChannel < Channel)
+	{
+		UpperChannel = Channel;
+		if (CentralChannel > 2)
+			LowerChannel = CentralChannel - 2;
+		else
+			return FALSE;
+	}
+	else if (CentralChannel > Channel)
+	{
+		UpperChannel = CentralChannel + 2;
+		LowerChannel = Channel;
+	}
+
+	for (k = 0;k < pAd->ChannelListNum;k++)
+	{
+		if (pAd->ChannelList[k].Channel == UpperChannel)
+		{
+			NoEffectChannelinList ++;
+		}
+		if (pAd->ChannelList[k].Channel == LowerChannel)
+		{
+			NoEffectChannelinList ++;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("Total Channel in Channel List = [%d]\n", NoEffectChannelinList));
+	if (NoEffectChannelinList == 2)
+		return TRUE;
+	else
+		return FALSE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for HT phy type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		FALSE if pAd->CommonCfg.SupportedHtPhy doesn't accept the pHtCapability.  (AP Mode)
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+BOOLEAN 	RTMPCheckHt(
+	IN	PRTMP_ADAPTER			pAd,
+	IN	UCHAR					Wcid,
+	IN 	HT_CAPABILITY_IE		*pHtCapability,
+	IN 	ADD_HT_INFO_IE			*pAddHtInfo)
+{
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	// If use AMSDU, set flag.
+	if (pAd->CommonCfg.DesiredHtPhy.AmsduEnable)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_AMSDU_INUSED);
+	// Save Peer Capability
+	if (pHtCapability->HtCapInfo.ShortGIfor20)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_SGI20_CAPABLE);
+	if (pHtCapability->HtCapInfo.ShortGIfor40)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_SGI40_CAPABLE);
+	if (pHtCapability->HtCapInfo.TxSTBC)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_TxSTBC_CAPABLE);
+	if (pHtCapability->HtCapInfo.RxSTBC)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_RxSTBC_CAPABLE);
+	if (pAd->CommonCfg.bRdg && pHtCapability->ExtHtCapInfo.RDGSupport)
+	{
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_RDG_CAPABLE);
+	}
+
+	if (Wcid < MAX_LEN_OF_MAC_TABLE)
+	{
+		pAd->MacTab.Content[Wcid].MpduDensity = pHtCapability->HtCapParm.MpduDensity;
+	}
+
+	// Will check ChannelWidth for MCSSet[4] below
+	pAd->MlmeAux.HtCapability.MCSSet[4] = 0x1;
+    switch (pAd->CommonCfg.RxStream)
+	{
+		case 1:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+		case 2:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+		case 3:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+	}
+
+	pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth = pAddHtInfo->AddHtInfo.RecomWidth & pAd->CommonCfg.DesiredHtPhy.ChannelWidth;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPCheckHt:: HtCapInfo.ChannelWidth=%d, RecomWidth=%d, DesiredHtPhy.ChannelWidth=%d, BW40MAvailForA/G=%d/%d, PhyMode=%d \n",
+		pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth, pAddHtInfo->AddHtInfo.RecomWidth, pAd->CommonCfg.DesiredHtPhy.ChannelWidth,
+		pAd->NicConfig2.field.BW40MAvailForA, pAd->NicConfig2.field.BW40MAvailForG, pAd->CommonCfg.PhyMode));
+
+	pAd->MlmeAux.HtCapability.HtCapInfo.GF =  pHtCapability->HtCapInfo.GF &pAd->CommonCfg.DesiredHtPhy.GF;
+
+	// Send Assoc Req with my HT capability.
+	pAd->MlmeAux.HtCapability.HtCapInfo.AMsduSize =  pAd->CommonCfg.DesiredHtPhy.AmsduSize;
+	pAd->MlmeAux.HtCapability.HtCapInfo.MimoPs =  pAd->CommonCfg.DesiredHtPhy.MimoPs;
+	pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor20 =  (pAd->CommonCfg.DesiredHtPhy.ShortGIfor20) & (pHtCapability->HtCapInfo.ShortGIfor20);
+	pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor40 =  (pAd->CommonCfg.DesiredHtPhy.ShortGIfor40) & (pHtCapability->HtCapInfo.ShortGIfor40);
+	pAd->MlmeAux.HtCapability.HtCapInfo.TxSTBC =  (pAd->CommonCfg.DesiredHtPhy.TxSTBC)&(pHtCapability->HtCapInfo.RxSTBC);
+	pAd->MlmeAux.HtCapability.HtCapInfo.RxSTBC =  (pAd->CommonCfg.DesiredHtPhy.RxSTBC)&(pHtCapability->HtCapInfo.TxSTBC);
+	pAd->MlmeAux.HtCapability.HtCapParm.MaxRAmpduFactor = pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor;
+    pAd->MlmeAux.HtCapability.HtCapParm.MpduDensity = pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity;
+	pAd->MlmeAux.HtCapability.ExtHtCapInfo.PlusHTC = pHtCapability->ExtHtCapInfo.PlusHTC;
+	pAd->MacTab.Content[Wcid].HTCapability.ExtHtCapInfo.PlusHTC = pHtCapability->ExtHtCapInfo.PlusHTC;
+	if (pAd->CommonCfg.bRdg)
+	{
+		pAd->MlmeAux.HtCapability.ExtHtCapInfo.RDGSupport = pHtCapability->ExtHtCapInfo.RDGSupport;
+        pAd->MlmeAux.HtCapability.ExtHtCapInfo.PlusHTC = 1;
+	}
+
+    if (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_20)
+        pAd->MlmeAux.HtCapability.MCSSet[4] = 0x0;  // BW20 can't transmit MCS32
+
+	COPY_AP_HTSETTINGS_FROM_BEACON(pAd, pHtCapability);
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for different PHY type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID RTMPUpdateMlmeRate(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UCHAR	MinimumRate;
+	UCHAR	ProperMlmeRate; //= RATE_54;
+	UCHAR	i, j, RateIdx = 12; //1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+	BOOLEAN	bMatch = FALSE;
+
+	switch (pAd->CommonCfg.PhyMode)
+	{
+		case PHY_11B:
+			ProperMlmeRate = RATE_11;
+			MinimumRate = RATE_1;
+			break;
+		case PHY_11BG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11ABGN_MIXED:
+		case PHY_11BGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			if ((pAd->MlmeAux.SupRateLen == 4) &&
+				(pAd->MlmeAux.ExtRateLen == 0))
+				// B only AP
+				ProperMlmeRate = RATE_11;
+			else
+				ProperMlmeRate = RATE_24;
+
+			if (pAd->MlmeAux.Channel <= 14)
+				MinimumRate = RATE_1;
+			else
+				MinimumRate = RATE_6;
+			break;
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11N_2_4G:	// rt2860 need to check mlmerate for 802.11n
+		case PHY_11GN_MIXED:
+		case PHY_11AGN_MIXED:
+		case PHY_11AN_MIXED:
+		case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+			ProperMlmeRate = RATE_24;
+			MinimumRate = RATE_6;
+			break;
+		case PHY_11ABG_MIXED:
+			ProperMlmeRate = RATE_24;
+			if (pAd->MlmeAux.Channel <= 14)
+			   MinimumRate = RATE_1;
+			else
+				MinimumRate = RATE_6;
+			break;
+		default: // error
+			ProperMlmeRate = RATE_1;
+			MinimumRate = RATE_1;
+			break;
+	}
+
+	for (i = 0; i < pAd->MlmeAux.SupRateLen; i++)
+	{
+		for (j = 0; j < RateIdx; j++)
+		{
+			if ((pAd->MlmeAux.SupRate[i] & 0x7f) == RateIdTo500Kbps[j])
+			{
+				if (j == ProperMlmeRate)
+				{
+					bMatch = TRUE;
+					break;
+				}
+			}
+		}
+
+		if (bMatch)
+			break;
+	}
+
+	if (bMatch == FALSE)
+	{
+		for (i = 0; i < pAd->MlmeAux.ExtRateLen; i++)
+		{
+			for (j = 0; j < RateIdx; j++)
+			{
+				if ((pAd->MlmeAux.ExtRate[i] & 0x7f) == RateIdTo500Kbps[j])
+				{
+					if (j == ProperMlmeRate)
+					{
+						bMatch = TRUE;
+						break;
+					}
+				}
+			}
+
+			if (bMatch)
+				break;
+		}
+	}
+
+	if (bMatch == FALSE)
+	{
+		ProperMlmeRate = MinimumRate;
+	}
+
+	pAd->CommonCfg.MlmeRate = MinimumRate;
+	pAd->CommonCfg.RtsRate = ProperMlmeRate;
+	if (pAd->CommonCfg.MlmeRate >= RATE_6)
+	{
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_OFDM;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+	}
+	else
+	{
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = pAd->CommonCfg.MlmeRate;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_CCK;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = pAd->CommonCfg.MlmeRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPUpdateMlmeRate ==>   MlmeTransmit = 0x%x  \n" , pAd->CommonCfg.MlmeTransmit.word));
+}
+
+CHAR RTMPMaxRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN CHAR				Rssi0,
+	IN CHAR				Rssi1,
+	IN CHAR				Rssi2)
+{
+	CHAR	larger = -127;
+
+	if ((pAd->Antenna.field.RxPath == 1) && (Rssi0 != 0))
+	{
+		larger = Rssi0;
+	}
+
+	if ((pAd->Antenna.field.RxPath >= 2) && (Rssi1 != 0))
+	{
+		larger = max(Rssi0, Rssi1);
+	}
+
+	if ((pAd->Antenna.field.RxPath == 3) && (Rssi2 != 0))
+	{
+		larger = max(larger, Rssi2);
+	}
+
+	if (larger == -127)
+		larger = 0;
+
+	return larger;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Periodic evaluate antenna link status
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID AsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UCHAR	BBPR3 = 0;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS	|
+								fRTMP_ADAPTER_HALT_IN_PROGRESS	|
+								fRTMP_ADAPTER_RADIO_OFF			|
+								fRTMP_ADAPTER_NIC_NOT_EXIST		|
+								fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			return;
+
+		if (pAd->StaCfg.Psm == PWR_SAVE)
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+	BBPR3 &= (~0x18);
+	if(pAd->Antenna.field.RxPath == 3)
+	{
+		BBPR3 |= (0x10);
+	}
+	else if(pAd->Antenna.field.RxPath == 2)
+	{
+		BBPR3 |= (0x8);
+	}
+	else if(pAd->Antenna.field.RxPath == 1)
+	{
+		BBPR3 |= (0x0);
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+		)
+	{
+		ULONG	TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+								pAd->RalinkCounters.OneSecTxRetryOkCount +
+								pAd->RalinkCounters.OneSecTxFailCount;
+
+		if (TxTotalCnt > 50)
+		{
+			RTMPSetTimer(&pAd->Mlme.RxAntEvalTimer, 20);
+			pAd->Mlme.bLowThroughput = FALSE;
+		}
+		else
+		{
+			RTMPSetTimer(&pAd->Mlme.RxAntEvalTimer, 300);
+			pAd->Mlme.bLowThroughput = TRUE;
+		}
+	}
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        After evaluation, check antenna link status
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID AsicRxAntEvalTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER	*pAd = (RTMP_ADAPTER *)FunctionContext;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR			BBPR3 = 0;
+	CHAR			larger = -127, rssi0, rssi1, rssi2;
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)	||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)		||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF)			||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+			return;
+
+		if (pAd->StaCfg.Psm == PWR_SAVE)
+			return;
+
+
+		// if the traffic is low, use average rssi as the criteria
+		if (pAd->Mlme.bLowThroughput == TRUE)
+		{
+			rssi0 = pAd->StaCfg.RssiSample.LastRssi0;
+			rssi1 = pAd->StaCfg.RssiSample.LastRssi1;
+			rssi2 = pAd->StaCfg.RssiSample.LastRssi2;
+		}
+		else
+		{
+			rssi0 = pAd->StaCfg.RssiSample.AvgRssi0;
+			rssi1 = pAd->StaCfg.RssiSample.AvgRssi1;
+			rssi2 = pAd->StaCfg.RssiSample.AvgRssi2;
+		}
+
+		if(pAd->Antenna.field.RxPath == 3)
+		{
+			larger = max(rssi0, rssi1);
+
+			if (larger > (rssi2 + 20))
+				pAd->Mlme.RealRxPath = 2;
+			else
+				pAd->Mlme.RealRxPath = 3;
+		}
+		else if(pAd->Antenna.field.RxPath == 2)
+		{
+			if (rssi0 > (rssi1 + 20))
+				pAd->Mlme.RealRxPath = 1;
+			else
+				pAd->Mlme.RealRxPath = 2;
+		}
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+		BBPR3 &= (~0x18);
+		if(pAd->Mlme.RealRxPath == 3)
+		{
+			BBPR3 |= (0x10);
+		}
+		else if(pAd->Mlme.RealRxPath == 2)
+		{
+			BBPR3 |= (0x8);
+		}
+		else if(pAd->Mlme.RealRxPath == 1)
+		{
+			BBPR3 |= (0x0);
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+	}
+
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+
+
+VOID APSDPeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		return;
+
+	pAd->CommonCfg.TriggerTimerCount++;
+
+// Driver should not send trigger frame, it should be send by application layer
+/*
+	if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable
+		&& (pAd->CommonCfg.bNeedSendTriggerFrame ||
+		(((pAd->CommonCfg.TriggerTimerCount%20) == 19) && (!pAd->CommonCfg.bAPSDAC_BE || !pAd->CommonCfg.bAPSDAC_BK || !pAd->CommonCfg.bAPSDAC_VI || !pAd->CommonCfg.bAPSDAC_VO))))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("Sending trigger frame and enter service period when support APSD\n"));
+		RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+		pAd->CommonCfg.bNeedSendTriggerFrame = FALSE;
+		pAd->CommonCfg.TriggerTimerCount = 0;
+		pAd->CommonCfg.bInServicePeriod = TRUE;
+	}*/
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Set/reset MAC registers according to bPiggyBack parameter
+
+    Arguments:
+        pAd         - Adapter pointer
+        bPiggyBack  - Enable / Disable Piggy-Back
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID RTMPSetPiggyBack(
+    IN PRTMP_ADAPTER    pAd,
+    IN BOOLEAN          bPiggyBack)
+{
+	TX_LINK_CFG_STRUC  TxLinkCfg;
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+
+	TxLinkCfg.field.TxCFAckEn = bPiggyBack;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        check if this entry need to switch rate automatically
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+BOOLEAN RTMPCheckEntryEnableAutoRateSwitch(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry)
+{
+	BOOLEAN		result = TRUE;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// only associated STA counts
+		if (pEntry && (pEntry->ValidAsCLI) && (pEntry->Sst == SST_ASSOC))
+		{
+			result = pAd->StaCfg.bAutoTxRateSwitch;
+		}
+		else
+			result = FALSE;
+
+#ifdef QOS_DLS_SUPPORT
+		if (pEntry && (pEntry->ValidAsDls))
+			result = pAd->StaCfg.bAutoTxRateSwitch;
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+	return result;
+}
+
+
+BOOLEAN RTMPAutoRateSwitchCheck(
+	IN PRTMP_ADAPTER    pAd)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.bAutoTxRateSwitch)
+			return TRUE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+	return FALSE;
+}
+
+
+/*
+    ========================================================================
+    Routine Description:
+        check if this entry need to fix tx legacy rate
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+UCHAR RTMPStaFixedTxMode(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry)
+{
+	UCHAR	tx_mode = FIXED_TXMODE_HT;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		tx_mode = (UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return tx_mode;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Overwrite HT Tx Mode by Fixed Legency Tx Mode, if specified.
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+VOID RTMPUpdateLegacyTxSetting(
+		UCHAR				fixed_tx_mode,
+		PMAC_TABLE_ENTRY	pEntry)
+{
+	HTTRANSMIT_SETTING TransmitSetting;
+
+	if (fixed_tx_mode == FIXED_TXMODE_HT)
+		return;
+
+	TransmitSetting.word = 0;
+
+	TransmitSetting.field.MODE = pEntry->HTPhyMode.field.MODE;
+	TransmitSetting.field.MCS = pEntry->HTPhyMode.field.MCS;
+
+	if (fixed_tx_mode == FIXED_TXMODE_CCK)
+	{
+		TransmitSetting.field.MODE = MODE_CCK;
+		// CCK mode allow MCS 0~3
+		if (TransmitSetting.field.MCS > MCS_3)
+			TransmitSetting.field.MCS = MCS_3;
+	}
+	else
+	{
+		TransmitSetting.field.MODE = MODE_OFDM;
+		// OFDM mode allow MCS 0~7
+		if (TransmitSetting.field.MCS > MCS_7)
+			TransmitSetting.field.MCS = MCS_7;
+	}
+
+	if (pEntry->HTPhyMode.field.MODE >= TransmitSetting.field.MODE)
+	{
+		pEntry->HTPhyMode.word = TransmitSetting.word;
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPUpdateLegacyTxSetting : wcid-%d, MODE=%s, MCS=%d \n",
+				pEntry->Aid, GetPhyMode(pEntry->HTPhyMode.field.MODE), pEntry->HTPhyMode.field.MCS));
+	}
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		dynamic tune BBP R66 to find a balance between sensibility and
+		noise isolation
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicStaBbpTuning(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR	OrigR66Value = 0, R66;//, R66UpperBound = 0x30, R66LowerBound = 0x30;
+	CHAR	Rssi;
+
+	// 2860C did not support Fase CCA, therefore can't tune
+	if (pAd->MACVersion == 0x28600100)
+		return;
+
+	//
+	// work as a STA
+	//
+	if (pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE)  // no R66 tuning when SCANNING
+		return;
+
+	if ((pAd->OpMode == OPMODE_STA)
+		&& (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+			)
+		&& !(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+		)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &OrigR66Value);
+		R66 = OrigR66Value;
+
+		if (pAd->Antenna.field.RxPath > 1)
+			Rssi = (pAd->StaCfg.RssiSample.AvgRssi0 + pAd->StaCfg.RssiSample.AvgRssi1) >> 1;
+		else
+			Rssi = pAd->StaCfg.RssiSample.AvgRssi0;
+
+		if (pAd->LatchRfRegs.Channel <= 14)
+		{	//BG band
+#ifdef RT2870
+			// RT3070 is a no LNA solution, it should have different control regarding to AGC gain control
+			// Otherwise, it will have some throughput side effect when low RSSI
+			if (IS_RT3070(pAd))
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x1C + 2*GET_LNA_GAIN(pAd) + 0x20;
+					if (OrigR66Value != R66)
+					{
+						RTUSBWriteBBPRegister(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x1C + 2*GET_LNA_GAIN(pAd);
+					if (OrigR66Value != R66)
+					{
+						RTUSBWriteBBPRegister(pAd, BBP_R66, R66);
+					}
+				}
+			}
+			else
+#endif // RT2870 //
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = (0x2E + GET_LNA_GAIN(pAd)) + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x2E + GET_LNA_GAIN(pAd);
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+		}
+		else
+		{	//A band
+			if (pAd->CommonCfg.BBPCurrentBW == BW_20)
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x32 + (GET_LNA_GAIN(pAd)*5)/3 + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x32 + (GET_LNA_GAIN(pAd)*5)/3;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+			else
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x3A + (GET_LNA_GAIN(pAd)*5)/3 + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x3A + (GET_LNA_GAIN(pAd)*5)/3;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+		}
+
+
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+VOID RTMPSetAGCInitValue(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			BandWidth)
+{
+	UCHAR	R66 = 0x30;
+
+	if (pAd->LatchRfRegs.Channel <= 14)
+	{	// BG band
+		R66 = 0x2E + GET_LNA_GAIN(pAd);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+	}
+	else
+	{	//A band
+		if (BandWidth == BW_20)
+		{
+			R66 = (UCHAR)(0x32 + (GET_LNA_GAIN(pAd)*5)/3);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+#ifdef DOT11_N_SUPPORT
+		else
+		{
+			R66 = (UCHAR)(0x3A + (GET_LNA_GAIN(pAd)*5)/3);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+#endif // DOT11_N_SUPPORT //
+	}
+
+}
+
+VOID AsicTurnOffRFClk(
+	IN PRTMP_ADAPTER pAd,
+	IN	UCHAR		Channel)
+{
+
+	// RF R2 bit 18 = 0
+	UINT32			R1 = 0, R2 = 0, R3 = 0;
+	UCHAR			index;
+	RTMP_RF_REGS	*RFRegTable;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R1 = RFRegTable[index].R1 & 0xffffdfff;
+					R2 = RFRegTable[index].R2 & 0xfffbffff;
+					R3 = RFRegTable[index].R3 & 0xfff3ffff;
+
+					RTMP_RF_IO_WRITE32(pAd, R1);
+					RTMP_RF_IO_WRITE32(pAd, R2);
+
+					// Program R1b13 to 1, R3/b18,19 to 0, R2b18 to 0.
+					// Set RF R2 bit18=0, R3 bit[18:19]=0
+					//if (pAd->StaCfg.bRadio == FALSE)
+					if (1)
+					{
+						RTMP_RF_IO_WRITE32(pAd, R3);
+
+						DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOffRFClk#%d(RF=%d, ) , R2=0x%08x,  R3 = 0x%08x \n",
+							Channel, pAd->RfIcType, R2, R3));
+					}
+					else
+						DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOffRFClk#%d(RF=%d, ) , R2=0x%08x \n",
+							Channel, pAd->RfIcType, R2));
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+}
+
+
+VOID AsicTurnOnRFClk(
+	IN PRTMP_ADAPTER pAd,
+	IN	UCHAR			Channel)
+{
+
+	// RF R2 bit 18 = 0
+	UINT32			R1 = 0, R2 = 0, R3 = 0;
+	UCHAR			index;
+	RTMP_RF_REGS	*RFRegTable;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R3 = pAd->LatchRfRegs.R3;
+					R3 &= 0xfff3ffff;
+					R3 |= 0x00080000;
+					RTMP_RF_IO_WRITE32(pAd, R3);
+
+					R1 = RFRegTable[index].R1;
+					RTMP_RF_IO_WRITE32(pAd, R1);
+
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						R2 |= 0x40;	// write 1 to off Rxpath.
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+					RTMP_RF_IO_WRITE32(pAd, R2);
+
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOnRFClk#%d(RF=%d, ) , R2=0x%08x\n",
+		Channel,
+		pAd->RfIcType,
+		R2));
+}
+
diff --git a/drivers/staging/rt2870/common/netif_block.c b/drivers/staging/rt2870/common/netif_block.c
new file mode 100644
index 00000000000..d3f7d087e7f
--- /dev/null
+++ b/drivers/staging/rt2870/common/netif_block.c
@@ -0,0 +1,144 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "../rt_config.h"
+#include "netif_block.h"
+
+static NETIF_ENTRY freeNetIfEntryPool[FREE_NETIF_POOL_SIZE];
+static LIST_HEADER freeNetIfEntryList;
+
+void initblockQueueTab(
+	IN PRTMP_ADAPTER pAd)
+{
+	int i;
+
+	initList(&freeNetIfEntryList);
+	for (i = 0; i < FREE_NETIF_POOL_SIZE; i++)
+		insertTailList(&freeNetIfEntryList, (PLIST_ENTRY)&freeNetIfEntryPool[i]);
+
+	for (i=0; i < NUM_OF_TX_RING; i++)
+		initList(&pAd->blockQueueTab[i].NetIfList);
+
+	return;
+}
+
+BOOLEAN blockNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry,
+	IN PNET_DEV pNetDev)
+{
+	PNETIF_ENTRY pNetIfEntry = NULL;
+
+	if ((pNetIfEntry = (PNETIF_ENTRY)removeHeadList(&freeNetIfEntryList)) != NULL)
+	{
+		netif_stop_queue(pNetDev);
+		pNetIfEntry->pNetDev = pNetDev;
+		insertTailList(&pBlockQueueEntry->NetIfList, (PLIST_ENTRY)pNetIfEntry);
+
+		pBlockQueueEntry->SwTxQueueBlockFlag = TRUE;
+		DBGPRINT(RT_DEBUG_TRACE, ("netif_stop_queue(%s)\n", pNetDev->name));
+	}
+	else
+		return FALSE;
+
+	return TRUE;
+}
+
+VOID releaseNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry)
+{
+	PNETIF_ENTRY pNetIfEntry = NULL;
+	PLIST_HEADER pNetIfList = &pBlockQueueEntry->NetIfList;
+
+	while((pNetIfEntry = (PNETIF_ENTRY)removeHeadList(pNetIfList)) !=  NULL)
+	{
+		PNET_DEV pNetDev = pNetIfEntry->pNetDev;
+		netif_wake_queue(pNetDev);
+		insertTailList(&freeNetIfEntryList, (PLIST_ENTRY)pNetIfEntry);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("netif_wake_queue(%s)\n", pNetDev->name));
+	}
+	pBlockQueueEntry->SwTxQueueBlockFlag = FALSE;
+	return;
+}
+
+
+VOID StopNetIfQueue(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR QueIdx,
+	IN PNDIS_PACKET pPacket)
+{
+	PNET_DEV NetDev = NULL;
+	UCHAR IfIdx = 0;
+	BOOLEAN valid = FALSE;
+
+#ifdef APCLI_SUPPORT
+	if (RTMP_GET_PACKET_NET_DEVICE(pPacket) >= MIN_NET_DEVICE_FOR_APCLI)
+	{
+		IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_APCLI) % MAX_APCLI_NUM;
+		NetDev = pAd->ApCfg.ApCliTab[IfIdx].dev;
+	}
+	else
+#endif // APCLI_SUPPORT //
+#ifdef WDS_SUPPORT
+	if (RTMP_GET_PACKET_NET_DEVICE(pPacket) >= MIN_NET_DEVICE_FOR_WDS)
+	{
+		IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_WDS) % MAX_WDS_ENTRY;
+		NetDev = pAd->WdsTab.WdsEntry[IfIdx].dev;
+	}
+	else
+#endif // WDS_SUPPORT //
+	{
+#ifdef MBSS_SUPPORT
+		if (pAd->OpMode == OPMODE_AP)
+		{
+			IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_MBSSID) % MAX_MBSSID_NUM;
+			NetDev = pAd->ApCfg.MBSSID[IfIdx].MSSIDDev;
+		}
+		else
+		{
+			IfIdx = MAIN_MBSSID;
+			NetDev = pAd->net_dev;
+		}
+#else
+		IfIdx = MAIN_MBSSID;
+		NetDev = pAd->net_dev;
+#endif
+	}
+
+	// WMM support 4 software queues.
+	// One software queue full doesn't mean device have no capbility to transmit packet.
+	// So disable block Net-If queue function while WMM enable.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		valid = (pAd->CommonCfg.bWmmCapable == TRUE) ? FALSE : TRUE;
+#endif // CONFIG_STA_SUPPORT //
+
+	if (valid)
+		blockNetIf(&pAd->blockQueueTab[QueIdx], NetDev);
+	return;
+}
+
diff --git a/drivers/staging/rt2870/common/rtmp_init.c b/drivers/staging/rt2870/common/rtmp_init.c
new file mode 100644
index 00000000000..87028fd60bc
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtmp_init.c
@@ -0,0 +1,4132 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_init.c
+
+	Abstract:
+	Miniport generic portion header file
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    2002-08-01    created
+    John Chang  2004-08-20    RT2561/2661 use scatter-gather scheme
+    Jan Lee  2006-09-15    RT2860. Change for 802.11n , EEPROM, Led, BA, HT.
+*/
+#include "../rt_config.h"
+#include "firmware.h"
+
+//#define BIN_IN_FILE /* use *.bin firmware */
+
+UCHAR    BIT8[] = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};
+ULONG    BIT32[] = {0x00000001, 0x00000002, 0x00000004, 0x00000008,
+					0x00000010, 0x00000020, 0x00000040, 0x00000080,
+					0x00000100, 0x00000200, 0x00000400, 0x00000800,
+					0x00001000, 0x00002000, 0x00004000, 0x00008000,
+					0x00010000, 0x00020000, 0x00040000, 0x00080000,
+					0x00100000, 0x00200000, 0x00400000, 0x00800000,
+					0x01000000, 0x02000000, 0x04000000, 0x08000000,
+					0x10000000, 0x20000000, 0x40000000, 0x80000000};
+
+char*   CipherName[] = {"none","wep64","wep128","TKIP","AES","CKIP64","CKIP128"};
+
+const unsigned short ccitt_16Table[] = {
+	0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
+	0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
+	0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
+	0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
+	0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
+	0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
+	0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
+	0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
+	0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
+	0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
+	0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
+	0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
+	0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
+	0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
+	0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
+	0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
+	0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
+	0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
+	0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
+	0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
+	0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
+	0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+	0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
+	0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
+	0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
+	0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
+	0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
+	0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
+	0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
+	0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
+	0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
+	0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
+};
+#define ByteCRC16(v, crc) \
+	(unsigned short)((crc << 8) ^  ccitt_16Table[((crc >> 8) ^ (v)) & 255])
+
+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;
+}
+
+//
+// BBP register initialization set
+//
+REG_PAIR   BBPRegTable[] = {
+	{BBP_R65,		0x2C},		// fix rssi issue
+	{BBP_R66,		0x38},	// Also set this default value to pAd->BbpTuning.R66CurrentValue at initial
+	{BBP_R69,		0x12},
+	{BBP_R70,		0xa},	// BBP_R70 will change to 0x8 in ApStartUp and LinkUp for rt2860C, otherwise value is 0xa
+	{BBP_R73,		0x10},
+	{BBP_R81,		0x37},
+	{BBP_R82,		0x62},
+	{BBP_R83,		0x6A},
+	{BBP_R84,		0x99},	// 0x19 is for rt2860E and after. This is for extension channel overlapping IOT. 0x99 is for rt2860D and before
+	{BBP_R86,		0x00},	// middle range issue, Rory @2008-01-28
+	{BBP_R91,		0x04},	// middle range issue, Rory @2008-01-28
+	{BBP_R92,		0x00},	// middle range issue, Rory @2008-01-28
+	{BBP_R103,  	0x00}, 	// near range high-power issue, requested from Gary @2008-0528
+	{BBP_R105,		0x05},	// 0x05 is for rt2860E to turn on FEQ control. It is safe for rt2860D and before, because Bit 7:2 are reserved in rt2860D and before.
+};
+#define	NUM_BBP_REG_PARMS	(sizeof(BBPRegTable) / sizeof(REG_PAIR))
+
+//
+// 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},
+        {RF_R10,          0x71},
+        {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},
+        {RF_R27,          0x03},
+        {RF_R29,          0x1F},
+};
+#define	NUM_RF_REG_PARMS	(sizeof(RT30xx_RFRegTable) / sizeof(REG_PAIR))
+#endif // RT2870 //
+
+//
+// ASIC register initialization sets
+//
+
+RTMP_REG_PAIR	MACRegTable[] =	{
+#if defined(HW_BEACON_OFFSET) && (HW_BEACON_OFFSET == 0x200)
+	{BCN_OFFSET0,			0xf8f0e8e0}, /* 0x3800(e0), 0x3A00(e8), 0x3C00(f0), 0x3E00(f8), 512B for each beacon */
+	{BCN_OFFSET1,			0x6f77d0c8}, /* 0x3200(c8), 0x3400(d0), 0x1DC0(77), 0x1BC0(6f), 512B for each beacon */
+#elif defined(HW_BEACON_OFFSET) && (HW_BEACON_OFFSET == 0x100)
+	{BCN_OFFSET0,			0xece8e4e0}, /* 0x3800, 0x3A00, 0x3C00, 0x3E00, 512B for each beacon */
+	{BCN_OFFSET1,			0xfcf8f4f0}, /* 0x3800, 0x3A00, 0x3C00, 0x3E00, 512B for each beacon */
+#else
+    #error You must re-calculate new value for BCN_OFFSET0 & BCN_OFFSET1 in MACRegTable[]!!!
+#endif // HW_BEACON_OFFSET //
+
+	{LEGACY_BASIC_RATE,		0x0000013f}, //  Basic rate set bitmap
+	{HT_BASIC_RATE,		0x00008003}, // Basic HT rate set , 20M, MCS=3, MM. Format is the same as in TXWI.
+	{MAC_SYS_CTRL,		0x00}, // 0x1004, , default Disable RX
+	{RX_FILTR_CFG,		0x17f97}, //0x1400  , RX filter control,
+	{BKOFF_SLOT_CFG,	0x209}, // default set short slot time, CC_DELAY_TIME should be 2
+	{TX_SW_CFG0,		0x0}, 		// Gary,2008-05-21 for CWC test
+	{TX_SW_CFG1,		0x80606}, // Gary,2006-08-23
+	{TX_LINK_CFG,		0x1020},		// Gary,2006-08-23
+	//{TX_TIMEOUT_CFG,	0x00182090},	// CCK has some problem. So increase timieout value. 2006-10-09// MArvek RT
+	{TX_TIMEOUT_CFG,	0x000a2090},	// CCK has some problem. So increase timieout value. 2006-10-09// MArvek RT , Modify for 2860E ,2007-08-01
+	{MAX_LEN_CFG,		MAX_AGGREGATION_SIZE | 0x00001000},	// 0x3018, MAX frame length. Max PSDU = 16kbytes.
+	{LED_CFG,		0x7f031e46}, // Gary, 2006-08-23
+	{PBF_MAX_PCNT,			0x1F3FBF9F}, 	//0x1F3f7f9f},		//Jan, 2006/04/20
+	//{TX_RTY_CFG,			0x6bb80408},	// Jan, 2006/11/16
+	{TX_RTY_CFG,			0x47d01f0f},	// Jan, 2006/11/16, Set TxWI->ACK =0 in Probe Rsp Modify for 2860E ,2007-08-03
+	{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.
+#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},
+	{TXOP_CTRL_CFG,			0x0000583f, /*0x0000243f*/ /*0x000024bf*/},	//Extension channel backoff.
+	{TX_RTS_CFG,			0x00092b20},
+//#ifdef WIFI_TEST
+	{EXP_ACK_TIME,			0x002400ca},	// default value
+//#else
+//	{EXP_ACK_TIME,			0x005400ca},	// suggested by Gray @ 20070323 for 11n intel-sta throughput
+//#endif // end - WIFI_TEST //
+	{TXOP_HLDR_ET, 			0x00000002},
+
+	/* Jerry comments 2008/01/16: we use SIFS = 10us in CCK defaultly, but it seems that 10us
+		is too small for INTEL 2200bg card, so in MBSS mode, the delta time between beacon0
+		and beacon1 is SIFS (10us), so if INTEL 2200bg card connects to BSS0, the ping
+		will always lost. So we change the SIFS of CCK from 10us to 16us. */
+	{XIFS_TIME_CFG,			0x33a41010},
+	{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
+#define FIRMWAREIMAGE_LENGTH		(sizeof (FirmwareImage) / sizeof(UCHAR))
+#define FIRMWARE_MAJOR_VERSION	0
+
+#define FIRMWAREIMAGEV1_LENGTH	0x1000
+#define FIRMWAREIMAGEV2_LENGTH	0x1000
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Allocate RTMP_ADAPTER data block and do some initialization
+
+	Arguments:
+		Adapter		Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPAllocAdapterBlock(
+	IN  PVOID	handle,
+	OUT	PRTMP_ADAPTER	*ppAdapter)
+{
+	PRTMP_ADAPTER	pAd;
+	NDIS_STATUS		Status;
+	INT 			index;
+	UCHAR			*pBeaconBuf = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RTMPAllocAdapterBlock\n"));
+
+	*ppAdapter = NULL;
+
+	do
+	{
+		// Allocate RTMP_ADAPTER memory block
+		pBeaconBuf = kmalloc(MAX_BEACON_SIZE, MEM_ALLOC_FLAG);
+		if (pBeaconBuf == NULL)
+		{
+			Status = NDIS_STATUS_FAILURE;
+			DBGPRINT_ERR(("Failed to allocate memory - BeaconBuf!\n"));
+			break;
+		}
+
+		Status = AdapterBlockAllocateMemory(handle, (PVOID *)&pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT_ERR(("Failed to allocate memory - ADAPTER\n"));
+			break;
+		}
+		pAd->BeaconBuf = pBeaconBuf;
+		printk("\n\n=== pAd = %p, size = %d ===\n\n", pAd, (UINT32)sizeof(RTMP_ADAPTER));
+
+
+		// Init spin locks
+		NdisAllocateSpinLock(&pAd->MgmtRingLock);
+
+		for (index =0 ; index < NUM_OF_TX_RING; index++)
+		{
+			NdisAllocateSpinLock(&pAd->TxSwQueueLock[index]);
+			NdisAllocateSpinLock(&pAd->DeQueueLock[index]);
+			pAd->DeQueueRunning[index] = FALSE;
+		}
+
+		NdisAllocateSpinLock(&pAd->irq_lock);
+
+	} while (FALSE);
+
+	if ((Status != NDIS_STATUS_SUCCESS) && (pBeaconBuf))
+		kfree(pBeaconBuf);
+
+	*ppAdapter = pAd;
+
+	DBGPRINT_S(Status, ("<-- RTMPAllocAdapterBlock, Status=%x\n", Status));
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial Tx power per MCS and BW from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReadTxPwrPerRate(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	ULONG		data, Adata, Gdata;
+	USHORT		i, value, value2;
+	INT			Apwrdelta, Gpwrdelta;
+	UCHAR		t1,t2,t3,t4;
+	BOOLEAN		bValid, bApwrdeltaMinus = TRUE, bGpwrdeltaMinus = TRUE;
+
+	//
+	// Get power delta for 20MHz and 40MHz.
+	//
+	DBGPRINT(RT_DEBUG_TRACE, ("Txpower per Rate\n"));
+	RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_DELTA, value2);
+	Apwrdelta = 0;
+	Gpwrdelta = 0;
+
+	if ((value2 & 0xff) != 0xff)
+	{
+		if ((value2 & 0x80))
+			Gpwrdelta = (value2&0xf);
+
+		if ((value2 & 0x40))
+			bGpwrdeltaMinus = FALSE;
+		else
+			bGpwrdeltaMinus = TRUE;
+	}
+	if ((value2 & 0xff00) != 0xff00)
+	{
+		if ((value2 & 0x8000))
+			Apwrdelta = ((value2&0xf00)>>8);
+
+		if ((value2 & 0x4000))
+			bApwrdeltaMinus = FALSE;
+		else
+			bApwrdeltaMinus = TRUE;
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("Gpwrdelta = %x, Apwrdelta = %x .\n", Gpwrdelta, Apwrdelta));
+
+	//
+	// Get Txpower per MCS for 20MHz in 2.4G.
+	//
+	for (i=0; i<5; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_2_4G + i*4, value);
+		data = value;
+		if (bApwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Apwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Apwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Apwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Apwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Apwrdelta)
+				t1 = (value&0xf)-(Apwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Apwrdelta)
+				t2 = ((value&0xf0)>>4)-(Apwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Apwrdelta)
+				t3 = ((value&0xf00)>>8)-(Apwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Apwrdelta)
+				t4 = ((value&0xf000)>>12)-(Apwrdelta);
+			else
+				t4 = 0;
+		}
+		Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+		if (bGpwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Gpwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Gpwrdelta)
+				t1 = (value&0xf)-(Gpwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Gpwrdelta)
+				t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Gpwrdelta)
+				t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Gpwrdelta)
+				t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+			else
+				t4 = 0;
+		}
+		Gdata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_2_4G + i*4 + 2, value);
+		if (bApwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Apwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Apwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Apwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Apwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Apwrdelta)
+				t1 = (value&0xf)-(Apwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Apwrdelta)
+				t2 = ((value&0xf0)>>4)-(Apwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Apwrdelta)
+				t3 = ((value&0xf00)>>8)-(Apwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Apwrdelta)
+				t4 = ((value&0xf000)>>12)-(Apwrdelta);
+			else
+				t4 = 0;
+		}
+		Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+		if (bGpwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Gpwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Gpwrdelta)
+				t1 = (value&0xf)-(Gpwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Gpwrdelta)
+				t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Gpwrdelta)
+				t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Gpwrdelta)
+				t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+			else
+				t4 = 0;
+		}
+		Gdata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+		data |= (value<<16);
+
+		pAd->Tx20MPwrCfgABand[i] = pAd->Tx40MPwrCfgABand[i] = Adata;
+		pAd->Tx20MPwrCfgGBand[i] = pAd->Tx40MPwrCfgGBand[i] = Gdata;
+
+		if (data != 0xffffffff)
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, data);
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("20MHz BW, 2.4G band-%lx,  Adata = %lx,  Gdata = %lx \n", data, Adata, Gdata));
+	}
+
+	//
+	// Check this block is valid for 40MHz in 2.4G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<6; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 40MHz in 2.4G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<4; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + i*4, value);
+			if (bGpwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Gpwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Gpwrdelta)
+					t1 = (value&0xf)-(Gpwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Gpwrdelta)
+					t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Gpwrdelta)
+					t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Gpwrdelta)
+					t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+				else
+					t4 = 0;
+			}
+			Gdata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + i*4 + 2, value);
+			if (bGpwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Gpwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Gpwrdelta)
+					t1 = (value&0xf)-(Gpwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Gpwrdelta)
+					t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Gpwrdelta)
+					t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Gpwrdelta)
+					t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+				else
+					t4 = 0;
+			}
+			Gdata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx40MPwrCfgGBand[i+1] = (pAd->Tx40MPwrCfgGBand[i+1] & 0x0000FFFF) | (Gdata & 0xFFFF0000);
+			else
+				pAd->Tx40MPwrCfgGBand[i+1] = Gdata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("40MHz BW, 2.4G band, Gdata = %lx \n", Gdata));
+		}
+	}
+
+	//
+	// Check this block is valid for 20MHz in 5G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<8; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 20MHz in 5G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<5; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + i*4, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + i*4 + 2, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx20MPwrCfgABand[i] = (pAd->Tx20MPwrCfgABand[i] & 0x0000FFFF) | (Adata & 0xFFFF0000);
+			else
+				pAd->Tx20MPwrCfgABand[i] = Adata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("20MHz BW, 5GHz band, Adata = %lx \n", Adata));
+		}
+	}
+
+	//
+	// Check this block is valid for 40MHz in 5G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<6; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 40MHz in 5G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<4; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + i*4, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + i*4 + 2, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx40MPwrCfgABand[i+1] = (pAd->Tx40MPwrCfgABand[i+1] & 0x0000FFFF) | (Adata & 0xFFFF0000);
+			else
+				pAd->Tx40MPwrCfgABand[i+1] = Adata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("40MHz BW, 5GHz band, Adata = %lx \n", Adata));
+		}
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial channel power parameters from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReadChannelPwr(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR				i, choffset;
+	EEPROM_TX_PWR_STRUC	    Power;
+	EEPROM_TX_PWR_STRUC	    Power2;
+
+	// Read Tx power value for all channels
+	// Value from 1 - 0x7f. Default value is 24.
+	// Power value : 2.4G 0x00 (0) ~ 0x1F (31)
+	//             : 5.5G 0xF9 (-7) ~ 0x0F (15)
+
+	// 0. 11b/g, ch1 - ch 14
+	for (i = 0; i < 7; i++)
+	{
+//		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_G_TX_PWR_OFFSET + i * 2);
+//		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_G_TX2_PWR_OFFSET + i * 2);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_G_TX_PWR_OFFSET + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_G_TX2_PWR_OFFSET + i * 2, Power2.word);
+		pAd->TxPower[i * 2].Channel = i * 2 + 1;
+		pAd->TxPower[i * 2 + 1].Channel = i * 2 + 2;
+
+		if ((Power.field.Byte0 > 31) || (Power.field.Byte0 < 0))
+			pAd->TxPower[i * 2].Power = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 > 31) || (Power.field.Byte1 < 0))
+			pAd->TxPower[i * 2 + 1].Power = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2 + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 > 31) || (Power2.field.Byte0 < 0))
+			pAd->TxPower[i * 2].Power2 = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 > 31) || (Power2.field.Byte1 < 0))
+			pAd->TxPower[i * 2 + 1].Power2 = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2 + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 1. U-NII lower/middle band: 36, 38, 40; 44, 46, 48; 52, 54, 56; 60, 62, 64 (including central frequency in BW 40MHz)
+	// 1.1 Fill up channel
+	choffset = 14;
+	for (i = 0; i < 4; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 36 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 36 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 36 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+
+	// 1.2 Fill up power
+	for (i = 0; i < 6; i++)
+	{
+//		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + i * 2);
+//		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + i * 2);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 2. HipperLAN 2 100, 102 ,104; 108, 110, 112; 116, 118, 120; 124, 126, 128; 132, 134, 136; 140 (including central frequency in BW 40MHz)
+	// 2.1 Fill up channel
+	choffset = 14 + 12;
+	for (i = 0; i < 5; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 100 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 100 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 100 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+	pAd->TxPower[3 * 5 + choffset + 0].Channel		= 140;
+	pAd->TxPower[3 * 5 + choffset + 0].Power		= DEFAULT_RF_TX_POWER;
+	pAd->TxPower[3 * 5 + choffset + 0].Power2		= DEFAULT_RF_TX_POWER;
+
+	// 2.2 Fill up power
+	for (i = 0; i < 8; i++)
+	{
+//		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2);
+//		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 3. U-NII upper band: 149, 151, 153; 157, 159, 161; 165 (including central frequency in BW 40MHz)
+	// 3.1 Fill up channel
+	choffset = 14 + 12 + 16;
+	for (i = 0; i < 2; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 149 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 149 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 149 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+	pAd->TxPower[3 * 2 + choffset + 0].Channel		= 165;
+	pAd->TxPower[3 * 2 + choffset + 0].Power		= DEFAULT_RF_TX_POWER;
+	pAd->TxPower[3 * 2 + choffset + 0].Power2		= DEFAULT_RF_TX_POWER;
+
+	// 3.2 Fill up power
+	for (i = 0; i < 4; i++)
+	{
+//		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2);
+//		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 4. Print and Debug
+	choffset = 14 + 12 + 16 + 7;
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read the following from the registry
+		1. All the parameters
+		2. NetworkAddres
+
+	Arguments:
+		Adapter						Pointer to our adapter
+		WrapperConfigurationContext	For use by NdisOpenConfiguration
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_RESOURCES
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICReadRegParameters(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	NDIS_HANDLE			WrapperConfigurationContext
+	)
+{
+	NDIS_STATUS						Status = NDIS_STATUS_SUCCESS;
+	DBGPRINT_S(Status, ("<-- NICReadRegParameters, Status=%x\n", Status));
+	return Status;
+}
+
+
+#ifdef RT2870
+/*
+	========================================================================
+
+	Routine Description:
+		For RF filter calibration purpose
+
+	Arguments:
+		pAd                          Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID RTUSBFilterCalibration(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR	R55x = 0, value, FilterTarget = 0x1E, BBPValue;
+	UINT	loop = 0, count = 0, loopcnt = 0, ReTry = 0;
+	UCHAR	RF_R24_Value = 0;
+
+	// Give bbp filter initial value
+	pAd->Mlme.CaliBW20RfR24 = 0x16;
+	pAd->Mlme.CaliBW40RfR24 = 0x36;  //Bit[5] must be 1 for BW 40
+
+	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);
+			FilterTarget = 0x19;
+
+			// when calibrate BW40, BBP mask must set to BW40.
+			RTUSBReadBBPRegister(pAd, BBP_R4, &BBPValue);
+			BBPValue&= (~0x18);
+			BBPValue|= (0x10);
+			RTUSBWriteBBPRegister(pAd, BBP_R4, BBPValue);
+		}
+		else			//BandWidth = 20 MHz
+		{
+			// Write 0x07 to RF_R24 to program filter
+			RF_R24_Value = 0x07;
+			RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+			FilterTarget = 0x16;
+		}
+
+		// 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
+		RTUSBWriteBBPRegister(pAd, BBP_R24, 0);
+
+		do
+		{
+			// Write 0x90 to BBP_R25 to transmit test tone
+			RTUSBWriteBBPRegister(pAd, BBP_R25, 0x90);
+
+			RTMPusecDelay(1000);
+			// Read BBP_R55[6:0] for received power, set R55x = BBP_R55[6:0]
+			RTUSBReadBBPRegister(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
+		RTUSBWriteBBPRegister(pAd, BBP_R24, 0x06);
+
+		while(TRUE)
+		{
+			// Write 0x90 to BBP_R25 to transmit test tone
+			RTUSBWriteBBPRegister(pAd, BBP_R25, 0x90);
+
+			//We need to wait for calibration
+			RTMPusecDelay(1000);
+			RTUSBReadBBPRegister(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, ("RTUSBFilterCalibration - 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
+	//
+	RTUSBWriteBBPRegister(pAd, BBP_R24, 0);
+
+	RT30xxReadRFRegister(pAd, RF_R22, &value);
+	value &= ~(0x01);
+	RT30xxWriteRFRegister(pAd, RF_R22, value);
+
+	// set BBP back to BW20
+	RTUSBReadBBPRegister(pAd, BBP_R4, &BBPValue);
+	BBPValue&= (~0x18);
+	RTUSBWriteBBPRegister(pAd, BBP_R4, BBPValue);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTUSBFilterCalibration - 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) && ((pAd->RfIcType == RFIC_3020) ||(pAd->RfIcType == RFIC_2020)))
+        {
+                // Init RF calibration
+                // Driver should toggle RF R30 bit7 before init RF registers
+                ULONG RfReg = 0;
+                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);
+                }
+
+                //For RF filter Calibration
+                RTUSBFilterCalibration(pAd);
+        }
+
+}
+#endif // RT2870 //
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial parameters from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	NICReadEEPROMParameters(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			mac_addr)
+{
+	UINT32			data = 0;
+	USHORT			i, value, value2;
+	UCHAR			TmpPhy;
+	EEPROM_TX_PWR_STRUC	    Power;
+	EEPROM_VERSION_STRUC    Version;
+	EEPROM_ANTENNA_STRUC	Antenna;
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICReadEEPROMParameters\n"));
+
+	// Init EEPROM Address Number, before access EEPROM; if 93c46, EEPROMAddressNum=6, else if 93c66, EEPROMAddressNum=8
+	RTMP_IO_READ32(pAd, E2PROM_CSR, &data);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> E2PROM_CSR = 0x%x\n", data));
+
+	if((data & 0x30) == 0)
+		pAd->EEPROMAddressNum = 6;		// 93C46
+	else if((data & 0x30) == 0x10)
+		pAd->EEPROMAddressNum = 8;     // 93C66
+	else
+		pAd->EEPROMAddressNum = 8;     // 93C86
+	DBGPRINT(RT_DEBUG_TRACE, ("--> EEPROMAddressNum = %d\n", pAd->EEPROMAddressNum ));
+
+	// RT2860 MAC no longer auto load MAC address from E2PROM. Driver has to intialize
+	// MAC address registers according to E2PROM setting
+	if (mac_addr == NULL ||
+		strlen(mac_addr) != 17 ||
+		mac_addr[2] != ':'  || mac_addr[5] != ':'  || mac_addr[8] != ':' ||
+		mac_addr[11] != ':' || mac_addr[14] != ':')
+	{
+		USHORT  Addr01,Addr23,Addr45 ;
+
+		RT28xx_EEPROM_READ16(pAd, 0x04, Addr01);
+		RT28xx_EEPROM_READ16(pAd, 0x06, Addr23);
+		RT28xx_EEPROM_READ16(pAd, 0x08, Addr45);
+
+		pAd->PermanentAddress[0] = (UCHAR)(Addr01 & 0xff);
+		pAd->PermanentAddress[1] = (UCHAR)(Addr01 >> 8);
+		pAd->PermanentAddress[2] = (UCHAR)(Addr23 & 0xff);
+		pAd->PermanentAddress[3] = (UCHAR)(Addr23 >> 8);
+		pAd->PermanentAddress[4] = (UCHAR)(Addr45 & 0xff);
+		pAd->PermanentAddress[5] = (UCHAR)(Addr45 >> 8);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Initialize MAC Address from E2PROM \n"));
+	}
+	else
+	{
+		INT		j;
+		PUCHAR	macptr;
+
+		macptr = mac_addr;
+
+		for (j=0; j<MAC_ADDR_LEN; j++)
+		{
+			AtoH(macptr, &pAd->PermanentAddress[j], 1);
+			macptr=macptr+3;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Initialize MAC Address from module parameter \n"));
+	}
+
+
+	{
+#if 0
+		USHORT  Addr01,Addr23,Addr45 ;
+
+		Addr01=RTMP_EEPROM_READ16(pAd, 0x04);
+		Addr23=RTMP_EEPROM_READ16(pAd, 0x06);
+		Addr45=RTMP_EEPROM_READ16(pAd, 0x08);
+
+		pAd->PermanentAddress[0] = (UCHAR)(Addr01 & 0xff);
+		pAd->PermanentAddress[1] = (UCHAR)(Addr01 >> 8);
+		pAd->PermanentAddress[2] = (UCHAR)(Addr23 & 0xff);
+		pAd->PermanentAddress[3] = (UCHAR)(Addr23 >> 8);
+		pAd->PermanentAddress[4] = (UCHAR)(Addr45 & 0xff);
+		pAd->PermanentAddress[5] = (UCHAR)(Addr45 >> 8);
+#endif
+		//more conveninet to test mbssid, so ap's bssid &0xf1
+		if (pAd->PermanentAddress[0] == 0xff)
+			pAd->PermanentAddress[0] = RandomByte(pAd)&0xf8;
+
+		//if (pAd->PermanentAddress[5] == 0xff)
+		//	pAd->PermanentAddress[5] = RandomByte(pAd)&0xf8;
+
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("E2PROM MAC: =%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pAd->PermanentAddress[0], pAd->PermanentAddress[1],
+			pAd->PermanentAddress[2], pAd->PermanentAddress[3],
+			pAd->PermanentAddress[4], pAd->PermanentAddress[5]));
+		if (pAd->bLocalAdminMAC == FALSE)
+		{
+			MAC_DW0_STRUC csr2;
+			MAC_DW1_STRUC csr3;
+			COPY_MAC_ADDR(pAd->CurrentAddress, pAd->PermanentAddress);
+			csr2.field.Byte0 = pAd->CurrentAddress[0];
+			csr2.field.Byte1 = pAd->CurrentAddress[1];
+			csr2.field.Byte2 = pAd->CurrentAddress[2];
+			csr2.field.Byte3 = pAd->CurrentAddress[3];
+			RTMP_IO_WRITE32(pAd, MAC_ADDR_DW0, csr2.word);
+			csr3.word = 0;
+			csr3.field.Byte4 = pAd->CurrentAddress[4];
+			csr3.field.Byte5 = pAd->CurrentAddress[5];
+			csr3.field.U2MeMask = 0xff;
+			RTMP_IO_WRITE32(pAd, MAC_ADDR_DW1, csr3.word);
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("E2PROM MAC: =%02x:%02x:%02x:%02x:%02x:%02x\n",
+				pAd->PermanentAddress[0], pAd->PermanentAddress[1],
+				pAd->PermanentAddress[2], pAd->PermanentAddress[3],
+				pAd->PermanentAddress[4], pAd->PermanentAddress[5]));
+		}
+	}
+
+	// if not return early. cause fail at emulation.
+	// Init the channel number for TX channel power
+	RTMPReadChannelPwr(pAd);
+
+	// if E2PROM version mismatch with driver's expectation, then skip
+	// all subsequent E2RPOM retieval and set a system error bit to notify GUI
+	RT28xx_EEPROM_READ16(pAd, EEPROM_VERSION_OFFSET, Version.word);
+	pAd->EepromVersion = Version.field.Version + Version.field.FaeReleaseNumber * 256;
+	DBGPRINT(RT_DEBUG_TRACE, ("E2PROM: Version = %d, FAE release #%d\n", Version.field.Version, Version.field.FaeReleaseNumber));
+
+	if (Version.field.Version > VALID_EEPROM_VERSION)
+	{
+		DBGPRINT_ERR(("E2PROM: WRONG VERSION 0x%x, should be %d\n",Version.field.Version, VALID_EEPROM_VERSION));
+		/*pAd->SystemErrorBitmap |= 0x00000001;
+
+		// hard-code default value when no proper E2PROM installed
+		pAd->bAutoTxAgcA = FALSE;
+		pAd->bAutoTxAgcG = FALSE;
+
+		// Default the channel power
+		for (i = 0; i < MAX_NUM_OF_CHANNELS; i++)
+			pAd->TxPower[i].Power = DEFAULT_RF_TX_POWER;
+
+		// Default the channel power
+		for (i = 0; i < MAX_NUM_OF_11JCHANNELS; i++)
+			pAd->TxPower11J[i].Power = DEFAULT_RF_TX_POWER;
+
+		for(i = 0; i < NUM_EEPROM_BBP_PARMS; i++)
+			pAd->EEPROMDefaultValue[i] = 0xffff;
+		return;  */
+	}
+
+	// Read BBP default value from EEPROM and store to array(EEPROMDefaultValue) in pAd
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC1_OFFSET, value);
+	pAd->EEPROMDefaultValue[0] = value;
+
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC2_OFFSET, value);
+	pAd->EEPROMDefaultValue[1] = value;
+
+	RT28xx_EEPROM_READ16(pAd, 0x38, value);	// Country Region
+	pAd->EEPROMDefaultValue[2] = value;
+
+	for(i = 0; i < 8; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_BBP_BASE_OFFSET + i*2, value);
+		pAd->EEPROMDefaultValue[i+3] = value;
+	}
+
+	// We have to parse NIC configuration 0 at here.
+	// If TSSI did not have preloaded value, it should reset the TxAutoAgc to false
+	// Therefore, we have to read TxAutoAgc control beforehand.
+	// Read Tx AGC control bit
+	Antenna.word = pAd->EEPROMDefaultValue[0];
+	if (Antenna.word == 0xFFFF)
+	{
+		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.
+	if ((pAd->CommonCfg.TxStream == 0) || (pAd->CommonCfg.TxStream > Antenna.field.TxPath))
+		pAd->CommonCfg.TxStream = Antenna.field.TxPath;
+
+	if ((pAd->CommonCfg.RxStream == 0) || (pAd->CommonCfg.RxStream > Antenna.field.RxPath))
+	{
+		pAd->CommonCfg.RxStream = Antenna.field.RxPath;
+
+		if ((pAd->MACVersion < RALINK_2883_VERSION) &&
+			(pAd->CommonCfg.RxStream > 2))
+		{
+			// only 2 Rx streams for RT2860 series
+			pAd->CommonCfg.RxStream = 2;
+		}
+	}
+
+	// 3*3
+	// read value from EEPROM and set them to CSR174 ~ 177 in chain0 ~ chain2
+	// yet implement
+	for(i=0; i<3; i++)
+	{
+	}
+
+	NicConfig2.word = pAd->EEPROMDefaultValue[1];
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		NicConfig2.word = 0;
+		if ((NicConfig2.word & 0x00ff) == 0xff)
+		{
+			NicConfig2.word &= 0xff00;
+		}
+
+		if ((NicConfig2.word >> 8) == 0xff)
+		{
+			NicConfig2.word &= 0x00ff;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (NicConfig2.field.DynamicTxAgcControl == 1)
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = TRUE;
+	else
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = FALSE;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("NICReadEEPROMParameters: RxPath = %d, TxPath = %d\n", Antenna.field.RxPath, Antenna.field.TxPath));
+
+	// Save the antenna for future use
+	pAd->Antenna.word = Antenna.word;
+
+	//
+	// Reset PhyMode if we don't support 802.11a
+	// Only RFIC_2850 & RFIC_2750 support 802.11a
+	//
+	if ((Antenna.field.RfIcType != RFIC_2850) && (Antenna.field.RfIcType != RFIC_2750))
+	{
+		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
+	// 0. 11b/g
+	{
+		/* these are tempature reference value (0x00 ~ 0xFE)
+		   ex: 0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+		   TssiPlusBoundaryG [4] [3] [2] [1] [0] (smaller) +
+		   TssiMinusBoundaryG[0] [1] [2] [3] [4] (larger) */
+		RT28xx_EEPROM_READ16(pAd, 0x6E, Power.word);
+		pAd->TssiMinusBoundaryG[4] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryG[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x70, Power.word);
+		pAd->TssiMinusBoundaryG[2] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryG[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x72, Power.word);
+		pAd->TssiRefG   = Power.field.Byte0; /* reference value [0] */
+		pAd->TssiPlusBoundaryG[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x74, Power.word);
+		pAd->TssiPlusBoundaryG[2] = Power.field.Byte0;
+		pAd->TssiPlusBoundaryG[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x76, Power.word);
+		pAd->TssiPlusBoundaryG[4] = Power.field.Byte0;
+		pAd->TxAgcStepG = Power.field.Byte1;
+		pAd->TxAgcCompensateG = 0;
+		pAd->TssiMinusBoundaryG[0] = pAd->TssiRefG;
+		pAd->TssiPlusBoundaryG[0]  = pAd->TssiRefG;
+
+		// Disable TxAgc if the based value is not right
+		if (pAd->TssiRefG == 0xff)
+			pAd->bAutoTxAgcG = FALSE;
+
+		DBGPRINT(RT_DEBUG_TRACE,("E2PROM: G Tssi[-4 .. +4] = %d %d %d %d - %d -%d %d %d %d, step=%d, tuning=%d\n",
+			pAd->TssiMinusBoundaryG[4], pAd->TssiMinusBoundaryG[3], pAd->TssiMinusBoundaryG[2], pAd->TssiMinusBoundaryG[1],
+			pAd->TssiRefG,
+			pAd->TssiPlusBoundaryG[1], pAd->TssiPlusBoundaryG[2], pAd->TssiPlusBoundaryG[3], pAd->TssiPlusBoundaryG[4],
+			pAd->TxAgcStepG, pAd->bAutoTxAgcG));
+	}
+	// 1. 11a
+	{
+		RT28xx_EEPROM_READ16(pAd, 0xD4, Power.word);
+		pAd->TssiMinusBoundaryA[4] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryA[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xD6, Power.word);
+		pAd->TssiMinusBoundaryA[2] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryA[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xD8, Power.word);
+		pAd->TssiRefA   = Power.field.Byte0;
+		pAd->TssiPlusBoundaryA[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xDA, Power.word);
+		pAd->TssiPlusBoundaryA[2] = Power.field.Byte0;
+		pAd->TssiPlusBoundaryA[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xDC, Power.word);
+		pAd->TssiPlusBoundaryA[4] = Power.field.Byte0;
+		pAd->TxAgcStepA = Power.field.Byte1;
+		pAd->TxAgcCompensateA = 0;
+		pAd->TssiMinusBoundaryA[0] = pAd->TssiRefA;
+		pAd->TssiPlusBoundaryA[0]  = pAd->TssiRefA;
+
+		// Disable TxAgc if the based value is not right
+		if (pAd->TssiRefA == 0xff)
+			pAd->bAutoTxAgcA = FALSE;
+
+		DBGPRINT(RT_DEBUG_TRACE,("E2PROM: A Tssi[-4 .. +4] = %d %d %d %d - %d -%d %d %d %d, step=%d, tuning=%d\n",
+			pAd->TssiMinusBoundaryA[4], pAd->TssiMinusBoundaryA[3], pAd->TssiMinusBoundaryA[2], pAd->TssiMinusBoundaryA[1],
+			pAd->TssiRefA,
+			pAd->TssiPlusBoundaryA[1], pAd->TssiPlusBoundaryA[2], pAd->TssiPlusBoundaryA[3], pAd->TssiPlusBoundaryA[4],
+			pAd->TxAgcStepA, pAd->bAutoTxAgcA));
+	}
+	pAd->BbpRssiToDbmDelta = 0x0;
+
+	// Read frequency offset setting for RF
+	RT28xx_EEPROM_READ16(pAd, EEPROM_FREQ_OFFSET, value);
+	if ((value & 0x00FF) != 0x00FF)
+		pAd->RfFreqOffset = (ULONG) (value & 0x00FF);
+	else
+		pAd->RfFreqOffset = 0;
+	DBGPRINT(RT_DEBUG_TRACE, ("E2PROM: RF FreqOffset=0x%lx \n", pAd->RfFreqOffset));
+
+	//CountryRegion byte offset (38h)
+	value = pAd->EEPROMDefaultValue[2] >> 8;		// 2.4G band
+	value2 = pAd->EEPROMDefaultValue[2] & 0x00FF;	// 5G band
+
+	if ((value <= REGION_MAXIMUM_BG_BAND) && (value2 <= REGION_MAXIMUM_A_BAND))
+	{
+		pAd->CommonCfg.CountryRegion = ((UCHAR) value) | 0x80;
+		pAd->CommonCfg.CountryRegionForABand = ((UCHAR) value2) | 0x80;
+		TmpPhy = pAd->CommonCfg.PhyMode;
+		pAd->CommonCfg.PhyMode = 0xff;
+		RTMPSetPhyMode(pAd, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+	}
+
+	//
+	// Get RSSI Offset on EEPROM 0x9Ah & 0x9Ch.
+	// The valid value are (-10 ~ 10)
+	//
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET, value);
+	pAd->BGRssiOffset0 = value & 0x00ff;
+	pAd->BGRssiOffset1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET+2, value);
+	pAd->BGRssiOffset2 = value & 0x00ff;
+	pAd->ALNAGain1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LNA_OFFSET, value);
+	pAd->BLNAGain = value & 0x00ff;
+	pAd->ALNAGain0 = (value >> 8);
+
+	// Validate 11b/g RSSI_0 offset.
+	if ((pAd->BGRssiOffset0 < -10) || (pAd->BGRssiOffset0 > 10))
+		pAd->BGRssiOffset0 = 0;
+
+	// Validate 11b/g RSSI_1 offset.
+	if ((pAd->BGRssiOffset1 < -10) || (pAd->BGRssiOffset1 > 10))
+		pAd->BGRssiOffset1 = 0;
+
+	// Validate 11b/g RSSI_2 offset.
+	if ((pAd->BGRssiOffset2 < -10) || (pAd->BGRssiOffset2 > 10))
+		pAd->BGRssiOffset2 = 0;
+
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_A_OFFSET, value);
+	pAd->ARssiOffset0 = value & 0x00ff;
+	pAd->ARssiOffset1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_A_OFFSET+2), value);
+	pAd->ARssiOffset2 = value & 0x00ff;
+	pAd->ALNAGain2 = (value >> 8);
+
+	if (((UCHAR)pAd->ALNAGain1 == 0xFF) || (pAd->ALNAGain1 == 0x00))
+		pAd->ALNAGain1 = pAd->ALNAGain0;
+	if (((UCHAR)pAd->ALNAGain2 == 0xFF) || (pAd->ALNAGain2 == 0x00))
+		pAd->ALNAGain2 = pAd->ALNAGain0;
+
+	// Validate 11a RSSI_0 offset.
+	if ((pAd->ARssiOffset0 < -10) || (pAd->ARssiOffset0 > 10))
+		pAd->ARssiOffset0 = 0;
+
+	// Validate 11a RSSI_1 offset.
+	if ((pAd->ARssiOffset1 < -10) || (pAd->ARssiOffset1 > 10))
+		pAd->ARssiOffset1 = 0;
+
+	//Validate 11a RSSI_2 offset.
+	if ((pAd->ARssiOffset2 < -10) || (pAd->ARssiOffset2 > 10))
+		pAd->ARssiOffset2 = 0;
+
+	//
+	// Get LED Setting.
+	//
+	RT28xx_EEPROM_READ16(pAd, 0x3a, value);
+	pAd->LedCntl.word = (value&0xff00) >> 8;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED1_OFFSET, value);
+	pAd->Led1 = value;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED2_OFFSET, value);
+	pAd->Led2 = value;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED3_OFFSET, value);
+	pAd->Led3 = value;
+
+	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"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set default value from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+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;
+	UCHAR	BBPR3 = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitAsicFromEEPROM\n"));
+	for(i = 3; i < NUM_EEPROM_BBP_PARMS; i++)
+	{
+		UCHAR BbpRegIdx, BbpValue;
+
+		if ((pAd->EEPROMDefaultValue[i] != 0xFFFF) && (pAd->EEPROMDefaultValue[i] != 0))
+		{
+			BbpRegIdx = (UCHAR)(pAd->EEPROMDefaultValue[i] >> 8);
+			BbpValue  = (UCHAR)(pAd->EEPROMDefaultValue[i] & 0xff);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BbpRegIdx, BbpValue);
+		}
+	}
+
+	Antenna.word = pAd->Antenna.word;
+	pAd->Mlme.RealRxPath = (UCHAR) Antenna.field.RxPath;
+	pAd->RfIcType = (UCHAR) Antenna.field.RfIcType;
+
+	NicConfig2.word = pAd->EEPROMDefaultValue[1];
+
+
+	// Save the antenna for future use
+	pAd->NicConfig2.word = NicConfig2.word;
+
+	//
+	// Send LED Setting to MCU.
+	//
+	if (pAd->LedCntl.word == 0xFF)
+	{
+		pAd->LedCntl.word = 0x01;
+		pAd->Led1 = 0x5555;
+		pAd->Led2 = 0x2221;
+
+#ifdef RT2870
+		pAd->Led3 = 0x5627;
+#endif // RT2870 //
+	}
+
+	AsicSendCommandToMcu(pAd, 0x52, 0xff, (UCHAR)pAd->Led1, (UCHAR)(pAd->Led1 >> 8));
+	AsicSendCommandToMcu(pAd, 0x53, 0xff, (UCHAR)pAd->Led2, (UCHAR)(pAd->Led2 >> 8));
+	AsicSendCommandToMcu(pAd, 0x54, 0xff, (UCHAR)pAd->Led3, (UCHAR)(pAd->Led3 >> 8));
+    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)
+		{
+			pAd->StaCfg.bHardwareRadio = TRUE;
+
+			// Read GPIO pin2 as Hardware controlled radio state
+			RTMP_IO_READ32(pAd, GPIO_CTRL_CFG, &data);
+			if ((data & 0x04) == 0)
+			{
+				pAd->StaCfg.bHwRadio = FALSE;
+				pAd->StaCfg.bRadio = FALSE;
+//				RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0x00001818);
+				RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+			}
+		}
+		else
+			pAd->StaCfg.bHardwareRadio = FALSE;
+
+		if (pAd->StaCfg.bRadio == FALSE)
+		{
+			RTMPSetLED(pAd, LED_RADIO_OFF);
+		}
+		else
+		{
+			RTMPSetLED(pAd, LED_RADIO_ON);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Turn off patching for cardbus controller
+	if (NicConfig2.field.CardbusAcceleration == 1)
+	{
+//		pAd->bTest1 = TRUE;
+	}
+
+	if (NicConfig2.field.DynamicTxAgcControl == 1)
+		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!!
+	//
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+	BBPR3 &= (~0x18);
+	if(pAd->Antenna.field.RxPath == 3)
+	{
+		BBPR3 |= (0x10);
+	}
+	else if(pAd->Antenna.field.RxPath == 2)
+	{
+		BBPR3 |= (0x8);
+	}
+	else if(pAd->Antenna.field.RxPath == 1)
+	{
+		BBPR3 |= (0x0);
+	}
+	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);
+		if(pAd->Antenna.field.TxPath == 1)
+		{
+		BBPR1 &= (~0x18);
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BBPR1);
+
+		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"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize NIC hardware
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICInitializeAdapter(
+	IN	PRTMP_ADAPTER	pAd,
+	IN   BOOLEAN    bHardReset)
+{
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+//	INT_MASK_CSR_STRUC		IntMask;
+	ULONG	i =0, j=0;
+	AC_TXOP_CSR0_STRUC	csr0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitializeAdapter\n"));
+
+	// 3. Set DMA global configuration except TX_DMA_EN and RX_DMA_EN bits:
+retry:
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i<100);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== DMA offset 0x208 = 0x%x\n", GloCfg.word));
+	GloCfg.word &= 0xff0;
+	GloCfg.field.EnTXWriteBackDDONE =1;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	// Record HW Beacon offset
+	pAd->BeaconOffset[0] = HW_BEACON_BASE0;
+	pAd->BeaconOffset[1] = HW_BEACON_BASE1;
+	pAd->BeaconOffset[2] = HW_BEACON_BASE2;
+	pAd->BeaconOffset[3] = HW_BEACON_BASE3;
+	pAd->BeaconOffset[4] = HW_BEACON_BASE4;
+	pAd->BeaconOffset[5] = HW_BEACON_BASE5;
+	pAd->BeaconOffset[6] = HW_BEACON_BASE6;
+	pAd->BeaconOffset[7] = HW_BEACON_BASE7;
+
+	//
+	// write all shared Ring's base address into ASIC
+	//
+
+	// asic simulation sequence put this ahead before loading firmware.
+	// pbf hardware reset
+
+	// Initialze ASIC for TX & Rx operation
+	if (NICInitializeAsic(pAd , bHardReset) != NDIS_STATUS_SUCCESS)
+	{
+		if (j++ == 0)
+		{
+			NICLoadFirmware(pAd);
+			goto retry;
+		}
+		return NDIS_STATUS_FAILURE;
+	}
+
+
+
+
+	// WMM parameter
+	csr0.word = 0;
+	RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+	if (pAd->CommonCfg.PhyMode == PHY_11B)
+	{
+		csr0.field.Ac0Txop = 192;	// AC_VI: 192*32us ~= 6ms
+		csr0.field.Ac1Txop = 96;	// AC_VO: 96*32us  ~= 3ms
+	}
+	else
+	{
+		csr0.field.Ac0Txop = 96;	// AC_VI: 96*32us ~= 3ms
+		csr0.field.Ac1Txop = 48;	// AC_VO: 48*32us ~= 1.5ms
+	}
+	RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr0.word);
+
+
+
+
+	// reset action
+	// Load firmware
+	//  Status = NICLoadFirmware(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitializeAdapter\n"));
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICInitializeAsic(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  BOOLEAN		bHardReset)
+{
+	ULONG			Index = 0;
+	UCHAR			R0 = 0xff;
+	UINT32			MacCsr12 = 0, Counter = 0;
+#ifdef RT2870
+	UINT32			MacCsr0 = 0;
+	NTSTATUS		Status;
+	UCHAR			Value = 0xff;
+#endif // RT2870 //
+	USHORT			KeyIdx;
+	INT				i,apidx;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitializeAsic\n"));
+
+
+#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);
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x0);
+
+	// Initialize MAC register to default value
+	for(Index=0; Index<NUM_MAC_REG_PARMS; Index++)
+	{
+		RTMP_IO_WRITE32(pAd, (USHORT)MACRegTable[Index].Register, MACRegTable[Index].Value);
+	}
+
+	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);
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		for (Index = 0; Index < NUM_STA_MAC_REG_PARMS; Index++)
+		{
+			RTMP_IO_WRITE32(pAd, (USHORT)STAMACRegTable[Index].Register, STAMACRegTable[Index].Value);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2870 //
+
+	//
+	// Before program BBP, we need to wait BBP/RF get wake up.
+	//
+	Index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, MAC_STATUS_CFG, &MacCsr12);
+
+		if ((MacCsr12 & 0x03) == 0)	// if BB.RF is stable
+			break;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Check MAC_STATUS_CFG  = Busy = %x\n", MacCsr12));
+		RTMPusecDelay(1000);
+	} while (Index++ < 100);
+
+    // The commands to firmware should be after these commands, these commands will init firmware
+	// PCI and USB are not the same because PCI driver needs to wait for PCI bus ready
+	RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, 0);	// initialize BBP R/W access agent
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, 0);
+	RTMPusecDelay(1000);
+
+	// Read BBP register, make sure BBP is up and running before write new data
+	Index = 0;
+	do
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R0, &R0);
+		DBGPRINT(RT_DEBUG_TRACE, ("BBP version = %x\n", R0));
+	} while ((++Index < 20) && ((R0 == 0xff) || (R0 == 0x00)));
+	//ASSERT(Index < 20); //this will cause BSOD on Check-build driver
+
+	if ((R0 == 0xff) || (R0 == 0x00))
+		return NDIS_STATUS_FAILURE;
+
+	// Initialize BBP register to default value
+	for (Index = 0; Index < NUM_BBP_REG_PARMS; Index++)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBPRegTable[Index].Register, BBPRegTable[Index].Value);
+	}
+
+	// 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 //
+
+	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);
+    }
+
+	if (pAd->MACVersion >= RALINK_2880E_VERSION && pAd->MACVersion < RALINK_3070_VERSION) // 3*3
+	{
+		// enlarge MAX_LEN_CFG
+		UINT32 csr;
+		RTMP_IO_READ32(pAd, MAX_LEN_CFG, &csr);
+		csr &= 0xFFF;
+		csr |= 0x2000;
+		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)
+		{
+//			RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0x00001818);
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+			DBGPRINT(RT_DEBUG_TRACE, ("Set Radio Off\n"));
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// 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);
+
+	// ASIC will keep garbage value after boot
+	// Clear all seared key table when initial
+	// This routine can be ignored in radio-ON/OFF operation.
+	if (bHardReset)
+	{
+		for (KeyIdx = 0; KeyIdx < 4; KeyIdx++)
+		{
+			RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE + 4*KeyIdx, 0);
+		}
+
+		// Clear all pairwise key table when initial
+		for (KeyIdx = 0; KeyIdx < 256; KeyIdx++)
+		{
+			RTMP_IO_WRITE32(pAd, MAC_WCID_ATTRIBUTE_BASE + (KeyIdx * HW_WCID_ATTRI_SIZE), 1);
+		}
+	}
+
+	// assert HOST ready bit
+//  RTMP_IO_WRITE32(pAd, MAC_CSR1, 0x0); // 2004-09-14 asked by Mark
+//  RTMP_IO_WRITE32(pAd, MAC_CSR1, 0x4);
+
+	// It isn't necessary to clear this space when not hard reset.
+	if (bHardReset == TRUE)
+	{
+		// clear all on-chip BEACON frame space
+		for (apidx = 0; apidx < HW_BEACON_MAX_COUNT; apidx++)
+		{
+			for (i = 0; i < HW_BEACON_OFFSET>>2; i+=4)
+				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);
+#endif // RT2870 //
+
+#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;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Reset NIC Asics
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Reset NIC to initial state AS IS system boot up time.
+
+	========================================================================
+*/
+VOID	NICIssueReset(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UINT32	Value = 0;
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICIssueReset\n"));
+
+	// Abort Tx, prevent ASIC from writing to Host memory
+	//RTMP_IO_WRITE32(pAd, TX_CNTL_CSR, 0x001f0000);
+
+	// Disable Rx, register value supposed will remain after reset
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value &= (0xfffffff3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+	// Issue reset and clear from reset state
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x03); // 2004-09-17 change from 0x01
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x00);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICIssueReset\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check ASIC registers and find any reason the system might hang
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+BOOLEAN	NICCheckForHang(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	return (FALSE);
+}
+
+VOID NICUpdateFifoStaCounters(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_STA_FIFO_STRUC	StaFifo;
+	MAC_TABLE_ENTRY		*pEntry;
+	UCHAR				i = 0;
+	UCHAR			pid = 0, wcid = 0;
+	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);
+
+			if (StaFifo.field.bValid == 0)
+				break;
+
+			wcid = (UCHAR)StaFifo.field.wcid;
+
+
+		/* ignore NoACK and MGMT frame use 0xFF as WCID */
+			if ((StaFifo.field.TxAckRequired == 0) || (wcid >= MAX_LEN_OF_MAC_TABLE))
+			{
+				i++;
+				continue;
+			}
+
+			/* PID store Tx MCS Rate */
+			pid = (UCHAR)StaFifo.field.PidType;
+
+			pEntry = &pAd->MacTab.Content[wcid];
+
+			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);
+#endif // UAPSD_AP_SUPPORT //
+
+			if (!StaFifo.field.TxSuccess)
+			{
+				pEntry->FIFOCount++;
+				pEntry->OneSecTxFailCount++;
+
+				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++;
+					}
+					else
+					{
+						// Clear the FIFOCount when sta in Power Save mode. Basically we assume
+						//     this tx error happened due to sta just go to sleep.
+						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--;
+					if (pEntry->NoBADataCountDown==0)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("@\n"));
+					}
+				}
+#endif // DOT11_N_SUPPORT //
+				pEntry->FIFOCount = 0;
+				pEntry->OneSecTxNoRetryOkCount++;
+				// update NoDataIdleCount when sucessful send packet to STA.
+				pEntry->NoDataIdleCount = 0;
+				pEntry->ContinueTxFailCnt = 0;
+			}
+
+			succMCS = StaFifo.field.SuccessRate & 0x7F;
+
+			reTry = pid - succMCS;
+
+			if (StaFifo.field.TxSuccess)
+			{
+				pEntry->TXMCSExpected[pid]++;
+				if (pid == succMCS)
+				{
+					pEntry->TXMCSSuccessful[pid]++;
+				}
+				else
+				{
+					pEntry->TXMCSAutoFallBack[pid][succMCS]++;
+				}
+			}
+			else
+			{
+				pEntry->TXMCSFailed[pid]++;
+			}
+
+			if (reTry > 0)
+			{
+				if ((pid >= 12) && succMCS <=7)
+				{
+					reTry -= 4;
+				}
+				pEntry->OneSecTxRetryOkCount += reTry;
+			}
+
+			i++;
+			// ASIC store 16 stack
+		} while ( i < (2*TX_RING_SIZE) );
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read statistical counters from hardware registers and record them
+		in software variables for later on query
+
+	Arguments:
+		pAd					Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID NICUpdateRawCounters(
+	IN PRTMP_ADAPTER pAd)
+{
+	UINT32	OldValue;
+	RX_STA_CNT0_STRUC	 RxStaCnt0;
+	RX_STA_CNT1_STRUC   RxStaCnt1;
+	RX_STA_CNT2_STRUC   RxStaCnt2;
+	TX_STA_CNT0_STRUC 	 TxStaCnt0;
+	TX_STA_CNT1_STRUC	 StaTx1;
+	TX_STA_CNT2_STRUC	 StaTx2;
+	TX_AGG_CNT_STRUC	TxAggCnt;
+	TX_AGG_CNT0_STRUC	TxAggCnt0;
+	TX_AGG_CNT1_STRUC	TxAggCnt1;
+	TX_AGG_CNT2_STRUC	TxAggCnt2;
+	TX_AGG_CNT3_STRUC	TxAggCnt3;
+	TX_AGG_CNT4_STRUC	TxAggCnt4;
+	TX_AGG_CNT5_STRUC	TxAggCnt5;
+	TX_AGG_CNT6_STRUC	TxAggCnt6;
+	TX_AGG_CNT7_STRUC	TxAggCnt7;
+
+
+	RTMP_IO_READ32(pAd, RX_STA_CNT0, &RxStaCnt0.word);
+	RTMP_IO_READ32(pAd, RX_STA_CNT2, &RxStaCnt2.word);
+
+	{
+		RTMP_IO_READ32(pAd, RX_STA_CNT1, &RxStaCnt1.word);
+	    // Update RX PLCP error counter
+	    pAd->PrivateInfo.PhyRxErrCnt += RxStaCnt1.field.PlcpErr;
+		// Update False CCA counter
+		pAd->RalinkCounters.OneSecFalseCCACnt += RxStaCnt1.field.FalseCca;
+	}
+
+	// Update FCS counters
+	OldValue= pAd->WlanCounters.FCSErrorCount.u.LowPart;
+	pAd->WlanCounters.FCSErrorCount.u.LowPart += (RxStaCnt0.field.CrcErr); // >> 7);
+	if (pAd->WlanCounters.FCSErrorCount.u.LowPart < OldValue)
+		pAd->WlanCounters.FCSErrorCount.u.HighPart++;
+
+	// Add FCS error count to private counters
+	pAd->RalinkCounters.OneSecRxFcsErrCnt += RxStaCnt0.field.CrcErr;
+	OldValue = pAd->RalinkCounters.RealFcsErrCount.u.LowPart;
+	pAd->RalinkCounters.RealFcsErrCount.u.LowPart += RxStaCnt0.field.CrcErr;
+	if (pAd->RalinkCounters.RealFcsErrCount.u.LowPart < OldValue)
+		pAd->RalinkCounters.RealFcsErrCount.u.HighPart++;
+
+	// Update Duplicate Rcv check
+	pAd->RalinkCounters.DuplicateRcv += RxStaCnt2.field.RxDupliCount;
+	pAd->WlanCounters.FrameDuplicateCount.u.LowPart += RxStaCnt2.field.RxDupliCount;
+	// Update RX Overflow counter
+	pAd->Counters8023.RxNoBuffer += (RxStaCnt2.field.RxFifoOverflowCount);
+
+	//pAd->RalinkCounters.RxCount = 0;
+#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 (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED) ||
+	//	(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED) && (pAd->MacTab.Size != 1)))
+	if (!pAd->bUpdateBcnCntDone)
+	{
+	// Update BEACON sent count
+	RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+	RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+	RTMP_IO_READ32(pAd, TX_STA_CNT2, &StaTx2.word);
+	pAd->RalinkCounters.OneSecBeaconSentCnt += TxStaCnt0.field.TxBeaconCount;
+	pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+	pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+	pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+	pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+	pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+	pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+	}
+
+#if 0
+	Retry = StaTx1.field.TxRetransmit;
+	Fail = TxStaCnt0.field.TxFailCount;
+	TxErrorRatio = 0;
+	OneSecTransmitCount = pAd->WlanCounters.TransmittedFragmentCount.u.LowPart- pAd->WlanCounters.LastTransmittedFragmentCount.u.LowPart;
+	if ((OneSecTransmitCount+Retry + Fail) > 0)
+		TxErrorRatio = (( Retry + Fail) *100) / (OneSecTransmitCount+Retry + Fail);
+
+	if ((OneSecTransmitCount+Retry + Fail) > 0)
+		TxErrorRatio = (( Retry + Fail) *100) / (OneSecTransmitCount+Retry + Fail);
+	DBGPRINT(RT_DEBUG_INFO, ("TX ERROR Rate = %ld %%, Retry = %ld, Fail = %ld, Total = %ld  \n",TxErrorRatio, Retry, Fail, (OneSecTransmitCount+Retry + Fail)));
+	pAd->WlanCounters.LastTransmittedFragmentCount.u.LowPart = pAd->WlanCounters.TransmittedFragmentCount.u.LowPart;
+#endif
+
+	//if (pAd->bStaFifoTest == TRUE)
+	{
+		RTMP_IO_READ32(pAd, TX_AGG_CNT, &TxAggCnt.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT0, &TxAggCnt0.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT1, &TxAggCnt1.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT2, &TxAggCnt2.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT3, &TxAggCnt3.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT4, &TxAggCnt4.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT5, &TxAggCnt5.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT6, &TxAggCnt6.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT7, &TxAggCnt7.word);
+		pAd->RalinkCounters.TxAggCount += TxAggCnt.field.AggTxCount;
+		pAd->RalinkCounters.TxNonAggCount += TxAggCnt.field.NonAggTxCount;
+		pAd->RalinkCounters.TxAgg1MPDUCount += TxAggCnt0.field.AggSize1Count;
+		pAd->RalinkCounters.TxAgg2MPDUCount += TxAggCnt0.field.AggSize2Count;
+
+		pAd->RalinkCounters.TxAgg3MPDUCount += TxAggCnt1.field.AggSize3Count;
+		pAd->RalinkCounters.TxAgg4MPDUCount += TxAggCnt1.field.AggSize4Count;
+		pAd->RalinkCounters.TxAgg5MPDUCount += TxAggCnt2.field.AggSize5Count;
+		pAd->RalinkCounters.TxAgg6MPDUCount += TxAggCnt2.field.AggSize6Count;
+
+		pAd->RalinkCounters.TxAgg7MPDUCount += TxAggCnt3.field.AggSize7Count;
+		pAd->RalinkCounters.TxAgg8MPDUCount += TxAggCnt3.field.AggSize8Count;
+		pAd->RalinkCounters.TxAgg9MPDUCount += TxAggCnt4.field.AggSize9Count;
+		pAd->RalinkCounters.TxAgg10MPDUCount += TxAggCnt4.field.AggSize10Count;
+
+		pAd->RalinkCounters.TxAgg11MPDUCount += TxAggCnt5.field.AggSize11Count;
+		pAd->RalinkCounters.TxAgg12MPDUCount += TxAggCnt5.field.AggSize12Count;
+		pAd->RalinkCounters.TxAgg13MPDUCount += TxAggCnt6.field.AggSize13Count;
+		pAd->RalinkCounters.TxAgg14MPDUCount += TxAggCnt6.field.AggSize14Count;
+
+		pAd->RalinkCounters.TxAgg15MPDUCount += TxAggCnt7.field.AggSize15Count;
+		pAd->RalinkCounters.TxAgg16MPDUCount += TxAggCnt7.field.AggSize16Count;
+
+		// Calculate the transmitted A-MPDU count
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += TxAggCnt0.field.AggSize1Count;
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt0.field.AggSize2Count / 2);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt1.field.AggSize3Count / 3);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt1.field.AggSize4Count / 4);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt2.field.AggSize5Count / 5);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt2.field.AggSize6Count / 6);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt3.field.AggSize7Count / 7);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt3.field.AggSize8Count / 8);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt4.field.AggSize9Count / 9);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt4.field.AggSize10Count / 10);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt5.field.AggSize11Count / 11);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt5.field.AggSize12Count / 12);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt6.field.AggSize13Count / 13);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt6.field.AggSize14Count / 14);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt7.field.AggSize15Count / 15);
+		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 < 16; i++)
+			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 //
+
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Reset NIC from error
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Reset NIC from error state
+
+	========================================================================
+*/
+VOID	NICResetFromError(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	// Reset BBP (according to alex, reset ASIC will force reset BBP
+	// Therefore, skip the reset BBP
+	// RTMP_IO_WRITE32(pAd, MAC_CSR1, 0x2);
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+	// Remove ASIC from reset state
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x0);
+
+	NICInitializeAdapter(pAd, FALSE);
+	NICInitAsicFromEEPROM(pAd);
+
+	// Switch to current channel, since during reset process, the connection should remains on.
+	AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		erase 8051 firmware image in MAC ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID NICEraseFirmware(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG i;
+
+	for(i=0; i<MAX_FIRMWARE_IMAGE_SIZE; i+=4)
+		RTMP_IO_WRITE32(pAd, FIRMWARE_IMAGE_BASE + i, 0);
+
+}/* End of NICEraseFirmware */
+
+/*
+	========================================================================
+
+	Routine Description:
+		Load 8051 firmware RT2561.BIN file into MAC ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS         firmware image load ok
+		NDIS_STATUS_FAILURE         image not found
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+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", __FUNCTION__);
+
+	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", __FUNCTION__));
+
+	/* 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", __FUNCTION__);
+		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",
+					   __FUNCTION__, -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", __FUNCTION__, 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",
+					   __FUNCTION__, 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(BitReverse(*ptr), crc);
+				/* End of for */
+
+				if ((pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-2] != \
+								(UCHAR)BitReverse((UCHAR)(crc>>8))) ||
+					(pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-1] != \
+								(UCHAR)BitReverse((UCHAR)crc)))
+				{
+					/* CRC fail */
+					printk("%s: CRC = 0x%02x 0x%02x "
+						   "error, should be 0x%02x 0x%02x\n",
+						   __FUNCTION__,
+						   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", __FUNCTION__);
+						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 //
+
+#if 0
+	/* enable Host program ram write selection */
+	RT28XX_FIRMUD_INIT(pAd);
+
+	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 */
+
+	RT28XX_FIRMUD_END(pAd);
+#else
+	RT28XX_WRITE_FIRMWARE(pAd, pFirmwareImage, FileLength);
+#endif
+
+#endif
+
+	/* check if MCU is ready */
+	Index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &MacReg);
+
+		if (MacReg & 0x80)
+			break;
+
+		RTMPusecDelay(1000);
+	} while (Index++ < 1000);
+
+    if (Index >= 1000)
+	{
+		Status = NDIS_STATUS_FAILURE;
+		DBGPRINT(RT_DEBUG_ERROR, ("NICLoadFirmware: MCU is not ready\n\n\n"));
+	} /* End of if */
+
+#if 0
+    DBGPRINT(RT_DEBUG_TRACE,
+			 ("<=== %s (src=%s, status=%d)\n", __FUNCTION__, src, Status));
+#else
+    DBGPRINT(RT_DEBUG_TRACE,
+			 ("<=== %s (status=%d)\n", __FUNCTION__, Status));
+#endif
+    return Status;
+} /* End of NICLoadFirmware */
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Load Tx rate switching parameters
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS         firmware image load ok
+		NDIS_STATUS_FAILURE         image not found
+
+	IRQL = PASSIVE_LEVEL
+
+	Rate Table Format:
+		1. (B0: Valid Item number) (B1:Initial item from zero)
+		2. Item Number(Dec)      Mode(Hex)     Current MCS(Dec)    TrainUp(Dec)    TrainDown(Dec)
+
+	========================================================================
+*/
+NDIS_STATUS NICLoadRateSwitchingParams(
+	IN PRTMP_ADAPTER pAd)
+{
+#if 0
+	NDIS_STATUS Status;
+
+	NDIS_HANDLE FileHandle;
+	UINT FileLength = 0, i, j;
+	PUCHAR pFirmwareImage;
+	NDIS_STRING FileName;
+	NDIS_PHYSICAL_ADDRESS HighestAcceptableMax = NDIS_PHYSICAL_ADDRESS_CONST(-1, -1);
+
+	DBGPRINT(RT_DEBUG_TRACE,("===> NICLoadRateSwitchingParams \n"));
+	pAd->CommonCfg.TxRateTableSize = 0;
+
+	if ((pAd->DeviceID == NIC2860_PCI_DEVICE_ID) || (pAd->DeviceID == NIC2860_PCIe_DEVICE_ID))
+	{
+		NdisInitializeString(&FileName,"rate.bin");
+		DBGPRINT(RT_DEBUG_TRACE, ("NICLoadRateSwitchingParams: load file - rate.bin for tx rate switch \n"));
+	}
+	else
+	{
+		DBGPRINT_ERR(("NICLoadRateSwitchingParams: wrong DeviceID = 0x%04x, can't find Tx rate switch parameters file\n", pAd->DeviceID));
+		return NDIS_STATUS_SUCCESS;
+	}
+	NdisOpenFile(&Status, &FileHandle, &FileLength, &FileName, HighestAcceptableMax);
+	NdisFreeString(FileName);
+
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("NICLoadRateSwitchingParams: NdisOpenFile() failed, used RateSwitchTable instead\n"));
+		return NDIS_STATUS_SUCCESS;
+	}
+
+	if ((FileLength == 0) || (FileLength > (MAX_STEP_OF_TX_RATE_SWITCH+1)*16))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("NICLoadRateSwitchingParams: file size is not reasonable, used RateSwitchTable instead\n"));
+
+		NdisCloseFile(FileHandle);
+		return NDIS_STATUS_SUCCESS;
+	}
+	else
+	{
+		//
+		// NDIS_STATUS_SUCCESS means
+		// The handle at FileHandle is valid for a subsequent call to NdisMapFile.
+		//
+		NdisMapFile(&Status, &pFirmwareImage, FileHandle);
+		DBGPRINT(RT_DEBUG_TRACE, ("NdisMapFile FileLength=%d\n", FileLength));
+	}
+
+	for (i=0, j=0; i<FileLength; i++)
+	{
+		if ((i%16) <= 4)	// trim reserved field
+		{
+			if (i%16 == 1)	// deal with DEC and HEX, only row0 is Hex, others are Dec
+			{
+				RateSwitchTable[j] = *(pFirmwareImage + i);
+			}
+			else
+			{
+				RateSwitchTable[j] = (*(pFirmwareImage + i)>>4) * 10 + (*(pFirmwareImage + i) & 0x0F);
+			}
+
+			j++;
+		}
+	}
+
+	pAd->CommonCfg.TxRateTableSize = RateSwitchTable[0];		// backup table size
+
+	if (Status == NDIS_STATUS_SUCCESS)
+	{
+		NdisUnmapFile(FileHandle);
+		NdisCloseFile(FileHandle);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("<=== NICLoadRateSwitchingParams(Valid TxRateTable item number=%d)\n", pAd->CommonCfg.TxRateTableSize));
+#endif
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		if  pSrc1 all zero with length Length, return 0.
+		If not all zero, return 1
+
+	Arguments:
+		pSrc1
+
+	Return Value:
+		1:			not all zero
+		0:			all zero
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPNotAllZero(
+	IN	PVOID	pSrc1,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	ULONG	Index = 0;
+
+	pMem1 = (PUCHAR) pSrc1;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		if (pMem1[Index] != 0x0)
+		{
+			break;
+		}
+	}
+
+	if (Index == Length)
+	{
+		return (0);
+	}
+	else
+	{
+		return (1);
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Compare two memory block
+
+	Arguments:
+		pSrc1		Pointer to first memory address
+		pSrc2		Pointer to second memory address
+
+	Return Value:
+		0:			memory is equal
+		1:			pSrc1 memory is larger
+		2:			pSrc2 memory is larger
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPCompareMemory(
+	IN	PVOID	pSrc1,
+	IN	PVOID	pSrc2,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	PUCHAR	pMem2;
+	ULONG	Index = 0;
+
+	pMem1 = (PUCHAR) pSrc1;
+	pMem2 = (PUCHAR) pSrc2;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		if (pMem1[Index] > pMem2[Index])
+			return (1);
+		else if (pMem1[Index] < pMem2[Index])
+			return (2);
+	}
+
+	// Equal
+	return (0);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Zero out memory block
+
+	Arguments:
+		pSrc1		Pointer to memory address
+		Length		Size
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPZeroMemory(
+	IN	PVOID	pSrc,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem;
+	ULONG	Index = 0;
+
+	pMem = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem[Index] = 0x00;
+	}
+}
+
+VOID	RTMPFillMemory(
+	IN	PVOID	pSrc,
+	IN	ULONG	Length,
+	IN	UCHAR	Fill)
+{
+	PUCHAR	pMem;
+	ULONG	Index = 0;
+
+	pMem = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem[Index] = Fill;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy data from memory block 1 to memory block 2
+
+	Arguments:
+		pDest		Pointer to destination memory address
+		pSrc		Pointer to source memory address
+		Length		Copy size
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPMoveMemory(
+	OUT	PVOID	pDest,
+	IN	PVOID	pSrc,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	PUCHAR	pMem2;
+	UINT	Index;
+
+	ASSERT((Length==0) || (pDest && pSrc));
+
+	pMem1 = (PUCHAR) pDest;
+	pMem2 = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem1[Index] = pMem2[Index];
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize port configuration structure
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	UserCfgInit(
+	IN	PRTMP_ADAPTER pAd)
+{
+//	EDCA_PARM DefaultEdcaParm;
+    UINT key_index, bss_index;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> UserCfgInit\n"));
+
+	//
+	//  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++)
+	{
+		for(bss_index = 0; bss_index < MAX_MBSSID_NUM; bss_index++)
+		{
+			pAd->SharedKey[bss_index][key_index].KeyLen = 0;
+			pAd->SharedKey[bss_index][key_index].CipherAlg = CIPHER_NONE;
+		}
+	}
+
+	pAd->Antenna.word = 0;
+	pAd->CommonCfg.BBPCurrentBW = BW_20;
+
+	pAd->LedCntl.word = 0;
+
+	pAd->bAutoTxAgcA = FALSE;			// Default is OFF
+	pAd->bAutoTxAgcG = FALSE;			// Default is OFF
+	pAd->RfIcType = RFIC_2820;
+
+	// Init timer for reset complete event
+	pAd->CommonCfg.CentralChannel = 1;
+	pAd->bForcePrintTX = FALSE;
+	pAd->bForcePrintRX = FALSE;
+	pAd->bStaFifoTest = FALSE;
+	pAd->bProtectionTest = FALSE;
+	pAd->bHCCATest = FALSE;
+	pAd->bGenOneHCCA = FALSE;
+	pAd->CommonCfg.Dsifs = 10;      // in units of usec
+	pAd->CommonCfg.TxPower = 100; //mW
+	pAd->CommonCfg.TxPowerPercentage = 0xffffffff; // AUTO
+	pAd->CommonCfg.TxPowerDefault = 0xffffffff; // AUTO
+	pAd->CommonCfg.TxPreamble = Rt802_11PreambleAuto; // use Long preamble on TX by defaut
+	pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+	pAd->CommonCfg.RtsThreshold = 2347;
+	pAd->CommonCfg.FragmentThreshold = 2346;
+	pAd->CommonCfg.UseBGProtection = 0;    // 0: AUTO
+	pAd->CommonCfg.bEnableTxBurst = TRUE; //0;
+	pAd->CommonCfg.PhyMode = 0xff;     // unknown
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+	pAd->CommonCfg.RadarDetect.CSPeriod = 10;
+	pAd->CommonCfg.RadarDetect.CSCount = 0;
+	pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+	pAd->CommonCfg.RadarDetect.ChMovingTime = 65;
+	pAd->CommonCfg.RadarDetect.LongPulseRadarTh = 3;
+	pAd->CommonCfg.bAPSDCapable = FALSE;
+	pAd->CommonCfg.bNeedSendTriggerFrame = FALSE;
+	pAd->CommonCfg.TriggerTimerCount = 0;
+	pAd->CommonCfg.bAPSDForcePowerSave = FALSE;
+	pAd->CommonCfg.bCountryFlag = FALSE;
+	pAd->CommonCfg.TxStream = 0;
+	pAd->CommonCfg.RxStream = 0;
+
+	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;
+	pAd->CommonCfg.BACapability.field.Policy = IMMED_BA;
+	pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64; //32;
+	pAd->CommonCfg.BACapability.field.TxBAWinLimit = 64; //32;
+	DBGPRINT(RT_DEBUG_TRACE, ("--> UserCfgInit. BACapability = 0x%x\n", pAd->CommonCfg.BACapability.word));
+
+	pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+	BATableInit(pAd, &pAd->BATable);
+
+	pAd->CommonCfg.bExtChannelSwitchAnnouncement = 1;
+	pAd->CommonCfg.bHTProtect = 1;
+	pAd->CommonCfg.bMIMOPSEnable = TRUE;
+	pAd->CommonCfg.bBADecline = FALSE;
+	pAd->CommonCfg.bDisableReordering = FALSE;
+
+	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;
+	//pAd->CommonCfg.HTPhyMode.field.ShortGI = GI_800;
+	//pAd->CommonCfg.HTPhyMode.field.STBC = STBC_NONE;
+	pAd->CommonCfg.TxRate = RATE_6;
+
+	pAd->CommonCfg.MlmeTransmit.field.MCS = MCS_RATE_6;
+	pAd->CommonCfg.MlmeTransmit.field.BW = BW_20;
+	pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+
+	pAd->CommonCfg.BeaconPeriod = 100;     // in mSec
+
+	//
+	// 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);
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_BROADCAST);
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_ALL_MULTICAST);
+
+		pAd->StaCfg.Psm = PWR_ACTIVE;
+
+		pAd->StaCfg.OrigWepStatus = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.PairCipher = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.GroupCipher = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.bMixCipher = FALSE;
+		pAd->StaCfg.DefaultKeyId = 0;
+
+		// 802.1x port control
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+		pAd->StaCfg.LastMicErrorTime = 0;
+		pAd->StaCfg.MicErrCnt        = 0;
+		pAd->StaCfg.bBlockAssoc      = FALSE;
+		pAd->StaCfg.WpaState         = SS_NOTUSE;
+
+		pAd->CommonCfg.NdisRadioStateOff = FALSE;		// New to support microsoft disable radio with OID command
+
+		pAd->StaCfg.RssiTrigger = 0;
+		NdisZeroMemory(&pAd->StaCfg.RssiSample, sizeof(RSSI_SAMPLE));
+		pAd->StaCfg.RssiTriggerMode = RSSI_TRIGGERED_UPON_BELOW_THRESHOLD;
+		pAd->StaCfg.AtimWin = 0;
+		pAd->StaCfg.DefaultListenCount = 3;//default listen count;
+		pAd->StaCfg.BssType = BSS_INFRA;  // BSS_INFRA or BSS_ADHOC or BSS_MONITOR
+		pAd->StaCfg.bScanReqIsFromWebUI = FALSE;
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_WAKEUP_NOW);
+
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		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);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+
+	// PHY specification
+	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;
+
+		// Patch for Ndtest
+		pAd->StaCfg.ScanCnt = 0;
+
+		// CCX 2.0 control flag init
+		pAd->StaCfg.CCXEnable = FALSE;
+		pAd->StaCfg.CCXReqType = MSRN_TYPE_UNUSED;
+		pAd->StaCfg.CCXQosECWMin	= 4;
+		pAd->StaCfg.CCXQosECWMax	= 10;
+
+		pAd->StaCfg.bHwRadio  = TRUE; // Default Hardware Radio status is On
+		pAd->StaCfg.bSwRadio  = TRUE; // Default Software Radio status is On
+		pAd->StaCfg.bRadio    = TRUE; // bHwRadio && bSwRadio
+		pAd->StaCfg.bHardwareRadio = FALSE;		// Default is OFF
+		pAd->StaCfg.bShowHiddenSSID = FALSE;		// Default no show
+
+		// Nitro mode control
+		pAd->StaCfg.bAutoReconnect = TRUE;
+
+		// Save the init time as last scan time, the system should do scan after 2 seconds.
+		// This patch is for driver wake up from standby mode, system will do scan right away.
+		pAd->StaCfg.LastScanTime = 0;
+		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;
+
+	// Default Config change flag
+	pAd->bConfigChanged = FALSE;
+
+	//
+	// part III. AP configurations
+	//
+
+
+	//
+	// part IV. others
+	//
+	// dynamic BBP R66:sensibity tuning to overcome background noise
+	pAd->BbpTuning.bEnable                = TRUE;
+	pAd->BbpTuning.FalseCcaLowerThreshold = 100;
+	pAd->BbpTuning.FalseCcaUpperThreshold = 512;
+	pAd->BbpTuning.R66Delta               = 4;
+	pAd->Mlme.bEnableAutoAntennaCheck = TRUE;
+
+	//
+	// Also initial R66CurrentValue, RTUSBResumeMsduTransmission might use this value.
+	// if not initial this value, the default value will be 0.
+	//
+	pAd->BbpTuning.R66CurrentValue = 0x38;
+
+	pAd->Bbp94 = BBPR94_DEFAULT;
+	pAd->BbpForCCK = FALSE;
+
+	// Default is FALSE for test bit 1
+	//pAd->bTest1 = FALSE;
+
+	// initialize MAC table and allocate spin lock
+	NdisZeroMemory(&pAd->MacTab, sizeof(MAC_TABLE));
+	InitializeQueueHeader(&pAd->MacTab.McastPsQueue);
+	NdisAllocateSpinLock(&pAd->MacTabLock);
+
+	//RTMPInitTimer(pAd, &pAd->RECBATimer, RECBATimerTimeout, pAd, TRUE);
+	//RTMPSetTimer(&pAd->RECBATimer, REORDER_EXEC_INTV);
+
+#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 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;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- UserCfgInit\n"));
+}
+
+// IRQL = PASSIVE_LEVEL
+UCHAR BtoH(char ch)
+{
+	if (ch >= '0' && ch <= '9') return (ch - '0');        // Handle numerals
+	if (ch >= 'A' && ch <= 'F') return (ch - 'A' + 0xA);  // Handle capitol hex digits
+	if (ch >= 'a' && ch <= 'f') return (ch - 'a' + 0xA);  // Handle small hex digits
+	return(255);
+}
+
+//
+//  FUNCTION: AtoH(char *, UCHAR *, int)
+//
+//  PURPOSE:  Converts ascii string to network order hex
+//
+//  PARAMETERS:
+//    src    - pointer to input ascii string
+//    dest   - pointer to output hex
+//    destlen - size of dest
+//
+//  COMMENTS:
+//
+//    2 ascii bytes make a hex byte so must put 1st ascii byte of pair
+//    into upper nibble and 2nd ascii byte of pair into lower nibble.
+//
+// IRQL = PASSIVE_LEVEL
+
+void AtoH(char * src, UCHAR * dest, int destlen)
+{
+	char * srcptr;
+	PUCHAR destTemp;
+
+	srcptr = src;
+	destTemp = (PUCHAR) dest;
+
+	while(destlen--)
+	{
+		*destTemp = BtoH(*srcptr++) << 4;    // Put 1st ascii byte in upper nibble.
+		*destTemp += BtoH(*srcptr++);      // Add 2nd ascii byte to above.
+		destTemp++;
+	}
+}
+
+VOID	RTMPPatchMacBbpBug(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	ULONG	Index;
+
+	// Initialize BBP register to default value
+	for (Index = 0; Index < NUM_BBP_REG_PARMS; Index++)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBPRegTable[Index].Register, (UCHAR)BBPRegTable[Index].Value);
+	}
+
+	// Initialize RF register to default value
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// Re-init BBP register from EEPROM value
+	NICInitAsicFromEEPROM(pAd);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pAd			Pointer to our adapter
+		pTimer				Timer structure
+		pTimerFunc			Function to execute when timer expired
+		Repeat				Ture for period timer
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitTimer(
+	IN	PRTMP_ADAPTER			pAd,
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	PVOID					pTimerFunc,
+	IN	PVOID					pData,
+	IN	BOOLEAN					Repeat)
+{
+	//
+	// Set Valid to TRUE for later used.
+	// It will crash if we cancel a timer or set a timer
+	// that we haven't initialize before.
+	//
+	pTimer->Valid      = TRUE;
+
+	pTimer->PeriodicType = Repeat;
+	pTimer->State      = FALSE;
+	pTimer->cookie = (ULONG) pData;
+
+#ifdef RT2870
+	pTimer->pAd = pAd;
+#endif // RT2870 //
+
+	RTMP_OS_Init_Timer(pAd,	&pTimer->TimerObj,	pTimerFunc, (PVOID) pTimer);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pTimer				Timer structure
+		Value				Timer value in milliseconds
+
+	Return Value:
+		None
+
+	Note:
+		To use this routine, must call RTMPInitTimer before.
+
+	========================================================================
+*/
+VOID	RTMPSetTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value)
+{
+	if (pTimer->Valid)
+	{
+		pTimer->TimerValue = Value;
+		pTimer->State      = FALSE;
+		if (pTimer->PeriodicType == TRUE)
+		{
+			pTimer->Repeat = TRUE;
+			RTMP_SetPeriodicTimer(&pTimer->TimerObj, Value);
+		}
+		else
+		{
+			pTimer->Repeat = FALSE;
+			RTMP_OS_Add_Timer(&pTimer->TimerObj, Value);
+		}
+	}
+	else
+	{
+		DBGPRINT_ERR(("RTMPSetTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pTimer				Timer structure
+		Value				Timer value in milliseconds
+
+	Return Value:
+		None
+
+	Note:
+		To use this routine, must call RTMPInitTimer before.
+
+	========================================================================
+*/
+VOID	RTMPModTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value)
+{
+	BOOLEAN	Cancel;
+
+	if (pTimer->Valid)
+	{
+		pTimer->TimerValue = Value;
+		pTimer->State      = FALSE;
+		if (pTimer->PeriodicType == TRUE)
+		{
+			RTMPCancelTimer(pTimer, &Cancel);
+			RTMPSetTimer(pTimer, Value);
+		}
+		else
+		{
+			RTMP_OS_Mod_Timer(&pTimer->TimerObj, Value);
+		}
+	}
+	else
+	{
+		DBGPRINT_ERR(("RTMPModTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Cancel timer objects
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		1.) To use this routine, must call RTMPInitTimer before.
+		2.) Reset NIC to initial state AS IS system boot up time.
+
+	========================================================================
+*/
+VOID	RTMPCancelTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	OUT	BOOLEAN					*pCancelled)
+{
+	if (pTimer->Valid)
+	{
+		if (pTimer->State == FALSE)
+			pTimer->Repeat = FALSE;
+			RTMP_OS_Del_Timer(&pTimer->TimerObj, pCancelled);
+
+		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
+	{
+		//
+		// NdisMCancelTimer just canced the timer and not mean release the timer.
+		// And don't set the "Valid" to False. So that we can use this timer again.
+		//
+		DBGPRINT_ERR(("RTMPCancelTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set LED Status
+
+	Arguments:
+		pAd						Pointer to our adapter
+		Status					LED Status
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPSetLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN UCHAR			Status)
+{
+	//ULONG			data;
+	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)
+	{
+		case LED_LINK_DOWN:
+			HighByte = 0x20;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			pAd->LedIndicatorStregth = 0;
+			break;
+		case LED_LINK_UP:
+			if (pAd->CommonCfg.Channel > 14)
+				HighByte = 0xa0;
+			else
+				HighByte = 0x60;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_RADIO_ON:
+			HighByte = 0x20;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_HALT:
+			LowByte = 0; // Driver sets MAC register and MAC controls LED
+		case LED_RADIO_OFF:
+			HighByte = 0;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+        case LED_WPS:
+			HighByte = 0x10;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_ON_SITE_SURVEY:
+			HighByte = 0x08;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_POWER_UP:
+			HighByte = 0x04;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_WARN, ("RTMPSetLED::Unknown Status %d\n", Status));
+			break;
+	}
+
+    //
+	// Keep LED status for LED SiteSurvey mode.
+	// After SiteSurvey, we will set the LED mode to previous status.
+	//
+	if ((Status != LED_ON_SITE_SURVEY) && (Status != LED_POWER_UP))
+		pAd->LedStatus = Status;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetLED::Mode=%d,HighByte=0x%02x,LowByte=0x%02x\n", pAd->LedCntl.field.LedMode, HighByte, LowByte));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set LED Signal Stregth
+
+	Arguments:
+		pAd						Pointer to our adapter
+		Dbm						Signal Stregth
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Can be run on any IRQL level.
+
+		According to Microsoft Zero Config Wireless Signal Stregth definition as belows.
+		<= -90  No Signal
+		<= -81  Very Low
+		<= -71  Low
+		<= -67  Good
+		<= -57  Very Good
+		 > -57  Excellent
+	========================================================================
+*/
+VOID RTMPSetSignalLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN NDIS_802_11_RSSI Dbm)
+{
+	UCHAR		nLed = 0;
+
+	//
+	// if not Signal Stregth, then do nothing.
+	//
+	if (pAd->LedCntl.field.LedMode != LED_MODE_SIGNAL_STREGTH)
+	{
+		return;
+	}
+
+	if (Dbm <= -90)
+		nLed = 0;
+	else if (Dbm <= -81)
+		nLed = 1;
+	else if (Dbm <= -71)
+		nLed = 3;
+	else if (Dbm <= -67)
+		nLed = 7;
+	else if (Dbm <= -57)
+		nLed = 15;
+	else
+		nLed = 31;
+
+	//
+	// Update Signal Stregth to firmware if changed.
+	//
+	if (pAd->LedIndicatorStregth != nLed)
+	{
+		AsicSendCommandToMcu(pAd, 0x51, 0xff, nLed, pAd->LedCntl.field.Polarity);
+		pAd->LedIndicatorStregth = nLed;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Enable RX
+
+	Arguments:
+		pAd						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL <= DISPATCH_LEVEL
+
+	Note:
+		Before Enable RX, make sure you have enabled Interrupt.
+	========================================================================
+*/
+VOID RTMPEnableRxTx(
+	IN PRTMP_ADAPTER	pAd)
+{
+//	WPDMA_GLO_CFG_STRUC	GloCfg;
+//	ULONG	i = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> RTMPEnableRxTx\n"));
+
+#if 0
+	// Enable Rx DMA.
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x4);
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("==>  DMABusy\n"));
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i <200);
+
+	RTMPusecDelay(50);
+	RT28XX_DMA_WRITE_INIT(GloCfg);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== WRITE DMA offset 0x208 = 0x%x\n", GloCfg.word));
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	RT28XX_DMA_POST_WRITE(pAd);
+#else
+	// Enable Rx DMA.
+	RT28XXDMAEnable(pAd);
+#endif
+
+	// enable RX of MAC block
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		UINT32 rx_filter_flag = APNORMAL;
+
+
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, rx_filter_flag);     // enable RX of DMA block
+	}
+	else
+	{
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL);     // Staion not drop control frame will fail WiFi Certification.
+	}
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0xc);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== RTMPEnableRxTx\n"));
+}
+
+
diff --git a/drivers/staging/rt2870/common/rtmp_tkip.c b/drivers/staging/rt2870/common/rtmp_tkip.c
new file mode 100644
index 00000000000..2847409ff08
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtmp_tkip.c
@@ -0,0 +1,1613 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_tkip.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Wu		02-25-02		Initial
+*/
+
+#include "../rt_config.h"
+
+// Rotation functions on 32 bit values
+#define ROL32( A, n ) \
+	( ((A) << (n)) | ( ((A)>>(32-(n))) & ( (1UL << (n)) - 1 ) ) )
+#define ROR32( A, n ) ROL32( (A), 32-(n) )
+
+UINT Tkip_Sbox_Lower[256] =
+{
+	0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
+	0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
+	0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
+	0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
+	0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
+	0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
+	0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
+	0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
+	0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
+	0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
+	0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
+	0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
+	0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
+	0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
+	0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
+	0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
+	0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
+	0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
+	0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
+	0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
+	0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
+	0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
+	0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
+	0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
+	0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
+	0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
+	0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
+	0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
+	0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
+	0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
+	0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
+	0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
+};
+
+UINT Tkip_Sbox_Upper[256] =
+{
+	0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
+	0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
+	0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
+	0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
+	0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
+	0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
+	0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
+	0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
+	0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
+	0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
+	0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
+	0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
+	0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
+	0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
+	0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
+	0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
+	0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
+	0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
+	0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
+	0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
+	0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
+	0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
+	0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
+	0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
+	0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
+	0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
+	0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
+	0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
+	0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
+	0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
+	0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
+	0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
+};
+
+/*****************************/
+/******** SBOX Table *********/
+/*****************************/
+
+UCHAR SboxTable[256] =
+{
+	0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
+	0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
+	0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
+	0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
+	0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
+	0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
+	0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
+	0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
+	0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
+	0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
+	0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
+	0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
+	0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
+	0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
+	0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
+	0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
+	0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
+	0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
+	0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
+	0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
+	0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
+	0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
+	0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
+	0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
+	0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
+	0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
+	0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
+	0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
+	0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
+	0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
+	0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
+	0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
+};
+
+VOID xor_32(
+	IN  PUCHAR              a,
+	IN  PUCHAR              b,
+	OUT PUCHAR              out);
+
+VOID xor_128(
+	IN  PUCHAR              a,
+	IN  PUCHAR              b,
+	OUT PUCHAR              out);
+
+VOID next_key(
+	IN  PUCHAR              key,
+	IN  INT                 round);
+
+VOID byte_sub(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+VOID shift_row(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+VOID mix_column(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+UCHAR RTMPCkipSbox(
+	IN  UCHAR               a);
+//
+// Expanded IV for TKIP function.
+//
+typedef	struct	PACKED _IV_CONTROL_
+{
+	union PACKED
+	{
+		struct PACKED
+		{
+			UCHAR		rc0;
+			UCHAR		rc1;
+			UCHAR		rc2;
+
+			union PACKED
+			{
+				struct PACKED
+				{
+#ifdef RT_BIG_ENDIAN
+					UCHAR	KeyID:2;
+					UCHAR	ExtIV:1;
+					UCHAR	Rsvd:5;
+#else
+					UCHAR	Rsvd:5;
+					UCHAR	ExtIV:1;
+					UCHAR	KeyID:2;
+#endif
+				}	field;
+				UCHAR		Byte;
+			}	CONTROL;
+		}	field;
+
+		ULONG	word;
+	}	IV16;
+
+	ULONG	IV32;
+}	TKIP_IV, *PTKIP_IV;
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Convert from UCHAR[] to ULONG in a portable way
+
+	Arguments:
+      pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPTkipGetUInt32(
+	IN	PUCHAR	pMICKey)
+{
+	ULONG	res = 0;
+	INT		i;
+
+	for (i = 0; i < 4; i++)
+	{
+		res |= (*pMICKey++) << (8 * i);
+	}
+
+	return res;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Convert from ULONG to UCHAR[] in a portable way
+
+	Arguments:
+      pDst			pointer to destination for convert ULONG to UCHAR[]
+      val			the value for convert
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipPutUInt32(
+	IN OUT	PUCHAR		pDst,
+	IN		ULONG		val)
+{
+	INT i;
+
+	for(i = 0; i < 4; i++)
+	{
+		*pDst++ = (UCHAR) (val & 0xff);
+		val >>= 8;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Set the MIC Key.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPTkipSetMICKey(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	PUCHAR			pMICKey)
+{
+	// Set the key
+	pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
+	pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
+	// and reset the message
+	pTkip->L = pTkip->K0;
+	pTkip->R = pTkip->K1;
+	pTkip->nBytesInM = 0;
+	pTkip->M = 0;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      uChar			Append this uChar
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipAppendByte(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	UCHAR 			uChar)
+{
+	// Append the byte to our word-sized buffer
+	pTkip->M |= (uChar << (8* pTkip->nBytesInM));
+	pTkip->nBytesInM++;
+	// Process the word if it is full.
+	if( pTkip->nBytesInM >= 4 )
+	{
+		pTkip->L ^= pTkip->M;
+		pTkip->R ^= ROL32( pTkip->L, 17 );
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ROL32( pTkip->L, 3 );
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ROR32( pTkip->L, 2 );
+		pTkip->L += pTkip->R;
+		// Clear the buffer
+		pTkip->M = 0;
+		pTkip->nBytesInM = 0;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pSrc			Pointer to source data for Calculate MIC Value
+      Len			Indicate the length of the source data
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipAppend(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	PUCHAR			pSrc,
+	IN	UINT			nBytes)
+{
+	// This is simple
+	while(nBytes > 0)
+	{
+		RTMPTkipAppendByte(pTkip, *pSrc++);
+		nBytes--;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		the MIC Value is store in pAd->PrivateInfo.MIC
+	========================================================================
+*/
+VOID	RTMPTkipGetMIC(
+	IN	PTKIP_KEY_INFO	pTkip)
+{
+	// Append the minimum padding
+	RTMPTkipAppendByte(pTkip, 0x5a );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	// and then zeroes until the length is a multiple of 4
+	while( pTkip->nBytesInM != 0 )
+	{
+		RTMPTkipAppendByte(pTkip, 0 );
+	}
+	// The appendByte function has already computed the result.
+	RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
+	RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init Tkip function.
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
+		KeyId		TK Key ID
+		pTA			Pointer to transmitter address
+		pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitTkipEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	UCHAR			KeyId,
+	IN	PUCHAR			pTA,
+	IN	PUCHAR			pMICKey,
+	IN	PUCHAR			pTSC,
+	OUT	PULONG			pIV16,
+	OUT	PULONG			pIV32)
+{
+	TKIP_IV	tkipIv;
+
+	// Prepare 8 bytes TKIP encapsulation for MPDU
+	NdisZeroMemory(&tkipIv, sizeof(TKIP_IV));
+	tkipIv.IV16.field.rc0 = *(pTSC + 1);
+	tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
+	tkipIv.IV16.field.rc2 = *pTSC;
+	tkipIv.IV16.field.CONTROL.field.ExtIV = 1;  // 0: non-extended IV, 1: an extended IV
+	tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
+//	tkipIv.IV32 = *(PULONG)(pTSC + 2);
+	NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4);   // Copy IV
+
+	*pIV16 = tkipIv.IV16.word;
+	*pIV32 = tkipIv.IV32;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init MIC Value calculation function which include set MIC key &
+		calculate first 16 bytes (DA + SA + priority +  0)
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitMICEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN  UCHAR           UserPriority,
+	IN	PUCHAR			pMICKey)
+{
+	ULONG Priority = UserPriority;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Compare MIC value of received MSDU
+
+	Arguments:
+		pAd	Pointer to our adapter
+		pSrc        Pointer to the received Plain text data
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+		Len         the length of the received plain text data exclude MIC value
+
+	Return Value:
+		TRUE        MIC value matched
+		FALSE       MIC value mismatched
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPTkipCompareMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN	PUCHAR			pMICKey,
+	IN	UCHAR			UserPriority,
+	IN	UINT			Len)
+{
+	UCHAR	OldMic[8];
+	ULONG	Priority = UserPriority;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
+
+	// Calculate MIC value from plain text data
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
+
+	// Get MIC valude from received frame
+	NdisMoveMemory(OldMic, pSrc + Len, 8);
+
+	// Get MIC value from decrypted plain data
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
+
+	// Move MIC value from MSDU, this steps should move to data path.
+	// Since the MIC value might cross MPDUs.
+	if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n"));  //MIC error.
+
+
+		return (FALSE);
+	}
+	return (TRUE);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Compare MIC value of received MSDU
+
+	Arguments:
+		pAd	Pointer to our adapter
+		pLLC		LLC header
+		pSrc        Pointer to the received Plain text data
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+		Len         the length of the received plain text data exclude MIC value
+
+	Return Value:
+		TRUE        MIC value matched
+		FALSE       MIC value mismatched
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPTkipCompareMICValueWithLLC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pLLC,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN	PUCHAR			pMICKey,
+	IN	UINT			Len)
+{
+	UCHAR	OldMic[8];
+	ULONG	Priority = 0;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
+
+	// Start with LLC header
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
+
+	// Calculate MIC value from plain text data
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
+
+	// Get MIC valude from received frame
+	NdisMoveMemory(OldMic, pSrc + Len, 8);
+
+	// Get MIC value from decrypted plain data
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
+
+	// Move MIC value from MSDU, this steps should move to data path.
+	// Since the MIC value might cross MPDUs.
+	if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n"));  //MIC error.
+
+
+		return (FALSE);
+	}
+	return (TRUE);
+}
+/*
+	========================================================================
+
+	Routine	Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware transmit function
+
+	Arguments:
+		pAd		Pointer	to our adapter
+		PNDIS_PACKET	Pointer to Ndis Packet for MIC calculation
+		pEncap			Pointer to LLC encap data
+		LenEncap		Total encap length, might be 0 which indicates no encap
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPCalculateMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pEncap,
+	IN	PCIPHER_KEY		pKey,
+	IN	UCHAR			apidx)
+{
+	PACKET_INFO		PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	PUCHAR			pSrc;
+    UCHAR           UserPriority;
+	UCHAR			vlan_offset = 0;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	UserPriority = RTMP_GET_PACKET_UP(pPacket);
+	pSrc = pSrcBufVA;
+
+	// determine if this is a vlan packet
+	if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
+		vlan_offset = 4;
+
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+	{
+		RTMPInitMICEngine(
+			pAd,
+			pKey->Key,
+			pSrc,
+			pSrc + 6,
+			UserPriority,
+			pKey->TxMic);
+	}
+
+
+	if (pEncap != NULL)
+	{
+		// LLC encapsulation
+		RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
+		// Protocol Type
+		RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
+	}
+	SrcBufLen -= (14 + vlan_offset);
+	pSrc += (14 + vlan_offset);
+	do
+	{
+		if (SrcBufLen > 0)
+		{
+			RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
+		}
+
+		break;	// No need handle next packet
+
+	}	while (TRUE);		// End of copying payload
+
+	// Compute the final MIC Value
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
+}
+
+
+/************************************************************/
+/* tkip_sbox()																*/
+/* Returns a 16 bit value from a 64K entry table. The Table */
+/* is synthesized from two 256 entry byte wide tables.		*/
+/************************************************************/
+
+UINT tkip_sbox(UINT index)
+{
+	UINT index_low;
+	UINT index_high;
+	UINT left, right;
+
+	index_low = (index % 256);
+	index_high = ((index >> 8) % 256);
+
+	left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
+	right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
+
+	return (left ^ right);
+}
+
+UINT rotr1(UINT a)
+{
+	unsigned int b;
+
+	if ((a & 0x01) == 0x01)
+	{
+		b = (a >> 1) | 0x8000;
+	}
+	else
+	{
+		b = (a >> 1) & 0x7fff;
+	}
+	b = b % 65536;
+	return b;
+}
+
+VOID RTMPTkipMixKey(
+	UCHAR *key,
+	UCHAR *ta,
+	ULONG pnl, /* Least significant 16 bits of PN */
+	ULONG pnh, /* Most significant 32 bits of PN */
+	UCHAR *rc4key,
+	UINT *p1k)
+{
+
+	UINT tsc0;
+	UINT tsc1;
+	UINT tsc2;
+
+	UINT ppk0;
+	UINT ppk1;
+	UINT ppk2;
+	UINT ppk3;
+	UINT ppk4;
+	UINT ppk5;
+
+	INT i;
+	INT j;
+
+	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
+	tsc1 = (unsigned int)(pnh % 65536);
+	tsc2 = (unsigned int)(pnl % 65536); /* lsb */
+
+	/* Phase 1, step 1 */
+	p1k[0] = tsc1;
+	p1k[1] = tsc0;
+	p1k[2] = (UINT)(ta[0] + (ta[1]*256));
+	p1k[3] = (UINT)(ta[2] + (ta[3]*256));
+	p1k[4] = (UINT)(ta[4] + (ta[5]*256));
+
+	/* Phase 1, step 2 */
+	for (i=0; i<8; i++)
+	{
+		j = 2*(i & 1);
+		p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
+		p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
+		p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
+		p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
+		p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
+		p1k[4] = (p1k[4] + i) % 65536;
+	}
+
+	/* Phase 2, Step 1 */
+	ppk0 = p1k[0];
+	ppk1 = p1k[1];
+	ppk2 = p1k[2];
+	ppk3 = p1k[3];
+	ppk4 = p1k[4];
+	ppk5 = (p1k[4] + tsc2) % 65536;
+
+	/* Phase2, Step 2 */
+	ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
+	ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
+	ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
+	ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
+	ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
+	ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
+
+	ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
+	ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
+	ppk2 = ppk2 + rotr1(ppk1);
+	ppk3 = ppk3 + rotr1(ppk2);
+	ppk4 = ppk4 + rotr1(ppk3);
+	ppk5 = ppk5 + rotr1(ppk4);
+
+	/* Phase 2, Step 3 */
+    /* Phase 2, Step 3 */
+
+	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
+	tsc1 = (unsigned int)(pnh % 65536);
+	tsc2 = (unsigned int)(pnl % 65536); /* lsb */
+
+	rc4key[0] = (tsc2 >> 8) % 256;
+	rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
+	rc4key[2] = tsc2 % 256;
+	rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
+
+	rc4key[4] = ppk0 % 256;
+	rc4key[5] = (ppk0 >> 8) % 256;
+
+	rc4key[6] = ppk1 % 256;
+	rc4key[7] = (ppk1 >> 8) % 256;
+
+	rc4key[8] = ppk2 % 256;
+	rc4key[9] = (ppk2 >> 8) % 256;
+
+	rc4key[10] = ppk3 % 256;
+	rc4key[11] = (ppk3 >> 8) % 256;
+
+	rc4key[12] = ppk4 % 256;
+	rc4key[13] = (ppk4 >> 8) % 256;
+
+	rc4key[14] = ppk5 % 256;
+	rc4key[15] = (ppk5 >> 8) % 256;
+}
+
+
+/************************************************/
+/* construct_mic_header1()                      */
+/* Builds the first MIC header block from       */
+/* header fields.                               */
+/************************************************/
+
+void construct_mic_header1(
+	unsigned char *mic_header1,
+	int header_length,
+	unsigned char *mpdu)
+{
+	mic_header1[0] = (unsigned char)((header_length - 2) / 256);
+	mic_header1[1] = (unsigned char)((header_length - 2) % 256);
+	mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
+	mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
+	mic_header1[4] = mpdu[4];       /* A1 */
+	mic_header1[5] = mpdu[5];
+	mic_header1[6] = mpdu[6];
+	mic_header1[7] = mpdu[7];
+	mic_header1[8] = mpdu[8];
+	mic_header1[9] = mpdu[9];
+	mic_header1[10] = mpdu[10];     /* A2 */
+	mic_header1[11] = mpdu[11];
+	mic_header1[12] = mpdu[12];
+	mic_header1[13] = mpdu[13];
+	mic_header1[14] = mpdu[14];
+	mic_header1[15] = mpdu[15];
+}
+
+/************************************************/
+/* construct_mic_header2()                      */
+/* Builds the last MIC header block from        */
+/* header fields.                               */
+/************************************************/
+
+void construct_mic_header2(
+	unsigned char *mic_header2,
+	unsigned char *mpdu,
+	int a4_exists,
+	int qc_exists)
+{
+	int i;
+
+	for (i = 0; i<16; i++) mic_header2[i]=0x00;
+
+	mic_header2[0] = mpdu[16];    /* A3 */
+	mic_header2[1] = mpdu[17];
+	mic_header2[2] = mpdu[18];
+	mic_header2[3] = mpdu[19];
+	mic_header2[4] = mpdu[20];
+	mic_header2[5] = mpdu[21];
+
+	// In Sequence Control field, mute sequence numer bits (12-bit)
+	mic_header2[6] = mpdu[22] & 0x0f;   /* SC */
+	mic_header2[7] = 0x00; /* mpdu[23]; */
+
+	if ((!qc_exists) & a4_exists)
+	{
+		for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+	}
+
+	if (qc_exists && (!a4_exists))
+	{
+		mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
+		mic_header2[9] = mpdu[25] & 0x00;
+	}
+
+	if (qc_exists && a4_exists)
+	{
+		for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+		mic_header2[14] = mpdu[30] & 0x0f;
+		mic_header2[15] = mpdu[31] & 0x00;
+	}
+}
+
+
+/************************************************/
+/* construct_mic_iv()                           */
+/* Builds the MIC IV from header fields and PN  */
+/************************************************/
+
+void construct_mic_iv(
+	unsigned char *mic_iv,
+	int qc_exists,
+	int a4_exists,
+	unsigned char *mpdu,
+	unsigned int payload_length,
+	unsigned char *pn_vector)
+{
+	int i;
+
+	mic_iv[0] = 0x59;
+	if (qc_exists && a4_exists)
+		mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
+	if (qc_exists && !a4_exists)
+		mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
+	if (!qc_exists)
+		mic_iv[1] = 0x00;
+	for (i = 2; i < 8; i++)
+		mic_iv[i] = mpdu[i + 8];                    /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+		for (i = 8; i < 14; i++)
+			mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
+#else
+		for (i = 8; i < 14; i++)
+			mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
+#endif
+	i = (payload_length / 256);
+	i = (payload_length % 256);
+	mic_iv[14] = (unsigned char) (payload_length / 256);
+	mic_iv[15] = (unsigned char) (payload_length % 256);
+
+}
+
+
+
+/************************************/
+/* bitwise_xor()                    */
+/* A 128 bit, bitwise exclusive or  */
+/************************************/
+
+void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
+{
+	int i;
+	for (i=0; i<16; i++)
+	{
+		out[i] = ina[i] ^ inb[i];
+	}
+}
+
+
+void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
+{
+	int round;
+	int i;
+	unsigned char intermediatea[16];
+	unsigned char intermediateb[16];
+	unsigned char round_key[16];
+
+	for(i=0; i<16; i++) round_key[i] = key[i];
+
+	for (round = 0; round < 11; round++)
+	{
+		if (round == 0)
+		{
+			xor_128(round_key, data, ciphertext);
+			next_key(round_key, round);
+		}
+		else if (round == 10)
+		{
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			xor_128(intermediateb, round_key, ciphertext);
+		}
+		else    /* 1 - 9 */
+		{
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			mix_column(&intermediateb[0], &intermediatea[0]);
+			mix_column(&intermediateb[4], &intermediatea[4]);
+			mix_column(&intermediateb[8], &intermediatea[8]);
+			mix_column(&intermediateb[12], &intermediatea[12]);
+			xor_128(intermediatea, round_key, ciphertext);
+			next_key(round_key, round);
+		}
+	}
+
+}
+
+void construct_ctr_preload(
+	unsigned char *ctr_preload,
+	int a4_exists,
+	int qc_exists,
+	unsigned char *mpdu,
+	unsigned char *pn_vector,
+	int c)
+{
+
+	int i = 0;
+	for (i=0; i<16; i++) ctr_preload[i] = 0x00;
+	i = 0;
+
+	ctr_preload[0] = 0x01;                                  /* flag */
+	if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control  */
+	if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
+
+	for (i = 2; i < 8; i++)
+		ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+	  for (i = 8; i < 14; i++)
+			ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
+#else
+	  for (i = 8; i < 14; i++)
+			ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
+#endif
+	ctr_preload[14] =  (unsigned char) (c / 256); // Ctr
+	ctr_preload[15] =  (unsigned char) (c % 256);
+
+}
+
+
+//
+// TRUE: Success!
+// FALSE: Decrypt Error!
+//
+BOOLEAN RTMPSoftDecryptTKIP(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN UCHAR    UserPriority,
+	IN PCIPHER_KEY	pWpaKey)
+{
+	UCHAR			KeyID;
+	UINT			HeaderLen;
+    UCHAR			fc0;
+	UCHAR			fc1;
+	USHORT			fc;
+	UINT			frame_type;
+	UINT			frame_subtype;
+    UINT			from_ds;
+    UINT			to_ds;
+	INT				a4_exists;
+	INT				qc_exists;
+	USHORT			duration;
+	USHORT			seq_control;
+	USHORT			qos_control;
+	UCHAR			TA[MAC_ADDR_LEN];
+	UCHAR			DA[MAC_ADDR_LEN];
+	UCHAR			SA[MAC_ADDR_LEN];
+	UCHAR			RC4Key[16];
+	UINT			p1k[5]; //for mix_key;
+	ULONG			pnl;/* Least significant 16 bits of PN */
+	ULONG			pnh;/* Most significant 32 bits of PN */
+	UINT			num_blocks;
+	UINT			payload_remainder;
+	ARCFOURCONTEXT 	ArcFourContext;
+	UINT			crc32 = 0;
+	UINT			trailfcs = 0;
+	UCHAR			MIC[8];
+	UCHAR			TrailMIC[8];
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	fc0 = *pData;
+	fc1 = *(pData + 1);
+
+	fc = *((PUSHORT)pData);
+
+	frame_type = ((fc0 >> 2) & 0x03);
+	frame_subtype = ((fc0 >> 4) & 0x0f);
+
+    from_ds = (fc1 & 0x2) >> 1;
+    to_ds = (fc1 & 0x1);
+
+    a4_exists = (from_ds & to_ds);
+    qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
+                  (frame_subtype == 0x09) ||   /* Likely to change.    */
+                  (frame_subtype == 0x0a) ||
+                  (frame_subtype == 0x0b)
+                 );
+
+	HeaderLen = 24;
+	if (a4_exists)
+		HeaderLen += 6;
+
+	KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
+	KeyID = KeyID >> 6;
+
+	if (pWpaKey[KeyID].KeyLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
+		return FALSE;
+	}
+
+	duration = *((PUSHORT)(pData+2));
+
+	seq_control = *((PUSHORT)(pData+22));
+
+	if (qc_exists)
+	{
+		if (a4_exists)
+		{
+			qos_control = *((PUSHORT)(pData+30));
+		}
+		else
+		{
+			qos_control = *((PUSHORT)(pData+24));
+		}
+	}
+
+	if (to_ds == 0 && from_ds == 1)
+	{
+		NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);  //BSSID
+	}
+	else if (to_ds == 0 && from_ds == 0 )
+	{
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
+	}
+	else if (to_ds == 1 && from_ds == 0)
+	{
+		NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
+	}
+	else if (to_ds == 1 && from_ds == 1)
+	{
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
+	}
+
+	num_blocks = (DataByteCnt - 16) / 16;
+	payload_remainder = (DataByteCnt - 16) % 16;
+
+	pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
+	pnh = *((PULONG)(pData + HeaderLen + 4));
+	pnh = cpu2le32(pnh);
+	RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
+
+	ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
+
+	ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
+	NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
+	crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4);  //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
+	crc32 ^= 0xffffffff;             /* complement */
+
+    if(crc32 != cpu2le32(trailfcs))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n"));	 //ICV error.
+
+		return (FALSE);
+	}
+
+	NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
+	RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
+	NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
+
+	if (!NdisEqualMemory(MIC, TrailMIC, 8))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n"));	 //MIC error.
+		//RTMPReportMicError(pAd, &pWpaKey[KeyID]);	// marked by AlbertY @ 20060630
+		return (FALSE);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+	//DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
+	return TRUE;
+}
+
+
+
+
+BOOLEAN RTMPSoftDecryptAES(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN PCIPHER_KEY	pWpaKey)
+{
+	UCHAR			KeyID;
+	UINT			HeaderLen;
+	UCHAR			PN[6];
+	UINT			payload_len;
+	UINT			num_blocks;
+	UINT			payload_remainder;
+	USHORT			fc;
+	UCHAR			fc0;
+	UCHAR			fc1;
+	UINT			frame_type;
+	UINT			frame_subtype;
+	UINT			from_ds;
+	UINT			to_ds;
+	INT				a4_exists;
+	INT				qc_exists;
+	UCHAR			aes_out[16];
+	int 			payload_index;
+	UINT 			i;
+	UCHAR 			ctr_preload[16];
+	UCHAR 			chain_buffer[16];
+	UCHAR 			padded_buffer[16];
+	UCHAR 			mic_iv[16];
+	UCHAR 			mic_header1[16];
+	UCHAR 			mic_header2[16];
+	UCHAR			MIC[8];
+	UCHAR			TrailMIC[8];
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	fc0 = *pData;
+	fc1 = *(pData + 1);
+
+	fc = *((PUSHORT)pData);
+
+	frame_type = ((fc0 >> 2) & 0x03);
+	frame_subtype = ((fc0 >> 4) & 0x0f);
+
+	from_ds = (fc1 & 0x2) >> 1;
+	to_ds = (fc1 & 0x1);
+
+	a4_exists = (from_ds & to_ds);
+	qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
+				  (frame_subtype == 0x09) ||   /* Likely to change.    */
+				  (frame_subtype == 0x0a) ||
+				  (frame_subtype == 0x0b)
+				 );
+
+	HeaderLen = 24;
+	if (a4_exists)
+		HeaderLen += 6;
+
+	KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
+	KeyID = KeyID >> 6;
+
+	if (pWpaKey[KeyID].KeyLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
+		return FALSE;
+	}
+
+	PN[0] = *(pData+ HeaderLen);
+	PN[1] = *(pData+ HeaderLen + 1);
+	PN[2] = *(pData+ HeaderLen + 4);
+	PN[3] = *(pData+ HeaderLen + 5);
+	PN[4] = *(pData+ HeaderLen + 6);
+	PN[5] = *(pData+ HeaderLen + 7);
+
+	payload_len = DataByteCnt - HeaderLen - 8 - 8;	// 8 bytes for CCMP header , 8 bytes for MIC
+	payload_remainder = (payload_len) % 16;
+	num_blocks = (payload_len) / 16;
+
+
+
+	// Find start of payload
+	payload_index = HeaderLen + 8; //IV+EIV
+
+	for (i=0; i< num_blocks; i++)
+	{
+		construct_ctr_preload(ctr_preload,
+								a4_exists,
+								qc_exists,
+								pData,
+								PN,
+								i+1 );
+
+		aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+		bitwise_xor(aes_out, pData + payload_index, chain_buffer);
+		NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
+		payload_index += 16;
+	}
+
+	//
+	// If there is a short final block, then pad it
+	// encrypt it and copy the unpadded part back
+	//
+	if (payload_remainder > 0)
+	{
+		construct_ctr_preload(ctr_preload,
+								a4_exists,
+								qc_exists,
+								pData,
+								PN,
+								num_blocks + 1);
+
+		NdisZeroMemory(padded_buffer, 16);
+		NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
+
+		aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
+		payload_index += payload_remainder;
+	}
+
+	//
+	// Descrypt the MIC
+	//
+	construct_ctr_preload(ctr_preload,
+							a4_exists,
+							qc_exists,
+							pData,
+							PN,
+							0);
+	NdisZeroMemory(padded_buffer, 16);
+	NdisMoveMemory(padded_buffer, pData + payload_index, 8);
+
+	aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+
+	NdisMoveMemory(TrailMIC, chain_buffer, 8);
+
+	//
+	// Calculate MIC
+	//
+
+	//Force the protected frame bit on
+	*(pData + 1) = *(pData + 1) | 0x40;
+
+	// Find start of payload
+	// Because the CCMP header has been removed
+	payload_index = HeaderLen;
+
+	construct_mic_iv(
+					mic_iv,
+					qc_exists,
+					a4_exists,
+					pData,
+					payload_len,
+					PN);
+
+	construct_mic_header1(
+						mic_header1,
+						HeaderLen,
+						pData);
+
+	construct_mic_header2(
+						mic_header2,
+						pData,
+						a4_exists,
+						qc_exists);
+
+	aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+
+	// iterate through each 16 byte payload block
+	for (i = 0; i < num_blocks; i++)
+	{
+		bitwise_xor(aes_out, pData + payload_index, chain_buffer);
+		payload_index += 16;
+		aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	}
+
+	// Add on the final payload block if it needs padding
+	if (payload_remainder > 0)
+	{
+		NdisZeroMemory(padded_buffer, 16);
+		NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
+
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	}
+
+	// aes_out contains padded mic, discard most significant
+	// 8 bytes to generate 64 bit MIC
+	for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
+
+	if (!NdisEqualMemory(MIC, TrailMIC, 8))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n"));	 //MIC error.
+		return FALSE;
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	return TRUE;
+}
+
+/****************************************/
+/* aes128k128d()                        */
+/* Performs a 128 bit AES encrypt with  */
+/* 128 bit data.                        */
+/****************************************/
+VOID xor_128(
+	IN  PUCHAR  a,
+	IN  PUCHAR  b,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0;i<16; i++)
+	{
+		out[i] = a[i] ^ b[i];
+	}
+}
+
+VOID next_key(
+	IN  PUCHAR  key,
+	IN  INT     round)
+{
+	UCHAR       rcon;
+	UCHAR       sbox_key[4];
+	UCHAR       rcon_table[12] =
+	{
+		0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
+		0x1b, 0x36, 0x36, 0x36
+	};
+
+	sbox_key[0] = RTMPCkipSbox(key[13]);
+	sbox_key[1] = RTMPCkipSbox(key[14]);
+	sbox_key[2] = RTMPCkipSbox(key[15]);
+	sbox_key[3] = RTMPCkipSbox(key[12]);
+
+	rcon = rcon_table[round];
+
+	xor_32(&key[0], sbox_key, &key[0]);
+	key[0] = key[0] ^ rcon;
+
+	xor_32(&key[4], &key[0], &key[4]);
+	xor_32(&key[8], &key[4], &key[8]);
+	xor_32(&key[12], &key[8], &key[12]);
+}
+
+VOID xor_32(
+	IN  PUCHAR  a,
+	IN  PUCHAR  b,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0;i<4; i++)
+	{
+		out[i] = a[i] ^ b[i];
+	}
+}
+
+VOID byte_sub(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0; i< 16; i++)
+	{
+		out[i] = RTMPCkipSbox(in[i]);
+	}
+}
+
+UCHAR RTMPCkipSbox(
+	IN  UCHAR   a)
+{
+	return SboxTable[(int)a];
+}
+
+VOID shift_row(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	out[0] =  in[0];
+	out[1] =  in[5];
+	out[2] =  in[10];
+	out[3] =  in[15];
+	out[4] =  in[4];
+	out[5] =  in[9];
+	out[6] =  in[14];
+	out[7] =  in[3];
+	out[8] =  in[8];
+	out[9] =  in[13];
+	out[10] = in[2];
+	out[11] = in[7];
+	out[12] = in[12];
+	out[13] = in[1];
+	out[14] = in[6];
+	out[15] = in[11];
+}
+
+VOID mix_column(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	INT         i;
+	UCHAR       add1b[4];
+	UCHAR       add1bf7[4];
+	UCHAR       rotl[4];
+	UCHAR       swap_halfs[4];
+	UCHAR       andf7[4];
+	UCHAR       rotr[4];
+	UCHAR       temp[4];
+	UCHAR       tempb[4];
+
+	for (i=0 ; i<4; i++)
+	{
+		if ((in[i] & 0x80)== 0x80)
+			add1b[i] = 0x1b;
+		else
+			add1b[i] = 0x00;
+	}
+
+	swap_halfs[0] = in[2];    /* Swap halfs */
+	swap_halfs[1] = in[3];
+	swap_halfs[2] = in[0];
+	swap_halfs[3] = in[1];
+
+	rotl[0] = in[3];        /* Rotate left 8 bits */
+	rotl[1] = in[0];
+	rotl[2] = in[1];
+	rotl[3] = in[2];
+
+	andf7[0] = in[0] & 0x7f;
+	andf7[1] = in[1] & 0x7f;
+	andf7[2] = in[2] & 0x7f;
+	andf7[3] = in[3] & 0x7f;
+
+	for (i = 3; i>0; i--)    /* logical shift left 1 bit */
+	{
+		andf7[i] = andf7[i] << 1;
+		if ((andf7[i-1] & 0x80) == 0x80)
+		{
+			andf7[i] = (andf7[i] | 0x01);
+		}
+	}
+	andf7[0] = andf7[0] << 1;
+	andf7[0] = andf7[0] & 0xfe;
+
+	xor_32(add1b, andf7, add1bf7);
+
+	xor_32(in, add1bf7, rotr);
+
+	temp[0] = rotr[0];         /* Rotate right 8 bits */
+	rotr[0] = rotr[1];
+	rotr[1] = rotr[2];
+	rotr[2] = rotr[3];
+	rotr[3] = temp[0];
+
+	xor_32(add1bf7, rotr, temp);
+	xor_32(swap_halfs, rotl,tempb);
+	xor_32(temp, tempb, out);
+}
+
diff --git a/drivers/staging/rt2870/common/rtmp_wep.c b/drivers/staging/rt2870/common/rtmp_wep.c
new file mode 100644
index 00000000000..62f9e58729d
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtmp_wep.c
@@ -0,0 +1,508 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_wep.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Wu		10-28-02		Initial
+*/
+
+#include "../rt_config.h"
+
+UINT FCSTAB_32[256] =
+{
+	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba,
+	0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
+	0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+	0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de,
+	0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,
+	0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
+	0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+	0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940,
+	0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116,
+	0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+	0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
+	0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+	0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a,
+	0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818,
+	0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+	0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
+	0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+	0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c,
+	0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2,
+	0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+	0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
+	0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+	0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086,
+	0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4,
+	0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+	0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
+	0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+	0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8,
+	0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe,
+	0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+	0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
+	0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+	0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252,
+	0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60,
+	0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+	0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
+	0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+	0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04,
+	0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a,
+	0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+	0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
+	0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+	0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e,
+	0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c,
+	0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+	0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
+	0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+	0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0,
+	0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6,
+	0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+	0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
+	0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+
+/*
+UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+ */
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init WEP function.
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pKey        Pointer to the WEP KEY
+		KeyId		   WEP Key ID
+		KeyLen      the length of WEP KEY
+		pDest       Pointer to the destination which Encryption data will store in.
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitWepEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	UCHAR			KeyId,
+	IN	UCHAR			KeyLen,
+	IN OUT	PUCHAR		pDest)
+{
+	UINT i;
+	UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+
+	pAd->PrivateInfo.FCSCRC32 = PPPINITFCS32;   //Init crc32.
+
+#ifdef CONFIG_STA_SUPPORT
+    if (pAd->StaCfg.bCkipOn && (pAd->StaCfg.CkipFlag & 0x10) && (pAd->OpMode == OPMODE_STA))
+    {
+        ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, pKey, KeyLen);  //INIT SBOX, KEYLEN+3(IV)
+        NdisMoveMemory(pDest, pKey, 3);  //Append Init Vector
+    }
+    else
+#endif // CONFIG_STA_SUPPORT //
+    {
+		NdisMoveMemory(WEPKEY + 3, pKey, KeyLen);
+
+        for(i = 0; i < 3; i++)
+			WEPKEY[i] = RandomByte(pAd);   //Call mlme RandomByte() function.
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, WEPKEY, KeyLen + 3);  //INIT SBOX, KEYLEN+3(IV)
+
+		NdisMoveMemory(pDest, WEPKEY, 3);  //Append Init Vector
+    }
+	*(pDest+3) = (KeyId << 6);       //Append KEYID
+
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Encrypt transimitted data
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pSrc        Pointer to the transimitted source data that will be encrypt
+      pDest       Pointer to the destination where entryption data will be store in.
+      Len			Indicate the length of the source data
+
+	Return Value:
+      None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPEncryptData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDest,
+	IN	UINT			Len)
+{
+	pAd->PrivateInfo.FCSCRC32 = RTMP_CALC_FCS32(pAd->PrivateInfo.FCSCRC32, pSrc, Len);
+	ARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, pDest, pSrc, Len);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Decrypt received WEP data
+
+	Arguments:
+		pAdapter		Pointer to our adapter
+		pSrc        Pointer to the received data
+		Len         the length of the received data
+
+	Return Value:
+		TRUE        Decrypt WEP data success
+		FALSE       Decrypt WEP data failed
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPSoftDecryptWEP(
+	IN PRTMP_ADAPTER 	pAd,
+	IN PUCHAR			pData,
+	IN ULONG			DataByteCnt,
+	IN PCIPHER_KEY		pGroupKey)
+{
+	UINT	trailfcs;
+	UINT    crc32;
+	UCHAR	KeyIdx;
+	UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+	UCHAR 	*pPayload = (UCHAR *)pData + LENGTH_802_11;
+	ULONG	payload_len = DataByteCnt - LENGTH_802_11;
+
+	NdisMoveMemory(WEPKEY, pPayload, 3);    //Get WEP IV
+
+	KeyIdx = (*(pPayload + 3) & 0xc0) >> 6;
+	if (pGroupKey[KeyIdx].KeyLen == 0)
+		return (FALSE);
+
+	NdisMoveMemory(WEPKEY + 3, pGroupKey[KeyIdx].Key, pGroupKey[KeyIdx].KeyLen);
+	ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, WEPKEY, pGroupKey[KeyIdx].KeyLen + 3);
+	ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, pPayload, pPayload + 4, payload_len - 4);
+	NdisMoveMemory(&trailfcs, pPayload + payload_len - 8, 4);
+	crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pPayload, payload_len - 8);  //Skip last 4 bytes(FCS).
+	crc32 ^= 0xffffffff;             /* complement */
+
+    if(crc32 != cpu2le32(trailfcs))
+    {
+		DBGPRINT(RT_DEBUG_TRACE, ("! WEP Data CRC Error !\n"));	 //CRC error.
+		return (FALSE);
+	}
+	return (TRUE);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm "ARCFOUR" initialize
+
+	Arguments:
+	   Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pKey        Pointer to the WEP KEY
+		KeyLen      Indicate the length fo the WEP KEY
+
+	Return Value:
+	   None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_INIT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pKey,
+	IN	UINT			KeyLen)
+{
+	UCHAR	t, u;
+	UINT	keyindex;
+	UINT	stateindex;
+	PUCHAR	state;
+	UINT	counter;
+
+	state = Ctx->STATE;
+	Ctx->X = 0;
+	Ctx->Y = 0;
+	for (counter = 0; counter < 256; counter++)
+		state[counter] = (UCHAR)counter;
+	keyindex = 0;
+	stateindex = 0;
+	for (counter = 0; counter < 256; counter++)
+	{
+		t = state[counter];
+		stateindex = (stateindex + pKey[keyindex] + t) & 0xff;
+		u = state[stateindex];
+		state[stateindex] = t;
+		state[counter] = u;
+		if (++keyindex >= KeyLen)
+			keyindex = 0;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get bytes from ARCFOUR CONTEXT (S-BOX)
+
+	Arguments:
+	   Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+
+	Return Value:
+	   UCHAR  - the value of the ARCFOUR CONTEXT (S-BOX)
+
+	Note:
+
+	========================================================================
+*/
+UCHAR	ARCFOUR_BYTE(
+	IN	PARCFOURCONTEXT		Ctx)
+{
+  UINT x;
+  UINT y;
+  UCHAR sx, sy;
+  PUCHAR state;
+
+  state = Ctx->STATE;
+  x = (Ctx->X + 1) & 0xff;
+  sx = state[x];
+  y = (sx + Ctx->Y) & 0xff;
+  sy = state[y];
+  Ctx->X = x;
+  Ctx->Y = y;
+  state[y] = sx;
+  state[x] = sy;
+
+  return(state[(sx + sy) & 0xff]);
+
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Decryption Algorithm
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source data
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_DECRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source dta
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_ENCRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm which conform to the special requirement to encrypt  GTK.
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source dta
+
+
+	========================================================================
+*/
+
+VOID	WPAARCFOUR_ENCRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+        //discard first 256 bytes
+	for (i = 0; i < 256; i++)
+            ARCFOUR_BYTE(Ctx);
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate a new FCS given the current FCS and the new data.
+
+	Arguments:
+		Fcs	      the original FCS value
+		Cp          pointer to the data which will be calculate the FCS
+		Len         the length of the data
+
+	Return Value:
+		UINT - FCS 32 bits
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+UINT	RTMP_CALC_FCS32(
+	IN	UINT	Fcs,
+	IN	PUCHAR	Cp,
+	IN	INT		Len)
+{
+	while (Len--)
+	   Fcs = (((Fcs) >> 8) ^ FCSTAB_32[((Fcs) ^ (*Cp++)) & 0xff]);
+
+	return (Fcs);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get last FCS and encrypt it to the destination
+
+	Arguments:
+		pDest			Pointer to the Destination
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPSetICV(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR	pDest)
+{
+	pAd->PrivateInfo.FCSCRC32 ^= 0xffffffff;             /* complement */
+	pAd->PrivateInfo.FCSCRC32 = cpu2le32(pAd->PrivateInfo.FCSCRC32);
+
+	ARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, pDest, (PUCHAR) &pAd->PrivateInfo.FCSCRC32, 4);
+}
+
diff --git a/drivers/staging/rt2870/common/rtusb_bulk.c b/drivers/staging/rt2870/common/rtusb_bulk.c
new file mode 100644
index 00000000000..3c6ba1b9deb
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtusb_bulk.c
@@ -0,0 +1,1981 @@
+ /*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtusb_bulk.c
+
+	Abstract:
+
+	Revision History:
+	Who			When		What
+	--------	----------	----------------------------------------------
+	Name		Date		Modification logs
+	Paul Lin	06-25-2004	created
+
+*/
+
+#include "../rt_config.h"
+// Match total 6 bulkout endpoint to corresponding queue.
+UCHAR	EpToQueue[6]={FIFO_EDCA, FIFO_EDCA, FIFO_EDCA, FIFO_EDCA, FIFO_EDCA, FIFO_MGMT};
+
+//static BOOLEAN SingleBulkOut = FALSE;
+
+void RTUSB_FILL_BULK_URB (struct urb *pUrb,
+	struct usb_device *pUsb_Dev,
+	unsigned int bulkpipe,
+	void *pTransferBuf,
+	int BufSize,
+	usb_complete_t Complete,
+	void *pContext)
+{
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	usb_fill_bulk_urb(pUrb, pUsb_Dev, bulkpipe, pTransferBuf, BufSize, (usb_complete_t)Complete, pContext);
+#else
+	FILL_BULK_URB(pUrb, pUsb_Dev, bulkpipe, pTransferBuf, BufSize, Complete, pContext);
+#endif
+
+}
+
+VOID	RTUSBInitTxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTX_CONTEXT		pTxContext,
+	IN	UCHAR			BulkOutPipeId,
+	IN	usb_complete_t	Func)
+{
+	PURB				pUrb;
+	PUCHAR				pSrc = NULL;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	pUrb = pTxContext->pUrb;
+	ASSERT(pUrb);
+
+	// Store BulkOut PipeId
+	pTxContext->BulkOutPipeId = BulkOutPipeId;
+
+	if (pTxContext->bAggregatible)
+	{
+		pSrc = &pTxContext->TransferBuffer->Aggregation[2];
+	}
+	else
+	{
+		pSrc = (PUCHAR) pTxContext->TransferBuffer->field.WirelessPacket;
+	}
+
+
+	//Initialize a tx bulk urb
+	RTUSB_FILL_BULK_URB(pUrb,
+						pObj->pUsb_Dev,
+						usb_sndbulkpipe(pObj->pUsb_Dev, pAd->BulkOutEpAddr[BulkOutPipeId]),
+						pSrc,
+						pTxContext->BulkOutSize,
+						Func,
+						pTxContext);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	if (pTxContext->bAggregatible)
+		pUrb->transfer_dma	= (pTxContext->data_dma + TX_BUFFER_NORMSIZE + 2);
+	else
+		pUrb->transfer_dma	= pTxContext->data_dma;
+
+	pUrb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+#endif
+
+}
+
+VOID	RTUSBInitHTTxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHT_TX_CONTEXT	pTxContext,
+	IN	UCHAR			BulkOutPipeId,
+	IN	ULONG			BulkOutSize,
+	IN	usb_complete_t	Func)
+{
+	PURB				pUrb;
+	PUCHAR				pSrc = NULL;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	pUrb = pTxContext->pUrb;
+	ASSERT(pUrb);
+
+	// Store BulkOut PipeId
+	pTxContext->BulkOutPipeId = BulkOutPipeId;
+
+	pSrc = &pTxContext->TransferBuffer->field.WirelessPacket[pTxContext->NextBulkOutPosition];
+
+
+	//Initialize a tx bulk urb
+	RTUSB_FILL_BULK_URB(pUrb,
+						pObj->pUsb_Dev,
+						usb_sndbulkpipe(pObj->pUsb_Dev, pAd->BulkOutEpAddr[BulkOutPipeId]),
+						pSrc,
+						BulkOutSize,
+						Func,
+						pTxContext);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	pUrb->transfer_dma	= (pTxContext->data_dma + pTxContext->NextBulkOutPosition);
+	pUrb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+#endif
+
+}
+
+VOID	RTUSBInitRxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PRX_CONTEXT		pRxContext)
+{
+	PURB				pUrb;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+	ULONG				RX_bulk_size;
+
+
+	pUrb = pRxContext->pUrb;
+	ASSERT(pUrb);
+
+	if ( pAd->BulkInMaxPacketSize == 64)
+		RX_bulk_size = 4096;
+	else
+		RX_bulk_size = MAX_RXBULK_SIZE;
+
+	//Initialize a rx bulk urb
+	RTUSB_FILL_BULK_URB(pUrb,
+						pObj->pUsb_Dev,
+						usb_rcvbulkpipe(pObj->pUsb_Dev, pAd->BulkInEpAddr),
+						&(pRxContext->TransferBuffer[pAd->NextRxBulkInPosition]),
+						RX_bulk_size - (pAd->NextRxBulkInPosition),
+						(usb_complete_t)RTUSBBulkRxComplete,
+						(void *)pRxContext);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	pUrb->transfer_dma	= pRxContext->data_dma + pAd->NextRxBulkInPosition;
+	pUrb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+#endif
+
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+
+#define BULK_OUT_LOCK(pLock, IrqFlags)	\
+		if(1 /*!(in_interrupt() & 0xffff0000)*/)	\
+			RTMP_IRQ_LOCK((pLock), IrqFlags);
+
+#define BULK_OUT_UNLOCK(pLock, IrqFlags)	\
+		if(1 /*!(in_interrupt() & 0xffff0000)*/)	\
+			RTMP_IRQ_UNLOCK((pLock), IrqFlags);
+
+
+VOID	RTUSBBulkOutDataPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId,
+	IN	UCHAR			Index)
+{
+
+	PHT_TX_CONTEXT	pHTTXContext;
+	PURB			pUrb;
+	int				ret = 0;
+	PTXINFO_STRUC	pTxInfo, pLastTxInfo = NULL;
+	PTXWI_STRUC             pTxWI;
+	ULONG			TmpBulkEndPos, ThisBulkSize;
+	unsigned long	IrqFlags = 0, IrqFlags2 = 0;
+	PUCHAR			pWirelessPkt, pAppendant;
+	BOOLEAN			bTxQLastRound = FALSE;
+	UCHAR			allzero[4]= {0x0,0x0,0x0,0x0};
+
+	BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	if ((pAd->BulkOutPending[BulkOutPipeId] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		return;
+	}
+	pAd->BulkOutPending[BulkOutPipeId] = TRUE;
+
+	if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+		)
+	{
+		pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		return;
+	}
+	BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+
+	pHTTXContext = &(pAd->TxContext[BulkOutPipeId]);
+
+	BULK_OUT_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags2);
+	if ((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition)
+		|| ((pHTTXContext->ENextBulkOutPosition-8) == pHTTXContext->CurWritePosition))
+	{
+		BULK_OUT_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags2);
+
+		BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+
+		// Clear Data flag
+		RTUSB_CLEAR_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_FRAG << BulkOutPipeId));
+		RTUSB_CLEAR_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		return;
+	}
+
+	// Clear Data flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_FRAG << BulkOutPipeId));
+	RTUSB_CLEAR_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+
+	//DBGPRINT(RT_DEBUG_TRACE,("BulkOut-B:I=0x%lx, CWPos=%ld, CWRPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", in_interrupt(),
+	//							pHTTXContext->CurWritePosition, pHTTXContext->CurWriteRealPos, pHTTXContext->NextBulkOutPosition,
+	//							pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+	pHTTXContext->NextBulkOutPosition = pHTTXContext->ENextBulkOutPosition;
+	ThisBulkSize = 0;
+	TmpBulkEndPos = pHTTXContext->NextBulkOutPosition;
+	pWirelessPkt = &pHTTXContext->TransferBuffer->field.WirelessPacket[0];
+
+	if ((pHTTXContext->bCopySavePad == TRUE))
+	{
+		if (RTMPEqualMemory(pHTTXContext->SavedPad, allzero,4))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("e1, allzero : %x  %x  %x  %x  %x  %x  %x  %x \n",
+				pHTTXContext->SavedPad[0], pHTTXContext->SavedPad[1], pHTTXContext->SavedPad[2],pHTTXContext->SavedPad[3]
+				,pHTTXContext->SavedPad[4], pHTTXContext->SavedPad[5], pHTTXContext->SavedPad[6],pHTTXContext->SavedPad[7]));
+		}
+		NdisMoveMemory(&pWirelessPkt[TmpBulkEndPos], pHTTXContext->SavedPad, 8);
+		pHTTXContext->bCopySavePad = FALSE;
+		if (pAd->bForcePrintTX == TRUE)
+			DBGPRINT(RT_DEBUG_TRACE,("RTUSBBulkOutDataPacket --> COPY PAD. CurWrite = %ld, NextBulk = %ld.   ENextBulk = %ld.\n",   pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition));
+	}
+
+	do
+	{
+		pTxInfo = (PTXINFO_STRUC)&pWirelessPkt[TmpBulkEndPos];
+		pTxWI = (PTXWI_STRUC)&pWirelessPkt[TmpBulkEndPos + TXINFO_SIZE];
+
+		if (pAd->bForcePrintTX == TRUE)
+			DBGPRINT(RT_DEBUG_TRACE, ("RTUSBBulkOutDataPacket AMPDU = %d.\n",   pTxWI->AMPDU));
+
+		// add by Iverson, limit BulkOut size to 4k to pass WMM b mode 2T1R test items
+		//if ((ThisBulkSize != 0)  && (pTxWI->AMPDU == 0))
+		if ((ThisBulkSize != 0) && (pTxWI->PHYMODE == MODE_CCK))
+		{
+			if (((ThisBulkSize&0xffff8000) != 0) || ((ThisBulkSize&0x1000) == 0x1000))
+			{
+				// Limit BulkOut size to about 4k bytes.
+				pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+				break;
+			}
+			else if (((pAd->BulkOutMaxPacketSize < 512) && ((ThisBulkSize&0xfffff800) != 0) ) /*|| ( (ThisBulkSize != 0)  && (pTxWI->AMPDU == 0))*/)
+			{
+				// For USB 1.1 or peer which didn't support AMPDU, limit the BulkOut size.
+				// For performence in b/g mode, now just check for USB 1.1 and didn't care about the APMDU or not! 2008/06/04.
+				pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+				break;
+			}
+		}
+		// end Iverson
+		else
+		{
+			if (((ThisBulkSize&0xffff8000) != 0) || ((ThisBulkSize&0x6000) == 0x6000))
+			{	// Limit BulkOut size to about 24k bytes.
+				pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+				break;
+			}
+			else if (((pAd->BulkOutMaxPacketSize < 512) && ((ThisBulkSize&0xfffff800) != 0) ) /*|| ( (ThisBulkSize != 0)  && (pTxWI->AMPDU == 0))*/)
+			{	// For USB 1.1 or peer which didn't support AMPDU, limit the BulkOut size.
+				// For performence in b/g mode, now just check for USB 1.1 and didn't care about the APMDU or not! 2008/06/04.
+				pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+				break;
+			}
+		}
+
+		if (TmpBulkEndPos == pHTTXContext->CurWritePosition)
+		{
+			pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+			break;
+		}
+
+		if (pTxInfo->QSEL != FIFO_EDCA)
+		{
+			printk("%s(): ====> pTxInfo->QueueSel(%d)!= FIFO_EDCA!!!!\n", __FUNCTION__, pTxInfo->QSEL);
+			printk("\tCWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad);
+			hex_dump("Wrong QSel Pkt:", (PUCHAR)&pWirelessPkt[TmpBulkEndPos], (pHTTXContext->CurWritePosition - pHTTXContext->NextBulkOutPosition));
+		}
+
+		if (pTxInfo->USBDMATxPktLen <= 8)
+		{
+			BULK_OUT_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags2);
+			DBGPRINT(RT_DEBUG_ERROR /*RT_DEBUG_TRACE*/,("e2, USBDMATxPktLen==0, Size=%ld, bCSPad=%d, CWPos=%ld, NBPos=%ld, CWRPos=%ld!\n",
+					pHTTXContext->BulkOutSize, pHTTXContext->bCopySavePad, pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->CurWriteRealPos));
+			{
+				DBGPRINT_RAW(RT_DEBUG_ERROR /*RT_DEBUG_TRACE*/,("%x  %x  %x  %x  %x  %x  %x  %x \n",
+					pHTTXContext->SavedPad[0], pHTTXContext->SavedPad[1], pHTTXContext->SavedPad[2],pHTTXContext->SavedPad[3]
+					,pHTTXContext->SavedPad[4], pHTTXContext->SavedPad[5], pHTTXContext->SavedPad[6],pHTTXContext->SavedPad[7]));
+			}
+			pAd->bForcePrintTX = TRUE;
+			BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+			pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+			BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+			//DBGPRINT(RT_DEBUG_LOUD,("Out:pTxInfo->USBDMATxPktLen=%d!\n", pTxInfo->USBDMATxPktLen));
+			return;
+		}
+
+			// Increase Total transmit byte counter
+		pAd->RalinkCounters.OneSecTransmittedByteCount +=  pTxWI->MPDUtotalByteCount;
+		pAd->RalinkCounters.TransmittedByteCount +=  pTxWI->MPDUtotalByteCount;
+
+		pLastTxInfo = pTxInfo;
+
+		// Make sure we use EDCA QUEUE.
+		pTxInfo->QSEL = FIFO_EDCA;
+		ThisBulkSize += (pTxInfo->USBDMATxPktLen+4);
+		TmpBulkEndPos += (pTxInfo->USBDMATxPktLen+4);
+
+		if (TmpBulkEndPos != pHTTXContext->CurWritePosition)
+			pTxInfo->USBDMANextVLD = 1;
+
+		if (pTxInfo->SwUseLastRound == 1)
+		{
+			if (pHTTXContext->CurWritePosition == 8)
+				pTxInfo->USBDMANextVLD = 0;
+			pTxInfo->SwUseLastRound = 0;
+
+			bTxQLastRound = TRUE;
+			pHTTXContext->ENextBulkOutPosition = 8;
+
+	#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pTxInfo, TYPE_TXINFO);
+			RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+	#endif // RT_BIG_ENDIAN //
+
+			break;
+		}
+
+#ifdef RT_BIG_ENDIAN
+		RTMPDescriptorEndianChange((PUCHAR)pTxInfo, TYPE_TXINFO);
+		RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+#endif // RT_BIG_ENDIAN //
+
+	}while (TRUE);
+
+	// adjust the pTxInfo->USBDMANextVLD value of last pTxInfo.
+	if (pLastTxInfo)
+	{
+#ifdef RT_BIG_ENDIAN
+		RTMPDescriptorEndianChange((PUCHAR)pLastTxInfo, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+		pLastTxInfo->USBDMANextVLD = 0;
+#ifdef RT_BIG_ENDIAN
+		RTMPDescriptorEndianChange((PUCHAR)pLastTxInfo, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+	}
+
+	/*
+		We need to copy SavedPad when following condition matched!
+			1. Not the last round of the TxQueue and
+			2. any match of following cases:
+				(1). The End Position of this bulk out is reach to the Currenct Write position and
+						the TxInfo and related header already write to the CurWritePosition.
+			   		=>(ENextBulkOutPosition == CurWritePosition) && (CurWriteRealPos > CurWritePosition)
+
+				(2). The EndPosition of the bulk out is not reach to the Current Write Position.
+					=>(ENextBulkOutPosition != CurWritePosition)
+	*/
+	if ((bTxQLastRound == FALSE) &&
+		 (((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition) && (pHTTXContext->CurWriteRealPos > pHTTXContext->CurWritePosition)) ||
+		  (pHTTXContext->ENextBulkOutPosition != pHTTXContext->CurWritePosition))
+		)
+	{
+		NdisMoveMemory(pHTTXContext->SavedPad, &pWirelessPkt[pHTTXContext->ENextBulkOutPosition], 8);
+		pHTTXContext->bCopySavePad = TRUE;
+		if (RTMPEqualMemory(pHTTXContext->SavedPad, allzero,4))
+		{
+			PUCHAR	pBuf = &pHTTXContext->SavedPad[0];
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("WARNING-Zero-3:%02x%02x%02x%02x%02x%02x%02x%02x,CWPos=%ld, CWRPos=%ld, bCW=%d, NBPos=%ld, TBPos=%ld, TBSize=%ld\n",
+				pBuf[0], pBuf[1], pBuf[2],pBuf[3],pBuf[4], pBuf[5], pBuf[6],pBuf[7], pHTTXContext->CurWritePosition, pHTTXContext->CurWriteRealPos,
+				pHTTXContext->bCurWriting, pHTTXContext->NextBulkOutPosition, TmpBulkEndPos, ThisBulkSize));
+
+			pBuf = &pWirelessPkt[pHTTXContext->CurWritePosition];
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("\tCWPos=%02x%02x%02x%02x%02x%02x%02x%02x\n", pBuf[0], pBuf[1], pBuf[2],pBuf[3],pBuf[4], pBuf[5], pBuf[6],pBuf[7]));
+		}
+		//DBGPRINT(RT_DEBUG_LOUD,("ENPos==CWPos=%ld, CWRPos=%ld, bCSPad=%d!\n", pHTTXContext->CurWritePosition, pHTTXContext->CurWriteRealPos, pHTTXContext->bCopySavePad));
+	}
+
+	if (pAd->bForcePrintTX == TRUE)
+		DBGPRINT(RT_DEBUG_TRACE,("BulkOut-A:Size=%ld, CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", ThisBulkSize, pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+	//DBGPRINT(RT_DEBUG_LOUD,("BulkOut-A:Size=%ld, CWPos=%ld, CWRPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d, bLRound=%d!\n", ThisBulkSize, pHTTXContext->CurWritePosition, pHTTXContext->CurWriteRealPos, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad, bTxQLastRound));
+
+		// USB DMA engine requires to pad extra 4 bytes. This pad doesn't count into real bulkoutsize.
+	pAppendant = &pWirelessPkt[TmpBulkEndPos];
+	NdisZeroMemory(pAppendant, 8);
+		ThisBulkSize += 4;
+		pHTTXContext->LastOne = TRUE;
+		if ((ThisBulkSize % pAd->BulkOutMaxPacketSize) == 0)
+			ThisBulkSize += 4;
+	pHTTXContext->BulkOutSize = ThisBulkSize;
+
+	pAd->watchDogTxPendingCnt[BulkOutPipeId] = 1;
+	BULK_OUT_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags2);
+
+	// Init Tx context descriptor
+	RTUSBInitHTTxDesc(pAd, pHTTXContext, BulkOutPipeId, ThisBulkSize, (usb_complete_t)RTUSBBulkOutDataPacketComplete);
+
+	pUrb = pHTTXContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutDataPacket: Submit Tx URB failed %d\n", ret));
+
+		BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+		pAd->watchDogTxPendingCnt[BulkOutPipeId] = 0;
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+		return;
+	}
+
+	BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pHTTXContext->IRPPending = TRUE;
+	BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pAd->BulkOutReq++;
+
+}
+
+
+VOID RTUSBBulkOutDataPacketComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+#if 0 // sample, IRQ LOCK
+	PRTMP_ADAPTER		pAd;
+	POS_COOKIE			pObj;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId;
+	NTSTATUS			Status;
+	unsigned long		IrqFlags;
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutDataPacketComplete\n"));
+
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd			= pHTTXContext->pAd;
+	pObj 		= (POS_COOKIE) pAd->OS_Cookie;
+	Status		= pUrb->status;
+
+	// Store BulkOut PipeId
+	BulkOutPipeId = pHTTXContext->BulkOutPipeId;
+	pAd->BulkOutDataOneSecCount++;
+
+	//DBGPRINT(RT_DEBUG_LOUD, ("Done-B(%d):I=0x%lx, CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", BulkOutPipeId, in_interrupt(), pHTTXContext->CurWritePosition,
+	//		pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+	pHTTXContext->IRPPending = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+	if (Status == USB_ST_NOERROR)
+	{
+		pAd->BulkOutComplete++;
+
+		pAd->Counters8023.GoodTransmits++;
+		//RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+		FREE_HTTX_RING(pAd, BulkOutPipeId, pHTTXContext);
+		//RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+
+	}
+	else	// STATUS_OTHER
+	{
+		PUCHAR	pBuf;
+
+		pAd->BulkOutCompleteOther++;
+
+		pBuf = &pHTTXContext->TransferBuffer->WirelessPacket[pHTTXContext->NextBulkOutPosition];
+
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("BulkOutDataPacket failed: ReasonCode=%d!\n", Status));
+		DBGPRINT_RAW(RT_DEBUG_ERROR, (">>BulkOut Req=0x%lx, Complete=0x%lx, Other=0x%lx\n", pAd->BulkOutReq, pAd->BulkOutComplete, pAd->BulkOutCompleteOther));
+		DBGPRINT_RAW(RT_DEBUG_ERROR, (">>BulkOut Header:%x %x %x %x %x %x %x %x\n", pBuf[0], pBuf[1], pBuf[2], pBuf[3], pBuf[4], pBuf[5], pBuf[6], pBuf[7]));
+		//DBGPRINT_RAW(RT_DEBUG_ERROR, (">>BulkOutCompleteCancel=0x%x, BulkOutCompleteOther=0x%x\n", pAd->BulkOutCompleteCancel, pAd->BulkOutCompleteOther));
+
+		if (!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+									fRTMP_ADAPTER_HALT_IN_PROGRESS |
+									fRTMP_ADAPTER_NIC_NOT_EXIST |
+									fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = BulkOutPipeId;
+		}
+	}
+
+	//
+	// bInUse = TRUE, means some process are filling TX data, after that must turn on bWaitingBulkOut
+	// bWaitingBulkOut = TRUE, means the TX data are waiting for bulk out.
+	//
+	//RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	if ((pHTTXContext->ENextBulkOutPosition != pHTTXContext->CurWritePosition) &&
+		(pHTTXContext->ENextBulkOutPosition != (pHTTXContext->CurWritePosition+8)) &&
+		!RTUSB_TEST_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_FRAG << BulkOutPipeId)))
+	{
+		// Indicate There is data avaliable
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	//RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protection of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+
+
+	//DBGPRINT(RT_DEBUG_LOUD,("Done-A(%d):I=0x%lx, CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d\n", BulkOutPipeId, in_interrupt(),
+	//		pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+
+	switch (BulkOutPipeId)
+	{
+		case 0:
+				pObj->ac0_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->ac0_dma_done_task);
+				break;
+		case 1:
+				pObj->ac1_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->ac1_dma_done_task);
+				break;
+		case 2:
+				pObj->ac2_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->ac2_dma_done_task);
+				break;
+		case 3:
+				pObj->ac3_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->ac3_dma_done_task);
+				break;
+		case 4:
+				pObj->hcca_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->hcca_dma_done_task);
+				break;
+	}
+#else
+
+{
+	PHT_TX_CONTEXT	pHTTXContext;
+	PRTMP_ADAPTER	pAd;
+	POS_COOKIE 		pObj;
+	UCHAR			BulkOutPipeId;
+
+
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd 			= pHTTXContext->pAd;
+	pObj 			= (POS_COOKIE) pAd->OS_Cookie;
+
+	// Store BulkOut PipeId
+	BulkOutPipeId	= pHTTXContext->BulkOutPipeId;
+	pAd->BulkOutDataOneSecCount++;
+
+	switch (BulkOutPipeId)
+	{
+		case 0:
+				pObj->ac0_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->ac0_dma_done_task);
+				break;
+		case 1:
+				pObj->ac1_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->ac1_dma_done_task);
+				break;
+		case 2:
+				pObj->ac2_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->ac2_dma_done_task);
+				break;
+		case 3:
+				pObj->ac3_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->ac3_dma_done_task);
+				break;
+		case 4:
+				pObj->hcca_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->hcca_dma_done_task);
+				break;
+	}
+}
+#endif
+
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note: NULL frame use BulkOutPipeId = 0
+
+	========================================================================
+*/
+VOID	RTUSBBulkOutNullFrame(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PTX_CONTEXT		pNullContext = &(pAd->NullContext);
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+	if ((pAd->BulkOutPending[0] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+		return;
+	}
+	pAd->BulkOutPending[0] = TRUE;
+	pAd->watchDogTxPendingCnt[0] = 1;
+	pNullContext->IRPPending = TRUE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+	// Increase Total transmit byte counter
+	pAd->RalinkCounters.TransmittedByteCount +=  pNullContext->BulkOutSize;
+
+
+	// Clear Null frame bulk flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NULL);
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pNullContext->TransferBuffer, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// Init Tx context descriptor
+	RTUSBInitTxDesc(pAd, pNullContext, 0, (usb_complete_t)RTUSBBulkOutNullFrameComplete);
+
+	pUrb = pNullContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+		pAd->BulkOutPending[0] = FALSE;
+		pAd->watchDogTxPendingCnt[0] = 0;
+		pNullContext->IRPPending = FALSE;
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutNullFrame: Submit Tx URB failed %d\n", ret));
+		return;
+	}
+
+}
+
+// NULL frame use BulkOutPipeId = 0
+VOID RTUSBBulkOutNullFrameComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	PRTMP_ADAPTER		pAd;
+	PTX_CONTEXT			pNullContext;
+	NTSTATUS			Status;
+#if 0 // sample, IRQ LOCK
+	unsigned long		IrqFlags;
+#endif
+	POS_COOKIE			pObj;
+
+
+	pNullContext	= (PTX_CONTEXT)pUrb->context;
+	pAd 			= pNullContext->pAd;
+	Status 			= pUrb->status;
+
+#if 0 // sample, IRQ LOCK
+	// Reset Null frame context flags
+	pNullContext->IRPPending 	= FALSE;
+	pNullContext->InUse 		= FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		// Don't worry about the queue is empty or not, this function will check itself
+		//RTMPUSBDeQueuePacket(pAd, 0);
+		RTMPDeQueuePacket(pAd, TRUE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else	// STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out Null Frame Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+	}
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+	pAd->BulkOutPending[0] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+#else
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pObj->null_frame_complete_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->null_frame_complete_task);
+#endif
+
+}
+
+#if 0	// For RT2870, RTS frame not used now, but maybe will use it latter.
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note: RTS frame use BulkOutPipeId = 0
+
+	========================================================================
+*/
+VOID	RTUSBBulkOutRTSFrame(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PTX_CONTEXT		pRTSContext = &(pAd->RTSContext);
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+	UCHAR			PipeID=0;
+
+	if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_4))
+		PipeID= 3;
+	else if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_3))
+		PipeID= 2;
+	else if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_2))
+		PipeID= 1;
+	else if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL))
+		PipeID= 0;
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[PipeID], IrqFlags);
+	if ((pAd->BulkOutPending[PipeID] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[PipeID], IrqFlags);
+		return;
+	}
+	pAd->BulkOutPending[PipeID] = TRUE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[PipeID], IrqFlags);
+
+	// Increase Total transmit byte counter
+	pAd->RalinkCounters.TransmittedByteCount +=  pRTSContext->BulkOutSize;
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutRTSFrame \n"));
+
+	// Clear RTS frame bulk flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_RTS);
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pRTSContext->TransferBuffer, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// Init Tx context descriptor
+	RTUSBInitTxDesc(pAd, pRTSContext, PipeID, (usb_complete_t)RTUSBBulkOutRTSFrameComplete);
+	pRTSContext->IRPPending = TRUE;
+
+	pUrb = pRTSContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutRTSFrame: Submit Tx URB failed %d\n", ret));
+		return;
+	}
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("<---RTUSBBulkOutRTSFrame \n"));
+
+}
+
+// RTS frame use BulkOutPipeId = 0
+VOID RTUSBBulkOutRTSFrameComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	PRTMP_ADAPTER	pAd;
+	PTX_CONTEXT		pRTSContext;
+	NTSTATUS		Status;
+#if 0 // sample, IRQ LOCK
+	unsigned long	IrqFlags;
+#endif
+	POS_COOKIE		pObj;
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutRTSFrameComplete\n"));
+
+	pRTSContext	= (PTX_CONTEXT)pUrb->context;
+	pAd			= pRTSContext->pAd;
+	Status		= pUrb->status;
+
+#if 0 // sample, IRQ LOCK
+	// Reset RTS frame context flags
+	pRTSContext->IRPPending = FALSE;
+	pRTSContext->InUse		= FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		// Don't worry about the queue is empty or not, this function will check itself
+		//RTMPUSBDeQueuePacket(pAd, pRTSContext->BulkOutPipeId);
+		RTMPDeQueuePacket(pAd, TRUE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else	// STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out RTS Frame Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+	}
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[pRTSContext->BulkOutPipeId], IrqFlags);
+	pAd->BulkOutPending[pRTSContext->BulkOutPipeId] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[pRTSContext->BulkOutPipeId], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+#else
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pObj->rts_frame_complete_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->rts_frame_complete_task);
+#endif
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("<---RTUSBBulkOutRTSFrameComplete\n"));
+
+}
+#endif
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note: MLME use BulkOutPipeId = 0
+
+	========================================================================
+*/
+VOID	RTUSBBulkOutMLMEPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PTX_CONTEXT		pMLMEContext;
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+	pMLMEContext = (PTX_CONTEXT)pAd->MgmtRing.Cell[pAd->MgmtRing.TxDmaIdx].AllocVa;
+	pUrb = pMLMEContext->pUrb;
+
+	if ((pAd->MgmtRing.TxSwFreeIdx >= MGMT_RING_SIZE) ||
+		(pMLMEContext->InUse == FALSE) ||
+		(pMLMEContext->bWaitingBulkOut == FALSE))
+	{
+
+
+		// Clear MLME bulk flag
+		RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+
+		return;
+	}
+
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+	if ((pAd->BulkOutPending[MGMTPIPEIDX] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+		return;
+	}
+
+	pAd->BulkOutPending[MGMTPIPEIDX] = TRUE;
+	pAd->watchDogTxPendingCnt[MGMTPIPEIDX] = 1;
+	pMLMEContext->IRPPending = TRUE;
+	pMLMEContext->bWaitingBulkOut = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+	// Increase Total transmit byte counter
+	pAd->RalinkCounters.TransmittedByteCount +=  pMLMEContext->BulkOutSize;
+
+	// Clear MLME bulk flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+
+
+	//DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutMLMEPacket\n"));
+#if 0 // for debug
+{
+	printk("MLME-Out, C=%d!, D=%d, F=%d!\n", pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx);
+
+	//TODO: Need to remove it when formal release
+	PTXINFO_STRUC pTxInfo;
+
+	pTxInfo = (PTXINFO_STRUC)pMLMEContext->TransferBuffer;
+	if (pTxInfo->QSEL != FIFO_EDCA)
+	{
+			printk("%s(): ====> pTxInfo->QueueSel(%d)!= FIFO_EDCA!!!!\n", __FUNCTION__, pTxInfo->QSEL);
+			printk("\tMLME_Index=%d!\n", Index);
+			hex_dump("Wrong QSel Pkt:", (PUCHAR)pMLMEContext->TransferBuffer, pTxInfo->USBDMATxPktLen);
+	}
+}
+#endif
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pMLMEContext->TransferBuffer, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// Init Tx context descriptor
+	RTUSBInitTxDesc(pAd, pMLMEContext, MGMTPIPEIDX, (usb_complete_t)RTUSBBulkOutMLMEPacketComplete);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	//For mgmt urb buffer, because we use sk_buff, so we need to notify the USB controller do dma mapping.
+	pUrb->transfer_dma	= 0;
+	pUrb->transfer_flags &= (~URB_NO_TRANSFER_DMA_MAP);
+#endif
+
+	pUrb = pMLMEContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutMLMEPacket: Submit MLME URB failed %d\n", ret));
+		RTMP_IRQ_LOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+		pAd->BulkOutPending[MGMTPIPEIDX] = FALSE;
+		pAd->watchDogTxPendingCnt[MGMTPIPEIDX] = 0;
+		pMLMEContext->IRPPending = FALSE;
+		pMLMEContext->bWaitingBulkOut = TRUE;
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+		return;
+	}
+
+	//DBGPRINT_RAW(RT_DEBUG_INFO, ("<---RTUSBBulkOutMLMEPacket \n"));
+//	printk("<---RTUSBBulkOutMLMEPacket,Cpu=%d!, Dma=%d, SwIdx=%d!\n", pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx);
+}
+
+
+VOID RTUSBBulkOutMLMEPacketComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	PTX_CONTEXT			pMLMEContext;
+	PRTMP_ADAPTER		pAd;
+	NTSTATUS			Status;
+	POS_COOKIE 			pObj;
+	int					index;
+#if 0 // sample, IRQ LOCK
+	unsigned long		IrqFlags;
+	PNDIS_PACKET		pPacket;
+#endif
+
+
+	//DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutMLMEPacketComplete\n"));
+	pMLMEContext	= (PTX_CONTEXT)pUrb->context;
+	pAd 			= pMLMEContext->pAd;
+	pObj 			= (POS_COOKIE)pAd->OS_Cookie;
+	Status			= pUrb->status;
+	index 			= pMLMEContext->SelfIdx;
+
+
+#if 0 // sample, IRQ LOCK
+	ASSERT((pAd->MgmtRing.TxDmaIdx == index));
+	//printk("MLME-Done-B: C=%d, D=%d, F=%d, Self=%d!\n", pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx, pMLMEContext->SelfIdx);
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+
+	if (Status != USB_ST_NOERROR)
+	{
+		//Bulk-Out fail status handle
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out MLME Failed, Status=%d!\n", Status));
+			// TODO: How to handle about the MLMEBulkOut failed issue. Need to resend the mgmt pkt?
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+		}
+	}
+	pAd->BulkOutPending[MGMTPIPEIDX] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+	RTMP_IRQ_LOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+	// Reset MLME context flags
+	pMLMEContext->IRPPending = FALSE;
+	pMLMEContext->InUse = FALSE;
+	pMLMEContext->bWaitingBulkOut = FALSE;
+	pMLMEContext->BulkOutSize = 0;
+
+	pPacket = pAd->MgmtRing.Cell[index].pNdisPacket;
+	pAd->MgmtRing.Cell[index].pNdisPacket = NULL;
+
+	// Increase MgmtRing Index
+	INC_RING_INDEX(pAd->MgmtRing.TxDmaIdx, MGMT_RING_SIZE);
+	pAd->MgmtRing.TxSwFreeIdx++;
+
+	RTMP_IRQ_UNLOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+
+	// No-matter success or fail, we free the mgmt packet.
+	if (pPacket)
+		RTMPFreeNdisPacket(pAd, pPacket);
+
+#if 0
+	//Bulk-Out fail status handle
+	if (Status != USB_ST_NOERROR)
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out MLME Failed, Status=%d!\n", Status));
+			// TODO: How to handle about the MLMEBulkOut failed issue. Need to reset the endpoint?
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+		}
+	}
+#endif
+
+	//printk("MLME-Done-A: C=%d, D=%d, F=%d!\n", pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx);
+
+	pObj->mgmt_dma_done_task.data = (unsigned long)pAd;
+	tasklet_hi_schedule(&pObj->mgmt_dma_done_task);
+
+	//DBGPRINT_RAW(RT_DEBUG_INFO, ("<---RTUSBBulkOutMLMEPacketComplete\n"));
+//	printk("<---RTUSBBulkOutMLMEPacketComplete, Cpu=%d, Dma=%d, SwIdx=%d!\n",
+//				pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx);
+
+#else
+
+	pObj->mgmt_dma_done_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->mgmt_dma_done_task);
+#endif
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note: PsPoll use BulkOutPipeId = 0
+
+	========================================================================
+*/
+VOID	RTUSBBulkOutPsPoll(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PTX_CONTEXT		pPsPollContext = &(pAd->PsPollContext);
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+	if ((pAd->BulkOutPending[0] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+		return;
+	}
+	pAd->BulkOutPending[0] = TRUE;
+	pAd->watchDogTxPendingCnt[0] = 1;
+	pPsPollContext->IRPPending = TRUE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+
+	// Clear PS-Poll bulk flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_PSPOLL);
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pPsPollContext->TransferBuffer, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// Init Tx context descriptor
+	RTUSBInitTxDesc(pAd, pPsPollContext, MGMTPIPEIDX, (usb_complete_t)RTUSBBulkOutPsPollComplete);
+
+	pUrb = pPsPollContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+		pAd->BulkOutPending[0] = FALSE;
+		pAd->watchDogTxPendingCnt[0] = 0;
+		pPsPollContext->IRPPending = FALSE;
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutPsPoll: Submit Tx URB failed %d\n", ret));
+		return;
+	}
+
+}
+
+// PS-Poll frame use BulkOutPipeId = 0
+VOID RTUSBBulkOutPsPollComplete(purbb_t pUrb,struct pt_regs *pt_regs)
+{
+	PRTMP_ADAPTER		pAd;
+	PTX_CONTEXT			pPsPollContext;
+	NTSTATUS			Status;
+#if 0 // sample, IRQ LOCK
+	unsigned long		IrqFlags;
+#endif
+	POS_COOKIE			pObj;
+
+
+	pPsPollContext= (PTX_CONTEXT)pUrb->context;
+	pAd = pPsPollContext->pAd;
+	Status = pUrb->status;
+
+#if 0 // sample, IRQ LOCK
+	// Reset PsPoll context flags
+	pPsPollContext->IRPPending	= FALSE;
+	pPsPollContext->InUse		= FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		// Don't worry about the queue is empty or not, this function will check itself
+		RTMPDeQueuePacket(pAd, TRUE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else // STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out PSPoll Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+	}
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+	pAd->BulkOutPending[0] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+#else
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pObj->pspoll_frame_complete_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->pspoll_frame_complete_task);
+#endif
+}
+
+
+#if 0
+/*
+	========================================================================
+
+	Routine Description:
+	USB_RxPacket initializes a URB and uses the Rx IRP to submit it
+	to USB. It checks if an Rx Descriptor is available and passes the
+	the coresponding buffer to be filled. If no descriptor is available
+	fails the request. When setting the completion routine we pass our
+	Adapter Object as Context.
+
+	Arguments:
+
+	Return Value:
+		TRUE			found matched tuple cache
+		FALSE			no matched found
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBBulkReceive(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+
+	/* device had been closed */
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_REMOVE_IN_PROGRESS))
+		return;
+
+	RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+
+	// Last is time point between 2 separate URB.
+	if (pAd->NextRxBulkInPosition == 0)
+	{
+		//pAd->NextRxBulkInIndex = (pAd->NextRxBulkInIndex + 1) % (RX_RING_SIZE);
+		INC_RING_INDEX(pAd->NextRxBulkInIndex, RX_RING_SIZE);
+	}
+	else if ((pAd->NextRxBulkInPosition&0x1ff) != 0)
+	{
+		//pAd->NextRxBulkInIndex = (pAd->NextRxBulkInIndex + 1) % (RX_RING_SIZE);
+		INC_RING_INDEX(pAd->NextRxBulkInIndex, RX_RING_SIZE);
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("pAd->NextRxBulkInPosition = 0x%lx.  End of URB.\n", pAd->NextRxBulkInPosition ));
+		pAd->NextRxBulkInPosition = 0;
+	}
+
+	if (pAd->NextRxBulkInPosition == MAX_RXBULK_SIZE)
+		pAd->NextRxBulkInPosition = 0;
+
+	pRxContext = &(pAd->RxContext[pAd->NextRxBulkInIndex]);
+
+	// TODO: Why need to check if pRxContext->InUsed == TRUE?
+	//if ((pRxContext->InUse == TRUE) || (pRxContext->Readable == TRUE))
+	if ((pRxContext->InUse == FALSE) && (pRxContext->Readable == TRUE))
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("pRxContext[%d] InUse = %d.pRxContext->Readable = %d.  Return.\n", pAd->NextRxBulkInIndex,pRxContext->InUse, pRxContext->Readable ));
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+		// read RxContext, Since not
+#ifdef CONFIG_STA_SUPPORT
+		STARxDoneInterruptHandle(pAd, TRUE);
+#endif // CONFIG_STA_SUPPORT //
+
+		//return;
+	}
+	pRxContext->InUse = TRUE;
+	pRxContext->IRPPending= TRUE;
+
+	RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+	// Init Rx context descriptor
+	NdisZeroMemory(pRxContext->TransferBuffer, BUFFER_SIZE);
+	RTUSBInitRxDesc(pAd, pRxContext);
+
+	pUrb = pRxContext->pUrb;
+	if ((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkReceive: Submit Rx URB failed %d\n", ret));
+		return;
+	}
+	else // success
+	{
+		NdisInterlockedIncrement(&pAd->PendingRx);
+		pAd->BulkInReq++;
+	}
+
+	// read RxContext, Since not
+#ifdef CONFIG_STA_SUPPORT
+	STARxDoneInterruptHandle(pAd, FALSE);
+#endif // CONFIG_STA_SUPPORT //
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		This routine process Rx Irp and call rx complete function.
+
+	Arguments:
+		DeviceObject	Pointer to the device object for next lower
+						device. DeviceObject passed in here belongs to
+						the next lower driver in the stack because we
+						were invoked via IoCallDriver in USB_RxPacket
+						AND it is not OUR device object
+	  Irp				Ptr to completed IRP
+	  Context			Ptr to our Adapter object (context specified
+						in IoSetCompletionRoutine
+
+	Return Value:
+		Always returns STATUS_MORE_PROCESSING_REQUIRED
+
+	Note:
+		Always returns STATUS_MORE_PROCESSING_REQUIRED
+	========================================================================
+*/
+VOID RTUSBBulkRxComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+#if 0
+	PRX_CONTEXT			pRxContext;
+	PRTMP_ADAPTER		pAd;
+	NTSTATUS			Status;
+//	POS_COOKIE			pObj;
+
+	pRxContext	= (PRX_CONTEXT)pUrb->context;
+	pAd 		= pRxContext->pAd;
+//	pObj		= (POS_COOKIE) pAd->OS_Cookie;
+
+
+	Status = pUrb->status;
+	//pRxContext->pIrp = NULL;
+
+	pRxContext->InUse = FALSE;
+	pRxContext->IRPPending = FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		pAd->BulkInComplete++;
+		pRxContext->Readable = TRUE;
+		pAd->NextRxBulkInPosition = 0;
+
+	}
+	else	 // STATUS_OTHER
+	{
+		pAd->BulkInCompleteFail++;
+		// Still read this packet although it may comtain wrong bytes.
+		pRxContext->Readable = FALSE;
+		// Parsing all packets. because after reset, the index will reset to all zero.
+
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+		{
+
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk In Failed. Status = %d\n", Status));
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("==>NextRxBulkInIndex=0x%x, NextRxBulkInReadIndex=0x%x, TransferBufferLength= 0x%x\n",
+				pAd->NextRxBulkInIndex, pAd->NextRxBulkInReadIndex, pRxContext->pUrb->actual_length));
+
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_IN, NULL, 0);
+		}
+		//pUrb = NULL;
+	}
+
+	if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET)) &&
+//		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		RTUSBBulkReceive(pAd);
+#if 0
+#if 1
+		STARxDoneInterruptHandle(pAd, FALSE);
+#else
+		pObj->rx_bh.data = (unsigned long)pUrb;
+		tasklet_schedule(&pObj->rx_bh);
+#endif
+#endif
+	}
+
+	// Call RxPacket to process packet and return the status
+	NdisInterlockedDecrement(&pAd->PendingRx);
+#else
+
+
+	// use a receive tasklet to handle received packets;
+	// or sometimes hardware IRQ will be disabled here, so we can not
+	// use spin_lock_bh()/spin_unlock_bh() after IRQ is disabled. :<
+	PRX_CONTEXT		pRxContext;
+	PRTMP_ADAPTER	pAd;
+	POS_COOKIE 		pObj;
+
+
+	pRxContext	= (PRX_CONTEXT)pUrb->context;
+	pAd 		= pRxContext->pAd;
+	pObj 		= (POS_COOKIE) pAd->OS_Cookie;
+
+	pObj->rx_done_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->rx_done_task);
+#endif
+}
+
+#else
+
+VOID DoBulkIn(IN RTMP_ADAPTER *pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+	pRxContext = &(pAd->RxContext[pAd->NextRxBulkInIndex]);
+	if ((pAd->PendingRx > 0) || (pRxContext->Readable == TRUE) || (pRxContext->InUse == TRUE))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+		return;
+	}
+	pRxContext->InUse = TRUE;
+	pRxContext->IRPPending = TRUE;
+	pAd->PendingRx++;
+	pAd->BulkInReq++;
+	RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+	// Init Rx context descriptor
+	NdisZeroMemory(pRxContext->TransferBuffer, pRxContext->BulkInOffset);
+	RTUSBInitRxDesc(pAd, pRxContext);
+
+	pUrb = pRxContext->pUrb;
+	if ((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{	// fail
+
+		RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+		pRxContext->InUse = FALSE;
+		pRxContext->IRPPending = FALSE;
+		pAd->PendingRx--;
+		pAd->BulkInReq--;
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkReceive: Submit Rx URB failed %d\n", ret));
+	}
+	else
+	{	// success
+#if 0
+		RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+		pRxContext->IRPPending = TRUE;
+		//NdisInterlockedIncrement(&pAd->PendingRx);
+		pAd->PendingRx++;
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+		pAd->BulkInReq++;
+#endif
+		ASSERT((pRxContext->InUse == pRxContext->IRPPending));
+		//printk("BIDone, Pend=%d,BIIdx=%d,BIRIdx=%d!\n", pAd->PendingRx, pAd->NextRxBulkInIndex, pAd->NextRxBulkInReadIndex);
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+	USB_RxPacket initializes a URB and uses the Rx IRP to submit it
+	to USB. It checks if an Rx Descriptor is available and passes the
+	the coresponding buffer to be filled. If no descriptor is available
+	fails the request. When setting the completion routine we pass our
+	Adapter Object as Context.
+
+	Arguments:
+
+	Return Value:
+		TRUE			found matched tuple cache
+		FALSE			no matched found
+
+	Note:
+
+	========================================================================
+*/
+#define fRTMP_ADAPTER_NEED_STOP_RX		\
+		(fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_HALT_IN_PROGRESS |	\
+		 fRTMP_ADAPTER_RADIO_OFF | fRTMP_ADAPTER_RESET_IN_PROGRESS | \
+		 fRTMP_ADAPTER_REMOVE_IN_PROGRESS | fRTMP_ADAPTER_BULKIN_RESET)
+
+#define fRTMP_ADAPTER_NEED_STOP_HANDLE_RX	\
+		(fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_HALT_IN_PROGRESS |	\
+		 fRTMP_ADAPTER_RADIO_OFF | fRTMP_ADAPTER_RESET_IN_PROGRESS | \
+		 fRTMP_ADAPTER_REMOVE_IN_PROGRESS)
+
+VOID	RTUSBBulkReceive(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	unsigned long	IrqFlags;
+
+
+	/* sanity check */
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_HANDLE_RX))
+		return;
+
+	while(1)
+	{
+
+		RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+		pRxContext = &(pAd->RxContext[pAd->NextRxBulkInReadIndex]);
+		if (((pRxContext->InUse == FALSE) && (pRxContext->Readable == TRUE)) &&
+			(pRxContext->bRxHandling == FALSE))
+		{
+			pRxContext->bRxHandling = TRUE;
+			RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+			// read RxContext, Since not
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				STARxDoneInterruptHandle(pAd, TRUE);
+#endif // CONFIG_STA_SUPPORT //
+
+			// Finish to handle this bulkIn buffer.
+			RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+			pRxContext->BulkInOffset = 0;
+			pRxContext->Readable = FALSE;
+			pRxContext->bRxHandling = FALSE;
+			pAd->ReadPosition = 0;
+			pAd->TransferBufferLength = 0;
+			INC_RING_INDEX(pAd->NextRxBulkInReadIndex, RX_RING_SIZE);
+			RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+			break;
+		}
+	}
+
+	if (!(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_RX)))
+		DoBulkIn(pAd);
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		This routine process Rx Irp and call rx complete function.
+
+	Arguments:
+		DeviceObject	Pointer to the device object for next lower
+						device. DeviceObject passed in here belongs to
+						the next lower driver in the stack because we
+						were invoked via IoCallDriver in USB_RxPacket
+						AND it is not OUR device object
+	  Irp				Ptr to completed IRP
+	  Context			Ptr to our Adapter object (context specified
+						in IoSetCompletionRoutine
+
+	Return Value:
+		Always returns STATUS_MORE_PROCESSING_REQUIRED
+
+	Note:
+		Always returns STATUS_MORE_PROCESSING_REQUIRED
+	========================================================================
+*/
+VOID RTUSBBulkRxComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	// use a receive tasklet to handle received packets;
+	// or sometimes hardware IRQ will be disabled here, so we can not
+	// use spin_lock_bh()/spin_unlock_bh() after IRQ is disabled. :<
+	PRX_CONTEXT		pRxContext;
+	PRTMP_ADAPTER	pAd;
+	POS_COOKIE 		pObj;
+
+
+	pRxContext	= (PRX_CONTEXT)pUrb->context;
+	pAd 		= pRxContext->pAd;
+	pObj 		= (POS_COOKIE) pAd->OS_Cookie;
+
+	pObj->rx_done_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->rx_done_task);
+
+}
+
+#endif
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBKickBulkOut(
+	IN	PRTMP_ADAPTER pAd)
+{
+	// BulkIn Reset will reset whole USB PHY. So we need to make sure fRTMP_ADAPTER_BULKIN_RESET not flaged.
+	if (!RTMP_TEST_FLAG(pAd ,fRTMP_ADAPTER_NEED_STOP_TX)
+#ifdef RALINK_ATE
+		&& !(ATE_ON(pAd))
+#endif // RALINK_ATE //
+		)
+	{
+#if 0	// not used now in RT28xx, but may used latter.
+		// 1. Data Fragment has highest priority
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_FRAG))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 0, pAd->NextBulkOutIndex[0]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_FRAG_2))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 1, pAd->NextBulkOutIndex[1]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_FRAG_3))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 2, pAd->NextBulkOutIndex[2]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_FRAG_4))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 3, pAd->NextBulkOutIndex[3]);
+			}
+		}
+#endif
+
+		// 2. PS-Poll frame is next
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_PSPOLL))
+		{
+			RTUSBBulkOutPsPoll(pAd);
+		}
+
+		// 5. Mlme frame is next
+		else if ((RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME)) &&
+				 (pAd->MgmtRing.TxSwFreeIdx < MGMT_RING_SIZE))
+		{
+			RTUSBBulkOutMLMEPacket(pAd, pAd->MgmtRing.TxDmaIdx);
+		}
+
+		// 6. Data frame normal is next
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 0, pAd->NextBulkOutIndex[0]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_2))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 1, pAd->NextBulkOutIndex[1]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_3))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 2, pAd->NextBulkOutIndex[2]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_4))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 3, pAd->NextBulkOutIndex[3]);
+			}
+		}
+
+		// 7. Null frame is the last
+		else if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NULL))
+		{
+			if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			{
+				RTUSBBulkOutNullFrame(pAd);
+			}
+		}
+
+		// 8. No data avaliable
+		else
+		{
+
+		}
+	}
+#ifdef RALINK_ATE
+	/* If the mode is in ATE mode. */
+	else if((ATE_ON(pAd)) &&
+		!RTMP_TEST_FLAG(pAd ,fRTMP_ADAPTER_NEED_STOP_TX))// PETER : watch out !
+	{
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE))
+		{
+			ATE_RTUSBBulkOutDataPacket(pAd, 0);
+		}
+	}
+#endif // RALINK_ATE //
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+	Call from Reset action after BulkOut failed.
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCleanUpDataBulkOutQueue(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR			Idx;
+	PHT_TX_CONTEXT	pTxContext;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->CleanUpDataBulkOutQueue\n"));
+
+	for (Idx = 0; Idx < 4; Idx++)
+	{
+		pTxContext = &pAd->TxContext[Idx];
+
+		pTxContext->CurWritePosition = pTxContext->NextBulkOutPosition;
+		pTxContext->LastOne = FALSE;
+		NdisAcquireSpinLock(&pAd->BulkOutLock[Idx]);
+		pAd->BulkOutPending[Idx] = FALSE;
+		NdisReleaseSpinLock(&pAd->BulkOutLock[Idx]);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<---CleanUpDataBulkOutQueue\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCleanUpMLMEBulkOutQueue(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("--->CleanUpMLMEBulkOutQueue\n"));
+
+#if 0	// Do nothing!
+	NdisAcquireSpinLock(&pAd->MLMEBulkOutLock);
+	while (pAd->PrioRingTxCnt > 0)
+	{
+		pAd->MLMEContext[pAd->PrioRingFirstIndex].InUse = FALSE;
+
+		pAd->PrioRingFirstIndex++;
+		if (pAd->PrioRingFirstIndex >= MGMT_RING_SIZE)
+		{
+			pAd->PrioRingFirstIndex = 0;
+		}
+
+		pAd->PrioRingTxCnt--;
+	}
+	NdisReleaseSpinLock(&pAd->MLMEBulkOutLock);
+#endif
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<---CleanUpMLMEBulkOutQueue\n"));
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCancelPendingIRPs(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	RTUSBCancelPendingBulkInIRP(pAd);
+	RTUSBCancelPendingBulkOutIRP(pAd);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCancelPendingBulkInIRP(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	UINT			i;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("--->RTUSBCancelPendingBulkInIRP\n"));
+	for ( i = 0; i < (RX_RING_SIZE); i++)
+	{
+		pRxContext = &(pAd->RxContext[i]);
+		if(pRxContext->IRPPending == TRUE)
+		{
+			RTUSB_UNLINK_URB(pRxContext->pUrb);
+			pRxContext->IRPPending = FALSE;
+			pRxContext->InUse = FALSE;
+			//NdisInterlockedDecrement(&pAd->PendingRx);
+			//pAd->PendingRx--;
+		}
+	}
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("<---RTUSBCancelPendingBulkInIRP\n"));
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCancelPendingBulkOutIRP(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PHT_TX_CONTEXT		pHTTXContext;
+	PTX_CONTEXT			pMLMEContext;
+	PTX_CONTEXT			pBeaconContext;
+	PTX_CONTEXT			pNullContext;
+	PTX_CONTEXT			pPsPollContext;
+	PTX_CONTEXT			pRTSContext;
+	UINT				i, Idx;
+//	unsigned int 		IrqFlags;
+//	NDIS_SPIN_LOCK		*pLock;
+//	BOOLEAN				*pPending;
+
+
+//	pLock = &pAd->BulkOutLock[MGMTPIPEIDX];
+//	pPending = &pAd->BulkOutPending[MGMTPIPEIDX];
+
+	for (Idx = 0; Idx < 4; Idx++)
+	{
+		pHTTXContext = &(pAd->TxContext[Idx]);
+
+		if (pHTTXContext->IRPPending == TRUE)
+		{
+
+			// Get the USB_CONTEXT and cancel it's IRP; the completion routine will itself
+			// remove it from the HeadPendingSendList and NULL out HeadPendingSendList
+			//	when the last IRP on the list has been	cancelled; that's how we exit this loop
+			//
+
+			RTUSB_UNLINK_URB(pHTTXContext->pUrb);
+
+			// Sleep 200 microseconds to give cancellation time to work
+			RTMPusecDelay(200);
+		}
+
+#ifdef RALINK_ATE
+		pHTTXContext->bCopySavePad = 0;
+		pHTTXContext->CurWritePosition = 0;
+		pHTTXContext->CurWriteRealPos = 0;
+		pHTTXContext->bCurWriting = FALSE;
+		pHTTXContext->NextBulkOutPosition = 0;
+		pHTTXContext->ENextBulkOutPosition = 0;
+#endif // RALINK_ATE //
+		pAd->BulkOutPending[Idx] = FALSE;
+	}
+
+	//RTMP_IRQ_LOCK(pLock, IrqFlags);
+	for (i = 0; i < MGMT_RING_SIZE; i++)
+	{
+		pMLMEContext = (PTX_CONTEXT)pAd->MgmtRing.Cell[i].AllocVa;
+		if(pMLMEContext && (pMLMEContext->IRPPending == TRUE))
+		{
+
+			// Get the USB_CONTEXT and cancel it's IRP; the completion routine will itself
+			// remove it from the HeadPendingSendList and NULL out HeadPendingSendList
+			//	when the last IRP on the list has been	cancelled; that's how we exit this loop
+			//
+
+			RTUSB_UNLINK_URB(pMLMEContext->pUrb);
+			pMLMEContext->IRPPending = FALSE;
+
+			// Sleep 200 microsecs to give cancellation time to work
+			RTMPusecDelay(200);
+		}
+	}
+	pAd->BulkOutPending[MGMTPIPEIDX] = FALSE;
+	//RTMP_IRQ_UNLOCK(pLock, IrqFlags);
+
+
+	for (i = 0; i < BEACON_RING_SIZE; i++)
+	{
+		pBeaconContext = &(pAd->BeaconContext[i]);
+
+		if(pBeaconContext->IRPPending == TRUE)
+		{
+
+			// Get the USB_CONTEXT and cancel it's IRP; the completion routine will itself
+			// remove it from the HeadPendingSendList and NULL out HeadPendingSendList
+			//	when the last IRP on the list has been	cancelled; that's how we exit this loop
+			//
+
+			RTUSB_UNLINK_URB(pBeaconContext->pUrb);
+
+			// Sleep 200 microsecs to give cancellation time to work
+			RTMPusecDelay(200);
+		}
+	}
+
+	pNullContext = &(pAd->NullContext);
+	if (pNullContext->IRPPending == TRUE)
+		RTUSB_UNLINK_URB(pNullContext->pUrb);
+
+	pRTSContext = &(pAd->RTSContext);
+	if (pRTSContext->IRPPending == TRUE)
+		RTUSB_UNLINK_URB(pRTSContext->pUrb);
+
+	pPsPollContext = &(pAd->PsPollContext);
+	if (pPsPollContext->IRPPending == TRUE)
+		RTUSB_UNLINK_URB(pPsPollContext->pUrb);
+
+	for (Idx = 0; Idx < 4; Idx++)
+	{
+		NdisAcquireSpinLock(&pAd->BulkOutLock[Idx]);
+		pAd->BulkOutPending[Idx] = FALSE;
+		NdisReleaseSpinLock(&pAd->BulkOutLock[Idx]);
+	}
+}
+
diff --git a/drivers/staging/rt2870/common/rtusb_data.c b/drivers/staging/rt2870/common/rtusb_data.c
new file mode 100644
index 00000000000..5a0d78389f4
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtusb_data.c
@@ -0,0 +1,229 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtusb_data.c
+
+	Abstract:
+	Ralink USB driver Tx/Rx functions.
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Jan            03-25-2006    created
+
+*/
+#include "../rt_config.h"
+
+extern  UCHAR Phy11BGNextRateUpward[]; // defined in mlme.c
+extern UCHAR	EpToQueue[];
+
+VOID REPORT_AMSDU_FRAMES_TO_LLC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize)
+{
+	PNDIS_PACKET	pPacket;
+	UINT			nMSDU;
+	struct			sk_buff *pSkb;
+
+	nMSDU = 0;
+	/* allocate a rx packet */
+	pSkb = dev_alloc_skb(RX_BUFFER_AGGRESIZE);
+	pPacket = (PNDIS_PACKET)OSPKT_TO_RTPKT(pSkb);
+	if (pSkb)
+	{
+
+		/* convert 802.11 to 802.3 packet */
+		pSkb->dev = get_netdev_from_bssid(pAd, BSS0);
+		RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
+		deaggregate_AMSDU_announce(pAd, pPacket, pData, DataSize);
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("Can't allocate skb\n"));
+	}
+}
+
+NDIS_STATUS	RTUSBFreeDescriptorRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId,
+	IN	UINT32			NumberRequired)
+{
+//	UCHAR			FreeNumber = 0;
+//	UINT			Index;
+	NDIS_STATUS		Status = NDIS_STATUS_FAILURE;
+	unsigned long   IrqFlags;
+	HT_TX_CONTEXT	*pHTTXContext;
+
+
+	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	if ((pHTTXContext->CurWritePosition < pHTTXContext->NextBulkOutPosition) && ((pHTTXContext->CurWritePosition + NumberRequired + LOCAL_TXBUF_SIZE) > pHTTXContext->NextBulkOutPosition))
+	{
+
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	else if ((pHTTXContext->CurWritePosition == 8) && (pHTTXContext->NextBulkOutPosition < (NumberRequired + LOCAL_TXBUF_SIZE)))
+	{
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	else if (pHTTXContext->bCurWriting == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("RTUSBFreeD c3 --> QueIdx=%d, CWPos=%ld, NBOutPos=%ld!\n", BulkOutPipeId, pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition));
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	else
+	{
+		Status = NDIS_STATUS_SUCCESS;
+	}
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+
+	return (Status);
+}
+
+NDIS_STATUS RTUSBFreeDescriptorRelease(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		BulkOutPipeId)
+{
+	unsigned long   IrqFlags;
+	HT_TX_CONTEXT	*pHTTXContext;
+
+	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	pHTTXContext->bCurWriting = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+	return (NDIS_STATUS_SUCCESS);
+}
+
+
+BOOLEAN	RTUSBNeedQueueBackForAgg(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		BulkOutPipeId)
+{
+	unsigned long   IrqFlags;
+	HT_TX_CONTEXT	*pHTTXContext;
+	BOOLEAN			needQueBack = FALSE;
+
+	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	if ((pHTTXContext->IRPPending == TRUE)  /*&& (pAd->TxSwQueue[BulkOutPipeId].Number == 0) */)
+	{
+#if 0
+		if ((pHTTXContext->CurWritePosition <= 8) &&
+			(pHTTXContext->NextBulkOutPosition > 8 && (pHTTXContext->NextBulkOutPosition+MAX_AGGREGATION_SIZE) < MAX_TXBULK_LIMIT))
+		{
+			needQueBack = TRUE;
+		}
+		else if ((pHTTXContext->CurWritePosition < pHTTXContext->NextBulkOutPosition) &&
+				 ((pHTTXContext->NextBulkOutPosition + MAX_AGGREGATION_SIZE) < MAX_TXBULK_LIMIT))
+		{
+			needQueBack = TRUE;
+		}
+#else
+		if ((pHTTXContext->CurWritePosition < pHTTXContext->ENextBulkOutPosition) &&
+			(((pHTTXContext->ENextBulkOutPosition+MAX_AGGREGATION_SIZE) < MAX_TXBULK_LIMIT) || (pHTTXContext->CurWritePosition > MAX_AGGREGATION_SIZE)))
+		{
+			needQueBack = TRUE;
+		}
+#endif
+		else if ((pHTTXContext->CurWritePosition > pHTTXContext->ENextBulkOutPosition) &&
+				 ((pHTTXContext->ENextBulkOutPosition + MAX_AGGREGATION_SIZE) < pHTTXContext->CurWritePosition))
+		{
+			needQueBack = TRUE;
+		}
+	}
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+	return needQueBack;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBRejectPendingPackets(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR			Index;
+	PQUEUE_ENTRY	pEntry;
+	PNDIS_PACKET	pPacket;
+	PQUEUE_HEADER	pQueue;
+
+
+	for (Index = 0; Index < 4; Index++)
+	{
+		NdisAcquireSpinLock(&pAd->TxSwQueueLock[Index]);
+		while (pAd->TxSwQueue[Index].Head != NULL)
+		{
+			pQueue = (PQUEUE_HEADER) &(pAd->TxSwQueue[Index]);
+			pEntry = RemoveHeadQueue(pQueue);
+			pPacket = QUEUE_ENTRY_TO_PACKET(pEntry);
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		}
+		NdisReleaseSpinLock(&pAd->TxSwQueueLock[Index]);
+
+	}
+
+}
+
+VOID RTMPWriteTxInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXINFO_STRUC 	pTxInfo,
+	IN	  USHORT		USBDMApktLen,
+	IN	  BOOLEAN		bWiv,
+	IN	  UCHAR			QueueSel,
+	IN	  UCHAR			NextValid,
+	IN	  UCHAR			TxBurst)
+{
+	pTxInfo->USBDMATxPktLen = USBDMApktLen;
+	pTxInfo->QSEL = QueueSel;
+	if (QueueSel != FIFO_EDCA)
+		DBGPRINT(RT_DEBUG_TRACE, ("====> QueueSel != FIFO_EDCA<============\n"));
+	pTxInfo->USBDMANextVLD = FALSE; //NextValid;  // Need to check with Jan about this.
+	pTxInfo->USBDMATxburst = TxBurst;
+	pTxInfo->WIV = bWiv;
+	pTxInfo->SwUseLastRound = 0;
+	pTxInfo->rsv = 0;
+	pTxInfo->rsv2 = 0;
+}
+
diff --git a/drivers/staging/rt2870/common/rtusb_io.c b/drivers/staging/rt2870/common/rtusb_io.c
new file mode 100644
index 00000000000..6db443e9268
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtusb_io.c
@@ -0,0 +1,2006 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+ 	Module Name:
+	rtusb_io.c
+
+	Abstract:
+
+	Revision History:
+	Who			When	    What
+	--------	----------  ----------------------------------------------
+	Name		Date	    Modification logs
+	Paul Lin    06-25-2004  created
+*/
+
+#include "../rt_config.h"
+
+
+/*
+	========================================================================
+
+	Routine Description: NIC initialization complete
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+
+NTSTATUS	RTUSBFirmwareRun(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x01,
+		0x8,
+		0,
+		NULL,
+		0);
+
+	return Status;
+}
+
+
+
+/*
+	========================================================================
+
+	Routine Description: Write Firmware to NIC.
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS RTUSBFirmwareWrite(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR		pFwImage,
+	IN ULONG		FwLen)
+{
+	UINT32		MacReg;
+	NTSTATUS 	Status;
+//	ULONG 		i;
+	USHORT		writeLen;
+
+	Status = RTUSBReadMACRegister(pAd, MAC_CSR0, &MacReg);
+
+
+	writeLen = FwLen;
+	RTUSBMultiWrite(pAd, FIRMWARE_IMAGE_BASE, pFwImage, writeLen);
+
+	Status = RTUSBWriteMACRegister(pAd, 0x7014, 0xffffffff);
+	Status = RTUSBWriteMACRegister(pAd, 0x701c, 0xffffffff);
+	Status = RTUSBFirmwareRun(pAd);
+
+	return Status;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description: Get current firmware operation mode (Return Value)
+
+	Arguments:
+
+	Return Value:
+		0 or 1 = Downloaded by host driver
+		others = Driver doesn't download firmware
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBFirmwareOpmode(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUINT32			pValue)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
+		DEVICE_VENDOR_REQUEST_IN,
+		0x1,
+		0x11,
+		0,
+		pValue,
+		4);
+	return Status;
+}
+NTSTATUS	RTUSBVenderReset(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NTSTATUS	Status;
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("-->RTUSBVenderReset\n"));
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x01,
+		0x1,
+		0,
+		NULL,
+		0);
+
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("<--RTUSBVenderReset\n"));
+	return Status;
+}
+/*
+	========================================================================
+
+	Routine Description: Read various length data from RT2573
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBMultiRead(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUCHAR			pData,
+	IN	USHORT			length)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
+		DEVICE_VENDOR_REQUEST_IN,
+		0x7,
+		0,
+		Offset,
+		pData,
+		length);
+
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description: Write various length data to RT2573
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBMultiWrite_OneByte(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData)
+{
+	NTSTATUS	Status;
+
+	// TODO: In 2870, use this funciton carefully cause it's not stable.
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x6,
+		0,
+		Offset,
+		pData,
+		1);
+
+	return Status;
+}
+
+NTSTATUS	RTUSBMultiWrite(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData,
+	IN	USHORT			length)
+{
+	NTSTATUS	Status;
+
+
+        USHORT          index = 0,Value;
+        PUCHAR          pSrc = pData;
+        USHORT          resude = 0;
+
+        resude = length % 2;
+		length  += resude;
+		do
+		{
+			Value =(USHORT)( *pSrc  | (*(pSrc + 1) << 8));
+		Status = RTUSBSingleWrite(pAd,Offset + index,Value);
+            index +=2;
+            length -= 2;
+            pSrc = pSrc + 2;
+        }while(length > 0);
+
+	return Status;
+}
+
+
+NTSTATUS RTUSBSingleWrite(
+	IN 	RTMP_ADAPTER 	*pAd,
+	IN	USHORT			Offset,
+	IN	USHORT			Value)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x2,
+		Value,
+		Offset,
+		NULL,
+		0);
+
+	return Status;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description: Read 32-bit MAC register
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBReadMACRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUINT32			pValue)
+{
+	NTSTATUS	Status;
+	UINT32		localVal;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
+		DEVICE_VENDOR_REQUEST_IN,
+		0x7,
+		0,
+		Offset,
+		&localVal,
+		4);
+
+	*pValue = le2cpu32(localVal);
+
+
+	if (Status < 0)
+		*pValue = 0xffffffff;
+
+	return Status;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description: Write 32-bit MAC register
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBWriteMACRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	UINT32			Value)
+{
+	NTSTATUS	Status;
+	UINT32		localVal;
+
+	localVal = Value;
+
+	Status = RTUSBSingleWrite(pAd, Offset, (USHORT)(localVal & 0xffff));
+	Status = RTUSBSingleWrite(pAd, Offset + 2, (USHORT)((localVal & 0xffff0000) >> 16));
+
+	return Status;
+}
+
+
+
+#if 1
+/*
+	========================================================================
+
+	Routine Description: Read 8-bit BBP register
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBReadBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	PUCHAR			pValue)
+{
+	BBP_CSR_CFG_STRUC	BbpCsr;
+	UINT			i = 0;
+	NTSTATUS		status;
+
+	// Verify the busy condition
+	do
+	{
+		status = RTUSBReadMACRegister(pAd, BBP_CSR_CFG, &BbpCsr.word);
+		if(status >= 0)
+		{
+		if (!(BbpCsr.field.Busy == BUSY))
+			break;
+		}
+		printk("RTUSBReadBBPRegister(BBP_CSR_CFG_1):retry count=%d!\n", i);
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		//
+		// Read failed then Return Default value.
+		//
+		*pValue = pAd->BbpWriteLatch[Id];
+
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	// Prepare for write material
+	BbpCsr.word 				= 0;
+	BbpCsr.field.fRead			= 1;
+	BbpCsr.field.Busy			= 1;
+	BbpCsr.field.RegNum 		= Id;
+	RTUSBWriteMACRegister(pAd, BBP_CSR_CFG, BbpCsr.word);
+
+	i = 0;
+	// Verify the busy condition
+	do
+	{
+		status = RTUSBReadMACRegister(pAd, BBP_CSR_CFG, &BbpCsr.word);
+		if (status >= 0)
+		{
+		if (!(BbpCsr.field.Busy == BUSY))
+		{
+			*pValue = (UCHAR)BbpCsr.field.Value;
+			break;
+		}
+		}
+		printk("RTUSBReadBBPRegister(BBP_CSR_CFG_2):retry count=%d!\n", i);
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		//
+		// Read failed then Return Default value.
+		//
+		*pValue = pAd->BbpWriteLatch[Id];
+
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	return STATUS_SUCCESS;
+}
+#else
+/*
+	========================================================================
+
+	Routine Description: Read 8-bit BBP register via firmware
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBReadBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	PUCHAR			pValue)
+{
+	BBP_CSR_CFG_STRUC	BbpCsr;
+	int					i, k;
+	for (i=0; i<MAX_BUSY_COUNT; i++)
+	{
+		RTUSBReadMACRegister(pAd, H2M_BBP_AGENT, &BbpCsr.word);
+		if (BbpCsr.field.Busy == BUSY)
+		{
+			continue;
+		}
+		BbpCsr.word = 0;
+		BbpCsr.field.fRead = 1;
+		BbpCsr.field.BBP_RW_MODE = 1;
+		BbpCsr.field.Busy = 1;
+		BbpCsr.field.RegNum = Id;
+		RTUSBWriteMACRegister(pAd, H2M_BBP_AGENT, BbpCsr.word);
+		AsicSendCommandToMcu(pAd, 0x80, 0xff, 0x0, 0x0);
+		for (k=0; k<MAX_BUSY_COUNT; k++)
+		{
+			RTUSBReadMACRegister(pAd, H2M_BBP_AGENT, &BbpCsr.word);
+			if (BbpCsr.field.Busy == IDLE)
+				break;
+		}
+		if ((BbpCsr.field.Busy == IDLE) &&
+			(BbpCsr.field.RegNum == Id))
+		{
+			*pValue = (UCHAR)BbpCsr.field.Value;
+			break;
+		}
+	}
+	if (BbpCsr.field.Busy == BUSY)
+	{
+		DBGPRINT_ERR(("BBP read R%d=0x%x fail\n", Id, BbpCsr.word));
+		*pValue = pAd->BbpWriteLatch[Id];
+		return STATUS_UNSUCCESSFUL;
+	}
+	return STATUS_SUCCESS;
+}
+#endif
+
+#if 1
+/*
+	========================================================================
+
+	Routine Description: Write 8-bit BBP register
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBWriteBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	UCHAR			Value)
+{
+	BBP_CSR_CFG_STRUC	BbpCsr;
+	UINT			i = 0;
+	NTSTATUS		status;
+	// Verify the busy condition
+	do
+	{
+		status = RTUSBReadMACRegister(pAd, BBP_CSR_CFG, &BbpCsr.word);
+		if (status >= 0)
+		{
+		if (!(BbpCsr.field.Busy == BUSY))
+			break;
+		}
+		printk("RTUSBWriteBBPRegister(BBP_CSR_CFG):retry count=%d!\n", i);
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	// Prepare for write material
+	BbpCsr.word 				= 0;
+	BbpCsr.field.fRead			= 0;
+	BbpCsr.field.Value			= Value;
+	BbpCsr.field.Busy			= 1;
+	BbpCsr.field.RegNum 		= Id;
+	RTUSBWriteMACRegister(pAd, BBP_CSR_CFG, BbpCsr.word);
+
+	pAd->BbpWriteLatch[Id] = Value;
+
+	return STATUS_SUCCESS;
+}
+#else
+/*
+	========================================================================
+
+	Routine Description: Write 8-bit BBP register via firmware
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+
+NTSTATUS	RTUSBWriteBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	UCHAR			Value)
+
+{
+	BBP_CSR_CFG_STRUC	BbpCsr;
+	int					BusyCnt;
+	for (BusyCnt=0; BusyCnt<MAX_BUSY_COUNT; BusyCnt++)
+	{
+		RTMP_IO_READ32(pAd, H2M_BBP_AGENT, &BbpCsr.word);
+		if (BbpCsr.field.Busy == BUSY)
+			continue;
+		BbpCsr.word = 0;
+		BbpCsr.field.fRead = 0;
+		BbpCsr.field.BBP_RW_MODE = 1;
+		BbpCsr.field.Busy = 1;
+		BbpCsr.field.Value = Value;
+		BbpCsr.field.RegNum = Id;
+		RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, BbpCsr.word);
+		AsicSendCommandToMcu(pAd, 0x80, 0xff, 0x0, 0x0);
+		pAd->BbpWriteLatch[Id] = Value;
+		break;
+	}
+	if (BusyCnt == MAX_BUSY_COUNT)
+	{
+		DBGPRINT_ERR(("BBP write R%d=0x%x fail\n", Id, BbpCsr.word));
+		return STATUS_UNSUCCESSFUL;
+	}
+	return STATUS_SUCCESS;
+}
+#endif
+/*
+	========================================================================
+
+	Routine Description: Write RF register through MAC
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBWriteRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT32			Value)
+{
+	PHY_CSR4_STRUC	PhyCsr4;
+	UINT			i = 0;
+	NTSTATUS		status;
+
+	NdisZeroMemory(&PhyCsr4, sizeof(PHY_CSR4_STRUC));
+	do
+	{
+		status = RTUSBReadMACRegister(pAd, RF_CSR_CFG0, &PhyCsr4.word);
+		if (status >= 0)
+		{
+		if (!(PhyCsr4.field.Busy))
+			break;
+		}
+		printk("RTUSBWriteRFRegister(RF_CSR_CFG0):retry count=%d!\n", i);
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	RTUSBWriteMACRegister(pAd, RF_CSR_CFG0, Value);
+
+	return STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description: Write RT3070 RF register through MAC
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RT30xxWriteRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			RegID,
+	IN	UCHAR			Value)
+{
+	RF_CSR_CFG_STRUC	rfcsr;
+	UINT				i = 0;
+
+	do
+	{
+		RTUSBReadMACRegister(pAd, RF_CSR_CFG, &rfcsr.word);
+
+		if (!rfcsr.field.RF_CSR_KICK)
+			break;
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	rfcsr.field.RF_CSR_WR = 1;
+	rfcsr.field.RF_CSR_KICK = 1;
+	rfcsr.field.TESTCSR_RFACC_REGNUM = RegID;
+	rfcsr.field.RF_CSR_DATA = Value;
+
+	RTUSBWriteMACRegister(pAd, RF_CSR_CFG, rfcsr.word);
+
+	return STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description: Read RT3070 RF register through MAC
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RT30xxReadRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			RegID,
+	IN	PUCHAR			pValue)
+{
+	RF_CSR_CFG_STRUC	rfcsr;
+	UINT				i=0, k;
+
+	for (i=0; i<MAX_BUSY_COUNT; i++)
+	{
+		RTUSBReadMACRegister(pAd, RF_CSR_CFG, &rfcsr.word);
+
+		if (rfcsr.field.RF_CSR_KICK == BUSY)
+		{
+			continue;
+		}
+		rfcsr.word = 0;
+		rfcsr.field.RF_CSR_WR = 0;
+		rfcsr.field.RF_CSR_KICK = 1;
+		rfcsr.field.TESTCSR_RFACC_REGNUM = RegID;
+		RTUSBWriteMACRegister(pAd, RF_CSR_CFG, rfcsr.word);
+		for (k=0; k<MAX_BUSY_COUNT; k++)
+		{
+			RTUSBReadMACRegister(pAd, RF_CSR_CFG, &rfcsr.word);
+
+			if (rfcsr.field.RF_CSR_KICK == IDLE)
+				break;
+		}
+		if ((rfcsr.field.RF_CSR_KICK == IDLE) &&
+			(rfcsr.field.TESTCSR_RFACC_REGNUM == RegID))
+		{
+			*pValue = (UCHAR)rfcsr.field.RF_CSR_DATA;
+			break;
+		}
+	}
+	if (rfcsr.field.RF_CSR_KICK == BUSY)
+	{
+		DBGPRINT_ERR(("RF read R%d=0x%x fail\n", RegID, rfcsr.word));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBReadEEPROM(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUCHAR			pData,
+	IN	USHORT			length)
+{
+	NTSTATUS	Status = STATUS_SUCCESS;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
+		DEVICE_VENDOR_REQUEST_IN,
+		0x9,
+		0,
+		Offset,
+		pData,
+		length);
+
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBWriteEEPROM(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData,
+	IN	USHORT			length)
+{
+	NTSTATUS	Status = STATUS_SUCCESS;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x8,
+		0,
+		Offset,
+		pData,
+		length);
+
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+VOID RTUSBPutToSleep(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UINT32		value;
+
+	// Timeout 0x40 x 50us
+	value = (SLEEPCID<<16)+(OWNERMCU<<24)+ (0x40<<8)+1;
+	RTUSBWriteMACRegister(pAd, 0x7010, value);
+	RTUSBWriteMACRegister(pAd, 0x404, 0x30);
+	//RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("Sleep Mailbox testvalue %x\n", value));
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS RTUSBWakeUp(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x01,
+		0x09,
+		0,
+		NULL,
+		0);
+
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBInitializeCmdQ(
+	IN	PCmdQ	cmdq)
+{
+	cmdq->head = NULL;
+	cmdq->tail = NULL;
+	cmdq->size = 0;
+	cmdq->CmdQState = RT2870_THREAD_INITED;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTUSBEnqueueCmdFromNdis(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	NDIS_OID		Oid,
+	IN	BOOLEAN			SetInformation,
+	IN	PVOID			pInformationBuffer,
+	IN	UINT32			InformationBufferLength)
+{
+	NDIS_STATUS	status;
+	PCmdQElmt	cmdqelmt = NULL;
+	POS_COOKIE pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+
+	CHECK_PID_LEGALITY(pObj->RTUSBCmdThr_pid)
+		return (NDIS_STATUS_RESOURCES);
+
+	status = RTMPAllocateMemory((PVOID *)&cmdqelmt, sizeof(CmdQElmt));
+	if ((status != NDIS_STATUS_SUCCESS) || (cmdqelmt == NULL))
+		return (NDIS_STATUS_RESOURCES);
+
+		cmdqelmt->buffer = NULL;
+		if (pInformationBuffer != NULL)
+		{
+			status = RTMPAllocateMemory((PVOID *)&cmdqelmt->buffer, InformationBufferLength);
+			if ((status != NDIS_STATUS_SUCCESS) || (cmdqelmt->buffer == NULL))
+			{
+				kfree(cmdqelmt);
+				return (NDIS_STATUS_RESOURCES);
+			}
+			else
+			{
+				NdisMoveMemory(cmdqelmt->buffer, pInformationBuffer, InformationBufferLength);
+				cmdqelmt->bufferlength = InformationBufferLength;
+			}
+		}
+		else
+			cmdqelmt->bufferlength = 0;
+
+	cmdqelmt->command = Oid;
+	cmdqelmt->CmdFromNdis = TRUE;
+	if (SetInformation == TRUE)
+		cmdqelmt->SetOperation = TRUE;
+	else
+		cmdqelmt->SetOperation = FALSE;
+
+	NdisAcquireSpinLock(&pAd->CmdQLock);
+	if (pAd->CmdQ.CmdQState & RT2870_THREAD_CAN_DO_INSERT)
+	{
+		EnqueueCmd((&pAd->CmdQ), cmdqelmt);
+		status = NDIS_STATUS_SUCCESS;
+	}
+	else
+	{
+		status = NDIS_STATUS_FAILURE;
+	}
+	NdisReleaseSpinLock(&pAd->CmdQLock);
+
+	if (status == NDIS_STATUS_FAILURE)
+	{
+		if (cmdqelmt->buffer)
+			NdisFreeMemory(cmdqelmt->buffer, cmdqelmt->bufferlength, 0);
+		NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+	}
+	else
+	RTUSBCMDUp(pAd);
+
+
+    return(NDIS_STATUS_SUCCESS);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS RTUSBEnqueueInternalCmd(
+	IN PRTMP_ADAPTER	pAd,
+	IN NDIS_OID			Oid,
+	IN PVOID			pInformationBuffer,
+	IN UINT32			InformationBufferLength)
+{
+	NDIS_STATUS	status;
+	PCmdQElmt	cmdqelmt = NULL;
+
+
+	status = RTMPAllocateMemory((PVOID *)&cmdqelmt, sizeof(CmdQElmt));
+	if ((status != NDIS_STATUS_SUCCESS) || (cmdqelmt == NULL))
+		return (NDIS_STATUS_RESOURCES);
+	NdisZeroMemory(cmdqelmt, sizeof(CmdQElmt));
+
+	if(InformationBufferLength > 0)
+	{
+		status = RTMPAllocateMemory((PVOID *)&cmdqelmt->buffer, InformationBufferLength);
+		if ((status != NDIS_STATUS_SUCCESS) || (cmdqelmt->buffer == NULL))
+		{
+			NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+			return (NDIS_STATUS_RESOURCES);
+		}
+		else
+		{
+			NdisMoveMemory(cmdqelmt->buffer, pInformationBuffer, InformationBufferLength);
+			cmdqelmt->bufferlength = InformationBufferLength;
+		}
+	}
+	else
+	{
+		cmdqelmt->buffer = NULL;
+		cmdqelmt->bufferlength = 0;
+	}
+
+	cmdqelmt->command = Oid;
+	cmdqelmt->CmdFromNdis = FALSE;
+
+	if (cmdqelmt != NULL)
+	{
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		if (pAd->CmdQ.CmdQState & RT2870_THREAD_CAN_DO_INSERT)
+		{
+			EnqueueCmd((&pAd->CmdQ), cmdqelmt);
+			status = NDIS_STATUS_SUCCESS;
+		}
+		else
+		{
+			status = NDIS_STATUS_FAILURE;
+		}
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+
+		if (status == NDIS_STATUS_FAILURE)
+		{
+			if (cmdqelmt->buffer)
+				NdisFreeMemory(cmdqelmt->buffer, cmdqelmt->bufferlength, 0);
+			NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+		}
+		else
+		RTUSBCMDUp(pAd);
+	}
+	return(NDIS_STATUS_SUCCESS);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBDequeueCmd(
+	IN	PCmdQ		cmdq,
+	OUT	PCmdQElmt	*pcmdqelmt)
+{
+	*pcmdqelmt = cmdq->head;
+
+	if (*pcmdqelmt != NULL)
+	{
+		cmdq->head = cmdq->head->next;
+		cmdq->size--;
+		if (cmdq->size == 0)
+			cmdq->tail = NULL;
+	}
+}
+
+/*
+    ========================================================================
+	  usb_control_msg - Builds a control urb, sends it off and waits for completion
+	  @dev: pointer to the usb device to send the message to
+	  @pipe: endpoint "pipe" to send the message to
+	  @request: USB message request value
+	  @requesttype: USB message request type value
+	  @value: USB message value
+	  @index: USB message index value
+	  @data: pointer to the data to send
+	  @size: length in bytes of the data to send
+	  @timeout: time in jiffies to wait for the message to complete before
+			  timing out (if 0 the wait is forever)
+	  Context: !in_interrupt ()
+
+	  This function sends a simple control message to a specified endpoint
+	  and waits for the message to complete, or timeout.
+	  If successful, it returns the number of bytes transferred, otherwise a negative error number.
+
+	 Don't use this function from within an interrupt context, like a
+	  bottom half handler.	If you need an asynchronous message, or need to send
+	  a message from within interrupt context, use usb_submit_urb()
+	  If a thread in your driver uses this call, make sure your disconnect()
+	  method can wait for it to complete.  Since you don't have a handle on
+	  the URB used, you can't cancel the request.
+
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS    RTUSB_VendorRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT32			TransferFlags,
+	IN	UCHAR			RequestType,
+	IN	UCHAR			Request,
+	IN	USHORT			Value,
+	IN	USHORT			Index,
+	IN	PVOID			TransferBuffer,
+	IN	UINT32			TransferBufferLength)
+{
+	int				ret;
+	POS_COOKIE		pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("device disconnected\n"));
+		return -1;
+	}
+	else if (in_interrupt())
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("in_interrupt, RTUSB_VendorRequest Request%02x Value%04x Offset%04x\n",Request,Value,Index));
+
+		return -1;
+	}
+	else
+	{
+#define MAX_RETRY_COUNT  10
+
+		int retryCount = 0;
+		void	*tmpBuf = TransferBuffer;
+
+		// Acquire Control token
+#ifdef INF_AMAZON_SE
+		//Semaphore fix INF_AMAZON_SE hang
+		//pAd->UsbVendorReqBuf is the swap for DEVICE_VENDOR_REQUEST_IN to fix dma bug.
+		ret = down_interruptible(&(pAd->UsbVendorReq_semaphore));
+		if (pAd->UsbVendorReqBuf)
+		{
+			ASSERT(TransferBufferLength <MAX_PARAM_BUFFER_SIZE);
+
+		   	tmpBuf = (void *)pAd->UsbVendorReqBuf;
+		   	NdisZeroMemory(pAd->UsbVendorReqBuf, TransferBufferLength);
+
+		   	if (RequestType == DEVICE_VENDOR_REQUEST_OUT)
+		   	 NdisMoveMemory(tmpBuf, TransferBuffer, TransferBufferLength);
+		}
+#endif // INF_AMAZON_SE //
+		do {
+		if( RequestType == DEVICE_VENDOR_REQUEST_OUT)
+			ret=usb_control_msg(pObj->pUsb_Dev, usb_sndctrlpipe( pObj->pUsb_Dev, 0 ), Request, RequestType, Value,Index, tmpBuf, TransferBufferLength, CONTROL_TIMEOUT_JIFFIES);
+		else if(RequestType == DEVICE_VENDOR_REQUEST_IN)
+			ret=usb_control_msg(pObj->pUsb_Dev, usb_rcvctrlpipe( pObj->pUsb_Dev, 0 ), Request, RequestType, Value,Index, tmpBuf, TransferBufferLength, CONTROL_TIMEOUT_JIFFIES);
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("vendor request direction is failed\n"));
+			ret = -1;
+		}
+
+			retryCount++;
+			if (ret < 0) {
+				printk("#\n");
+				RTMPusecDelay(5000);
+			}
+		} while((ret < 0) && (retryCount < MAX_RETRY_COUNT));
+
+#ifdef INF_AMAZON_SE
+	  	if ((pAd->UsbVendorReqBuf) && (RequestType == DEVICE_VENDOR_REQUEST_IN))
+			NdisMoveMemory(TransferBuffer, tmpBuf, TransferBufferLength);
+	  	up(&(pAd->UsbVendorReq_semaphore));
+#endif // INF_AMAZON_SE //
+
+        if (ret < 0) {
+//			DBGPRINT(RT_DEBUG_ERROR, ("USBVendorRequest failed ret=%d \n",ret));
+			DBGPRINT(RT_DEBUG_ERROR, ("RTUSB_VendorRequest failed(%d),TxFlags=0x%x, ReqType=%s, Req=0x%x, Index=0x%x\n",
+						ret, TransferFlags, (RequestType == DEVICE_VENDOR_REQUEST_OUT ? "OUT" : "IN"), Request, Index));
+			if (Request == 0x2)
+				DBGPRINT(RT_DEBUG_ERROR, ("\tRequest Value=0x%04x!\n", Value));
+
+			if ((TransferBuffer!= NULL) && (TransferBufferLength > 0))
+				hex_dump("Failed TransferBuffer value", TransferBuffer, TransferBufferLength);
+        }
+
+#if 0
+        // retry
+		if (ret < 0) {
+			int temp_i=0;
+			DBGPRINT(RT_DEBUG_ERROR, ("USBVendorRequest failed ret=%d, \n",ret));
+			ret = 0;
+			do
+			{
+				if( RequestType == DEVICE_VENDOR_REQUEST_OUT)
+					ret=usb_control_msg(pObj->pUsb_Dev, usb_sndctrlpipe( pObj->pUsb_Dev, 0 ), Request, RequestType, Value,Index, TransferBuffer, TransferBufferLength, CONTROL_TIMEOUT_JIFFIES);
+				else if(RequestType == DEVICE_VENDOR_REQUEST_IN)
+					ret=usb_control_msg(pObj->pUsb_Dev, usb_rcvctrlpipe( pObj->pUsb_Dev, 0 ), Request, RequestType, Value,Index, TransferBuffer, TransferBufferLength, CONTROL_TIMEOUT_JIFFIES);
+				temp_i++;
+			} while( (ret < 0) && (temp_i <= 1) );
+
+			if( ret >= 0)
+				return ret;
+
+		}
+#endif
+
+	}
+	return ret;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+	  Creates an IRP to submite an IOCTL_INTERNAL_USB_RESET_PORT
+	  synchronously. Callers of this function must be running at
+	  PASSIVE LEVEL.
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSB_ResetDevice(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NTSTATUS		Status = TRUE;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("--->USB_ResetDevice\n"));
+	//RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS);
+	return Status;
+}
+
+VOID CMDHandler(
+    IN PRTMP_ADAPTER pAd)
+{
+	PCmdQElmt		cmdqelmt;
+	PUCHAR			pData;
+	NDIS_STATUS		NdisStatus = NDIS_STATUS_SUCCESS;
+//	ULONG			Now = 0;
+	NTSTATUS		ntStatus;
+//	unsigned long	IrqFlags;
+
+	while (pAd->CmdQ.size > 0)
+	{
+		NdisStatus = NDIS_STATUS_SUCCESS;
+
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		RTUSBDequeueCmd(&pAd->CmdQ, &cmdqelmt);
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+
+		if (cmdqelmt == NULL)
+			break;
+
+		pData = cmdqelmt->buffer;
+
+		if(!(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)))
+		{
+			switch (cmdqelmt->command)
+			{
+				case CMDTHREAD_CHECK_GPIO:
+					{
+#ifdef CONFIG_STA_SUPPORT
+						UINT32 data;
+#endif // CONFIG_STA_SUPPORT //
+#ifdef RALINK_ATE
+       					if(ATE_ON(pAd))
+						{
+							DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+							break;
+						}
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+
+
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							// Read GPIO pin2 as Hardware controlled radio state
+
+							RTUSBReadMACRegister( pAd, GPIO_CTRL_CFG, &data);
+
+							if (data & 0x04)
+							{
+								pAd->StaCfg.bHwRadio = TRUE;
+							}
+							else
+							{
+								pAd->StaCfg.bHwRadio = FALSE;
+							}
+
+							if(pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+							{
+								pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+								if(pAd->StaCfg.bRadio == TRUE)
+								{
+									DBGPRINT_RAW(RT_DEBUG_ERROR, ("!!! Radio On !!!\n"));
+
+									MlmeRadioOn(pAd);
+									// Update extra information
+									pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+								}
+								else
+								{
+									DBGPRINT_RAW(RT_DEBUG_ERROR, ("!!! Radio Off !!!\n"));
+
+									MlmeRadioOff(pAd);
+									// Update extra information
+									pAd->ExtraInfo = HW_RADIO_OFF;
+								}
+							}
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					break;
+
+#ifdef CONFIG_STA_SUPPORT
+				case CMDTHREAD_QKERIODIC_EXECUT:
+					{
+						StaQuickResponeForRateUpExec(NULL, pAd, NULL, NULL);
+					}
+					break;
+#endif // CONFIG_STA_SUPPORT //
+
+				case CMDTHREAD_RESET_BULK_OUT:
+					{
+						UINT32		MACValue;
+						UCHAR		Index;
+						int			ret=0;
+						PHT_TX_CONTEXT	pHTTXContext;
+//						RTMP_TX_RING *pTxRing;
+						unsigned long IrqFlags;
+#ifdef RALINK_ATE
+						PTX_CONTEXT		pNullContext = &(pAd->NullContext);
+#endif // RALINK_ATE //
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_RESET_BULK_OUT(ResetPipeid=0x%0x)===>\n", pAd->bulkResetPipeid));
+						// All transfers must be aborted or cancelled before attempting to reset the pipe.
+						//RTUSBCancelPendingBulkOutIRP(pAd);
+						// Wait 10ms to let previous packet that are already in HW FIFO to clear. by MAXLEE 12-25-2007
+						Index = 0;
+						do
+						{
+							RTUSBReadMACRegister(pAd, TXRXQ_PCNT, &MACValue);
+							if ((MACValue & 0xf00000/*0x800000*/) == 0)
+								break;
+							Index++;
+							RTMPusecDelay(10000);
+						}while(Index < 100);
+						MACValue = 0;
+						RTUSBReadMACRegister(pAd, USB_DMA_CFG, &MACValue);
+						// To prevent Read Register error, we 2nd check the validity.
+						if ((MACValue & 0xc00000) == 0)
+							RTUSBReadMACRegister(pAd, USB_DMA_CFG, &MACValue);
+						// To prevent Read Register error, we 3rd check the validity.
+						if ((MACValue & 0xc00000) == 0)
+							RTUSBReadMACRegister(pAd, USB_DMA_CFG, &MACValue);
+						MACValue |= 0x80000;
+						RTUSBWriteMACRegister(pAd, USB_DMA_CFG, MACValue);
+
+						// Wait 1ms to prevent next URB to bulkout before HW reset. by MAXLEE 12-25-2007
+						RTMPusecDelay(1000);
+
+						MACValue &= (~0x80000);
+						RTUSBWriteMACRegister(pAd, USB_DMA_CFG, MACValue);
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("\tSet 0x2a0 bit19. Clear USB DMA TX path\n"));
+
+						// Wait 5ms to prevent next URB to bulkout before HW reset. by MAXLEE 12-25-2007
+						//RTMPusecDelay(5000);
+
+						if ((pAd->bulkResetPipeid & BULKOUT_MGMT_RESET_FLAG) == BULKOUT_MGMT_RESET_FLAG)
+						{
+							RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+							if (pAd->MgmtRing.TxSwFreeIdx < MGMT_RING_SIZE /* pMLMEContext->bWaitingBulkOut == TRUE */)
+							{
+								RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+							}
+							RTUSBKickBulkOut(pAd);
+
+							DBGPRINT_RAW(RT_DEBUG_TRACE, ("\tTX MGMT RECOVER Done!\n"));
+						}
+						else
+						{
+							pHTTXContext = &(pAd->TxContext[pAd->bulkResetPipeid]);
+							//NdisAcquireSpinLock(&pAd->BulkOutLock[pAd->bulkResetPipeid]);
+							RTMP_INT_LOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+							if ( pAd->BulkOutPending[pAd->bulkResetPipeid] == FALSE)
+							{
+								pAd->BulkOutPending[pAd->bulkResetPipeid] = TRUE;
+								pHTTXContext->IRPPending = TRUE;
+								pAd->watchDogTxPendingCnt[pAd->bulkResetPipeid] = 1;
+
+								// no matter what, clean the flag
+								RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+
+								//NdisReleaseSpinLock(&pAd->BulkOutLock[pAd->bulkResetPipeid]);
+								RTMP_INT_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+/*-----------------------------------------------------------------------------------------------*/
+#ifdef RALINK_ATE
+								if(ATE_ON(pAd))
+							    {
+									pNullContext->IRPPending = TRUE;
+									//
+									// If driver is still in ATE TXFRAME mode,
+									// keep on transmitting ATE frames.
+									//
+									DBGPRINT_RAW(RT_DEBUG_TRACE, ("pAd->ate.Mode == %d\npAd->ContinBulkOut == %d\npAd->BulkOutRemained == %d\n", pAd->ate.Mode, pAd->ContinBulkOut, atomic_read(&pAd->BulkOutRemained)));
+									if((pAd->ate.Mode == ATE_TXFRAME) && ((pAd->ContinBulkOut == TRUE) || (atomic_read(&pAd->BulkOutRemained) > 0)))
+								    {
+										DBGPRINT_RAW(RT_DEBUG_TRACE, ("After CMDTHREAD_RESET_BULK_OUT, continue to bulk out frames !\n"));
+
+										// Init Tx context descriptor
+										RTUSBInitTxDesc(pAd, pNullContext, 0/* pAd->bulkResetPipeid */, (usb_complete_t)ATE_RTUSBBulkOutDataPacketComplete);
+
+										if((ret = RTUSB_SUBMIT_URB(pNullContext->pUrb))!=0)
+										{
+											DBGPRINT(RT_DEBUG_ERROR, ("ATE_RTUSBBulkOutDataPacket: Submit Tx URB failed %d\n", ret));
+										}
+
+										pAd->BulkOutReq++;
+									}
+								}
+								else
+#endif // RALINK_ATE //
+/*-----------------------------------------------------------------------------------------------*/
+								{
+								RTUSBInitHTTxDesc(pAd, pHTTXContext, pAd->bulkResetPipeid, pHTTXContext->BulkOutSize, (usb_complete_t)RTUSBBulkOutDataPacketComplete);
+
+								if((ret = RTUSB_SUBMIT_URB(pHTTXContext->pUrb))!=0)
+								{
+										RTMP_INT_LOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+									pAd->BulkOutPending[pAd->bulkResetPipeid] = FALSE;
+									pHTTXContext->IRPPending = FALSE;
+										pAd->watchDogTxPendingCnt[pAd->bulkResetPipeid] = 0;
+										RTMP_INT_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+
+										DBGPRINT(RT_DEBUG_ERROR, ("CmdThread : CMDTHREAD_RESET_BULK_OUT: Submit Tx URB failed %d\n", ret));
+								}
+									else
+									{
+										RTMP_IRQ_LOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+										DBGPRINT_RAW(RT_DEBUG_TRACE,("\tCMDTHREAD_RESET_BULK_OUT: TxContext[%d]:CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d, pending=%d!\n",
+												pAd->bulkResetPipeid, pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition,
+															pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad, pAd->BulkOutPending[pAd->bulkResetPipeid]));
+										DBGPRINT_RAW(RT_DEBUG_TRACE,("\t\tBulkOut Req=0x%lx, Complete=0x%lx, Other=0x%lx\n",
+															pAd->BulkOutReq, pAd->BulkOutComplete, pAd->BulkOutCompleteOther));
+										RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+										DBGPRINT_RAW(RT_DEBUG_TRACE, ("\tCMDTHREAD_RESET_BULK_OUT: Submit Tx DATA URB for failed BulkReq(0x%lx) Done, status=%d!\n", pAd->bulkResetReq[pAd->bulkResetPipeid], pHTTXContext->pUrb->status));
+
+									}
+								}
+							}
+							else
+							{
+								//NdisReleaseSpinLock(&pAd->BulkOutLock[pAd->bulkResetPipeid]);
+								//RTMP_INT_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("CmdThread : TX DATA RECOVER FAIL for BulkReq(0x%lx) because BulkOutPending[%d] is TRUE!\n", pAd->bulkResetReq[pAd->bulkResetPipeid], pAd->bulkResetPipeid));
+								if (pAd->bulkResetPipeid == 0)
+								{
+									UCHAR	pendingContext = 0;
+									PHT_TX_CONTEXT pHTTXContext = (PHT_TX_CONTEXT)(&pAd->TxContext[pAd->bulkResetPipeid ]);
+									PTX_CONTEXT pMLMEContext = (PTX_CONTEXT)(pAd->MgmtRing.Cell[pAd->MgmtRing.TxDmaIdx].AllocVa);
+									PTX_CONTEXT pNULLContext = (PTX_CONTEXT)(&pAd->PsPollContext);
+									PTX_CONTEXT pPsPollContext = (PTX_CONTEXT)(&pAd->NullContext);
+
+									if (pHTTXContext->IRPPending)
+										pendingContext |= 1;
+									else if (pMLMEContext->IRPPending)
+										pendingContext |= 2;
+									else if (pNULLContext->IRPPending)
+										pendingContext |= 4;
+									else if (pPsPollContext->IRPPending)
+										pendingContext |= 8;
+									else
+										pendingContext = 0;
+
+									DBGPRINT_RAW(RT_DEBUG_ERROR, ("\tTX Occupied by %d!\n", pendingContext));
+								}
+
+							// no matter what, clean the flag
+							RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+
+								RTMP_INT_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+
+								RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << pAd->bulkResetPipeid));
+							}
+
+							RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+							//RTUSBKickBulkOut(pAd);
+						}
+
+					}
+					/*
+						// Don't cancel BULKIN.
+						while ((atomic_read(&pAd->PendingRx) > 0) &&
+								(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+						{
+							if (atomic_read(&pAd->PendingRx) > 0)
+							{
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("BulkIn IRP Pending!!cancel it!\n"));
+								RTUSBCancelPendingBulkInIRP(pAd);
+							}
+							RTMPusecDelay(100000);
+						}
+
+						if ((atomic_read(&pAd->PendingRx) == 0) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)))
+						{
+							UCHAR	i;
+							RTUSBRxPacket(pAd);
+							pAd->NextRxBulkInReadIndex = 0;	// Next Rx Read index
+							pAd->NextRxBulkInIndex		= 0;	// Rx Bulk pointer
+							for (i = 0; i < (RX_RING_SIZE); i++)
+							{
+								PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+								pRxContext->pAd	= pAd;
+								pRxContext->InUse		= FALSE;
+								pRxContext->IRPPending	= FALSE;
+								pRxContext->Readable	= FALSE;
+								pRxContext->ReorderInUse = FALSE;
+
+							}
+							RTUSBBulkReceive(pAd);
+							DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTUSBBulkReceive\n"));
+						}*/
+					DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_RESET_BULK_OUT<===\n"));
+    	   			break;
+
+				case CMDTHREAD_RESET_BULK_IN:
+					DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_RESET_BULK_IN === >\n"));
+
+					// All transfers must be aborted or cancelled before attempting to reset the pipe.
+					{
+						UINT32		MACValue;
+/*-----------------------------------------------------------------------------------------------*/
+#ifdef RALINK_ATE
+						if (ATE_ON(pAd))
+						{
+							if((pAd->PendingRx > 0) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+							{
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("ATE : BulkIn IRP Pending!!!\n"));
+								ATE_RTUSBCancelPendingBulkInIRP(pAd);
+								RTMPusecDelay(100000);
+								pAd->PendingRx = 0;
+							}
+						}
+						else
+#endif // RALINK_ATE //
+/*-----------------------------------------------------------------------------------------------*/
+						{
+						//while ((atomic_read(&pAd->PendingRx) > 0) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+						if((pAd->PendingRx > 0) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+						{
+							DBGPRINT_RAW(RT_DEBUG_ERROR, ("BulkIn IRP Pending!!!\n"));
+							RTUSBCancelPendingBulkInIRP(pAd);
+							RTMPusecDelay(100000);
+							pAd->PendingRx = 0;
+						}
+						}
+
+						// Wait 10ms before reading register.
+						RTMPusecDelay(10000);
+						ntStatus = RTUSBReadMACRegister(pAd, MAC_CSR0, &MACValue);
+
+						if ((NT_SUCCESS(ntStatus) == TRUE) &&
+							(!(RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS | fRTMP_ADAPTER_RADIO_OFF |
+													fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)))))
+						{
+							UCHAR	i;
+
+							if (RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS | fRTMP_ADAPTER_RADIO_OFF |
+														fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)))
+								break;
+							pAd->NextRxBulkInPosition = pAd->RxContext[pAd->NextRxBulkInIndex].BulkInOffset;
+							DBGPRINT(RT_DEBUG_TRACE, ("BULK_IN_RESET: NBIIdx=0x%x,NBIRIdx=0x%x, BIRPos=0x%lx. BIReq=x%lx, BIComplete=0x%lx, BICFail0x%lx\n",
+									pAd->NextRxBulkInIndex,  pAd->NextRxBulkInReadIndex, pAd->NextRxBulkInPosition, pAd->BulkInReq, pAd->BulkInComplete, pAd->BulkInCompleteFail));
+							for (i = 0; i < RX_RING_SIZE; i++)
+							{
+ 								DBGPRINT(RT_DEBUG_TRACE, ("\tRxContext[%d]: IRPPending=%d, InUse=%d, Readable=%d!\n"
+									, i, pAd->RxContext[i].IRPPending, pAd->RxContext[i].InUse, pAd->RxContext[i].Readable));
+							}
+ 							/*
+
+							DBGPRINT_RAW(RT_DEBUG_ERROR, ("==========================================\n"));
+
+							pAd->NextRxBulkInReadIndex = 0;	// Next Rx Read index
+							pAd->NextRxBulkInIndex		= 0;	// Rx Bulk pointer
+							for (i = 0; i < (RX_RING_SIZE); i++)
+							{
+								PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+								pRxContext->pAd	= pAd;
+								pRxContext->InUse		= FALSE;
+								pRxContext->IRPPending	= FALSE;
+								pRxContext->Readable	= FALSE;
+								pRxContext->ReorderInUse = FALSE;
+
+							}*/
+							RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET);
+							for (i = 0; i < pAd->CommonCfg.NumOfBulkInIRP; i++)
+							{
+								//RTUSBBulkReceive(pAd);
+								PRX_CONTEXT		pRxContext;
+								PURB			pUrb;
+								int				ret = 0;
+								unsigned long	IrqFlags;
+
+
+								RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+								pRxContext = &(pAd->RxContext[pAd->NextRxBulkInIndex]);
+								if ((pAd->PendingRx > 0) || (pRxContext->Readable == TRUE) || (pRxContext->InUse == TRUE))
+								{
+									RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+									break;
+								}
+								pRxContext->InUse = TRUE;
+								pRxContext->IRPPending = TRUE;
+								pAd->PendingRx++;
+								pAd->BulkInReq++;
+								RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+								// Init Rx context descriptor
+								RTUSBInitRxDesc(pAd, pRxContext);
+								pUrb = pRxContext->pUrb;
+								if ((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+								{	// fail
+
+									RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+									pRxContext->InUse = FALSE;
+									pRxContext->IRPPending = FALSE;
+									pAd->PendingRx--;
+									pAd->BulkInReq--;
+									RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+									DBGPRINT(RT_DEBUG_ERROR, ("CMDTHREAD_RESET_BULK_IN: Submit Rx URB failed(%d), status=%d\n", ret, pUrb->status));
+								}
+								else
+								{	// success
+#if 0
+									RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+									pRxContext->IRPPending = TRUE;
+									//NdisInterlockedIncrement(&pAd->PendingRx);
+									pAd->PendingRx++;
+									RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+									pAd->BulkInReq++;
+#endif
+									//printk("BIDone, Pend=%d,BIIdx=%d,BIRIdx=%d!\n", pAd->PendingRx, pAd->NextRxBulkInIndex, pAd->NextRxBulkInReadIndex);
+									DBGPRINT_RAW(RT_DEBUG_TRACE, ("CMDTHREAD_RESET_BULK_IN: Submit Rx URB Done, status=%d!\n", pUrb->status));
+									ASSERT((pRxContext->InUse == pRxContext->IRPPending));
+								}
+							}
+
+						}
+						else
+						{
+							// Card must be removed
+							if (NT_SUCCESS(ntStatus) != TRUE)
+							{
+							RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST);
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("CMDTHREAD_RESET_BULK_IN: Read Register Failed!Card must be removed!!\n\n"));
+							}
+							else
+							{
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("CMDTHREAD_RESET_BULK_IN: Cannot do bulk in because flags(0x%lx) on !\n", pAd->Flags));
+						}
+					}
+					}
+					DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_RESET_BULK_IN <===\n"));
+					break;
+
+				case CMDTHREAD_SET_ASIC_WCID:
+					{
+						RT_SET_ASIC_WCID	SetAsicWcid;
+						USHORT		offset;
+						UINT32		MACValue, MACRValue = 0;
+						SetAsicWcid = *((PRT_SET_ASIC_WCID)(pData));
+
+						if (SetAsicWcid.WCID >= MAX_LEN_OF_MAC_TABLE)
+							return;
+
+						offset = MAC_WCID_BASE + ((UCHAR)SetAsicWcid.WCID)*HW_WCID_ENTRY_SIZE;
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_SET_ASIC_WCID : WCID = %ld, SetTid  = %lx, DeleteTid = %lx.\n", SetAsicWcid.WCID, SetAsicWcid.SetTid, SetAsicWcid.DeleteTid));
+						MACValue = (pAd->MacTab.Content[SetAsicWcid.WCID].Addr[3]<<24)+(pAd->MacTab.Content[SetAsicWcid.WCID].Addr[2]<<16)+(pAd->MacTab.Content[SetAsicWcid.WCID].Addr[1]<<8)+(pAd->MacTab.Content[SetAsicWcid.WCID].Addr[0]);
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("1-MACValue= %x,\n", MACValue));
+						RTUSBWriteMACRegister(pAd, offset, MACValue);
+						// Read bitmask
+						RTUSBReadMACRegister(pAd, offset+4, &MACRValue);
+						if ( SetAsicWcid.DeleteTid != 0xffffffff)
+							MACRValue &= (~SetAsicWcid.DeleteTid);
+						if (SetAsicWcid.SetTid != 0xffffffff)
+							MACRValue |= (SetAsicWcid.SetTid);
+						MACRValue &= 0xffff0000;
+
+						MACValue = (pAd->MacTab.Content[SetAsicWcid.WCID].Addr[5]<<8)+pAd->MacTab.Content[SetAsicWcid.WCID].Addr[4];
+						MACValue |= MACRValue;
+						RTUSBWriteMACRegister(pAd, offset+4, MACValue);
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("2-MACValue= %x,\n", MACValue));
+					}
+					break;
+
+				case CMDTHREAD_SET_ASIC_WCID_CIPHER:
+					{
+#ifdef CONFIG_STA_SUPPORT
+						RT_SET_ASIC_WCID_ATTRI	SetAsicWcidAttri;
+						USHORT		offset;
+						UINT32		MACRValue = 0;
+						SHAREDKEY_MODE_STRUC csr1;
+						SetAsicWcidAttri = *((PRT_SET_ASIC_WCID_ATTRI)(pData));
+
+						if (SetAsicWcidAttri.WCID >= MAX_LEN_OF_MAC_TABLE)
+							return;
+
+						offset = MAC_WCID_ATTRIBUTE_BASE + ((UCHAR)SetAsicWcidAttri.WCID)*HW_WCID_ATTRI_SIZE;
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("Cmd : CMDTHREAD_SET_ASIC_WCID_CIPHER : WCID = %ld, Cipher = %lx.\n", SetAsicWcidAttri.WCID, SetAsicWcidAttri.Cipher));
+						// Read bitmask
+						RTUSBReadMACRegister(pAd, offset, &MACRValue);
+						MACRValue = 0;
+						MACRValue |= (((UCHAR)SetAsicWcidAttri.Cipher) << 1);
+
+						RTUSBWriteMACRegister(pAd, offset, MACRValue);
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("2-offset = %x , MACValue= %x,\n", offset, MACRValue));
+
+						offset = PAIRWISE_IVEIV_TABLE_BASE + ((UCHAR)SetAsicWcidAttri.WCID)*HW_IVEIV_ENTRY_SIZE;
+						MACRValue = 0;
+						if ( (SetAsicWcidAttri.Cipher <= CIPHER_WEP128))
+							MACRValue |= ( pAd->StaCfg.DefaultKeyId << 30);
+						else
+							MACRValue |= (0x20000000);
+						RTUSBWriteMACRegister(pAd, offset, MACRValue);
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("2-offset = %x , MACValue= %x,\n", offset, MACRValue));
+
+						//
+						// Update cipher algorithm. WSTA always use BSS0
+						//
+						// for adhoc mode only ,because wep status slow than add key, when use zero config
+						if (pAd->StaCfg.BssType == BSS_ADHOC )
+						{
+							offset = MAC_WCID_ATTRIBUTE_BASE;
+
+							RTUSBReadMACRegister(pAd, offset, &MACRValue);
+							MACRValue &= (~0xe);
+							MACRValue |= (((UCHAR)SetAsicWcidAttri.Cipher) << 1);
+
+							RTUSBWriteMACRegister(pAd, offset, MACRValue);
+
+							//Update group key cipher,,because wep status slow than add key, when use zero config
+							RTUSBReadMACRegister(pAd, SHARED_KEY_MODE_BASE+4*(0/2), &csr1.word);
+
+							csr1.field.Bss0Key0CipherAlg = SetAsicWcidAttri.Cipher;
+							csr1.field.Bss0Key1CipherAlg = SetAsicWcidAttri.Cipher;
+
+							RTUSBWriteMACRegister(pAd, SHARED_KEY_MODE_BASE+4*(0/2), csr1.word);
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					break;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+				// avoid in interrupt when write key
+				case RT_CMD_SET_KEY_TABLE: //General call for AsicAddPairwiseKeyEntry()
+					{
+						RT_ADD_PAIRWISE_KEY_ENTRY KeyInfo;
+						KeyInfo  = *((PRT_ADD_PAIRWISE_KEY_ENTRY)(pData));
+						AsicAddPairwiseKeyEntry(pAd,
+												KeyInfo.MacAddr,
+												(UCHAR)KeyInfo.MacTabMatchWCID,
+												&KeyInfo.CipherKey);
+					}
+					break;
+
+				case RT_CMD_SET_RX_WCID_TABLE: //General call for RTMPAddWcidAttributeEntry()
+					{
+						PMAC_TABLE_ENTRY pEntry ;
+						pEntry = (PMAC_TABLE_ENTRY)(pData);
+						RTMPAddWcidAttributeEntry(pAd,
+													BSS0,
+													0,
+													pEntry->PairwiseKey.CipherAlg,
+													pEntry);
+					}
+					break;
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+				case CMDTHREAD_SET_CLIENT_MAC_ENTRY:
+					{
+						MAC_TABLE_ENTRY *pEntry;
+						pEntry = (MAC_TABLE_ENTRY *)pData;
+
+
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							AsicRemovePairwiseKeyEntry(pAd, pEntry->apidx, (UCHAR)pEntry->Aid);
+							if ((pEntry->AuthMode <= Ndis802_11AuthModeAutoSwitch) && (pEntry->WepStatus == Ndis802_11Encryption1Enabled))
+							{
+								UINT32 uIV = 0;
+								PUCHAR  ptr;
+
+								ptr = (PUCHAR) &uIV;
+								*(ptr + 3) = (pAd->StaCfg.DefaultKeyId << 6);
+								AsicUpdateWCIDIVEIV(pAd, pEntry->Aid, uIV, 0);
+								AsicUpdateWCIDAttribute(pAd, pEntry->Aid, BSS0, pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg, FALSE);
+							}
+							else if (pEntry->AuthMode == Ndis802_11AuthModeWPANone)
+							{
+								UINT32 uIV = 0;
+								PUCHAR  ptr;
+
+								ptr = (PUCHAR) &uIV;
+								*(ptr + 3) = (pAd->StaCfg.DefaultKeyId << 6);
+								AsicUpdateWCIDIVEIV(pAd, pEntry->Aid, uIV, 0);
+								AsicUpdateWCIDAttribute(pAd, pEntry->Aid, BSS0, pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg, FALSE);
+							}
+							else
+							{
+								//
+								// Other case, disable engine.
+								// Don't worry WPA key, we will add WPA Key after 4-Way handshaking.
+								//
+								USHORT   offset;
+								offset = MAC_WCID_ATTRIBUTE_BASE + (pEntry->Aid * HW_WCID_ATTRI_SIZE);
+								// RX_PKEY_MODE:0 for no security; RX_KEY_TAB:0 for shared key table; BSS_IDX:0
+								RTUSBWriteMACRegister(pAd, offset, 0);
+							}
+						}
+#endif // CONFIG_STA_SUPPORT //
+
+						AsicUpdateRxWCIDTable(pAd, pEntry->Aid, pEntry->Addr);
+						printk("UpdateRxWCIDTable(): Aid=%d, Addr=%02x:%02x:%02x:%02x:%02x:%02x!\n", pEntry->Aid,
+								pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2], pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+					}
+					break;
+
+				case OID_802_11_ADD_WEP:
+					{
+#ifdef CONFIG_STA_SUPPORT
+						UINT	i;
+						UINT32	KeyIdx;
+						PNDIS_802_11_WEP	pWepKey;
+
+						DBGPRINT(RT_DEBUG_TRACE, ("CmdThread::OID_802_11_ADD_WEP  \n"));
+
+						pWepKey = (PNDIS_802_11_WEP)pData;
+						KeyIdx = pWepKey->KeyIndex & 0x0fffffff;
+
+						// it is a shared key
+						if ((KeyIdx >= 4) || ((pWepKey->KeyLength != 5) && (pWepKey->KeyLength != 13)))
+						{
+							NdisStatus = NDIS_STATUS_INVALID_DATA;
+							DBGPRINT(RT_DEBUG_ERROR, ("CmdThread::OID_802_11_ADD_WEP, INVALID_DATA!!\n"));
+						}
+						else
+						{
+							UCHAR CipherAlg;
+							pAd->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+							NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+							CipherAlg = (pAd->SharedKey[BSS0][KeyIdx].KeyLen == 5)? CIPHER_WEP64 : CIPHER_WEP128;
+
+							//
+							// Change the WEP cipher to CKIP cipher if CKIP KP on.
+							// Funk UI or Meetinghouse UI will add ckip key from this path.
+							//
+
+							if (pAd->OpMode == OPMODE_STA)
+						 	{
+								pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+								pAd->MacTab.Content[BSSID_WCID].PairwiseKey.KeyLen = pAd->SharedKey[BSS0][KeyIdx].KeyLen;
+						 	}
+							pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CipherAlg;
+							if (pWepKey->KeyIndex & 0x80000000)
+							{
+								// Default key for tx (shared key)
+								UCHAR	IVEIV[8];
+								UINT32	WCIDAttri, Value;
+								USHORT	offset, offset2;
+								NdisZeroMemory(IVEIV, 8);
+								pAd->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+								// Add BSSID to WCTable. because this is Tx wep key.
+								// WCID Attribute UDF:3, BSSIdx:3, Alg:3, Keytable:1=PAIRWISE KEY, BSSIdx is 0
+								WCIDAttri = (CipherAlg<<1)|SHAREDKEYTABLE;
+
+								offset = MAC_WCID_ATTRIBUTE_BASE + (BSSID_WCID* HW_WCID_ATTRI_SIZE);
+								RTUSBWriteMACRegister(pAd, offset, WCIDAttri);
+								// 1. IV/EIV
+								// Specify key index to find shared key.
+								IVEIV[3] = (UCHAR)(KeyIdx<< 6);	//WEP Eiv bit off. groupkey index is not 0
+								offset = PAIRWISE_IVEIV_TABLE_BASE + (BSS0Mcast_WCID * HW_IVEIV_ENTRY_SIZE);
+								offset2 = PAIRWISE_IVEIV_TABLE_BASE + (BSSID_WCID* HW_IVEIV_ENTRY_SIZE);
+								for (i=0; i<8;)
+								{
+									Value = IVEIV[i];
+									Value += (IVEIV[i+1]<<8);
+									Value += (IVEIV[i+2]<<16);
+									Value += (IVEIV[i+3]<<24);
+									RTUSBWriteMACRegister(pAd, offset+i, Value);
+									RTUSBWriteMACRegister(pAd, offset2+i, Value);
+									i+=4;
+								}
+
+								// 2. WCID Attribute UDF:3, BSSIdx:3, Alg:3, Keytable:use share key, BSSIdx is 0
+								WCIDAttri = (pAd->SharedKey[BSS0][KeyIdx].CipherAlg<<1)|SHAREDKEYTABLE;
+								offset = MAC_WCID_ATTRIBUTE_BASE + (BSS0Mcast_WCID* HW_WCID_ATTRI_SIZE);
+							        DBGPRINT(RT_DEBUG_TRACE, ("BSS0Mcast_WCID : offset = %x, WCIDAttri = %x\n", offset, WCIDAttri));
+								RTUSBWriteMACRegister(pAd, offset, WCIDAttri);
+
+							}
+							AsicAddSharedKeyEntry(pAd, BSS0, (UCHAR)KeyIdx, CipherAlg, pWepKey->KeyMaterial, NULL, NULL);
+							DBGPRINT(RT_DEBUG_TRACE, ("CmdThread::OID_802_11_ADD_WEP (KeyIdx=%d, Len=%d-byte)\n", KeyIdx, pWepKey->KeyLength));
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					break;
+
+				case CMDTHREAD_802_11_COUNTER_MEASURE:
+					break;
+				default:
+					DBGPRINT(RT_DEBUG_ERROR, ("--> Control Thread !! ERROR !! Unknown(cmdqelmt->command=0x%x) !! \n", cmdqelmt->command));
+					break;
+			}
+		}
+
+		if (cmdqelmt->CmdFromNdis == TRUE)
+		{
+				if (cmdqelmt->buffer != NULL)
+					NdisFreeMemory(cmdqelmt->buffer, cmdqelmt->bufferlength, 0);
+
+			NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+		}
+		else
+		{
+			if ((cmdqelmt->buffer != NULL) && (cmdqelmt->bufferlength != 0))
+				NdisFreeMemory(cmdqelmt->buffer, cmdqelmt->bufferlength, 0);
+            {
+				NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+			}
+		}
+	}	/* end of while */
+}
+
diff --git a/drivers/staging/rt2870/common/spectrum.c b/drivers/staging/rt2870/common/spectrum.c
new file mode 100644
index 00000000000..abba8405184
--- /dev/null
+++ b/drivers/staging/rt2870/common/spectrum.c
@@ -0,0 +1,1876 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+	action.c
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who          When          What
+    ---------    ----------    ----------------------------------------------
+	Fonchi Wu    2008	  	   created for 802.11h
+ */
+
+#include "../rt_config.h"
+#include "action.h"
+
+VOID MeasureReqTabInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisAllocateSpinLock(&pAd->CommonCfg.MeasureReqTabLock);
+
+	pAd->CommonCfg.pMeasureReqTab = kmalloc(sizeof(MEASURE_REQ_TAB), GFP_ATOMIC);
+	if (pAd->CommonCfg.pMeasureReqTab)
+		NdisZeroMemory(pAd->CommonCfg.pMeasureReqTab, sizeof(MEASURE_REQ_TAB));
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s Fail to alloc memory for pAd->CommonCfg.pMeasureReqTab.\n", __FUNCTION__));
+
+	return;
+}
+
+VOID MeasureReqTabExit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisFreeSpinLock(pAd->CommonCfg.MeasureReqTabLock);
+
+	if (pAd->CommonCfg.pMeasureReqTab)
+		kfree(pAd->CommonCfg.pMeasureReqTab);
+	pAd->CommonCfg.pMeasureReqTab = NULL;
+
+	return;
+}
+
+static PMEASURE_REQ_ENTRY MeasureReqLookUp(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	UINT HashIdx;
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL;
+	PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+
+	if (pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __FUNCTION__));
+		return NULL;
+	}
+
+	RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+
+	HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(DialogToken);
+	pEntry = pTab->Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if (pEntry->DialogToken == DialogToken)
+			break;
+		else
+		{
+			pPrevEntry = pEntry;
+			pEntry = pEntry->pNext;
+		}
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+
+	return pEntry;
+}
+
+static PMEASURE_REQ_ENTRY MeasureReqInsert(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	INT i;
+	ULONG HashIdx;
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL, pCurrEntry;
+	ULONG Now;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __FUNCTION__));
+		return NULL;
+	}
+
+	pEntry = MeasureReqLookUp(pAd, DialogToken);
+	if (pEntry == NULL)
+	{
+		RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+		for (i = 0; i < MAX_MEASURE_REQ_TAB_SIZE; i++)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry = &pTab->Content[i];
+
+			if ((pEntry->Valid == TRUE)
+				&& RTMP_TIME_AFTER((unsigned long)Now, (unsigned long)(pEntry->lastTime + MQ_REQ_AGE_OUT)))
+			{
+				PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+				ULONG HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+				PMEASURE_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+				// update Hash list
+				do
+				{
+					if (pProbeEntry == pEntry)
+					{
+						if (pPrevEntry == NULL)
+						{
+							pTab->Hash[HashIdx] = pEntry->pNext;
+						}
+						else
+						{
+							pPrevEntry->pNext = pEntry->pNext;
+						}
+						break;
+					}
+
+					pPrevEntry = pProbeEntry;
+					pProbeEntry = pProbeEntry->pNext;
+				} while (pProbeEntry);
+
+				NdisZeroMemory(pEntry, sizeof(MEASURE_REQ_ENTRY));
+				pTab->Size--;
+
+				break;
+			}
+
+			if (pEntry->Valid == FALSE)
+				break;
+		}
+
+		if (i < MAX_MEASURE_REQ_TAB_SIZE)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry->lastTime = Now;
+			pEntry->Valid = TRUE;
+			pEntry->DialogToken = DialogToken;
+			pTab->Size++;
+		}
+		else
+		{
+			pEntry = NULL;
+			DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab tab full.\n", __FUNCTION__));
+		}
+
+		// add this Neighbor entry into HASH table
+		if (pEntry)
+		{
+			HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(DialogToken);
+			if (pTab->Hash[HashIdx] == NULL)
+			{
+				pTab->Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pTab->Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+	}
+
+	return pEntry;
+}
+
+static VOID MeasureReqDelete(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __FUNCTION__));
+		return;
+	}
+
+	// if empty, return
+	if (pTab->Size == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pMeasureReqTab empty.\n"));
+		return;
+	}
+
+	pEntry = MeasureReqLookUp(pAd, DialogToken);
+	if (pEntry != NULL)
+	{
+		PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+		ULONG HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+		PMEASURE_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+		RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+		// update Hash list
+		do
+		{
+			if (pProbeEntry == pEntry)
+			{
+				if (pPrevEntry == NULL)
+				{
+					pTab->Hash[HashIdx] = pEntry->pNext;
+				}
+				else
+				{
+					pPrevEntry->pNext = pEntry->pNext;
+				}
+				break;
+			}
+
+			pPrevEntry = pProbeEntry;
+			pProbeEntry = pProbeEntry->pNext;
+		} while (pProbeEntry);
+
+		NdisZeroMemory(pEntry, sizeof(MEASURE_REQ_ENTRY));
+		pTab->Size--;
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+	}
+
+	return;
+}
+
+VOID TpcReqTabInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisAllocateSpinLock(&pAd->CommonCfg.TpcReqTabLock);
+
+	pAd->CommonCfg.pTpcReqTab = kmalloc(sizeof(TPC_REQ_TAB), GFP_ATOMIC);
+	if (pAd->CommonCfg.pTpcReqTab)
+		NdisZeroMemory(pAd->CommonCfg.pTpcReqTab, sizeof(TPC_REQ_TAB));
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s Fail to alloc memory for pAd->CommonCfg.pTpcReqTab.\n", __FUNCTION__));
+
+	return;
+}
+
+VOID TpcReqTabExit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisFreeSpinLock(pAd->CommonCfg.TpcReqTabLock);
+
+	if (pAd->CommonCfg.pTpcReqTab)
+		kfree(pAd->CommonCfg.pTpcReqTab);
+	pAd->CommonCfg.pTpcReqTab = NULL;
+
+	return;
+}
+
+static PTPC_REQ_ENTRY TpcReqLookUp(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	UINT HashIdx;
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL;
+	PTPC_REQ_ENTRY pPrevEntry = NULL;
+
+	if (pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __FUNCTION__));
+		return NULL;
+	}
+
+	RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+
+	HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(DialogToken);
+	pEntry = pTab->Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if (pEntry->DialogToken == DialogToken)
+			break;
+		else
+		{
+			pPrevEntry = pEntry;
+			pEntry = pEntry->pNext;
+		}
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+
+	return pEntry;
+}
+
+
+static PTPC_REQ_ENTRY TpcReqInsert(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	INT i;
+	ULONG HashIdx;
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL, pCurrEntry;
+	ULONG Now;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __FUNCTION__));
+		return NULL;
+	}
+
+	pEntry = TpcReqLookUp(pAd, DialogToken);
+	if (pEntry == NULL)
+	{
+		RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+		for (i = 0; i < MAX_TPC_REQ_TAB_SIZE; i++)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry = &pTab->Content[i];
+
+			if ((pEntry->Valid == TRUE)
+				&& RTMP_TIME_AFTER((unsigned long)Now, (unsigned long)(pEntry->lastTime + TPC_REQ_AGE_OUT)))
+			{
+				PTPC_REQ_ENTRY pPrevEntry = NULL;
+				ULONG HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+				PTPC_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+				// update Hash list
+				do
+				{
+					if (pProbeEntry == pEntry)
+					{
+						if (pPrevEntry == NULL)
+						{
+							pTab->Hash[HashIdx] = pEntry->pNext;
+						}
+						else
+						{
+							pPrevEntry->pNext = pEntry->pNext;
+						}
+						break;
+					}
+
+					pPrevEntry = pProbeEntry;
+					pProbeEntry = pProbeEntry->pNext;
+				} while (pProbeEntry);
+
+				NdisZeroMemory(pEntry, sizeof(TPC_REQ_ENTRY));
+				pTab->Size--;
+
+				break;
+			}
+
+			if (pEntry->Valid == FALSE)
+				break;
+		}
+
+		if (i < MAX_TPC_REQ_TAB_SIZE)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry->lastTime = Now;
+			pEntry->Valid = TRUE;
+			pEntry->DialogToken = DialogToken;
+			pTab->Size++;
+		}
+		else
+		{
+			pEntry = NULL;
+			DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab tab full.\n", __FUNCTION__));
+		}
+
+		// add this Neighbor entry into HASH table
+		if (pEntry)
+		{
+			HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(DialogToken);
+			if (pTab->Hash[HashIdx] == NULL)
+			{
+				pTab->Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pTab->Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+	}
+
+	return pEntry;
+}
+
+static VOID TpcReqDelete(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __FUNCTION__));
+		return;
+	}
+
+	// if empty, return
+	if (pTab->Size == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pTpcReqTab empty.\n"));
+		return;
+	}
+
+	pEntry = TpcReqLookUp(pAd, DialogToken);
+	if (pEntry != NULL)
+	{
+		PTPC_REQ_ENTRY pPrevEntry = NULL;
+		ULONG HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+		PTPC_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+		RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+		// update Hash list
+		do
+		{
+			if (pProbeEntry == pEntry)
+			{
+				if (pPrevEntry == NULL)
+				{
+					pTab->Hash[HashIdx] = pEntry->pNext;
+				}
+				else
+				{
+					pPrevEntry->pNext = pEntry->pNext;
+				}
+				break;
+			}
+
+			pPrevEntry = pProbeEntry;
+			pProbeEntry = pProbeEntry->pNext;
+		} while (pProbeEntry);
+
+		NdisZeroMemory(pEntry, sizeof(TPC_REQ_ENTRY));
+		pTab->Size--;
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Get Current TimeS tamp.
+
+	Parametrs:
+
+	Return	: Current Time Stamp.
+	==========================================================================
+ */
+static UINT64 GetCurrentTimeStamp(
+	IN PRTMP_ADAPTER pAd)
+{
+	// get current time stamp.
+	return 0;
+}
+
+/*
+	==========================================================================
+	Description:
+		Get Current Transmit Power.
+
+	Parametrs:
+
+	Return	: Current Time Stamp.
+	==========================================================================
+ */
+static UINT8 GetCurTxPwr(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 Wcid)
+{
+	return 16; /* 16 dBm */
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Dialog Token into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Dialog token.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertDialogToken(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 DialogToken)
+{
+	ULONG TempLen;
+	MakeOutgoingFrame(pFrameBuf,	&TempLen,
+					1,				&DialogToken,
+					END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert TPC Request IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+
+	Return	: None.
+	==========================================================================
+ */
+ static VOID InsertTpcReqIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen)
+{
+	ULONG TempLen;
+	ULONG Len = 0;
+	UINT8 ElementID = IE_TPC_REQUEST;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert TPC Report IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Transmit Power.
+		4. Link Margin.
+
+	Return	: None.
+	==========================================================================
+ */
+ static VOID InsertTpcReportIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin)
+{
+	ULONG TempLen;
+	ULONG Len = sizeof(TPC_REPORT_INFO);
+	UINT8 ElementID = IE_TPC_REPORT;
+	TPC_REPORT_INFO TpcReportIE;
+
+	TpcReportIE.TxPwr = TxPwr;
+	TpcReportIE.LinkMargin = LinkMargin;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						&TpcReportIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Channel Switch Announcement IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. channel switch announcement mode.
+		4. new selected channel.
+		5. channel switch announcement count.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertChSwAnnIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewChannel,
+	IN UINT8 ChSwCnt)
+{
+	ULONG TempLen;
+	ULONG Len = sizeof(CH_SW_ANN_INFO);
+	UINT8 ElementID = IE_CHANNEL_SWITCH_ANNOUNCEMENT;
+	CH_SW_ANN_INFO ChSwAnnIE;
+
+	ChSwAnnIE.ChSwMode = ChSwMode;
+	ChSwAnnIE.Channel = NewChannel;
+	ChSwAnnIE.ChSwCnt = ChSwCnt;
+
+	MakeOutgoingFrame(pFrameBuf,				&TempLen,
+						1,						&ElementID,
+						1,						&Len,
+						Len,					&ChSwAnnIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Measure Request IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Measure Token.
+		4. Measure Request Mode.
+		5. Measure Request Type.
+		6. Measure Channel.
+		7. Measure Start time.
+		8. Measure Duration.
+
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertMeasureReqIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN PMEASURE_REQ_INFO pMeasureReqIE)
+{
+	ULONG TempLen;
+	UINT8 Len = sizeof(MEASURE_REQ_INFO);
+	UINT8 ElementID = IE_MEASUREMENT_REQUEST;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						pMeasureReqIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Measure Report IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Measure Token.
+		4. Measure Request Mode.
+		5. Measure Request Type.
+		6. Length of Report Infomation
+		7. Pointer of Report Infomation Buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertMeasureReportIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN PMEASURE_REPORT_INFO pMeasureReportIE,
+	IN UINT8 ReportLnfoLen,
+	IN PUINT8 pReportInfo)
+{
+	ULONG TempLen;
+	ULONG Len;
+	UINT8 ElementID = IE_MEASUREMENT_REPORT;
+
+	Len = sizeof(MEASURE_REPORT_INFO) + ReportLnfoLen;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						pMeasureReportIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	if ((ReportLnfoLen > 0) && (pReportInfo != NULL))
+	{
+		MakeOutgoingFrame(pFrameBuf + *pFrameLen,		&TempLen,
+							ReportLnfoLen,				pReportInfo,
+							END_OF_ARGS);
+
+		*pFrameLen = *pFrameLen + TempLen;
+	}
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 MeasureCh,
+	IN UINT16 MeasureDuration)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+	HEADER_802_11 ActHdr;
+	MEASURE_REQ_INFO MeasureReqIE;
+	UINT8 RmReqDailogToken = RandomByte(pAd);
+	UINT64 MeasureStartTime = GetCurrentTimeStamp(pAd);
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_MRQ);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, MeasureToken);
+
+	// prepare Measurement IE.
+	NdisZeroMemory(&MeasureReqIE, sizeof(MEASURE_REQ_INFO));
+	MeasureReqIE.Token = RmReqDailogToken;
+	MeasureReqIE.ReqMode.word = MeasureReqMode;
+	MeasureReqIE.ReqType = MeasureReqType;
+	MeasureReqIE.MeasureReq.ChNum = MeasureCh;
+	MeasureReqIE.MeasureReq.MeasureStartTime = cpu2le64(MeasureStartTime);
+	MeasureReqIE.MeasureReq.MeasureDuration = cpu2le16(MeasureDuration);
+	InsertMeasureReqIE(pAd, (pOutBuffer + FrameLen), &FrameLen, &MeasureReqIE);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 ReportInfoLen,
+	IN PUINT8 pReportInfo)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+	HEADER_802_11 ActHdr;
+	MEASURE_REPORT_INFO MeasureRepIE;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_MRP);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// prepare Measurement IE.
+	NdisZeroMemory(&MeasureRepIE, sizeof(MEASURE_REPORT_INFO));
+	MeasureRepIE.Token = MeasureToken;
+	MeasureRepIE.ReportMode.word = MeasureReqMode;
+	MeasureRepIE.ReportType = MeasureReqType;
+	InsertMeasureReportIE(pAd, (pOutBuffer + FrameLen), &FrameLen, &MeasureRepIE, ReportInfoLen, pReportInfo);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UCHAR DialogToken)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_TPCRQ);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// Insert TPC Request IE.
+	InsertTpcReqIE(pAd, (pOutBuffer + FrameLen), &FrameLen);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_TPCRP);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// Insert TPC Request IE.
+	InsertTpcReportIE(pAd, (pOutBuffer + FrameLen), &FrameLen, TxPwr, LinkMargin);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Channel Switch Announcement action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+		2. Channel switch announcement mode.
+		2. a New selected channel.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueChSwAnn(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewCh)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_CHANNEL_SWITCH);
+
+	InsertChSwAnnIE(pAd, (pOutBuffer + FrameLen), &FrameLen, ChSwMode, NewCh, 0);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+static BOOLEAN DfsRequirementCheck(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 Channel)
+{
+	BOOLEAN Result = FALSE;
+	INT i;
+
+	do
+	{
+		// check DFS procedure is running.
+		// make sure DFS procedure won't start twice.
+		if (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE)
+		{
+			Result = FALSE;
+			break;
+		}
+
+		// check the new channel carried from Channel Switch Announcemnet is valid.
+		for (i=0; i<pAd->ChannelListNum; i++)
+		{
+			if ((Channel == pAd->ChannelList[i].Channel)
+				&&(pAd->ChannelList[i].RemainingTimeForUse == 0))
+			{
+				// found radar signal in the channel. the channel can't use at least for 30 minutes.
+				pAd->ChannelList[i].RemainingTimeForUse = 1800;//30 min = 1800 sec
+				Result = TRUE;
+				break;
+			}
+		}
+	} while(FALSE);
+
+	return Result;
+}
+
+VOID NotifyChSwAnnToPeerAPs(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pRA,
+	IN PUCHAR pTA,
+	IN UINT8 ChSwMode,
+	IN UINT8 Channel)
+{
+#ifdef WDS_SUPPORT
+	if (!((pRA[0] & 0xff) == 0xff)) // is pRA a broadcase address.
+	{
+		INT i;
+		// info neighbor APs that Radar signal found throgh WDS link.
+		for (i = 0; i < MAX_WDS_ENTRY; i++)
+		{
+			if (ValidWdsEntry(pAd, i))
+			{
+				PUCHAR pDA = pAd->WdsTab.WdsEntry[i].PeerWdsAddr;
+
+				// DA equal to SA. have no necessary orignal AP which found Radar signal.
+				if (MAC_ADDR_EQUAL(pTA, pDA))
+					continue;
+
+				// send Channel Switch Action frame to info Neighbro APs.
+				EnqueueChSwAnn(pAd, pDA, ChSwMode, Channel);
+			}
+		}
+	}
+#endif // WDS_SUPPORT //
+}
+
+static VOID StartDFSProcedure(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel,
+	IN UINT8 ChSwMode)
+{
+	// start DFS procedure
+	pAd->CommonCfg.Channel = Channel;
+#ifdef DOT11_N_SUPPORT
+	N_ChannelCheck(pAd);
+#endif // DOT11_N_SUPPORT //
+	pAd->CommonCfg.RadarDetect.RDMode = RD_SWITCHING_MODE;
+	pAd->CommonCfg.RadarDetect.CSCount = 0;
+}
+
+/*
+	==========================================================================
+	Description:
+		Channel Switch Announcement action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Channel switch announcement infomation buffer.
+
+
+	Return	: None.
+	==========================================================================
+ */
+
+/*
+  Channel Switch Announcement IE.
+  +----+-----+-----------+------------+-----------+
+  | ID | Len |Ch Sw Mode | New Ch Num | Ch Sw Cnt |
+  +----+-----+-----------+------------+-----------+
+    1    1        1           1            1
+*/
+static BOOLEAN PeerChSwAnnSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PCH_SW_ANN_INFO pChSwAnnInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pChSwAnnInfo == NULL)
+		return result;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_CHANNEL_SWITCH_ANNOUNCEMENT:
+				NdisMoveMemory(&pChSwAnnInfo->ChSwMode, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pChSwAnnInfo->Channel, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pChSwAnnInfo->ChSwCnt, eid_ptr->Octet + 2, 1);
+
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement request action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Measurement request infomation buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerMeasureReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PMEASURE_REQ_INFO pMeasureReqInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+	PUCHAR ptr;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pMeasureReqInfo == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_MEASUREMENT_REQUEST:
+				NdisMoveMemory(&pMeasureReqInfo->Token, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pMeasureReqInfo->ReqMode.word, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pMeasureReqInfo->ReqType, eid_ptr->Octet + 2, 1);
+				ptr = eid_ptr->Octet + 3;
+				NdisMoveMemory(&pMeasureReqInfo->MeasureReq.ChNum, ptr, 1);
+				NdisMoveMemory(&MeasureStartTime, ptr + 1, 8);
+				pMeasureReqInfo->MeasureReq.MeasureStartTime = SWAP64(MeasureStartTime);
+				NdisMoveMemory(&MeasureDuration, ptr + 9, 2);
+				pMeasureReqInfo->MeasureReq.MeasureDuration = SWAP16(MeasureDuration);
+
+				result = TRUE;
+				break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement report action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Measurement report infomation buffer.
+		4. basic report infomation buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+
+/*
+  Measurement Report IE.
+  +----+-----+-------+-------------+--------------+----------------+
+  | ID | Len | Token | Report Mode | Measure Type | Measure Report |
+  +----+-----+-------+-------------+--------------+----------------+
+    1     1      1          1             1            variable
+
+  Basic Report.
+  +--------+------------+----------+-----+
+  | Ch Num | Start Time | Duration | Map |
+  +--------+------------+----------+-----+
+      1          8           2        1
+
+  Map Field Bit Format.
+  +-----+---------------+---------------------+-------+------------+----------+
+  | Bss | OFDM Preamble | Unidentified signal | Radar | Unmeasured | Reserved |
+  +-----+---------------+---------------------+-------+------------+----------+
+     0          1                  2              3         4          5-7
+*/
+static BOOLEAN PeerMeasureReportSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PMEASURE_REPORT_INFO pMeasureReportInfo,
+	OUT PUINT8 pReportBuf)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+	PUCHAR ptr;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pMeasureReportInfo == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_MEASUREMENT_REPORT:
+				NdisMoveMemory(&pMeasureReportInfo->Token, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pMeasureReportInfo->ReportMode, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pMeasureReportInfo->ReportType, eid_ptr->Octet + 2, 1);
+				if (pMeasureReportInfo->ReportType == RM_BASIC)
+				{
+					PMEASURE_BASIC_REPORT pReport = (PMEASURE_BASIC_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->Map, ptr + 11, 1);
+
+				}
+				else if (pMeasureReportInfo->ReportType == RM_CCA)
+				{
+					PMEASURE_CCA_REPORT pReport = (PMEASURE_CCA_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->CCA_Busy_Fraction, ptr + 11, 1);
+
+				}
+				else if (pMeasureReportInfo->ReportType == RM_RPI_HISTOGRAM)
+				{
+					PMEASURE_RPI_REPORT pReport = (PMEASURE_RPI_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->RPI_Density, ptr + 11, 8);
+				}
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Request action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Dialog Token.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerTpcReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pDialogToken == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_TPC_REQUEST:
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Report action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Dialog Token.
+		4. TPC Report IE.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerTpcRepSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PTPC_REPORT_INFO pTpcRepInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pDialogToken == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_TPC_REPORT:
+				NdisMoveMemory(&pTpcRepInfo->TxPwr, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pTpcRepInfo->LinkMargin, eid_ptr->Octet + 1, 1);
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Channel Switch Announcement action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerChSwAnnAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	CH_SW_ANN_INFO ChSwAnnInfo;
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR index = 0, Channel = 0, NewChannel = 0;
+	ULONG Bssidx = 0;
+#endif // CONFIG_STA_SUPPORT //
+
+	NdisZeroMemory(&ChSwAnnInfo, sizeof(CH_SW_ANN_INFO));
+	if (! PeerChSwAnnSanity(pAd, Elem->Msg, Elem->MsgLen, &ChSwAnnInfo))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Invalid Channel Switch Action Frame.\n"));
+		return;
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+	{
+		Bssidx = BssTableSearch(&pAd->ScanTab, pFr->Hdr.Addr3, pAd->CommonCfg.Channel);
+		if (Bssidx == BSS_NOT_FOUND)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerChSwAnnAction - Bssidx is not found\n"));
+			return;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("\n****Bssidx is %d, Channel = %d\n", index, pAd->ScanTab.BssEntry[Bssidx].Channel));
+		hex_dump("SSID",pAd->ScanTab.BssEntry[Bssidx].Bssid ,6);
+
+		Channel = pAd->CommonCfg.Channel;
+		NewChannel = ChSwAnnInfo.Channel;
+
+		if ((pAd->CommonCfg.bIEEE80211H == 1) && (NewChannel != 0) && (Channel != NewChannel))
+		{
+			// Switching to channel 1 can prevent from rescanning the current channel immediately (by auto reconnection).
+			// In addition, clear the MLME queue and the scan table to discard the RX packets and previous scanning results.
+			AsicSwitchChannel(pAd, 1, FALSE);
+			AsicLockChannel(pAd, 1);
+		    LinkDown(pAd, FALSE);
+			MlmeQueueInit(&pAd->Mlme.Queue);
+			BssTableInit(&pAd->ScanTab);
+		    RTMPusecDelay(1000000);		// use delay to prevent STA do reassoc
+
+			// channel sanity check
+			for (index = 0 ; index < pAd->ChannelListNum; index++)
+			{
+				if (pAd->ChannelList[index].Channel == NewChannel)
+				{
+					pAd->ScanTab.BssEntry[Bssidx].Channel = NewChannel;
+					pAd->CommonCfg.Channel = NewChannel;
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("&&&&&&&&&&&&&&&&PeerChSwAnnAction - STA receive channel switch announcement IE (New Channel =%d)\n", NewChannel));
+					break;
+				}
+			}
+
+			if (index >= pAd->ChannelListNum)
+			{
+				DBGPRINT_ERR(("&&&&&&&&&&&&&&&&&&&&&&&&&&PeerChSwAnnAction(can not find New Channel=%d in ChannelList[%d]\n", pAd->CommonCfg.Channel, pAd->ChannelListNum));
+			}
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Measurement Request action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerMeasureReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	UINT8 DialogToken;
+	MEASURE_REQ_INFO MeasureReqInfo;
+	MEASURE_REPORT_MODE ReportMode;
+
+	if(PeerMeasureReqSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &MeasureReqInfo))
+	{
+		ReportMode.word = 0;
+		ReportMode.field.Incapable = 1;
+		EnqueueMeasurementRep(pAd, pFr->Hdr.Addr2, DialogToken, MeasureReqInfo.Token, ReportMode.word, MeasureReqInfo.ReqType, 0, NULL);
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement Report action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerMeasureReportAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MEASURE_REPORT_INFO MeasureReportInfo;
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	UINT8 DialogToken;
+	PUINT8 pMeasureReportInfo;
+
+//	if (pAd->CommonCfg.bIEEE80211H != TRUE)
+//		return;
+
+	if ((pMeasureReportInfo = kmalloc(sizeof(MEASURE_RPI_REPORT), GFP_ATOMIC)) == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s unable to alloc memory for measure report buffer (size=%d).\n", __FUNCTION__, sizeof(MEASURE_RPI_REPORT)));
+		return;
+	}
+
+	NdisZeroMemory(&MeasureReportInfo, sizeof(MEASURE_REPORT_INFO));
+	NdisZeroMemory(pMeasureReportInfo, sizeof(MEASURE_RPI_REPORT));
+	if (PeerMeasureReportSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &MeasureReportInfo, pMeasureReportInfo))
+	{
+		do {
+			PMEASURE_REQ_ENTRY pEntry = NULL;
+
+			// Not a autonomous measure report.
+			// check the dialog token field. drop it if the dialog token doesn't match.
+			if ((DialogToken != 0)
+				&& ((pEntry = MeasureReqLookUp(pAd, DialogToken)) == NULL))
+				break;
+
+			if (pEntry != NULL)
+				MeasureReqDelete(pAd, pEntry->DialogToken);
+
+			if (MeasureReportInfo.ReportType == RM_BASIC)
+			{
+				PMEASURE_BASIC_REPORT pBasicReport = (PMEASURE_BASIC_REPORT)pMeasureReportInfo;
+				if ((pBasicReport->Map.field.Radar)
+					&& (DfsRequirementCheck(pAd, pBasicReport->ChNum) == TRUE))
+				{
+					NotifyChSwAnnToPeerAPs(pAd, pFr->Hdr.Addr1, pFr->Hdr.Addr2, 1, pBasicReport->ChNum);
+					StartDFSProcedure(pAd, pBasicReport->ChNum, 1);
+				}
+			}
+		} while (FALSE);
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("Invalid Measurement Report Frame.\n"));
+
+	kfree(pMeasureReportInfo);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Request action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerTpcReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	PUCHAR pFramePtr = pFr->Octet;
+	UINT8 DialogToken;
+	UINT8 TxPwr = GetCurTxPwr(pAd, Elem->Wcid);
+	UINT8 LinkMargin = 0;
+	CHAR RealRssi;
+
+	// link margin: Ratio of the received signal power to the minimum desired by the station (STA). The
+	//				STA may incorporate rate information and channel conditions, including interference, into its computation
+	//				of link margin.
+
+	RealRssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0),
+								ConvertToRssi(pAd, Elem->Rssi1, RSSI_1),
+								ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+	// skip Category and action code.
+	pFramePtr += 2;
+
+	// Dialog token.
+	NdisMoveMemory(&DialogToken, pFramePtr, 1);
+
+	LinkMargin = (RealRssi / MIN_RCV_PWR);
+	if (PeerTpcReqSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken))
+		EnqueueTPCRep(pAd, pFr->Hdr.Addr2, DialogToken, TxPwr, LinkMargin);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Report action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerTpcRepAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UINT8 DialogToken;
+	TPC_REPORT_INFO TpcRepInfo;
+	PTPC_REQ_ENTRY pEntry = NULL;
+
+	NdisZeroMemory(&TpcRepInfo, sizeof(TPC_REPORT_INFO));
+	if (PeerTpcRepSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &TpcRepInfo))
+	{
+		if ((pEntry = TpcReqLookUp(pAd, DialogToken)) != NULL)
+		{
+			TpcReqDelete(pAd, pEntry->DialogToken);
+			DBGPRINT(RT_DEBUG_TRACE, ("%s: DialogToken=%x, TxPwr=%d, LinkMargin=%d\n",
+				__FUNCTION__, DialogToken, TpcRepInfo.TxPwr, TpcRepInfo.LinkMargin));
+		}
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Spectrun action frames Handler such as channel switch annoucement,
+		measurement report, measurement request actions frames.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+VOID PeerSpectrumAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	if (pAd->CommonCfg.bIEEE80211H != TRUE)
+		return;
+
+	switch(Action)
+	{
+		case SPEC_MRQ:
+			// current rt2860 unable do such measure specified in Measurement Request.
+			// reject all measurement request.
+			PeerMeasureReqAction(pAd, Elem);
+			break;
+
+		case SPEC_MRP:
+			PeerMeasureReportAction(pAd, Elem);
+			break;
+
+		case SPEC_TPCRQ:
+			PeerTpcReqAction(pAd, Elem);
+			break;
+
+		case SPEC_TPCRP:
+			PeerTpcRepAction(pAd, Elem);
+			break;
+
+		case SPEC_CHANNEL_SWITCH:
+{
+#ifdef DOT11N_DRAFT3
+				SEC_CHA_OFFSET_IE	Secondary;
+				CHA_SWITCH_ANNOUNCE_IE	ChannelSwitch;
+
+				// 802.11h only has Channel Switch Announcement IE.
+				RTMPMoveMemory(&ChannelSwitch, &Elem->Msg[LENGTH_802_11+4], sizeof (CHA_SWITCH_ANNOUNCE_IE));
+
+				// 802.11n D3.03 adds secondary channel offset element in the end.
+				if (Elem->MsgLen ==  (LENGTH_802_11 + 2 + sizeof (CHA_SWITCH_ANNOUNCE_IE) + sizeof (SEC_CHA_OFFSET_IE)))
+				{
+					RTMPMoveMemory(&Secondary, &Elem->Msg[LENGTH_802_11+9], sizeof (SEC_CHA_OFFSET_IE));
+				}
+				else
+				{
+					Secondary.SecondaryChannelOffset = 0;
+				}
+
+				if ((Elem->Msg[LENGTH_802_11+2] == IE_CHANNEL_SWITCH_ANNOUNCEMENT) && (Elem->Msg[LENGTH_802_11+3] == 3))
+				{
+					ChannelSwitchAction(pAd, Elem->Wcid, ChannelSwitch.NewChannel, Secondary.SecondaryChannelOffset);
+				}
+#endif // DOT11N_DRAFT3 //
+}
+			PeerChSwAnnAction(pAd, Elem);
+			break;
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	Parametrs:
+
+	Return	: None.
+	==========================================================================
+ */
+INT Set_MeasureReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT Aid = 1;
+	UINT ArgIdx;
+	PUCHAR thisChar;
+
+	MEASURE_REQ_MODE MeasureReqMode;
+	UINT8 MeasureReqToken = RandomByte(pAd);
+	UINT8 MeasureReqType = RM_BASIC;
+	UINT8 MeasureCh = 1;
+
+	ArgIdx = 1;
+	while ((thisChar = strsep((char **)&arg, "-")) != NULL)
+	{
+		switch(ArgIdx)
+		{
+			case 1:	// Aid.
+				Aid = simple_strtol(thisChar, 0, 16);
+				break;
+
+			case 2: // Measurement Request Type.
+				MeasureReqType = simple_strtol(thisChar, 0, 16);
+				if (MeasureReqType > 3)
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow MeasureReqType(%d)\n", __FUNCTION__, MeasureReqType));
+					return TRUE;
+				}
+				break;
+
+			case 3: // Measurement channel.
+				MeasureCh = simple_strtol(thisChar, 0, 16);
+				break;
+		}
+		ArgIdx++;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s::Aid = %d, MeasureReqType=%d MeasureCh=%d\n", __FUNCTION__, Aid, MeasureReqType, MeasureCh));
+	if (!VALID_WCID(Aid))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow sta of Aid(%d)\n", __FUNCTION__, Aid));
+		return TRUE;
+	}
+
+	MeasureReqMode.word = 0;
+	MeasureReqMode.field.Enable = 1;
+
+	MeasureReqInsert(pAd, MeasureReqToken);
+
+	EnqueueMeasurementReq(pAd, pAd->MacTab.Content[Aid].Addr,
+		MeasureReqToken, MeasureReqMode.word, MeasureReqType, MeasureCh, 2000);
+
+	return TRUE;
+}
+
+INT Set_TpcReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT Aid;
+
+	UINT8 TpcReqToken = RandomByte(pAd);
+
+	Aid = simple_strtol(arg, 0, 16);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s::Aid = %d\n", __FUNCTION__, Aid));
+	if (!VALID_WCID(Aid))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow sta of Aid(%d)\n", __FUNCTION__, Aid));
+		return TRUE;
+	}
+
+	TpcReqInsert(pAd, TpcReqToken);
+
+	EnqueueTPCReq(pAd, pAd->MacTab.Content[Aid].Addr, TpcReqToken);
+
+	return TRUE;
+}
+