/*************************************************************************** 
 * RT2x00 SourceForge Project - http://rt2x00.sourceforge.net              * 
 *                                                                         * 
 *   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.             * 
 *                                                                         * 
 *   Licensed under the GNU GPL                                            * 
 *   Original code supplied under license from RaLink Inc, 2004.           * 
 ***************************************************************************/

/*************************************************************************** 
 *	Module Name:	mlme.c
 *
 *	Abstract:
 *
 *	Revision History:
 *	Who		When		What
 *	--------	----------	-------------------------------
 *	Name		Date		Modification logs
 *	Jan Lee		2005-06-01	Release
 *	MathiasK	09-07-2005	kmalloc ATOMIC fixes
 *	RobinC		31-08-2005	Block mlme in MONITOR mode to keep from sending probe requests
 ***************************************************************************/

#include "rt_config.h"
                                  //  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 };

                                  //  1      2       5.5      11  
UCHAR Phy11BNextRateDownward[] = {RATE_1, RATE_1,   RATE_2,  RATE_5_5};
UCHAR Phy11BNextRateUpward[]   = {RATE_2, RATE_5_5, RATE_11, RATE_11};

                                  //  1      2       5.5      11        6        9        12      18       24       36       48       54
UCHAR Phy11BGNextRateDownward[]= {RATE_1, RATE_1,   RATE_2,  RATE_5_5,RATE_11,  RATE_6,  RATE_11, RATE_12, RATE_18, RATE_24, RATE_36, RATE_48};
UCHAR Phy11BGNextRateUpward[]  = {RATE_2, RATE_5_5, RATE_11, RATE_12, RATE_9,   RATE_12, RATE_18, RATE_24, RATE_36, RATE_48, RATE_54, RATE_54};

                                  //  1      2       5.5      11        6        9        12      18       24       36       48       54
UCHAR Phy11ANextRateDownward[] = {RATE_6, RATE_6,   RATE_6,  RATE_6,  RATE_6,   RATE_6,  RATE_9,  RATE_12, RATE_18, RATE_24, RATE_36, RATE_48};
UCHAR Phy11ANextRateUpward[]   = {RATE_9, RATE_9,   RATE_9,  RATE_9,  RATE_9,   RATE_12, RATE_18, RATE_24, RATE_36, RATE_48, RATE_54, RATE_54};

// 2560D and after has implemented ASIC-based OFDM rate switching, but not
// 2560C and before. thus software use different PER for rate switching
//                          RATE_1,  2, 5.5, 11,  6,  9, 12, 18, 24, 36, 48, 54
USHORT NewRateUpPER[]   = {    40,  40,  35, 20, 20, 20, 20, 16, 10, 16, 10,  6 }; // in percentage
USHORT NewRateDownPER[] = {    50,  50,  45, 45, 35, 35, 35, 35, 25, 25, 25, 13 }; // in percentage

USHORT OldRateUpPER[]   = {    40,  40,  40, 40, 30, 30, 30, 30, 20, 20, 10, 10 }; // in percentage
USHORT OldRateDownPER[] = {    45,  45,  45, 45, 35, 35, 35, 35, 25, 25, 25, 12 }; // in percentage
    
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};
    
RTMP_RF_REGS RF2522RegTable[] = {
//      ch   R1          R2          R3(TX0~4=0) R4
        {1,  0x94002050, 0x940c1fda, 0x94000101, 0},
        {2,  0x94002050, 0x940c1fee, 0x94000101, 0},
        {3,  0x94002050, 0x940c2002, 0x94000101, 0},
        {4,  0x94002050, 0x940c2016, 0x94000101, 0},
        {5,  0x94002050, 0x940c202a, 0x94000101, 0},
        {6,  0x94002050, 0x940c203e, 0x94000101, 0},
        {7,  0x94002050, 0x940c2052, 0x94000101, 0},
        {8,  0x94002050, 0x940c2066, 0x94000101, 0},
        {9,  0x94002050, 0x940c207a, 0x94000101, 0},
        {10, 0x94002050, 0x940c208e, 0x94000101, 0},
        {11, 0x94002050, 0x940c20a2, 0x94000101, 0},
        {12, 0x94002050, 0x940c20b6, 0x94000101, 0},
        {13, 0x94002050, 0x940c20ca, 0x94000101, 0},
        {14, 0x94002050, 0x940c20fa, 0x94000101, 0}
};
#define	NUM_OF_2522_CHNL	(sizeof(RF2522RegTable) / sizeof(RTMP_RF_REGS))

RTMP_RF_REGS RF2523RegTable[] = {
//      ch   R1          R2          R3(TX0~4=0) R4
        {1,  0x94022010, 0x94000c9e, 0x940e0111, 0x94000a1b},
        {2,  0x94022010, 0x94000ca2, 0x940e0111, 0x94000a1b},
        {3,  0x94022010, 0x94000ca6, 0x940e0111, 0x94000a1b},
        {4,  0x94022010, 0x94000caa, 0x940e0111, 0x94000a1b},
        {5,  0x94022010, 0x94000cae, 0x940e0111, 0x94000a1b},
        {6,  0x94022010, 0x94000cb2, 0x940e0111, 0x94000a1b},
        {7,  0x94022010, 0x94000cb6, 0x940e0111, 0x94000a1b},
        {8,  0x94022010, 0x94000cba, 0x940e0111, 0x94000a1b},
        {9,  0x94022010, 0x94000cbe, 0x940e0111, 0x94000a1b},
        {10, 0x94022010, 0x94000d02, 0x940e0111, 0x94000a1b},
        {11, 0x94022010, 0x94000d06, 0x940e0111, 0x94000a1b},
        {12, 0x94022010, 0x94000d0a, 0x940e0111, 0x94000a1b},
        {13, 0x94022010, 0x94000d0e, 0x940e0111, 0x94000a1b},
        {14, 0x94022010, 0x94000d1a, 0x940e0111, 0x94000a03}
};
#define	NUM_OF_2523_CHNL	(sizeof(RF2523RegTable) / sizeof(RTMP_RF_REGS))

RTMP_RF_REGS RF2524RegTable[] = {
//      ch   R1          R2          R3(TX0~4=0) R4
        {1,  0x94032020, 0x94000c9e, 0x94000101, 0x94000a1b},
        {2,  0x94032020, 0x94000ca2, 0x94000101, 0x94000a1b},
        {3,  0x94032020, 0x94000ca6, 0x94000101, 0x94000a1b},
        {4,  0x94032020, 0x94000caa, 0x94000101, 0x94000a1b},
        {5,  0x94032020, 0x94000cae, 0x94000101, 0x94000a1b},
        {6,  0x94032020, 0x94000cb2, 0x94000101, 0x94000a1b},
        {7,  0x94032020, 0x94000cb6, 0x94000101, 0x94000a1b},
        {8,  0x94032020, 0x94000cba, 0x94000101, 0x94000a1b},
        {9,  0x94032020, 0x94000cbe, 0x94000101, 0x94000a1b},
        {10, 0x94032020, 0x94000d02, 0x94000101, 0x94000a1b},
        {11, 0x94032020, 0x94000d06, 0x94000101, 0x94000a1b},
        {12, 0x94032020, 0x94000d0a, 0x94000101, 0x94000a1b},
        {13, 0x94032020, 0x94000d0e, 0x94000101, 0x94000a1b},
        {14, 0x94032020, 0x94000d1a, 0x94000101, 0x94000a03}
};
#define	NUM_OF_2524_CHNL	(sizeof(RF2524RegTable) / sizeof(RTMP_RF_REGS))
            
RTMP_RF_REGS_1 RF2525RegTable[] = {
//      ch   TempR2		 R1          R2          R3(TX0~4=0) R4
        {1,  0x94080cbe, 0x94022020, 0x94080c9e, 0x94060111, 0x94000a1b}, // {1,  0x94022010, 0x9408062e, 0x94060111, 0x94000a23}, 
        {2,  0x94080d02, 0x94022020, 0x94080ca2, 0x94060111, 0x94000a1b},
        {3,  0x94080d06, 0x94022020, 0x94080ca6, 0x94060111, 0x94000a1b},
        {4,  0x94080d0a, 0x94022020, 0x94080caa, 0x94060111, 0x94000a1b},
        {5,  0x94080d0e, 0x94022020, 0x94080cae, 0x94060111, 0x94000a1b},
        {6,  0x94080d12, 0x94022020, 0x94080cb2, 0x94060111, 0x94000a1b},
        {7,  0x94080d16, 0x94022020, 0x94080cb6, 0x94060111, 0x94000a1b},
        {8,  0x94080d1a, 0x94022020, 0x94080cba, 0x94060111, 0x94000a1b},
        {9,  0x94080d1e, 0x94022020, 0x94080cbe, 0x94060111, 0x94000a1b},
        {10, 0x94080d22, 0x94022020, 0x94080d02, 0x94060111, 0x94000a1b},
        {11, 0x94080d26, 0x94022020, 0x94080d06, 0x94060111, 0x94000a1b}, // {11, 0x94022010, 0x94080682, 0x94060111, 0x94000a23}, 
        {12, 0x94080d2a, 0x94022020, 0x94080d0a, 0x94060111, 0x94000a1b},
        {13, 0x94080d2e, 0x94022020, 0x94080d0e, 0x94060111, 0x94000a1b}, // {13, 0x94022010, 0x94080686, 0x94060111, 0x94000a23}, 
        {14, 0x94080d3a, 0x94022020, 0x94080d1a, 0x94060111, 0x94000a03}
};
#define	NUM_OF_2525_CHNL	(sizeof(RF2525RegTable) / sizeof(RTMP_RF_REGS_1))

RTMP_RF_REGS_1 RF2525eRegTable[] = {
// using 10 Mhz reference clock
//      ch   TempR2		 R1          R2          R3(TX0~4=0) R4
        {1,  0x940008aa, 0x94022010, 0x9400089a, 0x94060111, 0x94000e1b},
        {2,  0x940008ae, 0x94022010, 0x9400089e, 0x94060111, 0x94000e07},
        {3,  0x940008ae, 0x94022010, 0x9400089e, 0x94060111, 0x94000e1b},
        {4,  0x940008b2, 0x94022010, 0x940008a2, 0x94060111, 0x94000e07},
        {5,  0x940008b2, 0x94022010, 0x940008a2, 0x94060111, 0x94000e1b},
        {6,  0x940008b6, 0x94022010, 0x940008a6, 0x94060111, 0x94000e07},
        {7,  0x940008b6, 0x94022010, 0x940008a6, 0x94060111, 0x94000e1b},
        {8,  0x940008ba, 0x94022010, 0x940008aa, 0x94060111, 0x94000e07},
        {9,  0x940008ba, 0x94022010, 0x940008aa, 0x94060111, 0x94000e1b},
        {10, 0x940008be, 0x94022010, 0x940008ae, 0x94060111, 0x94000e07},
        {11, 0x940008b7, 0x94022010, 0x940008ae, 0x94060111, 0x94000e1b}, 
        {12, 0x94000902, 0x94022010, 0x940008b2, 0x94060111, 0x94000e07},
        {13, 0x94000902, 0x94022010, 0x940008b2, 0x94060111, 0x94000e1b},
        {14, 0x94000906, 0x94022010, 0x940008b6, 0x94060111, 0x94000e23}
};
#define	NUM_OF_2525E_CHNL	(sizeof(RF2525eRegTable) / sizeof(RTMP_RF_REGS_1))

RTMP_RF_REGS RF5222RegTable[] = {
//      ch   R1          R2          R3(TX0~4=0) R4
        {1,  0x94022020, 0x94001136, 0x94000101, 0x94000a0b},
        {2,  0x94022020, 0x9400113a, 0x94000101, 0x94000a0b},
        {3,  0x94022020, 0x9400113e, 0x94000101, 0x94000a0b},
        {4,  0x94022020, 0x94001182, 0x94000101, 0x94000a0b},
        {5,  0x94022020, 0x94001186, 0x94000101, 0x94000a0b},
        {6,  0x94022020, 0x9400118a, 0x94000101, 0x94000a0b},
        {7,  0x94022020, 0x9400118e, 0x94000101, 0x94000a0b},
        {8,  0x94022020, 0x94001192, 0x94000101, 0x94000a0b},
        {9,  0x94022020, 0x94001196, 0x94000101, 0x94000a0b},
        {10, 0x94022020, 0x9400119a, 0x94000101, 0x94000a0b},
        {11, 0x94022020, 0x9400119e, 0x94000101, 0x94000a0b},
        {12, 0x94022020, 0x940011a2, 0x94000101, 0x94000a0b},
        {13, 0x94022020, 0x940011a6, 0x94000101, 0x94000a0b},
        {14, 0x94022020, 0x940011ae, 0x94000101, 0x94000a1b},

        // still lack of MMAC(Japan) ch 34,38,42,46
        
        {36, 0x94022010, 0x94018896, 0x94000101, 0x94000a1f},
        {40, 0x94022010, 0x9401889a, 0x94000101, 0x94000a1f},
        {44, 0x94022010, 0x9401889e, 0x94000101, 0x94000a1f},
        {48, 0x94022010, 0x940188a2, 0x94000101, 0x94000a1f},
        {52, 0x94022010, 0x940188a6, 0x94000101, 0x94000a1f},
        {66, 0x94022010, 0x940188aa, 0x94000101, 0x94000a1f},
        {60, 0x94022010, 0x940188ae, 0x94000101, 0x94000a1f},
        {64, 0x94022010, 0x940188b2, 0x94000101, 0x94000a1f},
        
        {100, 0x94022010, 0x94008802, 0x94000101, 0x94000a0f},
        {104, 0x94022010, 0x94008806, 0x94000101, 0x94000a0f},
        {108, 0x94022010, 0x9400880a, 0x94000101, 0x94000a0f},
        {112, 0x94022010, 0x9400880e, 0x94000101, 0x94000a0f},
        {116, 0x94022010, 0x94008812, 0x94000101, 0x94000a0f},
        {120, 0x94022010, 0x94008816, 0x94000101, 0x94000a0f},
        {124, 0x94022010, 0x9400881a, 0x94000101, 0x94000a0f},
        {128, 0x94022010, 0x9400881e, 0x94000101, 0x94000a0f},
        {132, 0x94022010, 0x94008822, 0x94000101, 0x94000a0f},
        {136, 0x94022010, 0x94008826, 0x94000101, 0x94000a0f},
        {140, 0x94022010, 0x9400882a, 0x94000101, 0x94000a0f},
        
        {149, 0x94022020, 0x940090a6, 0x94000101, 0x94000a07},
        {153, 0x94022020, 0x940090ae, 0x94000101, 0x94000a07},
        {157, 0x94022020, 0x940090b6, 0x94000101, 0x94000a07},
        {161, 0x94022020, 0x940090be, 0x94000101, 0x94000a07}
};
#define	NUM_OF_5222_CHNL	(sizeof(RF5222RegTable) / sizeof(RTMP_RF_REGS))


/*
	==========================================================================
	Description:
		initialize the MLME task and its data structure (queue, spinlock, 
		timer, state machines).
	Return:
		always return NDIS_STATUS_SUCCESS
	==========================================================================
*/
NDIS_STATUS MlmeInit(
	IN PRT2570ADAPTER 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;
		}

		// Initialize Mlme Memory Handler
		// Allocate 20 nonpaged memory pool which size are MAX_LEN_OF_MLME_BUFFER for use
		Status = MlmeInitMemoryHandler(pAd, 20, MAX_LEN_OF_MLME_BUFFER);

		if(Status != NDIS_STATUS_SUCCESS) 
		{
			break;
		}

		pAd->Mlme.Running = FALSE;
		NdisAllocateSpinLock(&pAd->Mlme.TaskLock);

		// initialize the two tables
		// MacTableInit(pAd);
		BssTableInit(&pAd->PortCfg.BssTab);
		// init state machines
		ASSERT(ASSOC_FUNC_SIZE == MAX_ASSOC_MSG * MAX_ASSOC_STATE);
		AssocStateMachineInit(pAd, &pAd->Mlme.AssocMachine, pAd->Mlme.AssocFunc);
		
		ASSERT(AUTH_FUNC_SIZE == MAX_AUTH_MSG * MAX_AUTH_STATE);
		AuthStateMachineInit(pAd, &pAd->Mlme.AuthMachine, pAd->Mlme.AuthFunc);
		
		ASSERT(AUTH_RSP_FUNC_SIZE == MAX_AUTH_RSP_MSG * MAX_AUTH_RSP_STATE);
		AuthRspStateMachineInit(pAd, &pAd->Mlme.AuthRspMachine, pAd->Mlme.AuthRspFunc);

		ASSERT(WPA_PSK_FUNC_SIZE == MAX_WPA_PSK_MSG * MAX_WPA_PSK_STATE);
		WpaPskStateMachineInit(pAd,&pAd->Mlme.WpaPskMachine,pAd->Mlme.WpaPskFunc);

		ASSERT(SYNC_FUNC_SIZE == MAX_SYNC_MSG * MAX_SYNC_STATE);
		SyncStateMachineInit(pAd, &pAd->Mlme.SyncMachine, pAd->Mlme.SyncFunc);

		// 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);

		RTMPInitTimer(pAd, &pAd->Mlme.PeriodicTimer, &MlmePeriodicExec);
		//pAd->Mlme.PeriodicTimer.data = (unsigned long)pAd;
		//pAd->Mlme.PeriodicTimer.function = &MlmePeriodicExec;
		pAd->Mlme.PeriodicTimer.Timer.expires = jiffies + MLME_TASK_EXEC_INTV;
		// delay first mlme timer
		RTMPSetTimer(pAd, &pAd->Mlme.PeriodicTimer, 2*MLME_TASK_EXEC_INTV);

		if (pAd->PortCfg.LedMode == LED_MODE_TXRX_ACTIVITY)
		{
			RTMPInitTimer(pAd, &pAd->PortCfg.LedCntl.BlinkTimer, &AsicLedPeriodicExec);
			//pAd->PortCfg.LedCntl.BlinkTimer.data = (unsigned long)pAd;
			//pAd->PortCfg.LedCntl.BlinkTimer.function = &AsicLedPeriodicExec;
			pAd->PortCfg.LedCntl.BlinkTimer.Timer.expires = jiffies + (70 * HZ)/1000;
			RTMPSetTimer(pAd, &pAd->PortCfg.LedCntl.BlinkTimer, 70);
		}
	
	} while (FALSE);

	DBGPRINT(RT_DEBUG_TRACE,"<-- MLME Initialize\n");

	return Status;
}


/*
    ==========================================================================
    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 PRT2570ADAPTER pAd) 
{

    DBGPRINT(RT_DEBUG_TRACE, "==> MlmeHalt\n");

	// Cancel pending timers
	RTMPCancelTimer(&pAd->Mlme.AssocAux.AssocTimer);
	RTMPCancelTimer(&pAd->Mlme.AssocAux.ReassocTimer);
	RTMPCancelTimer(&pAd->Mlme.AssocAux.DisassocTimer);
	RTMPCancelTimer(&pAd->Mlme.AuthAux.AuthTimer);
	RTMPCancelTimer(&pAd->Mlme.SyncAux.BeaconTimer);
	RTMPCancelTimer(&pAd->Mlme.SyncAux.ScanTimer);
	RTMPCancelTimer(&pAd->Mlme.PeriodicTimer);
	if ((pAd->PortCfg.LedMode == LED_MODE_TXRX_ACTIVITY)||(pAd->PortCfg.LedMode == LED_MODE_ALPHA))
		RTMPCancelTimer(&pAd->PortCfg.LedCntl.BlinkTimer);
	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_REMOVE_IN_PROGRESS))
	{
		ASIC_LED_ACT_OFF(pAd);
	}

	// for timer callback routine to finish.
	NdisMSleep(1000);

	MlmeQueueDestroy(&pAd->Mlme.Queue);
	StateMachineDestroy(&pAd->Mlme.AssocMachine);
	StateMachineDestroy(&pAd->Mlme.AuthMachine);
	StateMachineDestroy(&pAd->Mlme.AuthRspMachine);
	StateMachineDestroy(&pAd->Mlme.SyncMachine);
	StateMachineDestroy(&pAd->Mlme.WpaPskMachine);
	//    StateMachineDestroy(&pAd->Mlme.CntlMachine);
	// no need free spinlock of pAd->Mlme.TaskLock

	MlmeFreeMemoryHandler(pAd); //Free MLME memory handler

	DBGPRINT(RT_DEBUG_TRACE, "<== MlmeHalt\n");
}

VOID MlmeHandler(
    IN PRT2570ADAPTER pAd) 
{
	MLME_QUEUE_ELEM        *Elem = NULL;
	// 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.Running) 
	{
	    NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
	    return;
	} 
	else 
	{
	    pAd->Mlme.Running = 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_REMOVE_IN_PROGRESS))
		{
			DBGPRINT(RT_DEBUG_TRACE, "Device Halted or Removed or MlmeRest, exit MlmeHandler! (queue num = %d)\n", pAd->Mlme.Queue.Num);
			break;
		}
		//From message type, determine which state machine I should drive
		if (MlmeDequeue(&pAd->Mlme.Queue, &Elem) && pAd->PortCfg.BssType!=BSS_MONITOR) 
		{
			// if dequeue success
			switch (Elem->Machine) 
			{
				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;
				default:
				    DBGPRINT(RT_DEBUG_TRACE, "ERROR: Illegal machine %d in MlmeHandler()\n",Elem->Machine);
				    break;
			} // end of switch

			// free MLME element
			Elem->Occupied = FALSE;
			Elem->MsgLen = 0;
            
		}
		else
		{
			DBGPRINT(RT_DEBUG_ERROR, "ERROR: empty Elem in MlmeQueue\n");
		}
	}

	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS))
	{
		DBGPRINT(RT_DEBUG_TRACE, "MlmeHandler, Reset Mlme! (queue num = %d)\n", pAd->Mlme.Queue.Num);
		MlmeQueueDestroy(&pAd->Mlme.Queue);
		// Cancel all timer events
		// Be careful to cancel new added timer
		RTMPCancelTimer(&pAd->Mlme.AssocAux.AssocTimer);
		RTMPCancelTimer(&pAd->Mlme.AssocAux.ReassocTimer);
		RTMPCancelTimer(&pAd->Mlme.AssocAux.DisassocTimer);
		RTMPCancelTimer(&pAd->Mlme.AuthAux.AuthTimer);
		//    RTMPCancelTimer(&pAd->Mlme.AuthRspAux.AuthRspTimer, &Cancelled);
		RTMPCancelTimer(&pAd->Mlme.SyncAux.BeaconTimer);
		RTMPCancelTimer(&pAd->Mlme.SyncAux.ScanTimer);
		// 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;
		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS))
			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS);
		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS))
			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS);
		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);

		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS);   
	}

	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
	pAd->Mlme.Running = FALSE;
	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);

}


VOID MlmeSuspend(
    IN PRT2570ADAPTER pAd) 
{
    MLME_QUEUE_ELEM		*Elem = NULL;

	DBGPRINT(RT_DEBUG_TRACE, "==>MlmeSuspend\n");

	// Cancel pending timers
	RTMPCancelTimer(&pAd->Mlme.AssocAux.AssocTimer);
	RTMPCancelTimer(&pAd->Mlme.AssocAux.ReassocTimer);
	RTMPCancelTimer(&pAd->Mlme.AssocAux.DisassocTimer);
	RTMPCancelTimer(&pAd->Mlme.AuthAux.AuthTimer);
	// RTMPCancelTimer(&pAd->Mlme.AuthRspAux.AuthRspTimer, &Cancelled);
	RTMPCancelTimer(&pAd->Mlme.SyncAux.BeaconTimer);
	RTMPCancelTimer(&pAd->Mlme.SyncAux.ScanTimer);

	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
	if(pAd->Mlme.Running) 
	{
	    NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
	    return;
	} 
	else 
	{
	    pAd->Mlme.Running = TRUE;
	}
	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);

	// Remove all Mlme queues elements
	while (!MlmeQueueEmpty(&pAd->Mlme.Queue)) 
	{
		//From message type, determine which state machine I should drive
		if (MlmeDequeue(&pAd->Mlme.Queue, &Elem)) 
		{
		    // free MLME element
		    Elem->Occupied = FALSE;
		    Elem->MsgLen = 0;
		    
		}
		else 
		{
		    DBGPRINT(RT_DEBUG_ERROR, "ERROR: empty Elem in MlmeQueue\n");
		}
	}
	
	// Remove running state
	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
	pAd->Mlme.Running = FALSE;
	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);

	RTUSBCleanUpMLMEWaitQueue(pAd);
	RTUSBCleanUpMLMEBulkOutQueue(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;
	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS))
		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS);//steven:for test
	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS))
		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS);//steven:for test
	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);//steven:for test

	DBGPRINT(RT_DEBUG_TRACE, "<==MlmeSuspend\n");
}

VOID	MlmeResume(
    IN	PRT2570ADAPTER	pAd)
{
    DBGPRINT(RT_DEBUG_TRACE, "==>MlmeResume\n");

	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS))
		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS);//steven:for test
	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS))
		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS);//steven:for test
	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);//steven:for test
	// 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;

	DBGPRINT(RT_DEBUG_TRACE, "<==MlmeResume\n");
}

/*
    ==========================================================================
    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      10000  // 10 sec
VOID MlmePeriodicExec(
    IN	unsigned long data)
{
	PRT2570ADAPTER pAd = (PRT2570ADAPTER)data;
	ULONG Now32;
	// Timer need to reset every time, so using do-while loop
	do
	{
		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_MLME_RESET_IN_PROGRESS)) ||
			(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_REMOVE_IN_PROGRESS)))
		{
			DBGPRINT_RAW(RT_DEBUG_TRACE, "<---MlmePeriodicExec\n");
			break;
		}

		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_PENDING))
		{
			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_PENDING);
			NdisMIndicateStatus(pAd->AdapterHandle, NDIS_STATUS_MEDIA_DISCONNECT, (PVOID)NULL, 0);
			NdisMIndicateStatusComplete(pAd->AdapterHandle);			
			DBGPRINT(RT_DEBUG_TRACE, "NDIS_STATUS_MEDIA_DISCONNECT Event B!\n");
		}

		if (pAd->PortCfg.bHardwareRadio == TRUE)
		{
			RTUSBEnqueueInternalCmd(pAd, RT_OID_CHECK_GPIO);
		}

		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
		{
			DBGPRINT_RAW(RT_DEBUG_TRACE, "<---RADIO OFF\n");
			break;
		}
		
		if (pAd->RalinkCounters.MgmtRingFullCount >= 2)
		{
			PCmdQElmt	cmdqelmt;

			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS);
			
			NdisAcquireSpinLock(&pAd->CmdQLock);
			while (pAd->CmdQ.size > 0)
			{
				RTUSBDequeueCmd(&pAd->CmdQ, &cmdqelmt);
				if (cmdqelmt->CmdFromNdis == TRUE)
				{
					if ((cmdqelmt->command != OID_802_11_BSSID_LIST_SCAN) &&
						(cmdqelmt->command != RT_OID_802_11_BSSID) &&
						(cmdqelmt->command != OID_802_11_SSID) &&
						(cmdqelmt->command != OID_802_11_DISASSOCIATE))
					{
#if 0
						if (cmdqelmt->SetOperation)
							NdisMSetInformationComplete(pAd->AdapterHandle, NDIS_STATUS_NOT_ACCEPTED);
						else
							NdisMQueryInformationComplete(pAd->AdapterHandle, NDIS_STATUS_NOT_ACCEPTED);
#endif
					}

					if ((cmdqelmt->command != RT_OID_SINGLE_READ_MAC) &&
						(cmdqelmt->command != RT_OID_MULTI_READ_MAC) &&
						(cmdqelmt->command != RT_OID_VENDOR_READ_BBP) &&
						(cmdqelmt->command != RT_OID_USB_VENDOR_EEPROM_READ))
					{
						if (cmdqelmt->buffer != NULL)
							kfree(cmdqelmt->buffer);
					}

					kfree(cmdqelmt);
				}
				else
					cmdqelmt->InUse = FALSE;
			}
			NdisReleaseSpinLock(&pAd->CmdQLock);
			
			RTUSBEnqueueInternalCmd(pAd, RT_OID_RESET_FROM_ERROR);

			DBGPRINT_RAW(RT_DEBUG_ERROR, "<---MlmePeriodicExec (Mgmt Ring Full)\n");
			break;
		}
		pAd->RalinkCounters.MgmtRingFullCount = 0;

		if ((RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS)) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS)))
		{
			break;//steven:for test
		}
		pAd->Mlme.PeriodicRound ++;
		RTUSBEnqueueInternalCmd(pAd, RT_OID_PERIODIC_EXECUT);
		Now32 = jiffies;
		if (INFRA_ON(pAd))
		{
			// Check for EAPOL frame sent after MIC countermeasures
			if (pAd->PortCfg.MicErrCnt >= 3)
			{
				MLME_DISASSOC_REQ_STRUCT	DisassocReq;
				
				// disassoc from current AP first
				DBGPRINT(RT_DEBUG_TRACE, "MLME - disassociate with current AP after sending second continuous EAPOL frame\n");
				DisassocParmFill(pAd, &DisassocReq, &pAd->PortCfg.Bssid, REASON_MIC_FAILURE);
				MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
					sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);

				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
				pAd->PortCfg.bBlockAssoc = TRUE;
			}
			else if ((pAd->PortCfg.LastBeaconRxTime + BEACON_LOST_TIME < Now32) )
			{
				DBGPRINT(RT_DEBUG_TRACE, "BEACON lost for more than %d msec, let CQI = 0\n", BEACON_LOST_TIME);
				pAd->Mlme.ChannelQuality = 0;
				
				// Lost AP, send disconnect & link down event
				RTUSBEnqueueInternalCmd(pAd, RT_OID_LINK_DOWN);
			}
			else
			{
				atomic_set(&(pAd->PortCfg.DataPacketsFromAP), 0);	
				MlmeCheckForPsmChange(pAd);
			}
		}
		else if (ADHOC_ON(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->PortCfg.LastBeaconRxTime + ADHOC_BEACON_LOST_TIME < Now32) &&
				(pAd->MediaState == NdisMediaStateConnected))
			{
				DBGPRINT(RT_DEBUG_TRACE, "MMCHK - excessive BEACON lost, last STA in this IBSS, MediaState=Disconnected\n");
				
				pAd->MediaState = NdisMediaStateDisconnected;
				NdisMIndicateStatus(pAd->AdapterHandle, NDIS_STATUS_MEDIA_DISCONNECT, (PVOID)NULL, 0);
				NdisMIndicateStatusComplete(pAd->AdapterHandle);
				
				// clean up previous SCAN result, add current BSS back to table if any
				BssTableDeleteEntry(&pAd->PortCfg.BssTab, &(pAd->PortCfg.Bssid));
				pAd->PortCfg.LastScanTime = Now32;
			}
			else
			{
				// if all 11b peers leave this BSS more than 5 seconds, update Tx rate
				if ((pAd->PortCfg.Channel <= 14)	&&
					(pAd->PortCfg.MaxTxRate <= RATE_11)	&&
					(pAd->PortCfg.MaxDesiredRate > RATE_11)	&&
					((pAd->PortCfg.Last11bBeaconRxTime + 5000) < Now32))
				{
					DBGPRINT(RT_DEBUG_TRACE, "last 11B peer left, update Tx rates\n");
					
					memcpy(pAd->PortCfg.SupportedRates, pAd->PortCfg.IbssConfig.SupportedRates, MAX_LEN_OF_SUPPORTED_RATES);
					pAd->PortCfg.SupportedRatesLen = pAd->PortCfg.IbssConfig.SupportedRatesLen;
					RTUSBEnqueueInternalCmd(pAd, RT_OID_UPDATE_TX_RATE);
				}
			}
		}
		else
		{
			if ((pAd->PortCfg.bBlockAssoc == TRUE) && (pAd->PortCfg.LastMicErrorTime + (60 * 1000) < Now32))
			{
				pAd->PortCfg.bBlockAssoc = FALSE;
			}

			if ((pAd->PortCfg.AutoReconnect == TRUE) &&
				(pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE) &&
				(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS)) &&
				(MlmeValidateSSID(pAd) == TRUE))
			{
				if (pAd->PortCfg.BssTab.BssNr==0)
				{
					MLME_SCAN_REQ_STRUCT	   ScanReq;
					CHAR					   BroadSsid[MAX_LEN_OF_SSID];
				
					if ((pAd->PortCfg.LastScanTime + 10 * 1000) < Now32)
					{
						DBGPRINT(RT_DEBUG_TRACE, "CNTL - No matching BSS, start a new scan\n");
	//					BroadSsid[0] = '\0';
						ScanParmFill(pAd, &ScanReq, BroadSsid, 0, 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->PortCfg.IgnoredScanNumber = 0;
						pAd->PortCfg.LastScanTime = Now32;
					}
					else if (pAd->PortCfg.BssType == BSS_INDEP)	// Quit the forever scan when in a very clean room
						MlmeAutoReconnectLastSSID(pAd);					
				}
				else
				{
					if ((pAd->Mlme.PeriodicRound % 10) == 7)
					{
						if ((pAd->PortCfg.LastScanTime + 10 * 1000) < Now32)
						{
							//MlmeAutoScan(pAd);
							pAd->PortCfg.LastScanTime = Now32;
						}
						MlmeAutoReconnectLastSSID(pAd);
					}
					else if ((pAd->Mlme.PeriodicRound % 30) == 8)
					{
						MlmeEnqueue(pAd, 
								MLME_CNTL_STATE_MACHINE, 
								OID_802_11_BSSID_LIST_SCAN, 
								0, 
								NULL);
					}
					
				}
			}
		}

		RTUSBUp(pAd, (&(pAd->mlme_semaphore)));
	}while(0);

	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_REMOVE_IN_PROGRESS)))
	{
		RTMPSetTimer(pAd, &pAd->Mlme.PeriodicTimer, MLME_TASK_EXEC_INTV);
	}

}

// IRQL = DISPATCH_LEVEL
VOID MlmeAutoScan(
    IN PRT2570ADAPTER 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");

        // tell CNTL state machine NOT to call NdisMSetInformationComplete() after completing
        // this request, because this request is initiated by driver itself.
        pAd->Mlme.CntlAux.CurrReqIsFromNdis = FALSE; 
        MlmeEnqueue(pAd, 
                    MLME_CNTL_STATE_MACHINE, 
                    OID_802_11_BSSID_LIST_SCAN, 
                    0, 
                    NULL);
	RTUSBUp(pAd, &pAd->mlme_semaphore);
    }
}
// IRQL = DISPATCH_LEVEL
VOID MlmeAutoRecoverNetwork(
    IN PRT2570ADAPTER pAd)
{
    // check CntlMachine.CurrState to avoid collision with NDIS SetOID request
    if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
    {
        NDIS_802_11_SSID OidSsid;
        OidSsid.SsidLength = pAd->PortCfg.SsidLen;
        memcpy(OidSsid.Ssid, pAd->PortCfg.Ssid, pAd->PortCfg.SsidLen);

        DBGPRINT(RT_DEBUG_TRACE, "MMCHK - Driver auto recovering network - %s\n", pAd->PortCfg.Ssid);

        // tell CNTL state machine NOT to call NdisMSetInformationComplete() after completing
        // this request, because this request is initiated by driver itself.
        pAd->Mlme.CntlAux.CurrReqIsFromNdis = FALSE; 
                    
        MlmeEnqueue(pAd, 
                    MLME_CNTL_STATE_MACHINE, 
                    OID_802_11_SSID, 
                    sizeof(NDIS_802_11_SSID), 
                    &OidSsid);
	RTUSBUp(pAd, (&(pAd->mlme_semaphore)));
	//KeSetEvent(&pAd->MLMEEvent, 0, FALSE);			
    }
}

// IRQL = DISPATCH_LEVEL
VOID MlmeAutoReconnectLastSSID(
    IN PRT2570ADAPTER pAd)
{
    // check CntlMachine.CurrState to avoid collision with NDIS SetOID request
    if ((pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE) && (MlmeValidateSSID(pAd) == TRUE))
    {
        NDIS_802_11_SSID OidSsid;
        OidSsid.SsidLength = pAd->PortCfg.SsidLen;
        memcpy(OidSsid.Ssid, pAd->PortCfg.Ssid, pAd->PortCfg.SsidLen);

        DBGPRINT(RT_DEBUG_TRACE, "Driver auto reconnect to last OID_802_11_SSID setting - %s\n", pAd->PortCfg.Ssid);

		// We will only try this attemp once, therefore change the AutoReconnect flag afterwards.
        pAd->Mlme.CntlAux.CurrReqIsFromNdis = FALSE; 
                    
        MlmeEnqueue(pAd, 
                    MLME_CNTL_STATE_MACHINE, 
                    OID_802_11_SSID, 
                    sizeof(NDIS_802_11_SSID), 
                    &OidSsid);

	RTUSBUp(pAd, &pAd->mlme_semaphore);

    }
}

// 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 PRT2570ADAPTER pAd)
{
	NDIS_802_11_SSID OidSsid;
	ULONG	index;
	
	// Copy the SSID into local buffer
	OidSsid.SsidLength = pAd->Mlme.CntlAux.SsidLen;
	memcpy(OidSsid.Ssid, pAd->Mlme.CntlAux.Ssid, pAd->Mlme.CntlAux.SsidLen);

	// First check the zero length "ANY" SSID
	if (OidSsid.SsidLength == 0)
		return (TRUE);
	else if (OidSsid.SsidLength > NDIS_802_11_LENGTH_SSID)
		return (FALSE);

	// Check each character value
	for (index = 0; index < OidSsid.SsidLength; index++)
	{
		if (OidSsid.Ssid[index] < 0x20)
			return (FALSE);
	}

	// All checked
	return (TRUE);
}

/*
    ==========================================================================
    Description:
        This routine checks if there're other APs out there capable for
        roaming. Caller should call this routine only when Massoc=TRUE and
        channel quality is below CQI_GOOD_THRESHOLD.
        
	IRQL = DISPATCH_LEVEL

    Output:
    ==========================================================================
 */
VOID MlmeCheckForRoaming(
    IN PRT2570ADAPTER pAd,
    IN ULONG    Now32)
{
	USHORT     i;
	BSS_TABLE  *pBssTab = &pAd->PortCfg.BssTab;
	BSS_TABLE  *pRoamTab = &pAd->Mlme.CntlAux.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 < pBssTab->BssNr; i++)
	{
		pBss = &pBssTab->BssEntry[i];

		if ((pBssTab->BssEntry[i].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(&pBssTab->BssEntry[i].Bssid, &pAd->PortCfg.Bssid))
			continue;    // skip current AP
		if (CQI_IS_FAIR(pAd->Mlme.ChannelQuality) && (pAd->PortCfg.LastRssi + RSSI_DELTA > pBss->Rssi)) 
			continue;    // we're still okay, only AP with stronger RSSI is eligible for roaming

		// AP passing all above rules is put into roaming candidate table        
		memcpy(&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) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS)))
		{
		// tell CNTL state machine NOT to call NdisMSetInformationComplete() after completing
		// this request, because this request is initiated by driver itself, not from NDIS.
		pAd->Mlme.CntlAux.CurrReqIsFromNdis = FALSE; 

		pAd->RalinkCounters.PoorCQIRoamingCount ++;
		DBGPRINT(RT_DEBUG_TRACE, "MMCHK - Roaming attempt #%d\n", pAd->RalinkCounters.PoorCQIRoamingCount);
		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_MLME_ROAMING_REQ, 0, NULL);
		RTUSBUp(pAd, (&(pAd->mlme_semaphore)));
		//KeSetEvent(&pAd->MLMEEvent, 0, FALSE);
		}
	}
	DBGPRINT(RT_DEBUG_TRACE, "<== MlmeCheckForRoaming\n");   
}

VOID PeriodicExec(
    IN PRT2570ADAPTER pAd)
{

    USHORT TxFailCount, TxNoRetryCnt, TxOneRetryCnt, TxMRetryCnt, TxRetryOkCount, TxOkCount, TxTotalCnt, TxPER, TxPRR = 0, TxRealOkCount;
	USHORT RxFailCnt;
    ULONG RxOkCnt, RxCnt, RxPER;
	ULONG Now32;
	ULONG OldValue;
	UCHAR UpRate, DownRate, CurrRate;

	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_REMOVE_IN_PROGRESS)))
	{
		DBGPRINT(RT_DEBUG_TRACE, "PeriodicExec not execute! (pAd->Flags=0x%08x)\n", pAd->Flags);
		return ;
	}
	AsicAdjustTxPower(pAd);
 	RTUSBMultiReadMAC(pAd, STA_CSR0, (PUCHAR)pAd->MACCounters, 22);
	OldValue = pAd->WlanCounters.FCSErrorCount.vv.LowPart;
	pAd->WlanCounters.FCSErrorCount.vv.LowPart += pAd->MACCounters[0];
	if (pAd->WlanCounters.FCSErrorCount.vv.LowPart < OldValue)
	{
		pAd->WlanCounters.FCSErrorCount.vv.HighPart++;
	}
	OldValue = pAd->WlanCounters.NoRetryCount.vv.LowPart;
	pAd->WlanCounters.NoRetryCount.vv.LowPart += pAd->MACCounters[6];
	if (pAd->WlanCounters.NoRetryCount.vv.LowPart < OldValue)
	{
		pAd->WlanCounters.NoRetryCount.vv.HighPart++;
	}
	OldValue = pAd->WlanCounters.RetryCount.vv.LowPart;
	pAd->WlanCounters.RetryCount.vv.LowPart += pAd->MACCounters[7];
	if (pAd->WlanCounters.RetryCount.vv.LowPart < OldValue)
	{
		pAd->WlanCounters.RetryCount.vv.HighPart++;
	}
	OldValue = pAd->WlanCounters.MultipleRetryCount.vv.LowPart;
	pAd->WlanCounters.MultipleRetryCount.vv.LowPart += pAd->MACCounters[8];
	if (pAd->WlanCounters.MultipleRetryCount.vv.LowPart < OldValue)
	{
		pAd->WlanCounters.MultipleRetryCount.vv.HighPart++;
	}
	OldValue = pAd->WlanCounters.FailedCount.vv.LowPart;
	pAd->WlanCounters.FailedCount.vv.LowPart += pAd->MACCounters[9];
	if (pAd->WlanCounters.FailedCount.vv.LowPart < OldValue)
	{
		pAd->WlanCounters.FailedCount.vv.HighPart++;
	}
	OldValue = pAd->QACounters.RXOverFlowCount.vv.LowPart;
	pAd->QACounters.RXOverFlowCount.vv.LowPart += pAd->MACCounters[4];
	if (pAd->QACounters.RXOverFlowCount.vv.LowPart < OldValue)
	{
		pAd->QACounters.RXOverFlowCount.vv.HighPart++;
	}

    //
    // monitor TX counters change for the past period
    //
    TxFailCount = pAd->MACCounters[9];
    TxNoRetryCnt = pAd->MACCounters[6];
    TxOneRetryCnt = pAd->MACCounters[7];
    TxMRetryCnt = pAd->MACCounters[8];
	TxRetryOkCount = TxOneRetryCnt + TxMRetryCnt;
	TxOkCount = TxNoRetryCnt + TxRetryOkCount;
	TxTotalCnt = TxOkCount + TxFailCount;
	if (TxTotalCnt > 100)
	{
		pAd->ScanAllowed = FALSE;
	}
	else
	{
		pAd->ScanAllowed = TRUE;
	}
#if 1//steven:exclude CTS
	if ((pAd->PortCfg.TxRate >= RATE_FIRST_OFDM_RATE) && (pAd->PortCfg.BGProtectionInUsed))
		TxTotalCnt = TxTotalCnt / 2;
#endif
	if (TxTotalCnt > TxFailCount)
		TxRealOkCount = TxTotalCnt - TxFailCount;
	else
		TxRealOkCount = 0;
	OldValue = pAd->WlanCounters.TransmittedFragmentCount.vv.LowPart;
	pAd->WlanCounters.TransmittedFragmentCount.vv.LowPart += TxRealOkCount;
	if (pAd->WlanCounters.TransmittedFragmentCount.vv.LowPart < OldValue)
	{
		pAd->WlanCounters.TransmittedFragmentCount.vv.HighPart++;
	}

	AsicBbpTuning(pAd);
	if (pAd->MediaState == NdisMediaStateConnected)
	{
    if (TxTotalCnt < 5) // if too few TX samples, skip TX related statistics
    {
        TxPER = 0;  // don't take TxPER into CQI consideration if too few sample
        TxPRR = 0;
    }
    else 
    {
		if (TxFailCount < TxTotalCnt)
		{
			USHORT temp = TxOneRetryCnt + TxMRetryCnt + TxFailCount;
			TxPER = (TxFailCount * 100) / TxTotalCnt;
			if (temp < TxTotalCnt)
				TxPRR = (temp * 100) / TxTotalCnt;
			else
				TxPRR = 100;
		}
		else
			TxPER = 100;
    }

    //
    // calculate RX PER
    //
	RxFailCnt = pAd->MACCounters[0];
    RxOkCnt   = pAd->WlanCounters.ReceivedFragmentCount.vv.LowPart - 
                pAd->Mlme.PrevWlanCounters.ReceivedFragmentCount.vv.LowPart;
    RxCnt = RxOkCnt + RxFailCnt;

    if (RxCnt < 5)
        RxPER = 0;  // don't take RxPER into ChannelQuality consideration if too few sample
    else
        RxPER = (RxFailCnt * 100) / RxCnt;

	if ((ADHOC_ON(pAd)) && (pAd->MediaState == NdisMediaStateConnected))
	{
		pAd->SentBeaconsCount += pAd->MACCounters[5];
		pAd->ReceivedBeaconsCount += pAd->MACCounters[10];

		if ((pAd->Mlme.PeriodicRound % 2) == 1)
		{
			TXRX_CSR18_STRUC Csr18;
			USHORT temp;

			DBGPRINT(RT_DEBUG_INFO, "SentBeaconsCount = %d ReceivedBeaconsCount = %d\n", pAd->SentBeaconsCount, pAd->ReceivedBeaconsCount);
			if (pAd->BeaconIntervalChangeAllowed == TRUE)
			{
				if (2 * pAd->SentBeaconsCount > pAd->ReceivedBeaconsCount)
				{
					temp = (pAd->PortCfg.BeaconPeriod << 8) + 1;
					Csr18.field.Offset = (temp & 0x000F);
					Csr18.field.Interval = (temp >> 6);
					RTUSBWriteMACRegister(pAd, TXRX_CSR18, Csr18.value);
					DBGPRINT(RT_DEBUG_INFO, "TXRX_CSR18 = 0x%x\n", Csr18.value);
					
					pAd->BeaconIntervalChangeAllowed = FALSE;
				}
				else if (pAd->ReceivedBeaconsCount > 9 * pAd->SentBeaconsCount)
				{
					temp = (pAd->PortCfg.BeaconPeriod << 8) - 5;
					Csr18.field.Offset = (temp & 0x000F);
					Csr18.field.Interval = (temp >> 6);
					RTUSBWriteMACRegister(pAd, TXRX_CSR18, Csr18.value);
					DBGPRINT(RT_DEBUG_INFO, "TXRX_CSR18 = 0x%x\n", Csr18.value);
					
					pAd->BeaconIntervalChangeAllowed = FALSE;
				}
				else if (pAd->ReceivedBeaconsCount > 3 * pAd->SentBeaconsCount)
				{
					temp = (pAd->PortCfg.BeaconPeriod << 8) - 1;
					Csr18.field.Offset = (temp & 0x000F);
					Csr18.field.Interval = (temp >> 6);
					RTUSBWriteMACRegister(pAd, TXRX_CSR18, Csr18.value);
					DBGPRINT(RT_DEBUG_INFO, "TXRX_CSR18 = 0x%x\n", Csr18.value);
					
					pAd->BeaconIntervalChangeAllowed = FALSE;
				}
#if 0
				else if (pAd->ReceivedBeaconsCount > 2 * pAd->SentBeaconsCount)
				{
					temp = (pAd->PortCfg.BeaconPeriod << 8) - 1;
					Csr18.field.Offset = (temp & 0x000F);
					Csr18.field.Interval = (temp >> 6);
					RTUSBWriteMACRegister(pAd, TXRX_CSR18, Csr18.value);
					DBGPRINT(RT_DEBUG_TRACE, ("TXRX_CSR18 = 0x%x\n", Csr18.value));
					
					pAd->BeaconIntervalChangeAllowed = FALSE;
				}
#endif
			}
#if 0
			else if (pAd->ReceivedBeaconsCount > 9 * pAd->SentBeaconsCount)
			{
				DBGPRINT(RT_DEBUG_TRACE, ("Do Nothing\n"));
			}
#endif
			else
			{
				temp = (pAd->PortCfg.BeaconPeriod << 8);
				Csr18.field.Offset = (temp & 0x000F);
				Csr18.field.Interval = (temp >> 6);
				RTUSBWriteMACRegister(pAd, TXRX_CSR18, Csr18.value);
				DBGPRINT(RT_DEBUG_INFO, "TXRX_CSR18 = 0x%x\n", Csr18.value);
				
				pAd->BeaconIntervalChangeAllowed = TRUE;
			}
			
			pAd->SentBeaconsCount = 0;
			pAd->ReceivedBeaconsCount = 0;
		}
	}
	Now32 = jiffies;

	{
	    // ChannelQuality = W1*RSSI + W2*TxPRR + W3*RxPER    (RSSI 0..100), (TxPER 100..0), (RxPER 100..0)
	    pAd->Mlme.ChannelQuality = (RSSI_WEIGHTING * pAd->PortCfg.LastRssi + 
	                         TX_WEIGHTING * (100 - TxPRR) + 
	                         RX_WEIGHTING* (100 - RxPER)) / 100;
	    if (pAd->Mlme.ChannelQuality >= 100)
	        pAd->Mlme.ChannelQuality = 100;
	}
    
    DBGPRINT(RT_DEBUG_INFO, "MMCHK - CQI= %d, (Tx Fail=%d/Retry=%d/Total=%d, Rx Fail=%d/Total=%d, RSSI=%d dbm)\n", 
        pAd->Mlme.ChannelQuality, TxFailCount, TxRetryOkCount, TxTotalCnt, RxFailCnt, RxCnt, pAd->PortCfg.LastRssi - pAd->BBPTuningParameters.RSSIToDbmOffset);

    // latch current WLAN counters for next check-for-roaming usage
    memcpy(&pAd->Mlme.PrevWlanCounters, &pAd->WlanCounters, sizeof(COUNTER_802_11));

#if 1//steven:move this from MlmePeriodicExec
	if (INFRA_ON(pAd))
	{
		if (CQI_IS_BAD(pAd->Mlme.ChannelQuality))
		{
			pAd->RalinkCounters.BadCQIAutoRecoveryCount ++;
           	DBGPRINT(RT_DEBUG_TRACE, "MMCHK - Bad CQI. Auto Recovery attempt #%d\n", pAd->RalinkCounters.BadCQIAutoRecoveryCount);
			MlmeAutoRecoverNetwork(pAd);
		}
		else if (CQI_IS_FAIR(pAd->Mlme.ChannelQuality) || CQI_IS_POOR(pAd->Mlme.ChannelQuality))
		{
			// perform aggresive roaming only when SECURITY OFF or WEP64/128;
			// WPA and WPA-PSK has no aggresive roaming because re-negotiation
			// between 802.1x supplicant and authenticator/AAA server is required
			// but can't be guaranteed.
			if (pAd->PortCfg.AuthMode < Ndis802_11AuthModeWPA)
				MlmeCheckForRoaming(pAd, Now32);
		}
	}
#endif

	pAd->PortCfg.CurrTxRateStableTime++;
	CurrRate = pAd->PortCfg.TxRate;
    do
    {
		USHORT TxErrorRatio;
		BOOLEAN fUpgradeQuality = FALSE;
		USHORT  *pRateUpPER, *pRateDownPER;

        if (pAd->PortCfg.EnableAutoRateSwitching == FALSE)
            break;
            
        // do nothing if no traffic in the past period
        if (TxTotalCnt == 0)
        {
            // TxRateUpPenalty maybe too large, we don't want this penalty to affect
            // next Chariot throughput test too much therefore zero it here.
            pAd->PortCfg.TxRateUpPenalty = 0;
            memset(pAd->DrsCounters.TxQuality, 0, MAX_LEN_OF_SUPPORTED_RATES);
            memset(pAd->DrsCounters.PER, 0, MAX_LEN_OF_SUPPORTED_RATES);
            break;
        }

        // decide the next upgrade rate and downgrade rate, if any
        if (pAd->PortCfg.PhyMode == PHY_11BG_MIXED)
        {
            UpRate = Phy11BGNextRateUpward[CurrRate];
            DownRate = Phy11BGNextRateDownward[CurrRate];
        }
        else if (pAd->PortCfg.PhyMode == PHY_11B)
        {
            UpRate = Phy11BNextRateUpward[CurrRate];
            DownRate = Phy11BNextRateDownward[CurrRate];
        }
        else if (pAd->PortCfg.PhyMode == PHY_11A)
        {
            UpRate = Phy11ANextRateUpward[CurrRate];
            DownRate = Phy11ANextRateDownward[CurrRate];
        }
        else // PHY_11ABG_MIXED
        {
            if (pAd->PortCfg.Channel > 14)  
            {
                UpRate = Phy11ANextRateUpward[CurrRate];
                DownRate = Phy11ANextRateDownward[CurrRate];
            }
            else
            {
                UpRate = Phy11BGNextRateUpward[CurrRate];
                DownRate = Phy11BGNextRateDownward[CurrRate];
            }
        }

        if (UpRate > pAd->PortCfg.MaxTxRate)
            UpRate = pAd->PortCfg.MaxTxRate;
    
        // decide TX quality based on Tx retry ratio when enough samples are available
        if (TxTotalCnt > 15)
        {
			USHORT temp = TxRetryOkCount + TxFailCount;
			if (temp < TxTotalCnt)
				TxErrorRatio = (temp *100) / TxTotalCnt;
			else
				TxErrorRatio = 100;

			pRateUpPER = &NewRateUpPER[0];
			pRateDownPER = &NewRateDownPER[0];

            // downgrade TX quality if retry+error ratio reached
            if (TxErrorRatio >= pRateDownPER[CurrRate])
            {
                pAd->DrsCounters.TxQuality[CurrRate] = DRS_TX_QUALITY_WORST_BOUND;
            }
            // upgrade TX quality if retry+error ratio reached
            else if (TxErrorRatio <= pRateUpPER[CurrRate])
            {
                fUpgradeQuality = TRUE;
                if (pAd->DrsCounters.TxQuality[CurrRate])
                    pAd->DrsCounters.TxQuality[CurrRate] --;  // quality very good in CurrRate
                    
                if (pAd->PortCfg.TxRateUpPenalty)
                    pAd->PortCfg.TxRateUpPenalty --;
                else if (pAd->DrsCounters.TxQuality[UpRate])
                    pAd->DrsCounters.TxQuality[UpRate] --;    // may improve next UP rate's quality
            }
            
        }
        
        // if not enough TX samples, decide by heuristic rules
        else
        {
            TxErrorRatio = 0;
            
            // Downgrade TX quality upon -
            // 1. any TX failure in the past second
            // 2. TX retry ratio too high when enough TX samples are available
            if (TxFailCount)
            {
                if ((TxFailCount <= 1) &&
                    (TxRealOkCount + TxRetryOkCount))
                {
                    pAd->DrsCounters.TxQuality[CurrRate] += 2;   // degrade quality
                    if (pAd->DrsCounters.TxQuality[CurrRate] > DRS_TX_QUALITY_WORST_BOUND)
                        pAd->DrsCounters.TxQuality[CurrRate] = DRS_TX_QUALITY_WORST_BOUND;
                }
                else // more than 2 failure, or no TX ok cases
                {
                    pAd->DrsCounters.TxQuality[CurrRate]  = DRS_TX_QUALITY_WORST_BOUND;   // quality bad
                }
            }
            // upgrade TX quality if -
            // 1. no TX failure but do have TX ok case, and
            // 2. there's more one-time-ok cases than retry-ok cases in the past second
            // 3. current rate's Tx retry ratio <= 10%
            else if ((TxRealOkCount > TxRetryOkCount))
            {
                fUpgradeQuality = TRUE;
                if (pAd->DrsCounters.TxQuality[CurrRate])
                    pAd->DrsCounters.TxQuality[CurrRate] --;  // quality very good in CurrRate

                if (pAd->PortCfg.TxRateUpPenalty)
                    pAd->PortCfg.TxRateUpPenalty --;
                else if (pAd->DrsCounters.TxQuality[UpRate])
                    pAd->DrsCounters.TxQuality[UpRate] --;    // may improve next UP rate's quality
            }
        }

        pAd->DrsCounters.PER[CurrRate] = (UCHAR)TxErrorRatio;

        if (pAd->DrsCounters.fNoisyEnvironment)
        {
            DBGPRINT(RT_DEBUG_TRACE,"DRS(noisy): ");
        }
        else
        {
            DBGPRINT(RT_DEBUG_TRACE,"DRS: ");
        }
        DBGPRINT_RAW(RT_DEBUG_TRACE,"Qty[%d]=%d PER=%d%% %d-sec, Qty[%d]=%d, Pty=%d\n", 
            RateIdToMbps[CurrRate], pAd->DrsCounters.TxQuality[CurrRate],
            TxErrorRatio,
            pAd->DrsCounters.CurrTxRateStableTime,
            RateIdToMbps[UpRate], pAd->DrsCounters.TxQuality[UpRate],
            pAd->DrsCounters.TxRateUpPenalty);
        
        // 2004-3-13 special case: Claim noisy environment
        //   decide if there was a false "rate down" in the past 2 sec due to noisy 
        //   environment. if so, we would rather switch back to the higher TX rate. 
        //   criteria -
        //     1. there's a higher rate available, AND
        //     2. there was a rate-down happened, AND
        //     3. current rate has 75% > PER > 25%, AND
        //     4. comparing to UpRate, current rate didn't improve PER more than 5 %
        if ((UpRate != CurrRate)                              &&
            (pAd->DrsCounters.LastSecTxRateChangeAction == 2) &&
            (((pAd->DrsCounters.PER[CurrRate] > 20) || (pAd->DrsCounters.fNoisyEnvironment)) && 
			(pAd->DrsCounters.PER[CurrRate] < 75)) && 
            ((pAd->DrsCounters.PER[CurrRate]+5) > pAd->DrsCounters.PER[UpRate]))
        {
            // we believe this is a noisy environment. better stay at UpRate
            DBGPRINT(RT_DEBUG_TRACE,"DRS: #### enter Noisy environment ####\n");
            pAd->DrsCounters.fNoisyEnvironment = TRUE;

            // 2004-3-14 when claiming noisy environment, we're not only switch back
            //   to UpRate, but can be more aggressive to use one more rate up
            UpRate++;
//          if (UpRate>RATE_54) UpRate=RATE_54;
            if ((UpRate==RATE_6) || (UpRate==RATE_9)) UpRate=RATE_12;
            if (UpRate > pAd->PortCfg.MaxTxRate)
                UpRate = pAd->PortCfg.MaxTxRate;
            pAd->PortCfg.TxRate = UpRate;
            break;
        }

        // 2004-3-12 special case: Leave noisy environment
        //   The interference has gone suddenly. reset TX rate to
        //   the theoritical value according to RSSI. Criteria -
        //     1. it's currently in noisy environment
        //     2. PER drops to be below 12%
        if ((pAd->DrsCounters.fNoisyEnvironment == TRUE) &&
            (TxTotalCnt > 15) && (pAd->DrsCounters.PER[CurrRate] <= 12))
        {
            UCHAR JumpUpRate;

            pAd->DrsCounters.fNoisyEnvironment = FALSE;
            for (JumpUpRate = RATE_54; JumpUpRate > RATE_1; JumpUpRate--)
            {
                if (pAd->PortCfg.AvgRssi > (RssiSafeLevelForTxRate[JumpUpRate] + pAd->BBPTuningParameters.RSSIToDbmOffset))
                    break;
            }

            if (JumpUpRate > pAd->PortCfg.MaxTxRate)
                JumpUpRate = pAd->PortCfg.MaxTxRate;
            
            DBGPRINT(RT_DEBUG_TRACE,"DRS: #### leave Noisy environment ####, RSSI=%d, JumpUpRate=%d\n",
                pAd->PortCfg.AvgRssi - pAd->BBPTuningParameters.RSSIToDbmOffset, RateIdToMbps[JumpUpRate]);
            
            if (JumpUpRate > CurrRate)
            {
                pAd->PortCfg.TxRate = JumpUpRate;
               	break;
            }
        }
        
        // perform DRS - consider TxRate Down first, then rate up.
        //     1. rate down, if current TX rate's quality is not good
        //     2. rate up, if UPRate's quality is very good
        if ((pAd->DrsCounters.TxQuality[CurrRate] >= DRS_TX_QUALITY_WORST_BOUND) &&
            (CurrRate != DownRate))
        {

			pAd->PortCfg.TxRate = DownRate;
        }
        else if ((pAd->DrsCounters.TxQuality[CurrRate] <= 0) && 
            (pAd->DrsCounters.TxQuality[UpRate] <=0)         &&
            (CurrRate != UpRate))
        {
            pAd->PortCfg.TxRate = UpRate;
        }
    }while (FALSE);

    // if rate-up happen, clear all bad history of all TX rates
    if (pAd->PortCfg.TxRate > CurrRate)
    {
       	DBGPRINT(RT_DEBUG_TRACE,"DRS: ++TX rate from %d to %d Mbps\n", RateIdToMbps[CurrRate],RateIdToMbps[pAd->PortCfg.TxRate]);
        pAd->DrsCounters.CurrTxRateStableTime = 0;
        pAd->DrsCounters.TxRateUpPenalty = 0;
        pAd->DrsCounters.LastSecTxRateChangeAction = 1; // rate UP
        memset(pAd->DrsCounters.TxQuality, 0, MAX_LEN_OF_SUPPORTED_RATES);
        memset(pAd->DrsCounters.PER, 0, MAX_LEN_OF_SUPPORTED_RATES);
    }
    // if rate-down happen, only clear DownRate's bad history
    else if (pAd->PortCfg.TxRate < CurrRate)
    {
       	DBGPRINT(RT_DEBUG_TRACE,"DRS: --TX rate from %d to %d Mbps\n", RateIdToMbps[CurrRate],RateIdToMbps[pAd->PortCfg.TxRate]);
	    // shorter stable time require more penalty in next rate UP criteria
       	if (pAd->DrsCounters.CurrTxRateStableTime < 4)      // less then 4 sec
       	    pAd->DrsCounters.TxRateUpPenalty = DRS_PENALTY; // add 8 sec penalty
       	else if (pAd->DrsCounters.CurrTxRateStableTime < 8) // less then 8 sec
       	    pAd->DrsCounters.TxRateUpPenalty = 2;           // add 2 sec penalty
       	else                                                // >= 8 sec
       	    pAd->DrsCounters.TxRateUpPenalty = 0;           // no penalty
       	    
        pAd->DrsCounters.CurrTxRateStableTime = 0;
        pAd->DrsCounters.LastSecTxRateChangeAction = 2; // rate DOWN
       	pAd->DrsCounters.TxQuality[pAd->PortCfg.TxRate] = 0;
       	pAd->DrsCounters.PER[pAd->PortCfg.TxRate] = 0;
    }
    else
        pAd->DrsCounters.LastSecTxRateChangeAction = 0; // rate no change
	}


}

/*
    ==========================================================================
    Description:
        This routine is executed periodically inside MlmePeriodicExec() after 
        association with an AP.
        It checks if PortCfg.Psm is consistent with user policy (recorded in
        PortCfg.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 Massoc==TRUE (INFRA mode), Psm should not be switch to PWR_SAVE
           if outgoing traffic available in TxRing or PrioRing.
    Output:
        1. change pAd->PortCfg.Psm to PWR_SAVE or leave it untouched
        
	IRQL = DISPATCH_LEVEL

    ==========================================================================
 */
VOID MlmeCheckForPsmChange(
    IN PRT2570ADAPTER pAd)
{
	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 period
    	PowerMode = pAd->PortCfg.WindowsPowerMode;
    
    if (INFRA_ON(pAd) &&
        (PowerMode != Ndis802_11PowerModeCAM) &&
		(pAd->BulkOutPending == FALSE) &&
		(LOCAL_TX_RING_EMPTY(pAd)) &&
		(skb_queue_empty(&pAd->SendTxWaitQueue)) &&
        (pAd->PortCfg.Psm == PWR_ACTIVE) &&
        (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE) &&
        (pAd->WlanCounters.TransmittedFragmentCount.vv.LowPart == pAd->Mlme.PrevTxCnt))
    {
		RTUSBEnqueueInternalCmd(pAd, RT_OID_SET_PSM_BIT_SAVE);
    }
    
    // latch current count for next-time comparison
    pAd->Mlme.PrevTxCnt = pAd->WlanCounters.TransmittedFragmentCount.vv.LowPart;

}

// IRQL = PASSIVE_LEVEL
// IRQL = DISPATCH_LEVEL
VOID MlmeSetPsmBit(
    IN PRT2570ADAPTER pAd, 
    IN USHORT psm)
{
    pAd->PortCfg.Psm = psm;    
    
    DBGPRINT(RT_DEBUG_TEMP, "MMCHK - change PSM bit to %d <<<\n", psm);
}

// IRQL = DISPATCH_LEVEL
// IRQL = DISPATCH_LEVEL
VOID MlmeSetTxPreamble(
    IN PRT2570ADAPTER pAd, 
    IN USHORT TxPreamble)
{
	USHORT	value;

    RTUSBReadMACRegister(pAd, TXRX_CSR10, &value);
    if (TxPreamble == Rt802_11PreambleShort)
    {
        DBGPRINT(RT_DEBUG_TRACE, "MlmeSetTxPreamble (= SHORT PREAMBLE)\n");
		value |= 0x0004;
        pAd->PortCfg.TxPreambleInUsed = Rt802_11PreambleShort;
    }
    else
    {
        DBGPRINT(RT_DEBUG_TRACE, "MlmeSetTxPreamble (= LONG PREAMBLE)\n");
		value &= 0xFFFB;
        pAd->PortCfg.TxPreambleInUsed = Rt802_11PreambleLong;
    }

    RTUSBWriteMACRegister(pAd, TXRX_CSR10, value);
}
    
// 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 PRT2570ADAPTER pAd,
    IN BOOLEAN		 bLinkUp)
{
    int i, num;
    UCHAR Rate, MaxDesire = RATE_1, MaxSupport = RATE_1;
    USHORT BasicRateBitmap = 0;
    UCHAR CurrBasicRate = RATE_1;

    // find max desired rate
    num = 0;
    for (i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
    {
        switch (pAd->PortCfg.DesiredRates[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;

		// Fixed the Maximum rate of 2426 to b only
		if (pAd->PortCfg.RfType == RFIC_2426)
			if (MaxDesire > RATE_11)
				MaxDesire = RATE_11;
    }

    // 2003-12-10 802.11g WIFI spec disallow OFDM rates in 802.11g ADHOC mode
    if ((pAd->PortCfg.BssType == BSS_INDEP)        &&
        (pAd->PortCfg.PhyMode == PHY_11BG_MIXED)   && 
        (pAd->PortCfg.AdhocMode == 0) &&
        (MaxDesire > RATE_11))
        MaxDesire = RATE_11;
    
    pAd->PortCfg.MaxDesiredRate = MaxDesire;
    
    // Auto rate switching is enabled only if more than one DESIRED RATES are 
    // specified; otherwise disabled
    if (num <= 1)
        pAd->PortCfg.EnableAutoRateSwitching = FALSE;
    else
        pAd->PortCfg.EnableAutoRateSwitching = TRUE;

    // find max supported rate
    for (i=0; i<pAd->PortCfg.SupportedRatesLen; i++)
    {
        switch (pAd->PortCfg.SupportedRates[i] & 0x7f)
        {
            case 2: Rate = RATE_1;   
                    if (pAd->PortCfg.SupportedRates[i] & 0x80) 
                        BasicRateBitmap |= 0x0001;  
                    break;
            case 4: Rate = RATE_2;   
                    if (pAd->PortCfg.SupportedRates[i] & 0x80) 
                        BasicRateBitmap |= 0x0002;  
                    break;
            case 11: 
                    Rate = RATE_5_5; 
                    if (pAd->PortCfg.SupportedRates[i] & 0x80) 
                        BasicRateBitmap |= 0x0004;  
                    break;
            case 22: 
                    Rate = RATE_11;  
                    if (pAd->PortCfg.SupportedRates[i] & 0x80) 
                        BasicRateBitmap |= 0x0008;  
                    break;
            case 12: 
                    Rate = RATE_6;   
//                    if (pAd->PortCfg.SupportedRates[i] & 0x80) 
                        BasicRateBitmap |= 0x0010;  
                    break;
            case 18: 
                    Rate = RATE_9;   
                    if (pAd->PortCfg.SupportedRates[i] & 0x80) 
                        BasicRateBitmap |= 0x0020;  
                    break;
            case 24: 
                    Rate = RATE_12;  
//                    if (pAd->PortCfg.SupportedRates[i] & 0x80) 
                        BasicRateBitmap |= 0x0040;  
                    break;
            case 36: 
                    Rate = RATE_18;  
                    if (pAd->PortCfg.SupportedRates[i] & 0x80) 
                        BasicRateBitmap |= 0x0080;  
                    break;
            case 48: 
                    Rate = RATE_24;  
//                  if (pAd->PortCfg.SupportedRates[i] & 0x80) 
						BasicRateBitmap |= 0x0100;  
                    break;
            case 72: 
                    Rate = RATE_36;  
                    if (pAd->PortCfg.SupportedRates[i] & 0x80) 
                        BasicRateBitmap |= 0x0200;  
                    break;
            case 96: 
                    Rate = RATE_48;  
                    if (pAd->PortCfg.SupportedRates[i] & 0x80) 
                        BasicRateBitmap |= 0x0400;  
                    break;
            case 108: 
                    Rate = RATE_54; 
                    if (pAd->PortCfg.SupportedRates[i] & 0x80) 
                        BasicRateBitmap |= 0x0800;  
                    break;
            default:  
                    Rate = RATE_1;   
                    break;
        }
        if (MaxSupport < Rate)  MaxSupport = Rate;
    }

	// Limit the max support rate and basic rate map to 11b only
	if (pAd->PortCfg.RfType == RFIC_2426)
	{
		if (MaxSupport > RATE_11)
		{
			MaxSupport = RATE_11;
			BasicRateBitmap &= 0x000f;
		}
	}
	
	RTUSBWriteMACRegister(pAd, TXRX_CSR11, 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->PortCfg.ExpectedACKRate[i] = CurrBasicRate;
        DBGPRINT(RT_DEBUG_INFO,"Exptected ACK rate[%d] = %d Mbps\n", RateIdToMbps[i], RateIdToMbps[CurrBasicRate]);
    }

    // max tx rate = min {max desire rate, max supported rate}
    if (MaxSupport < MaxDesire)
        pAd->PortCfg.MaxTxRate = MaxSupport;
    else
        pAd->PortCfg.MaxTxRate = MaxDesire;

    // 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
    if (pAd->PortCfg.EnableAutoRateSwitching)
    {
        if (pAd->PortCfg.Channel > 14)
            pAd->PortCfg.TxRate = RATE_6; // 802.11a
        else 
		{
			int dbm = pAd->PortCfg.AvgRssi - pAd->BBPTuningParameters.RSSIToDbmOffset;
			if (dbm > -70)
			{
				pAd->PortCfg.TxRate = pAd->PortCfg.MaxTxRate;
			}
			else if (dbm > -75)
			{
				pAd->PortCfg.TxRate = min((INT)(pAd->PortCfg.MaxTxRate), RATE_36);
			}
			else
			{
				pAd->PortCfg.TxRate = min((INT)(pAd->PortCfg.MaxTxRate), RATE_11);
			}
		}
    }
    else
        pAd->PortCfg.TxRate = pAd->PortCfg.MaxTxRate;

    switch (pAd->PortCfg.PhyMode) {
        case PHY_11BG_MIXED:
        case PHY_11B:
            pAd->PortCfg.MlmeRate = RATE_2;
#ifdef	WIFI_TEST			
            pAd->PortCfg.RtsRate = RATE_11;
#else
            pAd->PortCfg.RtsRate = RATE_2;
#endif
            break;
        case PHY_11A:
            pAd->PortCfg.MlmeRate = RATE_6;
            pAd->PortCfg.RtsRate = RATE_6;
            break;
        case PHY_11ABG_MIXED:
            if (pAd->PortCfg.Channel <= 14)
            {
                pAd->PortCfg.MlmeRate = RATE_2;
                pAd->PortCfg.RtsRate = RATE_2;
            }
            else
            {
                pAd->PortCfg.MlmeRate = RATE_6;
                pAd->PortCfg.RtsRate = RATE_6;
            }
            break;
        default: // error
            pAd->PortCfg.MlmeRate = RATE_2;
            pAd->PortCfg.RtsRate = RATE_2;
            break;
    }
    
    DBGPRINT(RT_DEBUG_TRACE, " MlmeUpdateTxRates (MaxDesire=%d, MaxSupport=%d, MaxTxRate=%d, Rate Switching =%d)\n", 
             RateIdToMbps[MaxDesire], RateIdToMbps[MaxSupport], RateIdToMbps[pAd->PortCfg.MaxTxRate], pAd->PortCfg.EnableAutoRateSwitching);
    DBGPRINT(RT_DEBUG_TRACE, " MlmeUpdateTxRates (TxRate=%d, RtsRate=%d, BasicRateBitmap=0x%04x)\n", 
             RateIdToMbps[pAd->PortCfg.TxRate], RateIdToMbps[pAd->PortCfg.RtsRate], BasicRateBitmap);
}

// IRQL = DISPATCH_LEVEL
VOID MlmeRadioOff(
    IN PRT2570ADAPTER pAd)
{
	MLME_DISASSOC_REQ_STRUCT DisReq;
	MLME_QUEUE_ELEM          MsgElem;
	UINT	i = 0;

	//
	// Since set flag fRTMP_ADAPTER_RADIO_OFF will diable RTUSBKickBulkOut function.
	// So before set flag fRTMP_ADAPTER_RADIO_OFF, 
	// we should send a disassoc frame to our AP if neend.
	//
	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_REMOVE_IN_PROGRESS))
	{
		if (INFRA_ON(pAd)) 
		{
			COPY_MAC_ADDR(&DisReq.Addr, &pAd->PortCfg.Bssid);
			DisReq.Reason =  REASON_DISASSOC_STA_LEAVING;

			MsgElem.Machine = ASSOC_STATE_MACHINE;
			MsgElem.MsgType = MT2_MLME_DISASSOC_REQ;
			MsgElem.MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
			memcpy(MsgElem.Msg, &DisReq, sizeof(MLME_DISASSOC_REQ_STRUCT));

			MlmeDisassocReqAction(pAd, &MsgElem);
			NdisMSleep(1);
		}
		// Set Radio off flag will turn off RTUSBKickBulkOut function
		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
        	DBGPRINT(RT_DEBUG_ERROR, "3Set fRTMP_ADAPTER_RADIO_OFF ");
	
		LinkDown(pAd);
	}
	else
	{
		// Set Radio off flag will turn off RTUSBKickBulkOut function
		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
        	DBGPRINT(RT_DEBUG_ERROR, "4Set fRTMP_ADAPTER_RADIO_OFF ");
	}	
	DBGPRINT_RAW(RT_DEBUG_TEMP,"MlmeRadioOff \n");
	RTUSBRejectPendingPackets(pAd);//reject all NDIS packets waiting in TX queue
	RTUSBCleanUpDataBulkOutQueue(pAd);
	MlmeSuspend(pAd);
	DBGPRINT_RAW(RT_DEBUG_TEMP,"2MlmeRadioOff \n");
	// Abort Tx
	// Disable Rx
	RTUSBWriteMACRegister(pAd, TXRX_CSR2, 1);
	//Ask our device to complete any pending bulk in IRP.
	while (( atomic_read(&pAd->PendingRx) > 0) || (pAd->BulkOutPending == TRUE))
	{
		if (atomic_read(&pAd->PendingRx) > 0)
		{
			DBGPRINT(RT_DEBUG_TRACE, "BulkIn IRP Pending!!!\n");
			RTUSB_VendorRequest(pAd,
				0,
				DEVICE_VENDOR_REQUEST_OUT,
				0x0C,
				0x0,
				0x0,
				NULL,
				0);
		}
		
		if (pAd->BulkOutPending == TRUE)
		{
			DBGPRINT(RT_DEBUG_TRACE, "BulkOut IRP Pending!!!\n");
			if (i == 0)
			{
				RTUSBCancelPendingBulkOutIRP(pAd);
				i++;
			}
		}
		
		NdisMSleep(5);
	}
#if 1

	// Turn off radio
	RTUSBWriteMACRegister(pAd, MAC_CSR13, 0x2121);
	RTUSBWriteMACRegister(pAd, MAC_CSR14, 0x2121);

	if (pAd->PortCfg.LedMode == LED_MODE_ASUS)
	{
		RTUSBWriteMACRegister(pAd, MAC_CSR20, 0x0001); 		
	}
	
	// Clean up old bss table
	BssTableInit(&pAd->PortCfg.BssTab);
	DBGPRINT(RT_DEBUG_TEMP,"<==MlmeRadioOff \n" );
#endif
}

// IRQL = DISPATCH_LEVEL
VOID MlmeRadioOn(
    IN PRT2570ADAPTER pAd)
{	
	// Turn on radio
	RTUSBWriteMACRegister(pAd, MAC_CSR13, 0x3100);
	RTUSBWriteMACRegister(pAd, MAC_CSR14, 0x3f3b);

	// Abort Tx
	//steven:don't have this	RTMP_IO_WRITE32(pAd, TXCSR0, 0x08);
	// Disable Rx
	RTUSBWriteMACRegister(pAd, TXRX_CSR2, 1);

	NICResetFromError(pAd);
	// Clear Radio off flag
	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
	MlmeResume(pAd);
	RTUSBBulkReceive(pAd);
	RTUSBWriteMACRegister(pAd, TXRX_CSR2, 0x7e);
	if (pAd->PortCfg.LedMode == LED_MODE_ASUS)
	{
		RTUSBWriteMACRegister(pAd, MAC_CSR20, 0x0003);
	}
}


// ===========================================================================================
// 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;
    for (i = 0; i < MAX_LEN_OF_BSS_TABLE; i++) 
    {
        memset(&Tab->BssEntry[i], 0, sizeof(BSS_ENTRY));
    }
}

/*! \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 PMACADDR Bssid) 
{
    UCHAR i;
    
    for (i = 0; i < Tab->BssNr; i++) 
    {
        //printf("comparing %s and %s\n", p_tab->bss[i].ssid, ssid);
        if (MAC_ADDR_EQUAL(&(Tab->BssEntry[i].Bssid), Bssid)) 
        { 
            return i;
        }
    }
    return (ULONG)BSS_NOT_FOUND;
}

// IRQL = DISPATCH_LEVEL
VOID BssTableDeleteEntry(
    IN OUT	BSS_TABLE *Tab, 
    IN		PMACADDR Bssid) 
{
    UCHAR i, j;
    
    for (i = 0; i < Tab->BssNr; i++) 
    {
        //printf("comparing %s and %s\n", p_tab->bss[i].ssid, ssid);
        if (MAC_ADDR_EQUAL(&(Tab->BssEntry[i].Bssid), Bssid)) 
        {
        	for (j = i; j < Tab->BssNr - 1; j++)
        	{
        		memcpy(&(Tab->BssEntry[j]), &(Tab->BssEntry[j + 1]), sizeof(BSS_ENTRY));
        	}
	        Tab->BssNr -= 1;
            return;
        }
    }
}

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};
/*! \brief
 *  \param 
 *  \return
 *  \pre
 *  \post
	 
 IRQL = DISPATCH_LEVEL
 
 */
VOID BssEntrySet(
    IN	PRT2570ADAPTER	pAd, 
    OUT BSS_ENTRY *pBss, 
    IN MACADDR *pBssid, 
    IN CHAR Ssid[], 
    IN UCHAR SsidLen, 
    IN UCHAR BssType, 
    IN USHORT BeaconPeriod, 
    IN BOOLEAN CfExist,
    IN CF_PARM *pCfParm, 
    IN USHORT AtimWin, 
    IN USHORT CapabilityInfo, 
    IN UCHAR Rates[], 
    IN UCHAR RatesLen,
    IN BOOLEAN ExtendedRateIeExist,
    IN UCHAR Channel,
    IN UCHAR Rssi,
    IN LARGE_INTEGER TimeStamp,
    IN UCHAR 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 (memcmp(Ssid, ZeroSsid, SsidLen))
		{
    		memcpy(pBss->Ssid, Ssid, SsidLen);
    		pBss->SsidLen = SsidLen;
			pBss->Hidden = 0;
		}
	}
    pBss->BssType = BssType;
    pBss->BeaconPeriod = BeaconPeriod;
    if (BssType == BSS_INFRA) 
    {
        if (CfExist) 
        {
            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);
    memcpy(pBss->Rates, Rates, RatesLen);
    pBss->RatesLen = RatesLen;
    pBss->ExtendedRateIeExist = ExtendedRateIeExist;
    pBss->Channel = Channel;
    pBss->Rssi = Rssi;

	// New for microsoft Fixed IEs
	memcpy(pBss->FixIEs.Timestamp, &TimeStamp, 8);
	pBss->FixIEs.BeaconInterval = BeaconPeriod;
	pBss->FixIEs.Capabilities = CapabilityInfo;

	// New for microsoft Variable IEs
	if (LengthVIE != 0)
	{
		pBss->VarIELen = LengthVIE;
		memcpy(pBss->VarIEs, pVIE, pBss->VarIELen);
		BssCipherParse(pBss);
	}
	else
	{
		pBss->VarIELen = 0;
		// No SSN ID, if security is on, this is WEP algorithm
		if  (pBss->Privacy)
			pBss->WepStatus = Ndis802_11WEPEnabled;
		// No SSN ID, security is also off.
		else
			pBss->WepStatus = Ndis802_11WEPDisabled;
	}
}

/*! 
 *  \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	PRT2570ADAPTER	pAd, 
    OUT BSS_TABLE *Tab, 
    IN MACADDR *Bssid, 
    IN CHAR Ssid[], 
    IN UCHAR SsidLen, 
    IN UCHAR BssType, 
    IN USHORT BeaconPeriod, 
    IN BOOLEAN CfExist, 
    IN CF_PARM *CfParm, 
    IN USHORT AtimWin, 
    IN USHORT CapabilityInfo, 
    IN UCHAR Rates[],
    IN UCHAR RatesLen,
    IN BOOLEAN ExtendedRateIeExist,
    IN UCHAR ChannelNo,
    IN UCHAR Rssi,
    IN LARGE_INTEGER TimeStamp,
    IN UCHAR LengthVIE,
    IN PNDIS_802_11_VARIABLE_IEs pVIE)
{
    ULONG   Idx;

    Idx = BssTableSearch(Tab, Bssid);
    if (Idx == BSS_NOT_FOUND) 
    {
        if (Tab->BssNr >= MAX_LEN_OF_BSS_TABLE)
            return BSS_NOT_FOUND;
        Idx = Tab->BssNr;
        BssEntrySet(pAd, &Tab->BssEntry[Idx], Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
                    CfExist, CfParm, AtimWin, CapabilityInfo, Rates, RatesLen, ExtendedRateIeExist,
                    ChannelNo, Rssi, TimeStamp, LengthVIE, pVIE);
        Tab->BssNr++;
    } 
    else
    {
        BssEntrySet(pAd, &Tab->BssEntry[Idx], Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
                    CfExist, CfParm, AtimWin, CapabilityInfo, Rates, RatesLen, ExtendedRateIeExist,
                    ChannelNo, Rssi, TimeStamp, LengthVIE, pVIE);
    }
	//DBGPRINT_RAW(RT_DEBUG_TEMP," %s   Wep %d auth %d \n",Ssid, Tab->BssEntry[Idx].WepStatus, Tab->BssEntry[Idx].AuthMode);
    
    return Idx;
}


// IRQL = DISPATCH_LEVEL
VOID BssTableSsidSort(
    IN	PRT2570ADAPTER	pAd, 
    OUT BSS_TABLE *OutTab, 
    IN  CHAR Ssid[], 
    IN  UCHAR SsidLen) 
{
    INT i;
    
    BssTableInit(OutTab);

    for (i = 0; i < pAd->PortCfg.BssTab.BssNr; i++) 
    {
        BSS_ENTRY *pInBss = &pAd->PortCfg.BssTab.BssEntry[i];
        
        if ((pInBss->BssType == pAd->PortCfg.BssType) && 
			((pInBss->SsidLen==SsidLen) && (memcmp(pInBss->Ssid, Ssid, (ULONG) SsidLen) == 0)))
        {
            BSS_ENTRY *pOutBss = &OutTab->BssEntry[OutTab->BssNr];
			// New for WPA2
			// Check the Authmode first
			if (pAd->PortCfg.AuthMode >= Ndis802_11AuthModeWPA)
			{
				// Check AuthMode and AuthModeAux for matching, in case AP support dual-mode
				if ((pAd->PortCfg.AuthMode != pInBss->AuthMode) && (pAd->PortCfg.AuthMode != pInBss->AuthModeAux))
					// None matched
					continue;
				
				// Check cipher suite, AP must have more secured cipher than station setting
				if ((pAd->PortCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->PortCfg.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->PortCfg.WepStatus != pInBss->WPA.GroupCipher)
							continue;						
				
					// check group cipher
					if (pAd->PortCfg.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->PortCfg.WepStatus == Ndis802_11Encryption2Enabled) && 
						(pAd->PortCfg.WepStatus != pInBss->WPA.PairCipher) && 
						(pAd->PortCfg.WepStatus != pInBss->WPA.PairCipherAux))
						continue;						
				}
				else if ((pAd->PortCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->PortCfg.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->PortCfg.WepStatus != pInBss->WPA2.GroupCipher)
							continue;						
				
					// check group cipher
					if (pAd->PortCfg.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->PortCfg.WepStatus == Ndis802_11Encryption2Enabled) && 
						(pAd->PortCfg.WepStatus != pInBss->WPA2.PairCipher) && 
						(pAd->PortCfg.WepStatus != pInBss->WPA2.PairCipherAux))
						continue;						
				}
			}			
			// Bss Type matched, SSID matched. 
			// We will check wepstatus for qualification Bss
			else if (pAd->PortCfg.WepStatus != pInBss->WepStatus)
					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;
			
            // copy matching BSS from InTab to OutTab
            memcpy(pOutBss, pInBss, sizeof(BSS_ENTRY));
            
            OutTab->BssNr++;
        }
        else if ((pInBss->BssType == pAd->PortCfg.BssType) && (SsidLen == 0))
        {
            BSS_ENTRY *pOutBss = &OutTab->BssEntry[OutTab->BssNr];

			// New for WPA2
			// Check the Authmode first
			if (pAd->PortCfg.AuthMode >= Ndis802_11AuthModeWPA)
			{
				// Check AuthMode and AuthModeAux for matching, in case AP support dual-mode
				if ((pAd->PortCfg.AuthMode != pInBss->AuthMode) && (pAd->PortCfg.AuthMode != pInBss->AuthModeAux))
					// None matched
					continue;
			
				// Check cipher suite, AP must have more secured cipher than station setting
				if ((pAd->PortCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->PortCfg.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->PortCfg.WepStatus != pInBss->WPA.GroupCipher)
							continue;						
				
					// check group cipher
					if (pAd->PortCfg.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->PortCfg.WepStatus == Ndis802_11Encryption2Enabled) && 
						(pAd->PortCfg.WepStatus != pInBss->WPA.PairCipher) && 
						(pAd->PortCfg.WepStatus != pInBss->WPA.PairCipherAux))
						continue;						
				}
				else if ((pAd->PortCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->PortCfg.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->PortCfg.WepStatus != pInBss->WPA2.GroupCipher)
							continue;
				
					// check group cipher
					if (pAd->PortCfg.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->PortCfg.WepStatus == Ndis802_11Encryption2Enabled) && 
						(pAd->PortCfg.WepStatus != pInBss->WPA2.PairCipher) && 
						(pAd->PortCfg.WepStatus != pInBss->WPA2.PairCipherAux))
						continue;						
				}
			}
			// Bss Type matched, SSID matched. 
			// We will check wepstatus for qualification Bss
			else if (pAd->PortCfg.WepStatus != pInBss->WepStatus)
					continue;
			
            // copy matching BSS from InTab to OutTab
            memcpy(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) 
            {
                memcpy(&TmpBss, &OutTab->BssEntry[j], sizeof(BSS_ENTRY));
                memcpy(&OutTab->BssEntry[j], &OutTab->BssEntry[i], sizeof(BSS_ENTRY));
                memcpy(&OutTab->BssEntry[i], &TmpBss, sizeof(BSS_ENTRY));
            }
        }
    }
}

extern	UCHAR	RSN_OUI[];		// in sanity.c

VOID		BssCipherParse(
	IN OUT	PBSS_ENTRY	pBss)
{
#if 1
	PBEACON_EID_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;

	// Set default to disable & open authentication before parsing variable IE
	pBss->WepStatus     = Ndis802_11WEPDisabled;
	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 = (PBEACON_EID_STRUCT) pTmp;
		switch (pEid->Eid)
		{
			case IE_WPA:
				// 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:
					case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
						pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
						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;				
				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;
				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_INDEP)
				{
					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 (pRsnHeader->Version != 1)
					break;
				pTmp   += sizeof(RSN_IE_HEADER_STRUCT);

				// 1. Check group cipher
				pCipher = (PCIPHER_SUITE_STRUCT) pTmp;
				if (memcmp(pTmp, RSN_OUI, 3) != 0)
					break;

				// Parse group cipher
				switch (pCipher->Type)
				{
					case 1:
					case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
						pBss->WPA2.GroupCipher = Ndis802_11Encryption1Enabled;
						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;
				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;
				pTmp   += sizeof(USHORT);

				// 5. Get AKM ciphers
				pAKM = (PAKM_SUITE_STRUCT) pTmp;
				if (memcmp(pTmp, RSN_OUI, 3) != 0)
					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_INDEP)
				{
					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;
				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);
	}
#endif
	return;
}

// ===========================================================================================
// 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 PRT2570ADAPTER pAd, 
    OUT MACADDR *Addr) 
{
    INT i;

    for (i = 0; i < MAC_ADDR_LEN; i++) 
    {
        Addr->Octet[i] = RandomByte(pAd);
    }
    
    Addr->Octet[0] = (Addr->Octet[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->PortCfg
 *   - bssid
 *   - station address
 *  \post
 *  \note this function initializes the following field

 IRQL = PASSIVE_LEVEL
 IRQL = DISPATCH_LEVEL
  
 */
VOID MgtMacHeaderInit(
    IN	PRT2570ADAPTER	pAd, 
    IN OUT PMACHDR Hdr, 
    IN UCHAR Subtype, 
    IN UCHAR ToDs, 
    IN PMACADDR Ds, 
    IN PMACADDR Bssid) 
{
    memset(Hdr, 0, sizeof(MACHDR));
    Hdr->Type = BTYPE_MGMT;
    Hdr->SubType = Subtype;
    Hdr->Tods = ToDs;
    COPY_MAC_ADDR(&Hdr->Addr1, Ds);
    COPY_MAC_ADDR(&Hdr->Addr2, &pAd->CurrentAddress);
    COPY_MAC_ADDR(&Hdr->Addr3, Bssid);
}



// ===========================================================================================
// 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);
        memcpy(&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
 */
NDIS_STATUS MlmeQueueInit(
	IN MLME_QUEUE *Queue) 
{
	INT i;

	DBGPRINT(RT_DEBUG_INFO,"--> MlmeQueueInit\n");

	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;
		memset(Queue->Entry[i].Msg, 0, MAX_LEN_OF_MLME_BUFFER);
	}

	DBGPRINT(RT_DEBUG_INFO,"<-- MlmeQueueInit\n");
	return NDIS_STATUS_SUCCESS;
}

BOOLEAN MlmeEnqueue(
    IN PRT2570ADAPTER pAd, 
	IN ULONG Machine, 
	IN ULONG MsgType, 
	IN ULONG MsgLen, 
	IN VOID *Msg) 
{
	INT Tail;
	MLME_QUEUE	*Queue = (MLME_QUEUE *)&pAd->Mlme.Queue;

#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 5, 0)
	if (pAd->MLMEThr_pid > 0)
#else
	if (pAd->MLMEThr_active)
#endif
	{

		if (MlmeQueueFull(Queue)) 
		{
			DBGPRINT(RT_DEBUG_ERROR,"MlmeEnqueue full, msg dropped and may corrupt MLME\n");
			return FALSE;
		}

		NdisAcquireSpinLock(&(Queue->Lock));
		Tail = Queue->Tail;
		Queue->Tail++;
		Queue->Num++;
		if (Queue->Tail == MAX_LEN_OF_MLME_QUEUE) 
		{
			Queue->Tail = 0;
		}
		NdisReleaseSpinLock(&(Queue->Lock));
		DBGPRINT(RT_DEBUG_INFO,"MlmeEnqueue, num=%d\n",Queue->Num);
	 
		Queue->Entry[Tail].Occupied = TRUE;
		Queue->Entry[Tail].Machine = Machine;
		Queue->Entry[Tail].MsgType = MsgType;
		Queue->Entry[Tail].MsgLen  = MsgLen;
		memcpy(Queue->Entry[Tail].Msg, Msg, MsgLen);
	}
	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 PRT2570ADAPTER	pAd, 
    OUT MLME_QUEUE *Queue, 
    IN UCHAR Rssi, 
    IN ULONG MsgLen, 
    IN VOID *Msg) 
{
    INT          Tail, Machine;
    MACFRAME    *Fr = (MACFRAME *)Msg;
    ULONG        MsgType;

	//DBGPRINT(RT_DEBUG_ERROR,"MlmeEnqueueForRecv ");
#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 5, 0)
	if((pAd->MLMEThr_pid <= 0))
		return FALSE;
#else
	if(!pAd->MLMEThr_active)
		return FALSE;
#endif
	// First check the size, it MUST not exceed the mlme queue size
	if (MsgLen > MAX_LEN_OF_MLME_BUFFER)
	{
		DBGPRINT(RT_DEBUG_ERROR, "MlmeEnqueueForRecv mlme frame too large, size = %d \n", MsgLen);
		return FALSE;
	}
	
	if ((RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) ||
		(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_REMOVE_IN_PROGRESS)))
	{
		DBGPRINT(RT_DEBUG_TRACE, "MlmeEnqueueForRecv (not in operation) Flag = 0x%x\n", pAd->Flags);
		return FALSE;
	}

    if (MlmeQueueFull(Queue)) 
    {
        //DBGPRINT(RT_DEBUG_ERROR, "MlmeEnqueueForRecv (queue full error) \n");
        return FALSE;
    }

    if (!MsgTypeSubst(pAd, Fr, &Machine, &MsgType)) 
    {
        DBGPRINT(RT_DEBUG_ERROR, "MlmeEnqueueForRecv (drop mgmt->subtype=%d)\n",Fr->Hdr.SubType);
        return FALSE;
    }
    
    // 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;
    }
    NdisReleaseSpinLock(&(Queue->Lock));
    DBGPRINT(RT_DEBUG_INFO, "MlmeEnqueueForRecv, num=%d\n",Queue->Num);
    
    Queue->Entry[Tail].Occupied = TRUE;
    Queue->Entry[Tail].Machine = Machine;
    Queue->Entry[Tail].MsgType = MsgType;
    Queue->Entry[Tail].MsgLen  = MsgLen;

    Queue->Entry[Tail].Rssi = Rssi;
    memcpy(Queue->Entry[Tail].Msg, Msg, MsgLen);
    
	RTUSBUp(pAd, (&(pAd->mlme_semaphore)));

    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));
	if (Queue->Num == 0)
	{
		NdisReleaseSpinLock(&(Queue->Lock));
		return FALSE; //Empty
	}

    *Elem = &(Queue->Entry[Queue->Head]);
    Queue->Num--;
    Queue->Head++;
    if (Queue->Head == MAX_LEN_OF_MLME_QUEUE) 
    {
        Queue->Head = 0;
    }
    
    NdisReleaseSpinLock(&(Queue->Lock));
    DBGPRINT(RT_DEBUG_INFO, "MlmeDequeue, num=%d\n",Queue->Num);

    return TRUE;
}

// IRQL = DISPATCH_LEVEL
VOID	MlmeRestartStateMachine(
    IN	PRT2570ADAPTER	pAd)
{

	DBGPRINT(RT_DEBUG_TRACE, "==> MlmeRestartStateMachine\n");

	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS);

	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
	if(pAd->Mlme.Running) 
	{
		DBGPRINT(RT_DEBUG_TRACE, "<== MlmeRestartStateMachine, Mlme is Runing[Queue=%d]!\n", pAd->Mlme.Queue.Num);
		NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
		return;
	} 
	pAd->Mlme.Running = TRUE;
	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);

	MlmeQueueDestroy(&pAd->Mlme.Queue);

	// Cancel all timer events
	// Be careful to cancel new added timer
	RTMPCancelTimer(&pAd->Mlme.AssocAux.AssocTimer);
	RTMPCancelTimer(&pAd->Mlme.AssocAux.ReassocTimer);
	RTMPCancelTimer(&pAd->Mlme.AssocAux.DisassocTimer);
	RTMPCancelTimer(&pAd->Mlme.AuthAux.AuthTimer);
	//    RTMPCancelTimer(&pAd->Mlme.AuthRspAux.AuthRspTimer, &Cancelled);
	RTMPCancelTimer(&pAd->Mlme.SyncAux.BeaconTimer);
	RTMPCancelTimer(&pAd->Mlme.SyncAux.ScanTimer);

	// 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;
	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS))
		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS);//steven:for test
	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS))
		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS);//steven:for test
	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);//steven:for test

	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS);

	// Remove running state
	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
	pAd->Mlme.Running = FALSE;
	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);

	DBGPRINT(RT_DEBUG_TRACE, "<== MlmeRestartStateMachine\n");
}

VOID	MlmePostRestartStateMachine(
    IN	PRT2570ADAPTER	pAd)
{
	//
	// Flag fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS will be clear at MlmeHandler
	// when required to do MLME Reset!
	// Since MlmeRestartStateMachine will do nothing when Mlme is running.
	//
	while (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS))
		NdisMSleep(100);

	// Change back to original channel in case of doing scan
	AsicSwitchChannel(pAd, pAd->PortCfg.Channel);
	AsicLockChannel(pAd, pAd->PortCfg.Channel);

	// Resume MSDU which is turned off durning scan
	RTUSBResumeMsduTransmission(pAd);
}

/*! \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);
    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 *Queue) 
{
    NdisAcquireSpinLock(&(Queue->Lock));
    Queue->Num  = 0;
    Queue->Head = 0;
    Queue->Tail = 0;
    NdisReleaseSpinLock(&(Queue->Lock));
}

/*! \brief   To substitute the message type if the message is coming from external
 *  \param  *Fr            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
 
 */
BOOLEAN MsgTypeSubst(
	IN PRT2570ADAPTER pAd,
	IN MACFRAME *Fr, 
	OUT INT *Machine, 
	OUT INT *MsgType) 
{
	USHORT Seq;
	UCHAR	EAPType;
	PHEADER_802_11 pHeader;
	
	// The only data type will pass to this function is EAPOL frame
	if (Fr->Hdr.Type == BTYPE_DATA) 
	{	 
		*Machine = WPA_PSK_STATE_MACHINE;

	 pHeader	= (PHEADER_802_11)Fr;
		
	 if( (pAd->PortCfg.CipherAlg == CIPHER_TKIP || pAd->PortCfg.CipherAlg == CIPHER_AES) && pHeader->Controlhead.Frame.Wep)
		EAPType = *((UCHAR*)Fr + LENGTH_802_11 + 8 + LENGTH_802_1_H + 1);//8 is IV + EIV
	 else
			EAPType = *((UCHAR*)Fr + LENGTH_802_11 + LENGTH_802_1_H + 1);
	 
	 
		return(WpaMsgTypeSubst(EAPType, MsgType));
	}	


    switch (Fr->Hdr.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
            memcpy(&Seq, &Fr->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;
        default:
            return FALSE;
            break;
    }

    return TRUE;
}

// ===========================================================================================
// 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   The destructor of the state machine
 *  \param  *S the statemachine
 *  \note    doing nothing at this moment, may need to do something if the implementation changed

 IRQL = PASSIVE_LEVEL
 
 */
VOID
StateMachineDestroy(IN STATE_MACHINE *S) 
{
}
/*! \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	PRT2570ADAPTER	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 PRT2570ADAPTER pAd, 
    IN MLME_QUEUE_ELEM *Elem) 
{
}

/*
    ==========================================================================
    Description:
        
	IRQL = PASSIVE_LEVEL

    ==========================================================================
 */
VOID LfsrInit(
    IN PRT2570ADAPTER pAd, 
    IN ULONG Seed) 
{
    if (Seed == 0) 
        pAd->Mlme.ShiftReg = 1;
    else 
        pAd->Mlme.ShiftReg = Seed;
}


/*
    ==========================================================================
    Description:
    ==========================================================================
 */
UCHAR RandomByte(
    IN PRT2570ADAPTER pAd) 
{
    ULONG i;
    UCHAR R, Result;

    R = 0;

    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;
}

/*
	==========================================================================
	Description:
	==========================================================================
 */
VOID AsicSwitchChannel(
	IN PRT2570ADAPTER pAd, 
	IN UCHAR Channel) 
{
	ULONG R3;
	UCHAR index;

	// TODO: need to update E2PROM format to add 802.11a channel's TX power calibration values
	if (Channel <= 14)	  
	R3 = pAd->PortCfg.ChannelTxPower[Channel - 1];
	else 
		R3 = pAd->PortCfg.ChannelTxPower[0];

	if (R3 > 31)  R3 = 31;

	// 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->PortCfg.TxPowerPercentage > 90)	   // 91 ~ 100%, treat as 100% in terms of mW
		;
	else if (pAd->PortCfg.TxPowerPercentage > 60)  // 61 ~ 90%, treat as 75% in terms of mW
		R3 -= 1;
	else if (pAd->PortCfg.TxPowerPercentage > 30)  // 31 ~ 60%, treat as 50% in terms of mW
		R3 -= 3;
	else if (pAd->PortCfg.TxPowerPercentage > 15)  // 16 ~ 30%, treat as 25% in terms of mW
		R3 -= 6;
	else if (pAd->PortCfg.TxPowerPercentage > 9)   // 10 ~ 15%, treat as 12.5% in terms of mW
		R3 -= 9;
	else										   // 0 ~ 9 %, treat as 6.25% in terms of mW
		R3 -= 12;

	R3 = R3 << 9; // shift TX power control to correct RF R3 bit position
	switch (pAd->PortCfg.RfType)
	{
		case RFIC_2522:
			for (index = 0; index < NUM_OF_2522_CHNL; index++)
			{
				if (Channel == RF2522RegTable[index].Channel)
				{
					R3 = R3 | RF2522RegTable[index].R3; // set TX power
					RTUSBWriteRFRegister(pAd, RF2522RegTable[index].R1);
					RTUSBWriteRFRegister(pAd, RF2522RegTable[index].R2);
					RTUSBWriteRFRegister(pAd, R3);
					pAd->PortCfg.LatchRfRegs.Channel = Channel;
					pAd->PortCfg.LatchRfRegs.R1 = RF2522RegTable[index].R1;
					pAd->PortCfg.LatchRfRegs.R2 = RF2522RegTable[index].R2;
					pAd->PortCfg.LatchRfRegs.R3 = R3;
					pAd->PortCfg.LatchRfRegs.R4 = RF2522RegTable[index].R4;
					break;
				}
			}
			break;

		case RFIC_2523:
			for (index = 0; index < NUM_OF_2523_CHNL; index++)
			{
				if (Channel == RF2523RegTable[index].Channel)
				{
					R3 = R3 | RF2523RegTable[index].R3; // set TX power
					RTUSBWriteRFRegister(pAd, RF2523RegTable[index].R1);
					RTUSBWriteRFRegister(pAd, RF2523RegTable[index].R2);
					RTUSBWriteRFRegister(pAd, R3);
					RTUSBWriteRFRegister(pAd, RF2523RegTable[index].R4);
					pAd->PortCfg.LatchRfRegs.Channel = Channel;
					pAd->PortCfg.LatchRfRegs.R1 = RF2523RegTable[index].R1;
					pAd->PortCfg.LatchRfRegs.R2 = RF2523RegTable[index].R2;
					pAd->PortCfg.LatchRfRegs.R3 = R3;
					pAd->PortCfg.LatchRfRegs.R4 = RF2523RegTable[index].R4;
					break;
				}
			}
			break;

		case RFIC_2524:
			for (index = 0; index < NUM_OF_2524_CHNL; index++)
			{
				if (Channel == RF2524RegTable[index].Channel)
				{
					R3 = R3 | RF2524RegTable[index].R3; // set TX power
					RTUSBWriteRFRegister(pAd, RF2524RegTable[index].R1);
					RTUSBWriteRFRegister(pAd, RF2524RegTable[index].R2);
					RTUSBWriteRFRegister(pAd, R3);
					RTUSBWriteRFRegister(pAd, RF2524RegTable[index].R4);
					pAd->PortCfg.LatchRfRegs.Channel = Channel;
					pAd->PortCfg.LatchRfRegs.R1 = RF2524RegTable[index].R1;
					pAd->PortCfg.LatchRfRegs.R2 = RF2524RegTable[index].R2;
					pAd->PortCfg.LatchRfRegs.R3 = R3;
					pAd->PortCfg.LatchRfRegs.R4 = RF2524RegTable[index].R4;
					break;
				}
			}
			break;
			
		case RFIC_2525:
			for (index = 0; index < NUM_OF_2525_CHNL; index++)
			{
				if (Channel == RF2525RegTable[index].Channel)
				{
					R3 = R3 | RF2525RegTable[index].R3; // set TX power
					RTUSBWriteRFRegister(pAd, RF2525RegTable[index].R1);

					RTUSBWriteRFRegister(pAd, RF2525RegTable[index].R2);

					RTUSBWriteRFRegister(pAd, R3);

					RTUSBWriteRFRegister(pAd, RF2525RegTable[index].R4);

					pAd->PortCfg.LatchRfRegs.Channel = Channel;
					pAd->PortCfg.LatchRfRegs.R1 = RF2525RegTable[index].R1;
					pAd->PortCfg.LatchRfRegs.R2 = RF2525RegTable[index].R2;
					pAd->PortCfg.LatchRfRegs.R3 = R3;
					pAd->PortCfg.LatchRfRegs.R4 = RF2525RegTable[index].R4;
					break;
				}
			}
			break;
			
		case RFIC_2525E:
			for (index = 0; index < NUM_OF_2525E_CHNL; index++)
			{
				if (Channel == RF2525eRegTable[index].Channel)
				{
					RTUSBWriteRFRegister(pAd, RF2525eRegTable[index].TempR2);
					RTUSBWriteRFRegister(pAd, RF2525eRegTable[index].R4);
					R3 = R3 | RF2525eRegTable[index].R3; // set TX power
					RTUSBWriteRFRegister(pAd, RF2525eRegTable[index].R1);
					RTUSBWriteRFRegister(pAd, RF2525eRegTable[index].R2);
					RTUSBWriteRFRegister(pAd, R3);
					RTUSBWriteRFRegister(pAd, RF2525eRegTable[index].R4);
					pAd->PortCfg.LatchRfRegs.Channel = Channel;
					pAd->PortCfg.LatchRfRegs.R1 = RF2525eRegTable[index].R1;
					pAd->PortCfg.LatchRfRegs.R2 = RF2525eRegTable[index].R2;
					pAd->PortCfg.LatchRfRegs.R3 = R3;
					pAd->PortCfg.LatchRfRegs.R4 = RF2525eRegTable[index].R4;
					break;
				}
			}
			break;
			
		case RFIC_5222:
			for (index = 0; index < NUM_OF_5222_CHNL; index++)
			{
				if (Channel == RF5222RegTable[index].Channel)
				{
					R3 = R3 | RF5222RegTable[index].R3; // set TX power
					RTUSBWriteRFRegister(pAd, RF5222RegTable[index].R1);
					RTUSBWriteRFRegister(pAd, RF5222RegTable[index].R2);
					RTUSBWriteRFRegister(pAd, R3);
					RTUSBWriteRFRegister(pAd, RF5222RegTable[index].R4);
					pAd->PortCfg.LatchRfRegs.Channel = Channel;
					pAd->PortCfg.LatchRfRegs.R1 = RF5222RegTable[index].R1;
					pAd->PortCfg.LatchRfRegs.R2 = RF5222RegTable[index].R2;
					pAd->PortCfg.LatchRfRegs.R3 = R3;
					pAd->PortCfg.LatchRfRegs.R4 = RF5222RegTable[index].R4;
					break;
				}
			}
			break;

		default:
			break;
	}

	DBGPRINT(RT_DEBUG_INFO, "AsicSwitchChannel(RF=%d) to #%d, TXPwr=%x%% = %x, R1=0x%08x, R2=0x%08x, R3=0x%08x, R4=0x%08x\n",
		pAd->PortCfg.RfType, 
		pAd->PortCfg.LatchRfRegs.Channel, 
		pAd->PortCfg.TxPowerPercentage,
		pAd->PortCfg.ChannelTxPower[Channel - 1],
		pAd->PortCfg.LatchRfRegs.R1, 
		pAd->PortCfg.LatchRfRegs.R2, 
		pAd->PortCfg.LatchRfRegs.R3, 
		pAd->PortCfg.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().
	==========================================================================
 */
VOID AsicLockChannel(
	IN PRT2570ADAPTER pAd, 
	IN ULONG Channel) 
{
	UCHAR	r70;
	USHORT	RxFailCnt;

	return;
	RTUSBReadBBPRegister(pAd, 70, &r70);
	if (Channel == 14)
		r70 |= 0x08;	// turn on Japan filter bit
	else
		r70 &= 0xf7;	// turn off Japan filter bit
	RTUSBWriteBBPRegister(pAd, 70, r70);

	// Clear false CRC durning switch channel
	RTUSBReadMACRegister(pAd, STA_CSR0, &RxFailCnt);


	NdisMSleep(10); 
 
	switch (pAd->PortCfg.RfType)
	{
		case RFIC_2522:
		case RFIC_2524:
		case RFIC_2525:
		case RFIC_5222:
		case RFIC_2525E:
			pAd->PortCfg.LatchRfRegs.R1 &= 0xfffdffff;	// RF R1.bit17 "tune_en1" OFF
			pAd->PortCfg.LatchRfRegs.R3 &= 0xfffffeff;	 // RF R3.bit8 "tune_en2" OFF
			RTUSBWriteRFRegister(pAd, pAd->PortCfg.LatchRfRegs.R1); 
			RTUSBWriteRFRegister(pAd, pAd->PortCfg.LatchRfRegs.R3); 
			DBGPRINT(RT_DEBUG_INFO, "AsicRfTuningExec(R1=0x%x,R3=0x%x)\n",pAd->PortCfg.LatchRfRegs.R1,pAd->PortCfg.LatchRfRegs.R3);
			break;
			
		case RFIC_2523:
			pAd->PortCfg.LatchRfRegs.R3 &= 0xfffffeff;	 // RF R3.bit8 "tune_en2" OFF
			RTUSBWriteRFRegister(pAd, pAd->PortCfg.LatchRfRegs.R3); 
			DBGPRINT(RT_DEBUG_INFO, "AsicRfTuningExec(R3=0x%x)\n",pAd->PortCfg.LatchRfRegs.R3);
			break;

		default:
			break;
	}
}

/*
	==========================================================================
	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 -
		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
	==========================================================================
 */
VOID AsicAdjustTxPower(
	IN PRT2570ADAPTER pAd) 
{
	ULONG R3, Channel, CurrTxPwr;

	if ((pAd->PortCfg.Channel >= 1) && (pAd->PortCfg.Channel <= 14))
		Channel = pAd->PortCfg.Channel;
	else 
		Channel = 1;  // don't have calibration info for 11A, temporarily use Channel 1
	
	// get TX Power base from E2PROM
	R3 = pAd->PortCfg.ChannelTxPower[Channel - 1];
	if (R3 > 31)  R3 = 31;

	// 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->PortCfg.TxPowerPercentage == 0xffffffff)       // AUTO TX POWER control
	{
		// only INFRASTRUCTURE mode and 100% TX power need furthur calibration
		if (pAd->MediaState == NdisMediaStateConnected)
		{
			// low TX power upon very-short distance to AP to solve some vendor's AP RX problem
			// in this case, no TSSI compensation is required.
			if ((pAd->DrsCounters.fNoisyEnvironment == FALSE) && 
			    (pAd->PortCfg.AvgRssi > (pAd->BBPTuningParameters.RSSIToDbmOffset - RSSI_FOR_LOWEST_TX_POWER)))
				R3 -= LOWEST_TX_POWER_DELTA;
			else if ((pAd->DrsCounters.fNoisyEnvironment == FALSE) && 
			    (pAd->PortCfg.AvgRssi > (pAd->BBPTuningParameters.RSSIToDbmOffset - RSSI_FOR_LOW_TX_POWER)))
				R3 -= LOW_TX_POWER_DELTA;
			// 2004-03-16 give OFDM rates lower than 48 mbps 2 more DB
			else if ((pAd->PortCfg.TxRate <= RATE_36) && (pAd->PortCfg.TxRate > RATE_11))
			{
			    R3 +=2;
			    if (R3 > 31) R3 = 31;
			}
            

			// 2 exclusive rules applied on CCK rates only -
			//	  1. always plus 2 db for CCK
			//	  2. adjust TX Power based on TSSI
			else 
			if (pAd->PortCfg.TxRate <= RATE_11)
			{
				// if "auto calibration based on TSSI" is not required, then
				// always give CCK 2 more db
				if (pAd->PortCfg.bAutoTxAgc == FALSE)
				{
					// do not give RATE_11 2 more db to avoid the case that
					// the outgoing NULL frame (for rate upgrade evaluation) 
					// using 12 Mbps OFDM rate is over-powered
					if (pAd->PortCfg.TxRate <= RATE_5_5)
					{
						R3 += 2;  // plus 2 db
						if (R3 > 31) R3 = 31;
					}
				}
				
				// Auto calibrate Tx AGC if bAutoTxAgc is TRUE and TX rate is CCK, 
				// because E2PROM's TSSI reference is valid only in CCK range.
				else
				{
					UCHAR	R1,TxPowerRef, TssiRef;

					R3 = (pAd->PortCfg.LatchRfRegs.R3 >> 9) & 0x0000001f;
					if (pAd->Mlme.PeriodicRound % 4 == 0) // every 4 second
					{
					TxPowerRef = pAd->PortCfg.ChannelTxPower[Channel - 1];
					TssiRef    = pAd->PortCfg.ChannelTssiRef[Channel - 1];
					RTUSBReadBBPRegister(pAd, BBP_Tx_Tssi,	&R1);
					if ((TssiRef >= (R1 + pAd->PortCfg.ChannelTssiDelta)) ||
						(TssiRef <= (R1 - pAd->PortCfg.ChannelTssiDelta)))
					{
						// Need R3 adjustment. However, we have to make sure there is only
						// plus / minus 5 variation allowed
						if (TssiRef > R1)
						{				
							R3 = (R3 < (ULONG) (TxPowerRef + 5)) ? (R3 + 1) : R3;
							if (R3 > 31)
								R3 = 31;
								DBGPRINT(RT_DEBUG_TRACE,"TSSI(R1)=%d, ++TxPwr=%d\n", R1, R3);
							}
							else
							{
								R3 = (R3 > (ULONG) (TxPowerRef - 5)) ? (R3 - 1) : R3;
								DBGPRINT(RT_DEBUG_TRACE,"TSSI(R1)=%d, --TxPwr=%d\n", R1, R3);
							}
						}
					}
				}
			}
			
		}
	}
	else // fixed AUTO TX power
	{
		if (pAd->PortCfg.TxPowerPercentage > 90)  // 91 ~ 100%, treat as 100% in terms of mW
		    ;
		else if (pAd->PortCfg.TxPowerPercentage > 60)  // 61 ~ 90%, treat as 75% in terms of mW
		    R3 -= 1;
		else if (pAd->PortCfg.TxPowerPercentage > 30)  // 31 ~ 60%, treat as 50% in terms of mW
		    R3 -= 3;
		else if (pAd->PortCfg.TxPowerPercentage > 15)  // 16 ~ 30%, treat as 25% in terms of mW
		    R3 -= 6;
		else if (pAd->PortCfg.TxPowerPercentage > 9)   // 10 ~ 15%, treat as 12.5% in terms of mW
		    R3 -= 9;
		else                                           // 0 ~ 9 %, treat as MIN(~3%) in terms of mW
		    R3 -= 12;
		if (R3 > 31)  R3 = 0;   // negative value, set as minimum 0

		// 2004-03-16 give TX rates <= 36 mbps 2 more DB
		if (pAd->PortCfg.TxRate <= RATE_36)
		{
		    R3 +=2;
		    if (R3 > 31) R3 = 31;
		}
	}

	
	// compare the desired R3.TxPwr value with current R3, if not equal
	// set new R3.TxPwr
	CurrTxPwr = (pAd->PortCfg.LatchRfRegs.R3 >> 9) & 0x0000001f;
	if (CurrTxPwr != R3)
	{
		CurrTxPwr = R3;
		R3 = (pAd->PortCfg.LatchRfRegs.R3 & 0xffffc1ff) | (R3 << 9);
		RTUSBWriteRFRegister(pAd, R3);
		pAd->PortCfg.LatchRfRegs.R3 = R3;
	}
    DBGPRINT(RT_DEBUG_INFO, "AsicAdjustTxPower = %d, AvgRssi = %d\n",
        CurrTxPwr, pAd->PortCfg.AvgRssi - pAd->BBPTuningParameters.RSSIToDbmOffset);
	
}



/*
	==========================================================================
	Description:
		put PHY to sleep here, and set next wakeup timer
	==========================================================================
 */
VOID AsicSleepThenAutoWakeup(
	IN PRT2570ADAPTER pAd, 
	IN USHORT TbttNumToNextWakeUp) 
{
	MAC_CSR18_STRUC Csr20;
	MAC_CSR17_STRUC Pwrcsr1;

	// we have decided to SLEEP, so at least do it for a BEACON period.
	if (TbttNumToNextWakeUp==0)
		TbttNumToNextWakeUp=1;
	
	// PWRCSR0 remains untouched
	
	// set CSR20 for next wakeup
	Csr20.value = 0;
	Csr20.field.NumBcnBeforeWakeup = TbttNumToNextWakeUp - 1;
	Csr20.field.DelayAfterBcn = (pAd->PortCfg.BeaconPeriod - 20); // 20 TU ahead of desired TBTT
	Csr20.field.AutoWake = 1;
	RTUSBWriteMACRegister(pAd, MAC_CSR18, Csr20.value);

	// set PWRCSR1 to put PHY into SLEEP state
	Pwrcsr1.value = 0;
	Pwrcsr1.field.PutToSleep = 1;
	Pwrcsr1.field.BbpDesireState = 1; // 01:SLEEP
	Pwrcsr1.field.RfDesireState = 1;  // 01:SLEEP
	RTUSBWriteMACRegister(pAd, MAC_CSR17, Pwrcsr1.value);
	Pwrcsr1.field.PutToSleep = 1;
	RTUSBWriteMACRegister(pAd, MAC_CSR17, Pwrcsr1.value);
	DBGPRINT(RT_DEBUG_TRACE,"MAC_CSR17 = 0x%x (AsicSleepThenAutoWakeup)\n", Pwrcsr1.value);
	pAd->PortCfg.Pss = PWR_SAVE;
}

/*
	==========================================================================
	Description:
		AsicForceWakeup() is used whenever manual wakeup is required
		AsicForceSleep() should only be used when Massoc==FALSE. When
		Massoc==TRUE, we should use AsicSleepThenAutoWakeup() instead.
	==========================================================================
 */
VOID AsicForceSleep(
	IN PRT2570ADAPTER pAd)
{
	MAC_CSR17_STRUC Pwrcsr1;

	if (pAd->PortCfg.Pss == PWR_ACTIVE)
	{
		DBGPRINT(RT_DEBUG_TRACE,">>>AsicForceSleep<<<\n");
		Pwrcsr1.value = 0;
		Pwrcsr1.field.RfDesireState = 1; // 01:SLEEP state
		Pwrcsr1.field.BbpDesireState = 1; // 01:SLEEP state
		Pwrcsr1.field.SetState = 1;
		RTUSBWriteMACRegister(pAd, MAC_CSR17, Pwrcsr1.value);
		pAd->PortCfg.Pss = PWR_SAVE;
	}
}

VOID AsicForceWakeup(
	IN PRT2570ADAPTER pAd)
{
	MAC_CSR17_STRUC Pwrcsr1;

	if (pAd->PortCfg.Pss == PWR_SAVE)
	{
		UINT i, j;
		USHORT temp;
		DBGPRINT(RT_DEBUG_TRACE,"--->AsicForceWakeup\n");

		// 2003-12-19 turn OFF auto wakeup first
		RTUSBWriteMACRegister(pAd, MAC_CSR18, 0);
		Pwrcsr1.value = 0;
		Pwrcsr1.field.RfDesireState = 3; // 11:AWAKE state
		Pwrcsr1.field.BbpDesireState = 3; // 11:AWAKE state
		RTUSBWriteMACRegister(pAd, MAC_CSR17, Pwrcsr1.value);
		Pwrcsr1.field.SetState = 1;
		RTUSBWriteMACRegister(pAd, MAC_CSR17, Pwrcsr1.value);
		//RTMPusecDelay(30000);
		NdisMSleep(30);
		RTUSBReadMACRegister(pAd, MAC_CSR17, &temp);
		if ((temp & 0x01E0) == 0x01E0)
		{
			DBGPRINT_RAW(RT_DEBUG_TRACE, "ASIC awaken!!!\n");
			i = 0;
		}
		else
		{
			DBGPRINT_RAW(RT_DEBUG_TRACE, "ASIC not awaken yet!!!\n");
		        Pwrcsr1.value = 0;
		        Pwrcsr1.field.RfDesireState = 3; // 11:AWAKE state
		        Pwrcsr1.field.BbpDesireState = 3; // 11:AWAKE state
		        Pwrcsr1.field.SetState = 1;
			i = 5;
		}
		for (j = 0; j < 3; j++)
		{
			if (i == 5)
			{
				RTUSBWriteMACRegister(pAd, MAC_CSR17, Pwrcsr1.value);
				DBGPRINT(RT_DEBUG_TRACE,"MAC_CSR17 = 0x%x (AsicForceWakeup)\n", Pwrcsr1.value);
			}
			else
				break;

			for (i = 0; i < 5; i++)
			{
				//RTMPusecDelay(5000);
				NdisMSleep(5);
				RTUSBReadMACRegister(pAd, MAC_CSR17, &temp);
				if ((temp & 0x01E0) == 0x01E0)
				{
					DBGPRINT_RAW(RT_DEBUG_TRACE, "ASIC awaken!!!\n");
					break;
				}
				else
				{
					DBGPRINT_RAW(RT_DEBUG_TRACE, "ASIC not awaken yet!!!\n");
				}
			}
		}
	
		pAd->PortCfg.Pss = PWR_ACTIVE;
		DBGPRINT(RT_DEBUG_TRACE,"<---AsicForceWakeup\n");
	}
}

/*
	==========================================================================
	Description:
	==========================================================================
 */
VOID AsicSetBssid(
	IN PRT2570ADAPTER pAd, 
	IN MACADDR *Bssid) 
{
	USHORT		   Addr4;

	Addr4 = (USHORT)(Bssid->Octet[0]) | (USHORT)(Bssid->Octet[1] << 8);
	RTUSBWriteMACRegister(pAd, MAC_CSR5, Addr4);

	Addr4 = (USHORT)(Bssid->Octet[2]) | (USHORT)(Bssid->Octet[3] << 8);
	RTUSBWriteMACRegister(pAd, MAC_CSR6, Addr4);

	Addr4 = (USHORT)(Bssid->Octet[4]) | (USHORT)(Bssid->Octet[5] << 8);
	RTUSBWriteMACRegister(pAd, MAC_CSR7, Addr4);
}

/*
	==========================================================================
	Description:
	==========================================================================
 */
VOID AsicDisableSync(
	IN PRT2570ADAPTER pAd) 
{
	DBGPRINT(RT_DEBUG_TRACE,"--->Disable TSF synchronization\n");
	//per jerry's request   	RTUSBReadMACRegister(pAd, TXRX_CSR19, 9);
}

/*
	==========================================================================
	Description:
	==========================================================================
 */
VOID AsicEnableBssSync(
	IN PRT2570ADAPTER pAd) 
{
	USHORT temp;
	TXRX_CSR18_STRUC Csr18;
	TXRX_CSR19_STRUC Csr14;
	TXRX_CSR20_STRUC Bcncsr1;

	DBGPRINT(RT_DEBUG_TRACE,"--->AsicEnableBssSync(INFRA mode)\n");
	
	RTUSBWriteMACRegister(pAd, TXRX_CSR19, 0);
	
	temp = (pAd->PortCfg.BeaconPeriod << 8);
	Csr18.field.Offset = (temp & 0x000F);
	Csr18.field.Interval = (temp >> 6);
	RTUSBWriteMACRegister(pAd, TXRX_CSR18, Csr18.value);
	
	Bcncsr1.value = 0;
	Bcncsr1.field.BeaconExpectWindow = 2;
	Bcncsr1.field.Offset = TBTT_PRELOAD_TIME >> 6; // we guess TBTT is 2 TU ahead of BEACON-RxEnd time
	RTUSBWriteMACRegister(pAd, TXRX_CSR20, Bcncsr1.value);

	
	Csr14.value = 0;
	Csr14.field.TsfCount = 1;
	Csr14.field.TsfSync = 1; // sync TSF in INFRASTRUCTURE mode
	Csr14.field.BeaconGen = 0;
//	Csr14.field.TbcnPreload = (pAd->PortCfg.BeaconPeriod - 30) << 4; // TODO: ???? 1 TU ???
	Csr14.field.Tbcn = 1;
	RTUSBWriteMACRegister(pAd, TXRX_CSR19, Csr14.value);
}

/*
	==========================================================================
	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.
	==========================================================================
 */
VOID AsicEnableIbssSync(
	IN PRT2570ADAPTER pAd)
{
	USHORT temp;
	TXRX_CSR18_STRUC Csr18;
	TXRX_CSR19_STRUC Csr14;
	TXRX_CSR20_STRUC Bcncsr1;
	
	DBGPRINT(RT_DEBUG_TRACE,"--->AsicEnableIbssSync(ADHOC mode)\n");

	RTUSBWriteMACRegister(pAd, TXRX_CSR19, 0);
	MakeIbssBeacon(pAd);

	temp = (pAd->PortCfg.BeaconPeriod << 8);
	Csr18.field.Offset = (temp & 0x000F);
	Csr18.field.Interval = (temp >> 6);
	RTUSBWriteMACRegister(pAd, TXRX_CSR18, Csr18.value);

	Bcncsr1.value = 0;
	Bcncsr1.field.Offset = 0x140;
	RTUSBWriteMACRegister(pAd, TXRX_CSR20, Bcncsr1.value);
	
	Csr14.value = 0;
	Csr14.field.TsfCount = 1;
	Csr14.field.TsfSync = 2; // sync TSF in IBSS mode
	Csr14.field.Tbcn = 1;
	Csr14.field.BeaconGen = 1;
	RTUSBWriteMACRegister(pAd, TXRX_CSR19, Csr14.value);
	RTUSBWriteMACRegister(pAd, TXRX_CSR19, 0);
	Csr14.value = 0;
	Csr14.field.TsfCount = 1;
	Csr14.field.TsfSync = 2; // sync TSF in IBSS mode
	Csr14.field.Tbcn = 1;
	Csr14.field.BeaconGen = 1;
	RTUSBWriteMACRegister(pAd, TXRX_CSR19, Csr14.value);
	//
	// Snice ADHOC Beacon Generation will get failed.
	// To prevent that, program the Register TXRX_CSR19 again.
	//    
	RTUSBWriteMACRegister(pAd, TXRX_CSR19, 0);

	Csr14.value = 0;
	Csr14.field.TsfCount = 1;
	Csr14.field.TsfSync = 2; // sync TSF in IBSS mode
	Csr14.field.Tbcn = 1;
	Csr14.field.BeaconGen = 1;
	RTUSBWriteMACRegister(pAd, TXRX_CSR19, Csr14.value);
}

/* Flash the S/W LED periodically. - Paul Chen */
VOID AsicLedPeriodicExec(
	IN	unsigned long data) 
{
}

/*
	==========================================================================
	Description:
	==========================================================================
 */
VOID AsicSetSlotTime(
	IN PRT2570ADAPTER pAd,
	IN BOOLEAN UseShortSlotTime) 
{
	USHORT		slottime=20;
	UCHAR PhyMode;
	pAd->PortCfg.ShortSlotInUsed = UseShortSlotTime;
	
	PhyMode = pAd->PortCfg.PhyMode;
	if (PhyMode == PHY_11ABG_MIXED)
	{
		if (pAd->PortCfg.Channel <=14)
			PhyMode = PHY_11BG_MIXED;
		else
			PhyMode = PHY_11A;
	}

	if ((INFRA_ON(pAd)) && (pAd->PortCfg.EnableTxBurst) && (pAd->PortCfg.WepStatus != Ndis802_11EncryptionDisabled))
		//Extend slot time if any encryption method is used to give ASIC more time to do encryption/decryption during Tx burst mode.
		RTUSBWriteMACRegister(pAd, MAC_CSR10, 0x20);
	else
	{
		if (PhyMode == PHY_11A)
			slottime = 9;
		else
			slottime = (UseShortSlotTime)? 9 : 20;
	
		RTUSBWriteMACRegister(pAd, MAC_CSR10, slottime);
	}
	//set IFS values
	if (PhyMode == PHY_11B)
	{
		RTUSBWriteMACRegister(pAd, MAC_CSR11, 16 - PHY_TR_SWITCH_TIME);
		RTUSBWriteMACRegister(pAd, MAC_CSR12, 64);
	}
	else
	{
		RTUSBWriteMACRegister(pAd, MAC_CSR11, 10 - PHY_TR_SWITCH_TIME);
		RTUSBWriteMACRegister(pAd, MAC_CSR12, 364);
	}
}
VOID AsicBbpTuning(
    IN PRT2570ADAPTER pAd)
{
	UCHAR R17;
	ULONG FalseCcaUpperThreshold = pAd->PortCfg.BbpTuning.FalseCcaUpperThreshold << 7;
	int dbm = pAd->PortCfg.AvgRssi - pAd->BBPTuningParameters.RSSIToDbmOffset;
	if ((! pAd->PortCfg.BbpTuningEnable) || (pAd->PortCfg.BbpTuning.VgcDelta==0))
	    return;

	R17 = pAd->PortCfg.BbpWriteLatch[17];
	DBGPRINT(RT_DEBUG_INFO, "RSSI = %d dbm, False CCA = %d BBPTuningThreshold = %x\n", dbm, pAd->MACCounters[3], pAd->BBPTuningParameters.BBPTuningThreshold);
	if (pAd->MediaState == NdisMediaStateConnected)
	{
		if ((dbm + (INT)(pAd->BBPTuningParameters.BBPTuningThreshold)) > 0)
		{
			DBGPRINT_RAW(RT_DEBUG_INFO, "RSSI > -%ddbm\n", pAd->BBPTuningParameters.BBPTuningThreshold);
			if (pAd->BBPTuningParameters.LargeCurrentRSSI == FALSE)
			{
				RTUSBWriteBBPRegister(pAd, 24, pAd->BBPTuningParameters.R24HigherValue);
				RTUSBWriteBBPRegister(pAd, 25, pAd->BBPTuningParameters.R25HigherValue);
				RTUSBWriteBBPRegister(pAd, 61, pAd->BBPTuningParameters.R61HigherValue);
				pAd->BBPTuningParameters.LargeCurrentRSSI = TRUE;
			}
		}
		else
		{
			DBGPRINT_RAW(RT_DEBUG_INFO, "RSSI <= -%ddbm\n", pAd->BBPTuningParameters.BBPTuningThreshold);
			if (pAd->BBPTuningParameters.LargeCurrentRSSI == TRUE)
			{
				RTUSBWriteBBPRegister(pAd, 24, pAd->BBPTuningParameters.R24LowerValue);
				RTUSBWriteBBPRegister(pAd, 25, pAd->BBPTuningParameters.R25LowerValue);
				RTUSBWriteBBPRegister(pAd, 61, pAd->BBPTuningParameters.R61LowerValue);
				pAd->BBPTuningParameters.LargeCurrentRSSI = FALSE;
			}
		}
		// Rule 0.
		// when RSSI is too weak, many signals will become false CCA thus affect R17 tuning.
		// so in this case, just stop R17 tuning (be sure R17 remains in <E2PROM-6, BBP_R17_DYNAMIC_UP_BOUND> range)
#if 1
		if ((dbm >= -40) && 
			(pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE))
		{
			if (R17 != 0x60)
			{
				R17 = 0x60;
				RTUSBWriteBBPRegister(pAd, 17, R17);
			}
			DBGPRINT(RT_DEBUG_INFO, "RSSI = %d dbm, fixed R17 at 0x%x\n", dbm, R17);
			return;
		}

#else
		if ((dbm < -80) && (pAd->Mlme.PeriodicRound > 20))
		{
		    if (R17 >= pAd->BBPTuningParameters.BBPR17MidSensitivity)
		    {
		        R17 = pAd->PortCfg.LastR17Value;
			    RTUSBWriteBBPRegister(pAd, 17, R17);
		    }
		    DBGPRINT(RT_DEBUG_TRACE, "RSSI = %d dbm, stop R17 at 0x%x\n", dbm, R17);
		    return;
		}
#endif
		// Rule 1. "special big-R17 for short-distance" when not SCANNING
		else if ((dbm >= RSSI_FOR_LOW_SENSIBILITY) && 
		    (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE))
		{
		    if (R17 != pAd->BBPTuningParameters.BBPR17LowSensitivity)
		    {
		        R17 = pAd->BBPTuningParameters.BBPR17LowSensitivity;
		        RTUSBWriteBBPRegister(pAd, 17, R17);
		    }
		    DBGPRINT(RT_DEBUG_INFO, "RSSI = %d dbm, fixed R17 at Low 0x%x\n", dbm, R17);
		    return;
		}
		// Rule 2. "special mid-R17 for mid-distance" when not SCANNING
		else if ((dbm >= RSSI_FOR_MID_SENSIBILITY) && 
		    (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE))
		{
		    if (R17 != pAd->BBPTuningParameters.BBPR17MidSensitivity)
		    {
		        R17 = pAd->BBPTuningParameters.BBPR17MidSensitivity;
		        RTUSBWriteBBPRegister(pAd, 17, R17);
		    }
		    DBGPRINT(RT_DEBUG_INFO, "RSSI = %d dbm, fixed R17 at Mid 0x%x\n", dbm, R17);
		    return;
		}
		// Rule 3. leave "short or mid-distance" condition, restore R17 to the 
		//    dynamic tuning range <E2PROM-6, BBP_R17_DYNAMIC_UP_BOUND>
#if 1
		else if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
		{
			UCHAR UpperBond;
			UCHAR LowerBond;

			LowerBond = pAd->BBPR17InitValue;
			if (dbm >= -77)
				UpperBond = pAd->PortCfg.BbpTuning.VgcUpperBound;
			else
				UpperBond = pAd->PortCfg.BbpTuning.VgcUpperBound - (-77 - dbm);

			if (UpperBond < LowerBond)
				UpperBond = LowerBond;

			DBGPRINT(RT_DEBUG_INFO, "R17 = %x , upper = %x, lower = %x\n", R17, UpperBond, LowerBond);

			if (R17 > UpperBond)
			{
				pAd->PortCfg.LastR17Value = R17 = UpperBond;
				RTUSBWriteBBPRegister(pAd, 17, R17);
				DBGPRINT(RT_DEBUG_INFO, "RSSI = %d dbm, restore R17 to 0x%x\n", dbm, R17);
				return;
			}
			if ((pAd->MACCounters[3] > FalseCcaUpperThreshold) &&
				(R17 < UpperBond))
			{
				R17 += pAd->PortCfg.BbpTuning.VgcDelta;
				pAd->PortCfg.LastR17Value = R17;
				RTUSBWriteBBPRegister(pAd, 17, R17);
				DBGPRINT(RT_DEBUG_INFO, "++R17= 0x%x\n", R17);
			}
			else if ((pAd->MACCounters[3] < pAd->PortCfg.BbpTuning.FalseCcaLowerThreshold) &&
					(R17 > LowerBond))
			{
				R17 -= pAd->PortCfg.BbpTuning.VgcDelta;
				pAd->PortCfg.LastR17Value = R17;
				RTUSBWriteBBPRegister(pAd, 17, R17);
				DBGPRINT(RT_DEBUG_INFO, "--R17= 0x%x\n", R17);
			}
			return;
		}
#else
		else if (R17 >= pAd->BBPTuningParameters.BBPR17MidSensitivity)
		{
		    R17 = pAd->PortCfg.LastR17Value;
		    RTUSBWriteBBPRegister(pAd, 17, R17);
		    DBGPRINT(RT_DEBUG_TRACE, "RSSI = %d dbm, restore R17 to 0x%x\n", dbm, R17);
		    return;
		}
#endif

	}

	// Rule 3. otherwise, R17 is currenly in dyanmic tuning range: <E2PROM-6, BBP_R17_DYNAMIC_UP_BOUND>.
	//    Keep dynamic tuning based on False CCA conter

	DBGPRINT(RT_DEBUG_INFO, "CCA flase alarm = %d, Avg RSSI= %d dbm\n", pAd->MACCounters[3], dbm);
	if ((pAd->MACCounters[3] > FalseCcaUpperThreshold) &&
	    (R17 < pAd->PortCfg.BbpTuning.VgcUpperBound))
	{
		R17 += pAd->PortCfg.BbpTuning.VgcDelta;
		pAd->PortCfg.LastR17Value = R17;
		RTUSBWriteBBPRegister(pAd, 17, R17);
		DBGPRINT(RT_DEBUG_INFO, "++R17= 0x%x\n", R17);
	}
	else if ((pAd->MACCounters[3] < pAd->PortCfg.BbpTuning.FalseCcaLowerThreshold) &&
	    (R17 > pAd->PortCfg.VgcLowerBound))
	{
		R17 -= pAd->PortCfg.BbpTuning.VgcDelta;
		pAd->PortCfg.LastR17Value = R17;
		RTUSBWriteBBPRegister(pAd, 17, R17);
		DBGPRINT(RT_DEBUG_INFO, "--R17= 0x%x\n", R17);
	}

}

// stop and restore R17 value upon SITE-SURVEY and LINK-DOWN
VOID AsicRestoreBbpSensibility(
	IN PRT2570ADAPTER pAd)
{
	UCHAR R17;
		DBGPRINT(RT_DEBUG_TRACE, "AsicRestoreBbpSensibility\n");

	R17 = pAd->PortCfg.BbpWriteLatch[17];
	if (R17 >= pAd->BBPTuningParameters.BBPR17MidSensitivity)
	{
		DBGPRINT(RT_DEBUG_TRACE, "1AsicRestoreBbpSensibility\n");
		R17 = pAd->PortCfg.LastR17Value;
		RTUSBWriteBBPRegister(pAd, 17, R17);
		DBGPRINT(RT_DEBUG_ERROR, "AsicRestoreBbpSensibility(set R17= 0x%x)\n", R17);
	}

}

/*
	========================================================================

	Routine Description:
		Mlme free the in-used nonpaged memory,
		move it to the unused memory link list

	Arguments:
		pAd 				Pointer to our adapter
		AllocVa 			Pointer to the base virtual address for free

	Return Value:
		None

	Note:

	========================================================================
*/
VOID	MlmeFreeMemory(
	IN PRT2570ADAPTER pAd,
	IN PVOID		 AllocVa)
{
	PMLME_MEMORY_STRUCT 			pPrevious;
	PMLME_MEMORY_STRUCT 			pMlmeMemoryStruct;
	BOOLEAN 						bIsFound;

	if (AllocVa == NULL)
		return;

 	bIsFound = FALSE;
	pPrevious = NULL;
	pMlmeMemoryStruct = pAd->Mlme.MemHandler.pInUseHead;
	while (pMlmeMemoryStruct)
	{
//		DBGPRINT(RT_DEBUG_TRACE,"pMlmeMemoryStruct->AllocVa=%x\n",(int)pMlmeMemoryStruct->AllocVa);
		if (pMlmeMemoryStruct->AllocVa == AllocVa)
		{ 
			//Found virtual address in the in-used link list
			//Remove it from the memory in-used link list, and move it to the unused link list
			if (pPrevious == NULL)
				pAd->Mlme.MemHandler.pInUseHead = pMlmeMemoryStruct->Next;
			else
				pPrevious->Next = pMlmeMemoryStruct->Next;
			pAd->Mlme.MemHandler.InUseCount--;

			//append it to the tail of unused list
			pMlmeMemoryStruct->Next = NULL;
 			if ((pAd->Mlme.MemHandler.pUnUseHead == NULL))
			{ //No head, add it as head
				pAd->Mlme.MemHandler.pUnUseHead = pMlmeMemoryStruct;
				pAd->Mlme.MemHandler.pUnUseTail = pMlmeMemoryStruct;
			}
			else
			{
				//Append it to the tail in pAd->Mlme.MemHandler.pUnUseTail
				pAd->Mlme.MemHandler.pUnUseTail->Next = pMlmeMemoryStruct;
				pAd->Mlme.MemHandler.pUnUseTail = pAd->Mlme.MemHandler.pUnUseTail->Next;
			}
			pAd->Mlme.MemHandler.UnUseCount++;
 
			bIsFound = TRUE;
			break;
		}
		else
		{
			pPrevious = pMlmeMemoryStruct;
			pMlmeMemoryStruct = pMlmeMemoryStruct->Next;
		}
	}
 }

/*
	========================================================================

	Routine Description:
		Get an unused nonpaged system-space memory for use

	Arguments:
		pAd 				Pointer to our adapter
		AllocVa 			Pointer to the base virtual address for later use

	Return Value:
		NDIS_STATUS_SUCCESS
		NDIS_STATUS_FAILURE
		NDIS_STATUS_RESOURCES

	Note:

	========================================================================
*/

NDIS_STATUS MlmeAllocateMemory(
	IN PRT2570ADAPTER pAd,
	OUT PVOID		 *AllocVa)
{
	PMLME_MEMORY_STRUCT 			pMlmeMemoryStruct = NULL;

	DBGPRINT(RT_DEBUG_INFO,"==> MlmeAllocateMemory\n");


 	if (pAd->Mlme.MemHandler.pUnUseHead == NULL)
	{ //There are no available memory for caller use
 		DBGPRINT(RT_DEBUG_ERROR,"MlmeAllocateMemory, failed!! (There are no available memory in list)\n");
		DBGPRINT(RT_DEBUG_ERROR,"<== MlmeAllocateMemory\n");
		return (NDIS_STATUS_RESOURCES);
	}
	pMlmeMemoryStruct = pAd->Mlme.MemHandler.pUnUseHead;
	//Unused memory point to next available
	pAd->Mlme.MemHandler.pUnUseHead = pAd->Mlme.MemHandler.pUnUseHead->Next;
	if (pAd->Mlme.MemHandler.pUnUseHead == NULL)
		pAd->Mlme.MemHandler.pUnUseTail = NULL;
	pAd->Mlme.MemHandler.UnUseCount--;
 
	*AllocVa = pMlmeMemoryStruct->AllocVa;			//Saved porint to Pointer the base virtual address of the nonpaged memory for caller use.

	pMlmeMemoryStruct->Next = NULL;
	//Append the unused memory link list to the in-used link list tail
	if (pAd->Mlme.MemHandler.pInUseHead == NULL)
	{//no head, so current Item assign to In-use Head.
		pAd->Mlme.MemHandler.pInUseHead = pMlmeMemoryStruct;
		pAd->Mlme.MemHandler.pInUseTail = pMlmeMemoryStruct;
	}
	else
	{
		pAd->Mlme.MemHandler.pInUseTail->Next = pMlmeMemoryStruct;
		pAd->Mlme.MemHandler.pInUseTail = pAd->Mlme.MemHandler.pInUseTail->Next;
	}
	pAd->Mlme.MemHandler.InUseCount++;
 


	DBGPRINT(RT_DEBUG_INFO, "MlmeAllocateMemory [pMlmeMemoryStruct=0x%lx][VA=0x%lx]\n", (unsigned long)pMlmeMemoryStruct, (unsigned long)pMlmeMemoryStruct->AllocVa);
	DBGPRINT(RT_DEBUG_INFO, "<== MlmeAllocateMemory[IN:%d][UN:%d]\n",
				pAd->Mlme.MemHandler.InUseCount, pAd->Mlme.MemHandler.UnUseCount);
	return (NDIS_STATUS_SUCCESS);
}


/*
	========================================================================

	Routine Description:
		Allocates resident (nonpaged) system-space memory for MLME send frames

	Arguments:
		pAd 				Pointer to our adapter
		Number				Total nonpaged memory for use
		Size				Each nonpaged memory size

	Return Value:
		NDIS_STATUS_SUCCESS
		NDIS_STATUS_RESOURCES

	Note:

	========================================================================
*/
NDIS_STATUS MlmeInitMemoryHandler(
	IN PRT2570ADAPTER pAd,
	IN UINT  Number,
	IN UINT  Size)
{
	PMLME_MEMORY_STRUCT 		Current = NULL;
	NDIS_STATUS 				Status = NDIS_STATUS_SUCCESS;
	UINT						i;

	DBGPRINT(RT_DEBUG_INFO,"==> MlmeInitMemory\n");
	pAd->Mlme.MemHandler.MemoryCount = 0;
	pAd->Mlme.MemHandler.pInUseHead = NULL;
	pAd->Mlme.MemHandler.pInUseTail = NULL;
	pAd->Mlme.MemHandler.pUnUseHead = NULL;
	pAd->Mlme.MemHandler.pUnUseTail = NULL;
	pAd->Mlme.MemHandler.MemRunning = FALSE;

 	//initial the memory free-pending array all to NULL;
	for (i = 0; i < MAX_MLME_HANDLER_MEMORY; i++)
		pAd->Mlme.MemHandler.MemFreePending[i] = NULL;

 	//
	// Available nonpaged memory counts MAX_MLME_HANDLER_MEMORY
	//
	if (Number > MAX_MLME_HANDLER_MEMORY)
		Number = MAX_MLME_HANDLER_MEMORY;
        
	for (i = 0; i < Number; i++)
	{
		//Allocate a nonpaged memory for link list use.
		//Status = NdisAllocateMemoryWithTag((VOID *)&Current, sizeof(MLME_MEMORY_STRUCT), NIC_TAG);
		 Current= (PMLME_MEMORY_STRUCT) kmalloc(sizeof(MLME_MEMORY_STRUCT), GFP_ATOMIC);
		 if (!Current) {
				DBGPRINT(RT_DEBUG_ERROR,"Not enough memory\n");
				Status = NDIS_STATUS_RESOURCES;
				break;
		 }	
		

		Current->AllocVa= (VOID *) kmalloc(Size, GFP_ATOMIC);
		 if (!Current->AllocVa) {
				DBGPRINT(RT_DEBUG_ERROR,"Not enough memory\n");
				kfree((VOID *)Current);
				Status = NDIS_STATUS_RESOURCES;
				break;
		 }

		memset(Current->AllocVa, 0, Size);

		pAd->Mlme.MemHandler.MemoryCount++;

		//build up the link list
		if (pAd->Mlme.MemHandler.pUnUseHead != NULL)
		{
			Current->Next = pAd->Mlme.MemHandler.pUnUseHead;
			pAd->Mlme.MemHandler.pUnUseHead = Current;
		}
		else
		{
			Current->Next = NULL;
			pAd->Mlme.MemHandler.pUnUseHead = Current;
		}

		if (pAd->Mlme.MemHandler.pUnUseTail == NULL)
			pAd->Mlme.MemHandler.pUnUseTail = Current;

	}
	
	if (pAd->Mlme.MemHandler.MemoryCount < Number)
	{
		Status = NDIS_STATUS_RESOURCES;
		DBGPRINT(RT_DEBUG_TRACE,"MlmeInitMemory Initial failed [Require=%d, available=%d]\n", Number, pAd->Mlme.MemHandler.MemoryCount);
	}

	pAd->Mlme.MemHandler.InUseCount = 0;
	pAd->Mlme.MemHandler.UnUseCount = Number;
	pAd->Mlme.MemHandler.PendingCount = 0;
 	DBGPRINT(RT_DEBUG_INFO,"<== MlmeInitMemory\n");
	
	return (Status);
}


VOID DeleteTimer(
	IN PRT2570ADAPTER	 pAd)
{
}


/*
    ========================================================================

    Routine Description:
        Free Mlme memory handler (link list, nonpaged memory, spin lock)

    Arguments:
        pAd                 Pointer to our adapter

    Return Value:
        None
    ========================================================================
*/
VOID MlmeFreeMemoryHandler(
    IN PRT2570ADAPTER pAd)
{
	PMLME_MEMORY_STRUCT      pMlmeMemoryStruct = NULL;

	DBGPRINT_RAW(RT_DEBUG_INFO, "--->MlmeFreeMemoryHandler\n");

	while (pAd->Mlme.MemHandler.pInUseHead != NULL)
	{
	    pMlmeMemoryStruct = pAd->Mlme.MemHandler.pInUseHead;
	    pAd->Mlme.MemHandler.pInUseHead = pAd->Mlme.MemHandler.pInUseHead->Next;
	    //Free the virtual address in AllocVa which size is MAX_LEN_OF_MLME_BUFFER
	    kfree(pMlmeMemoryStruct->AllocVa);
	    //Free the link list item self
	    kfree(pMlmeMemoryStruct);
	}

	//Free nonpaged memory, free it in the *Unused* link list.
	while (pAd->Mlme.MemHandler.pUnUseHead != NULL)
	{
	    pMlmeMemoryStruct = pAd->Mlme.MemHandler.pUnUseHead;
	    pAd->Mlme.MemHandler.pUnUseHead = pAd->Mlme.MemHandler.pUnUseHead->Next;
	    //Free the virtual address in AllocVa which size is MAX_LEN_OF_MLME_BUFFER
	    kfree(pMlmeMemoryStruct->AllocVa);
	    //Free the link list item self
	    kfree(pMlmeMemoryStruct);
	}
	
	DBGPRINT_RAW(RT_DEBUG_INFO, "<---MlmeFreeMemoryHandler\n");
}
/*
    ========================================================================

    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		PRT2570ADAPTER	pAd,
	IN OUT	UCHAR			SupRate[],
	IN OUT	UCHAR			*SupRateLen)
{
	UCHAR	RateIdx, i, j;
	UCHAR	NewRate[12], NewRateLen;
	
	NewRateLen = 0;
	
	if (pAd->PortCfg.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;
	memcpy(SupRate, NewRate, NewRateLen);
}
