/*************************************************************************** 
 * 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:	connect.c
 *
 *	Abstract:
 *
 *	Revision History:
 *	Who		When		What
 *	--------	----------	-------------------------------
 *	Name		Date		Modification logs
 *	Jan Lee		2005-06-01	Release
 ***************************************************************************/

#include "rt_config.h"

UCHAR	CipherSuiteWpaNoneTkip[] = {
		0x00, 0x50, 0xf2, 0x01,	// oui
		0x01, 0x00,				// Version
		0x00, 0x50, 0xf2, 0x02,	// Multicast
		0x01, 0x00,				// Number of unicast
		0x00, 0x50, 0xf2, 0x00,	// unicast
		0x01, 0x00,				// number of authentication method
		0x00, 0x50, 0xf2, 0x00	// authentication
		};
UCHAR	CipherSuiteWpaNoneTkipLen = (sizeof(CipherSuiteWpaNoneTkip) / sizeof(UCHAR));

UCHAR	CipherSuiteWpaNoneAes[] = {
		0x00, 0x50, 0xf2, 0x01,	// oui
		0x01, 0x00,				// Version
		0x00, 0x50, 0xf2, 0x04,	// Multicast
		0x01, 0x00,				// Number of unicast
		0x00, 0x50, 0xf2, 0x00,	// unicast
		0x01, 0x00,				// number of authentication method
		0x00, 0x50, 0xf2, 0x00	// authentication
		};
UCHAR	CipherSuiteWpaNoneAesLen = (sizeof(CipherSuiteWpaNoneAes) / sizeof(UCHAR));

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID MlmeCntlInit(
	IN PRT2570ADAPTER pAd, 
	IN STATE_MACHINE *S, 
	OUT STATE_MACHINE_FUNC Trans[]) 
{
	// Control state machine differs from other state machines, the interface 
	// follows the standard interface
	pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID MlmeCntlMachinePerformAction(
	IN PRT2570ADAPTER pAd, 
	IN STATE_MACHINE *S, 
	IN MLME_QUEUE_ELEM *Elem) 
{
	switch(pAd->Mlme.CntlMachine.CurrState) 
	{
		case CNTL_IDLE:
			CntlIdleProc(pAd, Elem);
			break;
		case CNTL_WAIT_DISASSOC:
			CntlWaitDisassocProc(pAd, Elem);
			break;
		case CNTL_WAIT_JOIN:
			CntlWaitJoinProc(pAd, Elem);
			break;
			
		// CNTL_WAIT_REASSOC is the only state in CNTL machine that does
		// not triggered directly or indirectly by "RTMPSetInformation(OID_xxx)". 
		// Therefore not protected by NDIS's "only one outstanding OID request" 
		// rule. Which means NDIS may SET OID in the middle of ROAMing attempts.
		// Current approach is to block new SET request at RTMPSetInformation()
		// when CntlMachine.CurrState is not CNTL_IDLE
		case CNTL_WAIT_REASSOC:
			CntlWaitReassocProc(pAd, Elem);
			break;
			
		case CNTL_WAIT_START:
			CntlWaitStartProc(pAd, Elem);
			break;
		case CNTL_WAIT_AUTH:
			CntlWaitAuthProc(pAd, Elem);
			break;
		case CNTL_WAIT_AUTH2:
			CntlWaitAuthProc2(pAd, Elem);
			break;
		case CNTL_WAIT_ASSOC:
			CntlWaitAssocProc(pAd, Elem);
			break;

		case CNTL_WAIT_OID_LIST_SCAN:
			if(Elem->MsgType == MT2_SCAN_CONF) 
			{
				// Resume TxRing after SCANING complete. We hope the out-of-service time
				// won't be too long to let upper layer time-out the waiting frames
				RTUSBResumeMsduTransmission(pAd);
				if (pAd->Mlme.CntlAux.CurrReqIsFromNdis)
				{
					NdisMSetInformationComplete(pAd->AdapterHandle, NDIS_STATUS_SUCCESS);
				}
				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
			}
			//
			// Following line will potentially cause infinite loop
			//
			//if (pAd->MediaState == NdisMediaStateDisconnected)
			//	  MlmeAutoReconnectLastSSID(pAd);
			break;
			
		case CNTL_WAIT_OID_DISASSOC:
			if (Elem->MsgType == MT2_DISASSOC_CONF) 
			{
				DBGPRINT_RAW(RT_DEBUG_TRACE, "LinkDown(MlmeCntlMachinePerformAction)\n");
				LinkDown(pAd);
				
				if (pAd->Mlme.CntlAux.CurrReqIsFromNdis)
				{
					NdisMSetInformationComplete(pAd->AdapterHandle, NDIS_STATUS_SUCCESS);
				}
				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
			}
			break;

		default:
			DBGPRINT(RT_DEBUG_ERROR, "CNTL - Illegal message type(=%d)", Elem->MsgType);
			break;
	}
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID CntlIdleProc(
	IN PRT2570ADAPTER pAd, 
	IN MLME_QUEUE_ELEM *Elem) 
{
	MLME_DISASSOC_REQ_STRUCT   DisassocReq;
		
	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
	{
		if (pAd->Mlme.CntlAux.CurrReqIsFromNdis)
		{
			NdisMSetInformationComplete(pAd->AdapterHandle, NDIS_STATUS_FAILURE);
			pAd->Mlme.CntlAux.CurrReqIsFromNdis = FALSE;
		}
		return;
	}
		
	switch(Elem->MsgType) 
	{
		case OID_802_11_SSID:
			CntlOidSsidProc(pAd, Elem);
			break;

		case RT_OID_802_11_BSSID:
			CntlOidRTBssidProc(pAd,Elem);
			break;

		case OID_802_11_BSSID_LIST_SCAN:
			CntlOidScanProc(pAd,Elem);
			break;
		
		case OID_802_11_DISASSOCIATE:
			DisassocParmFill(pAd, &DisassocReq, &pAd->PortCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
			// Set the control aux SSID to prevent it reconnect to old SSID
			// Since calling this indicate user don't want to connect to that SSID anymore.
			pAd->Mlme.CntlAux.SsidLen = 32;
			memset(pAd->Mlme.CntlAux.Ssid, 0, pAd->Mlme.CntlAux.SsidLen);
			break;

		case MT2_MLME_ROAMING_REQ:
			CntlMlmeRoamingProc(pAd, Elem);
			break;
			
		default:
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - Illegal message in CntlIdleProc(MsgType=%d)\n",Elem->MsgType);
			break;
	}
}

VOID CntlOidScanProc(
	IN PRT2570ADAPTER pAd,
	IN MLME_QUEUE_ELEM *Elem)
{
	MLME_SCAN_REQ_STRUCT	   ScanReq;
	CHAR					   BroadSsid[MAX_LEN_OF_SSID];
	ULONG					   BssIdx = BSS_NOT_FOUND;
	BSS_ENTRY				   CurrBss;
	ULONG		Now;

	// record current BSS if network is connected. 
	// 2003-2-13 do not include current IBSS if this is the only STA in this IBSS.
	if (pAd->MediaState == NdisMediaStateConnected) //	if (INFRA_ON(pAd) || ADHOC_ON(pAd))
	{
		BssIdx = BssTableSearch(&pAd->PortCfg.BssTab, &pAd->PortCfg.Bssid);
		if (BssIdx != BSS_NOT_FOUND)
		{
			memcpy(&CurrBss, &pAd->PortCfg.BssTab.BssEntry[BssIdx], sizeof(BSS_ENTRY));

			// 2003-2-20 reset this RSSI to a low value but not zero. In normal case, the coming SCAN
			//	   should return a correct RSSI to overwrite this. If no BEEACON received after SCAN, 
			//	   at least we still report a "greater than 0" RSSI since we claim it's CONNECTED.
			CurrBss.Rssi = 18; // about -82 dB
		}
	}
			
	// clean up previous SCAN result, add current BSS back to table if any
	BssTableInit(&pAd->PortCfg.BssTab); 
	if (BssIdx != BSS_NOT_FOUND)
	{
		// DDK Note: If the NIC is associated with a particular BSSID and SSID 
		//	  that are not contained in the list of BSSIDs generated by this scan, the 
		//	  BSSID description of the currently associated BSSID and SSID should be 
		//	  appended to the list of BSSIDs in the NIC's database.
		// To ensure this, we append this BSS as the first entry in SCAN result
		memcpy(&pAd->PortCfg.BssTab.BssEntry[0], &CurrBss, sizeof(BSS_ENTRY));
		pAd->PortCfg.BssTab.BssNr = 1;
	}

	BroadSsid[0] = '\0';
	ScanParmFill(pAd, &ScanReq, BroadSsid, 0, BSS_ANY, SCAN_PASSIVE);
	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, 
		sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
	Now = jiffies;
	pAd->PortCfg.LastScanTime = Now;
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID CntlOidSsidProc(
	IN PRT2570ADAPTER pAd, 
	IN MLME_QUEUE_ELEM * Elem) 
{
	NDIS_802_11_SSID		  *OidSsid = (NDIS_802_11_SSID *)Elem->Msg;
	MLME_DISASSOC_REQ_STRUCT   DisassocReq;
	ULONG					   Now;

	// Step 0. 
	//	  record the desired SSID and all matching BSSes into CntlAux.SsidBssTab for 
	//	  later-on iteration. Sort by RSSI order
	if (OidSsid->Ssid[0] == 0)
	{
		DBGPRINT_RAW(RT_DEBUG_TRACE, "empty string SSID\n");
		pAd->Mlme.CntlAux.SsidLen = 0;
	}
	else
		pAd->Mlme.CntlAux.SsidLen = (UCHAR)OidSsid->SsidLength;

	memcpy(pAd->Mlme.CntlAux.Ssid, OidSsid->Ssid, pAd->Mlme.CntlAux.SsidLen);
	BssTableSsidSort(pAd, &pAd->Mlme.CntlAux.SsidBssTab, pAd->Mlme.CntlAux.Ssid, pAd->Mlme.CntlAux.SsidLen);
	pAd->Mlme.CntlAux.BssIdx = 0;
	DBGPRINT(RT_DEBUG_TRACE,"CNTL - %d BSS match the desire SSID %s\n",pAd->Mlme.CntlAux.SsidBssTab.BssNr, pAd->Mlme.CntlAux.Ssid);
	Now = jiffies;
	
	if ((pAd->MediaState == NdisMediaStateConnected) &&
		MAC_ADDR_EQUAL(&pAd->PortCfg.Bssid, &pAd->Mlme.CntlAux.SsidBssTab.BssEntry[0].Bssid))
	{
		if (((pAd->PortCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->PortCfg.AuthMode == Ndis802_11AuthModeWPAPSK)) &&
			(pAd->PortCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
		{
			// For WPA, WPA-PSK, if the 1x port is not secured, we have to redo 
			// connection process
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - disassociate with current AP...\n");
			DisassocParmFill(pAd, &DisassocReq, &pAd->PortCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, 
						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
		}
		else if (pAd->bConfigChanged == TRUE)
		{
			// Config has changed, we have to reconnect the same AP
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - disassociate with current AP Because config changed...\n");
			DisassocParmFill(pAd, &DisassocReq, &pAd->PortCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, 
						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
		}
		else
		{
			// We only check if same to the BSSID with highest RSSI.
			// If roaming of same SSID required, we still do the reconnection.
			// same BSSID, go back to idle state directly
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - already with this BSSID. ignore this SET_SSID request\n");
			if (pAd->Mlme.CntlAux.CurrReqIsFromNdis)
			{
				NdisMSetInformationComplete(pAd->AdapterHandle, NDIS_STATUS_SUCCESS);
			}
			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
		} 
	} 
	else if (INFRA_ON(pAd)) 
	{
		// case 1. active association existent
		//	  roaming is done within miniport driver, nothing to do with configuration
		//	  utility. so upon a new SET(OID_802_11_SSID) is received, we just 
		//	  disassociate with the current (or previous) associated AP, if any, 
		//	  then perform a new association with this new SSID, no matter the 
		//	  new/old SSID are the same or npt.
		DBGPRINT(RT_DEBUG_TRACE, "CNTL - disassociate with current AP...\n");
		DisassocParmFill(pAd, &DisassocReq, &pAd->PortCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
		MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, 
					sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
	}
	else
	{	
		if (ADHOC_ON(pAd))
		{
//			  DBGPRINT(RT_DEBUG_TRACE, ("CNTL - drop current ADHOC\n"));
			DBGPRINT_RAW(RT_DEBUG_TRACE, "LinkDown(CntlOidSsidProc)\n");
			LinkDown(pAd);
			pAd->MediaState = NdisMediaStateDisconnected;
			NdisMIndicateStatus(pAd->AdapterHandle, NDIS_STATUS_MEDIA_DISCONNECT, (PVOID)NULL, 0);
			NdisMIndicateStatusComplete(pAd->AdapterHandle);
			DBGPRINT(RT_DEBUG_TRACE, "NDIS_STATUS_MEDIA_DISCONNECT Event C!\n");
		}

		if ((pAd->Mlme.CntlAux.SsidBssTab.BssNr == 0) &&
			(pAd->PortCfg.AutoReconnect == TRUE) &&
			(pAd->PortCfg.BssType == BSS_INFRA) &&
			(MlmeValidateSSID(pAd) == TRUE))
		{

		    MLME_SCAN_REQ_STRUCT       ScanReq;
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - No matching BSS, start a new scan\n");
			// BroadSsid[0] = '\0';
			ScanParmFill(pAd, &ScanReq, pAd->Mlme.CntlAux.Ssid, pAd->Mlme.CntlAux.SsidLen, 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 = Now;
		}
		else
		{
			IterateOnBssTab(pAd);
		}
	} 
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID CntlOidRTBssidProc(
	IN PRT2570ADAPTER pAd, 
	IN MLME_QUEUE_ELEM * Elem) 
{
	ULONG		BssIdx;
	MACADDR 	*pOidBssid = (MACADDR *)Elem->Msg;
	MLME_DISASSOC_REQ_STRUCT	DisassocReq;
	MLME_JOIN_REQ_STRUCT		JoinReq;
 
	COPY_MAC_ADDR(&pAd->Mlme.CntlAux.Bssid, pOidBssid);
	BssIdx = BssTableSearch(&pAd->PortCfg.BssTab, pOidBssid);
	   
	if (BssIdx == BSS_NOT_FOUND) 
	{
		DBGPRINT(RT_DEBUG_TRACE, "CNTL - BSSID not found. reply NDIS_STATUS_NOT_ACCEPTED\n");
		if (pAd->Mlme.CntlAux.CurrReqIsFromNdis)
		{
			//NdisMSetInformationComplete(pAd->AdapterHandle, NDIS_STATUS_NOT_ACCEPTED);
		}
		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
		return;
	}

	// copy the matched BSS entry from PortCfg.BssTab to CntlAux.SsidBssTab
	pAd->Mlme.CntlAux.BssIdx = 0;
	pAd->Mlme.CntlAux.SsidBssTab.BssNr = 1;
	memcpy(&pAd->Mlme.CntlAux.SsidBssTab.BssEntry[0], &pAd->PortCfg.BssTab.BssEntry[BssIdx], sizeof(BSS_ENTRY));

	// Add SSID into Mlme.CntlAux for site surey joining hidden SSID
	pAd->Mlme.CntlAux.SsidLen = pAd->Mlme.CntlAux.SsidBssTab.BssEntry[0].SsidLen;
	memcpy(pAd->Mlme.CntlAux.Ssid, pAd->Mlme.CntlAux.SsidBssTab.BssEntry[0].Ssid, pAd->Mlme.CntlAux.SsidLen);

	// 2002-11-26 skip the following checking. i.e. if user wants to re-connect to same AP
	// we just follow normal procedure. The reason of user doing this may because he/she changed
	// AP to another channel, but we still received BEACON from it thus don't claim Link Down.
	// Since user knows he's chnged AP channel, he'll re-connect again. By skipping the following
	// checking, we'll disassociate then re-do normal association with this AP at the new channel.
	// 2003-1-6 Re-enable this feature based on microsoft requirement which prefer not to re-do
	// connection when setting the same BSSID.
	if ( (pAd->MediaState == NdisMediaStateConnected) && //(INFRA_ON(pAd) || ADHOC_ON(pAd)) &&
		MAC_ADDR_EQUAL(&pAd->PortCfg.Bssid, pOidBssid))
	{
		// same BSSID, go back to idle state directly
		DBGPRINT(RT_DEBUG_TRACE, "CNTL - already in this BSSID. ignore this SET_BSSID request\n");
		if (pAd->Mlme.CntlAux.CurrReqIsFromNdis)
		{
			NdisMSetInformationComplete(pAd->AdapterHandle, NDIS_STATUS_SUCCESS);
		}
		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
	} 
	else 
	{
		if (INFRA_ON(pAd))
		{
			// disassoc from current AP first
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - disassociate with current AP ...\n");
			DisassocParmFill(pAd, &DisassocReq, &pAd->PortCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, 
						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);

			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
		}
		else
		{
			if (ADHOC_ON(pAd))
			{
				DBGPRINT_RAW(RT_DEBUG_TRACE, "LinkDown(CntlOidRTBssidProc)\n");
				LinkDown(pAd);
				pAd->MediaState = NdisMediaStateDisconnected;
				NdisMIndicateStatus(pAd->AdapterHandle, NDIS_STATUS_MEDIA_DISCONNECT, (PVOID)NULL, 0);
				NdisMIndicateStatusComplete(pAd->AdapterHandle);
				DBGPRINT(RT_DEBUG_TRACE, "NDIS_STATUS_MEDIA_DISCONNECT Event C!\n");
			}
			
			// No active association, join the BSS immediately
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - joining %02x:%02x:%02x:%02x:%02x:%02x ...\n",
				pOidBssid->Octet[0],pOidBssid->Octet[1],pOidBssid->Octet[2],
				pOidBssid->Octet[3],pOidBssid->Octet[4],pOidBssid->Octet[5]);
			JoinParmFill(pAd, &JoinReq, pAd->Mlme.CntlAux.BssIdx);
			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_JOIN_REQ, sizeof(MLME_JOIN_REQ_STRUCT), &JoinReq);

			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_JOIN;
		}
	} 
}

// Roaming is the only external request triggering CNTL state machine
// despite of other "SET OID" operation. All "SET OID" related oerations 
// happen in sequence, because no other SET OID will be sent to this device
// until the the previous SET operation is complete (successful o failed).
// So, how do we quarantee this ROAMING request won't corrupt other "SET OID"?
// or been corrupted by other "SET OID"?
VOID CntlMlmeRoamingProc(
	IN PRT2570ADAPTER pAd, 
	IN MLME_QUEUE_ELEM *Elem) 
{
	// TODO: 
	// AP in different channel may show lower RSSI than actual value??
	// should we add a weighting factor to compensate it?
	DBGPRINT(RT_DEBUG_TRACE,"CNTL - Roaming in CntlAux.RoamTab...\n");
	BssTableSortByRssi(&pAd->Mlme.CntlAux.RoamTab);
	pAd->Mlme.CntlAux.RoamIdx=0;
	IterateOnBssTab2(pAd);
	
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID CntlWaitDisassocProc(
	IN PRT2570ADAPTER pAd, 
	IN MLME_QUEUE_ELEM *Elem) 
{
	MLME_START_REQ_STRUCT	  StartReq;
	
	if (Elem->MsgType == MT2_DISASSOC_CONF) 
	{
		DBGPRINT_RAW(RT_DEBUG_TRACE, "LinkDown(CntlWaitDisassocProc)\n");
		LinkDown(pAd);
		
		// case 1. no matching BSS, and user wants ADHOC, so we just start a new one		
		if ((pAd->Mlme.CntlAux.SsidBssTab.BssNr==0) && (pAd->PortCfg.BssType == BSS_INDEP))
		{
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - No matching BSS, start a new ADHOC (Ssid=%s)...\n",pAd->Mlme.CntlAux.Ssid);
			StartParmFill(pAd, &StartReq, pAd->Mlme.CntlAux.Ssid, pAd->Mlme.CntlAux.SsidLen);
			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
		}
		// case 2. try each matched BSS
		else
		{
			IterateOnBssTab(pAd);
		}
	}
}
			
/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID CntlWaitJoinProc(
	IN PRT2570ADAPTER pAd, 
	IN MLME_QUEUE_ELEM *Elem) 
{
	USHORT						Reason;
	MLME_AUTH_REQ_STRUCT		AuthReq;

	if (Elem->MsgType == MT2_JOIN_CONF) 
	{
		memcpy(&Reason, Elem->Msg, sizeof(USHORT));
		if (Reason == MLME_SUCCESS) 
		{
			// 1. joined an IBSS, we are pretty much done here
			if (pAd->PortCfg.BssType == BSS_INDEP)
			{
				LinkUp(pAd, BSS_INDEP);
				if (pAd->Mlme.CntlAux.CurrReqIsFromNdis)
				{
					NdisMSetInformationComplete(pAd->AdapterHandle, NDIS_STATUS_SUCCESS);
				}
				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;

				RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS);//steven:for test
			} 
			// 2. joined a new INFRA network, start from authentication
			else 
			{
//				RTUSBWriteMACRegister(pAd, TXRX_CSR2, 0x67e);//steven:for test
				// either Ndis802_11AuthModeShared or Ndis802_11AuthModeAutoSwitch, try shared key first
				if ((pAd->PortCfg.AuthMode == Ndis802_11AuthModeShared) ||
					(pAd->PortCfg.AuthMode == Ndis802_11AuthModeAutoSwitch))
				{
					AuthParmFill(pAd, &AuthReq, &pAd->PortCfg.Bssid, Ndis802_11AuthModeShared);
				}
				else
				{
					AuthParmFill(pAd, &AuthReq, &pAd->PortCfg.Bssid, Ndis802_11AuthModeOpen);
				}
				MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ, 
							sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);

				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH;
			}
		}
		else
		{
			// 3. failed, try next BSS
			pAd->Mlme.CntlAux.BssIdx++;
			IterateOnBssTab(pAd);
		} 
	}	 
}
			

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID CntlWaitStartProc(
	IN PRT2570ADAPTER pAd, 
	IN MLME_QUEUE_ELEM *Elem) 
{
	USHORT		Result;

	if (Elem->MsgType == MT2_START_CONF) 
	{
		memcpy(&Result, Elem->Msg, sizeof(USHORT));
		if (Result == MLME_SUCCESS) 
		{
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - We have started a new ADHOC network\n");
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - BSSID %02x:%02x:%02x:%02x:%02x:%02x ...\n", 
				pAd->PortCfg.Bssid.Octet[0],
				pAd->PortCfg.Bssid.Octet[1],
				pAd->PortCfg.Bssid.Octet[2],
				pAd->PortCfg.Bssid.Octet[3],
				pAd->PortCfg.Bssid.Octet[4],
				pAd->PortCfg.Bssid.Octet[5]);
			LinkUp(pAd, BSS_INDEP);
			if (pAd->Mlme.CntlAux.CurrReqIsFromNdis)
			{
				NdisMSetInformationComplete(pAd->AdapterHandle, NDIS_STATUS_SUCCESS);
			}
			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
			
			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS);//steven:for test
		}
		else
		{
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - Start FAIL. BUG!!!!!\n");
			if (pAd->Mlme.CntlAux.CurrReqIsFromNdis)
			{
				NdisMSetInformationComplete(pAd->AdapterHandle, NDIS_STATUS_FAILURE);
			}
			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
			
			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS);//steven:for test
		}
	}
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID CntlWaitAuthProc(
	IN PRT2570ADAPTER pAd, 
	IN MLME_QUEUE_ELEM *Elem) 
{
	USHORT						 Reason;
	MLME_ASSOC_REQ_STRUCT		 AssocReq;
	MLME_AUTH_REQ_STRUCT		 AuthReq;

	if (Elem->MsgType == MT2_AUTH_CONF) 
	{
		memcpy(&Reason, Elem->Msg, sizeof(USHORT));
		if (Reason == MLME_SUCCESS) 
		{
			AssocParmFill(pAd, &AssocReq, &pAd->PortCfg.Bssid, pAd->PortCfg.CapabilityInfo, 
						  ASSOC_TIMEOUT, pAd->PortCfg.DefaultListenCount);
			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_ASSOC_REQ, 
						sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);

			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_ASSOC;
		} 
		else
		{
			// This fail may because of the AP already keep us in its MAC table without 
			// ageing-out. The previous authentication attempt must have let it remove us.
			// so try Authentication again may help. For D-Link DWL-900AP+ compatibility.
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - AUTH FAIL, try again...\n");
			if ((pAd->PortCfg.AuthMode == Ndis802_11AuthModeShared) ||
				(pAd->PortCfg.AuthMode == Ndis802_11AuthModeAutoSwitch))
			{
				// either Ndis802_11AuthModeShared or Ndis802_11AuthModeAutoSwitch, try shared key first
				AuthParmFill(pAd, &AuthReq, &pAd->PortCfg.Bssid, Ndis802_11AuthModeShared);
			}
			else
			{
				AuthParmFill(pAd, &AuthReq, &pAd->PortCfg.Bssid, Ndis802_11AuthModeOpen);
			}
			
			MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ, 
						sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);

			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH2;
		}
	}	 
}
			
/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID CntlWaitAuthProc2(
	IN PRT2570ADAPTER pAd, 
	IN MLME_QUEUE_ELEM *Elem) 
{
	USHORT						 Reason;
	MLME_ASSOC_REQ_STRUCT		 AssocReq;
	MLME_AUTH_REQ_STRUCT		 AuthReq;

	if (Elem->MsgType == MT2_AUTH_CONF) 
	{
		memcpy(&Reason, Elem->Msg, sizeof(USHORT));
		if (Reason == MLME_SUCCESS) 
		{
			AssocParmFill(pAd, &AssocReq, &pAd->PortCfg.Bssid, pAd->PortCfg.CapabilityInfo, 
						  ASSOC_TIMEOUT, pAd->PortCfg.DefaultListenCount);
			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_ASSOC_REQ, 
						sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);

			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_ASSOC;
		} 
		else
		{
			if ((pAd->PortCfg.AuthMode == Ndis802_11AuthModeAutoSwitch) &&
				 (pAd->Mlme.AuthAux.Alg == Ndis802_11AuthModeShared))
			{
				DBGPRINT(RT_DEBUG_TRACE, "CNTL - AUTH FAIL, try OPEN system...\n");
				AuthParmFill(pAd, &AuthReq, &pAd->PortCfg.Bssid, Ndis802_11AuthModeOpen);
				MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ, 
							sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);

				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH2;
			}
			else 
			{
				// not success, try next BSS
				DBGPRINT(RT_DEBUG_TRACE, "CNTL - AUTH FAIL, give up; try next BSS\n");
				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE; //???????
				pAd->Mlme.CntlAux.BssIdx++;
//				RTUSBWriteMACRegister(pAd, TXRX_CSR2, 0x7e);//steven:for test
				IterateOnBssTab(pAd);
			}
		}
	}	 
}
			
/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID CntlWaitAssocProc(
	IN PRT2570ADAPTER pAd, 
	IN MLME_QUEUE_ELEM *Elem) 
{
	USHORT		Reason;

	if (Elem->MsgType == MT2_ASSOC_CONF) 
	{
		memcpy(&Reason, Elem->Msg, sizeof(USHORT));
		if (Reason == MLME_SUCCESS) 
		{
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - Association successful on BSS #%d\n",pAd->Mlme.CntlAux.BssIdx);
//			RTUSBWriteMACRegister(pAd, TXRX_CSR2, 0x7e);//steven:for test
			LinkUp(pAd, BSS_INFRA);
			if (pAd->Mlme.CntlAux.CurrReqIsFromNdis)
			{
				NdisMSetInformationComplete(pAd->AdapterHandle, NDIS_STATUS_SUCCESS);
			}
			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;

			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS);//steven:for test
		} 
		else 
		{
			// not success, try next BSS
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - Association fails on BSS #%d\n",pAd->Mlme.CntlAux.BssIdx);
//			  pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;//steven:for test
			pAd->Mlme.CntlAux.BssIdx++;
//			RTUSBWriteMACRegister(pAd, TXRX_CSR2, 0x7e);//steven:for test
			IterateOnBssTab(pAd);
		}
	}
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID CntlWaitReassocProc(
	IN PRT2570ADAPTER pAd, 
	IN MLME_QUEUE_ELEM *Elem) 
{
	USHORT		Result;

	if (Elem->MsgType == MT2_REASSOC_CONF) 
	{
		memcpy(&Result, Elem->Msg, sizeof(USHORT));
		if (Result == MLME_SUCCESS) 
		{
			BSS_ENTRY *pBss = &pAd->Mlme.CntlAux.RoamTab.BssEntry[pAd->Mlme.CntlAux.RoamIdx];

			// COPY_MAC_ADDR(&pAd->PortCfg.Bssid, &pBss->Bssid);
			// AsicSetBssid(pAd, &pAd->PortCfg.Bssid);
			
			// The following steps are supposed to be done after JOIN in normal procedure
			// But since this RE-ASSOC skips the JOIN procedure, we have to do it after
			// RE-ASSOC succeeds. If RE-ASSOC fails, then stay at original AP without any change
			pAd->PortCfg.BeaconPeriod = pBss->BeaconPeriod;
			pAd->PortCfg.Channel = pBss->Channel;
			// The security setting should always follow upper layer definition, not from frame
			//pAd->PortCfg.PrivacyInvoked = CAP_IS_PRIVACY_ON(pBss->CapabilityInfo);
			pAd->PortCfg.SupportedRatesLen = pBss->RatesLen;
			memcpy(pAd->PortCfg.SupportedRates, pBss->Rates, pBss->RatesLen);

			// Check for 802.11g information, if 802.11 b /g mixed mode.
			pAd->PortCfg.CapabilityInfo = pBss->CapabilityInfo;

			pAd->PortCfg.CfpPeriod = pBss->CfpPeriod;
			pAd->PortCfg.CfpMaxDuration = pBss->CfpMaxDuration;
			pAd->PortCfg.CfpDurRemain = pBss->CfpDurRemaining;
			pAd->PortCfg.CfpCount = pBss->CfpCount;

			// 
			// NDIS requires a new Link UP indication but no Link Down for RE-ASSOC
			//
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - Re-assocition successful on BSS #%d\n", pAd->Mlme.CntlAux.RoamIdx);
			LinkUp(pAd, BSS_INFRA);
			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
			
			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS);//steven:for test
		} 
		else 
		{
			// reassoc failed, try to pick next BSS in the BSS Table
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - Re-assocition fails on BSS #%d\n", pAd->Mlme.CntlAux.RoamIdx);
			pAd->Mlme.CntlAux.RoamIdx++;
			IterateOnBssTab2(pAd);
		}
	}
}
			
/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID LinkUp(
	IN PRT2570ADAPTER pAd,
	IN UCHAR BssType) 
{
	ULONG	Now;
	UCHAR			buffer[22];
	
	DBGPRINT(RT_DEBUG_TRACE, "CNTL - !!! LINK UP !!!\n");
	MlmeUpdateTxRates(pAd, TRUE);
	RTUSBMultiReadMAC(pAd, STA_CSR0, buffer, 22);
	memcpy(&pAd->Mlme.PrevWlanCounters, &pAd->WlanCounters, sizeof(COUNTER_802_11));
	memset(&pAd->DrsCounters, 0, sizeof(COUNTER_DRS));

	Now = jiffies;
	pAd->PortCfg.LastBeaconRxTime = Now;   // last RX timestamp
	
	if ((pAd->PortCfg.WindowsTxPreamble != Rt802_11PreambleLong) &&
		CAP_IS_SHORT_PREAMBLE_ON(pAd->PortCfg.CapabilityInfo))
	{

		DBGPRINT(RT_DEBUG_TRACE, "CNTL - !!! Set to short preamble!!!\n");
		MlmeSetTxPreamble(pAd, Rt802_11PreambleShort);
	}
	
	pAd->PortCfg.BssType = BssType;
	if (BssType == BSS_INDEP)
	{
//		USHORT SentBeaconsCount, ReceivedBeaconsCount;
//		RTUSBReadMACRegister(pAd, STA_CSR5, &SentBeaconsCount);
//		RTUSBReadMACRegister(pAd, STA_CSR10, &ReceivedBeaconsCount);
		pAd->PortCfg.Mibss = TRUE;
		pAd->PortCfg.Massoc = FALSE;
		AsicEnableIbssSync(pAd);
		
#ifdef	SINGLE_ADHOC_LINKUP
		// Although this did not follow microsoft's recommendation.
		//Change based on customer's request
		pAd->MediaState = NdisMediaStateConnected;
		NdisMIndicateStatus(pAd->AdapterHandle, NDIS_STATUS_MEDIA_CONNECT, (PVOID)NULL, 0);
		NdisMIndicateStatusComplete(pAd->AdapterHandle);
#endif

	}
	else // BSS_INFRA
	{
            // need to check
		//InterlockedExchange(&(pAd->PortCfg.DataPacketsFromAP), 0);
		pAd->PortCfg.Massoc = TRUE;
		pAd->PortCfg.Mibss = FALSE;

		// NOTE:
		// the decision of using "short slot time" or not may change dynamically due to
		// new STA association to the AP. so we have to decide that upon parsing BEACON, not here

		// NOTE:
		// the decision to use "RTC/CTS" or "CTS-to-self" protection or not may change dynamically
		// due to new STA association to the AP. so we have to decide that upon parsing BEACON, not here
		
		ComposePsPoll(pAd);
		ComposeNullFrame(pAd);
		AsicEnableBssSync(pAd);
		if (pAd->PortCfg.EnableTxBurst)
		{
			//Set CWmin/CWmax to 0.
			RTUSBWriteMACRegister(pAd, MAC_CSR22, 0x100);
		}
		else
		{
			RTUSBWriteMACRegister(pAd, MAC_CSR22, 0x53);
		}
	
		// only INFRASTRUCTURE mode need to indicate connectivity immediately; ADHOC mode
		// should wait until at least 2 active nodes in this BSSID.
		pAd->MediaState = NdisMediaStateConnected;
		NdisMIndicateStatus(pAd->AdapterHandle, NDIS_STATUS_MEDIA_CONNECT, (PVOID)NULL, 0);
		NdisMIndicateStatusComplete(pAd->AdapterHandle);
	}

	DBGPRINT(RT_DEBUG_TRACE, "NDIS_STATUS_MEDIA_CONNECT Event B!\n");

	if ((pAd->PortCfg.LedMode != LED_MODE_SINGLE)&&(pAd->PortCfg.LedMode != LED_MODE_TXRX_ACTIVITY))
	{
		ASIC_LED_ACT_ON(pAd);
	}
	if (pAd->PortCfg.LedMode == LED_MODE_ALPHA){
		pAd->PortCfg.LedCntl.fSiteSurvey = FALSE;
		pAd->PortCfg.LedCntl.fLinkUp = TRUE;
	}	
	AsicSetSlotTime(pAd, FALSE);
	pAd->Mlme.PeriodicRound = 0;
	// Reset config flag
	pAd->bConfigChanged = FALSE;
	// Update extra information to link is up
	pAd->ExtraInfo = GENERAL_LINK_UP;
	pAd->PortCfg.WpaState = SS_START;

	RTUSBKickBulkOut(pAd);

}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID LinkDown(
	IN PRT2570ADAPTER pAd) 
{
	DBGPRINT(RT_DEBUG_TRACE, "CNTL - !!! LINK DOWN !!!\n");

	if (ADHOC_ON(pAd))		// Adhoc mode link down
	{
		pAd->PortCfg.Mibss = FALSE;

#ifdef	SINGLE_ADHOC_LINKUP
		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));
#else
		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
		{
			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));
		}
#endif
		
	}
	else					// Infra structure mode
	{
		pAd->PortCfg.Massoc = FALSE;
		pAd->MediaState = NdisMediaStateDisconnected;
		DBGPRINT(RT_DEBUG_TRACE, "NDIS_STATUS_MEDIA_DISCONNECT Event A!\n");
		BssTableDeleteEntry(&pAd->PortCfg.BssTab, &(pAd->PortCfg.Bssid));

		// restore back to - 
		//		1. long slot (20 us) or short slot (9 us) time
		//		2. turn on/off RTS/CTS and/or CTS-to-self protection
		//		3. short preamble
		if (pAd->PortCfg.BGProtectionInUsed == TRUE)
		{
			pAd->PortCfg.BGProtectionInUsed = FALSE;
			DBGPRINT(RT_DEBUG_TRACE, "Link down - turn off B/G protection\n");
		}

		if (pAd->PortCfg.Pss == PWR_SAVE)
		{
			RTUSBWriteMACRegister(pAd, MAC_CSR1, 1);
			RTUSBWriteMACRegister(pAd, MAC_CSR1, 4);
			pAd->PortCfg.Pss = PWR_ACTIVE;
		}
	}
	
	AsicSetSlotTime(pAd, FALSE);
	RTUSBWriteMACRegister(pAd, MAC_CSR22, 0x53);
	AsicRestoreBbpSensibility(pAd);
	
	if (pAd->PortCfg.WindowsTxPreamble == Rt802_11PreambleShort)
		MlmeSetTxPreamble(pAd, Rt802_11PreambleShort);
	else
		MlmeSetTxPreamble(pAd, Rt802_11PreambleLong);

	if ((pAd->PortCfg.LedMode != LED_MODE_SINGLE) && (pAd->PortCfg.LedMode != LED_MODE_ASUS))
	{
		ASIC_LED_ACT_OFF(pAd);
	}
	else if ((pAd->PortCfg.LedMode == LED_MODE_ASUS) && (pAd->PortCfg.bRadio == TRUE))
	{
		RTUSBWriteMACRegister(pAd, MAC_CSR20, 0x0002);
	}
	AsicDisableSync(pAd);
	pAd->Mlme.PeriodicRound = 0;
	pAd->ScanAllowed = TRUE;

	// Remove PortCfg Information after link down
	memset(&(pAd->PortCfg.Bssid), 0, MAC_ADDR_LEN);
	
	// Reset WPA-PSK state. Only reset when supplicant enabled
	if (pAd->PortCfg.WpaState != SS_NOTUSE)
	{
		pAd->PortCfg.WpaState = SS_START;
		// Clear Replay counter
		memset(pAd->PortCfg.ReplayCounter, 0, 8);
	}
	// Remove all WPA keys after link down
	RTMPWPARemoveAllKeys(pAd);
	// 802.1x port control
	pAd->PortCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
	pAd->PortCfg.MicErrCnt = 0;
	if (pAd->PortCfg.LedMode == LED_MODE_ALPHA)
		pAd->PortCfg.LedCntl.fLinkUp = FALSE;

	// Update extra information to link is up
	pAd->ExtraInfo = GENERAL_LINK_DOWN;
	// Start STA supplicant state machine
	//pAd->PortCfg.WpaState = SS_NOTUSE;

}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID MlmeCntlConfirm(
	IN PRT2570ADAPTER pAd, 
	IN ULONG MsgType, 
	IN USHORT Msg) 
{
	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MsgType, sizeof(USHORT), &Msg);
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID IterateOnBssTab(
	IN PRT2570ADAPTER pAd) 
{
	MLME_START_REQ_STRUCT	StartReq;
	MLME_JOIN_REQ_STRUCT	JoinReq;
	ULONG					BssIdx;

	BssIdx = pAd->Mlme.CntlAux.BssIdx;
	if (BssIdx < pAd->Mlme.CntlAux.SsidBssTab.BssNr) 
	{
		if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS))
			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS);//steven:for test

		DBGPRINT(RT_DEBUG_TRACE, "CNTL - Trying BSSID %02x:%02x:%02x:%02x:%02x:%02x ...\n", 
			pAd->Mlme.CntlAux.SsidBssTab.BssEntry[BssIdx].Bssid.Octet[0],
			pAd->Mlme.CntlAux.SsidBssTab.BssEntry[BssIdx].Bssid.Octet[1],
			pAd->Mlme.CntlAux.SsidBssTab.BssEntry[BssIdx].Bssid.Octet[2],
			pAd->Mlme.CntlAux.SsidBssTab.BssEntry[BssIdx].Bssid.Octet[3],
			pAd->Mlme.CntlAux.SsidBssTab.BssEntry[BssIdx].Bssid.Octet[4],
			pAd->Mlme.CntlAux.SsidBssTab.BssEntry[BssIdx].Bssid.Octet[5]);
		JoinParmFill(pAd, &JoinReq, BssIdx);
		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_JOIN_REQ, sizeof(MLME_JOIN_REQ_STRUCT),
					&JoinReq);
		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_JOIN;
	}
	else if (pAd->PortCfg.BssType == BSS_INDEP)
	{
		DBGPRINT(RT_DEBUG_TRACE, "CNTL - All BSS fail; start a new ADHOC (Ssid=%s)...\n",pAd->Mlme.CntlAux.Ssid);
		StartParmFill(pAd, &StartReq, pAd->Mlme.CntlAux.Ssid, (UCHAR)pAd->Mlme.CntlAux.SsidLen);
		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
	}
	else // no more BSS
	{
		if (pAd->Mlme.CntlAux.CurrReqIsFromNdis)
		{
			DBGPRINT(RT_DEBUG_TRACE, "CNTL - All BSS fail; reply NDIS_STATUS_NOT_ACCEPTED\n");
			NdisMSetInformationComplete(pAd->AdapterHandle, NDIS_STATUS_SUCCESS);
		}
		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;

		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_JOIN_IN_PROGRESS);//steven:for test
	} 
}

// for re-association only
VOID IterateOnBssTab2(
	IN PRT2570ADAPTER pAd) 
{
	MLME_REASSOC_REQ_STRUCT ReassocReq;
	ULONG					BssIdx;
	BSS_ENTRY				*pBss;
	
	BssIdx = pAd->Mlme.CntlAux.RoamIdx;
	pBss = &pAd->Mlme.CntlAux.RoamTab.BssEntry[BssIdx];

	if (BssIdx < pAd->Mlme.CntlAux.RoamTab.BssNr)
	{
		if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS))
			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS);//steven:for test

		DBGPRINT(RT_DEBUG_TRACE, "CNTL - try BSS #%d %02x:%02x:%02x:%02x:%02x:%02x ...\n", 
			BssIdx, pBss->Bssid.Octet[0],pBss->Bssid.Octet[1],pBss->Bssid.Octet[2],
			pBss->Bssid.Octet[3],pBss->Bssid.Octet[4],pBss->Bssid.Octet[5]);

		AsicSwitchChannel(pAd, pBss->Channel);
		AsicLockChannel(pAd, pBss->Channel);
		
		// reassociate message has the same structure as associate message
		AssocParmFill(pAd, &ReassocReq, &pBss->Bssid, pBss->CapabilityInfo, 
					  ASSOC_TIMEOUT, pAd->PortCfg.DefaultListenCount);
		MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_REASSOC_REQ, 
					sizeof(MLME_REASSOC_REQ_STRUCT), &ReassocReq);
		
		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_REASSOC;
	}
	else // no more BSS
	{
		DBGPRINT(RT_DEBUG_TRACE, "CNTL - All roaming failed, stay with original AP\n");
		AsicSwitchChannel(pAd, pAd->PortCfg.Channel);
		AsicLockChannel(pAd, pAd->PortCfg.Channel);
		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;

		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_REASSOC_IN_PROGRESS);//steven:for test
	} 
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID JoinParmFill(
	IN PRT2570ADAPTER pAd, 
	IN OUT MLME_JOIN_REQ_STRUCT *JoinReq, 
	IN ULONG BssIdx) 
{
	JoinReq->BssIdx = BssIdx;
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID AssocParmFill(
	IN PRT2570ADAPTER pAd, 
	IN OUT MLME_ASSOC_REQ_STRUCT *AssocReq, 
	IN MACADDR					 *Addr, 
	IN USHORT					  CapabilityInfo, 
	IN ULONG					  Timeout, 
	IN USHORT					  ListenIntv) 
{
	COPY_MAC_ADDR(&AssocReq->Addr, Addr);
	// Add mask to support 802.11b mode only
	AssocReq->CapabilityInfo = CapabilityInfo & 0xfff3; // not cf-pollable, not cf-poll-request
	AssocReq->Timeout = Timeout;
	AssocReq->ListenIntv = ListenIntv;
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID ScanParmFill(
	IN PRT2570ADAPTER pAd, 
	IN OUT MLME_SCAN_REQ_STRUCT *ScanReq, 
	IN CHAR Ssid[], 
	IN UCHAR SsidLen, 
	IN UCHAR BssType, 
	IN UCHAR ScanType) 
{
	ScanReq->SsidLen = SsidLen;
	memcpy(ScanReq->Ssid, Ssid, SsidLen);
	ScanReq->BssType = BssType;
	ScanReq->ScanType = ScanType;
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID DisassocParmFill(
	IN PRT2570ADAPTER pAd, 
	IN OUT MLME_DISASSOC_REQ_STRUCT *DisassocReq, 
	IN MACADDR *Addr, 
	IN USHORT Reason) 
{
	COPY_MAC_ADDR(&DisassocReq->Addr, Addr);
	DisassocReq->Reason = Reason;
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID StartParmFill(
	IN PRT2570ADAPTER pAd, 
	IN OUT MLME_START_REQ_STRUCT *StartReq, 
	IN CHAR Ssid[], 
	IN UCHAR SsidLen) 
{
	memcpy(StartReq->Ssid, Ssid, SsidLen);
	StartReq->SsidLen = SsidLen;
}

/*
	==========================================================================
	Description:
	==========================================================================
*/
VOID AuthParmFill(
	IN PRT2570ADAPTER pAd, 
	IN OUT MLME_AUTH_REQ_STRUCT *AuthReq, 
	IN MACADDR *Addr, 
	IN USHORT Alg) 
{
	COPY_MAC_ADDR(&AuthReq->Addr, Addr);
	AuthReq->Alg = Alg;
	AuthReq->Timeout = AUTH_TIMEOUT;
}

/*
	==========================================================================
	Description:
	==========================================================================
 */
VOID ComposePsPoll(
	IN PRT2570ADAPTER pAd)
{
	PSPOLL_FRAME *pPsPoll = (PSPOLL_FRAME *)&(pAd->PsPollContext.TransferBuffer->WirelessPacket);
	memset(pPsPoll, 0, sizeof(PSPOLL_FRAME));
	pPsPoll->Type = BTYPE_CNTL;
	pPsPoll->SubType = SUBTYPE_PS_POLL;
	pPsPoll->Aid = pAd->PortCfg.Aid | 0xC000;
	COPY_MAC_ADDR(&(pPsPoll->Bssid), &pAd->PortCfg.Bssid);
	COPY_MAC_ADDR(&(pPsPoll->Ta), &(pAd->CurrentAddress));
}

VOID ComposeNullFrame(
	IN PRT2570ADAPTER pAd)
{
	PHEADER_802_11 pNullFrame = (PHEADER_802_11)&(pAd->NullContext.TransferBuffer->WirelessPacket);
	MgtMacHeaderInit(pAd, (PMACHDR)pNullFrame, SUBTYPE_NULL_FUNC, 1, &pAd->PortCfg.Bssid, &pAd->PortCfg.Bssid);
	pNullFrame->Controlhead.Duration = 0;
	pNullFrame->Controlhead.Frame.Type = BTYPE_DATA;
}

/*
	==========================================================================
	Description:
		Pre-build a BEACON frame in the shared memory
	==========================================================================
*/
VOID MakeIbssBeacon(
	IN PRT2570ADAPTER pAd) 
{
	UCHAR			SsidIe = IE_SSID, DsIe = IE_DS_PARM, IbssIe = IE_IBSS_PARM, SuppIe = IE_SUPP_RATES, 
					DsLen = 1, IbssLen = 2;
	UCHAR			i, ExtRateIe = IE_EXT_SUPP_RATES, ExtRatesLen;
	UCHAR			ErpIe[3] = {IE_ERP, 1, 0x04};
	MACHDR			BcnHdr;
	USHORT			CapabilityInfo;
	LARGE_INTEGER	FakeTimestamp;
	ULONG			FrameLen;
	UCHAR			SupportedRatesLen = 0;
	UCHAR			SupportedRates[MAX_LEN_OF_SUPPORTED_RATES];
	BOOLEAN			Privacy;
	PUCHAR			pBeaconFrame;
	PTXD_STRUC		pTxD;
	ULONG			BulkOutLength;
	PTX_CONTEXT		pBeaconContext = &(pAd->BeaconContext[0]);

#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 5, 0)
	if (pAd->MLMEThr_pid <= 0)
	    return;
#else
	if (!pAd->MLMEThr_active)
	    return;
#endif
    // 2003-12-10 802.11g WIFI spec disallow OFDM rates in 802.11g ADHOC mode
    // make sure 1,2,5.5,11 are the firt 4 rates in PortCfg.SupportedRates[] array
    if ((pAd->PortCfg.PhyMode == PHY_11BG_MIXED) && (pAd->PortCfg.AdhocMode == 0))
    {
	    for (i = 0; i < pAd->PortCfg.SupportedRatesLen; i++)
	    {
	        switch (pAd->PortCfg.SupportedRates[i] & 0x7f)
	        {
	            case 2:
	            case 4:
	            case 11:
	            case 22:
	                SupportedRates[SupportedRatesLen] = pAd->PortCfg.SupportedRates[i];
	                SupportedRatesLen ++;
	                break;
	            default:
	                break;
	        }
	    }
	    // error handling - should never happen
	    if (SupportedRatesLen != 4)
	    {
            SupportedRatesLen = 4;
            SupportedRates[0] = 0x82;
            SupportedRates[1] = 0x84;
            SupportedRates[2] = 0x8b;
            SupportedRates[3] = 0x96;
	    }
    }
    else
    {
        SupportedRatesLen = pAd->PortCfg.SupportedRatesLen;
        memcpy(SupportedRates, pAd->PortCfg.SupportedRates, SupportedRatesLen);
    }

	if (pBeaconContext->InUse == TRUE)
		return;
	else
		pBeaconContext->InUse = TRUE;

    pAd->PortCfg.AtimWin = 0;  // ??????
    
    // compose IBSS beacon frame
    MgtMacHeaderInit(pAd, &BcnHdr, SUBTYPE_BEACON, 0, &pAd->PortCfg.Broadcast, &pAd->PortCfg.Bssid);
    Privacy = (pAd->PortCfg.WepStatus == Ndis802_11Encryption1Enabled) || 
              (pAd->PortCfg.WepStatus == Ndis802_11Encryption2Enabled) || 
              (pAd->PortCfg.WepStatus == Ndis802_11Encryption3Enabled);
    CapabilityInfo = CAP_GENERATE(0, 1, 0, 0, Privacy, (pAd->PortCfg.WindowsTxPreamble == Rt802_11PreambleShort));

	// Prepare beacon frame, this should go to beacon_ring[1] which contains the real body.
	pBeaconFrame = (PUCHAR) pAd->BeaconContext[1].TransferBuffer->WirelessPacket;
	
    if (SupportedRatesLen <= 8)
    {
        MakeOutgoingFrame(pBeaconFrame,                &FrameLen,
                      MAC_HDR_LEN,                     &BcnHdr, 
                      TIMESTAMP_LEN,                   &FakeTimestamp,
                      2,                               &pAd->PortCfg.BeaconPeriod,
                      2,                               &CapabilityInfo,
                      1,                               &SsidIe, 
                      1,                               &pAd->PortCfg.SsidLen, 
                      pAd->PortCfg.SsidLen,             pAd->PortCfg.Ssid,
                      1,                               &SuppIe, 
                      1,                               &SupportedRatesLen,
                      SupportedRatesLen,               SupportedRates, 
                      1,                               &DsIe, 
                      1,                               &DsLen, 
                      1,                               &pAd->PortCfg.Channel,
                      1,                               &IbssIe, 
                      1,                               &IbssLen, 
                      2,                               &pAd->PortCfg.AtimWin,
                      END_OF_ARGS);
    }
    else
    {
        ExtRatesLen = SupportedRatesLen - 8;
        SupportedRatesLen = 8;
        MakeOutgoingFrame(pBeaconFrame,                &FrameLen,
                      MAC_HDR_LEN,                     &BcnHdr, 
                      TIMESTAMP_LEN,                   &FakeTimestamp,
                      2,                               &pAd->PortCfg.BeaconPeriod,
                      2,                               &CapabilityInfo,
                      1,                               &SsidIe, 
                      1,                               &pAd->PortCfg.SsidLen, 
                      pAd->PortCfg.SsidLen,             pAd->PortCfg.Ssid,
                      1,                               &SuppIe, 
                      1,                               &SupportedRatesLen,
                      SupportedRatesLen,                SupportedRates, 
                      1,                               &DsIe, 
                      1,                               &DsLen, 
                      1,                               &pAd->PortCfg.Channel,
                      1,                               &IbssIe, 
                      1,                               &IbssLen, 
                      2,                               &pAd->PortCfg.AtimWin,
                      3,                               ErpIe,
                      1,                               &ExtRateIe,
                      1,                               &ExtRatesLen,
                      ExtRatesLen,                     &SupportedRates[SupportedRatesLen],
                      END_OF_ARGS);
    }
    
	// If adhoc secruity is set for WPA-None, append the cipher suite IE
	if (pAd->PortCfg.AuthMode == Ndis802_11AuthModeWPANone)
	{
		ULONG	tmp = 0;
		UCHAR	WpaIe = IE_WPA;
		
		if (pAd->PortCfg.WepStatus == Ndis802_11Encryption2Enabled)		// Tkip
		{
        	MakeOutgoingFrame(pBeaconFrame + FrameLen,                    &tmp,
				1,						  &WpaIe,
				1,						  &CipherSuiteWpaNoneTkipLen,
				CipherSuiteWpaNoneTkipLen,	  &CipherSuiteWpaNoneTkip[0],
				END_OF_ARGS);
			FrameLen += tmp;
		}
		else if (pAd->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)	// Aes
		{
        	MakeOutgoingFrame(pBeaconFrame + FrameLen,                    &tmp,
				1,						  &WpaIe,
				1,						  &CipherSuiteWpaNoneAesLen,
				CipherSuiteWpaNoneAesLen,	  &CipherSuiteWpaNoneAes[0],
				END_OF_ARGS);
			FrameLen += tmp;
		}
	}
	
    BulkOutLength = sizeof(TXD_STRUC) + FrameLen;
	if ((BulkOutLength % 2) == 1)
		BulkOutLength ++;
//	if (BulkOutLength % pAd->BulkOutMaxPacketSize == 0)
//		BulkOutLength += 2;

    for (i = 0; i < BEACON_RING_SIZE; i++)
	{
		pTxD = (PTXD_STRUC) &pAd->BeaconContext[i].TransferBuffer->TxDesc;
		memset(pTxD, 0, sizeof(TXD_STRUC));
		pBeaconContext = &(pAd->BeaconContext[i]);;

		// Both TxD need to put in the right descriptor
    	RTUSBWriteBeaconDescriptor(pTxD, FrameLen, FrameLen + 4, pAd->PortCfg.MlmeRate, 4, pAd->PortCfg.TxPreambleInUsed);
		if (i == 0)
			pBeaconContext->BulkOutSize = 1;
		else
			pBeaconContext->BulkOutSize = BulkOutLength;			
    DBGPRINT(RT_DEBUG_TRACE, "  (pBeaconContext->BulkOutSize=%d)\n", pBeaconContext->BulkOutSize);
	}

	// The flags will always start from beacon 0
	RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_BEACON_0);

	// Kick bulk out 
	RTUSBKickBulkOut(pAd);

}
